diff --git a/.github/ISSUE_TEMPLATE.md b/.github/ISSUE_TEMPLATE.md index e55cdc40d3..f705392bb3 100644 --- a/.github/ISSUE_TEMPLATE.md +++ b/.github/ISSUE_TEMPLATE.md @@ -1,5 +1,6 @@ diff --git a/.github/workflows/mirror-rolling-to-master.yaml b/.github/workflows/mirror-rolling-to-master.yaml new file mode 100644 index 0000000000..2885eb4a4f --- /dev/null +++ b/.github/workflows/mirror-rolling-to-master.yaml @@ -0,0 +1,13 @@ +name: Mirror rolling to master + +on: + push: + branches: [ rolling ] + +jobs: + mirror-to-master: + runs-on: ubuntu-latest + steps: + - uses: zofrex/mirror-branch@v1 + with: + target-branch: master diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000000..e43b0f9889 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +.DS_Store diff --git a/CODEOWNERS b/CODEOWNERS new file mode 100644 index 0000000000..7ee4b9af4d --- /dev/null +++ b/CODEOWNERS @@ -0,0 +1,2 @@ +# This file was generated by https://github.com/audrow/update-ros2-repos +* @ivanpauno @hidmic @wjwwood diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index 6f63de9e5d..cfba094dac 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -11,3 +11,8 @@ be under the Apache 2 License, as dictated by that the terms of any separate license agreement you may have executed with Licensor regarding such Contributions. ~~~ + +Contributors must sign-off each commit by adding a `Signed-off-by: ...` +line to commit messages to certify that they have the right to submit +the code they are contributing to the project according to the +[Developer Certificate of Origin (DCO)](https://developercertificate.org/). diff --git a/README.md b/README.md new file mode 100644 index 0000000000..baf82c8520 --- /dev/null +++ b/README.md @@ -0,0 +1,18 @@ +# rclcpp + +This repository contains the source code for the ROS Client Library for C++ package, included with a standard install of any ROS 2 distro. + +rclcpp provides the standard C++ API for interacting with ROS 2. + +## Usage + +`#include "rclcpp/rclcpp.hpp"` allows use of the most common elements of the ROS 2 system. + +The link to the latest API documentation can be found on the [rclcpp package info page](https://docs.ros.org/en/rolling/p/rclcpp). + + +### Examples + +The ROS 2 tutorials [Writing a simple publisher and subscriber](https://docs.ros.org/en/rolling/Tutorials/Writing-A-Simple-Cpp-Publisher-And-Subscriber.html). +and [Writing a simple service and client](https://docs.ros.org/en/rolling/Tutorials/Writing-A-Simple-Cpp-Service-And-Client.html) +contain some examples of rclcpp APIs in use. diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst new file mode 100644 index 0000000000..07f7d2b4d2 --- /dev/null +++ b/rclcpp/CHANGELOG.rst @@ -0,0 +1,1410 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package rclcpp +^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +29.2.0 (2024-11-25) +------------------- +* accept custom allocator for LoanedMessage. (`#2672 `_) +* Contributors: Tomoya Fujita + +29.1.0 (2024-11-20) +------------------- +* a couple of typo fixes in doc section for LoanedMessage. (`#2676 `_) +* Make sure callback_end tracepoint is triggered in AnyServiceCallback (`#2670 `_) +* Correct the incorrect comments in generic_client.hpp (`#2662 `_) +* Fix NodeOptions assignment operator (`#2656 `_) +* set QoS History KEEP_ALL explicitly for statistics publisher. (`#2650 `_) +* Fix test_intra_process_manager.cpp with rmw_zenoh_cpp (`#2653 `_) +* Fixed test_events_executors in zenoh (`#2643 `_) +* rmw_fastrtps supports service event gid uniqueness test. (`#2638 `_) +* print warning if event callback is not supported instead of passing exception. (`#2648 `_) +* Implement callback support of async_send_request for service generic client (`#2614 `_) +* Contributors: Alejandro Hernández Cordero, Barry Xu, Chris Lalancette, Christophe Bedard, Romain DESILLE, Tomoya Fujita + +29.0.0 (2024-10-03) +------------------- +* Fixed test qos rmw zenoh (`#2639 `_) +* verify client gid uniqueness for a single service event. (`#2636 `_) +* Skip some tests in test_qos_event and run others with event types supported by rmw_zenoh (`#2626 `_) +* Shutdown the context before context's destructor is invoked in tests (`#2633 `_) +* Skip rmw zenoh content filtering tests (`#2627 `_) +* Use InvalidServiceTypeError for unavailable service type in GenericClient (`#2629 `_) +* Implement generic service (`#2617 `_) +* fix events-executor warm-up bug and add unit-tests (`#2591 `_) +* remove unnecessary gtest-skip in test_executors (`#2600 `_) +* Correct node name in service test code (`#2615 `_) +* Minor naming fixes for ParameterValue to_string() function (`#2609 `_) +* Removed clang warnings (`#2605 `_) +* Fix a couple of issues in the documentation. (`#2608 `_) +* deprecate the static single threaded executor (`#2598 `_) +* Fix name of ParameterEventHandler class in doc (`#2604 `_) +* subscriber_statistics_collectors\_ should be protected by mutex. (`#2592 `_) +* Fix bug in timers lifecycle for events executor (`#2586 `_) +* fix rclcpp/test/rclcpp/CMakeLists.txt to check for the correct targets existance (`#2596 `_) +* Shut down context during init if logging config fails (`#2594 `_) +* Make more of the Waitable API abstract (`#2593 `_) +* Contributors: Alberto Soragna, Alejandro Hernández Cordero, Alexis Pojomovsky, Barry Xu, Chris Lalancette, Christophe Bedard, Kang, Hsin-Yi, Tomoya Fujita + +28.3.3 (2024-07-29) +------------------- +* Only compile the tests once. (`#2590 `_) +* Contributors: Chris Lalancette + +28.3.2 (2024-07-24) +------------------- +* Updated rcpputils path API (`#2579 `_) +* Make the subscriber_triggered_to_receive_message test more reliable. (`#2584 `_) + * Make the subscriber_triggered_to_receive_message test more reliable. + In the current code, inside of the timer we create the subscription + and the publisher, publish immediately, and expect the subscription + to get it immediately. But it may be the case that discovery + hasn't even happened between the publisher and the subscription + by the time the publish call happens. + To make this more reliable, create the subscription and publish *before* + we ever create and spin on the timer. This at least gives 100 + milliseconds for discovery to happen. That may not be quite enough + to make this reliable on all platforms, but in my local testing this + helps a lot. Prior to this change I can make this fail one out of 10 + times, and after the change I've run 100 times with no failures. +* Have the EventsExecutor use more common code (`#2570 `_) + * move notify waitable setup to its own function + * move mutex lock to retrieve_entity utility + * use entities_need_rebuild\_ atomic bool in events-executors + * remove duplicated set_on_ready_callback for notify_waitable + * use mutex from base class rather than a new recursive mutex + * use current_collection\_ member in events-executor + * delay adding notify waitable to collection + * postpone clearing the current collection + * commonize notify waitable and collection + * commonize add/remove node/cbg methods + * fix linter errors + --------- +* Removed deprecated methods and classes (`#2575 `_) +* Release ownership of entities after spinning cancelled (`#2556 `_) + * Release ownership of entities after spinning cancelled + * Move release action to every exit point in different spin functions + * Move wait_result\_.reset() before setting spinning to false + * Update test code + * Move test code to test_executors.cpp + --------- +* Split test_executors.cpp even further. (`#2572 `_) + That's because it is too large for Windows Debug to compile, + so split into smaller bits. + Even with this split, the file is too big; that's likely + because we are using TYPED_TEST here, which generates multiple + symbols per test case. To deal with this, without further + breaking up the file, also add in the /bigobj flag when + compiling on Windows Debug. +* avoid adding notify waitable twice to events-executor collection (`#2564 `_) + * avoid adding notify waitable twice to events-executor entities collection + * remove redundant mutex lock + --------- +* Contributors: Alberto Soragna, Alejandro Hernández Cordero, Barry Xu, Chris Lalancette + +28.3.1 (2024-06-25) +------------------- +* Remove unnecessary msg includes in tests (`#2566 `_) +* Fix copy-paste errors in function docs (`#2565 `_) +* Fix typo in function doc (`#2563 `_) +* Contributors: Christophe Bedard + +28.3.0 (2024-06-17) +------------------- +* Add test creating two content filter topics with the same topic name (`#2546 `_) (`#2549 `_) +* add impl pointer for ExecutorOptions (`#2523 `_) +* Fixup Executor::spin_all() regression fix (`#2517 `_) +* Add 'mimick' label to tests which use Mimick (`#2516 `_) +* Contributors: Alejandro Hernández Cordero, Scott K Logan, William Woodall + +28.2.0 (2024-04-26) +------------------- +* Check for negative time in rclcpp::Time(int64_t nanoseconds, ...) constructor (`#2510 `_) +* Revise the description of service configure_introspection() (`#2511 `_) +* Contributors: Barry Xu, Sharmin Ramli + +28.1.0 (2024-04-16) +------------------- +* Remove references to index.ros.org. (`#2504 `_) +* Reduce overhead for inheriting from rclcpp::Executor when base functionality is not reused (`#2506 `_) +* Contributors: Chris Lalancette, William Woodall, jmachowinski + +28.0.1 (2024-04-16) +------------------- +* [wjwwood] Updated "Data race fixes" (`#2500 `_) + * Fix callback group logic in executor + * fix: Fixed unnecessary copy of wait_set + * fix(executor): Fixed race conditions with rebuild of wait_sets + Before this change, the rebuild of wait set would be triggered + after the wait set was waken up. With bad timing, this could + lead to the rebuild not happening with multi threaded executor. + * fix(Executor): Fixed lost of entities rebuild request + * chore: Added assert for not set callback_group in execute_any_executable + * Add test for cbg getting reset + Co-authored-by: Janosch Machowinski + * chore: renamed test cases to snake_case + * style + * fixup test to avoid polling and short timeouts + * fix: Use correct notify_waitable\_ instance + * fix(StaticSingleThreadedExecutor): Added missing special case handling for current_notify_waitable\_ + * fix(TestCallbackGroup): Fixed test after change to timers + --------- + Co-authored-by: Janosch Machowinski + Co-authored-by: Michael Carroll + Co-authored-by: Janosch Machowinski +* fixup var names to snake case (`#2501 `_) +* Added optional TimerInfo to timer callback (`#2343 `_) + Co-authored-by: Alexis Tsogias + Co-authored-by: Janosch Machowinski +* Fix uninitialized memory in test (`#2498 `_) + When I added in the tests for large messages, I made a mistake and reserved space in the strings, but didn't actually expand it. Thus, we were writing into uninitialized memory. Fix this by just using the correct constructor for string, which will allocate and initialize the memory properly. +* Ensure waitables handle guard condition retriggering (`#2483 `_) + Co-authored-by: Michael Carroll +* fix: init concatenated_vector with begin() & end() (`#2492 `_) + * this commit will fix the warning [-Wstringop-overflow=] `#2461 `_ +* Use the same context for the specified node in rclcpp::spin functions (`#2433 `_) + * Use the same conext for the specified node in rclcpp::spin_xx functions + * Add test for spinning with non-default-context + * Format code + --------- +* Disable compare-function-pointers in test_utilities (`#2489 `_) +* address ambiguous auto variable. (`#2481 `_) +* Increase the cppcheck timeout to 1200 seconds (`#2484 `_) +* Removed test_timers_manager clang warning (`#2479 `_) +* Flaky timer test fix (`#2469 `_) + * fix(time_source): Fixed possible race condition + * fix(test_executors_time_cancel_behaviour): Fixed multiple race conditions + --------- + Co-authored-by: Janosch Machowinski +* Add tracepoint for generic publisher/subscriber (`#2448 `_) +* update rclcpp::Waitable API to use references and const (`#2467 `_) +* Utilize rclcpp::WaitSet as part of the executors (`#2142 `_) + * Deprecate callback_group call taking context + * Add base executor objects that can be used by implementors + * Template common operations + * Address reviewer feedback: + * Add callback to EntitiesCollector constructor + * Make function to check automatically added callback groups take a list + * Lint + * Address reviewer feedback and fix templates + * Lint and docs + * Make executor own the notify waitable + * Add pending queue to collector, remove from waitable + Also change node's get_guard_condition to return shared_ptr + * Change interrupt guard condition to shared_ptr + Check if guard condition is valid before adding it to the waitable + * Lint and docs + * Utilize rclcpp::WaitSet as part of the executors + * Don't exchange atomic twice + * Fix add_node and add more tests + * Make get_notify_guard_condition follow API tick-tock + * Improve callback group tick-tocking + * Don't lock twice + * Address reviewer feedback + * Add thread safety annotations and make locks consistent + * @wip + * Reset callback groups for multithreaded executor + * Avoid many small function calls when building executables + * Re-trigger guard condition if buffer has data + * Address reviewer feedback + * Trace points + * Remove tracepoints + * Reducing diff + * Reduce diff + * Uncrustify + * Restore tests + * Back to weak_ptr and reduce test time + * reduce diff and lint + * Restore static single threaded tests that weren't working before + * Restore more tests + * Fix multithreaded test + * Fix assert + * Fix constructor test + * Change ready_executables signature back + * Don't enforce removing callback groups before nodes + * Remove the "add_valid_node" API + * Only notify if the trigger condition is valid + * Only trigger if valid and needed + * Fix spin_some/spin_all implementation + * Restore single threaded executor + * Picking ABI-incompatible executor changes + * Add PIMPL + * Additional waitset prune + * Fix bad merge + * Expand test timeout + * Introduce method to clear expired entities from a collection + * Make sure to call remove_expired_entities(). + * Prune queued work when callback group is removed + * Prune subscriptions from dynamic storage + * Styles fixes. + * Re-trigger guard conditions + * Condense to just use watiable.take_data + * Lint + * Address reviewer comments (nits) + * Lock mutex when copying + * Refactors to static single threaded based on reviewers + * More small refactoring + * Lint + * Lint + * Add ready executable accessors to WaitResult + * Make use of accessors from wait_set + * Fix tests + * Fix more tests + * Tidy up single threaded executor implementation + * Don't null out timer, rely on call + * change how timers are checked from wait result in executors + * peak -> peek + * fix bug in next_waitable logic + * fix bug in StaticSTE that broke the add callback groups to executor tests + * style + --------- + Co-authored-by: Chris Lalancette + Co-authored-by: William Woodall +* fix flakiness in TestTimersManager unit-test (`#2468 `_) + the previous version of the test was relying on the assumption that a timer with 1ms period gets called at least 6 times if the main thread waits 15ms. this is true most of the times, but it's not guaranteed, especially when running the test on windows CI servers. the new version of the test makes no assumptions on how much time it takes for the timers manager to invoke the timers, but rather focuses on ensuring that they are called the right amount of times, which is what's important for the purpose of the test +* Contributors: Alberto Soragna, Alejandro Hernández Cordero, Chris Lalancette, Homalozoa X, Kotaro Yoshimoto, Michael Carroll, Tomoya Fujita, William Woodall, h-suzuki-isp, jmachowinski + +28.0.0 (2024-03-28) +------------------- +* fix spin_some_max_duration unit-test for events-executor (`#2465 `_) +* refactor and improve the parameterized spin_some tests for executors (`#2460 `_) + * refactor and improve the spin_some parameterized tests for executors + * disable spin_some_max_duration for the StaticSingleThreadedExecutor and EventsExecutor + * fixup and clarify the docstring for Executor::spin_some() + * style + * review comments + --------- +* enable simulation clock for timer canceling test. (`#2458 `_) + * enable simulation clock for timer canceling test. + * move MainExecutorTypes to test_executors_timer_cancel_behavior.cpp. + --------- +* Revert "relax the test simulation rate for timer canceling tests. (`#2453 `_)" (`#2456 `_) + This reverts commit 1c350d0d7fb9c7158e0a39057112486ddbd38e9a. +* relax the test simulation rate for timer canceling tests. (`#2453 `_) +* Fix TypeAdapted publishing with large messages. (`#2443 `_) + Mostly by ensuring we aren't attempting to store + large messages on the stack. Also add in tests. + I verified that before these changes, the tests failed, + while after them they succeed. +* Implement generic client (`#2358 `_) + * Implement generic client + * Fix the incorrect parameter declaration + * Deleted copy constructor and assignment for FutureAndRequestId + * Update codes after rebase + * Address review comments + * Address review comments from iuhilnehc-ynos + * Correct an error in a description + * Fix window build errors + * Address review comments from William + * Add doc strings to create_generic_client + --------- +* Rule of five: implement move operators (`#2425 `_) +* Various cleanups to deal with uncrustify 0.78. (`#2439 `_) + These should also work with uncrustify 0.72. +* Remove the set_deprecated signatures in any_subscription_callback. (`#2431 `_) + These have been deprecated since April 2021, so it is safe + to remove them now. +* fix doxygen syntax for NodeInterfaces (`#2428 `_) +* Set hints to find the python version we actually want. (`#2426 `_) + The comment in the commit explains the reasoning behind it. +* Update quality declaration documents (`#2427 `_) +* feat: add/minus for msg::Time and rclcpp::Duration (`#2419 `_) + * feat: add/minus for msg::Time and rclcpp::Duration +* Contributors: Alberto Soragna, Barry Xu, Chris Lalancette, Christophe Bedard, HuaTsai, Jonas Otto, Tim Clephas, Tomoya Fujita, William Woodall + +27.0.0 (2024-02-07) +------------------- +* Split test_executors up into smaller chunks. (`#2421 `_) +* [events executor] - Fix Behavior with Timer Cancel (`#2375 `_) +* Removed deprecated header (`#2413 `_) +* Make sure to mark RingBuffer methods as 'override'. (`#2410 `_) +* Contributors: Alejandro Hernández Cordero, Chris Lalancette, Matt Condino + +26.0.0 (2024-01-24) +------------------- +* Increase the cppcheck timeout to 600 seconds. (`#2409 `_) +* Add transient local durability support to publisher and subscriptions when using intra-process communication (`#2303 `_) +* Stop storing the context in the guard condition. (`#2400 `_) +* Contributors: Chris Lalancette, Jeffery Hsu + +25.0.0 (2023-12-26) +------------------- +* Updated GenericSubscription to AnySubscriptionCallback (`#1928 `_) +* make type support helper supported for service (`#2209 `_) +* Adding QoS to subscription options (`#2323 `_) +* Switch to target_link_libraries. (`#2374 `_) +* aligh with rcl that a rosout publisher of a node might not exist (`#2357 `_) +* Fix data race in EventHandlerBase (`#2349 `_) +* Support users holding onto shared pointers in the message memory pool (`#2336 `_) +* Contributors: Chen Lihui, Chris Lalancette, DensoADAS, Lucas Wendland, mauropasse + +24.0.0 (2023-11-06) +------------------- +* fix (signal_handler.hpp): spelling (`#2356 `_) +* Updates to not use std::move in some places. (`#2353 `_) +* rclcpp::Time::max() clock type support. (`#2352 `_) +* Serialized Messages with Topic Statistics (`#2274 `_) +* Add a custom deleter when constructing rcl_service_t (`#2351 `_) +* Disable the loaned messages inside the executor. (`#2335 `_) +* Use message_info in SubscriptionTopicStatistics instead of typed message (`#2337 `_) +* Add missing 'enable_rosout' comments (`#2345 `_) +* Adjust rclcpp usage of type description service (`#2344 `_) +* address rate related flaky tests. (`#2329 `_) +* Fixes pointed out by the clang analyzer. (`#2339 `_) +* Remove useless ROSRate class (`#2326 `_) +* Contributors: Alexey Merzlyakov, Chris Lalancette, Jiaqi Li, Lucas Wendland, Michael Carroll, Michael Orlov, Tomoya Fujita, Zard-C + +23.2.0 (2023-10-09) +------------------- +* add clients & services count (`#2072 `_) +* remove invalid sized allocation test for SerializedMessage. (`#2330 `_) +* Adding API to copy all parameters from one node to another (`#2304 `_) +* Contributors: Minju, Lee, Steve Macenski, Tomoya Fujita + +23.1.0 (2023-10-04) +------------------- +* Add locking to protect the TimeSource::NodeState::node_base\_ (`#2320 `_) +* Update SignalHandler get_global_signal_handler to avoid complex types in static memory (`#2316 `_) +* Removing Old Connext Tests (`#2313 `_) +* Documentation for list_parameters (`#2315 `_) +* Decouple rosout publisher init from node init. (`#2174 `_) +* fix the depth to relative in list_parameters (`#2300 `_) +* Contributors: Chris Lalancette, Lucas Wendland, Minju, Lee, Tomoya Fujita, Tully Foote + +23.0.0 (2023-09-08) +------------------- +* Fix the return type of Rate::period. (`#2301 `_) +* Update API docs links in package READMEs (`#2302 `_) +* Cleanup flaky timers_manager tests. (`#2299 `_) +* Contributors: Chris Lalancette, Christophe Bedard + +22.2.0 (2023-09-07) +------------------- +* Topic correct typeadapter deduction (`#2294 `_) +* Fix C++20 allocator construct deprecation (`#2292 `_) +* Make Rate to select the clock to work with (`#2123 `_) +* Correct the position of a comment. (`#2290 `_) +* Remove unnecessary lambda captures in the tests. (`#2289 `_) +* Add rcl_logging_interface as an explicit dependency. (`#2284 `_) +* Revamp list_parameters to be more efficient and easier to read. (`#2282 `_) +* Contributors: AiVerisimilitude, Alexey Merzlyakov, Chen Lihui, Chris Lalancette, Jiaqi Li + +22.1.0 (2023-08-21) +------------------- +* Do not crash Executor when send_response fails due to client failure. (`#2276 `_) +* Adding Custom Unknown Type Error (`#2272 `_) +* Add a pimpl inside rclcpp::Node for future distro backports (`#2228 `_) +* Remove an unused variable from the events executor tests. (`#2270 `_) +* Add spin_all shortcut (`#2246 `_) +* Adding Missing Group Exceptions (`#2256 `_) +* Change associated clocks storage to unordered_set (`#2257 `_) +* associated clocks should be protected by mutex. (`#2255 `_) +* Instrument loaned message publication code path (`#2240 `_) +* Contributors: Chris Lalancette, Christophe Bedard, Emerson Knapp, Luca Della Vedova, Lucas Wendland, Tomoya Fujita, Tony Najjar + +22.0.0 (2023-07-11) +------------------- +* Implement get_node_type_descriptions_interface for lifecyclenode and add smoke test for it (`#2237 `_) +* Add new node interface TypeDescriptionsInterface to provide GetTypeDescription service (`#2224 `_) +* Move always_false_v to detail namespace (`#2232 `_) +* Revamp the test_subscription.cpp tests. (`#2227 `_) +* warning: comparison of integer expressions of different signedness (`#2219 `_) +* Modifies timers API to select autostart state (`#2005 `_) +* Enable callback group tests for connextdds (`#2182 `_) +* Contributors: Chris Lalancette, Christopher Wecht, Eloy Briceno, Emerson Knapp, Nathan Wiebe Neufeldt, Tomoya Fujita + +21.3.0 (2023-06-12) +------------------- +* Fix up misspellings of "receive". (`#2208 `_) +* Remove flaky stressAddRemoveNode test (`#2206 `_) +* Use TRACETOOLS\_ prefix for tracepoint-related macros (`#2162 `_) +* Contributors: Chris Lalancette, Christophe Bedard, Michael Carroll + +21.2.0 (2023-06-07) +------------------- +* remove nolint since ament_cpplint updated for the c++17 header (`#2198 `_) +* Feature/available capacity of ipm (`#2173 `_) +* add mutex to protect events_executor current entity collection (`#2187 `_) +* Declare rclcpp callbacks before the rcl entities (`#2024 `_) +* Contributors: Alberto Soragna, Chen Lihui, DensoADAS, mauropasse + +21.1.1 (2023-05-11) +------------------- +* Fix race condition in events-executor (`#2177 `_) +* Add missing stdexcept include (`#2186 `_) +* Fix a format-security warning when building with clang (`#2171 `_) +* Fix delivered message kind (`#2175 `_) +* Contributors: Alberto Soragna, Chris Lalancette, methylDragon, Øystein Sture + +21.1.0 (2023-04-27) +------------------- + +21.0.0 (2023-04-18) +------------------- +* Add support for logging service. (`#2122 `_) +* Picking ABI-incompatible executor changes (`#2170 `_) +* add events-executor and timers-manager in rclcpp (`#2155 `_) +* Create common structures for executors to use (`#2143 `_) +* Implement deliver message kind (`#2168 `_) +* Contributors: Alberto Soragna, Lei Liu, Michael Carroll, methylDragon + +20.0.0 (2023-04-13) +------------------- +* applied tracepoints for ring_buffer (`#2091 `_) +* Dynamic Subscription (REP-2011 Subset): Stubs for rclcpp (`#2165 `_) +* Add type_hash to cpp TopicEndpointInfo (`#2137 `_) +* Trigger the intraprocess guard condition with data (`#2164 `_) +* Minor grammar fix (`#2149 `_) +* Fix unnecessary allocations in executor.cpp (`#2135 `_) +* add Logger::get_effective_level(). (`#2141 `_) +* Remove deprecated header (`#2139 `_) +* Implement matched event (`#2105 `_) +* use allocator via init_options argument. (`#2129 `_) +* Fixes to silence some clang warnings. (`#2127 `_) +* Documentation improvements on the executor (`#2125 `_) +* Avoid losing waitable handles while using MultiThreadedExecutor (`#2109 `_) +* Hook up the incompatible type event inside of rclcpp (`#2069 `_) +* Update all rclcpp packages to C++17. (`#2121 `_) +* Fix clang warning: bugprone-use-after-move (`#2116 `_) +* Contributors: Barry Xu, Chris Lalancette, Christopher Wecht, Emerson Knapp, Michael Carroll, Tomoya Fujita, Yadu, mauropasse, methylDragon, ymski + +19.3.0 (2023-03-01) +------------------- +* Fix memory leak in tracetools::get_symbol() (`#2104 `_) +* Service introspection (`#1985 `_) +* Allow publishing borrowed messages with intra-process enabled (`#2108 `_) +* to fix flaky test about TestTimeSource.callbacks (`#2111 `_) +* Contributors: Brian, Chen Lihui, Christophe Bedard, Miguel Company + +19.2.0 (2023-02-24) +------------------- +* to create a sublogger while getting child of Logger (`#1717 `_) +* Fix documentation of Context class (`#2107 `_) +* fixes for rmw callbacks in qos_event class (`#2102 `_) +* Contributors: Alberto Soragna, Chen Lihui, Silvio Traversaro + +19.1.0 (2023-02-14) +------------------- +* Add support for timers on reset callback (`#1979 `_) +* Topic node guard condition in executor (`#2074 `_) +* Fix bug on the disorder of calling shutdown callback (`#2097 `_) +* Contributors: Barry Xu, Chen Lihui, mauropasse + +19.0.0 (2023-01-30) +------------------- +* Add default constructor to NodeInterfaces (`#2094 `_) +* Fix clock state cached time to be a copy, not a reference. (`#2092 `_) +* Fix -Wmaybe-uninitialized warning (`#2081 `_) +* Fix the keep_last warning when using system defaults. (`#2082 `_) +* Add in a fix for older compilers. (`#2075 `_) +* Contributors: Alexander Hans, Chris Lalancette, Shane Loretz + +18.0.0 (2022-12-29) +------------------- +* Implement Unified Node Interface (NodeInterfaces class) (`#2041 `_) +* Do not throw exception if trying to dequeue an empty intra-process buffer (`#2061 `_) +* Move event callback binding to PublisherBase and SubscriptionBase (`#2066 `_) +* Implement validity checks for rclcpp::Clock (`#2040 `_) +* Explicitly set callback type (`#2059 `_) +* Fix logging macros to build with msvc and cpp20 (`#2063 `_) +* Add clock type to node_options (`#1982 `_) +* Fix nullptr dereference in prune_requests_older_than (`#2008 `_) +* Remove templating on to_rcl_subscription_options (`#2056 `_) +* Fix SharedFuture from async_send_request never becoming valid (`#2044 `_) +* Add in a warning for a KeepLast depth of 0. (`#2048 `_) +* Mark rclcpp::Clock::now() as const (`#2050 `_) +* Fix a case that did not throw ParameterUninitializedException (`#2036 `_) +* Update maintainers (`#2043 `_) +* Contributors: Alberto Soragna, Audrow Nash, Chen Lihui, Chris Lalancette, Jeffery Hsu, Lei Liu, Mateusz Szczygielski, Shane Loretz, andrei, mauropasse, methylDragon + +17.1.0 (2022-11-02) +------------------- +* MultiThreadExecutor number of threads is at least 2+ in default. (`#2032 `_) +* Fix bug that a callback not reached (`#1640 `_) +* Set the minimum number of threads of the Multithreaded executor to 2 (`#2030 `_) +* check thread whether joinable before join (`#2019 `_) +* Set cpplint test timeout to 3 minutes (`#2022 `_) +* Make sure to include-what-you-use in the node_interfaces. (`#2018 `_) +* Do not clear entities callbacks on destruction (`#2002 `_) +* fix mismatched issue if using zero_allocate (`#1995 `_) +* Contributors: Alexis Paques, Chen Lihui, Chris Lalancette, Cristóbal Arroyo, Tomoya Fujita, mauropasse, uupks + +17.0.0 (2022-09-13) +------------------- +* Make ParameterService and Sync/AsyncParameterClient accept rclcpp::QoS (`#1978 `_) +* support regex match for parameter client (`#1992 `_) +* operator+= and operator-= for Duration (`#1988 `_) +* Revert "Revert "Add a create_timer method to Node and `LifecycleNode` classes (`#1975 `_)" (`#2009 `_) (`#2010 `_) +* force compiler warning if callback handles not captured (`#2000 `_) +* Revert "Add a `create_timer` method to `Node` and `LifecycleNode` classes (`#1975 `_)" (`#2009 `_) +* Add a `create_timer` method to `Node` and `LifecycleNode` classes (`#1975 `_) +* [docs] add note about callback lifetime for {on, post}_set_parameter_callback (`#1981 `_) +* fix memory leak (`#1994 `_) +* Support pre-set and post-set parameter callbacks in addition to on-set-parameter-callback. (`#1947 `_) +* Make create_service accept rclcpp::QoS (`#1969 `_) +* Make create_client accept rclcpp::QoS (`#1964 `_) +* Fix the documentation for rclcpp::ok to be accurate. (`#1965 `_) +* use regex for wildcard matching (`#1839 `_) +* Revert "Introduce executors new spin_for method, replace spin_until_future_complete with spin_until_complete. (`#1821 `_) (`#1874 `_)" (`#1956 `_) +* Introduce executors new spin_for method, replace spin_until_future_complete with spin_until_complete. (`#1821 `_) (`#1874 `_) +* test adjustment for LoanedMessage. (`#1951 `_) +* fix virtual dispatch issues identified by clang-tidy (`#1816 `_) +* Remove unused on_parameters_set_callback\_ (`#1945 `_) +* Fix subscription.is_serialized() for callbacks with message info (`#1950 `_) +* wait for subscriptions on another thread. (`#1940 `_) +* Fix documentation of `RCLCPP\_[INFO,WARN,...]` (`#1943 `_) +* Always trigger guard condition waitset (`#1923 `_) +* Add statistics for handle_loaned_message (`#1927 `_) +* Drop wrong template specialization (`#1926 `_) +* Contributors: Alberto Soragna, Andrew Symington, Barry Xu, Brian, Chen Lihui, Chris Lalancette, Daniel Reuter, Deepanshu Bansal, Hubert Liberacki, Ivan Santiago Paunovic, Jochen Sprickerhof, Nikolai Morin, Shane Loretz, Tomoya Fujita, Tyler Weaver, William Woodall, schrodinbug + +16.2.0 (2022-05-03) +------------------- +* Update get_parameter_from_event to follow the function description (`#1922 `_) +* Add 'best available' QoS enum values and methods (`#1920 `_) +* Contributors: Barry Xu, Jacob Perron + +16.1.0 (2022-04-29) +------------------- +* use reinterpret_cast for function pointer conversion. (`#1919 `_) +* Contributors: Tomoya Fujita + +16.0.1 (2022-04-13) +------------------- +* remove DEFINE_CONTENT_FILTER cmake option (`#1914 `_) +* Contributors: Chen Lihui + +16.0.0 (2022-04-08) +------------------- +* remove things that were deprecated during galactic (`#1913 `_) +* Contributors: William Woodall + +15.4.0 (2022-04-05) +------------------- +* add take_data_by_entity_id API to waitable (`#1892 `_) +* add content-filtered-topic interfaces (`#1561 `_) +* Contributors: Alberto Soragna, Chen Lihui + +15.3.0 (2022-03-30) +------------------- +* [NodeParameters] Set name in param info pre-check (`#1908 `_) +* Add test-dep ament_cmake_google_benchmark (`#1904 `_) +* Add publish by loaned message in GenericPublisher (`#1856 `_) +* Contributors: Abrar Rahman Protyasha, Barry Xu, Gaël Écorchard + +15.2.0 (2022-03-24) +------------------- +* Add missing ament dependency on rcl_interfaces (`#1903 `_) +* Update data callback tests to account for all published samples (`#1900 `_) +* Increase timeout for acknowledgments to account for slower Connext settings (`#1901 `_) +* clang-tidy: explicit constructors (`#1782 `_) +* Add client/service QoS getters (`#1784 `_) +* Fix a bunch more rosdoc2 issues in rclcpp. (`#1897 `_) +* time_until_trigger returns max time if timer is cancelled (`#1893 `_) +* Micro-optimizations in rclcpp (`#1896 `_) +* Contributors: Andrea Sorbini, Chris Lalancette, Mauro Passerino, Scott K Logan, William Woodall + +15.1.0 (2022-03-01) +------------------- +* spin_all with a zero timeout. (`#1878 `_) +* Add RMW listener APIs (`#1579 `_) +* Remove fastrtps customization on tests (`#1887 `_) +* Install headers to include/${PROJECT_NAME} (`#1888 `_) +* Use ament_generate_version_header (`#1886 `_) +* use universal reference to support rvalue. (`#1883 `_) +* fix one subscription can wait_for_message twice (`#1870 `_) +* Add return value version of get_parameter_or (`#1813 `_) +* Cleanup time source object lifetimes (`#1867 `_) +* add is_spinning() method to executor base class +* Contributors: Alberto Soragna, Chen Lihui, Chris Lalancette, Kenji Miyake, Miguel Company, Shane Loretz, Tomoya Fujita, iRobot ROS + +15.0.0 (2022-01-14) +------------------- +* Cleanup the TypeAdapt tests (`#1858 `_) +* Cleanup includes (`#1857 `_) +* Fix include order and relative paths for cpplint (`#1859 `_) +* Rename stringstream in macros to a more unique name (`#1862 `_) +* Add non transform capabilities for intra-process (`#1849 `_) +* Fix rclcpp documentation build (`#1779 `_) +* Contributors: Chris Lalancette, Doug Smith, Gonzo, Jacob Perron, Michel Hidalgo + +14.1.0 (2022-01-05) +------------------- +* Use UninitializedStaticallyTypedParameterException (`#1689 `_) +* Add wait_for_all_acked support (`#1662 `_) +* Add tests for function templates of declare_parameter (`#1747 `_) +* Contributors: Barry Xu, Bi0T1N, M. Mostafa Farzan + +14.0.0 (2021-12-17) +------------------- +* Fixes for uncrustify 0.72 (`#1844 `_) +* use private member to keep the all reference underneath. (`#1845 `_) +* Make node base sharable (`#1832 `_) +* Add Clock::sleep_for() (`#1828 `_) +* Synchronize rcl and std::chrono steady clocks in Clock::sleep_until (`#1830 `_) +* Use rclcpp::guard_condition (`#1612 `_) +* Call CMake function to generate version header (`#1805 `_) +* Use parantheses around logging macro parameter (`#1820 `_) +* Remove author by request (`#1818 `_) +* Update maintainers (`#1817 `_) +* min_forward & min_backward thresholds must not be disabled (`#1815 `_) +* Re-add Clock::sleep_until (`#1814 `_) +* Fix lifetime of context so it remains alive while its dependent node handles are still in use (`#1754 `_) +* Add the interface for pre-shutdown callback (`#1714 `_) +* Take message ownership from moved LoanedMessage (`#1808 `_) +* Suppress clang dead-store warnings in the benchmarks. (`#1802 `_) +* Wait for publisher and subscription to match (`#1777 `_) +* Fix unused QoS profile for clock subscription and make ClockQoS the default (`#1801 `_) +* Contributors: Abrar Rahman Protyasha, Barry Xu, Chen Lihui, Chris Lalancette, Grey, Jacob Perron, Nikolai Morin, Shane Loretz, Tomoya Fujita, mauropasse + +13.1.0 (2021-10-18) +------------------- +* Fix dangerous std::bind capture in TimeSource implementation. (`#1768 `_) +* Fix dangerous std::bind capture in ParameterEventHandler implementation. (`#1770 `_) +* Handle sigterm, in the same way sigint is being handled. (`#1771 `_) +* rclcpp::Node copy constructor: make copy of node_waitables\_ member. (`#1799 `_) +* Extend NodeGraph to match what rcl provides. (`#1484 `_) +* Context::sleep_for(): replace recursion with do-while to avoid potential stack-overflow. (`#1765 `_) +* extend_sub_namespace(): Verify string::empty() before calling string::front(). (`#1764 `_) +* Deprecate the `void shared_ptr` subscription callback signatures. (`#1713 `_) +* Contributors: Abrar Rahman Protyasha, Chris Lalancette, Emerson Knapp, Geoffrey Biggs, Ivan Santiago Paunovic, Jorge Perez, Tomoya Fujita, William Woodall, Yong-Hao Zou, livanov93 + +13.0.0 (2021-08-23) +------------------- +* Remove can_be_nullptr assignment check for QNX case. (`#1752 `_) +* Update client API to be able to remove pending requests. (`#1734 `_) +* Fix: Allow to add a node while spinning in the StaticSingleThreadedExecutor. (`#1690 `_) +* Add tracing instrumentation for executor and message taking. (`#1738 `_) +* Fix: Reset timer trigger time before execute in StaticSingleThreadedExecutor. (`#1739 `_) +* Use FindPython3 and make python3 dependency explicit. (`#1745 `_) +* Use rosidl_get_typesupport_target(). (`#1729 `_) +* Fix returning invalid namespace if sub_namespace is empty. (`#1658 `_) +* Add free function to wait for a subscription message. (`#1705 `_) +* Use rcpputils/scope_exit.hpp and remove rclcpp/scope_exit.hpp. (`#1727 `_) +* Contributors: Ahmed Sobhy, Christophe Bedard, Ivan Santiago Paunovic, Karsten Knese, M. Hofstätter, Mauro Passerino, Shane Loretz, mauropasse + +12.0.0 (2021-07-26) +------------------- +* Remove unsafe get_callback_groups API. + Callers should change to using for_each_callback_group(), or + store the callback groups they need internally. +* Add in callback_groups_for_each. + The main reason to add this method in is to make accesses to the + callback_groups\_ vector thread-safe. By having a + callback_groups_for_each that accepts a std::function, we can + just have the callers give us the callback they are interested + in, and we can take care of the locking. + The rest of this fairly large PR is cleaning up all of the places + that use get_callback_groups() to instead use + callback_groups_for_each(). +* Use a different mechanism to avoid timers being scheduled multiple times by the MultiThreadedExecutor (`#1692 `_) +* Fix windows CI (`#1726 `_) + Fix bug in AnyServiceCallback introduced in `#1709 `_. +* Contributors: Chris Lalancette, Ivan Santiago Paunovic + +11.2.0 (2021-07-21) +------------------- +* Support to defer to send a response in services. (`#1709 `_) + Signed-off-by: Ivan Santiago Paunovic +* Fix documentation bug. (`#1719 `_) + Signed-off-by: William Woodall +* Contributors: Ivan Santiago Paunovic, William Woodall + +11.1.0 (2021-07-13) +------------------- +* Removed left over ``is_initialized()`` implementation (`#1711 `_) + Leftover from https://github.com/ros2/rclcpp/pull/1622 +* Fixed declare parameter methods for int and float vectors (`#1696 `_) +* Cleaned up implementation of the intra-process manager (`#1695 `_) +* Added the node name to an executor ``runtime_error`` (`#1686 `_) +* Fixed a typo "Attack" -> "Attach" (`#1687 `_) +* Removed use of std::allocator<>::rebind (`#1678 `_) + rebind is deprecated in c++17 and removed in c++20 +* Contributors: Alberto Soragna, Chen Lihui, Chris Lalancette, Petter Nilsson, Steve Macenski, William Woodall + +11.0.0 (2021-05-18) +------------------- +* Allow declare uninitialized parameters (`#1673 `_) +* Fix syntax issue with gcc (`#1674 `_) +* [service] Don't use a weak_ptr to avoid leaking (`#1668 `_) +* Contributors: Ivan Santiago Paunovic, Jacob Perron, William Woodall + +10.0.0 (2021-05-11) +------------------- +* Fix doc typo (`#1663 `_) +* [rclcpp] Type Adaptation feature (`#1557 `_) +* Do not attempt to use void allocators for memory allocation. (`#1657 `_) +* Keep custom allocator in publisher and subscription options alive. (`#1647 `_) +* Fix get_publishers_subscriptions_info_by_topic test in test_node.cpp (`#1648 `_) +* Use OnShutdown callback handle instead of OnShutdown callback (`#1639 `_) +* use dynamic_pointer_cast to detect allocator mismatch in intra process manager (`#1643 `_) +* Increase cppcheck timeout to 500s (`#1634 `_) +* Clarify node parameters docs (`#1631 `_) +* Contributors: Audrow Nash, Barry Xu, Jacob Perron, Michel Hidalgo, Shane Loretz, William Woodall + +9.0.2 (2021-04-14) +------------------ +* Avoid returning loan when none was obtained. (`#1629 `_) +* Use a different implementation of mutex two priorities (`#1628 `_) +* Do not test the value of the history policy when testing the get_publishers/subscriptions_info_by_topic() methods (`#1626 `_) +* Check first parameter type and range before calling the user validation callbacks (`#1627 `_) +* Contributors: Ivan Santiago Paunovic, Miguel Company + +9.0.1 (2021-04-12) +------------------ +* Restore test exception for Connext (`#1625 `_) +* Fix race condition in TimeSource clock thread setup (`#1623 `_) +* Contributors: Andrea Sorbini, Michel Hidalgo + +9.0.0 (2021-04-06) +------------------ +* remove deprecated code which was deprecated in foxy and should be removed in galactic (`#1622 `_) +* Change index.ros.org -> docs.ros.org. (`#1620 `_) +* Unique network flows (`#1496 `_) +* Add spin_some support to the StaticSingleThreadedExecutor (`#1338 `_) +* Add publishing instrumentation (`#1600 `_) +* Create load_parameters and delete_parameters methods (`#1596 `_) +* refactor AnySubscriptionCallback and add/deprecate callback signatures (`#1598 `_) +* Add generic publisher and generic subscription for serialized messages (`#1452 `_) +* use context from `node_base\_` for clock executor. (`#1617 `_) +* updating quality declaration links (re: `ros2/docs.ros2.org#52 `_) (`#1615 `_) +* Contributors: Ananya Muddukrishna, BriceRenaudeau, Chris Lalancette, Christophe Bedard, Nikolai Morin, Tomoya Fujita, William Woodall, mauropasse, shonigmann + +8.2.0 (2021-03-31) +------------------ +* Initialize integers in test_parameter_event_handler.cpp to avoid undefined behavior (`#1609 `_) +* Namespace tracetools C++ functions (`#1608 `_) +* Revert "Namespace tracetools C++ functions (`#1603 `_)" (`#1607 `_) +* Namespace tracetools C++ functions (`#1603 `_) +* Clock subscription callback group spins in its own thread (`#1556 `_) +* Contributors: Chris Lalancette, Christophe Bedard, Ivan Santiago Paunovic, anaelle-sw + +8.1.0 (2021-03-25) +------------------ +* Remove rmw_connext_cpp references. (`#1595 `_) +* Add API for checking QoS profile compatibility (`#1554 `_) +* Document misuse of parameters callback (`#1590 `_) +* use const auto & to iterate over parameters (`#1593 `_) +* Contributors: Chris Lalancette, Jacob Perron, Karsten Knese + +8.0.0 (2021-03-23) +------------------ +* Guard against integer overflow in duration conversion (`#1584 `_) +* Contributors: Jacob Perron + +7.0.1 (2021-03-22) +------------------ +* get_parameters service should return empty if undeclared parameters are allowed (`#1514 `_) +* Made 'Context::shutdown_reason' function a const function (`#1578 `_) +* Contributors: Tomoya Fujita, suab321321 + +7.0.0 (2021-03-18) +------------------ +* Document design decisions that were made for statically typed parameters (`#1568 `_) +* Fix doc typo in CallbackGroup constructor (`#1582 `_) +* Enable qos parameter overrides for the /parameter_events topic (`#1532 `_) +* Add support for rmw_connextdds (`#1574 `_) +* Remove 'struct' from the rcl_time_jump_t. (`#1577 `_) +* Add tests for declaring statically typed parameters when undeclared parameters are allowed (`#1575 `_) +* Quiet clang memory leak warning on "DoNotOptimize". (`#1571 `_) +* Add ParameterEventsSubscriber class (`#829 `_) +* When a parameter change is rejected, the parameters map shouldn't be updated. (`#1567 `_) +* Fix when to throw the NoParameterOverrideProvided exception. (`#1567 `_) +* Fix SEGV caused by order of destruction of Node sub-interfaces (`#1469 `_) +* Fix benchmark test failure introduced in `#1522 `_ (`#1564 `_) +* Fix documented example in create_publisher (`#1558 `_) +* Enforce static parameter types (`#1522 `_) +* Allow timers to keep up the intended rate in MultiThreadedExecutor (`#1516 `_) +* Fix UBSAN warnings in any_subscription_callback. (`#1551 `_) +* Fix runtime error: reference binding to null pointer of type (`#1547 `_) +* Contributors: Andrea Sorbini, Chris Lalancette, Colin MacKenzie, Ivan Santiago Paunovic, Jacob Perron, Steven! Ragnarök, bpwilcox, tomoya + +6.3.1 (2021-02-08) +------------------ +* Reference test resources directly from source tree (`#1543 `_) +* clear statistics after window reset (`#1531 `_) (`#1535 `_) +* Fix a minor string error in the topic_statistics test. (`#1541 `_) +* Avoid `Resource deadlock avoided` if use intra_process_comms (`#1530 `_) +* Avoid an object copy in parameter_value.cpp. (`#1538 `_) +* Assert that the publisher_list size is 1. (`#1537 `_) +* Don't access objects after they have been std::move (`#1536 `_) +* Update for checking correct variable (`#1534 `_) +* Destroy msg extracted from LoanedMessage. (`#1305 `_) +* Contributors: Chen Lihui, Chris Lalancette, Ivan Santiago Paunovic, Miaofei Mei, Scott K Logan, William Woodall, hsgwa + +6.3.0 (2021-01-25) +------------------ +* Add instrumentation for linking a timer to a node (`#1500 `_) +* Fix error when using IPC with StaticSingleThreadExecutor (`#1520 `_) +* Change to using unique_ptrs for DummyExecutor. (`#1517 `_) +* Allow reconfiguring 'clock' topic qos (`#1512 `_) +* Allow to add/remove nodes thread safely in rclcpp::Executor (`#1505 `_) +* Call rclcpp::shutdown in test_node for clean shutdown on Windows (`#1515 `_) +* Reapply "Add get_logging_directory method to rclcpp::Logger (`#1509 `_)" (`#1513 `_) +* use describe_parameters of parameter client for test (`#1499 `_) +* Revert "Add get_logging_directory method to rclcpp::Logger (`#1509 `_)" (`#1511 `_) +* Add get_logging_directory method to rclcpp::Logger (`#1509 `_) +* Contributors: Chris Lalancette, Christophe Bedard, Ivan Santiago Paunovic, eboasson, mauropasse, tomoya + +6.2.0 (2021-01-08) +------------------ +* Better documentation for the QoS class (`#1508 `_) +* Modify excluding callback duration from topic statistics (`#1492 `_) +* Make the test of graph users more robust. (`#1504 `_) +* Make sure to wait for graph change events in test_node_graph. (`#1503 `_) +* add timeout to SyncParametersClient methods (`#1493 `_) +* Fix wrong test expectations (`#1497 `_) +* Update create_publisher/subscription documentation, clarifying when a parameters interface is required (`#1494 `_) +* Fix string literal warnings (`#1442 `_) +* support describe_parameters methods to parameter client. (`#1453 `_) +* Contributors: Audrow Nash, Chris Lalancette, Ivan Santiago Paunovic, Nikolai Morin, hsgwa, tomoya + +6.1.0 (2020-12-10) +------------------ +* Add getters to rclcpp::qos and rclcpp::Policy enum classes (`#1467 `_) +* Change nullptr checks to use ASSERT_TRUE. (`#1486 `_) +* Adjust logic around finding and erasing guard_condition (`#1474 `_) +* Update QDs to QL 1 (`#1477 `_) +* Add performance tests for parameter transport (`#1463 `_) +* Contributors: Chris Lalancette, Ivan Santiago Paunovic, Scott K Logan, Stephen Brawner + +6.0.0 (2020-11-18) +------------------ +* Move ownership of shutdown_guard_condition to executors/graph_listener (`#1404 `_) +* Add options to automatically declare qos parameters when creating a publisher/subscription (`#1465 `_) +* Add `take_data` to `Waitable` and `data` to `AnyExecutable` (`#1241 `_) +* Add benchmarks for node parameters interface (`#1444 `_) +* Remove allocation from executor::remove_node() (`#1448 `_) +* Fix test crashes on CentOS 7 (`#1449 `_) +* Bump rclcpp packages to Quality Level 2 (`#1445 `_) +* Added executor benchmark tests (`#1413 `_) +* Add fully-qualified namespace to WeakCallbackGroupsToNodesMap (`#1435 `_) +* Contributors: Alejandro Hernández Cordero, Audrow Nash, Chris Lalancette, Ivan Santiago Paunovic, Louise Poubel, Scott K Logan, brawner + +5.1.0 (2020-11-02) +------------------ +* Deprecate Duration(rcl_duration_value_t) in favor of static Duration::from_nanoseconds(rcl_duration_value_t) (`#1432 `_) +* Avoid parsing arguments twice in `rclcpp::init_and_remove_ros_arguments` (`#1415 `_) +* Add service and client benchmarks (`#1425 `_) +* Set CMakeLists to only use default rmw for benchmarks (`#1427 `_) +* Update tracetools' QL in rclcpp's QD (`#1428 `_) +* Add missing locking to the rclcpp_action::ServerBase. (`#1421 `_) +* Initial benchmark tests for rclcpp::init/shutdown create/destroy node (`#1411 `_) +* Refactor test CMakeLists in prep for benchmarks (`#1422 `_) +* Add methods in topic and service interface to resolve a name (`#1410 `_) +* Update deprecated gtest macros (`#1370 `_) +* Clear members for StaticExecutorEntitiesCollector to avoid shared_ptr dependency (`#1303 `_) +* Increase test timeouts of slow running tests with rmw_connext_cpp (`#1400 `_) +* Avoid self dependency that not destoryed (`#1301 `_) +* Update maintainers (`#1384 `_) +* Add clock qos to node options (`#1375 `_) +* Fix NodeOptions copy constructor (`#1376 `_) +* Make sure to clean the external client/service handle. (`#1296 `_) +* Increase coverage of WaitSetTemplate (`#1368 `_) +* Increase coverage of guard_condition.cpp to 100% (`#1369 `_) +* Add coverage statement (`#1367 `_) +* Tests for LoanedMessage with mocked loaned message publisher (`#1366 `_) +* Add unit tests for qos and qos_event files (`#1352 `_) +* Finish coverage of publisher API (`#1365 `_) +* Finish API coverage on executors. (`#1364 `_) +* Add test for ParameterService (`#1355 `_) +* Add time API coverage tests (`#1347 `_) +* Add timer coverage tests (`#1363 `_) +* Add in additional tests for parameter_client.cpp coverage. +* Minor fixes to the parameter_service.cpp file. +* reset rcl_context shared_ptr after calling rcl_init sucessfully (`#1357 `_) +* Improved test publisher - zero qos history depth value exception (`#1360 `_) +* Covered resolve_use_intra_process (`#1359 `_) +* Improve test_subscription_options (`#1358 `_) +* Add in more tests for init_options coverage. (`#1353 `_) +* Test the remaining node public API (`#1342 `_) +* Complete coverage of Parameter and ParameterValue API (`#1344 `_) +* Add in more tests for the utilities. (`#1349 `_) +* Add in two more tests for expand_topic_or_service_name. (`#1350 `_) +* Add tests for node_options API (`#1343 `_) +* Add in more coverage for expand_topic_or_service_name. (`#1346 `_) +* Test exception in spin_until_future_complete. (`#1345 `_) +* Add coverage tests graph_listener (`#1330 `_) +* Add in unit tests for the Executor class. +* Allow mimick patching of methods with up to 9 arguments. +* Improve the error messages in the Executor class. +* Add coverage for client API (`#1329 `_) +* Increase service coverage (`#1332 `_) +* Make more of the static entity collector API private. +* Const-ify more of the static executor. +* Add more tests for the static single threaded executor. +* Many more tests for the static_executor_entities_collector. +* Get one more line of code coverage in memory_strategy.cpp +* Bugfix when adding callback group. +* Fix typos in comments. +* Remove deprecated executor::FutureReturnCode APIs. (`#1327 `_) +* Increase coverage of publisher/subscription API (`#1325 `_) +* Not finalize guard condition while destructing SubscriptionIntraProcess (`#1307 `_) +* Expose qos setting for /rosout (`#1247 `_) +* Add coverage for missing API (except executors) (`#1326 `_) +* Include topic name in QoS mismatch warning messages (`#1286 `_) +* Add coverage tests context functions (`#1321 `_) +* Increase coverage of node_interfaces, including with mocking rcl errors (`#1322 `_) +* Contributors: Ada-King, Alejandro Hernández Cordero, Audrow Nash, Barry Xu, Chen Lihui, Chris Lalancette, Christophe Bedard, Ivan Santiago Paunovic, Jorge Perez, Morgan Quigley, brawner + +5.0.0 (2020-09-18) +------------------ +* Make node_graph::count_graph_users() const (`#1320 `_) +* Add coverage for wait_set_policies (`#1316 `_) +* Only exchange intra_process waitable if nonnull (`#1317 `_) +* Check waitable for nullptr during constructor (`#1315 `_) +* Call vector.erase with end iterator overload (`#1314 `_) +* Use best effort, keep last, history depth 1 QoS Profile for '/clock' subscriptions (`#1312 `_) +* Add tests type_support module (`#1308 `_) +* Replace std_msgs with test_msgs in executors test (`#1310 `_) +* Add set_level for rclcpp::Logger (`#1284 `_) +* Remove unused private function (rclcpp::Node and rclcpp_lifecycle::Node) (`#1294 `_) +* Adding tests basic getters (`#1291 `_) +* Adding callback groups in executor (`#1218 `_) +* Refactor Subscription Topic Statistics Tests (`#1281 `_) +* Add operator!= for duration (`#1236 `_) +* Fix clock thread issue (`#1266 `_) (`#1267 `_) +* Fix topic stats test, wait for more messages, only check the ones with samples (`#1274 `_) +* Add get_domain_id method to rclcpp::Context (`#1271 `_) +* Fixes for unit tests that fail under cyclonedds (`#1270 `_) +* initialize_logging\_ should be copied (`#1272 `_) +* Use static_cast instead of C-style cast for instrumentation (`#1263 `_) +* Make parameter clients use template constructors (`#1249 `_) +* Ability to configure domain_id via InitOptions. (`#1165 `_) +* Simplify and fix allocator memory strategy unit test for connext (`#1252 `_) +* Use global namespace for parameter events subscription topic (`#1257 `_) +* Increase timeouts for connext for long tests (`#1253 `_) +* Adjust test_static_executor_entities_collector for rmw_connext_cpp (`#1251 `_) +* Fix failing test with Connext since it doesn't wait for discovery (`#1246 `_) +* Fix node graph test with Connext and CycloneDDS returning actual data (`#1245 `_) +* Warn about unused result of add_on_set_parameters_callback (`#1238 `_) +* Unittests for memory strategy files, except allocator_memory_strategy (`#1189 `_) +* EXPECT_THROW_EQ and ASSERT_THROW_EQ macros for unittests (`#1232 `_) +* Add unit test for static_executor_entities_collector (`#1221 `_) +* Parameterize test executors for all executor types (`#1222 `_) +* Unit tests for allocator_memory_strategy.cpp part 2 (`#1198 `_) +* Unit tests for allocator_memory_strategy.hpp (`#1197 `_) +* Derive and throw exception in spin_some spin_all for StaticSingleThreadedExecutor (`#1220 `_) +* Make ring buffer thread-safe (`#1213 `_) +* Add missing RCLCPP_PUBLIC to ~StaticExecutorEntitiesCollector (`#1227 `_) +* Document graph functions don't apply remap rules (`#1225 `_) +* Remove recreation of entities_collector (`#1217 `_) +* Contributors: Audrow Nash, Chen Lihui, Christophe Bedard, Daisuke Sato, Devin Bonnie, Dirk Thomas, Ivan Santiago Paunovic, Jacob Perron, Jannik Abbenseth, Jorge Perez, Pedro Pena, Shane Loretz, Stephen Brawner, Tomoya Fujita + +4.0.0 (2020-07-09) +------------------ +* Fix rclcpp::NodeOptions::operator= (`#1211 `_) +* Link against thread library where necessary (`#1210 `_) +* Unit tests for node interfaces (`#1202 `_) +* Remove usage of domain id in node options (`#1205 `_) +* Remove deprecated set_on_parameters_set_callback function (`#1199 `_) +* Fix conversion of negative durations to messages (`#1188 `_) +* Fix implementation of NodeOptions::use_global_arguments() (`#1176 `_) +* Bump to QD to level 3 and fixed links (`#1158 `_) +* Fix pub/sub count API tests (`#1203 `_) +* Update tracetools' QL to 2 in rclcpp's QD (`#1187 `_) +* Fix exception message on rcl_clock_init (`#1182 `_) +* Throw exception if rcl_timer_init fails (`#1179 `_) +* Unit tests for some header-only functions/classes (`#1181 `_) +* Callback should be perfectly-forwarded (`#1183 `_) +* Add unit tests for logging functionality (`#1184 `_) +* Add create_publisher include to create_subscription (`#1180 `_) +* Contributors: Alejandro Hernández Cordero, Christophe Bedard, Claire Wang, Dirk Thomas, Ivan Santiago Paunovic, Johannes Meyer, Michel Hidalgo, Stephen Brawner, tomoya + +3.0.0 (2020-06-18) +------------------ +* Check period duration in create_wall_timer (`#1178 `_) +* Fix get_node_time_source_interface() docstring (`#988 `_) +* Add message lost subscription event (`#1164 `_) +* Add spin_all method to Executor (`#1156 `_) +* Reorganize test directory and split CMakeLists.txt (`#1173 `_) +* Check if context is valid when looping in spin_some (`#1167 `_) +* Add check for invalid topic statistics publish period (`#1151 `_) +* Fix spin_until_future_complete: check spinning value (`#1023 `_) +* Fix doxygen warnings (`#1163 `_) +* Fix reference to rclcpp in its Quality declaration (`#1161 `_) +* Allow spin_until_future_complete to accept any future like object (`#1113 `_) +* Contributors: Alejandro Hernández Cordero, Christophe Bedard, Devin Bonnie, Dirk Thomas, DongheeYe, Ivan Santiago Paunovic, Jacob Perron, Sarthak Mittal, brawner, tomoya + +2.0.0 (2020-06-01) +------------------ +* Added missing virtual destructors. (`#1149 `_) +* Fixed a test which was using different types on the same topic. (`#1150 `_) +* Made ``test_rate`` more reliable on Windows and improve error output when it fails (`#1146 `_) +* Added Security Vulnerability Policy pointing to REP-2006. (`#1130 `_) +* Added missing header in ``logging_mutex.cpp``. (`#1145 `_) +* Changed the WaitSet API to pass a shared pointer by value instead than by const reference when possible. (`#1141 `_) +* Changed ``SubscriptionBase::get_subscription_handle() const`` to return a shared pointer to const value. (`#1140 `_) +* Extended the lifetime of ``rcl_publisher_t`` by holding onto the shared pointer in order to avoid a use after free situation. (`#1119 `_) +* Improved some docblocks (`#1127 `_) +* Fixed a lock-order-inversion (potential deadlock) (`#1135 `_) +* Fixed a potential Construction/Destruction order problem between global contexts vector and Context of static lifetime (`#1132 `_) +* Contributors: Alejandro Hernández Cordero, Chris Lalancette, Ivan Santiago Paunovic, Michel Hidalgo, tomoya + +1.1.0 (2020-05-26) +------------------ +* Deprecate set_on_parameters_set_callback (`#1123 `_) +* Expose get_service_names_and_types_by_node from rcl in rclcpp (`#1131 `_) +* Fix thread safety issues related to logging (`#1125 `_) +* Make sure rmw_publisher_options is initialized in to_rcl_publisher_options (`#1099 `_) +* Remove empty lines within method signatures (`#1128 `_) +* Add API review March 2020 document (`#1031 `_) +* Improve documentation (`#1106 `_) +* Make test multi threaded executor more reliable (`#1105 `_) +* Fixed rep links and added more details to dependencies in quality declaration (`#1116 `_) +* Update quality declarations to reflect version 1.0 (`#1115 `_) +* Contributors: Alejandro Hernández Cordero, ChenYing Kuo, Claire Wang, Dirk Thomas, Ivan Santiago Paunovic, William Woodall, Stephen Brawner + +1.0.0 (2020-05-12) +------------------ +* Remove MANUAL_BY_NODE liveliness API (`#1107 `_) +* Use rosidl_default_generators dependency in test (`#1114 `_) +* Make sure to include what you use (`#1112 `_) +* Mark flaky test with xfail: TestMultiThreadedExecutor (`#1109 `_) +* Contributors: Chris Lalancette, Ivan Santiago Paunovic, Karsten Knese, Louise Poubel + +0.9.1 (2020-05-08) +------------------ +* Fix tests that were not properly torn down (`#1073 `_) +* Added docblock in rclcpp (`#1103 `_) +* Added Quality declaration: rclcpp, rclpp_action, rclcpp_components andrclcpp_lifecycle (`#1100 `_) +* Use RCL_RET_SERVICE_TAKE_FAILED and not RCL_RET_CLIENT_TAKE_FAILED when checking a request take (`#1101 `_) +* Update comment about return value in Executor::get_next_ready_executable (`#1085 `_) +* Contributors: Alejandro Hernández Cordero, Christophe Bedard, Devin Bonnie, Ivan Santiago Paunovic + +0.9.0 (2020-04-29) +------------------ +* Serialized message move constructor (`#1097 `_) +* Enforce a precedence for wildcard matching in parameter overrides. (`#1094 `_) +* Add serialized_message.hpp header (`#1095 `_) +* Add received message age metric to topic statistics (`#1080 `_) +* Deprecate redundant namespaces (`#1083 `_) +* Export targets in addition to include directories / libraries (`#1088 `_) +* Ensure logging is initialized just once (`#998 `_) +* Adapt subscription traits to rclcpp::SerializedMessage (`#1092 `_) +* Protect subscriber_statistics_collectors\_ with a mutex (`#1084 `_) +* Remove unused test variable (`#1087 `_) +* Use serialized message (`#1081 `_) +* Integrate topic statistics (`#1072 `_) +* Fix rclcpp interface traits test (`#1086 `_) +* Generate node interfaces' getters and traits (`#1069 `_) +* Use composition for serialized message (`#1082 `_) +* Dnae adas/serialized message (`#1075 `_) +* Reflect changes in rclcpp API (`#1079 `_) +* Fix build regression (`#1078 `_) +* Add NodeDefault option for enabling topic statistics (`#1074 `_) +* Topic Statistics: Add SubscriptionTopicStatistics class (`#1050 `_) +* Add SubscriptionOptions for topic statistics (`#1057 `_) +* Remove warning message from failing to register default callback (`#1067 `_) +* Create a default warning for qos incompatibility (`#1051 `_) +* Add WaitSet class and modify entities to work without executor (`#1047 `_) +* Include what you use (`#1059 `_) +* Rename rosidl_generator_cpp namespace to rosidl_runtime_cpp (`#1060 `_) +* Changed rosidl_generator_c/cpp to rosidl_runtime_c/cpp (`#1014 `_) +* Use constexpr for endpoint type name (`#1055 `_) +* Add InvalidParameterTypeException (`#1027 `_) +* Support for ON_REQUESTED_INCOMPATIBLE_QOS and ON_OFFERED_INCOMPATIBLE_QOS events (`#924 `_) +* Fixup clang warning (`#1040 `_) +* Adding a "static" single threaded executor (`#1034 `_) +* Add equality operators for QoS profile (`#1032 `_) +* Remove extra vertical whitespace (`#1030 `_) +* Switch IntraProcessMessage to test_msgs/Empty (`#1017 `_) +* Add new type of exception that may be thrown during creation of publisher/subscription (`#1026 `_) +* Don't check lifespan on publisher QoS (`#1002 `_) +* Fix get_parameter_tyeps of AsyncPrameterClient results are always empty (`#1019 `_) +* Cleanup node interfaces includes (`#1016 `_) +* Add ifdefs to remove tracing-related calls if tracing is disabled (`#1001 `_) +* Include missing header in node_graph.cpp (`#994 `_) +* Add missing includes of logging.hpp (`#995 `_) +* Zero initialize publisher GID in subscription intra process callback (`#1011 `_) +* Removed ament_cmake dependency (`#989 `_) +* Switch to using new rcutils_strerror (`#993 `_) +* Ensure all rclcpp::Clock accesses are thread-safe +* Use a PIMPL for rclcpp::Clock implementation +* Replace rmw_implementation for rmw dependency in package.xml (`#990 `_) +* Add missing service callback registration tracepoint (`#986 `_) +* Rename rmw_topic_endpoint_info_array count to size (`#996 `_) +* Implement functions to get publisher and subcription informations like QoS policies from topic name (`#960 `_) +* Code style only: wrap after open parenthesis if not in one line (`#977 `_) +* Accept taking an rvalue ref future in spin_until_future_complete (`#971 `_) +* Allow node clock use in logging macros (`#969 `_) (`#970 `_) +* Change order of deprecated and visibility attributes (`#968 `_) +* Deprecated is_initialized() (`#967 `_) +* Don't specify calling convention in std::_Binder template (`#952 `_) +* Added missing include to logging.hpp (`#964 `_) +* Assigning make_shared result to variables in test (`#963 `_) +* Fix unused parameter warning (`#962 `_) +* Stop retaining ownership of the rcl context in GraphListener (`#946 `_) +* Clear sub contexts when starting another init-shutdown cycle (`#947 `_) +* Avoid possible UB in Clock jump callbacks (`#954 `_) +* Handle unknown global ROS arguments (`#951 `_) +* Mark get_clock() as override to fix clang warnings (`#939 `_) +* Create node clock calls const (try 2) (`#922 `_) +* Fix asserts on shared_ptr::use_count; expects long, got uint32 (`#936 `_) +* Use absolute topic name for parameter events (`#929 `_) +* Add enable_rosout into NodeOptions. (`#900 `_) +* Removing "virtual", adding "override" keywords (`#897 `_) +* Use weak_ptr to store context in GraphListener (`#906 `_) +* Complete published event message when declaring a parameter (`#928 `_) +* Fix duration.cpp lint error (`#930 `_) +* Intra-process subscriber should use RMW actual qos. (ros2`#913 `_) (`#914 `_) +* Type conversions fixes (`#901 `_) +* Add override keyword to functions +* Remove unnecessary virtual keywords +* Only check for new work once in spin_some (`#471 `_) (`#844 `_) +* Add addition/subtraction assignment operators to Time (`#748 `_) +* Contributors: Alberto Soragna, Alejandro Hernández Cordero, Barry Xu, Chris Lalancette, Christophe Bedard, Claire Wang, Dan Rose, DensoADAS, Devin Bonnie, Dino Hüllmann, Dirk Thomas, DongheeYe, Emerson Knapp, Ivan Santiago Paunovic, Jacob Perron, Jaison Titus, Karsten Knese, Matt Schickler, Miaofei Mei, Michel Hidalgo, Mikael Arguedas, Monika Idzik, Prajakta Gokhale, Roger Strain, Scott K Logan, Sean Kelly, Stephen Brawner, Steven Macenski, Steven! Ragnarök, Todd Malsbary, Tomoya Fujita, William Woodall, Zachary Michaels + +0.8.3 (2019-11-19) +------------------ + +0.8.2 (2019-11-18) +------------------ +* Updated tracing logic to match changes in rclcpp's intra-process system (`#918 `_) +* Fixed a bug that prevented the ``shutdown_on_sigint`` option to not work correctly (`#850 `_) +* Added support for STREAM logging macros (`#926 `_) +* Relaxed multithreaded test constraint (`#907 `_) +* Contributors: Anas Abou Allaban, Christophe Bedard, Dirk Thomas, alexfneves + +0.8.1 (2019-10-23) +------------------ +* De-flake tests for rmw_connext (`#899 `_) +* rename return functions for loaned messages (`#896 `_) +* Enable throttling logs (`#879 `_) +* New Intra-Process Communication (`#778 `_) +* Instrumentation update (`#789 `_) +* Zero copy api (`#864 `_) +* Drop rclcpp remove_ros_arguments_null test case. (`#894 `_) +* add mechanism to pass rmw impl specific payloads during pub/sub creation (`#882 `_) +* make get_actual_qos return a rclcpp::QoS (`#883 `_) +* Fix Compiler Warning (`#881 `_) +* Add callback handler for use_sim_time parameter `#802 `_ (`#875 `_) +* Contributors: Alberto Soragna, Brian Marchi, Hunter L. Allen, Ingo Lütkebohle, Karsten Knese, Michael Carroll, Michel Hidalgo, William Woodall + +0.8.0 (2019-09-26) +------------------ +* clean up publisher and subscription creation logic (`#867 `_) +* Take parameter overrides provided through the CLI. (`#865 `_) +* add more context to exception message (`#858 `_) +* remove features and related code which were deprecated in dashing (`#852 `_) +* check valid timer handler 1st to reduce the time window for scan. (`#841 `_) +* Add throwing parameter name if parameter is not set (`#833 `_) +* Fix typo in deprecated warning. (`#848 `_) +* Fail on invalid and unknown ROS specific arguments (`#842 `_) +* Force explicit --ros-args in NodeOptions::arguments(). (`#845 `_) +* Use of -r/--remap flags where appropriate. (`#834 `_) +* Fix hang with timers in MultiThreadedExecutor (`#835 `_) (`#836 `_) +* add mutex in add/remove_node and wait_for_work to protect concurrent use/change of memory_strategy\_ (`#837 `_) +* Crash in callback group pointer vector iterator (`#814 `_) +* Wrap documentation examples in code blocks (`#830 `_) +* add callback group as member variable and constructor arg (`#811 `_) +* Fix get_node_interfaces functions taking a pointer (`#821 `_) +* Delete unnecessary call for get_node_by_group (`#823 `_) +* Allow passing logger by const ref (`#820 `_) +* Explain return value of spin_until_future_complete (`#792 `_) +* Adapt to '--ros-args ... [--]'-based ROS args extraction (`#816 `_) +* Add line break after first open paren in multiline function call (`#785 `_) +* remove mock msgs from rclcpp (`#800 `_) +* Make TimeSource ignore use_sim_time events coming from other nodes. (`#799 `_) +* Allow registering multiple on_parameters_set_callback (`#772 `_) +* Add free function for creating service clients (`#788 `_) +* Include missing rcl headers in use. (`#782 `_) +* Switch the NodeParameters lock to recursive. (`#781 `_) +* changed on_parameter_event qos profile to rmw_qos_profile_parameter_events (`#774 `_) +* Adding a factory method to create a Duration from seconds (`#567 `_) +* Fix a comparison with a sign mismatch (`#771 `_) +* delete superfluous spaces (`#770 `_) +* Use params from node '/\*\*' from parameter YAML file (`#762 `_) +* Add ignore override argument to declare parameter (`#767 `_) +* use default parameter descriptor in parameters interface (`#765 `_) +* Added support for const member functions (`#763 `_) +* add get_actual_qos() feature to subscriptions (`#754 `_) +* Ignore parameters overrides in set parameter methods when allowing undeclared parameters (`#756 `_) +* Add rclcpp::create_timer() (`#757 `_) +* checking origin of intra-process msg before taking them (`#753 `_) +* Contributors: Alberto Soragna, Carl Delsey, Chris Lalancette, Dan Rose, Dirk Thomas, Esteve Fernandez, Guillaume Autran, Jacob Perron, Karsten Knese, Luca Della Vedova, M. M, Michel Hidalgo, Scott K Logan, Shane Loretz, Todd Malsbary, William Woodall, bpwilcox, fujitatomoya, ivanpauno + +0.7.5 (2019-05-30) +------------------ +* Avoid 'Intra process message no longer being stored when trying to handle it' warning (`#749 `_) +* Contributors: ivanpauno + +0.7.4 (2019-05-29) +------------------ +* Rename parameter options (`#745 `_) +* Bionic use of strerror_r (`#742 `_) +* Enforce parameter ranges (`#735 `_) +* removed not used parameter client (`#740 `_) +* ensure removal of guard conditions of expired nodes from memory strategy (`#741 `_) +* Fix typo in log warning message (`#737 `_) +* Throw nice errors when creating a publisher with intraprocess communication and incompatible qos policy (`#729 `_) +* Contributors: Alberto Soragna, Dirk Thomas, Jacob Perron, William Woodall, ivanpauno, roderick-koehle + +0.7.3 (2019-05-20) +------------------ +* Fixed misspelling, volitile -> volatile (`#724 `_), and then fixed that since it is a C++ keyword to be ``durability_volatile`` (`#725 `_) +* Fixed a clang warning (`#723 `_) +* Added ``on_parameter_event`` static method to the ``AsyncParametersClient`` (`#688 `_) +* Added a guard against ``ParameterNotDeclaredException`` throwing from within the parameter service callbacks. (`#718 `_) +* Added missing template functionality to lifecycle_node. (`#707 `_) +* Fixed heap-use-after-free and memory leaks reported from ``test_node.cpp`` (`#719 `_) +* Contributors: Alberto Soragna, Dirk Thomas, Emerson Knapp, Jacob Perron, Michael Jeronimo, Prajakta Gokhale + +0.7.2 (2019-05-08) +------------------ +* Added new way to specify QoS settings for publishers and subscriptions. (`#713 `_) + * The new way requires that you specify a history depth when creating a publisher or subscription. + * In the past it was possible to create one without specifying any history depth, but these signatures have been deprecated. +* Deprecated ``shared_ptr`` and raw pointer versions of ``Publisher::publish()``. (`#709 `_) +* Implemented API to set callbacks for liveliness and deadline QoS events for publishers and subscriptions. (`#695 `_) +* Fixed a segmentation fault when publishing a parameter event when they ought to be disabled. (`#714 `_) +* Changes required for upcoming pre-allocation API. (`#711 `_) +* Changed ``Node::get_node_names()`` to return the full node names rather than just the base name. (`#698 `_) +* Remove logic made redundant by the `ros2/rcl#255 `_ pull request. (`#712 `_) +* Various improvements for ``rclcpp::Clock``. (`#696 `_) + * Fixed uninitialized bool in ``clock.cpp``. + * Fixed up includes of ``clock.hpp/cpp``. + * Added documentation for exceptions to ``clock.hpp``. + * Adjusted function signature of getters of ``clock.hpp/cpp``. + * Removed raw pointers to ``Clock::create_jump_callback``. + * Removed unnecessary ``rclcpp`` namespace reference from ``clock.cpp``. + * Changed exception to ``bad_alloc`` on ``JumpHandler`` allocation failure. + * Fixed missing ``nullptr`` check in ``Clock::on_time_jump``. + * Added ``JumpHandler::callback`` types. + * Added warning for lifetime of Clock and JumpHandler +* Fixed bug left over from the `pull request #495 `_. (`#708 `_) +* Changed the ``IntraProcessManager`` to be capable of storing ``shared_ptr`` in addition to ``unique_ptr``. (`#690 `_) +* Contributors: Alberto Soragna, Dima Dorezyuk, M. M, Michael Carroll, Michael Jeronimo, Tully Foote, William Woodall, ivanpauno, jhdcs + +0.7.1 (2019-04-26) +------------------ +* Added read only parameters. (`#495 `_) +* Fixed a concurrency problem in the multithreaded executor. (`#703 `_) +* Fixup utilities. (`#692 `_) +* Added method to read timer cancellation. (`#697 `_) +* Added Exception Generator function for implementing "from_rcl_error". (`#678 `_) +* Updated initialization of rmw_qos_profile_t struct instances. (`#691 `_) +* Removed the const value from the logger before comparison. (`#680 `_) +* Contributors: Devin Bonnie, Dima Dorezyuk, Guillaume Autran, M. M, Shane Loretz, Víctor Mayoral Vilches, William Woodall, jhdcs + +0.7.0 (2019-04-14) +------------------ +* Added Options-struct interfaces for creating publishers/subscribers (pre-QoS, standalone). (`#673 `_) +* Replaced strncpy with memcpy. (`#684 `_) +* Replaced const char * with a std::array as the key of IPM IDTopicMap. (`#671 `_) +* Refactored SignalHandler logger to avoid race during destruction. (`#682 `_) +* Introduce rclcpp_components to implement composition. (`#665 `_) +* Added QoS policy check when configuring intraprocess, skip interprocess publish when possible. (`#674 `_) +* Updated to use do { .. } while(0) around content of logging macros. (`#681 `_) +* Added function to get publisher's actual QoS settings. (`#667 `_) +* Updated to avoid race that triggers timer too often. (`#621 `_) +* Exposed get_fully_qualified_name in NodeBase API. (`#662 `_) +* Updated to use ament_target_dependencies where possible. (`#659 `_) +* Fixed wait for service memory leak bug. (`#656 `_) +* Fixed test_time_source test. (`#639 `_) +* Fixed hard-coded duration type representation so int64_t isn't assumed. (`#648 `_) +* Fixed cppcheck warning. (`#646 `_) +* Added count matching api and intra-process subscriber count. (`#628 `_) +* Added Sub Node alternative. (`#581 `_) +* Replaced 'auto' with 'const auto &'. (`#630 `_) +* Set Parameter Event Publisher settings. `#591 `_ (`#614 `_) +* Replaced node constructor arguments with NodeOptions. (`#622 `_) +* Updated to pass context to wait set (`#617 `_) +* Added API to get parameters in a map. (`#575 `_) +* Updated Bind usage since it is is no longer in std::__1. (`#618 `_) +* Fixed errors from uncrustify v0.68. (`#613 `_) +* Added new constructors for SyncParameterClient. (`#612 `_) +* Contributors: Alberto Soragna, Chris Lalancette, Dirk Thomas, Emerson Knapp, Francisco Martín Rico, Jacob Perron, Marko Durkovic, Michael Carroll, Peter Baughman, Shane Loretz, Wei Liu, William Woodall, Yutaka Kondo, ivanpauno, kuzai, rarvolt + +0.6.2 (2018-12-13) +------------------ +* Updated to use signal safe synchronization with platform specific semaphores (`#607 `_) +* Resolved startup race condition for sim time (`#608 `_) + Resolves `#595 `_ +* Contributors: Tully Foote, William Woodall + +0.6.1 (2018-12-07) +------------------ +* Added wait_for_action_server() for action clients (`#598 `_) +* Added node path and time stamp to parameter event message (`#584 `_) +* Updated to allow removing a waitable (`#597 `_) +* Refactored init to allow for non-global init (`#587 `_) +* Fixed wrong use of constructor and hanging test (`#596 `_) +* Added class Waitable (`#589 `_) +* Updated rcl_wait_set_add\_* calls (`#586 `_) +* Contributors: Dirk Thomas, Jacob Perron, Shane Loretz, William Woodall, bpwilcox + +0.6.0 (2018-11-19) +------------------ +* Updated to use new error handling API from rcutils (`#577 `_) +* Added a warning when publishing if publisher is not active (`#574 `_) +* Added logging macro signature that accepts std::string (`#573 `_) +* Added virtual destructors to classes with virtual functions. (`#566 `_) +* Added semicolons to all RCLCPP and RCUTILS macros. (`#565 `_) +* Removed std::binary_function usage (`#561 `_) +* Updated to avoid auto-activating ROS time if clock topic is being published (`#559 `_) +* Fixed cpplint on xenial (`#556 `_) +* Added get_parameter_or_set_default. (`#551 `_) +* Added max_duration to spin_some() (`#558 `_) +* Updated to output rcl error message when yaml parsing fails (`#557 `_) +* Updated to make sure timer is fini'd before clock (`#553 `_) +* Get node names and namespaces (`#545 `_) +* Fixed and improved documentation (`#546 `_) +* Updated to use rcl_clock_t jump callbacks (`#543 `_) +* Updated to use rcl consolidated wait set functions (`#540 `_) +* Addeed TIME_MAX and DURATION_MAX functions (`#538 `_) +* Updated to publish shared_ptr of rcl_serialized_message (`#541 `_) +* Added Time::is_zero and Duration::seconds (`#536 `_) +* Changed to log an error message instead of throwing exception in destructor (`#535 `_) +* Updated to relax tolerance of now test because timing affected by OS scheduling (`#533 `_) +* Removed incorrect exception on sec < 0 (`#527 `_) +* Added rclcpp::Time::seconds() (`#526 `_) +* Updated Timer API to construct TimerBase/GenericTimer with Clock (`#523 `_) +* Added rclcpp::is_initialized() (`#522 `_) +* Added support for jump handlers with only pre- or post-jump callback (`#517 `_) +* Removed use of uninitialized CMake var (`#512 `_) +* Updated for Uncrustify 0.67 (`#510 `_) +* Added get_node_names API from node. (`#508 `_) +* Contributors: Anis Ladram, Chris Lalancette, Dirk Thomas, Francisco Martín Rico, Karsten Knese, Michael Carroll, Mikael Arguedas, Sagnik Basu, Shane Loretz, Sriram Raghunathan, William Woodall, chapulina, dhood + +0.5.0 (2018-06-25) +------------------ +* Fixed a bug in the multi-threaded executor which could cause it to take a timer (potentially other types of wait-able items) more than once to be worked one. (`#383 `_) + * Specifically this could result in a timer getting called more often that it should when using the multi-threaded executor. +* Added functions that allow you to publish serialized messages and received serialized messages in your subscription callback. (`#388 `_) +* Changed code to always get the Service name from ``rcl`` to ensure the remapped name is returned. (`#498 `_) +* Added previously missing ``set_parameters_atomically()`` method to the Service client interface. (`#494 `_) +* Added ability to initialize parameter values in a Node via a YAML file passed on the command line. (`#488 `_) +* Fixed the ROS parameter interface which got parameters that aren't set. (`#493 `_) +* Added ability to initialize parameter values in a node with an argument to the Node constructor. (`#486 `_) +* Added a ``Subscription`` tests which uses ``std::bind`` to a class member callback. (`#480 `_) +* Refactored the ``ParameterVariant`` class into the ``Parameter`` and ``ParameterValue`` classes. (`#481 `_) +* Relaxed template matching rules for ``std::bind`` and ``GNU C++ >= 7.1``. (`#484 `_) +* Changed to use the new ``rosgraph_msgs/Clock`` message type for the ``/clock`` topic. (`#474 `_) +* Fixed a flaky ROS time test due to not spinning before getting the time. (`#483 `_) +* Nodes now autostart the ROS parameter services which let you get, set, and list parameters in a node. (`#478 `_) +* Added support for arrays in Parameters. (`#443 `_) +* Changed how executors use ``AnyExecutable`` objects so that they are a reference instead of a shared pointer, in order to avoid memory allocation in the "common case". (`#463 `_) +* Added ability to pass command line arguments to the Node constructor. (`#461 `_) +* Added an argument to specify the number of threads a multithreaded executor should create. (`#442 `_) +* Changed library export order for static linking. (`#446 `_) +* Fixed some typos in the time unit tests. (`#453 `_) + Obviously it mean RCL_SYSTEM_TIME but not RCL_ROS_TIME in some test cases + * Signed-off-by: jwang +* Added the scale operation to ``rclcpp::Duration``. + * Signed-off-by: jwang +* Changed API of the log location parameter to be ``const``. (`#451 `_) +* Changed how the subscriber, client, service, and timer handles are stored to resolve shutdown order issues. (`#431 `_ and `#448 `_) +* Updated to get the node's logger name from ``rcl``. (`#433 `_) +* Now depends on ``ament_cmake_ros``. (`#444 `_) +* Updaed code to use logging macros rather than ``fprintf()``. (`#439 `_) +* Fixed a bug that was using an invalid iterator when erasing items using an iterator in a loop. (`#436 `_) +* Changed code to support move of ``rcutils_time_point_value_t`` type from ``uint64_t`` to ``int64_t``. (`#429 `_) +* Renamed parameter byte type to ``byte_values`` from ``bytes_value``. (`#428 `_) +* Changed executor code to clear the wait set before resizing and waiting. (`#427 `_) +* Fixed a potential dereference of nullptr in the topic name validation error string. (`#405 `_) + * Signed-off-by: Ethan Gao +* Changed to use ``rcl_count_publishers()`` like API's rather than the lower level ``rmw_count_publishers()`` API. (`#425 `_) + * Signed-off-by: Sriram Raghunathan +* Fix potential segmentation fault due to ``get_topic_name()`` or ``rcl_service_get_service_name()`` returning nullptr and that not being checked before access in ``rclcpp``. (`#426 `_) + * Signed-off-by: Ethan Gao +* Contributors: Denise Eng, Dirk Thomas, Ernesto Corbellini, Esteve Fernandez, Ethan Gao, Guillaume Autran, Karsten Knese, Matthew, Michael Carroll, Mikael Arguedas, Shane Loretz, Sriram Raghunathan, Tom Moore, William Woodall, dhood, jwang, jwang11, serge-nikulin diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 7190709afc..8712b48856 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -1,32 +1,43 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.20) project(rclcpp) -find_package(ament_cmake REQUIRED) +find_package(Threads REQUIRED) + +find_package(ament_cmake_ros REQUIRED) +find_package(ament_index_cpp REQUIRED) find_package(builtin_interfaces REQUIRED) +find_package(libstatistics_collector REQUIRED) find_package(rcl REQUIRED) find_package(rcl_interfaces REQUIRED) +find_package(rcl_logging_interface REQUIRED) +find_package(rcl_yaml_param_parser REQUIRED) +find_package(rcpputils REQUIRED) +find_package(rcutils REQUIRED) find_package(rmw REQUIRED) -find_package(rmw_implementation REQUIRED) -find_package(rosidl_generator_cpp REQUIRED) +find_package(rosgraph_msgs REQUIRED) +find_package(rosidl_dynamic_typesupport REQUIRED) +find_package(rosidl_runtime_c REQUIRED) +find_package(rosidl_runtime_cpp REQUIRED) find_package(rosidl_typesupport_c REQUIRED) find_package(rosidl_typesupport_cpp REQUIRED) +find_package(statistics_msgs REQUIRED) +find_package(tracetools REQUIRED) -# Default to C++14 +# TODO(wjwwood): remove this when gtest can build on its own, when using target_compile_features() +# Default to C++17 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - # we dont use add_compile_options with pedantic in message packages - # because the Python C extensions dont comply with it - # TODO(mikaelarguedas) change to add_compile_options - # once this is not a message package anymore - # https://github.com/ros2/system_tests/issues/191 - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic") + # About -Wno-sign-conversion: With Clang, -Wconversion implies -Wsign-conversion. There are a number of + # implicit sign conversions in rclcpp and gtest.cc, see https://ci.ros2.org/job/ci_osx/9265/. + # Hence disabling -Wsign-conversion for now until all those have eventually been fixed. + # (from https://github.com/ros2/rclcpp/pull/1188#issuecomment-650229140) + add_compile_options(-Wall -Wextra -Wconversion -Wno-sign-conversion -Wpedantic -Wnon-virtual-dtor -Woverloaded-virtual) endif() -include_directories(include) - set(${PROJECT_NAME}_SRCS src/rclcpp/any_executable.cpp src/rclcpp/callback_group.cpp @@ -34,20 +45,50 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/clock.cpp src/rclcpp/context.cpp src/rclcpp/contexts/default_context.cpp + src/rclcpp/create_generic_client.cpp + src/rclcpp/detail/add_guard_condition_to_rcl_wait_set.cpp + src/rclcpp/detail/resolve_intra_process_buffer_type.cpp + src/rclcpp/detail/resolve_parameter_overrides.cpp + src/rclcpp/detail/rmw_implementation_specific_payload.cpp + src/rclcpp/detail/rmw_implementation_specific_publisher_payload.cpp + src/rclcpp/detail/rmw_implementation_specific_subscription_payload.cpp + src/rclcpp/detail/utilities.cpp src/rclcpp/duration.cpp + src/rclcpp/dynamic_typesupport/dynamic_message.cpp + src/rclcpp/dynamic_typesupport/dynamic_message_type.cpp + src/rclcpp/dynamic_typesupport/dynamic_message_type_builder.cpp + src/rclcpp/dynamic_typesupport/dynamic_message_type_support.cpp + src/rclcpp/dynamic_typesupport/dynamic_serialization_support.cpp src/rclcpp/event.cpp - src/rclcpp/exceptions.cpp + src/rclcpp/exceptions/exceptions.cpp + src/rclcpp/executable_list.cpp src/rclcpp/executor.cpp + src/rclcpp/executor_options.cpp src/rclcpp/executors.cpp - src/rclcpp/expand_topic_or_service_name.cpp + src/rclcpp/executors/executor_entities_collection.cpp + src/rclcpp/executors/executor_entities_collector.cpp + src/rclcpp/executors/executor_notify_waitable.cpp src/rclcpp/executors/multi_threaded_executor.cpp src/rclcpp/executors/single_threaded_executor.cpp + src/rclcpp/executors/static_single_threaded_executor.cpp + src/rclcpp/expand_topic_or_service_name.cpp + src/rclcpp/experimental/executors/events_executor/events_executor.cpp + src/rclcpp/experimental/timers_manager.cpp + src/rclcpp/future_return_code.cpp + src/rclcpp/generic_client.cpp + src/rclcpp/generic_publisher.cpp + src/rclcpp/generic_service.cpp + src/rclcpp/generic_subscription.cpp src/rclcpp/graph_listener.cpp + src/rclcpp/guard_condition.cpp + src/rclcpp/init_options.cpp src/rclcpp/intra_process_manager.cpp - src/rclcpp/intra_process_manager_impl.cpp src/rclcpp/logger.cpp + src/rclcpp/logging_mutex.cpp src/rclcpp/memory_strategies.cpp src/rclcpp/memory_strategy.cpp + src/rclcpp/message_info.cpp + src/rclcpp/network_flow_endpoint.cpp src/rclcpp/node.cpp src/rclcpp/node_interfaces/node_base.cpp src/rclcpp/node_interfaces/node_clock.cpp @@ -55,22 +96,57 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/node_interfaces/node_logging.cpp src/rclcpp/node_interfaces/node_parameters.cpp src/rclcpp/node_interfaces/node_services.cpp + src/rclcpp/node_interfaces/node_time_source.cpp src/rclcpp/node_interfaces/node_timers.cpp src/rclcpp/node_interfaces/node_topics.cpp + src/rclcpp/node_interfaces/node_type_descriptions.cpp + src/rclcpp/node_interfaces/node_waitables.cpp + src/rclcpp/node_options.cpp src/rclcpp/parameter.cpp src/rclcpp/parameter_client.cpp + src/rclcpp/parameter_event_handler.cpp src/rclcpp/parameter_events_filter.cpp + src/rclcpp/parameter_map.cpp src/rclcpp/parameter_service.cpp - src/rclcpp/publisher.cpp + src/rclcpp/parameter_value.cpp + src/rclcpp/publisher_base.cpp + src/rclcpp/qos.cpp + src/rclcpp/event_handler.cpp + src/rclcpp/qos_overriding_options.cpp + src/rclcpp/rate.cpp + src/rclcpp/serialization.cpp + src/rclcpp/serialized_message.cpp src/rclcpp/service.cpp - src/rclcpp/subscription.cpp + src/rclcpp/signal_handler.cpp + src/rclcpp/subscription_base.cpp + src/rclcpp/subscription_intra_process_base.cpp src/rclcpp/time.cpp src/rclcpp/time_source.cpp src/rclcpp/timer.cpp src/rclcpp/type_support.cpp + src/rclcpp/typesupport_helpers.cpp src/rclcpp/utilities.cpp + src/rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.cpp + src/rclcpp/waitable.cpp ) +# By default, without the settings below, find_package(Python3) will attempt +# to find the newest python version it can, and additionally will find the +# most specific version. For instance, on a system that has +# /usr/bin/python3.10, /usr/bin/python3.11, and /usr/bin/python3, it will find +# /usr/bin/python3.11, even if /usr/bin/python3 points to /usr/bin/python3.10. +# The behavior we want is to prefer the "system" installed version unless the +# user specifically tells us othewise through the Python3_EXECUTABLE hint. +# Setting CMP0094 to NEW means that the search will stop after the first +# python version is found. Setting Python3_FIND_UNVERSIONED_NAMES means that +# the search will prefer /usr/bin/python3 over /usr/bin/python3.11. And that +# latter functionality is only available in CMake 3.20 or later, so we need +# at least that version. +cmake_policy(SET CMP0094 NEW) +set(Python3_FIND_UNVERSIONED_NAMES FIRST) + +find_package(Python3 REQUIRED COMPONENTS Interpreter) + # "watch" template for changes configure_file( "resource/logging.hpp.em" @@ -78,28 +154,98 @@ configure_file( COPYONLY ) # generate header with logging macros -set(python_code +set(python_code_logging "import em" "em.invoke(['-o', 'include/rclcpp/logging.hpp', '${CMAKE_CURRENT_SOURCE_DIR}/resource/logging.hpp.em'])") -string(REPLACE ";" "$" python_code "${python_code}") +string(REPLACE ";" "$" python_code_logging "${python_code_logging}") add_custom_command(OUTPUT include/rclcpp/logging.hpp COMMAND ${CMAKE_COMMAND} -E make_directory "include/rclcpp" - COMMAND ${PYTHON_EXECUTABLE} ARGS -c "${python_code}" + COMMAND Python3::Interpreter ARGS -c "${python_code_logging}" DEPENDS "${CMAKE_CURRENT_BINARY_DIR}/logging.hpp.em.watch" COMMENT "Expanding logging.hpp.em" VERBATIM ) list(APPEND ${PROJECT_NAME}_SRCS include/rclcpp/logging.hpp) -include_directories("${CMAKE_CURRENT_BINARY_DIR}/include") -add_library(${PROJECT_NAME} SHARED - ${${PROJECT_NAME}_SRCS}) -ament_target_dependencies(${PROJECT_NAME} - "builtin_interfaces" - "rcl" - "rosidl_generator_cpp" - "rosidl_typesupport_cpp") +file(GLOB interface_files "include/rclcpp/node_interfaces/node_*_interface.hpp") +foreach(interface_file ${interface_files}) + get_filename_component(interface_name ${interface_file} NAME_WE) + + # "watch" template for changes + configure_file( + "resource/interface_traits.hpp.em" + "${CMAKE_CURRENT_BINARY_DIR}/${interface_name}_traits.hpp.em.watch" + COPYONLY + ) + set(python_${interface_name}_traits + "import em" + "em.invoke(['-D', 'interface_name = \\'${interface_name}\\'', '-o', 'include/rclcpp/node_interfaces/${interface_name}_traits.hpp', '${CMAKE_CURRENT_SOURCE_DIR}/resource/interface_traits.hpp.em'])") + string(REPLACE ";" "$" python_${interface_name}_traits "${python_${interface_name}_traits}") + add_custom_command(OUTPUT include/rclcpp/node_interfaces/${interface_name}_traits.hpp + COMMAND ${CMAKE_COMMAND} -E make_directory "include/rclcpp/node_interfaces" + COMMAND Python3::Interpreter ARGS -c "${python_${interface_name}_traits}" + DEPENDS "${CMAKE_CURRENT_BINARY_DIR}/${interface_name}_traits.hpp.em.watch" + COMMENT "Expanding interface_traits.hpp.em into ${interface_name}_traits.hpp" + VERBATIM + ) + list(APPEND ${PROJECT_NAME}_SRCS + include/rclcpp/node_interfaces/${interface_name}_traits.hpp) + + # "watch" template for changes + configure_file( + "resource/get_interface.hpp.em" + "get_${interface_name}.hpp.em.watch" + COPYONLY + ) + set(python_get_${interface_name} + "import em" + "em.invoke(['-D', 'interface_name = \\'${interface_name}\\'', '-o', 'include/rclcpp/node_interfaces/get_${interface_name}.hpp', '${CMAKE_CURRENT_SOURCE_DIR}/resource/get_interface.hpp.em'])") + string(REPLACE ";" "$" python_get_${interface_name} "${python_get_${interface_name}}") + add_custom_command(OUTPUT include/rclcpp/node_interfaces/get_${interface_name}.hpp + COMMAND ${CMAKE_COMMAND} -E make_directory "include/rclcpp/node_interfaces" + COMMAND Python3::Interpreter ARGS -c "${python_get_${interface_name}}" + DEPENDS "${CMAKE_CURRENT_BINARY_DIR}/get_${interface_name}.hpp.em.watch" + COMMENT "Expanding get_interface.hpp.em into get_${interface_file}.hpp" + VERBATIM + ) + list(APPEND ${PROJECT_NAME}_SRCS + include/rclcpp/node_interfaces/get_${interface_name}.hpp) +endforeach() + +add_library(${PROJECT_NAME} ${${PROJECT_NAME}_SRCS}) +target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17) +# TODO(wjwwood): address all deprecation warnings and then remove this +if(WIN32) + target_compile_definitions(${PROJECT_NAME} PUBLIC "_SILENCE_ALL_CXX17_DEPRECATION_WARNINGS") +endif() +target_include_directories(${PROJECT_NAME} PUBLIC + "$" + "$" + "$") +target_link_libraries(${PROJECT_NAME} PUBLIC + ${builtin_interfaces_TARGETS} + libstatistics_collector::libstatistics_collector + rcl::rcl + ${rcl_interfaces_TARGETS} + rcl_yaml_param_parser::rcl_yaml_param_parser + rcpputils::rcpputils + rcutils::rcutils + rmw::rmw + ${rosgraph_msgs_TARGETS} + rosidl_dynamic_typesupport::rosidl_dynamic_typesupport + rosidl_runtime_c::rosidl_runtime_c + rosidl_runtime_cpp::rosidl_runtime_cpp + rosidl_typesupport_cpp::rosidl_typesupport_cpp + ${statistics_msgs_TARGETS} + tracetools::tracetools + ${CMAKE_THREAD_LIBS_INIT} +) + +target_link_libraries(${PROJECT_NAME} PRIVATE + ament_index_cpp::ament_index_cpp + rcl_logging_interface::rcl_logging_interface +) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. @@ -107,231 +253,58 @@ target_compile_definitions(${PROJECT_NAME} PRIVATE "RCLCPP_BUILDING_LIBRARY") install( - TARGETS ${PROJECT_NAME} + TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin ) -ament_export_dependencies(ament_cmake) -ament_export_dependencies(builtin_interfaces) -ament_export_dependencies(rcl) -ament_export_dependencies(rosidl_generator_cpp) -ament_export_dependencies(rosidl_typesupport_c) -ament_export_dependencies(rosidl_typesupport_cpp) +# Export old-style CMake variables +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_libraries(${PROJECT_NAME}) -ament_export_include_directories(include) +# Export modern CMake targets +ament_export_targets(${PROJECT_NAME}) -ament_export_libraries(${PROJECT_NAME}) +ament_export_dependencies( + builtin_interfaces + libstatistics_collector + rcl + rcl_interfaces + rcl_yaml_param_parser + rcpputils + rcutils + rmw + rosgraph_msgs + rosidl_dynamic_typesupport + rosidl_runtime_c + rosidl_runtime_cpp + rosidl_typesupport_cpp + statistics_msgs + tracetools +) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() - find_package(rmw_implementation_cmake REQUIRED) - - ament_add_gtest(test_client test/test_client.cpp) - if(TARGET test_client) - target_include_directories(test_client PUBLIC - ${rcl_interfaces_INCLUDE_DIRS} - ${rmw_INCLUDE_DIRS} - ${rosidl_generator_cpp_INCLUDE_DIRS} - ${rosidl_typesupport_cpp_INCLUDE_DIRS} - ) - target_link_libraries(test_client ${PROJECT_NAME}) - endif() - ament_add_gtest(test_expand_topic_or_service_name test/test_expand_topic_or_service_name.cpp) - if(TARGET test_expand_topic_or_service_name) - target_include_directories(test_expand_topic_or_service_name PUBLIC - ${rcl_interfaces_INCLUDE_DIRS} - ${rmw_INCLUDE_DIRS} - ${rosidl_generator_cpp_INCLUDE_DIRS} - ${rosidl_typesupport_cpp_INCLUDE_DIRS} - ) - target_link_libraries(test_expand_topic_or_service_name ${PROJECT_NAME}) - endif() - ament_add_gtest(test_function_traits test/test_function_traits.cpp) - if(TARGET test_function_traits) - target_include_directories(test_function_traits PUBLIC - ${rcl_interfaces_INCLUDE_DIRS} - ${rmw_INCLUDE_DIRS} - ${rosidl_generator_cpp_INCLUDE_DIRS} - ${rosidl_typesupport_cpp_INCLUDE_DIRS} - ) - endif() - ament_add_gtest(test_mapped_ring_buffer test/test_mapped_ring_buffer.cpp) - if(TARGET test_mapped_ring_buffer) - target_include_directories(test_mapped_ring_buffer PUBLIC - ${rcl_INCLUDE_DIRS} - ${rcl_interfaces_INCLUDE_DIRS} - ${rmw_INCLUDE_DIRS} - ${rosidl_generator_cpp_INCLUDE_DIRS} - ${rosidl_typesupport_cpp_INCLUDE_DIRS} - ) - endif() - ament_add_gtest(test_intra_process_manager test/test_intra_process_manager.cpp) - if(TARGET test_intra_process_manager) - target_include_directories(test_intra_process_manager PUBLIC - ${rcl_INCLUDE_DIRS} - ${rcl_interfaces_INCLUDE_DIRS} - ${rmw_INCLUDE_DIRS} - ${rosidl_generator_cpp_INCLUDE_DIRS} - ${rosidl_typesupport_cpp_INCLUDE_DIRS} - ) - endif() - ament_add_gtest(test_node test/test_node.cpp) - if(TARGET test_node) - target_include_directories(test_node PUBLIC - ${rcl_interfaces_INCLUDE_DIRS} - ${rmw_INCLUDE_DIRS} - ${rosidl_generator_cpp_INCLUDE_DIRS} - ${rosidl_typesupport_cpp_INCLUDE_DIRS} - ) - target_link_libraries(test_node ${PROJECT_NAME}) - endif() - ament_add_gtest(test_parameter_events_filter test/test_parameter_events_filter.cpp) - if(TARGET test_parameter_events_filter) - target_include_directories(test_parameter_events_filter PUBLIC - ${rcl_interfaces_INCLUDE_DIRS} - ${rmw_INCLUDE_DIRS} - ${rosidl_generator_cpp_INCLUDE_DIRS} - ${rosidl_typesupport_cpp_INCLUDE_DIRS} - ) - target_link_libraries(test_parameter_events_filter ${PROJECT_NAME}) - endif() - ament_add_gtest(test_publisher test/test_publisher.cpp) - if(TARGET test_publisher) - target_include_directories(test_publisher PUBLIC - ${rcl_interfaces_INCLUDE_DIRS} - ${rmw_INCLUDE_DIRS} - ${rosidl_generator_cpp_INCLUDE_DIRS} - ${rosidl_typesupport_cpp_INCLUDE_DIRS} - ) - target_link_libraries(test_publisher ${PROJECT_NAME}) - endif() - ament_add_gtest(test_rate test/test_rate.cpp - ENV RCL_ASSERT_RMW_ID_MATCHES=${rmw_implementation}) - if(TARGET test_rate) - target_include_directories(test_rate PUBLIC - ${rcl_interfaces_INCLUDE_DIRS} - ${rmw_INCLUDE_DIRS} - ${rosidl_generator_cpp_INCLUDE_DIRS} - ${rosidl_typesupport_cpp_INCLUDE_DIRS} - ) - target_link_libraries(test_rate - ${PROJECT_NAME} - ) - endif() - ament_add_gtest(test_service test/test_service.cpp) - if(TARGET test_service) - target_include_directories(test_service PUBLIC - ${rcl_interfaces_INCLUDE_DIRS} - ${rmw_INCLUDE_DIRS} - ${rosidl_generator_cpp_INCLUDE_DIRS} - ${rosidl_typesupport_cpp_INCLUDE_DIRS} - ) - target_link_libraries(test_service ${PROJECT_NAME}) - endif() - ament_add_gtest(test_subscription test/test_subscription.cpp) - if(TARGET test_subscription) - target_include_directories(test_subscription PUBLIC - ${rcl_interfaces_INCLUDE_DIRS} - ${rmw_INCLUDE_DIRS} - ${rosidl_generator_cpp_INCLUDE_DIRS} - ${rosidl_typesupport_cpp_INCLUDE_DIRS} - ) - target_link_libraries(test_subscription ${PROJECT_NAME}) - endif() - ament_add_gtest(test_find_weak_nodes test/test_find_weak_nodes.cpp) - if(TARGET test_find_weak_nodes) - target_include_directories(test_find_weak_nodes PUBLIC - ${rcl_INCLUDE_DIRS} - ) - target_link_libraries(test_find_weak_nodes ${PROJECT_NAME}) - endif() - - get_default_rmw_implementation(default_rmw) - find_package(${default_rmw} REQUIRED) - get_rmw_typesupport(typesupport_impls_cpp "${default_rmw}" LANGUAGE "cpp") - get_rmw_typesupport(typesupport_impls_c "${default_rmw}" LANGUAGE "c") - set(mock_msg_files - "test/mock_msgs/srv/Mock.srv") - rosidl_generate_interfaces(mock_msgs - ${mock_msg_files} - LIBRARY_NAME "rclcpp" - SKIP_INSTALL) - - set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR}") - if(WIN32) - set(append_library_dirs "${append_library_dirs}/$") - endif() - - ament_add_gtest(test_externally_defined_services test/test_externally_defined_services.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}") - if(TARGET test_externally_defined_services) - target_include_directories(test_externally_defined_services PUBLIC - ${rcl_INCLUDE_DIRS} - ) - target_link_libraries(test_externally_defined_services ${PROJECT_NAME}) - foreach(typesupport_impl_cpp ${typesupport_impls_cpp}) - rosidl_target_interfaces(test_externally_defined_services - mock_msgs ${typesupport_impl_cpp}) - endforeach() - foreach(typesupport_impl_c ${typesupport_impls_c}) - rosidl_target_interfaces(test_externally_defined_services - mock_msgs ${typesupport_impl_c}) - endforeach() - endif() - - ament_add_gtest(test_duration test/test_duration.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}") - if(TARGET test_duration) - ament_target_dependencies(test_duration - "rcl") - target_link_libraries(test_duration ${PROJECT_NAME}) - endif() - - ament_add_gtest(test_executor test/test_executor.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}") - if(TARGET test_executor) - ament_target_dependencies(test_executor - "rcl") - target_link_libraries(test_executor ${PROJECT_NAME}) - endif() - - ament_add_gtest(test_logger test/test_logger.cpp) - target_link_libraries(test_logger ${PROJECT_NAME}) - - ament_add_gmock(test_logging test/test_logging.cpp) - target_link_libraries(test_logging ${PROJECT_NAME}) - - ament_add_gtest(test_time test/test_time.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}") - if(TARGET test_time) - ament_target_dependencies(test_time - "rcl") - target_link_libraries(test_time ${PROJECT_NAME}) - endif() - - ament_add_gtest(test_time_source test/test_time_source.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}") - if(TARGET test_time_source) - ament_target_dependencies(test_time_source - "rcl") - target_link_libraries(test_time_source ${PROJECT_NAME}) - endif() + add_subdirectory(test) endif() -ament_package( - CONFIG_EXTRAS rclcpp-extras.cmake -) - -install( - DIRECTORY cmake - DESTINATION share/${PROJECT_NAME} -) +ament_package() install( DIRECTORY include/ ${CMAKE_CURRENT_BINARY_DIR}/include/ - DESTINATION include + DESTINATION include/${PROJECT_NAME} ) + +if(TEST cppcheck) + # must set the property after ament_package() + set_tests_properties(cppcheck PROPERTIES TIMEOUT 1200) +endif() + +if(TEST cpplint) + set_tests_properties(cpplint PROPERTIES TIMEOUT 180) +endif() + +ament_generate_version_header(${PROJECT_NAME}) diff --git a/rclcpp/Doxyfile b/rclcpp/Doxyfile index 2a7846b1f8..3c76693821 100644 --- a/rclcpp/Doxyfile +++ b/rclcpp/Doxyfile @@ -21,13 +21,22 @@ GENERATE_LATEX = NO ENABLE_PREPROCESSING = YES MACRO_EXPANSION = YES EXPAND_ONLY_PREDEF = YES -PREDEFINED = RCLCPP_PUBLIC= +PREDEFINED += DOXYGEN_ONLY +PREDEFINED += RCLCPP_LOCAL= +PREDEFINED += RCLCPP_PUBLIC= +PREDEFINED += RCLCPP_PUBLIC_TYPE= +PREDEFINED += RCUTILS_WARN_UNUSED= +PREDEFINED += RCPPUTILS_TSA_GUARDED_BY(x)= +PREDEFINED += RCPPUTILS_TSA_PT_GUARDED_BY(x)= +PREDEFINED += RCPPUTILS_TSA_REQUIRES(x)= + +DOT_GRAPH_MAX_NODES = 101 # 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/" +#TAGFILES += "../../../../doxygen_tag_files/cppreference-doxygen-web.tag.xml=http://en.cppreference.com/w/" # Consider changing "latest" to the version you want to reference (e.g. beta1 or 1.0.0) -TAGFILES += "../../../../doxygen_tag_files/rcl.tag=http://docs.ros2.org/latest/api/rcl/" -TAGFILES += "../../../../doxygen_tag_files/rmw.tag=http://docs.ros2.org/latest/api/rmw/" -TAGFILES += "../../../../doxygen_tag_files/rcutils.tag=http://docs.ros2.org/latest/api/rcutils/" +#TAGFILES += "../../../../doxygen_tag_files/rcl.tag=http://docs.ros2.org/latest/api/rcl/" +#TAGFILES += "../../../../doxygen_tag_files/rmw.tag=http://docs.ros2.org/latest/api/rmw/" +#TAGFILES += "../../../../doxygen_tag_files/rcutils.tag=http://docs.ros2.org/latest/api/rcutils/" # Uncomment to generate tag files for cross-project linking. #GENERATE_TAGFILE = "../../../../doxygen_tag_files/rclcpp.tag" diff --git a/rclcpp/QUALITY_DECLARATION.md b/rclcpp/QUALITY_DECLARATION.md new file mode 100644 index 0000000000..53c2908f7c --- /dev/null +++ b/rclcpp/QUALITY_DECLARATION.md @@ -0,0 +1,228 @@ +This document is a declaration of software quality for the `rclcpp` package, based on the guidelines in [REP-2004](https://www.ros.org/reps/rep-2004.html). + +# rclcpp Quality Declaration + +The package `rclcpp` 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 Requirements for Quality Level 1 in REP-2004](https://www.ros.org/reps/rep-2004.html). + +## Version Policy [1] + +### Version Scheme [1.i] + +`rclcpp` 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] + +`rclcpp` 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. + +Except for the exclusions listed below, all installed headers are in the `include` directory of the package, headers in any other folders are not installed and considered private. +Headers under the folder `experimental` are not considered part of the public API as they have not yet been stabilized. These symbols are namespaced `rclcpp::experimental`. +Headers under the folder `detail` are not considered part of the public API and are subject to change without notice. These symbols are namespaced `rclcpp::detail`. + +### API Stability Policy [1.iv] + +`rclcpp` 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] + +`rclcpp` 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] + +`rclcpp` 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] + +`rclcpp` follows the recommended guidelines for ROS Core packages in the [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#change-control-process). + +### Change Requests [2.i] + +All changes will occur through a pull request, check [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/rclcpp/) +* [linux_release](https://ci.ros2.org/view/nightly/job/nightly_linux_release/lastBuild/testReport/rclcpp/) +* [mac_osx_release](https://ci.ros2.org/view/nightly/job/nightly_osx_release/lastBuild/testReport/rclcpp/) +* [windows_release](https://ci.ros2.org/view/nightly/job/nightly_win_rel/lastBuild/testReport/rclcpp/) + +### Documentation Policy [2.v] + +All pull requests must resolve related documentation changes before merging. + +## Documentation [3] + +### Feature Documentation [3.i] + +`rclcpp` has a [feature list](http://docs.ros2.org/latest/api/rclcpp/) and each item in the list links to the corresponding feature documentation. There is documentation for all of the features, and new features require documentation before being added. + +### Public API Documentation [3.ii] + +The API is publicly available in its [ROS 2 API documentation](http://docs.ros2.org/latest/api/rclcpp/). + +### License [3.iii] + +The license for `rclcpp` 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](http://build.ros2.org/view/Rpr/job/Rpr__rclcpp__ubuntu_focal_amd64/lastCompletedBuild/testReport/rclcpp/) 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 `rclcpp`. + +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](http://build.ros2.org/view/Rpr/job/Rpr__rclcpp__ubuntu_focal_amd64/lastCompletedBuild/testReport/rclcpp/copyright/). + +## Testing [4] + +### Feature Testing [4.i] + +Each feature in `rclcpp` has corresponding tests which simulate typical usage, and they are located in the [`test`](https://github.com/ros2/rclcpp/tree/rolling/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/rclcpp/) +* [linux_release](https://ci.ros2.org/view/nightly/job/nightly_linux_release/lastBuild/testReport/rclcpp/) +* [mac_osx_release](https://ci.ros2.org/view/nightly/job/nightly_osx_release/lastBuild/testReport/rclcpp/) +* [windows_release](https://ci.ros2.org/view/nightly/job/nightly_win_rel/lastBuild/testReport/rclcpp/) + +### 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. + +### Coverage [4.iii] + +`rclcpp` 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 +- achieving and maintaining a reasonable branch line coverage (90-100%) +- no lines are manually skipped in coverage calculations + +Changes are required to make a best effort to keep or increase coverage before being accepted, but decreases are allowed if properly justified and accepted by reviewers. + +Current coverage statistics can be viewed [here](https://ci.ros2.org/job/nightly_linux_coverage/lastCompletedBuild/cobertura/src_ros2_rclcpp_rclcpp_src_rclcpp/). 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). + +`rclcpp` has a line coverage `>= 95%`, which is calculated over all directories within `rclcpp` with the exception of the `experimental` directory. + +### Performance [4.iv] + +`rclcpp` follows the recommendations for performance testing of C/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. + +The performance tests of `rclcpp` are located in the [test/benchmark directory](https://github.com/ros2/rclcpp/tree/rolling/rclcpp/test/benchmark). + +System level performance benchmarks that cover features of `rclcpp` 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] + +`rclcpp` uses and passes all the ROS 2 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/rclcpp/) +* [linux_release](https://ci.ros2.org/view/nightly/job/nightly_linux_release/lastBuild/testReport/rclcpp/) +* [mac_osx_release](https://ci.ros2.org/view/nightly/job/nightly_osx_release/lastBuild/testReport/rclcpp/) +* [windows_release](https://ci.ros2.org/view/nightly/job/nightly_win_rel/lastBuild/testReport/rclcpp/) + +## Dependencies [5] + +Below are evaluations of each of `rclcpp`'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 and Optional Runtime ROS Dependencies [5.i]/[5.ii] + +`rclcpp` has the following runtime ROS dependencies: + +#### `libstatistics_collector` + +The `libstatistics_collector` package provides lightweight aggregation utilities to collect statistics and measure message metrics. + +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros-tooling/libstatistics_collector/tree/rolling/QUALITY_DECLARATION.md). + +#### `rcl` + +`rcl` a library to support implementation of language specific ROS 2 Client Libraries. + +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcl/blob/rolling/rcl/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/tree/rolling/rcl_yaml_param_parser/QUALITY_DECLARATION.md). + +#### `rcpputils` + +The `rcpputils` package provides an API which contains common utilities and data structures useful when programming in C++. + +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcpputils/blob/rolling/QUALITY_DECLARATION.md). + +#### `rcutils` + +The `rcutils` package provides an API which contains common utilities and data structures useful when programming in C. + +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcutils/blob/rolling/QUALITY_DECLARATION.md). + +#### `rmw` + +`rmw` is the ROS 2 middleware library. + +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rmw/blob/rolling/rmw/QUALITY_DECLARATION.md). + +#### `statistics_msgs` + +The `statistics_msgs` package contains ROS 2 message definitions for reporting statistics for topics and system resources. + +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcl_interfaces/blob/rolling/statistics_msgs/QUALITY_DECLARATION.md). + +#### `tracetools` + +The `tracetools` package provides utilities for instrumenting the code in `rclcpp` so that it may be traced for debugging and performance analysis. + +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/ros2_tracing/blob/rolling/tracetools/QUALITY_DECLARATION.md). + +### Direct Runtime non-ROS Dependency [5.iii] + +`rclcpp` has no run-time or build-time dependencies that need to be considered for this declaration. + +## Platform Support [6] + +`rclcpp` supports all of the tier 1 platforms as described in [REP-2000](https://www.ros.org/reps/rep-2000.html#support-tiers), and tests each change against all of them. + +Currently nightly build status can be seen here: +* [linux-aarch64_release](https://ci.ros2.org/view/nightly/job/nightly_linux-aarch64_release/lastBuild/rclcpp/) +* [linux_release](https://ci.ros2.org/view/nightly/job/nightly_linux_release/lastBuild/rclcpp/) +* [mac_osx_release](https://ci.ros2.org/view/nightly/job/nightly_osx_release/lastBuild/rclcpp/) +* [windows_release](https://ci.ros2.org/view/nightly/job/nightly_win_rel/lastBuild/rclcpp/) + +## Security + +### Vulnerability Disclosure Policy [7.i] + +This package conforms to the Vulnerability Disclosure Policy in [REP-2006](https://www.ros.org/reps/rep-2006.html). diff --git a/rclcpp/README.md b/rclcpp/README.md new file mode 100644 index 0000000000..8ec47772f1 --- /dev/null +++ b/rclcpp/README.md @@ -0,0 +1,9 @@ +# `rclcpp` + +The ROS client library in C++. + +The link to the latest rclcpp API documentation, which includes a complete list of its main components and features, can be found on the [rclcpp package info page](https://docs.ros.org/en/rolling/p/rclcpp). + +## Quality Declaration + +This package claims to be in the **Quality Level 1** category, see the [Quality Declaration](QUALITY_DECLARATION.md) for more details. diff --git a/rclcpp/cmake/rclcpp_create_node_main.cmake b/rclcpp/cmake/rclcpp_create_node_main.cmake deleted file mode 100644 index 086d5f6f13..0000000000 --- a/rclcpp/cmake/rclcpp_create_node_main.cmake +++ /dev/null @@ -1,26 +0,0 @@ -# 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. - - -set(rclcpp_node_main_SRC "${rclcpp_DIR}/../../../src/rclcpp/node_main.cpp") - -function(rclcpp_create_node_main node_library_target) - if(NOT TARGET ${node_library_target}) - message(FATAL_ERROR "rclcpp_create_node_main() the first argument must be a valid target name") - endif() - set(executable_name_ ${node_library_target}_node) - add_executable(${executable_name_} ${rclcpp_node_main_SRC}) - target_link_libraries(${executable_name_} ${node_library_target}) - install(TARGETS ${executable_name_} DESTINATION bin) -endfunction() diff --git a/rclcpp/doc/api_review_march_2020.md b/rclcpp/doc/api_review_march_2020.md new file mode 100644 index 0000000000..49ffa1feb8 --- /dev/null +++ b/rclcpp/doc/api_review_march_2020.md @@ -0,0 +1,437 @@ +# API Review for `rclcpp` from March 2020 + +## Notes + +### Off-Topic Questions + +> [rclcpp_action] There exists a thread-safe and non-thread-safe way to get the goal result from an action client. We probably want to remove the public interface to the non-thread safe call (or consolidate somehow): https://github.com/ros2/rclcpp/issues/955 + +`rclcpp_action` is out of scope atm. + +**Notes from 2020-03-19**: To be handled in separate API review. + +## Architecture + +### Calling Syntax and Keeping Node-like Class APIs in Sync + +> Currently, much of the API is exposed via the `rclcpp::Node` class, and due to the nature of the current architecture there is a lot of repeated code to expose these methods and then call the implementations which are in other classes like `rclcpp::node_interfaces::NodeTopics`, for example. +> +> Also, we have other versions of the class `rclcpp::Node` with different semantics and interfaces, like `rclcpp_lifecycle::LifecycleNode`, and we have been having trouble keeping the interface provided there up to date with how things are done in `rclcpp::Node`. Since `LifecycleNode` has a different API from `Node` in some important cases, it does not just inherit from `Node`. +> +> There are two main proposals (as I see it) to try and address this issue, either (a) break up the functionality in `Node` so that it is in separate classes and make `Node` multiple inherit from those classes, and then `LifecycleNode` could selectively inherit from those as well, or (b) change our calling convention from `node->do_thing(...)` to be `do_thing(node, ...)`. +> +> For (a) which commonly referred to as the [Policy Based Design Pattern](https://en.wikipedia.org/wiki/Modern_C%2B%2B_Design#Policy-based_design), we'd be reversing previous design decisions which we discussed at length where we decided to use composition over inheritance for various reasons. +> One of the reasons was testing, with the theory that having simpler separate interfaces we could more easily mock them as needed for testing. +> The testing goal would still be met, either by keeping the "node_interface" classes as-is or by mocking the classes that node would multiple inherit from, however it's harder to indicate that a function needs a class that multiple inherits from several classes but not others. +> Also having interdependency between the classes which are inherited from is a bit complicated in this design pattern. +> +> For (b), we would be changing how we recommend all code be written (not a trivial thing to do at all), because example code like `auto pub = node->create_publsiher(...);` would be come `auto pub = create_publisher(node, ...);`. +> This has some distinct advantages, however, in that it allows us to write these functions, like `create_publisher(node, ...)`, so that the node argument can be any class that meets the criteria of the function. +> That not only means that when we add a feature it automatically works with `Node` and `LifecycleNode` without adding anything to them, it also means that user defined `Node`-like classes will also work, even if they do not inherit from or provide the complete interface for `rclcpp::Node`. +> Another major downside of this approach is discoverability of the API when using auto-completion in text editors, as `node->` will often give you a list of methods to explore, but with the new calling convention, there's not way to get an auto complete for code who's first argument is a `Node`-like class. +> +> Both of the above approaches address some of the main concerns, which are: keeping `Node` and `LifecycleNode` in sync, reducing the size of the `Node` class so it is more easily maintained, documented, and so that related functions are grouped more clearly. + +- https://github.com/ros2/rclcpp/issues/898 +- https://github.com/ros2/rclcpp/issues/509 +- https://github.com/ros2/rclcpp/issues/855 +- https://github.com/ros2/rclcpp/issues/985 + - subnode feature is in rclcpp::Node only, complicating "node using" API designs +- http://www.open-std.org/jtc1/sc22/wg21/docs/papers/2014/n4174.pdf +- https://en.wikipedia.org/wiki/Uniform_Function_Call_Syntax#C++_proposal + - "Many programmers are tempted to write member functions to get the benefits of the member function syntax (e.g. "dot-autocomplete" to list member functions);[6] however, this leads to excessive coupling between classes.[7]" + +**Suggested Action**: Document the discussion and defer until Foxy. + +**Notes from 2020-03-19**: + +- Another version of (b) could be to have classes that are constructed with node, e.g. `Publisher(node, ...)` rather than `node->create_publisher(...)` +- (tfoote) interface class? `NodeInterface::something(node_like)` + - DRY? + - `NodeInterface::` -> only life cycle node methods +- (karsten) use interface classes directly, e.g. node->get_X_interface()->do_thing() +- (dirk) use macros to add methods to class + - Question: Do we want tab-completable API (specifically list of member functions)? +- Question: Is consistency in calling between core and non-core features more important than tab-completion? +- Add better example of adding new feature and not needing to touch `rclcpp::Node`. +- (dirk) methods and free functions not mutually exclusive. + +### Scoped Versus Non-Scoped Entities (e.g. Publishers/Subscriptions) + +> Currently, Publisher and Subscription (and similar entities) are scoped, meaning that when they are created they are added to the ROS graph as a side effect, and when they are let out of scope they remove themselves from the graph too. +> Additionally, they necessarily have shared state with the system, for instance when you are spinning on a node, the executor shares ownership of the Subscriptions with the user. +> Therefore, the Subscription only gets removed when both the user and executor are done with it. +> +> This shared ownership is accomplished with the use of shared pointers and weak pointers. +> +> There are a few concerns here, (a) use of shared pointers confuses users, (b) overhead of shared pointers and lack of an ability to use these classes on the stack rather than the heap, and (c) complexity of shutdown of an entity from the users perspective. +> +> For (a), some users are overwhelmed by the need to use a shared pointer. +> In ROS 1 this was avoided by having a class which itself just thinly wraps a shared pointer (see: https://github.com/ros/ros_comm/blob/ac9f88c59a676ca6895e13445fc7d71f398ebe1f/clients/roscpp/include/ros/subscriber.h#L108-L111). +> This could be achieved in ROS 2 either by doing the same with a wrapper class (at the expense of lots of repeated code), or by eliminating the need for using shared ownership. +> +> For (b), for some use cases, especially resource constrained / real-time / safety-critical environments, requiring these classes to be on the heap rather than the stack is at least inconvenient. +> Additionally, there is a cost associated with using shared pointers, in the storage of shared state and in some implementation the use of locks or at least atomics for thread-safety. +> +> For (c), this is the most concerning drawback, because right now when a user lets their shared pointer to a, for example, Subscription go out of scope, a post condition is not that the Subscription is destroyed, nor that it has been removed from the graph. +> In stead, the behavior is more like "at some point in the future the Subscription will be destroyed and removed from the graph, when the system is done with it". +> This isn't a very satisfactory contract, as some users may wish to know when the Subscription has been deleted, but cannot easily know that. +> +> The benefit to the shared state is a safety net for users. +> The alternative would be to document that a Subscription, again for example, cannot be deleted until the system is done with it. +> We'd basically be pushing the responsibility onto the user to ensure the shared ownership is handled properly by the execution of their application, i.e. they create the Subscription, share a reference with the system (adding it by reference to an executor, for example), and they have to make sure the system is done with it before deleting the Subscription. +> +>Separately, from the above points, there is the related concern of forcing the user to keep a copy of their entities in scope, whether it be with a shared pointer or a class wrapping one. +> There is the desire to create it and forget it in some cases. +> The downside to this is that if/when the user wants to destroy the entity, they have no way of doing that as they have no handle or unique way to address the entity. +> +> One proposed solution would be to have a set of "named X" APIs, e.g. `create_named_subscription` rather than just `create_subscription`. +> This would allow the user to address the Subscription in the future in order to obtain a new reference to it or delete it. + +- https://github.com/ros2/rclcpp/issues/506 +- https://github.com/ros2/rclcpp/issues/726 + +**Suggested Action**: Consolidate to a single issue, and defer. + +**Notes from 2020-03-23**: + +- (chris) Putting ownership mechanics on user is hard. +- (dirk) add documentation clearly outlining ownership +- (shane) warn on unused to catch issues with immediately deleted items +- (tfoote) debugging output for destruction so it easy to see when reviewing logs +- (chris) possible to create API that checks for destruction + - (william) might lead to complex synchronization issues +- (tfoote) could add helper classes to make scoped things non-scoped + - (shane) concerned that there is no longer "one good way" to do it + +### Allow QoS to be configured externally, like we allow remapping of topic names + +> Suggestion from @stonier: allow the qos setting on a topic to be changed externally at startup, similar to how we do topic remapping (e.g., do it on the command-line using appropriate syntax). +> +> To keep the syntax manageable, we might just allow profiles to be picked. + +- https://github.com/ros2/rclcpp/issues/239 + +**Suggested Action**: Update issue, defer for now. + +**Notes from 2020-03-19**: + +- (wjwwood) it depends on the QoS setting, but many don't make sense, mostly because they can change some of the behaviors of underlying API +- (dirk) Should developers expose a parameter instead? +- (multiple) should be a feature that makes configuring them (after opt-in) consistent +- (jacob) customers feedback was that this was expected, surprised it was not allowed +- (karsten) could limit to profiles + +## Init/shutdown and Context + +### Consider renaming `rclcpp::ok()` + +> Old discussion to rename `rclcpp::ok()` to something more specific, like `rclcpp::is_not_shutdown()` or the corollary `rclcpp::is_shutdown()`. + +- https://github.com/ros2/rclcpp/issues/3 + +**Suggested Action**: Defer. + +**Notes from 2020-03-19**: + +- (shane) preference to not have a negative in the function name + +## Executor + +### Exposing Scheduling of Tasks in Executor and a Better Default + +> Currently there is a hard coded procedure for handling ready tasks in the executor, first timers, then subscriptions, and so on. +> This scheduling is not fair and results in non-deterministic behavior and starvation issues. +> +> We should provide a better default scheduling which is fairer and ideally deterministic, something like round-robin or FIFO. +> +> Additionally, we should make it easier to let the user override the scheduling logic in the executor. + +- https://github.com/ros2/rclcpp/pull/614 +- https://github.com/ros2/rclcpp/issues/633 +- https://github.com/ros2/rclcpp/issues/392 + +**Suggested Action**: Follow up on proposals to implement FIFO scheduling and refactor the Executor design to more easily expose the scheduling logic. + +**Notes from 2020-03-19**: + +- No comments. + +### Make it possible to wait on entities (e.g. Subscriptions) without an Executor + +> Currently, it is only possible to use things like Timers and Subscriptions and Services with an executor. +> It should be possible, however, to either poll these entities or wait on them and then decide which to process as a user. +> +> This is most easily accomplished with a WaitSet-like class. + +- https://github.com/ros2/rclcpp/issues/520 + +**Suggested Action**: implement WaitSet class in rclcpp so that this is possible, and make "waitable" entities such that they can be polled, e.g. `Subscription`s should have a user facing `take()` method, which can fail if no data is available. + +**Notes from 2020-03-19**: + +- No comments. + +### Make it possible to use multiple executors per node + +> Currently, you cannot use more than one executor per node, this limits your options when it comes to distributing work within a node across threads. +> You can use a multi-threaded executor, or make your own executor which does this, but it is often convenient to be able to spin part of the node separately from the the rest of the node. + +- https://github.com/ros2/rclcpp/issues/519 + +**Suggested Action**: Make this possible, moving the exclusivity to be between an executor and callback groups rather than nodes. + +**Notes from 2020-03-19**: + +- No comments. + +### Implement a Lock-free Executor + +> This would presumably be useful for real-time and safety critical systems where locks and any kind of blocking code is considered undesirable. + +- https://github.com/ros2/rclcpp/issues/77 + +**Suggested Action**: Keep in backlog until someone needs it specifically. + +**Notes from 2020-03-19**: + +- No comments. + +### Add implementation of `spin_some()` to the `MultiThreadedExecutor` + +> Currently `spin_some()` is only available in the `SingleThreadedExecutor`. + +- https://github.com/ros2/rclcpp/issues/85 + +**Suggested Action**: Defer. + +**Notes from 2020-03-19**: + +- No comments. + +## Node + +### Do argument parsing outside of node constructor + +> Things that come from command line arguments should be separately passed into the node's constructor rather than passing in arguments and asking the node to do the parsing. + +- https://github.com/ros2/rclcpp/issues/492 + +**Suggested Action**: Defer until after foxy. + +**Notes from 2020-03-23**: + +- (dirk) may be related to ROS 1 heritage of argc/argv being passed to node directly +- (shane) impacts rcl API as well, two parts "global options" as well node specific options +- (dirk) what is the recommendation to users that want to add arguments programmatically + - user should be able to get non-ros argc/argv somehow (seems like you can now) +- (jacob) the argument in NodeOptions are used for application specific argument via component loading as well + +## Timer + +### Timer based on ROS Time + +> `node->create_wall_timer` does exactly what it says; creates a timer that will call the callback when the wall time expires. But this is almost never what the user wants, since this won’t work properly in simulation. Suggestion: deprecate `create_wall_timer`, add a new method called `create_timer` that takes the timer to use as one of the arguments, which defaults to ROS_TIME. + +- https://github.com/ros2/rclcpp/blob/96ebf59a6045a535730d98fff25e522807c7aa75/rclcpp/include/rclcpp/node.hpp#L219-L230 +- https://github.com/ros2/rclcpp/issues/465 + +**Suggested Action**: Promote `rclcpp::create_timer()` which is templated on a clock type, as suggested, but leave `create_wall_timer` as a convenience. + +**Notes from 2020-03-19**: + +- (shane) may be a `rclcpp::create_timer()` that can be used to create a non-wall timer already + +## Publisher + +## Subscription + +### Callback Signature + +> Is there a reason the subscription callback must have a smart pointer argument instead of accepting a const-reference argument? + +- https://github.com/ros2/rclcpp/blob/96ebf59a6045a535730d98fff25e522807c7aa75/rclcpp/include/rclcpp/any_subscription_callback.hpp#L44-L52 +- https://github.com/ros2/rclcpp/issues/281 + +**Suggested Action**: Provide const reference as an option, add documentation as to the implications of one callback signature versus others. + +**Notes from 2020-03-19**: + +- (dirk) have const reference and document it + +## Service Server + +### Allow for asynchronous Service Server callbacks + +> Currently, the only callback signature for Service Servers takes a request and must return a response. +> This means that all of the activity of the service server has to happen within that function. +> This can cause issues, specifically if you want to call another service within the current service server's callback, as it causes deadlock issues trying to synchronously call the second service within a spin callback. +> More generally, it seems likely that long running service server callbacks may be necessary in the future and requiring them to be synchronous would tie up at least on thread in the spinning executor unnecessarily. + +- https://github.com/ros2/rclcpp/issues/491 + +**Suggested Action**: Defer. + +**Notes from 2020-03-23**: + +- (dirk) likely new API, so possible to backport + +## Service Client + +### Callback has SharedFuture rather than const reference to response + +> Why does the Client::send_async_request take in a callback that has a SharedFuture argument instead of an argument that is simply a const-reference (or smart pointer) to the service response type? The current API seems to imply that the callback ought to check whether the promise is broken or fulfilled before trying to access it. Is that the case? If so, it should be documented in the header. + +- https://github.com/ros2/rclcpp/blob/7c1721a0b390be8242a6b824489d0bc861f6a0ad/rclcpp/include/rclcpp/client.hpp#L134 + +**Suggested Action**: Update ticket and defer. + +**Notes from 2020-03-19**: + +- (wjwwood) we wanted the user to handle error cases with the future? +- (dirk) future allows for single callback (rather than one for response and one for error) +- (jacob) actions uses a "wrapped result" object + +### rclcpp missing synchronous `send_request` and issues with deadlocks + +> This has been reported by several users, but there is only an `async_send_request` currently. `rclpy` has a synchronous `send_request` but it has issues with deadlock, specifically if you call it without spinning in another thread then it will deadlock. Or if you call it from within a spin callback when using a single threaded executor, it will deadlock. + +- https://discourse.ros.org/t/synchronous-request-to-service-in-callback-results-in-deadlock/12767 +- https://github.com/ros2/rclcpp/issues/975 +- https://github.com/ros2/demos/blob/948b4f4869298f39cfe99d3ae517ad60a72a8909/demo_nodes_cpp/src/services/add_two_ints_client.cpp#L24-L39 + +**Suggested Action**: Update issue and defer. Also defer decision on reconciling rclpy's send_request. + +**Notes from 2020-03-23**: + +- (karsten/shane) async spinner helps in rclpy version, rclcpp could emulate +- (chris) sees three options: + - only async (current case in rclcpp) + - have sync version, add lots of docs that spinning needs to happen elsewhere (current case for rclpy) + - reentrant spinning +- (william) you either need async/await from language or ".then" syntax (we have this in async_send_request()) +- (chris) more error checking for recursive spinning +- (chris) weird that rclcpp and rclpy have different API +- (shane) thinks it is ok to have different API, but rclpy is not ideal + +## Parameters + +### Expected vs Unexpected parameters + +> Allow node author to define expected parameters and what happens when an unexpected parameter is set. + +- https://github.com/ros2/rclcpp/issues/475 +- https://github.com/ros2/rclcpp/tree/check_parameters + +**Suggested Action**: Defer as nice to have. + +**Notes from 2020-03-23**: + +- None. + +### Implicitly cast integer values for double parameters + +> If we try to pass an integer value to a double parameter from the command line or from a parameters YAML file we get a `rclcpp::ParameterTypeException`. +> For example, passing a parameter from the command line: +> +> ros2 run foo_package foo_node --ros-args -p foo_arg:=1 +> +> results in the following error: +> +> terminate called after throwing an instance of 'rclcpp::ParameterTypeException' +> what(): expected [double] got [integer] +> +> and we can fix it by explicitly making our value a floating point number: +> +> ros2 run foo_package foo_node --ros-args -p foo_arg:=1.0 +> +> But, it seems reasonable to me that if a user forgets to explicitly provide a floating point value that we should implicitly cast an integer to a float (as is done in many programming languages). + +- https://github.com/ros2/rclcpp/issues/979 + +**Suggested Action**: Continue with issue. + +**Notes from 2020-03-23**: + +- (shane) says "yes please" :) + +### Use `std::variant` instead of custom `ParameterValue` class + +> This is only possible if C++17 is available, but it would simplify our code, make our interface more standard, and allow us to use constexpr-if to simply our templated code. + +**Suggested Action**: Create an issue for future work. + +**Notes from 2020-03-23**: + +- (chris) not sure churn is worth +- (ivan) other places for std::variant, like AnySubscriptionCallback + +### Cannot set name or value on `Parameter`/`ParameterValue` + +> Both `Parameter` and `ParameterValue` are read-only after construction. + +- https://github.com/ros2/rclcpp/issues/238 + +**Suggested Action**: Update issue, possibly close. + +**Notes from 2020-03-23**: + +- (chris/william) setting values on temporary (local) objects is not reflected in the node, so misleading + +## Parameter Clients + +### No timeout option with synchronous parameter client calls + +> As an example, SyncParametersClient::set_parameters doesn't take a timeout option. So, if anything goes wrong in the service call (e.g. the server goes down), we will get stuck waiting indefinitely. + +- https://github.com/ros2/rclcpp/issues/360 +- https://github.com/ros2/rclcpp/blob/96ebf59a6045a535730d98fff25e522807c7aa75/rclcpp/src/rclcpp/parameter_client.cpp#L453-L468 + +**Suggested Action**: Update issue, decide if it can be taken for Foxy or not. + +**Notes from 2020-03-23**: + +- (tfoote) Seems like adding a timeout is a good idea. + +### Name of AsyncParametersClient inconsistent + +> AsyncParameter**s**Client uses plural, when filename is singular (and ParameterService is singular): + +- https://github.com/ros2/rclcpp/blob/7c1721a0b390be8242a6b824489d0bc861f6a0ad/rclcpp/include/rclcpp/parameter_client.hpp#L44 + +**Suggested Action**: Reconcile class and file name, switch to singular name? + +**Notes from on-line, post 2020-03-23 meeting**: + +- (tfoote) +1 for homogenizing to singular + +### `SyncParametersClient::get_parameters` doesn't allow you to detect error cases + +> E.g. https://github.com/ros2/rclcpp/blob/249b7d80d8f677edcda05052f598de84f4c2181c/rclcpp/src/rclcpp/parameter_client.cpp#L246-L257 returns an empty vector if something goes wrong which is also a valid response. + +- https://github.com/ros2/rclcpp/issues/200 +- https://github.com/ros2/rclcpp/blob/96ebf59a6045a535730d98fff25e522807c7aa75/rclcpp/src/rclcpp/parameter_client.cpp#L412-L426 + +**Suggested Action**: Throw an exception to indicate if something went wrong and document other expected conditions of the API. + +**Notes from on-line, post 2020-03-23 meeting**: + +- (tfoote) An empty list is not a valid response unless you passed in an empty list. The return should have the same length as the request in the same order. Any parameters that are not set should return a ParameterVariant with type PARAMETER_NOT_SET. to indicate that it was polled and determined to not be set. Suggested action improve documentation of the API to clarify a short or incomplete. +- (jacobperron) I think throwing an exception is also a valid action, making it clear that an error occurred. +- (wjwwood) Using exceptions to indicate an exceptional case (something went wrong) seems reasonable to me too. + +## Clock + +### Clock Jump callbacks on System or Steady time? + +> Currently time jump callbacks are registered via Clock::create_jump_handler(). Jump handlers are only invoked by TimeSource::set_clock(). This is only called if the clock type is RCL_ROS_TIME and ROS time is active. + +- https://github.com/ros2/rclcpp/issues/528 + +**Suggested Action**: Document that time jumping is only detected with ROS time, consider a warning. + +**Notes from on-line, post 2020-03-23 meeting**: + +- (tfoote) There should be no jumps in steady time. If there's a big change in system time, it doesn't necessarily mean that time jumped, just that you might have been sleeping for a long time. Most ntp systems adjust the slew rate these days instead of jumping but still that's an external process and I don't know of any APIs to introspect the state of the clock. I'm not sure that we have a way to detect jumps in time for system or steady time. To that end I think that we should be clear that we only provide callbacks when simulation time starts or stops, or simulation time jumps. We should also strongly recommend that operators not actively adjust their system clocks while running ROS nodes. +- (jacobperron) I agree with Tully, if we don't have a way to detect system time jumps then I think we should just document that this only works with ROS time. In addition to documentation, we could log an info or warning message if the user registers jump callback with steady or system time, but it may be unnecessarily noisy. + diff --git a/rclcpp/doc/notes_on_statically_typed_parameters.md b/rclcpp/doc/notes_on_statically_typed_parameters.md new file mode 100644 index 0000000000..b96866c639 --- /dev/null +++ b/rclcpp/doc/notes_on_statically_typed_parameters.md @@ -0,0 +1,141 @@ +# Notes on statically typed parameters + +## Introduction + +Until ROS 2 Foxy, all parameters could change type anytime, except if the user installed a parameter callback to reject a change. +This could generate confusing errors, for example: + +``` +$ ros2 run demo_nodes_py listener & +$ ros2 param set /listener use_sim_time not_a_boolean +[ERROR] [1614712713.233733147] [listener]: use_sim_time parameter set to something besides a bool +Set parameter successful +$ ros2 param get /listener use_sim_time +String value is: not_a_boolean +``` + +For most use cases, having static parameter types is enough. +This article documents some of the decisions that were made when implementing static parameter types enforcement in: + +* https://github.com/ros2/rclcpp/pull/1522 +* https://github.com/ros2/rclpy/pull/683 + +## Allowing dynamically typed parameters + +There might be some scenarios where dynamic typing is desired, so a `dynamic_typing` field was added to the [parameter descriptor](https://github.com/ros2/rcl_interfaces/blob/09b5ed93a733adb9deddc47f9a4a8c6e9f584667/rcl_interfaces/msg/ParameterDescriptor.msg#L25). +It defaults to `false`. + +For example: + +```cpp +rcl_interfaces::msg::ParameterDescriptor descriptor; +descriptor.dynamic_typing = true; + +node->declare_parameter("dynamically_typed_parameter", rclcpp::ParameterValue{}, descriptor); +``` + +```py +rcl_interfaces.msg.ParameterDescriptor descriptor; +descriptor.dynamic_typing = True; + +node.declare_parameter("dynamically_typed_parameter", None, descriptor); +``` + +## How is the parameter type specified? + +The parameter type will be inferred from the default value provided when declaring it. + +## Statically typed parameters when allowing undeclared parameters + +When undeclared parameters are allowed and a parameter is set without a previous declaration, the parameter will be dynamically typed. +This is consistent with other allowed behaviors when undeclared parameters are allowed, e.g. trying to get an undeclared parameter returns "NOT_SET". +Parameter declarations will remain special and dynamic or static typing will be used based on the parameter descriptor (default to static). + +## Declaring a parameter without a default value + +There might be cases were a default value does not make sense and the user must always provide an override. +In those cases, the parameter type can be specified explicitly: + +```cpp +// method signature +template +Node::declare_parameter(std::string name, rcl_interfaces::msg::ParameterDescriptor = rcl_interfaces::msg::ParameterDescriptor{}); +// or alternatively +Node::declare_parameter(std::string name, rclcpp::ParameterType type, rcl_interfaces::msg::ParameterDescriptor = rcl_interfaces::msg::ParameterDescriptor{}); + +// examples +node->declare_parameter("my_integer_parameter"); // declare an integer parameter +node->declare_parameter("another_integer_parameter", rclcpp::ParameterType::PARAMETER_INTEGER); // another way to do the same +``` + +```py +# method signature +Node.declare_parameter(name: str, param_type: rclpy.Parameter.Type, descriptor: rcl_interfaces.msg.ParameterDescriptor = rcl_interfaces.msg.ParameterDescriptor()) + +# example +node.declare_parameter('my_integer_parameter', rclpy.Parameter.Type.INTEGER); # declare an integer parameter +``` + +If the parameter may be unused, but when used requires a parameter override, then you could conditionally declare it: + +```cpp +auto mode = node->declare_parameter("mode", "modeA"); // "mode" parameter is an string +if (mode == "modeB") { + node->declare_parameter("param_needed_when_mode_b"); // when "modeB", the user must provide "param_needed_when_mode_b" +} +``` + +## Other migration notes + +Declaring a parameter with only a name is deprecated: + +```cpp +node->declare_parameter("my_param"); // this generates a build warning +``` + +```py +node.declare_parameter("my_param"); # this generates a python user warning +``` + +Before, you could initialize a parameter with the "NOT SET" value (in cpp `rclcpp::ParameterValue{}`, in python `None`). +Now this will throw an exception in both cases: + +```cpp +node->declare_parameter("my_param", rclcpp::ParameterValue{}); // not valid, will throw exception +``` + +```py +node.declare_parameter("my_param", None); # not valid, will raise error +``` + +## Possible improvements + +### Easier way to declare dynamically typed parameters + +Declaring a dynamically typed parameter in `rclcpp` could be considered to be a bit verbose: + +```cpp +rcl_interfaces::msg::ParameterDescriptor descriptor; +descriptor.dynamic_typing = true; + +node->declare_parameter(name, rclcpp::ParameterValue{}, descriptor); +``` + +the following ways could be supported to make it simpler: + +```cpp +node->declare_parameter(name, rclcpp::PARAMETER_DYNAMIC); +node->declare_parameter(name, default_value, rclcpp::PARAMETER_DYNAMIC); +``` + +or alternatively: + +```cpp +node->declare_parameter(name, default_value, rclcpp::ParameterDescriptor{}.dynamic_typing()); +``` + +For `rclpy`, there's already a short way to do it: + +```py +node.declare_parameter(name, default_value, rclpy.ParameterDescriptor(dynamic_typing=true)); +``` diff --git a/rclcpp/doc/param_callback_design.png b/rclcpp/doc/param_callback_design.png new file mode 100644 index 0000000000..485b5b9532 Binary files /dev/null and b/rclcpp/doc/param_callback_design.png differ diff --git a/rclcpp/doc/proposed_node_parameter_callbacks.md b/rclcpp/doc/proposed_node_parameter_callbacks.md new file mode 100644 index 0000000000..cd39f352db --- /dev/null +++ b/rclcpp/doc/proposed_node_parameter_callbacks.md @@ -0,0 +1,29 @@ +# Proposed node parameters callback Design + +## Introduction: + +The original requirement came in **gazebo_ros_pkgs** for setting individual wheel slip parameters based on global wheel slip value [link to original issue](https://github.com/ros-simulation/gazebo_ros_pkgs/pull/1365). + +The main requirement is to set one or more parameters after another parameter is set successfully. + +Additionally, it would be nice if users could be notified locally (via a callback) when parameters have been set successfully (i.e. post validation). + +Related discussion can be found in [#609](https://github.com/ros2/rclcpp/issues/609) [#1789](https://github.com/ros2/rclcpp/pull/1789) + +With the current parameters API, the `add_on_set_parameters_callback` is intended for validation of parameter values before they are set, it should **not** cause any side-effects. + +There is also the `ParameterEventHandler` that publishes changes to node parameters on `/parameter_events` topic for external nodes to see. Though the node could subscribe to the `/parameter_events` topic to be notified of changes to its own parameters, it is less than ideal since there is a delay caused by waiting for an executor to process the callback. + +We propose adding a `PostSetParametersCallbackHandle` for successful parameter set similar to `OnSetParametersCallbackHandle` for parameter validation. Also, we propose adding a `PreSetParametersCallbackHandle` useful for modifying list of parameters being set. + +The validation callback is often abused to trigger side effects in the code, for instance updating class attributes even before a parameter has been set successfully. Instead of relying on the `/parameter_events` topic to be notified of parameter changes, users can register a callback with a new API, `add_post_set_parameters_callback`. + +It is possible to use the proposed `add_post_set_parameters_callback` for setting additional parameters, but this might result in infinite recursion and does not allow those additional parameters to be set atomically with the original parameter(s) changed. +To workaround these issues, we propose adding a "pre set" callback type that can be registered with `add_pre_set_parameters_callback`, which will be triggered before the validation callbacks and can be used to modify the parameter list. + +![Desgin API](https://github.com/ros2/rclcpp/blob/deepanshu/local-param-changed-callback-support/rclcpp/doc/param_callback_design.png?raw=true) + +## Alternatives + +* Users could call `set_parameter` while processing a message from the `/parameter_events` topic, however, there is extra overhead in having to create subscription (as noted earlier). +* Users could call `set_parameter` inside the "on set" parameters callback, however it is not well-defined how side-effects should handle cases where parameter validation fails. \ No newline at end of file diff --git a/rclcpp/include/rclcpp/allocator/allocator_common.hpp b/rclcpp/include/rclcpp/allocator/allocator_common.hpp index 1e60f7faf6..12b2f383b6 100644 --- a/rclcpp/include/rclcpp/allocator/allocator_common.hpp +++ b/rclcpp/include/rclcpp/allocator/allocator_common.hpp @@ -15,6 +15,7 @@ #ifndef RCLCPP__ALLOCATOR__ALLOCATOR_COMMON_HPP_ #define RCLCPP__ALLOCATOR__ALLOCATOR_COMMON_HPP_ +#include #include #include "rcl/allocator.h" @@ -39,6 +40,22 @@ void * retyped_allocate(size_t size, void * untyped_allocator) return std::allocator_traits::allocate(*typed_allocator, size); } +template +void * retyped_zero_allocate(size_t number_of_elem, size_t size_of_elem, void * untyped_allocator) +{ + auto typed_allocator = static_cast(untyped_allocator); + if (!typed_allocator) { + throw std::runtime_error("Received incorrect allocator type"); + } + size_t size = number_of_elem * size_of_elem; + void * allocated_memory = + std::allocator_traits::allocate(*typed_allocator, size); + if (allocated_memory) { + std::memset(allocated_memory, 0, size); + } + return allocated_memory; +} + template void retyped_deallocate(void * untyped_pointer, void * untyped_allocator) { @@ -65,13 +82,15 @@ void * retyped_reallocate(void * untyped_pointer, size_t size, void * untyped_al // Convert a std::allocator_traits-formatted Allocator into an rcl allocator template< - typename T, typename Alloc, + typename T, + typename Alloc, typename std::enable_if>::value>::type * = nullptr> rcl_allocator_t get_rcl_allocator(Alloc & allocator) { rcl_allocator_t rcl_allocator = rcl_get_default_allocator(); #ifndef _WIN32 rcl_allocator.allocate = &retyped_allocate; + rcl_allocator.zero_allocate = &retyped_zero_allocate; rcl_allocator.deallocate = &retyped_deallocate; rcl_allocator.reallocate = &retyped_reallocate; rcl_allocator.state = &allocator; @@ -83,7 +102,8 @@ rcl_allocator_t get_rcl_allocator(Alloc & allocator) // TODO(jacquelinekay) Workaround for an incomplete implementation of std::allocator template< - typename T, typename Alloc, + typename T, + typename Alloc, typename std::enable_if>::value>::type * = nullptr> rcl_allocator_t get_rcl_allocator(Alloc & allocator) { diff --git a/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp b/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp index ff79c4009c..73fbc7b9f7 100644 --- a/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp +++ b/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp @@ -41,7 +41,7 @@ class AllocatorDeleter } template - AllocatorDeleter(const AllocatorDeleter & a) + explicit AllocatorDeleter(const AllocatorDeleter & a) { allocator_ = a.get_allocator(); } @@ -94,11 +94,11 @@ void set_allocator_for_deleter(AllocatorDeleter * deleter, Alloc * alloc) template using Deleter = typename std::conditional< - std::is_same::template rebind_alloc, - typename std::allocator::template rebind::other>::value, - std::default_delete, - AllocatorDeleter - >::type; + std::is_same::template rebind_alloc, + std::allocator>::value, + std::default_delete, + AllocatorDeleter + >::type; } // namespace allocator } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/any_executable.hpp b/rclcpp/include/rclcpp/any_executable.hpp index 4820b4b8d4..e4e9eaecb0 100644 --- a/rclcpp/include/rclcpp/any_executable.hpp +++ b/rclcpp/include/rclcpp/any_executable.hpp @@ -25,16 +25,13 @@ #include "rclcpp/subscription.hpp" #include "rclcpp/timer.hpp" #include "rclcpp/visibility_control.hpp" +#include "rclcpp/waitable.hpp" namespace rclcpp { -namespace executor -{ struct AnyExecutable { - RCLCPP_SMART_PTR_DEFINITIONS(AnyExecutable) - RCLCPP_PUBLIC AnyExecutable(); @@ -43,16 +40,16 @@ struct AnyExecutable // Only one of the following pointers will be set. rclcpp::SubscriptionBase::SharedPtr subscription; - rclcpp::SubscriptionBase::SharedPtr subscription_intra_process; rclcpp::TimerBase::SharedPtr timer; rclcpp::ServiceBase::SharedPtr service; rclcpp::ClientBase::SharedPtr client; + rclcpp::Waitable::SharedPtr waitable; // These are used to keep the scope on the containing items - rclcpp::callback_group::CallbackGroup::SharedPtr callback_group; - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base; + rclcpp::CallbackGroup::SharedPtr callback_group {nullptr}; + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base {nullptr}; + std::shared_ptr data {nullptr}; }; -} // namespace executor } // namespace rclcpp #endif // RCLCPP__ANY_EXECUTABLE_HPP_ diff --git a/rclcpp/include/rclcpp/any_service_callback.hpp b/rclcpp/include/rclcpp/any_service_callback.hpp index 598e0ef8ab..5d6f3ee7b7 100644 --- a/rclcpp/include/rclcpp/any_service_callback.hpp +++ b/rclcpp/include/rclcpp/any_service_callback.hpp @@ -15,86 +15,226 @@ #ifndef RCLCPP__ANY_SERVICE_CALLBACK_HPP_ #define RCLCPP__ANY_SERVICE_CALLBACK_HPP_ +#include #include #include #include #include +#include #include "rclcpp/function_traits.hpp" #include "rclcpp/visibility_control.hpp" #include "rmw/types.h" +#include "tracetools/tracetools.h" +#include "tracetools/utils.hpp" namespace rclcpp { -template -class AnyServiceCallback +namespace detail { -private: - using SharedPtrCallback = std::function< - void( - const std::shared_ptr, - std::shared_ptr - )>; - using SharedPtrWithRequestHeaderCallback = std::function< - void( - const std::shared_ptr, - const std::shared_ptr, - std::shared_ptr - )>; +template +struct can_be_nullptr : std::false_type {}; + +// Some lambdas define a comparison with nullptr, +// but we see a warning that they can never be null when using it. +// We also test if `T &` can be assigned to `nullptr` to avoid the issue. +template +#ifdef __QNXNTO__ +struct can_be_nullptr() == nullptr)>>: std::true_type {}; +#else +struct can_be_nullptr() == nullptr), decltype(std::declval() = nullptr)>> + : std::true_type {}; +#endif +} // namespace detail - SharedPtrCallback shared_ptr_callback_; - SharedPtrWithRequestHeaderCallback shared_ptr_with_request_header_callback_; +// Forward declare +template +class Service; +template +class AnyServiceCallback +{ public: AnyServiceCallback() - : shared_ptr_callback_(nullptr), shared_ptr_with_request_header_callback_(nullptr) + : callback_(std::monostate{}) {} - AnyServiceCallback(const AnyServiceCallback &) = default; - template< typename CallbackT, - typename std::enable_if< + typename std::enable_if_t::value, int> = 0> + void + set(CallbackT && callback) + { + // Workaround Windows issue with std::bind + if constexpr ( rclcpp::function_traits::same_arguments< CallbackT, SharedPtrCallback - >::value - >::type * = nullptr - > - void set(CallbackT callback) - { - shared_ptr_callback_ = callback; + >::value) + { + callback_.template emplace(callback); + } else if constexpr ( // NOLINT, can't satisfy both cpplint and uncrustify + rclcpp::function_traits::same_arguments< + CallbackT, + SharedPtrWithRequestHeaderCallback + >::value) + { + callback_.template emplace(callback); + } else if constexpr ( // NOLINT + rclcpp::function_traits::same_arguments< + CallbackT, + SharedPtrDeferResponseCallback + >::value) + { + callback_.template emplace(callback); + } else if constexpr ( // NOLINT + rclcpp::function_traits::same_arguments< + CallbackT, + SharedPtrDeferResponseCallbackWithServiceHandle + >::value) + { + callback_.template emplace(callback); + } else { + // the else clause is not needed, but anyways we should only be doing this instead + // of all the above workaround ... + callback_ = std::forward(callback); + } } template< typename CallbackT, - typename std::enable_if< + typename std::enable_if_t::value, int> = 0> + void + set(CallbackT && callback) + { + if (!callback) { + throw std::invalid_argument("AnyServiceCallback::set(): callback cannot be nullptr"); + } + // Workaround Windows issue with std::bind + if constexpr ( + rclcpp::function_traits::same_arguments< + CallbackT, + SharedPtrCallback + >::value) + { + callback_.template emplace(callback); + } else if constexpr ( // NOLINT rclcpp::function_traits::same_arguments< CallbackT, SharedPtrWithRequestHeaderCallback - >::value - >::type * = nullptr - > - void set(CallbackT callback) - { - shared_ptr_with_request_header_callback_ = callback; + >::value) + { + callback_.template emplace(callback); + } else if constexpr ( // NOLINT + rclcpp::function_traits::same_arguments< + CallbackT, + SharedPtrDeferResponseCallback + >::value) + { + callback_.template emplace(callback); + } else if constexpr ( // NOLINT + rclcpp::function_traits::same_arguments< + CallbackT, + SharedPtrDeferResponseCallbackWithServiceHandle + >::value) + { + callback_.template emplace(callback); + } else { + // the else clause is not needed, but anyways we should only be doing this instead + // of all the above workaround ... + callback_ = std::forward(callback); + } } - void dispatch( - std::shared_ptr request_header, - std::shared_ptr request, - std::shared_ptr response) + // template> + std::shared_ptr + dispatch( + const std::shared_ptr> & service_handle, + const std::shared_ptr & request_header, + std::shared_ptr request) { - if (shared_ptr_callback_ != nullptr) { + TRACETOOLS_TRACEPOINT(callback_start, static_cast(this), false); + if (std::holds_alternative(callback_)) { + // TODO(ivanpauno): Remove the set method, and force the users of this class + // to pass a callback at construnciton. + throw std::runtime_error{"unexpected request without any callback set"}; + } + if (std::holds_alternative(callback_)) { + const auto & cb = std::get(callback_); + cb(request_header, std::move(request)); + TRACETOOLS_TRACEPOINT(callback_end, static_cast(this)); + return nullptr; + } + if (std::holds_alternative(callback_)) { + const auto & cb = std::get(callback_); + cb(service_handle, request_header, std::move(request)); + TRACETOOLS_TRACEPOINT(callback_end, static_cast(this)); + return nullptr; + } + // auto response = allocate_shared(); + auto response = std::make_shared(); + if (std::holds_alternative(callback_)) { (void)request_header; - shared_ptr_callback_(request, response); - } else if (shared_ptr_with_request_header_callback_ != nullptr) { - shared_ptr_with_request_header_callback_(request_header, request, response); - } else { - throw std::runtime_error("unexpected request without any callback set"); + const auto & cb = std::get(callback_); + cb(std::move(request), response); + } else if (std::holds_alternative(callback_)) { + const auto & cb = std::get(callback_); + cb(request_header, std::move(request), response); } + TRACETOOLS_TRACEPOINT(callback_end, static_cast(this)); + return response; + } + + void register_callback_for_tracing() + { +#ifndef TRACETOOLS_DISABLED + std::visit( + [this](auto && arg) { + if (TRACETOOLS_TRACEPOINT_ENABLED(rclcpp_callback_register)) { + char * symbol = tracetools::get_symbol(arg); + TRACETOOLS_DO_TRACEPOINT( + rclcpp_callback_register, + static_cast(this), + symbol); + std::free(symbol); + } + }, callback_); +#endif // TRACETOOLS_DISABLED } + +private: + using SharedPtrCallback = std::function< + void ( + std::shared_ptr, + std::shared_ptr + )>; + using SharedPtrWithRequestHeaderCallback = std::function< + void ( + std::shared_ptr, + std::shared_ptr, + std::shared_ptr + )>; + using SharedPtrDeferResponseCallback = std::function< + void ( + std::shared_ptr, + std::shared_ptr + )>; + using SharedPtrDeferResponseCallbackWithServiceHandle = std::function< + void ( + std::shared_ptr>, + std::shared_ptr, + std::shared_ptr + )>; + + std::variant< + std::monostate, + SharedPtrCallback, + SharedPtrWithRequestHeaderCallback, + SharedPtrDeferResponseCallback, + SharedPtrDeferResponseCallbackWithServiceHandle> callback_; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/any_subscription_callback.hpp b/rclcpp/include/rclcpp/any_subscription_callback.hpp index 5850126548..da5abe6c53 100644 --- a/rclcpp/include/rclcpp/any_subscription_callback.hpp +++ b/rclcpp/include/rclcpp/any_subscription_callback.hpp @@ -15,195 +15,970 @@ #ifndef RCLCPP__ANY_SUBSCRIPTION_CALLBACK_HPP_ #define RCLCPP__ANY_SUBSCRIPTION_CALLBACK_HPP_ -#include - #include #include #include #include #include +#include + +#include "rosidl_runtime_cpp/traits.hpp" +#include "tracetools/tracetools.h" +#include "tracetools/utils.hpp" #include "rclcpp/allocator/allocator_common.hpp" +#include "rclcpp/detail/subscription_callback_type_helper.hpp" #include "rclcpp/function_traits.hpp" -#include "rclcpp/visibility_control.hpp" +#include "rclcpp/message_info.hpp" +#include "rclcpp/serialization.hpp" +#include "rclcpp/serialized_message.hpp" +#include "rclcpp/type_adapter.hpp" namespace rclcpp { -template -class AnySubscriptionCallback +namespace detail { - using MessageAllocTraits = allocator::AllocRebind; - using MessageAlloc = typename MessageAllocTraits::allocator_type; - using MessageDeleter = allocator::Deleter; - using MessageUniquePtr = std::unique_ptr; - using SharedPtrCallback = std::function)>; +template +inline constexpr bool always_false_v = false; + +template +struct MessageDeleterHelper +{ + using AllocTraits = allocator::AllocRebind; + using Alloc = typename AllocTraits::allocator_type; + using Deleter = allocator::Deleter; +}; + +/// Struct which contains all possible callback signatures, with or without a TypeAdapter.s +template +struct AnySubscriptionCallbackPossibleTypes +{ + /// MessageT::custom_type if MessageT is a TypeAdapter, otherwise just MessageT. + using SubscribedType = typename rclcpp::TypeAdapter::custom_type; + /// MessageT::ros_message_type if MessageT is a TypeAdapter, otherwise just MessageT. + using ROSMessageType = typename rclcpp::TypeAdapter::ros_message_type; + + using SubscribedMessageDeleter = + typename MessageDeleterHelper::Deleter; + using ROSMessageDeleter = + typename MessageDeleterHelper::Deleter; + using SerializedMessageDeleter = + typename MessageDeleterHelper::Deleter; + + using ConstRefCallback = + std::function; + using ConstRefROSMessageCallback = + std::function; + using ConstRefWithInfoCallback = + std::function; + using ConstRefWithInfoROSMessageCallback = + std::function; + using ConstRefSerializedMessageCallback = + std::function; + using ConstRefSerializedMessageWithInfoCallback = + std::function; + + using UniquePtrCallback = + std::function)>; + using UniquePtrROSMessageCallback = + std::function)>; + using UniquePtrWithInfoCallback = + std::function, + const rclcpp::MessageInfo &)>; + using UniquePtrWithInfoROSMessageCallback = + std::function, + const rclcpp::MessageInfo &)>; + using UniquePtrSerializedMessageCallback = + std::function)>; + using UniquePtrSerializedMessageWithInfoCallback = + std::function, + const rclcpp::MessageInfo &)>; + + using SharedConstPtrCallback = + std::function)>; + using SharedConstPtrROSMessageCallback = + std::function)>; + using SharedConstPtrWithInfoCallback = + std::function, + const rclcpp::MessageInfo &)>; + using SharedConstPtrWithInfoROSMessageCallback = + std::function, + const rclcpp::MessageInfo &)>; + using SharedConstPtrSerializedMessageCallback = + std::function)>; + using SharedConstPtrSerializedMessageWithInfoCallback = + std::function, + const rclcpp::MessageInfo &)>; + + using ConstRefSharedConstPtrCallback = + std::function &)>; + using ConstRefSharedConstPtrROSMessageCallback = + std::function &)>; + using ConstRefSharedConstPtrWithInfoCallback = + std::function &, + const rclcpp::MessageInfo &)>; + using ConstRefSharedConstPtrWithInfoROSMessageCallback = + std::function &, + const rclcpp::MessageInfo &)>; + using ConstRefSharedConstPtrSerializedMessageCallback = + std::function &)>; + using ConstRefSharedConstPtrSerializedMessageWithInfoCallback = + std::function &, + const rclcpp::MessageInfo &)>; + + // Deprecated signatures: + using SharedPtrCallback = + std::function)>; + using SharedPtrROSMessageCallback = + std::function)>; using SharedPtrWithInfoCallback = - std::function, const rmw_message_info_t &)>; - using ConstSharedPtrCallback = std::function)>; - using ConstSharedPtrWithInfoCallback = - std::function, const rmw_message_info_t &)>; - using UniquePtrCallback = std::function; + std::function, const rclcpp::MessageInfo &)>; + using SharedPtrWithInfoROSMessageCallback = + std::function, + const rclcpp::MessageInfo &)>; + using SharedPtrSerializedMessageCallback = + std::function)>; + using SharedPtrSerializedMessageWithInfoCallback = + std::function, const rclcpp::MessageInfo &)>; +}; + +/// Template helper to select the variant type based on whether or not MessageT is a TypeAdapter. +template< + typename MessageT, + typename AllocatorT, + bool is_adapted_type = rclcpp::TypeAdapter::is_specialized::value, + bool is_serialized_type = serialization_traits::is_serialized_message_class::value +> +struct AnySubscriptionCallbackHelper; + +/// Specialization for when MessageT is not a TypeAdapter. +template +struct AnySubscriptionCallbackHelper +{ + using CallbackTypes = AnySubscriptionCallbackPossibleTypes; + + using variant_type = std::variant< + typename CallbackTypes::ConstRefCallback, + typename CallbackTypes::ConstRefWithInfoCallback, + typename CallbackTypes::ConstRefSerializedMessageCallback, + typename CallbackTypes::ConstRefSerializedMessageWithInfoCallback, + typename CallbackTypes::UniquePtrCallback, + typename CallbackTypes::UniquePtrWithInfoCallback, + typename CallbackTypes::UniquePtrSerializedMessageCallback, + typename CallbackTypes::UniquePtrSerializedMessageWithInfoCallback, + typename CallbackTypes::SharedConstPtrCallback, + typename CallbackTypes::SharedConstPtrWithInfoCallback, + typename CallbackTypes::SharedConstPtrSerializedMessageCallback, + typename CallbackTypes::SharedConstPtrSerializedMessageWithInfoCallback, + typename CallbackTypes::ConstRefSharedConstPtrCallback, + typename CallbackTypes::ConstRefSharedConstPtrWithInfoCallback, + typename CallbackTypes::ConstRefSharedConstPtrSerializedMessageCallback, + typename CallbackTypes::ConstRefSharedConstPtrSerializedMessageWithInfoCallback, + typename CallbackTypes::SharedPtrCallback, + typename CallbackTypes::SharedPtrWithInfoCallback, + typename CallbackTypes::SharedPtrSerializedMessageCallback, + typename CallbackTypes::SharedPtrSerializedMessageWithInfoCallback + >; +}; + +/// Specialization for when MessageT is a TypeAdapter. +template +struct AnySubscriptionCallbackHelper +{ + using CallbackTypes = AnySubscriptionCallbackPossibleTypes; + + using variant_type = std::variant< + typename CallbackTypes::ConstRefCallback, + typename CallbackTypes::ConstRefROSMessageCallback, + typename CallbackTypes::ConstRefWithInfoCallback, + typename CallbackTypes::ConstRefWithInfoROSMessageCallback, + typename CallbackTypes::ConstRefSerializedMessageCallback, + typename CallbackTypes::ConstRefSerializedMessageWithInfoCallback, + typename CallbackTypes::UniquePtrCallback, + typename CallbackTypes::UniquePtrROSMessageCallback, + typename CallbackTypes::UniquePtrWithInfoCallback, + typename CallbackTypes::UniquePtrWithInfoROSMessageCallback, + typename CallbackTypes::UniquePtrSerializedMessageCallback, + typename CallbackTypes::UniquePtrSerializedMessageWithInfoCallback, + typename CallbackTypes::SharedConstPtrCallback, + typename CallbackTypes::SharedConstPtrROSMessageCallback, + typename CallbackTypes::SharedConstPtrWithInfoCallback, + typename CallbackTypes::SharedConstPtrWithInfoROSMessageCallback, + typename CallbackTypes::SharedConstPtrSerializedMessageCallback, + typename CallbackTypes::SharedConstPtrSerializedMessageWithInfoCallback, + typename CallbackTypes::ConstRefSharedConstPtrCallback, + typename CallbackTypes::ConstRefSharedConstPtrROSMessageCallback, + typename CallbackTypes::ConstRefSharedConstPtrWithInfoCallback, + typename CallbackTypes::ConstRefSharedConstPtrWithInfoROSMessageCallback, + typename CallbackTypes::ConstRefSharedConstPtrSerializedMessageCallback, + typename CallbackTypes::ConstRefSharedConstPtrSerializedMessageWithInfoCallback, + typename CallbackTypes::SharedPtrCallback, + typename CallbackTypes::SharedPtrROSMessageCallback, + typename CallbackTypes::SharedPtrWithInfoCallback, + typename CallbackTypes::SharedPtrWithInfoROSMessageCallback, + typename CallbackTypes::SharedPtrSerializedMessageCallback, + typename CallbackTypes::SharedPtrSerializedMessageWithInfoCallback + >; +}; + +/// Specialization for when MessageT is a SerializedMessage to exclude duplicated declarations. +template +struct AnySubscriptionCallbackHelper +{ + using CallbackTypes = AnySubscriptionCallbackPossibleTypes; + + using variant_type = std::variant< + typename CallbackTypes::ConstRefSerializedMessageCallback, + typename CallbackTypes::ConstRefSerializedMessageWithInfoCallback, + typename CallbackTypes::UniquePtrSerializedMessageCallback, + typename CallbackTypes::UniquePtrSerializedMessageWithInfoCallback, + typename CallbackTypes::SharedConstPtrSerializedMessageCallback, + typename CallbackTypes::SharedConstPtrSerializedMessageWithInfoCallback, + typename CallbackTypes::ConstRefSharedConstPtrSerializedMessageCallback, + typename CallbackTypes::ConstRefSharedConstPtrSerializedMessageWithInfoCallback, + typename CallbackTypes::SharedPtrSerializedMessageCallback, + typename CallbackTypes::SharedPtrSerializedMessageWithInfoCallback + >; +}; + +} // namespace detail + +template< + typename MessageT, + typename AllocatorT = std::allocator +> +class AnySubscriptionCallback +{ +private: + /// MessageT::custom_type if MessageT is a TypeAdapter, otherwise just MessageT. + using SubscribedType = typename rclcpp::TypeAdapter::custom_type; + /// MessageT::ros_message_type if MessageT is a TypeAdapter, otherwise just MessageT. + using ROSMessageType = typename rclcpp::TypeAdapter::ros_message_type; + + using HelperT = typename rclcpp::detail::AnySubscriptionCallbackHelper; + + using SubscribedTypeDeleterHelper = + rclcpp::detail::MessageDeleterHelper; + using SubscribedTypeAllocatorTraits = typename SubscribedTypeDeleterHelper::AllocTraits; + using SubscribedTypeAllocator = typename SubscribedTypeDeleterHelper::Alloc; + using SubscribedTypeDeleter = typename SubscribedTypeDeleterHelper::Deleter; + + using ROSMessageTypeDeleterHelper = + rclcpp::detail::MessageDeleterHelper; + using ROSMessageTypeAllocatorTraits = typename ROSMessageTypeDeleterHelper::AllocTraits; + using ROSMessageTypeAllocator = typename ROSMessageTypeDeleterHelper::Alloc; + using ROSMessageTypeDeleter = typename ROSMessageTypeDeleterHelper::Deleter; + + using SerializedMessageDeleterHelper = + rclcpp::detail::MessageDeleterHelper; + using SerializedMessageAllocatorTraits = typename SerializedMessageDeleterHelper::AllocTraits; + using SerializedMessageAllocator = typename SerializedMessageDeleterHelper::Alloc; + using SerializedMessageDeleter = typename SerializedMessageDeleterHelper::Deleter; + + // See AnySubscriptionCallbackPossibleTypes for the types of these. + using CallbackTypes = detail::AnySubscriptionCallbackPossibleTypes; + + using ConstRefCallback = + typename CallbackTypes::ConstRefCallback; + using ConstRefROSMessageCallback = + typename CallbackTypes::ConstRefROSMessageCallback; + using ConstRefWithInfoCallback = + typename CallbackTypes::ConstRefWithInfoCallback; + using ConstRefWithInfoROSMessageCallback = + typename CallbackTypes::ConstRefWithInfoROSMessageCallback; + using ConstRefSerializedMessageCallback = + typename CallbackTypes::ConstRefSerializedMessageCallback; + using ConstRefSerializedMessageWithInfoCallback = + typename CallbackTypes::ConstRefSerializedMessageWithInfoCallback; + using UniquePtrCallback = + typename CallbackTypes::UniquePtrCallback; + using UniquePtrROSMessageCallback = + typename CallbackTypes::UniquePtrROSMessageCallback; using UniquePtrWithInfoCallback = - std::function; + typename CallbackTypes::UniquePtrWithInfoCallback; + using UniquePtrWithInfoROSMessageCallback = + typename CallbackTypes::UniquePtrWithInfoROSMessageCallback; + using UniquePtrSerializedMessageCallback = + typename CallbackTypes::UniquePtrSerializedMessageCallback; + using UniquePtrSerializedMessageWithInfoCallback = + typename CallbackTypes::UniquePtrSerializedMessageWithInfoCallback; + using SharedConstPtrCallback = + typename CallbackTypes::SharedConstPtrCallback; + using SharedConstPtrROSMessageCallback = + typename CallbackTypes::SharedConstPtrROSMessageCallback; + using SharedConstPtrWithInfoCallback = + typename CallbackTypes::SharedConstPtrWithInfoCallback; + using SharedConstPtrWithInfoROSMessageCallback = + typename CallbackTypes::SharedConstPtrWithInfoROSMessageCallback; + using SharedConstPtrSerializedMessageCallback = + typename CallbackTypes::SharedConstPtrSerializedMessageCallback; + using SharedConstPtrSerializedMessageWithInfoCallback = + typename CallbackTypes::SharedConstPtrSerializedMessageWithInfoCallback; + using ConstRefSharedConstPtrCallback = + typename CallbackTypes::ConstRefSharedConstPtrCallback; + using ConstRefSharedConstPtrROSMessageCallback = + typename CallbackTypes::ConstRefSharedConstPtrROSMessageCallback; + using ConstRefSharedConstPtrWithInfoCallback = + typename CallbackTypes::ConstRefSharedConstPtrWithInfoCallback; + using ConstRefSharedConstPtrWithInfoROSMessageCallback = + typename CallbackTypes::ConstRefSharedConstPtrWithInfoROSMessageCallback; + using ConstRefSharedConstPtrSerializedMessageCallback = + typename CallbackTypes::ConstRefSharedConstPtrSerializedMessageCallback; + using ConstRefSharedConstPtrSerializedMessageWithInfoCallback = + typename CallbackTypes::ConstRefSharedConstPtrSerializedMessageWithInfoCallback; + using SharedPtrCallback = + typename CallbackTypes::SharedPtrCallback; + using SharedPtrROSMessageCallback = + typename CallbackTypes::SharedPtrROSMessageCallback; + using SharedPtrWithInfoCallback = + typename CallbackTypes::SharedPtrWithInfoCallback; + using SharedPtrWithInfoROSMessageCallback = + typename CallbackTypes::SharedPtrWithInfoROSMessageCallback; + using SharedPtrSerializedMessageCallback = + typename CallbackTypes::SharedPtrSerializedMessageCallback; + using SharedPtrSerializedMessageWithInfoCallback = + typename CallbackTypes::SharedPtrSerializedMessageWithInfoCallback; + + template + struct NotNull + { + NotNull(const T * pointer_in, const char * msg) + : pointer(pointer_in) + { + if (pointer == nullptr) { + throw std::invalid_argument(msg); + } + } - SharedPtrCallback shared_ptr_callback_; - SharedPtrWithInfoCallback shared_ptr_with_info_callback_; - ConstSharedPtrCallback const_shared_ptr_callback_; - ConstSharedPtrWithInfoCallback const_shared_ptr_with_info_callback_; - UniquePtrCallback unique_ptr_callback_; - UniquePtrWithInfoCallback unique_ptr_with_info_callback_; + const T * pointer; + }; public: - explicit AnySubscriptionCallback(std::shared_ptr allocator) - : shared_ptr_callback_(nullptr), shared_ptr_with_info_callback_(nullptr), - const_shared_ptr_callback_(nullptr), const_shared_ptr_with_info_callback_(nullptr), - unique_ptr_callback_(nullptr), unique_ptr_with_info_callback_(nullptr) + explicit + AnySubscriptionCallback(const AllocatorT & allocator = AllocatorT()) // NOLINT[runtime/explicit] + : subscribed_type_allocator_(allocator), + ros_message_type_allocator_(allocator) { - message_allocator_ = std::make_shared(*allocator.get()); - allocator::set_allocator_for_deleter(&message_deleter_, message_allocator_.get()); + allocator::set_allocator_for_deleter(&subscribed_type_deleter_, &subscribed_type_allocator_); + allocator::set_allocator_for_deleter(&ros_message_type_deleter_, &ros_message_type_allocator_); } AnySubscriptionCallback(const AnySubscriptionCallback &) = default; - template< - typename CallbackT, - typename std::enable_if< - rclcpp::function_traits::same_arguments< - CallbackT, - SharedPtrCallback - >::value - >::type * = nullptr - > - void set(CallbackT callback) - { - shared_ptr_callback_ = callback; - } - - template< - typename CallbackT, - typename std::enable_if< - rclcpp::function_traits::same_arguments< - CallbackT, - SharedPtrWithInfoCallback - >::value - >::type * = nullptr - > - void set(CallbackT callback) - { - shared_ptr_with_info_callback_ = callback; - } - - template< - typename CallbackT, - typename std::enable_if< - rclcpp::function_traits::same_arguments< - CallbackT, - ConstSharedPtrCallback - >::value - >::type * = nullptr - > - void set(CallbackT callback) - { - const_shared_ptr_callback_ = callback; - } - - template< - typename CallbackT, - typename std::enable_if< - rclcpp::function_traits::same_arguments< - CallbackT, - ConstSharedPtrWithInfoCallback - >::value - >::type * = nullptr - > - void set(CallbackT callback) - { - const_shared_ptr_with_info_callback_ = callback; - } - - template< - typename CallbackT, - typename std::enable_if< - rclcpp::function_traits::same_arguments< - CallbackT, - UniquePtrCallback - >::value - >::type * = nullptr - > - void set(CallbackT callback) - { - unique_ptr_callback_ = callback; - } - - template< - typename CallbackT, - typename std::enable_if< - rclcpp::function_traits::same_arguments< - CallbackT, - UniquePtrWithInfoCallback - >::value - >::type * = nullptr - > - void set(CallbackT callback) - { - unique_ptr_with_info_callback_ = callback; - } - - void dispatch( - std::shared_ptr message, const rmw_message_info_t & message_info) - { - (void)message_info; - if (shared_ptr_callback_) { - shared_ptr_callback_(message); - } else if (shared_ptr_with_info_callback_) { - shared_ptr_with_info_callback_(message, message_info); - } else if (const_shared_ptr_callback_) { - const_shared_ptr_callback_(message); - } else if (const_shared_ptr_with_info_callback_) { - const_shared_ptr_with_info_callback_(message, message_info); - } else if (unique_ptr_callback_) { - auto ptr = MessageAllocTraits::allocate(*message_allocator_.get(), 1); - MessageAllocTraits::construct(*message_allocator_.get(), ptr, *message); - unique_ptr_callback_(MessageUniquePtr(ptr, message_deleter_)); - } else if (unique_ptr_with_info_callback_) { - auto ptr = MessageAllocTraits::allocate(*message_allocator_.get(), 1); - MessageAllocTraits::construct(*message_allocator_.get(), ptr, *message); - unique_ptr_with_info_callback_(MessageUniquePtr(ptr, message_deleter_), message_info); + /// Generic function for setting the callback. + /** + * There are specializations that overload this in order to deprecate some + * callback signatures, and also to fix ambiguity between shared_ptr and + * unique_ptr callback signatures when using them with lambda functions. + */ + template + AnySubscriptionCallback + set(CallbackT callback) + { + // Use the SubscriptionCallbackTypeHelper to determine the actual type of + // the CallbackT, in terms of std::function<...>, which does not happen + // automatically with lambda functions in cases where the arguments can be + // converted to one another, e.g. shared_ptr and unique_ptr. + using scbth = detail::SubscriptionCallbackTypeHelper; + + callback_variant_ = static_cast(callback); + + // Return copy of self for easier testing, normally will be compiled out. + return *this; + } + + std::unique_ptr + create_ros_unique_ptr_from_ros_shared_ptr_message( + const std::shared_ptr & message) + { + auto ptr = ROSMessageTypeAllocatorTraits::allocate(ros_message_type_allocator_, 1); + ROSMessageTypeAllocatorTraits::construct(ros_message_type_allocator_, ptr, *message); + return std::unique_ptr(ptr, ros_message_type_deleter_); + } + + std::unique_ptr + create_serialized_message_unique_ptr_from_shared_ptr( + const std::shared_ptr & serialized_message) + { + auto ptr = SerializedMessageAllocatorTraits::allocate(serialized_message_allocator_, 1); + SerializedMessageAllocatorTraits::construct( + serialized_message_allocator_, ptr, *serialized_message); + return std::unique_ptr< + rclcpp::SerializedMessage, + SerializedMessageDeleter + >(ptr, serialized_message_deleter_); + } + + std::unique_ptr + create_custom_unique_ptr_from_custom_shared_ptr_message( + const std::shared_ptr & message) + { + auto ptr = SubscribedTypeAllocatorTraits::allocate(subscribed_type_allocator_, 1); + SubscribedTypeAllocatorTraits::construct(subscribed_type_allocator_, ptr, *message); + return std::unique_ptr(ptr, subscribed_type_deleter_); + } + + std::unique_ptr + convert_ros_message_to_custom_type_unique_ptr(const ROSMessageType & msg) + { + if constexpr (rclcpp::TypeAdapter::is_specialized::value) { + auto ptr = SubscribedTypeAllocatorTraits::allocate(subscribed_type_allocator_, 1); + SubscribedTypeAllocatorTraits::construct(subscribed_type_allocator_, ptr); + rclcpp::TypeAdapter::convert_to_custom(msg, *ptr); + return std::unique_ptr(ptr, subscribed_type_deleter_); } else { - throw std::runtime_error("unexpected message without any callback set"); + throw std::runtime_error( + "convert_ros_message_to_custom_type_unique_ptr " + "unexpectedly called without TypeAdapter"); } } - void dispatch_intra_process( - MessageUniquePtr & message, const rmw_message_info_t & message_info) - { - (void)message_info; - if (shared_ptr_callback_) { - typename std::shared_ptr shared_message = std::move(message); - shared_ptr_callback_(shared_message); - } else if (shared_ptr_with_info_callback_) { - typename std::shared_ptr shared_message = std::move(message); - shared_ptr_with_info_callback_(shared_message, message_info); - } else if (const_shared_ptr_callback_) { - typename std::shared_ptr const_shared_message = std::move(message); - const_shared_ptr_callback_(const_shared_message); - } else if (const_shared_ptr_with_info_callback_) { - typename std::shared_ptr const_shared_message = std::move(message); - const_shared_ptr_with_info_callback_(const_shared_message, message_info); - } else if (unique_ptr_callback_) { - unique_ptr_callback_(std::move(message)); - } else if (unique_ptr_with_info_callback_) { - unique_ptr_with_info_callback_(std::move(message), message_info); + std::unique_ptr + convert_custom_type_to_ros_message_unique_ptr(const SubscribedType & msg) + { + if constexpr (rclcpp::TypeAdapter::is_specialized::value) { + auto ptr = ROSMessageTypeAllocatorTraits::allocate(ros_message_type_allocator_, 1); + ROSMessageTypeAllocatorTraits::construct(ros_message_type_allocator_, ptr); + rclcpp::TypeAdapter::convert_to_ros_message(msg, *ptr); + return std::unique_ptr(ptr, ros_message_type_deleter_); } else { - throw std::runtime_error("unexpected message without any callback set"); + static_assert( + !sizeof(MessageT *), + "convert_custom_type_to_ros_message_unique_ptr() " + "unexpectedly called without specialized TypeAdapter"); + } + } + + // Dispatch when input is a ros message and the output could be anything. + template + typename std::enable_if::value, + void>::type + dispatch( + std::shared_ptr message, + const rclcpp::MessageInfo & message_info) + { + TRACETOOLS_TRACEPOINT(callback_start, static_cast(this), false); + // Check if the variant is "unset", throw if it is. + if (callback_variant_.index() == 0) { + if (std::get<0>(callback_variant_) == nullptr) { + // This can happen if it is default initialized, or if it is assigned nullptr. + throw std::runtime_error("dispatch called on an unset AnySubscriptionCallback"); + } + } + // Dispatch. + std::visit( + [&message, &message_info, this](auto && callback) { + using T = std::decay_t; + static constexpr bool is_ta = rclcpp::TypeAdapter::is_specialized::value; + + // conditions for output is custom message + if constexpr (is_ta && std::is_same_v) { + // TODO(wjwwood): consider avoiding heap allocation for small messages + // maybe something like: + // if constexpr (rosidl_generator_traits::has_fixed_size && sizeof(T) < N) { + // ... on stack + // } + auto local_message = convert_ros_message_to_custom_type_unique_ptr(*message); + callback(*local_message); + } else if constexpr (is_ta && std::is_same_v) { // NOLINT + auto local_message = convert_ros_message_to_custom_type_unique_ptr(*message); + callback(*local_message, message_info); + } else if constexpr (is_ta && std::is_same_v) { + callback(convert_ros_message_to_custom_type_unique_ptr(*message)); + } else if constexpr (is_ta && std::is_same_v) { + callback(convert_ros_message_to_custom_type_unique_ptr(*message), message_info); + } else if constexpr ( // NOLINT[readability/braces] + is_ta && ( + std::is_same_v|| + std::is_same_v|| + std::is_same_v + )) + { + callback(convert_ros_message_to_custom_type_unique_ptr(*message)); + } else if constexpr ( // NOLINT[readability/braces] + is_ta && ( + std::is_same_v|| + std::is_same_v|| + std::is_same_v + )) + { + callback(convert_ros_message_to_custom_type_unique_ptr(*message), message_info); + } + // conditions for output is ros message + else if constexpr (std::is_same_v) { // NOLINT + callback(*message); + } else if constexpr (std::is_same_v) { + callback(*message, message_info); + } else if constexpr (std::is_same_v) { + callback(create_ros_unique_ptr_from_ros_shared_ptr_message(message)); + } else if constexpr (std::is_same_v) { + callback(create_ros_unique_ptr_from_ros_shared_ptr_message(message), message_info); + } else if constexpr ( // NOLINT[readability/braces] + std::is_same_v|| + std::is_same_v|| + std::is_same_v) + { + callback(message); + } else if constexpr ( // NOLINT[readability/braces] + std::is_same_v|| + std::is_same_v|| + std::is_same_v) + { + callback(message, message_info); + } + // condition to catch SerializedMessage types + else if constexpr ( // NOLINT[readability/braces] + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v) + { + throw std::runtime_error( + "Cannot dispatch std::shared_ptr message " + "to rclcpp::SerializedMessage"); + } + // condition to catch unhandled callback types + else { // NOLINT[readability/braces] + static_assert(detail::always_false_v, "unhandled callback type"); + } + }, callback_variant_); + TRACETOOLS_TRACEPOINT(callback_end, static_cast(this)); + } + + // Dispatch when input is a serialized message and the output could be anything. + void + dispatch( + std::shared_ptr serialized_message, + const rclcpp::MessageInfo & message_info) + { + TRACETOOLS_TRACEPOINT(callback_start, static_cast(this), false); + // Check if the variant is "unset", throw if it is. + if (callback_variant_.index() == 0) { + if (std::get<0>(callback_variant_) == nullptr) { + // This can happen if it is default initialized, or if it is assigned nullptr. + throw std::runtime_error("dispatch called on an unset AnySubscriptionCallback"); + } + } + // Dispatch. + std::visit( + [&serialized_message, &message_info, this](auto && callback) { + using T = std::decay_t; + + // condition to catch SerializedMessage types + if constexpr (std::is_same_v) { + callback(*serialized_message); + } else if constexpr (std::is_same_v) { + callback(*serialized_message, message_info); + } else if constexpr (std::is_same_v) { + callback(create_serialized_message_unique_ptr_from_shared_ptr(serialized_message)); + } else if constexpr (std::is_same_v) { + callback( + create_serialized_message_unique_ptr_from_shared_ptr(serialized_message), + message_info); + } else if constexpr ( // NOLINT[readability/braces] + std::is_same_v|| + std::is_same_v|| + std::is_same_v) + { + callback(create_serialized_message_unique_ptr_from_shared_ptr(serialized_message)); + } else if constexpr ( // NOLINT[readability/braces] + std::is_same_v|| + std::is_same_v|| + std::is_same_v) + { + callback( + create_serialized_message_unique_ptr_from_shared_ptr(serialized_message), + message_info); + } + // conditions for output anything else + else if constexpr ( // NOLINT[whitespace/newline] + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v) + { + throw std::runtime_error( + "cannot dispatch rclcpp::SerializedMessage to " + "non-rclcpp::SerializedMessage callbacks"); + } + // condition to catch unhandled callback types + else { // NOLINT[readability/braces] + static_assert(detail::always_false_v, "unhandled callback type"); + } + }, callback_variant_); + TRACETOOLS_TRACEPOINT(callback_end, static_cast(this)); + } + + void + dispatch_intra_process( + std::shared_ptr message, + const rclcpp::MessageInfo & message_info) + { + TRACETOOLS_TRACEPOINT(callback_start, static_cast(this), true); + // Check if the variant is "unset", throw if it is. + if (callback_variant_.index() == 0) { + if (std::get<0>(callback_variant_) == nullptr) { + // This can happen if it is default initialized, or if it is assigned nullptr. + throw std::runtime_error("dispatch called on an unset AnySubscriptionCallback"); + } + } + // Dispatch. + std::visit( + [&message, &message_info, this](auto && callback) { + using T = std::decay_t; + static constexpr bool is_ta = rclcpp::TypeAdapter::is_specialized::value; + + // conditions for custom type + if constexpr (is_ta && std::is_same_v) { + callback(*message); + } else if constexpr (is_ta && std::is_same_v) { // NOLINT + callback(*message, message_info); + } else if constexpr ( // NOLINT[readability/braces] + is_ta && ( + std::is_same_v|| + std::is_same_v + )) + { + callback(create_custom_unique_ptr_from_custom_shared_ptr_message(message)); + } else if constexpr ( // NOLINT[readability/braces] + is_ta && ( + std::is_same_v|| + std::is_same_v + )) + { + callback(create_custom_unique_ptr_from_custom_shared_ptr_message(message), message_info); + } else if constexpr ( // NOLINT[readability/braces] + is_ta && ( + std::is_same_v|| + std::is_same_v + )) + { + callback(message); + } else if constexpr ( // NOLINT[readability/braces] + is_ta && ( + std::is_same_v|| + std::is_same_v + )) + { + callback(message, message_info); + } + // conditions for ros message type + else if constexpr (std::is_same_v) { // NOLINT[readability/braces] + if constexpr (is_ta) { + auto local = convert_custom_type_to_ros_message_unique_ptr(*message); + callback(*local); + } else { + callback(*message); + } + } else if constexpr (std::is_same_v) { // NOLINT[readability/braces] + if constexpr (is_ta) { + auto local = convert_custom_type_to_ros_message_unique_ptr(*message); + callback(*local, message_info); + } else { + callback(*message, message_info); + } + } else if constexpr ( // NOLINT[readability/braces] + std::is_same_v|| + std::is_same_v) + { + if constexpr (is_ta) { + callback(convert_custom_type_to_ros_message_unique_ptr(*message)); + } else { + callback(create_ros_unique_ptr_from_ros_shared_ptr_message(message)); + } + } else if constexpr ( // NOLINT[readability/braces] + std::is_same_v|| + std::is_same_v) + { + if constexpr (is_ta) { + callback(convert_custom_type_to_ros_message_unique_ptr(*message), message_info); + } else { + callback(create_ros_unique_ptr_from_ros_shared_ptr_message(message), message_info); + } + } else if constexpr ( // NOLINT[readability/braces] + std::is_same_v|| + std::is_same_v) + { + if constexpr (is_ta) { + callback(convert_custom_type_to_ros_message_unique_ptr(*message)); + } else { + callback(message); + } + } else if constexpr ( // NOLINT[readability/braces] + std::is_same_v|| + std::is_same_v) + { + if constexpr (is_ta) { + callback(convert_custom_type_to_ros_message_unique_ptr(*message), message_info); + } else { + callback(message, message_info); + } + } + // condition to catch SerializedMessage types + else if constexpr ( // NOLINT[readability/braces] + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v) + { + throw std::runtime_error( + "Cannot dispatch std::shared_ptr message " + "to rclcpp::SerializedMessage"); + } + // condition to catch unhandled callback types + else { // NOLINT[readability/braces] + static_assert(detail::always_false_v, "unhandled callback type"); + } + }, callback_variant_); + TRACETOOLS_TRACEPOINT(callback_end, static_cast(this)); + } + + void + dispatch_intra_process( + std::unique_ptr message, + const rclcpp::MessageInfo & message_info) + { + TRACETOOLS_TRACEPOINT(callback_start, static_cast(this), true); + // Check if the variant is "unset", throw if it is. + if (callback_variant_.index() == 0) { + if (std::get<0>(callback_variant_) == nullptr) { + // This can happen if it is default initialized, or if it is assigned nullptr. + throw std::runtime_error("dispatch called on an unset AnySubscriptionCallback"); + } } + // Dispatch. + std::visit( + [&message, &message_info, this](auto && callback) { + // clang complains that 'this' lambda capture is unused, which is true + // in *some* specializations of this template, but not others. Just + // quiet it down. + (void)this; + + using T = std::decay_t; + static constexpr bool is_ta = rclcpp::TypeAdapter::is_specialized::value; + + // conditions for custom type + if constexpr (is_ta && std::is_same_v) { + callback(*message); + } else if constexpr (is_ta && std::is_same_v) { // NOLINT + callback(*message, message_info); + } else if constexpr ( // NOLINT[readability/braces] + is_ta && ( + std::is_same_v|| + std::is_same_v)) + { + callback(std::move(message)); + } else if constexpr ( // NOLINT[readability/braces] + is_ta && ( + std::is_same_v|| + std::is_same_v + )) + { + callback(std::move(message), message_info); + } else if constexpr ( // NOLINT[readability/braces] + is_ta && ( + std::is_same_v|| + std::is_same_v + )) + { + callback(std::move(message)); + } else if constexpr ( // NOLINT[readability/braces] + is_ta && ( + std::is_same_v|| + std::is_same_v + )) + { + callback(std::move(message), message_info); + } + // conditions for ros message type + else if constexpr (std::is_same_v) { // NOLINT[readability/braces] + if constexpr (is_ta) { + auto local = convert_custom_type_to_ros_message_unique_ptr(*message); + callback(*local); + } else { + callback(*message); + } + } else if constexpr (std::is_same_v) { // NOLINT[readability/braces] + if constexpr (is_ta) { + auto local = convert_custom_type_to_ros_message_unique_ptr(*message); + callback(*local, message_info); + } else { + callback(*message, message_info); + } + } else if constexpr ( // NOLINT[readability/braces] + std::is_same_v|| + std::is_same_v) + { + if constexpr (is_ta) { + callback(convert_custom_type_to_ros_message_unique_ptr(*message)); + } else { + callback(std::move(message)); + } + } else if constexpr ( // NOLINT[readability/braces] + std::is_same_v|| + std::is_same_v) + { + if constexpr (is_ta) { + callback(convert_custom_type_to_ros_message_unique_ptr(*message), message_info); + } else { + callback(std::move(message), message_info); + } + } else if constexpr ( // NOLINT[readability/braces] + std::is_same_v|| + std::is_same_v) + { + if constexpr (is_ta) { + callback(convert_custom_type_to_ros_message_unique_ptr(*message)); + } else { + callback(std::move(message)); + } + } else if constexpr ( // NOLINT[readability/braces] + std::is_same_v|| + std::is_same_v) + { + if constexpr (is_ta) { + callback(convert_custom_type_to_ros_message_unique_ptr(*message), message_info); + } else { + callback(std::move(message), message_info); + } + } + // condition to catch SerializedMessage types + else if constexpr ( // NOLINT[readability/braces] + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v) + { + throw std::runtime_error( + "Cannot dispatch std::unique_ptr message " + "to rclcpp::SerializedMessage"); + } + // condition to catch unhandled callback types + else { // NOLINT[readability/braces] + static_assert(detail::always_false_v, "unhandled callback type"); + } + }, callback_variant_); + TRACETOOLS_TRACEPOINT(callback_end, static_cast(this)); + } + + constexpr + bool + use_take_shared_method() const + { + return + std::holds_alternative(callback_variant_) || + std::holds_alternative(callback_variant_) || + std::holds_alternative(callback_variant_) || + std::holds_alternative(callback_variant_); + } + + constexpr + bool + is_serialized_message_callback() const + { + return + std::holds_alternative(callback_variant_) || + std::holds_alternative(callback_variant_) || + std::holds_alternative(callback_variant_) || + std::holds_alternative(callback_variant_) || + std::holds_alternative(callback_variant_) || + std::holds_alternative(callback_variant_) || + std::holds_alternative(callback_variant_) || + std::holds_alternative(callback_variant_) || + std::holds_alternative( + callback_variant_) || + std::holds_alternative(callback_variant_); + } + + void + register_callback_for_tracing() + { +#ifndef TRACETOOLS_DISABLED + std::visit( + [this](auto && callback) { + if (TRACETOOLS_TRACEPOINT_ENABLED(rclcpp_callback_register)) { + char * symbol = tracetools::get_symbol(callback); + TRACETOOLS_DO_TRACEPOINT( + rclcpp_callback_register, + static_cast(this), + symbol); + std::free(symbol); + } + }, callback_variant_); +#endif // TRACETOOLS_DISABLED + } + + typename HelperT::variant_type & + get_variant() + { + return callback_variant_; + } + + const typename HelperT::variant_type & + get_variant() const + { + return callback_variant_; } private: - std::shared_ptr message_allocator_; - MessageDeleter message_deleter_; + // TODO(wjwwood): switch to inheriting from std::variant (i.e. HelperT::variant_type) once + // inheriting from std::variant is realistic (maybe C++23?), see: + // http://www.open-std.org/jtc1/sc22/wg21/docs/papers/2020/p2162r0.html + // For now, compose the variant into this class as a private attribute. + typename HelperT::variant_type callback_variant_; + + SubscribedTypeAllocator subscribed_type_allocator_; + SubscribedTypeDeleter subscribed_type_deleter_; + ROSMessageTypeAllocator ros_message_type_allocator_; + ROSMessageTypeDeleter ros_message_type_deleter_; + SerializedMessageAllocator serialized_message_allocator_; + SerializedMessageDeleter serialized_message_deleter_; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/callback_group.hpp b/rclcpp/include/rclcpp/callback_group.hpp index 810bfc3a4e..b4a4d4e9a6 100644 --- a/rclcpp/include/rclcpp/callback_group.hpp +++ b/rclcpp/include/rclcpp/callback_group.hpp @@ -16,15 +16,20 @@ #define RCLCPP__CALLBACK_GROUP_HPP_ #include +#include +#include #include -#include #include #include "rclcpp/client.hpp" +#include "rclcpp/context.hpp" +#include "rclcpp/guard_condition.hpp" +#include "rclcpp/publisher_base.hpp" #include "rclcpp/service.hpp" -#include "rclcpp/subscription.hpp" +#include "rclcpp/subscription_base.hpp" #include "rclcpp/timer.hpp" #include "rclcpp/visibility_control.hpp" +#include "rclcpp/waitable.hpp" namespace rclcpp { @@ -35,11 +40,9 @@ namespace node_interfaces class NodeServices; class NodeTimers; class NodeTopics; +class NodeWaitables; } // namespace node_interfaces -namespace callback_group -{ - enum class CallbackGroupType { MutuallyExclusive, @@ -51,40 +54,182 @@ class CallbackGroup friend class rclcpp::node_interfaces::NodeServices; friend class rclcpp::node_interfaces::NodeTimers; friend class rclcpp::node_interfaces::NodeTopics; + friend class rclcpp::node_interfaces::NodeWaitables; public: RCLCPP_SMART_PTR_DEFINITIONS(CallbackGroup) + /// Constructor for CallbackGroup. + /** + * Callback Groups have a type, either 'Mutually Exclusive' or 'Reentrant' + * and when creating one the type must be specified. + * + * Callbacks in Reentrant Callback Groups must be able to: + * - run at the same time as themselves (reentrant) + * - run at the same time as other callbacks in their group + * - run at the same time as other callbacks in other groups + * + * Callbacks in Mutually Exclusive Callback Groups: + * - will not be run multiple times simultaneously (non-reentrant) + * - will not be run at the same time as other callbacks in their group + * - but must run at the same time as callbacks in other groups + * + * Additionally, callback groups have a property which determines whether or + * not they are added to an executor with their associated node automatically. + * When creating a callback group the automatically_add_to_executor_with_node + * argument determines this behavior, and if true it will cause the newly + * created callback group to be added to an executor with the node when the + * Executor::add_node method is used. + * If false, this callback group will not be added automatically and would + * have to be added to an executor manually using the + * Executor::add_callback_group method. + * + * Whether the node was added to the executor before creating the callback + * group, or after, is irrelevant; the callback group will be automatically + * added to the executor in either case. + * + * \param[in] group_type The type of the callback group. + * \param[in] context A weak pointer to the context associated with this callback group. + * \param[in] automatically_add_to_executor_with_node A boolean that + * determines whether a callback group is automatically added to an executor + * with the node with which it is associated. + */ RCLCPP_PUBLIC - explicit CallbackGroup(CallbackGroupType group_type); + explicit CallbackGroup( + CallbackGroupType group_type, + rclcpp::Context::WeakPtr context, + bool automatically_add_to_executor_with_node = true); + /// Default destructor. RCLCPP_PUBLIC - const std::vector & - get_subscription_ptrs() const; + ~CallbackGroup(); - RCLCPP_PUBLIC - const std::vector & - get_timer_ptrs() const; + template + rclcpp::SubscriptionBase::SharedPtr + find_subscription_ptrs_if(Function func) const + { + return _find_ptrs_if_impl(func, subscription_ptrs_); + } - RCLCPP_PUBLIC - const std::vector & - get_service_ptrs() const; + template + rclcpp::TimerBase::SharedPtr + find_timer_ptrs_if(Function func) const + { + return _find_ptrs_if_impl(func, timer_ptrs_); + } + template + rclcpp::ServiceBase::SharedPtr + find_service_ptrs_if(Function func) const + { + return _find_ptrs_if_impl(func, service_ptrs_); + } + + template + rclcpp::ClientBase::SharedPtr + find_client_ptrs_if(Function func) const + { + return _find_ptrs_if_impl(func, client_ptrs_); + } + + template + rclcpp::Waitable::SharedPtr + find_waitable_ptrs_if(Function func) const + { + return _find_ptrs_if_impl(func, waitable_ptrs_); + } + + /// Get the total number of entities in this callback group. + /** + * \return the number of entities in the callback group. + */ RCLCPP_PUBLIC - const std::vector & - get_client_ptrs() const; + size_t + size() const; + /// Return a reference to the 'can be taken' atomic boolean. + /** + * The resulting bool will be true in the case that no executor is currently + * using an executable entity from this group. + * The resulting bool will be false in the case that an executor is currently + * using an executable entity from this group, and the group policy doesn't + * allow a second take (eg mutual exclusion) + * \return a reference to the flag + */ RCLCPP_PUBLIC std::atomic_bool & can_be_taken_from(); + /// Get the group type. + /** + * \return the group type + */ RCLCPP_PUBLIC const CallbackGroupType & type() const; + /// Collect all of the entity pointers contained in this callback group. + /** + * \param[in] sub_func Function to execute for each subscription + * \param[in] service_func Function to execute for each service + * \param[in] client_func Function to execute for each client + * \param[in] timer_func Function to execute for each timer + * \param[in] waitable_fuinc Function to execute for each waitable + */ + RCLCPP_PUBLIC + void + collect_all_ptrs( + std::function sub_func, + std::function service_func, + std::function client_func, + std::function timer_func, + std::function waitable_func) const; + + /// Return a reference to the 'associated with executor' atomic boolean. + /** + * When a callback group is added to an executor this boolean is checked + * to ensure it has not already been added to another executor. + * If it has not been, then this boolean is set to true to indicate it is + * now associated with an executor. + * + * When the callback group is removed from the executor, this atomic boolean + * is set back to false. + * + * \return the 'associated with executor' atomic boolean + */ + RCLCPP_PUBLIC + std::atomic_bool & + get_associated_with_executor_atomic(); + + /// Return true if this callback group should be automatically added to an executor by the node. + /** + * \return boolean true if this callback group should be automatically added + * to an executor when the associated node is added, otherwise false. + */ + RCLCPP_PUBLIC + bool + automatically_add_to_executor_with_node() const; + + /// Retrieve the guard condition used to signal changes to this callback group. + /** + * \return guard condition if it is valid, otherwise nullptr. + */ + RCLCPP_PUBLIC + rclcpp::GuardCondition::SharedPtr + get_notify_guard_condition(); + + /// Trigger the notify guard condition. + RCLCPP_PUBLIC + void + trigger_notify_guard_condition(); + protected: RCLCPP_DISABLE_COPY(CallbackGroup) + RCLCPP_PUBLIC + void + add_publisher(const rclcpp::PublisherBase::SharedPtr publisher_ptr); + RCLCPP_PUBLIC void add_subscription(const rclcpp::SubscriptionBase::SharedPtr subscription_ptr); @@ -101,17 +246,47 @@ class CallbackGroup void add_client(const rclcpp::ClientBase::SharedPtr client_ptr); + RCLCPP_PUBLIC + void + add_waitable(const rclcpp::Waitable::SharedPtr waitable_ptr); + + RCLCPP_PUBLIC + void + remove_waitable(const rclcpp::Waitable::SharedPtr waitable_ptr) noexcept; + CallbackGroupType type_; // Mutex to protect the subsequent vectors of pointers. mutable std::mutex mutex_; + std::atomic_bool associated_with_executor_; std::vector subscription_ptrs_; std::vector timer_ptrs_; std::vector service_ptrs_; std::vector client_ptrs_; + std::vector waitable_ptrs_; std::atomic_bool can_be_taken_from_; + const bool automatically_add_to_executor_with_node_; + // defer the creation of the guard condition + std::shared_ptr notify_guard_condition_ = nullptr; + std::recursive_mutex notify_guard_condition_mutex_; + + rclcpp::Context::WeakPtr context_; + +private: + template + typename TypeT::SharedPtr _find_ptrs_if_impl( + Function func, const std::vector & vect_ptrs) const + { + std::lock_guard lock(mutex_); + for (auto & weak_ptr : vect_ptrs) { + auto ref_ptr = weak_ptr.lock(); + if (ref_ptr && func(ref_ptr)) { + return ref_ptr; + } + } + return typename TypeT::SharedPtr(); + } }; -} // namespace callback_group } // namespace rclcpp #endif // RCLCPP__CALLBACK_GROUP_HPP_ diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index f6eb9d2509..ff90dcb346 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -15,33 +15,123 @@ #ifndef RCLCPP__CLIENT_HPP_ #define RCLCPP__CLIENT_HPP_ +#include +#include #include -#include #include +#include +#include #include #include #include +#include #include +#include +#include #include "rcl/client.h" #include "rcl/error_handling.h" +#include "rcl/event_callback.h" +#include "rcl/service_introspection.h" #include "rcl/wait.h" +#include "rclcpp/clock.hpp" +#include "rclcpp/detail/cpp_callback_trampoline.hpp" #include "rclcpp/exceptions.hpp" +#include "rclcpp/expand_topic_or_service_name.hpp" #include "rclcpp/function_traits.hpp" +#include "rclcpp/logging.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/node_interfaces/node_graph_interface.hpp" +#include "rclcpp/qos.hpp" #include "rclcpp/type_support_decl.hpp" #include "rclcpp/utilities.hpp" -#include "rclcpp/expand_topic_or_service_name.hpp" #include "rclcpp/visibility_control.hpp" #include "rmw/error_handling.h" +#include "rmw/impl/cpp/demangle.hpp" #include "rmw/rmw.h" namespace rclcpp { +namespace detail +{ +template +struct FutureAndRequestId +{ + FutureT future; + int64_t request_id; + + FutureAndRequestId(FutureT impl, int64_t req_id) + : future(std::move(impl)), request_id(req_id) + {} + + /// Allow implicit conversions to `std::future` by reference. + operator FutureT &() {return this->future;} + + // delegate future like methods in the std::future impl_ + + /// See std::future::get(). + auto get() {return this->future.get();} + /// See std::future::valid(). + bool valid() const noexcept {return this->future.valid();} + /// See std::future::wait(). + void wait() const {return this->future.wait();} + /// See std::future::wait_for(). + template + std::future_status wait_for( + const std::chrono::duration & timeout_duration) const + { + return this->future.wait_for(timeout_duration); + } + /// See std::future::wait_until(). + template + std::future_status wait_until( + const std::chrono::time_point & timeout_time) const + { + return this->future.wait_until(timeout_time); + } + + // Rule of five, we could use the rule of zero here, but better be explicit as some of the + // methods are deleted. + + /// Move constructor. + FutureAndRequestId(FutureAndRequestId && other) noexcept = default; + /// Deleted copy constructor, each instance is a unique owner of the future. + FutureAndRequestId(const FutureAndRequestId & other) = delete; + /// Move assignment. + FutureAndRequestId & operator=(FutureAndRequestId && other) noexcept = default; + /// Deleted copy assignment, each instance is a unique owner of the future. + FutureAndRequestId & operator=(const FutureAndRequestId & other) = delete; + /// Destructor. + ~FutureAndRequestId() = default; +}; + +template> +size_t +prune_requests_older_than_impl( + PendingRequestsT & pending_requests, + std::mutex & pending_requests_mutex, + std::chrono::time_point time_point, + std::vector * pruned_requests = nullptr) +{ + std::lock_guard guard(pending_requests_mutex); + auto old_size = pending_requests.size(); + for (auto it = pending_requests.begin(), last = pending_requests.end(); it != last; ) { + if (it->second.first < time_point) { + if (pruned_requests) { + pruned_requests->push_back(it->first); + } + it = pending_requests.erase(it); + } else { + ++it; + } + } + return old_size - pending_requests.size(); +} +} // namespace detail + namespace node_interfaces { class NodeBaseInterface; @@ -55,32 +145,73 @@ class ClientBase RCLCPP_PUBLIC ClientBase( rclcpp::node_interfaces::NodeBaseInterface * node_base, - rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, - const std::string & service_name); + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph); RCLCPP_PUBLIC - virtual ~ClientBase(); + virtual ~ClientBase() = default; + + /// Take the next response for this client as a type erased pointer. + /** + * The type erased pointer allows for this method to be used in a type + * agnostic way along with ClientBase::create_response(), + * ClientBase::create_request_header(), and ClientBase::handle_response(). + * The typed version of this can be used if the Service type is known, + * \sa Client::take_response(). + * + * \param[out] response_out The type erased pointer to a Service Response into + * which the middleware will copy the response being taken. + * \param[out] request_header_out The request header to be filled by the + * middleware when taking, and which can be used to associte the response + * to a specific request. + * \returns true if the response was taken, otherwise false. + * \throws rclcpp::exceptions::RCLError based exceptions if the underlying + * rcl function fail. + */ + RCLCPP_PUBLIC + bool + take_type_erased_response(void * response_out, rmw_request_id_t & request_header_out); + /// Return the name of the service. + /** \return The name of the service. */ RCLCPP_PUBLIC - const std::string & + const char * get_service_name() const; + /// Return the rcl_client_t client handle in a std::shared_ptr. + /** + * This handle remains valid after the Client is destroyed. + * The actual rcl client is not finalized until it is out of scope everywhere. + */ RCLCPP_PUBLIC - rcl_client_t * + std::shared_ptr get_client_handle(); + /// Return the rcl_client_t client handle in a std::shared_ptr. + /** + * This handle remains valid after the Client is destroyed. + * The actual rcl client is not finalized until it is out of scope everywhere. + */ RCLCPP_PUBLIC - const rcl_client_t * + std::shared_ptr get_client_handle() const; + /// Return if the service is ready. + /** + * \return `true` if the service is ready, `false` otherwise + */ RCLCPP_PUBLIC bool service_is_ready() const; - template + /// Wait for a service to be ready. + /** + * \param timeout maximum time to wait + * \return `true` if the service is ready and the timeout is not over, `false` otherwise + */ + template bool wait_for_service( - std::chrono::duration timeout = std::chrono::duration(-1)) + std::chrono::duration timeout = std::chrono::duration(-1)) { return wait_for_service_nanoseconds( std::chrono::duration_cast(timeout) @@ -92,6 +223,137 @@ class ClientBase virtual void handle_response( std::shared_ptr request_header, std::shared_ptr response) = 0; + /// Exchange the "in use by wait set" state for this client. + /** + * This is used to ensure this client is not used by multiple + * wait sets at the same time. + * + * \param[in] in_use_state the new state to exchange into the state, true + * indicates it is now in use by a wait set, and false is that it is no + * longer in use by a wait set. + * \returns the previous state. + */ + RCLCPP_PUBLIC + bool + exchange_in_use_by_wait_set_state(bool in_use_state); + + /// Get the actual request publsher QoS settings, after the defaults have been determined. + /** + * The actual configuration applied when using RMW_QOS_POLICY_*_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_QOS_POLICY_*_UNKNOWN. + * May throw runtime_error when an unexpected error occurs. + * + * \return The actual request publsher qos settings. + * \throws std::runtime_error if failed to get qos settings + */ + RCLCPP_PUBLIC + rclcpp::QoS + get_request_publisher_actual_qos() const; + + /// Get the actual response subscription QoS settings, after the defaults have been determined. + /** + * The actual configuration applied when using RMW_QOS_POLICY_*_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_QOS_POLICY_*_UNKNOWN. + * May throw runtime_error when an unexpected error occurs. + * + * \return The actual response subscription qos settings. + * \throws std::runtime_error if failed to get qos settings + */ + RCLCPP_PUBLIC + rclcpp::QoS + get_response_subscription_actual_qos() const; + + /// Set a callback to be called when each new response is received. + /** + * The callback receives a size_t which is the number of responses received + * since the last time this callback was called. + * Normally this is 1, but can be > 1 if responses were received before any + * callback was set. + * + * Since this callback is called from the middleware, you should aim to make + * it fast and not blocking. + * If you need to do a lot of work or wait for some other event, you should + * spin it off to another thread, otherwise you risk blocking the middleware. + * + * Calling it again will clear any previously set callback. + * + * An exception will be thrown if the callback is not callable. + * + * This function is thread-safe. + * + * If you want more information available in the callback, like the client + * or other information, you may use a lambda with captures or std::bind. + * + * \sa rmw_client_set_on_new_response_callback + * \sa rcl_client_set_on_new_response_callback + * + * \param[in] callback functor to be called when a new response is received + */ + void + set_on_new_response_callback(std::function callback) + { + if (!callback) { + throw std::invalid_argument( + "The callback passed to set_on_new_response_callback " + "is not callable."); + } + + auto new_callback = + [callback, this](size_t number_of_responses) { + try { + callback(number_of_responses); + } catch (const std::exception & exception) { + RCLCPP_ERROR_STREAM( + node_logger_, + "rclcpp::ClientBase@" << this << + " caught " << rmw::impl::cpp::demangle(exception) << + " exception in user-provided callback for the 'on new response' callback: " << + exception.what()); + } catch (...) { + RCLCPP_ERROR_STREAM( + node_logger_, + "rclcpp::ClientBase@" << this << + " caught unhandled exception in user-provided callback " << + "for the 'on new response' callback"); + } + }; + + std::lock_guard lock(callback_mutex_); + + // Set it temporarily to the new callback, while we replace the old one. + // This two-step setting, prevents a gap where the old std::function has + // been replaced but the middleware hasn't been told about the new one yet. + set_on_new_response_callback( + rclcpp::detail::cpp_callback_trampoline, + static_cast(&new_callback)); + + // Store the std::function to keep it in scope, also overwrites the existing one. + on_new_response_callback_ = new_callback; + + // Set it again, now using the permanent storage. + set_on_new_response_callback( + rclcpp::detail::cpp_callback_trampoline< + decltype(on_new_response_callback_), const void *, size_t>, + static_cast(&on_new_response_callback_)); + } + + /// Unset the callback registered for new responses, if any. + void + clear_on_new_response_callback() + { + std::lock_guard lock(callback_mutex_); + if (on_new_response_callback_) { + set_on_new_response_callback(nullptr, nullptr); + on_new_response_callback_ = nullptr; + } + } + protected: RCLCPP_DISABLE_COPY(ClientBase) @@ -107,17 +369,34 @@ class ClientBase const rcl_node_t * get_rcl_node_handle() const; + RCLCPP_PUBLIC + void + set_on_new_response_callback(rcl_event_callback_t callback, const void * user_data); + rclcpp::node_interfaces::NodeGraphInterface::WeakPtr node_graph_; std::shared_ptr node_handle_; - - rcl_client_t client_handle_ = rcl_get_zero_initialized_client(); - std::string service_name_; + std::shared_ptr context_; + rclcpp::Logger node_logger_; + + std::recursive_mutex callback_mutex_; + // It is important to declare on_new_response_callback_ before + // client_handle_, so on destruction the client is + // destroyed first. Otherwise, the rmw client callback + // would point briefly to a destroyed function. + std::function on_new_response_callback_{nullptr}; + // Declare client_handle_ after callback + std::shared_ptr client_handle_; + + std::atomic in_use_by_wait_set_{false}; }; template class Client : public ClientBase { public: + using Request = typename ServiceT::Request; + using Response = typename ServiceT::Response; + using SharedRequest = typename ServiceT::Request::SharedPtr; using SharedResponse = typename ServiceT::Response::SharedPtr; @@ -127,28 +406,87 @@ class Client : public ClientBase using SharedPromise = std::shared_ptr; using SharedPromiseWithRequest = std::shared_ptr; + using Future = std::future; using SharedFuture = std::shared_future; using SharedFutureWithRequest = std::shared_future>; - using CallbackType = std::function; - using CallbackWithRequestType = std::function; + using CallbackType = std::function; + using CallbackWithRequestType = std::function; RCLCPP_SMART_PTR_DEFINITIONS(Client) + /// A convenient Client::Future and request id pair. + /** + * Public members: + * - future: a std::future. + * - request_id: the request id associated with the future. + * + * All the other methods are equivalent to the ones std::future provides. + */ + struct FutureAndRequestId + : detail::FutureAndRequestId> + { + using detail::FutureAndRequestId>::FutureAndRequestId; + + // delegate future like methods in the std::future impl_ + + /// See std::future::share(). + SharedFuture share() noexcept {return this->future.share();} + }; + + /// A convenient Client::SharedFuture and request id pair. + /** + * Public members: + * - future: a std::shared_future. + * - request_id: the request id associated with the future. + * + * All the other methods are equivalent to the ones std::shared_future provides. + */ + struct SharedFutureAndRequestId + : detail::FutureAndRequestId> + { + using detail::FutureAndRequestId>::FutureAndRequestId; + }; + + /// A convenient Client::SharedFutureWithRequest and request id pair. + /** + * Public members: + * - future: a std::shared_future. + * - request_id: the request id associated with the future. + * + * All the other methods are equivalent to the ones std::shared_future provides. + */ + struct SharedFutureWithRequestAndRequestId + : detail::FutureAndRequestId>> + { + using detail::FutureAndRequestId< + std::shared_future> + >::FutureAndRequestId; + }; + + /// Default constructor. + /** + * The constructor for a Client is almost never called directly. + * Instead, clients should be instantiated through the function + * rclcpp::create_client(). + * + * \param[in] node_base NodeBaseInterface pointer that is used in part of the setup. + * \param[in] node_graph The node graph interface of the corresponding node. + * \param[in] service_name Name of the topic to publish to. + * \param[in] client_options options for the client. + */ Client( rclcpp::node_interfaces::NodeBaseInterface * node_base, rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, const std::string & service_name, rcl_client_options_t & client_options) - : ClientBase(node_base, node_graph, service_name) + : ClientBase(node_base, node_graph), + srv_type_support_handle_(rosidl_typesupport_cpp::get_service_type_support_handle()) { - using rosidl_typesupport_cpp::get_service_type_support_handle; - auto service_type_support_handle = - get_service_type_support_handle(); rcl_ret_t ret = rcl_client_init( - &client_handle_, + this->get_client_handle().get(), this->get_rcl_node_handle(), - service_type_support_handle, + srv_type_support_handle_, service_name.c_str(), &client_options); if (ret != RCL_RET_OK) { @@ -168,58 +506,142 @@ class Client : public ClientBase virtual ~Client() { - if (rcl_client_fini(&client_handle_, this->get_rcl_node_handle()) != RCL_RET_OK) { - fprintf(stderr, - "Error in destruction of rcl client handle: %s\n", rcl_get_error_string_safe()); - rcl_reset_error(); - } } + /// Take the next response for this client. + /** + * \sa ClientBase::take_type_erased_response(). + * + * \param[out] response_out The reference to a Service Response into + * which the middleware will copy the response being taken. + * \param[out] request_header_out The request header to be filled by the + * middleware when taking, and which can be used to associte the response + * to a specific request. + * \returns true if the response was taken, otherwise false. + * \throws rclcpp::exceptions::RCLError based exceptions if the underlying + * rcl function fail. + */ + bool + take_response(typename ServiceT::Response & response_out, rmw_request_id_t & request_header_out) + { + return this->take_type_erased_response(&response_out, request_header_out); + } + + /// Create a shared pointer with the response type + /** + * \return shared pointer with the response type + */ std::shared_ptr - create_response() + create_response() override { return std::shared_ptr(new typename ServiceT::Response()); } + /// Create a shared pointer with a rmw_request_id_t + /** + * \return shared pointer with a rmw_request_id_t + */ std::shared_ptr - create_request_header() + create_request_header() override { // TODO(wjwwood): This should probably use rmw_request_id's allocator. // (since it is a C type) return std::shared_ptr(new rmw_request_id_t); } + /// Handle a server response + /** + * \param[in] request_header used to check if the secuence number is valid + * \param[in] response message with the server response + */ void handle_response( std::shared_ptr request_header, - std::shared_ptr response) + std::shared_ptr response) override { - std::unique_lock lock(pending_requests_mutex_); - auto typed_response = std::static_pointer_cast(response); - int64_t sequence_number = request_header->sequence_number; - // TODO(esteve) this should throw instead since it is not expected to happen in the first place - if (this->pending_requests_.count(sequence_number) == 0) { - fprintf(stderr, "Received invalid sequence number. Ignoring...\n"); + std::optional + optional_pending_request = this->get_and_erase_pending_request(request_header->sequence_number); + if (!optional_pending_request) { return; } - auto tuple = this->pending_requests_[sequence_number]; - auto call_promise = std::get<0>(tuple); - auto callback = std::get<1>(tuple); - auto future = std::get<2>(tuple); - this->pending_requests_.erase(sequence_number); - // Unlock here to allow the service to be called recursively from one of its callbacks. - lock.unlock(); - - call_promise->set_value(typed_response); - callback(future); + auto & value = *optional_pending_request; + auto typed_response = std::static_pointer_cast( + std::move(response)); + if (std::holds_alternative(value)) { + auto & promise = std::get(value); + promise.set_value(std::move(typed_response)); + } else if (std::holds_alternative(value)) { + auto & inner = std::get(value); + const auto & callback = std::get(inner); + auto & promise = std::get(inner); + auto & future = std::get(inner); + promise.set_value(std::move(typed_response)); + callback(std::move(future)); + } else if (std::holds_alternative(value)) { + auto & inner = std::get(value); + const auto & callback = std::get(inner); + auto & promise = std::get(inner); + auto & future = std::get(inner); + auto & request = std::get(inner); + promise.set_value(std::make_pair(std::move(request), std::move(typed_response))); + callback(std::move(future)); + } } - SharedFuture + /// Send a request to the service server. + /** + * This method returns a `FutureAndRequestId` instance + * that can be passed to Executor::spin_until_future_complete() to + * wait until it has been completed. + * + * If the future never completes, + * e.g. the call to Executor::spin_until_future_complete() times out, + * Client::remove_pending_request() must be called to clean the client internal state. + * Not doing so will make the `Client` instance to use more memory each time a response is not + * received from the service server. + * + * ```cpp + * auto future = client->async_send_request(my_request); + * if ( + * rclcpp::FutureReturnCode::TIMEOUT == + * executor->spin_until_future_complete(future, timeout)) + * { + * client->remove_pending_request(future); + * // handle timeout + * } else { + * handle_response(future.get()); + * } + * ``` + * + * \param[in] request request to be send. + * \return a FutureAndRequestId instance. + */ + FutureAndRequestId async_send_request(SharedRequest request) { - return async_send_request(request, [](SharedFuture) {}); + Promise promise; + auto future = promise.get_future(); + auto req_id = async_send_request_impl( + *request, + std::move(promise)); + return FutureAndRequestId(std::move(future), req_id); } + /// Send a request to the service server and schedule a callback in the executor. + /** + * Similar to the previous overload, but a callback will automatically be called when a response is received. + * + * If the callback is never called, because we never got a reply for the service server, remove_pending_request() + * has to be called with the returned request id or prune_pending_requests(). + * Not doing so will make the `Client` instance use more memory each time a response is not + * received from the service server. + * In this case, it's convenient to setup a timer to cleanup the pending requests. + * See for example the `examples_rclcpp_async_client` package in https://github.com/ros2/examples. + * + * \param[in] request request to be send. + * \param[in] cb callback that will be called when we get a response for this request. + * \return the request id representing the request just sent. + */ template< typename CallbackT, typename std::enable_if< @@ -229,23 +651,28 @@ class Client : public ClientBase >::value >::type * = nullptr > - SharedFuture + SharedFutureAndRequestId async_send_request(SharedRequest request, CallbackT && cb) { - std::lock_guard lock(pending_requests_mutex_); - int64_t sequence_number; - rcl_ret_t ret = rcl_send_request(get_client_handle(), request.get(), &sequence_number); - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send request"); - } - - SharedPromise call_promise = std::make_shared(); - SharedFuture f(call_promise->get_future()); - pending_requests_[sequence_number] = - std::make_tuple(call_promise, std::forward(cb), f); - return f; + Promise promise; + auto shared_future = promise.get_future().share(); + auto req_id = async_send_request_impl( + *request, + std::make_tuple( + CallbackType{std::forward(cb)}, + shared_future, + std::move(promise))); + return SharedFutureAndRequestId{std::move(shared_future), req_id}; } + /// Send a request to the service server and schedule a callback in the executor. + /** + * Similar to the previous method, but you can get both the request and response in the callback. + * + * \param[in] request request to be send. + * \param[in] cb callback that will be called when we get a response for this request. + * \return the request id representing the request just sent. + */ template< typename CallbackT, typename std::enable_if< @@ -255,28 +682,188 @@ class Client : public ClientBase >::value >::type * = nullptr > - SharedFutureWithRequest + SharedFutureWithRequestAndRequestId async_send_request(SharedRequest request, CallbackT && cb) { - SharedPromiseWithRequest promise = std::make_shared(); - SharedFutureWithRequest future_with_request(promise->get_future()); + PromiseWithRequest promise; + auto shared_future = promise.get_future().share(); + auto req_id = async_send_request_impl( + *request, + std::make_tuple( + CallbackWithRequestType{std::forward(cb)}, + request, + shared_future, + std::move(promise))); + return SharedFutureWithRequestAndRequestId{std::move(shared_future), req_id}; + } - auto wrapping_cb = [future_with_request, promise, request, &cb](SharedFuture future) { - auto response = future.get(); - promise->set_value(std::make_pair(request, response)); - cb(future_with_request); - }; + /// Cleanup a pending request. + /** + * This notifies the client that we have waited long enough for a response from the server + * to come, we have given up and we are not waiting for a response anymore. + * + * Not calling this will make the client start using more memory for each request + * that never got a reply from the server. + * + * \param[in] request_id request id returned by async_send_request(). + * \return true when a pending request was removed, false if not (e.g. a response was received). + */ + bool + remove_pending_request(int64_t request_id) + { + std::lock_guard guard(pending_requests_mutex_); + return pending_requests_.erase(request_id) != 0u; + } - async_send_request(request, wrapping_cb); + /// Cleanup a pending request. + /** + * Convenient overload, same as: + * + * `Client::remove_pending_request(this, future.request_id)`. + */ + bool + remove_pending_request(const FutureAndRequestId & future) + { + return this->remove_pending_request(future.request_id); + } - return future_with_request; + /// Cleanup a pending request. + /** + * Convenient overload, same as: + * + * `Client::remove_pending_request(this, future.request_id)`. + */ + bool + remove_pending_request(const SharedFutureAndRequestId & future) + { + return this->remove_pending_request(future.request_id); + } + + /// Cleanup a pending request. + /** + * Convenient overload, same as: + * + * `Client::remove_pending_request(this, future.request_id)`. + */ + bool + remove_pending_request(const SharedFutureWithRequestAndRequestId & future) + { + return this->remove_pending_request(future.request_id); + } + + /// Clean all pending requests. + /** + * \return number of pending requests that were removed. + */ + size_t + prune_pending_requests() + { + std::lock_guard guard(pending_requests_mutex_); + auto ret = pending_requests_.size(); + pending_requests_.clear(); + return ret; + } + + /// Clean all pending requests older than a time_point. + /** + * \param[in] time_point Requests that were sent before this point are going to be removed. + * \param[inout] pruned_requests Removed requests id will be pushed to the vector + * if a pointer is provided. + * \return number of pending requests that were removed. + */ + template> + size_t + prune_requests_older_than( + std::chrono::time_point time_point, + std::vector * pruned_requests = nullptr) + { + return detail::prune_requests_older_than_impl( + pending_requests_, + pending_requests_mutex_, + time_point, + pruned_requests); + } + + /// Configure client introspection. + /** + * \param[in] clock clock to use to generate introspection timestamps + * \param[in] qos_service_event_pub QoS settings to use when creating the introspection publisher + * \param[in] introspection_state the state to set introspection to + */ + void + configure_introspection( + Clock::SharedPtr clock, const QoS & qos_service_event_pub, + rcl_service_introspection_state_t introspection_state) + { + rcl_publisher_options_t pub_opts = rcl_publisher_get_default_options(); + pub_opts.qos = qos_service_event_pub.get_rmw_qos_profile(); + + rcl_ret_t ret = rcl_client_configure_service_introspection( + client_handle_.get(), + node_handle_.get(), + clock->get_clock_handle(), + srv_type_support_handle_, + pub_opts, + introspection_state); + + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to configure client introspection"); + } + } + +protected: + using CallbackTypeValueVariant = std::tuple; + using CallbackWithRequestTypeValueVariant = std::tuple< + CallbackWithRequestType, SharedRequest, SharedFutureWithRequest, PromiseWithRequest>; + + using CallbackInfoVariant = std::variant< + std::promise, + CallbackTypeValueVariant, + CallbackWithRequestTypeValueVariant>; + + int64_t + async_send_request_impl(const Request & request, CallbackInfoVariant value) + { + int64_t sequence_number; + std::lock_guard lock(pending_requests_mutex_); + rcl_ret_t ret = rcl_send_request(get_client_handle().get(), &request, &sequence_number); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send request"); + } + pending_requests_.try_emplace( + sequence_number, + std::make_pair(std::chrono::system_clock::now(), std::move(value))); + return sequence_number; + } + + std::optional + get_and_erase_pending_request(int64_t request_number) + { + std::unique_lock lock(pending_requests_mutex_); + auto it = this->pending_requests_.find(request_number); + if (it == this->pending_requests_.end()) { + RCUTILS_LOG_DEBUG_NAMED( + "rclcpp", + "Received invalid sequence number. Ignoring..."); + return std::nullopt; + } + std::optional value = std::move(it->second.second); + this->pending_requests_.erase(request_number); + return value; } -private: RCLCPP_DISABLE_COPY(Client) - std::map> pending_requests_; + std::unordered_map< + int64_t, + std::pair< + std::chrono::time_point, + CallbackInfoVariant>> + pending_requests_; std::mutex pending_requests_mutex_; + +private: + const rosidl_service_type_support_t * srv_type_support_handle_; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/clock.hpp b/rclcpp/include/rclcpp/clock.hpp index 84ddcd1c1c..2050bf53e6 100644 --- a/rclcpp/include/rclcpp/clock.hpp +++ b/rclcpp/include/rclcpp/clock.hpp @@ -18,66 +18,37 @@ #include #include #include -#include +#include "rclcpp/contexts/default_context.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/time.hpp" #include "rclcpp/visibility_control.hpp" #include "rcl/time.h" +#include "rcutils/time.h" +#include "rcutils/types/rcutils_ret.h" namespace rclcpp { class TimeSource; -/// A struct to represent a timejump. -/** - * It represents the time jump duration and whether it changed clock type. - */ -struct TimeJump -{ - typedef enum ClockChange_t - { - ROS_TIME_NO_CHANGE, ///< The time type before and after the jump is ROS_TIME - ROS_TIME_ACTIVATED, ///< The time type switched to ROS_TIME from SYSTEM_TIME - ROS_TIME_DEACTIVATED, ///< The time type switched to SYSTEM_TIME from ROS_TIME - SYSTEM_TIME_NO_CHANGE ///< The time type before and after the jump is SYSTEM_TIME - } ClockChange_t; - - ClockChange_t jump_type_; ///< The change in clock_type if there is one. - rcl_duration_t delta_; ///< The change in time value. -}; - -/// A class to store a threshold for a TimeJump. -/** - * This class can be used to evaluate a time jump's magnitude. - */ -class JumpThreshold -{ -public: - uint64_t min_forward_; ///< The minimum jump forward to be considered exceeded.. - uint64_t min_backward_; ///< The minimum backwards jump to be considered exceeded. - bool on_clock_change_; ///< Whether to trigger on any clock type change. - - // Test if the threshold is exceeded by a TimeJump - RCLCPP_PUBLIC - bool - is_exceeded(const TimeJump & jump); -}; - class JumpHandler { public: RCLCPP_SMART_PTR_DEFINITIONS(JumpHandler) + + using pre_callback_t = std::function; + using post_callback_t = std::function; + JumpHandler( - std::function pre_callback, - std::function post_callback, - JumpThreshold & threshold); + pre_callback_t pre_callback, + post_callback_t post_callback, + const rcl_jump_threshold_t & threshold); - std::function pre_callback; - std::function post_callback; - JumpThreshold notice_threshold; + pre_callback_t pre_callback; + post_callback_t post_callback; + rcl_jump_threshold_t notice_threshold; }; class Clock @@ -85,59 +56,208 @@ class Clock public: RCLCPP_SMART_PTR_DEFINITIONS(Clock) + /// Default c'tor + /** + * Initializes the clock instance with the given clock_type. + * + * \param clock_type type of the clock. + * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw. + */ RCLCPP_PUBLIC explicit Clock(rcl_clock_type_t clock_type = RCL_SYSTEM_TIME); RCLCPP_PUBLIC ~Clock(); + /** + * Returns current time from the time source specified by clock_type. + * + * \return current time. + * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw. + */ RCLCPP_PUBLIC Time - now(); + now() const; + + /** + * Sleep until a specified Time, according to clock type. + * + * Notes for RCL_ROS_TIME clock type: + * - Can sleep forever if ros time is active and received clock never reaches `until` + * - If ROS time enabled state changes during the sleep, this method will immediately return + * false. There is not a consistent choice of sleeping time when the time source changes, + * so this is up to the caller to call again if needed. + * + * \warning When using gcc < 10 or when using gcc >= 10 and pthreads lacks the function + * `pthread_cond_clockwait`, steady clocks may sleep using the system clock. + * If so, steady clock sleep times can be affected by system clock time jumps. + * Depending on the steady clock's epoch and resolution in comparison to the system clock's, + * an overflow when converting steady clock durations to system clock times may cause + * undefined behavior. + * For more info see these issues: + * https://gcc.gnu.org/bugzilla/show_bug.cgi?id=41861 + * https://gcc.gnu.org/bugzilla/show_bug.cgi?id=58931 + * + * \param until absolute time according to current clock type to sleep until. + * \param context the rclcpp context the clock should use to check that ROS is still initialized. + * \return true immediately if `until` is in the past + * \return true when the time `until` is reached + * \return false if time cannot be reached reliably, for example from shutdown or a change + * of time source. + * \throws std::runtime_error if the context is invalid + * \throws std::runtime_error if `until` has a different clock type from this clock + */ + RCLCPP_PUBLIC + bool + sleep_until( + Time until, + Context::SharedPtr context = contexts::get_global_default_context()); + /** + * Sleep for a specified Duration. + * + * Equivalent to + * + * ```cpp + * clock->sleep_until(clock->now() + rel_time, context) + * ``` + * + * The function will return immediately if `rel_time` is zero or negative. + * + * \param rel_time the length of time to sleep for. + * \param context the rclcpp context the clock should use to check that ROS is still initialized. + * \return true when the end time is reached + * \return false if time cannot be reached reliably, for example from shutdown or a change + * of time source. + * \throws std::runtime_error if the context is invalid + */ + RCLCPP_PUBLIC + bool + sleep_for( + Duration rel_time, + Context::SharedPtr context = contexts::get_global_default_context()); + + /** + * Check if the clock is started. + * + * A started clock is a clock that reflects non-zero time. + * Typically a clock will be unstarted if it is using RCL_ROS_TIME with ROS time and + * nothing has been published on the clock topic yet. + * + * \return true if clock is started + * \throws std::runtime_error if the clock is not rcl_clock_valid + */ + RCLCPP_PUBLIC + bool + started(); + + /** + * Wait until clock to start. + * + * \rclcpp::Clock::started + * \param context the context to wait in + * \return true if clock was already started or became started + * \throws std::runtime_error if the context is invalid or clock is not rcl_clock_valid + */ + RCLCPP_PUBLIC + bool + wait_until_started(Context::SharedPtr context = contexts::get_global_default_context()); + + /** + * Wait for clock to start, with timeout. + * + * The timeout is waited in steady time. + * + * \rclcpp::Clock::started + * \param timeout the maximum time to wait for. + * \param context the context to wait in. + * \param wait_tick_ns the time to wait between each iteration of the wait loop (in nanoseconds). + * \return true if clock was or became valid + * \throws std::runtime_error if the context is invalid or clock is not rcl_clock_valid + */ + RCLCPP_PUBLIC + bool + wait_until_started( + const rclcpp::Duration & timeout, + Context::SharedPtr context = contexts::get_global_default_context(), + const rclcpp::Duration & wait_tick_ns = rclcpp::Duration(0, static_cast(1e7))); + + /** + * Returns the clock of the type `RCL_ROS_TIME` is active. + * + * \return true if the clock is active + * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw if + * the current clock does not have the clock_type `RCL_ROS_TIME`. + */ RCLCPP_PUBLIC bool ros_time_is_active(); + /** + * Cancels an ongoing or future sleep operation of one thread. + * + * This function can be used by one thread, to wakeup another thread that is + * blocked using any of the sleep_ or wait_ methods of this class. + */ + RCLCPP_PUBLIC + void + cancel_sleep_or_wait(); + + /// Return the rcl_clock_t clock handle + RCLCPP_PUBLIC + rcl_clock_t * + get_clock_handle() noexcept; + RCLCPP_PUBLIC rcl_clock_type_t - get_clock_type(); + get_clock_type() const noexcept; - // Add a callback to invoke if the jump threshold is exceeded. + /// Get the clock's mutex + RCLCPP_PUBLIC + std::mutex & + get_clock_mutex() noexcept; + + /// Add a callback to invoke if the jump threshold is exceeded. /** * These callback functions must remain valid as long as the * returned shared pointer is valid. + * + * Function will register callbacks to the callback queue. On time jump all + * callbacks will be executed whose threshold is greater than the time jump; + * The logic will first call selected pre_callbacks and then all selected + * post_callbacks. + * + * Function is only applicable if the clock_type is `RCL_ROS_TIME` + * + * \param pre_callback Must be non-throwing + * \param post_callback Must be non-throwing. + * \param threshold Callbacks will be triggered if the time jump is greater + * than the threshold. + * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw. + * \throws std::bad_alloc if the allocation of the JumpHandler fails. + * \warning the instance of the clock must remain valid as long as any created + * JumpHandler. */ RCLCPP_PUBLIC JumpHandler::SharedPtr create_jump_callback( - std::function pre_callback, - std::function post_callback, - JumpThreshold & threshold); + JumpHandler::pre_callback_t pre_callback, + JumpHandler::post_callback_t post_callback, + const rcl_jump_threshold_t & threshold); private: - // A method for TimeSource to get a list of callbacks to invoke while updating - RCLCPP_PUBLIC - std::vector - get_triggered_callback_handlers(const TimeJump & jump); - - // Invoke callbacks that are valid and outside threshold. - RCLCPP_PUBLIC - static void invoke_prejump_callbacks( - const std::vector & callbacks); - + // Invoke time jump callback RCLCPP_PUBLIC - static void invoke_postjump_callbacks( - const std::vector & callbacks, - const TimeJump & jump); + static void + on_time_jump( + const rcl_time_jump_t * time_jump, + bool before_jump, + void * user_data); - /// Internal storage backed by rcl - rcl_clock_t rcl_clock_; - friend TimeSource; /// Allow TimeSource to access the rcl_clock_ datatype. - rcl_allocator_t allocator_; + /// Private internal storage + class Impl; - std::mutex callback_list_mutex_; - std::vector> active_jump_handlers_; + std::shared_ptr impl_; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/context.hpp b/rclcpp/include/rclcpp/context.hpp index 79ea48383a..4417f4d675 100644 --- a/rclcpp/include/rclcpp/context.hpp +++ b/rclcpp/include/rclcpp/context.hpp @@ -15,34 +15,327 @@ #ifndef RCLCPP__CONTEXT_HPP_ #define RCLCPP__CONTEXT_HPP_ -#include +#include +#include #include #include +#include #include #include #include +#include #include +#include +#include +#include "rcl/context.h" +#include "rcl/guard_condition.h" +#include "rcl/wait.h" +#include "rclcpp/init_options.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/visibility_control.hpp" -#include "rmw/rmw.h" namespace rclcpp { -class Context +/// Thrown when init is called on an already initialized context. +class ContextAlreadyInitialized : public std::runtime_error +{ +public: + ContextAlreadyInitialized() + : std::runtime_error("context is already initialized") {} +}; + +/// Forward declare WeakContextsWrapper +class WeakContextsWrapper; + +class ShutdownCallbackHandle +{ + friend class Context; + +public: + using ShutdownCallbackType = std::function; + +private: + std::weak_ptr callback; +}; + +using OnShutdownCallbackHandle = ShutdownCallbackHandle; +using PreShutdownCallbackHandle = ShutdownCallbackHandle; + +/// Context which encapsulates shared state between nodes and other similar entities. +/** + * A context also represents the lifecycle between init and shutdown of rclcpp. + * Nodes may be attached to a particular context by passing to the rclcpp::Node + * constructor a rclcpp::NodeOptions instance in which the Context is set via + * rclcpp::NodeOptions::context. + * Nodes will be automatically removed from the context when destructed. + * Contexts may be shutdown by calling rclcpp::shutdown. + */ +class Context : public std::enable_shared_from_this { public: RCLCPP_SMART_PTR_DEFINITIONS(Context) + /// Default constructor, after which the Context is still not "initialized". + /** + * Every context which is constructed is added to a global vector of contexts, + * which is used by the signal handler to conditionally shutdown each context + * on SIGINT. + * See the shutdown_on_signal option in the InitOptions class. + */ RCLCPP_PUBLIC Context(); + RCLCPP_PUBLIC + virtual + ~Context(); + + /// Initialize the context, and the underlying elements like the rcl context. + /** + * This method must be called before passing this context to things like the + * constructor of Node. + * It must also be called before trying to shutdown the context. + * + * Note that this function does not setup any signal handlers, so if you want + * it to be shutdown by the signal handler, then you need to either install + * them manually with rclcpp::install_signal_handlers() or use rclcpp::init(). + * In addition to installing the signal handlers, the shutdown_on_signal + * of the InitOptions needs to be `true` for this context to be shutdown by + * the signal handler, otherwise it will be passed over. + * + * After calling this method, shutdown() can be called to invalidate the + * context for derived entities, e.g. nodes, guard conditions, etc. + * However, the underlying rcl context is not finalized until this Context's + * destructor is called or this function is called again. + * Allowing this class to go out of scope and get destructed or calling this + * function a second time while derived entities are still using the context + * is undefined behavior and should be avoided. + * It's a good idea to not reuse context objects and instead create a new one + * each time you need to shutdown and init again. + * This allows derived entities to hold on to shard pointers to the first + * context object until they are done. + * + * This function is thread-safe. + * + * \param[in] argc number of arguments + * \param[in] argv argument array which may contain arguments intended for ROS + * \param[in] init_options initialization options for rclcpp and underlying layers + * \throw ContextAlreadyInitialized if called if init is called more than once + * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw. + * \throws std::runtime_error if the global logging configure mutex is NULL + * \throws exceptions::UnknownROSArgsError if there are unknown ROS arguments + */ + RCLCPP_PUBLIC + virtual + void + init( + int argc, + char const * const * argv, + const rclcpp::InitOptions & init_options = rclcpp::InitOptions()); + + /// Return true if the context is valid, otherwise false. + /** + * The context is valid if it has been initialized but not shutdown. + * + * This function is thread-safe. + * This function is lock free so long as pointers and uint64_t atomics are + * lock free. + * + * \return true if valid, otherwise false + */ + RCLCPP_PUBLIC + bool + is_valid() const; + + /// Return the init options used during init. + RCLCPP_PUBLIC + const rclcpp::InitOptions & + get_init_options() const; + + /// Return a copy of the init options used during init. + RCLCPP_PUBLIC + rclcpp::InitOptions + get_init_options(); + + /// Return actual domain id. + RCLCPP_PUBLIC + size_t + get_domain_id() const; + + /// Return the shutdown reason, or empty string if not shutdown. + /** + * This function is thread-safe. + */ + RCLCPP_PUBLIC + std::string + shutdown_reason() const; + + /// Shutdown the context, making it uninitialized and therefore invalid for derived entities. + /** + * Several things happen when the context is shutdown, in this order: + * + * - acquires a lock to prevent race conditions with init, on_shutdown, etc. + * - if the context is not initialized, return false + * - rcl_shutdown() is called on the internal rcl_context_t instance + * - the shutdown reason is set + * - each on_shutdown callback is called, in the order that they were added + * - interrupt blocking sleep_for() calls, so they return early due to shutdown + * - interrupt blocking executors and wait sets + * + * The underlying rcl context is not finalized by this function. + * + * This function is thread-safe. + * + * Note that if you override this method, but leave shutdown to be called in + * the destruction of this base class, it will not call the overridden + * version from your base class. + * So you need to ensure you call your class's shutdown() in its destructor. + * + * \param[in] reason the description of why shutdown happened + * \return true if shutdown was successful, false if context was already shutdown + * \throw various exceptions derived from rclcpp::exceptions::RCLError, if rcl_shutdown fails + */ + RCLCPP_PUBLIC + virtual + bool + shutdown(const std::string & reason); + + using OnShutdownCallback = OnShutdownCallbackHandle::ShutdownCallbackType; + + /// Add a on_shutdown callback to be called when shutdown is called for this context. + /** + * These callbacks will be called in the order they are added as the second + * to last step in shutdown(). + * + * When shutdown occurs due to the signal handler, these callbacks are run + * asynchronously in the dedicated singal handling thread. + * + * Also, shutdown() may be called from the destructor of this function. + * Therefore, it is not safe to throw exceptions from these callbacks. + * Instead, log errors or use some other mechanism to indicate an error has + * occurred. + * + * On shutdown callbacks may be registered before init and after shutdown, + * and persist on repeated init's. + * + * \param[in] callback the on shutdown callback to be registered + * \return the callback passed, for convenience when storing a passed lambda + */ + RCLCPP_PUBLIC + virtual + OnShutdownCallback + on_shutdown(OnShutdownCallback callback); + + /// Add a on_shutdown callback to be called when shutdown is called for this context. + /** + * These callbacks will be called in the order they are added as the second + * to last step in shutdown(). + * + * When shutdown occurs due to the signal handler, these callbacks are run + * asynchronously in the dedicated signal handling thread. + * + * Also, shutdown() may be called from the destructor of this function. + * Therefore, it is not safe to throw exceptions from these callbacks. + * Instead, log errors or use some other mechanism to indicate an error has + * occurred. + * + * On shutdown callbacks may be registered before init and after shutdown, + * and persist on repeated init's. + * + * \param[in] callback the on_shutdown callback to be registered + * \return the created callback handle + */ + RCLCPP_PUBLIC + virtual + OnShutdownCallbackHandle + add_on_shutdown_callback(OnShutdownCallback callback); + + /// Remove an registered on_shutdown callbacks. + /** + * \param[in] callback_handle the on_shutdown callback handle to be removed. + * \return true if the callback is found and removed, otherwise false. + */ + RCLCPP_PUBLIC + virtual + bool + remove_on_shutdown_callback(const OnShutdownCallbackHandle & callback_handle); + + using PreShutdownCallback = PreShutdownCallbackHandle::ShutdownCallbackType; + + /// Add a pre_shutdown callback to be called before shutdown is called for this context. + /** + * These callbacks will be called in the order they are added. + * + * When shutdown occurs due to the signal handler, these callbacks are run + * asynchronously in the dedicated signal handling thread. + * + * \param[in] callback the pre_shutdown callback to be registered + * \return the created callback handle + */ + RCLCPP_PUBLIC + virtual + PreShutdownCallbackHandle + add_pre_shutdown_callback(PreShutdownCallback callback); + + /// Remove an registered pre_shutdown callback. + /** + * \param[in] callback_handle the pre_shutdown callback handle to be removed. + * \return true if the callback is found and removed, otherwise false. + */ + RCLCPP_PUBLIC + virtual + bool + remove_pre_shutdown_callback(const PreShutdownCallbackHandle & callback_handle); + + /// Return the shutdown callbacks. + /** + * Returned callbacks are a copy of the registered callbacks. + */ + RCLCPP_PUBLIC + std::vector + get_on_shutdown_callbacks() const; + + /// Return the pre-shutdown callbacks. + /** + * Returned callbacks are a copy of the registered callbacks. + */ + RCLCPP_PUBLIC + std::vector + get_pre_shutdown_callbacks() const; + + /// Return the internal rcl context. + RCLCPP_PUBLIC + std::shared_ptr + get_rcl_context(); + + /// Sleep for a given period of time or until shutdown() is called. + /** + * This function can be interrupted early if: + * + * - this context is shutdown() + * - this context is destructed (resulting in shutdown) + * - this context has shutdown_on_signal=true and SIGINT/SIGTERM occurs (resulting in shutdown) + * - interrupt_all_sleep_for() is called + * + * \param[in] nanoseconds A std::chrono::duration representing how long to sleep for. + * \return true if the condition variable did not timeout, i.e. you were interrupted. + */ + RCLCPP_PUBLIC + bool + sleep_for(const std::chrono::nanoseconds & nanoseconds); + + /// Interrupt any blocking sleep_for calls, causing them to return immediately and return true. + RCLCPP_PUBLIC + void + interrupt_all_sleep_for(); + + /// Return a singleton instance for the SubContext type, constructing one if necessary. template std::shared_ptr get_sub_context(Args && ... args) { - std::lock_guard lock(mutex_); + std::lock_guard lock(sub_contexts_mutex_); std::type_index type_i(typeid(SubContext)); std::shared_ptr sub_context; @@ -62,13 +355,79 @@ class Context return sub_context; } +protected: + // Called by constructor and destructor to clean up by finalizing the + // shutdown rcl context and preparing for a new init cycle. + RCLCPP_PUBLIC + void + clean_up(); + private: RCLCPP_DISABLE_COPY(Context) + // This mutex is recursive so that the destructor can ensure atomicity + // between is_initialized and shutdown. + mutable std::recursive_mutex init_mutex_; + std::shared_ptr rcl_context_; + rclcpp::InitOptions init_options_; + std::string shutdown_reason_; + + // Keep shared ownership of the global logging mutex. + std::shared_ptr logging_mutex_; + std::unordered_map> sub_contexts_; - std::mutex mutex_; + // This mutex is recursive so that the constructor of a sub context may + // attempt to acquire another sub context. + std::recursive_mutex sub_contexts_mutex_; + + std::vector> on_shutdown_callbacks_; + mutable std::mutex on_shutdown_callbacks_mutex_; + + std::vector> pre_shutdown_callbacks_; + mutable std::mutex pre_shutdown_callbacks_mutex_; + + /// Condition variable for timed sleep (see sleep_for). + std::condition_variable interrupt_condition_variable_; + /// Mutex for protecting the global condition variable. + std::mutex interrupt_mutex_; + + /// Keep shared ownership of global vector of weak contexts + std::shared_ptr weak_contexts_; + + enum class ShutdownType + { + pre_shutdown, + on_shutdown + }; + + using ShutdownCallback = ShutdownCallbackHandle::ShutdownCallbackType; + + template + RCLCPP_LOCAL + ShutdownCallbackHandle + add_shutdown_callback( + ShutdownCallback callback); + + template + RCLCPP_LOCAL + bool + remove_shutdown_callback( + const ShutdownCallbackHandle & callback_handle); + + template + RCLCPP_LOCAL + std::vector + get_shutdown_callback() const; }; +/// Return a copy of the list of context shared pointers. +/** + * This function is thread-safe. + */ +RCLCPP_PUBLIC +std::vector +get_contexts(); + } // namespace rclcpp #endif // RCLCPP__CONTEXT_HPP_ diff --git a/rclcpp/include/rclcpp/contexts/default_context.hpp b/rclcpp/include/rclcpp/contexts/default_context.hpp index 2ed9bcbbf1..7c4ac63ef0 100644 --- a/rclcpp/include/rclcpp/contexts/default_context.hpp +++ b/rclcpp/include/rclcpp/contexts/default_context.hpp @@ -22,8 +22,6 @@ namespace rclcpp { namespace contexts { -namespace default_context -{ class DefaultContext : public rclcpp::Context { @@ -38,7 +36,6 @@ RCLCPP_PUBLIC DefaultContext::SharedPtr get_global_default_context(); -} // namespace default_context } // namespace contexts } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/copy_all_parameter_values.hpp b/rclcpp/include/rclcpp/copy_all_parameter_values.hpp new file mode 100644 index 0000000000..cc61b621e1 --- /dev/null +++ b/rclcpp/include/rclcpp/copy_all_parameter_values.hpp @@ -0,0 +1,82 @@ +// Copyright 2023 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT 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 RCLCPP__COPY_ALL_PARAMETER_VALUES_HPP_ +#define RCLCPP__COPY_ALL_PARAMETER_VALUES_HPP_ + +#include +#include + +#include "rcl_interfaces/srv/list_parameters.hpp" +#include "rcl_interfaces/msg/parameter_descriptor.hpp" +#include "rcl_interfaces/msg/set_parameters_result.hpp" + +#include "rclcpp/parameter.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/logging.hpp" + +namespace rclcpp +{ + +/** + * Copy all parameters from one source node to another destination node. + * May throw exceptions if parameters from source are uninitialized or undeclared. + * \param source Node to copy parameters from + * \param destination Node to copy parameters to + * \param override_existing_params Default false. Whether to override existing destination params + * if both the source and destination contain the same parameter. + */ +template +void +copy_all_parameter_values( + const NodeT1 & source, const NodeT2 & destination, const bool override_existing_params = false) +{ + using Parameters = std::vector; + using Descriptions = std::vector; + auto source_params = source->get_node_parameters_interface(); + auto dest_params = destination->get_node_parameters_interface(); + rclcpp::Logger logger = destination->get_node_logging_interface()->get_logger(); + + std::vector param_names = source_params->list_parameters({}, 0).names; + Parameters params = source_params->get_parameters(param_names); + Descriptions descriptions = source_params->describe_parameters(param_names); + + for (unsigned int idx = 0; idx != params.size(); idx++) { + if (!dest_params->has_parameter(params[idx].get_name())) { + dest_params->declare_parameter( + params[idx].get_name(), params[idx].get_parameter_value(), descriptions[idx]); + } else if (override_existing_params) { + try { + rcl_interfaces::msg::SetParametersResult result = + dest_params->set_parameters_atomically({params[idx]}); + if (!result.successful) { + // Parameter update rejected or read-only + RCLCPP_WARN( + logger, + "Unable to set parameter (%s): %s!", + params[idx].get_name().c_str(), result.reason.c_str()); + } + } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { + RCLCPP_WARN( + logger, + "Unable to set parameter (%s): incompatable parameter type (%s)!", + params[idx].get_name().c_str(), e.what()); + } + } + } +} + +} // namespace rclcpp + +#endif // RCLCPP__COPY_ALL_PARAMETER_VALUES_HPP_ diff --git a/rclcpp/include/rclcpp/create_client.hpp b/rclcpp/include/rclcpp/create_client.hpp new file mode 100644 index 0000000000..8cc73c7500 --- /dev/null +++ b/rclcpp/include/rclcpp/create_client.hpp @@ -0,0 +1,66 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__CREATE_CLIENT_HPP_ +#define RCLCPP__CREATE_CLIENT_HPP_ + +#include +#include + +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_services_interface.hpp" +#include "rclcpp/qos.hpp" +#include "rmw/rmw.h" + +namespace rclcpp +{ +/// Create a service client with a given type. +/** + * \param[in] node_base NodeBaseInterface implementation of the node on which + * to create the client. + * \param[in] node_graph NodeGraphInterface implementation of the node on which + * to create the client. + * \param[in] node_services NodeServicesInterface implementation of the node on + * which to create the client. + * \param[in] service_name The name on which the service is accessible. + * \param[in] qos Quality of service profile for client. + * \param[in] group Callback group to handle the reply to service calls. + * \return Shared pointer to the created client. + */ +template +typename rclcpp::Client::SharedPtr +create_client( + std::shared_ptr node_base, + std::shared_ptr node_graph, + std::shared_ptr node_services, + const std::string & service_name, + const rclcpp::QoS & qos = rclcpp::ServicesQoS(), + rclcpp::CallbackGroup::SharedPtr group = nullptr) +{ + rcl_client_options_t options = rcl_client_get_default_options(); + options.qos = qos.get_rmw_qos_profile(); + + auto cli = rclcpp::Client::make_shared( + node_base.get(), + node_graph, + service_name, + options); + + auto cli_base_ptr = std::dynamic_pointer_cast(cli); + node_services->add_client(cli_base_ptr, group); + return cli; +} +} // namespace rclcpp + +#endif // RCLCPP__CREATE_CLIENT_HPP_ diff --git a/rclcpp/include/rclcpp/create_generic_client.hpp b/rclcpp/include/rclcpp/create_generic_client.hpp new file mode 100644 index 0000000000..eade7bd9f1 --- /dev/null +++ b/rclcpp/include/rclcpp/create_generic_client.hpp @@ -0,0 +1,90 @@ +// Copyright 2023 Sony Group Corporation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT 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 RCLCPP__CREATE_GENERIC_CLIENT_HPP_ +#define RCLCPP__CREATE_GENERIC_CLIENT_HPP_ + +#include +#include + +#include "rclcpp/generic_client.hpp" +#include "rclcpp/node_interfaces/get_node_base_interface.hpp" +#include "rclcpp/node_interfaces/get_node_graph_interface.hpp" +#include "rclcpp/node_interfaces/get_node_services_interface.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_graph_interface.hpp" +#include "rclcpp/node_interfaces/node_services_interface.hpp" +#include "rclcpp/qos.hpp" + +namespace rclcpp +{ +/// Create a generic service client with a name of given type. +/** + * \param[in] node_base NodeBaseInterface implementation of the node on which + * to create the client. + * \param[in] node_graph NodeGraphInterface implementation of the node on which + * to create the client. + * \param[in] node_services NodeServicesInterface implementation of the node on + * which to create the client. + * \param[in] service_name The name on which the service is accessible. + * \param[in] service_type The name of service type, e.g. "test_msgs/srv/BasicTypes" + * \param[in] qos Quality of service profile for client. + * \param[in] group Callback group to handle the reply to service calls. + * \return Shared pointer to the created client. + */ +RCLCPP_PUBLIC +rclcpp::GenericClient::SharedPtr +create_generic_client( + std::shared_ptr node_base, + std::shared_ptr node_graph, + std::shared_ptr node_services, + const std::string & service_name, + const std::string & service_type, + const rclcpp::QoS & qos = rclcpp::ServicesQoS(), + rclcpp::CallbackGroup::SharedPtr group = nullptr); + +/// Create a generic service client with a name of given type. +/** + * The NodeT type needs to have NodeBaseInterface implementation, NodeGraphInterface implementation + * and NodeServicesInterface implementation of the node which to create the client. + * + * \param[in] node The node on which to create the client. + * \param[in] service_name The name on which the service is accessible. + * \param[in] service_type The name of service type, e.g. "test_msgs/srv/BasicTypes" + * \param[in] qos Quality of service profile for client. + * \param[in] group Callback group to handle the reply to service calls. + * \return Shared pointer to the created client. + */ +template +rclcpp::GenericClient::SharedPtr +create_generic_client( + NodeT node, + const std::string & service_name, + const std::string & service_type, + const rclcpp::QoS & qos = rclcpp::ServicesQoS(), + rclcpp::CallbackGroup::SharedPtr group = nullptr) +{ + return create_generic_client( + rclcpp::node_interfaces::get_node_base_interface(node), + rclcpp::node_interfaces::get_node_graph_interface(node), + rclcpp::node_interfaces::get_node_services_interface(node), + service_name, + service_type, + qos, + group + ); +} +} // namespace rclcpp + +#endif // RCLCPP__CREATE_GENERIC_CLIENT_HPP_ diff --git a/rclcpp/include/rclcpp/create_generic_publisher.hpp b/rclcpp/include/rclcpp/create_generic_publisher.hpp new file mode 100644 index 0000000000..296446f7b9 --- /dev/null +++ b/rclcpp/include/rclcpp/create_generic_publisher.hpp @@ -0,0 +1,69 @@ +// Copyright 2020, Apex.AI 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 RCLCPP__CREATE_GENERIC_PUBLISHER_HPP_ +#define RCLCPP__CREATE_GENERIC_PUBLISHER_HPP_ + +#include +#include +#include + +#include "rclcpp/generic_publisher.hpp" +#include "rclcpp/node_interfaces/node_topics_interface.hpp" +#include "rclcpp/publisher_options.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/typesupport_helpers.hpp" + +namespace rclcpp +{ + +/// Create and return a GenericPublisher. +/** + * The returned pointer will never be empty, but this function can throw various exceptions, for + * instance when the message's package can not be found on the AMENT_PREFIX_PATH. + * + * \param topics_interface NodeTopicsInterface pointer used in parts of the setup + * \param topic_name Topic name + * \param topic_type Topic type + * \param qos %QoS settings + * \param options %Publisher options. + * Not all publisher options are currently respected, the only relevant options for this + * publisher are `event_callbacks`, `use_default_callbacks`, and `%callback_group`. + */ +template> +std::shared_ptr create_generic_publisher( + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr topics_interface, + const std::string & topic_name, + const std::string & topic_type, + const rclcpp::QoS & qos, + const rclcpp::PublisherOptionsWithAllocator & options = ( + rclcpp::PublisherOptionsWithAllocator() + ) +) +{ + auto ts_lib = rclcpp::get_typesupport_library(topic_type, "rosidl_typesupport_cpp"); + auto pub = std::make_shared( + topics_interface->get_node_base_interface(), + std::move(ts_lib), + topic_name, + topic_type, + qos, + options); + topics_interface->add_publisher(pub, options.callback_group); + return pub; +} + +} // namespace rclcpp + +#endif // RCLCPP__CREATE_GENERIC_PUBLISHER_HPP_ diff --git a/rclcpp/include/rclcpp/create_generic_service.hpp b/rclcpp/include/rclcpp/create_generic_service.hpp new file mode 100644 index 0000000000..9cb032ef76 --- /dev/null +++ b/rclcpp/include/rclcpp/create_generic_service.hpp @@ -0,0 +1,102 @@ +// Copyright 2024 Sony Group Corporation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT 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 RCLCPP__CREATE_GENERIC_SERVICE_HPP_ +#define RCLCPP__CREATE_GENERIC_SERVICE_HPP_ + +#include +#include +#include + +#include "rclcpp/generic_service.hpp" +#include "rclcpp/node_interfaces/get_node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/get_node_services_interface.hpp" +#include "rclcpp/node_interfaces/node_services_interface.hpp" +#include "rclcpp/visibility_control.hpp" +#include "rmw/rmw.h" + +namespace rclcpp +{ +/// Create a generic service with a given type. +/** + * \param[in] node_base NodeBaseInterface implementation of the node on which + * to create the generic service. + * \param[in] node_services NodeServicesInterface implementation of the node on + * which to create the service. + * \param[in] service_name The name on which the service is accessible. + * \param[in] service_type The name of service type, e.g. "std_srvs/srv/SetBool". + * \param[in] callback The callback to call when the service gets a request. + * \param[in] qos Quality of service profile for the service. + * \param[in] group Callback group to handle the reply to service calls. + * \return Shared pointer to the created service. + */ +template +typename rclcpp::GenericService::SharedPtr +create_generic_service( + std::shared_ptr node_base, + std::shared_ptr node_services, + const std::string & service_name, + const std::string & service_type, + CallbackT && callback, + const rclcpp::QoS & qos, + rclcpp::CallbackGroup::SharedPtr group) +{ + rclcpp::GenericServiceCallback any_service_callback; + any_service_callback.set(std::forward(callback)); + + rcl_service_options_t service_options = rcl_service_get_default_options(); + service_options.qos = qos.get_rmw_qos_profile(); + + auto serv = GenericService::make_shared( + node_base->get_shared_rcl_node_handle(), + service_name, service_type, any_service_callback, service_options); + auto serv_base_ptr = std::dynamic_pointer_cast(serv); + node_services->add_service(serv_base_ptr, group); + return serv; +} + +/// Create a generic service with a given type. +/** + * The NodeT type needs to have NodeBaseInterface implementation and NodeServicesInterface + * implementation of the node which to create the generic service. + * + * \param[in] node The node on which to create the generic service. + * \param[in] service_name The name on which the service is accessible. + * \param[in] service_type The name of service type, e.g. "std_srvs/srv/SetBool". + * \param[in] callback The callback to call when the service gets a request. + * \param[in] qos Quality of service profile for the service. + * \param[in] group Callback group to handle the reply to service calls. + * \return Shared pointer to the created service. + */ +template +typename rclcpp::GenericService::SharedPtr +create_generic_service( + NodeT node, + const std::string & service_name, + const std::string & service_type, + CallbackT && callback, + const rclcpp::QoS & qos, + rclcpp::CallbackGroup::SharedPtr group) +{ + return create_generic_service( + rclcpp::node_interfaces::get_node_base_interface(node), + rclcpp::node_interfaces::get_node_services_interface(node), + service_name, + service_type, + std::forward(callback), qos.get_rmw_qos_profile(), group); +} +} // namespace rclcpp + +#endif // RCLCPP__CREATE_GENERIC_SERVICE_HPP_ diff --git a/rclcpp/include/rclcpp/create_generic_subscription.hpp b/rclcpp/include/rclcpp/create_generic_subscription.hpp new file mode 100644 index 0000000000..c2549721b5 --- /dev/null +++ b/rclcpp/include/rclcpp/create_generic_subscription.hpp @@ -0,0 +1,88 @@ +// Copyright 2020, Apex.AI 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 RCLCPP__CREATE_GENERIC_SUBSCRIPTION_HPP_ +#define RCLCPP__CREATE_GENERIC_SUBSCRIPTION_HPP_ + +#include +#include +#include +#include + +#include "rcl/subscription.h" +#include "rclcpp/generic_subscription.hpp" +#include "rclcpp/node_interfaces/node_topics_interface.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/serialized_message.hpp" +#include "rclcpp/subscription_options.hpp" +#include "rclcpp/typesupport_helpers.hpp" + +namespace rclcpp +{ + +/// Create and return a GenericSubscription. +/** + * The returned pointer will never be empty, but this function can throw various exceptions, for + * instance when the message's package can not be found on the AMENT_PREFIX_PATH. + * + * \param topics_interface NodeTopicsInterface pointer used in parts of the setup. + * \param topic_name Topic name + * \param topic_type Topic type + * \param qos %QoS settings + * \param callback Callback for new messages of serialized form + * \param options %Publisher options. + * Not all publisher options are currently respected, the only relevant options for this + * publisher are `event_callbacks`, `use_default_callbacks`, and `%callback_group`. + */ +template< + typename CallbackT, + typename AllocatorT = std::allocator> +std::shared_ptr create_generic_subscription( + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr topics_interface, + const std::string & topic_name, + const std::string & topic_type, + const rclcpp::QoS & qos, + CallbackT && callback, + const rclcpp::SubscriptionOptionsWithAllocator & options = ( + rclcpp::SubscriptionOptionsWithAllocator() + ) +) +{ + auto ts_lib = rclcpp::get_typesupport_library( + topic_type, "rosidl_typesupport_cpp"); + + auto allocator = options.get_allocator(); + + using rclcpp::AnySubscriptionCallback; + AnySubscriptionCallback + any_subscription_callback(*allocator); + any_subscription_callback.set(std::forward(callback)); + + auto subscription = std::make_shared( + topics_interface->get_node_base_interface(), + std::move(ts_lib), + topic_name, + topic_type, + qos, + any_subscription_callback, + options); + + topics_interface->add_subscription(subscription, options.callback_group); + + return subscription; +} + +} // namespace rclcpp + +#endif // RCLCPP__CREATE_GENERIC_SUBSCRIPTION_HPP_ diff --git a/rclcpp/include/rclcpp/create_publisher.hpp b/rclcpp/include/rclcpp/create_publisher.hpp index f136f33653..f6088a33c3 100644 --- a/rclcpp/include/rclcpp/create_publisher.hpp +++ b/rclcpp/include/rclcpp/create_publisher.hpp @@ -17,34 +17,112 @@ #include #include +#include +#include "rclcpp/node_interfaces/get_node_topics_interface.hpp" #include "rclcpp/node_interfaces/node_topics_interface.hpp" +#include "rclcpp/node_options.hpp" #include "rclcpp/publisher_factory.hpp" +#include "rclcpp/publisher_options.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/qos_overriding_options.hpp" +#include "rclcpp/detail/qos_parameters.hpp" + #include "rmw/qos_profiles.h" namespace rclcpp { -template +namespace detail +{ +/// Create and return a publisher of the given MessageT type. +template< + typename MessageT, + typename AllocatorT = std::allocator, + typename PublisherT = rclcpp::Publisher, + typename NodeParametersT, + typename NodeTopicsT> std::shared_ptr create_publisher( - rclcpp::node_interfaces::NodeTopicsInterface * node_topics, + NodeParametersT & node_parameters, + NodeTopicsT & node_topics, const std::string & topic_name, - const rmw_qos_profile_t & qos_profile, - bool use_intra_process_comms, - std::shared_ptr allocator) + const rclcpp::QoS & qos, + const rclcpp::PublisherOptionsWithAllocator & options = ( + rclcpp::PublisherOptionsWithAllocator() + ) +) { - auto publisher_options = rcl_publisher_get_default_options(); - publisher_options.qos = qos_profile; + auto node_topics_interface = rclcpp::node_interfaces::get_node_topics_interface(node_topics); + const rclcpp::QoS & actual_qos = options.qos_overriding_options.get_policy_kinds().size() ? + rclcpp::detail::declare_qos_parameters( + options.qos_overriding_options, node_parameters, + node_topics_interface->resolve_topic_name(topic_name), + qos, rclcpp::detail::PublisherQosParametersTraits{}) : + qos; - auto pub = node_topics->create_publisher( + // Create the publisher. + auto pub = node_topics_interface->create_publisher( topic_name, - rclcpp::create_publisher_factory(allocator), - publisher_options, - use_intra_process_comms); - node_topics->add_publisher(pub); + rclcpp::create_publisher_factory(options), + actual_qos + ); + + // Add the publisher to the node topics interface. + node_topics_interface->add_publisher(pub, options.callback_group); + return std::dynamic_pointer_cast(pub); } +} // namespace detail + + +/// Create and return a publisher of the given MessageT type. +/** + * The NodeT type only needs to have a method called get_node_topics_interface() + * which returns a shared_ptr to a NodeTopicsInterface. + * + * In case `options.qos_overriding_options` is enabling qos parameter overrides, + * NodeT must also have a method called get_node_parameters_interface() + * which returns a shared_ptr to a NodeParametersInterface. + */ +template< + typename MessageT, + typename AllocatorT = std::allocator, + typename PublisherT = rclcpp::Publisher, + typename NodeT> +std::shared_ptr +create_publisher( + NodeT && node, + const std::string & topic_name, + const rclcpp::QoS & qos, + const rclcpp::PublisherOptionsWithAllocator & options = ( + rclcpp::PublisherOptionsWithAllocator() + ) +) +{ + return detail::create_publisher( + node, node, topic_name, qos, options); +} + +/// Create and return a publisher of the given MessageT type. +template< + typename MessageT, + typename AllocatorT = std::allocator, + typename PublisherT = rclcpp::Publisher> +std::shared_ptr +create_publisher( + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node_parameters, + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr & node_topics, + const std::string & topic_name, + const rclcpp::QoS & qos, + const rclcpp::PublisherOptionsWithAllocator & options = ( + rclcpp::PublisherOptionsWithAllocator() + ) +) +{ + return detail::create_publisher( + node_parameters, node_topics, topic_name, qos, options); +} } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/create_service.hpp b/rclcpp/include/rclcpp/create_service.hpp new file mode 100644 index 0000000000..42c253a526 --- /dev/null +++ b/rclcpp/include/rclcpp/create_service.hpp @@ -0,0 +1,84 @@ +// 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 RCLCPP__CREATE_SERVICE_HPP_ +#define RCLCPP__CREATE_SERVICE_HPP_ + +#include +#include +#include + +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_services_interface.hpp" +#include "rclcpp/visibility_control.hpp" +#include "rmw/rmw.h" + +namespace rclcpp +{ +/// Create a service with a given type. +/** + * \param[in] node_base NodeBaseInterface implementation of the node on which + * to create the service. + * \param[in] node_services NodeServicesInterface implementation of the node on + * which to create the service. + * \param[in] service_name The name on which the service is accessible. + * \param[in] callback The callback to call when the service gets a request. + * \param[in] qos Quality of service profile for the service. + * \param[in] group Callback group to handle the reply to service calls. + * \return Shared pointer to the created service. + */ +template +typename rclcpp::Service::SharedPtr +create_service( + std::shared_ptr node_base, + std::shared_ptr node_services, + const std::string & service_name, + CallbackT && callback, + const rclcpp::QoS & qos, + rclcpp::CallbackGroup::SharedPtr group) +{ + return create_service( + node_base, node_services, service_name, + std::forward(callback), qos.get_rmw_qos_profile(), group); +} + +/// Create a service with a given type. +/// \internal +template +typename rclcpp::Service::SharedPtr +create_service( + std::shared_ptr node_base, + std::shared_ptr node_services, + const std::string & service_name, + CallbackT && callback, + const rmw_qos_profile_t & qos_profile, + rclcpp::CallbackGroup::SharedPtr group) +{ + rclcpp::AnyServiceCallback any_service_callback; + any_service_callback.set(std::forward(callback)); + + rcl_service_options_t service_options = rcl_service_get_default_options(); + service_options.qos = qos_profile; + + auto serv = Service::make_shared( + node_base->get_shared_rcl_node_handle(), + service_name, any_service_callback, service_options); + auto serv_base_ptr = std::dynamic_pointer_cast(serv); + node_services->add_service(serv_base_ptr, group); + return serv; +} + +} // namespace rclcpp + +#endif // RCLCPP__CREATE_SERVICE_HPP_ diff --git a/rclcpp/include/rclcpp/create_subscription.hpp b/rclcpp/include/rclcpp/create_subscription.hpp index 5125d9b16f..3a1e4b1a13 100644 --- a/rclcpp/include/rclcpp/create_subscription.hpp +++ b/rclcpp/include/rclcpp/create_subscription.hpp @@ -15,47 +15,212 @@ #ifndef RCLCPP__CREATE_SUBSCRIPTION_HPP_ #define RCLCPP__CREATE_SUBSCRIPTION_HPP_ +#include +#include #include +#include #include #include +#include "rclcpp/detail/resolve_enable_topic_statistics.hpp" + +#include "rclcpp/node_interfaces/get_node_timers_interface.hpp" +#include "rclcpp/node_interfaces/get_node_topics_interface.hpp" +#include "rclcpp/node_interfaces/node_timers_interface.hpp" #include "rclcpp/node_interfaces/node_topics_interface.hpp" + +#include "rclcpp/create_publisher.hpp" +#include "rclcpp/create_timer.hpp" +#include "rclcpp/qos.hpp" #include "rclcpp/subscription_factory.hpp" +#include "rclcpp/subscription_options.hpp" +#include "rclcpp/timer.hpp" +#include "rclcpp/topic_statistics/subscription_topic_statistics.hpp" #include "rmw/qos_profiles.h" namespace rclcpp { -template -typename rclcpp::Subscription::SharedPtr +namespace detail +{ +template< + typename MessageT, + typename CallbackT, + typename AllocatorT, + typename SubscriptionT, + typename MessageMemoryStrategyT, + typename NodeParametersT, + typename NodeTopicsT +> +typename std::shared_ptr create_subscription( - rclcpp::node_interfaces::NodeTopicsInterface * node_topics, + NodeParametersT & node_parameters, + NodeTopicsT & node_topics, const std::string & topic_name, + const rclcpp::QoS & qos, CallbackT && callback, - const rmw_qos_profile_t & qos_profile, - rclcpp::callback_group::CallbackGroup::SharedPtr group, - bool ignore_local_publications, - bool use_intra_process_comms, - typename rclcpp::message_memory_strategy::MessageMemoryStrategy::SharedPtr - msg_mem_strat, - typename std::shared_ptr allocator) + const rclcpp::SubscriptionOptionsWithAllocator & options = ( + rclcpp::SubscriptionOptionsWithAllocator() + ), + typename MessageMemoryStrategyT::SharedPtr msg_mem_strat = ( + MessageMemoryStrategyT::create_default() + ) +) { - auto subscription_options = rcl_subscription_get_default_options(); - subscription_options.qos = qos_profile; - subscription_options.ignore_local_publications = ignore_local_publications; - - auto factory = - rclcpp::create_subscription_factory( - std::forward(callback), msg_mem_strat, allocator); - - auto sub = node_topics->create_subscription( - topic_name, - factory, - subscription_options, - use_intra_process_comms); - node_topics->add_subscription(sub, group); + using rclcpp::node_interfaces::get_node_topics_interface; + auto node_topics_interface = get_node_topics_interface(node_topics); + + std::shared_ptr + subscription_topic_stats = nullptr; + + if (rclcpp::detail::resolve_enable_topic_statistics( + options, + *node_topics_interface->get_node_base_interface())) + { + if (options.topic_stats_options.publish_period <= std::chrono::milliseconds(0)) { + throw std::invalid_argument( + "topic_stats_options.publish_period must be greater than 0, specified value of " + + std::to_string(options.topic_stats_options.publish_period.count()) + " ms"); + } + + std::shared_ptr> + publisher = rclcpp::detail::create_publisher( + node_parameters, + node_topics_interface, + options.topic_stats_options.publish_topic, + options.topic_stats_options.qos); + + subscription_topic_stats = + std::make_shared( + node_topics_interface->get_node_base_interface()->get_name(), publisher); + + std::weak_ptr< + rclcpp::topic_statistics::SubscriptionTopicStatistics + > weak_subscription_topic_stats(subscription_topic_stats); + auto sub_call_back = [weak_subscription_topic_stats]() { + auto subscription_topic_stats = weak_subscription_topic_stats.lock(); + if (subscription_topic_stats) { + subscription_topic_stats->publish_message_and_reset_measurements(); + } + }; + + auto node_timer_interface = node_topics_interface->get_node_timers_interface(); + + auto timer = create_wall_timer( + std::chrono::duration_cast( + options.topic_stats_options.publish_period), + sub_call_back, + options.callback_group, + node_topics_interface->get_node_base_interface(), + node_timer_interface + ); + + subscription_topic_stats->set_publisher_timer(timer); + } + + auto factory = rclcpp::create_subscription_factory( + std::forward(callback), + options, + msg_mem_strat, + subscription_topic_stats + ); + + const rclcpp::QoS & actual_qos = options.qos_overriding_options.get_policy_kinds().size() ? + rclcpp::detail::declare_qos_parameters( + options.qos_overriding_options, node_parameters, + node_topics_interface->resolve_topic_name(topic_name), + qos, rclcpp::detail::SubscriptionQosParametersTraits{}) : + qos; + + auto sub = node_topics_interface->create_subscription(topic_name, factory, actual_qos); + node_topics_interface->add_subscription(sub, options.callback_group); + return std::dynamic_pointer_cast(sub); } +} // namespace detail + +/// Create and return a subscription of the given MessageT type. +/** + * The NodeT type only needs to have a method called get_node_topics_interface() + * which returns a shared_ptr to a NodeTopicsInterface, or be a + * NodeTopicsInterface pointer itself. + * + * In case `options.qos_overriding_options` is enabling qos parameter overrides, + * NodeT must also have a method called get_node_parameters_interface() + * which returns a shared_ptr to a NodeParametersInterface. + * + * \tparam MessageT + * \tparam CallbackT + * \tparam AllocatorT + * \tparam SubscriptionT + * \tparam MessageMemoryStrategyT + * \tparam NodeT + * \param node + * \param topic_name + * \param qos + * \param callback + * \param options + * \param msg_mem_strat + * \return the created subscription + * \throws std::invalid_argument if topic statistics is enabled and the publish period is + * less than or equal to zero. + */ +template< + typename MessageT, + typename CallbackT, + typename AllocatorT = std::allocator, + typename SubscriptionT = rclcpp::Subscription, + typename MessageMemoryStrategyT = typename SubscriptionT::MessageMemoryStrategyType, + typename NodeT> +typename std::shared_ptr +create_subscription( + NodeT & node, + const std::string & topic_name, + const rclcpp::QoS & qos, + CallbackT && callback, + const rclcpp::SubscriptionOptionsWithAllocator & options = ( + rclcpp::SubscriptionOptionsWithAllocator() + ), + typename MessageMemoryStrategyT::SharedPtr msg_mem_strat = ( + MessageMemoryStrategyT::create_default() + ) +) +{ + return rclcpp::detail::create_subscription< + MessageT, CallbackT, AllocatorT, SubscriptionT, MessageMemoryStrategyT>( + node, node, topic_name, qos, std::forward(callback), options, msg_mem_strat); +} + +/// Create and return a subscription of the given MessageT type. +/** + * See \ref create_subscription(). + */ +template< + typename MessageT, + typename CallbackT, + typename AllocatorT = std::allocator, + typename SubscriptionT = rclcpp::Subscription, + typename MessageMemoryStrategyT = typename SubscriptionT::MessageMemoryStrategyType> +typename std::shared_ptr +create_subscription( + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node_parameters, + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr & node_topics, + const std::string & topic_name, + const rclcpp::QoS & qos, + CallbackT && callback, + const rclcpp::SubscriptionOptionsWithAllocator & options = ( + rclcpp::SubscriptionOptionsWithAllocator() + ), + typename MessageMemoryStrategyT::SharedPtr msg_mem_strat = ( + MessageMemoryStrategyT::create_default() + ) +) +{ + return rclcpp::detail::create_subscription< + MessageT, CallbackT, AllocatorT, SubscriptionT, MessageMemoryStrategyT>( + node_parameters, node_topics, topic_name, qos, + std::forward(callback), options, msg_mem_strat); +} } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/create_timer.hpp b/rclcpp/include/rclcpp/create_timer.hpp new file mode 100644 index 0000000000..64d5b8e322 --- /dev/null +++ b/rclcpp/include/rclcpp/create_timer.hpp @@ -0,0 +1,217 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__CREATE_TIMER_HPP_ +#define RCLCPP__CREATE_TIMER_HPP_ + +#include +#include +#include +#include +#include + +#include "rclcpp/duration.hpp" +#include "rclcpp/node_interfaces/get_node_base_interface.hpp" +#include "rclcpp/node_interfaces/get_node_clock_interface.hpp" +#include "rclcpp/node_interfaces/get_node_timers_interface.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_clock_interface.hpp" +#include "rclcpp/node_interfaces/node_timers_interface.hpp" + +namespace rclcpp +{ +namespace detail +{ +/// Perform a safe cast to a timer period in nanoseconds +/** + * + * \tparam DurationRepT + * \tparam DurationT + * \param period period to execute callback. This duration must be 0 <= period < nanoseconds::max() + * \return period, expressed as chrono::duration::nanoseconds + * \throws std::invalid_argument if period is negative or too large + */ +template +std::chrono::nanoseconds +safe_cast_to_period_in_ns(std::chrono::duration period) +{ + if (period < std::chrono::duration::zero()) { + throw std::invalid_argument{"timer period cannot be negative"}; + } + + // Casting to a double representation might lose precision and allow the check below to succeed + // but the actual cast to nanoseconds fail. Using 1 DurationT worth of nanoseconds less than max. + constexpr auto maximum_safe_cast_ns = + std::chrono::nanoseconds::max() - std::chrono::duration(1); + + // If period is greater than nanoseconds::max(), the duration_cast to nanoseconds will overflow + // a signed integer, which is undefined behavior. Checking whether any std::chrono::duration is + // greater than nanoseconds::max() is a difficult general problem. This is a more conservative + // version of Howard Hinnant's (the guy>) response here: + // https://stackoverflow.com/a/44637334/2089061 + // However, this doesn't solve the issue for all possible duration types of period. + // Follow-up issue: https://github.com/ros2/rclcpp/issues/1177 + constexpr auto ns_max_as_double = + std::chrono::duration_cast>( + maximum_safe_cast_ns); + if (period > ns_max_as_double) { + throw std::invalid_argument{ + "timer period must be less than std::chrono::nanoseconds::max()"}; + } + + const auto period_ns = std::chrono::duration_cast(period); + if (period_ns < std::chrono::nanoseconds::zero()) { + throw std::runtime_error{ + "Casting timer period to nanoseconds resulted in integer overflow."}; + } + + return period_ns; +} +} // namespace detail + +/// Create a timer with a given clock +/// \internal +template +typename rclcpp::TimerBase::SharedPtr +create_timer( + std::shared_ptr node_base, + std::shared_ptr node_timers, + rclcpp::Clock::SharedPtr clock, + rclcpp::Duration period, + CallbackT && callback, + rclcpp::CallbackGroup::SharedPtr group = nullptr, + bool autostart = true) +{ + return create_timer( + clock, + period.to_chrono(), + std::forward(callback), + group, + node_base.get(), + node_timers.get(), + autostart); +} + +/// Create a timer with a given clock +template +typename rclcpp::TimerBase::SharedPtr +create_timer( + NodeT node, + rclcpp::Clock::SharedPtr clock, + rclcpp::Duration period, + CallbackT && callback, + rclcpp::CallbackGroup::SharedPtr group = nullptr, + bool autostart = true) +{ + return create_timer( + clock, + period.to_chrono(), + std::forward(callback), + group, + rclcpp::node_interfaces::get_node_base_interface(node).get(), + rclcpp::node_interfaces::get_node_timers_interface(node).get(), + autostart); +} + +/// Convenience method to create a general timer with node resources. +/** + * + * \tparam DurationRepT + * \tparam DurationT + * \tparam CallbackT + * \param clock clock to be used + * \param period period to execute callback. This duration must be 0 <= period < nanoseconds::max() + * \param callback callback to execute via the timer period + * \param group callback group + * \param node_base node base interface + * \param node_timers node timer interface + * \param autostart defines if the timer should start it's countdown on initialization or not. + * \return shared pointer to a generic timer + * \throws std::invalid_argument if either clock, node_base or node_timers + * are nullptr, or period is negative or too large + */ +template +typename rclcpp::GenericTimer::SharedPtr +create_timer( + rclcpp::Clock::SharedPtr clock, + std::chrono::duration period, + CallbackT callback, + rclcpp::CallbackGroup::SharedPtr group, + node_interfaces::NodeBaseInterface * node_base, + node_interfaces::NodeTimersInterface * node_timers, + bool autostart = true) +{ + if (clock == nullptr) { + throw std::invalid_argument{"clock cannot be null"}; + } + if (node_base == nullptr) { + throw std::invalid_argument{"input node_base cannot be null"}; + } + if (node_timers == nullptr) { + throw std::invalid_argument{"input node_timers cannot be null"}; + } + + const std::chrono::nanoseconds period_ns = detail::safe_cast_to_period_in_ns(period); + + // Add a new generic timer. + auto timer = rclcpp::GenericTimer::make_shared( + std::move(clock), period_ns, std::move(callback), node_base->get_context(), autostart); + node_timers->add_timer(timer, group); + return timer; +} + +/// Convenience method to create a wall timer with node resources. +/** + * + * \tparam DurationRepT + * \tparam DurationT + * \tparam CallbackT + * \param period period to execute callback. This duration must be 0 <= period < nanoseconds::max() + * \param callback callback to execute via the timer period + * \param group callback group + * \param node_base node base interface + * \param node_timers node timer interface + * \return shared pointer to a wall timer + * \throws std::invalid_argument if either node_base or node_timers + * are null, or period is negative or too large + */ +template +typename rclcpp::WallTimer::SharedPtr +create_wall_timer( + std::chrono::duration period, + CallbackT callback, + rclcpp::CallbackGroup::SharedPtr group, + node_interfaces::NodeBaseInterface * node_base, + node_interfaces::NodeTimersInterface * node_timers, + bool autostart = true) +{ + if (node_base == nullptr) { + throw std::invalid_argument{"input node_base cannot be null"}; + } + + if (node_timers == nullptr) { + throw std::invalid_argument{"input node_timers cannot be null"}; + } + + const std::chrono::nanoseconds period_ns = detail::safe_cast_to_period_in_ns(period); + + // Add a new wall timer. + auto timer = rclcpp::WallTimer::make_shared( + period_ns, std::move(callback), node_base->get_context(), autostart); + node_timers->add_timer(timer, group); + return timer; +} +} // namespace rclcpp + +#endif // RCLCPP__CREATE_TIMER_HPP_ diff --git a/rclcpp/include/rclcpp/detail/README.md b/rclcpp/include/rclcpp/detail/README.md new file mode 100644 index 0000000000..a2712d6cd3 --- /dev/null +++ b/rclcpp/include/rclcpp/detail/README.md @@ -0,0 +1,3 @@ +Notice that headers in this folder should only provide symbols in the rclcpp::detail namespace. + +Also that these headers are not considered part of the public API and are subject to change without notice. diff --git a/rclcpp/include/rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp b/rclcpp/include/rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp new file mode 100644 index 0000000000..5f1a7e0df8 --- /dev/null +++ b/rclcpp/include/rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp @@ -0,0 +1,39 @@ +// 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 RCLCPP__DETAIL__ADD_GUARD_CONDITION_TO_RCL_WAIT_SET_HPP_ +#define RCLCPP__DETAIL__ADD_GUARD_CONDITION_TO_RCL_WAIT_SET_HPP_ + +#include "rclcpp/guard_condition.hpp" + +namespace rclcpp +{ +namespace detail +{ + +/// Adds the guard condition to a waitset +/** + * \param[in] wait_set reference to a wait set where to add the guard condition + * \param[in] guard_condition reference to the guard_condition to be added + */ +RCLCPP_PUBLIC +void +add_guard_condition_to_rcl_wait_set( + rcl_wait_set_t & wait_set, + const rclcpp::GuardCondition & guard_condition); + +} // namespace detail +} // namespace rclcpp + +#endif // RCLCPP__DETAIL__ADD_GUARD_CONDITION_TO_RCL_WAIT_SET_HPP_ diff --git a/rclcpp/include/rclcpp/detail/cpp_callback_trampoline.hpp b/rclcpp/include/rclcpp/detail/cpp_callback_trampoline.hpp new file mode 100644 index 0000000000..e9c07b71e0 --- /dev/null +++ b/rclcpp/include/rclcpp/detail/cpp_callback_trampoline.hpp @@ -0,0 +1,70 @@ +// 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 RCLCPP__DETAIL__CPP_CALLBACK_TRAMPOLINE_HPP_ +#define RCLCPP__DETAIL__CPP_CALLBACK_TRAMPOLINE_HPP_ + +#include + +namespace rclcpp +{ + +namespace detail +{ + +/// Trampoline pattern for wrapping std::function into C-style callbacks. +/** + * A common pattern in C is for a function to take a function pointer and a + * void pointer for "user data" which is passed to the function pointer when it + * is called from within C. + * + * It works by using the user data pointer to store a pointer to a + * std::function instance. + * So when called from C, this function will cast the user data to the right + * std::function type and call it. + * + * This should allow you to use free functions, lambdas with and without + * captures, and various kinds of std::bind instances. + * + * The interior of this function is likely to be executed within a C runtime, + * so no exceptions should be thrown at this point, and doing so results in + * undefined behavior. + * + * \tparam UserDataRealT Declared type of the passed function + * \tparam UserDataT Deduced type based on what is passed for user data, + * usually this type is either `void *` or `const void *`. + * \tparam Args the arguments being passed to the callback + * \tparam ReturnT the return type of this function and the callback, default void + * \param user_data the function pointer, possibly type erased + * \param args the arguments to be forwarded to the callback + * \returns whatever the callback returns, if anything + */ +template< + typename UserDataRealT, + typename UserDataT, + typename ... Args, + typename ReturnT = void +> +ReturnT +cpp_callback_trampoline(UserDataT user_data, Args ... args) noexcept +{ + auto & actual_callback = *static_cast(user_data); + return actual_callback(args ...); +} + +} // namespace detail + +} // namespace rclcpp + +#endif // RCLCPP__DETAIL__CPP_CALLBACK_TRAMPOLINE_HPP_ diff --git a/rclcpp/include/rclcpp/detail/qos_parameters.hpp b/rclcpp/include/rclcpp/detail/qos_parameters.hpp new file mode 100644 index 0000000000..651e58e7d2 --- /dev/null +++ b/rclcpp/include/rclcpp/detail/qos_parameters.hpp @@ -0,0 +1,340 @@ +// 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 RCLCPP__DETAIL__QOS_PARAMETERS_HPP_ +#define RCLCPP__DETAIL__QOS_PARAMETERS_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rcl_interfaces/msg/parameter_descriptor.hpp" +#include "rcpputils/pointer_traits.hpp" +#include "rmw/qos_string_conversions.h" + +#include "rclcpp/duration.hpp" +#include "rclcpp/node_interfaces/get_node_parameters_interface.hpp" +#include "rclcpp/node_interfaces/node_parameters_interface.hpp" +#include "rclcpp/qos_overriding_options.hpp" + +namespace rclcpp +{ +namespace detail +{ + +/// \internal Trait used to specialize `declare_qos_parameters()` for publishers. +struct PublisherQosParametersTraits +{ + static constexpr const char * entity_type() {return "publisher";} + static constexpr auto allowed_policies() + { + return std::array<::rclcpp::QosPolicyKind, 9> { + QosPolicyKind::AvoidRosNamespaceConventions, + QosPolicyKind::Deadline, + QosPolicyKind::Durability, + QosPolicyKind::History, + QosPolicyKind::Depth, + QosPolicyKind::Lifespan, + QosPolicyKind::Liveliness, + QosPolicyKind::LivelinessLeaseDuration, + QosPolicyKind::Reliability, + }; + } +}; + +/// \internal Trait used to specialize `declare_qos_parameters()` for subscriptions. +struct SubscriptionQosParametersTraits +{ + static constexpr const char * entity_type() {return "subscription";} + static constexpr auto allowed_policies() + { + return std::array<::rclcpp::QosPolicyKind, 8> { + QosPolicyKind::AvoidRosNamespaceConventions, + QosPolicyKind::Deadline, + QosPolicyKind::Durability, + QosPolicyKind::History, + QosPolicyKind::Depth, + QosPolicyKind::Liveliness, + QosPolicyKind::LivelinessLeaseDuration, + QosPolicyKind::Reliability, + }; + } +}; + +/// \internal Returns the given `policy` of the profile `qos` converted to a parameter value. +inline +::rclcpp::ParameterValue +get_default_qos_param_value(rclcpp::QosPolicyKind policy, const rclcpp::QoS & qos); + +/// \internal Modify the given `policy` in `qos` to be `value`. +inline +void +apply_qos_override( + rclcpp::QosPolicyKind policy, rclcpp::ParameterValue value, rclcpp::QoS & qos); + +inline +rclcpp::ParameterValue +declare_parameter_or_get( + rclcpp::node_interfaces::NodeParametersInterface & parameters_interface, + const std::string & param_name, + rclcpp::ParameterValue param_value, + rcl_interfaces::msg::ParameterDescriptor descriptor) +{ + try { + return parameters_interface.declare_parameter( + param_name, param_value, descriptor); + } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException &) { + return parameters_interface.get_parameter(param_name).get_parameter_value(); + } +} + +#ifdef DOXYGEN_ONLY +/// \internal Declare QoS parameters for the given entity. +/** + * \tparam NodeT Node pointer or reference type. + * \tparam EntityQosParametersTraits A class with two static methods: `entity_type()` and + * `allowed_policies()`. See `PublisherQosParametersTraits` and `SubscriptionQosParametersTraits`. + * \param options User provided options that indicate if QoS parameter overrides should be + * declared or not, which policy can have overrides, and optionally a callback to validate the profile. + * \param node Parameters will be declared using this node. + * \param topic_name Name of the topic of the entity. + * \param default_qos User provided qos. It will be used as a default for the parameters declared. + * \return qos profile based on the user provided parameter overrides. + */ +template +rclcpp::QoS + declare_qos_parameters( + const ::rclcpp::QosOverridingOptions & options, + NodeT & node, + const std::string & topic_name, + const ::rclcpp::QoS & default_qos, + EntityQosParametersTraits); + +#else + +template +std::enable_if_t< + (rclcpp::node_interfaces::has_node_parameters_interface< + decltype(std::declval::type>())>::value || + std::is_same, + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr>::value), + rclcpp::QoS> +declare_qos_parameters( + const ::rclcpp::QosOverridingOptions & options, + NodeT & node, + const std::string & topic_name, + const ::rclcpp::QoS & default_qos, + EntityQosParametersTraits) +{ + auto & parameters_interface = *rclcpp::node_interfaces::get_node_parameters_interface(node); + std::string param_prefix; + const auto & id = options.get_id(); + { + std::ostringstream oss{"qos_overrides.", std::ios::ate}; + oss << topic_name << "." << EntityQosParametersTraits::entity_type(); + if (!id.empty()) { + oss << "_" << id; + } + oss << "."; + param_prefix = oss.str(); + } + std::string param_description_suffix; + { + std::ostringstream oss{"} for ", std::ios::ate}; + oss << EntityQosParametersTraits::entity_type() << " {" << topic_name << "}"; + if (!id.empty()) { + oss << " with id {" << id << "}"; + } + param_description_suffix = oss.str(); + } + rclcpp::QoS qos = default_qos; + for (auto policy : EntityQosParametersTraits::allowed_policies()) { + if ( + std::count(options.get_policy_kinds().begin(), options.get_policy_kinds().end(), policy)) + { + std::ostringstream param_name{param_prefix, std::ios::ate}; + param_name << qos_policy_kind_to_cstr(policy); + std::ostringstream param_desciption{"qos policy {", std::ios::ate}; + param_desciption << qos_policy_kind_to_cstr(policy) << param_description_suffix; + rcl_interfaces::msg::ParameterDescriptor descriptor{}; + descriptor.description = param_desciption.str(); + descriptor.read_only = true; + auto value = declare_parameter_or_get( + parameters_interface, param_name.str(), + get_default_qos_param_value(policy, qos), descriptor); + ::rclcpp::detail::apply_qos_override(policy, value, qos); + } + } + const auto & validation_callback = options.get_validation_callback(); + if (validation_callback) { + auto result = validation_callback(qos); + if (!result.successful) { + throw rclcpp::exceptions::InvalidQosOverridesException{ + "validation callback failed: " + result.reason}; + } + } + return qos; +} + +// TODO(ivanpauno): This overload cannot declare the QoS parameters, as a node parameters interface +// was not provided. +template +std::enable_if_t< + !(rclcpp::node_interfaces::has_node_parameters_interface< + decltype(std::declval::type>())>::value || + std::is_same, + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr>::value), + rclcpp::QoS> +declare_qos_parameters( + const ::rclcpp::QosOverridingOptions & options, + NodeT &, + const std::string &, + const ::rclcpp::QoS & default_qos, + EntityQosParametersTraits) +{ + if (options.get_policy_kinds().size()) { + std::runtime_error exc{ + "passed non-default qos overriding options without providing a parameters interface"}; + throw exc; + } + return default_qos; +} + +#endif + +/// \internal Helper function to get a rmw qos policy value from a string. +#define RCLCPP_DETAIL_APPLY_QOS_OVERRIDE_FROM_PARAMETER_STRING( \ + kind_lower, kind_upper, parameter_value, rclcpp_qos) \ + do { \ + auto policy_string = (parameter_value).get(); \ + auto policy_value = rmw_qos_ ## kind_lower ## _policy_from_str(policy_string.c_str()); \ + if (RMW_QOS_POLICY_ ## kind_upper ## _UNKNOWN == policy_value) { \ + throw std::invalid_argument{"unknown QoS policy " #kind_lower " value: " + policy_string}; \ + } \ + ((rclcpp_qos).kind_lower)(policy_value); \ + } while (0) + +inline +void +apply_qos_override( + rclcpp::QosPolicyKind policy, rclcpp::ParameterValue value, rclcpp::QoS & qos) +{ + switch (policy) { + case QosPolicyKind::AvoidRosNamespaceConventions: + qos.avoid_ros_namespace_conventions(value.get()); + break; + case QosPolicyKind::Deadline: + qos.deadline(::rclcpp::Duration::from_nanoseconds(value.get())); + break; + case QosPolicyKind::Durability: + RCLCPP_DETAIL_APPLY_QOS_OVERRIDE_FROM_PARAMETER_STRING( + durability, DURABILITY, value, qos); + break; + case QosPolicyKind::History: + RCLCPP_DETAIL_APPLY_QOS_OVERRIDE_FROM_PARAMETER_STRING( + history, HISTORY, value, qos); + break; + case QosPolicyKind::Depth: + qos.get_rmw_qos_profile().depth = static_cast(value.get()); + break; + case QosPolicyKind::Lifespan: + qos.lifespan(::rclcpp::Duration::from_nanoseconds(value.get())); + break; + case QosPolicyKind::Liveliness: + RCLCPP_DETAIL_APPLY_QOS_OVERRIDE_FROM_PARAMETER_STRING( + liveliness, LIVELINESS, value, qos); + break; + case QosPolicyKind::LivelinessLeaseDuration: + qos.liveliness_lease_duration(::rclcpp::Duration::from_nanoseconds(value.get())); + break; + case QosPolicyKind::Reliability: + RCLCPP_DETAIL_APPLY_QOS_OVERRIDE_FROM_PARAMETER_STRING( + reliability, RELIABILITY, value, qos); + break; + default: + throw std::invalid_argument{"unknown QosPolicyKind"}; + } +} + +/// Convert `rmw_time_t` to `int64_t` that can be used as a parameter value. +inline +int64_t +rmw_duration_to_int64_t(rmw_time_t rmw_duration) +{ + return ::rclcpp::Duration( + static_cast(rmw_duration.sec), + static_cast(rmw_duration.nsec) + ).nanoseconds(); +} + +/// \internal Throw an exception if `policy_value_stringified` is NULL. +inline +const char * +check_if_stringified_policy_is_null(const char * policy_value_stringified, QosPolicyKind kind) +{ + if (!policy_value_stringified) { + std::ostringstream oss{"unknown value for policy kind {", std::ios::ate}; + oss << kind << "}"; + throw std::invalid_argument{oss.str()}; + } + return policy_value_stringified; +} + +inline +::rclcpp::ParameterValue +get_default_qos_param_value(rclcpp::QosPolicyKind kind, const rclcpp::QoS & qos) +{ + using ParameterValue = ::rclcpp::ParameterValue; + const auto & rmw_qos = qos.get_rmw_qos_profile(); + switch (kind) { + case QosPolicyKind::AvoidRosNamespaceConventions: + return ParameterValue(rmw_qos.avoid_ros_namespace_conventions); + case QosPolicyKind::Deadline: + return ParameterValue(rmw_duration_to_int64_t(rmw_qos.deadline)); + case QosPolicyKind::Durability: + return ParameterValue( + check_if_stringified_policy_is_null( + rmw_qos_durability_policy_to_str(rmw_qos.durability), kind)); + case QosPolicyKind::History: + return ParameterValue( + check_if_stringified_policy_is_null( + rmw_qos_history_policy_to_str(rmw_qos.history), kind)); + case QosPolicyKind::Depth: + return ParameterValue(static_cast(rmw_qos.depth)); + case QosPolicyKind::Lifespan: + return ParameterValue(rmw_duration_to_int64_t(rmw_qos.lifespan)); + case QosPolicyKind::Liveliness: + return ParameterValue( + check_if_stringified_policy_is_null( + rmw_qos_liveliness_policy_to_str(rmw_qos.liveliness), kind)); + case QosPolicyKind::LivelinessLeaseDuration: + return ParameterValue(rmw_duration_to_int64_t(rmw_qos.liveliness_lease_duration)); + case QosPolicyKind::Reliability: + return ParameterValue( + check_if_stringified_policy_is_null( + rmw_qos_reliability_policy_to_str(rmw_qos.reliability), kind)); + default: + throw std::invalid_argument{"unknown QoS policy kind"}; + } +} + +} // namespace detail +} // namespace rclcpp + +#endif // RCLCPP__DETAIL__QOS_PARAMETERS_HPP_ diff --git a/rclcpp/include/rclcpp/detail/resolve_enable_topic_statistics.hpp b/rclcpp/include/rclcpp/detail/resolve_enable_topic_statistics.hpp new file mode 100644 index 0000000000..e10a92ef10 --- /dev/null +++ b/rclcpp/include/rclcpp/detail/resolve_enable_topic_statistics.hpp @@ -0,0 +1,53 @@ +// Copyright 2020 Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__DETAIL__RESOLVE_ENABLE_TOPIC_STATISTICS_HPP_ +#define RCLCPP__DETAIL__RESOLVE_ENABLE_TOPIC_STATISTICS_HPP_ + +#include + +#include "rclcpp/topic_statistics_state.hpp" + +namespace rclcpp +{ +namespace detail +{ + +/// Return whether or not topic statistics is enabled, resolving "NodeDefault" if needed. +template +bool +resolve_enable_topic_statistics(const OptionsT & options, const NodeBaseT & node_base) +{ + bool topic_stats_enabled; + switch (options.topic_stats_options.state) { + case TopicStatisticsState::Enable: + topic_stats_enabled = true; + break; + case TopicStatisticsState::Disable: + topic_stats_enabled = false; + break; + case TopicStatisticsState::NodeDefault: + topic_stats_enabled = node_base.get_enable_topic_statistics_default(); + break; + default: + throw std::runtime_error("Unrecognized EnableTopicStatistics value"); + } + + return topic_stats_enabled; +} + +} // namespace detail +} // namespace rclcpp + +#endif // RCLCPP__DETAIL__RESOLVE_ENABLE_TOPIC_STATISTICS_HPP_ diff --git a/rclcpp/include/rclcpp/detail/resolve_intra_process_buffer_type.hpp b/rclcpp/include/rclcpp/detail/resolve_intra_process_buffer_type.hpp new file mode 100644 index 0000000000..234316a8f0 --- /dev/null +++ b/rclcpp/include/rclcpp/detail/resolve_intra_process_buffer_type.hpp @@ -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. + +#ifndef RCLCPP__DETAIL__RESOLVE_INTRA_PROCESS_BUFFER_TYPE_HPP_ +#define RCLCPP__DETAIL__RESOLVE_INTRA_PROCESS_BUFFER_TYPE_HPP_ + +#include + +#include "rclcpp/any_subscription_callback.hpp" +#include "rclcpp/intra_process_buffer_type.hpp" + +namespace rclcpp +{ + +namespace detail +{ + +/// Return the buffer type, resolving the "CallbackDefault" type to an actual type if needed. +template +rclcpp::IntraProcessBufferType +resolve_intra_process_buffer_type( + const rclcpp::IntraProcessBufferType buffer_type, + const rclcpp::AnySubscriptionCallback & any_subscription_callback) +{ + rclcpp::IntraProcessBufferType resolved_buffer_type = buffer_type; + + // If the user has not specified a type for the intra-process buffer, use the callback's type. + if (resolved_buffer_type == IntraProcessBufferType::CallbackDefault) { + if (any_subscription_callback.use_take_shared_method()) { + resolved_buffer_type = IntraProcessBufferType::SharedPtr; + } else { + resolved_buffer_type = IntraProcessBufferType::UniquePtr; + } + } + + return resolved_buffer_type; +} + +RCLCPP_PUBLIC +rclcpp::IntraProcessBufferType +resolve_intra_process_buffer_type( + const rclcpp::IntraProcessBufferType buffer_type); + +} // namespace detail + +} // namespace rclcpp + +#endif // RCLCPP__DETAIL__RESOLVE_INTRA_PROCESS_BUFFER_TYPE_HPP_ diff --git a/rclcpp/include/rclcpp/detail/resolve_use_intra_process.hpp b/rclcpp/include/rclcpp/detail/resolve_use_intra_process.hpp new file mode 100644 index 0000000000..9098bfe695 --- /dev/null +++ b/rclcpp/include/rclcpp/detail/resolve_use_intra_process.hpp @@ -0,0 +1,56 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__DETAIL__RESOLVE_USE_INTRA_PROCESS_HPP_ +#define RCLCPP__DETAIL__RESOLVE_USE_INTRA_PROCESS_HPP_ + +#include + +#include "rclcpp/intra_process_setting.hpp" + +namespace rclcpp +{ + +namespace detail +{ + +/// Return whether or not intra process is enabled, resolving "NodeDefault" if needed. +template +bool +resolve_use_intra_process(const OptionsT & options, const NodeBaseT & node_base) +{ + bool use_intra_process; + switch (options.use_intra_process_comm) { + case IntraProcessSetting::Enable: + use_intra_process = true; + break; + case IntraProcessSetting::Disable: + use_intra_process = false; + break; + case IntraProcessSetting::NodeDefault: + use_intra_process = node_base.get_use_intra_process_default(); + break; + default: + throw std::runtime_error("Unrecognized IntraProcessSetting value"); + break; + } + + return use_intra_process; +} + +} // namespace detail + +} // namespace rclcpp + +#endif // RCLCPP__DETAIL__RESOLVE_USE_INTRA_PROCESS_HPP_ diff --git a/rclcpp/include/rclcpp/detail/rmw_implementation_specific_payload.hpp b/rclcpp/include/rclcpp/detail/rmw_implementation_specific_payload.hpp new file mode 100644 index 0000000000..60bc930f9d --- /dev/null +++ b/rclcpp/include/rclcpp/detail/rmw_implementation_specific_payload.hpp @@ -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. + +#ifndef RCLCPP__DETAIL__RMW_IMPLEMENTATION_SPECIFIC_PAYLOAD_HPP_ +#define RCLCPP__DETAIL__RMW_IMPLEMENTATION_SPECIFIC_PAYLOAD_HPP_ + +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace detail +{ + +/// Mechanism for passing rmw implementation specific settings through the ROS interfaces. +class RCLCPP_PUBLIC RMWImplementationSpecificPayload +{ +public: + virtual + ~RMWImplementationSpecificPayload() = default; + + /// Return false if this class has not been customized, otherwise true. + /** + * It does this based on the value of the rmw implementation identifier that + * this class reports, and so it is important for a specialization of this + * class to override the get_rmw_implementation_identifier() method to return + * something other than nullptr. + */ + bool + has_been_customized() const; + + /// Derrived classes should override this and return the identifier of its rmw implementation. + virtual + const char * + get_implementation_identifier() const; +}; + +} // namespace detail +} // namespace rclcpp + +#endif // RCLCPP__DETAIL__RMW_IMPLEMENTATION_SPECIFIC_PAYLOAD_HPP_ diff --git a/rclcpp/include/rclcpp/detail/rmw_implementation_specific_publisher_payload.hpp b/rclcpp/include/rclcpp/detail/rmw_implementation_specific_publisher_payload.hpp new file mode 100644 index 0000000000..08925a17d1 --- /dev/null +++ b/rclcpp/include/rclcpp/detail/rmw_implementation_specific_publisher_payload.hpp @@ -0,0 +1,52 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__DETAIL__RMW_IMPLEMENTATION_SPECIFIC_PUBLISHER_PAYLOAD_HPP_ +#define RCLCPP__DETAIL__RMW_IMPLEMENTATION_SPECIFIC_PUBLISHER_PAYLOAD_HPP_ + +#include "rcl/publisher.h" + +#include "rclcpp/detail/rmw_implementation_specific_payload.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace detail +{ + +class RCLCPP_PUBLIC RMWImplementationSpecificPublisherPayload + : public RMWImplementationSpecificPayload +{ +public: + ~RMWImplementationSpecificPublisherPayload() override = default; + + /// Opportunity for a derived class to inject information into the rcl options. + /** + * This is called after the rcl_publisher_options_t has been prepared by + * rclcpp, but before rcl_publisher_init() is called. + * + * The passed option is the rmw_publisher_options field of the + * rcl_publisher_options_t that will be passed to rcl_publisher_init(). + * + * By default the options are unmodified. + */ + virtual + void + modify_rmw_publisher_options(rmw_publisher_options_t & rmw_publisher_options) const; +}; + +} // namespace detail +} // namespace rclcpp + +#endif // RCLCPP__DETAIL__RMW_IMPLEMENTATION_SPECIFIC_PUBLISHER_PAYLOAD_HPP_ diff --git a/rclcpp/include/rclcpp/detail/rmw_implementation_specific_subscription_payload.hpp b/rclcpp/include/rclcpp/detail/rmw_implementation_specific_subscription_payload.hpp new file mode 100644 index 0000000000..b909a35179 --- /dev/null +++ b/rclcpp/include/rclcpp/detail/rmw_implementation_specific_subscription_payload.hpp @@ -0,0 +1,53 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__DETAIL__RMW_IMPLEMENTATION_SPECIFIC_SUBSCRIPTION_PAYLOAD_HPP_ +#define RCLCPP__DETAIL__RMW_IMPLEMENTATION_SPECIFIC_SUBSCRIPTION_PAYLOAD_HPP_ + +#include "rcl/subscription.h" + +#include "rclcpp/detail/rmw_implementation_specific_payload.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace detail +{ + +/// Subscription payload that may be rmw implementation specific. +class RCLCPP_PUBLIC RMWImplementationSpecificSubscriptionPayload + : public RMWImplementationSpecificPayload +{ +public: + ~RMWImplementationSpecificSubscriptionPayload() override = default; + + /// Opportunity for a derived class to inject information into the rcl options. + /** + * This is called after the rcl_subscription_options_t has been prepared by + * rclcpp, but before rcl_subscription_init() is called. + * + * The passed option is the rmw_subscription_options field of the + * rcl_subscription_options_t that will be passed to rcl_subscription_init(). + * + * By default the options are unmodified. + */ + virtual + void + modify_rmw_subscription_options(rmw_subscription_options_t & rmw_subscription_options) const; +}; + +} // namespace detail +} // namespace rclcpp + +#endif // RCLCPP__DETAIL__RMW_IMPLEMENTATION_SPECIFIC_SUBSCRIPTION_PAYLOAD_HPP_ diff --git a/rclcpp/include/rclcpp/detail/subscription_callback_type_helper.hpp b/rclcpp/include/rclcpp/detail/subscription_callback_type_helper.hpp new file mode 100644 index 0000000000..0ebe6efd66 --- /dev/null +++ b/rclcpp/include/rclcpp/detail/subscription_callback_type_helper.hpp @@ -0,0 +1,166 @@ +// 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 RCLCPP__DETAIL__SUBSCRIPTION_CALLBACK_TYPE_HELPER_HPP_ +#define RCLCPP__DETAIL__SUBSCRIPTION_CALLBACK_TYPE_HELPER_HPP_ + +#include +#include + +#include "rclcpp/function_traits.hpp" +#include "rclcpp/message_info.hpp" + +namespace rclcpp +{ +namespace detail +{ + +/// Template metaprogramming helper used to resolve the callback argument into a std::function. +/** + * Sometimes the CallbackT is a std::function already, but it could also be a + * function pointer, lambda, bind, or some variant of those. + * In some cases, like a lambda where the arguments can be converted between one + * another, e.g. std::function)> and + * std::function)>, you need to make that not ambiguous + * by checking the arguments independently using function traits rather than + * rely on overloading the two std::function types. + * + * This issue, with the lambda's, can be demonstrated with this minimal program: + * + * \code{.cpp} + * #include + * #include + * + * void f(std::function)>) {} + * void f(std::function)>) {} + * + * int main() { + * // Fails to compile with an "ambiguous call" error. + * f([](std::shared_ptr){}); + * + * // Works. + * std::function)> cb = [](std::shared_ptr){}; + * f(cb); + * } + * \endcode + * + * If this program ever starts working in a future version of C++, this class + * may become redundant. + * + * This helper works by using SFINAE with rclcpp::function_traits::same_arguments<> + * to narrow down the exact std::function<> type for the given CallbackT. + */ +template +struct SubscriptionCallbackTypeHelper +{ + using callback_type = typename rclcpp::function_traits::as_std_function::type; +}; + +template +struct SubscriptionCallbackTypeHelper< + MessageT, + CallbackT, + typename std::enable_if_t< + rclcpp::function_traits::same_arguments< + CallbackT, + std::function)> + >::value + > +> +{ + using callback_type = std::function)>; +}; + +template +struct SubscriptionCallbackTypeHelper< + MessageT, + CallbackT, + typename std::enable_if_t< + rclcpp::function_traits::same_arguments< + CallbackT, + std::function, const rclcpp::MessageInfo &)> + >::value + > +> +{ + using callback_type = + std::function, const rclcpp::MessageInfo &)>; +}; + +template +struct SubscriptionCallbackTypeHelper< + MessageT, + CallbackT, + typename std::enable_if_t< + rclcpp::function_traits::same_arguments< + CallbackT, + std::function &)> + >::value + > +> +{ + using callback_type = std::function &)>; +}; + +template +struct SubscriptionCallbackTypeHelper< + MessageT, + CallbackT, + typename std::enable_if_t< + rclcpp::function_traits::same_arguments< + CallbackT, + std::function &, const rclcpp::MessageInfo &)> + >::value + > +> +{ + using callback_type = + std::function &, const rclcpp::MessageInfo &)>; +}; + +template +struct SubscriptionCallbackTypeHelper< + MessageT, + CallbackT, + typename std::enable_if_t< + rclcpp::function_traits::same_arguments< + CallbackT, + std::function)> + >::value + > +> +{ + using callback_type = std::function)>; +}; + +template +struct SubscriptionCallbackTypeHelper< + MessageT, + CallbackT, + typename std::enable_if_t< + rclcpp::function_traits::same_arguments< + CallbackT, + std::function, const rclcpp::MessageInfo &)> + >::value + > +> +{ + using callback_type = + std::function, const rclcpp::MessageInfo &)>; +}; + +} // namespace detail +} // namespace rclcpp + +#endif // RCLCPP__DETAIL__SUBSCRIPTION_CALLBACK_TYPE_HELPER_HPP_ diff --git a/rclcpp/include/rclcpp/detail/template_contains.hpp b/rclcpp/include/rclcpp/detail/template_contains.hpp new file mode 100644 index 0000000000..b60a75f36d --- /dev/null +++ b/rclcpp/include/rclcpp/detail/template_contains.hpp @@ -0,0 +1,47 @@ +// Copyright 2022 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__DETAIL__TEMPLATE_CONTAINS_HPP_ +#define RCLCPP__DETAIL__TEMPLATE_CONTAINS_HPP_ + +#include + +namespace rclcpp +{ +namespace detail +{ + +/// Template meta-function that checks if a given T is contained in the list Us. +template +struct template_contains; + +template +inline constexpr bool template_contains_v = template_contains::value; + +template +struct template_contains +{ + enum { value = (std::is_same_v|| template_contains_v)}; +}; + +template +struct template_contains +{ + enum { value = false }; +}; + +} // namespace detail +} // namespace rclcpp + +#endif // RCLCPP__DETAIL__TEMPLATE_CONTAINS_HPP_ diff --git a/rclcpp/include/rclcpp/detail/template_unique.hpp b/rclcpp/include/rclcpp/detail/template_unique.hpp new file mode 100644 index 0000000000..4986102d78 --- /dev/null +++ b/rclcpp/include/rclcpp/detail/template_unique.hpp @@ -0,0 +1,49 @@ +// Copyright 2022 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__DETAIL__TEMPLATE_UNIQUE_HPP_ +#define RCLCPP__DETAIL__TEMPLATE_UNIQUE_HPP_ + +#include + +#include "rclcpp/detail/template_contains.hpp" + +namespace rclcpp +{ +namespace detail +{ + +/// Template meta-function that checks if a given list Ts contains unique types. +template +struct template_unique; + +template +inline constexpr bool template_unique_v = template_unique::value; + +template +struct template_unique +{ + enum { value = !template_contains_v&& template_unique_v}; +}; + +template +struct template_unique +{ + enum { value = true }; +}; + +} // namespace detail +} // namespace rclcpp + +#endif // RCLCPP__DETAIL__TEMPLATE_UNIQUE_HPP_ diff --git a/rclcpp/include/rclcpp/detail/utilities.hpp b/rclcpp/include/rclcpp/detail/utilities.hpp new file mode 100644 index 0000000000..d1c5d2549a --- /dev/null +++ b/rclcpp/include/rclcpp/detail/utilities.hpp @@ -0,0 +1,40 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__DETAIL__UTILITIES_HPP_ +#define RCLCPP__DETAIL__UTILITIES_HPP_ + +#include "rclcpp/detail/utilities.hpp" + +#include +#include + +#include "rcl/allocator.h" +#include "rcl/arguments.h" + +namespace rclcpp +{ +namespace detail +{ + +std::vector +get_unparsed_ros_arguments( + int argc, char const * const * argv, + rcl_arguments_t * arguments, + rcl_allocator_t allocator); + +} // namespace detail +} // namespace rclcpp + +#endif // RCLCPP__DETAIL__UTILITIES_HPP_ diff --git a/rclcpp/include/rclcpp/duration.hpp b/rclcpp/include/rclcpp/duration.hpp index 102ab74938..6b48f441e3 100644 --- a/rclcpp/include/rclcpp/duration.hpp +++ b/rclcpp/include/rclcpp/duration.hpp @@ -18,85 +18,155 @@ #include #include "builtin_interfaces/msg/duration.hpp" +#include "builtin_interfaces/msg/time.hpp" #include "rcl/time.h" #include "rclcpp/visibility_control.hpp" namespace rclcpp { -class Duration +class RCLCPP_PUBLIC Duration { public: - RCLCPP_PUBLIC + /// Duration constructor. + /** + * Initializes the time values for seconds and nanoseconds individually. + * Large values for nsecs are wrapped automatically with the remainder added to secs. + * Both inputs must be integers. + * Seconds can be negative. + * + * \param seconds time in seconds + * \param nanoseconds time in nanoseconds + */ Duration(int32_t seconds, uint32_t nanoseconds); - RCLCPP_PUBLIC - explicit Duration( - rcl_duration_value_t nanoseconds); + /// Construct duration from the specified std::chrono::nanoseconds. + explicit Duration(std::chrono::nanoseconds nanoseconds); - RCLCPP_PUBLIC - explicit Duration( - std::chrono::nanoseconds nanoseconds); + // This constructor matches any std::chrono value other than nanoseconds + // intentionally not using explicit to create a conversion constructor + template + // cppcheck-suppress noExplicitConstructor + Duration(const std::chrono::duration & duration) // NOLINT(runtime/explicit) + : Duration(std::chrono::duration_cast(duration)) + {} - RCLCPP_PUBLIC - Duration( - const builtin_interfaces::msg::Duration & duration_msg); + // cppcheck-suppress noExplicitConstructor + Duration(const builtin_interfaces::msg::Duration & duration_msg); // NOLINT(runtime/explicit) - RCLCPP_PUBLIC + /// Time constructor + /** + * \param duration rcl_duration_t structure to copy. + */ explicit Duration(const rcl_duration_t & duration); - RCLCPP_PUBLIC Duration(const Duration & rhs); - RCLCPP_PUBLIC - virtual ~Duration(); + virtual ~Duration() = default; - RCLCPP_PUBLIC operator builtin_interfaces::msg::Duration() const; - RCLCPP_PUBLIC + // cppcheck-suppress operatorEq // this is a false positive from cppcheck Duration & operator=(const Duration & rhs); - RCLCPP_PUBLIC Duration & - operator=(const builtin_interfaces::msg::Duration & Duration_msg); + operator=(const builtin_interfaces::msg::Duration & duration_msg); - RCLCPP_PUBLIC bool operator==(const rclcpp::Duration & rhs) const; - RCLCPP_PUBLIC + bool + operator!=(const rclcpp::Duration & rhs) const; + bool operator<(const rclcpp::Duration & rhs) const; - RCLCPP_PUBLIC bool operator<=(const rclcpp::Duration & rhs) const; - RCLCPP_PUBLIC bool operator>=(const rclcpp::Duration & rhs) const; - RCLCPP_PUBLIC bool operator>(const rclcpp::Duration & rhs) const; - RCLCPP_PUBLIC Duration operator+(const rclcpp::Duration & rhs) const; - RCLCPP_PUBLIC + Duration & operator+=(const rclcpp::Duration & rhs); + Duration operator-(const rclcpp::Duration & rhs) const; - RCLCPP_PUBLIC + Duration & operator-=(const rclcpp::Duration & rhs); + + /// Get the maximum representable value. + /** + * \return the maximum representable value + */ + static + Duration + max(); + + Duration + operator*(double scale) const; + + Duration & + operator*=(double scale); + + /// Get duration in nanosecods + /** + * \return the duration in nanoseconds as a rcl_duration_value_t. + */ rcl_duration_value_t nanoseconds() const; + /// Get duration in seconds + /** + * \warning Depending on sizeof(double) there could be significant precision loss. + * When an exact time is required use nanoseconds() instead. + * \return the duration in seconds as a floating point number. + */ + double + seconds() const; + + /// Create a duration object from a floating point number representing seconds + static Duration + from_seconds(double seconds); + + /// Create a duration object from an integer number representing nanoseconds + static Duration + from_nanoseconds(rcl_duration_value_t nanoseconds); + + static Duration + from_rmw_time(rmw_time_t duration); + + /// Convert Duration into a std::chrono::Duration. + template + DurationT + to_chrono() const + { + return std::chrono::duration_cast(std::chrono::nanoseconds(this->nanoseconds())); + } + + /// Convert Duration into rmw_time_t. + rmw_time_t + to_rmw_time() const; + private: rcl_duration_t rcl_duration_; + + Duration() = default; }; +RCLCPP_PUBLIC +builtin_interfaces::msg::Time +operator+(const builtin_interfaces::msg::Time & lhs, const rclcpp::Duration & rhs); + +RCLCPP_PUBLIC +builtin_interfaces::msg::Time +operator-(const builtin_interfaces::msg::Time & lhs, const rclcpp::Duration & rhs); + } // namespace rclcpp #endif // RCLCPP__DURATION_HPP_ diff --git a/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message.hpp b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message.hpp new file mode 100644 index 0000000000..f9586aabb7 --- /dev/null +++ b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message.hpp @@ -0,0 +1,70 @@ +// Copyright 2023 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 RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_HPP_ +#define RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_HPP_ + +#include +#include +#include + +#include +#include + +#include "rclcpp/dynamic_typesupport/dynamic_message_type.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace dynamic_typesupport +{ + +/// Utility wrapper class for rosidl_dynamic_typesupport_dynamic_data_t +/// STUBBED OUT +class DynamicMessage : public std::enable_shared_from_this +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicMessage) + + RCLCPP_PUBLIC + virtual ~DynamicMessage(); + +protected: + // NOTE(methylDragon): + // This is just here to extend the lifetime of the serialization support + // It isn't actually used by the builder since the builder should compose its own support + // + // ... Though ideally it should be the exact same support as the one stored in the + // DynamicSerializationSupport + DynamicSerializationSupport::SharedPtr serialization_support_; + + rosidl_dynamic_typesupport_dynamic_data_t rosidl_dynamic_data_; + bool is_loaned_; + + // Used for returning the loaned value, and lifetime management + DynamicMessage::SharedPtr parent_data_; + +private: + RCLCPP_DISABLE_COPY(DynamicMessage) + + RCLCPP_PUBLIC + DynamicMessage(); +}; + +} // namespace dynamic_typesupport +} // namespace rclcpp + +#endif // RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_HPP_ diff --git a/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type.hpp b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type.hpp new file mode 100644 index 0000000000..93cbabdade --- /dev/null +++ b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type.hpp @@ -0,0 +1,64 @@ +// Copyright 2023 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 RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_HPP_ +#define RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_HPP_ + +#include +#include + +#include +#include + +#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace dynamic_typesupport +{ + +/// Utility wrapper class for `rosidl_dynamic_typesupport_dynamic_type_t` +/// STUBBED OUT +class DynamicMessageType : public std::enable_shared_from_this +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicMessageType) + + RCLCPP_PUBLIC + virtual ~DynamicMessageType(); + +protected: + // NOTE(methylDragon): + // This is just here to extend the lifetime of the serialization support + // It isn't actually used by the builder since the builder should compose its own support + // + // ... Though ideally it should be the exact same support as the one stored in the + // `DynamicSerializationSupport` + DynamicSerializationSupport::SharedPtr serialization_support_; + + rosidl_dynamic_typesupport_dynamic_type_t rosidl_dynamic_type_; + +private: + RCLCPP_DISABLE_COPY(DynamicMessageType) + + RCLCPP_PUBLIC + DynamicMessageType(); +}; + +} // namespace dynamic_typesupport +} // namespace rclcpp + +#endif // RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_HPP_ diff --git a/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type_builder.hpp b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type_builder.hpp new file mode 100644 index 0000000000..90a768ead9 --- /dev/null +++ b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type_builder.hpp @@ -0,0 +1,65 @@ +// Copyright 2023 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 RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_BUILDER_HPP_ +#define RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_BUILDER_HPP_ + +#include +#include + +#include +#include + +#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace dynamic_typesupport +{ + +/// Utility wrapper class for `rosidl_dynamic_typesupport_dynamic_type_builder_t *` +/// STUBBED OUT +class DynamicMessageTypeBuilder : public std::enable_shared_from_this +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicMessageTypeBuilder) + + RCLCPP_PUBLIC + virtual ~DynamicMessageTypeBuilder(); + +protected: + // NOTE(methylDragon): + // This is just here to extend the lifetime of the serialization support + // It isn't actually used by the builder since the builder should compose its own support + // + // ... Though ideally it should be the exact same support as the one stored in the + // `DynamicSerializationSupport` + DynamicSerializationSupport::SharedPtr serialization_support_; + + rosidl_dynamic_typesupport_dynamic_type_builder_t rosidl_dynamic_type_builder_; + +private: + RCLCPP_DISABLE_COPY(DynamicMessageTypeBuilder) + + RCLCPP_PUBLIC + DynamicMessageTypeBuilder(); +}; + +} // namespace dynamic_typesupport +} // namespace rclcpp + + +#endif // RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_BUILDER_HPP_ diff --git a/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type_support.hpp b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type_support.hpp new file mode 100644 index 0000000000..5b9f68532c --- /dev/null +++ b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type_support.hpp @@ -0,0 +1,67 @@ +// Copyright 2023 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 RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_SUPPORT_HPP_ +#define RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_SUPPORT_HPP_ + +#include + +#include +#include +#include +#include + +#include +#include + +#include "rclcpp/dynamic_typesupport/dynamic_message.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_message_type.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp" + +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace dynamic_typesupport +{ + +/// Utility wrapper class for `rosidl_message_type_support_t` containing managed +/// STUBBED OUT +class DynamicMessageTypeSupport : public std::enable_shared_from_this +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicMessageTypeSupport) + + RCLCPP_PUBLIC + virtual ~DynamicMessageTypeSupport(); + +protected: + DynamicSerializationSupport::SharedPtr serialization_support_; + DynamicMessageType::SharedPtr dynamic_message_type_; + DynamicMessage::SharedPtr dynamic_message_; + + rosidl_message_type_support_t rosidl_message_type_support_; + +private: + RCLCPP_DISABLE_COPY(DynamicMessageTypeSupport) + + RCLCPP_PUBLIC + DynamicMessageTypeSupport(); +}; + +} // namespace dynamic_typesupport +} // namespace rclcpp + +#endif // RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_SUPPORT_HPP_ diff --git a/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp new file mode 100644 index 0000000000..dde0710d25 --- /dev/null +++ b/rclcpp/include/rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp @@ -0,0 +1,60 @@ +// Copyright 2023 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 RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_SERIALIZATION_SUPPORT_HPP_ +#define RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_SERIALIZATION_SUPPORT_HPP_ + +#include +#include +#include + +#include +#include + +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace dynamic_typesupport +{ + +/// Utility wrapper class for rosidl_dynamic_typesupport_serialization_support_t +class DynamicSerializationSupport : public std::enable_shared_from_this +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(DynamicSerializationSupport) + + RCLCPP_PUBLIC + explicit DynamicSerializationSupport(rcl_allocator_t allocator = rcl_get_default_allocator()); + + RCLCPP_PUBLIC + DynamicSerializationSupport( + const std::string & serialization_library_name, + rcl_allocator_t allocator = rcl_get_default_allocator()); + + RCLCPP_PUBLIC + virtual ~DynamicSerializationSupport(); + +protected: + rosidl_dynamic_typesupport_serialization_support_t rosidl_serialization_support_; + +private: + RCLCPP_DISABLE_COPY(DynamicSerializationSupport) +}; + +} // namespace dynamic_typesupport +} // namespace rclcpp + +#endif // RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_SERIALIZATION_SUPPORT_HPP_ diff --git a/rclcpp/include/rclcpp/event.hpp b/rclcpp/include/rclcpp/event.hpp index 988dba29e2..715eb40ad5 100644 --- a/rclcpp/include/rclcpp/event.hpp +++ b/rclcpp/include/rclcpp/event.hpp @@ -29,17 +29,33 @@ class Event public: RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Event) + /// Default construct + /** + * Set the default value to false + */ RCLCPP_PUBLIC Event(); + /// Set the Event state value to true + /** + * \return The state value before the call. + */ RCLCPP_PUBLIC bool set(); + /// Get the state value of the Event + /** + * \return the Event state value + */ RCLCPP_PUBLIC bool check(); + /// Get the state value of the Event and set to false + /** + * \return the Event state value + */ RCLCPP_PUBLIC bool check_and_clear(); diff --git a/rclcpp/include/rclcpp/event_handler.hpp b/rclcpp/include/rclcpp/event_handler.hpp new file mode 100644 index 0000000000..61f198700c --- /dev/null +++ b/rclcpp/include/rclcpp/event_handler.hpp @@ -0,0 +1,314 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__EVENT_HANDLER_HPP_ +#define RCLCPP__EVENT_HANDLER_HPP_ + +#include +#include +#include +#include +#include + +#include "rcl/error_handling.h" +#include "rcl/event_callback.h" +#include "rmw/impl/cpp/demangle.hpp" +#include "rmw/incompatible_qos_events_statuses.h" +#include "rmw/events_statuses/incompatible_type.h" + +#include "rcutils/logging_macros.h" + +#include "rclcpp/detail/cpp_callback_trampoline.hpp" +#include "rclcpp/exceptions.hpp" +#include "rclcpp/function_traits.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp/waitable.hpp" + +namespace rclcpp +{ + +using QOSDeadlineRequestedInfo = rmw_requested_deadline_missed_status_t; +using QOSDeadlineOfferedInfo = rmw_offered_deadline_missed_status_t; +using QOSLivelinessChangedInfo = rmw_liveliness_changed_status_t; +using QOSLivelinessLostInfo = rmw_liveliness_lost_status_t; +using QOSMessageLostInfo = rmw_message_lost_status_t; +using QOSOfferedIncompatibleQoSInfo = rmw_offered_qos_incompatible_event_status_t; +using QOSRequestedIncompatibleQoSInfo = rmw_requested_qos_incompatible_event_status_t; + +using IncompatibleTypeInfo = rmw_incompatible_type_status_t; +using MatchedInfo = rmw_matched_status_t; + +using QOSDeadlineRequestedCallbackType = std::function; +using QOSDeadlineOfferedCallbackType = std::function; +using QOSLivelinessChangedCallbackType = std::function; +using QOSLivelinessLostCallbackType = std::function; +using QOSMessageLostCallbackType = std::function; +using QOSOfferedIncompatibleQoSCallbackType = std::function; +using QOSRequestedIncompatibleQoSCallbackType = + std::function; + +using IncompatibleTypeCallbackType = std::function; +using PublisherMatchedCallbackType = std::function; +using SubscriptionMatchedCallbackType = std::function; + +/// Contains callbacks for various types of events a Publisher can receive from the middleware. +struct PublisherEventCallbacks +{ + QOSDeadlineOfferedCallbackType deadline_callback; + QOSLivelinessLostCallbackType liveliness_callback; + QOSOfferedIncompatibleQoSCallbackType incompatible_qos_callback; + IncompatibleTypeCallbackType incompatible_type_callback; + PublisherMatchedCallbackType matched_callback; +}; + +/// Contains callbacks for non-message events that a Subscription can receive from the middleware. +struct SubscriptionEventCallbacks +{ + QOSDeadlineRequestedCallbackType deadline_callback; + QOSLivelinessChangedCallbackType liveliness_callback; + QOSRequestedIncompatibleQoSCallbackType incompatible_qos_callback; + QOSMessageLostCallbackType message_lost_callback; + IncompatibleTypeCallbackType incompatible_type_callback; + SubscriptionMatchedCallbackType matched_callback; +}; + +class UnsupportedEventTypeException : public exceptions::RCLErrorBase, public std::runtime_error +{ +public: + RCLCPP_PUBLIC + UnsupportedEventTypeException( + rcl_ret_t ret, + const rcl_error_state_t * error_state, + const std::string & prefix); + + RCLCPP_PUBLIC + UnsupportedEventTypeException( + const exceptions::RCLErrorBase & base_exc, + const std::string & prefix); +}; + +class EventHandlerBase : public Waitable +{ +public: + enum class EntityType : std::size_t + { + Event, + }; + + RCLCPP_PUBLIC + virtual ~EventHandlerBase(); + + /// Get the number of ready events + RCLCPP_PUBLIC + size_t + get_number_of_ready_events() override; + + /// Add the Waitable to a wait set. + RCLCPP_PUBLIC + void + add_to_wait_set(rcl_wait_set_t & wait_set) override; + + /// Check if the Waitable is ready. + RCLCPP_PUBLIC + bool + is_ready(const rcl_wait_set_t & wait_set) override; + + /// Set a callback to be called when each new event instance occurs. + /** + * The callback receives a size_t which is the number of events that occurred + * since the last time this callback was called. + * Normally this is 1, but can be > 1 if events occurred before any + * callback was set. + * + * The callback also receives an int identifier argument. + * This is needed because a Waitable may be composed of several distinct entities, + * such as subscriptions, services, etc. + * The application should provide a generic callback function that will be then + * forwarded by the waitable to all of its entities. + * Before forwarding, a different value for the identifier argument will be + * bond to the function. + * This implies that the provided callback can use the identifier to behave + * differently depending on which entity triggered the waitable to become ready. + * + * Since this callback is called from the middleware, you should aim to make + * it fast and not blocking. + * If you need to do a lot of work or wait for some other event, you should + * spin it off to another thread, otherwise you risk blocking the middleware. + * + * Calling it again will clear any previously set callback. + * + * An exception will be thrown if the callback is not callable. + * + * This function is thread-safe. + * + * If you want more information available in the callback, like the qos event + * or other information, you may use a lambda with captures or std::bind. + * + * \sa rmw_event_set_callback + * \sa rcl_event_set_callback + * + * \param[in] callback functor to be called when a new event occurs + */ + void + set_on_ready_callback(std::function callback) override + { + if (!callback) { + throw std::invalid_argument( + "The callback passed to set_on_ready_callback " + "is not callable."); + } + + // Note: we bind the int identifier argument to this waitable's entity types + auto new_callback = + [callback, this](size_t number_of_events) { + try { + callback(number_of_events, static_cast(EntityType::Event)); + } catch (const std::exception & exception) { + RCLCPP_ERROR_STREAM( + // TODO(wjwwood): get this class access to the node logger it is associated with + rclcpp::get_logger("rclcpp"), + "rclcpp::EventHandlerBase@" << this << + " caught " << rmw::impl::cpp::demangle(exception) << + " exception in user-provided callback for the 'on ready' callback: " << + exception.what()); + } catch (...) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("rclcpp"), + "rclcpp::EventHandlerBase@" << this << + " caught unhandled exception in user-provided callback " << + "for the 'on ready' callback"); + } + }; + + std::lock_guard lock(callback_mutex_); + + // Set it temporarily to the new callback, while we replace the old one. + // This two-step setting, prevents a gap where the old std::function has + // been replaced but the middleware hasn't been told about the new one yet. + set_on_new_event_callback( + rclcpp::detail::cpp_callback_trampoline, + static_cast(&new_callback)); + + // Store the std::function to keep it in scope, also overwrites the existing one. + on_new_event_callback_ = new_callback; + + // Set it again, now using the permanent storage. + set_on_new_event_callback( + rclcpp::detail::cpp_callback_trampoline< + decltype(on_new_event_callback_), const void *, size_t>, + static_cast(&on_new_event_callback_)); + } + + /// Unset the callback registered for new events, if any. + void + clear_on_ready_callback() override + { + std::lock_guard lock(callback_mutex_); + if (on_new_event_callback_) { + set_on_new_event_callback(nullptr, nullptr); + on_new_event_callback_ = nullptr; + } + } + +protected: + RCLCPP_PUBLIC + void + set_on_new_event_callback(rcl_event_callback_t callback, const void * user_data); + + std::recursive_mutex callback_mutex_; + std::function on_new_event_callback_{nullptr}; + + rcl_event_t event_handle_; + size_t wait_set_event_index_; +}; + +template +class EventHandler : public EventHandlerBase +{ +public: + template + EventHandler( + const EventCallbackT & callback, + InitFuncT init_func, + ParentHandleT parent_handle, + EventTypeEnum event_type) + : parent_handle_(parent_handle), event_callback_(callback) + { + event_handle_ = rcl_get_zero_initialized_event(); + rcl_ret_t ret = init_func(&event_handle_, parent_handle.get(), event_type); + if (ret != RCL_RET_OK) { + if (ret == RCL_RET_UNSUPPORTED) { + UnsupportedEventTypeException exc(ret, rcl_get_error_state(), "Failed to initialize event"); + rcl_reset_error(); + throw exc; + } else { + rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to initialize event"); + } + } + } + + ~EventHandler() + { + // Since the rmw event listener holds a reference to the + // "on ready" callback, we need to clear it on destruction of this class. + // This clearing is not needed for other rclcpp entities like pub/subs, since + // they do own the underlying rmw entities, which are destroyed + // on their rclcpp destructors, thus no risk of dangling pointers. + clear_on_ready_callback(); + } + + /// Take data so that the callback cannot be scheduled again + std::shared_ptr + take_data() override + { + EventCallbackInfoT callback_info; + rcl_ret_t ret = rcl_take_event(&event_handle_, &callback_info); + if (ret != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Couldn't take event info: %s", rcl_get_error_string().str); + return nullptr; + } + return std::static_pointer_cast(std::make_shared(callback_info)); + } + + std::shared_ptr + take_data_by_entity_id(size_t id) override + { + (void)id; + return take_data(); + } + + /// Execute any entities of the Waitable that are ready. + void + execute(const std::shared_ptr & data) override + { + if (!data) { + throw std::runtime_error("'data' is empty"); + } + auto callback_ptr = std::static_pointer_cast(data); + event_callback_(*callback_ptr); + callback_ptr.reset(); + } + +private: + using EventCallbackInfoT = typename std::remove_reference::template argument_type<0>>::type; + + ParentHandleT parent_handle_; + EventCallbackT event_callback_; +}; +} // namespace rclcpp + +#endif // RCLCPP__EVENT_HANDLER_HPP_ diff --git a/rclcpp/include/rclcpp/exceptions.hpp b/rclcpp/include/rclcpp/exceptions.hpp index 23da40e972..dcac558512 100644 --- a/rclcpp/include/rclcpp/exceptions.hpp +++ b/rclcpp/include/rclcpp/exceptions.hpp @@ -15,173 +15,6 @@ #ifndef RCLCPP__EXCEPTIONS_HPP_ #define RCLCPP__EXCEPTIONS_HPP_ -#include -#include - -#include "rcl/error_handling.h" -#include "rcl/types.h" -#include "rclcpp/visibility_control.hpp" - -namespace rclcpp -{ -namespace exceptions -{ - -/// Thrown when a method is trying to use a node, but it is invalid. -class InvalidNodeError : public std::runtime_error -{ -public: - InvalidNodeError() - : std::runtime_error("node is invalid") {} -}; - -/// Thrown when a any kind of name (node, namespace, topic, etc.) is invalid. -class NameValidationError : public std::invalid_argument -{ -public: - NameValidationError( - const char * name_type_, - const char * name_, - const char * error_msg_, - size_t invalid_index_) - : std::invalid_argument(format_error(name_type_, name_, error_msg_, invalid_index_)), - name_type(name_type_), name(name_), error_msg(error_msg_), invalid_index(invalid_index_) - {} - - static std::string - format_error( - const char * name_type, - const char * name, - const char * error_msg, - size_t invalid_index); - - const std::string name_type; - const std::string name; - const std::string error_msg; - const size_t invalid_index; -}; - -/// Thrown when a node name is invalid. -class InvalidNodeNameError : public NameValidationError -{ -public: - InvalidNodeNameError(const char * node_name, const char * error_msg, size_t invalid_index) - : NameValidationError("node name", node_name, error_msg, invalid_index) - {} -}; - -/// Thrown when a node namespace is invalid. -class InvalidNamespaceError : public NameValidationError -{ -public: - InvalidNamespaceError(const char * namespace_, const char * error_msg, size_t invalid_index) - : NameValidationError("namespace", namespace_, error_msg, invalid_index) - {} -}; - -/// Thrown when a topic name is invalid. -class InvalidTopicNameError : public NameValidationError -{ -public: - InvalidTopicNameError(const char * namespace_, const char * error_msg, size_t invalid_index) - : NameValidationError("topic name", namespace_, error_msg, invalid_index) - {} -}; - -/// Thrown when a service name is invalid. -class InvalidServiceNameError : public NameValidationError -{ -public: - InvalidServiceNameError(const char * namespace_, const char * error_msg, size_t invalid_index) - : NameValidationError("service name", namespace_, error_msg, invalid_index) - {} -}; - -/// Throw a C++ std::exception which was created based on an rcl error. -/** - * Passing nullptr for reset_error is safe and will avoid calling any function - * to reset the error. - * - * \param ret the return code for the current error state - * \param prefix string to prefix to the error if applicable (not all errors have custom messages) - * \param error_state error state to create exception from, if nullptr rcl_get_error_state is used - * \param reset_error function to be called before throwing which whill clear the error state - * \throws std::invalid_argument if ret is RCL_RET_OK - * \throws std::runtime_error if the rcl_get_error_state returns 0 - * \throws RCLErrorBase some child class exception based on ret - */ -RCLCPP_PUBLIC -void -throw_from_rcl_error( - rcl_ret_t ret, - const std::string & prefix = "", - const rcl_error_state_t * error_state = nullptr, - void (*reset_error)() = rcl_reset_error); - -class RCLErrorBase -{ -public: - RCLCPP_PUBLIC - RCLErrorBase(rcl_ret_t ret, const rcl_error_state_t * error_state); - virtual ~RCLErrorBase() {} - - rcl_ret_t ret; - std::string message; - std::string file; - size_t line; - std::string formatted_message; -}; - -/// Created when the return code does not match one of the other specialized exceptions. -class RCLError : public RCLErrorBase, public std::runtime_error -{ -public: - RCLCPP_PUBLIC - RCLError(rcl_ret_t ret, const rcl_error_state_t * error_state, const std::string & prefix); - RCLCPP_PUBLIC - RCLError(const RCLErrorBase & base_exc, const std::string & prefix); -}; - -/// Created when the ret is RCL_RET_BAD_ALLOC. -class RCLBadAlloc : public RCLErrorBase, public std::bad_alloc -{ -public: - RCLCPP_PUBLIC - RCLBadAlloc(rcl_ret_t ret, const rcl_error_state_t * error_state); - RCLCPP_PUBLIC - explicit RCLBadAlloc(const RCLErrorBase & base_exc); -}; - -/// Created when the ret is RCL_RET_INVALID_ARGUMENT. -class RCLInvalidArgument : public RCLErrorBase, public std::invalid_argument -{ -public: - RCLCPP_PUBLIC - RCLInvalidArgument( - rcl_ret_t ret, - const rcl_error_state_t * error_state, - const std::string & prefix); - RCLCPP_PUBLIC - RCLInvalidArgument(const RCLErrorBase & base_exc, const std::string & prefix); -}; - -/// Thrown when an invalid rclcpp::Event object or SharedPtr is encountered. -class InvalidEventError : public std::runtime_error -{ -public: - InvalidEventError() - : std::runtime_error("event is invalid") {} -}; - -/// Thrown when an unregistered rclcpp::Event is encountered where a registered one was expected. -class EventNotRegisteredError : public std::runtime_error -{ -public: - EventNotRegisteredError() - : std::runtime_error("event already registered") {} -}; - -} // namespace exceptions -} // namespace rclcpp +#include "rclcpp/exceptions/exceptions.hpp" #endif // RCLCPP__EXCEPTIONS_HPP_ diff --git a/rclcpp/include/rclcpp/exceptions/exceptions.hpp b/rclcpp/include/rclcpp/exceptions/exceptions.hpp new file mode 100644 index 0000000000..08c6b88250 --- /dev/null +++ b/rclcpp/include/rclcpp/exceptions/exceptions.hpp @@ -0,0 +1,359 @@ +// 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 RCLCPP__EXCEPTIONS__EXCEPTIONS_HPP_ +#define RCLCPP__EXCEPTIONS__EXCEPTIONS_HPP_ + +#include +#include +#include + +#include "rcl/error_handling.h" +#include "rcl/types.h" +#include "rclcpp/visibility_control.hpp" + +#include "rcpputils/join.hpp" + +namespace rclcpp +{ +namespace exceptions +{ + +/// Thrown when a method is trying to use a node, but it is invalid. +class InvalidNodeError : public std::runtime_error +{ +public: + InvalidNodeError() + : std::runtime_error("node is invalid") {} +}; + +/// Thrown when a any kind of name (node, namespace, topic, etc.) is invalid. +class NameValidationError : public std::invalid_argument +{ +public: + NameValidationError( + const char * name_type_, + const char * name_, + const char * error_msg_, + size_t invalid_index_) + : std::invalid_argument(format_error(name_type_, name_, error_msg_, invalid_index_)), + name_type(name_type_), name(name_), error_msg(error_msg_), invalid_index(invalid_index_) + {} + + static std::string + format_error( + const char * name_type, + const char * name, + const char * error_msg, + size_t invalid_index); + + const std::string name_type; + const std::string name; + const std::string error_msg; + const size_t invalid_index; +}; + +/// Thrown when a node name is invalid. +class InvalidNodeNameError : public NameValidationError +{ +public: + InvalidNodeNameError(const char * node_name, const char * error_msg, size_t invalid_index) + : NameValidationError("node name", node_name, error_msg, invalid_index) + {} +}; + +/// Thrown when a node namespace is invalid. +class InvalidNamespaceError : public NameValidationError +{ +public: + InvalidNamespaceError(const char * namespace_, const char * error_msg, size_t invalid_index) + : NameValidationError("namespace", namespace_, error_msg, invalid_index) + {} +}; + +/// Thrown when a topic name is invalid. +class InvalidTopicNameError : public NameValidationError +{ +public: + InvalidTopicNameError(const char * namespace_, const char * error_msg, size_t invalid_index) + : NameValidationError("topic name", namespace_, error_msg, invalid_index) + {} +}; + +/// Thrown when a service name is invalid. +class InvalidServiceNameError : public NameValidationError +{ +public: + InvalidServiceNameError(const char * namespace_, const char * error_msg, size_t invalid_index) + : NameValidationError("service name", namespace_, error_msg, invalid_index) + {} +}; + +class InvalidServiceTypeError : public std::runtime_error +{ +public: + InvalidServiceTypeError() + : std::runtime_error("Service type is invalid.") {} + explicit InvalidServiceTypeError(const std::string & msg) + : std::runtime_error(msg) {} +}; + +class UnimplementedError : public std::runtime_error +{ +public: + UnimplementedError() + : std::runtime_error("This code is unimplemented.") {} + explicit UnimplementedError(const std::string & msg) + : std::runtime_error(msg) {} +}; + +typedef void (* reset_error_function_t)(); + +/// Throw a C++ std::exception which was created based on an rcl error. +/** + * Passing nullptr for reset_error is safe and will avoid calling any function + * to reset the error. + * + * \param ret the return code for the current error state + * \param prefix string to prefix to the error if applicable (not all errors have custom messages) + * \param error_state error state to create exception from, if nullptr rcl_get_error_state is used + * \param reset_error function to be called before throwing which whill clear the error state + * \throws std::invalid_argument if ret is RCL_RET_OK + * \throws std::runtime_error if the rcl_get_error_state returns 0 + * \throws RCLErrorBase some child class exception based on ret + */ +/* *INDENT-OFF* */ // Uncrustify cannot yet understand [[noreturn]] properly +RCLCPP_PUBLIC +void +throw_from_rcl_error [[noreturn]] ( + rcl_ret_t ret, + const std::string & prefix = "", + const rcl_error_state_t * error_state = nullptr, + reset_error_function_t reset_error = rcl_reset_error); +/* *INDENT-ON* */ + +class RCLErrorBase +{ +public: + RCLCPP_PUBLIC + RCLErrorBase(rcl_ret_t ret, const rcl_error_state_t * error_state); + virtual ~RCLErrorBase() {} + + rcl_ret_t ret; + std::string message; + std::string file; + size_t line; + std::string formatted_message; +}; + +/// Created when the return code does not match one of the other specialized exceptions. +class RCLError : public RCLErrorBase, public std::runtime_error +{ +public: + RCLCPP_PUBLIC + RCLError(rcl_ret_t ret, const rcl_error_state_t * error_state, const std::string & prefix); + RCLCPP_PUBLIC + RCLError(const RCLErrorBase & base_exc, const std::string & prefix); +}; + +/// Created when the ret is RCL_RET_BAD_ALLOC. +class RCLBadAlloc : public RCLErrorBase, public std::bad_alloc +{ +public: + RCLCPP_PUBLIC + RCLBadAlloc(rcl_ret_t ret, const rcl_error_state_t * error_state); + RCLCPP_PUBLIC + explicit RCLBadAlloc(const RCLErrorBase & base_exc); +}; + +/// Created when the ret is RCL_RET_INVALID_ARGUMENT. +class RCLInvalidArgument : public RCLErrorBase, public std::invalid_argument +{ +public: + RCLCPP_PUBLIC + RCLInvalidArgument( + rcl_ret_t ret, + const rcl_error_state_t * error_state, + const std::string & prefix); + RCLCPP_PUBLIC + RCLInvalidArgument(const RCLErrorBase & base_exc, const std::string & prefix); +}; + +/// Created when the ret is RCL_RET_INVALID_ROS_ARGS. +class RCLInvalidROSArgsError : public RCLErrorBase, public std::runtime_error +{ +public: + RCLCPP_PUBLIC + RCLInvalidROSArgsError( + rcl_ret_t ret, const rcl_error_state_t * error_state, const std::string & prefix); + RCLCPP_PUBLIC + RCLInvalidROSArgsError(const RCLErrorBase & base_exc, const std::string & prefix); +}; + +/// Thrown when unparsed ROS specific arguments are found. +class UnknownROSArgsError : public std::runtime_error +{ +public: + explicit UnknownROSArgsError(std::vector && unknown_ros_args_in) + : std::runtime_error( + "found unknown ROS arguments: '" + rcpputils::join(unknown_ros_args_in, "', '") + "'"), + unknown_ros_args(unknown_ros_args_in) + { + } + + const std::vector unknown_ros_args; +}; + +/// Thrown when an unknown type is passed +class UnknownTypeError : public std::runtime_error +{ +public: + explicit UnknownTypeError(const std::string & type) + : std::runtime_error("Unknown type: " + type) {} +}; + +/// Thrown when an invalid rclcpp::Event object or SharedPtr is encountered. +class InvalidEventError : public std::runtime_error +{ +public: + InvalidEventError() + : std::runtime_error("event is invalid") {} +}; + +/// Thrown when an unregistered rclcpp::Event is encountered where a registered one was expected. +class EventNotRegisteredError : public std::runtime_error +{ +public: + EventNotRegisteredError() + : std::runtime_error("event already registered") {} +}; + +/// Thrown when a callback group is missing from the node, when it wants to utilize the group. +class MissingGroupNodeException : public std::runtime_error +{ +public: + explicit MissingGroupNodeException(const std::string & obj_type) + : std::runtime_error("cannot create: " + obj_type + " , callback group not in node") {} +}; + +/// Thrown if passed parameters are inconsistent or invalid +class InvalidParametersException : public std::runtime_error +{ +public: + // Inherit constructors from runtime_error. + using std::runtime_error::runtime_error; +}; + +/// Thrown if passed parameter value is invalid. +class InvalidParameterValueException : public std::runtime_error +{ + // Inherit constructors from runtime_error. + using std::runtime_error::runtime_error; +}; + +/// Thrown if requested parameter type is invalid. +/** + * Essentially the same as rclcpp::ParameterTypeException, but with parameter + * name in the error message. + */ +class InvalidParameterTypeException : public std::runtime_error +{ +public: + /// Construct an instance. + /** + * \param[in] name the name of the parameter. + * \param[in] message custom exception message. + */ + RCLCPP_PUBLIC + InvalidParameterTypeException(const std::string & name, const std::string message) + : std::runtime_error("parameter '" + name + "' has invalid type: " + message) + {} +}; + +/// Thrown if user attempts to create an uninitialized statically typed parameter +/** + * (see https://github.com/ros2/rclcpp/issues/1691) + */ +class UninitializedStaticallyTypedParameterException : public std::runtime_error +{ +public: + /// Construct an instance. + /** + * \param[in] name the name of the parameter. + */ + RCLCPP_PUBLIC + explicit UninitializedStaticallyTypedParameterException(const std::string & name) + : std::runtime_error("Statically typed parameter '" + name + "' must be initialized.") + {} +}; + +/// Thrown if parameter is already declared. +class ParameterAlreadyDeclaredException : public std::runtime_error +{ + // Inherit constructors from runtime_error. + using std::runtime_error::runtime_error; +}; + +/// Thrown if parameter is not declared, e.g. either set or get was called without first declaring. +class ParameterNotDeclaredException : public std::runtime_error +{ + // Inherit constructors from runtime_error. + using std::runtime_error::runtime_error; +}; + +/// Thrown if parameter is immutable and therefore cannot be undeclared. +class ParameterImmutableException : public std::runtime_error +{ + // Inherit constructors from runtime_error. + using std::runtime_error::runtime_error; +}; + +/// Thrown if parameter is modified while in a set callback. +class ParameterModifiedInCallbackException : public std::runtime_error +{ + // Inherit constructors from runtime_error. + using std::runtime_error::runtime_error; +}; + +/// Thrown when an uninitialized parameter is accessed. +class ParameterUninitializedException : public std::runtime_error +{ +public: + /// Construct an instance. + /** + * \param[in] name the name of the parameter. + */ + explicit ParameterUninitializedException(const std::string & name) + : std::runtime_error("parameter '" + name + "' is not initialized") + {} +}; + +/// Thrown if the QoS overrides provided aren't valid. +class InvalidQosOverridesException : public std::runtime_error +{ + // Inherit constructors from runtime_error. + using std::runtime_error::runtime_error; +}; + +/// Thrown if a QoS compatibility check fails. +class QoSCheckCompatibleException : public std::runtime_error +{ + // Inherit constructors from runtime_error. + using std::runtime_error::runtime_error; +}; + +} // namespace exceptions +} // namespace rclcpp + +#endif // RCLCPP__EXCEPTIONS__EXCEPTIONS_HPP_ diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 3059a549e8..ae2087bbc5 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -21,61 +21,35 @@ #include #include #include +#include #include +#include #include #include #include "rcl/guard_condition.h" #include "rcl/wait.h" - +#include "rclcpp/executors/executor_notify_waitable.hpp" +#include "rcpputils/scope_exit.hpp" + +#include "rclcpp/context.hpp" +#include "rclcpp/contexts/default_context.hpp" +#include "rclcpp/guard_condition.hpp" +#include "rclcpp/executor_options.hpp" +#include "rclcpp/executors/executor_entities_collection.hpp" +#include "rclcpp/executors/executor_entities_collector.hpp" +#include "rclcpp/future_return_code.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" -#include "rclcpp/memory_strategies.hpp" -#include "rclcpp/memory_strategy.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp/visibility_control.hpp" +#include "rclcpp/wait_set.hpp" namespace rclcpp { // Forward declaration is used in convenience method signature. class Node; - -namespace executor -{ - -/// Return codes to be used with spin_until_future_complete. -/** - * SUCCESS: The future is complete and can be accessed with "get" without blocking. - * INTERRUPTED: The future is not complete, spinning was interrupted by Ctrl-C or another error. - * TIMEOUT: Spinning timed out. - */ -enum class FutureReturnCode {SUCCESS, INTERRUPTED, TIMEOUT}; - -RCLCPP_PUBLIC -std::ostream & -operator<<(std::ostream & os, const FutureReturnCode & future_return_code); - -RCLCPP_PUBLIC -std::string -to_string(const FutureReturnCode & future_return_code); - -/// -/** - * Options to be passed to the executor constructor. - */ -struct ExecutorArgs -{ - memory_strategy::MemoryStrategy::SharedPtr memory_strategy; - size_t max_conditions = 0; -}; - -static inline ExecutorArgs create_default_executor_arguments() -{ - ExecutorArgs args; - args.memory_strategy = memory_strategies::create_default_strategy(); - args.max_conditions = 0; - return args; -} +class ExecutorImplementation; /// Coordinate the order and timing of available communication tasks. /** @@ -93,9 +67,11 @@ class Executor RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Executor) /// Default constructor. - // \param[in] ms The memory strategy to be used with this executor. + /** + * \param[in] options Options used to configure the executor. + */ RCLCPP_PUBLIC - explicit Executor(const ExecutorArgs & args = create_default_executor_arguments()); + explicit Executor(const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions()); /// Default destructor. RCLCPP_PUBLIC @@ -106,35 +82,155 @@ class Executor virtual void spin() = 0; + /// Add a callback group to an executor. + /** + * An executor can have zero or more callback groups which provide work during `spin` functions. + * When an executor attempts to add a callback group, the executor checks to see if it is already + * associated with another executor, and if it has been, then an exception is thrown. + * Otherwise, the callback group is added to the executor. + * + * Adding a callback group with this method does not associate its node with this executor + * in any way + * + * \param[in] group_ptr a shared ptr that points to a callback group + * \param[in] node_ptr a shared pointer that points to a node base interface + * \param[in] notify True to trigger the interrupt guard condition during this function. If + * the executor is blocked at the rmw layer while waiting for work and it is notified that a new + * callback group was added, it will wake up. + * \throw std::runtime_error if the callback group is associated to an executor + */ + RCLCPP_PUBLIC + virtual void + add_callback_group( + rclcpp::CallbackGroup::SharedPtr group_ptr, + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, + bool notify = true); + + /// Get callback groups that belong to executor. + /** + * This function returns a vector of weak pointers that point to callback groups that were + * associated with the executor. + * The callback groups associated with this executor may have been added with + * `add_callback_group`, or added when a node was added to the executor with `add_node`, or + * automatically added when it created by a node already associated with this executor and the + * automatically_add_to_executor_with_node parameter was true. + * + * \return a vector of weak pointers that point to callback groups that are associated with + * the executor + */ + RCLCPP_PUBLIC + virtual std::vector + get_all_callback_groups(); + + /// Get callback groups that belong to executor. + /** + * This function returns a vector of weak pointers that point to callback groups that were + * associated with the executor. + * The callback groups associated with this executor have been added with + * `add_callback_group`. + * + * \return a vector of weak pointers that point to callback groups that are associated with + * the executor + */ + RCLCPP_PUBLIC + virtual std::vector + get_manually_added_callback_groups(); + + /// Get callback groups that belong to executor. + /** + * This function returns a vector of weak pointers that point to callback groups that were + * added from a node that is associated with the executor. + * The callback groups are added when a node is added to the executor with `add_node`, or + * automatically if they are created in the future by that node and have the + * automatically_add_to_executor_with_node argument set to true. + * + * \return a vector of weak pointers that point to callback groups from a node associated with + * the executor + */ + RCLCPP_PUBLIC + virtual std::vector + get_automatically_added_callback_groups_from_nodes(); + + /// Remove a callback group from the executor. + /** + * The callback group is removed from and disassociated with the executor. + * If the callback group removed was the last callback group from the node + * that is associated with the executor, the interrupt guard condition + * is triggered and node's guard condition is removed from the executor. + * + * This function only removes a callback group that was manually added with + * rclcpp::Executor::add_callback_group. + * To remove callback groups that were added from a node using + * rclcpp::Executor::add_node, use rclcpp::Executor::remove_node instead. + * + * \param[in] group_ptr Shared pointer to the callback group to be added. + * \param[in] notify True to trigger the interrupt guard condition during this function. If + * the executor is blocked at the rmw layer while waiting for work and it is notified that a + * callback group was removed, it will wake up. + * \throw std::runtime_error if node is deleted before callback group + * \throw std::runtime_error if the callback group is not associated with the executor + */ + RCLCPP_PUBLIC + virtual void + remove_callback_group( + rclcpp::CallbackGroup::SharedPtr group_ptr, + bool notify = true); + /// Add a node to the executor. /** - * An executor can have zero or more nodes which provide work during `spin` functions. + * Nodes have associated callback groups, and this method adds any of those callback groups + * to this executor which have their automatically_add_to_executor_with_node parameter true. + * The node is also associated with the executor so that future callback groups which are + * created on the node with the automatically_add_to_executor_with_node parameter set to true + * are also automatically associated with this executor. + * + * Callback groups with the automatically_add_to_executor_with_node parameter set to false must + * be manually added to an executor using the rclcpp::Executor::add_callback_group method. + * + * If a node is already associated with an executor, this method throws an exception. + * * \param[in] node_ptr Shared pointer to the node to be added. * \param[in] notify True to trigger the interrupt guard condition during this function. If * the executor is blocked at the rmw layer while waiting for work and it is notified that a new * node was added, it will wake up. + * \throw std::runtime_error if a node is already associated to an executor */ RCLCPP_PUBLIC virtual void add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify = true); /// Convenience function which takes Node and forwards NodeBaseInterface. + /** + * \see rclcpp::Executor::add_node + */ RCLCPP_PUBLIC virtual void add_node(std::shared_ptr node_ptr, bool notify = true); /// Remove a node from the executor. /** + * Any callback groups automatically added when this node was added with + * rclcpp::Executor::add_node are automatically removed, and the node is no longer associated + * with this executor. + * + * This also means that future callback groups created by the given node are no longer + * automatically added to this executor. + * * \param[in] node_ptr Shared pointer to the node to remove. * \param[in] notify True to trigger the interrupt guard condition and wake up the executor. * This is useful if the last node was removed from the executor while the executor was blocked * waiting for work in another thread, because otherwise the executor would never be notified. + * \throw std::runtime_error if the node is not associated with an executor. + * \throw std::runtime_error if the node is not associated with this executor. */ RCLCPP_PUBLIC virtual void remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify = true); /// Convenience function which takes Node and forwards NodeBaseInterface. + /** + * \see rclcpp::Executor::remove_node + */ RCLCPP_PUBLIC virtual void remove_node(std::shared_ptr node_ptr, bool notify = true); @@ -146,11 +242,11 @@ class Executor * spin_node_once to block indefinitely (the default behavior). A timeout of 0 causes this * function to be non-blocking. */ - template + template void spin_node_once( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node, - std::chrono::duration timeout = std::chrono::duration(-1)) + std::chrono::duration timeout = std::chrono::duration(-1)) { return spin_node_once_nanoseconds( node, @@ -159,11 +255,11 @@ class Executor } /// Convenience function which takes Node and forwards NodeBaseInterface. - template + template void spin_node_once( std::shared_ptr node, - std::chrono::duration timeout = std::chrono::duration(-1)) + std::chrono::duration timeout = std::chrono::duration(-1)) { return spin_node_once_nanoseconds( node->get_node_base_interface(), @@ -176,184 +272,332 @@ class Executor * \param[in] node Shared pointer to the node to add. */ RCLCPP_PUBLIC - void + virtual void spin_node_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node); /// Convenience function which takes Node and forwards NodeBaseInterface. RCLCPP_PUBLIC - void + virtual void spin_node_some(std::shared_ptr node); - /// Complete all available queued work without blocking. + /// Collect work once and execute all available work, optionally within a max duration. + /** + * This function can be overridden. + * The default implementation is suitable for a single-threaded model of execution. + * Adding subscriptions, timers, services, etc. with blocking or long running + * callbacks may cause the function exceed the max_duration significantly. + * + * If there is no work to be done when this called, it will return immediately + * because the collecting of available work is non-blocking. + * Before each piece of ready work is executed this function checks if the + * max_duration has been exceeded, and if it has it returns without starting + * the execution of the next piece of work. + * + * If a max_duration of 0 is given, then all of the collected work will be + * executed before the function returns. + * + * \param[in] max_duration The maximum amount of time to spend executing work, or 0 for no limit. + */ + RCLCPP_PUBLIC + virtual void + spin_some(std::chrono::nanoseconds max_duration = std::chrono::nanoseconds(0)); + + /// Add a node, complete all immediately available work exhaustively, and remove the node. + /** + * \param[in] node Shared pointer to the node to add. + */ + RCLCPP_PUBLIC + virtual void + spin_node_all( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node, + std::chrono::nanoseconds max_duration); + + /// Convenience function which takes Node and forwards NodeBaseInterface. + RCLCPP_PUBLIC + virtual void + spin_node_all(std::shared_ptr node, std::chrono::nanoseconds max_duration); + + /// Collect and execute work repeatedly within a duration or until no more work is available. /** * This function can be overridden. The default implementation is suitable for a * single-threaded model of execution. * Adding subscriptions, timers, services, etc. with blocking callbacks will cause this function * to block (which may have unintended consequences). + * If the time that waitables take to be executed is longer than the period on which new waitables + * become ready, this method will execute work repeatedly until `max_duration` has elapsed. + * + * \param[in] max_duration The maximum amount of time to spend executing work, must be >= 0. + * `0` is potentially block forever until no more work is available. + * \throw std::invalid_argument if max_duration is less than 0. + * Note that spin_all() may take longer than this time as it only returns once max_duration has + * been exceeded. */ RCLCPP_PUBLIC virtual void - spin_some(); + spin_all(std::chrono::nanoseconds max_duration); + + /// Collect work once and execute the next available work, optionally within a duration. + /** + * This function can be overridden. The default implementation is suitable for + * a single-thread model of execution. + * Adding subscriptions, timers, services, etc. with blocking callbacks will cause this function + * to block (which may have unintended consequences). + * \param[in] timeout The maximum amount of time to spend waiting for work. + * `-1` is potentially block forever waiting for work. + */ RCLCPP_PUBLIC virtual void spin_once(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); /// Spin (blocking) until the future is complete, it times out waiting, or rclcpp is interrupted. /** - * \param[in] future The future to wait on. If SUCCESS, the future is safe to access after this - * function. + * \param[in] future The future to wait on. If this function returns SUCCESS, the future can be + * accessed without blocking (though it may still throw an exception). * \param[in] timeout Optional timeout parameter, which gets passed to Executor::spin_node_once. * `-1` is block forever, `0` is non-blocking. * If the time spent inside the blocking loop exceeds this timeout, return a TIMEOUT return * code. * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`. */ - template + template FutureReturnCode spin_until_future_complete( - std::shared_future & future, - std::chrono::duration timeout = std::chrono::duration(-1)) + const FutureT & future, + std::chrono::duration timeout = std::chrono::duration(-1)) { - // TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete - // inside a callback executed by an executor. - - // Check the future before entering the while loop. - // If the future is already complete, don't try to spin. - std::future_status status = future.wait_for(std::chrono::seconds(0)); - if (status == std::future_status::ready) { - return FutureReturnCode::SUCCESS; - } - - auto end_time = std::chrono::steady_clock::now(); - std::chrono::nanoseconds timeout_ns = std::chrono::duration_cast( - timeout); - if (timeout_ns > std::chrono::nanoseconds::zero()) { - end_time += timeout_ns; - } - std::chrono::nanoseconds timeout_left = timeout_ns; - - while (rclcpp::ok()) { - // Do one item of work. - spin_once(timeout_left); - // Check if the future is set, return SUCCESS if it is. - status = future.wait_for(std::chrono::seconds(0)); - if (status == std::future_status::ready) { - return FutureReturnCode::SUCCESS; - } - // If the original timeout is < 0, then this is blocking, never TIMEOUT. - if (timeout_ns < std::chrono::nanoseconds::zero()) { - continue; + return spin_until_future_complete_impl( + std::chrono::duration_cast(timeout), + [&future](std::chrono::nanoseconds wait_time) { + return future.wait_for(wait_time); } - // Otherwise check if we still have time to wait, return TIMEOUT if not. - auto now = std::chrono::steady_clock::now(); - if (now >= end_time) { - return FutureReturnCode::TIMEOUT; - } - // Subtract the elapsed time from the original timeout. - timeout_left = std::chrono::duration_cast(end_time - now); - } - - // The future did not complete before ok() returned false, return INTERRUPTED. - return FutureReturnCode::INTERRUPTED; + ); } /// Cancel any running spin* function, causing it to return. - /* This function can be called asynchonously from any thread. */ + /** + * This function can be called asynchonously from any thread. + * \throws std::runtime_error if there is an issue triggering the guard condition + */ RCLCPP_PUBLIC - void + virtual void cancel(); - /// Support dynamic switching of the memory strategy. + /// Returns true if the executor is currently spinning. /** - * Switching the memory strategy while the executor is spinning in another threading could have - * unintended consequences. - * \param[in] memory_strategy Shared pointer to the memory strategy to set. + * This function can be called asynchronously from any thread. + * \return True if the executor is currently spinning. */ RCLCPP_PUBLIC - void - set_memory_strategy(memory_strategy::MemoryStrategy::SharedPtr memory_strategy); + bool + is_spinning(); protected: + /// Constructor that will not initialize any non-trivial members. + /** + * This constructor is intended to be used by any derived executor + * that explicitly does not want to use the default implementation provided + * by this class. + */ + explicit Executor(const std::shared_ptr & context); + + /// Add a node to executor, execute the next available unit of work, and remove the node. + /** + * Implementation of spin_node_once using std::chrono::nanoseconds + * \param[in] node Shared pointer to the node to add. + * \param[in] timeout How long to wait for work to become available. Negative values cause + * spin_node_once to block indefinitely (the default behavior). A timeout of 0 causes this + * function to be non-blocking. + */ RCLCPP_PUBLIC void spin_node_once_nanoseconds( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node, std::chrono::nanoseconds timeout); + /// Spin (blocking) until the future is complete, it times out waiting, or rclcpp is interrupted. + /** + * \sa spin_until_future_complete() + * The only difference with spin_until_future_complete() is that the future's + * type is obscured through a std::function which lets you wait on it + * reguardless of type. + * + * \param[in] timeout see spin_until_future_complete() for details + * \param[in] wait_for_future function to wait on the future and get the + * status after waiting + */ + RCLCPP_PUBLIC + virtual FutureReturnCode + spin_until_future_complete_impl( + std::chrono::nanoseconds timeout, + const std::function & wait_for_future); + + /// Collect work and execute available work, optionally within a duration. + /** + * Implementation of spin_some and spin_all. + * The exhaustive flag controls if the function will re-collect available work within the duration. + * + * \param[in] max_duration The maximum amount of time to spend executing work, or 0 for no limit. + * \param[in] exhaustive when set to true, continue to collect work and execute (spin_all) + * when set to false, return when all collected work is executed (spin_some) + */ + RCLCPP_PUBLIC + void + spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive); + /// Find the next available executable and do the work associated with it. - /** \param[in] any_exec Union structure that can hold any executable type (timer, subscription, + /** + * \param[in] any_exec Union structure that can hold any executable type (timer, subscription, * service, client). + * \throws std::runtime_error if there is an issue triggering the guard condition */ RCLCPP_PUBLIC void - execute_any_executable(AnyExecutable::SharedPtr any_exec); + execute_any_executable(AnyExecutable & any_exec); + /// Run subscription executable. + /** + * Do necessary setup and tear-down as well as executing the subscription. + * \param[in] subscription Subscription to execute + */ RCLCPP_PUBLIC static void execute_subscription( rclcpp::SubscriptionBase::SharedPtr subscription); + /// Run timer executable. + /** + * Do necessary setup and tear-down as well as executing the timer callback. + * \param[in] timer Timer to execute + */ RCLCPP_PUBLIC static void - execute_intra_process_subscription( - rclcpp::SubscriptionBase::SharedPtr subscription); - - RCLCPP_PUBLIC - static void - execute_timer(rclcpp::TimerBase::SharedPtr timer); + execute_timer(rclcpp::TimerBase::SharedPtr timer, const std::shared_ptr & data_ptr); + /// Run service server executable. + /** + * Do necessary setup and tear-down as well as executing the service server callback. + * \param[in] service Service to execute + */ RCLCPP_PUBLIC static void execute_service(rclcpp::ServiceBase::SharedPtr service); + /// Run service client executable. + /** + * Do necessary setup and tear-down as well as executing the service client callback. + * \param[in] service Service to execute + */ RCLCPP_PUBLIC static void execute_client(rclcpp::ClientBase::SharedPtr client); + /// Gather all of the waitable entities from associated nodes and callback groups. RCLCPP_PUBLIC void - wait_for_work(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); - - RCLCPP_PUBLIC - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr - get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group); + collect_entities(); + /// Block until more work becomes avilable or timeout is reached. + /** + * Builds a set of waitable entities, which are passed to the middleware. + * After building wait set, waits on middleware to notify. + * \param[in] timeout duration to wait for new work to become available. + * \throws std::runtime_error if the wait set can be cleared + */ RCLCPP_PUBLIC - rclcpp::callback_group::CallbackGroup::SharedPtr - get_group_by_timer(rclcpp::TimerBase::SharedPtr timer); + void + wait_for_work(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); + /// Check for executable in ready state and populate union structure. + /** + * \param[out] any_executable populated union structure of ready executable + * \return true if an executable was ready and any_executable was populated, + * otherwise false + */ RCLCPP_PUBLIC - void - get_next_timer(AnyExecutable::SharedPtr any_exec); + bool + get_next_ready_executable(AnyExecutable & any_executable); + /// Wait for executable in ready state and populate union structure. + /** + * If an executable is ready, it will return immediately, otherwise + * block based on the timeout for work to become ready. + * + * \param[out] any_executable populated union structure of ready executable + * \param[in] timeout duration of time to wait for work, a negative value + * (the defualt behavior), will make this function block indefinitely + * \return true if an executable was ready and any_executable was populated, + * otherwise false + */ RCLCPP_PUBLIC - AnyExecutable::SharedPtr - get_next_ready_executable(); + bool + get_next_executable( + AnyExecutable & any_executable, + std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); + /// This function triggers a recollect of all entities that are registered to the executor. + /** + * Calling this function is thread safe. + * + * \param[in] notify if true will execute a trigger that will wake up a waiting executor + */ RCLCPP_PUBLIC - AnyExecutable::SharedPtr - get_next_executable(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); + virtual void + handle_updated_entities(bool notify); /// Spinning state, used to prevent multi threaded calls to spin and to cancel blocking spins. std::atomic_bool spinning; /// Guard condition for signaling the rmw layer to wake up for special events. - rcl_guard_condition_t interrupt_guard_condition_ = rcl_get_zero_initialized_guard_condition(); + std::shared_ptr interrupt_guard_condition_; + + /// Guard condition for signaling the rmw layer to wake up for system shutdown. + std::shared_ptr shutdown_guard_condition_; - /// Wait set for managing entities that the rmw layer waits on. - rcl_wait_set_t wait_set_ = rcl_get_zero_initialized_wait_set(); + mutable std::mutex mutex_; - /// The memory strategy: an interface for handling user-defined memory allocation strategies. - memory_strategy::MemoryStrategy::SharedPtr memory_strategy_; + /// The context associated with this executor. + std::shared_ptr context_; -private: RCLCPP_DISABLE_COPY(Executor) - std::vector weak_nodes_; + RCLCPP_PUBLIC + virtual void + spin_once_impl(std::chrono::nanoseconds timeout); + + /// Waitable containing guard conditions controlling the executor flow. + /** + * This waitable contains the interrupt and shutdown guard condition, as well + * as the guard condition associated with each node and callback group. + * By default, if any change is detected in the monitored entities, the notify + * waitable will awake the executor and rebuild the collections. + */ + std::shared_ptr notify_waitable_; + + std::atomic_bool entities_need_rebuild_; + + /// Collector used to associate executable entities from nodes and guard conditions + rclcpp::executors::ExecutorEntitiesCollector collector_; + + /// WaitSet to be waited on. + rclcpp::WaitSet wait_set_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + std::optional> wait_result_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + + /// Hold the current state of the collection being waited on by the waitset + rclcpp::executors::ExecutorEntitiesCollection current_collection_ RCPPUTILS_TSA_GUARDED_BY( + mutex_); + + /// Hold the current state of the notify waitable being waited on by the waitset + std::shared_ptr current_notify_waitable_ + RCPPUTILS_TSA_GUARDED_BY(mutex_); + + /// shutdown callback handle registered to Context + rclcpp::OnShutdownCallbackHandle shutdown_callback_handle_; + + /// Pointer to implementation + std::unique_ptr impl_; }; -} // namespace executor } // namespace rclcpp #endif // RCLCPP__EXECUTOR_HPP_ diff --git a/rclcpp/include/rclcpp/executor_options.hpp b/rclcpp/include/rclcpp/executor_options.hpp new file mode 100644 index 0000000000..a8fd5b73bf --- /dev/null +++ b/rclcpp/include/rclcpp/executor_options.hpp @@ -0,0 +1,57 @@ +// Copyright 2014-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 RCLCPP__EXECUTOR_OPTIONS_HPP_ +#define RCLCPP__EXECUTOR_OPTIONS_HPP_ + +#include + +#include "rclcpp/context.hpp" +#include "rclcpp/contexts/default_context.hpp" +#include "rclcpp/memory_strategies.hpp" +#include "rclcpp/memory_strategy.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ + +class ExecutorOptionsImplementation; + +/// Options to be passed to the executor constructor. +struct ExecutorOptions +{ + RCLCPP_PUBLIC + ExecutorOptions(); + + RCLCPP_PUBLIC + virtual ~ExecutorOptions(); + + RCLCPP_PUBLIC + ExecutorOptions(const ExecutorOptions &); + + RCLCPP_PUBLIC + ExecutorOptions & operator=(const ExecutorOptions &); + + rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy; + rclcpp::Context::SharedPtr context; + size_t max_conditions; + +private: + /// Pointer to implementation + std::unique_ptr impl_; +}; + +} // namespace rclcpp + +#endif // RCLCPP__EXECUTOR_OPTIONS_HPP_ diff --git a/rclcpp/include/rclcpp/executors.hpp b/rclcpp/include/rclcpp/executors.hpp index af0bdedc00..adb9c6d6c6 100644 --- a/rclcpp/include/rclcpp/executors.hpp +++ b/rclcpp/include/rclcpp/executors.hpp @@ -20,6 +20,8 @@ #include "rclcpp/executors/multi_threaded_executor.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/executors/static_single_threaded_executor.hpp" +#include "rclcpp/experimental/executors/events_executor/events_executor.hpp" #include "rclcpp/node.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp/visibility_control.hpp" @@ -27,6 +29,18 @@ namespace rclcpp { +/// Create a default single-threaded executor and execute all available work exhaustively. +/** \param[in] node_ptr Shared pointer to the node to spin. */ +RCLCPP_PUBLIC +void +spin_all( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, + std::chrono::nanoseconds max_duration); + +RCLCPP_PUBLIC +void +spin_all(rclcpp::Node::SharedPtr node_ptr, std::chrono::nanoseconds max_duration); + /// Create a default single-threaded executor and execute any immediately available work. /** \param[in] node_ptr Shared pointer to the node to spin. */ RCLCPP_PUBLIC @@ -65,13 +79,13 @@ using rclcpp::executors::SingleThreadedExecutor; * If the time spent inside the blocking loop exceeds this timeout, return a `TIMEOUT` return code. * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`. */ -template -rclcpp::executor::FutureReturnCode +template +rclcpp::FutureReturnCode spin_node_until_future_complete( - rclcpp::executor::Executor & executor, + rclcpp::Executor & executor, rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - std::shared_future & future, - std::chrono::duration timeout = std::chrono::duration(-1)) + const FutureT & future, + std::chrono::duration timeout = std::chrono::duration(-1)) { // TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete // inside a callback executed by an executor. @@ -81,13 +95,14 @@ spin_node_until_future_complete( return retcode; } -template -rclcpp::executor::FutureReturnCode +template +rclcpp::FutureReturnCode spin_node_until_future_complete( - rclcpp::executor::Executor & executor, + rclcpp::Executor & executor, std::shared_ptr node_ptr, - std::shared_future & future, - std::chrono::duration timeout = std::chrono::duration(-1)) + const FutureT & future, + std::chrono::duration timeout = std::chrono::duration(-1)) { return rclcpp::executors::spin_node_until_future_complete( executor, @@ -98,23 +113,26 @@ spin_node_until_future_complete( } // namespace executors -template -rclcpp::executor::FutureReturnCode +template +rclcpp::FutureReturnCode spin_until_future_complete( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - std::shared_future & future, - std::chrono::duration timeout = std::chrono::duration(-1)) + const FutureT & future, + std::chrono::duration timeout = std::chrono::duration(-1)) { - rclcpp::executors::SingleThreadedExecutor executor; + rclcpp::ExecutorOptions options; + options.context = node_ptr->get_context(); + rclcpp::executors::SingleThreadedExecutor executor(options); return executors::spin_node_until_future_complete(executor, node_ptr, future, timeout); } -template -rclcpp::executor::FutureReturnCode +template +rclcpp::FutureReturnCode spin_until_future_complete( std::shared_ptr node_ptr, - std::shared_future & future, - std::chrono::duration timeout = std::chrono::duration(-1)) + const FutureT & future, + std::chrono::duration timeout = std::chrono::duration(-1)) { return rclcpp::spin_until_future_complete(node_ptr->get_node_base_interface(), future, timeout); } diff --git a/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp b/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp new file mode 100644 index 0000000000..517894a2a2 --- /dev/null +++ b/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp @@ -0,0 +1,219 @@ +// Copyright 2023 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 RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTION_HPP_ +#define RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTION_HPP_ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace rclcpp +{ +namespace executors +{ + +/// Structure to represent a single entity's entry in a collection +template +struct CollectionEntry +{ + /// Weak pointer to entity type + using EntityWeakPtr = typename EntityValueType::WeakPtr; + /// Shared pointer to entity type + using EntitySharedPtr = typename EntityValueType::SharedPtr; + + /// The entity + EntityWeakPtr entity; + + /// If relevant, the entity's corresponding callback_group + rclcpp::CallbackGroup::WeakPtr callback_group; +}; + +/// Update a collection based on another collection +/* + * Iterates update_from and update_to to see which entities have been added/removed between + * the two collections. + * + * For each new entry (in update_from, but not in update_to), + * add the entity and fire the on_added callback + * For each removed entry (in update_to, but not in update_from), + * remove the entity and fire the on_removed callback. + * + * \param[in] update_from The collection representing the next iteration's state + * \param[inout] update_to The collection representing the current iteration's state + * \param[in] on_added Callback fired when a new entity is detected + * \param[in] on_removed Callback fired when an entity is removed + */ +template +void update_entities( + const CollectionType & update_from, + CollectionType & update_to, + std::function on_added, + std::function on_removed +) +{ + for (auto it = update_to.begin(); it != update_to.end(); ) { + if (update_from.count(it->first) == 0) { + auto entity = it->second.entity.lock(); + if (entity) { + on_removed(entity); + } + it = update_to.erase(it); + } else { + ++it; + } + } + for (auto it = update_from.begin(); it != update_from.end(); ++it) { + if (update_to.count(it->first) == 0) { + auto entity = it->second.entity.lock(); + if (entity) { + on_added(entity); + } + update_to.insert(*it); + } + } +} + +/// A collection of entities, indexed by their corresponding handles +template +class EntityCollection + : public std::unordered_map> +{ +public: + /// Key type of the map + using Key = const EntityKeyType *; + + /// Weak pointer to entity type + using EntityWeakPtr = typename EntityValueType::WeakPtr; + + /// Shared pointer to entity type + using EntitySharedPtr = typename EntityValueType::SharedPtr; + + /// Update this collection based on the contents of another collection + /** + * Update the internal state of this collection, firing callbacks when entities have been + * added or removed. + * + * \param[in] other Collection to compare to + * \param[in] on_added Callback for when entities have been added + * \param[in] on_removed Callback for when entities have been removed + */ + void update( + const EntityCollection & other, + std::function on_added, + std::function on_removed) + { + update_entities(other, *this, on_added, on_removed); + } +}; + +/// Represent the total set of entities for a single executor +/** + * This allows the entities to be stored from ExecutorEntitiesCollector. + * The structure also makes in convenient to re-evaluate when entities have been added or removed. + */ +struct ExecutorEntitiesCollection +{ + /// Collection type for timer entities + using TimerCollection = EntityCollection; + + /// Collection type for subscription entities + using SubscriptionCollection = EntityCollection; + + /// Collection type for client entities + using ClientCollection = EntityCollection; + + /// Collection type for service entities + using ServiceCollection = EntityCollection; + + /// Collection type for waitable entities + using WaitableCollection = EntityCollection; + + /// Collection type for guard condition entities + using GuardConditionCollection = EntityCollection; + + /// Collection of timers currently in use by the executor. + TimerCollection timers; + + /// Collection of subscriptions currently in use by the executor. + SubscriptionCollection subscriptions; + + /// Collection of clients currently in use by the executor. + ClientCollection clients; + + /// Collection of services currently in use by the executor. + ServiceCollection services; + + /// Collection of guard conditions currently in use by the executor. + GuardConditionCollection guard_conditions; + + /// Collection of waitables currently in use by the executor. + WaitableCollection waitables; + + /// Check if the entities collection is empty + /** + * \return true if all member collections are empty, false otherwise + */ + bool empty() const; + + /// Clear the entities collection + void clear(); + + /// Remove entities that have expired weak ownership + /** + * \return The total number of removed entities + */ + size_t remove_expired_entities(); +}; + +/// Build an entities collection from callback groups +/** + * Iterates a list of callback groups and adds entities from each valid group + * + * \param[in] callback_groups List of callback groups to check for entities + * \param[inout] colletion Entities collection to populate with found entities + */ +void +build_entities_collection( + const std::vector & callback_groups, + ExecutorEntitiesCollection & collection); + +/// Build a queue of executables ready to be executed +/** + * Iterates a list of entities and adds them to a queue if they are ready. + * + * \param[in] collection Collection of entities corresponding to the current wait set. + * \param[in] wait_result Result of rclcpp::WaitSet::wait corresponding to the collection. + * \param[inout] queue of executables to append new ready executables to + * \return number of new ready executables + */ +size_t +ready_executables( + const ExecutorEntitiesCollection & collection, + rclcpp::WaitResult & wait_result, + std::deque & executables +); +} // namespace executors +} // namespace rclcpp + +#endif // RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTION_HPP_ diff --git a/rclcpp/include/rclcpp/executors/executor_entities_collector.hpp b/rclcpp/include/rclcpp/executors/executor_entities_collector.hpp new file mode 100644 index 0000000000..ad9bc84fad --- /dev/null +++ b/rclcpp/include/rclcpp/executors/executor_entities_collector.hpp @@ -0,0 +1,270 @@ +// Copyright 2023 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 RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTOR_HPP_ +#define RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTOR_HPP_ + +#include +#include +#include +#include +#include + +#include "rcpputils/thread_safety_annotations.hpp" + +#include +#include +#include +#include +#include +#include +#include + +namespace rclcpp +{ +namespace executors +{ + +/// Class to monitor a set of nodes and callback groups for changes in entity membership +/** + * This is to be used with an executor to track the membership of various nodes, groups, + * and entities (timers, subscriptions, clients, services, etc) and report status to the + * executor. + * + * In general, users will add either nodes or callback groups to an executor. + * Each node may have callback groups that are automatically associated with executors, + * or callback groups that must be manually associated with an executor. + * + * This object tracks both types of callback groups as well as nodes that have been + * previously added to the executor. + * When a new callback group is added/removed or new entities are added/removed, the + * corresponding node or callback group will signal this to the executor so that the + * entity collection may be rebuilt according to that executor's implementation. + * + */ +class ExecutorEntitiesCollector +{ +public: + /// Constructor + /** + * \param[in] notify_waitable Waitable that is used to signal to the executor + * when nodes or callback groups have been added or removed. + */ + RCLCPP_PUBLIC + explicit ExecutorEntitiesCollector( + std::shared_ptr notify_waitable); + + /// Destructor + RCLCPP_PUBLIC + ~ExecutorEntitiesCollector(); + + /// Indicate if the entities collector has pending additions or removals. + /** + * \return true if there are pending additions or removals + */ + bool has_pending() const; + + /// Add a node to the entity collector + /** + * \param[in] node_ptr a shared pointer that points to a node base interface + * \throw std::runtime_error if the node is associated with an executor + */ + RCLCPP_PUBLIC + void + add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr); + + /// Remove a node from the entity collector + /** + * \param[in] node_ptr a shared pointer that points to a node base interface + * \throw std::runtime_error if the node is associated with an executor + * \throw std::runtime_error if the node is associated with this executor + */ + RCLCPP_PUBLIC + void + remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr); + + /// Add a callback group to the entity collector + /** + * \param[in] group_ptr a shared pointer that points to a callback group + * \throw std::runtime_error if the callback_group is associated with an executor + */ + RCLCPP_PUBLIC + void + add_callback_group(rclcpp::CallbackGroup::SharedPtr group_ptr); + + /// Remove a callback group from the entity collector + /** + * \param[in] group_ptr a shared pointer that points to a callback group + * \throw std::runtime_error if the callback_group is not associated with an executor + * \throw std::runtime_error if the callback_group is not associated with this executor + */ + RCLCPP_PUBLIC + void + remove_callback_group(rclcpp::CallbackGroup::SharedPtr group_ptr); + + /// Get all callback groups known to this entity collector + /** + * This will include manually added and automatically added (node associated) groups + * \return vector of all callback groups + */ + RCLCPP_PUBLIC + std::vector + get_all_callback_groups() const; + + /// Get manually-added callback groups known to this entity collector + /** + * This will include callback groups that have been added via add_callback_group + * \return vector of manually-added callback groups + */ + RCLCPP_PUBLIC + std::vector + get_manually_added_callback_groups() const; + + /// Get automatically-added callback groups known to this entity collector + /** + * This will include callback groups that are associated with nodes added via add_node + * \return vector of automatically-added callback groups + */ + RCLCPP_PUBLIC + std::vector + get_automatically_added_callback_groups() const; + + /// Update the underlying collections + /** + * This will prune nodes and callback groups that are no longer valid as well + * as add new callback groups from any associated nodes. + */ + RCLCPP_PUBLIC + void + update_collections(); + +protected: + using NodeCollection = std::set< + rclcpp::node_interfaces::NodeBaseInterface::WeakPtr, + std::owner_less>; + + using CallbackGroupCollection = std::set< + rclcpp::CallbackGroup::WeakPtr, + std::owner_less>; + + using WeakNodesToGuardConditionsMap = std::map< + rclcpp::node_interfaces::NodeBaseInterface::WeakPtr, + rclcpp::GuardCondition::WeakPtr, + std::owner_less>; + + using WeakGroupsToGuardConditionsMap = std::map< + rclcpp::CallbackGroup::WeakPtr, + rclcpp::GuardCondition::WeakPtr, + std::owner_less>; + + /// Implementation of removing a node from the collector. + /** + * This will disassociate the node from the collector and remove any + * automatically-added callback groups + * + * This takes and returns an iterator so it may be used as: + * + * it = remove_weak_node(it); + * + * \param[in] weak_node iterator to the weak node to be removed + * \return Valid updated iterator in the same collection + */ + RCLCPP_PUBLIC + NodeCollection::iterator + remove_weak_node(NodeCollection::iterator weak_node) RCPPUTILS_TSA_REQUIRES(mutex_); + + /// Implementation of removing a callback group from the collector. + /** + * This will disassociate the callback group from the collector + * + * This takes and returns an iterator so it may be used as: + * + * it = remove_weak_callback_group(it); + * + * \param[in] weak_group_it iterator to the weak group to be removed + * \param[in] collection the collection to remove the group from + * (manually or automatically added) + * \return Valid updated iterator in the same collection + */ + RCLCPP_PUBLIC + CallbackGroupCollection::iterator + remove_weak_callback_group( + CallbackGroupCollection::iterator weak_group_it, + CallbackGroupCollection & collection) RCPPUTILS_TSA_REQUIRES(mutex_); + + /// Implementation of adding a callback group + /** + * \param[in] group_ptr the group to add + * \param[in] collection the collection to add the group to + */ + RCLCPP_PUBLIC + void + add_callback_group_to_collection( + rclcpp::CallbackGroup::SharedPtr group_ptr, + CallbackGroupCollection & collection) RCPPUTILS_TSA_REQUIRES(mutex_); + + /// Iterate over queued added/remove nodes and callback_groups + RCLCPP_PUBLIC + void + process_queues() RCPPUTILS_TSA_REQUIRES(mutex_); + + /// Check a collection of nodes and add any new callback_groups that + /// are set to be automatically associated via the node. + RCLCPP_PUBLIC + void + add_automatically_associated_callback_groups( + const NodeCollection & nodes_to_check) RCPPUTILS_TSA_REQUIRES(mutex_); + + /// Check all nodes and group for expired weak pointers and remove them. + RCLCPP_PUBLIC + void + prune_invalid_nodes_and_groups() RCPPUTILS_TSA_REQUIRES(mutex_); + + /// mutex to protect collections and pending queues + mutable std::mutex mutex_; + + /// Callback groups that were added via `add_callback_group` + CallbackGroupCollection manually_added_groups_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + + /// Callback groups that were added by their association with added nodes + CallbackGroupCollection automatically_added_groups_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + + /// nodes that are associated with the executor + NodeCollection weak_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + + /// Track guard conditions associated with added nodes + WeakNodesToGuardConditionsMap weak_nodes_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + + /// Track guard conditions associated with added callback groups + WeakGroupsToGuardConditionsMap weak_groups_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + + /// nodes that have been added since the last update. + NodeCollection pending_added_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + + /// nodes that have been removed since the last update. + NodeCollection pending_removed_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + + /// callback groups that have been added since the last update. + CallbackGroupCollection pending_manually_added_groups_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + + /// callback groups that have been removed since the last update. + CallbackGroupCollection pending_manually_removed_groups_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + + /// Waitable to add guard conditions to + std::shared_ptr notify_waitable_; +}; +} // namespace executors +} // namespace rclcpp +// +#endif // RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTOR_HPP_ diff --git a/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp b/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp new file mode 100644 index 0000000000..7ae1c3b875 --- /dev/null +++ b/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp @@ -0,0 +1,168 @@ +// Copyright 2023 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 RCLCPP__EXECUTORS__EXECUTOR_NOTIFY_WAITABLE_HPP_ +#define RCLCPP__EXECUTORS__EXECUTOR_NOTIFY_WAITABLE_HPP_ + +#include +#include +#include +#include + +#include "rclcpp/guard_condition.hpp" +#include "rclcpp/waitable.hpp" + +namespace rclcpp +{ +namespace executors +{ + +/// Maintain a collection of guard conditions from associated nodes and callback groups +/// to signal to the executor when associated entities have changed. +class ExecutorNotifyWaitable : public rclcpp::Waitable +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(ExecutorNotifyWaitable) + + // Constructor + /** + * \param[in] on_execute_callback Callback to execute when one of the conditions + * of this waitable has signaled the wait_set. + */ + RCLCPP_PUBLIC + explicit ExecutorNotifyWaitable(std::function on_execute_callback = {}); + + // Destructor + RCLCPP_PUBLIC + ~ExecutorNotifyWaitable() override = default; + + RCLCPP_PUBLIC + ExecutorNotifyWaitable(ExecutorNotifyWaitable & other); + + RCLCPP_PUBLIC + ExecutorNotifyWaitable & operator=(ExecutorNotifyWaitable & other); + + /// Add conditions to the wait set + /** + * \param[inout] wait_set structure that conditions will be added to + */ + RCLCPP_PUBLIC + void + add_to_wait_set(rcl_wait_set_t & wait_set) override; + + /// Check conditions against the wait set + /** + * \param[inout] wait_set structure that internal elements will be checked against. + * \return true if this waitable is ready to be executed, false otherwise. + */ + RCLCPP_PUBLIC + bool + is_ready(const rcl_wait_set_t & wait_set) override; + + /// Perform work associated with the waitable. + /** + * This will call the callback provided in the constructor. + * \param[in] data Data to be use for the execute, if available, else nullptr. + */ + RCLCPP_PUBLIC + void + execute(const std::shared_ptr & data) override; + + /// Retrieve data to be used in the next execute call. + /** + * \return If available, data to be used, otherwise nullptr + */ + RCLCPP_PUBLIC + std::shared_ptr + take_data() override; + + /// Take the data from an entity ID so that it can be consumed with `execute`. + /** + * \param[in] id ID of the entity to take data from. + * \return If available, data to be used, otherwise nullptr + * \sa rclcpp::Waitable::take_data_by_entity_id + */ + RCLCPP_PUBLIC + std::shared_ptr + take_data_by_entity_id(size_t id) override; + + /// Set a callback to be called whenever the waitable becomes ready. + /** + * \param[in] callback callback to set + * \sa rclcpp::Waitable::set_on_ready_callback + */ + RCLCPP_PUBLIC + void + set_on_ready_callback(std::function callback) override; + + /// Add a guard condition to be waited on. + /** + * \param[in] guard_condition The guard condition to add. + */ + RCLCPP_PUBLIC + void + add_guard_condition(rclcpp::GuardCondition::WeakPtr guard_condition); + + /// Unset any callback registered via set_on_ready_callback. + /** + * \sa rclcpp::Waitable::clear_on_ready_callback + */ + RCLCPP_PUBLIC + void + clear_on_ready_callback() override; + + /// Set a new callback to be called whenever this waitable is executed. + /** + * \param[in] on_execute_callback The new callback + */ + RCLCPP_PUBLIC + void + set_execute_callback(std::function on_execute_callback); + + /// Remove a guard condition from being waited on. + /** + * \param[in] weak_guard_condition The guard condition to remove. + */ + RCLCPP_PUBLIC + void + remove_guard_condition(rclcpp::GuardCondition::WeakPtr weak_guard_condition); + + /// Get the number of ready guard_conditions + /** + * \return The number of guard_conditions associated with the Waitable. + */ + RCLCPP_PUBLIC + size_t + get_number_of_ready_guard_conditions() override; + +private: + /// Callback to run when waitable executes + std::function execute_callback_; + + /// Mutex to procetect the guard conditions + std::mutex guard_condition_mutex_; + /// Mutex to protect the execute callback + std::mutex execute_mutex_; + + std::function on_ready_callback_; + + /// The collection of guard conditions to be waited on. + std::set> notify_guard_conditions_; +}; + +} // namespace executors +} // namespace rclcpp + +#endif // RCLCPP__EXECUTORS__EXECUTOR_NOTIFY_WAITABLE_HPP_ diff --git a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp index 40abbc0041..119013ebfb 100644 --- a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp @@ -15,7 +15,10 @@ #ifndef RCLCPP__EXECUTORS__MULTI_THREADED_EXECUTOR_HPP_ #define RCLCPP__EXECUTORS__MULTI_THREADED_EXECUTOR_HPP_ +#include +#include #include +#include #include #include @@ -29,21 +32,42 @@ namespace rclcpp namespace executors { -class MultiThreadedExecutor : public executor::Executor +class MultiThreadedExecutor : public rclcpp::Executor { public: RCLCPP_SMART_PTR_DEFINITIONS(MultiThreadedExecutor) + /// Constructor for MultiThreadedExecutor. + /** + * For the yield_before_execute option, when true std::this_thread::yield() + * will be called after acquiring work (as an AnyExecutable) and + * releasing the spinning lock, but before executing the work. + * This is useful for reproducing some bugs related to taking work more than + * once. + * + * \param options common options for all executors + * \param number_of_threads number of threads to have in the thread pool, + * the default 0 will use the number of cpu cores found (minimum of 2) + * \param yield_before_execute if true std::this_thread::yield() is called + * \param timeout maximum time to wait + */ RCLCPP_PUBLIC - MultiThreadedExecutor( - const executor::ExecutorArgs & args = rclcpp::executor::create_default_executor_arguments()); + explicit MultiThreadedExecutor( + const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions(), + size_t number_of_threads = 0, + bool yield_before_execute = false, + std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); RCLCPP_PUBLIC virtual ~MultiThreadedExecutor(); + /** + * \sa rclcpp::Executor:spin() for more details + * \throws std::runtime_error when spin() called while already spinning + */ RCLCPP_PUBLIC void - spin(); + spin() override; RCLCPP_PUBLIC size_t @@ -59,6 +83,8 @@ class MultiThreadedExecutor : public executor::Executor std::mutex wait_mutex_; size_t number_of_threads_; + bool yield_before_execute_; + std::chrono::nanoseconds next_exec_timeout_; }; } // namespace executors diff --git a/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp index 50e83469f4..9dc6dec57b 100644 --- a/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp @@ -35,28 +35,35 @@ namespace rclcpp namespace executors { -/// Single-threaded executor implementation -// This is the default executor created by rclcpp::spin. -class SingleThreadedExecutor : public executor::Executor +/// Single-threaded executor implementation. +/** + * This is the default executor created by rclcpp::spin. + */ +class SingleThreadedExecutor : public rclcpp::Executor { public: RCLCPP_SMART_PTR_DEFINITIONS(SingleThreadedExecutor) /// Default constructor. See the default constructor for Executor. RCLCPP_PUBLIC - SingleThreadedExecutor( - const executor::ExecutorArgs & args = rclcpp::executor::create_default_executor_arguments()); + explicit SingleThreadedExecutor( + const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions()); - /// Default destrcutor. + /// Default destructor. RCLCPP_PUBLIC virtual ~SingleThreadedExecutor(); /// Single-threaded implementation of spin. - // This function will block until work comes in, execute it, and keep blocking. - // It will only be interrupt by a CTRL-C (managed by the global signal handler). + /** + * This function will block until work comes in, execute it, and then repeat + * the process until canceled. + * It may be interrupt by a call to rclcpp::Executor::cancel() or by ctrl-c + * if the associated context is configured to shutdown on SIGINT. + * \throws std::runtime_error when spin() called while already spinning + */ RCLCPP_PUBLIC void - spin(); + spin() override; private: RCLCPP_DISABLE_COPY(SingleThreadedExecutor) diff --git a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp new file mode 100644 index 0000000000..33674465a8 --- /dev/null +++ b/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp @@ -0,0 +1,145 @@ +// Copyright 2019 Nobleo Technology +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT 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 RCLCPP__EXECUTORS__STATIC_SINGLE_THREADED_EXECUTOR_HPP_ +#define RCLCPP__EXECUTORS__STATIC_SINGLE_THREADED_EXECUTOR_HPP_ + +#include +#include +#include + +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/executor_entities_collection.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" + +namespace rclcpp +{ +namespace executors +{ + +/// Static executor implementation +/** + * This executor is a static version of the original single threaded executor. + * It contains some performance optimization to avoid unnecessary reconstructions of + * the executable list for every iteration. + * All nodes, callbackgroups, timers, subscriptions etc. are created before + * spin() is called, and modified only when an entity is added/removed to/from a node. + * This executor is deprecated because these performance improvements have now been + * applied to all other executors. + * This executor is also considered unstable due to known bugs. + * See the unit-tests that are only applied to `StandardExecutors` for information + * on the known limitations. + * + * To run this executor instead of SingleThreadedExecutor replace: + * rclcpp::executors::SingleThreadedExecutor exec; + * by + * rclcpp::executors::StaticSingleThreadedExecutor exec; + * in your source code and spin node(s) in the following way: + * exec.add_node(node); + * exec.spin(); + * exec.remove_node(node); + */ +class [[deprecated("Use rclcpp::executors::SingleThreadedExecutor")]] StaticSingleThreadedExecutor + : public rclcpp::Executor +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(StaticSingleThreadedExecutor) + + /// Default constructor. See the default constructor for Executor. + RCLCPP_PUBLIC + explicit StaticSingleThreadedExecutor( + const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions()); + + /// Default destructor. + RCLCPP_PUBLIC + virtual ~StaticSingleThreadedExecutor(); + + /// Static executor implementation of spin. + /** + * This function will block until work comes in, execute it, and keep blocking. + * It will only be interrupted by a CTRL-C (managed by the global signal handler). + * \throws std::runtime_error when spin() called while already spinning + */ + RCLCPP_PUBLIC + void + spin() override; + + /// Static executor implementation of spin some + /** + * This non-blocking function will execute entities that + * were ready when this API was called, until timeout or no + * more work available. Entities that got ready while + * executing work, won't be taken into account here. + * + * Example: + * while(condition) { + * spin_some(); + * sleep(); // User should have some sync work or + * // sleep to avoid a 100% CPU usage + * } + */ + RCLCPP_PUBLIC + void + spin_some(std::chrono::nanoseconds max_duration = std::chrono::nanoseconds(0)) override; + + /// Static executor implementation of spin all + /** + * This non-blocking function will execute entities until timeout (must be >= 0) + * or no more work available. + * If timeout is `0`, potentially it blocks forever until no more work is available. + * If new entities get ready while executing work available, they will be executed + * as long as the timeout hasn't expired. + * + * Example: + * while(condition) { + * spin_all(); + * sleep(); // User should have some sync work or + * // sleep to avoid a 100% CPU usage + * } + */ + RCLCPP_PUBLIC + void + spin_all(std::chrono::nanoseconds max_duration) override; + +protected: + /** + * @brief Executes ready executables from wait set. + * @param collection entities to evaluate for ready executables. + * @param wait_result result to check for ready executables. + * @param spin_once if true executes only the first ready executable. + * @return true if any executable was ready. + */ + bool + execute_ready_executables( + const rclcpp::executors::ExecutorEntitiesCollection & collection, + rclcpp::WaitResult & wait_result, + bool spin_once); + + void + spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive); + + void + spin_once_impl(std::chrono::nanoseconds timeout) override; + + std::optional> + collect_and_wait(std::chrono::nanoseconds timeout); + +private: + RCLCPP_DISABLE_COPY(StaticSingleThreadedExecutor) +}; + +} // namespace executors +} // namespace rclcpp + +#endif // RCLCPP__EXECUTORS__STATIC_SINGLE_THREADED_EXECUTOR_HPP_ diff --git a/rclcpp/include/rclcpp/expand_topic_or_service_name.hpp b/rclcpp/include/rclcpp/expand_topic_or_service_name.hpp index 5454dfac87..79fbc3e104 100644 --- a/rclcpp/include/rclcpp/expand_topic_or_service_name.hpp +++ b/rclcpp/include/rclcpp/expand_topic_or_service_name.hpp @@ -49,6 +49,8 @@ namespace rclcpp * \throws InvalidServiceNameError if name is invalid and is_service is true * \throws std::bad_alloc if memory cannot be allocated * \throws RCLError if an unexpect error occurs + * \throws std::runtime_error if the topic name is unexpectedly valid or, + * if the rcl name is invalid or if the rcl namespace is invalid */ RCLCPP_PUBLIC std::string diff --git a/rclcpp/include/rclcpp/experimental/README.md b/rclcpp/include/rclcpp/experimental/README.md new file mode 100644 index 0000000000..38ca07c1cf --- /dev/null +++ b/rclcpp/include/rclcpp/experimental/README.md @@ -0,0 +1,4 @@ +Notice that headers in this folder should only provide symbols in the rclcpp::experimental namespace. + +Also notice that these headers are not considered part of the public API as they have not yet been stabilized. +And therefore they are subject to change without notice. diff --git a/rclcpp/include/rclcpp/experimental/buffers/buffer_implementation_base.hpp b/rclcpp/include/rclcpp/experimental/buffers/buffer_implementation_base.hpp new file mode 100644 index 0000000000..1d50b1659e --- /dev/null +++ b/rclcpp/include/rclcpp/experimental/buffers/buffer_implementation_base.hpp @@ -0,0 +1,47 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__EXPERIMENTAL__BUFFERS__BUFFER_IMPLEMENTATION_BASE_HPP_ +#define RCLCPP__EXPERIMENTAL__BUFFERS__BUFFER_IMPLEMENTATION_BASE_HPP_ + +#include + +namespace rclcpp +{ +namespace experimental +{ +namespace buffers +{ + +template +class BufferImplementationBase +{ +public: + virtual ~BufferImplementationBase() {} + + virtual BufferT dequeue() = 0; + virtual void enqueue(BufferT request) = 0; + + virtual std::vector get_all_data() = 0; + + virtual void clear() = 0; + virtual bool has_data() const = 0; + virtual size_t available_capacity() const = 0; +}; + +} // namespace buffers +} // namespace experimental +} // namespace rclcpp + +#endif // RCLCPP__EXPERIMENTAL__BUFFERS__BUFFER_IMPLEMENTATION_BASE_HPP_ diff --git a/rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp new file mode 100644 index 0000000000..268c3f6649 --- /dev/null +++ b/rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp @@ -0,0 +1,332 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__EXPERIMENTAL__BUFFERS__INTRA_PROCESS_BUFFER_HPP_ +#define RCLCPP__EXPERIMENTAL__BUFFERS__INTRA_PROCESS_BUFFER_HPP_ + +#include +#include +#include +#include +#include + +#include "rclcpp/allocator/allocator_common.hpp" +#include "rclcpp/allocator/allocator_deleter.hpp" +#include "rclcpp/experimental/buffers/buffer_implementation_base.hpp" +#include "rclcpp/macros.hpp" +#include "tracetools/tracetools.h" + +namespace rclcpp +{ +namespace experimental +{ +namespace buffers +{ + +class IntraProcessBufferBase +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(IntraProcessBufferBase) + + virtual ~IntraProcessBufferBase() {} + + virtual void clear() = 0; + + virtual bool has_data() const = 0; + virtual bool use_take_shared_method() const = 0; + virtual size_t available_capacity() const = 0; +}; + +template< + typename MessageT, + typename Alloc = std::allocator, + typename MessageDeleter = std::default_delete> +class IntraProcessBuffer : public IntraProcessBufferBase +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(IntraProcessBuffer) + + virtual ~IntraProcessBuffer() {} + + using MessageUniquePtr = std::unique_ptr; + using MessageSharedPtr = std::shared_ptr; + + virtual void add_shared(MessageSharedPtr msg) = 0; + virtual void add_unique(MessageUniquePtr msg) = 0; + + virtual MessageSharedPtr consume_shared() = 0; + virtual MessageUniquePtr consume_unique() = 0; + + virtual std::vector get_all_data_shared() = 0; + virtual std::vector get_all_data_unique() = 0; +}; + +template< + typename MessageT, + typename Alloc = std::allocator, + typename MessageDeleter = std::default_delete, + typename BufferT = std::unique_ptr> +class TypedIntraProcessBuffer : public IntraProcessBuffer +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(TypedIntraProcessBuffer) + + using MessageAllocTraits = allocator::AllocRebind; + using MessageAlloc = typename MessageAllocTraits::allocator_type; + using MessageUniquePtr = std::unique_ptr; + using MessageSharedPtr = std::shared_ptr; + + explicit + TypedIntraProcessBuffer( + std::unique_ptr> buffer_impl, + std::shared_ptr allocator = nullptr) + { + bool valid_type = (std::is_same::value || + std::is_same::value); + if (!valid_type) { + throw std::runtime_error("Creating TypedIntraProcessBuffer with not valid BufferT"); + } + + buffer_ = std::move(buffer_impl); + + TRACETOOLS_TRACEPOINT( + rclcpp_buffer_to_ipb, + static_cast(buffer_.get()), + static_cast(this)); + if (!allocator) { + message_allocator_ = std::make_shared(); + } else { + message_allocator_ = std::make_shared(*allocator.get()); + } + } + + virtual ~TypedIntraProcessBuffer() {} + + void add_shared(MessageSharedPtr msg) override + { + add_shared_impl(std::move(msg)); + } + + void add_unique(MessageUniquePtr msg) override + { + buffer_->enqueue(std::move(msg)); + } + + MessageSharedPtr consume_shared() override + { + return consume_shared_impl(); + } + + MessageUniquePtr consume_unique() override + { + return consume_unique_impl(); + } + + std::vector get_all_data_shared() override + { + return get_all_data_shared_impl(); + } + + std::vector get_all_data_unique() override + { + return get_all_data_unique_impl(); + } + + bool has_data() const override + { + return buffer_->has_data(); + } + + void clear() override + { + buffer_->clear(); + } + + bool use_take_shared_method() const override + { + return std::is_same::value; + } + + size_t available_capacity() const override + { + return buffer_->available_capacity(); + } + +private: + std::unique_ptr> buffer_; + + std::shared_ptr message_allocator_; + + // MessageSharedPtr to MessageSharedPtr + template + typename std::enable_if< + std::is_same::value + >::type + add_shared_impl(MessageSharedPtr shared_msg) + { + buffer_->enqueue(std::move(shared_msg)); + } + + // MessageSharedPtr to MessageUniquePtr + template + typename std::enable_if< + std::is_same::value + >::type + add_shared_impl(MessageSharedPtr shared_msg) + { + // This should not happen: here a copy is unconditionally made, while the intra-process manager + // can decide whether a copy is needed depending on the number and the type of buffers + + MessageUniquePtr unique_msg; + MessageDeleter * deleter = std::get_deleter(shared_msg); + auto ptr = MessageAllocTraits::allocate(*message_allocator_.get(), 1); + MessageAllocTraits::construct(*message_allocator_.get(), ptr, *shared_msg); + if (deleter) { + unique_msg = MessageUniquePtr(ptr, *deleter); + } else { + unique_msg = MessageUniquePtr(ptr); + } + + buffer_->enqueue(std::move(unique_msg)); + } + + // MessageSharedPtr to MessageSharedPtr + template + typename std::enable_if< + std::is_same::value, + MessageSharedPtr + >::type + consume_shared_impl() + { + return buffer_->dequeue(); + } + + // MessageUniquePtr to MessageSharedPtr + template + typename std::enable_if< + (std::is_same::value), + MessageSharedPtr + >::type + consume_shared_impl() + { + // automatic cast from unique ptr to shared ptr + return buffer_->dequeue(); + } + + // MessageSharedPtr to MessageUniquePtr + template + typename std::enable_if< + (std::is_same::value), + MessageUniquePtr + >::type + consume_unique_impl() + { + MessageSharedPtr buffer_msg = buffer_->dequeue(); + + MessageUniquePtr unique_msg; + MessageDeleter * deleter = std::get_deleter(buffer_msg); + auto ptr = MessageAllocTraits::allocate(*message_allocator_.get(), 1); + MessageAllocTraits::construct(*message_allocator_.get(), ptr, *buffer_msg); + if (deleter) { + unique_msg = MessageUniquePtr(ptr, *deleter); + } else { + unique_msg = MessageUniquePtr(ptr); + } + + return unique_msg; + } + + // MessageUniquePtr to MessageUniquePtr + template + typename std::enable_if< + (std::is_same::value), + MessageUniquePtr + >::type + consume_unique_impl() + { + return buffer_->dequeue(); + } + + // MessageSharedPtr to MessageSharedPtr + template + typename std::enable_if< + std::is_same::value, + std::vector + >::type + get_all_data_shared_impl() + { + return buffer_->get_all_data(); + } + + // MessageUniquePtr to MessageSharedPtr + template + typename std::enable_if< + std::is_same::value, + std::vector + >::type + get_all_data_shared_impl() + { + std::vector result; + auto uni_ptr_vec = buffer_->get_all_data(); + result.reserve(uni_ptr_vec.size()); + for (MessageUniquePtr & uni_ptr : uni_ptr_vec) { + result.emplace_back(std::move(uni_ptr)); + } + return result; + } + + // MessageSharedPtr to MessageUniquePtr + template + typename std::enable_if< + std::is_same::value, + std::vector + >::type + get_all_data_unique_impl() + { + std::vector result; + auto shared_ptr_vec = buffer_->get_all_data(); + result.reserve(shared_ptr_vec.size()); + for (MessageSharedPtr shared_msg : shared_ptr_vec) { + MessageUniquePtr unique_msg; + MessageDeleter * deleter = std::get_deleter(shared_msg); + auto ptr = MessageAllocTraits::allocate(*message_allocator_.get(), 1); + MessageAllocTraits::construct(*message_allocator_.get(), ptr, *shared_msg); + if (deleter) { + unique_msg = MessageUniquePtr(ptr, *deleter); + } else { + unique_msg = MessageUniquePtr(ptr); + } + result.push_back(std::move(unique_msg)); + } + return result; + } + + // MessageUniquePtr to MessageUniquePtr + template + typename std::enable_if< + std::is_same::value, + std::vector + >::type + get_all_data_unique_impl() + { + return buffer_->get_all_data(); + } +}; + +} // namespace buffers +} // namespace experimental +} // namespace rclcpp + + +#endif // RCLCPP__EXPERIMENTAL__BUFFERS__INTRA_PROCESS_BUFFER_HPP_ diff --git a/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp b/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp new file mode 100644 index 0000000000..b8fe79a5ff --- /dev/null +++ b/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp @@ -0,0 +1,310 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__EXPERIMENTAL__BUFFERS__RING_BUFFER_IMPLEMENTATION_HPP_ +#define RCLCPP__EXPERIMENTAL__BUFFERS__RING_BUFFER_IMPLEMENTATION_HPP_ + +#include +#include +#include +#include +#include + +#include "rclcpp/experimental/buffers/buffer_implementation_base.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" +#include "tracetools/tracetools.h" + +namespace rclcpp +{ +namespace experimental +{ +namespace buffers +{ + +/// Store elements in a fixed-size, FIFO buffer +/** + * All public member functions are thread-safe. + */ +template +class RingBufferImplementation : public BufferImplementationBase +{ +public: + explicit RingBufferImplementation(size_t capacity) + : capacity_(capacity), + ring_buffer_(capacity), + write_index_(capacity_ - 1), + read_index_(0), + size_(0) + { + if (capacity == 0) { + throw std::invalid_argument("capacity must be a positive, non-zero value"); + } + TRACETOOLS_TRACEPOINT( + rclcpp_construct_ring_buffer, + static_cast(this), + capacity_); + } + + virtual ~RingBufferImplementation() {} + + /// Add a new element to store in the ring buffer + /** + * This member function is thread-safe. + * + * \param request the element to be stored in the ring buffer + */ + void enqueue(BufferT request) override + { + std::lock_guard lock(mutex_); + + write_index_ = next_(write_index_); + ring_buffer_[write_index_] = std::move(request); + TRACETOOLS_TRACEPOINT( + rclcpp_ring_buffer_enqueue, + static_cast(this), + write_index_, + size_ + 1, + is_full_()); + + if (is_full_()) { + read_index_ = next_(read_index_); + } else { + size_++; + } + } + + /// Remove the oldest element from ring buffer + /** + * This member function is thread-safe. + * + * \return the element that is being removed from the ring buffer + */ + BufferT dequeue() override + { + std::lock_guard lock(mutex_); + + if (!has_data_()) { + return BufferT(); + } + + auto request = std::move(ring_buffer_[read_index_]); + TRACETOOLS_TRACEPOINT( + rclcpp_ring_buffer_dequeue, + static_cast(this), + read_index_, + size_ - 1); + read_index_ = next_(read_index_); + + size_--; + + return request; + } + + /// Get all the elements from the ring buffer + /** + * This member function is thread-safe. + * + * \return a vector containing all the elements from the ring buffer + */ + std::vector get_all_data() override + { + return get_all_data_impl(); + } + + /// Get the next index value for the ring buffer + /** + * This member function is thread-safe. + * + * \param val the current index value + * \return the next index value + */ + inline size_t next(size_t val) + { + std::lock_guard lock(mutex_); + return next_(val); + } + + /// Get if the ring buffer has at least one element stored + /** + * This member function is thread-safe. + * + * \return `true` if there is data and `false` otherwise + */ + inline bool has_data() const override + { + std::lock_guard lock(mutex_); + return has_data_(); + } + + /// Get if the size of the buffer is equal to its capacity + /** + * This member function is thread-safe. + * + * \return `true` if the size of the buffer is equal is capacity + * and `false` otherwise + */ + inline bool is_full() const + { + std::lock_guard lock(mutex_); + return is_full_(); + } + + /// Get the remaining capacity to store messages + /** + * This member function is thread-safe. + * + * \return the number of free capacity for new messages + */ + size_t available_capacity() const override + { + std::lock_guard lock(mutex_); + return available_capacity_(); + } + + void clear() override + { + TRACETOOLS_TRACEPOINT(rclcpp_ring_buffer_clear, static_cast(this)); + } + +private: + /// Get the next index value for the ring buffer + /** + * This member function is not thread-safe. + * + * \param val the current index value + * \return the next index value + */ + inline size_t next_(size_t val) + { + return (val + 1) % capacity_; + } + + /// Get if the ring buffer has at least one element stored + /** + * This member function is not thread-safe. + * + * \return `true` if there is data and `false` otherwise + */ + inline bool has_data_() const + { + return size_ != 0; + } + + /// Get if the size of the buffer is equal to its capacity + /** + * This member function is not thread-safe. + * + * \return `true` if the size of the buffer is equal is capacity + * and `false` otherwise + */ + inline bool is_full_() const + { + return size_ == capacity_; + } + + /// Get the remaining capacity to store messages + /** + * This member function is not thread-safe. + * + * \return the number of free capacity for new messages + */ + inline size_t available_capacity_() const + { + return capacity_ - size_; + } + + /// Traits for checking if a type is std::unique_ptr + template + struct is_std_unique_ptr final : std::false_type {}; + template + struct is_std_unique_ptr> final : std::true_type + { + typedef T Ptr_type; + }; + + /// Get all the elements from the ring buffer + /** + * This member function is thread-safe. + * Two versions for the implementation of the function. + * One for buffer containing unique_ptr and the other for other types + * + * \return a vector containing all the elements from the ring buffer + */ + template::value && + std::is_copy_constructible< + typename is_std_unique_ptr::Ptr_type + >::value, + void> * = nullptr> + std::vector get_all_data_impl() + { + std::lock_guard lock(mutex_); + std::vector result_vtr; + result_vtr.reserve(size_); + for (size_t id = 0; id < size_; ++id) { + result_vtr.emplace_back( + new typename is_std_unique_ptr::Ptr_type( + *(ring_buffer_[(read_index_ + id) % capacity_]))); + } + return result_vtr; + } + + template::value, void> * = nullptr> + std::vector get_all_data_impl() + { + std::lock_guard lock(mutex_); + std::vector result_vtr; + result_vtr.reserve(size_); + for (size_t id = 0; id < size_; ++id) { + result_vtr.emplace_back(ring_buffer_[(read_index_ + id) % capacity_]); + } + return result_vtr; + } + + template::value && + !std::is_copy_constructible::value, void> * = nullptr> + std::vector get_all_data_impl() + { + throw std::logic_error("Underlined type results in invalid get_all_data_impl()"); + return {}; + } + + template::value && + !std::is_copy_constructible::Ptr_type>::value, + void> * = nullptr> + std::vector get_all_data_impl() + { + throw std::logic_error("Underlined type in unique_ptr results in invalid get_all_data_impl()"); + return {}; + } + + size_t capacity_; + + std::vector ring_buffer_; + + size_t write_index_; + size_t read_index_; + size_t size_; + + mutable std::mutex mutex_; +}; + +} // namespace buffers +} // namespace experimental +} // namespace rclcpp + +#endif // RCLCPP__EXPERIMENTAL__BUFFERS__RING_BUFFER_IMPLEMENTATION_HPP_ diff --git a/rclcpp/include/rclcpp/experimental/create_intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/create_intra_process_buffer.hpp new file mode 100644 index 0000000000..4d7668b964 --- /dev/null +++ b/rclcpp/include/rclcpp/experimental/create_intra_process_buffer.hpp @@ -0,0 +1,98 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__EXPERIMENTAL__CREATE_INTRA_PROCESS_BUFFER_HPP_ +#define RCLCPP__EXPERIMENTAL__CREATE_INTRA_PROCESS_BUFFER_HPP_ + +#include +#include +#include + +#include "rclcpp/experimental/buffers/intra_process_buffer.hpp" +#include "rclcpp/experimental/buffers/ring_buffer_implementation.hpp" +#include "rclcpp/intra_process_buffer_type.hpp" +#include "rclcpp/qos.hpp" + +namespace rclcpp +{ +namespace experimental +{ + +template< + typename MessageT, + typename Alloc = std::allocator, + typename Deleter = std::default_delete> +typename rclcpp::experimental::buffers::IntraProcessBuffer::UniquePtr +create_intra_process_buffer( + IntraProcessBufferType buffer_type, + const rclcpp::QoS & qos, + std::shared_ptr allocator) +{ + using MessageSharedPtr = std::shared_ptr; + using MessageUniquePtr = std::unique_ptr; + + size_t buffer_size = qos.depth(); + + using rclcpp::experimental::buffers::IntraProcessBuffer; + typename IntraProcessBuffer::UniquePtr buffer; + + switch (buffer_type) { + case IntraProcessBufferType::SharedPtr: + { + using BufferT = MessageSharedPtr; + + auto buffer_implementation = + std::make_unique>( + buffer_size); + + // Construct the intra_process_buffer + buffer = + std::make_unique>( + std::move(buffer_implementation), + allocator); + + break; + } + case IntraProcessBufferType::UniquePtr: + { + using BufferT = MessageUniquePtr; + + auto buffer_implementation = + std::make_unique>( + buffer_size); + + // Construct the intra_process_buffer + buffer = + std::make_unique>( + std::move(buffer_implementation), + allocator); + + break; + } + default: + { + throw std::runtime_error("Unrecognized IntraProcessBufferType value"); + break; + } + } + + return buffer; +} + +} // namespace experimental +} // namespace rclcpp + +#endif // RCLCPP__EXPERIMENTAL__CREATE_INTRA_PROCESS_BUFFER_HPP_ diff --git a/rclcpp/include/rclcpp/experimental/executable_list.hpp b/rclcpp/include/rclcpp/experimental/executable_list.hpp new file mode 100644 index 0000000000..887dd6cd8f --- /dev/null +++ b/rclcpp/include/rclcpp/experimental/executable_list.hpp @@ -0,0 +1,91 @@ +// Copyright 2019 Nobleo Technology +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT 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 RCLCPP__EXPERIMENTAL__EXECUTABLE_LIST_HPP_ +#define RCLCPP__EXPERIMENTAL__EXECUTABLE_LIST_HPP_ + +#include + +#include "rclcpp/client.hpp" +#include "rclcpp/service.hpp" +#include "rclcpp/subscription_base.hpp" +#include "rclcpp/timer.hpp" +#include "rclcpp/visibility_control.hpp" +#include "rclcpp/waitable.hpp" + +namespace rclcpp +{ +namespace experimental +{ + +/// This class contains subscriptionbase, timerbase, etc. which can be used to run callbacks. +class ExecutableList final +{ +public: + RCLCPP_PUBLIC + ExecutableList(); + + RCLCPP_PUBLIC + ~ExecutableList(); + + RCLCPP_PUBLIC + void + clear(); + + RCLCPP_PUBLIC + void + add_subscription(rclcpp::SubscriptionBase::SharedPtr subscription); + + RCLCPP_PUBLIC + void + add_timer(rclcpp::TimerBase::SharedPtr timer); + + RCLCPP_PUBLIC + void + add_service(rclcpp::ServiceBase::SharedPtr service); + + RCLCPP_PUBLIC + void + add_client(rclcpp::ClientBase::SharedPtr client); + + RCLCPP_PUBLIC + void + add_waitable(rclcpp::Waitable::SharedPtr waitable); + + // Vector containing the SubscriptionBase of all the subscriptions added to the executor. + std::vector subscription; + // Contains the count of added subscriptions + size_t number_of_subscriptions; + // Vector containing the TimerBase of all the timers added to the executor. + std::vector timer; + // Contains the count of added timers + size_t number_of_timers; + // Vector containing the ServiceBase of all the services added to the executor. + std::vector service; + // Contains the count of added services + size_t number_of_services; + // Vector containing the ClientBase of all the clients added to the executor. + std::vector client; + // Contains the count of added clients + size_t number_of_clients; + // Vector containing all the waitables added to the executor. + std::vector waitable; + // Contains the count of added waitables + size_t number_of_waitables; +}; + +} // namespace experimental +} // namespace rclcpp + +#endif // RCLCPP__EXPERIMENTAL__EXECUTABLE_LIST_HPP_ diff --git a/rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor.hpp b/rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor.hpp new file mode 100644 index 0000000000..163d8d2367 --- /dev/null +++ b/rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor.hpp @@ -0,0 +1,211 @@ +// Copyright 2023 iRobot Corporation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT 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 RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_HPP_ +#define RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_HPP_ + +#include +#include +#include +#include + +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/executor_entities_collection.hpp" +#include "rclcpp/executors/executor_entities_collector.hpp" +#include "rclcpp/experimental/executors/events_executor/events_executor_event_types.hpp" +#include "rclcpp/experimental/executors/events_executor/events_queue.hpp" +#include "rclcpp/experimental/executors/events_executor/simple_events_queue.hpp" +#include "rclcpp/experimental/timers_manager.hpp" +#include "rclcpp/node.hpp" + +namespace rclcpp +{ +namespace experimental +{ +namespace executors +{ + +/// Events executor implementation +/** + * This executor uses an events queue and a timers manager to execute entities from its + * associated nodes and callback groups. + * ROS 2 entities allow to set callback functions that are invoked when the entity is triggered + * or has work to do. The events-executor sets these callbacks such that they push an + * event into its queue. + * + * This executor tries to reduce as much as possible the amount of maintenance operations. + * This allows to use customized `EventsQueue` classes to achieve different goals such + * as very low CPU usage, bounded memory requirement, determinism, etc. + * + * The executor uses a weak ownership model and it locks entities only while executing + * their related events. + * + * To run this executor: + * rclcpp::experimental::executors::EventsExecutor executor; + * executor.add_node(node); + * executor.spin(); + * executor.remove_node(node); + */ +class EventsExecutor : public rclcpp::Executor +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(EventsExecutor) + + /// Default constructor. See the default constructor for Executor. + /** + * \param[in] events_queue The queue used to store events. + * \param[in] execute_timers_separate_thread If true, timers are executed in a separate + * thread. If false, timers are executed in the same thread as all other entities. + * \param[in] options Options used to configure the executor. + */ + RCLCPP_PUBLIC + EventsExecutor( + rclcpp::experimental::executors::EventsQueue::UniquePtr events_queue = std::make_unique< + rclcpp::experimental::executors::SimpleEventsQueue>(), + bool execute_timers_separate_thread = false, + const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions()); + + /// Default destructor. + RCLCPP_PUBLIC + virtual ~EventsExecutor(); + + /// Events executor implementation of spin. + /** + * This function will block until work comes in, execute it, and keep blocking. + * It will only be interrupted by a CTRL-C (managed by the global signal handler). + * \throws std::runtime_error when spin() called while already spinning + */ + RCLCPP_PUBLIC + void + spin() override; + + /// Events executor implementation of spin some + /** + * This non-blocking function will execute the timers and events + * that were ready when this API was called, until timeout or no + * more work available. New ready-timers/events arrived while + * executing work, won't be taken into account here. + * + * Example: + * while(condition) { + * spin_some(); + * sleep(); // User should have some sync work or + * // sleep to avoid a 100% CPU usage + * } + */ + RCLCPP_PUBLIC + void + spin_some(std::chrono::nanoseconds max_duration = std::chrono::nanoseconds(0)) override; + + /// Events executor implementation of spin all + /** + * This non-blocking function will execute timers and events + * until timeout or no more work available. If new ready-timers/events + * arrive while executing work available, they will be executed + * as long as the timeout hasn't expired. + * + * Example: + * while(condition) { + * spin_all(); + * sleep(); // User should have some sync work or + * // sleep to avoid a 100% CPU usage + * } + */ + RCLCPP_PUBLIC + void + spin_all(std::chrono::nanoseconds max_duration) override; + +protected: + /// Internal implementation of spin_once + RCLCPP_PUBLIC + void + spin_once_impl(std::chrono::nanoseconds timeout) override; + + /// Internal implementation of spin_some + RCLCPP_PUBLIC + void + spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive); + + /// Collect entities from callback groups and refresh the current collection with them + RCLCPP_PUBLIC + void + handle_updated_entities(bool notify) override; + +private: + RCLCPP_DISABLE_COPY(EventsExecutor) + + /// Execute a provided executor event if its associated entities are available + void + execute_event(const ExecutorEvent & event); + + /// Rebuilds the executor's notify waitable, as we can't use the one built in the base class + void + setup_notify_waitable(); + + /// Refresh the current collection using the provided new_collection + void + refresh_current_collection(const rclcpp::executors::ExecutorEntitiesCollection & new_collection); + + /// Create a listener callback function for the provided entity + std::function + create_entity_callback(void * entity_key, ExecutorEventType type); + + /// Create a listener callback function for the provided waitable entity + std::function + create_waitable_callback(const rclcpp::Waitable * waitable_id); + + /// Utility to add the notify waitable to an entities collection + void + add_notify_waitable_to_collection( + rclcpp::executors::ExecutorEntitiesCollection::WaitableCollection & collection); + + /// Searches for the provided entity_id in the collection and returns the entity if valid + template + typename CollectionType::EntitySharedPtr + retrieve_entity(typename CollectionType::Key entity_id, CollectionType & collection) + { + // Note: we lock the mutex because we assume that you are trying to get an element from the + // current collection... If there will be a use-case to retrieve elements also from other + // collections, we can move the mutex back to the calling codes. + std::lock_guard guard(mutex_); + + // Check if the entity_id is in the collection + auto it = collection.find(entity_id); + if (it == collection.end()) { + return nullptr; + } + + // Check if the entity associated with the entity_id is valid + // and remove it from the collection if it isn't + auto entity = it->second.entity.lock(); + if (!entity) { + collection.erase(it); + } + + // Return the retrieved entity (this can be a nullptr if the entity was not valid) + return entity; + } + + /// Queue where entities can push events + rclcpp::experimental::executors::EventsQueue::UniquePtr events_queue_; + + /// Timers manager used to track and/or execute associated timers + std::shared_ptr timers_manager_; +}; + +} // namespace executors +} // namespace experimental +} // namespace rclcpp + +#endif // RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_HPP_ diff --git a/rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor_event_types.hpp b/rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor_event_types.hpp new file mode 100644 index 0000000000..0da641ea6e --- /dev/null +++ b/rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor_event_types.hpp @@ -0,0 +1,49 @@ +// Copyright 2023 iRobot Corporation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT 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 RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_EVENT_TYPES_HPP_ +#define RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_EVENT_TYPES_HPP_ + +#include + +namespace rclcpp +{ +namespace experimental +{ +namespace executors +{ + +enum ExecutorEventType +{ + CLIENT_EVENT, + SUBSCRIPTION_EVENT, + SERVICE_EVENT, + TIMER_EVENT, + WAITABLE_EVENT +}; + +struct ExecutorEvent +{ + const void * entity_key; + std::shared_ptr data; + int waitable_data; + ExecutorEventType type; + size_t num_events; +}; + +} // namespace executors +} // namespace experimental +} // namespace rclcpp + +#endif // RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_EVENT_TYPES_HPP_ diff --git a/rclcpp/include/rclcpp/experimental/executors/events_executor/events_queue.hpp b/rclcpp/include/rclcpp/experimental/executors/events_executor/events_queue.hpp new file mode 100644 index 0000000000..24282d6027 --- /dev/null +++ b/rclcpp/include/rclcpp/experimental/executors/events_executor/events_queue.hpp @@ -0,0 +1,100 @@ +// Copyright 2023 iRobot Corporation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT 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 RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_QUEUE_HPP_ +#define RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_QUEUE_HPP_ + +#include + +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" + +#include "rclcpp/experimental/executors/events_executor/events_executor_event_types.hpp" + +namespace rclcpp +{ +namespace experimental +{ +namespace executors +{ + +/** + * @brief This abstract class can be used to implement different types of queues + * where `ExecutorEvent` can be stored. + * The derived classes should choose which underlying container to use and + * the strategy for pushing and popping events. + * For example a queue implementation may be bounded or unbounded and have + * different pruning strategies. + * Implementations may or may not check the validity of events and decide how to handle + * the situation where an event is not valid anymore (e.g. a subscription history cache overruns) + */ +class EventsQueue +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(EventsQueue) + + RCLCPP_PUBLIC + EventsQueue() = default; + + /** + * @brief Destruct the object. + */ + RCLCPP_PUBLIC + virtual ~EventsQueue() = default; + + /** + * @brief push event into the queue + * @param event The event to push into the queue + */ + RCLCPP_PUBLIC + virtual + void + enqueue(const rclcpp::experimental::executors::ExecutorEvent & event) = 0; + + /** + * @brief Extracts an event from the queue, eventually waiting until timeout + * if none is available. + * @return true if event has been found, false if timeout + */ + RCLCPP_PUBLIC + virtual + bool + dequeue( + rclcpp::experimental::executors::ExecutorEvent & event, + std::chrono::nanoseconds timeout = std::chrono::nanoseconds::max()) = 0; + + /** + * @brief Test whether queue is empty + * @return true if the queue's size is 0, false otherwise. + */ + RCLCPP_PUBLIC + virtual + bool + empty() const = 0; + + /** + * @brief Returns the number of elements in the queue. + * @return the number of elements in the queue. + */ + RCLCPP_PUBLIC + virtual + size_t + size() const = 0; +}; + +} // namespace executors +} // namespace experimental +} // namespace rclcpp + +#endif // RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_QUEUE_HPP_ diff --git a/rclcpp/include/rclcpp/experimental/executors/events_executor/simple_events_queue.hpp b/rclcpp/include/rclcpp/experimental/executors/events_executor/simple_events_queue.hpp new file mode 100644 index 0000000000..7b18a95fcf --- /dev/null +++ b/rclcpp/include/rclcpp/experimental/executors/events_executor/simple_events_queue.hpp @@ -0,0 +1,134 @@ +// Copyright 2023 iRobot Corporation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT 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 RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__SIMPLE_EVENTS_QUEUE_HPP_ +#define RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__SIMPLE_EVENTS_QUEUE_HPP_ + +#include +#include +#include +#include + +#include "rclcpp/experimental/executors/events_executor/events_queue.hpp" + +namespace rclcpp +{ +namespace experimental +{ +namespace executors +{ + +/** + * @brief This class implements an EventsQueue as a simple wrapper around a std::queue. + * It does not perform any checks about the size of queue, which can grow + * unbounded without being pruned. + * The simplicity of this implementation makes it suitable for optimizing CPU usage. + */ +class SimpleEventsQueue : public EventsQueue +{ +public: + RCLCPP_PUBLIC + ~SimpleEventsQueue() override = default; + + /** + * @brief enqueue event into the queue + * Thread safe + * @param event The event to enqueue into the queue + */ + RCLCPP_PUBLIC + void + enqueue(const rclcpp::experimental::executors::ExecutorEvent & event) override + { + rclcpp::experimental::executors::ExecutorEvent single_event = event; + single_event.num_events = 1; + { + std::unique_lock lock(mutex_); + for (size_t ev = 0; ev < event.num_events; ev++) { + event_queue_.push(single_event); + } + } + events_queue_cv_.notify_one(); + } + + /** + * @brief waits for an event until timeout, gets a single event + * Thread safe + * @return true if event, false if timeout + */ + RCLCPP_PUBLIC + bool + dequeue( + rclcpp::experimental::executors::ExecutorEvent & event, + std::chrono::nanoseconds timeout = std::chrono::nanoseconds::max()) override + { + std::unique_lock lock(mutex_); + + // Initialize to true because it's only needed if we have a valid timeout + bool has_data = true; + if (timeout != std::chrono::nanoseconds::max()) { + has_data = + events_queue_cv_.wait_for(lock, timeout, [this]() {return !event_queue_.empty();}); + } else { + events_queue_cv_.wait(lock, [this]() {return !event_queue_.empty();}); + } + + if (has_data) { + event = event_queue_.front(); + event_queue_.pop(); + return true; + } + + return false; + } + + /** + * @brief Test whether queue is empty + * Thread safe + * @return true if the queue's size is 0, false otherwise. + */ + RCLCPP_PUBLIC + bool + empty() const override + { + std::unique_lock lock(mutex_); + return event_queue_.empty(); + } + + /** + * @brief Returns the number of elements in the queue. + * Thread safe + * @return the number of elements in the queue. + */ + RCLCPP_PUBLIC + size_t + size() const override + { + std::unique_lock lock(mutex_); + return event_queue_.size(); + } + +private: + // The underlying queue implementation + std::queue event_queue_; + // Mutex to protect read/write access to the queue + mutable std::mutex mutex_; + // Variable used to notify when an event is added to the queue + std::condition_variable events_queue_cv_; +}; + +} // namespace executors +} // namespace experimental +} // namespace rclcpp + +#endif // RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__SIMPLE_EVENTS_QUEUE_HPP_ diff --git a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp new file mode 100644 index 0000000000..a403c20c8b --- /dev/null +++ b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp @@ -0,0 +1,650 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__EXPERIMENTAL__INTRA_PROCESS_MANAGER_HPP_ +#define RCLCPP__EXPERIMENTAL__INTRA_PROCESS_MANAGER_HPP_ + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "rclcpp/allocator/allocator_deleter.hpp" +#include "rclcpp/experimental/buffers/intra_process_buffer.hpp" +#include "rclcpp/experimental/ros_message_intra_process_buffer.hpp" +#include "rclcpp/experimental/subscription_intra_process.hpp" +#include "rclcpp/experimental/subscription_intra_process_base.hpp" +#include "rclcpp/experimental/subscription_intra_process_buffer.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/publisher_base.hpp" +#include "rclcpp/type_adapter.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ + +namespace experimental +{ + +/// This class performs intra process communication between nodes. +/** + * This class is used in the creation of publishers and subscriptions. + * A singleton instance of this class is owned by a rclcpp::Context and a + * rclcpp::Node can use an associated Context to get an instance of this class. + * Nodes which do not have a common Context will not exchange intra process + * messages because they do not share access to the same instance of this class. + * + * When a Node creates a subscription, it can also create a helper class, + * called SubscriptionIntraProcess, meant to receive intra process messages. + * It can be registered with this class. + * It is also allocated an id which is unique among all publishers + * and subscriptions in this process and that is associated to the subscription. + * + * When a Node creates a publisher, as with subscriptions, a helper class can + * be registered with this class. + * This is required in order to publish intra-process messages. + * It is also allocated an id which is unique among all publishers + * and subscriptions in this process and that is associated to the publisher. + * + * When a publisher or a subscription are registered, this class checks to see + * which other subscriptions or publishers it will communicate with, + * i.e. they have the same topic and compatible QoS. + * + * When the user publishes a message, if intra-process communication is enabled + * on the publisher, the message is given to this class. + * Using the publisher id, a list of recipients for the message is selected. + * For each subscription in the list, this class stores the message, whether + * sharing ownership or making a copy, in a buffer associated with the + * subscription helper class. + * + * The subscription helper class contains a buffer where published + * intra-process messages are stored until they are taken from the subscription. + * Depending on the data type stored in the buffer, the subscription helper + * class can request either shared or exclusive ownership on the message. + * + * Thus, when an intra-process message is published, this class knows how many + * intra-process subscriptions needs it and how many require ownership. + * This information allows this class to operate efficiently by performing the + * fewest number of copies of the message required. + * + * This class is neither CopyConstructable nor CopyAssignable. + */ +class IntraProcessManager +{ +private: + RCLCPP_DISABLE_COPY(IntraProcessManager) + +public: + RCLCPP_SMART_PTR_DEFINITIONS(IntraProcessManager) + + RCLCPP_PUBLIC + IntraProcessManager(); + + RCLCPP_PUBLIC + virtual ~IntraProcessManager(); + + /// Register a subscription with the manager, returns subscriptions unique id. + /** + * This method stores the subscription intra process object, together with + * the information of its wrapped subscription (i.e. topic name and QoS). + * + * In addition this generates a unique intra process id for the subscription. + * + * \param subscription the SubscriptionIntraProcess to register. + * \return an unsigned 64-bit integer which is the subscription's unique id. + */ + template< + typename ROSMessageType, + typename Alloc = std::allocator + > + uint64_t + add_subscription(rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr subscription) + { + std::unique_lock lock(mutex_); + + uint64_t sub_id = IntraProcessManager::get_next_unique_id(); + + subscriptions_[sub_id] = subscription; + + // adds the subscription id to all the matchable publishers + for (auto & pair : publishers_) { + auto publisher = pair.second.lock(); + if (!publisher) { + continue; + } + if (can_communicate(publisher, subscription)) { + uint64_t pub_id = pair.first; + insert_sub_id_for_pub(sub_id, pub_id, subscription->use_take_shared_method()); + if (publisher->is_durability_transient_local() && + subscription->is_durability_transient_local()) + { + do_transient_local_publish( + pub_id, sub_id, + subscription->use_take_shared_method()); + } + } + } + + return sub_id; + } + + /// Unregister a subscription using the subscription's unique id. + /** + * This method does not allocate memory. + * + * \param intra_process_subscription_id id of the subscription to remove. + */ + RCLCPP_PUBLIC + void + remove_subscription(uint64_t intra_process_subscription_id); + + /// Register a publisher with the manager, returns the publisher unique id. + /** + * This method stores the publisher intra process object, together with + * the information of its wrapped publisher (i.e. topic name and QoS). + * + * If the publisher's durability is transient local, its buffer pointer should + * be passed and the method will store it as well. + * + * In addition this generates a unique intra process id for the publisher. + * + * \param publisher publisher to be registered with the manager. + * \param buffer publisher's buffer to be stored if its duability is transient local. + * \return an unsigned 64-bit integer which is the publisher's unique id. + */ + RCLCPP_PUBLIC + uint64_t + add_publisher( + rclcpp::PublisherBase::SharedPtr publisher, + rclcpp::experimental::buffers::IntraProcessBufferBase::SharedPtr buffer = + rclcpp::experimental::buffers::IntraProcessBufferBase::SharedPtr()); + + /// Unregister a publisher using the publisher's unique id. + /** + * This method does not allocate memory. + * + * \param intra_process_publisher_id id of the publisher to remove. + */ + RCLCPP_PUBLIC + void + remove_publisher(uint64_t intra_process_publisher_id); + + /// Publishes an intra-process message, passed as a unique pointer. + /** + * This is one of the two methods for publishing intra-process. + * + * Using the intra-process publisher id, a list of recipients is obtained. + * This list is split in half, depending whether they require ownership or not. + * + * This particular method takes a unique pointer as input. + * The pointer can be promoted to a shared pointer and passed to all the subscriptions + * that do not require ownership. + * In case of subscriptions requiring ownership, the message will be copied for all of + * them except the last one, when ownership can be transferred. + * + * This method can save an additional copy compared to the shared pointer one. + * + * This method can throw an exception if the publisher id is not found or + * if the publisher shared_ptr given to add_publisher has gone out of scope. + * + * This method does allocate memory. + * + * \param intra_process_publisher_id the id of the publisher of this message. + * \param message the message that is being stored. + * \param allocator for allocations when buffering messages. + */ + template< + typename MessageT, + typename ROSMessageType, + typename Alloc, + typename Deleter = std::default_delete + > + void + do_intra_process_publish( + uint64_t intra_process_publisher_id, + std::unique_ptr message, + typename allocator::AllocRebind::allocator_type & allocator) + { + using MessageAllocTraits = allocator::AllocRebind; + using MessageAllocatorT = typename MessageAllocTraits::allocator_type; + + std::shared_lock lock(mutex_); + + auto publisher_it = pub_to_subs_.find(intra_process_publisher_id); + if (publisher_it == pub_to_subs_.end()) { + // Publisher is either invalid or no longer exists. + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Calling do_intra_process_publish for invalid or no longer existing publisher id"); + return; + } + const auto & sub_ids = publisher_it->second; + + if (sub_ids.take_ownership_subscriptions.empty()) { + // None of the buffers require ownership, so we promote the pointer + std::shared_ptr msg = std::move(message); + + this->template add_shared_msg_to_buffers( + msg, sub_ids.take_shared_subscriptions); + } else if (!sub_ids.take_ownership_subscriptions.empty() && // NOLINT + sub_ids.take_shared_subscriptions.size() <= 1) + { + // There is at maximum 1 buffer that does not require ownership. + // So this case is equivalent to all the buffers requiring ownership + + // Merge the two vector of ids into a unique one + std::vector concatenated_vector( + sub_ids.take_shared_subscriptions.begin(), sub_ids.take_shared_subscriptions.end()); + concatenated_vector.insert( + concatenated_vector.end(), + sub_ids.take_ownership_subscriptions.begin(), + sub_ids.take_ownership_subscriptions.end()); + this->template add_owned_msg_to_buffers( + std::move(message), + concatenated_vector, + allocator); + } else if (!sub_ids.take_ownership_subscriptions.empty() && // NOLINT + sub_ids.take_shared_subscriptions.size() > 1) + { + // Construct a new shared pointer from the message + // for the buffers that do not require ownership + auto shared_msg = std::allocate_shared(allocator, *message); + + this->template add_shared_msg_to_buffers( + shared_msg, sub_ids.take_shared_subscriptions); + this->template add_owned_msg_to_buffers( + std::move(message), sub_ids.take_ownership_subscriptions, allocator); + } + } + + template< + typename MessageT, + typename ROSMessageType, + typename Alloc, + typename Deleter = std::default_delete + > + std::shared_ptr + do_intra_process_publish_and_return_shared( + uint64_t intra_process_publisher_id, + std::unique_ptr message, + typename allocator::AllocRebind::allocator_type & allocator) + { + using MessageAllocTraits = allocator::AllocRebind; + using MessageAllocatorT = typename MessageAllocTraits::allocator_type; + + std::shared_lock lock(mutex_); + + auto publisher_it = pub_to_subs_.find(intra_process_publisher_id); + if (publisher_it == pub_to_subs_.end()) { + // Publisher is either invalid or no longer exists. + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Calling do_intra_process_publish for invalid or no longer existing publisher id"); + return nullptr; + } + const auto & sub_ids = publisher_it->second; + + if (sub_ids.take_ownership_subscriptions.empty()) { + // If there are no owning, just convert to shared. + std::shared_ptr shared_msg = std::move(message); + if (!sub_ids.take_shared_subscriptions.empty()) { + this->template add_shared_msg_to_buffers( + shared_msg, sub_ids.take_shared_subscriptions); + } + return shared_msg; + } else { + // Construct a new shared pointer from the message for the buffers that + // do not require ownership and to return. + auto shared_msg = std::allocate_shared(allocator, *message); + + if (!sub_ids.take_shared_subscriptions.empty()) { + this->template add_shared_msg_to_buffers( + shared_msg, + sub_ids.take_shared_subscriptions); + } + if (!sub_ids.take_ownership_subscriptions.empty()) { + this->template add_owned_msg_to_buffers( + std::move(message), + sub_ids.take_ownership_subscriptions, + allocator); + } + return shared_msg; + } + } + + template< + typename MessageT, + typename Alloc, + typename Deleter, + typename ROSMessageType> + void + add_shared_msg_to_buffer( + std::shared_ptr message, + uint64_t subscription_id) + { + add_shared_msg_to_buffers(message, {subscription_id}); + } + + template< + typename MessageT, + typename Alloc, + typename Deleter, + typename ROSMessageType> + void + add_owned_msg_to_buffer( + std::unique_ptr message, + uint64_t subscription_id, + typename allocator::AllocRebind::allocator_type & allocator) + { + add_owned_msg_to_buffers( + std::move(message), {subscription_id}, allocator); + } + + /// Return true if the given rmw_gid_t matches any stored Publishers. + RCLCPP_PUBLIC + bool + matches_any_publishers(const rmw_gid_t * id) const; + + /// Return the number of intraprocess subscriptions that are matched with a given publisher id. + RCLCPP_PUBLIC + size_t + get_subscription_count(uint64_t intra_process_publisher_id) const; + + RCLCPP_PUBLIC + rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr + get_subscription_intra_process(uint64_t intra_process_subscription_id); + + /// Return the lowest available capacity for all subscription buffers for a publisher id. + RCLCPP_PUBLIC + size_t + lowest_available_capacity(const uint64_t intra_process_publisher_id) const; + +private: + struct SplittedSubscriptions + { + std::vector take_shared_subscriptions; + std::vector take_ownership_subscriptions; + }; + + using SubscriptionMap = + std::unordered_map; + + using PublisherMap = + std::unordered_map; + + using PublisherBufferMap = + std::unordered_map; + + using PublisherToSubscriptionIdsMap = + std::unordered_map; + + RCLCPP_PUBLIC + static + uint64_t + get_next_unique_id(); + + RCLCPP_PUBLIC + void + insert_sub_id_for_pub(uint64_t sub_id, uint64_t pub_id, bool use_take_shared_method); + + RCLCPP_PUBLIC + bool + can_communicate( + rclcpp::PublisherBase::SharedPtr pub, + rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr sub) const; + + template< + typename ROSMessageType, + typename Alloc = std::allocator + > + void do_transient_local_publish( + const uint64_t pub_id, const uint64_t sub_id, + const bool use_take_shared_method) + { + using ROSMessageTypeAllocatorTraits = allocator::AllocRebind; + using ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type; + using ROSMessageTypeDeleter = allocator::Deleter; + + auto publisher_buffer = publisher_buffers_[pub_id].lock(); + if (!publisher_buffer) { + throw std::runtime_error("publisher buffer has unexpectedly gone out of scope"); + } + auto buffer = std::dynamic_pointer_cast< + rclcpp::experimental::buffers::IntraProcessBuffer< + ROSMessageType, + ROSMessageTypeAllocator, + ROSMessageTypeDeleter + > + >(publisher_buffer); + if (!buffer) { + throw std::runtime_error( + "failed to dynamic cast publisher's IntraProcessBufferBase to " + "IntraProcessBuffer which can happen when the publisher and " + "subscription use different allocator types, which is not supported"); + } + if (use_take_shared_method) { + auto data_vec = buffer->get_all_data_shared(); + for (auto shared_data : data_vec) { + this->template add_shared_msg_to_buffer< + ROSMessageType, ROSMessageTypeAllocator, ROSMessageTypeDeleter, ROSMessageType>( + shared_data, sub_id); + } + } else { + auto data_vec = buffer->get_all_data_unique(); + for (auto & owned_data : data_vec) { + auto allocator = ROSMessageTypeAllocator(); + this->template add_owned_msg_to_buffer< + ROSMessageType, ROSMessageTypeAllocator, ROSMessageTypeDeleter, ROSMessageType>( + std::move(owned_data), sub_id, allocator); + } + } + } + + template< + typename MessageT, + typename Alloc, + typename Deleter, + typename ROSMessageType> + void + add_shared_msg_to_buffers( + std::shared_ptr message, + std::vector subscription_ids) + { + using ROSMessageTypeAllocatorTraits = allocator::AllocRebind; + using ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type; + using ROSMessageTypeDeleter = allocator::Deleter; + + using PublishedType = typename rclcpp::TypeAdapter::custom_type; + using PublishedTypeAllocatorTraits = allocator::AllocRebind; + using PublishedTypeAllocator = typename PublishedTypeAllocatorTraits::allocator_type; + using PublishedTypeDeleter = allocator::Deleter; + + for (auto id : subscription_ids) { + auto subscription_it = subscriptions_.find(id); + if (subscription_it == subscriptions_.end()) { + throw std::runtime_error("subscription has unexpectedly gone out of scope"); + } + auto subscription_base = subscription_it->second.lock(); + if (subscription_base == nullptr) { + subscriptions_.erase(id); + continue; + } + + auto subscription = std::dynamic_pointer_cast< + rclcpp::experimental::SubscriptionIntraProcessBuffer + >(subscription_base); + if (subscription != nullptr) { + subscription->provide_intra_process_data(message); + continue; + } + + auto ros_message_subscription = std::dynamic_pointer_cast< + rclcpp::experimental::SubscriptionROSMsgIntraProcessBuffer + >(subscription_base); + if (nullptr == ros_message_subscription) { + throw std::runtime_error( + "failed to dynamic cast SubscriptionIntraProcessBase to " + "SubscriptionIntraProcessBuffer, or to " + "SubscriptionROSMsgIntraProcessBuffer which can happen when the publisher and " + "subscription use different allocator types, which is not supported"); + } + + if constexpr (rclcpp::TypeAdapter::is_specialized::value) { + ROSMessageType ros_msg; + rclcpp::TypeAdapter::convert_to_ros_message(*message, ros_msg); + ros_message_subscription->provide_intra_process_message( + std::make_shared(ros_msg)); + } else { + if constexpr (std::is_same::value) { + ros_message_subscription->provide_intra_process_message(message); + } else { + if constexpr (std::is_same::ros_message_type, ROSMessageType>::value) + { + ROSMessageType ros_msg; + rclcpp::TypeAdapter::convert_to_ros_message( + *message, ros_msg); + ros_message_subscription->provide_intra_process_message( + std::make_shared(ros_msg)); + } + } + } + } + } + + template< + typename MessageT, + typename Alloc, + typename Deleter, + typename ROSMessageType> + void + add_owned_msg_to_buffers( + std::unique_ptr message, + std::vector subscription_ids, + typename allocator::AllocRebind::allocator_type & allocator) + { + using MessageAllocTraits = allocator::AllocRebind; + using MessageUniquePtr = std::unique_ptr; + + using ROSMessageTypeAllocatorTraits = allocator::AllocRebind; + using ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type; + using ROSMessageTypeDeleter = allocator::Deleter; + + using PublishedType = typename rclcpp::TypeAdapter::custom_type; + using PublishedTypeAllocatorTraits = allocator::AllocRebind; + using PublishedTypeAllocator = typename PublishedTypeAllocatorTraits::allocator_type; + using PublishedTypeDeleter = allocator::Deleter; + + for (auto it = subscription_ids.begin(); it != subscription_ids.end(); it++) { + auto subscription_it = subscriptions_.find(*it); + if (subscription_it == subscriptions_.end()) { + throw std::runtime_error("subscription has unexpectedly gone out of scope"); + } + auto subscription_base = subscription_it->second.lock(); + if (subscription_base == nullptr) { + subscriptions_.erase(subscription_it); + continue; + } + + auto subscription = std::dynamic_pointer_cast< + rclcpp::experimental::SubscriptionIntraProcessBuffer + >(subscription_base); + if (subscription != nullptr) { + if (std::next(it) == subscription_ids.end()) { + // If this is the last subscription, give up ownership + subscription->provide_intra_process_data(std::move(message)); + // Last message delivered, break from for loop + break; + } else { + // Copy the message since we have additional subscriptions to serve + Deleter deleter = message.get_deleter(); + auto ptr = MessageAllocTraits::allocate(allocator, 1); + MessageAllocTraits::construct(allocator, ptr, *message); + + subscription->provide_intra_process_data(MessageUniquePtr(ptr, deleter)); + } + + continue; + } + + auto ros_message_subscription = std::dynamic_pointer_cast< + rclcpp::experimental::SubscriptionROSMsgIntraProcessBuffer + >(subscription_base); + if (nullptr == ros_message_subscription) { + throw std::runtime_error( + "failed to dynamic cast SubscriptionIntraProcessBase to " + "SubscriptionIntraProcessBuffer, or to " + "SubscriptionROSMsgIntraProcessBuffer which can happen when the publisher and " + "subscription use different allocator types, which is not supported"); + } + + if constexpr (rclcpp::TypeAdapter::is_specialized::value) { + ROSMessageTypeAllocator ros_message_alloc(allocator); + auto ptr = ROSMessageTypeAllocatorTraits::allocate(ros_message_alloc, 1); + ROSMessageTypeAllocatorTraits::construct(ros_message_alloc, ptr); + ROSMessageTypeDeleter deleter; + allocator::set_allocator_for_deleter(&deleter, &allocator); + rclcpp::TypeAdapter::convert_to_ros_message(*message, *ptr); + auto ros_msg = std::unique_ptr(ptr, deleter); + ros_message_subscription->provide_intra_process_message(std::move(ros_msg)); + } else { + if constexpr (std::is_same::value) { + if (std::next(it) == subscription_ids.end()) { + // If this is the last subscription, give up ownership + ros_message_subscription->provide_intra_process_message(std::move(message)); + // Last message delivered, break from for loop + break; + } else { + // Copy the message since we have additional subscriptions to serve + Deleter deleter = message.get_deleter(); + allocator::set_allocator_for_deleter(&deleter, &allocator); + auto ptr = MessageAllocTraits::allocate(allocator, 1); + MessageAllocTraits::construct(allocator, ptr, *message); + + ros_message_subscription->provide_intra_process_message( + MessageUniquePtr(ptr, deleter)); + } + } + } + } + } + + PublisherToSubscriptionIdsMap pub_to_subs_; + SubscriptionMap subscriptions_; + PublisherMap publishers_; + PublisherBufferMap publisher_buffers_; + + mutable std::shared_timed_mutex mutex_; +}; + +} // namespace experimental +} // namespace rclcpp + +#endif // RCLCPP__EXPERIMENTAL__INTRA_PROCESS_MANAGER_HPP_ diff --git a/rclcpp/include/rclcpp/experimental/ros_message_intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/ros_message_intra_process_buffer.hpp new file mode 100644 index 0000000000..7b8b3b833d --- /dev/null +++ b/rclcpp/include/rclcpp/experimental/ros_message_intra_process_buffer.hpp @@ -0,0 +1,68 @@ +// 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 RCLCPP__EXPERIMENTAL__ROS_MESSAGE_INTRA_PROCESS_BUFFER_HPP_ +#define RCLCPP__EXPERIMENTAL__ROS_MESSAGE_INTRA_PROCESS_BUFFER_HPP_ + +#include +#include + +#include "rcl/error_handling.h" + +#include "rclcpp/any_subscription_callback.hpp" +#include "rclcpp/context.hpp" +#include "rclcpp/experimental/subscription_intra_process_base.hpp" +#include "tracetools/tracetools.h" + +namespace rclcpp +{ +namespace experimental +{ + +template< + typename RosMessageT, + typename Alloc = std::allocator, + typename Deleter = std::default_delete +> +class SubscriptionROSMsgIntraProcessBuffer : public SubscriptionIntraProcessBase +{ +public: + using ROSMessageTypeAllocatorTraits = allocator::AllocRebind; + using ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type; + using ROSMessageTypeDeleter = allocator::Deleter; + + using ConstMessageSharedPtr = std::shared_ptr; + using MessageUniquePtr = std::unique_ptr; + + SubscriptionROSMsgIntraProcessBuffer( + rclcpp::Context::SharedPtr context, + const std::string & topic_name, + const rclcpp::QoS & qos_profile) + : SubscriptionIntraProcessBase(context, topic_name, qos_profile) + {} + + virtual ~SubscriptionROSMsgIntraProcessBuffer() + {} + + virtual void + provide_intra_process_message(ConstMessageSharedPtr message) = 0; + + virtual void + provide_intra_process_message(MessageUniquePtr message) = 0; +}; + +} // namespace experimental +} // namespace rclcpp + +#endif // RCLCPP__EXPERIMENTAL__ROS_MESSAGE_INTRA_PROCESS_BUFFER_HPP_ diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp new file mode 100644 index 0000000000..0624f92c62 --- /dev/null +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp @@ -0,0 +1,196 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_HPP_ +#define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_HPP_ + +#include + +#include +#include +#include +#include +#include + +#include "rcl/types.h" + +#include "rclcpp/any_subscription_callback.hpp" +#include "rclcpp/context.hpp" +#include "rclcpp/experimental/buffers/intra_process_buffer.hpp" +#include "rclcpp/experimental/subscription_intra_process_buffer.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/type_support_decl.hpp" +#include "tracetools/tracetools.h" + +namespace rclcpp +{ +namespace experimental +{ + +template< + typename MessageT, + typename SubscribedType, + typename SubscribedTypeAlloc = std::allocator, + typename SubscribedTypeDeleter = std::default_delete, + typename ROSMessageType = SubscribedType, + typename Alloc = std::allocator +> +class SubscriptionIntraProcess + : public SubscriptionIntraProcessBuffer< + SubscribedType, + SubscribedTypeAlloc, + SubscribedTypeDeleter, + ROSMessageType + > +{ + using SubscriptionIntraProcessBufferT = SubscriptionIntraProcessBuffer< + SubscribedType, + SubscribedTypeAlloc, + SubscribedTypeDeleter, + ROSMessageType + >; + +public: + RCLCPP_SMART_PTR_DEFINITIONS(SubscriptionIntraProcess) + + using MessageAllocTraits = + typename SubscriptionIntraProcessBufferT::SubscribedTypeAllocatorTraits; + using MessageAlloc = typename SubscriptionIntraProcessBufferT::SubscribedTypeAllocator; + using ConstMessageSharedPtr = typename SubscriptionIntraProcessBufferT::ConstDataSharedPtr; + using MessageUniquePtr = typename SubscriptionIntraProcessBufferT::SubscribedTypeUniquePtr; + using BufferUniquePtr = typename SubscriptionIntraProcessBufferT::BufferUniquePtr; + + SubscriptionIntraProcess( + AnySubscriptionCallback callback, + std::shared_ptr allocator, + rclcpp::Context::SharedPtr context, + const std::string & topic_name, + const rclcpp::QoS & qos_profile, + rclcpp::IntraProcessBufferType buffer_type) + : SubscriptionIntraProcessBuffer( + std::make_shared(*allocator), + context, + topic_name, + qos_profile, + buffer_type), + any_callback_(callback) + { + TRACETOOLS_TRACEPOINT( + rclcpp_subscription_callback_added, + static_cast(this), + static_cast(&any_callback_)); + // The callback object gets copied, so if registration is done too early/before this point + // (e.g. in `AnySubscriptionCallback::set()`), its address won't match any address used later + // in subsequent tracepoints. +#ifndef TRACETOOLS_DISABLED + any_callback_.register_callback_for_tracing(); +#endif + } + + virtual ~SubscriptionIntraProcess() = default; + + void + add_to_wait_set(rcl_wait_set_t & wait_set) override + { + // This block is necessary when the guard condition wakes the wait set, but + // the intra process waitable was not handled before the wait set is waited + // on again. + // Basically we're keeping the guard condition triggered so long as there is + // data in the buffer. + if (this->buffer_->has_data()) { + // If there is data still to be processed, indicate to the + // executor or waitset by triggering the guard condition. + this->trigger_guard_condition(); + } + // Let the parent classes handle the rest of the work: + return SubscriptionIntraProcessBufferT::add_to_wait_set(wait_set); + } + + std::shared_ptr + take_data() override + { + ConstMessageSharedPtr shared_msg; + MessageUniquePtr unique_msg; + + if (any_callback_.use_take_shared_method()) { + shared_msg = this->buffer_->consume_shared(); + if (!shared_msg) { + return nullptr; + } + } else { + unique_msg = this->buffer_->consume_unique(); + if (!unique_msg) { + return nullptr; + } + } + + if (this->buffer_->has_data()) { + // If there is data still to be processed, indicate to the + // executor or waitset by triggering the guard condition. + this->trigger_guard_condition(); + } + + return std::static_pointer_cast( + std::make_shared>( + std::pair( + shared_msg, std::move(unique_msg))) + ); + } + + void execute(const std::shared_ptr & data) override + { + execute_impl(data); + } + +protected: + template + typename std::enable_if::value, void>::type + execute_impl(const std::shared_ptr &) + { + throw std::runtime_error("Subscription intra-process can't handle serialized messages"); + } + + template + typename std::enable_if::value, void>::type + execute_impl(const std::shared_ptr & data) + { + if (nullptr == data) { + return; + } + + rmw_message_info_t msg_info; + msg_info.publisher_gid = {0, {0}}; + msg_info.from_intra_process = true; + + auto shared_ptr = std::static_pointer_cast>( + data); + + if (any_callback_.use_take_shared_method()) { + ConstMessageSharedPtr shared_msg = shared_ptr->first; + any_callback_.dispatch_intra_process(shared_msg, msg_info); + } else { + MessageUniquePtr unique_msg = std::move(shared_ptr->second); + any_callback_.dispatch_intra_process(std::move(unique_msg), msg_info); + } + shared_ptr.reset(); + } + + AnySubscriptionCallback any_callback_; +}; + +} // namespace experimental +} // namespace rclcpp + +#endif // RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_HPP_ diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp new file mode 100644 index 0000000000..74792e8751 --- /dev/null +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp @@ -0,0 +1,211 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_ +#define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_ + +#include +#include +#include +#include + +#include "rcl/wait.h" +#include "rmw/impl/cpp/demangle.hpp" + +#include "rclcpp/guard_condition.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/waitable.hpp" + +namespace rclcpp +{ +namespace experimental +{ + +class SubscriptionIntraProcessBase : public rclcpp::Waitable +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(SubscriptionIntraProcessBase) + + enum class EntityType : std::size_t + { + Subscription, + }; + + RCLCPP_PUBLIC + SubscriptionIntraProcessBase( + rclcpp::Context::SharedPtr context, + const std::string & topic_name, + const rclcpp::QoS & qos_profile) + : gc_(context), topic_name_(topic_name), qos_profile_(qos_profile) + {} + + RCLCPP_PUBLIC + virtual ~SubscriptionIntraProcessBase() = default; + + RCLCPP_PUBLIC + size_t + get_number_of_ready_guard_conditions() override {return 1;} + + RCLCPP_PUBLIC + void + add_to_wait_set(rcl_wait_set_t & wait_set) override; + + RCLCPP_PUBLIC + virtual + size_t + available_capacity() const = 0; + + RCLCPP_PUBLIC + bool + is_durability_transient_local() const; + + bool + is_ready(const rcl_wait_set_t & wait_set) override = 0; + + std::shared_ptr + take_data() override = 0; + + std::shared_ptr + take_data_by_entity_id(size_t id) override + { + (void)id; + return take_data(); + } + + void + execute(const std::shared_ptr & data) override = 0; + + virtual + bool + use_take_shared_method() const = 0; + + RCLCPP_PUBLIC + const char * + get_topic_name() const; + + RCLCPP_PUBLIC + QoS + get_actual_qos() const; + + /// Set a callback to be called when each new message arrives. + /** + * The callback receives a size_t which is the number of messages received + * since the last time this callback was called. + * Normally this is 1, but can be > 1 if messages were received before any + * callback was set. + * + * The callback also receives an int identifier argument. + * This is needed because a Waitable may be composed of several distinct entities, + * such as subscriptions, services, etc. + * The application should provide a generic callback function that will be then + * forwarded by the waitable to all of its entities. + * Before forwarding, a different value for the identifier argument will be + * bound to the function. + * This implies that the provided callback can use the identifier to behave + * differently depending on which entity triggered the waitable to become ready. + * + * Calling it again will clear any previously set callback. + * + * An exception will be thrown if the callback is not callable. + * + * This function is thread-safe. + * + * If you want more information available in the callback, like the subscription + * or other information, you may use a lambda with captures or std::bind. + * + * \param[in] callback functor to be called when a new message is received. + */ + void + set_on_ready_callback(std::function callback) override + { + if (!callback) { + throw std::invalid_argument( + "The callback passed to set_on_ready_callback " + "is not callable."); + } + + // Note: we bind the int identifier argument to this waitable's entity types + auto new_callback = + [callback, this](size_t number_of_events) { + try { + callback(number_of_events, static_cast(EntityType::Subscription)); + } catch (const std::exception & exception) { + RCLCPP_ERROR_STREAM( + // TODO(wjwwood): get this class access to the node logger it is associated with + rclcpp::get_logger("rclcpp"), + "rclcpp::SubscriptionIntraProcessBase@" << this << + " caught " << rmw::impl::cpp::demangle(exception) << + " exception in user-provided callback for the 'on ready' callback: " << + exception.what()); + } catch (...) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("rclcpp"), + "rclcpp::SubscriptionIntraProcessBase@" << this << + " caught unhandled exception in user-provided callback " << + "for the 'on ready' callback"); + } + }; + + std::lock_guard lock(callback_mutex_); + on_new_message_callback_ = new_callback; + + if (unread_count_ > 0) { + if (qos_profile_.history() == HistoryPolicy::KeepAll) { + on_new_message_callback_(unread_count_); + } else { + // Use qos profile depth as upper bound for unread_count_ + on_new_message_callback_(std::min(unread_count_, qos_profile_.depth())); + } + unread_count_ = 0; + } + } + + /// Unset the callback registered for new messages, if any. + void + clear_on_ready_callback() override + { + std::lock_guard lock(callback_mutex_); + on_new_message_callback_ = nullptr; + } + +protected: + std::recursive_mutex callback_mutex_; + std::function on_new_message_callback_ {nullptr}; + size_t unread_count_{0}; + rclcpp::GuardCondition gc_; + + virtual void + trigger_guard_condition() = 0; + + void + invoke_on_new_message() + { + std::lock_guard lock(this->callback_mutex_); + if (this->on_new_message_callback_) { + this->on_new_message_callback_(1); + } else { + this->unread_count_++; + } + } + +private: + std::string topic_name_; + QoS qos_profile_; +}; + +} // namespace experimental +} // namespace rclcpp + +#endif // RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_ diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp new file mode 100644 index 0000000000..2f384f351c --- /dev/null +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp @@ -0,0 +1,202 @@ +// 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 RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BUFFER_HPP_ +#define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BUFFER_HPP_ + +#include +#include +#include +#include + +#include "rcl/error_handling.h" +#include "rcl/guard_condition.h" +#include "rcl/wait.h" + +#include "rclcpp/experimental/buffers/intra_process_buffer.hpp" +#include "rclcpp/experimental/create_intra_process_buffer.hpp" +#include "rclcpp/experimental/subscription_intra_process_base.hpp" +#include "rclcpp/experimental/ros_message_intra_process_buffer.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/type_support_decl.hpp" +#include "rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp" + +#include "tracetools/tracetools.h" + +namespace rclcpp +{ +namespace experimental +{ + +template< + typename SubscribedType, + typename Alloc = std::allocator, + typename Deleter = std::default_delete, + /// MessageT::ros_message_type if MessageT is a TypeAdapter, + /// otherwise just MessageT. + typename ROSMessageType = SubscribedType +> +class SubscriptionIntraProcessBuffer : public SubscriptionROSMsgIntraProcessBuffer::allocator_type, + allocator::Deleter::allocator_type, + ROSMessageType>> +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(SubscriptionIntraProcessBuffer) + + using SubscribedTypeAllocatorTraits = allocator::AllocRebind; + using SubscribedTypeAllocator = typename SubscribedTypeAllocatorTraits::allocator_type; + using SubscribedTypeDeleter = allocator::Deleter; + + using ROSMessageTypeAllocatorTraits = allocator::AllocRebind; + using ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type; + using ROSMessageTypeDeleter = allocator::Deleter; + + using ConstMessageSharedPtr = std::shared_ptr; + using MessageUniquePtr = std::unique_ptr; + + using ConstDataSharedPtr = std::shared_ptr; + using SubscribedTypeUniquePtr = std::unique_ptr; + + using BufferUniquePtr = typename rclcpp::experimental::buffers::IntraProcessBuffer< + SubscribedType, + Alloc, + SubscribedTypeDeleter + >::UniquePtr; + + SubscriptionIntraProcessBuffer( + std::shared_ptr allocator, + rclcpp::Context::SharedPtr context, + const std::string & topic_name, + const rclcpp::QoS & qos_profile, + rclcpp::IntraProcessBufferType buffer_type) + : SubscriptionROSMsgIntraProcessBuffer( + context, topic_name, qos_profile), + subscribed_type_allocator_(*allocator) + { + allocator::set_allocator_for_deleter(&subscribed_type_deleter_, &subscribed_type_allocator_); + + // Create the intra-process buffer. + buffer_ = rclcpp::experimental::create_intra_process_buffer( + buffer_type, + qos_profile, + std::make_shared(subscribed_type_allocator_)); + TRACETOOLS_TRACEPOINT( + rclcpp_ipb_to_subscription, + static_cast(buffer_.get()), + static_cast(this)); + } + + void + add_to_wait_set(rcl_wait_set_t & wait_set) override + { + if (this->buffer_->has_data()) { + this->trigger_guard_condition(); + } + detail::add_guard_condition_to_rcl_wait_set(wait_set, this->gc_); + } + + bool + is_ready(const rcl_wait_set_t & wait_set) override + { + (void) wait_set; + return buffer_->has_data(); + } + + SubscribedTypeUniquePtr + convert_ros_message_to_subscribed_type_unique_ptr(const ROSMessageType & msg) + { + if constexpr (!std::is_same::value) { + auto ptr = SubscribedTypeAllocatorTraits::allocate(subscribed_type_allocator_, 1); + SubscribedTypeAllocatorTraits::construct(subscribed_type_allocator_, ptr); + rclcpp::TypeAdapter::convert_to_custom(msg, *ptr); + return SubscribedTypeUniquePtr(ptr, subscribed_type_deleter_); + } else { + throw std::runtime_error( + "convert_ros_message_to_subscribed_type_unique_ptr " + "unexpectedly called without TypeAdapter"); + } + } + + void + provide_intra_process_message(ConstMessageSharedPtr message) override + { + if constexpr (std::is_same::value) { + buffer_->add_shared(std::move(message)); + trigger_guard_condition(); + } else { + buffer_->add_shared(convert_ros_message_to_subscribed_type_unique_ptr(*message)); + trigger_guard_condition(); + } + this->invoke_on_new_message(); + } + + void + provide_intra_process_message(MessageUniquePtr message) override + { + if constexpr (std::is_same::value) { + buffer_->add_unique(std::move(message)); + trigger_guard_condition(); + } else { + buffer_->add_unique(convert_ros_message_to_subscribed_type_unique_ptr(*message)); + trigger_guard_condition(); + } + this->invoke_on_new_message(); + } + + void + provide_intra_process_data(ConstDataSharedPtr message) + { + buffer_->add_shared(std::move(message)); + trigger_guard_condition(); + this->invoke_on_new_message(); + } + + void + provide_intra_process_data(SubscribedTypeUniquePtr message) + { + buffer_->add_unique(std::move(message)); + trigger_guard_condition(); + this->invoke_on_new_message(); + } + + bool + use_take_shared_method() const override + { + return buffer_->use_take_shared_method(); + } + + size_t available_capacity() const override + { + return buffer_->available_capacity(); + } + +protected: + void + trigger_guard_condition() override + { + this->gc_.trigger(); + } + + BufferUniquePtr buffer_; + SubscribedTypeAllocator subscribed_type_allocator_; + SubscribedTypeDeleter subscribed_type_deleter_; +}; + +} // namespace experimental +} // namespace rclcpp + +#endif // RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BUFFER_HPP_ diff --git a/rclcpp/include/rclcpp/experimental/timers_manager.hpp b/rclcpp/include/rclcpp/experimental/timers_manager.hpp new file mode 100644 index 0000000000..af3337bfd6 --- /dev/null +++ b/rclcpp/include/rclcpp/experimental/timers_manager.hpp @@ -0,0 +1,558 @@ +// Copyright 2023 iRobot Corporation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT 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 RCLCPP__EXPERIMENTAL__TIMERS_MANAGER_HPP_ +#define RCLCPP__EXPERIMENTAL__TIMERS_MANAGER_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "rclcpp/context.hpp" +#include "rclcpp/timer.hpp" + +namespace rclcpp +{ +namespace experimental +{ + +/** + * @brief This class provides a way for storing and executing timer objects. + * It provides APIs to suit the needs of different applications and execution models. + * All public APIs provided by this class are thread-safe. + * + * Timers management + * This class provides APIs to add/remove timers to/from an internal storage. + * It keeps a list of weak pointers from added timers, and locks them only when + * they need to be executed or modified. + * Timers are kept ordered in a binary-heap priority queue. + * Calls to add/remove APIs will temporarily block the execution of the timers and + * will require to reorder the internal priority queue. + * Because of this, they have a not-negligible impact on the performance. + * + * Timers execution + * The most efficient use of this class consists in letting a TimersManager object + * to spawn a thread where timers are monitored and optionally executed. + * This can be controlled via the `start` and `stop` methods. + * Ready timers can either be executed or an on_ready_callback can be used to notify + * other entities that they are ready and need to be executed. + * Other APIs allow to directly execute a given timer. + * + * This class assumes that the `execute_callback()` API of the stored timers is never + * called by other entities, but it can only be called from here. + * If this assumption is not respected, the heap property may be invalidated, + * so timers may be executed out of order, without this object noticing it. + * + */ +class TimersManager +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(TimersManager) + + /** + * @brief Construct a new TimersManager object + * + * @param context custom context to be used. + * Shared ownership of the context is held until destruction. + * @param on_ready_callback The timers on ready callback. The presence of this function + * indicates what to do when the TimersManager is running and a timer becomes ready. + * The TimersManager is considered "running" when the `start` method has been called. + * If it's callable, it will be invoked instead of the timer callback. + * If it's not callable, then the TimersManager will + * directly execute timers when they are ready. + * All the methods that execute a given timer (e.g. `execute_head_timer` + * or `execute_ready_timer`) without the TimersManager being `running`, i.e. + * without actually explicitly waiting for the timer to become ready, will ignore this + * callback. + */ + RCLCPP_PUBLIC + TimersManager( + std::shared_ptr context, + std::function &)> on_ready_callback = nullptr); + + /** + * @brief Destruct the TimersManager object making sure to stop thread and release memory. + */ + RCLCPP_PUBLIC + ~TimersManager(); + + /** + * @brief Adds a new timer to the storage, maintaining weak ownership of it. + * Function is thread safe and it can be called regardless of the state of the timers thread. + * + * @param timer the timer to add. + * @throws std::invalid_argument if timer is a nullptr. + */ + RCLCPP_PUBLIC + void add_timer(rclcpp::TimerBase::SharedPtr timer); + + /** + * @brief Remove a single timer from the object storage. + * Will do nothing if the timer was not being stored here. + * Function is thread safe and it can be called regardless of the state of the timers thread. + * + * @param timer the timer to remove. + */ + RCLCPP_PUBLIC + void remove_timer(rclcpp::TimerBase::SharedPtr timer); + + /** + * @brief Remove all the timers stored in the object. + * Function is thread safe and it can be called regardless of the state of the timers thread. + */ + RCLCPP_PUBLIC + void clear(); + + /** + * @brief Starts a thread that takes care of executing the timers stored in this object. + * Function will throw an error if the timers thread was already running. + */ + RCLCPP_PUBLIC + void start(); + + /** + * @brief Stops the timers thread. + * Will do nothing if the timer thread was not running. + */ + RCLCPP_PUBLIC + void stop(); + + /** + * @brief Get the number of timers that are currently ready. + * This function is thread safe. + * + * @return size_t number of ready timers. + * @throws std::runtime_error if the timers thread was already running. + */ + RCLCPP_PUBLIC + size_t get_number_ready_timers(); + + /** + * @brief Executes head timer if ready. + * This function is thread safe. + * This function will try to execute the timer callback regardless of whether + * the TimersManager on_ready_callback was passed during construction. + * + * @return true if head timer was ready. + * @throws std::runtime_error if the timers thread was already running. + */ + RCLCPP_PUBLIC + bool execute_head_timer(); + + /** + * @brief Executes timer identified by its ID. + * This function is thread safe. + * This function will try to execute the timer callback regardless of whether + * the TimersManager on_ready_callback was passed during construction. + * + * @param timer_id the timer ID of the timer to execute + * @param data internal data of the timer + */ + RCLCPP_PUBLIC + void execute_ready_timer(const rclcpp::TimerBase * timer_id, const std::shared_ptr & data); + + /** + * @brief Get the amount of time before the next timer triggers. + * This function is thread safe. + * + * @return std::optional to wait, + * the returned value could be negative if the timer is already expired + * or std::chrono::nanoseconds::max() if there are no timers stored in the object. + * If the head timer was cancelled, then this will return a nullopt. + * @throws std::runtime_error if the timers thread was already running. + */ + RCLCPP_PUBLIC + std::optional get_head_timeout(); + +private: + RCLCPP_DISABLE_COPY(TimersManager) + + using TimerPtr = rclcpp::TimerBase::SharedPtr; + using WeakTimerPtr = rclcpp::TimerBase::WeakPtr; + + // Forward declaration + class TimersHeap; + + /** + * @brief This class allows to store weak pointers to timers in a heap-like data structure. + * The root of the heap is the timer that triggers first. + * Since this class uses weak ownership, it is not guaranteed that it represents a valid heap + * at any point in time as timers could go out of scope, thus invalidating it. + * The "validate_and_lock" API allows to restore the heap property and also returns a locked version + * of the timers heap. + * This class is not thread safe and requires external mutexes to protect its usage. + */ + class WeakTimersHeap + { +public: + /** + * @brief Add a new timer to the heap. After the addition, the heap property is enforced. + * + * @param timer new timer to add. + * @return true if timer has been added, false if it was already there. + */ + bool add_timer(TimerPtr timer) + { + TimersHeap locked_heap = this->validate_and_lock(); + bool added = locked_heap.add_timer(std::move(timer)); + + if (added) { + // Re-create the weak heap with the new timer added + this->store(locked_heap); + } + + return added; + } + + /** + * @brief Remove a timer from the heap. After the removal, the heap property is enforced. + * + * @param timer timer to remove. + * @return true if timer has been removed, false if it was not there. + */ + bool remove_timer(TimerPtr timer) + { + TimersHeap locked_heap = this->validate_and_lock(); + bool removed = locked_heap.remove_timer(std::move(timer)); + + if (removed) { + // Re-create the weak heap with the timer removed + this->store(locked_heap); + } + + return removed; + } + + /** + * @brief Retrieve the timer identified by the key + * @param timer_id The ID of the timer to retrieve. + * @return TimerPtr if there's a timer associated with the ID, nullptr otherwise + */ + TimerPtr get_timer(const rclcpp::TimerBase * timer_id) + { + for (auto & weak_timer : weak_heap_) { + auto timer = weak_timer.lock(); + if (timer.get() == timer_id) { + return timer; + } + } + return nullptr; + } + + /** + * @brief Returns a const reference to the front element. + */ + const WeakTimerPtr & front() const + { + return weak_heap_.front(); + } + + /** + * @brief Returns whether the heap is empty or not. + */ + bool empty() const + { + return weak_heap_.empty(); + } + + /** + * @brief This function restores the current object as a valid heap + * and it returns a locked version of it. + * Timers that went out of scope are removed from the container. + * It is the only public API to access and manipulate the stored timers. + * + * @return TimersHeap owned timers corresponding to the current object + */ + TimersHeap validate_and_lock() + { + TimersHeap locked_heap; + bool any_timer_destroyed = false; + + for (auto weak_timer : weak_heap_) { + auto timer = weak_timer.lock(); + if (timer) { + // This timer is valid, so add it to the locked heap + // Note: we access friend private `owned_heap_` member field. + locked_heap.owned_heap_.push_back(std::move(timer)); + } else { + // This timer went out of scope, so we don't add it to locked heap + // and we mark the corresponding flag. + // It's not needed to erase it from weak heap, as we are going to re-heapify. + // Note: we can't exit from the loop here, as we need to find all valid timers. + any_timer_destroyed = true; + } + } + + // If a timer has gone out of scope, then the remaining elements do not represent + // a valid heap anymore. We need to re-heapify the timers heap. + if (any_timer_destroyed) { + locked_heap.heapify(); + // Re-create the weak heap now that elements have been heapified again + this->store(locked_heap); + } + + return locked_heap; + } + + /** + * @brief This function allows to recreate the heap of weak pointers + * from an heap of owned pointers. + * It is required to be called after a locked TimersHeap generated from this object + * has been modified in any way (e.g. timers triggered, added, removed). + * + * @param heap timers heap to store as weak pointers + */ + void store(const TimersHeap & heap) + { + weak_heap_.clear(); + // Note: we access friend private `owned_heap_` member field. + for (auto t : heap.owned_heap_) { + weak_heap_.push_back(t); + } + } + + /** + * @brief Remove all timers from the heap. + */ + void clear() + { + weak_heap_.clear(); + } + +private: + std::vector weak_heap_; + }; + + /** + * @brief This class is the equivalent of WeakTimersHeap but with ownership of the timers. + * It can be generated by locking the weak version. + * It provides operations to manipulate the heap. + * This class is not thread safe and requires external mutexes to protect its usage. + */ + class TimersHeap + { +public: + /** + * @brief Try to add a new timer to the heap. + * After the addition, the heap property is preserved. + * @param timer new timer to add. + * @return true if timer has been added, false if it was already there. + */ + bool add_timer(TimerPtr timer) + { + // Nothing to do if the timer is already stored here + auto it = std::find(owned_heap_.begin(), owned_heap_.end(), timer); + if (it != owned_heap_.end()) { + return false; + } + + owned_heap_.push_back(std::move(timer)); + std::push_heap(owned_heap_.begin(), owned_heap_.end(), timer_greater); + + return true; + } + + /** + * @brief Try to remove a timer from the heap. + * After the removal, the heap property is preserved. + * @param timer timer to remove. + * @return true if timer has been removed, false if it was not there. + */ + bool remove_timer(TimerPtr timer) + { + // Nothing to do if the timer is not stored here + auto it = std::find(owned_heap_.begin(), owned_heap_.end(), timer); + if (it == owned_heap_.end()) { + return false; + } + + owned_heap_.erase(it); + this->heapify(); + + return true; + } + + /** + * @brief Returns a reference to the front element. + * @return reference to front element. + */ + TimerPtr & front() + { + return owned_heap_.front(); + } + + /** + * @brief Returns a const reference to the front element. + * @return const reference to front element. + */ + const TimerPtr & front() const + { + return owned_heap_.front(); + } + + /** + * @brief Returns whether the heap is empty or not. + * @return true if the heap is empty. + */ + bool empty() const + { + return owned_heap_.empty(); + } + + /** + * @brief Returns the size of the heap. + * @return the number of valid timers in the heap. + */ + size_t size() const + { + return owned_heap_.size(); + } + + /** + * @brief Get the number of timers that are currently ready. + * @return size_t number of ready timers. + */ + size_t get_number_ready_timers() const + { + size_t ready_timers = 0; + + for (TimerPtr t : owned_heap_) { + if (t->is_ready()) { + ready_timers++; + } + } + + return ready_timers; + } + + /** + * @brief Restore a valid heap after the root value has been replaced (e.g. timer triggered). + */ + void heapify_root() + { + // The following code is a more efficient version than doing + // pop_heap, pop_back, push_back, push_heap + // as it removes the need for the last push_heap + + // Push the modified element (i.e. the current root) at the bottom of the heap + owned_heap_.push_back(owned_heap_[0]); + // Exchange first and last-1 elements and reheapify + std::pop_heap(owned_heap_.begin(), owned_heap_.end(), timer_greater); + // Remove last element + owned_heap_.pop_back(); + } + + /** + * @brief Completely restores the structure to a valid heap + */ + void heapify() + { + std::make_heap(owned_heap_.begin(), owned_heap_.end(), timer_greater); + } + + /** + * @brief Helper function to clear the "on_reset_callback" on all associated timers. + */ + void clear_timers_on_reset_callbacks() + { + for (TimerPtr & t : owned_heap_) { + t->clear_on_reset_callback(); + } + } + + /** + * @brief Friend declaration to allow the `validate_and_lock()` function to access the + * underlying heap container + */ + friend TimersHeap WeakTimersHeap::validate_and_lock(); + + /** + * @brief Friend declaration to allow the `store()` function to access the + * underlying heap container + */ + friend void WeakTimersHeap::store(const TimersHeap & heap); + +private: + /** + * @brief Comparison function between timers. + * @return true if `a` triggers after `b`. + */ + static bool timer_greater(TimerPtr a, TimerPtr b) + { + // TODO(alsora): this can cause an error if timers are using different clocks + return a->time_until_trigger() > b->time_until_trigger(); + } + + std::vector owned_heap_; + }; + + /** + * @brief Implements a loop that keeps executing ready timers. + * This function is executed in the timers thread. + */ + void run_timers(); + + /** + * @brief Get the amount of time before the next timer triggers. + * This function is not thread safe, acquire a mutex before calling it. + * + * @return std::optional to wait, + * the returned value could be negative if the timer is already expired + * or std::chrono::nanoseconds::max() if the heap is empty. + * If the head timer was cancelled, then this will return a nullopt. + * This function is not thread safe, acquire the timers_mutex_ before calling it. + */ + std::optional get_head_timeout_unsafe(); + + /** + * @brief Executes all the timers currently ready when the function is invoked + * while keeping the heap correctly sorted. + * This function is not thread safe, acquire the timers_mutex_ before calling it. + */ + void execute_ready_timers_unsafe(); + + // Callback to be called when timer is ready + std::function &)> on_ready_callback_ = nullptr; + + // Thread used to run the timers execution task + std::thread timers_thread_; + // Protects access to timers + std::mutex timers_mutex_; + // Protects access to stop() + std::mutex stop_mutex_; + // Notifies the timers thread whenever timers are added/removed + std::condition_variable timers_cv_; + // Flag used as predicate by timers_cv_ that denotes one or more timers being added/removed + bool timers_updated_ {false}; + // Indicates whether the timers thread is currently running or not + std::atomic running_ {false}; + // Parent context used to understand if ROS is still active + std::shared_ptr context_; + // Timers heap storage with weak ownership + WeakTimersHeap weak_timers_heap_; +}; + +} // namespace experimental +} // namespace rclcpp + +#endif // RCLCPP__EXPERIMENTAL__TIMERS_MANAGER_HPP_ diff --git a/rclcpp/include/rclcpp/function_traits.hpp b/rclcpp/include/rclcpp/function_traits.hpp index 5f55cff594..b6af0aeb47 100644 --- a/rclcpp/include/rclcpp/function_traits.hpp +++ b/rclcpp/include/rclcpp/function_traits.hpp @@ -49,14 +49,14 @@ template struct function_traits { using arguments = typename tuple_tail< - typename function_traits::arguments>::type; + typename function_traits::arguments>::type; static constexpr std::size_t arity = std::tuple_size::value; template using argument_type = typename std::tuple_element::type; - using return_type = typename function_traits::return_type; + using return_type = typename function_traits::return_type; }; // Free functions @@ -80,16 +80,36 @@ struct function_traits: function_traits -#if defined _LIBCPP_VERSION // libc++ (Clang) -struct function_traits> +#if defined DOXYGEN_ONLY +struct function_traits> +#elif defined _LIBCPP_VERSION // libc++ (Clang) +struct function_traits> #elif defined _GLIBCXX_RELEASE // glibc++ (GNU C++ >= 7.1) -struct function_traits> +struct function_traits> #elif defined __GLIBCXX__ // glibc++ (GNU C++) struct function_traits(FArgs ...)>> #elif defined _MSC_VER // MS Visual Studio struct function_traits< - std::_Binder -> + std::_Binder> +#else +#error "Unsupported C++ compiler / standard library" +#endif + : function_traits +{}; + +// std::bind for object const methods +template +#if defined DOXYGEN_ONLY +struct function_traits> +#elif defined _LIBCPP_VERSION // libc++ (Clang) +struct function_traits> +#elif defined _GLIBCXX_RELEASE // glibc++ (GNU C++ >= 7.1) +struct function_traits> +#elif defined __GLIBCXX__ // glibc++ (GNU C++) +struct function_traits(FArgs ...)>> +#elif defined _MSC_VER // MS Visual Studio +struct function_traits< + std::_Binder> #else #error "Unsupported C++ compiler / standard library" #endif @@ -98,12 +118,14 @@ struct function_traits< // std::bind for free functions template -#if defined _LIBCPP_VERSION // libc++ (Clang) -struct function_traits> +#if defined DOXYGEN_ONLY +struct function_traits> +#elif defined _LIBCPP_VERSION // libc++ (Clang) +struct function_traits> #elif defined __GLIBCXX__ // glibc++ (GNU C++) struct function_traits> #elif defined _MSC_VER // MS Visual Studio -struct function_traits> +struct function_traits> #else #error "Unsupported C++ compiler / standard library" #endif @@ -130,22 +152,48 @@ struct function_traits: function_traits */ template struct arity_comparator : std::integral_constant< - bool, (Arity == function_traits::arity)>{}; + bool, (Arity == function_traits::arity)> {}; template struct check_arguments : std::is_same< typename function_traits::arguments, std::tuple - > +> {}; template struct same_arguments : std::is_same< typename function_traits::arguments, typename function_traits::arguments - > +> {}; +namespace detail +{ + +template +struct as_std_function_helper; + +template +struct as_std_function_helper> +{ + using type = std::function; +}; + +} // namespace detail + +template< + typename FunctorT, + typename FunctionTraits = function_traits +> +struct as_std_function +{ + using type = typename detail::as_std_function_helper< + typename FunctionTraits::return_type, + typename FunctionTraits::arguments + >::type; +}; + } // namespace function_traits } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/future_return_code.hpp b/rclcpp/include/rclcpp/future_return_code.hpp new file mode 100644 index 0000000000..0da67d7f7b --- /dev/null +++ b/rclcpp/include/rclcpp/future_return_code.hpp @@ -0,0 +1,47 @@ +// 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. + +#ifndef RCLCPP__FUTURE_RETURN_CODE_HPP_ +#define RCLCPP__FUTURE_RETURN_CODE_HPP_ + +#include +#include + +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ + +/// Return codes to be used with spin_until_future_complete. +/** + * SUCCESS: The future is complete and can be accessed with "get" without blocking. + * This does not indicate that the operation succeeded; "get" may still throw an exception. + * INTERRUPTED: The future is not complete, spinning was interrupted by Ctrl-C or another error. + * TIMEOUT: Spinning timed out. + */ +enum class FutureReturnCode {SUCCESS, INTERRUPTED, TIMEOUT}; + +/// Stream operator for FutureReturnCode. +RCLCPP_PUBLIC +std::ostream & +operator<<(std::ostream & os, const FutureReturnCode & future_return_code); + +/// String conversion function for FutureReturnCode. +RCLCPP_PUBLIC +std::string +to_string(const FutureReturnCode & future_return_code); + +} // namespace rclcpp + +#endif // RCLCPP__FUTURE_RETURN_CODE_HPP_ diff --git a/rclcpp/include/rclcpp/generic_client.hpp b/rclcpp/include/rclcpp/generic_client.hpp new file mode 100644 index 0000000000..1b853e0b6e --- /dev/null +++ b/rclcpp/include/rclcpp/generic_client.hpp @@ -0,0 +1,303 @@ +// Copyright 2023 Sony Group Corporation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT 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 RCLCPP__GENERIC_CLIENT_HPP_ +#define RCLCPP__GENERIC_CLIENT_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "rcl/client.h" + +#include "rclcpp/client.hpp" +#include "rclcpp/visibility_control.hpp" +#include "rcpputils/shared_library.hpp" + +#include "rosidl_typesupport_introspection_cpp/message_introspection.hpp" + +namespace rclcpp +{ +class GenericClient : public ClientBase +{ +public: + using Request = void *; // Deserialized data pointer of request message + using Response = void *; // Deserialized data pointer of response message + + using SharedResponse = std::shared_ptr; + + using Promise = std::promise; + using SharedPromise = std::shared_ptr; + + using Future = std::future; + using SharedFuture = std::shared_future; + + using CallbackType = std::function; + + RCLCPP_SMART_PTR_DEFINITIONS(GenericClient) + + /// A convenient GenericClient::Future and request id pair. + /** + * Public members: + * - future: a std::future. + * - request_id: the request id associated with the future. + * + * All the other methods are equivalent to the ones std::future provides. + */ + struct FutureAndRequestId + : detail::FutureAndRequestId + { + using detail::FutureAndRequestId::FutureAndRequestId; + + /// See std::future::share(). + SharedFuture share() noexcept {return this->future.share();} + + /// Move constructor. + FutureAndRequestId(FutureAndRequestId && other) noexcept = default; + /// Deleted copy constructor, each instance is a unique owner of the future. + FutureAndRequestId(const FutureAndRequestId & other) = delete; + /// Move assignment. + FutureAndRequestId & operator=(FutureAndRequestId && other) noexcept = default; + /// Deleted copy assignment, each instance is a unique owner of the future. + FutureAndRequestId & operator=(const FutureAndRequestId & other) = delete; + /// Destructor. + ~FutureAndRequestId() = default; + }; + + /// A convenient GenericClient::SharedFuture and request id pair. + /** + * Public members: + * - future: a std::shared_future. + * - request_id: the request id associated with the future. + * + * All the other methods are equivalent to the ones std::shared_future provides. + */ + struct SharedFutureAndRequestId + : detail::FutureAndRequestId> + { + using detail::FutureAndRequestId>::FutureAndRequestId; + }; + + GenericClient( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, + const std::string & service_name, + const std::string & service_type, + rcl_client_options_t & client_options); + + RCLCPP_PUBLIC + SharedResponse + create_response() override; + + RCLCPP_PUBLIC + std::shared_ptr + create_request_header() override; + + RCLCPP_PUBLIC + void + handle_response( + std::shared_ptr request_header, + std::shared_ptr response) override; + + /// Send a request to the service server. + /** + * This method returns a `FutureAndRequestId` instance + * that can be passed to Executor::spin_until_future_complete() to + * wait until it has been completed. + * + * If the future never completes, + * e.g. the call to Executor::spin_until_future_complete() times out, + * GenericClient::remove_pending_request() must be called to clean the client internal state. + * Not doing so will make the `GenericClient` instance to use more memory each time a response is + * not received from the service server. + * + * ```cpp + * auto future = generic_client->async_send_request(my_request); + * if ( + * rclcpp::FutureReturnCode::TIMEOUT == + * executor->spin_until_future_complete(future, timeout)) + * { + * generic_client->remove_pending_request(future); + * // handle timeout + * } else { + * handle_response(future.get()); + * } + * ``` + * + * \param[in] request request to be send. + * \return a FutureAndRequestId instance. + */ + RCLCPP_PUBLIC + FutureAndRequestId + async_send_request(const Request request); + + /// Send a request to the service server and schedule a callback in the executor. + /** + * Similar to the previous overload, but a callback will automatically be called when a response + * is received. + * + * If the callback is never called, because we never got a reply for the service server, + * remove_pending_request() has to be called with the returned request id or + * prune_pending_requests(). + * Not doing so will make the `GenericClient` instance use more memory each time a response is not + * received from the service server. + * In this case, it's convenient to setup a timer to cleanup the pending requests. + * + * \param[in] request request to be send. + * \param[in] cb callback that will be called when we get a response for this request. + * \return the request id representing the request just sent. + */ + template< + typename CallbackT, + typename std::enable_if< + rclcpp::function_traits::same_arguments< + CallbackT, + CallbackType + >::value + >::type * = nullptr + > + SharedFutureAndRequestId + async_send_request(const Request request, CallbackT && cb) + { + Promise promise; + auto shared_future = promise.get_future().share(); + auto req_id = async_send_request_impl( + request, + std::make_tuple( + CallbackType{std::forward(cb)}, + shared_future, + std::move(promise))); + return SharedFutureAndRequestId{std::move(shared_future), req_id}; + } + + /// Clean all pending requests older than a time_point. + /** + * \param[in] time_point Requests that were sent before this point are going to be removed. + * \param[inout] pruned_requests Removed requests id will be pushed to the vector + * if a pointer is provided. + * \return number of pending requests that were removed. + */ + template> + size_t + prune_requests_older_than( + std::chrono::time_point time_point, + std::vector * pruned_requests = nullptr) + { + return detail::prune_requests_older_than_impl( + pending_requests_, + pending_requests_mutex_, + time_point, + pruned_requests); + } + + /// Clean all pending requests. + /** + * \return number of pending requests that were removed. + */ + RCLCPP_PUBLIC + size_t + prune_pending_requests(); + + /// Cleanup a pending request. + /** + * This notifies the client that we have waited long enough for a response from the server + * to come, we have given up and we are not waiting for a response anymore. + * + * Not calling this will make the client start using more memory for each request + * that never got a reply from the server. + * + * \param[in] request_id request id returned by async_send_request(). + * \return true when a pending request was removed, false if not (e.g. a response was received). + */ + RCLCPP_PUBLIC + bool + remove_pending_request( + int64_t request_id); + + /// Cleanup a pending request. + /** + * Convenient overload, same as: + * + * `GenericClient::remove_pending_request(this, future.request_id)`. + */ + RCLCPP_PUBLIC + bool + remove_pending_request( + const FutureAndRequestId & future); + + /// Cleanup a pending request. + /** + * Convenient overload, same as: + * + * `GenericClient::remove_pending_request(this, future.request_id)`. + */ + RCLCPP_PUBLIC + bool + remove_pending_request( + const SharedFutureAndRequestId & future); + + /// Take the next response for this client. + /** + * \sa ClientBase::take_type_erased_response(). + * + * \param[out] response_out The reference to a Service Response into + * which the middleware will copy the response being taken. + * \param[out] request_header_out The request header to be filled by the + * middleware when taking, and which can be used to associate the response + * to a specific request. + * \returns true if the response was taken, otherwise false. + * \throws rclcpp::exceptions::RCLError based exceptions if the underlying + * rcl function fail. + */ + RCLCPP_PUBLIC + bool + take_response(Response response_out, rmw_request_id_t & request_header_out) + { + return this->take_type_erased_response(response_out, request_header_out); + } + +protected: + using CallbackTypeValueVariant = std::tuple; + using CallbackInfoVariant = std::variant< + std::promise, + CallbackTypeValueVariant>; // Use variant for extension + + RCLCPP_PUBLIC + int64_t + async_send_request_impl( + const Request request, + CallbackInfoVariant value); + + std::optional + get_and_erase_pending_request( + int64_t request_number); + + RCLCPP_DISABLE_COPY(GenericClient) + + std::map, + CallbackInfoVariant>> pending_requests_; + std::mutex pending_requests_mutex_; + +private: + std::shared_ptr ts_lib_; + const rosidl_typesupport_introspection_cpp::MessageMembers * response_members_; +}; +} // namespace rclcpp + +#endif // RCLCPP__GENERIC_CLIENT_HPP_ diff --git a/rclcpp/include/rclcpp/generic_publisher.hpp b/rclcpp/include/rclcpp/generic_publisher.hpp new file mode 100644 index 0000000000..292e6900d3 --- /dev/null +++ b/rclcpp/include/rclcpp/generic_publisher.hpp @@ -0,0 +1,117 @@ +// Copyright 2018, Bosch Software Innovations GmbH. +// Copyright 2021, Apex.AI 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 RCLCPP__GENERIC_PUBLISHER_HPP_ +#define RCLCPP__GENERIC_PUBLISHER_HPP_ + +#include +#include + +#include "rcpputils/shared_library.hpp" + +#include "rclcpp/callback_group.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_topics_interface.hpp" +#include "rclcpp/publisher_base.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/serialized_message.hpp" +#include "rclcpp/typesupport_helpers.hpp" +#include "rclcpp/visibility_control.hpp" + +#include "rmw/rmw.h" + +namespace rclcpp +{ + +/// %Publisher for serialized messages whose type is not known at compile time. +/** + * Since the type is not known at compile time, this is not a template, and the dynamic library + * containing type support information has to be identified and loaded based on the type name. + * + * It does not support intra-process handling. + */ +class GenericPublisher : public rclcpp::PublisherBase +{ +public: + // cppcheck-suppress unknownMacro + RCLCPP_SMART_PTR_DEFINITIONS(GenericPublisher) + + /// Constructor. + /** + * In order to properly publish to a topic, this publisher needs to be added to + * the node_topic_interface of the node passed into this constructor. + * + * \sa rclcpp::Node::create_generic_publisher() or rclcpp::create_generic_publisher() for + * creating an instance of this class and adding it to the node_topic_interface. + * + * \param node_base Pointer to parent node's NodeBaseInterface + * \param ts_lib Type support library, needs to correspond to topic_type + * \param topic_name Topic name + * \param topic_type Topic type + * \param qos %QoS settings + * \param options %Publisher options. + * Not all publisher options are currently respected, the only relevant options for this + * publisher are `event_callbacks`, `use_default_callbacks`, and `%callback_group`. + */ + template> + GenericPublisher( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + std::shared_ptr ts_lib, + const std::string & topic_name, + const std::string & topic_type, + const rclcpp::QoS & qos, + const rclcpp::PublisherOptionsWithAllocator & options) + : rclcpp::PublisherBase( + node_base, + topic_name, + *rclcpp::get_message_typesupport_handle(topic_type, "rosidl_typesupport_cpp", *ts_lib), + options.template to_rcl_publisher_options(qos), + // NOTE(methylDragon): Passing these args separately is necessary for event binding + options.event_callbacks, + options.use_default_callbacks), + ts_lib_(ts_lib) + {} + + RCLCPP_PUBLIC + virtual ~GenericPublisher() = default; + + /// Publish a rclcpp::SerializedMessage. + RCLCPP_PUBLIC + void publish(const rclcpp::SerializedMessage & message); + + /** + * Publish a rclcpp::SerializedMessage via loaned message after de-serialization. + * + * \param message a serialized message + * \throws anything rclcpp::exceptions::throw_from_rcl_error can show + */ + RCLCPP_PUBLIC + void publish_as_loaned_msg(const rclcpp::SerializedMessage & message); + +private: + // The type support library should stay loaded, so it is stored in the GenericPublisher + std::shared_ptr ts_lib_; + + void * borrow_loaned_message(); + void deserialize_message( + const rmw_serialized_message_t & serialized_message, + void * deserialized_msg); + void publish_loaned_message(void * loaned_message); +}; + +} // namespace rclcpp + +#endif // RCLCPP__GENERIC_PUBLISHER_HPP_ diff --git a/rclcpp/include/rclcpp/generic_service.hpp b/rclcpp/include/rclcpp/generic_service.hpp new file mode 100644 index 0000000000..b4c8d5d9a7 --- /dev/null +++ b/rclcpp/include/rclcpp/generic_service.hpp @@ -0,0 +1,308 @@ +// Copyright 2024 Sony Group Corporation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT 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 RCLCPP__GENERIC_SERVICE_HPP_ +#define RCLCPP__GENERIC_SERVICE_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "rclcpp/typesupport_helpers.hpp" + +#include "rosidl_runtime_c/service_type_support_struct.h" +#include "rosidl_typesupport_introspection_cpp/identifier.hpp" +#include "rosidl_typesupport_introspection_cpp/service_introspection.hpp" + +#include "service.hpp" + +namespace rclcpp +{ +class GenericService; + +class GenericServiceCallback +{ +public: + using SharedRequest = std::shared_ptr; + using SharedResponse = std::shared_ptr; + + GenericServiceCallback() + : callback_(std::monostate{}) + {} + + template< + typename CallbackT, + typename std::enable_if_t::value, int> = 0> + void + set(CallbackT && callback) + { + // Workaround Windows issue with std::bind + if constexpr ( + rclcpp::function_traits::same_arguments< + CallbackT, + SharedPtrCallback + >::value) + { + callback_.template emplace(callback); + } else if constexpr ( // NOLINT, can't satisfy both cpplint and uncrustify + rclcpp::function_traits::same_arguments< + CallbackT, + SharedPtrWithRequestHeaderCallback + >::value) + { + callback_.template emplace(callback); + } else if constexpr ( // NOLINT + rclcpp::function_traits::same_arguments< + CallbackT, + SharedPtrDeferResponseCallback + >::value) + { + callback_.template emplace(callback); + } else if constexpr ( // NOLINT + rclcpp::function_traits::same_arguments< + CallbackT, + SharedPtrDeferResponseCallbackWithServiceHandle + >::value) + { + callback_.template emplace(callback); + } else { + // the else clause is not needed, but anyways we should only be doing this instead + // of all the above workaround ... + callback_ = std::forward(callback); + } + } + + template< + typename CallbackT, + typename std::enable_if_t::value, int> = 0> + void + set(CallbackT && callback) + { + if (!callback) { + throw std::invalid_argument("AnyServiceCallback::set(): callback cannot be nullptr"); + } + // Workaround Windows issue with std::bind + if constexpr ( + rclcpp::function_traits::same_arguments< + CallbackT, + SharedPtrCallback + >::value) + { + callback_.template emplace(callback); + } else if constexpr ( // NOLINT + rclcpp::function_traits::same_arguments< + CallbackT, + SharedPtrWithRequestHeaderCallback + >::value) + { + callback_.template emplace(callback); + } else if constexpr ( // NOLINT + rclcpp::function_traits::same_arguments< + CallbackT, + SharedPtrDeferResponseCallback + >::value) + { + callback_.template emplace(callback); + } else if constexpr ( // NOLINT + rclcpp::function_traits::same_arguments< + CallbackT, + SharedPtrDeferResponseCallbackWithServiceHandle + >::value) + { + callback_.template emplace(callback); + } else { + // the else clause is not needed, but anyways we should only be doing this instead + // of all the above workaround ... + callback_ = std::forward(callback); + } + } + + SharedResponse + dispatch( + const std::shared_ptr & service_handle, + const std::shared_ptr & request_header, + SharedRequest request, + SharedRequest response) + { + TRACETOOLS_TRACEPOINT(callback_start, static_cast(this), false); + if (std::holds_alternative(callback_)) { + // TODO(ivanpauno): Remove the set method, and force the users of this class + // to pass a callback at construnciton. + throw std::runtime_error{"unexpected request without any callback set"}; + } + if (std::holds_alternative(callback_)) { + const auto & cb = std::get(callback_); + cb(request_header, std::move(request)); + return nullptr; + } + if (std::holds_alternative(callback_)) { + const auto & cb = std::get(callback_); + cb(service_handle, request_header, std::move(request)); + return nullptr; + } + + if (std::holds_alternative(callback_)) { + (void)request_header; + const auto & cb = std::get(callback_); + cb(std::move(request), std::move(response)); + } else if (std::holds_alternative(callback_)) { + const auto & cb = std::get(callback_); + cb(request_header, std::move(request), std::move(response)); + } + TRACETOOLS_TRACEPOINT(callback_end, static_cast(this)); + return response; + } + + void register_callback_for_tracing() + { +#ifndef TRACETOOLS_DISABLED + std::visit( + [this](auto && arg) { + if (TRACETOOLS_TRACEPOINT_ENABLED(rclcpp_callback_register)) { + char * symbol = tracetools::get_symbol(arg); + TRACETOOLS_DO_TRACEPOINT( + rclcpp_callback_register, + static_cast(this), + symbol); + std::free(symbol); + } + }, callback_); +#endif // TRACETOOLS_DISABLED + } + +private: + using SharedPtrCallback = std::function; + using SharedPtrWithRequestHeaderCallback = std::function< + void ( + std::shared_ptr, + SharedRequest, + SharedResponse + )>; + using SharedPtrDeferResponseCallback = std::function< + void ( + std::shared_ptr, + SharedRequest + )>; + using SharedPtrDeferResponseCallbackWithServiceHandle = std::function< + void ( + std::shared_ptr, + std::shared_ptr, + SharedRequest + )>; + + std::variant< + std::monostate, + SharedPtrCallback, + SharedPtrWithRequestHeaderCallback, + SharedPtrDeferResponseCallback, + SharedPtrDeferResponseCallbackWithServiceHandle> callback_; +}; + +class GenericService + : public ServiceBase, + public std::enable_shared_from_this +{ +public: + using Request = void *; // Serialized/Deserialized data pointer of request message + using Response = void *; // Serialized/Deserialized data pointer of response message + using SharedRequest = std::shared_ptr; + using SharedResponse = std::shared_ptr; + using CallbackType = std::function; + + using CallbackWithHeaderType = + std::function, + const SharedRequest, + SharedResponse)>; + + RCLCPP_SMART_PTR_DEFINITIONS(GenericService) + + /// Default constructor. + /** + * The constructor for a Service is almost never called directly. + * Instead, services should be instantiated through the function + * rclcpp::create_service(). + * + * \param[in] node_handle NodeBaseInterface pointer that is used in part of the setup. + * \param[in] service_name Name of the topic to publish to. + * \param[in] service_type The name of service type, e.g. "std_srvs/srv/SetBool". + * \param[in] any_callback User defined callback to call when a client request is received. + * \param[in] service_options options for the service. + */ + RCLCPP_PUBLIC + GenericService( + std::shared_ptr node_handle, + const std::string & service_name, + const std::string & service_type, + GenericServiceCallback any_callback, + rcl_service_options_t & service_options); + + GenericService() = delete; + + RCLCPP_PUBLIC + virtual ~GenericService() {} + + /// Take the next request from the service. + /** + * \sa ServiceBase::take_type_erased_request(). + * + * \param[out] request_out The reference to a service deserialized request object + * into which the middleware will copy the taken request. + * \param[out] request_id_out The output id for the request which can be used + * to associate response with this request in the future. + * \returns true if the request was taken, otherwise false. + * \throws rclcpp::exceptions::RCLError based exceptions if the underlying + * rcl calls fail. + */ + RCLCPP_PUBLIC + bool + take_request(SharedRequest request_out, rmw_request_id_t & request_id_out); + + RCLCPP_PUBLIC + std::shared_ptr + create_request() override; + + RCLCPP_PUBLIC + std::shared_ptr + create_response(); + + RCLCPP_PUBLIC + std::shared_ptr + create_request_header() override; + + RCLCPP_PUBLIC + void + handle_request( + std::shared_ptr request_header, + std::shared_ptr request) override; + + RCLCPP_PUBLIC + void + send_response(rmw_request_id_t & req_id, SharedResponse & response); + +private: + RCLCPP_DISABLE_COPY(GenericService) + + GenericServiceCallback any_callback_; + + std::shared_ptr ts_lib_; + const rosidl_typesupport_introspection_cpp::MessageMembers * request_members_; + const rosidl_typesupport_introspection_cpp::MessageMembers * response_members_; +}; + +} // namespace rclcpp +#endif // RCLCPP__GENERIC_SERVICE_HPP_ diff --git a/rclcpp/include/rclcpp/generic_subscription.hpp b/rclcpp/include/rclcpp/generic_subscription.hpp new file mode 100644 index 0000000000..dd0e8be94d --- /dev/null +++ b/rclcpp/include/rclcpp/generic_subscription.hpp @@ -0,0 +1,172 @@ +// Copyright 2018, Bosch Software Innovations GmbH. +// Copyright 2021, Apex.AI 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 RCLCPP__GENERIC_SUBSCRIPTION_HPP_ +#define RCLCPP__GENERIC_SUBSCRIPTION_HPP_ + +#include +#include +#include + +#include "rcpputils/shared_library.hpp" + +#include "rclcpp/callback_group.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_topics_interface.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/serialized_message.hpp" +#include "rclcpp/subscription_base.hpp" +#include "rclcpp/typesupport_helpers.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ + +/// %Subscription for serialized messages whose type is not known at compile time. +/** + * Since the type is not known at compile time, this is not a template, and the dynamic library + * containing type support information has to be identified and loaded based on the type name. + * + * It does not support intra-process handling. + */ +class GenericSubscription : public rclcpp::SubscriptionBase +{ +public: + // cppcheck-suppress unknownMacro + RCLCPP_SMART_PTR_DEFINITIONS(GenericSubscription) + + /// Constructor. + /** + * In order to properly subscribe to a topic, this subscription needs to be added to + * the node_topic_interface of the node passed into this constructor. + * + * \sa rclcpp::Node::create_generic_subscription() or rclcpp::create_generic_subscription() for + * creating an instance of this class and adding it to the node_topic_interface. + * + * \param node_base Pointer to parent node's NodeBaseInterface + * \param ts_lib Type support library, needs to correspond to topic_type + * \param topic_name Topic name + * \param topic_type Topic type + * \param qos %QoS settings + * \param callback Callback for new messages of serialized form + * \param options %Subscription options. + * Not all subscription options are currently respected, the only relevant options for this + * subscription are `event_callbacks`, `use_default_callbacks`, `ignore_local_publications`, and + * `%callback_group`. + */ + template> + GenericSubscription( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + const std::shared_ptr ts_lib, + const std::string & topic_name, + const std::string & topic_type, + const rclcpp::QoS & qos, + AnySubscriptionCallback callback, + const rclcpp::SubscriptionOptionsWithAllocator & options) + : SubscriptionBase( + node_base, + *rclcpp::get_message_typesupport_handle(topic_type, "rosidl_typesupport_cpp", *ts_lib), + topic_name, + options.to_rcl_subscription_options(qos), + options.event_callbacks, + options.use_default_callbacks, + DeliveredMessageKind::SERIALIZED_MESSAGE), + any_callback_(callback), + ts_lib_(ts_lib) + { + TRACETOOLS_TRACEPOINT( + rclcpp_subscription_init, + static_cast(get_subscription_handle().get()), + static_cast(this)); + TRACETOOLS_TRACEPOINT( + rclcpp_subscription_callback_added, + static_cast(this), + static_cast(&any_callback_)); + +#ifndef TRACETOOLS_DISABLED + any_callback_.register_callback_for_tracing(); +#endif + } + + RCLCPP_PUBLIC + virtual ~GenericSubscription() = default; + + // Same as create_serialized_message() as the subscription is to serialized_messages only + RCLCPP_PUBLIC + std::shared_ptr create_message() override; + + RCLCPP_PUBLIC + std::shared_ptr create_serialized_message() override; + + /// Cast the message to a rclcpp::SerializedMessage and call the callback. + RCLCPP_PUBLIC + void handle_message( + std::shared_ptr & message, const rclcpp::MessageInfo & message_info) override; + + /// Handle dispatching rclcpp::SerializedMessage to user callback. + RCLCPP_PUBLIC + void + handle_serialized_message( + const std::shared_ptr & serialized_message, + const rclcpp::MessageInfo & message_info) override; + + /// This function is currently not implemented. + RCLCPP_PUBLIC + void handle_loaned_message( + void * loaned_message, const rclcpp::MessageInfo & message_info) override; + + // Same as return_serialized_message() as the subscription is to serialized_messages only + RCLCPP_PUBLIC + void return_message(std::shared_ptr & message) override; + + RCLCPP_PUBLIC + void return_serialized_message(std::shared_ptr & message) override; + + + // DYNAMIC TYPE ================================================================================== + RCLCPP_PUBLIC + rclcpp::dynamic_typesupport::DynamicMessageType::SharedPtr get_shared_dynamic_message_type() + override; + + RCLCPP_PUBLIC + rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr get_shared_dynamic_message() override; + + RCLCPP_PUBLIC + rclcpp::dynamic_typesupport::DynamicSerializationSupport::SharedPtr + get_shared_dynamic_serialization_support() override; + + RCLCPP_PUBLIC + rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr create_dynamic_message() override; + + RCLCPP_PUBLIC + void return_dynamic_message( + rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message) override; + + RCLCPP_PUBLIC + void handle_dynamic_message( + const rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message, + const rclcpp::MessageInfo & message_info) override; + +private: + RCLCPP_DISABLE_COPY(GenericSubscription) + AnySubscriptionCallback> any_callback_; + // The type support library should stay loaded, so it is stored in the GenericSubscription + std::shared_ptr ts_lib_; +}; + +} // namespace rclcpp + +#endif // RCLCPP__GENERIC_SUBSCRIPTION_HPP_ diff --git a/rclcpp/include/rclcpp/get_message_type_support_handle.hpp b/rclcpp/include/rclcpp/get_message_type_support_handle.hpp new file mode 100644 index 0000000000..cbc2e6035b --- /dev/null +++ b/rclcpp/include/rclcpp/get_message_type_support_handle.hpp @@ -0,0 +1,98 @@ +// 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 RCLCPP__GET_MESSAGE_TYPE_SUPPORT_HANDLE_HPP_ +#define RCLCPP__GET_MESSAGE_TYPE_SUPPORT_HANDLE_HPP_ + +#include + +#include "rosidl_runtime_cpp/traits.hpp" +#include "rosidl_runtime_cpp/message_type_support_decl.hpp" +#include "rosidl_typesupport_cpp/message_type_support.hpp" + +#include "rclcpp/type_adapter.hpp" + +/// Versions of rosidl_typesupport_cpp::get_message_type_support_handle that handle adapted types. + +namespace rclcpp +{ + +#ifdef DOXYGEN_ONLY + +/// Returns the message type support for the given `MessageT` type. +/** + * \tparam MessageT an actual ROS message type or an adapted type using `rclcpp::TypeAdapter` + */ +template +constexpr const rosidl_message_type_support_t & get_message_type_support_handle(); + +#else + +template +constexpr +typename std::enable_if_t< + rosidl_generator_traits::is_message::value, + const rosidl_message_type_support_t & +> +get_message_type_support_handle() +{ + auto handle = rosidl_typesupport_cpp::get_message_type_support_handle(); + if (!handle) { + throw std::runtime_error("Type support handle unexpectedly nullptr"); + } + return *handle; +} + +template +constexpr +typename std::enable_if_t< + !rosidl_generator_traits::is_message::value && + rclcpp::TypeAdapter::is_specialized::value, + const rosidl_message_type_support_t & +> +get_message_type_support_handle() +{ + auto handle = rosidl_typesupport_cpp::get_message_type_support_handle< + typename TypeAdapter::ros_message_type + >(); + if (!handle) { + throw std::runtime_error("Type support handle unexpectedly nullptr"); + } + return *handle; +} + +// This specialization is a pass through runtime check, which allows a better +// static_assert to catch this issue further down the line. +// This should never get to be called in practice, and is purely defensive. +template< + typename AdaptedType +> +constexpr +typename std::enable_if_t< + !rosidl_generator_traits::is_message::value && + !TypeAdapter::is_specialized::value, + const rosidl_message_type_support_t & +> +get_message_type_support_handle() +{ + throw std::runtime_error( + "this specialization of rclcpp::get_message_type_support_handle() " + "should never be called"); +} + +#endif + +} // namespace rclcpp + +#endif // RCLCPP__GET_MESSAGE_TYPE_SUPPORT_HANDLE_HPP_ diff --git a/rclcpp/include/rclcpp/graph_listener.hpp b/rclcpp/include/rclcpp/graph_listener.hpp index 44cc37fb47..7d7f787fe8 100644 --- a/rclcpp/include/rclcpp/graph_listener.hpp +++ b/rclcpp/include/rclcpp/graph_listener.hpp @@ -23,6 +23,8 @@ #include "rcl/guard_condition.h" #include "rcl/wait.h" +#include "rclcpp/context.hpp" +#include "rclcpp/guard_condition.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/node_interfaces/node_graph_interface.hpp" #include "rclcpp/visibility_control.hpp" @@ -62,7 +64,7 @@ class GraphListener : public std::enable_shared_from_this { public: RCLCPP_PUBLIC - GraphListener(); + explicit GraphListener(const rclcpp::Context::SharedPtr & parent_context); RCLCPP_PUBLIC virtual ~GraphListener(); @@ -72,6 +74,8 @@ class GraphListener : public std::enable_shared_from_this * This function is thread-safe. * * \throws GraphListenerShutdownError if the GraphListener is shutdown + * \throws std::runtime if the parent context was destroyed + * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw. */ RCLCPP_PUBLIC virtual @@ -124,6 +128,11 @@ class GraphListener : public std::enable_shared_from_this * If start_if_not_started() was never called, this function still succeeds, * but start_if_not_started() still cannot be called after this function. * + * Note that if you override this method, but leave shutdown to be called in + * the destruction of this base class, it will not call the overridden + * version from your base class. + * So you need to ensure you call your class's shutdown() in its destructor. + * * \throws rclcpp::execptions::RCLError from rcl_guard_condition_fini() * \throws rclcpp::execptions::RCLError from rcl_wait_set_fini() * \throws std::system_error anything std::mutex::lock() throws @@ -133,6 +142,12 @@ class GraphListener : public std::enable_shared_from_this void shutdown(); + /// Nothrow version of shutdown(), logs to RCLCPP_ERROR instead. + RCLCPP_PUBLIC + virtual + void + shutdown(const std::nothrow_t &) noexcept; + /// Return true if shutdown() has been called, else false. RCLCPP_PUBLIC virtual @@ -151,9 +166,24 @@ class GraphListener : public std::enable_shared_from_this void run_loop(); + RCLCPP_PUBLIC + void + init_wait_set(); + + RCLCPP_PUBLIC + void + cleanup_wait_set(); + private: RCLCPP_DISABLE_COPY(GraphListener) + /** \internal */ + void + __shutdown(); + + std::weak_ptr weak_parent_context_; + std::shared_ptr rcl_parent_context_; + std::thread listener_thread_; bool is_started_; std::atomic_bool is_shutdown_; @@ -163,8 +193,7 @@ class GraphListener : public std::enable_shared_from_this mutable std::mutex node_graph_interfaces_mutex_; std::vector node_graph_interfaces_; - rcl_guard_condition_t interrupt_guard_condition_ = rcl_get_zero_initialized_guard_condition(); - rcl_guard_condition_t * shutdown_guard_condition_; + rclcpp::GuardCondition interrupt_guard_condition_; rcl_wait_set_t wait_set_ = rcl_get_zero_initialized_wait_set(); }; diff --git a/rclcpp/include/rclcpp/guard_condition.hpp b/rclcpp/include/rclcpp/guard_condition.hpp new file mode 100644 index 0000000000..594234657c --- /dev/null +++ b/rclcpp/include/rclcpp/guard_condition.hpp @@ -0,0 +1,138 @@ +// 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 RCLCPP__GUARD_CONDITION_HPP_ +#define RCLCPP__GUARD_CONDITION_HPP_ + +#include + +#include "rcl/guard_condition.h" + +#include "rclcpp/context.hpp" +#include "rclcpp/contexts/default_context.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ + +/// A condition that can be waited on in a single wait set and asynchronously triggered. +class GuardCondition +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(GuardCondition) + + // TODO(wjwwood): support custom allocator, maybe restrict to polymorphic allocator + /// Construct the guard condition, optionally specifying which Context to use. + /** + * \param[in] context Optional custom context to be used. + * Defaults to using the global default context singleton. + * Shared ownership of the context is held with the guard condition until + * destruction. + * \param[in] guard_condition_options Optional guard condition options to be used. + * Defaults to using the default guard condition options. + * \throws std::invalid_argument if the context is nullptr. + * \throws rclcpp::exceptions::RCLError based exceptions when underlying + * rcl functions fail. + */ + RCLCPP_PUBLIC + explicit GuardCondition( + const rclcpp::Context::SharedPtr & context = + rclcpp::contexts::get_global_default_context(), + rcl_guard_condition_options_t guard_condition_options = + rcl_guard_condition_get_default_options()); + + RCLCPP_PUBLIC + virtual + ~GuardCondition(); + + /// Return the underlying rcl guard condition structure. + RCLCPP_PUBLIC + rcl_guard_condition_t & + get_rcl_guard_condition(); + + /// Return the underlying rcl guard condition structure. + RCLCPP_PUBLIC + const rcl_guard_condition_t & + get_rcl_guard_condition() const; + + /// Signal that the condition has been met, notifying both the wait set and listeners, if any. + /** + * This function is thread-safe, and may be called concurrently with waiting + * on this guard condition in a wait set. + * + * \throws rclcpp::exceptions::RCLError based exceptions when underlying + * rcl functions fail. + */ + RCLCPP_PUBLIC + void + trigger(); + + /// Exchange the "in use by wait set" state for this guard condition. + /** + * This is used to ensure this guard condition is not used by multiple + * wait sets at the same time. + * + * \param[in] in_use_state the new state to exchange into the state, true + * indicates it is now in use by a wait set, and false is that it is no + * longer in use by a wait set. + * \returns the previous state. + */ + RCLCPP_PUBLIC + bool + exchange_in_use_by_wait_set_state(bool in_use_state); + + /// Adds the guard condition to a waitset + /** + * This function is thread-safe. + * \param[in] wait_set pointer to a wait set where to add the guard condition + */ + RCLCPP_PUBLIC + void + add_to_wait_set(rcl_wait_set_t & wait_set); + + /// Set a callback to be called whenever the guard condition is triggered. + /** + * The callback receives a size_t which is the number of times the guard condition was triggered + * since the last time this callback was called. + * Normally this is 1, but can be > 1 if the guard condition was triggered before any + * callback was set. + * + * Calling it again will clear any previously set callback. + * + * This function is thread-safe. + * + * If you want more information available in the callback, like the guard condition + * or other information, you may use a lambda with captures or std::bind. + * + * \param[in] callback functor to be called when the guard condition is triggered + */ + RCLCPP_PUBLIC + void + set_on_trigger_callback(std::function callback); + +protected: + rcl_guard_condition_t rcl_guard_condition_; + std::atomic in_use_by_wait_set_{false}; + std::recursive_mutex reentrant_mutex_; + std::function on_trigger_callback_{nullptr}; + size_t unread_count_{0}; + // the type of wait_set_ is actually rcl_wait_set_t *, but it's never + // dereferenced, only compared to, so make it void * to avoid accidental use + void * wait_set_{nullptr}; +}; + +} // namespace rclcpp + +#endif // RCLCPP__GUARD_CONDITION_HPP_ diff --git a/rclcpp/include/rclcpp/init_options.hpp b/rclcpp/include/rclcpp/init_options.hpp new file mode 100644 index 0000000000..ab1346db32 --- /dev/null +++ b/rclcpp/include/rclcpp/init_options.hpp @@ -0,0 +1,114 @@ +// 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 RCLCPP__INIT_OPTIONS_HPP_ +#define RCLCPP__INIT_OPTIONS_HPP_ + +#include +#include + +#include "rcl/init_options.h" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ + +/// Encapsulation of options for initializing rclcpp. +class InitOptions +{ +public: + /// If true, the context will be shutdown on SIGINT by the signal handler (if it was installed). + bool shutdown_on_signal = true; + + /// Constructor + /** + * It allows you to specify the allocator used within the init options. + * \param[in] allocator used allocate memory within the init options + * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw. + */ + RCLCPP_PUBLIC + explicit InitOptions(rcl_allocator_t allocator = rcl_get_default_allocator()); + + /// Constructor which is initialized by an existing init_options. + /** + * Initialized by an existing init_options. + * \param[in] init_options rcl_init_options_t to initialized + * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw. + */ + RCLCPP_PUBLIC + explicit InitOptions(const rcl_init_options_t & init_options); + + /// Copy constructor. + RCLCPP_PUBLIC + InitOptions(const InitOptions & other); + + /// Return `true` if logging should be initialized when `rclcpp::Context::init` is called. + RCLCPP_PUBLIC + bool + auto_initialize_logging() const; + + /// Set flag indicating if logging should be initialized or not. + RCLCPP_PUBLIC + InitOptions & + auto_initialize_logging(bool initialize_logging); + + /// Assignment operator. + RCLCPP_PUBLIC + InitOptions & + operator=(const InitOptions & other); + + RCLCPP_PUBLIC + virtual + ~InitOptions(); + + /// Return the rcl init options. + /** + * \return the rcl init options. + * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw. + */ + RCLCPP_PUBLIC + const rcl_init_options_t * + get_rcl_init_options() const; + + /// Retrieve default domain id and set. + RCLCPP_PUBLIC + void + use_default_domain_id(); + + /// Set the domain id. + RCLCPP_PUBLIC + void + set_domain_id(size_t domain_id); + + /// Return domain id. + RCLCPP_PUBLIC + size_t + get_domain_id() const; + +protected: + void + finalize_init_options(); + +private: + void + finalize_init_options_impl(); + + mutable std::mutex init_options_mutex_; + std::unique_ptr init_options_; + bool initialize_logging_{true}; +}; + +} // namespace rclcpp + +#endif // RCLCPP__INIT_OPTIONS_HPP_ diff --git a/rclcpp/include/rclcpp/intra_process_buffer_type.hpp b/rclcpp/include/rclcpp/intra_process_buffer_type.hpp new file mode 100644 index 0000000000..828a96c765 --- /dev/null +++ b/rclcpp/include/rclcpp/intra_process_buffer_type.hpp @@ -0,0 +1,35 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__INTRA_PROCESS_BUFFER_TYPE_HPP_ +#define RCLCPP__INTRA_PROCESS_BUFFER_TYPE_HPP_ + +namespace rclcpp +{ + +/// Used as argument in create_publisher and create_subscriber +/// when intra-process communication is enabled +enum class IntraProcessBufferType +{ + /// Set the data type used in the intra-process buffer as std::shared_ptr + SharedPtr, + /// Set the data type used in the intra-process buffer as std::unique_ptr + UniquePtr, + /// Set the data type used in the intra-process buffer as the same used in the callback + CallbackDefault +}; + +} // namespace rclcpp + +#endif // RCLCPP__INTRA_PROCESS_BUFFER_TYPE_HPP_ diff --git a/rclcpp/include/rclcpp/intra_process_manager.hpp b/rclcpp/include/rclcpp/intra_process_manager.hpp deleted file mode 100644 index 4eed5c35fc..0000000000 --- a/rclcpp/include/rclcpp/intra_process_manager.hpp +++ /dev/null @@ -1,361 +0,0 @@ -// 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 RCLCPP__INTRA_PROCESS_MANAGER_HPP_ -#define RCLCPP__INTRA_PROCESS_MANAGER_HPP_ - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "rclcpp/allocator/allocator_deleter.hpp" -#include "rclcpp/intra_process_manager_impl.hpp" -#include "rclcpp/mapped_ring_buffer.hpp" -#include "rclcpp/macros.hpp" -#include "rclcpp/publisher.hpp" -#include "rclcpp/subscription.hpp" -#include "rclcpp/visibility_control.hpp" - -namespace rclcpp -{ -namespace intra_process_manager -{ - -/// This class facilitates intra process communication between nodes. -/** - * This class is used in the creation of publishers and subscriptions. - * A singleton instance of this class is owned by a rclcpp::Context and a - * rclcpp::Node can use an associated Context to get an instance of this class. - * Nodes which do not have a common Context will not exchange intra process - * messages because they will not share access to an instance of this class. - * - * When a Node creates a publisher or subscription, it will register them - * with this class. - * The node will also hook into the publisher's publish call - * in order to do intra process related work. - * - * When a publisher is created, it advertises on the topic the user provided, - * as well as a "shadowing" topic of type rcl_interfaces/IntraProcessMessage. - * For instance, if the user specified the topic '/namespace/chatter', then the - * corresponding intra process topic might be '/namespace/chatter/_intra'. - * The publisher is also allocated an id which is unique among all publishers - * and subscriptions in this process. - * Additionally, when registered with this class a ring buffer is created and - * owned by this class as a temporary place to hold messages destined for intra - * process subscriptions. - * - * When a subscription is created, it subscribes to the topic provided by the - * user as well as to the corresponding intra process topic. - * It is also gets a unique id from the singleton instance of this class which - * is unique among publishers and subscriptions. - * - * When the user publishes a message, the message is stored by calling - * store_intra_process_message on this class. - * The instance of that message is uniquely identified by a publisher id and a - * message sequence number. - * The publisher id, message sequence pair is unique with in the process. - * At that point a list of the id's of intra process subscriptions which have - * been registered with the singleton instance of this class are stored with - * the message instance so that delivery is only made to those subscriptions. - * Then an instance of rcl_interfaces/IntraProcessMessage is published to the - * intra process topic which is specific to the topic specified by the user. - * - * When an instance of rcl_interfaces/IntraProcessMessage is received by a - * subscription, then it is handled by calling take_intra_process_message - * on a singleton of this class. - * The subscription passes a publisher id, message sequence pair which - * uniquely identifies the message instance it was suppose to receive as well - * as the subscriptions unique id. - * If the message is still being held by this class and the subscription's id - * is in the list of intended subscriptions then the message is returned. - * If either of those predicates are not satisfied then the message is not - * returned and the subscription does not call the users callback. - * - * Since the publisher builds a list of destined subscriptions on publish, and - * other requests are ignored, this class knows how many times a message - * instance should be requested. - * The final time a message is requested, the ownership is passed out of this - * class and passed to the final subscription, effectively freeing space in - * this class's internal storage. - * - * Since a topic is being used to ferry notifications about new intra process - * messages between publishers and subscriptions, it is possible for that - * notification to be lost. - * It is also possible that a subscription which was available when publish was - * called will no longer exist once the notification gets posted. - * In both cases this might result in a message instance getting requested - * fewer times than expected. - * This is why the internal storage of this class is a ring buffer. - * That way if a message is orphaned it will eventually be dropped from storage - * when a new message instance is stored and will not result in a memory leak. - * - * However, since the storage system is finite, this also means that a message - * instance might get displaced by an incoming message instance before all - * interested parties have called take_intra_process_message. - * Because of this the size of the internal storage should be carefully - * considered. - * - * /TODO(wjwwood): update to include information about handling latching. - * /TODO(wjwwood): consider thread safety of the class. - * - * This class is neither CopyConstructable nor CopyAssignable. - */ -class IntraProcessManager -{ -private: - RCLCPP_DISABLE_COPY(IntraProcessManager) - -public: - RCLCPP_SMART_PTR_DEFINITIONS(IntraProcessManager) - - RCLCPP_PUBLIC - explicit IntraProcessManager( - IntraProcessManagerImplBase::SharedPtr state = create_default_impl()); - - RCLCPP_PUBLIC - virtual ~IntraProcessManager(); - - /// Register a subscription with the manager, returns subscriptions unique id. - /** - * In addition to generating a unique intra process id for the subscription, - * this method also stores the topic name of the subscription. - * - * This method is normally called during the creation of a subscription, - * but after it creates the internal intra process rmw_subscription_t. - * - * This method will allocate memory. - * - * \param subscription the Subscription to register. - * \return an unsigned 64-bit integer which is the subscription's unique id. - */ - RCLCPP_PUBLIC - uint64_t - add_subscription(SubscriptionBase::SharedPtr subscription); - - /// Unregister a subscription using the subscription's unique id. - /** - * This method does not allocate memory. - * - * \param intra_process_subscription_id id of the subscription to remove. - */ - RCLCPP_PUBLIC - void - remove_subscription(uint64_t intra_process_subscription_id); - - /// Register a publisher with the manager, returns the publisher unique id. - /** - * In addition to generating and returning a unique id for the publisher, - * this method creates internal ring buffer storage for "in-flight" intra - * process messages which are stored when store_intra_process_message is - * called with this publisher's unique id. - * - * The buffer_size must be less than or equal to the max uint64_t value. - * If the buffer_size is 0 then a buffer size is calculated using the - * publisher's QoS settings. - * The default is to use the depth field of the publisher's QoS. - * TODO(wjwwood): Consider doing depth *= 1.2, round up, or similar. - * TODO(wjwwood): Consider what to do for keep all. - * - * This method is templated on the publisher's message type so that internal - * storage of the same type can be allocated. - * - * This method will allocate memory. - * - * \param publisher publisher to be registered with the manager. - * \param buffer_size if 0 (default) a size is calculated based on the QoS. - * \return an unsigned 64-bit integer which is the publisher's unique id. - */ - template - uint64_t - add_publisher( - typename Publisher::SharedPtr publisher, - size_t buffer_size = 0) - { - auto id = IntraProcessManager::get_next_unique_id(); - size_t size = buffer_size > 0 ? buffer_size : publisher->get_queue_size(); - auto mrb = mapped_ring_buffer::MappedRingBuffer< - MessageT, - typename Publisher::MessageAlloc - >::make_shared(size, publisher->get_allocator()); - impl_->add_publisher(id, publisher, mrb, size); - return id; - } - - /// Unregister a publisher using the publisher's unique id. - /** - * This method does not allocate memory. - * - * \param intra_process_publisher_id id of the publisher to remove. - */ - RCLCPP_PUBLIC - void - remove_publisher(uint64_t intra_process_publisher_id); - - /// Store a message in the manager, and return the message sequence number. - /** - * The given message is stored in internal storage using the given publisher - * id and the newly generated message sequence, which is also returned. - * The combination of publisher id and message sequence number can later - * be used with a subscription id to retrieve the message by calling - * take_intra_process_message. - * The number of times take_intra_process_message can be called with this - * unique pair of id's is determined by the number of subscriptions currently - * subscribed to the same topic and which share the same Context, i.e. once - * for each subscription which should receive the intra process message. - * - * The ownership of the incoming message is transfered to the internal - * storage in order to avoid copying the message data. - * Therefore, the message parameter will no longer contain the original - * message after calling this method. - * Instead it will either be a nullptr or it will contain the ownership of - * the message instance which was displaced. - * If the message parameter is not equal to nullptr after calling this method - * then a message was prematurely displaced, i.e. take_intra_process_message - * had not been called on it as many times as was expected. - * - * This method can throw an exception if the publisher id is not found or - * if the publisher shared_ptr given to add_publisher has gone out of scope. - * - * This method does allocate memory. - * - * \param intra_process_publisher_id the id of the publisher of this message. - * \param message the message that is being stored. - * \return the message sequence number. - */ - template< - typename MessageT, typename Alloc = std::allocator, - typename Deleter = std::default_delete> - uint64_t - store_intra_process_message( - uint64_t intra_process_publisher_id, - std::unique_ptr & message) - { - using MRBMessageAlloc = typename std::allocator_traits::template rebind_alloc; - using TypedMRB = typename mapped_ring_buffer::MappedRingBuffer; - uint64_t message_seq = 0; - mapped_ring_buffer::MappedRingBufferBase::SharedPtr buffer = impl_->get_publisher_info_for_id( - intra_process_publisher_id, message_seq); - typename TypedMRB::SharedPtr typed_buffer = std::static_pointer_cast(buffer); - if (!typed_buffer) { - throw std::runtime_error("Typecast failed due to incorrect message type"); - } - - // Insert the message into the ring buffer using the message_seq to identify it. - bool did_replace = typed_buffer->push_and_replace(message_seq, message); - // TODO(wjwwood): do something when a message was displaced. log debug? - (void)did_replace; // Avoid unused variable warning. - - impl_->store_intra_process_message(intra_process_publisher_id, message_seq); - - // Return the message sequence which is sent to the subscription. - return message_seq; - } - - /// Take an intra process message. - /** - * The intra_process_publisher_id and message_sequence_number parameters - * uniquely identify a message instance, which should be taken. - * - * The requesting_subscriptions_intra_process_id parameter is used to make - * sure the requesting subscription was intended to receive this message - * instance. - * This check is made because it could happen that the requester - * comes up after the publish event, so it still receives the notification of - * a new intra process message, but it wasn't registered with the manager at - * the time of publishing, causing it to take when it wasn't intended. - * This should be avioded unless latching-like behavior is involved. - * - * The message parameter is used to store the taken message. - * On the last expected call to this method, the ownership is transfered out - * of internal storage and into the message parameter. - * On all previous calls a copy of the internally stored message is made and - * the ownership of the copy is transfered to the message parameter. - * TODO(wjwwood): update this documentation when latching is supported. - * - * The message parameter can be set to nullptr if: - * - * - The publisher id is not found. - * - The message sequence is not found for the given publisher id. - * - The requesting subscription's id is not in the list of intended takers. - * - The requesting subscription's id has been used before with this message. - * - * This method may allocate memory to copy the stored message. - * - * \param intra_process_publisher_id the id of the message's publisher. - * \param message_sequence_number the sequence number of the message. - * \param requesting_subscriptions_intra_process_id the subscription's id. - * \param message the message typed unique_ptr used to return the message. - */ - template< - typename MessageT, typename Alloc = std::allocator, - typename Deleter = std::default_delete> - void - take_intra_process_message( - uint64_t intra_process_publisher_id, - uint64_t message_sequence_number, - uint64_t requesting_subscriptions_intra_process_id, - std::unique_ptr & message) - { - using MRBMessageAlloc = typename std::allocator_traits::template rebind_alloc; - using TypedMRB = mapped_ring_buffer::MappedRingBuffer; - message = nullptr; - - size_t target_subs_size = 0; - std::lock_guard lock(take_mutex_); - mapped_ring_buffer::MappedRingBufferBase::SharedPtr buffer = impl_->take_intra_process_message( - intra_process_publisher_id, - message_sequence_number, - requesting_subscriptions_intra_process_id, - target_subs_size - ); - typename TypedMRB::SharedPtr typed_buffer = std::static_pointer_cast(buffer); - if (!typed_buffer) { - return; - } - // Return a copy or the unique_ptr (ownership) depending on how many subscriptions are left. - if (target_subs_size) { - // There are more subscriptions to serve, return a copy. - typed_buffer->get_copy_at_key(message_sequence_number, message); - } else { - // This is the last one to be returned, transfer ownership. - typed_buffer->pop_at_key(message_sequence_number, message); - } - } - - /// Return true if the given rmw_gid_t matches any stored Publishers. - RCLCPP_PUBLIC - bool - matches_any_publishers(const rmw_gid_t * id) const; - -private: - RCLCPP_PUBLIC - static uint64_t - get_next_unique_id(); - - IntraProcessManagerImplBase::SharedPtr impl_; - std::mutex take_mutex_; -}; - -} // namespace intra_process_manager -} // namespace rclcpp - -#endif // RCLCPP__INTRA_PROCESS_MANAGER_HPP_ diff --git a/rclcpp/include/rclcpp/intra_process_manager_impl.hpp b/rclcpp/include/rclcpp/intra_process_manager_impl.hpp deleted file mode 100644 index 553c44ec1d..0000000000 --- a/rclcpp/include/rclcpp/intra_process_manager_impl.hpp +++ /dev/null @@ -1,317 +0,0 @@ -// 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 RCLCPP__INTRA_PROCESS_MANAGER_IMPL_HPP_ -#define RCLCPP__INTRA_PROCESS_MANAGER_IMPL_HPP_ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "rclcpp/macros.hpp" -#include "rclcpp/mapped_ring_buffer.hpp" -#include "rclcpp/publisher.hpp" -#include "rclcpp/subscription.hpp" -#include "rclcpp/visibility_control.hpp" - -namespace rclcpp -{ -namespace intra_process_manager -{ - -class IntraProcessManagerImplBase -{ -public: - RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(IntraProcessManagerImplBase) - - IntraProcessManagerImplBase() = default; - ~IntraProcessManagerImplBase() = default; - - virtual void - add_subscription(uint64_t id, SubscriptionBase::SharedPtr subscription) = 0; - - virtual void - remove_subscription(uint64_t intra_process_subscription_id) = 0; - - virtual void add_publisher( - uint64_t id, - PublisherBase::WeakPtr publisher, - mapped_ring_buffer::MappedRingBufferBase::SharedPtr mrb, - size_t size) = 0; - - virtual void - remove_publisher(uint64_t intra_process_publisher_id) = 0; - - virtual mapped_ring_buffer::MappedRingBufferBase::SharedPtr - get_publisher_info_for_id( - uint64_t intra_process_publisher_id, - uint64_t & message_seq) = 0; - - virtual void - store_intra_process_message(uint64_t intra_process_publisher_id, uint64_t message_seq) = 0; - - virtual mapped_ring_buffer::MappedRingBufferBase::SharedPtr - take_intra_process_message( - uint64_t intra_process_publisher_id, - uint64_t message_sequence_number, - uint64_t requesting_subscriptions_intra_process_id, - size_t & size) = 0; - - virtual bool - matches_any_publishers(const rmw_gid_t * id) const = 0; - -private: - RCLCPP_DISABLE_COPY(IntraProcessManagerImplBase) -}; - -template> -class IntraProcessManagerImpl : public IntraProcessManagerImplBase -{ -public: - IntraProcessManagerImpl() = default; - ~IntraProcessManagerImpl() = default; - - void - add_subscription(uint64_t id, SubscriptionBase::SharedPtr subscription) - { - subscriptions_[id] = subscription; - // subscription->get_topic_name() -> const char * can be used as the key, - // since subscriptions_ shares the ownership of subscription - subscription_ids_by_topic_[subscription->get_topic_name()].insert(id); - } - - void - remove_subscription(uint64_t intra_process_subscription_id) - { - subscriptions_.erase(intra_process_subscription_id); - for (auto & pair : subscription_ids_by_topic_) { - pair.second.erase(intra_process_subscription_id); - } - // Iterate over all publisher infos and all stored subscription id's and - // remove references to this subscription's id. - for (auto & publisher_pair : publishers_) { - for (auto & sub_pair : publisher_pair.second.target_subscriptions_by_message_sequence) { - sub_pair.second.erase(intra_process_subscription_id); - } - } - } - - void add_publisher( - uint64_t id, - PublisherBase::WeakPtr publisher, - mapped_ring_buffer::MappedRingBufferBase::SharedPtr mrb, - size_t size) - { - publishers_[id].publisher = publisher; - // As long as the size of the ring buffer is less than the max sequence number, we're safe. - if (size > std::numeric_limits::max()) { - throw std::invalid_argument("the calculated buffer size is too large"); - } - publishers_[id].sequence_number.store(0); - - publishers_[id].buffer = mrb; - publishers_[id].target_subscriptions_by_message_sequence.reserve(size); - } - - void - remove_publisher(uint64_t intra_process_publisher_id) - { - publishers_.erase(intra_process_publisher_id); - } - - // return message_seq and mrb - mapped_ring_buffer::MappedRingBufferBase::SharedPtr - get_publisher_info_for_id( - uint64_t intra_process_publisher_id, - uint64_t & message_seq) - { - std::lock_guard lock(runtime_mutex_); - auto it = publishers_.find(intra_process_publisher_id); - if (it == publishers_.end()) { - throw std::runtime_error("get_publisher_info_for_id called with invalid publisher id"); - } - PublisherInfo & info = it->second; - // Calculate the next message sequence number. - message_seq = info.sequence_number.fetch_add(1); - - return info.buffer; - } - - void - store_intra_process_message(uint64_t intra_process_publisher_id, uint64_t message_seq) - { - std::lock_guard lock(runtime_mutex_); - auto it = publishers_.find(intra_process_publisher_id); - if (it == publishers_.end()) { - throw std::runtime_error("store_intra_process_message called with invalid publisher id"); - } - PublisherInfo & info = it->second; - auto publisher = info.publisher.lock(); - if (!publisher) { - throw std::runtime_error("publisher has unexpectedly gone out of scope"); - } - - // Figure out what subscriptions should receive the message. - auto & destined_subscriptions = subscription_ids_by_topic_[publisher->get_topic_name()]; - // Store the list for later comparison. - if (info.target_subscriptions_by_message_sequence.count(message_seq) == 0) { - info.target_subscriptions_by_message_sequence.emplace( - message_seq, AllocSet(std::less(), uint64_allocator)); - } else { - info.target_subscriptions_by_message_sequence[message_seq].clear(); - } - std::copy( - destined_subscriptions.begin(), destined_subscriptions.end(), - // Memory allocation occurs in info.target_subscriptions_by_message_sequence[message_seq] - std::inserter( - info.target_subscriptions_by_message_sequence[message_seq], - // This ends up only being a hint to std::set, could also be .begin(). - info.target_subscriptions_by_message_sequence[message_seq].end() - ) - ); - } - - mapped_ring_buffer::MappedRingBufferBase::SharedPtr - take_intra_process_message( - uint64_t intra_process_publisher_id, - uint64_t message_sequence_number, - uint64_t requesting_subscriptions_intra_process_id, - size_t & size - ) - { - std::lock_guard lock(runtime_mutex_); - PublisherInfo * info; - { - auto it = publishers_.find(intra_process_publisher_id); - if (it == publishers_.end()) { - // Publisher is either invalid or no longer exists. - return 0; - } - info = &it->second; - } - // Figure out how many subscriptions are left. - AllocSet * target_subs; - { - auto it = info->target_subscriptions_by_message_sequence.find(message_sequence_number); - if (it == info->target_subscriptions_by_message_sequence.end()) { - // Message is no longer being stored by this publisher. - return 0; - } - target_subs = &it->second; - } - { - auto it = std::find( - target_subs->begin(), target_subs->end(), - requesting_subscriptions_intra_process_id); - if (it == target_subs->end()) { - // This publisher id/message seq pair was not intended for this subscription. - return 0; - } - target_subs->erase(it); - } - size = target_subs->size(); - return info->buffer; - } - - bool - matches_any_publishers(const rmw_gid_t * id) const - { - for (auto & publisher_pair : publishers_) { - auto publisher = publisher_pair.second.publisher.lock(); - if (!publisher) { - continue; - } - if (*publisher.get() == id) { - return true; - } - } - return false; - } - -private: - RCLCPP_DISABLE_COPY(IntraProcessManagerImpl) - - template - using RebindAlloc = typename std::allocator_traits::template rebind_alloc; - - RebindAlloc uint64_allocator; - - using AllocSet = std::set, RebindAlloc>; - using SubscriptionMap = std::unordered_map< - uint64_t, SubscriptionBase::WeakPtr, - std::hash, std::equal_to, - RebindAlloc>>; - - struct strcmp_wrapper : public std::binary_function - { - bool - operator()(const char * lhs, const char * rhs) const - { - return std::strcmp(lhs, rhs) < 0; - } - }; - using IDTopicMap = std::map< - const char *, - AllocSet, - strcmp_wrapper, - RebindAlloc>>; - - SubscriptionMap subscriptions_; - - IDTopicMap subscription_ids_by_topic_; - - struct PublisherInfo - { - RCLCPP_DISABLE_COPY(PublisherInfo) - - PublisherInfo() = default; - - PublisherBase::WeakPtr publisher; - std::atomic sequence_number; - mapped_ring_buffer::MappedRingBufferBase::SharedPtr buffer; - - using TargetSubscriptionsMap = std::unordered_map< - uint64_t, AllocSet, - std::hash, std::equal_to, - RebindAlloc>>; - TargetSubscriptionsMap target_subscriptions_by_message_sequence; - }; - - using PublisherMap = std::unordered_map< - uint64_t, PublisherInfo, - std::hash, std::equal_to, - RebindAlloc>>; - - PublisherMap publishers_; - - std::mutex runtime_mutex_; -}; - -RCLCPP_PUBLIC -IntraProcessManagerImplBase::SharedPtr -create_default_impl(); - -} // namespace intra_process_manager -} // namespace rclcpp - -#endif // RCLCPP__INTRA_PROCESS_MANAGER_IMPL_HPP_ diff --git a/rclcpp/include/rclcpp/intra_process_setting.hpp b/rclcpp/include/rclcpp/intra_process_setting.hpp new file mode 100644 index 0000000000..8e4b44eb64 --- /dev/null +++ b/rclcpp/include/rclcpp/intra_process_setting.hpp @@ -0,0 +1,34 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__INTRA_PROCESS_SETTING_HPP_ +#define RCLCPP__INTRA_PROCESS_SETTING_HPP_ + +namespace rclcpp +{ + +/// Used as argument in create_publisher and create_subscriber. +enum class IntraProcessSetting +{ + /// Explicitly enable intraprocess comm at publisher/subscription level. + Enable, + /// Explicitly disable intraprocess comm at publisher/subscription level. + Disable, + /// Take intraprocess configuration from the node. + NodeDefault +}; + +} // namespace rclcpp + +#endif // RCLCPP__INTRA_PROCESS_SETTING_HPP_ diff --git a/rclcpp/include/rclcpp/is_ros_compatible_type.hpp b/rclcpp/include/rclcpp/is_ros_compatible_type.hpp new file mode 100644 index 0000000000..97c6ad6a27 --- /dev/null +++ b/rclcpp/include/rclcpp/is_ros_compatible_type.hpp @@ -0,0 +1,35 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__IS_ROS_COMPATIBLE_TYPE_HPP_ +#define RCLCPP__IS_ROS_COMPATIBLE_TYPE_HPP_ + +#include "rosidl_runtime_cpp/traits.hpp" + +#include "rclcpp/type_adapter.hpp" + +namespace rclcpp +{ + +template +struct is_ros_compatible_type +{ + static constexpr bool value = + rosidl_generator_traits::is_message::value || + rclcpp::TypeAdapter::is_specialized::value; +}; + +} // namespace rclcpp + +#endif // RCLCPP__IS_ROS_COMPATIBLE_TYPE_HPP_ diff --git a/rclcpp/include/rclcpp/loaned_message.hpp b/rclcpp/include/rclcpp/loaned_message.hpp new file mode 100644 index 0000000000..34a1633ef4 --- /dev/null +++ b/rclcpp/include/rclcpp/loaned_message.hpp @@ -0,0 +1,199 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__LOANED_MESSAGE_HPP_ +#define RCLCPP__LOANED_MESSAGE_HPP_ + +#include +#include + +#include "rclcpp/allocator/allocator_common.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp/publisher_base.hpp" + +#include "rcl/allocator.h" +#include "rcl/publisher.h" + +namespace rclcpp +{ + +template> +class LoanedMessage +{ +public: + using MessageAllocatorTraits = rclcpp::allocator::AllocRebind; + using MessageAllocator = typename MessageAllocatorTraits::allocator_type; + + /// Constructor of the LoanedMessage class. + /** + * The constructor of this class allocates memory for a given message type + * and associates this with a given publisher. + * + * The underlying middleware is queried to determine whether it is able to allocate the + * appropriate memory for this message type or not. + * In the case that the middleware cannot loan messages, the passed in allocator instance + * is used to allocate the message within the scope of this class. + * Otherwise, the allocator is ignored and the allocation is solely performed + * in the underlying middleware with its appropriate allocation strategy. + * The need for this arises as the user code can be written explicitly targeting a middleware + * capable of loaning messages. + * However, this user code is ought to be usable even when dynamically linked against + * a middleware which doesn't support message loaning in which case the allocator will be used. + * + * \param[in] pub rclcpp::Publisher instance to which the memory belongs + * \param[in] allocator Allocator instance in case middleware cannot allocate messages + * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw. + */ + LoanedMessage( + const rclcpp::PublisherBase & pub, + MessageAllocator allocator) + : pub_(pub), + message_(nullptr), + message_allocator_(std::move(allocator)) + { + if (pub_.can_loan_messages()) { + void * message_ptr = nullptr; + auto ret = rcl_borrow_loaned_message( + pub_.get_publisher_handle().get(), + rosidl_typesupport_cpp::get_message_type_support_handle(), + &message_ptr); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + message_ = static_cast(message_ptr); + } else { + RCLCPP_INFO_ONCE( + rclcpp::get_logger("rclcpp"), + "Currently used middleware can't loan messages. Local allocator will be used."); + message_ = message_allocator_.allocate(1); + new (message_) MessageT(); + } + } + + /// Move semantic for RVO + LoanedMessage(LoanedMessage && other) + : pub_(std::move(other.pub_)), + message_(std::move(other.message_)), + message_allocator_(std::move(other.message_allocator_)) + { + other.message_ = nullptr; + } + + /// Destructor of the LoanedMessage class. + /** + * The destructor has the explicit task to return the allocated memory for its message + * instance. + * If the message was previously allocated via the middleware, the message is getting + * returned to the middleware to cleanly destroy the allocation. + * In the case that the local allocator instance was used, the same instance is then + * being used to destroy the allocated memory. + * + * The contract here is that the memory for this message is valid as long as this instance + * of the LoanedMessage class is alive. + */ + virtual ~LoanedMessage() + { + auto error_logger = rclcpp::get_logger("LoanedMessage"); + + if (message_ == nullptr) { + return; + } + + if (pub_.can_loan_messages()) { + // return allocated memory to the middleware + auto ret = + rcl_return_loaned_message_from_publisher(pub_.get_publisher_handle().get(), message_); + if (ret != RCL_RET_OK) { + RCLCPP_ERROR( + error_logger, "rcl_deallocate_loaned_message failed: %s", rcl_get_error_string().str); + rcl_reset_error(); + } + } else { + // call destructor before deallocating + message_->~MessageT(); + message_allocator_.deallocate(message_, 1); + } + message_ = nullptr; + } + + /// Validate if the message was correctly allocated. + /** + * The allocated memory might not be always consistent and valid. + * Reasons why this could fail is that an allocation step was failing, + * e.g. just like malloc could fail or a maximum amount of previously allocated + * messages is exceeded in which case the loaned messages have to be returned + * to the middleware prior to be able to allocate a new one. + */ + bool is_valid() const + { + return message_ != nullptr; + } + + /// Access the ROS message instance. + /** + * A call to `get()` will return a mutable reference to the underlying ROS message instance. + * This allows a user to modify the content of the message prior to publishing it. + * + * If this reference is copied, the memory for this copy is no longer managed + * by the LoanedMessage instance and has to be cleanup individually. + */ + MessageT & get() const + { + return *message_; + } + + /// Release ownership of the ROS message instance. + /** + * A call to `release()` will unmanage the memory for the ROS message. + * That means that the destructor of this class will not free the memory on scope exit. + * If the message is loaned from the middleware but not be published, the user needs to call + * `rcl_return_loaned_message_from_publisher` manually. + * If the memory is from the local allocator, the memory is freed when the unique pointer + * goes out instead. + * + * \return std::unique_ptr to the message instance. + */ + std::unique_ptr> + release() + { + auto msg = message_; + message_ = nullptr; + + if (pub_.can_loan_messages()) { + return std::unique_ptr>(msg, [](MessageT *) {}); + } + + return std::unique_ptr>( + msg, + [allocator = message_allocator_](MessageT * msg_ptr) mutable { + // call destructor before deallocating + msg_ptr->~MessageT(); + allocator.deallocate(msg_ptr, 1); + }); + } + +protected: + const rclcpp::PublisherBase & pub_; + + MessageT * message_; + + MessageAllocator message_allocator_; + + /// Deleted copy constructor to preserve memory integrity. + LoanedMessage(const LoanedMessage & other) = delete; +}; + +} // namespace rclcpp + +#endif // RCLCPP__LOANED_MESSAGE_HPP_ diff --git a/rclcpp/include/rclcpp/logger.hpp b/rclcpp/include/rclcpp/logger.hpp index e9b110baaf..ee244fd988 100644 --- a/rclcpp/include/rclcpp/logger.hpp +++ b/rclcpp/include/rclcpp/logger.hpp @@ -15,11 +15,17 @@ #ifndef RCLCPP__LOGGER_HPP_ #define RCLCPP__LOGGER_HPP_ +#include #include #include +#include #include "rclcpp/visibility_control.hpp" +#include "rcl/node.h" +#include "rcutils/logging.h" +#include "rcpputils/filesystem_helper.hpp" + /** * \def RCLCPP_LOGGING_ENABLED * When this define evaluates to true (default), logger factory functions will @@ -60,8 +66,72 @@ RCLCPP_PUBLIC Logger get_logger(const std::string & name); +/// Return a named logger using an rcl_node_t. +/** + * This is a convenience function that does error checking and returns the node + * logger name, or "rclcpp" if it is unable to get the node name. + * + * \param[in] node the rcl node from which to get the logger name + * \return a logger based on the node name, or "rclcpp" if there's an error + */ +RCLCPP_PUBLIC +Logger +get_node_logger(const rcl_node_t * node); + +// TODO(ahcorde): Remove deprecated class on the next release (in Rolling after Kilted). +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif +/// Get the current logging directory. +/** + * For more details of how the logging directory is determined, + * see rcl_logging_get_logging_directory(). + * + * \returns the logging directory being used. + * \throws rclcpp::exceptions::RCLError if an unexpected error occurs. + */ +[[deprecated("use rclcpp::get_log_directory instead of rclcpp::get_logging_directory")]] +RCLCPP_PUBLIC +rcpputils::fs::path +get_logging_directory(); + +// remove warning suppression +#if !defined(_WIN32) +# pragma GCC diagnostic pop +#else // !defined(_WIN32) +# pragma warning(pop) +#endif + +/// Get the current logging directory. +/** + * For more details of how the logging directory is determined, + * see rcl_logging_get_logging_directory(). + * + * \returns the logging directory being used. + * \throws rclcpp::exceptions::RCLError if an unexpected error occurs. + */ +RCLCPP_PUBLIC +std::filesystem::path +get_log_directory(); + class Logger { +public: + /// An enum for the type of logger level. + enum class Level + { + Unset = RCUTILS_LOG_SEVERITY_UNSET, ///< The unset log level + Debug = RCUTILS_LOG_SEVERITY_DEBUG, ///< The debug log level + Info = RCUTILS_LOG_SEVERITY_INFO, ///< The info log level + Warn = RCUTILS_LOG_SEVERITY_WARN, ///< The warn log level + Error = RCUTILS_LOG_SEVERITY_ERROR, ///< The error log level + Fatal = RCUTILS_LOG_SEVERITY_FATAL, ///< The fatal log level + }; + private: friend Logger rclcpp::get_logger(const std::string & name); friend ::rclcpp::node_interfaces::NodeLogging; @@ -82,11 +152,9 @@ class Logger : name_(new std::string(name)) {} std::shared_ptr name_; + std::shared_ptr> logger_sublogger_pairname_ = nullptr; public: - RCLCPP_PUBLIC - Logger(const Logger &) = default; - /// Get the name of this logger. /** * \return the full name of the logger including any prefixes, or @@ -117,13 +185,35 @@ class Logger */ RCLCPP_PUBLIC Logger - get_child(const std::string & suffix) - { - if (!name_) { - return Logger(); - } - return Logger(*name_ + "." + suffix); - } + get_child(const std::string & suffix); + + /// Set level for current logger. + /** + * \param[in] level the logger's level + * \throws rclcpp::exceptions::RCLInvalidArgument if level is invalid. + * \throws rclcpp::exceptions::RCLError if other error happens. + */ + RCLCPP_PUBLIC + void + set_level(Level level); + + /// Get effective level for current logger. + /** + * The effective level is determined as the severity level of + * the logger if it is set, otherwise it is the first specified severity + * level of the logger's ancestors, starting with its closest ancestor. + * The ancestor hierarchy is signified by logger names being separated by dots: + * a logger named `x` is an ancestor of `x.y`, and both `x` and `x.y` are + * ancestors of `x.y.z`, etc. + * If the level has not been set for the logger nor any of its + * ancestors, the default level is used. + * + * \throws rclcpp::exceptions::RCLError if any error happens. + * \return Level for the current logger. + */ + RCLCPP_PUBLIC + Level + get_effective_level() const; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/macros.hpp b/rclcpp/include/rclcpp/macros.hpp index f224abe60a..d13c5d21b9 100644 --- a/rclcpp/include/rclcpp/macros.hpp +++ b/rclcpp/include/rclcpp/macros.hpp @@ -66,6 +66,7 @@ #define RCLCPP_SMART_PTR_ALIASES_ONLY(...) \ __RCLCPP_SHARED_PTR_ALIAS(__VA_ARGS__) \ __RCLCPP_WEAK_PTR_ALIAS(__VA_ARGS__) \ + __RCLCPP_UNIQUE_PTR_ALIAS(__VA_ARGS__) \ __RCLCPP_MAKE_SHARED_DEFINITION(__VA_ARGS__) #define __RCLCPP_SHARED_PTR_ALIAS(...) \ diff --git a/rclcpp/include/rclcpp/mapped_ring_buffer.hpp b/rclcpp/include/rclcpp/mapped_ring_buffer.hpp deleted file mode 100644 index 69ce8b9264..0000000000 --- a/rclcpp/include/rclcpp/mapped_ring_buffer.hpp +++ /dev/null @@ -1,248 +0,0 @@ -// 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 RCLCPP__MAPPED_RING_BUFFER_HPP_ -#define RCLCPP__MAPPED_RING_BUFFER_HPP_ - -#include -#include -#include -#include -#include -#include -#include - -#include "rclcpp/allocator/allocator_common.hpp" -#include "rclcpp/macros.hpp" -#include "rclcpp/visibility_control.hpp" - -namespace rclcpp -{ -namespace mapped_ring_buffer -{ - -class RCLCPP_PUBLIC MappedRingBufferBase -{ -public: - RCLCPP_SMART_PTR_DEFINITIONS(MappedRingBufferBase) -}; - -/// Ring buffer container of unique_ptr's of T, which can be accessed by a key. -/** - * T must be a CopyConstructable and CopyAssignable. - * This class can be used in a container by using the base class MappedRingBufferBase. - * This class must have a positive, non-zero size. - * This class cannot be resized nor can it reserve additional space after construction. - * This class is not CopyConstructable nor CopyAssignable. - * - * The key's are not guaranteed to be unique because push_and_replace does not - * check for colliding keys. - * It is up to the user to only use unique keys. - * A side effect of this is that when get_copy_at_key or pop_at_key are called, - * they return the first encountered instance of the key. - * But iteration does not begin with the ring buffer's head, and therefore - * there is no guarantee on which value is returned if a key is used multiple - * times. - */ -template> -class MappedRingBuffer : public MappedRingBufferBase -{ -public: - RCLCPP_SMART_PTR_DEFINITIONS(MappedRingBuffer) - using ElemAllocTraits = allocator::AllocRebind; - using ElemAlloc = typename ElemAllocTraits::allocator_type; - using ElemDeleter = allocator::Deleter; - - using ElemUniquePtr = std::unique_ptr; - - /// Constructor. - /** - * The constructor will allocate memory while reserving space. - * - * \param size size of the ring buffer; must be positive and non-zero. - * \param allocator optional custom allocator - */ - explicit MappedRingBuffer(size_t size, std::shared_ptr allocator = nullptr) - : elements_(size), head_(0) - { - if (size == 0) { - throw std::invalid_argument("size must be a positive, non-zero value"); - } - if (!allocator) { - allocator_ = std::make_shared(); - } else { - allocator_ = std::make_shared(*allocator.get()); - } - } - - virtual ~MappedRingBuffer() {} - - /// Return a copy of the value stored in the ring buffer at the given key. - /** - * The key is matched if an element in the ring buffer has a matching key. - * This method will allocate in order to return a copy. - * - * The key is not guaranteed to be unique, see the class docs for more. - * - * The contents of value before the method is called are discarded. - * - * \param key the key associated with the stored value - * \param value if the key is found, the value is stored in this parameter - */ - void - get_copy_at_key(uint64_t key, ElemUniquePtr & value) - { - std::lock_guard lock(data_mutex_); - auto it = get_iterator_of_key(key); - value = nullptr; - if (it != elements_.end() && it->in_use) { - auto ptr = ElemAllocTraits::allocate(*allocator_.get(), 1); - ElemAllocTraits::construct(*allocator_.get(), ptr, *it->value); - value = ElemUniquePtr(ptr); - } - } - - /// Return ownership of the value stored in the ring buffer, leaving a copy. - /** - * The key is matched if an element in the ring bufer has a matching key. - * This method will allocate in order to store a copy. - * - * The key is not guaranteed to be unique, see the class docs for more. - * - * The ownership of the currently stored object is returned, but a copy is - * made and stored in its place. - * This means that multiple calls to this function for a particular element - * will result in returning the copied and stored object not the original. - * This also means that later calls to pop_at_key will not return the - * originally stored object, since it was returned by the first call to this - * method. - * - * The contents of value before the method is called are discarded. - * - * \param key the key associated with the stored value - * \param value if the key is found, the value is stored in this parameter - */ - void - get_ownership_at_key(uint64_t key, ElemUniquePtr & value) - { - std::lock_guard lock(data_mutex_); - auto it = get_iterator_of_key(key); - value = nullptr; - if (it != elements_.end() && it->in_use) { - // Make a copy. - auto ptr = ElemAllocTraits::allocate(*allocator_.get(), 1); - ElemAllocTraits::construct(*allocator_.get(), ptr, *it->value); - auto copy = ElemUniquePtr(ptr); - // Return the original. - value.swap(it->value); - // Store the copy. - it->value.swap(copy); - } - } - - /// Return ownership of the value stored in the ring buffer at the given key. - /** - * The key is matched if an element in the ring buffer has a matching key. - * - * The key is not guaranteed to be unique, see the class docs for more. - * - * The contents of value before the method is called are discarded. - * - * \param key the key associated with the stored value - * \param value if the key is found, the value is stored in this parameter - */ - void - pop_at_key(uint64_t key, ElemUniquePtr & value) - { - std::lock_guard lock(data_mutex_); - auto it = get_iterator_of_key(key); - value = nullptr; - if (it != elements_.end() && it->in_use) { - value.swap(it->value); - it->in_use = false; - } - } - - /// Insert a key-value pair, displacing an existing pair if necessary. - /** - * The key's uniqueness is not checked on insertion. - * It is up to the user to ensure the key is unique. - * This method should not allocate memory. - * - * After insertion, if a pair was replaced, then value will contain ownership - * of that displaced value. Otherwise it will be a nullptr. - * - * \param key the key associated with the value to be stored - * \param value the value to store, and optionally the value displaced - */ - bool - push_and_replace(uint64_t key, ElemUniquePtr & value) - { - std::lock_guard lock(data_mutex_); - bool did_replace = elements_[head_].in_use; - elements_[head_].key = key; - elements_[head_].value.swap(value); - elements_[head_].in_use = true; - head_ = (head_ + 1) % elements_.size(); - return did_replace; - } - - bool - push_and_replace(uint64_t key, ElemUniquePtr && value) - { - ElemUniquePtr temp = std::move(value); - return push_and_replace(key, temp); - } - - /// Return true if the key is found in the ring buffer, otherwise false. - bool - has_key(uint64_t key) - { - std::lock_guard lock(data_mutex_); - return elements_.end() != get_iterator_of_key(key); - } - -private: - RCLCPP_DISABLE_COPY(MappedRingBuffer) - - struct element - { - uint64_t key; - ElemUniquePtr value; - bool in_use; - }; - - using VectorAlloc = typename std::allocator_traits::template rebind_alloc; - - typename std::vector::iterator - get_iterator_of_key(uint64_t key) - { - auto it = std::find_if( - elements_.begin(), elements_.end(), - [key](element & e) -> bool { - return e.key == key && e.in_use; - }); - return it; - } - - std::vector elements_; - size_t head_; - std::shared_ptr allocator_; - std::mutex data_mutex_; -}; - -} // namespace mapped_ring_buffer -} // namespace rclcpp - -#endif // RCLCPP__MAPPED_RING_BUFFER_HPP_ diff --git a/rclcpp/include/rclcpp/memory_strategies.hpp b/rclcpp/include/rclcpp/memory_strategies.hpp index a3950d9bd0..12730fc6f6 100644 --- a/rclcpp/include/rclcpp/memory_strategies.hpp +++ b/rclcpp/include/rclcpp/memory_strategies.hpp @@ -23,6 +23,10 @@ namespace rclcpp namespace memory_strategies { +/// Create a MemoryStrategy sharedPtr +/** + * \return a MemoryStrategy sharedPtr + */ RCLCPP_PUBLIC memory_strategy::MemoryStrategy::SharedPtr create_default_strategy(); diff --git a/rclcpp/include/rclcpp/memory_strategy.hpp b/rclcpp/include/rclcpp/memory_strategy.hpp index 0421555c6c..fb5ba2a63f 100644 --- a/rclcpp/include/rclcpp/memory_strategy.hpp +++ b/rclcpp/include/rclcpp/memory_strategy.hpp @@ -15,8 +15,9 @@ #ifndef RCLCPP__MEMORY_STRATEGY_HPP_ #define RCLCPP__MEMORY_STRATEGY_HPP_ +#include +#include #include -#include #include "rcl/allocator.h" #include "rcl/wait.h" @@ -25,6 +26,7 @@ #include "rclcpp/macros.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/visibility_control.hpp" +#include "rclcpp/waitable.hpp" namespace rclcpp { @@ -41,76 +43,110 @@ class RCLCPP_PUBLIC MemoryStrategy { public: RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(MemoryStrategy) - using WeakNodeVector = std::vector; + using WeakCallbackGroupsToNodesMap = std::map>; - virtual bool collect_entities(const WeakNodeVector & weak_nodes) = 0; + virtual ~MemoryStrategy() = default; + + virtual bool collect_entities(const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) = 0; virtual size_t number_of_ready_subscriptions() const = 0; virtual size_t number_of_ready_services() const = 0; virtual size_t number_of_ready_clients() const = 0; + virtual size_t number_of_ready_events() const = 0; virtual size_t number_of_ready_timers() const = 0; virtual size_t number_of_guard_conditions() const = 0; + virtual size_t number_of_waitables() const = 0; + virtual void add_waitable_handle(const rclcpp::Waitable::SharedPtr & waitable) = 0; virtual bool add_handles_to_wait_set(rcl_wait_set_t * wait_set) = 0; virtual void clear_handles() = 0; virtual void remove_null_handles(rcl_wait_set_t * wait_set) = 0; - /// Provide a newly initialized AnyExecutable object. - // \return Shared pointer to the fresh executable. - virtual rclcpp::executor::AnyExecutable::SharedPtr instantiate_next_executable() = 0; - - virtual void add_guard_condition(const rcl_guard_condition_t * guard_condition) = 0; + virtual void + add_guard_condition(const rclcpp::GuardCondition & guard_condition) = 0; - virtual void remove_guard_condition(const rcl_guard_condition_t * guard_condition) = 0; + virtual void + remove_guard_condition(const rclcpp::GuardCondition * guard_condition) = 0; virtual void get_next_subscription( - rclcpp::executor::AnyExecutable::SharedPtr any_exec, - const WeakNodeVector & weak_nodes) = 0; + rclcpp::AnyExecutable & any_exec, + const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) = 0; virtual void get_next_service( - rclcpp::executor::AnyExecutable::SharedPtr any_exec, - const WeakNodeVector & weak_nodes) = 0; + rclcpp::AnyExecutable & any_exec, + const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) = 0; virtual void get_next_client( - rclcpp::executor::AnyExecutable::SharedPtr any_exec, - const WeakNodeVector & weak_nodes) = 0; + rclcpp::AnyExecutable & any_exec, + const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) = 0; + + virtual void + get_next_timer( + rclcpp::AnyExecutable & any_exec, + const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) = 0; + + virtual void + get_next_waitable( + rclcpp::AnyExecutable & any_exec, + const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) = 0; virtual rcl_allocator_t get_allocator() = 0; static rclcpp::SubscriptionBase::SharedPtr get_subscription_by_handle( - const rcl_subscription_t * subscriber_handle, - const WeakNodeVector & weak_nodes); + const std::shared_ptr & subscriber_handle, + const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); static rclcpp::ServiceBase::SharedPtr - get_service_by_handle(const rcl_service_t * service_handle, const WeakNodeVector & weak_nodes); + get_service_by_handle( + const std::shared_ptr & service_handle, + const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); static rclcpp::ClientBase::SharedPtr - get_client_by_handle(const rcl_client_t * client_handle, const WeakNodeVector & weak_nodes); + get_client_by_handle( + const std::shared_ptr & client_handle, + const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); + + static rclcpp::TimerBase::SharedPtr + get_timer_by_handle( + const std::shared_ptr & timer_handle, + const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); static rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_by_group( - rclcpp::callback_group::CallbackGroup::SharedPtr group, - const WeakNodeVector & weak_nodes); + const rclcpp::CallbackGroup::SharedPtr & group, + const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); - static rclcpp::callback_group::CallbackGroup::SharedPtr + static rclcpp::CallbackGroup::SharedPtr get_group_by_subscription( - rclcpp::SubscriptionBase::SharedPtr subscription, - const WeakNodeVector & weak_nodes); + const rclcpp::SubscriptionBase::SharedPtr & subscription, + const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); - static rclcpp::callback_group::CallbackGroup::SharedPtr + static rclcpp::CallbackGroup::SharedPtr get_group_by_service( - rclcpp::ServiceBase::SharedPtr service, - const WeakNodeVector & weak_nodes); + const rclcpp::ServiceBase::SharedPtr & service, + const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); - static rclcpp::callback_group::CallbackGroup::SharedPtr + static rclcpp::CallbackGroup::SharedPtr get_group_by_client( - rclcpp::ClientBase::SharedPtr client, - const WeakNodeVector & weak_nodes); + const rclcpp::ClientBase::SharedPtr & client, + const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); + + static rclcpp::CallbackGroup::SharedPtr + get_group_by_timer( + const rclcpp::TimerBase::SharedPtr & timer, + const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); + + static rclcpp::CallbackGroup::SharedPtr + get_group_by_waitable( + const rclcpp::Waitable::SharedPtr & waitable, + const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); }; } // namespace memory_strategy diff --git a/rclcpp/include/rclcpp/message_info.hpp b/rclcpp/include/rclcpp/message_info.hpp new file mode 100644 index 0000000000..452712afdb --- /dev/null +++ b/rclcpp/include/rclcpp/message_info.hpp @@ -0,0 +1,55 @@ +// 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 RCLCPP__MESSAGE_INFO_HPP_ +#define RCLCPP__MESSAGE_INFO_HPP_ + +#include "rmw/types.h" + +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ + +/// Additional meta data about messages taken from subscriptions. +class RCLCPP_PUBLIC MessageInfo +{ +public: + /// Default empty constructor. + MessageInfo() = default; + + /// Conversion constructor, which is intentionally not marked as explicit. + /** + * \param[in] rmw_message_info message info to initialize the class + */ + // cppcheck-suppress noExplicitConstructor + MessageInfo(const rmw_message_info_t & rmw_message_info); // NOLINT(runtime/explicit) + + virtual ~MessageInfo(); + + /// Return the message info as the underlying rmw message info type. + const rmw_message_info_t & + get_rmw_message_info() const; + + /// Return the message info as the underlying rmw message info type. + rmw_message_info_t & + get_rmw_message_info(); + +private: + rmw_message_info_t rmw_message_info_; +}; + +} // namespace rclcpp + +#endif // RCLCPP__MESSAGE_INFO_HPP_ diff --git a/rclcpp/include/rclcpp/message_memory_strategy.hpp b/rclcpp/include/rclcpp/message_memory_strategy.hpp index fafd0025ef..f548d953c2 100644 --- a/rclcpp/include/rclcpp/message_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/message_memory_strategy.hpp @@ -18,10 +18,18 @@ #include #include +#include "rcl/types.h" + #include "rclcpp/allocator/allocator_common.hpp" +#include "rclcpp/exceptions.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/serialized_message.hpp" #include "rclcpp/visibility_control.hpp" +#include "rcutils/logging_macros.h" + +#include "rmw/serialized_message.h" + namespace rclcpp { namespace message_memory_strategy @@ -39,16 +47,33 @@ class MessageMemoryStrategy using MessageAlloc = typename MessageAllocTraits::allocator_type; using MessageDeleter = allocator::Deleter; + using SerializedMessageAllocTraits = allocator::AllocRebind; + using SerializedMessageAlloc = typename SerializedMessageAllocTraits::allocator_type; + using SerializedMessageDeleter = + allocator::Deleter; + + using BufferAllocTraits = allocator::AllocRebind; + using BufferAlloc = typename BufferAllocTraits::allocator_type; + using BufferDeleter = allocator::Deleter; + MessageMemoryStrategy() { message_allocator_ = std::make_shared(); + serialized_message_allocator_ = std::make_shared(); + buffer_allocator_ = std::make_shared(); + rcutils_allocator_ = allocator::get_rcl_allocator(*buffer_allocator_.get()); } explicit MessageMemoryStrategy(std::shared_ptr allocator) { message_allocator_ = std::make_shared(*allocator.get()); + serialized_message_allocator_ = std::make_shared(*allocator.get()); + buffer_allocator_ = std::make_shared(*allocator.get()); + rcutils_allocator_ = allocator::get_rcl_allocator(*buffer_allocator_.get()); } + virtual ~MessageMemoryStrategy() = default; + /// Default factory method static SharedPtr create_default() { @@ -62,6 +87,21 @@ class MessageMemoryStrategy return std::allocate_shared(*message_allocator_.get()); } + virtual std::shared_ptr borrow_serialized_message(size_t capacity) + { + return std::make_shared(capacity); + } + + virtual std::shared_ptr borrow_serialized_message() + { + return borrow_serialized_message(default_buffer_capacity_); + } + + virtual void set_default_buffer_capacity(size_t capacity) + { + default_buffer_capacity_ = capacity; + } + /// Release ownership of the message, which will deallocate it if it has no more owners. /** \param[in] msg Shared pointer to the message we are returning. */ virtual void return_message(std::shared_ptr & msg) @@ -69,8 +109,23 @@ class MessageMemoryStrategy msg.reset(); } + virtual void return_serialized_message( + std::shared_ptr & serialized_msg) + { + serialized_msg.reset(); + } + std::shared_ptr message_allocator_; MessageDeleter message_deleter_; + + std::shared_ptr serialized_message_allocator_; + SerializedMessageDeleter serialized_message_deleter_; + + std::shared_ptr buffer_allocator_; + BufferDeleter buffer_deleter_; + size_t default_buffer_capacity_ = 0; + + rcutils_allocator_t rcutils_allocator_; }; } // namespace message_memory_strategy diff --git a/rclcpp/include/rclcpp/network_flow_endpoint.hpp b/rclcpp/include/rclcpp/network_flow_endpoint.hpp new file mode 100644 index 0000000000..a4f0b1c666 --- /dev/null +++ b/rclcpp/include/rclcpp/network_flow_endpoint.hpp @@ -0,0 +1,115 @@ +// 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 RCLCPP__NETWORK_FLOW_ENDPOINT_HPP_ +#define RCLCPP__NETWORK_FLOW_ENDPOINT_HPP_ + +#include +#include +#include + +#include "rcl/network_flow_endpoints.h" + +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ + +/// Forward declaration +class NetworkFlowEndpoint; + +/// Check if two NetworkFlowEndpoint instances are equal +RCLCPP_PUBLIC +bool operator==(const NetworkFlowEndpoint & left, const NetworkFlowEndpoint & right); + +/// Check if two NetworkFlowEndpoint instances are not equal +RCLCPP_PUBLIC +bool operator!=(const NetworkFlowEndpoint & left, const NetworkFlowEndpoint & right); + +/// Streaming helper for NetworkFlowEndpoint +RCLCPP_PUBLIC +std::ostream & operator<<(std::ostream & os, const NetworkFlowEndpoint & network_flow_endpoint); + +/** + * Class describes a network flow endpoint based on the counterpart definition + * in the RMW layer. + */ +class NetworkFlowEndpoint +{ +public: + /// Construct from rcl_network_flow_endpoint_t + RCLCPP_PUBLIC + explicit NetworkFlowEndpoint(const rcl_network_flow_endpoint_t & network_flow_endpoint) + : transport_protocol_( + rcl_network_flow_endpoint_get_transport_protocol_string(network_flow_endpoint. + transport_protocol)), + internet_protocol_( + rcl_network_flow_endpoint_get_internet_protocol_string( + network_flow_endpoint.internet_protocol)), + transport_port_(network_flow_endpoint.transport_port), + flow_label_(network_flow_endpoint.flow_label), + dscp_(network_flow_endpoint.dscp), + internet_address_(network_flow_endpoint.internet_address) + { + } + + /// Get transport protocol + RCLCPP_PUBLIC + const std::string & transport_protocol() const; + + /// Get internet protocol + RCLCPP_PUBLIC + const std::string & internet_protocol() const; + + /// Get transport port + RCLCPP_PUBLIC + uint16_t transport_port() const; + + /// Get flow label + RCLCPP_PUBLIC + uint32_t flow_label() const; + + /// Get DSCP + RCLCPP_PUBLIC + uint8_t dscp() const; + + /// Get internet address + RCLCPP_PUBLIC + const std::string & internet_address() const; + + /// Compare two NetworkFlowEndpoint instances + friend bool rclcpp::operator==( + const NetworkFlowEndpoint & left, + const NetworkFlowEndpoint & right); + friend bool rclcpp::operator!=( + const NetworkFlowEndpoint & left, + const NetworkFlowEndpoint & right); + + /// Streaming helper + friend std::ostream & rclcpp::operator<<( + std::ostream & os, + const NetworkFlowEndpoint & network_flow_endpoint); + +private: + std::string transport_protocol_; + std::string internet_protocol_; + uint16_t transport_port_; + uint32_t flow_label_; + uint8_t dscp_; + std::string internet_address_; +}; + +} // namespace rclcpp + +#endif // RCLCPP__NETWORK_FLOW_ENDPOINT_HPP_ diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index e90a04e0fd..930bf419f1 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -17,14 +17,18 @@ #include #include +#include #include #include #include #include #include #include +#include #include +#include "rcutils/macros.h" + #include "rcl/error_handling.h" #include "rcl/node.h" @@ -38,6 +42,10 @@ #include "rclcpp/clock.hpp" #include "rclcpp/context.hpp" #include "rclcpp/event.hpp" +#include "rclcpp/generic_client.hpp" +#include "rclcpp/generic_publisher.hpp" +#include "rclcpp/generic_service.hpp" +#include "rclcpp/generic_subscription.hpp" #include "rclcpp/logger.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/message_memory_strategy.hpp" @@ -47,12 +55,20 @@ #include "rclcpp/node_interfaces/node_logging_interface.hpp" #include "rclcpp/node_interfaces/node_parameters_interface.hpp" #include "rclcpp/node_interfaces/node_services_interface.hpp" +#include "rclcpp/node_interfaces/node_time_source_interface.hpp" #include "rclcpp/node_interfaces/node_timers_interface.hpp" #include "rclcpp/node_interfaces/node_topics_interface.hpp" +#include "rclcpp/node_interfaces/node_type_descriptions_interface.hpp" +#include "rclcpp/node_interfaces/node_waitables_interface.hpp" +#include "rclcpp/node_options.hpp" #include "rclcpp/parameter.hpp" #include "rclcpp/publisher.hpp" +#include "rclcpp/publisher_options.hpp" +#include "rclcpp/qos.hpp" #include "rclcpp/service.hpp" #include "rclcpp/subscription.hpp" +#include "rclcpp/subscription_options.hpp" +#include "rclcpp/subscription_traits.hpp" #include "rclcpp/time.hpp" #include "rclcpp/timer.hpp" #include "rclcpp/visibility_control.hpp" @@ -69,30 +85,26 @@ class Node : public std::enable_shared_from_this /// Create a new node with the specified name. /** * \param[in] node_name Name of the node. - * \param[in] namespace_ Namespace of the node. - * \param[in] use_intra_process_comms True to use the optimized intra-process communication - * pipeline to pass messages between nodes in the same process using shared memory. + * \param[in] options Additional options to control creation of the node. + * \throws InvalidNamespaceError if the namespace is invalid */ RCLCPP_PUBLIC explicit Node( const std::string & node_name, - const std::string & namespace_ = "", - bool use_intra_process_comms = false); + const NodeOptions & options = NodeOptions()); - /// Create a node based on the node name and a rclcpp::Context. + /// Create a new node with the specified name. /** * \param[in] node_name Name of the node. * \param[in] namespace_ Namespace of the node. - * \param[in] context The context for the node (usually represents the state of a process). - * \param[in] use_intra_process_comms True to use the optimized intra-process communication - * pipeline to pass messages between nodes in the same process using shared memory. + * \param[in] options Additional options to control creation of the node. + * \throws InvalidNamespaceError if the namespace is invalid */ RCLCPP_PUBLIC - Node( + explicit Node( const std::string & node_name, const std::string & namespace_, - rclcpp::Context::SharedPtr context, - bool use_intra_process_comms = false); + const NodeOptions & options = NodeOptions()); RCLCPP_PUBLIC virtual ~Node(); @@ -104,11 +116,28 @@ class Node : public std::enable_shared_from_this get_name() const; /// Get the namespace of the node. - /** \return The namespace of the node. */ + /** + * This namespace is the "node's" namespace, and therefore is not affected + * by any sub-namespace's that may affect entities created with this instance. + * Use get_effective_namespace() to get the full namespace used by entities. + * + * \sa get_sub_namespace() + * \sa get_effective_namespace() + * \return The namespace of the node. + */ RCLCPP_PUBLIC const char * get_namespace() const; + /// Get the fully-qualified name of the node. + /** + * The fully-qualified name includes the local namespace and name of the node. + * \return fully-qualified name of the node. + */ + RCLCPP_PUBLIC + const char * + get_fully_qualified_name() const; + /// Get the logger of the node. /** \return The logger of the node. */ RCLCPP_PUBLIC @@ -117,236 +146,1251 @@ class Node : public std::enable_shared_from_this /// Create and return a callback group. RCLCPP_PUBLIC - rclcpp::callback_group::CallbackGroup::SharedPtr - create_callback_group(rclcpp::callback_group::CallbackGroupType group_type); + rclcpp::CallbackGroup::SharedPtr + create_callback_group( + rclcpp::CallbackGroupType group_type, + bool automatically_add_to_executor_with_node = true); - /// Return the list of callback groups in the node. - RCLCPP_PUBLIC - const std::vector & - get_callback_groups() const; - - /// Create and return a Publisher. + /// Iterate over the callback groups in the node, calling the given function on each valid one. /** - * \param[in] topic_name The topic for this publisher to publish on. - * \param[in] qos_history_depth The depth of the publisher message queue. - * \param[in] allocator Optional custom allocator. - * \return Shared pointer to the created publisher. + * This method is called in a thread-safe way, and also makes sure to only call the given + * function on those items that are still valid. + * + * \param[in] func The callback function to call on each valid callback group. */ - template< - typename MessageT, typename Alloc = std::allocator, - typename PublisherT = ::rclcpp::Publisher> - std::shared_ptr - create_publisher( - const std::string & topic_name, size_t qos_history_depth, - std::shared_ptr allocator = nullptr); + RCLCPP_PUBLIC + void + for_each_callback_group(const node_interfaces::NodeBaseInterface::CallbackGroupFunction & func); /// Create and return a Publisher. /** + * The rclcpp::QoS has several convenient constructors, including a + * conversion constructor for size_t, which mimics older API's that + * allows just a string and size_t to create a publisher. + * + * For example, all of these cases will work: + * + * ```cpp + * pub = node->create_publisher("chatter", 10); // implicitly KeepLast + * pub = node->create_publisher("chatter", QoS(10)); // implicitly KeepLast + * pub = node->create_publisher("chatter", QoS(KeepLast(10))); + * pub = node->create_publisher("chatter", QoS(KeepAll())); + * pub = node->create_publisher("chatter", QoS(1).best_effort().durability_volatile()); + * { + * rclcpp::QoS custom_qos(KeepLast(10), rmw_qos_profile_sensor_data); + * pub = node->create_publisher("chatter", custom_qos); + * } + * ``` + * + * The publisher options may optionally be passed as the third argument for + * any of the above cases. + * * \param[in] topic_name The topic for this publisher to publish on. - * \param[in] qos_profile The quality of service profile to pass on to the rmw implementation. - * \param[in] allocator Optional custom allocator. + * \param[in] qos The Quality of Service settings for the publisher. + * \param[in] options Additional options for the created Publisher. * \return Shared pointer to the created publisher. */ template< - typename MessageT, typename Alloc = std::allocator, - typename PublisherT = ::rclcpp::Publisher> + typename MessageT, + typename AllocatorT = std::allocator, + typename PublisherT = rclcpp::Publisher> std::shared_ptr create_publisher( const std::string & topic_name, - const rmw_qos_profile_t & qos_profile = rmw_qos_profile_default, - std::shared_ptr allocator = nullptr); + const rclcpp::QoS & qos, + const PublisherOptionsWithAllocator & options = + PublisherOptionsWithAllocator() + ); /// Create and return a Subscription. /** * \param[in] topic_name The topic to subscribe on. - * \param[in] callback The user-defined callback function. - * \param[in] qos_profile The quality of service profile to pass on to the rmw implementation. - * \param[in] group The callback group for this subscription. NULL for no callback group. - * \param[in] ignore_local_publications True to ignore local publications. + * \param[in] qos QoS profile for Subcription. + * \param[in] callback The user-defined callback function to receive a message + * \param[in] options Additional options for the creation of the Subscription. * \param[in] msg_mem_strat The message memory strategy to use for allocating messages. - * \param[in] allocator Optional custom allocator. * \return Shared pointer to the created subscription. */ - /* TODO(jacquelinekay): - Windows build breaks when static member function passed as default - argument to msg_mem_strat, nullptr is a workaround. - */ template< typename MessageT, typename CallbackT, - typename Alloc = std::allocator, - typename SubscriptionT = rclcpp::Subscription> + typename AllocatorT = std::allocator, + typename SubscriptionT = rclcpp::Subscription, + typename MessageMemoryStrategyT = typename SubscriptionT::MessageMemoryStrategyType + > std::shared_ptr create_subscription( const std::string & topic_name, + const rclcpp::QoS & qos, CallbackT && callback, - const rmw_qos_profile_t & qos_profile = rmw_qos_profile_default, - rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr, - bool ignore_local_publications = false, - typename rclcpp::message_memory_strategy::MessageMemoryStrategy::SharedPtr - msg_mem_strat = nullptr, - std::shared_ptr allocator = nullptr); + const SubscriptionOptionsWithAllocator & options = + SubscriptionOptionsWithAllocator(), + typename MessageMemoryStrategyT::SharedPtr msg_mem_strat = ( + MessageMemoryStrategyT::create_default() + ) + ); - /// Create and return a Subscription. + /// Create a wall timer that uses the wall clock to drive the callback. /** - * \param[in] topic_name The topic to subscribe on. - * \param[in] qos_history_depth The depth of the subscription's incoming message queue. - * \param[in] callback The user-defined callback function. - * \param[in] group The callback group for this subscription. NULL for no callback group. - * \param[in] ignore_local_publications True to ignore local publications. - * \param[in] msg_mem_strat The message memory strategy to use for allocating messages. - * \param[in] allocator Optional custom allocator. - * \return Shared pointer to the created subscription. - */ - /* TODO(jacquelinekay): - Windows build breaks when static member function passed as default - argument to msg_mem_strat, nullptr is a workaround. + * \param[in] period Time interval between triggers of the callback. + * \param[in] callback User-defined callback function. + * \param[in] group Callback group to execute this timer's callback in. + * \param[in] autostart The state of the clock on initialization. */ - template< - typename MessageT, - typename CallbackT, - typename Alloc = std::allocator, - typename SubscriptionT = rclcpp::Subscription> - std::shared_ptr - create_subscription( - const std::string & topic_name, - size_t qos_history_depth, - CallbackT && callback, - rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr, - bool ignore_local_publications = false, - typename rclcpp::message_memory_strategy::MessageMemoryStrategy::SharedPtr - msg_mem_strat = nullptr, - std::shared_ptr allocator = nullptr); + template + typename rclcpp::WallTimer::SharedPtr + create_wall_timer( + std::chrono::duration period, + CallbackT callback, + rclcpp::CallbackGroup::SharedPtr group = nullptr, + bool autostart = true); - /// Create a timer. + /// Create a timer that uses the node clock to drive the callback. /** * \param[in] period Time interval between triggers of the callback. * \param[in] callback User-defined callback function. * \param[in] group Callback group to execute this timer's callback in. */ - template - typename rclcpp::WallTimer::SharedPtr - create_wall_timer( - std::chrono::duration period, + template + typename rclcpp::GenericTimer::SharedPtr + create_timer( + std::chrono::duration period, CallbackT callback, - rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); + rclcpp::CallbackGroup::SharedPtr group = nullptr); - /* Create and return a Client. */ + /// Create and return a Client. + /** + * \param[in] service_name The name on which the service is accessible. + * \param[in] qos Quality of service profile for client. + * \param[in] group Callback group to handle the reply to service calls. + * \return Shared pointer to the created client. + */ template typename rclcpp::Client::SharedPtr create_client( const std::string & service_name, - const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default, - rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); + const rclcpp::QoS & qos = rclcpp::ServicesQoS(), + rclcpp::CallbackGroup::SharedPtr group = nullptr); - /* Create and return a Service. */ + /// Create and return a Service. + /** + * \param[in] service_name The topic to service on. + * \param[in] callback User-defined callback function. + * \param[in] qos Quality of service profile for the service. + * \param[in] group Callback group to call the service. + * \return Shared pointer to the created service. + */ template typename rclcpp::Service::SharedPtr create_service( const std::string & service_name, CallbackT && callback, - const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default, - rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); + const rclcpp::QoS & qos = rclcpp::ServicesQoS(), + rclcpp::CallbackGroup::SharedPtr group = nullptr); + /// Create and return a GenericClient. + /** + * \param[in] service_name The name on which the service is accessible. + * \param[in] service_type The name of service type, e.g. "std_srvs/srv/SetBool" + * \param[in] qos Quality of service profile for client. + * \param[in] group Callback group to handle the reply to service calls. + * \return Shared pointer to the created GenericClient. + */ RCLCPP_PUBLIC - std::vector - set_parameters(const std::vector & parameters); + rclcpp::GenericClient::SharedPtr + create_generic_client( + const std::string & service_name, + const std::string & service_type, + const rclcpp::QoS & qos = rclcpp::ServicesQoS(), + rclcpp::CallbackGroup::SharedPtr group = nullptr); + + /// Create and return a GenericService. + /** + * \param[in] service_name The topic to service on. + * \param[in] service_type The name of service type, e.g. "std_srvs/srv/SetBool" + * \param[in] callback User-defined callback function. + * \param[in] qos Quality of service profile for the service. + * \param[in] group Callback group to call the service. + * \return Shared pointer to the created service. + */ + template + typename rclcpp::GenericService::SharedPtr + create_generic_service( + const std::string & service_name, + const std::string & service_type, + CallbackT && callback, + const rclcpp::QoS & qos = rclcpp::ServicesQoS(), + rclcpp::CallbackGroup::SharedPtr group = nullptr); + + /// Create and return a GenericPublisher. + /** + * The returned pointer will never be empty, but this function can throw various exceptions, for + * instance when the message's package can not be found on the AMENT_PREFIX_PATH. + * + * \param[in] topic_name Topic name + * \param[in] topic_type Topic type + * \param[in] qos %QoS settings + * \param options %Publisher options. + * Not all publisher options are currently respected, the only relevant options for this + * publisher are `event_callbacks`, `use_default_callbacks`, and `%callback_group`. + * \return Shared pointer to the created generic publisher. + */ + template> + std::shared_ptr create_generic_publisher( + const std::string & topic_name, + const std::string & topic_type, + const rclcpp::QoS & qos, + const rclcpp::PublisherOptionsWithAllocator & options = ( + rclcpp::PublisherOptionsWithAllocator() + ) + ); + + /// Create and return a GenericSubscription. + /** + * The returned pointer will never be empty, but this function can throw various exceptions, for + * instance when the message's package can not be found on the AMENT_PREFIX_PATH. + * + * \param[in] topic_name Topic name + * \param[in] topic_type Topic type + * \param[in] qos %QoS settings + * \param[in] callback Callback for new messages of serialized form + * \param[in] options %Subscription options. + * Not all subscription options are currently respected, the only relevant options for this + * subscription are `event_callbacks`, `use_default_callbacks`, `ignore_local_publications`, and + * `%callback_group`. + * \return Shared pointer to the created generic subscription. + */ + template< + typename CallbackT, + typename AllocatorT = std::allocator> + std::shared_ptr create_generic_subscription( + const std::string & topic_name, + const std::string & topic_type, + const rclcpp::QoS & qos, + CallbackT && callback, + const rclcpp::SubscriptionOptionsWithAllocator & options = ( + rclcpp::SubscriptionOptionsWithAllocator() + ) + ); + /// Declare and initialize a parameter, return the effective value. + /** + * This method is used to declare that a parameter exists on this node. + * If, at run-time, the user has provided an initial value then it will be + * set in this method, otherwise the given default_value will be set. + * In either case, the resulting value is returned, whether or not it is + * based on the default value or the user provided initial value. + * + * If no parameter_descriptor is given, then the default values from the + * message definition will be used, e.g. read_only will be false. + * + * The name and type in the given rcl_interfaces::msg::ParameterDescriptor + * are ignored, and should be specified using the name argument to this + * function and the default value's type instead. + * + * If `ignore_override` is `true`, the parameter override will be ignored. + * + * This method will result in any callback registered with + * `add_on_set_parameters_callback` and `add_post_set_parameters_callback` + * to be called for the parameter being set. + * + * If a callback was registered previously with `add_on_set_parameters_callback`, + * it will be called prior to setting the parameter for the node. + * If that callback prevents the initial value for the parameter from being + * set then rclcpp::exceptions::InvalidParameterValueException is thrown. + * + * If a callback was registered previously with `add_post_set_parameters_callback`, + * it will be called after setting the parameter successfully for the node. + * + * This method will _not_ result in any callbacks registered with + * `add_pre_set_parameters_callback` to be called. + * + * The returned reference will remain valid until the parameter is + * undeclared. + * + * \param[in] name The name of the parameter. + * \param[in] default_value An initial value to be used if at run-time user + * did not override it. + * \param[in] parameter_descriptor An optional, custom description for + * the parameter. + * \param[in] ignore_override When `true`, the parameter override is ignored. + * Default to `false`. + * \return A const reference to the value of the parameter. + * \throws rclcpp::exceptions::ParameterAlreadyDeclaredException if parameter + * has already been declared. + * \throws rclcpp::exceptions::InvalidParametersException if a parameter + * name is invalid. + * \throws rclcpp::exceptions::InvalidParameterValueException if initial + * value fails to be set. + * \throws rclcpp::exceptions::InvalidParameterTypeException + * if the type of the default value or override is wrong. + */ RCLCPP_PUBLIC - rcl_interfaces::msg::SetParametersResult - set_parameters_atomically(const std::vector & parameters); + const rclcpp::ParameterValue & + declare_parameter( + const std::string & name, + const rclcpp::ParameterValue & default_value, + const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor = + rcl_interfaces::msg::ParameterDescriptor(), + bool ignore_override = false); + /// Declare and initialize a parameter, return the effective value. + /** + * Same as the previous one, but a default value is not provided and the user + * must provide a parameter override of the correct type. + * + * \param[in] name The name of the parameter. + * \param[in] type Desired type of the parameter, which will enforced at runtime. + * \param[in] parameter_descriptor An optional, custom description for + * the parameter. + * \param[in] ignore_override When `true`, the parameter override is ignored. + * Default to `false`. + * \return A const reference to the value of the parameter. + * \throws Same as the previous overload taking a default value. + * \throws rclcpp::exceptions::InvalidParameterTypeException + * if an override is not provided or the provided override is of the wrong type. + */ + RCLCPP_PUBLIC + const rclcpp::ParameterValue & + declare_parameter( + const std::string & name, + rclcpp::ParameterType type, + const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor = + rcl_interfaces::msg::ParameterDescriptor{}, + bool ignore_override = false); + + /// Declare and initialize a parameter with a type. + /** + * See the non-templated declare_parameter() on this class for details. + * + * If the type of the default value, and therefore also the type of return + * value, differs from the initial value provided in the node options, then + * a rclcpp::exceptions::InvalidParameterTypeException may be thrown. + * To avoid this, use the declare_parameter() method which returns an + * rclcpp::ParameterValue instead. + * + * Note, this method cannot return a const reference, because extending the + * lifetime of a temporary only works recursively with member initializers, + * and cannot be extended to members of a class returned. + * The return value of this class is a copy of the member of a ParameterValue + * which is returned by the other version of declare_parameter(). + * See also: + * + * - https://en.cppreference.com/w/cpp/language/lifetime + * - https://herbsutter.com/2008/01/01/gotw-88-a-candidate-for-the-most-important-const/ + * - https://www.youtube.com/watch?v=uQyT-5iWUow (cppnow 2018 presentation) + */ template - void - set_parameter_if_not_set( + auto + declare_parameter( + const std::string & name, + const ParameterT & default_value, + const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor = + rcl_interfaces::msg::ParameterDescriptor(), + bool ignore_override = false); + + /// Declare and initialize a parameter with a type. + /** + * See the non-templated declare_parameter() on this class for details. + */ + template + auto + declare_parameter( const std::string & name, - const ParameterT & value); + const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor = + rcl_interfaces::msg::ParameterDescriptor(), + bool ignore_override = false); + /// Declare and initialize several parameters with the same namespace and type. + /** + * For each key in the map, a parameter with a name of "namespace.key" + * will be set to the value in the map. + * The resulting value for each declared parameter will be returned. + * + * The name expansion is naive, so if you set the namespace to be "foo.", + * then the resulting parameter names will be like "foo..key". + * However, if the namespace is an empty string, then no leading '.' will be + * placed before each key, which would have been the case when naively + * expanding "namespace.key". + * This allows you to declare several parameters at once without a namespace. + * + * The map contains default values for parameters. + * There is another overload which takes the std::pair with the default value + * and descriptor. + * + * If `ignore_overrides` is `true`, all the overrides of the parameters declared + * by the function call will be ignored. + * + * This method will result in any callback registered with + * `add_on_set_parameters_callback` and `add_post_set_parameters_callback` + * to be called once for each parameter. + * + * This method, if successful, will result in any callback registered with + * `add_on_set_parameters_callback` to be called, once for each parameter. + * If that callback prevents the initial value for any parameter from being + * set then rclcpp::exceptions::InvalidParameterValueException is thrown. + * + * If a callback was registered previously with `add_post_set_parameters_callback`, + * it will be called after setting the parameters successfully for the node, + * once for each parameter. + * + * This method will _not_ result in any callbacks registered with + * `add_pre_set_parameters_callback` to be called. + * + * \param[in] namespace_ The namespace in which to declare the parameters. + * \param[in] parameters The parameters to set in the given namespace. + * \param[in] ignore_overrides When `true`, the parameters overrides are ignored. + * Default to `false`. + * \throws rclcpp::exceptions::ParameterAlreadyDeclaredException if parameter + * has already been declared. + * \throws rclcpp::exceptions::InvalidParametersException if a parameter + * name is invalid. + * \throws rclcpp::exceptions::InvalidParameterValueException if initial + * value fails to be set. + */ + template + std::vector + declare_parameters( + const std::string & namespace_, + const std::map & parameters, + bool ignore_overrides = false); + + /// Declare and initialize several parameters with the same namespace and type. + /** + * This version will take a map where the value is a pair, with the default + * parameter value as the first item and a parameter descriptor as the second. + * + * See the simpler declare_parameters() on this class for more details. + */ + template + std::vector + declare_parameters( + const std::string & namespace_, + const std::map< + std::string, + std::pair + > & parameters, + bool ignore_overrides = false); + + /// Undeclare a previously declared parameter. + /** + * This method will _not_ cause a callback registered with any of the + * `add_pre_set_parameters_callback`, `add_on_set_parameters_callback` and + * `add_post_set_parameters_callback` to be called. + * + * \param[in] name The name of the parameter to be undeclared. + * \throws rclcpp::exceptions::ParameterNotDeclaredException if the parameter + * has not been declared. + * \throws rclcpp::exceptions::ParameterImmutableException if the parameter + * was create as read_only (immutable). + */ RCLCPP_PUBLIC - std::vector - get_parameters(const std::vector & names) const; + void + undeclare_parameter(const std::string & name); + /// Return true if a given parameter is declared. + /** + * \param[in] name The name of the parameter to check for being declared. + * \return true if the parameter name has been declared, otherwise false. + */ + RCLCPP_PUBLIC + bool + has_parameter(const std::string & name) const; + + /// Set a single parameter. + /** + * Set the given parameter and then return result of the set action. + * + * If the parameter has not been declared this function may throw the + * rclcpp::exceptions::ParameterNotDeclaredException exception, but only if + * the node was not created with the + * rclcpp::NodeOptions::allow_undeclared_parameters set to true. + * If undeclared parameters are allowed, then the parameter is implicitly + * declared with the default parameter meta data before being set. + * Parameter overrides are ignored by set_parameter. + * + * This method will result in any callback registered with + * `add_pre_set_parameters_callback`, add_on_set_parameters_callback` and + * `add_post_set_parameters_callback` to be called once for the parameter + * being set. + * + * This method will result in any callback registered with + * `add_on_set_parameters_callback` to be called. + * If the callback prevents the parameter from being set, then it will be + * reflected in the SetParametersResult that is returned, but no exception + * will be thrown. + * + * If a callback was registered previously with `add_pre_set_parameters_callback`, + * it will be called once prior to the validation of the parameter for the node. + * If this callback makes modified parameter list empty, then it will be reflected + * in the returned result; no exceptions will be raised in this case. + * + * If a callback was registered previously with `add_post_set_parameters_callback`, + * it will be called once after setting the parameter successfully for the node. + * + * If the value type of the parameter is rclcpp::PARAMETER_NOT_SET, and the + * existing parameter type is something else, then the parameter will be + * implicitly undeclared. + * This will result in a parameter event indicating that the parameter was + * deleted. + * + * \param[in] parameter The parameter to be set. + * \return The result of the set action. + * \throws rclcpp::exceptions::ParameterNotDeclaredException if the parameter + * has not been declared and undeclared parameters are not allowed. + */ RCLCPP_PUBLIC - rclcpp::parameter::ParameterVariant + rcl_interfaces::msg::SetParametersResult + set_parameter(const rclcpp::Parameter & parameter); + + /// Set one or more parameters, one at a time. + /** + * Set the given parameters, one at a time, and then return result of each + * set action. + * + * Parameters are set in the order they are given within the input vector. + * + * Like set_parameter, if any of the parameters to be set have not first been + * declared, and undeclared parameters are not allowed (the default), then + * this method will throw rclcpp::exceptions::ParameterNotDeclaredException. + * + * If setting a parameter fails due to not being declared, then the + * parameters which have already been set will stay set, and no attempt will + * be made to set the parameters which come after. + * + * If a parameter fails to be set due to any other reason, like being + * rejected by the user's callback (basically any reason other than not + * having been declared beforehand), then that is reflected in the + * corresponding SetParametersResult in the vector returned by this function. + * + * This method will result in any callback registered with + * `add_pre_set_parameters_callback`, `add_on_set_parameters_callback` and + * `add_post_set_parameters_callback` to be called once for each parameter. + + * If a callback was registered previously with `add_pre_set_parameters_callback`, + * it will be called prior to the validation of parameters for the node, + * once for each parameter. + * If this callback makes modified parameter list empty, then it will be reflected + * in the returned result; no exceptions will be raised in this case. + * + * This method will result in any callback registered with + * `add_on_set_parameters_callback` to be called, once for each parameter. + * If the callback prevents the parameter from being set, then, as mentioned + * before, it will be reflected in the corresponding SetParametersResult + * that is returned, but no exception will be thrown. + * + * If a callback was registered previously with `add_post_set_parameters_callback`, + * it will be called after setting the parameters successfully for the node, + * once for each parameter. + * + * Like set_parameter() this method will implicitly undeclare parameters + * with the type rclcpp::PARAMETER_NOT_SET. + * + * \param[in] parameters The vector of parameters to be set. + * \return The results for each set action as a vector. + * \throws rclcpp::exceptions::ParameterNotDeclaredException if any parameter + * has not been declared and undeclared parameters are not allowed. + */ + RCLCPP_PUBLIC + std::vector + set_parameters(const std::vector & parameters); + + /// Set one or more parameters, all at once. + /** + * Set the given parameters, all at one time, and then aggregate result. + * + * Behaves like set_parameter, except that it sets multiple parameters, + * failing all if just one of the parameters are unsuccessfully set. + * Either all of the parameters are set or none of them are set. + * + * Like set_parameter and set_parameters, this method may throw an + * rclcpp::exceptions::ParameterNotDeclaredException exception if any of the + * parameters to be set have not first been declared. + * If the exception is thrown then none of the parameters will have been set. + * + * This method will result in any callback registered with + * `add_pre_set_parameters_callback`, `add_on_set_parameters_callback` and + * `add_post_set_parameters_callback` to be called only 'once' for all parameters. + * + * If a callback was registered previously with `add_pre_set_parameters_callback`, + * it will be called prior to the validation of node parameters, just one time + * for all parameters. + * If this callback makes modified parameter list empty, then it will be reflected + * in the returned result; no exceptions will be raised in this case. + * + * This method will result in any callback registered with + * 'add_on_set_parameters_callback' to be called, just one time. + * If the callback prevents the parameters from being set, then it will be + * reflected in the SetParametersResult which is returned, but no exception + * will be thrown. + * + * If a callback was registered previously with `add_post_set_parameters_callback`, + * it will be called after setting the node parameters successfully, just one time + * for all parameters. + * + * If you pass multiple rclcpp::Parameter instances with the same name, then + * only the last one in the vector (forward iteration) will be set. + * + * Like set_parameter() this method will implicitly undeclare parameters + * with the type rclcpp::PARAMETER_NOT_SET. + * + * \param[in] parameters The vector of parameters to be set. + * \return The aggregate result of setting all the parameters atomically. + * \throws rclcpp::exceptions::ParameterNotDeclaredException if any parameter + * has not been declared and undeclared parameters are not allowed. + */ + RCLCPP_PUBLIC + rcl_interfaces::msg::SetParametersResult + set_parameters_atomically(const std::vector & parameters); + + /// Return the parameter by the given name. + /** + * If the parameter has not been declared, then this method may throw the + * rclcpp::exceptions::ParameterNotDeclaredException exception. + * If the parameter has not been initialized, then this method may throw the + * rclcpp::exceptions::ParameterUninitializedException exception. + * + * If undeclared parameters are allowed, see the node option + * rclcpp::NodeOptions::allow_undeclared_parameters, then this method will + * not throw the rclcpp::exceptions::ParameterNotDeclaredException exception, + * and instead return a default initialized rclcpp::Parameter, which has a type of + * rclcpp::ParameterType::PARAMETER_NOT_SET. + * + * \param[in] name The name of the parameter to get. + * \return The requested parameter inside of a rclcpp parameter object. + * \throws rclcpp::exceptions::ParameterNotDeclaredException if the parameter + * has not been declared and undeclared parameters are not allowed. + * \throws rclcpp::exceptions::ParameterUninitializedException if the parameter + * has not been initialized. + */ + RCLCPP_PUBLIC + rclcpp::Parameter get_parameter(const std::string & name) const; + /// Get the value of a parameter by the given name, and return true if it was set. + /** + * This method will never throw the + * rclcpp::exceptions::ParameterNotDeclaredException exception, but will + * instead return false if the parameter has not be previously declared. + * + * If the parameter was not declared, then the output argument for this + * method which is called "parameter" will not be assigned a value. + * If the parameter was declared, and therefore has a value, then it is + * assigned into the "parameter" argument of this method. + * + * \param[in] name The name of the parameter to get. + * \param[out] parameter The output storage for the parameter being retrieved. + * \return true if the parameter was previously declared, otherwise false. + */ RCLCPP_PUBLIC bool - get_parameter( - const std::string & name, - rclcpp::parameter::ParameterVariant & parameter) const; + get_parameter(const std::string & name, rclcpp::Parameter & parameter) const; - /// Assign the value of the parameter if set into the parameter argument. + /// Get the value of a parameter by the given name, and return true if it was set. /** - * If the parameter was not set, then the "parameter" argument is never assigned a value. + * Identical to the non-templated version of this method, except that when + * assigning the output argument called "parameter", this method will attempt + * to coerce the parameter value into the type requested by the given + * template argument, which may fail and throw an exception. * - * \param[in] name The name of the parameter to get. - * \param[out] parameter The output where the value of the parameter should be assigned. - * \returns true if the parameter was set, false otherwise + * If the parameter has not been declared, it will not attempt to coerce the + * value into the requested type, as it is known that the type is not set. + * + * \throws rclcpp::ParameterTypeException if the requested type does not + * match the value of the parameter which is stored. */ template bool get_parameter(const std::string & name, ParameterT & parameter) const; - /// Get the parameter value, or the "alternative value" if not set, and assign it to "value". + /// Get the parameter value, or the "alternative_value" if not set, and assign it to "parameter". /** - * If the parameter was not set, then the "value" argument is assigned + * If the parameter was not set, then the "parameter" argument is assigned * the "alternative_value". - * In all cases, the parameter remains not set after this function is called. + * + * Like the version of get_parameter() which returns a bool, this method will + * not throw the rclcpp::exceptions::ParameterNotDeclaredException exception. + * + * In all cases, the parameter is never set or declared within the node. * * \param[in] name The name of the parameter to get. - * \param[out] value The output where the value of the parameter should be assigned. + * \param[out] parameter The output where the value of the parameter should be assigned. * \param[in] alternative_value Value to be stored in output if the parameter was not set. - * \returns true if the parameter was set, false otherwise + * \returns true if the parameter was set, false otherwise. */ template bool get_parameter_or( const std::string & name, - ParameterT & value, + ParameterT & parameter, + const ParameterT & alternative_value) const; + + /// Return the parameter value, or the "alternative_value" if not set. + /** + * If the parameter was not set, then the "alternative_value" argument is returned. + * + * This method will not throw the rclcpp::exceptions::ParameterNotDeclaredException exception. + * + * In all cases, the parameter is never set or declared within the node. + * + * \param[in] name The name of the parameter to get. + * \param[in] alternative_value Value to be stored in output if the parameter was not set. + * \returns The value of the parameter. + */ + template + ParameterT + get_parameter_or( + const std::string & name, const ParameterT & alternative_value) const; + /// Return the parameters by the given parameter names. + /** + * Like get_parameter(const std::string &), this method may throw the + * rclcpp::exceptions::ParameterNotDeclaredException exception if the + * requested parameter has not been declared and undeclared parameters are + * not allowed, and may throw the rclcpp::exceptions::ParameterUninitializedException exception. + * + * Also like get_parameter(const std::string &), if undeclared parameters are allowed and the + * parameter has not been declared, then the corresponding rclcpp::Parameter + * will be default initialized and therefore have the type + * rclcpp::ParameterType::PARAMETER_NOT_SET. + * + * \param[in] names The names of the parameters to be retrieved. + * \return The parameters that were retrieved. + * \throws rclcpp::exceptions::ParameterNotDeclaredException if any of the + * parameters have not been declared and undeclared parameters are not + * allowed. + * \throws rclcpp::exceptions::ParameterUninitializedException if any of the + * parameters have not been initialized. + */ + RCLCPP_PUBLIC + std::vector + get_parameters(const std::vector & names) const; + + /// Get the parameter values for all parameters that have a given prefix. + /** + * The "prefix" argument is used to list the parameters which are prefixed + * with that prefix, see also list_parameters(). + * + * The resulting list of parameter names are used to get the values of the + * parameters. + * + * The names which are used as keys in the values map have the prefix removed. + * For example, if you use the prefix "foo" and the parameters "foo.ping" and + * "foo.pong" exist, then the returned map will have the keys "ping" and + * "pong". + * + * An empty string for the prefix will match all parameters. + * + * If no parameters with the prefix are found, then the output parameter + * "values" will be unchanged and false will be returned. + * Otherwise, the parameter names and values will be stored in the map and + * true will be returned to indicate "values" was mutated. + * + * This method will never throw the + * rclcpp::exceptions::ParameterNotDeclaredException exception because the + * action of listing the parameters is done atomically with getting the + * values, and therefore they are only listed if already declared and cannot + * be undeclared before being retrieved. + * + * Like the templated get_parameter() variant, this method will attempt to + * coerce the parameter values into the type requested by the given + * template argument, which may fail and throw an exception. + * + * \param[in] prefix The prefix of the parameters to get. + * \param[out] values The map used to store the parameter names and values, + * respectively, with one entry per parameter matching prefix. + * \returns true if output "values" was changed, false otherwise. + * \throws rclcpp::ParameterTypeException if the requested type does not + * match the value of the parameter which is stored. + */ + template + bool + get_parameters( + const std::string & prefix, + std::map & values) const; + + /// Return the parameter descriptor for the given parameter name. + /** + * Like get_parameters(), this method may throw the + * rclcpp::exceptions::ParameterNotDeclaredException exception if the + * requested parameter has not been declared and undeclared parameters are + * not allowed. + * + * If undeclared parameters are allowed, then a default initialized + * descriptor will be returned. + * + * \param[in] name The name of the parameter to describe. + * \return The descriptor for the given parameter name. + * \throws rclcpp::exceptions::ParameterNotDeclaredException if the + * parameter has not been declared and undeclared parameters are not + * allowed. + * \throws std::runtime_error if the number of described parameters is more than one + */ + RCLCPP_PUBLIC + rcl_interfaces::msg::ParameterDescriptor + describe_parameter(const std::string & name) const; + + /// Return a vector of parameter descriptors, one for each of the given names. + /** + * Like get_parameters(), this method may throw the + * rclcpp::exceptions::ParameterNotDeclaredException exception if any of the + * requested parameters have not been declared and undeclared parameters are + * not allowed. + * + * If undeclared parameters are allowed, then a default initialized + * descriptor will be returned for the undeclared parameter's descriptor. + * + * If the names vector is empty, then an empty vector will be returned. + * + * \param[in] names The list of parameter names to describe. + * \return A list of parameter descriptors, one for each parameter given. + * \throws rclcpp::exceptions::ParameterNotDeclaredException if any of the + * parameters have not been declared and undeclared parameters are not + * allowed. + * \throws std::runtime_error if the number of described parameters is more than one + */ RCLCPP_PUBLIC std::vector describe_parameters(const std::vector & names) const; + /// Return a vector of parameter types, one for each of the given names. + /** + * Like get_parameters(), this method may throw the + * rclcpp::exceptions::ParameterNotDeclaredException exception if any of the + * requested parameters have not been declared and undeclared parameters are + * not allowed. + * + * If undeclared parameters are allowed, then the default type + * rclcpp::ParameterType::PARAMETER_NOT_SET will be returned. + * + * \param[in] names The list of parameter names to get the types. + * \return A list of parameter types, one for each parameter given. + * \throws rclcpp::exceptions::ParameterNotDeclaredException if any of the + * parameters have not been declared and undeclared parameters are not + * allowed. + */ RCLCPP_PUBLIC std::vector get_parameter_types(const std::vector & names) const; + /// Return a list of parameters with any of the given prefixes, up to the given depth. + /** + * Parameters are separated into a hierarchy using the "." (dot) character. + * The "prefixes" argument is a way to select only particular parts of the hierarchy. + * + * \param[in] prefixes The list of prefixes that should be searched for within the + * current parameters. If this vector of prefixes is empty, then list_parameters + * will return all parameters. + * \param[in] depth An unsigned integer that represents the recursive depth to search. + * If this depth = 0, then all parameters that fit the prefixes will be returned. + * \returns A ListParametersResult message which contains both an array of unique prefixes + * and an array of names that were matched to the prefixes given. + */ RCLCPP_PUBLIC rcl_interfaces::msg::ListParametersResult list_parameters(const std::vector & prefixes, uint64_t depth) const; - /// Register the callback for parameter changes + using PreSetParametersCallbackHandle = + rclcpp::node_interfaces::PreSetParametersCallbackHandle; + using PreSetParametersCallbackType = + rclcpp::node_interfaces::NodeParametersInterface::PreSetParametersCallbackType; + + using OnSetParametersCallbackHandle = + rclcpp::node_interfaces::OnSetParametersCallbackHandle; + using OnSetParametersCallbackType = + rclcpp::node_interfaces::NodeParametersInterface::OnSetParametersCallbackType; + + using PostSetParametersCallbackHandle = + rclcpp::node_interfaces::PostSetParametersCallbackHandle; + using PostSetParametersCallbackType = + rclcpp::node_interfaces::NodeParametersInterface::PostSetParametersCallbackType; + + /// Add a callback that gets triggered before parameters are validated. /** - * \param[in] callback User defined callback function. - * It is expected to atomically set parameters. - * \note Repeated invocations of this function will overwrite previous callbacks + * This callback can be used to modify the original list of parameters being + * set by the user. + * + * The modified list of parameters is then forwarded to the "on set parameter" + * callback for validation. + * + * The callback is called whenever any of the `set_parameter*` methods are called + * or when a set parameter service request is received. + * + * The callback takes a reference to the vector of parameters to be set. + * + * The vector of parameters may be modified by the callback. + * + * One of the use case of "pre set callback" can be updating additional parameters + * conditioned on changes to a parameter. + * + * Users should retain a copy of the returned shared pointer, as the callback + * is valid only as long as the smart pointer is alive. + * + * For an example callback: + * + *```cpp + * void + * preSetParameterCallback(std::vector & parameters) + * { + * for (auto & param : parameters) { + * if (param.get_name() == "param1") { + * parameters.push_back(rclcpp::Parameter("param2", 4.0)); + * } + * } + * } + * ``` + * The above callback appends 'param2' to the list of parameters to be set if + * 'param1' is being set by the user. + * + * All parameters in the vector will be set atomically. + * + * Note that the callback is only called while setting parameters with `set_parameter`, + * `set_parameters`, `set_parameters_atomically`, or externally with a parameters service. + * + * The callback is not called when parameters are declared with `declare_parameter` + * or `declare_parameters`. + * + * The callback is not called when parameters are undeclared with `undeclare_parameter`. + * + * An empty modified parameter list from the callback will result in "set_parameter*" + * returning an unsuccessful result. + * + * The `remove_pre_set_parameters_callback` can be used to deregister the callback. + * + * \param callback The callback to register. + * \returns A shared pointer. The callback is valid as long as the smart pointer is alive. + * \throws std::bad_alloc if the allocation of the PreSetParametersCallbackHandle fails. */ - template + RCLCPP_PUBLIC + RCUTILS_WARN_UNUSED + PreSetParametersCallbackHandle::SharedPtr + add_pre_set_parameters_callback(PreSetParametersCallbackType callback); + + /// Add a callback to validate parameters before they are set. + /** + * The callback signature is designed to allow handling of any of the above + * `set_parameter*` or `declare_parameter*` methods, and so it takes a const + * reference to a vector of parameters to be set, and returns an instance of + * rcl_interfaces::msg::SetParametersResult to indicate whether or not the + * parameter should be set or not, and if not why. + * + * Users should retain a copy of the returned shared pointer, as the callback + * is valid only as long as the smart pointer is alive. + * + * For an example callback: + * + * ```cpp + * rcl_interfaces::msg::SetParametersResult + * my_callback(const std::vector & parameters) + * { + * rcl_interfaces::msg::SetParametersResult result; + * result.successful = true; + * for (const auto & parameter : parameters) { + * if (!some_condition) { + * result.successful = false; + * result.reason = "the reason it could not be allowed"; + * } + * } + * return result; + * } + * ``` + * + * You can see that the SetParametersResult is a boolean flag for success + * and an optional reason that can be used in error reporting when it fails. + * + * This allows the node developer to control which parameters may be changed. + * + * It is considered bad practice to reject changes for "unknown" parameters as this prevents + * other parts of the node (that may be aware of these parameters) from handling them. + * + * Note that the callback is called when declare_parameter() and its variants + * are called, and so you cannot assume the parameter has been set before + * this callback, so when checking a new value against the existing one, you + * must account for the case where the parameter is not yet set. + * + * The callback is not called when parameters are undeclared with `undeclare_parameter`. + * + * Some constraints like read_only are enforced before the callback is called. + * + * The callback may introspect other already set parameters (by calling any + * of the {get,list,describe}_parameter() methods), but may *not* modify + * other parameters (by calling any of the {set,declare}_parameter() methods) + * or modify the registered callback itself (by calling the + * add_on_set_parameters_callback() method). If a callback tries to do any + * of the latter things, + * rclcpp::exceptions::ParameterModifiedInCallbackException will be thrown. + * + * The callback functions must remain valid as long as the + * returned smart pointer is valid. + * The returned smart pointer can be promoted to a shared version. + * + * Resetting or letting the smart pointer go out of scope unregisters the callback. + * `remove_on_set_parameters_callback` can also be used. + * + * The registered callbacks are called when a parameter is set. + * When a callback returns a not successful result, the remaining callbacks aren't called. + * The order of the callback is the reverse from the registration order. + * + * \param callback The callback to register. + * \returns A shared pointer. The callback is valid as long as the smart pointer is alive. + * \throws std::bad_alloc if the allocation of the OnSetParametersCallbackHandle fails. + */ + RCLCPP_PUBLIC + RCUTILS_WARN_UNUSED + OnSetParametersCallbackHandle::SharedPtr + add_on_set_parameters_callback(OnSetParametersCallbackType callback); + + /// Add a callback that gets triggered after parameters are set successfully. + /** + * The callback is called when any of the `set_parameter*` or `declare_parameter*` + * methods are successful. + * + * Users should retain a copy of the returned shared pointer, as the callback + * is valid only as long as the smart pointer is alive. + * + * The callback takes a reference to a const vector of parameters that have been + * set successfully. + * + * The post callback can be valuable as a place to cause side-effects based on + * parameter changes. + * For instance updating internally tracked class attributes once parameters + * have been changed successfully. + * + * For an example callback: + * + * ```cpp + * void + * postSetParameterCallback(const std::vector & parameters) + * { + * for(const auto & param:parameters) { + * // the internal class member can be changed after + * // successful change to param1 or param2 + * if(param.get_name() == "param1") { + * internal_tracked_class_parameter_1_ = param.get_value(); + * } + * else if(param.get_name() == "param2") { + * internal_tracked_class_parameter_2_ = param.get_value(); + * } + * } + * } + * ``` + * + * The above callback takes a const reference to list of parameters that have been + * set successfully and as a result of this updates the internally tracked class attributes + * `internal_tracked_class_parameter_1_` and `internal_tracked_class_parameter_2_` + * respectively. + * + * This callback should not modify parameters. + * + * The callback is called when parameters are declared with `declare_parameter` + * or `declare_parameters`. See `declare_parameter` or `declare_parameters` above. + * + * The callback is not called when parameters are undeclared with `undeclare_parameter`. + * + * If you want to make changes to parameters based on changes to another, use + * `add_pre_set_parameters_callback`. + * + * The `remove_post_set_parameters_callback` can be used to deregister the callback. + * + * \param callback The callback to register. + * \returns A shared pointer. The callback is valid as long as the smart pointer is alive. + * \throws std::bad_alloc if the allocation of the OnSetParametersCallbackHandle fails. + */ + RCLCPP_PUBLIC + RCUTILS_WARN_UNUSED + PostSetParametersCallbackHandle::SharedPtr + add_post_set_parameters_callback(PostSetParametersCallbackType callback); + + /// Remove a callback registered with `add_pre_set_parameters_callback`. + /** + * Delete a handler returned by `add_pre_set_parameters_callback`. + * + * \param handler The callback handler to remove. + * \throws std::runtime_error if the handler was not created with `add_pre_set_parameters_callback`, + * or if it has been removed before. + */ + RCLCPP_PUBLIC + void + remove_pre_set_parameters_callback(const PreSetParametersCallbackHandle * const handler); + + /// Remove a callback registered with `add_on_set_parameters_callback`. + /** + * Delete a handler returned by `add_on_set_parameters_callback`. + * + * e.g.: + * + * `remove_on_set_parameters_callback(scoped_callback.get())` + * + * As an alternative, the smart pointer can be reset: + * + * `scoped_callback.reset()` + * + * Supposing that `scoped_callback` was the only owner. + * + * Calling `remove_on_set_parameters_callback` more than once with the same handler, + * or calling it after the shared pointer has been reset is an error. + * Resetting or letting the smart pointer go out of scope after calling + * `remove_on_set_parameters_callback` is not a problem. + * + * \param handler The callback handler to remove. + * \throws std::runtime_error if the handler was not created with `add_on_set_parameters_callback`, + * or if it has been removed before. + */ + RCLCPP_PUBLIC + void + remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const handler); + + /// Remove a callback registered with `add_post_set_parameters_callback`. + /** + * Delete a handler returned by `add_post_set_parameters_callback`. + * + * \param handler The callback handler to remove. + * \throws std::runtime_error if the handler was not created with `add_post_set_parameters_callback`, + * or if it has been removed before. + */ + RCLCPP_PUBLIC void - register_param_change_callback(CallbackT && callback); + remove_post_set_parameters_callback(const PostSetParametersCallbackHandle * const handler); + /// Get the fully-qualified names of all available nodes. + /** + * The fully-qualified name includes the local namespace and name of the node. + * \return A vector of fully-qualified names of nodes. + */ + RCLCPP_PUBLIC + std::vector + get_node_names() const; + + /// Return a map of existing topic names to list of topic types. + /** + * \return a map of existing topic names to list of topic types. + * \throws std::runtime_error anything that rcl_error can throw + */ RCLCPP_PUBLIC std::map> get_topic_names_and_types() const; + /// Return a map of existing service names to list of service types. + /** + * \return a map of existing service names to list of service types. + * \throws std::runtime_error anything that rcl_error can throw + */ RCLCPP_PUBLIC std::map> get_service_names_and_types() const; + /// Return a map of existing service names to list of service types for a specific node. + /** + * This function only considers services - not clients. + * The returned names are the actual names used and do not have remap rules applied. + * + * \param[in] node_name name of the node. + * \param[in] namespace_ namespace of the node. + * \return a map of existing service names to list of service types. + * \throws std::runtime_error anything that rcl_error can throw. + */ + RCLCPP_PUBLIC + std::map> + get_service_names_and_types_by_node( + const std::string & node_name, + const std::string & namespace_) const; + + /// Return the number of publishers created for a given topic. + /** + * \param[in] topic_name the actual topic name used; it will not be automatically remapped. + * \return number of publishers that have been created for the given topic. + * \throws std::runtime_error if publishers could not be counted + */ RCLCPP_PUBLIC size_t count_publishers(const std::string & topic_name) const; + /// Return the number of subscribers created for a given topic. + /** + * \param[in] topic_name the actual topic name used; it will not be automatically remapped. + * \return number of subscribers that have been created for the given topic. + * \throws std::runtime_error if subscribers could not be counted + */ RCLCPP_PUBLIC size_t count_subscribers(const std::string & topic_name) const; + /// Return the number of clients created for a given service. + /** + * \param[in] service_name the actual service name used; it will not be automatically remapped. + * \return number of clients that have been created for the given service. + * \throws std::runtime_error if clients could not be counted + */ + RCLCPP_PUBLIC + size_t + count_clients(const std::string & service_name) const; + + /// Return the number of services created for a given service. + /** + * \param[in] service_name the actual service name used; it will not be automatically remapped. + * \return number of services that have been created for the given service. + * \throws std::runtime_error if services could not be counted + */ + RCLCPP_PUBLIC + size_t + count_services(const std::string & service_name) const; + + /// Return the topic endpoint information about publishers on a given topic. + /** + * The returned parameter is a list of topic endpoint information, where each item will contain + * the node name, node namespace, topic type, endpoint type, topic endpoint's GID, and its QoS + * profile. + * + * 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. + * + * `topic_name` may be a relative, private, or fully qualified topic name. + * A relative or private topic will be expanded using this node's namespace and name. + * The queried `topic_name` is not remapped. + * + * \param[in] topic_name the actual topic name used; it will not be automatically remapped. + * \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. Defaults to `false`. + * \return a list of TopicEndpointInfo representing all the publishers on this topic. + * \throws InvalidTopicNameError if the given topic_name is invalid. + * \throws std::runtime_error if internal error happens. + */ + RCLCPP_PUBLIC + std::vector + get_publishers_info_by_topic(const std::string & topic_name, bool no_mangle = false) const; + + /// Return the topic endpoint information about subscriptions on a given topic. + /** + * The returned parameter is a list of topic endpoint information, where each item will contain + * the node name, node namespace, topic type, endpoint type, topic endpoint's GID, and its QoS + * profile. + * + * 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. + * + * `topic_name` may be a relative, private, or fully qualified topic name. + * A relative or private topic will be expanded using this node's namespace and name. + * The queried `topic_name` is not remapped. + * + * \param[in] topic_name the actual topic name used; it will not be automatically remapped. + * \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. Defaults to `false`. + * \return a list of TopicEndpointInfo representing all the subscriptions on this topic. + * \throws InvalidTopicNameError if the given topic_name is invalid. + * \throws std::runtime_error if internal error happens. + */ + RCLCPP_PUBLIC + std::vector + get_subscriptions_info_by_topic(const std::string & topic_name, bool no_mangle = false) const; + /// Return a graph event, which will be set anytime a graph change occurs. /* The graph Event object is a loan which must be returned. - * The Event object is scoped and therefore to return the load just let it go + * The Event object is scoped and therefore to return the loan just let it go * out of scope. */ RCLCPP_PUBLIC @@ -357,6 +1401,9 @@ class Node : public std::enable_shared_from_this /** * The given Event must be acquire through the get_graph_event() method. * + * \param[in] event pointer to an Event to wait for + * \param[in] timeout nanoseconds to wait for the Event to change the state + * * \throws InvalidEventError if the given event is nullptr * \throws EventNotRegisteredError if the given event was not acquired with * get_graph_event(). @@ -367,13 +1414,29 @@ class Node : public std::enable_shared_from_this rclcpp::Event::SharedPtr event, std::chrono::nanoseconds timeout); + /// Get a clock as a non-const shared pointer which is managed by the node. + /** + * \sa rclcpp::node_interfaces::NodeClock::get_clock + */ RCLCPP_PUBLIC rclcpp::Clock::SharedPtr get_clock(); + /// Get a clock as a const shared pointer which is managed by the node. + /** + * \sa rclcpp::node_interfaces::NodeClock::get_clock + */ + RCLCPP_PUBLIC + rclcpp::Clock::ConstSharedPtr + get_clock() const; + + /// Returns current time from the time source specified by clock_type. + /** + * \sa rclcpp::Clock::now + */ RCLCPP_PUBLIC Time - now(); + now() const; /// Return the Node's internal NodeBaseInterface implementation. RCLCPP_PUBLIC @@ -390,6 +1453,11 @@ class Node : public std::enable_shared_from_this rclcpp::node_interfaces::NodeGraphInterface::SharedPtr get_node_graph_interface(); + /// Return the Node's internal NodeLoggingInterface implementation. + RCLCPP_PUBLIC + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr + get_node_logging_interface(); + /// Return the Node's internal NodeTimersInterface implementation. RCLCPP_PUBLIC rclcpp::node_interfaces::NodeTimersInterface::SharedPtr @@ -405,17 +1473,148 @@ class Node : public std::enable_shared_from_this rclcpp::node_interfaces::NodeServicesInterface::SharedPtr get_node_services_interface(); + /// Return the Node's internal NodeWaitablesInterface implementation. + RCLCPP_PUBLIC + rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr + get_node_waitables_interface(); + /// Return the Node's internal NodeParametersInterface implementation. RCLCPP_PUBLIC rclcpp::node_interfaces::NodeParametersInterface::SharedPtr get_node_parameters_interface(); -private: - RCLCPP_DISABLE_COPY(Node) + /// Return the Node's internal NodeTimeSourceInterface implementation. + RCLCPP_PUBLIC + rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr + get_node_time_source_interface(); + /// Return the Node's internal NodeTypeDescriptionsInterface implementation. RCLCPP_PUBLIC - bool - group_in_node(callback_group::CallbackGroup::SharedPtr group); + rclcpp::node_interfaces::NodeTypeDescriptionsInterface::SharedPtr + get_node_type_descriptions_interface(); + + /// Return the sub-namespace, if this is a sub-node, otherwise an empty string. + /** + * The returned sub-namespace is either the accumulated sub-namespaces which + * were given to one-to-many create_sub_node() calls, or an empty string if + * this is an original node instance, i.e. not a sub-node. + * + * For example, consider: + * + * ```cpp + * auto node = std::make_shared("my_node", "my_ns"); + * node->get_sub_namespace(); // -> "" + * auto sub_node1 = node->create_sub_node("a"); + * sub_node1->get_sub_namespace(); // -> "a" + * auto sub_node2 = sub_node1->create_sub_node("b"); + * sub_node2->get_sub_namespace(); // -> "a/b" + * auto sub_node3 = node->create_sub_node("foo"); + * sub_node3->get_sub_namespace(); // -> "foo" + * node->get_sub_namespace(); // -> "" + * ``` + * + * get_namespace() will return the original node namespace, and will not + * include the sub-namespace if one exists. + * To get that you need to call the get_effective_namespace() method. + * + * \sa get_namespace() + * \sa get_effective_namespace() + * \return the sub-namespace string, not including the node's original namespace + */ + RCLCPP_PUBLIC + const std::string & + get_sub_namespace() const; + + /// Return the effective namespace that is used when creating entities. + /** + * The returned namespace is a concatenation of the node namespace and the + * accumulated sub-namespaces, which is used as the namespace when creating + * entities which have relative names. + * + * For example, consider: + * + * ```cpp + * auto node = std::make_shared("my_node", "my_ns"); + * node->get_effective_namespace(); // -> "/my_ns" + * auto sub_node1 = node->create_sub_node("a"); + * sub_node1->get_effective_namespace(); // -> "/my_ns/a" + * auto sub_node2 = sub_node1->create_sub_node("b"); + * sub_node2->get_effective_namespace(); // -> "/my_ns/a/b" + * auto sub_node3 = node->create_sub_node("foo"); + * sub_node3->get_effective_namespace(); // -> "/my_ns/foo" + * node->get_effective_namespace(); // -> "/my_ns" + * ``` + * + * \sa get_namespace() + * \sa get_sub_namespace() + * \return the sub-namespace string, not including the node's original namespace + */ + RCLCPP_PUBLIC + const std::string & + get_effective_namespace() const; + + /// Create a sub-node, which will extend the namespace of all entities created with it. + /** + * A sub-node (short for subordinate node) is an instance of this class + * which has been created using an existing instance of this class, but which + * has an additional sub-namespace (short for subordinate namespace) + * associated with it. + * The sub-namespace will extend the node's namespace for the purpose of + * creating additional entities, such as Publishers, Subscriptions, Service + * Clients and Servers, and so on. + * + * By default, when an instance of this class is created using one of the + * public constructors, it has no sub-namespace associated with it, and + * therefore is not a sub-node. + * That "normal" node instance may, however, be used to create further + * instances of this class, based on the original instance, which have an + * additional sub-namespace associated with them. + * This may be done by using this method, create_sub_node(). + * + * Furthermore, a sub-node may be used to create additional sub-node's, in + * which case the sub-namespace passed to this function will further + * extend the sub-namespace of the existing sub-node. + * See get_sub_namespace() and get_effective_namespace() for examples. + * + * Note that entities which use absolute names are not affected by any + * namespaces, neither the normal node namespace nor any sub-namespace. + * Note also that the fully qualified node name is unaffected by a + * sub-namespace. + * + * The sub-namespace should be relative, and an exception will be thrown if + * the sub-namespace is absolute, i.e. if it starts with a leading '/'. + * + * \sa get_sub_namespace() + * \sa get_effective_namespace() + * \param[in] sub_namespace sub-namespace of the sub-node. + * \return newly created sub-node + * \throws NameValidationError if the sub-namespace is absolute, i.e. starts + * with a leading '/'. + */ + RCLCPP_PUBLIC + rclcpp::Node::SharedPtr + create_sub_node(const std::string & sub_namespace); + + /// Return the NodeOptions used when creating this node. + RCLCPP_PUBLIC + const rclcpp::NodeOptions & + get_node_options() const; + +protected: + /// Construct a sub-node, which will extend the namespace of all entities created with it. + /** + * \sa create_sub_node() + * + * \param[in] other The node from which a new sub-node is created. + * \param[in] sub_namespace The sub-namespace of the sub-node. + */ + RCLCPP_PUBLIC + Node( + const Node & other, + const std::string & sub_namespace); + +private: + RCLCPP_DISABLE_COPY(Node) rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_; @@ -423,10 +1622,21 @@ class Node : public std::enable_shared_from_this rclcpp::node_interfaces::NodeTimersInterface::SharedPtr node_timers_; rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_; rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_; - rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_; rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_; + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_; + rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr node_time_source_; + rclcpp::node_interfaces::NodeTypeDescriptionsInterface::SharedPtr node_type_descriptions_; + rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_; + + const rclcpp::NodeOptions node_options_; + const std::string sub_namespace_; + const std::string effective_namespace_; - bool use_intra_process_comms_; + class NodeImpl; + // This member is meant to be a place to backport features into stable distributions, + // and new features targeting Rolling should not use this. + // See the comment in node.cpp for more information. + std::shared_ptr hidden_impl_{nullptr}; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 3b355053ec..5b81bdcba4 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -19,6 +19,7 @@ #include #include +#include #include #include #include @@ -33,13 +34,19 @@ #include "rcl/publisher.h" #include "rcl/subscription.h" -#include "rcl_interfaces/msg/intra_process_message.hpp" - #include "rclcpp/contexts/default_context.hpp" -#include "rclcpp/intra_process_manager.hpp" -#include "rclcpp/parameter.hpp" +#include "rclcpp/create_client.hpp" +#include "rclcpp/create_generic_publisher.hpp" +#include "rclcpp/create_generic_subscription.hpp" #include "rclcpp/create_publisher.hpp" +#include "rclcpp/create_service.hpp" +#include "rclcpp/create_generic_service.hpp" #include "rclcpp/create_subscription.hpp" +#include "rclcpp/create_timer.hpp" +#include "rclcpp/detail/resolve_enable_topic_statistics.hpp" +#include "rclcpp/parameter.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/timer.hpp" #include "rclcpp/type_support_decl.hpp" #include "rclcpp/visibility_control.hpp" @@ -50,130 +57,102 @@ namespace rclcpp { -template -std::shared_ptr -Node::create_publisher( - const std::string & topic_name, size_t qos_history_depth, - std::shared_ptr allocator) +RCLCPP_LOCAL +inline +std::string +extend_name_with_sub_namespace(const std::string & name, const std::string & sub_namespace) { - if (!allocator) { - allocator = std::make_shared(); + std::string name_with_sub_namespace(name); + if (sub_namespace != "" && name.front() != '/' && name.front() != '~') { + name_with_sub_namespace = sub_namespace + "/" + name; } - rmw_qos_profile_t qos = rmw_qos_profile_default; - qos.depth = qos_history_depth; - return this->create_publisher(topic_name, qos, allocator); + return name_with_sub_namespace; } -template +template std::shared_ptr Node::create_publisher( - const std::string & topic_name, const rmw_qos_profile_t & qos_profile, - std::shared_ptr allocator) + const std::string & topic_name, + const rclcpp::QoS & qos, + const PublisherOptionsWithAllocator & options) { - if (!allocator) { - allocator = std::make_shared(); - } - return rclcpp::create_publisher( - this->node_topics_.get(), - topic_name, - qos_profile, - use_intra_process_comms_, - allocator); + return rclcpp::create_publisher( + *this, + extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()), + qos, + options); } -template +template< + typename MessageT, + typename CallbackT, + typename AllocatorT, + typename SubscriptionT, + typename MessageMemoryStrategyT> std::shared_ptr Node::create_subscription( const std::string & topic_name, + const rclcpp::QoS & qos, CallbackT && callback, - const rmw_qos_profile_t & qos_profile, - rclcpp::callback_group::CallbackGroup::SharedPtr group, - bool ignore_local_publications, - typename rclcpp::message_memory_strategy::MessageMemoryStrategy::SharedPtr - msg_mem_strat, - std::shared_ptr allocator) -{ - if (!allocator) { - allocator = std::make_shared(); - } - - if (!msg_mem_strat) { - using rclcpp::message_memory_strategy::MessageMemoryStrategy; - msg_mem_strat = MessageMemoryStrategy::create_default(); - } - - return rclcpp::create_subscription( - this->node_topics_.get(), - topic_name, + const SubscriptionOptionsWithAllocator & options, + typename MessageMemoryStrategyT::SharedPtr msg_mem_strat) +{ + return rclcpp::create_subscription( + *this, + extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()), + qos, std::forward(callback), - qos_profile, - group, - ignore_local_publications, - use_intra_process_comms_, - msg_mem_strat, - allocator); + options, + msg_mem_strat); } -template -std::shared_ptr -Node::create_subscription( - const std::string & topic_name, - size_t qos_history_depth, - CallbackT && callback, - rclcpp::callback_group::CallbackGroup::SharedPtr group, - bool ignore_local_publications, - typename rclcpp::message_memory_strategy::MessageMemoryStrategy::SharedPtr - msg_mem_strat, - std::shared_ptr allocator) -{ - rmw_qos_profile_t qos = rmw_qos_profile_default; - qos.depth = qos_history_depth; - return this->create_subscription( - topic_name, - std::forward(callback), - qos, +template +typename rclcpp::WallTimer::SharedPtr +Node::create_wall_timer( + std::chrono::duration period, + CallbackT callback, + rclcpp::CallbackGroup::SharedPtr group, + bool autostart) +{ + return rclcpp::create_wall_timer( + period, + std::move(callback), group, - ignore_local_publications, - msg_mem_strat, - allocator); + this->node_base_.get(), + this->node_timers_.get(), + autostart); } -template -typename rclcpp::WallTimer::SharedPtr -Node::create_wall_timer( - std::chrono::duration period, +template +typename rclcpp::GenericTimer::SharedPtr +Node::create_timer( + std::chrono::duration period, CallbackT callback, - rclcpp::callback_group::CallbackGroup::SharedPtr group) + rclcpp::CallbackGroup::SharedPtr group) { - auto timer = rclcpp::WallTimer::make_shared( - std::chrono::duration_cast(period), - std::move(callback)); - node_timers_->add_timer(timer, group); - return timer; + return rclcpp::create_timer( + this->get_clock(), + period, + std::move(callback), + group, + this->node_base_.get(), + this->node_timers_.get()); } template typename Client::SharedPtr Node::create_client( const std::string & service_name, - const rmw_qos_profile_t & qos_profile, - rclcpp::callback_group::CallbackGroup::SharedPtr group) + const rclcpp::QoS & qos, + rclcpp::CallbackGroup::SharedPtr group) { - rcl_client_options_t options = rcl_client_get_default_options(); - options.qos = qos_profile; - - using rclcpp::Client; - using rclcpp::ClientBase; - - auto cli = Client::make_shared( - node_base_.get(), + return rclcpp::create_client( + node_base_, node_graph_, - service_name, - options); - - auto cli_base_ptr = std::dynamic_pointer_cast(cli); - node_services_->add_client(cli_base_ptr, group); - return cli; + node_services_, + extend_name_with_sub_namespace(service_name, this->get_sub_namespace()), + qos, + group); } template @@ -181,52 +160,176 @@ typename rclcpp::Service::SharedPtr Node::create_service( const std::string & service_name, CallbackT && callback, - const rmw_qos_profile_t & qos_profile, - rclcpp::callback_group::CallbackGroup::SharedPtr group) + const rclcpp::QoS & qos, + rclcpp::CallbackGroup::SharedPtr group) { - rclcpp::AnyServiceCallback any_service_callback; - any_service_callback.set(std::forward(callback)); + return rclcpp::create_service( + node_base_, + node_services_, + extend_name_with_sub_namespace(service_name, this->get_sub_namespace()), + std::forward(callback), + qos, + group); +} + +template +typename rclcpp::GenericService::SharedPtr +Node::create_generic_service( + const std::string & service_name, + const std::string & service_type, + CallbackT && callback, + const rclcpp::QoS & qos, + rclcpp::CallbackGroup::SharedPtr group) +{ + return rclcpp::create_generic_service( + node_base_, + node_services_, + extend_name_with_sub_namespace(service_name, this->get_sub_namespace()), + service_type, + std::forward(callback), + qos, + group); +} - rcl_service_options_t service_options = rcl_service_get_default_options(); - service_options.qos = qos_profile; +template +std::shared_ptr +Node::create_generic_publisher( + const std::string & topic_name, + const std::string & topic_type, + const rclcpp::QoS & qos, + const rclcpp::PublisherOptionsWithAllocator & options) +{ + return rclcpp::create_generic_publisher( + node_topics_, + extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()), + topic_type, + qos, + options + ); +} - auto serv = Service::make_shared( - node_base_->get_shared_rcl_node_handle(), - service_name, any_service_callback, service_options); - auto serv_base_ptr = std::dynamic_pointer_cast(serv); - node_services_->add_service(serv_base_ptr, group); - return serv; +template +std::shared_ptr +Node::create_generic_subscription( + const std::string & topic_name, + const std::string & topic_type, + const rclcpp::QoS & qos, + CallbackT && callback, + const rclcpp::SubscriptionOptionsWithAllocator & options) +{ + return rclcpp::create_generic_subscription( + node_topics_, + extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()), + topic_type, + qos, + std::forward(callback), + options + ); } -template -void -Node::register_param_change_callback(CallbackT && callback) + +template +auto +Node::declare_parameter( + const std::string & name, + const ParameterT & default_value, + const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor, + bool ignore_override) { - this->node_parameters_->register_param_change_callback(std::forward(callback)); + try { + return this->declare_parameter( + name, + rclcpp::ParameterValue(default_value), + parameter_descriptor, + ignore_override + ).get(); + } catch (const ParameterTypeException & ex) { + throw exceptions::InvalidParameterTypeException(name, ex.what()); + } } template -void -Node::set_parameter_if_not_set( +auto +Node::declare_parameter( const std::string & name, - const ParameterT & value) + const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor, + bool ignore_override) { - rclcpp::parameter::ParameterVariant parameter_variant; - if (!this->get_parameter(name, parameter_variant)) { - this->set_parameters({ - rclcpp::parameter::ParameterVariant(name, value), - }); + // get advantage of parameter value template magic to get + // the correct rclcpp::ParameterType from ParameterT + rclcpp::ParameterValue value{ParameterT{}}; + try { + return this->declare_parameter( + name, + value.get_type(), + parameter_descriptor, + ignore_override + ).get(); + } catch (const ParameterTypeException &) { + throw exceptions::UninitializedStaticallyTypedParameterException(name); } } +template +std::vector +Node::declare_parameters( + const std::string & namespace_, + const std::map & parameters, + bool ignore_overrides) +{ + std::vector result; + std::string normalized_namespace = namespace_.empty() ? "" : (namespace_ + "."); + std::transform( + parameters.begin(), parameters.end(), std::back_inserter(result), + [this, &normalized_namespace, ignore_overrides](auto element) { + return this->declare_parameter( + normalized_namespace + element.first, + element.second, + rcl_interfaces::msg::ParameterDescriptor(), + ignore_overrides); + } + ); + return result; +} + +template +std::vector +Node::declare_parameters( + const std::string & namespace_, + const std::map< + std::string, + std::pair + > & parameters, + bool ignore_overrides) +{ + std::vector result; + std::string normalized_namespace = namespace_.empty() ? "" : (namespace_ + "."); + std::transform( + parameters.begin(), parameters.end(), std::back_inserter(result), + [this, &normalized_namespace, ignore_overrides](auto element) { + return static_cast( + this->declare_parameter( + normalized_namespace + element.first, + element.second.first, + element.second.second, + ignore_overrides) + ); + } + ); + return result; +} + template bool -Node::get_parameter(const std::string & name, ParameterT & value) const +Node::get_parameter(const std::string & name, ParameterT & parameter) const { - rclcpp::parameter::ParameterVariant parameter_variant; - bool result = get_parameter(name, parameter_variant); + std::string sub_name = extend_name_with_sub_namespace(name, this->get_sub_namespace()); + + rclcpp::Parameter parameter_variant; + + bool result = get_parameter(sub_name, parameter_variant); if (result) { - value = parameter_variant.get_value(); + parameter = static_cast(parameter_variant.get_value()); } return result; @@ -236,16 +339,49 @@ template bool Node::get_parameter_or( const std::string & name, - ParameterT & value, + ParameterT & parameter, const ParameterT & alternative_value) const { - bool got_parameter = get_parameter(name, value); + std::string sub_name = extend_name_with_sub_namespace(name, this->get_sub_namespace()); + + bool got_parameter = get_parameter(sub_name, parameter); if (!got_parameter) { - value = alternative_value; + parameter = alternative_value; } return got_parameter; } +template +ParameterT +Node::get_parameter_or( + const std::string & name, + const ParameterT & alternative_value) const +{ + ParameterT parameter; + get_parameter_or(name, parameter, alternative_value); + return parameter; +} + +// this is a partially-specialized version of get_parameter above, +// where our concrete type for ParameterT is std::map, but the to-be-determined +// type is the value in the map. +template +bool +Node::get_parameters( + const std::string & prefix, + std::map & values) const +{ + std::map params; + bool result = node_parameters_->get_parameters_by_prefix(prefix, params); + if (result) { + for (const auto & param : params) { + values[param.first] = static_cast(param.second.get_value()); + } + } + + return result; +} + } // namespace rclcpp #endif // RCLCPP__NODE_IMPL_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp b/rclcpp/include/rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp new file mode 100644 index 0000000000..a243e9611a --- /dev/null +++ b/rclcpp/include/rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp @@ -0,0 +1,210 @@ +// Copyright 2022 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__NODE_INTERFACES__DETAIL__NODE_INTERFACES_HELPERS_HPP_ +#define RCLCPP__NODE_INTERFACES__DETAIL__NODE_INTERFACES_HELPERS_HPP_ + +#include +#include +#include +#include + +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace node_interfaces +{ +namespace detail +{ + +// Support and Helper template classes for the NodeInterfaces class. + +template +std::tuple...> +init_tuple(NodeT & n); + +/// Stores the interfaces in a tuple, provides constructors, and getters. +template +struct NodeInterfacesStorage +{ + template + NodeInterfacesStorage(NodeT & node) // NOLINT(runtime/explicit) + : interfaces_(init_tuple(node)) + {} + + NodeInterfacesStorage() + : interfaces_() + {} + + explicit NodeInterfacesStorage(std::shared_ptr... args) + : interfaces_(args ...) + {} + + /// Individual Node Interface non-const getter. + template + std::shared_ptr + get() + { + static_assert( + (std::is_same_v|| ...), + "NodeInterfaces class does not contain given NodeInterfaceT"); + return std::get>(interfaces_); + } + + /// Individual Node Interface const getter. + template + std::shared_ptr + get() const + { + static_assert( + (std::is_same_v|| ...), + "NodeInterfaces class does not contain given NodeInterfaceT"); + return std::get>(interfaces_); + } + +protected: + std::tuple...> interfaces_; +}; + +/// Prototype of NodeInterfacesSupports. +/** + * Should read NodeInterfacesSupports<..., T, ...> as "NodeInterfaces supports T", and + * if NodeInterfacesSupport is specialized for T, the is_supported should be + * set to std::true_type, but by default it is std::false_type, which will + * lead to a compiler error when trying to use T with NodeInterfaces. + */ +template +struct NodeInterfacesSupports; + +/// Prototype of NodeInterfacesSupportCheck template meta-function. +/** + * This meta-function checks that all the types given are supported, + * throwing a more human-readable error if an unsupported type is used. + */ +template +struct NodeInterfacesSupportCheck; + +/// Iterating specialization that ensures classes are supported and inherited. +template +struct NodeInterfacesSupportCheck + : public NodeInterfacesSupportCheck +{ + static_assert( + NodeInterfacesSupports::is_supported::value, + "given NodeInterfaceT is not supported by rclcpp::node_interfaces::NodeInterfaces"); +}; + +/// Terminating case when there are no more "RemainingInterfaceTs". +template +struct NodeInterfacesSupportCheck +{}; + +/// Default specialization, needs to be specialized for each supported interface. +template +struct NodeInterfacesSupports +{ + // Specializations need to set this to std::true_type in addition to other interfaces. + using is_supported = std::false_type; +}; + +/// Terminating specialization of NodeInterfacesSupports. +template +struct NodeInterfacesSupports + : public StorageClassT +{ + /// Perfect forwarding constructor to get arguments down to StorageClassT. + template + explicit NodeInterfacesSupports(ArgsT && ... args) + : StorageClassT(std::forward(args) ...) + {} +}; + +// Helper functions to initialize the tuple in NodeInterfaces. + +template +void +init_element(TupleT & t, NodeT & n) +{ + std::get>(t) = + NodeInterfacesSupports::get_from_node_like(n); +} + +template +std::tuple...> +init_tuple(NodeT & n) +{ + using StorageClassT = NodeInterfacesStorage; + std::tuple...> t; + (init_element(t, n), ...); + return t; +} + +/// Macro for creating specializations with less boilerplate. +/** + * You can use this macro to add support for your interface class if: + * + * - The standard getter is get_node_{NodeInterfaceName}_interface(), and + * - the getter returns a non-const shared_ptr<{NodeInterfaceType}> + * + * Examples of using this can be seen in the standard node interface headers + * in rclcpp, e.g. rclcpp/node_interfaces/node_base_interface.hpp has: + * + * RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeBaseInterface, base) + * + * If your interface has a non-standard getter, or you want to instrument it or + * something like that, then you'll need to create your own specialization of + * the NodeInterfacesSupports struct without this macro. + */ +// *INDENT-OFF* +#define RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(NodeInterfaceType, NodeInterfaceName) \ + namespace rclcpp::node_interfaces::detail { \ + template \ + struct NodeInterfacesSupports< \ + StorageClassT, \ + NodeInterfaceType, \ + RemainingInterfaceTs ...> \ + : public NodeInterfacesSupports \ + { \ + using is_supported = std::true_type; \ + \ + template \ + static \ + std::shared_ptr \ + get_from_node_like(NodeT & node_like) \ + { \ + return node_like.get_node_ ## NodeInterfaceName ## _interface(); \ + } \ + \ + /* Perfect forwarding constructor to get arguments down to StorageClassT (eventually). */ \ + template \ + explicit NodeInterfacesSupports(ArgsT && ... args) \ + : NodeInterfacesSupports( \ + std::forward(args) ...) \ + {} \ + \ + std::shared_ptr \ + get_node_ ## NodeInterfaceName ## _interface() \ + { \ + return StorageClassT::template get(); \ + } \ + }; \ + } // namespace rclcpp::node_interfaces::detail +// *INDENT-ON* + +} // namespace detail +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__DETAIL__NODE_INTERFACES_HELPERS_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp index b0754cd69a..6173a08d50 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp @@ -15,10 +15,14 @@ #ifndef RCLCPP__NODE_INTERFACES__NODE_BASE_HPP_ #define RCLCPP__NODE_INTERFACES__NODE_BASE_HPP_ +#include #include +#include #include #include +#include "rcl/node.h" +#include "rclcpp/callback_group.hpp" #include "rclcpp/context.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" @@ -30,106 +34,135 @@ namespace node_interfaces { /// Implementation of the NodeBase part of the Node API. -class NodeBase : public NodeBaseInterface +class NodeBase : public NodeBaseInterface, public std::enable_shared_from_this { public: - RCLCPP_SMART_PTR_ALIASES_ONLY(NodeBaseInterface) + RCLCPP_SMART_PTR_ALIASES_ONLY(NodeBase) + /// Constructor. + /** + * If nullptr (default) is given for the default_callback_group, one will + * be created by the constructor using the create_callback_group() method, + * but virtual dispatch will not occur so overrides of that method will not + * be used. + */ RCLCPP_PUBLIC NodeBase( const std::string & node_name, const std::string & namespace_, - rclcpp::Context::SharedPtr context); + rclcpp::Context::SharedPtr context, + const rcl_node_options_t & rcl_node_options, + bool use_intra_process_default, + bool enable_topic_statistics_default, + rclcpp::CallbackGroup::SharedPtr default_callback_group = nullptr); RCLCPP_PUBLIC virtual ~NodeBase(); RCLCPP_PUBLIC - virtual const char * - get_name() const; + get_name() const override; RCLCPP_PUBLIC - virtual const char * - get_namespace() const; + get_namespace() const override; + + RCLCPP_PUBLIC + const char * + get_fully_qualified_name() const override; RCLCPP_PUBLIC - virtual rclcpp::Context::SharedPtr - get_context(); + get_context() override; RCLCPP_PUBLIC - virtual rcl_node_t * - get_rcl_node_handle(); + get_rcl_node_handle() override; RCLCPP_PUBLIC - virtual const rcl_node_t * - get_rcl_node_handle() const; + get_rcl_node_handle() const override; RCLCPP_PUBLIC - virtual std::shared_ptr - get_shared_rcl_node_handle(); + get_shared_rcl_node_handle() override; RCLCPP_PUBLIC - virtual std::shared_ptr - get_shared_rcl_node_handle() const; + get_shared_rcl_node_handle() const override; RCLCPP_PUBLIC - virtual - rclcpp::callback_group::CallbackGroup::SharedPtr - create_callback_group(rclcpp::callback_group::CallbackGroupType group_type); + rclcpp::CallbackGroup::SharedPtr + create_callback_group( + rclcpp::CallbackGroupType group_type, + bool automatically_add_to_executor_with_node = true) override; RCLCPP_PUBLIC - virtual - rclcpp::callback_group::CallbackGroup::SharedPtr - get_default_callback_group(); + rclcpp::CallbackGroup::SharedPtr + get_default_callback_group() override; RCLCPP_PUBLIC - virtual bool - callback_group_in_node(rclcpp::callback_group::CallbackGroup::SharedPtr group); + callback_group_in_node(rclcpp::CallbackGroup::SharedPtr group) override; + /// Iterate over the stored callback groups, calling the given function on each valid one. + /** + * This method is called in a thread-safe way, and also makes sure to only call the given + * function on those items that are still valid. + * + * \param[in] func The callback function to call on each valid callback group. + */ RCLCPP_PUBLIC - virtual - const std::vector & - get_callback_groups() const; + void + for_each_callback_group(const CallbackGroupFunction & func) override; RCLCPP_PUBLIC - virtual std::atomic_bool & - get_associated_with_executor_atomic(); + get_associated_with_executor_atomic() override; + [[deprecated("Use get_shared_notify_guard_condition or trigger_notify_guard_condition instead")]] RCLCPP_PUBLIC - virtual - rcl_guard_condition_t * - get_notify_guard_condition(); + rclcpp::GuardCondition & + get_notify_guard_condition() override; RCLCPP_PUBLIC - virtual - std::unique_lock - acquire_notify_guard_condition_lock() const; + rclcpp::GuardCondition::SharedPtr + get_shared_notify_guard_condition() override; + + RCLCPP_PUBLIC + void + trigger_notify_guard_condition() override; + + RCLCPP_PUBLIC + bool + get_use_intra_process_default() const override; + + bool + get_enable_topic_statistics_default() const override; + + std::string + resolve_topic_or_service_name( + const std::string & name, bool is_service, bool only_expand = false) const override; private: RCLCPP_DISABLE_COPY(NodeBase) rclcpp::Context::SharedPtr context_; + bool use_intra_process_default_; + bool enable_topic_statistics_default_; std::shared_ptr node_handle_; - rclcpp::callback_group::CallbackGroup::SharedPtr default_callback_group_; - std::vector callback_groups_; + rclcpp::CallbackGroup::SharedPtr default_callback_group_; + std::mutex callback_groups_mutex_; + std::vector callback_groups_; std::atomic_bool associated_with_executor_; /// Guard condition for notifying the Executor of changes to this node. mutable std::recursive_mutex notify_guard_condition_mutex_; - rcl_guard_condition_t notify_guard_condition_ = rcl_get_zero_initialized_guard_condition(); + std::shared_ptr notify_guard_condition_; bool notify_guard_condition_is_valid_; }; diff --git a/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp index 7869cde178..e5a3198275 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp @@ -15,16 +15,18 @@ #ifndef RCLCPP__NODE_INTERFACES__NODE_BASE_INTERFACE_HPP_ #define RCLCPP__NODE_INTERFACES__NODE_BASE_INTERFACE_HPP_ +#include +#include #include -#include #include -#include #include "rcl/node.h" #include "rclcpp/callback_group.hpp" #include "rclcpp/context.hpp" +#include "rclcpp/guard_condition.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp" #include "rclcpp/visibility_control.hpp" namespace rclcpp @@ -38,6 +40,10 @@ class NodeBaseInterface public: RCLCPP_SMART_PTR_ALIASES_ONLY(NodeBaseInterface) + RCLCPP_PUBLIC + virtual + ~NodeBaseInterface() = default; + /// Return the name of the node. /** \return The name of the node. */ RCLCPP_PUBLIC @@ -52,6 +58,13 @@ class NodeBaseInterface const char * get_namespace() const = 0; + /// Return the fully qualified name of the node. + /** \return The fully qualified name of the node. */ + RCLCPP_PUBLIC + virtual + const char * + get_fully_qualified_name() const = 0; + /// Return the context of the node. /** \return SharedPtr to the node's context. */ RCLCPP_PUBLIC @@ -94,26 +107,36 @@ class NodeBaseInterface /// Create and return a callback group. RCLCPP_PUBLIC virtual - rclcpp::callback_group::CallbackGroup::SharedPtr - create_callback_group(rclcpp::callback_group::CallbackGroupType group_type) = 0; + rclcpp::CallbackGroup::SharedPtr + create_callback_group( + rclcpp::CallbackGroupType group_type, + bool automatically_add_to_executor_with_node = true) = 0; /// Return the default callback group. RCLCPP_PUBLIC virtual - rclcpp::callback_group::CallbackGroup::SharedPtr + rclcpp::CallbackGroup::SharedPtr get_default_callback_group() = 0; /// Return true if the given callback group is associated with this node. RCLCPP_PUBLIC virtual bool - callback_group_in_node(rclcpp::callback_group::CallbackGroup::SharedPtr group) = 0; + callback_group_in_node(rclcpp::CallbackGroup::SharedPtr group) = 0; - /// Return list of callback groups associated with this node. + using CallbackGroupFunction = std::function; + + /// Iterate over the stored callback groups, calling the given function on each valid one. + /** + * This method is called in a thread-safe way, and also makes sure to only call the given + * function on those items that are still valid. + * + * \param[in] func The callback function to call on each valid callback group. + */ RCLCPP_PUBLIC virtual - const std::vector & - get_callback_groups() const = 0; + void + for_each_callback_group(const CallbackGroupFunction & func) = 0; /// Return the atomic bool which is used to ensure only one executor is used. RCLCPP_PUBLIC @@ -121,26 +144,60 @@ class NodeBaseInterface std::atomic_bool & get_associated_with_executor_atomic() = 0; - /// Return guard condition that should be notified when the internal node state changes. + /// Return a guard condition that should be notified when the internal node state changes. /** * For example, this should be notified when a publisher is added or removed. * - * \return the rcl_guard_condition_t if it is valid, else nullptr + * \return the GuardCondition if it is valid, else throw runtime error */ RCLCPP_PUBLIC virtual - rcl_guard_condition_t * + rclcpp::GuardCondition & get_notify_guard_condition() = 0; - /// Acquire and return a scoped lock that protects the notify guard condition. - /** This should be used when triggering the notify guard condition. */ + /// Return a guard condition that should be notified when the internal node state changes. + /** + * For example, this should be notified when a publisher is added or removed. + * + * \return the GuardCondition if it is valid, else nullptr + */ RCLCPP_PUBLIC virtual - std::unique_lock - acquire_notify_guard_condition_lock() const = 0; + rclcpp::GuardCondition::SharedPtr + get_shared_notify_guard_condition() = 0; + + /// Trigger the guard condition that notifies of internal node state changes. + /** + * For example, this should be notified when a publisher is added or removed. + */ + RCLCPP_PUBLIC + virtual + void + trigger_notify_guard_condition() = 0; + + /// Return the default preference for using intra process communication. + RCLCPP_PUBLIC + virtual + bool + get_use_intra_process_default() const = 0; + + /// Return the default preference for enabling topic statistics collection. + RCLCPP_PUBLIC + virtual + bool + get_enable_topic_statistics_default() const = 0; + + /// Expand and remap a given topic or service name. + RCLCPP_PUBLIC + virtual + std::string + resolve_topic_or_service_name( + const std::string & name, bool is_service, bool only_expand = false) const = 0; }; } // namespace node_interfaces } // namespace rclcpp +RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeBaseInterface, base) + #endif // RCLCPP__NODE_INTERFACES__NODE_BASE_INTERFACE_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_clock.hpp b/rclcpp/include/rclcpp/node_interfaces/node_clock.hpp index 3bdc95e142..fb2e0670a7 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_clock.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_clock.hpp @@ -15,14 +15,15 @@ #ifndef RCLCPP__NODE_INTERFACES__NODE_CLOCK_HPP_ #define RCLCPP__NODE_INTERFACES__NODE_CLOCK_HPP_ -#include "rclcpp/callback_group.hpp" +#include "rcl/time.h" #include "rclcpp/clock.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/node_interfaces/node_clock_interface.hpp" -#include "rclcpp/node_interfaces/node_parameters_interface.hpp" +#include "rclcpp/node_interfaces/node_graph_interface.hpp" +#include "rclcpp/node_interfaces/node_logging_interface.hpp" +#include "rclcpp/node_interfaces/node_services_interface.hpp" #include "rclcpp/node_interfaces/node_topics_interface.hpp" -#include "rclcpp/time_source.hpp" #include "rclcpp/visibility_control.hpp" namespace rclcpp @@ -41,7 +42,9 @@ class NodeClock : public NodeClockInterface rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics, rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, - rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services); + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + rcl_clock_type_t clock_type); RCLCPP_PUBLIC virtual @@ -49,9 +52,13 @@ class NodeClock : public NodeClockInterface /// Get a clock which will be kept up to date by the node. RCLCPP_PUBLIC - virtual rclcpp::Clock::SharedPtr - get_clock(); + get_clock() override; + + /// Get a clock which will be kept up to date by the node. + RCLCPP_PUBLIC + rclcpp::Clock::ConstSharedPtr + get_clock() const override; private: RCLCPP_DISABLE_COPY(NodeClock) @@ -60,9 +67,9 @@ class NodeClock : public NodeClockInterface rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_; rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_; rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_; + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_; - rclcpp::Clock::SharedPtr ros_clock_; - rclcpp::TimeSource time_source_; + rclcpp::Clock::SharedPtr clock_; }; } // namespace node_interfaces diff --git a/rclcpp/include/rclcpp/node_interfaces/node_clock_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_clock_interface.hpp index ef06b0c849..744eef4ce6 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_clock_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_clock_interface.hpp @@ -15,9 +15,9 @@ #ifndef RCLCPP__NODE_INTERFACES__NODE_CLOCK_INTERFACE_HPP_ #define RCLCPP__NODE_INTERFACES__NODE_CLOCK_INTERFACE_HPP_ -#include "rclcpp/callback_group.hpp" #include "rclcpp/clock.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp" #include "rclcpp/visibility_control.hpp" namespace rclcpp @@ -31,14 +31,26 @@ class NodeClockInterface public: RCLCPP_SMART_PTR_ALIASES_ONLY(NodeClockInterface) + RCLCPP_PUBLIC + virtual + ~NodeClockInterface() = default; + /// Get a ROS clock which will be kept up to date by the node. RCLCPP_PUBLIC virtual rclcpp::Clock::SharedPtr get_clock() = 0; + + /// Get a const ROS clock which will be kept up to date by the node. + RCLCPP_PUBLIC + virtual + rclcpp::Clock::ConstSharedPtr + get_clock() const = 0; }; } // namespace node_interfaces } // namespace rclcpp +RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeClockInterface, clock) + #endif // RCLCPP__NODE_INTERFACES__NODE_CLOCK_INTERFACE_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp b/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp index 5e747bc463..863dbee1bf 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp @@ -15,11 +15,15 @@ #ifndef RCLCPP__NODE_INTERFACES__NODE_GRAPH_HPP_ #define RCLCPP__NODE_INTERFACES__NODE_GRAPH_HPP_ +#include #include +#include #include #include #include #include +#include +#include #include #include "rcl/guard_condition.h" @@ -29,6 +33,7 @@ #include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/node_interfaces/node_graph_interface.hpp" #include "rclcpp/visibility_control.hpp" +#include "rmw/topic_endpoint_info_array.h" namespace rclcpp { @@ -55,61 +60,104 @@ class NodeGraph : public NodeGraphInterface ~NodeGraph(); RCLCPP_PUBLIC - virtual std::map> - get_topic_names_and_types(bool no_demangle = false) const; + get_topic_names_and_types(bool no_demangle = false) const override; RCLCPP_PUBLIC - virtual std::map> - get_service_names_and_types() const; + get_service_names_and_types() const override; + + RCLCPP_PUBLIC + std::map> + get_service_names_and_types_by_node( + const std::string & node_name, + const std::string & namespace_) const override; + + RCLCPP_PUBLIC + std::map> + get_client_names_and_types_by_node( + const std::string & node_name, + const std::string & namespace_) const override; + + RCLCPP_PUBLIC + std::map> + get_publisher_names_and_types_by_node( + const std::string & node_name, + const std::string & namespace_, + bool no_demangle = false) const override; + + RCLCPP_PUBLIC + std::map> + get_subscriber_names_and_types_by_node( + const std::string & node_name, + const std::string & namespace_, + bool no_demangle = false) const override; RCLCPP_PUBLIC - virtual std::vector - get_node_names() const; + get_node_names() const override; + + RCLCPP_PUBLIC + std::vector> + get_node_names_with_enclaves() const override; + + RCLCPP_PUBLIC + std::vector> + get_node_names_and_namespaces() const override; RCLCPP_PUBLIC - virtual size_t - count_publishers(const std::string & topic_name) const; + count_publishers(const std::string & topic_name) const override; RCLCPP_PUBLIC - virtual size_t - count_subscribers(const std::string & topic_name) const; + count_subscribers(const std::string & topic_name) const override; + + RCLCPP_PUBLIC + size_t + count_clients(const std::string & service_name) const override; + + RCLCPP_PUBLIC + size_t + count_services(const std::string & service_name) const override; RCLCPP_PUBLIC - virtual const rcl_guard_condition_t * - get_graph_guard_condition() const; + get_graph_guard_condition() const override; RCLCPP_PUBLIC - virtual void - notify_graph_change(); + notify_graph_change() override; RCLCPP_PUBLIC - virtual void - notify_shutdown(); + notify_shutdown() override; RCLCPP_PUBLIC - virtual rclcpp::Event::SharedPtr - get_graph_event(); + get_graph_event() override; RCLCPP_PUBLIC - virtual void wait_for_graph_change( rclcpp::Event::SharedPtr event, - std::chrono::nanoseconds timeout); + std::chrono::nanoseconds timeout) override; RCLCPP_PUBLIC - virtual size_t - count_graph_users(); + count_graph_users() const override; + + RCLCPP_PUBLIC + std::vector + get_publishers_info_by_topic( + const std::string & topic_name, + bool no_mangle = false) const override; + + RCLCPP_PUBLIC + std::vector + get_subscriptions_info_by_topic( + const std::string & topic_name, + bool no_mangle = false) const override; private: RCLCPP_DISABLE_COPY(NodeGraph) diff --git a/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp index 136ec9d7d1..80abc308c1 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp @@ -15,19 +15,134 @@ #ifndef RCLCPP__NODE_INTERFACES__NODE_GRAPH_INTERFACE_HPP_ #define RCLCPP__NODE_INTERFACES__NODE_GRAPH_INTERFACE_HPP_ +#include +#include #include #include #include +#include +#include #include +#include "rcl/graph.h" #include "rcl/guard_condition.h" #include "rclcpp/event.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp" +#include "rclcpp/qos.hpp" #include "rclcpp/visibility_control.hpp" namespace rclcpp { + +enum class EndpointType +{ + Invalid = RMW_ENDPOINT_INVALID, + Publisher = RMW_ENDPOINT_PUBLISHER, + Subscription = RMW_ENDPOINT_SUBSCRIPTION +}; + +/** + * Struct that contains topic endpoint information like the associated node name, node namespace, + * topic type, endpoint type, endpoint GID, and its QoS. + */ +class TopicEndpointInfo +{ +public: + /// Construct a TopicEndpointInfo from a rcl_topic_endpoint_info_t. + RCLCPP_PUBLIC + explicit TopicEndpointInfo(const rcl_topic_endpoint_info_t & info) + : node_name_(info.node_name), + node_namespace_(info.node_namespace), + topic_type_(info.topic_type), + endpoint_type_(static_cast(info.endpoint_type)), + qos_profile_({info.qos_profile.history, info.qos_profile.depth}, info.qos_profile), + topic_type_hash_(info.topic_type_hash) + { + std::copy(info.endpoint_gid, info.endpoint_gid + RMW_GID_STORAGE_SIZE, endpoint_gid_.begin()); + } + + /// Get a mutable reference to the node name. + RCLCPP_PUBLIC + std::string & + node_name(); + + /// Get a const reference to the node name. + RCLCPP_PUBLIC + const std::string & + node_name() const; + + /// Get a mutable reference to the node namespace. + RCLCPP_PUBLIC + std::string & + node_namespace(); + + /// Get a const reference to the node namespace. + RCLCPP_PUBLIC + const std::string & + node_namespace() const; + + /// Get a mutable reference to the topic type string. + RCLCPP_PUBLIC + std::string & + topic_type(); + + /// Get a const reference to the topic type string. + RCLCPP_PUBLIC + const std::string & + topic_type() const; + + /// Get a mutable reference to the topic endpoint type. + RCLCPP_PUBLIC + rclcpp::EndpointType & + endpoint_type(); + + /// Get a const reference to the topic endpoint type. + RCLCPP_PUBLIC + const rclcpp::EndpointType & + endpoint_type() const; + + /// Get a mutable reference to the GID of the topic endpoint. + RCLCPP_PUBLIC + std::array & + endpoint_gid(); + + /// Get a const reference to the GID of the topic endpoint. + RCLCPP_PUBLIC + const std::array & + endpoint_gid() const; + + /// Get a mutable reference to the QoS profile of the topic endpoint. + RCLCPP_PUBLIC + rclcpp::QoS & + qos_profile(); + + /// Get a const reference to the QoS profile of the topic endpoint. + RCLCPP_PUBLIC + const rclcpp::QoS & + qos_profile() const; + + /// Get a mutable reference to the type hash of the topic endpoint. + RCLCPP_PUBLIC + rosidl_type_hash_t & + topic_type_hash(); + + /// Get a const reference to the type hash of the topic endpoint. + RCLCPP_PUBLIC + const rosidl_type_hash_t & + topic_type_hash() const; + +private: + std::string node_name_; + std::string node_namespace_; + std::string topic_type_; + rclcpp::EndpointType endpoint_type_; + std::array endpoint_gid_; + rclcpp::QoS qos_profile_; + rosidl_type_hash_t topic_type_hash_; +}; + namespace node_interfaces { @@ -37,10 +152,17 @@ class NodeGraphInterface public: RCLCPP_SMART_PTR_ALIASES_ONLY(NodeGraphInterface) + RCLCPP_PUBLIC + virtual + ~NodeGraphInterface() = default; + /// Return a map of existing topic names to list of topic types. /** * A topic is considered to exist when at least one publisher or subscriber * exists for it, whether they be local or remote to this process. + * The returned names are the actual names of the topics, either announced by another nodes or by this one. + * 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. * * \param[in] no_demangle if true, topic names and types are not demangled */ @@ -54,30 +176,153 @@ class NodeGraphInterface * A service is considered to exist when at least one service server or * service client exists for it, whether they be local or remote to this * process. + * The returned names are the actual names of the services, either announced by another nodes or by this one. + * 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. */ RCLCPP_PUBLIC virtual std::map> get_service_names_and_types() const = 0; + /// Return a map of existing service names to list of service types for a specific node. + /** + * This function only considers services - not clients. + * The returned names are the actual names after remap rules applied. + * 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. + * + * \param[in] node_name name of the node + * \param[in] namespace_ namespace of the node + */ + RCLCPP_PUBLIC + virtual + std::map> + get_service_names_and_types_by_node( + const std::string & node_name, + const std::string & namespace_) const = 0; + + /// Return a map of existing service names and types with a specific node. + /** + * This function only considers clients - not service servers. + * The returned names are the actual names after remap rules applied. + * 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. + * + * \param[in] node_name name of the node + * \param[in] namespace_ namespace of the node + */ + RCLCPP_PUBLIC + virtual + std::map> + get_client_names_and_types_by_node( + const std::string & node_name, + const std::string & namespace_) const = 0; + + /// Return a map of existing topic names to list of topic types for a specific node. + /** + * This function only considers publishers - not subscribers. + * The returned names are the actual names after remap rules applied. + * 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. + * + * \param[in] node_name name of the node + * \param[in] namespace_ namespace of the node + * \param[in] no_demangle if true, topic names and types are not demangled + */ + RCLCPP_PUBLIC + virtual + std::map> + get_publisher_names_and_types_by_node( + const std::string & node_name, + const std::string & namespace_, + bool no_demangle = false) const = 0; + + /// Return a map of existing topic names to list of topic types for a specific node. + /** + * This function only considers subscribers - not publishers. + * The returned names are the actual names after remap rules applied. + * 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. + * + * \param[in] node_name name of the node + * \param[in] namespace_ namespace of the node + * \param[in] no_demangle if true, topic names and types are not demangled + */ + RCLCPP_PUBLIC + virtual + std::map> + get_subscriber_names_and_types_by_node( + const std::string & node_name, + const std::string & namespace_, + bool no_demangle = false) const = 0; + /// Return a vector of existing node names (string). + /* + * The returned names are the actual names after remap rules applied. + */ RCLCPP_PUBLIC virtual std::vector get_node_names() const = 0; + /// Return a vector of existing node names, namespaces and enclaves (tuple of string). + /* + * The returned names are the actual names after remap rules applied. + * The enclaves contain the runtime security artifacts, those can be + * used to establish secured network. + * See https://design.ros2.org/articles/ros2_security_enclaves.html + */ + RCLCPP_PUBLIC + virtual + std::vector> + get_node_names_with_enclaves() const = 0; + + /// Return a vector of existing node names and namespaces (pair of string). + /* + * The returned names are the actual names after remap rules applied. + */ + RCLCPP_PUBLIC + virtual + std::vector> + get_node_names_and_namespaces() const = 0; + /// Return the number of publishers that are advertised on a given topic. + /* + * \param[in] topic_name the actual topic name used; it will not be automatically remapped. + */ RCLCPP_PUBLIC virtual size_t count_publishers(const std::string & topic_name) const = 0; /// Return the number of subscribers who have created a subscription for a given topic. + /* + * \param[in] topic_name the actual topic name used; it will not be automatically remapped. + */ RCLCPP_PUBLIC virtual size_t count_subscribers(const std::string & topic_name) const = 0; + /// Return the number of clients created for a given service. + /* + * \param[in] service_name the actual service name used; it will not be automatically remapped. + */ + RCLCPP_PUBLIC + virtual + size_t + count_clients(const std::string & service_name) const = 0; + + /// Return the number of services created for a given service. + /* + * \param[in] service_name the actual service name used; it will not be automatically remapped. + */ + RCLCPP_PUBLIC + virtual + size_t + count_services(const std::string & service_name) const = 0; + /// Return the rcl guard condition which is triggered when the ROS graph changes. RCLCPP_PUBLIC virtual @@ -138,10 +383,36 @@ class NodeGraphInterface RCLCPP_PUBLIC virtual size_t - count_graph_users() = 0; + count_graph_users() const = 0; + + /// Return the topic endpoint information about publishers on a given topic. + /** + * \param[in] topic_name the actual topic name used; it will not be automatically remapped. + * \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. + * \sa rclcpp::Node::get_publishers_info_by_topic + */ + RCLCPP_PUBLIC + virtual + std::vector + get_publishers_info_by_topic(const std::string & topic_name, bool no_mangle = false) const = 0; + + /// Return the topic endpoint information about subscriptions on a given topic. + /** + * \param[in] topic_name the actual topic name used; it will not be automatically remapped. + * \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. + * \sa rclcpp::Node::get_subscriptions_info_by_topic + */ + RCLCPP_PUBLIC + virtual + std::vector + get_subscriptions_info_by_topic(const std::string & topic_name, bool no_mangle = false) const = 0; }; } // namespace node_interfaces } // namespace rclcpp +RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeGraphInterface, graph) + #endif // RCLCPP__NODE_INTERFACES__NODE_GRAPH_INTERFACE_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_interfaces.hpp b/rclcpp/include/rclcpp/node_interfaces/node_interfaces.hpp new file mode 100644 index 0000000000..80a20fb4a3 --- /dev/null +++ b/rclcpp/include/rclcpp/node_interfaces/node_interfaces.hpp @@ -0,0 +1,168 @@ +// Copyright 2022 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__NODE_INTERFACES__NODE_INTERFACES_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_INTERFACES_HPP_ + +#include + +#include "rclcpp/detail/template_unique.hpp" +#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp" + +#define ALL_RCLCPP_NODE_INTERFACES \ + rclcpp::node_interfaces::NodeBaseInterface, \ + rclcpp::node_interfaces::NodeClockInterface, \ + rclcpp::node_interfaces::NodeGraphInterface, \ + rclcpp::node_interfaces::NodeLoggingInterface, \ + rclcpp::node_interfaces::NodeParametersInterface, \ + rclcpp::node_interfaces::NodeServicesInterface, \ + rclcpp::node_interfaces::NodeTimeSourceInterface, \ + rclcpp::node_interfaces::NodeTimersInterface, \ + rclcpp::node_interfaces::NodeTopicsInterface, \ + rclcpp::node_interfaces::NodeTypeDescriptionsInterface, \ + rclcpp::node_interfaces::NodeWaitablesInterface + + +namespace rclcpp +{ +namespace node_interfaces +{ + + +/// A helper class for aggregating node interfaces. +template +class NodeInterfaces + : public detail::NodeInterfacesSupportCheck< + detail::NodeInterfacesStorage, + InterfaceTs ... + >, + public detail::NodeInterfacesSupports< + detail::NodeInterfacesStorage, + InterfaceTs ... + > +{ + static_assert( + 0 != sizeof ...(InterfaceTs), + "must provide at least one interface as a template argument"); + static_assert( + rclcpp::detail::template_unique_v, + "must provide unique template parameters"); + + using NodeInterfacesSupportsT = detail::NodeInterfacesSupports< + detail::NodeInterfacesStorage, + InterfaceTs ... + >; + +public: + /// Create a new NodeInterfaces object using the given node-like object's interfaces. + /** + * Specify which interfaces you need by passing them as template parameters. + * + * This allows you to aggregate interfaces from different sources together to pass as a single + * aggregate object to any functions that take node interfaces or node-likes, without needing to + * templatize that function. + * + * You may also use this constructor to create a NodeInterfaces that contains a subset of + * another NodeInterfaces' interfaces. + * + * Finally, this class supports implicit conversion from node-like objects, allowing you to + * directly pass a node-like to a function that takes a NodeInterfaces object. + * + * Usage examples: + * ```cpp + * // Suppose we have some function: + * void fn(NodeInterfaces interfaces); + * + * // Then we can, explicitly: + * rclcpp::Node node("some_node"); + * auto ni = NodeInterfaces(node); + * fn(ni); + * + * // But also: + * fn(node); + * + * // Subsetting a NodeInterfaces object also works! + * auto ni_base = NodeInterfaces(ni); + * + * // Or aggregate them (you could aggregate interfaces from disparate node-likes) + * auto ni_aggregated = NodeInterfaces( + * node->get_node_base_interface(), + * node->get_node_clock_interface() + * ) + * + * // And then to access the interfaces: + * // Get with get<> + * auto base = ni.get(); + * + * // Or the appropriate getter + * auto clock = ni.get_clock_interface(); + * ``` + * + * You may use any of the standard node interfaces that come with rclcpp: + * - rclcpp::node_interfaces::NodeBaseInterface + * - rclcpp::node_interfaces::NodeClockInterface + * - rclcpp::node_interfaces::NodeGraphInterface + * - rclcpp::node_interfaces::NodeLoggingInterface + * - rclcpp::node_interfaces::NodeParametersInterface + * - rclcpp::node_interfaces::NodeServicesInterface + * - rclcpp::node_interfaces::NodeTimeSourceInterface + * - rclcpp::node_interfaces::NodeTimersInterface + * - rclcpp::node_interfaces::NodeTopicsInterface + * - rclcpp::node_interfaces::NodeTypeDescriptionsInterface + * - rclcpp::node_interfaces::NodeWaitablesInterface + * + * Or you use custom interfaces as long as you make a template specialization + * of the rclcpp::node_interfaces::detail::NodeInterfacesSupport struct using + * the RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT macro. + * + * Usage example: + * ```cpp + * RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeBaseInterface, base) + * ``` + * + * If you choose not to use the helper macro, then you can specialize the + * template yourself, but you must: + * + * - Provide a template specialization of the get_from_node_like method that gets the interface + * from any node-like that stores the interface, using the node-like's getter + * - Designate the is_supported type as std::true_type using a using directive + * - Provide any number of getter methods to be used to obtain the interface with the + * NodeInterface object, noting that the getters of the storage class will apply to all + * supported interfaces. + * - The getter method names should not clash in name with any other interface getter + * specializations if those other interfaces are meant to be aggregated in the same + * NodeInterfaces object. + * + * \param[in] node Node-like object from which to get the node interfaces + */ + template + NodeInterfaces(NodeT & node) // NOLINT(runtime/explicit) + : NodeInterfacesSupportsT(node) + {} + + // Create a NodeInterfaces object with no bound interfaces + NodeInterfaces() + : NodeInterfacesSupportsT() + {} + + explicit NodeInterfaces(std::shared_ptr... args) + : NodeInterfacesSupportsT(args ...) + {} +}; + + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__NODE_INTERFACES_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_logging.hpp b/rclcpp/include/rclcpp/node_interfaces/node_logging.hpp index 0cffed11d6..30e7495368 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_logging.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_logging.hpp @@ -23,6 +23,9 @@ #include "rclcpp/node_interfaces/node_logging_interface.hpp" #include "rclcpp/visibility_control.hpp" +#include "rcl_interfaces/srv/get_logger_levels.hpp" +#include "rcl_interfaces/srv/set_logger_levels.hpp" + namespace rclcpp { namespace node_interfaces @@ -35,24 +38,35 @@ class NodeLogging : public NodeLoggingInterface RCLCPP_SMART_PTR_ALIASES_ONLY(NodeLoggingInterface) RCLCPP_PUBLIC - explicit NodeLogging(rclcpp::node_interfaces::NodeBaseInterface * node_base); + explicit NodeLogging(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base); RCLCPP_PUBLIC virtual ~NodeLogging(); RCLCPP_PUBLIC - virtual rclcpp::Logger - get_logger() const; + get_logger() const override; + + RCLCPP_PUBLIC + const char * + get_logger_name() const override; + + RCLCPP_PUBLIC + void + create_logger_services( + node_interfaces::NodeServicesInterface::SharedPtr node_services) override; private: RCLCPP_DISABLE_COPY(NodeLogging) /// Handle to the NodeBaseInterface given in the constructor. - rclcpp::node_interfaces::NodeBaseInterface * node_base_; + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; rclcpp::Logger logger_; + + rclcpp::Service::SharedPtr get_loggers_service_; + rclcpp::Service::SharedPtr set_loggers_service_; }; } // namespace node_interfaces diff --git a/rclcpp/include/rclcpp/node_interfaces/node_logging_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_logging_interface.hpp index c549ca28be..7f9776cf36 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_logging_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_logging_interface.hpp @@ -19,7 +19,8 @@ #include "rclcpp/logger.hpp" #include "rclcpp/macros.hpp" -#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_services_interface.hpp" +#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp" #include "rclcpp/visibility_control.hpp" namespace rclcpp @@ -33,15 +34,39 @@ class NodeLoggingInterface public: RCLCPP_SMART_PTR_ALIASES_ONLY(NodeLoggingInterface) + RCLCPP_PUBLIC + virtual + ~NodeLoggingInterface() = default; + /// Return the logger of the node. - /** \return The logger of the node. */ + /** + * \return The logger of the node. + */ RCLCPP_PUBLIC virtual rclcpp::Logger get_logger() const = 0; + + /// Return the logger name associated with the node. + /** + * \return The logger name associated with the node. + */ + RCLCPP_PUBLIC + virtual + const char * + get_logger_name() const = 0; + + /// create logger services + RCLCPP_PUBLIC + virtual + void + create_logger_services( + node_interfaces::NodeServicesInterface::SharedPtr node_services) = 0; }; } // namespace node_interfaces } // namespace rclcpp +RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeLoggingInterface, logging) + #endif // RCLCPP__NODE_INTERFACES__NODE_LOGGING_INTERFACE_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp b/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp index 0649e24635..ffbb400c11 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp @@ -15,10 +15,15 @@ #ifndef RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_HPP_ #define RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_HPP_ +#include #include +#include +#include #include #include +#include "rcutils/macros.h" + #include "rcl_interfaces/msg/list_parameters_result.hpp" #include "rcl_interfaces/msg/parameter_descriptor.hpp" #include "rcl_interfaces/msg/parameter_event.hpp" @@ -26,10 +31,12 @@ #include "rclcpp/macros.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_logging_interface.hpp" #include "rclcpp/node_interfaces/node_parameters_interface.hpp" #include "rclcpp/node_interfaces/node_services_interface.hpp" #include "rclcpp/node_interfaces/node_topics_interface.hpp" #include "rclcpp/parameter.hpp" +#include "rclcpp/parameter_service.hpp" #include "rclcpp/publisher.hpp" #include "rclcpp/visibility_control.hpp" @@ -38,82 +45,216 @@ namespace rclcpp namespace node_interfaces { +// Internal struct for holding useful info about parameters +struct ParameterInfo +{ + /// Current value of the parameter. + rclcpp::ParameterValue value; + + /// A description of the parameter + rcl_interfaces::msg::ParameterDescriptor descriptor; +}; + +// Internal RAII-style guard for mutation recursion +class ParameterMutationRecursionGuard +{ +public: + explicit ParameterMutationRecursionGuard(bool & allow_mod) + : allow_modification_(allow_mod) + { + if (!allow_modification_) { + throw rclcpp::exceptions::ParameterModifiedInCallbackException( + "cannot set or declare a parameter, or change the callback from within set callback"); + } + + allow_modification_ = false; + } + + ~ParameterMutationRecursionGuard() + { + allow_modification_ = true; + } + +private: + bool & allow_modification_; +}; + /// Implementation of the NodeParameters part of the Node API. class NodeParameters : public NodeParametersInterface { public: RCLCPP_SMART_PTR_ALIASES_ONLY(NodeParameters) + /// Constructor. + /** + * If using automatically_declare_parameters_from_overrides, overrides of + * get_parameter_overrides(), has_parameter(), declare_parameter() will not + * be respected. + * If this is an issue, pass false for + * automatically_declare_parameters_from_overrides and invoke + * perform_automatically_declare_parameters_from_overrides() manually after + * construction. + */ RCLCPP_PUBLIC NodeParameters( - rclcpp::node_interfaces::NodeTopicsInterface * node_topics, - bool use_intra_process); + const node_interfaces::NodeBaseInterface::SharedPtr node_base, + const node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + const node_interfaces::NodeTopicsInterface::SharedPtr node_topics, + const node_interfaces::NodeServicesInterface::SharedPtr node_services, + const node_interfaces::NodeClockInterface::SharedPtr node_clock, + const std::vector & parameter_overrides, + bool start_parameter_services, + bool start_parameter_event_publisher, + const rclcpp::QoS & parameter_event_qos, + const rclcpp::PublisherOptionsBase & parameter_event_publisher_options, + bool allow_undeclared_parameters, + bool automatically_declare_parameters_from_overrides); RCLCPP_PUBLIC virtual ~NodeParameters(); RCLCPP_PUBLIC - virtual + const rclcpp::ParameterValue & + declare_parameter( + const std::string & name, + const rclcpp::ParameterValue & default_value, + const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor = + rcl_interfaces::msg::ParameterDescriptor{}, + bool ignore_override = false) override; + + RCLCPP_PUBLIC + const rclcpp::ParameterValue & + declare_parameter( + const std::string & name, + rclcpp::ParameterType type, + const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor = + rcl_interfaces::msg::ParameterDescriptor(), + bool ignore_override = false) override; + + RCLCPP_PUBLIC + void + undeclare_parameter(const std::string & name) override; + + RCLCPP_PUBLIC + bool + has_parameter(const std::string & name) const override; + + RCLCPP_PUBLIC std::vector set_parameters( - const std::vector & parameters); + const std::vector & parameters) override; RCLCPP_PUBLIC - virtual rcl_interfaces::msg::SetParametersResult set_parameters_atomically( - const std::vector & parameters); + const std::vector & parameters) override; RCLCPP_PUBLIC - virtual - std::vector - get_parameters(const std::vector & names) const; + std::vector + get_parameters(const std::vector & names) const override; RCLCPP_PUBLIC - virtual - rclcpp::parameter::ParameterVariant - get_parameter(const std::string & name) const; + rclcpp::Parameter + get_parameter(const std::string & name) const override; RCLCPP_PUBLIC - virtual bool get_parameter( const std::string & name, - rclcpp::parameter::ParameterVariant & parameter) const; + rclcpp::Parameter & parameter) const override; + + RCLCPP_PUBLIC + bool + get_parameters_by_prefix( + const std::string & prefix, + std::map & parameters) const override; RCLCPP_PUBLIC - virtual std::vector - describe_parameters(const std::vector & names) const; + describe_parameters(const std::vector & names) const override; RCLCPP_PUBLIC - virtual std::vector - get_parameter_types(const std::vector & names) const; + get_parameter_types(const std::vector & names) const override; RCLCPP_PUBLIC - virtual rcl_interfaces::msg::ListParametersResult - list_parameters(const std::vector & prefixes, uint64_t depth) const; + list_parameters(const std::vector & prefixes, uint64_t depth) const override; + + RCLCPP_PUBLIC + RCUTILS_WARN_UNUSED + PreSetParametersCallbackHandle::SharedPtr + add_pre_set_parameters_callback(PreSetParametersCallbackType callback) override; + + RCLCPP_PUBLIC + RCUTILS_WARN_UNUSED + OnSetParametersCallbackHandle::SharedPtr + add_on_set_parameters_callback(OnSetParametersCallbackType callback) override; + + RCLCPP_PUBLIC + RCUTILS_WARN_UNUSED + PostSetParametersCallbackHandle::SharedPtr + add_post_set_parameters_callback(PostSetParametersCallbackType callback) override; RCLCPP_PUBLIC - virtual void - register_param_change_callback(ParametersCallbackFunction callback); + remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const handler) override; + + RCLCPP_PUBLIC + void + remove_post_set_parameters_callback(const PostSetParametersCallbackHandle * const handler) + override; + + RCLCPP_PUBLIC + void + remove_pre_set_parameters_callback(const PreSetParametersCallbackHandle * const handler) override; + + RCLCPP_PUBLIC + const std::map & + get_parameter_overrides() const override; + + using PreSetCallbacksHandleContainer = std::list; + using OnSetCallbacksHandleContainer = std::list; + using PostSetCallbacksHandleContainer = std::list; + using CallbacksContainerType [[deprecated("use OnSetCallbacksHandleContainer instead")]] = + OnSetCallbacksHandleContainer; + +protected: + RCLCPP_PUBLIC + void + perform_automatically_declare_parameters_from_overrides(); private: RCLCPP_DISABLE_COPY(NodeParameters) - rclcpp::node_interfaces::NodeTopicsInterface * node_topics_; + mutable std::recursive_mutex mutex_; - mutable std::mutex mutex_; + // There are times when we don't want to allow modifications to parameters + // (particularly when a set_parameter callback tries to call set_parameter, + // declare_parameter, etc). In those cases, this will be set to false. + bool parameter_modification_enabled_{true}; - ParametersCallbackFunction parameters_callback_ = nullptr; + PreSetCallbacksHandleContainer pre_set_parameters_callback_container_; - std::map parameters_; + OnSetCallbacksHandleContainer on_set_parameters_callback_container_; + + PostSetCallbacksHandleContainer post_set_parameters_callback_container_; + + std::map parameters_; + + std::map parameter_overrides_; + + bool allow_undeclared_ = false; Publisher::SharedPtr events_publisher_; + + std::shared_ptr parameter_service_; + + std::string combined_name_; + + node_interfaces::NodeLoggingInterface::SharedPtr node_logging_; + node_interfaces::NodeClockInterface::SharedPtr node_clock_; }; } // namespace node_interfaces diff --git a/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp index c936728f17..1cf10c1a97 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp @@ -15,6 +15,8 @@ #ifndef RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_INTERFACE_HPP_ #define RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_INTERFACE_HPP_ +#include +#include #include #include @@ -23,6 +25,7 @@ #include "rcl_interfaces/msg/set_parameters_result.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp" #include "rclcpp/parameter.hpp" #include "rclcpp/visibility_control.hpp" @@ -31,40 +34,162 @@ namespace rclcpp namespace node_interfaces { +struct PreSetParametersCallbackHandle +{ + RCLCPP_SMART_PTR_DEFINITIONS(PreSetParametersCallbackHandle) + + using PreSetParametersCallbackType = + std::function &)>; + + PreSetParametersCallbackType callback; +}; + +struct OnSetParametersCallbackHandle +{ + RCLCPP_SMART_PTR_DEFINITIONS(OnSetParametersCallbackHandle) + + using OnSetParametersCallbackType = + std::function< + rcl_interfaces::msg::SetParametersResult( + const std::vector &)>; + + OnSetParametersCallbackType callback; +}; + +struct PostSetParametersCallbackHandle +{ + RCLCPP_SMART_PTR_DEFINITIONS(PostSetParametersCallbackHandle) + + using PostSetParametersCallbackType = + std::function &)>; + + PostSetParametersCallbackType callback; +}; + /// Pure virtual interface class for the NodeParameters part of the Node API. class NodeParametersInterface { public: RCLCPP_SMART_PTR_ALIASES_ONLY(NodeParametersInterface) + RCLCPP_PUBLIC + virtual + ~NodeParametersInterface() = default; + + /// Declare and initialize a parameter. + /** + * \sa rclcpp::Node::declare_parameter + */ + RCLCPP_PUBLIC + virtual + const rclcpp::ParameterValue & + declare_parameter( + const std::string & name, + const rclcpp::ParameterValue & default_value, + const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor = + rcl_interfaces::msg::ParameterDescriptor(), + bool ignore_override = false) = 0; + + /// Declare a parameter. + /** + * \sa rclcpp::Node::declare_parameter + */ + RCLCPP_PUBLIC + virtual + const rclcpp::ParameterValue & + declare_parameter( + const std::string & name, + rclcpp::ParameterType type, + const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor = + rcl_interfaces::msg::ParameterDescriptor(), + bool ignore_override = false) = 0; + + /// Undeclare a parameter. + /** + * \sa rclcpp::Node::undeclare_parameter + */ + RCLCPP_PUBLIC + virtual + void + undeclare_parameter(const std::string & name) = 0; + + /// Return true if the parameter has been declared, otherwise false. + /** + * \sa rclcpp::Node::has_parameter + */ + RCLCPP_PUBLIC + virtual + bool + has_parameter(const std::string & name) const = 0; + + /// Set one or more parameters, one at a time. + /** + * \sa rclcpp::Node::set_parameters + */ RCLCPP_PUBLIC virtual std::vector - set_parameters( - const std::vector & parameters) = 0; + set_parameters(const std::vector & parameters) = 0; + /// Set one or more parameters, all at once. + /** + * \sa rclcpp::Node::set_parameters_atomically + */ RCLCPP_PUBLIC virtual rcl_interfaces::msg::SetParametersResult set_parameters_atomically( - const std::vector & parameters) = 0; + const std::vector & parameters) = 0; + /// Get descriptions of parameters given their names. + /* + * \param[in] names a list of parameter names to check. + * \return the list of parameters that were found. + * Any parameter not found is omitted from the returned list. + */ RCLCPP_PUBLIC virtual - std::vector + std::vector get_parameters(const std::vector & names) const = 0; + /// Get the description of one parameter given a name. + /* + * \param[in] name the name of the parameter to look for. + * \return the parameter if it exists on the node. + * \throws std::out_of_range if the parameter does not exist on the node. + */ RCLCPP_PUBLIC virtual - rclcpp::parameter::ParameterVariant + rclcpp::Parameter get_parameter(const std::string & name) const = 0; + /// Get the description of one parameter given a name. + /* + * \param[in] name the name of the parameter to look for. + * \param[out] parameter the description if parameter exists on the node. + * \return true if the parameter exists on the node, or + * \return false if the parameter does not exist. + */ RCLCPP_PUBLIC virtual bool get_parameter( const std::string & name, - rclcpp::parameter::ParameterVariant & parameter) const = 0; + rclcpp::Parameter & parameter) const = 0; + + /// Get all parameters that have the specified prefix into the parameters map. + /* + * \param[in] prefix the name of the prefix to look for. + * \param[out] parameters a map of parameters that matched the prefix. + * \return true if any parameters with the prefix exists on the node, or + * \return false otherwise. + */ + RCLCPP_PUBLIC + virtual + bool + get_parameters_by_prefix( + const std::string & prefix, + std::map & parameters) const = 0; RCLCPP_PUBLIC virtual @@ -81,17 +206,75 @@ class NodeParametersInterface rcl_interfaces::msg::ListParametersResult list_parameters(const std::vector & prefixes, uint64_t depth) const = 0; - using ParametersCallbackFunction = std::function< - rcl_interfaces::msg::SetParametersResult( - const std::vector &)>; + using OnSetParametersCallbackType = OnSetParametersCallbackHandle::OnSetParametersCallbackType; + using PostSetParametersCallbackType = + PostSetParametersCallbackHandle::PostSetParametersCallbackType; + using PreSetParametersCallbackType = PreSetParametersCallbackHandle::PreSetParametersCallbackType; + /// Add a callback that gets triggered before parameters are validated. + /** + * \sa rclcpp::Node::add_pre_set_parameters_callback + */ + RCLCPP_PUBLIC + virtual + PreSetParametersCallbackHandle::SharedPtr + add_pre_set_parameters_callback(PreSetParametersCallbackType callback) = 0; + + /// Add a callback to validate parameters before they are set. + /** + * \sa rclcpp::Node::add_on_set_parameters_callback + */ + RCLCPP_PUBLIC + virtual + OnSetParametersCallbackHandle::SharedPtr + add_on_set_parameters_callback(OnSetParametersCallbackType callback) = 0; + + /// Add a callback that gets triggered after parameters are set successfully. + /** + * \sa rclcpp::Node::add_post_set_parameters_callback + */ + RCLCPP_PUBLIC + virtual + PostSetParametersCallbackHandle::SharedPtr + add_post_set_parameters_callback(PostSetParametersCallbackType callback) = 0; + + /// Remove a callback registered with `add_pre_set_parameters_callback`. + /** + * \sa rclcpp::Node::remove_pre_set_parameters_callback + */ + RCLCPP_PUBLIC + virtual + void + remove_pre_set_parameters_callback(const PreSetParametersCallbackHandle * const handler) = 0; + + /// Remove a callback registered with `add_on_set_parameters_callback`. + /** + * \sa rclcpp::Node::remove_on_set_parameters_callback + */ + RCLCPP_PUBLIC + virtual + void + remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const handler) = 0; + + /// Remove a callback registered with `add_post_set_parameters_callback`. + /** + * \sa rclcpp::Node::remove_post_set_parameters_callback + */ RCLCPP_PUBLIC virtual void - register_param_change_callback(ParametersCallbackFunction callback) = 0; + remove_post_set_parameters_callback(const PostSetParametersCallbackHandle * const handler) = 0; + + /// Return the initial parameter values used by the NodeParameters to override default values. + RCLCPP_PUBLIC + virtual + const std::map & + get_parameter_overrides() const = 0; }; } // namespace node_interfaces } // namespace rclcpp +RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeParametersInterface, parameters) + #endif // RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_INTERFACE_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_services.hpp b/rclcpp/include/rclcpp/node_interfaces/node_services.hpp index 0b0d99dceb..b538ab4ef2 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_services.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_services.hpp @@ -15,6 +15,8 @@ #ifndef RCLCPP__NODE_INTERFACES__NODE_SERVICES_HPP_ #define RCLCPP__NODE_INTERFACES__NODE_SERVICES_HPP_ +#include + #include "rclcpp/callback_group.hpp" #include "rclcpp/client.hpp" #include "rclcpp/macros.hpp" @@ -42,18 +44,20 @@ class NodeServices : public NodeServicesInterface ~NodeServices(); RCLCPP_PUBLIC - virtual void add_client( rclcpp::ClientBase::SharedPtr client_base_ptr, - rclcpp::callback_group::CallbackGroup::SharedPtr group); + rclcpp::CallbackGroup::SharedPtr group) override; RCLCPP_PUBLIC - virtual void add_service( rclcpp::ServiceBase::SharedPtr service_base_ptr, - rclcpp::callback_group::CallbackGroup::SharedPtr group); + rclcpp::CallbackGroup::SharedPtr group) override; + + RCLCPP_PUBLIC + std::string + resolve_service_name(const std::string & name, bool only_expand = false) const override; private: RCLCPP_DISABLE_COPY(NodeServices) diff --git a/rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp index 7f591cecae..ff58ef36b6 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp @@ -15,9 +15,12 @@ #ifndef RCLCPP__NODE_INTERFACES__NODE_SERVICES_INTERFACE_HPP_ #define RCLCPP__NODE_INTERFACES__NODE_SERVICES_INTERFACE_HPP_ +#include + #include "rclcpp/callback_group.hpp" #include "rclcpp/client.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp" #include "rclcpp/service.hpp" #include "rclcpp/visibility_control.hpp" @@ -32,22 +35,34 @@ class NodeServicesInterface public: RCLCPP_SMART_PTR_ALIASES_ONLY(NodeServicesInterface) + RCLCPP_PUBLIC + virtual + ~NodeServicesInterface() = default; + RCLCPP_PUBLIC virtual void add_client( rclcpp::ClientBase::SharedPtr client_base_ptr, - rclcpp::callback_group::CallbackGroup::SharedPtr group) = 0; + rclcpp::CallbackGroup::SharedPtr group) = 0; RCLCPP_PUBLIC virtual void add_service( rclcpp::ServiceBase::SharedPtr service_base_ptr, - rclcpp::callback_group::CallbackGroup::SharedPtr group) = 0; + rclcpp::CallbackGroup::SharedPtr group) = 0; + + /// Get the remapped and expanded service name given a input name. + RCLCPP_PUBLIC + virtual + std::string + resolve_service_name(const std::string & name, bool only_expand = false) const = 0; }; } // namespace node_interfaces } // namespace rclcpp +RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeServicesInterface, services) + #endif // RCLCPP__NODE_INTERFACES__NODE_SERVICES_INTERFACE_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_time_source.hpp b/rclcpp/include/rclcpp/node_interfaces/node_time_source.hpp new file mode 100644 index 0000000000..1594d5f8e5 --- /dev/null +++ b/rclcpp/include/rclcpp/node_interfaces/node_time_source.hpp @@ -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 RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_HPP_ + +#include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_clock_interface.hpp" +#include "rclcpp/node_interfaces/node_graph_interface.hpp" +#include "rclcpp/node_interfaces/node_logging_interface.hpp" +#include "rclcpp/node_interfaces/node_parameters_interface.hpp" +#include "rclcpp/node_interfaces/node_services_interface.hpp" +#include "rclcpp/node_interfaces/node_time_source_interface.hpp" +#include "rclcpp/node_interfaces/node_topics_interface.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/time_source.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace node_interfaces +{ + +/// Implementation of the NodeTimeSource part of the Node API. +class NodeTimeSource : public NodeTimeSourceInterface +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTimeSource) + + RCLCPP_PUBLIC + explicit NodeTimeSource( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics, + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock, + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters, + const rclcpp::QoS & qos = rclcpp::ClockQoS(), + bool use_clock_thread = true + ); + + RCLCPP_PUBLIC + virtual + ~NodeTimeSource(); + +private: + RCLCPP_DISABLE_COPY(NodeTimeSource) + + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_; + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_; + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_; + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_; + rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_; + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_; + + rclcpp::TimeSource time_source_; +}; + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_time_source_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_time_source_interface.hpp new file mode 100644 index 0000000000..3783e5d83a --- /dev/null +++ b/rclcpp/include/rclcpp/node_interfaces/node_time_source_interface.hpp @@ -0,0 +1,43 @@ +// 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 RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_INTERFACE_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_INTERFACE_HPP_ + +#include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace node_interfaces +{ + +/// Pure virtual interface class for the NodeTimeSource part of the Node API. +class NodeTimeSourceInterface +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTimeSourceInterface) + + RCLCPP_PUBLIC + virtual + ~NodeTimeSourceInterface() = default; +}; + +} // namespace node_interfaces +} // namespace rclcpp + +RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeTimeSourceInterface, time_source) + +#endif // RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_INTERFACE_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_timers.hpp b/rclcpp/include/rclcpp/node_interfaces/node_timers.hpp index 955e39cbff..9f26721d4b 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_timers.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_timers.hpp @@ -42,11 +42,10 @@ class NodeTimers : public NodeTimersInterface /// Add a timer to the node. RCLCPP_PUBLIC - virtual void add_timer( rclcpp::TimerBase::SharedPtr timer, - rclcpp::callback_group::CallbackGroup::SharedPtr callback_group); + rclcpp::CallbackGroup::SharedPtr callback_group) override; private: RCLCPP_DISABLE_COPY(NodeTimers) diff --git a/rclcpp/include/rclcpp/node_interfaces/node_timers_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_timers_interface.hpp index 09edbadbc1..2f1aaef51e 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_timers_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_timers_interface.hpp @@ -17,6 +17,7 @@ #include "rclcpp/callback_group.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp" #include "rclcpp/timer.hpp" #include "rclcpp/visibility_control.hpp" @@ -31,16 +32,22 @@ class NodeTimersInterface public: RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTimersInterface) + RCLCPP_PUBLIC + virtual + ~NodeTimersInterface() = default; + /// Add a timer to the node. RCLCPP_PUBLIC virtual void add_timer( rclcpp::TimerBase::SharedPtr timer, - rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) = 0; + rclcpp::CallbackGroup::SharedPtr callback_group) = 0; }; } // namespace node_interfaces } // namespace rclcpp +RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeTimersInterface, timers) + #endif // RCLCPP__NODE_INTERFACES__NODE_TIMERS_INTERFACE_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp b/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp index 31cf62e54c..33a87732d6 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp @@ -20,11 +20,16 @@ #include "rcl/publisher.h" #include "rcl/subscription.h" +#include "rclcpp/callback_group.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_timers_interface.hpp" #include "rclcpp/node_interfaces/node_topics_interface.hpp" -#include "rclcpp/publisher.hpp" +#include "rclcpp/publisher_base.hpp" #include "rclcpp/publisher_factory.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/subscription_base.hpp" +#include "rclcpp/subscription_factory.hpp" #include "rclcpp/visibility_control.hpp" namespace rclcpp @@ -39,47 +44,56 @@ class NodeTopics : public NodeTopicsInterface RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTopicsInterface) RCLCPP_PUBLIC - explicit NodeTopics(rclcpp::node_interfaces::NodeBaseInterface * node_base); + NodeTopics( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + rclcpp::node_interfaces::NodeTimersInterface * node_timers); RCLCPP_PUBLIC - virtual - ~NodeTopics(); + ~NodeTopics() override; RCLCPP_PUBLIC - virtual rclcpp::PublisherBase::SharedPtr create_publisher( const std::string & topic_name, const rclcpp::PublisherFactory & publisher_factory, - rcl_publisher_options_t & publisher_options, - bool use_intra_process); + const rclcpp::QoS & qos) override; RCLCPP_PUBLIC - virtual void add_publisher( - rclcpp::PublisherBase::SharedPtr publisher); + rclcpp::PublisherBase::SharedPtr publisher, + rclcpp::CallbackGroup::SharedPtr callback_group) override; RCLCPP_PUBLIC - virtual rclcpp::SubscriptionBase::SharedPtr create_subscription( const std::string & topic_name, const rclcpp::SubscriptionFactory & subscription_factory, - rcl_subscription_options_t & subscription_options, - bool use_intra_process); + const rclcpp::QoS & qos) override; RCLCPP_PUBLIC - virtual void add_subscription( rclcpp::SubscriptionBase::SharedPtr subscription, - rclcpp::callback_group::CallbackGroup::SharedPtr callback_group); + rclcpp::CallbackGroup::SharedPtr callback_group) override; + + RCLCPP_PUBLIC + rclcpp::node_interfaces::NodeBaseInterface * + get_node_base_interface() const override; + + RCLCPP_PUBLIC + rclcpp::node_interfaces::NodeTimersInterface * + get_node_timers_interface() const override; + + RCLCPP_PUBLIC + std::string + resolve_topic_name(const std::string & name, bool only_expand = false) const override; private: RCLCPP_DISABLE_COPY(NodeTopics) - NodeBaseInterface * node_base_; + rclcpp::node_interfaces::NodeBaseInterface * node_base_; + rclcpp::node_interfaces::NodeTimersInterface * node_timers_; }; } // namespace node_interfaces diff --git a/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp index 2a2e628f62..aa5530f8dd 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp @@ -15,14 +15,16 @@ #ifndef RCLCPP__NODE_INTERFACES__NODE_TOPICS_INTERFACE_HPP_ #define RCLCPP__NODE_INTERFACES__NODE_TOPICS_INTERFACE_HPP_ -#include -#include #include #include "rcl/publisher.h" #include "rcl/subscription.h" +#include "rclcpp/callback_group.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_timers_interface.hpp" #include "rclcpp/publisher.hpp" #include "rclcpp/publisher_factory.hpp" #include "rclcpp/subscription.hpp" @@ -40,20 +42,24 @@ class NodeTopicsInterface public: RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTopicsInterface) + RCLCPP_PUBLIC + virtual + ~NodeTopicsInterface() = default; + RCLCPP_PUBLIC virtual rclcpp::PublisherBase::SharedPtr create_publisher( const std::string & topic_name, const rclcpp::PublisherFactory & publisher_factory, - rcl_publisher_options_t & publisher_options, - bool use_intra_process) = 0; + const rclcpp::QoS & qos) = 0; RCLCPP_PUBLIC virtual void add_publisher( - rclcpp::PublisherBase::SharedPtr publisher) = 0; + rclcpp::PublisherBase::SharedPtr publisher, + rclcpp::CallbackGroup::SharedPtr callback_group) = 0; RCLCPP_PUBLIC virtual @@ -61,18 +67,35 @@ class NodeTopicsInterface create_subscription( const std::string & topic_name, const rclcpp::SubscriptionFactory & subscription_factory, - rcl_subscription_options_t & subscription_options, - bool use_intra_process) = 0; + const rclcpp::QoS & qos) = 0; RCLCPP_PUBLIC virtual void add_subscription( rclcpp::SubscriptionBase::SharedPtr subscription, - rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) = 0; + rclcpp::CallbackGroup::SharedPtr callback_group) = 0; + + RCLCPP_PUBLIC + virtual + rclcpp::node_interfaces::NodeBaseInterface * + get_node_base_interface() const = 0; + + RCLCPP_PUBLIC + virtual + rclcpp::node_interfaces::NodeTimersInterface * + get_node_timers_interface() const = 0; + + /// Get a remapped and expanded topic name given an input name. + RCLCPP_PUBLIC + virtual + std::string + resolve_topic_name(const std::string & name, bool only_expand = false) const = 0; }; } // namespace node_interfaces } // namespace rclcpp +RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeTopicsInterface, topics) + #endif // RCLCPP__NODE_INTERFACES__NODE_TOPICS_INTERFACE_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_type_descriptions.hpp b/rclcpp/include/rclcpp/node_interfaces/node_type_descriptions.hpp new file mode 100644 index 0000000000..8aa563bba2 --- /dev/null +++ b/rclcpp/include/rclcpp/node_interfaces/node_type_descriptions.hpp @@ -0,0 +1,63 @@ +// Copyright 2023 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 RCLCPP__NODE_INTERFACES__NODE_TYPE_DESCRIPTIONS_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_TYPE_DESCRIPTIONS_HPP_ + +#include + +#include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_logging_interface.hpp" +#include "rclcpp/node_interfaces/node_parameters_interface.hpp" +#include "rclcpp/node_interfaces/node_services_interface.hpp" +#include "rclcpp/node_interfaces/node_topics_interface.hpp" +#include "rclcpp/node_interfaces/node_type_descriptions_interface.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace node_interfaces +{ + +/// Implementation of the NodeTypeDescriptions part of the Node API. +class NodeTypeDescriptions : public NodeTypeDescriptionsInterface +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTypeDescriptions) + + RCLCPP_PUBLIC + explicit NodeTypeDescriptions( + NodeBaseInterface::SharedPtr node_base, + NodeLoggingInterface::SharedPtr node_logging, + NodeParametersInterface::SharedPtr node_parameters, + NodeServicesInterface::SharedPtr node_services); + + RCLCPP_PUBLIC + virtual + ~NodeTypeDescriptions(); + +private: + RCLCPP_DISABLE_COPY(NodeTypeDescriptions) + + // Pimpl hides helper types and functions used for wrapping a C service, which would be + // awkward to expose in this header. + class NodeTypeDescriptionsImpl; + std::unique_ptr impl_; +}; + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__NODE_TYPE_DESCRIPTIONS_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_type_descriptions_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_type_descriptions_interface.hpp new file mode 100644 index 0000000000..e7e0b0af2e --- /dev/null +++ b/rclcpp/include/rclcpp/node_interfaces/node_type_descriptions_interface.hpp @@ -0,0 +1,44 @@ +// Copyright 2023 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 RCLCPP__NODE_INTERFACES__NODE_TYPE_DESCRIPTIONS_INTERFACE_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_TYPE_DESCRIPTIONS_INTERFACE_HPP_ + +#include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace node_interfaces +{ + +/// Pure virtual interface class for the NodeTypeDescriptions part of the Node API. +class NodeTypeDescriptionsInterface +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(NodeTypeDescriptionsInterface) + + RCLCPP_PUBLIC + virtual + ~NodeTypeDescriptionsInterface() = default; +}; + +} // namespace node_interfaces +} // namespace rclcpp + +RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT( + rclcpp::node_interfaces::NodeTypeDescriptionsInterface, type_descriptions) + +#endif // RCLCPP__NODE_INTERFACES__NODE_TYPE_DESCRIPTIONS_INTERFACE_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_waitables.hpp b/rclcpp/include/rclcpp/node_interfaces/node_waitables.hpp new file mode 100644 index 0000000000..46c24d9aa1 --- /dev/null +++ b/rclcpp/include/rclcpp/node_interfaces/node_waitables.hpp @@ -0,0 +1,64 @@ +// 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 RCLCPP__NODE_INTERFACES__NODE_WAITABLES_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_WAITABLES_HPP_ + +#include "rclcpp/callback_group.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_waitables_interface.hpp" +#include "rclcpp/waitable.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace node_interfaces +{ + +/// Implementation of the NodeWaitables part of the Node API. +class NodeWaitables : public NodeWaitablesInterface +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(NodeWaitables) + + RCLCPP_PUBLIC + explicit NodeWaitables(rclcpp::node_interfaces::NodeBaseInterface * node_base); + + RCLCPP_PUBLIC + virtual + ~NodeWaitables(); + + RCLCPP_PUBLIC + void + add_waitable( + rclcpp::Waitable::SharedPtr waitable_base_ptr, + rclcpp::CallbackGroup::SharedPtr group) override; + + RCLCPP_PUBLIC + void + remove_waitable( + rclcpp::Waitable::SharedPtr waitable_ptr, + rclcpp::CallbackGroup::SharedPtr group) noexcept override; + +private: + RCLCPP_DISABLE_COPY(NodeWaitables) + + rclcpp::node_interfaces::NodeBaseInterface * node_base_; +}; + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__NODE_WAITABLES_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_waitables_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_waitables_interface.hpp new file mode 100644 index 0000000000..fd0029a89b --- /dev/null +++ b/rclcpp/include/rclcpp/node_interfaces/node_waitables_interface.hpp @@ -0,0 +1,60 @@ +// 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 RCLCPP__NODE_INTERFACES__NODE_WAITABLES_INTERFACE_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_WAITABLES_INTERFACE_HPP_ + +#include "rclcpp/callback_group.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp" +#include "rclcpp/visibility_control.hpp" +#include "rclcpp/waitable.hpp" + +namespace rclcpp +{ +namespace node_interfaces +{ + +/// Pure virtual interface class for the NodeWaitables part of the Node API. +class NodeWaitablesInterface +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(NodeWaitablesInterface) + + RCLCPP_PUBLIC + virtual + ~NodeWaitablesInterface() = default; + + RCLCPP_PUBLIC + virtual + void + add_waitable( + rclcpp::Waitable::SharedPtr waitable_ptr, + rclcpp::CallbackGroup::SharedPtr group) = 0; + + /// \note this function should not throw because it may be called in destructors + RCLCPP_PUBLIC + virtual + void + remove_waitable( + rclcpp::Waitable::SharedPtr waitable_ptr, + rclcpp::CallbackGroup::SharedPtr group) noexcept = 0; +}; + +} // namespace node_interfaces +} // namespace rclcpp + +RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeWaitablesInterface, waitables) + +#endif // RCLCPP__NODE_INTERFACES__NODE_WAITABLES_INTERFACE_HPP_ diff --git a/rclcpp/include/rclcpp/node_options.hpp b/rclcpp/include/rclcpp/node_options.hpp new file mode 100644 index 0000000000..71b0d997cf --- /dev/null +++ b/rclcpp/include/rclcpp/node_options.hpp @@ -0,0 +1,463 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__NODE_OPTIONS_HPP_ +#define RCLCPP__NODE_OPTIONS_HPP_ + +#include +#include +#include + +#include "rcl/time.h" +#include "rcl/node_options.h" +#include "rclcpp/context.hpp" +#include "rclcpp/contexts/default_context.hpp" +#include "rclcpp/parameter.hpp" +#include "rclcpp/publisher_options.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ + +/// Encapsulation of options for node initialization. +class NodeOptions +{ +public: + /// Create NodeOptions with default values, optionally specifying the allocator to use. + /** + * Default values for the node options: + * + * - context = rclcpp::contexts::get_global_default_context() + * - arguments = {} + * - parameter_overrides = {} + * - use_global_arguments = true + * - enable_rosout = true + * - use_intra_process_comms = false + * - enable_topic_statistics = false + * - start_parameter_services = true + * - start_parameter_event_publisher = true + * - clock_type = RCL_ROS_TIME + * - clock_qos = rclcpp::ClockQoS() + * - use_clock_thread = true + * - enable_logger_service = false + * - rosout_qos = rclcpp::RosoutQoS() + * - parameter_event_qos = rclcpp::ParameterEventQoS + * - with history setting and depth from rmw_qos_profile_parameter_events + * - parameter_event_publisher_options = rclcpp::PublisherOptionsBase + * - allow_undeclared_parameters = false + * - automatically_declare_parameters_from_overrides = false + * - allocator = rcl_get_default_allocator() + * + * \param[in] allocator allocator to use in construction of NodeOptions. + */ + RCLCPP_PUBLIC + explicit NodeOptions(rcl_allocator_t allocator = rcl_get_default_allocator()); + + /// Destructor. + RCLCPP_PUBLIC + virtual + ~NodeOptions() = default; + + /// Copy constructor. + RCLCPP_PUBLIC + NodeOptions(const NodeOptions & other); + + /// Assignment operator. + RCLCPP_PUBLIC + NodeOptions & + operator=(const NodeOptions & other); + + /// Return the rcl_node_options used by the node. + /** + * This data structure is created lazily, on the first call to this function. + * Repeated calls will not regenerate it unless one of the input settings + * changed, like arguments, use_global_arguments, or the rcl allocator. + * + * \return a const rcl_node_options_t structure used by the node + * \throws exceptions::UnknownROSArgsError if there are unknown ROS arguments + */ + RCLCPP_PUBLIC + const rcl_node_options_t * + get_rcl_node_options() const; + + /// Return the context to be used by the node. + RCLCPP_PUBLIC + rclcpp::Context::SharedPtr + context() const; + + /// Set the context, return this for parameter idiom. + RCLCPP_PUBLIC + NodeOptions & + context(rclcpp::Context::SharedPtr context); + + /// Return a reference to the list of arguments for the node. + RCLCPP_PUBLIC + const std::vector & + arguments() const; + + /// Set the arguments, return this for parameter idiom. + /** + * These arguments are used to extract remappings used by the node and other + * ROS specific settings, as well as user defined non-ROS arguments. + * + * This will cause the internal rcl_node_options_t struct to be invalidated. + */ + RCLCPP_PUBLIC + NodeOptions & + arguments(const std::vector & arguments); + + /// Return a reference to the list of parameter overrides. + RCLCPP_PUBLIC + std::vector & + parameter_overrides(); + + RCLCPP_PUBLIC + const std::vector & + parameter_overrides() const; + + /// Set the parameters overrides, return this for parameter idiom. + /** + * These parameter overrides are used to change the initial value + * of declared parameters within the node, overriding hard coded default + * values if necessary. + */ + RCLCPP_PUBLIC + NodeOptions & + parameter_overrides(const std::vector & parameter_overrides); + + /// Append a single parameter override, parameter idiom style. + template + NodeOptions & + append_parameter_override(const std::string & name, const ParameterT & value) + { + this->parameter_overrides().emplace_back(name, rclcpp::ParameterValue(value)); + return *this; + } + + /// Return the use_global_arguments flag. + RCLCPP_PUBLIC + bool + use_global_arguments() const; + + /// Set the use_global_arguments flag, return this for parameter idiom. + /** + * If true this will cause the node's behavior to be influenced by "global" + * arguments, i.e. arguments not targeted at specific nodes, as well as the + * arguments targeted at the current node. + * + * This will cause the internal rcl_node_options_t struct to be invalidated. + */ + RCLCPP_PUBLIC + NodeOptions & + use_global_arguments(bool use_global_arguments); + + /// Return the enable_rosout flag. + RCLCPP_PUBLIC + bool + enable_rosout() const; + + /// Set the enable_rosout flag, return this for parameter idiom. + /** + * If false this will cause the node not to use rosout logging. + * + * Defaults to true for now, as there are still some cases where it is + * desirable. + */ + RCLCPP_PUBLIC + NodeOptions & + enable_rosout(bool enable_rosout); + + /// Return the use_intra_process_comms flag. + RCLCPP_PUBLIC + bool + use_intra_process_comms() const; + + /// Set the use_intra_process_comms flag, return this for parameter idiom. + /** + * If true, messages on topics which are published and subscribed to within + * this context will go through a special intra-process communication code + * code path which can avoid serialization and deserialization, unnecessary + * copies, and achieve lower latencies in some cases. + * + * Defaults to false for now, as there are still some cases where it is not + * desirable. + */ + RCLCPP_PUBLIC + NodeOptions & + use_intra_process_comms(bool use_intra_process_comms); + + /// Return the enable_topic_statistics flag. + RCLCPP_PUBLIC + bool + enable_topic_statistics() const; + + /// Set the enable_topic_statistics flag, return this for parameter idiom. + /** + * If true, topic statistics collection and publication will be enabled + * for all subscriptions. + * This can be used to override the global topic statistics setting. + * + * Defaults to false. + */ + RCLCPP_PUBLIC + NodeOptions & + enable_topic_statistics(bool enable_topic_statistics); + + /// Return the start_parameter_services flag. + RCLCPP_PUBLIC + bool + start_parameter_services() const; + + /// Set the start_parameter_services flag, return this for parameter idiom. + /** + * If true, ROS services are created to allow external nodes to list, get, + * and request to set parameters of this node. + * + * If false, parameters will still work locally, but will not be accessible + * remotely. + * + * \sa start_parameter_event_publisher() + */ + RCLCPP_PUBLIC + NodeOptions & + start_parameter_services(bool start_parameter_services); + + /// Return the enable_logger_service flag. + RCLCPP_PUBLIC + bool + enable_logger_service() const; + + /// Set the enable_logger_service flag, return this for logger idiom. + /** + * If true, ROS services are created to allow external nodes to get + * and set logger levels of this node. + * + * If false, loggers will still be configured and set logger levels locally, + * but logger levels cannot be changed remotely . + * + */ + RCLCPP_PUBLIC + NodeOptions & + enable_logger_service(bool enable_log_service); + + /// Return the start_parameter_event_publisher flag. + RCLCPP_PUBLIC + bool + start_parameter_event_publisher() const; + + /// Set the start_parameter_event_publisher flag, return this for parameter idiom. + /** + * If true, a publisher is created on which an event message is published + * each time a parameter's state changes. + * This is used for recording and introspection, but is configurable + * separately from the other parameter services. + */ + RCLCPP_PUBLIC + NodeOptions & + start_parameter_event_publisher(bool start_parameter_event_publisher); + + /// Return a reference to the clock type. + RCLCPP_PUBLIC + const rcl_clock_type_t & + clock_type() const; + + /// Set the clock type. + /** + * The clock type to be used by the node. + */ + RCLCPP_PUBLIC + NodeOptions & + clock_type(const rcl_clock_type_t & clock_type); + + /// Return a reference to the clock QoS. + RCLCPP_PUBLIC + const rclcpp::QoS & + clock_qos() const; + + /// Set the clock QoS. + /** + * The QoS settings to be used for the publisher on /clock topic, if enabled. + */ + RCLCPP_PUBLIC + NodeOptions & + clock_qos(const rclcpp::QoS & clock_qos); + + + /// Return the use_clock_thread flag. + RCLCPP_PUBLIC + bool + use_clock_thread() const; + + /// Set the use_clock_thread flag, return this for parameter idiom. + /** + * If true, a dedicated thread will be used to subscribe to "/clock" topic. + */ + RCLCPP_PUBLIC + NodeOptions & + use_clock_thread(bool use_clock_thread); + + /// Return a reference to the parameter_event_qos QoS. + RCLCPP_PUBLIC + const rclcpp::QoS & + parameter_event_qos() const; + + /// Set the parameter_event_qos QoS, return this for parameter idiom. + /** + * The QoS settings to be used for the parameter event publisher, if enabled. + */ + RCLCPP_PUBLIC + NodeOptions & + parameter_event_qos(const rclcpp::QoS & parameter_event_qos); + + /// Return a reference to the rosout QoS. + RCLCPP_PUBLIC + const rclcpp::QoS & + rosout_qos() const; + + /// Set the rosout QoS. + /** + * The QoS settings to be used for the publisher on /rosout topic, if enabled. + */ + RCLCPP_PUBLIC + NodeOptions & + rosout_qos(const rclcpp::QoS & rosout_qos); + + /// Return a reference to the parameter_event_publisher_options. + RCLCPP_PUBLIC + const rclcpp::PublisherOptionsBase & + parameter_event_publisher_options() const; + + /// Set the parameter_event_publisher_options, return this for parameter idiom. + /** + * The QoS settings to be used for the parameter event publisher, if enabled. + * + * \todo(wjwwood): make this take/store an instance of + * rclcpp::PublisherOptionsWithAllocator, but to do that requires + * NodeOptions to also be templated based on the Allocator type. + */ + RCLCPP_PUBLIC + NodeOptions & + parameter_event_publisher_options( + const rclcpp::PublisherOptionsBase & parameter_event_publisher_options); + + /// Return the allow_undeclared_parameters flag. + RCLCPP_PUBLIC + bool + allow_undeclared_parameters() const; + + /// Set the allow_undeclared_parameters, return this for parameter idiom. + /** + * If true, allow any parameter name to be set on the node without first + * being declared. + * Otherwise, setting an undeclared parameter will raise an exception. + * + * This option being true does not affect parameter_overrides, as the first + * set action will implicitly declare the parameter and therefore consider + * any parameter overrides. + */ + RCLCPP_PUBLIC + NodeOptions & + allow_undeclared_parameters(bool allow_undeclared_parameters); + + /// Return the automatically_declare_parameters_from_overrides flag. + RCLCPP_PUBLIC + bool + automatically_declare_parameters_from_overrides() const; + + /// Set the automatically_declare_parameters_from_overrides, return this. + /** + * If true, automatically iterate through the node's parameter overrides and + * implicitly declare any that have not already been declared. + * Otherwise, parameters passed to the node's parameter_overrides, and/or the + * global arguments (e.g. parameter overrides from a YAML file), which are + * not explicitly declared will not appear on the node at all, even if + * `allow_undeclared_parameters` is true. + * Parameter declaration from overrides is done in the node's base constructor, + * so the user must take care to check if the parameter is already (e.g. + * automatically) declared before declaring it themselves. + * Already declared parameters will not be re-declared, and parameters + * declared in this way will use the default constructed ParameterDescriptor. + */ + RCLCPP_PUBLIC + NodeOptions & + automatically_declare_parameters_from_overrides( + bool automatically_declare_parameters_from_overrides); + + /// Return the rcl_allocator_t to be used. + RCLCPP_PUBLIC + const rcl_allocator_t & + allocator() const; + + /// Set the rcl_allocator_t to be used, may cause deallocation of existing rcl_node_options_t. + /** + * This will cause the internal rcl_node_options_t struct to be invalidated. + */ + RCLCPP_PUBLIC + NodeOptions & + allocator(rcl_allocator_t allocator); + +private: + // This is mutable to allow for a const accessor which lazily creates the node options instance. + /// Underlying rcl_node_options structure. + mutable std::unique_ptr node_options_; + + // IMPORTANT: if any of these default values are changed, please update the + // documentation in this class. + + rclcpp::Context::SharedPtr context_ { + rclcpp::contexts::get_global_default_context()}; + + std::vector arguments_ {}; + + std::vector parameter_overrides_ {}; + + bool use_global_arguments_ {true}; + + bool enable_rosout_ {true}; + + bool use_intra_process_comms_ {false}; + + bool enable_topic_statistics_ {false}; + + bool start_parameter_services_ {true}; + + bool start_parameter_event_publisher_ {true}; + + rcl_clock_type_t clock_type_ {RCL_ROS_TIME}; + + rclcpp::QoS clock_qos_ = rclcpp::ClockQoS(); + + bool use_clock_thread_ {true}; + + bool enable_logger_service_ {false}; + + rclcpp::QoS parameter_event_qos_ = rclcpp::ParameterEventsQoS( + rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_parameter_events) + ); + + rclcpp::QoS rosout_qos_ = rclcpp::RosoutQoS(); + + rclcpp::PublisherOptionsBase parameter_event_publisher_options_ = rclcpp::PublisherOptionsBase(); + + bool allow_undeclared_parameters_ {false}; + + bool automatically_declare_parameters_from_overrides_ {false}; + + rcl_allocator_t allocator_ {rcl_get_default_allocator()}; +}; + +} // namespace rclcpp + +#endif // RCLCPP__NODE_OPTIONS_HPP_ diff --git a/rclcpp/include/rclcpp/parameter.hpp b/rclcpp/include/rclcpp/parameter.hpp index ce8de2e80f..85f088d025 100644 --- a/rclcpp/include/rclcpp/parameter.hpp +++ b/rclcpp/include/rclcpp/parameter.hpp @@ -15,223 +15,269 @@ #ifndef RCLCPP__PARAMETER_HPP_ #define RCLCPP__PARAMETER_HPP_ +#include #include #include #include #include #include "rcl_interfaces/msg/parameter.hpp" -#include "rcl_interfaces/msg/parameter_type.hpp" -#include "rcl_interfaces/msg/parameter_value.hpp" +#include "rclcpp/exceptions.hpp" +#include "rclcpp/parameter_value.hpp" #include "rclcpp/visibility_control.hpp" -#include "rmw/rmw.h" namespace rclcpp { -namespace parameter + +class Parameter; + +namespace node_interfaces { +struct ParameterInfo; +} // namespace node_interfaces -enum ParameterType +namespace detail { - PARAMETER_NOT_SET = rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET, - PARAMETER_BOOL = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL, - PARAMETER_INTEGER = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER, - PARAMETER_DOUBLE = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE, - PARAMETER_STRING = rcl_interfaces::msg::ParameterType::PARAMETER_STRING, - PARAMETER_BYTE_ARRAY = rcl_interfaces::msg::ParameterType::PARAMETER_BYTE_ARRAY, -}; -// Structure to store an arbitrary parameter with templated get/set methods -class ParameterVariant +// This helper function is required because you cannot do specialization on a +// class method, so instead we specialize this template function and call it +// from the unspecialized, but dependent, class method. +template +auto +get_value_helper(const rclcpp::Parameter * parameter); + +} // namespace detail + +/// Structure to store an arbitrary parameter with templated get/set methods. +class Parameter { public: + /// Construct with an empty name and a parameter value of type rclcpp::PARAMETER_NOT_SET. RCLCPP_PUBLIC - ParameterVariant(); - RCLCPP_PUBLIC - explicit ParameterVariant(const std::string & name, const bool bool_value); - RCLCPP_PUBLIC - explicit ParameterVariant(const std::string & name, const int int_value); - RCLCPP_PUBLIC - explicit ParameterVariant(const std::string & name, const int64_t int_value); + Parameter(); + + /// Construct with given name and a parameter value of type rclcpp::PARAMETER_NOT_SET. RCLCPP_PUBLIC - explicit ParameterVariant(const std::string & name, const float double_value); + explicit Parameter(const std::string & name); + + /// Construct with given name and given parameter value. RCLCPP_PUBLIC - explicit ParameterVariant(const std::string & name, const double double_value); + Parameter(const std::string & name, const ParameterValue & value); + + /// Construct with given name and given parameter value. + template + Parameter(const std::string & name, ValueTypeT value) + : Parameter(name, ParameterValue(value)) + {} + RCLCPP_PUBLIC - explicit ParameterVariant(const std::string & name, const std::string & string_value); + explicit Parameter(const rclcpp::node_interfaces::ParameterInfo & parameter_info); + + /// Equal operator. RCLCPP_PUBLIC - explicit ParameterVariant(const std::string & name, const char * string_value); + bool + operator==(const Parameter & rhs) const; + + /// Not equal operator. RCLCPP_PUBLIC - explicit ParameterVariant( - const std::string & name, - const std::vector & byte_array_value); + bool + operator!=(const Parameter & rhs) const; + /// Get the type of the parameter RCLCPP_PUBLIC ParameterType get_type() const; + /// Get the type name of the parameter RCLCPP_PUBLIC std::string get_type_name() const; + /// Get the name of the parameter RCLCPP_PUBLIC const std::string & get_name() const; + /// Get value of parameter as a parameter message. RCLCPP_PUBLIC rcl_interfaces::msg::ParameterValue - get_parameter_value() const; - - // The following get_value() variants require the use of ParameterType - - template - typename std::enable_if::type - get_value() const - { - if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER) { - // TODO(wjwwood): use custom exception - throw std::runtime_error("Invalid type"); - } - return value_.integer_value; - } - - template - typename std::enable_if::type - get_value() const - { - if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE) { - // TODO(wjwwood): use custom exception - throw std::runtime_error("Invalid type"); - } - return value_.double_value; - } - - template - typename std::enable_if::type - get_value() const - { - if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_STRING) { - // TODO(wjwwood): use custom exception - throw std::runtime_error("Invalid type"); - } - return value_.string_value; - } - - template - typename std::enable_if::type - get_value() const - { - if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_BOOL) { - // TODO(wjwwood): use custom exception - throw std::runtime_error("Invalid type"); - } - return value_.bool_value; - } - - template - typename std::enable_if< - type == ParameterType::PARAMETER_BYTE_ARRAY, const std::vector &>::type - get_value() const - { - if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_BYTE_ARRAY) { - // TODO(wjwwood): use custom exception - throw std::runtime_error("Invalid type"); - } - return value_.byte_array_value; - } + get_value_message() const; - // The following get_value() variants allow the use of primitive types - - template - typename std::enable_if< - std::is_integral::value && !std::is_same::value, int64_t>::type - get_value() const - { - return get_value(); - } + /// Get the internal storage for the parameter value. + RCLCPP_PUBLIC + const rclcpp::ParameterValue & + get_parameter_value() const; - template - typename std::enable_if::value, double>::type + /// Get value of parameter using rclcpp::ParameterType as template argument. + /** + * \throws rclcpp::exceptions::InvalidParameterTypeException if the type doesn't match + */ + template + decltype(auto) get_value() const { - return get_value(); + return value_.get(); } - template - typename std::enable_if::value, const std::string &>::type - get_value() const - { - return get_value(); - } + /// Get value of parameter using c++ types as template argument. + template + decltype(auto) + get_value() const; - template - typename std::enable_if::value, bool>::type - get_value() const - { - return get_value(); - } - - template - typename std::enable_if< - std::is_convertible< - type, const std::vector &>::value, const std::vector &>::type - get_value() const - { - return get_value(); - } + /// Get value of parameter as boolean. + /** + * \throws rclcpp::ParameterTypeException if the type doesn't match + */ + RCLCPP_PUBLIC + bool + as_bool() const; + /// Get value of parameter as integer. + /** + * \throws rclcpp::ParameterTypeException if the type doesn't match + */ RCLCPP_PUBLIC int64_t as_int() const; + /// Get value of parameter as double. + /** + * \throws rclcpp::ParameterTypeException if the type doesn't match + */ RCLCPP_PUBLIC double as_double() const; + /// Get value of parameter as string. + /** + * \throws rclcpp::ParameterTypeException if the type doesn't match + */ RCLCPP_PUBLIC const std::string & as_string() const; + /// Get value of parameter as byte array (vector). + /** + * \throws rclcpp::ParameterTypeException if the type doesn't match + */ RCLCPP_PUBLIC - bool - as_bool() const; + const std::vector & + as_byte_array() const; + /// Get value of parameter as bool array (vector). + /** + * \throws rclcpp::ParameterTypeException if the type doesn't match + */ RCLCPP_PUBLIC - const std::vector & - as_bytes() const; + const std::vector & + as_bool_array() const; + + /// Get value of parameter as integer array (vector). + /** + * \throws rclcpp::ParameterTypeException if the type doesn't match + */ + RCLCPP_PUBLIC + const std::vector & + as_integer_array() const; + + /// Get value of parameter as double array (vector). + /** + * \throws rclcpp::ParameterTypeException if the type doesn't match + */ + RCLCPP_PUBLIC + const std::vector & + as_double_array() const; + + /// Get value of parameter as string array (vector). + /** + * \throws rclcpp::ParameterTypeException if the type doesn't match + */ + RCLCPP_PUBLIC + const std::vector & + as_string_array() const; + /// Convert a parameter message in a Parameter class object. RCLCPP_PUBLIC - static ParameterVariant - from_parameter(const rcl_interfaces::msg::Parameter & parameter); + static Parameter + from_parameter_msg(const rcl_interfaces::msg::Parameter & parameter); + /// Convert the class in a parameter message. RCLCPP_PUBLIC rcl_interfaces::msg::Parameter - to_parameter(); + to_parameter_msg() const; + /// Get value of parameter as a string. RCLCPP_PUBLIC std::string value_to_string() const; private: std::string name_; - rcl_interfaces::msg::ParameterValue value_; + ParameterValue value_; }; - /// Return a json encoded version of the parameter intended for a dict. RCLCPP_PUBLIC std::string -_to_json_dict_entry(const ParameterVariant & param); +_to_json_dict_entry(const Parameter & param); RCLCPP_PUBLIC std::ostream & -operator<<(std::ostream & os, const rclcpp::parameter::ParameterVariant & pv); +operator<<(std::ostream & os, const rclcpp::Parameter & pv); RCLCPP_PUBLIC std::ostream & -operator<<(std::ostream & os, const std::vector & parameters); +operator<<(std::ostream & os, const std::vector & parameters); + +namespace detail +{ + +template +auto +get_value_helper(const rclcpp::Parameter * parameter) +{ + return parameter->get_parameter_value().get(); +} + +// Specialization allowing Parameter::get() to return a const ref to the parameter value object. +template<> +inline +auto +get_value_helper(const rclcpp::Parameter * parameter) +{ + return parameter->get_parameter_value(); +} + +// Specialization allowing Parameter::get() to return a const ref to the parameter itself. +template<> +inline +auto +get_value_helper(const rclcpp::Parameter * parameter) +{ + // Use this lambda to ensure it's a const reference being returned (and not a copy). + auto type_enforcing_lambda = + [¶meter]() -> const rclcpp::Parameter & { + return *parameter; + }; + return type_enforcing_lambda(); +} + +} // namespace detail + +/// \cond +template +decltype(auto) +Parameter::get_value() const +{ + try { + // use the helper to specialize for the ParameterValue and Parameter cases. + return detail::get_value_helper(this); + } catch (const ParameterTypeException & ex) { + throw exceptions::InvalidParameterTypeException(this->name_, ex.what()); + } +} +/// \endcond -} // namespace parameter } // namespace rclcpp namespace std @@ -240,12 +286,12 @@ namespace std /// Return a json encoded version of the parameter intended for a list. RCLCPP_PUBLIC std::string -to_string(const rclcpp::parameter::ParameterVariant & param); +to_string(const rclcpp::Parameter & param); /// Return a json encoded version of a vector of parameters, as a string. RCLCPP_PUBLIC std::string -to_string(const std::vector & parameters); +to_string(const std::vector & parameters); } // namespace std diff --git a/rclcpp/include/rclcpp/parameter_client.hpp b/rclcpp/include/rclcpp/parameter_client.hpp index 2e36aba87d..67fbae5054 100644 --- a/rclcpp/include/rclcpp/parameter_client.hpp +++ b/rclcpp/include/rclcpp/parameter_client.hpp @@ -15,6 +15,8 @@ #ifndef RCLCPP__PARAMETER_CLIENT_HPP_ #define RCLCPP__PARAMETER_CLIENT_HPP_ +#include +#include #include #include #include @@ -29,11 +31,15 @@ #include "rcl_interfaces/srv/list_parameters.hpp" #include "rcl_interfaces/srv/set_parameters.hpp" #include "rcl_interfaces/srv/set_parameters_atomically.hpp" +#include "rcl_yaml_param_parser/parser.h" +#include "rclcpp/exceptions.hpp" #include "rclcpp/executors.hpp" #include "rclcpp/create_subscription.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/node.hpp" #include "rclcpp/parameter.hpp" +#include "rclcpp/parameter_map.hpp" +#include "rclcpp/qos.hpp" #include "rclcpp/type_support_decl.hpp" #include "rclcpp/visibility_control.hpp" #include "rmw/rmw.h" @@ -46,6 +52,16 @@ class AsyncParametersClient public: RCLCPP_SMART_PTR_DEFINITIONS(AsyncParametersClient) + /// Create an async parameters client. + /** + * \param[in] node_base_interface The node base interface of the corresponding node. + * \param[in] node_topics_interface Node topic base interface. + * \param[in] node_graph_interface The node graph interface of the corresponding node. + * \param[in] node_services_interface Node service interface. + * \param[in] remote_node_name (optional) name of the remote node + * \param[in] qos_profile (optional) The qos profile to use to subscribe + * \param[in] group (optional) The async parameter client will be added to this callback group. + */ RCLCPP_PUBLIC AsyncParametersClient( const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, @@ -53,40 +69,81 @@ class AsyncParametersClient const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface, const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface, const std::string & remote_node_name = "", - const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters); - - RCLCPP_PUBLIC - AsyncParametersClient( - const rclcpp::Node::SharedPtr node, + const rclcpp::QoS & qos_profile = rclcpp::ParametersQoS(), + rclcpp::CallbackGroup::SharedPtr group = nullptr); + + /** + * \param[in] node The async parameters client will be added to this node. + * \param[in] remote_node_name (optional) name of the remote node + * \param[in] qos_profile (optional) The qos profile to use to subscribe + * \param[in] group (optional) The async parameter client will be added to this callback group. + */ + template + explicit AsyncParametersClient( + const std::shared_ptr node, const std::string & remote_node_name = "", - const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters); - - RCLCPP_PUBLIC - AsyncParametersClient( - rclcpp::Node * node, + const rclcpp::QoS & qos_profile = rclcpp::ParametersQoS(), + rclcpp::CallbackGroup::SharedPtr group = nullptr) + : AsyncParametersClient( + node->get_node_base_interface(), + node->get_node_topics_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface(), + remote_node_name, + qos_profile, + group) + {} + + /** + * \param[in] node The async parameters client will be added to this node. + * \param[in] remote_node_name (optional) name of the remote node + * \param[in] qos_profile (optional) The qos profile to use to subscribe + * \param[in] group (optional) The async parameter client will be added to this callback group. + */ + template + explicit AsyncParametersClient( + NodeT * node, const std::string & remote_node_name = "", - const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters); + const rclcpp::QoS & qos_profile = rclcpp::ParametersQoS(), + rclcpp::CallbackGroup::SharedPtr group = nullptr) + : AsyncParametersClient( + node->get_node_base_interface(), + node->get_node_topics_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface(), + remote_node_name, + qos_profile, + group) + {} RCLCPP_PUBLIC - std::shared_future> + std::shared_future> get_parameters( const std::vector & names, std::function< - void(std::shared_future>) + void(std::shared_future>) > callback = nullptr); RCLCPP_PUBLIC - std::shared_future> + std::shared_future> + describe_parameters( + const std::vector & names, + std::function< + void(std::shared_future>) + > callback = nullptr); + + RCLCPP_PUBLIC + std::shared_future> get_parameter_types( const std::vector & names, std::function< - void(std::shared_future>) + void(std::shared_future>) > callback = nullptr); RCLCPP_PUBLIC std::shared_future> set_parameters( - const std::vector & parameters, + const std::vector & parameters, std::function< void(std::shared_future>) > callback = nullptr); @@ -94,11 +151,50 @@ class AsyncParametersClient RCLCPP_PUBLIC std::shared_future set_parameters_atomically( - const std::vector & parameters, + const std::vector & parameters, std::function< void(std::shared_future) > callback = nullptr); + /// Delete several parameters at once. + /** + * This function behaves like command-line tool `ros2 param delete` would. + * + * \param parameters_names vector of parameters names + * \return the future of the set_parameter service used to delete the parameters + */ + RCLCPP_PUBLIC + std::shared_future> + delete_parameters( + const std::vector & parameters_names); + + /// Load parameters from yaml file. + /** + * This function behaves like command-line tool `ros2 param load` would. + * + * \param yaml_filename the full name of the yaml file + * \return the future of the set_parameter service used to load the parameters + */ + RCLCPP_PUBLIC + std::shared_future> + load_parameters( + const std::string & yaml_filename); + + /// Load parameters from parameter map. + /** + * This function filters the parameters to be set based on the node name. + * + * If two duplicate keys exist in node names belongs to one FQN, there is no guarantee + * which one could be set. + * + * \param parameter_map named parameters to be loaded + * \return the future of the set_parameter service used to load the parameters + * \throw InvalidParametersException if there is no parameter to set + */ + RCLCPP_PUBLIC + std::shared_future> + load_parameters(const rclcpp::ParameterMap & parameter_map); + RCLCPP_PUBLIC std::shared_future list_parameters( @@ -110,37 +206,72 @@ class AsyncParametersClient template< typename CallbackT, - typename Alloc = std::allocator, - typename SubscriptionT = - rclcpp::Subscription> + typename AllocatorT = std::allocator> typename rclcpp::Subscription::SharedPtr - on_parameter_event(CallbackT && callback) + on_parameter_event( + CallbackT && callback, + const rclcpp::QoS & qos = rclcpp::ParameterEventsQoS(), + const rclcpp::SubscriptionOptionsWithAllocator & options = ( + rclcpp::SubscriptionOptionsWithAllocator() + )) { - using rclcpp::message_memory_strategy::MessageMemoryStrategy; - auto msg_mem_strat = - MessageMemoryStrategy::create_default(); - - return rclcpp::create_subscription< - rcl_interfaces::msg::ParameterEvent, CallbackT, Alloc, SubscriptionT>( - this->node_topics_interface_.get(), - "parameter_events", + return this->on_parameter_event( + this->node_topics_interface_, + callback, + qos, + options); + } + + /** + * The NodeT type only needs to have a method called get_node_topics_interface() + * which returns a shared_ptr to a NodeTopicsInterface, or be a + * NodeTopicsInterface pointer itself. + */ + template< + typename CallbackT, + typename NodeT, + typename AllocatorT = std::allocator> + static typename rclcpp::Subscription::SharedPtr + on_parameter_event( + NodeT && node, + CallbackT && callback, + const rclcpp::QoS & qos = rclcpp::ParameterEventsQoS(), + const rclcpp::SubscriptionOptionsWithAllocator & options = ( + rclcpp::SubscriptionOptionsWithAllocator() + )) + { + return rclcpp::create_subscription( + node, + "/parameter_events", + qos, std::forward(callback), - rmw_qos_profile_default, - nullptr, // group, - false, // ignore_local_publications, - false, // use_intra_process_comms_, - msg_mem_strat, - std::make_shared()); + options); } + /// Return if the parameter services are ready. + /** + * This method checks the following services: + * - get parameter + * - get parameter + * - set parameters + * - list parameters + * - describe parameters + * + * \return `true` if the service is ready, `false` otherwise + */ RCLCPP_PUBLIC bool service_is_ready() const; - template + /// Wait for the services to be ready. + /** + * \param timeout maximum time to wait + * \return `true` if the services are ready and the timeout is not over, `false` otherwise + */ + template bool wait_for_service( - std::chrono::duration timeout = std::chrono::duration(-1)) + std::chrono::duration timeout = std::chrono::duration(-1)) { return wait_for_service_nanoseconds( std::chrono::duration_cast(timeout) @@ -153,7 +284,7 @@ class AsyncParametersClient wait_for_service_nanoseconds(std::chrono::nanoseconds timeout); private: - const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface_; + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface_; rclcpp::Client::SharedPtr get_parameters_client_; rclcpp::Client::SharedPtr get_parameter_types_client_; @@ -171,22 +302,94 @@ class SyncParametersClient public: RCLCPP_SMART_PTR_DEFINITIONS(SyncParametersClient) - RCLCPP_PUBLIC + template explicit SyncParametersClient( - rclcpp::Node::SharedPtr node, + std::shared_ptr node, + const std::string & remote_node_name = "", + const rclcpp::QoS & qos_profile = rclcpp::ParametersQoS()) + : SyncParametersClient( + std::make_shared(), + node, + remote_node_name, + qos_profile) + {} + + template + SyncParametersClient( + rclcpp::Executor::SharedPtr executor, + std::shared_ptr node, + const std::string & remote_node_name = "", + const rclcpp::QoS & qos_profile = rclcpp::ParametersQoS()) + : SyncParametersClient( + executor, + node->get_node_base_interface(), + node->get_node_topics_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface(), + remote_node_name, + qos_profile) + {} + + template + explicit SyncParametersClient( + NodeT * node, + const std::string & remote_node_name = "", + const rclcpp::QoS & qos_profile = rclcpp::ParametersQoS()) + : SyncParametersClient( + std::make_shared(), + node, + remote_node_name, + qos_profile) + {} + + template + SyncParametersClient( + rclcpp::Executor::SharedPtr executor, + NodeT * node, const std::string & remote_node_name = "", - const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters); + const rclcpp::QoS & qos_profile = rclcpp::ParametersQoS()) + : SyncParametersClient( + executor, + node->get_node_base_interface(), + node->get_node_topics_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface(), + remote_node_name, + qos_profile) + {} RCLCPP_PUBLIC SyncParametersClient( - rclcpp::executor::Executor::SharedPtr executor, - rclcpp::Node::SharedPtr node, + rclcpp::Executor::SharedPtr executor, + const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, + const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface, + const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface, + const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface, const std::string & remote_node_name = "", - const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters); + const rclcpp::QoS & qos_profile = rclcpp::ParametersQoS()) + : executor_(executor), node_base_interface_(node_base_interface) + { + async_parameters_client_ = + std::make_shared( + node_base_interface, + node_topics_interface, + node_graph_interface, + node_services_interface, + remote_node_name, + qos_profile); + } - RCLCPP_PUBLIC - std::vector - get_parameters(const std::vector & parameter_names); + template + std::vector + get_parameters( + const std::vector & parameter_names, + std::chrono::duration timeout = std::chrono::duration(-1)) + { + return get_parameters( + parameter_names, + std::chrono::duration_cast(timeout) + ); + } RCLCPP_PUBLIC bool @@ -200,7 +403,7 @@ class SyncParametersClient std::vector names; names.push_back(parameter_name); auto vars = get_parameters(names); - if ((vars.size() != 1) || (vars[0].get_type() == rclcpp::parameter::PARAMETER_NOT_SET)) { + if ((vars.size() != 1) || (vars[0].get_type() == rclcpp::ParameterType::PARAMETER_NOT_SET)) { return parameter_not_found_handler(); } else { return static_cast(vars[0].get_value()); @@ -222,32 +425,140 @@ class SyncParametersClient { return get_parameter_impl( parameter_name, - std::function([]() -> T {throw std::runtime_error("Parameter not set");})); + std::function( + [¶meter_name]() -> T + { + throw std::runtime_error("Parameter '" + parameter_name + "' is not set"); + }) + ); } - RCLCPP_PUBLIC - std::vector - get_parameter_types(const std::vector & parameter_names); + template + std::vector + describe_parameters( + const std::vector & parameter_names, + std::chrono::duration timeout = std::chrono::duration(-1)) + { + return describe_parameters( + parameter_names, + std::chrono::duration_cast(timeout) + ); + } - RCLCPP_PUBLIC + template + std::vector + get_parameter_types( + const std::vector & parameter_names, + std::chrono::duration timeout = std::chrono::duration(-1)) + { + return get_parameter_types( + parameter_names, + std::chrono::duration_cast(timeout) + ); + } + + template std::vector - set_parameters(const std::vector & parameters); + set_parameters( + const std::vector & parameters, + std::chrono::duration timeout = std::chrono::duration(-1)) + { + return set_parameters( + parameters, + std::chrono::duration_cast(timeout) + ); + } - RCLCPP_PUBLIC + template rcl_interfaces::msg::SetParametersResult - set_parameters_atomically(const std::vector & parameters); + set_parameters_atomically( + const std::vector & parameters, + std::chrono::duration timeout = std::chrono::duration(-1)) + { + return set_parameters_atomically( + parameters, + std::chrono::duration_cast(timeout) + ); + } - RCLCPP_PUBLIC + /// Delete several parameters at once. + /** + * This function behaves like command-line tool `ros2 param delete` would. + * + * \param parameters_names vector of parameters names + * \param timeout for the spin used to make it synchronous + * \return the future of the set_parameter service used to delete the parameters + */ + template + std::vector + delete_parameters( + const std::vector & parameters_names, + std::chrono::duration timeout = std::chrono::duration(-1)) + { + return delete_parameters( + parameters_names, + std::chrono::duration_cast(timeout) + ); + } + + /// Load parameters from yaml file. + /** + * This function behaves like command-line tool `ros2 param load` would. + * + * \param yaml_filename the full name of the yaml file + * \param timeout for the spin used to make it synchronous + * \return the future of the set_parameter service used to load the parameters + */ + template + std::vector + load_parameters( + const std::string & yaml_filename, + std::chrono::duration timeout = std::chrono::duration(-1)) + { + return load_parameters( + yaml_filename, + std::chrono::duration_cast(timeout) + ); + } + + template rcl_interfaces::msg::ListParametersResult list_parameters( const std::vector & parameter_prefixes, - uint64_t depth); + uint64_t depth, + std::chrono::duration timeout = std::chrono::duration(-1)) + { + return list_parameters( + parameter_prefixes, + depth, + std::chrono::duration_cast(timeout) + ); + } template typename rclcpp::Subscription::SharedPtr on_parameter_event(CallbackT && callback) { - return async_parameters_client_->on_parameter_event(std::forward(callback)); + return async_parameters_client_->on_parameter_event( + std::forward(callback)); + } + + /** + * The NodeT type only needs to have a method called get_node_topics_interface() + * which returns a shared_ptr to a NodeTopicsInterface, or be a + * NodeTopicsInterface pointer itself. + */ + template< + typename CallbackT, + typename NodeT> + static typename rclcpp::Subscription::SharedPtr + on_parameter_event( + NodeT && node, + CallbackT && callback) + { + return AsyncParametersClient::on_parameter_event( + node, + std::forward(callback)); } RCLCPP_PUBLIC @@ -257,17 +568,67 @@ class SyncParametersClient return async_parameters_client_->service_is_ready(); } - template + template bool wait_for_service( - std::chrono::duration timeout = std::chrono::duration(-1)) + std::chrono::duration timeout = std::chrono::duration(-1)) { return async_parameters_client_->wait_for_service(timeout); } +protected: + RCLCPP_PUBLIC + std::vector + get_parameters( + const std::vector & parameter_names, + std::chrono::nanoseconds timeout); + + RCLCPP_PUBLIC + std::vector + describe_parameters( + const std::vector & parameter_names, + std::chrono::nanoseconds timeout); + + RCLCPP_PUBLIC + std::vector + get_parameter_types( + const std::vector & parameter_names, + std::chrono::nanoseconds timeout); + + RCLCPP_PUBLIC + std::vector + set_parameters( + const std::vector & parameters, + std::chrono::nanoseconds timeout); + + RCLCPP_PUBLIC + std::vector + delete_parameters( + const std::vector & parameters_names, + std::chrono::nanoseconds timeout); + + RCLCPP_PUBLIC + std::vector + load_parameters( + const std::string & yaml_filename, + std::chrono::nanoseconds timeout); + + RCLCPP_PUBLIC + rcl_interfaces::msg::SetParametersResult + set_parameters_atomically( + const std::vector & parameters, + std::chrono::nanoseconds timeout); + + RCLCPP_PUBLIC + rcl_interfaces::msg::ListParametersResult + list_parameters( + const std::vector & parameter_prefixes, + uint64_t depth, + std::chrono::nanoseconds timeout); + private: - rclcpp::executor::Executor::SharedPtr executor_; - rclcpp::Node::SharedPtr node_; + rclcpp::Executor::SharedPtr executor_; + const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface_; AsyncParametersClient::SharedPtr async_parameters_client_; }; diff --git a/rclcpp/include/rclcpp/parameter_event_handler.hpp b/rclcpp/include/rclcpp/parameter_event_handler.hpp new file mode 100644 index 0000000000..589fb1144c --- /dev/null +++ b/rclcpp/include/rclcpp/parameter_event_handler.hpp @@ -0,0 +1,375 @@ +// Copyright 2019 Intel Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT 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 RCLCPP__PARAMETER_EVENT_HANDLER_HPP_ +#define RCLCPP__PARAMETER_EVENT_HANDLER_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "rclcpp/create_subscription.hpp" +#include "rclcpp/node_interfaces/get_node_base_interface.hpp" +#include "rclcpp/node_interfaces/get_node_topics_interface.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_topics_interface.hpp" +#include "rclcpp/parameter.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/subscription.hpp" +#include "rclcpp/visibility_control.hpp" +#include "rcl_interfaces/msg/parameter_event.hpp" + +namespace rclcpp +{ + +struct ParameterCallbackHandle +{ + RCLCPP_SMART_PTR_DEFINITIONS(ParameterCallbackHandle) + + using ParameterCallbackType = std::function; + + std::string parameter_name; + std::string node_name; + ParameterCallbackType callback; +}; + +struct ParameterEventCallbackHandle +{ + RCLCPP_SMART_PTR_DEFINITIONS(ParameterEventCallbackHandle) + + using ParameterEventCallbackType = + std::function; + + ParameterEventCallbackType callback; +}; + +/// A class used to "handle" (monitor and respond to) changes to parameters. +/** + * The ParameterEventHandler class allows for the monitoring of changes to node parameters, + * either a node's own parameters or parameters owned by other nodes in the system. + * Multiple parameter callbacks can be set and will be invoked when the specified parameter + * changes. + * + * The first step is to instantiate a ParameterEventHandler, providing a ROS node to use + * to create any required subscriptions: + * + * ```cpp + * auto param_handler = std::make_shared(node); + * ``` + * + * Next, you can supply a callback to the add_parameter_callback method, as follows: + * + * ```cpp + * auto cb1 = [&node](const rclcpp::Parameter & p) { + * RCLCPP_INFO( + * node->get_logger(), + * "cb1: Received an update to parameter \"%s\" of type %s: \"%ld\"", + * p.get_name().c_str(), + * p.get_type_name().c_str(), + * p.as_int()); + * }; + * auto handle1 = param_handler->add_parameter_callback("an_int_param", cb1); + * ``` + * + * In this case, we didn't supply a node name (the third, optional, parameter) so the + * default will be to monitor for changes to the "an_int_param" parameter associated with + * the ROS node supplied in the ParameterEventHandler constructor. + * The callback, a lambda function in this case, simply prints out the value of the parameter. + * + * Note: the object returned from add_parameter_callback must be captured or the callback will + * be immediately unregistered. + * + * You may also monitor for changes to parameters in other nodes by supplying the node + * name to add_parameter_callback: + * + * ```cpp + * auto cb2 = [&node](const rclcpp::Parameter & p) { + * RCLCPP_INFO( + * node->get_logger(), + * "cb2: Received an update to parameter \"%s\" of type: %s: \"%s\"", + * p.get_name().c_str(), + * p.get_type_name().c_str(), + * p.as_string().c_str()); + * }; + * auto handle2 = param_handler->add_parameter_callback( + * "some_remote_param_name", cb2, "some_remote_node_name"); + * ``` + * + * In this case, the callback will be invoked whenever "some_remote_param_name" changes + * on remote node "some_remote_node_name". + * + * To remove a parameter callback, reset the callback handle smart pointer or call + * remove_parameter_callback, passing the handle returned from add_parameter_callback: + * + * ```cpp + * param_handler->remove_parameter_callback(handle2); + * ``` + * + * You can also monitor for *all* parameter changes, using add_parameter_event_callback. + * In this case, the callback will be invoked whenever any parameter changes in the system. + * You are likely interested in a subset of these parameter changes, so in the callback it + * is convenient to use a regular expression on the node names or namespaces of interest. + * For example: + * + * ```cpp + * auto cb3 = + * [fqn, remote_param_name, &node](const rcl_interfaces::msg::ParameterEvent & event) { + * // Look for any updates to parameters in "/a_namespace" as well as any parameter changes + * // to our own node ("this_node") + * std::regex re("(/a_namespace/.*)|(/this_node)"); + * if (regex_match(event.node, re)) { + * // Now that we know the event matches the regular expression we scanned for, we can + * // use 'get_parameter_from_event' to get a specific parameter name that we're looking for + * rclcpp::Parameter p; + * if (rclcpp::ParameterEventHandler::get_parameter_from_event( + * event, p, remote_param_name, fqn)) + * { + * RCLCPP_INFO( + * node->get_logger(), + * "cb3: Received an update to parameter \"%s\" of type: %s: \"%s\"", + * p.get_name().c_str(), + * p.get_type_name().c_str(), + * p.as_string().c_str()); + * } + * + * // You can also use 'get_parameter*s*_from_event' to enumerate all changes that came + * // in on this event + * auto params = rclcpp::ParameterEventHandler::get_parameters_from_event(event); + * for (auto & p : params) { + * RCLCPP_INFO( + * node->get_logger(), + * "cb3: Received an update to parameter \"%s\" of type: %s: \"%s\"", + * p.get_name().c_str(), + * p.get_type_name().c_str(), + * p.value_to_string().c_str()); + * } + * } + * }; + * auto handle3 = param_handler->add_parameter_event_callback(cb3); + * ``` + * + * For both parameter callbacks and parameter event callbacks, when multiple callbacks are added, + * the callbacks are invoked last-in, first-called order (LIFO). + * + * Note: the callback handle returned from add_parameter_event_callback must be captured or + * the callback will immediately be unregistered. + * + * To remove a parameter event callback, reset the callback smart pointer or use: + * + * ```cpp + * param_handler->remove_event_parameter_callback(handle3); + * ``` + */ +class ParameterEventHandler +{ +public: + /// Construct a parameter events monitor. + /** + * \param[in] node The node to use to create any required subscribers. + * \param[in] qos The QoS settings to use for any subscriptions. + */ + template + explicit ParameterEventHandler( + NodeT node, + const rclcpp::QoS & qos = + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_parameter_events))) + : node_base_(rclcpp::node_interfaces::get_node_base_interface(node)) + { + auto node_topics = rclcpp::node_interfaces::get_node_topics_interface(node); + + callbacks_ = std::make_shared(); + + event_subscription_ = rclcpp::create_subscription( + node_topics, "/parameter_events", qos, + [callbacks = callbacks_](const rcl_interfaces::msg::ParameterEvent & event) { + callbacks->event_callback(event); + }); + } + + using ParameterEventCallbackType = + ParameterEventCallbackHandle::ParameterEventCallbackType; + + /// Set a callback for all parameter events. + /** + * This function may be called multiple times to set multiple parameter event callbacks. + * + * Note: if the returned callback handle smart pointer is not captured, the callback is + * immediatedly unregistered. A compiler warning should be generated to warn of this. + * + * \param[in] callback Function callback to be invoked on parameter updates. + * \returns A handle used to refer to the callback. + */ + RCLCPP_PUBLIC + RCUTILS_WARN_UNUSED + ParameterEventCallbackHandle::SharedPtr + add_parameter_event_callback( + ParameterEventCallbackType callback); + + /// Remove parameter event callback registered with add_parameter_event_callback. + /** + * \param[in] callback_handle Handle of the callback to remove. + */ + RCLCPP_PUBLIC + void + remove_parameter_event_callback( + ParameterEventCallbackHandle::SharedPtr callback_handle); + + using ParameterCallbackType = ParameterCallbackHandle::ParameterCallbackType; + + /// Add a callback for a specified parameter. + /** + * If a node_name is not provided, defaults to the current node. + * + * Note: if the returned callback handle smart pointer is not captured, the callback + * is immediately unregistered. A compiler warning should be generated to warn + * of this. + * + * \param[in] parameter_name Name of parameter to monitor. + * \param[in] callback Function callback to be invoked upon parameter update. + * \param[in] node_name Name of node which hosts the parameter. + * \returns A handle used to refer to the callback. + */ + RCLCPP_PUBLIC + RCUTILS_WARN_UNUSED + ParameterCallbackHandle::SharedPtr + add_parameter_callback( + const std::string & parameter_name, + ParameterCallbackType callback, + const std::string & node_name = ""); + + /// Remove a parameter callback registered with add_parameter_callback. + /** + * The parameter name and node name are inspected from the callback handle. The callback handle + * is erased from the list of callback handles on the {parameter_name, node_name} in the map. + * An error is thrown if the handle does not exist and/or was already removed. + * + * \param[in] callback_handle Handle of the callback to remove. + */ + RCLCPP_PUBLIC + void + remove_parameter_callback( + ParameterCallbackHandle::SharedPtr callback_handle); + + /// Get an rclcpp::Parameter from a parameter event. + /** + * If a node_name is not provided, defaults to the current node. + * + * \param[in] event Event msg to be inspected. + * \param[out] parameter Reference to rclcpp::Parameter to be assigned. + * \param[in] parameter_name Name of parameter. + * \param[in] node_name Name of node which hosts the parameter. + * \returns Output parameter is set with requested parameter info and returns true if + * requested parameter name and node is in event. Otherwise, returns false. + */ + RCLCPP_PUBLIC + static bool + get_parameter_from_event( + const rcl_interfaces::msg::ParameterEvent & event, + rclcpp::Parameter & parameter, + const std::string & parameter_name, + const std::string & node_name = ""); + + /// Get an rclcpp::Parameter from parameter event + /** + * If a node_name is not provided, defaults to the current node. + * + * The user is responsible to check if the returned parameter has been properly assigned. + * By default, if the requested parameter is not found in the event, the returned parameter + * has parameter value of type rclcpp::PARAMETER_NOT_SET. + * + * \param[in] event Event msg to be inspected. + * \param[in] parameter_name Name of parameter. + * \param[in] node_name Name of node which hosts the parameter. + * \returns The resultant rclcpp::Parameter from the event. + * \throws std::runtime_error if input node name doesn't match the node name in parameter event. + */ + RCLCPP_PUBLIC + static rclcpp::Parameter + get_parameter_from_event( + const rcl_interfaces::msg::ParameterEvent & event, + const std::string & parameter_name, + const std::string & node_name = ""); + + /// Get all rclcpp::Parameter values from a parameter event + /** + * \param[in] event Event msg to be inspected. + * \returns A vector rclcpp::Parameter values from the event. + */ + RCLCPP_PUBLIC + static std::vector + get_parameters_from_event( + const rcl_interfaces::msg::ParameterEvent & event); + + using CallbacksContainerType = std::list; + +protected: + // *INDENT-OFF* Uncrustify doesn't handle indented public/private labels + // Hash function for string pair required in std::unordered_map + // See: https://stackoverflow.com/questions/35985960/c-why-is-boosthash-combine-the-best-way-to-combine-hash-values + class StringPairHash + { + public: + template + inline void hash_combine(std::size_t & seed, const T & v) const + { + std::hash hasher; + seed ^= hasher(v) + 0x9e3779b9 + (seed << 6) + (seed >> 2); + } + + inline size_t operator()(const std::pair & s) const + { + size_t seed = 0; + hash_combine(seed, s.first); + hash_combine(seed, s.second); + return seed; + } + }; + // *INDENT-ON* + + struct Callbacks + { + std::recursive_mutex mutex_; + + // Map container for registered parameters + std::unordered_map< + std::pair, + CallbacksContainerType, + StringPairHash + > parameter_callbacks_; + + std::list event_callbacks_; + + /// Callback for parameter events subscriptions. + RCLCPP_PUBLIC + void + event_callback(const rcl_interfaces::msg::ParameterEvent & event); + }; + + std::shared_ptr callbacks_; + + // Utility function for resolving node path. + std::string resolve_path(const std::string & path); + + // Node interface used for base functionality + std::shared_ptr node_base_; + + rclcpp::Subscription::SharedPtr event_subscription_; +}; + +} // namespace rclcpp + +#endif // RCLCPP__PARAMETER_EVENT_HANDLER_HPP_ diff --git a/rclcpp/include/rclcpp/parameter_events_filter.hpp b/rclcpp/include/rclcpp/parameter_events_filter.hpp index fa5b1deb22..6960a1bccf 100644 --- a/rclcpp/include/rclcpp/parameter_events_filter.hpp +++ b/rclcpp/include/rclcpp/parameter_events_filter.hpp @@ -37,26 +37,30 @@ class ParameterEventsFilter RCLCPP_SMART_PTR_DEFINITIONS(ParameterEventsFilter) enum class EventType {NEW, DELETED, CHANGED}; ///< An enum for the type of event. /// Used for the listed results - using EventPair = std::pair; + using EventPair = std::pair; /// Construct a filtered view of a parameter event. /** * \param[in] event The parameter event message to filter. * \param[in] names A list of parameter names of interest. * \param[in] types A list of the types of parameter events of iterest. - * EventType NEW, DELETED, or CHANGED - * - * Example Usage: - * If you have recieved a parameter event and are only interested in parameters foo and - * bar being added or changed but don't care about deletion. - * auto res = rclcpp::ParameterEventsFilter( - * event_shared_ptr, - * {"foo", "bar"}, - * {rclcpp::ParameterEventsFilter::EventType::NEW, rclcpp::ParameterEventsFilter::EventType::CHANGED}); - */ + * EventType NEW, DELETED, or CHANGED + * + * Example Usage: + * + * If you have received a parameter event and are only interested in parameters foo and + * bar being added or changed but don't care about deletion. + * + * ```cpp + * auto res = rclcpp::ParameterEventsFilter( + * event_shared_ptr, + * {"foo", "bar"}, + * {rclcpp::ParameterEventsFilter::EventType::NEW, rclcpp::ParameterEventsFilter::EventType::CHANGED}); + * ``` + */ RCLCPP_PUBLIC ParameterEventsFilter( - rcl_interfaces::msg::ParameterEvent::SharedPtr event, + std::shared_ptr event, const std::vector & names, const std::vector & types); @@ -70,7 +74,7 @@ class ParameterEventsFilter private: // access only allowed via const accessor. std::vector result_; ///< Storage of the resultant vector - rcl_interfaces::msg::ParameterEvent::SharedPtr event_; ///< Keep event in scope + std::shared_ptr event_; ///< Keep event in scope }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/parameter_map.hpp b/rclcpp/include/rclcpp/parameter_map.hpp new file mode 100644 index 0000000000..17e2128a7b --- /dev/null +++ b/rclcpp/include/rclcpp/parameter_map.hpp @@ -0,0 +1,73 @@ +// 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 RCLCPP__PARAMETER_MAP_HPP_ +#define RCLCPP__PARAMETER_MAP_HPP_ + +#include +#include + +#include +#include +#include + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/parameter.hpp" +#include "rclcpp/parameter_value.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ + +/// A map of fully qualified node names to a list of parameters +using ParameterMap = std::unordered_map>; + +/// Convert parameters from rcl_yaml_param_parser into C++ class instances. +/// \param[in] c_params C structures containing parameters for multiple nodes. +/// \param[in] node_fqn a Fully Qualified Name of node, default value is nullptr. +/// If it's not nullptr, return the relative node parameters belonging to this node_fqn. +/// \returns a map where the keys are fully qualified node names and values a list of parameters. +/// \throws InvalidParametersException if the `rcl_params_t` is inconsistent or invalid. +RCLCPP_PUBLIC +ParameterMap +parameter_map_from(const rcl_params_t * const c_params, const char * node_fqn = nullptr); + +/// Convert parameter value from rcl_yaml_param_parser into a C++ class instance. +/// \param[in] c_value C structure containing a value of a parameter. +/// \returns an instance of a parameter value +/// \throws InvalidParameterValueException if the `rcl_variant_t` is inconsistent or invalid. +RCLCPP_PUBLIC +ParameterValue +parameter_value_from(const rcl_variant_t * const c_value); + +/// Get the ParameterMap from a yaml file. +/// \param[in] yaml_filename full name of the yaml file. +/// \param[in] node_fqn a Fully Qualified Name of node, default value is nullptr. +/// \returns an instance of a parameter map +/// \throws from rcl error of rcl_parse_yaml_file() +RCLCPP_PUBLIC +ParameterMap +parameter_map_from_yaml_file(const std::string & yaml_filename, const char * node_fqn = nullptr); + +/// Get the Parameters from ParameterMap. +/// \param[in] parameter_map a parameter map. +/// \param[in] node_fqn a Fully Qualified Name of node, default value is nullptr. +/// \returns a list of a parameter +RCLCPP_PUBLIC +std::vector +parameters_from_map(const ParameterMap & parameter_map, const char * node_fqn = nullptr); + +} // namespace rclcpp + +#endif // RCLCPP__PARAMETER_MAP_HPP_ diff --git a/rclcpp/include/rclcpp/parameter_service.hpp b/rclcpp/include/rclcpp/parameter_service.hpp index 6970cbc230..54353e9579 100644 --- a/rclcpp/include/rclcpp/parameter_service.hpp +++ b/rclcpp/include/rclcpp/parameter_service.hpp @@ -15,6 +15,7 @@ #ifndef RCLCPP__PARAMETER_SERVICE_HPP_ #define RCLCPP__PARAMETER_SERVICE_HPP_ +#include #include #include "rcl_interfaces/srv/describe_parameters.hpp" @@ -27,6 +28,7 @@ #include "rclcpp/macros.hpp" #include "rclcpp/node.hpp" #include "rclcpp/parameter.hpp" +#include "rclcpp/qos.hpp" #include "rclcpp/visibility_control.hpp" #include "rmw/rmw.h" @@ -39,12 +41,13 @@ class ParameterService RCLCPP_SMART_PTR_DEFINITIONS(ParameterService) RCLCPP_PUBLIC - explicit ParameterService( - const rclcpp::Node::SharedPtr node, - const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters); + ParameterService( + const std::shared_ptr node_base, + const std::shared_ptr node_services, + rclcpp::node_interfaces::NodeParametersInterface * node_params, + const rclcpp::QoS & qos_profile = rclcpp::ParametersQoS()); private: - const rclcpp::Node::SharedPtr node_; rclcpp::Service::SharedPtr get_parameters_service_; rclcpp::Service::SharedPtr get_parameter_types_service_; diff --git a/rclcpp/include/rclcpp/parameter_value.hpp b/rclcpp/include/rclcpp/parameter_value.hpp new file mode 100644 index 0000000000..549429aa85 --- /dev/null +++ b/rclcpp/include/rclcpp/parameter_value.hpp @@ -0,0 +1,366 @@ +// 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 RCLCPP__PARAMETER_VALUE_HPP_ +#define RCLCPP__PARAMETER_VALUE_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "rcl_interfaces/msg/parameter_type.hpp" +#include "rcl_interfaces/msg/parameter_value.hpp" +#include "rclcpp/exceptions/exceptions.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ + +enum ParameterType : uint8_t +{ + PARAMETER_NOT_SET = rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET, + PARAMETER_BOOL = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL, + PARAMETER_INTEGER = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER, + PARAMETER_DOUBLE = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE, + PARAMETER_STRING = rcl_interfaces::msg::ParameterType::PARAMETER_STRING, + PARAMETER_BYTE_ARRAY = rcl_interfaces::msg::ParameterType::PARAMETER_BYTE_ARRAY, + PARAMETER_BOOL_ARRAY = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL_ARRAY, + PARAMETER_INTEGER_ARRAY = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY, + PARAMETER_DOUBLE_ARRAY = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY, + PARAMETER_STRING_ARRAY = rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY, +}; + +/// Return the name of a parameter type +RCLCPP_PUBLIC +std::string +to_string(ParameterType type); + +RCLCPP_PUBLIC +std::ostream & +operator<<(std::ostream & os, ParameterType type); + +/// Indicate the parameter type does not match the expected type. +class ParameterTypeException : public std::runtime_error +{ +public: + /// Construct an instance. + /** + * \param[in] expected the expected parameter type. + * \param[in] actual the actual parameter type. + */ + RCLCPP_PUBLIC + ParameterTypeException(ParameterType expected, ParameterType actual) + : std::runtime_error("expected [" + to_string(expected) + "] got [" + to_string(actual) + "]") + {} +}; + +/// Store the type and value of a parameter. +class ParameterValue +{ +public: + /// Construct a parameter value with type PARAMETER_NOT_SET. + RCLCPP_PUBLIC + ParameterValue(); + /// Construct a parameter value from a message. + RCLCPP_PUBLIC + explicit ParameterValue(const rcl_interfaces::msg::ParameterValue & value); + /// Construct a parameter value with type PARAMETER_BOOL. + RCLCPP_PUBLIC + explicit ParameterValue(const bool bool_value); + /// Construct a parameter value with type PARAMETER_INTEGER. + RCLCPP_PUBLIC + explicit ParameterValue(const int int_value); + /// Construct a parameter value with type PARAMETER_INTEGER. + RCLCPP_PUBLIC + explicit ParameterValue(const int64_t int_value); + /// Construct a parameter value with type PARAMETER_DOUBLE. + RCLCPP_PUBLIC + explicit ParameterValue(const float double_value); + /// Construct a parameter value with type PARAMETER_DOUBLE. + RCLCPP_PUBLIC + explicit ParameterValue(const double double_value); + /// Construct a parameter value with type PARAMETER_STRING. + RCLCPP_PUBLIC + explicit ParameterValue(const std::string & string_value); + /// Construct a parameter value with type PARAMETER_STRING. + RCLCPP_PUBLIC + explicit ParameterValue(const char * string_value); + /// Construct a parameter value with type PARAMETER_BYTE_ARRAY. + RCLCPP_PUBLIC + explicit ParameterValue(const std::vector & byte_array_value); + /// Construct a parameter value with type PARAMETER_BOOL_ARRAY. + RCLCPP_PUBLIC + explicit ParameterValue(const std::vector & bool_array_value); + /// Construct a parameter value with type PARAMETER_INTEGER_ARRAY. + RCLCPP_PUBLIC + explicit ParameterValue(const std::vector & int_array_value); + /// Construct a parameter value with type PARAMETER_INTEGER_ARRAY. + RCLCPP_PUBLIC + explicit ParameterValue(const std::vector & int_array_value); + /// Construct a parameter value with type PARAMETER_DOUBLE_ARRAY. + RCLCPP_PUBLIC + explicit ParameterValue(const std::vector & double_array_value); + /// Construct a parameter value with type PARAMETER_DOUBLE_ARRAY. + RCLCPP_PUBLIC + explicit ParameterValue(const std::vector & double_array_value); + /// Construct a parameter value with type PARAMETER_STRING_ARRAY. + RCLCPP_PUBLIC + explicit ParameterValue(const std::vector & string_array_value); + + /// Return an enum indicating the type of the set value. + RCLCPP_PUBLIC + ParameterType + get_type() const; + + /// Return a message populated with the parameter value + RCLCPP_PUBLIC + rcl_interfaces::msg::ParameterValue + to_value_msg() const; + + /// Equal operator. + RCLCPP_PUBLIC + bool + operator==(const ParameterValue & rhs) const; + + /// Not equal operator. + RCLCPP_PUBLIC + bool + operator!=(const ParameterValue & rhs) const; + + // The following get() variants require the use of ParameterType + + template + constexpr + typename std::enable_if::type + get() const + { + if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_BOOL) { + throw ParameterTypeException(ParameterType::PARAMETER_BOOL, get_type()); + } + return value_.bool_value; + } + + template + constexpr + typename std::enable_if::type + get() const + { + if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER) { + throw ParameterTypeException(ParameterType::PARAMETER_INTEGER, get_type()); + } + return value_.integer_value; + } + + template + constexpr + typename std::enable_if::type + get() const + { + if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE) { + throw ParameterTypeException(ParameterType::PARAMETER_DOUBLE, get_type()); + } + return value_.double_value; + } + + template + constexpr + typename std::enable_if::type + get() const + { + if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_STRING) { + throw ParameterTypeException(ParameterType::PARAMETER_STRING, get_type()); + } + return value_.string_value; + } + + template + constexpr + typename std::enable_if< + type == ParameterType::PARAMETER_BYTE_ARRAY, const std::vector &>::type + get() const + { + if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_BYTE_ARRAY) { + throw ParameterTypeException(ParameterType::PARAMETER_BYTE_ARRAY, get_type()); + } + return value_.byte_array_value; + } + + template + constexpr + typename std::enable_if< + type == ParameterType::PARAMETER_BOOL_ARRAY, const std::vector &>::type + get() const + { + if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_BOOL_ARRAY) { + throw ParameterTypeException(ParameterType::PARAMETER_BOOL_ARRAY, get_type()); + } + return value_.bool_array_value; + } + + template + constexpr + typename std::enable_if< + type == ParameterType::PARAMETER_INTEGER_ARRAY, const std::vector &>::type + get() const + { + if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY) { + throw ParameterTypeException(ParameterType::PARAMETER_INTEGER_ARRAY, get_type()); + } + return value_.integer_array_value; + } + + template + constexpr + typename std::enable_if< + type == ParameterType::PARAMETER_DOUBLE_ARRAY, const std::vector &>::type + get() const + { + if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY) { + throw ParameterTypeException(ParameterType::PARAMETER_DOUBLE_ARRAY, get_type()); + } + return value_.double_array_value; + } + + template + constexpr + typename std::enable_if< + type == ParameterType::PARAMETER_STRING_ARRAY, const std::vector &>::type + get() const + { + if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY) { + throw ParameterTypeException(ParameterType::PARAMETER_STRING_ARRAY, get_type()); + } + return value_.string_array_value; + } + + // The following get() variants allow the use of primitive types + + template + constexpr + typename std::enable_if::value, const bool &>::type + get() const + { + return get(); + } + + template + constexpr + typename std::enable_if< + std::is_integral::value && !std::is_same::value, const int64_t &>::type + get() const + { + return get(); + } + + template + constexpr + typename std::enable_if::value, const double &>::type + get() const + { + return get(); + } + + template + constexpr + typename std::enable_if::value, const std::string &>::type + get() const + { + return get(); + } + + template + constexpr + typename std::enable_if< + std::is_convertible< + type, const std::vector &>::value, const std::vector &>::type + get() const + { + return get(); + } + + template + constexpr + typename std::enable_if< + std::is_convertible< + type, const std::vector &>::value, const std::vector &>::type + get() const + { + return get(); + } + + template + constexpr + typename std::enable_if< + std::is_convertible< + type, const std::vector &>::value, const std::vector &>::type + get() const + { + return get(); + } + + template + constexpr + typename std::enable_if< + std::is_convertible< + type, const std::vector &>::value, const std::vector &>::type + get() const + { + return get(); + } + + template + constexpr + typename std::enable_if< + std::is_convertible< + type, const std::vector &>::value, const std::vector &>::type + get() const + { + return get(); + } + + template + constexpr + typename std::enable_if< + std::is_convertible< + type, const std::vector &>::value, const std::vector &>::type + get() const + { + return get(); + } + + template + constexpr + typename std::enable_if< + std::is_convertible< + type, const std::vector &>::value, const std::vector &>::type + get() const + { + return get(); + } + +private: + rcl_interfaces::msg::ParameterValue value_; +}; + +/// Return the value of a parameter as a string +RCLCPP_PUBLIC +std::string +to_string(const ParameterValue & value); + +} // namespace rclcpp + +#endif // RCLCPP__PARAMETER_VALUE_HPP_ diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index f505bef7a5..22825daf89 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -15,291 +15,576 @@ #ifndef RCLCPP__PUBLISHER_HPP_ #define RCLCPP__PUBLISHER_HPP_ -#include -#include - #include #include #include #include #include +#include +#include #include "rcl/error_handling.h" #include "rcl/publisher.h" - -#include "rcl_interfaces/msg/intra_process_message.hpp" +#include "rmw/error_handling.h" +#include "rmw/rmw.h" +#include "rosidl_runtime_cpp/traits.hpp" #include "rclcpp/allocator/allocator_common.hpp" #include "rclcpp/allocator/allocator_deleter.hpp" +#include "rclcpp/detail/resolve_use_intra_process.hpp" +#include "rclcpp/detail/resolve_intra_process_buffer_type.hpp" +#include "rclcpp/experimental/buffers/intra_process_buffer.hpp" +#include "rclcpp/experimental/create_intra_process_buffer.hpp" +#include "rclcpp/experimental/intra_process_manager.hpp" +#include "rclcpp/get_message_type_support_handle.hpp" +#include "rclcpp/is_ros_compatible_type.hpp" +#include "rclcpp/loaned_message.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/publisher_base.hpp" +#include "rclcpp/publisher_options.hpp" +#include "rclcpp/type_adapter.hpp" #include "rclcpp/type_support_decl.hpp" #include "rclcpp/visibility_control.hpp" +#include "tracetools/tracetools.h" + namespace rclcpp { -// Forward declaration is used for friend statement. -namespace node_interfaces -{ -class NodeTopicsInterface; -} +template +class LoanedMessage; -class PublisherBase +/// A publisher publishes messages of any type to a topic. +/** + * MessageT must be a: + * + * - ROS message type with its own type support (e.g. std_msgs::msgs::String), or a + * - rclcpp::TypeAdapter + * (e.g. rclcpp::TypeAdapter type + * (e.g. TypeAdapter), PublishedType will + * be the custom type, and ROSMessageType will be the ros message type. + * + * This is achieved because of the "identity specialization" for TypeAdapter, + * which returns itself if it is already a TypeAdapter, and the default + * specialization which allows ROSMessageType to be void. + * \sa rclcpp::TypeAdapter for more details. + */ +template> +class Publisher : public PublisherBase { - friend ::rclcpp::node_interfaces::NodeTopicsInterface; - public: - RCLCPP_SMART_PTR_DEFINITIONS(PublisherBase) - - /// Default constructor. - /** - * Typically, a publisher is not created through this method, but instead is created through a - * call to `Node::create_publisher`. - * \param[in] node_base A pointer to the NodeBaseInterface for the parent node. - * \param[in] topic The topic that this publisher publishes on. - * \param[in] type_support The type support structure for the type to be published. - * \param[in] publisher_options QoS settings for this publisher. - */ - RCLCPP_PUBLIC - PublisherBase( - rclcpp::node_interfaces::NodeBaseInterface * node_base, - const std::string & topic, - const rosidl_message_type_support_t & type_support, - const rcl_publisher_options_t & publisher_options); - - RCLCPP_PUBLIC - virtual ~PublisherBase(); - - /// Get the topic that this publisher publishes on. - /** \return The topic name. */ - RCLCPP_PUBLIC - const char * - get_topic_name() const; - - /// Get the queue size for this publisher. - /** \return The queue size. */ - RCLCPP_PUBLIC - size_t - get_queue_size() const; - - /// Get the global identifier for this publisher (used in rmw and by DDS). - /** \return The gid. */ - RCLCPP_PUBLIC - const rmw_gid_t & - get_gid() const; - - /// Get the global identifier for this publisher used by intra-process communication. - /** \return The intra-process gid. */ - RCLCPP_PUBLIC - const rmw_gid_t & - get_intra_process_gid() const; - - /// Get the rcl publisher handle. - /** \return The rcl publisher handle. */ - RCLCPP_PUBLIC - rcl_publisher_t * - get_publisher_handle(); - - /// Get the rcl publisher handle. - /** \return The rcl publisher handle. */ - RCLCPP_PUBLIC - const rcl_publisher_t * - get_publisher_handle() const; - - /// Compare this publisher to a gid. - /** - * Note that this function calls the next function. - * \param[in] gid Reference to a gid. - * \return True if the publisher's gid matches the input. - */ - RCLCPP_PUBLIC - bool - operator==(const rmw_gid_t & gid) const; - - /// Compare this publisher to a pointer gid. - /** - * A wrapper for comparing this publisher's gid to the input using rmw_compare_gids_equal. - * \param[in] gid A pointer to a gid. - * \return True if this publisher's gid matches the input. - */ - RCLCPP_PUBLIC - bool - operator==(const rmw_gid_t * gid) const; + static_assert( + rclcpp::is_ros_compatible_type::value, + "given message type is not compatible with ROS and cannot be used with a Publisher"); - using StoreMessageCallbackT = std::function; + /// MessageT::custom_type if MessageT is a TypeAdapter, otherwise just MessageT. + using PublishedType = typename rclcpp::TypeAdapter::custom_type; + using ROSMessageType = typename rclcpp::TypeAdapter::ros_message_type; - /// Implementation utility function used to setup intra process publishing after creation. - RCLCPP_PUBLIC - void - setup_intra_process( - uint64_t intra_process_publisher_id, - StoreMessageCallbackT callback, - const rcl_publisher_options_t & intra_process_options); + using PublishedTypeAllocatorTraits = allocator::AllocRebind; + using PublishedTypeAllocator = typename PublishedTypeAllocatorTraits::allocator_type; + using PublishedTypeDeleter = allocator::Deleter; -protected: - std::shared_ptr rcl_node_handle_; + using ROSMessageTypeAllocatorTraits = allocator::AllocRebind; + using ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type; + using ROSMessageTypeDeleter = allocator::Deleter; - rcl_publisher_t publisher_handle_ = rcl_get_zero_initialized_publisher(); - rcl_publisher_t intra_process_publisher_handle_ = rcl_get_zero_initialized_publisher(); + using BufferSharedPtr = typename rclcpp::experimental::buffers::IntraProcessBuffer< + ROSMessageType, + ROSMessageTypeAllocator, + ROSMessageTypeDeleter + >::SharedPtr; - uint64_t intra_process_publisher_id_; - StoreMessageCallbackT store_intra_process_message_; - - rmw_gid_t rmw_gid_; - rmw_gid_t intra_process_rmw_gid_; -}; - -/// A publisher publishes messages of any type to a topic. -template> -class Publisher : public PublisherBase -{ -public: - using MessageAllocTraits = allocator::AllocRebind; - using MessageAlloc = typename MessageAllocTraits::allocator_type; - using MessageDeleter = allocator::Deleter; - using MessageUniquePtr = std::unique_ptr; - - RCLCPP_SMART_PTR_DEFINITIONS(Publisher) + RCLCPP_SMART_PTR_DEFINITIONS(Publisher) + /// Default constructor. + /** + * The constructor for a Publisher is almost never called directly. + * Instead, subscriptions should be instantiated through the function + * rclcpp::create_publisher(). + * + * \param[in] node_base NodeBaseInterface pointer that is used in part of the setup. + * \param[in] topic Name of the topic to publish to. + * \param[in] qos QoS profile for the publisher. + * \param[in] options Options for the publisher. + */ Publisher( rclcpp::node_interfaces::NodeBaseInterface * node_base, const std::string & topic, - const rcl_publisher_options_t & publisher_options, - const std::shared_ptr & allocator) + const rclcpp::QoS & qos, + const rclcpp::PublisherOptionsWithAllocator & options) : PublisherBase( node_base, topic, - *rosidl_typesupport_cpp::get_message_type_support_handle(), - publisher_options), - message_allocator_(allocator) + rclcpp::get_message_type_support_handle(), + options.template to_rcl_publisher_options(qos), + // NOTE(methylDragon): Passing these args separately is necessary for event binding + options.event_callbacks, + options.use_default_callbacks), + options_(options), + published_type_allocator_(*options.get_allocator()), + ros_message_type_allocator_(*options.get_allocator()) { - allocator::set_allocator_for_deleter(&message_deleter_, message_allocator_.get()); + allocator::set_allocator_for_deleter(&published_type_deleter_, &published_type_allocator_); + allocator::set_allocator_for_deleter(&ros_message_type_deleter_, &ros_message_type_allocator_); + // Setup continues in the post construction method, post_init_setup(). + } + + /// Called post construction, so that construction may continue after shared_from_this() works. + virtual + void + post_init_setup( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + const std::string & topic, + const rclcpp::QoS & qos, + const rclcpp::PublisherOptionsWithAllocator & options) + { + // Topic is unused for now. + (void)topic; + (void)options; + + // If needed, setup intra process communication. + if (rclcpp::detail::resolve_use_intra_process(options_, *node_base)) { + auto context = node_base->get_context(); + // Get the intra process manager instance for this context. + auto ipm = context->get_sub_context(); + // Register the publisher with the intra process manager. + if (qos.history() != rclcpp::HistoryPolicy::KeepLast) { + throw std::invalid_argument( + "intraprocess communication allowed only with keep last history qos policy"); + } + if (qos.depth() == 0) { + throw std::invalid_argument( + "intraprocess communication is not allowed with a zero qos history depth value"); + } + if (qos.durability() == rclcpp::DurabilityPolicy::TransientLocal) { + buffer_ = rclcpp::experimental::create_intra_process_buffer< + ROSMessageType, ROSMessageTypeAllocator, ROSMessageTypeDeleter>( + rclcpp::detail::resolve_intra_process_buffer_type(options_.intra_process_buffer_type), + qos, + std::make_shared(ros_message_type_allocator_)); + } + uint64_t intra_process_publisher_id = ipm->add_publisher(this->shared_from_this(), buffer_); + this->setup_intra_process( + intra_process_publisher_id, + ipm); + } } virtual ~Publisher() {} - /// Send a message to the topic for this publisher. + /// Borrow a loaned ROS message from the middleware. /** - * This function is templated on the input message type, MessageT. - * \param[in] msg A shared pointer to the message to send. + * If the middleware is capable of loaning memory for a ROS message instance, + * the loaned message will be directly allocated in the middleware. + * If not, the message allocator of this rclcpp::Publisher instance is being used. + * + * With a call to `publish` the LoanedMessage instance is being returned to the middleware + * or free'd accordingly to the allocator. + * If the message is not being published but processed differently, the destructor of this + * class will either return the message to the middleware or deallocate it via the internal + * allocator. + * \sa rclcpp::LoanedMessage for details of the LoanedMessage class. + * + * \return LoanedMessage containing memory for a ROS message of type ROSMessageType */ - virtual void - publish(std::unique_ptr & msg) + rclcpp::LoanedMessage + borrow_loaned_message() { - this->do_inter_process_publish(msg.get()); - if (store_intra_process_message_) { - // Take the pointer from the unique_msg, release it and pass as a void * - // to the ipm. The ipm should then capture it again as a unique_ptr of - // the correct type. - // TODO(wjwwood): - // investigate how to transfer the custom deleter (if there is one) - // from the incoming unique_ptr through to the ipm's unique_ptr. - // See: http://stackoverflow.com/questions/11002641/dynamic-casting-for-unique-ptr - MessageT * msg_ptr = msg.get(); - msg.release(); - uint64_t message_seq = - store_intra_process_message_(intra_process_publisher_id_, msg_ptr, typeid(MessageT)); - rcl_interfaces::msg::IntraProcessMessage ipm; - ipm.publisher_id = intra_process_publisher_id_; - ipm.message_sequence = message_seq; - auto status = rcl_publish(&intra_process_publisher_handle_, &ipm); - if (status != RCL_RET_OK) { - // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here) - throw std::runtime_error( - std::string("failed to publish intra process message: ") + rcl_get_error_string_safe()); - // *INDENT-ON* + return rclcpp::LoanedMessage( + *this, + this->get_ros_message_type_allocator()); + } + + /// Publish a message on the topic. + /** + * This signature is enabled if the element_type of the std::unique_ptr is + * a ROS message type, as opposed to the custom_type of a TypeAdapter, and + * that type matches the type given when creating the publisher. + * + * This signature allows the user to give ownership of the message to rclcpp, + * allowing for more efficient intra-process communication optimizations. + * + * \param[in] msg A unique pointer to the message to send. + */ + template + typename std::enable_if_t< + rosidl_generator_traits::is_message::value && + std::is_same::value + > + publish(std::unique_ptr msg) + { + if (!intra_process_is_enabled_) { + this->do_inter_process_publish(*msg); + return; + } + // If an interprocess subscription exist, then the unique_ptr is promoted + // to a shared_ptr and published. + // This allows doing the intraprocess publish first and then doing the + // interprocess publish, resulting in lower publish-to-subscribe latency. + // It's not possible to do that with an unique_ptr, + // as do_intra_process_publish takes the ownership of the message. + bool inter_process_publish_needed = + get_subscription_count() > get_intra_process_subscription_count(); + + if (inter_process_publish_needed) { + auto shared_msg = + this->do_intra_process_ros_message_publish_and_return_shared(std::move(msg)); + if (buffer_) { + buffer_->add_shared(shared_msg); } + this->do_inter_process_publish(*shared_msg); } else { - // Always destroy the message, even if we don't consume it, for consistency. - msg.reset(); + if (buffer_) { + auto shared_msg = + this->do_intra_process_ros_message_publish_and_return_shared(std::move(msg)); + buffer_->add_shared(shared_msg); + } else { + this->do_intra_process_ros_message_publish(std::move(msg)); + } } } - virtual void - publish(const std::shared_ptr & msg) + /// Publish a message on the topic. + /** + * This signature is enabled if the object being published is + * a ROS message type, as opposed to the custom_type of a TypeAdapter, and + * that type matches the type given when creating the publisher. + * + * This signature allows the user to give a reference to a message, which is + * copied onto the heap without modification so that a copy can be owned by + * rclcpp and ownership of the copy can be moved later if needed. + * + * \param[in] msg A const reference to the message to send. + */ + template + typename std::enable_if_t< + rosidl_generator_traits::is_message::value && + std::is_same::value + > + publish(const T & msg) { // Avoid allocating when not using intra process. - if (!store_intra_process_message_) { - // In this case we're not using intra process. - return this->do_inter_process_publish(msg.get()); + if (!intra_process_is_enabled_) { + this->do_inter_process_publish(msg); + return; } // Otherwise we have to allocate memory in a unique_ptr and pass it along. - // TODO(wjwwood): - // The intra process manager should probably also be able to store - // shared_ptr's and do the "smart" thing based on other intra process - // subscriptions. For now call the other publish(). - auto ptr = MessageAllocTraits::allocate(*message_allocator_.get(), 1); - MessageAllocTraits::construct(*message_allocator_.get(), ptr, *msg.get()); - MessageUniquePtr unique_msg(ptr, message_deleter_); - return this->publish(unique_msg); + // As the message is not const, a copy should be made. + // A shared_ptr could also be constructed here. + auto unique_msg = this->duplicate_ros_message_as_unique_ptr(msg); + this->publish(std::move(unique_msg)); } - virtual void - publish(std::shared_ptr msg) + /// Publish a message on the topic. + /** + * This signature is enabled if this class was created with a TypeAdapter and + * the element_type of the std::unique_ptr matches the custom_type for the + * TypeAdapter used with this class. + * + * This signature allows the user to give ownership of the message to rclcpp, + * allowing for more efficient intra-process communication optimizations. + * + * \param[in] msg A unique pointer to the message to send. + */ + template + typename std::enable_if_t< + rclcpp::TypeAdapter::is_specialized::value && + std::is_same::value + > + publish(std::unique_ptr msg) { - // Avoid allocating when not using intra process. - if (!store_intra_process_message_) { + if (!intra_process_is_enabled_) { // In this case we're not using intra process. - return this->do_inter_process_publish(msg.get()); + auto ros_msg_ptr = std::make_unique(); + rclcpp::TypeAdapter::convert_to_ros_message(*msg, *ros_msg_ptr); + this->do_inter_process_publish(*ros_msg_ptr); + return; + } + + bool inter_process_publish_needed = + get_subscription_count() > get_intra_process_subscription_count(); + + if (inter_process_publish_needed) { + auto ros_msg_ptr = std::make_shared(); + rclcpp::TypeAdapter::convert_to_ros_message(*msg, *ros_msg_ptr); + this->do_intra_process_publish(std::move(msg)); + this->do_inter_process_publish(*ros_msg_ptr); + if (buffer_) { + buffer_->add_shared(ros_msg_ptr); + } + } else { + if (buffer_) { + auto ros_msg_ptr = std::make_shared(); + rclcpp::TypeAdapter::convert_to_ros_message(*msg, *ros_msg_ptr); + buffer_->add_shared(ros_msg_ptr); + } + this->do_intra_process_publish(std::move(msg)); } - // Otherwise we have to allocate memory in a unique_ptr and pass it along. - // TODO(wjwwood): - // The intra process manager should probably also be able to store - // shared_ptr's and do the "smart" thing based on other intra process - // subscriptions. For now call the other publish(). - auto ptr = MessageAllocTraits::allocate(*message_allocator_.get(), 1); - MessageAllocTraits::construct(*message_allocator_.get(), ptr, *msg.get()); - MessageUniquePtr unique_msg(ptr, message_deleter_); - return this->publish(unique_msg); } - virtual void - publish(const MessageT & msg) + /// Publish a message on the topic. + /** + * This signature is enabled if this class was created with a TypeAdapter and + * the given type matches the custom_type of the TypeAdapter. + * + * This signature allows the user to give a reference to a message, which is + * copied onto the heap without modification so that a copy can be owned by + * rclcpp and ownership of the copy can be moved later if needed. + * + * \param[in] msg A const reference to the message to send. + */ + template + typename std::enable_if_t< + rclcpp::TypeAdapter::is_specialized::value && + std::is_same::value + > + publish(const T & msg) { - // Avoid allocating when not using intra process. - if (!store_intra_process_message_) { - // In this case we're not using intra process. - return this->do_inter_process_publish(&msg); + if (!intra_process_is_enabled_) { + // Convert to the ROS message equivalent and publish it. + auto ros_msg_ptr = std::make_unique(); + rclcpp::TypeAdapter::convert_to_ros_message(msg, *ros_msg_ptr); + this->do_inter_process_publish(*ros_msg_ptr); + return; } + // Otherwise we have to allocate memory in a unique_ptr and pass it along. - auto ptr = MessageAllocTraits::allocate(*message_allocator_.get(), 1); - MessageAllocTraits::construct(*message_allocator_.get(), ptr, msg); - MessageUniquePtr unique_msg(ptr, message_deleter_); - return this->publish(unique_msg); + // As the message is not const, a copy should be made. + // A shared_ptr could also be constructed here. + auto unique_msg = this->duplicate_type_adapt_message_as_unique_ptr(msg); + this->publish(std::move(unique_msg)); + } + + void + publish(const rcl_serialized_message_t & serialized_msg) + { + return this->do_serialized_publish(&serialized_msg); } - virtual void - publish(const MessageT * msg) + void + publish(const SerializedMessage & serialized_msg) { - if (!msg) { - throw std::runtime_error("msg argument is nullptr"); + return this->do_serialized_publish(&serialized_msg.get_rcl_serialized_message()); + } + + /// Publish an instance of a LoanedMessage. + /** + * When publishing a loaned message, the memory for this ROS message will be deallocated + * after being published. + * The instance of the loaned message is no longer valid after this call. + * + * \param loaned_msg The LoanedMessage instance to be published. + */ + void + publish(rclcpp::LoanedMessage && loaned_msg) + { + if (!loaned_msg.is_valid()) { + throw std::runtime_error("loaned message is not valid"); + } + + // verify that publisher supports loaned messages + // TODO(Karsten1987): This case separation has to be done in rclcpp + // otherwise we have to ensure that every middleware implements + // `rmw_publish_loaned_message` explicitly the same way as `rmw_publish` + // by taking a copy of the ros message. + if (this->can_loan_messages()) { + // we release the ownership from the rclpp::LoanedMessage instance + // and let the middleware clean up the memory. + this->do_loaned_message_publish(loaned_msg.release()); + } else { + // we don't release the ownership, let the middleware copy the ros message + // and thus the destructor of rclcpp::LoanedMessage cleans up the memory. + this->publish(loaned_msg.get()); } - return this->publish(*msg); } - std::shared_ptr get_allocator() const + PublishedTypeAllocator + get_published_type_allocator() const + { + return published_type_allocator_; + } + + ROSMessageTypeAllocator + get_ros_message_type_allocator() const { - return message_allocator_; + return ros_message_type_allocator_; } protected: void - do_inter_process_publish(const MessageT * msg) + do_inter_process_publish(const ROSMessageType & msg) { - auto status = rcl_publish(&publisher_handle_, msg); - if (status != RCL_RET_OK) { - // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here) + TRACETOOLS_TRACEPOINT(rclcpp_publish, nullptr, static_cast(&msg)); + auto status = rcl_publish(publisher_handle_.get(), &msg, nullptr); + + if (RCL_RET_PUBLISHER_INVALID == status) { + rcl_reset_error(); // next call will reset error message if not context + if (rcl_publisher_is_valid_except_context(publisher_handle_.get())) { + rcl_context_t * context = rcl_publisher_get_context(publisher_handle_.get()); + if (nullptr != context && !rcl_context_is_valid(context)) { + // publisher is invalid due to context being shutdown + return; + } + } + } + if (RCL_RET_OK != status) { + rclcpp::exceptions::throw_from_rcl_error(status, "failed to publish message"); + } + } + + void + do_serialized_publish(const rcl_serialized_message_t * serialized_msg) + { + if (intra_process_is_enabled_) { + // TODO(Karsten1987): support serialized message passed by intraprocess + throw std::runtime_error("storing serialized messages in intra process is not supported yet"); + } + auto status = rcl_publish_serialized_message(publisher_handle_.get(), serialized_msg, nullptr); + if (RCL_RET_OK != status) { + rclcpp::exceptions::throw_from_rcl_error(status, "failed to publish serialized message"); + } + } + + void + do_loaned_message_publish( + std::unique_ptr> msg) + { + TRACETOOLS_TRACEPOINT(rclcpp_publish, nullptr, static_cast(msg.get())); + auto status = rcl_publish_loaned_message(publisher_handle_.get(), msg.get(), nullptr); + + if (RCL_RET_PUBLISHER_INVALID == status) { + rcl_reset_error(); // next call will reset error message if not context + if (rcl_publisher_is_valid_except_context(publisher_handle_.get())) { + rcl_context_t * context = rcl_publisher_get_context(publisher_handle_.get()); + if (nullptr != context && !rcl_context_is_valid(context)) { + // publisher is invalid due to context being shutdown + return; + } + } + } + if (RCL_RET_OK != status) { + rclcpp::exceptions::throw_from_rcl_error(status, "failed to publish message"); + } + } + + void + do_intra_process_publish(std::unique_ptr msg) + { + auto ipm = weak_ipm_.lock(); + if (!ipm) { throw std::runtime_error( - std::string("failed to publish message: ") + rcl_get_error_string_safe()); - // *INDENT-ON* + "intra process publish called after destruction of intra process manager"); + } + if (!msg) { + throw std::runtime_error("cannot publish msg which is a null pointer"); } + TRACETOOLS_TRACEPOINT( + rclcpp_intra_publish, + static_cast(publisher_handle_.get()), + msg.get()); + + ipm->template do_intra_process_publish( + intra_process_publisher_id_, + std::move(msg), + published_type_allocator_); } - std::shared_ptr message_allocator_; + void + do_intra_process_ros_message_publish(std::unique_ptr msg) + { + auto ipm = weak_ipm_.lock(); + if (!ipm) { + throw std::runtime_error( + "intra process publish called after destruction of intra process manager"); + } + if (!msg) { + throw std::runtime_error("cannot publish msg which is a null pointer"); + } + TRACETOOLS_TRACEPOINT( + rclcpp_intra_publish, + static_cast(publisher_handle_.get()), + msg.get()); + + ipm->template do_intra_process_publish( + intra_process_publisher_id_, + std::move(msg), + ros_message_type_allocator_); + } + + std::shared_ptr + do_intra_process_ros_message_publish_and_return_shared( + std::unique_ptr msg) + { + auto ipm = weak_ipm_.lock(); + if (!ipm) { + throw std::runtime_error( + "intra process publish called after destruction of intra process manager"); + } + if (!msg) { + throw std::runtime_error("cannot publish msg which is a null pointer"); + } + TRACETOOLS_TRACEPOINT( + rclcpp_intra_publish, + static_cast(publisher_handle_.get()), + msg.get()); + + return ipm->template do_intra_process_publish_and_return_shared( + intra_process_publisher_id_, + std::move(msg), + ros_message_type_allocator_); + } + + + /// Return a new unique_ptr using the ROSMessageType of the publisher. + std::unique_ptr + create_ros_message_unique_ptr() + { + auto ptr = ROSMessageTypeAllocatorTraits::allocate(ros_message_type_allocator_, 1); + ROSMessageTypeAllocatorTraits::construct(ros_message_type_allocator_, ptr); + return std::unique_ptr(ptr, ros_message_type_deleter_); + } + + /// Duplicate a given ros message as a unique_ptr. + std::unique_ptr + duplicate_ros_message_as_unique_ptr(const ROSMessageType & msg) + { + auto ptr = ROSMessageTypeAllocatorTraits::allocate(ros_message_type_allocator_, 1); + ROSMessageTypeAllocatorTraits::construct(ros_message_type_allocator_, ptr, msg); + return std::unique_ptr(ptr, ros_message_type_deleter_); + } + + /// Duplicate a given type adapted message as a unique_ptr. + std::unique_ptr + duplicate_type_adapt_message_as_unique_ptr(const PublishedType & msg) + { + auto ptr = PublishedTypeAllocatorTraits::allocate(published_type_allocator_, 1); + PublishedTypeAllocatorTraits::construct(published_type_allocator_, ptr, msg); + return std::unique_ptr(ptr, published_type_deleter_); + } + + /// Copy of original options passed during construction. + /** + * It is important to save a copy of this so that the rmw payload which it + * may contain is kept alive for the duration of the publisher. + */ + const rclcpp::PublisherOptionsWithAllocator options_; + + PublishedTypeAllocator published_type_allocator_; + PublishedTypeDeleter published_type_deleter_; + ROSMessageTypeAllocator ros_message_type_allocator_; + ROSMessageTypeDeleter ros_message_type_deleter_; - MessageDeleter message_deleter_; + BufferSharedPtr buffer_{nullptr}; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/publisher_base.hpp b/rclcpp/include/rclcpp/publisher_base.hpp new file mode 100644 index 0000000000..9a6c398eeb --- /dev/null +++ b/rclcpp/include/rclcpp/publisher_base.hpp @@ -0,0 +1,384 @@ +// 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. + +#ifndef RCLCPP__PUBLISHER_BASE_HPP_ +#define RCLCPP__PUBLISHER_BASE_HPP_ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rcl/publisher.h" + +#include "rclcpp/macros.hpp" +#include "rclcpp/network_flow_endpoint.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/event_handler.hpp" +#include "rclcpp/type_support_decl.hpp" +#include "rclcpp/visibility_control.hpp" +#include "rcpputils/time.hpp" + +namespace rclcpp +{ + +// Forward declaration is used for friend statement. +namespace node_interfaces +{ +class NodeBaseInterface; +class NodeTopicsInterface; +} // namespace node_interfaces + +namespace experimental +{ +/** + * IntraProcessManager is forward declared here, avoiding a circular inclusion between + * `intra_process_manager.hpp` and `publisher_base.hpp`. + */ +class IntraProcessManager; +} // namespace experimental + +class PublisherBase : public std::enable_shared_from_this +{ + friend ::rclcpp::node_interfaces::NodeTopicsInterface; + +public: + RCLCPP_SMART_PTR_DEFINITIONS(PublisherBase) + + /// Default constructor. + /** + * Typically, a publisher is not created through this method, but instead is created through a + * call to `Node::create_publisher`. + * \param[in] node_base A pointer to the NodeBaseInterface for the parent node. + * \param[in] topic The topic that this publisher publishes on. + * \param[in] type_support The type support structure for the type to be published. + * \param[in] publisher_options QoS settings for this publisher. + */ + RCLCPP_PUBLIC + PublisherBase( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + const std::string & topic, + const rosidl_message_type_support_t & type_support, + const rcl_publisher_options_t & publisher_options, + const PublisherEventCallbacks & event_callbacks, + bool use_default_callbacks); + + RCLCPP_PUBLIC + virtual ~PublisherBase(); + + /// Add event handlers for passed in event_callbacks. + RCLCPP_PUBLIC + void + bind_event_callbacks(const PublisherEventCallbacks & event_callbacks, bool use_default_callbacks); + + /// Get the topic that this publisher publishes on. + /** \return The topic name. */ + RCLCPP_PUBLIC + const char * + get_topic_name() const; + + /// Get the queue size for this publisher. + /** \return The queue size. */ + RCLCPP_PUBLIC + size_t + get_queue_size() const; + + /// Get the global identifier for this publisher (used in rmw and by DDS). + /** \return The gid. */ + RCLCPP_PUBLIC + const rmw_gid_t & + get_gid() const; + + /// Get the rcl publisher handle. + /** \return The rcl publisher handle. */ + RCLCPP_PUBLIC + std::shared_ptr + get_publisher_handle(); + + /// Get the rcl publisher handle. + /** \return The rcl publisher handle. */ + RCLCPP_PUBLIC + std::shared_ptr + get_publisher_handle() const; + + /// Get all the QoS event handlers associated with this publisher. + /** \return The map of QoS event handlers. */ + RCLCPP_PUBLIC + const + std::unordered_map> & + get_event_handlers() const; + + /// Get subscription count + /** \return The number of subscriptions. */ + RCLCPP_PUBLIC + size_t + get_subscription_count() const; + + /// Get intraprocess subscription count + /** \return The number of intraprocess subscriptions. */ + RCLCPP_PUBLIC + size_t + get_intra_process_subscription_count() const; + + /// Get if durability is transient local + /** \return If durability is transient local*/ + RCLCPP_PUBLIC + bool + is_durability_transient_local() const; + + /// 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. + * + * \return `true` if the liveliness was asserted successfully, otherwise `false` + */ + RCLCPP_PUBLIC + RCUTILS_WARN_UNUSED + bool + assert_liveliness() const; + + /// Get the actual QoS settings, after the defaults have been determined. + /** + * The actual configuration applied when using RMW_QOS_POLICY_*_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_QOS_POLICY_*_UNKNOWN. + * May throw runtime_error when an unexpected error occurs. + * + * \return The actual qos settings. + */ + RCLCPP_PUBLIC + rclcpp::QoS + get_actual_qos() const; + + /// 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. + */ + RCLCPP_PUBLIC + bool + can_loan_messages() const; + + /// Compare this publisher to a gid. + /** + * Note that this function calls the next function. + * \param[in] gid Reference to a gid. + * \return True if the publisher's gid matches the input. + */ + RCLCPP_PUBLIC + bool + operator==(const rmw_gid_t & gid) const; + + /// Compare this publisher to a pointer gid. + /** + * A wrapper for comparing this publisher's gid to the input using rmw_compare_gids_equal. + * \param[in] gid A pointer to a gid. + * \return True if this publisher's gid matches the input. + */ + RCLCPP_PUBLIC + bool + operator==(const rmw_gid_t * gid) const; + + using IntraProcessManagerSharedPtr = + std::shared_ptr; + + /// Implementation utility function used to setup intra process publishing after creation. + RCLCPP_PUBLIC + void + setup_intra_process( + uint64_t intra_process_publisher_id, + IntraProcessManagerSharedPtr ipm); + + /// Get network flow endpoints + /** + * Describes network flow endpoints that this publisher is sending messages out on + * \return vector of NetworkFlowEndpoint + */ + RCLCPP_PUBLIC + std::vector + get_network_flow_endpoints() const; + + /// Return the lowest available capacity for all subscription buffers. + /** + * For intraprocess communication return the lowest buffer capacity for all subscriptions. + * If intraprocess is disabled or no intraprocess subscriptions present, return maximum of size_t. + * On failure return 0. + * \return lowest buffer capacity for all subscriptions + */ + RCLCPP_PUBLIC + size_t + lowest_available_ipm_capacity() const; + + /// Wait until all published messages are acknowledged or until the specified timeout elapses. + /** + * This method waits until all published messages are acknowledged by all matching + * subscriptions or the given timeout elapses. + * + * If the timeout is negative then this method will block indefinitely until all published + * messages are acknowledged. + * If the timeout is zero then this method will not block, it will check if all published + * messages are acknowledged and return immediately. + * If the timeout is greater than zero, this method will wait until all published messages are + * acknowledged or the timeout elapses. + * + * This method only waits for acknowledgments if the publisher's QoS profile is RELIABLE. + * Otherwise this method will immediately return `true`. + * + * \param[in] timeout the duration to wait for all published messages to be acknowledged. + * \return `true` if all published messages were acknowledged before the given timeout + * elapsed, otherwise `false`. + * \throws rclcpp::exceptions::RCLError if middleware doesn't support or internal error occurs + * \throws std::invalid_argument if timeout is greater than std::chrono::nanoseconds::max() or + * less than std::chrono::nanoseconds::min() + */ + template + bool + wait_for_all_acked( + std::chrono::duration timeout = + std::chrono::duration(-1)) const + { + rcl_duration_value_t rcl_timeout = rcpputils::convert_to_nanoseconds(timeout).count(); + + rcl_ret_t ret = rcl_publisher_wait_for_all_acked(publisher_handle_.get(), rcl_timeout); + if (ret == RCL_RET_OK) { + return true; + } else if (ret == RCL_RET_TIMEOUT) { + return false; + } else { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + } + + /// Set a callback to be called when each new qos event instance occurs. + /** + * The callback receives a size_t which is the number of events that occurred + * since the last time this callback was called. + * Normally this is 1, but can be > 1 if events occurred before any + * callback was set. + * + * Since this callback is called from the middleware, you should aim to make + * it fast and not blocking. + * If you need to do a lot of work or wait for some other event, you should + * spin it off to another thread, otherwise you risk blocking the middleware. + * + * Calling it again will clear any previously set callback. + * + * An exception will be thrown if the callback is not callable. + * + * This function is thread-safe. + * + * If you want more information available in the callback, like the qos event + * or other information, you may use a lambda with captures or std::bind. + * + * \sa rclcpp::EventHandlerBase::set_on_ready_callback + * + * \param[in] callback functor to be called when a new event occurs + * \param[in] event_type identifier for the qos event we want to attach the callback to + */ + void + set_on_new_qos_event_callback( + std::function callback, + rcl_publisher_event_type_t event_type) + { + if (event_handlers_.count(event_type) == 0) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Calling set_on_new_qos_event_callback for non registered publisher event_type"); + return; + } + + if (!callback) { + throw std::invalid_argument( + "The callback passed to set_on_new_qos_event_callback " + "is not callable."); + } + + // The on_ready_callback signature has an extra `int` argument used to disambiguate between + // possible different entities within a generic waitable. + // We hide that detail to users of this method. + std::function new_callback = std::bind(callback, std::placeholders::_1); + event_handlers_[event_type]->set_on_ready_callback(new_callback); + } + + /// Unset the callback registered for new qos events, if any. + void + clear_on_new_qos_event_callback(rcl_publisher_event_type_t event_type) + { + if (event_handlers_.count(event_type) == 0) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Calling clear_on_new_qos_event_callback for non registered event_type"); + return; + } + + event_handlers_[event_type]->clear_on_ready_callback(); + } + +protected: + template + void + add_event_handler( + const EventCallbackT & callback, + const rcl_publisher_event_type_t event_type) + { + auto handler = std::make_shared>>( + callback, + rcl_publisher_event_init, + publisher_handle_, + event_type); + event_handlers_.insert(std::make_pair(event_type, handler)); + } + + RCLCPP_PUBLIC + void default_incompatible_qos_callback(QOSOfferedIncompatibleQoSInfo & info) const; + + RCLCPP_PUBLIC + void default_incompatible_type_callback(IncompatibleTypeInfo & info) const; + + std::shared_ptr rcl_node_handle_; + + std::shared_ptr publisher_handle_; + + std::unordered_map> event_handlers_; + + using IntraProcessManagerWeakPtr = + std::weak_ptr; + bool intra_process_is_enabled_; + IntraProcessManagerWeakPtr weak_ipm_; + uint64_t intra_process_publisher_id_; + + rmw_gid_t rmw_gid_; + + const rosidl_message_type_support_t type_support_; + + const PublisherEventCallbacks event_callbacks_; +}; + +} // namespace rclcpp + +#endif // RCLCPP__PUBLISHER_BASE_HPP_ diff --git a/rclcpp/include/rclcpp/publisher_factory.hpp b/rclcpp/include/rclcpp/publisher_factory.hpp index def1bc3e53..87def3cc17 100644 --- a/rclcpp/include/rclcpp/publisher_factory.hpp +++ b/rclcpp/include/rclcpp/publisher_factory.hpp @@ -24,8 +24,10 @@ #include "rosidl_typesupport_cpp/message_type_support.hpp" #include "rclcpp/publisher.hpp" -#include "rclcpp/intra_process_manager.hpp" +#include "rclcpp/publisher_base.hpp" +#include "rclcpp/publisher_options.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/qos.hpp" #include "rclcpp/visibility_control.hpp" namespace rclcpp @@ -41,103 +43,44 @@ namespace rclcpp * called from a templated "create_publisher" method on the Node class, and * is passed to the non-templated "create_publisher" method on the NodeTopics * class where it is used to create and setup the Publisher. + * + * It also handles the two step construction of Publishers, first calling + * the constructor and then the post_init_setup() method. */ struct PublisherFactory { // Creates a PublisherT publisher object and returns it as a PublisherBase. using PublisherFactoryFunction = std::function< - rclcpp::PublisherBase::SharedPtr( - rclcpp::node_interfaces::NodeBaseInterface * node_base, - const std::string & topic_name, - rcl_publisher_options_t & publisher_options)>; - - PublisherFactoryFunction create_typed_publisher; - - // Adds the PublisherBase to the intraprocess manager with the correctly - // templated call to IntraProcessManager::store_intra_process_message. - using AddPublisherToIntraProcessManagerFunction = std::function< - uint64_t( - rclcpp::intra_process_manager::IntraProcessManager * ipm, - rclcpp::PublisherBase::SharedPtr publisher)>; - - AddPublisherToIntraProcessManagerFunction add_publisher_to_intra_process_manager; + rclcpp::PublisherBase::SharedPtr( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + const std::string & topic_name, + const rclcpp::QoS & qos + )>; - // Creates the callback function which is called on each - // PublisherT::publish() and which handles the intra process transmission of - // the message being published. - using SharedPublishCallbackFactoryFunction = std::function< - rclcpp::PublisherBase::StoreMessageCallbackT( - rclcpp::intra_process_manager::IntraProcessManager::SharedPtr ipm)>; - - SharedPublishCallbackFactoryFunction create_shared_publish_callback; + const PublisherFactoryFunction create_typed_publisher; }; -/// Return a PublisherFactory with functions setup for creating a PublisherT. -template +/// Return a PublisherFactory with functions setup for creating a PublisherT. +template PublisherFactory -create_publisher_factory(std::shared_ptr allocator) +create_publisher_factory(const rclcpp::PublisherOptionsWithAllocator & options) { - PublisherFactory factory; - - // factory function that creates a MessageT specific PublisherT - factory.create_typed_publisher = - [allocator]( - rclcpp::node_interfaces::NodeBaseInterface * node_base, - const std::string & topic_name, - rcl_publisher_options_t & publisher_options) -> std::shared_ptr + PublisherFactory factory { + // factory function that creates a MessageT specific PublisherT + [options]( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + const std::string & topic_name, + const rclcpp::QoS & qos + ) -> std::shared_ptr { - auto message_alloc = std::make_shared(*allocator.get()); - publisher_options.allocator = allocator::get_rcl_allocator(*message_alloc.get()); - - return std::make_shared(node_base, topic_name, publisher_options, message_alloc); - }; - - // function to add a publisher to the intra process manager - factory.add_publisher_to_intra_process_manager = - []( - rclcpp::intra_process_manager::IntraProcessManager * ipm, - rclcpp::PublisherBase::SharedPtr publisher) -> uint64_t - { - return ipm->add_publisher(std::dynamic_pointer_cast(publisher)); - }; - - // function to create a shared publish callback std::function - using StoreMessageCallbackT = rclcpp::PublisherBase::StoreMessageCallbackT; - factory.create_shared_publish_callback = - [](rclcpp::intra_process_manager::IntraProcessManager::SharedPtr ipm) -> StoreMessageCallbackT - { - rclcpp::intra_process_manager::IntraProcessManager::WeakPtr weak_ipm = ipm; - - // this function is called on each call to publish() and handles storing - // of the published message in the intra process manager - auto shared_publish_callback = - [weak_ipm](uint64_t publisher_id, void * msg, const std::type_info & type_info) -> uint64_t - { - auto ipm = weak_ipm.lock(); - if (!ipm) { - // TODO(wjwwood): should this just return silently? Or maybe return with a warning? - throw std::runtime_error( - "intra process publish called after destruction of intra process manager"); - } - if (!msg) { - throw std::runtime_error("cannot publisher msg which is a null pointer"); - } - auto & message_type_info = typeid(MessageT); - if (message_type_info != type_info) { - throw std::runtime_error( - std::string("published type '") + type_info.name() + - "' is incompatible from the publisher type '" + message_type_info.name() + "'"); - } - MessageT * typed_message_ptr = static_cast(msg); - using MessageDeleter = typename Publisher::MessageDeleter; - std::unique_ptr unique_msg(typed_message_ptr); - uint64_t message_seq = - ipm->store_intra_process_message(publisher_id, unique_msg); - return message_seq; - }; - - return shared_publish_callback; - }; + auto publisher = std::make_shared(node_base, topic_name, qos, options); + // This is used for setting up things like intra process comms which + // require this->shared_from_this() which cannot be called from + // the constructor. + publisher->post_init_setup(node_base, topic_name, qos, options); + return publisher; + } + }; // return the factory now that it is populated return factory; diff --git a/rclcpp/include/rclcpp/publisher_options.hpp b/rclcpp/include/rclcpp/publisher_options.hpp new file mode 100644 index 0000000000..01fd314f49 --- /dev/null +++ b/rclcpp/include/rclcpp/publisher_options.hpp @@ -0,0 +1,146 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__PUBLISHER_OPTIONS_HPP_ +#define RCLCPP__PUBLISHER_OPTIONS_HPP_ + +#include +#include +#include +#include + +#include "rcl/publisher.h" + +#include "rclcpp/allocator/allocator_common.hpp" +#include "rclcpp/detail/rmw_implementation_specific_publisher_payload.hpp" +#include "rclcpp/intra_process_buffer_type.hpp" +#include "rclcpp/intra_process_setting.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/event_handler.hpp" +#include "rclcpp/qos_overriding_options.hpp" + +namespace rclcpp +{ + +class CallbackGroup; + +/// Non-templated part of PublisherOptionsWithAllocator. +struct PublisherOptionsBase +{ + /// Setting to explicitly set intraprocess communications. + IntraProcessSetting use_intra_process_comm = IntraProcessSetting::NodeDefault; + + /// Setting the data-type stored in the intraprocess buffer + IntraProcessBufferType intra_process_buffer_type = IntraProcessBufferType::SharedPtr; + + /// Callbacks for various events related to publishers. + PublisherEventCallbacks event_callbacks; + + /// Whether or not to use default callbacks when user doesn't supply any in event_callbacks + bool use_default_callbacks = true; + + /// Require middleware to generate unique network flow endpoints + /// Disabled by default + rmw_unique_network_flow_endpoints_requirement_t require_unique_network_flow_endpoints = + RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_NOT_REQUIRED; + + /// Callback group in which the waitable items from the publisher should be placed. + std::shared_ptr callback_group; + + /// Optional RMW implementation specific payload to be used during creation of the publisher. + std::shared_ptr + rmw_implementation_payload = nullptr; + + QosOverridingOptions qos_overriding_options; +}; + +/// Structure containing optional configuration for Publishers. +template +struct PublisherOptionsWithAllocator : public PublisherOptionsBase +{ + static_assert( + std::is_void_v::value_type>, + "Publisher allocator value type must be void"); + + /// Optional custom allocator. + std::shared_ptr allocator = nullptr; + + PublisherOptionsWithAllocator() {} + + /// Constructor using base class as input. + explicit PublisherOptionsWithAllocator(const PublisherOptionsBase & publisher_options_base) + : PublisherOptionsBase(publisher_options_base) + {} + + /// Convert this class, and a rclcpp::QoS, into an rcl_publisher_options_t. + template + rcl_publisher_options_t + to_rcl_publisher_options(const rclcpp::QoS & qos) const + { + rcl_publisher_options_t result = rcl_publisher_get_default_options(); + result.allocator = this->get_rcl_allocator(); + result.qos = qos.get_rmw_qos_profile(); + result.rmw_publisher_options.require_unique_network_flow_endpoints = + this->require_unique_network_flow_endpoints; + + // Apply payload to rcl_publisher_options if necessary. + if (rmw_implementation_payload && rmw_implementation_payload->has_been_customized()) { + rmw_implementation_payload->modify_rmw_publisher_options(result.rmw_publisher_options); + } + + return result; + } + + + /// Get the allocator, creating one if needed. + std::shared_ptr + get_allocator() const + { + if (!this->allocator) { + if (!allocator_storage_) { + allocator_storage_ = std::make_shared(); + } + return allocator_storage_; + } + return this->allocator; + } + +private: + using PlainAllocator = + typename std::allocator_traits::template rebind_alloc; + + rcl_allocator_t + get_rcl_allocator() const + { + if (!plain_allocator_storage_) { + plain_allocator_storage_ = + std::make_shared(*this->get_allocator()); + } + return rclcpp::allocator::get_rcl_allocator(*plain_allocator_storage_); + } + + // This is a temporal workaround, to make sure that get_allocator() + // always returns a copy of the same allocator. + mutable std::shared_ptr allocator_storage_; + + // This is a temporal workaround, to keep the plain allocator that backs + // up the rcl allocator returned in rcl_publisher_options_t alive. + mutable std::shared_ptr plain_allocator_storage_; +}; + +using PublisherOptions = PublisherOptionsWithAllocator>; + +} // namespace rclcpp + +#endif // RCLCPP__PUBLISHER_OPTIONS_HPP_ diff --git a/rclcpp/include/rclcpp/qos.hpp b/rclcpp/include/rclcpp/qos.hpp new file mode 100644 index 0000000000..2ad49487c5 --- /dev/null +++ b/rclcpp/include/rclcpp/qos.hpp @@ -0,0 +1,536 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__QOS_HPP_ +#define RCLCPP__QOS_HPP_ + +#include + +#include "rclcpp/duration.hpp" +#include "rclcpp/exceptions.hpp" +#include "rclcpp/visibility_control.hpp" +#include "rcl/logging_rosout.h" +#include "rmw/incompatible_qos_events_statuses.h" +#include "rmw/qos_profiles.h" +#include "rmw/types.h" + +namespace rclcpp +{ + +RCLCPP_PUBLIC +std::string qos_policy_name_from_kind(rmw_qos_policy_kind_t policy_kind); + +enum class HistoryPolicy +{ + KeepLast = RMW_QOS_POLICY_HISTORY_KEEP_LAST, + KeepAll = RMW_QOS_POLICY_HISTORY_KEEP_ALL, + SystemDefault = RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT, + Unknown = RMW_QOS_POLICY_HISTORY_UNKNOWN, +}; + +enum class ReliabilityPolicy +{ + BestEffort = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, + Reliable = RMW_QOS_POLICY_RELIABILITY_RELIABLE, + SystemDefault = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT, + BestAvailable = RMW_QOS_POLICY_RELIABILITY_BEST_AVAILABLE, + Unknown = RMW_QOS_POLICY_RELIABILITY_UNKNOWN, +}; + +enum class DurabilityPolicy +{ + Volatile = RMW_QOS_POLICY_DURABILITY_VOLATILE, + TransientLocal = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, + SystemDefault = RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT, + BestAvailable = RMW_QOS_POLICY_DURABILITY_BEST_AVAILABLE, + Unknown = RMW_QOS_POLICY_DURABILITY_UNKNOWN, +}; + +enum class LivelinessPolicy +{ + Automatic = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC, + ManualByTopic = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC, + SystemDefault = RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, + BestAvailable = RMW_QOS_POLICY_LIVELINESS_BEST_AVAILABLE, + Unknown = RMW_QOS_POLICY_LIVELINESS_UNKNOWN, +}; + +enum class QoSCompatibility +{ + Ok = RMW_QOS_COMPATIBILITY_OK, + Warning = RMW_QOS_COMPATIBILITY_WARNING, + Error = RMW_QOS_COMPATIBILITY_ERROR, +}; + +/// QoS initialization values, cannot be created directly, use KeepAll or KeepLast instead. +struct RCLCPP_PUBLIC QoSInitialization +{ + rmw_qos_history_policy_t history_policy; + size_t depth; + + /// Constructor which takes both a history policy and a depth (even if it would be unused). + QoSInitialization( + rmw_qos_history_policy_t history_policy_arg, size_t depth_arg, + bool print_depth_warning = true); + + /// Create a QoSInitialization from an existing rmw_qos_profile_t, using its history and depth. + static + QoSInitialization + from_rmw(const rmw_qos_profile_t & rmw_qos); +}; + +/// Use to initialize the QoS with the keep_all history setting. +struct RCLCPP_PUBLIC KeepAll : public rclcpp::QoSInitialization +{ + KeepAll(); +}; + +/// Use to initialize the QoS with the keep_last history setting and the given depth. +struct RCLCPP_PUBLIC KeepLast : public rclcpp::QoSInitialization +{ + explicit KeepLast(size_t depth, bool print_depth_warning = true); +}; + +/// Encapsulation of Quality of Service settings. +/** + * Quality of Service settings control the behavior of publishers, subscriptions, + * and other entities, and includes things like how data is sent or resent, + * how data is buffered on the publishing and subscribing side, and other things. + * See: + * + * https://docs.ros.org/en/rolling/Concepts/About-Quality-of-Service-Settings.html + * + */ +class RCLCPP_PUBLIC QoS +{ +public: + /// Create a QoS by specifying only the history policy and history depth. + /** + * When using the default initial profile, the defaults will include: + * + * - \link rclcpp::ReliabilityPolicy::Reliable ReliabilityPolicy::Reliable\endlink + * - \link rclcpp::DurabilityPolicy::Volatile DurabilityPolicy::Volatile\endlink + * + * See rmw_qos_profile_default for a full list of default settings. + * If some other rmw_qos_profile_t is passed to initial_profile, then the defaults will derive from + * that profile instead. + * + * \param[in] qos_initialization Specifies history policy and history depth. + * \param[in] initial_profile The rmw_qos_profile_t instance on which to base the default settings. + */ + explicit + QoS( + const QoSInitialization & qos_initialization, + const rmw_qos_profile_t & initial_profile = rmw_qos_profile_default); + + /// Conversion constructor to ease construction in the common case of just specifying depth. + /** + * This is a convenience constructor that calls QoS(KeepLast(history_depth)). + * + * \param[in] history_depth How many messages can be queued when publishing + * with a Publisher, or how many messages can be queued before being replaced + * by a Subscription. + */ + // cppcheck-suppress noExplicitConstructor + QoS(size_t history_depth); // NOLINT(runtime/explicit): conversion constructor + + /// Return the rmw qos profile. + rmw_qos_profile_t & + get_rmw_qos_profile(); + + /// Return the rmw qos profile. + const rmw_qos_profile_t & + get_rmw_qos_profile() const; + + /// Set the history policy. + QoS & + history(HistoryPolicy history); + + /// Set the history policy. + QoS & + history(rmw_qos_history_policy_t history); + + /// Set the history to keep last. + QoS & + keep_last(size_t depth); + + /// Set the history to keep all. + QoS & + keep_all(); + + /// Set the reliability setting. + QoS & + reliability(rmw_qos_reliability_policy_t reliability); + + /// Set the reliability setting. + QoS & + reliability(ReliabilityPolicy reliability); + + /// Set the reliability setting to reliable. + QoS & + reliable(); + + /// Set the reliability setting to best effort. + QoS & + best_effort(); + + /// Set the reliability setting to best available. + QoS & + reliability_best_available(); + + /// Set the durability setting. + QoS & + durability(rmw_qos_durability_policy_t durability); + + /// Set the durability setting. + QoS & + durability(DurabilityPolicy durability); + + /// Set the durability setting to volatile. + /** + * Note that this cannot be named `volatile` because it is a C++ keyword. + */ + QoS & + durability_volatile(); + + /// Set the durability setting to transient local. + QoS & + transient_local(); + + /// Set the durability setting to best available. + QoS & + durability_best_available(); + + /// Set the deadline setting. + QoS & + deadline(rmw_time_t deadline); + + /// Set the deadline setting, rclcpp::Duration. + QoS & + deadline(const rclcpp::Duration & deadline); + + /// Set the lifespan setting. + QoS & + lifespan(rmw_time_t lifespan); + + /// Set the lifespan setting, rclcpp::Duration. + QoS & + lifespan(const rclcpp::Duration & lifespan); + + /// Set the liveliness setting. + QoS & + liveliness(rmw_qos_liveliness_policy_t liveliness); + + /// Set the liveliness setting. + QoS & + liveliness(LivelinessPolicy liveliness); + + /// Set the liveliness_lease_duration setting. + QoS & + liveliness_lease_duration(rmw_time_t liveliness_lease_duration); + + /// Set the liveliness_lease_duration setting, rclcpp::Duration. + QoS & + liveliness_lease_duration(const rclcpp::Duration & liveliness_lease_duration); + + /// Set the avoid_ros_namespace_conventions setting. + QoS & + avoid_ros_namespace_conventions(bool avoid_ros_namespace_conventions); + + /// Get the history qos policy. + HistoryPolicy + history() const; + + /// Get the history depth. + size_t + depth() const; + + /// Get the reliability policy. + ReliabilityPolicy + reliability() const; + + /// Get the durability policy. + DurabilityPolicy + durability() const; + + /// Get the deadline duration setting. + rclcpp::Duration + deadline() const; + + /// Get the lifespan duration setting. + rclcpp::Duration + lifespan() const; + + /// Get the liveliness policy. + LivelinessPolicy + liveliness() const; + + /// Get the liveliness lease duration setting. + rclcpp::Duration + liveliness_lease_duration() const; + + /// Get the `avoid ros namespace convention` setting. + bool + avoid_ros_namespace_conventions() const; + +private: + rmw_qos_profile_t rmw_qos_profile_; +}; + +/// Check if two QoS profiles are exactly equal in all policy values. +RCLCPP_PUBLIC +bool operator==(const QoS & left, const QoS & right); +RCLCPP_PUBLIC +bool operator!=(const QoS & left, const QoS & right); + +/// Result type for checking QoS compatibility +/** + * \see rclcpp::qos_check_compatible() + */ +struct QoSCheckCompatibleResult +{ + /// Compatibility result. + QoSCompatibility compatibility; + + /// Reason for a (possible) incompatibility. + /** + * Set if compatiblity is QoSCompatibility::Warning or QoSCompatiblity::Error. + * Not set if the QoS profiles are compatible. + */ + std::string reason; +}; + +/// Check if two QoS profiles are compatible. +/** + * Two QoS profiles are compatible if a publisher and subcription + * using the QoS policies can communicate with each other. + * + * If any policies have value "system default" or "unknown" then it is possible that + * compatiblity cannot be determined. + * In this case, the value QoSCompatility::Warning is set as part of + * the returned structure. + * + * Example usage: + * + * ```cpp + * rclcpp::QoSCheckCompatibleResult result = rclcpp::qos_check_compatible( + * publisher_qos, subscription_qos); + * if (rclcpp::QoSCompatibility::Error != result.compatibility) { + * // QoS not compatible ... + * // result.reason contains info about the incompatibility + * } else if (rclcpp::QoSCompatibility::Warning != result.compatibility) { + * // QoS may not be compatible ... + * // result.reason contains info about the possible incompatibility + * } + * ``` + * + * \param[in] publisher_qos: The QoS profile for a publisher. + * \param[in] subscription_qos: The QoS profile for a subscription. + * \return Struct with compatiblity set to QoSCompatibility::Ok if the QoS profiles are + * compatible, or + * \return Struct with compatibility set to QoSCompatibility::Warning if there is a chance + * the QoS profiles are not compatible, or + * \return Struct with compatibility set to QoSCompatibility::Error if the QoS profiles are + * not compatible. + * \throws rclcpp::exceptions::QoSCheckCompatibilityException if an unexpected error occurs. + */ +RCLCPP_PUBLIC +QoSCheckCompatibleResult +qos_check_compatible(const QoS & publisher_qos, const QoS & subscription_qos); + +/** + * Clock QoS class + * - History: Keep last, + * - Depth: 1, + * - Reliability: Best effort, + * - Durability: Volatile, + * - Deadline: Default, + * - Lifespan: Default, + * - Liveliness: System default, + * - Liveliness lease duration: default, + * - avoid ros namespace conventions: false + */ +class RCLCPP_PUBLIC ClockQoS : public QoS +{ +public: + explicit + ClockQoS( + const QoSInitialization & qos_initialization = KeepLast(1)); +}; + +/** + * Sensor Data QoS class + * - History: Keep last, + * - Depth: 5, + * - Reliability: Best effort, + * - Durability: Volatile, + * - Deadline: Default, + * - Lifespan: Default, + * - Liveliness: System default, + * - Liveliness lease duration: default, + * - avoid ros namespace conventions: false + */ +class RCLCPP_PUBLIC SensorDataQoS : public QoS +{ +public: + explicit + SensorDataQoS( + const QoSInitialization & qos_initialization = ( + QoSInitialization::from_rmw(rmw_qos_profile_sensor_data) + )); +}; + +/** + * Parameters QoS class + * - History: Keep last, + * - Depth: 1000, + * - Reliability: Reliable, + * - Durability: Volatile, + * - Deadline: Default, + * - Lifespan: Default, + * - Liveliness: System default, + * - Liveliness lease duration: default, + * - Avoid ros namespace conventions: false + */ +class RCLCPP_PUBLIC ParametersQoS : public QoS +{ +public: + explicit + ParametersQoS( + const QoSInitialization & qos_initialization = ( + QoSInitialization::from_rmw(rmw_qos_profile_parameters) + )); +}; + +/** + * Services QoS class + * - History: Keep last, + * - Depth: 10, + * - Reliability: Reliable, + * - Durability: Volatile, + * - Deadline: Default, + * - Lifespan: Default, + * - Liveliness: System default, + * - Liveliness lease duration: default, + * - Avoid ros namespace conventions: false + */ +class RCLCPP_PUBLIC ServicesQoS : public QoS +{ +public: + explicit + ServicesQoS( + const QoSInitialization & qos_initialization = ( + QoSInitialization::from_rmw(rmw_qos_profile_services_default) + )); +}; + +/** + * Parameter events QoS class + * - History: Keep last, + * - Depth: 1000, + * - Reliability: Reliable, + * - Durability: Volatile, + * - Deadline: Default, + * - Lifespan: Default, + * - Liveliness: System default, + * - Liveliness lease duration: default, + * - Avoid ros namespace conventions: false + */ +class RCLCPP_PUBLIC ParameterEventsQoS : public QoS +{ +public: + explicit + ParameterEventsQoS( + const QoSInitialization & qos_initialization = ( + QoSInitialization::from_rmw(rmw_qos_profile_parameter_events) + )); +}; + +/** + * Rosout QoS class + * - History: Keep last, + * - Depth: 1000, + * - Reliability: Reliable, + * - Durability: TRANSIENT_LOCAL, + * - Deadline: Default, + * - Lifespan: {10, 0}, + * - Liveliness: System default, + * - Liveliness lease duration: default, + * - Avoid ros namespace conventions: false + */ +class RCLCPP_PUBLIC RosoutQoS : public QoS +{ +public: + explicit + RosoutQoS( + const QoSInitialization & rosout_qos_initialization = ( + QoSInitialization::from_rmw(rcl_qos_profile_rosout_default) + )); +}; + +/** + * System defaults QoS class + * - History: System default, + * - Depth: System default, + * - Reliability: System default, + * - Durability: System default, + * - Deadline: Default, + * - Lifespan: Default, + * - Liveliness: System default, + * - Liveliness lease duration: System default, + * - Avoid ros namespace conventions: false + */ +class RCLCPP_PUBLIC SystemDefaultsQoS : public QoS +{ +public: + explicit + SystemDefaultsQoS( + const QoSInitialization & qos_initialization = ( + QoSInitialization::from_rmw(rmw_qos_profile_system_default) + )); +}; + +/** + * Best available QoS class + * + * Match majority of endpoints currently available while maintaining the highest level of service. + * Policies are chosen at the time of creating a subscription or publisher. + * The middleware is not expected to update policies after creating a subscription or publisher, + * even if one or more policies are incompatible with newly discovered endpoints. + * Therefore, this profile should be used with care since non-deterministic behavior can occur due + * to races with discovery. + * + * - History: Keep last, + * - Depth: 10, + * - Reliability: Best available, + * - Durability: Best available, + * - Deadline: Best available, + * - Lifespan: Default, + * - Liveliness: Best available, + * - Liveliness lease duration: Best available, + * - avoid ros namespace conventions: false + */ +class RCLCPP_PUBLIC BestAvailableQoS : public QoS +{ +public: + explicit + BestAvailableQoS( + const QoSInitialization & qos_initialization = ( + QoSInitialization::from_rmw(rmw_qos_profile_best_available) + )); +}; + +} // namespace rclcpp + +#endif // RCLCPP__QOS_HPP_ diff --git a/rclcpp/include/rclcpp/qos_overriding_options.hpp b/rclcpp/include/rclcpp/qos_overriding_options.hpp new file mode 100644 index 0000000000..c20248de6e --- /dev/null +++ b/rclcpp/include/rclcpp/qos_overriding_options.hpp @@ -0,0 +1,154 @@ +// 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 RCLCPP__QOS_OVERRIDING_OPTIONS_HPP_ +#define RCLCPP__QOS_OVERRIDING_OPTIONS_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "rclcpp/qos.hpp" +#include "rclcpp/visibility_control.hpp" + +#include "rcl_interfaces/msg/set_parameters_result.hpp" +#include "rmw/qos_policy_kind.h" + +namespace rclcpp +{ + +enum class RCLCPP_PUBLIC_TYPE QosPolicyKind +{ + AvoidRosNamespaceConventions = RMW_QOS_POLICY_AVOID_ROS_NAMESPACE_CONVENTIONS, + Deadline = RMW_QOS_POLICY_DEADLINE, + Depth = RMW_QOS_POLICY_DEPTH, + Durability = RMW_QOS_POLICY_DURABILITY, + History = RMW_QOS_POLICY_HISTORY, + Lifespan = RMW_QOS_POLICY_LIFESPAN, + Liveliness = RMW_QOS_POLICY_LIVELINESS, + LivelinessLeaseDuration = RMW_QOS_POLICY_LIVELINESS_LEASE_DURATION, + Reliability = RMW_QOS_POLICY_RELIABILITY, + Invalid = RMW_QOS_POLICY_INVALID, +}; + +RCLCPP_PUBLIC +const char * +qos_policy_kind_to_cstr(const QosPolicyKind & qpk); + +RCLCPP_PUBLIC +std::ostream & +operator<<(std::ostream & os, const QosPolicyKind & qpk); + +using QosCallbackResult = rcl_interfaces::msg::SetParametersResult; +using QosCallback = std::function; + +namespace detail +{ +// forward declare +template +class QosParameters; +} + +/// Options that are passed in subscription/publisher constructor to specify QoSConfigurability. +/** + * This options struct allows configuring: + * - Which policy kinds will have declared parameters. + * - An optional callback, that will be called to validate the final qos profile. + * - An optional id. In the case that different qos are desired for two publishers/subscriptions in + * the same topic, this id will allow disambiguating them. + * + * Example parameter file: + * + * ```yaml + * my_node_name: + * ros__parameters: + * qos_overrides: + * /my/topic/name: + * publisher: # publisher without provided id + * reliability: reliable + * depth: 100 + * publisher_my_id: # publisher with `id="my_id" + * reliability: reliable + * depth: 10 + * ``` + */ +class QosOverridingOptions +{ +public: + /// Default constructor, no overrides allowed. + RCLCPP_PUBLIC + QosOverridingOptions() = default; + + /// Construct passing a list of QoS policies and a verification callback. + /** + * This constructor is implicit, e.g.: + * ```cpp + * node->create_publisher( + * "topic_name", + * default_qos_profile, + * { + * {QosPolicyKind::Reliability}, + * [] (auto && qos) {return check_qos_validity(qos)}, + * "my_id" + * }); + * ``` + * \param policy_kinds list of policy kinds that will be reconfigurable. + * \param validation_callback callbak that will be called to validate the validity of + * the qos profile set by the user. + * \param id id of the entity. + */ + RCLCPP_PUBLIC + QosOverridingOptions( + std::initializer_list policy_kinds, + QosCallback validation_callback = nullptr, + std::string id = {}); + + RCLCPP_PUBLIC + const std::string & + get_id() const; + + RCLCPP_PUBLIC + const std::vector & + get_policy_kinds() const; + + RCLCPP_PUBLIC + const QosCallback & + get_validation_callback() const; + + /// Construct passing a list of QoS policies and a verification callback. + /** + * Same as `QosOverridingOptions` constructor, but only declares the default policies: + * + * History, Depth, Reliability. + */ + RCLCPP_PUBLIC + static + QosOverridingOptions + with_default_policies(QosCallback validation_callback = nullptr, std::string id = {}); + +private: + /// \internal Id of the entity requesting to create parameters. + std::string id_; + /// \internal Policy kinds that are allowed to be reconfigured. + std::vector policy_kinds_; + /// \internal Validation callback that will be called to verify the profile. + QosCallback validation_callback_; +}; + +} // namespace rclcpp + +#endif // RCLCPP__QOS_OVERRIDING_OPTIONS_HPP_ diff --git a/rclcpp/include/rclcpp/rate.hpp b/rclcpp/include/rclcpp/rate.hpp index 296cce14a1..281ddf9de3 100644 --- a/rclcpp/include/rclcpp/rate.hpp +++ b/rclcpp/include/rclcpp/rate.hpp @@ -19,6 +19,8 @@ #include #include +#include "rclcpp/clock.hpp" +#include "rclcpp/duration.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp/visibility_control.hpp" @@ -31,8 +33,16 @@ class RateBase public: RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(RateBase) + RCLCPP_PUBLIC + virtual ~RateBase() {} + + RCLCPP_PUBLIC virtual bool sleep() = 0; - virtual bool is_steady() const = 0; + + RCLCPP_PUBLIC + virtual rcl_clock_type_t get_type() const = 0; + + RCLCPP_PUBLIC virtual void reset() = 0; }; @@ -40,79 +50,54 @@ using std::chrono::duration; using std::chrono::duration_cast; using std::chrono::nanoseconds; -template -class GenericRate : public RateBase +class Rate : public RateBase { public: - RCLCPP_SMART_PTR_DEFINITIONS(GenericRate) + RCLCPP_SMART_PTR_DEFINITIONS(Rate) - explicit GenericRate(double rate) - : GenericRate( - duration_cast(duration(1.0 / rate))) - {} - explicit GenericRate(std::chrono::nanoseconds period) - : period_(period), last_interval_(Clock::now()) - {} + RCLCPP_PUBLIC + explicit Rate( + const double rate, + Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME)); - virtual bool - sleep() - { - // Time coming into sleep - auto now = Clock::now(); - // Time of next interval - auto next_interval = last_interval_ + period_; - // Detect backwards time flow - if (now < last_interval_) { - // Best thing to do is to set the next_interval to now + period - next_interval = now + period_; - } - // Calculate the time to sleep - auto time_to_sleep = next_interval - now; - // Update the interval - last_interval_ += period_; - // If the time_to_sleep is negative or zero, don't sleep - if (time_to_sleep <= std::chrono::seconds(0)) { - // If an entire cycle was missed then reset next interval. - // This might happen if the loop took more than a cycle. - // Or if time jumps forward. - if (now > next_interval + period_) { - last_interval_ = now + period_; - } - // Either way do not sleep and return false - return false; - } - // Sleep (will get interrupted by ctrl-c, may not sleep full time) - rclcpp::sleep_for(time_to_sleep); - return true; - } + RCLCPP_PUBLIC + explicit Rate( + const Duration & period, + Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME)); + RCLCPP_PUBLIC virtual bool - is_steady() const - { - return Clock::is_steady; - } + sleep(); + + RCLCPP_PUBLIC + virtual rcl_clock_type_t + get_type() const; + RCLCPP_PUBLIC virtual void - reset() - { - last_interval_ = Clock::now(); - } + reset(); - std::chrono::nanoseconds period() const - { - return period_; - } + RCLCPP_PUBLIC + std::chrono::nanoseconds + period() const; private: - RCLCPP_DISABLE_COPY(GenericRate) + RCLCPP_DISABLE_COPY(Rate) - std::chrono::nanoseconds period_; - using ClockDurationNano = std::chrono::duration; - std::chrono::time_point last_interval_; + Clock::SharedPtr clock_; + Duration period_; + Time last_interval_; }; -using Rate = GenericRate; -using WallRate = GenericRate; +class WallRate : public Rate +{ +public: + RCLCPP_PUBLIC + explicit WallRate(const double rate); + + RCLCPP_PUBLIC + explicit WallRate(const Duration & period); +}; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/rclcpp.hpp b/rclcpp/include/rclcpp/rclcpp.hpp index 518ecac05f..50af3f1a89 100644 --- a/rclcpp/include/rclcpp/rclcpp.hpp +++ b/rclcpp/include/rclcpp/rclcpp.hpp @@ -48,11 +48,15 @@ * - rclcpp::Node::get_parameter() * - rclcpp::Node::describe_parameters() * - rclcpp::Node::list_parameters() - * - rclcpp::Node::register_param_change_callback() - * - rclcpp::parameter::ParameterVariant + * - rclcpp::Node::add_on_set_parameters_callback() + * - rclcpp::Node::remove_on_set_parameters_callback() + * - rclcpp::Parameter + * - rclcpp::ParameterValue * - rclcpp::AsyncParametersClient * - rclcpp::SyncParametersClient + * - rclcpp::copy_all_parameter_values() * - rclcpp/parameter.hpp + * - rclcpp/parameter_value.hpp * - rclcpp/parameter_client.hpp * - rclcpp/parameter_service.hpp * - Rate: @@ -78,7 +82,7 @@ * - rclcpp/executors/multi_threaded_executor.hpp * - CallbackGroups (mechanism for enforcing concurrency rules for callbacks): * - rclcpp::Node::create_callback_group() - * - rclcpp::callback_group::CallbackGroup + * - rclcpp::CallbackGroup * - rclcpp/callback_group.hpp * * Additionally, there are some methods for introspecting the ROS graph: @@ -92,6 +96,9 @@ * - Get the number of publishers or subscribers on a topic: * - rclcpp::Node::count_publishers() * - rclcpp::Node::count_subscribers() + * - Get the number of clients or servers on a service: + * - rclcpp::Node::count_clients() + * - rclcpp::Node::count_services() * * And components related to logging: * @@ -114,6 +121,27 @@ * - Allocator related items: * - rclcpp/allocator/allocator_common.hpp * - rclcpp/allocator/allocator_deleter.hpp + * - Dynamic typesupport wrappers + * - rclcpp::dynamic_typesupport::DynamicMessage + * - rclcpp::dynamic_typesupport::DynamicMessageType + * - rclcpp::dynamic_typesupport::DynamicMessageTypeBuilder + * - rclcpp::dynamic_typesupport::DynamicSerializationSupport + * - rclcpp/dynamic_typesupport/dynamic_message.hpp + * - rclcpp/dynamic_typesupport/dynamic_message_type.hpp + * - rclcpp/dynamic_typesupport/dynamic_message_type_builder.hpp + * - rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp + * - Dynamic typesupport + * - rclcpp::dynamic_typesupport::DynamicMessageTypeSupport + * - rclcpp/dynamic_typesupport/dynamic_message_type_support.hpp + * - Generic publisher + * - rclcpp::Node::create_generic_publisher() + * - rclcpp::GenericPublisher + * - rclcpp::GenericPublisher::publish() + * - rclcpp/generic_publisher.hpp + * - Generic subscription + * - rclcpp::Node::create_generic_subscription() + * - rclcpp::GenericSubscription + * - rclcpp/generic_subscription.hpp * - Memory management tools: * - rclcpp/memory_strategies.hpp * - rclcpp/memory_strategy.hpp @@ -125,11 +153,12 @@ * - rclcpp/context.hpp * - rclcpp/contexts/default_context.hpp * - Various utilities: + * - rclcpp/duration.hpp * - rclcpp/function_traits.hpp * - rclcpp/macros.hpp - * - rclcpp/scope_exit.hpp * - rclcpp/time.hpp * - rclcpp/utilities.hpp + * - rclcpp/typesupport_helpers.hpp * - rclcpp/visibility_control.hpp */ @@ -139,15 +168,20 @@ #include #include +#include "rclcpp/copy_all_parameter_values.hpp" #include "rclcpp/executors.hpp" +#include "rclcpp/guard_condition.hpp" #include "rclcpp/logging.hpp" #include "rclcpp/node.hpp" -#include "rclcpp/parameter.hpp" #include "rclcpp/parameter_client.hpp" +#include "rclcpp/parameter_event_handler.hpp" +#include "rclcpp/parameter.hpp" #include "rclcpp/parameter_service.hpp" #include "rclcpp/rate.hpp" #include "rclcpp/time.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp/visibility_control.hpp" +#include "rclcpp/waitable.hpp" +#include "rclcpp/wait_set.hpp" #endif // RCLCPP__RCLCPP_HPP_ diff --git a/rclcpp/include/rclcpp/scope_exit.hpp b/rclcpp/include/rclcpp/scope_exit.hpp deleted file mode 100644 index 7865de5e48..0000000000 --- a/rclcpp/include/rclcpp/scope_exit.hpp +++ /dev/null @@ -1,52 +0,0 @@ -// 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. - -// Based on: http://the-witness.net/news/2012/11/scopeexit-in-c11/ -// But I changed the lambda to include by reference rather than value, see: -// http://the-witness.net/news/2012/11/scopeexit-in-c11/comment-page-1/#comment-86873 - -#ifndef RCLCPP__SCOPE_EXIT_HPP_ -#define RCLCPP__SCOPE_EXIT_HPP_ - -#include - -#include "rclcpp/macros.hpp" - -namespace rclcpp -{ - -template -struct ScopeExit -{ - explicit ScopeExit(Callable callable) - : callable_(callable) {} - ~ScopeExit() {callable_();} - -private: - Callable callable_; -}; - -template -ScopeExit -make_scope_exit(Callable callable) -{ - return ScopeExit(callable); -} - -} // namespace rclcpp - -#define RCLCPP_SCOPE_EXIT(code) \ - auto RCLCPP_STRING_JOIN(scope_exit_, __LINE__) = rclcpp::make_scope_exit([&]() {code;}) - -#endif // RCLCPP__SCOPE_EXIT_HPP_ diff --git a/rclcpp/include/rclcpp/serialization.hpp b/rclcpp/include/rclcpp/serialization.hpp new file mode 100644 index 0000000000..862c625217 --- /dev/null +++ b/rclcpp/include/rclcpp/serialization.hpp @@ -0,0 +1,98 @@ +// 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 RCLCPP__SERIALIZATION_HPP_ +#define RCLCPP__SERIALIZATION_HPP_ + +#include +#include +#include + +#include "rclcpp/visibility_control.hpp" + +#include "rcl/types.h" + +#include "rosidl_runtime_c/message_type_support_struct.h" + +#include "rosidl_typesupport_cpp/message_type_support.hpp" + +namespace rclcpp +{ + +class SerializedMessage; + +namespace serialization_traits +{ +// trait to check if type is the object oriented serialized message +template +struct is_serialized_message_class : std::false_type +{}; + +template<> +struct is_serialized_message_class: std::true_type +{}; +} // namespace serialization_traits + +/// Interface to (de)serialize a message +class RCLCPP_PUBLIC_TYPE SerializationBase +{ +public: + /// Constructor of SerializationBase + /** + * \param[in] type_support handle for the message type support + * to be used for serialization and deserialization. + */ + explicit SerializationBase(const rosidl_message_type_support_t * type_support); + + /// Destructor of SerializationBase + virtual ~SerializationBase() = default; + + /// Serialize a ROS2 message to a serialized stream + /** + * \param[in] ros_message The ROS2 message which is read and serialized by rmw. + * \param[out] serialized_message The serialized message. + */ + void serialize_message( + const void * ros_message, SerializedMessage * serialized_message) const; + + /// Deserialize a serialized stream to a ROS message + /** + * \param[in] serialized_message The serialized message to be converted to ROS2 by rmw. + * \param[out] ros_message The deserialized ROS2 message. + */ + void deserialize_message( + const SerializedMessage * serialized_message, void * ros_message) const; + +private: + const rosidl_message_type_support_t * type_support_; +}; + +/// Default implementation to (de)serialize a message by using rmw_(de)serialize +template +class Serialization : public SerializationBase +{ +public: + /// Constructor of Serialization + Serialization() + : SerializationBase(rosidl_typesupport_cpp::get_message_type_support_handle()) + { + static_assert( + !serialization_traits::is_serialized_message_class::value, + "Serialization of serialized message to serialized message is not possible."); + } +}; + +} // namespace rclcpp + +#endif // RCLCPP__SERIALIZATION_HPP_ diff --git a/rclcpp/include/rclcpp/serialized_message.hpp b/rclcpp/include/rclcpp/serialized_message.hpp new file mode 100644 index 0000000000..b8f68dc77e --- /dev/null +++ b/rclcpp/include/rclcpp/serialized_message.hpp @@ -0,0 +1,121 @@ +// 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 RCLCPP__SERIALIZED_MESSAGE_HPP_ +#define RCLCPP__SERIALIZED_MESSAGE_HPP_ + +#include "rcl/allocator.h" +#include "rcl/types.h" + +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ + +/// Object oriented version of rcl_serialized_message_t with destructor to avoid memory leaks +class RCLCPP_PUBLIC_TYPE SerializedMessage +{ +public: + /// Default constructor for a SerializedMessage + /** + * Default constructs a serialized message and initalizes it + * with initial capacity of 0. + * The allocator defaults to `rcl_get_default_allocator()`. + * + * \param[in] allocator The allocator to be used for the initialization. + */ + explicit SerializedMessage( + const rcl_allocator_t & allocator = rcl_get_default_allocator()); + + /// Default constructor for a SerializedMessage + /** + * Default constructs a serialized message and initalizes it + * with the provided capacity. + * The allocator defaults to `rcl_get_default_allocator()`. + * + * \param[in] initial_capacity The amount of memory to be allocated. + * \param[in] allocator The allocator to be used for the initialization. + */ + explicit SerializedMessage( + size_t initial_capacity, + const rcl_allocator_t & allocator = rcl_get_default_allocator()); + + /// Copy Constructor for a SerializedMessage + SerializedMessage(const SerializedMessage & other); + + /// Constructor for a SerializedMessage from a rcl_serialized_message_t + explicit SerializedMessage(const rcl_serialized_message_t & other); + + /// Move Constructor for a SerializedMessage + SerializedMessage(SerializedMessage && other); + + /// Constructor for a SerializedMessage from a moved rcl_serialized_message_t + explicit SerializedMessage(rcl_serialized_message_t && other); + + /// Copy assignment operator + SerializedMessage & operator=(const SerializedMessage & other); + + /// Copy assignment operator from a rcl_serialized_message_t + SerializedMessage & operator=(const rcl_serialized_message_t & other); + + /// Move assignment operator + SerializedMessage & operator=(SerializedMessage && other); + + /// Move assignment operator from a rcl_serialized_message_t + SerializedMessage & operator=(rcl_serialized_message_t && other); + + /// Destructor for a SerializedMessage + virtual ~SerializedMessage(); + + /// Get the underlying rcl_serialized_t handle + rcl_serialized_message_t & get_rcl_serialized_message(); + + // Get a const handle to the underlying rcl_serialized_message_t + const rcl_serialized_message_t & get_rcl_serialized_message() const; + + /// Get the size of the serialized data buffer + /** + * Note, this is different from the actual amount of allocated memory. + * This can be obtained via a call to `capacity`. + */ + size_t size() const; + + /// Get the size of allocated memory for the data buffer + /** + * Note, this is different from the amount of content in the buffer. + * This can be obtained via a call to `size`. + */ + size_t capacity() const; + + /// Allocate memory in the data buffer + /** + * The data buffer of the underlying rcl_serialized_message_t will be resized. + * This might change the data layout and invalidates all pointers to the data. + */ + void reserve(size_t capacity); + + /// Release the underlying rcl_serialized_message_t + /** + * The memory (i.e. the data buffer) of the serialized message will no longer + * be managed by this instance and the memory won't be deallocated on destruction. + */ + rcl_serialized_message_t release_rcl_serialized_message(); + +private: + rcl_serialized_message_t serialized_message_; +}; + +} // namespace rclcpp + +#endif // RCLCPP__SERIALIZED_MESSAGE_HPP_ diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 79038acb98..8296cb5962 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -15,23 +15,35 @@ #ifndef RCLCPP__SERVICE_HPP_ #define RCLCPP__SERVICE_HPP_ +#include #include #include #include +#include #include #include #include "rcl/error_handling.h" +#include "rcl/event_callback.h" #include "rcl/service.h" +#include "rcl/service_introspection.h" + +#include "rmw/error_handling.h" +#include "rmw/impl/cpp/demangle.hpp" +#include "rmw/rmw.h" + +#include "tracetools/tracetools.h" #include "rclcpp/any_service_callback.hpp" +#include "rclcpp/clock.hpp" +#include "rclcpp/detail/cpp_callback_trampoline.hpp" #include "rclcpp/exceptions.hpp" +#include "rclcpp/expand_topic_or_service_name.hpp" +#include "rclcpp/logging.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/qos.hpp" #include "rclcpp/type_support_decl.hpp" -#include "rclcpp/expand_topic_or_service_name.hpp" #include "rclcpp/visibility_control.hpp" -#include "rmw/error_handling.h" -#include "rmw/rmw.h" namespace rclcpp { @@ -42,35 +54,200 @@ class ServiceBase RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ServiceBase) RCLCPP_PUBLIC - ServiceBase( - std::shared_ptr node_handle, - const std::string & service_name); - - RCLCPP_PUBLIC - explicit ServiceBase( - std::shared_ptr node_handle); + explicit ServiceBase(std::shared_ptr node_handle); RCLCPP_PUBLIC - virtual ~ServiceBase(); + virtual ~ServiceBase() = default; + /// Return the name of the service. + /** \return The name of the service. */ RCLCPP_PUBLIC - std::string + const char * get_service_name(); + /// Return the rcl_service_t service handle in a std::shared_ptr. + /** + * This handle remains valid after the Service is destroyed. + * The actual rcl service is not finalized until it is out of scope everywhere. + */ RCLCPP_PUBLIC - rcl_service_t * + std::shared_ptr get_service_handle(); + /// Return the rcl_service_t service handle in a std::shared_ptr. + /** + * This handle remains valid after the Service is destroyed. + * The actual rcl service is not finalized until it is out of scope everywhere. + */ RCLCPP_PUBLIC - const rcl_service_t * + std::shared_ptr get_service_handle() const; - virtual std::shared_ptr create_request() = 0; - virtual std::shared_ptr create_request_header() = 0; - virtual void handle_request( + /// Take the next request from the service as a type erased pointer. + /** + * This type erased version of \sa Service::take_request() is useful when + * using the service in a type agnostic way with methods like + * ServiceBase::create_request(), ServiceBase::create_request_header(), and + * ServiceBase::handle_request(). + * + * \param[out] request_out The type erased pointer to a service request object + * into which the middleware will copy the taken request. + * \param[out] request_id_out The output id for the request which can be used + * to associate response with this request in the future. + * \returns true if the request was taken, otherwise false. + * \throws rclcpp::exceptions::RCLError based exceptions if the underlying + * rcl calls fail. + */ + RCLCPP_PUBLIC + bool + take_type_erased_request(void * request_out, rmw_request_id_t & request_id_out); + + virtual + std::shared_ptr + create_request() = 0; + + virtual + std::shared_ptr + create_request_header() = 0; + + virtual + void + handle_request( std::shared_ptr request_header, std::shared_ptr request) = 0; + /// Exchange the "in use by wait set" state for this service. + /** + * This is used to ensure this service is not used by multiple + * wait sets at the same time. + * + * \param[in] in_use_state the new state to exchange into the state, true + * indicates it is now in use by a wait set, and false is that it is no + * longer in use by a wait set. + * \returns the previous state. + */ + RCLCPP_PUBLIC + bool + exchange_in_use_by_wait_set_state(bool in_use_state); + + /// Get the actual response publisher QoS settings, after the defaults have been determined. + /** + * The actual configuration applied when using RMW_QOS_POLICY_*_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_QOS_POLICY_*_UNKNOWN. + * May throw runtime_error when an unexpected error occurs. + * + * \return The actual response publisher qos settings. + * \throws std::runtime_error if failed to get qos settings + */ + RCLCPP_PUBLIC + rclcpp::QoS + get_response_publisher_actual_qos() const; + + /// Get the actual request subscription QoS settings, after the defaults have been determined. + /** + * The actual configuration applied when using RMW_QOS_POLICY_*_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_QOS_POLICY_*_UNKNOWN. + * May throw runtime_error when an unexpected error occurs. + * + * \return The actual request subscription qos settings. + * \throws std::runtime_error if failed to get qos settings + */ + RCLCPP_PUBLIC + rclcpp::QoS + get_request_subscription_actual_qos() const; + + /// Set a callback to be called when each new request is received. + /** + * The callback receives a size_t which is the number of requests received + * since the last time this callback was called. + * Normally this is 1, but can be > 1 if requests were received before any + * callback was set. + * + * Since this callback is called from the middleware, you should aim to make + * it fast and not blocking. + * If you need to do a lot of work or wait for some other event, you should + * spin it off to another thread, otherwise you risk blocking the middleware. + * + * Calling it again will clear any previously set callback. + * + * + * An exception will be thrown if the callback is not callable. + * + * This function is thread-safe. + * + * If you want more information available in the callback, like the service + * or other information, you may use a lambda with captures or std::bind. + * + * \sa rmw_service_set_on_new_request_callback + * \sa rcl_service_set_on_new_request_callback + * + * \param[in] callback functor to be called when a new request is received + */ + void + set_on_new_request_callback(std::function callback) + { + if (!callback) { + throw std::invalid_argument( + "The callback passed to set_on_new_request_callback " + "is not callable."); + } + + auto new_callback = + [callback, this](size_t number_of_requests) { + try { + callback(number_of_requests); + } catch (const std::exception & exception) { + RCLCPP_ERROR_STREAM( + node_logger_, + "rclcpp::ServiceBase@" << this << + " caught " << rmw::impl::cpp::demangle(exception) << + " exception in user-provided callback for the 'on new request' callback: " << + exception.what()); + } catch (...) { + RCLCPP_ERROR_STREAM( + node_logger_, + "rclcpp::ServiceBase@" << this << + " caught unhandled exception in user-provided callback " << + "for the 'on new request' callback"); + } + }; + + std::lock_guard lock(callback_mutex_); + + // Set it temporarily to the new callback, while we replace the old one. + // This two-step setting, prevents a gap where the old std::function has + // been replaced but the middleware hasn't been told about the new one yet. + set_on_new_request_callback( + rclcpp::detail::cpp_callback_trampoline, + static_cast(&new_callback)); + + // Store the std::function to keep it in scope, also overwrites the existing one. + on_new_request_callback_ = new_callback; + + // Set it again, now using the permanent storage. + set_on_new_request_callback( + rclcpp::detail::cpp_callback_trampoline< + decltype(on_new_request_callback_), const void *, size_t>, + static_cast(&on_new_request_callback_)); + } + + /// Unset the callback registered for new requests, if any. + void + clear_on_new_request_callback() + { + std::lock_guard lock(callback_mutex_); + if (on_new_request_callback_) { + set_on_new_request_callback(nullptr, nullptr); + on_new_request_callback_ = nullptr; + } + } + protected: RCLCPP_DISABLE_COPY(ServiceBase) @@ -82,47 +259,83 @@ class ServiceBase const rcl_node_t * get_rcl_node_handle() const; + RCLCPP_PUBLIC + void + set_on_new_request_callback(rcl_event_callback_t callback, const void * user_data); + std::shared_ptr node_handle_; - rcl_service_t * service_handle_ = nullptr; - std::string service_name_; + std::recursive_mutex callback_mutex_; + // It is important to declare on_new_request_callback_ before + // service_handle_, so on destruction the service is + // destroyed first. Otherwise, the rmw service callback + // would point briefly to a destroyed function. + std::function on_new_request_callback_{nullptr}; + // Declare service_handle_ after callback + std::shared_ptr service_handle_; bool owns_rcl_handle_ = true; + + rclcpp::Logger node_logger_; + + std::atomic in_use_by_wait_set_{false}; }; template -class Service : public ServiceBase +class Service + : public ServiceBase, + public std::enable_shared_from_this> { public: using CallbackType = std::function< - void( - const std::shared_ptr, - std::shared_ptr)>; + void ( + const std::shared_ptr, + std::shared_ptr)>; using CallbackWithHeaderType = std::function< - void( - const std::shared_ptr, - const std::shared_ptr, - std::shared_ptr)>; + void ( + const std::shared_ptr, + const std::shared_ptr, + std::shared_ptr)>; RCLCPP_SMART_PTR_DEFINITIONS(Service) + /// Default constructor. + /** + * The constructor for a Service is almost never called directly. + * Instead, services should be instantiated through the function + * rclcpp::create_service(). + * + * \param[in] node_handle NodeBaseInterface pointer that is used in part of the setup. + * \param[in] service_name Name of the topic to publish to. + * \param[in] any_callback User defined callback to call when a client request is received. + * \param[in] service_options options for the service. + */ Service( std::shared_ptr node_handle, const std::string & service_name, AnyServiceCallback any_callback, rcl_service_options_t & service_options) - : ServiceBase(node_handle, service_name), any_callback_(any_callback) + : ServiceBase(node_handle), any_callback_(any_callback), + srv_type_support_handle_(rosidl_typesupport_cpp::get_service_type_support_handle()) { - using rosidl_typesupport_cpp::get_service_type_support_handle; - auto service_type_support_handle = get_service_type_support_handle(); - // rcl does the static memory allocation here - service_handle_ = new rcl_service_t; - *service_handle_ = rcl_get_zero_initialized_service(); + service_handle_ = std::shared_ptr( + new rcl_service_t, [handle = node_handle_, service_name](rcl_service_t * service) + { + if (rcl_service_fini(service, handle.get()) != RCL_RET_OK) { + RCLCPP_ERROR( + rclcpp::get_node_logger(handle.get()).get_child("rclcpp"), + "Error in destruction of rcl service handle: %s", + rcl_get_error_string().str); + rcl_reset_error(); + } + delete service; + }); + *service_handle_.get() = rcl_get_zero_initialized_service(); rcl_ret_t ret = rcl_service_init( - service_handle_, + service_handle_.get(), node_handle.get(), - service_type_support_handle, + srv_type_support_handle_, service_name.c_str(), &service_options); if (ret != RCL_RET_OK) { @@ -139,82 +352,177 @@ class Service : public ServiceBase rclcpp::exceptions::throw_from_rcl_error(ret, "could not create service"); } + TRACETOOLS_TRACEPOINT( + rclcpp_service_callback_added, + static_cast(get_service_handle().get()), + static_cast(&any_callback_)); +#ifndef TRACETOOLS_DISABLED + any_callback_.register_callback_for_tracing(); +#endif } + /// Default constructor. + /** + * The constructor for a Service is almost never called directly. + * Instead, services should be instantiated through the function + * rclcpp::create_service(). + * + * \param[in] node_handle NodeBaseInterface pointer that is used in part of the setup. + * \param[in] service_handle service handle. + * \param[in] any_callback User defined callback to call when a client request is received. + */ Service( std::shared_ptr node_handle, - rcl_service_t * service_handle, + std::shared_ptr service_handle, AnyServiceCallback any_callback) - : ServiceBase(node_handle), - any_callback_(any_callback) + : ServiceBase(node_handle), any_callback_(any_callback), + srv_type_support_handle_(rosidl_typesupport_cpp::get_service_type_support_handle()) { // check if service handle was initialized - // TODO(karsten1987): Take this verification - // directly in rcl_*_t - // see: https://github.com/ros2/rcl/issues/81 - if (!service_handle->impl) { + if (!rcl_service_is_valid(service_handle.get())) { // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here) throw std::runtime_error( std::string("rcl_service_t in constructor argument must be initialized beforehand.")); // *INDENT-ON* } - const char * service_name = rcl_service_get_service_name(service_handle); - if (!service_name) { - throw std::runtime_error("failed to get service name"); - } service_handle_ = service_handle; - service_name_ = std::string(service_name); - owns_rcl_handle_ = false; + TRACETOOLS_TRACEPOINT( + rclcpp_service_callback_added, + static_cast(get_service_handle().get()), + static_cast(&any_callback_)); +#ifndef TRACETOOLS_DISABLED + any_callback_.register_callback_for_tracing(); +#endif + } + + /// Default constructor. + /** + * The constructor for a Service is almost never called directly. + * Instead, services should be instantiated through the function + * rclcpp::create_service(). + * + * \param[in] node_handle NodeBaseInterface pointer that is used in part of the setup. + * \param[in] service_handle service handle. + * \param[in] any_callback User defined callback to call when a client request is received. + */ + Service( + std::shared_ptr node_handle, + rcl_service_t * service_handle, + AnyServiceCallback any_callback) + : ServiceBase(node_handle), any_callback_(any_callback), + srv_type_support_handle_(rosidl_typesupport_cpp::get_service_type_support_handle()) + { + // check if service handle was initialized + if (!rcl_service_is_valid(service_handle)) { + // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here) + throw std::runtime_error( + std::string("rcl_service_t in constructor argument must be initialized beforehand.")); + // *INDENT-ON* + } + + // In this case, rcl owns the service handle memory + service_handle_ = std::shared_ptr(new rcl_service_t); + service_handle_->impl = service_handle->impl; + TRACETOOLS_TRACEPOINT( + rclcpp_service_callback_added, + static_cast(get_service_handle().get()), + static_cast(&any_callback_)); +#ifndef TRACETOOLS_DISABLED + any_callback_.register_callback_for_tracing(); +#endif } Service() = delete; virtual ~Service() { - // check if you have ownership of the handle - if (owns_rcl_handle_) { - if (rcl_service_fini(service_handle_, node_handle_.get()) != RCL_RET_OK) { - std::stringstream ss; - ss << "Error in destruction of rcl service_handle_ handle: " << - rcl_get_error_string_safe() << '\n'; - (std::cerr << ss.str()).flush(); - rcl_reset_error(); - } - delete service_handle_; - } } - std::shared_ptr create_request() + /// Take the next request from the service. + /** + * \sa ServiceBase::take_type_erased_request(). + * + * \param[out] request_out The reference to a service request object + * into which the middleware will copy the taken request. + * \param[out] request_id_out The output id for the request which can be used + * to associate response with this request in the future. + * \returns true if the request was taken, otherwise false. + * \throws rclcpp::exceptions::RCLError based exceptions if the underlying + * rcl calls fail. + */ + bool + take_request(typename ServiceT::Request & request_out, rmw_request_id_t & request_id_out) + { + return this->take_type_erased_request(&request_out, request_id_out); + } + + std::shared_ptr + create_request() override { - return std::shared_ptr(new typename ServiceT::Request()); + return std::make_shared(); } - std::shared_ptr create_request_header() + std::shared_ptr + create_request_header() override { - // TODO(wjwwood): This should probably use rmw_request_id's allocator. - // (since it is a C type) - return std::shared_ptr(new rmw_request_id_t); + return std::make_shared(); } - void handle_request( + void + handle_request( std::shared_ptr request_header, - std::shared_ptr request) + std::shared_ptr request) override { auto typed_request = std::static_pointer_cast(request); - auto response = std::shared_ptr(new typename ServiceT::Response); - any_callback_.dispatch(request_header, typed_request, response); - send_response(request_header, response); + auto response = any_callback_.dispatch(this->shared_from_this(), request_header, typed_request); + if (response) { + send_response(*request_header, *response); + } } - void send_response( - std::shared_ptr req_id, - std::shared_ptr response) + void + send_response(rmw_request_id_t & req_id, typename ServiceT::Response & response) { - rcl_ret_t status = rcl_send_response(get_service_handle(), req_id.get(), response.get()); + rcl_ret_t ret = rcl_send_response(get_service_handle().get(), &req_id, &response); + + if (ret == RCL_RET_TIMEOUT) { + RCLCPP_WARN( + node_logger_.get_child("rclcpp"), + "failed to send response to %s (timeout): %s", + this->get_service_name(), rcl_get_error_string().str); + rcl_reset_error(); + return; + } + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send response"); + } + } - if (status != RCL_RET_OK) { - rclcpp::exceptions::throw_from_rcl_error(status, "failed to send response"); + /// Configure service introspection. + /** + * \param[in] clock clock to use to generate introspection timestamps + * \param[in] qos_service_event_pub QoS settings to use when creating the introspection publisher + * \param[in] introspection_state the state to set introspection to + */ + void + configure_introspection( + Clock::SharedPtr clock, const QoS & qos_service_event_pub, + rcl_service_introspection_state_t introspection_state) + { + rcl_publisher_options_t pub_opts = rcl_publisher_get_default_options(); + pub_opts.qos = qos_service_event_pub.get_rmw_qos_profile(); + + rcl_ret_t ret = rcl_service_configure_service_introspection( + service_handle_.get(), + node_handle_.get(), + clock->get_clock_handle(), + srv_type_support_handle_, + pub_opts, + introspection_state); + + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to configure service introspection"); } } @@ -222,6 +530,8 @@ class Service : public ServiceBase RCLCPP_DISABLE_COPY(Service) AnyServiceCallback any_callback_; + + const rosidl_service_type_support_t * srv_type_support_handle_; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index 56ac07a0d9..28eff94aed 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -21,10 +21,13 @@ #include "rcl/allocator.h" #include "rclcpp/allocator/allocator_common.hpp" +#include "rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp" #include "rclcpp/memory_strategy.hpp" #include "rclcpp/node.hpp" #include "rclcpp/visibility_control.hpp" +#include "rcutils/logging_macros.h" + #include "rmw/types.h" namespace rclcpp @@ -46,35 +49,30 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy public: RCLCPP_SMART_PTR_DEFINITIONS(AllocatorMemoryStrategy) - using ExecAllocTraits = allocator::AllocRebind; - using ExecAlloc = typename ExecAllocTraits::allocator_type; - using ExecDeleter = allocator::Deleter; using VoidAllocTraits = typename allocator::AllocRebind; using VoidAlloc = typename VoidAllocTraits::allocator_type; explicit AllocatorMemoryStrategy(std::shared_ptr allocator) { - executable_allocator_ = std::make_shared(*allocator.get()); allocator_ = std::make_shared(*allocator.get()); } AllocatorMemoryStrategy() { - executable_allocator_ = std::make_shared(); allocator_ = std::make_shared(); } - void add_guard_condition(const rcl_guard_condition_t * guard_condition) + void add_guard_condition(const rclcpp::GuardCondition & guard_condition) override { for (const auto & existing_guard_condition : guard_conditions_) { - if (existing_guard_condition == guard_condition) { + if (existing_guard_condition == &guard_condition) { return; } } - guard_conditions_.push_back(guard_condition); + guard_conditions_.push_back(&guard_condition); } - void remove_guard_condition(const rcl_guard_condition_t * guard_condition) + void remove_guard_condition(const rclcpp::GuardCondition * guard_condition) override { for (auto it = guard_conditions_.begin(); it != guard_conditions_.end(); ++it) { if (*it == guard_condition) { @@ -84,34 +82,46 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } } - void clear_handles() + void clear_handles() override { subscription_handles_.clear(); service_handles_.clear(); client_handles_.clear(); timer_handles_.clear(); + waitable_handles_.clear(); } - virtual void remove_null_handles(rcl_wait_set_t * wait_set) + void remove_null_handles(rcl_wait_set_t * wait_set) override { - for (size_t i = 0; i < wait_set->size_of_subscriptions; ++i) { + // TODO(jacobperron): Check if wait set sizes are what we expect them to be? + // e.g. wait_set->size_of_clients == client_handles_.size() + + // Important to use subscription_handles_.size() instead of wait set's size since + // there may be more subscriptions in the wait set due to Waitables added to the end. + // The same logic applies for other entities. + for (size_t i = 0; i < subscription_handles_.size(); ++i) { if (!wait_set->subscriptions[i]) { - subscription_handles_[i] = nullptr; + subscription_handles_[i].reset(); } } - for (size_t i = 0; i < wait_set->size_of_services; ++i) { + for (size_t i = 0; i < service_handles_.size(); ++i) { if (!wait_set->services[i]) { - service_handles_[i] = nullptr; + service_handles_[i].reset(); } } - for (size_t i = 0; i < wait_set->size_of_clients; ++i) { + for (size_t i = 0; i < client_handles_.size(); ++i) { if (!wait_set->clients[i]) { - client_handles_[i] = nullptr; + client_handles_[i].reset(); } } - for (size_t i = 0; i < wait_set->size_of_timers; ++i) { + for (size_t i = 0; i < timer_handles_.size(); ++i) { if (!wait_set->timers[i]) { - timer_handles_[i] = nullptr; + timer_handles_[i].reset(); + } + } + for (size_t i = 0; i < waitable_handles_.size(); ++i) { + if (!waitable_handles_[i]->is_ready(*wait_set)) { + waitable_handles_[i].reset(); } } @@ -134,118 +144,115 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy std::remove(timer_handles_.begin(), timer_handles_.end(), nullptr), timer_handles_.end() ); + + waitable_handles_.erase( + std::remove(waitable_handles_.begin(), waitable_handles_.end(), nullptr), + waitable_handles_.end() + ); } - bool collect_entities(const WeakNodeVector & weak_nodes) + bool collect_entities(const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) override { - bool has_invalid_weak_nodes = false; - for (auto & weak_node : weak_nodes) { - auto node = weak_node.lock(); - if (!node) { - has_invalid_weak_nodes = true; + bool has_invalid_weak_groups_or_nodes = false; + for (const auto & pair : weak_groups_to_nodes) { + auto group = pair.first.lock(); + auto node = pair.second.lock(); + if (group == nullptr || node == nullptr) { + has_invalid_weak_groups_or_nodes = true; continue; } - for (auto & weak_group : node->get_callback_groups()) { - auto group = weak_group.lock(); - if (!group || !group->can_be_taken_from().load()) { - continue; - } - for (auto & weak_subscription : group->get_subscription_ptrs()) { - auto subscription = weak_subscription.lock(); - if (subscription) { - subscription_handles_.push_back(subscription->get_subscription_handle()); - if (subscription->get_intra_process_subscription_handle()) { - subscription_handles_.push_back( - subscription->get_intra_process_subscription_handle()); - } - } - } - for (auto & weak_service : group->get_service_ptrs()) { - auto service = weak_service.lock(); - if (service) { - service_handles_.push_back(service->get_service_handle()); - } - } - for (auto & weak_client : group->get_client_ptrs()) { - auto client = weak_client.lock(); - if (client) { - client_handles_.push_back(client->get_client_handle()); - } - } - for (auto & weak_timer : group->get_timer_ptrs()) { - auto timer = weak_timer.lock(); - if (timer) { - timer_handles_.push_back(timer->get_timer_handle()); - } - } + if (!group || !group->can_be_taken_from().load()) { + continue; } + + group->collect_all_ptrs( + [this](const rclcpp::SubscriptionBase::SharedPtr & subscription) { + subscription_handles_.push_back(subscription->get_subscription_handle()); + }, + [this](const rclcpp::ServiceBase::SharedPtr & service) { + service_handles_.push_back(service->get_service_handle()); + }, + [this](const rclcpp::ClientBase::SharedPtr & client) { + client_handles_.push_back(client->get_client_handle()); + }, + [this](const rclcpp::TimerBase::SharedPtr & timer) { + timer_handles_.push_back(timer->get_timer_handle()); + }, + [this](const rclcpp::Waitable::SharedPtr & waitable) { + waitable_handles_.push_back(waitable); + }); + } + + return has_invalid_weak_groups_or_nodes; + } + + void add_waitable_handle(const rclcpp::Waitable::SharedPtr & waitable) override + { + if (nullptr == waitable) { + throw std::runtime_error("waitable object unexpectedly nullptr"); } - return has_invalid_weak_nodes; + waitable_handles_.push_back(waitable); } - bool add_handles_to_wait_set(rcl_wait_set_t * wait_set) + bool add_handles_to_wait_set(rcl_wait_set_t * wait_set) override { - for (auto subscription : subscription_handles_) { - if (rcl_wait_set_add_subscription(wait_set, subscription) != RCL_RET_OK) { - fprintf(stderr, "Couldn't add subscription to wait set: %s\n", rcl_get_error_string_safe()); + for (const std::shared_ptr & subscription : subscription_handles_) { + if (rcl_wait_set_add_subscription(wait_set, subscription.get(), NULL) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Couldn't add subscription to wait set: %s", rcl_get_error_string().str); return false; } } - for (auto client : client_handles_) { - if (rcl_wait_set_add_client(wait_set, client) != RCL_RET_OK) { - fprintf(stderr, "Couldn't add client to wait set: %s\n", rcl_get_error_string_safe()); + for (const std::shared_ptr & client : client_handles_) { + if (rcl_wait_set_add_client(wait_set, client.get(), NULL) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Couldn't add client to wait set: %s", rcl_get_error_string().str); return false; } } - for (auto service : service_handles_) { - if (rcl_wait_set_add_service(wait_set, service) != RCL_RET_OK) { - fprintf(stderr, "Couldn't add service to wait set: %s\n", rcl_get_error_string_safe()); + for (const std::shared_ptr & service : service_handles_) { + if (rcl_wait_set_add_service(wait_set, service.get(), NULL) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Couldn't add service to wait set: %s", rcl_get_error_string().str); return false; } } - for (auto timer : timer_handles_) { - if (rcl_wait_set_add_timer(wait_set, timer) != RCL_RET_OK) { - fprintf(stderr, "Couldn't add timer to wait set: %s\n", rcl_get_error_string_safe()); + for (const std::shared_ptr & timer : timer_handles_) { + if (rcl_wait_set_add_timer(wait_set, timer.get(), NULL) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Couldn't add timer to wait set: %s", rcl_get_error_string().str); return false; } } for (auto guard_condition : guard_conditions_) { - if (rcl_wait_set_add_guard_condition(wait_set, guard_condition) != RCL_RET_OK) { - fprintf(stderr, "Couldn't add guard_condition to wait set: %s\n", - rcl_get_error_string_safe()); - return false; - } + detail::add_guard_condition_to_rcl_wait_set(*wait_set, *guard_condition); } - return true; - } - /// Provide a newly initialized AnyExecutable object. - // \return Shared pointer to the fresh executable. - executor::AnyExecutable::SharedPtr instantiate_next_executable() - { - return std::allocate_shared(*executable_allocator_.get()); + for (const std::shared_ptr & waitable : waitable_handles_) { + waitable->add_to_wait_set(*wait_set); + } + return true; } - virtual void + void get_next_subscription( - executor::AnyExecutable::SharedPtr any_exec, - const WeakNodeVector & weak_nodes) + rclcpp::AnyExecutable & any_exec, + const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) override { auto it = subscription_handles_.begin(); while (it != subscription_handles_.end()) { - auto subscription = get_subscription_by_handle(*it, weak_nodes); + auto subscription = get_subscription_by_handle(*it, weak_groups_to_nodes); if (subscription) { - // Figure out if this is for intra-process or not. - bool is_intra_process = false; - if (subscription->get_intra_process_subscription_handle()) { - is_intra_process = subscription->get_intra_process_subscription_handle() == *it; - } // Find the group for this handle and see if it can be serviced - auto group = get_group_by_subscription(subscription, weak_nodes); + auto group = get_group_by_subscription(subscription, weak_groups_to_nodes); if (!group) { // Group was not found, meaning the subscription is not valid... // Remove it from the ready list and continue looking @@ -259,13 +266,9 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy continue; } // Otherwise it is safe to set and return the any_exec - if (is_intra_process) { - any_exec->subscription_intra_process = subscription; - } else { - any_exec->subscription = subscription; - } - any_exec->callback_group = group; - any_exec->node_base = get_node_by_group(group, weak_nodes); + any_exec.subscription = subscription; + any_exec.callback_group = group; + any_exec.node_base = get_node_by_group(group, weak_groups_to_nodes); subscription_handles_.erase(it); return; } @@ -274,17 +277,17 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } } - virtual void + void get_next_service( - executor::AnyExecutable::SharedPtr any_exec, - const WeakNodeVector & weak_nodes) + rclcpp::AnyExecutable & any_exec, + const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) override { auto it = service_handles_.begin(); while (it != service_handles_.end()) { - auto service = get_service_by_handle(*it, weak_nodes); + auto service = get_service_by_handle(*it, weak_groups_to_nodes); if (service) { // Find the group for this handle and see if it can be serviced - auto group = get_group_by_service(service, weak_nodes); + auto group = get_group_by_service(service, weak_groups_to_nodes); if (!group) { // Group was not found, meaning the service is not valid... // Remove it from the ready list and continue looking @@ -298,9 +301,9 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy continue; } // Otherwise it is safe to set and return the any_exec - any_exec->service = service; - any_exec->callback_group = group; - any_exec->node_base = get_node_by_group(group, weak_nodes); + any_exec.service = service; + any_exec.callback_group = group; + any_exec.node_base = get_node_by_group(group, weak_groups_to_nodes); service_handles_.erase(it); return; } @@ -309,15 +312,17 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } } - virtual void - get_next_client(executor::AnyExecutable::SharedPtr any_exec, const WeakNodeVector & weak_nodes) + void + get_next_client( + rclcpp::AnyExecutable & any_exec, + const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) override { auto it = client_handles_.begin(); while (it != client_handles_.end()) { - auto client = get_client_by_handle(*it, weak_nodes); + auto client = get_client_by_handle(*it, weak_groups_to_nodes); if (client) { // Find the group for this handle and see if it can be serviced - auto group = get_group_by_client(client, weak_nodes); + auto group = get_group_by_client(client, weak_groups_to_nodes); if (!group) { // Group was not found, meaning the service is not valid... // Remove it from the ready list and continue looking @@ -331,9 +336,9 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy continue; } // Otherwise it is safe to set and return the any_exec - any_exec->client = client; - any_exec->callback_group = group; - any_exec->node_base = get_node_by_group(group, weak_nodes); + any_exec.client = client; + any_exec.callback_group = group; + any_exec.node_base = get_node_by_group(group, weak_groups_to_nodes); client_handles_.erase(it); return; } @@ -342,49 +347,160 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } } - virtual rcl_allocator_t get_allocator() + void + get_next_timer( + rclcpp::AnyExecutable & any_exec, + const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) override + { + auto it = timer_handles_.begin(); + while (it != timer_handles_.end()) { + auto timer = get_timer_by_handle(*it, weak_groups_to_nodes); + if (timer) { + // Find the group for this handle and see if it can be serviced + auto group = get_group_by_timer(timer, weak_groups_to_nodes); + if (!group) { + // Group was not found, meaning the timer is not valid... + // Remove it from the ready list and continue looking + it = timer_handles_.erase(it); + continue; + } + if (!group->can_be_taken_from().load()) { + // Group is mutually exclusive and is being used, so skip it for now + // Leave it to be checked next time, but continue searching + ++it; + continue; + } + auto data = timer->call(); + if (!data) { + // timer was cancelled, skip it. + ++it; + continue; + } + // Otherwise it is safe to set and return the any_exec + any_exec.timer = timer; + any_exec.callback_group = group; + any_exec.node_base = get_node_by_group(group, weak_groups_to_nodes); + any_exec.data = data; + timer_handles_.erase(it); + return; + } + // Else, the timer is no longer valid, remove it and continue + it = timer_handles_.erase(it); + } + } + + void + get_next_waitable( + rclcpp::AnyExecutable & any_exec, + const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) override + { + auto it = waitable_handles_.begin(); + while (it != waitable_handles_.end()) { + std::shared_ptr & waitable = *it; + if (waitable) { + // Find the group for this handle and see if it can be serviced + auto group = get_group_by_waitable(waitable, weak_groups_to_nodes); + if (!group) { + // Group was not found, meaning the waitable is not valid... + // Remove it from the ready list and continue looking + it = waitable_handles_.erase(it); + continue; + } + if (!group->can_be_taken_from().load()) { + // Group is mutually exclusive and is being used, so skip it for now + // Leave it to be checked next time, but continue searching + ++it; + continue; + } + // Otherwise it is safe to set and return the any_exec + any_exec.waitable = waitable; + any_exec.callback_group = group; + any_exec.node_base = get_node_by_group(group, weak_groups_to_nodes); + waitable_handles_.erase(it); + return; + } + // Else, the waitable is no longer valid, remove it and continue + it = waitable_handles_.erase(it); + } + } + + rcl_allocator_t get_allocator() override { return rclcpp::allocator::get_rcl_allocator(*allocator_.get()); } - size_t number_of_ready_subscriptions() const + size_t number_of_ready_subscriptions() const override { - return subscription_handles_.size(); + size_t number_of_subscriptions = subscription_handles_.size(); + for (const std::shared_ptr & waitable : waitable_handles_) { + number_of_subscriptions += waitable->get_number_of_ready_subscriptions(); + } + return number_of_subscriptions; } - size_t number_of_ready_services() const + size_t number_of_ready_services() const override { - return service_handles_.size(); + size_t number_of_services = service_handles_.size(); + for (const std::shared_ptr & waitable : waitable_handles_) { + number_of_services += waitable->get_number_of_ready_services(); + } + return number_of_services; } - size_t number_of_ready_clients() const + size_t number_of_ready_events() const override { - return client_handles_.size(); + size_t number_of_events = 0; + for (const std::shared_ptr & waitable : waitable_handles_) { + number_of_events += waitable->get_number_of_ready_events(); + } + return number_of_events; } - size_t number_of_guard_conditions() const + size_t number_of_ready_clients() const override { - return guard_conditions_.size(); + size_t number_of_clients = client_handles_.size(); + for (const std::shared_ptr & waitable : waitable_handles_) { + number_of_clients += waitable->get_number_of_ready_clients(); + } + return number_of_clients; + } + + size_t number_of_guard_conditions() const override + { + size_t number_of_guard_conditions = guard_conditions_.size(); + for (const std::shared_ptr & waitable : waitable_handles_) { + number_of_guard_conditions += waitable->get_number_of_ready_guard_conditions(); + } + return number_of_guard_conditions; + } + + size_t number_of_ready_timers() const override + { + size_t number_of_timers = timer_handles_.size(); + for (const std::shared_ptr & waitable : waitable_handles_) { + number_of_timers += waitable->get_number_of_ready_timers(); + } + return number_of_timers; } - size_t number_of_ready_timers() const + size_t number_of_waitables() const override { - return timer_handles_.size(); + return waitable_handles_.size(); } private: template using VectorRebind = - std::vector::template rebind_alloc>; + std::vector::template rebind_alloc>; - VectorRebind guard_conditions_; + VectorRebind guard_conditions_; - VectorRebind subscription_handles_; - VectorRebind service_handles_; - VectorRebind client_handles_; - VectorRebind timer_handles_; + VectorRebind> subscription_handles_; + VectorRebind> service_handles_; + VectorRebind> client_handles_; + VectorRebind> timer_handles_; + VectorRebind> waitable_handles_; - std::shared_ptr executable_allocator_; std::shared_ptr allocator_; }; diff --git a/rclcpp/include/rclcpp/strategies/message_pool_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/message_pool_memory_strategy.hpp index 0eb1d54b92..703066fa3f 100644 --- a/rclcpp/include/rclcpp/strategies/message_pool_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/message_pool_memory_strategy.hpp @@ -15,8 +15,17 @@ #ifndef RCLCPP__STRATEGIES__MESSAGE_POOL_MEMORY_STRATEGY_HPP_ #define RCLCPP__STRATEGIES__MESSAGE_POOL_MEMORY_STRATEGY_HPP_ +#include +#include #include +#include +#include +#include +#include "rosidl_runtime_cpp/traits.hpp" + +#include "rclcpp/logger.hpp" +#include "rclcpp/logging.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/message_memory_strategy.hpp" #include "rclcpp/visibility_control.hpp" @@ -48,13 +57,24 @@ class MessagePoolMemoryStrategy public: RCLCPP_SMART_PTR_DEFINITIONS(MessagePoolMemoryStrategy) - /// Default constructor MessagePoolMemoryStrategy() - : next_array_index_(0) { + pool_mutex_ = std::make_shared(); + + pool_ = std::shared_ptr>( + new std::array, + [](std::array * arr) { + for (size_t i = 0; i < Size; ++i) { + free((*arr)[i]); + } + delete arr; + }); + + free_list_ = std::make_shared>(); + for (size_t i = 0; i < Size; ++i) { - pool_[i].msg_ptr_ = std::make_shared(); - pool_[i].used = false; + (*pool_)[i] = static_cast(malloc(sizeof(MessageT))); + free_list_->push_back(i); } } @@ -66,43 +86,85 @@ class MessagePoolMemoryStrategy */ std::shared_ptr borrow_message() { - size_t current_index = next_array_index_; - next_array_index_ = (next_array_index_ + 1) % Size; - if (pool_[current_index].used) { - throw std::runtime_error("Tried to access message that was still in use! Abort."); + std::lock_guard lock(*pool_mutex_); + if (free_list_->size() == 0) { + throw std::runtime_error("No more free slots in the pool"); } - pool_[current_index].msg_ptr_->~MessageT(); - new (pool_[current_index].msg_ptr_.get())MessageT; - pool_[current_index].used = true; - return pool_[current_index].msg_ptr_; + size_t current_index = free_list_->pop_front(); + + return std::shared_ptr( + new((*pool_)[current_index]) MessageT(), + [pool = this->pool_, pool_mutex = this->pool_mutex_, + free_list = this->free_list_](MessageT * p) { + std::lock_guard lock(*pool_mutex); + for (size_t i = 0; i < Size; ++i) { + if ((*pool)[i] == p) { + p->~MessageT(); + free_list->push_back(i); + break; + } + } + }); } /// Return a message to the message pool. /** - * Manage metadata in the message pool ring buffer to release the message. + * This does nothing since the message isn't returned to the pool until the user has dropped + * all references. * \param[in] msg Shared pointer to the message to return. */ void return_message(std::shared_ptr & msg) { - for (size_t i = 0; i < Size; ++i) { - if (pool_[i].msg_ptr_ == msg) { - pool_[i].used = false; - return; - } - } - throw std::runtime_error("Unrecognized message ptr in return_message."); + (void)msg; } protected: - struct PoolMember + template + class CircularArray { - std::shared_ptr msg_ptr_; - bool used; +public: + void push_back(const size_t v) + { + if (size_ + 1 > N) { + throw std::runtime_error("Tried to push too many items into the array"); + } + array_[(front_ + size_) % N] = v; + ++size_; + } + + size_t pop_front() + { + if (size_ < 1) { + throw std::runtime_error("Tried to pop item from empty array"); + } + + size_t val = array_[front_]; + + front_ = (front_ + 1) % N; + --size_; + + return val; + } + + size_t size() const + { + return size_; + } + +private: + size_t front_ = 0; + size_t size_ = 0; + std::array array_; }; - std::array pool_; - size_t next_array_index_; + // It's very important that these are shared_ptrs, since users of this class might hold a + // reference to a pool item longer than the lifetime of the class. In that scenario, the + // shared_ptr ensures that the lifetime of these variables outlives this class, and hence ensures + // the custom destructor for each pool item can successfully run. + std::shared_ptr pool_mutex_; + std::shared_ptr> pool_; + std::shared_ptr> free_list_; }; } // namespace message_pool_memory_strategy diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 7967dc56ec..4e552eb1df 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -18,24 +18,37 @@ #include #include +#include #include #include #include #include #include +#include #include "rcl/error_handling.h" #include "rcl/subscription.h" -#include "rcl_interfaces/msg/intra_process_message.hpp" - +#include "rclcpp/any_subscription_callback.hpp" +#include "rclcpp/detail/resolve_use_intra_process.hpp" +#include "rclcpp/detail/resolve_intra_process_buffer_type.hpp" #include "rclcpp/exceptions.hpp" +#include "rclcpp/expand_topic_or_service_name.hpp" +#include "rclcpp/experimental/intra_process_manager.hpp" +#include "rclcpp/experimental/subscription_intra_process.hpp" +#include "rclcpp/logging.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/message_info.hpp" #include "rclcpp/message_memory_strategy.hpp" -#include "rclcpp/any_subscription_callback.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/subscription_base.hpp" +#include "rclcpp/subscription_options.hpp" +#include "rclcpp/subscription_traits.hpp" #include "rclcpp/type_support_decl.hpp" -#include "rclcpp/expand_topic_or_service_name.hpp" #include "rclcpp/visibility_control.hpp" +#include "rclcpp/waitable.hpp" +#include "rclcpp/topic_statistics/subscription_topic_statistics.hpp" +#include "tracetools/tracetools.h" namespace rclcpp { @@ -45,134 +58,213 @@ namespace node_interfaces class NodeTopicsInterface; } // namespace node_interfaces -/// Virtual base class for subscriptions. This pattern allows us to iterate over different template -/// specializations of Subscription, among other things. -class SubscriptionBase +/// Subscription implementation, templated on the type of message this subscription receives. +template< + typename MessageT, + typename AllocatorT = std::allocator, + /// MessageT::custom_type if MessageT is a TypeAdapter, + /// otherwise just MessageT. + typename SubscribedT = typename rclcpp::TypeAdapter::custom_type, + /// MessageT::ros_message_type if MessageT is a TypeAdapter, + /// otherwise just MessageT. + typename ROSMessageT = typename rclcpp::TypeAdapter::ros_message_type, + typename MessageMemoryStrategyT = rclcpp::message_memory_strategy::MessageMemoryStrategy< + ROSMessageT, + AllocatorT + >> +class Subscription : public SubscriptionBase { -public: - RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(SubscriptionBase) - - /// Default constructor. - /** - * \param[in] node_handle The rcl representation of the node that owns this subscription. - * \param[in] type_support_handle rosidl type support struct, for the Message type of the topic. - * \param[in] topic_name Name of the topic to subscribe to. - * \param[in] subscription_options options for the subscription. - */ - RCLCPP_PUBLIC - SubscriptionBase( - std::shared_ptr node_handle, - const rosidl_message_type_support_t & type_support_handle, - const std::string & topic_name, - const rcl_subscription_options_t & subscription_options); - - /// Default destructor. - RCLCPP_PUBLIC - virtual ~SubscriptionBase(); - - /// Get the topic that this subscription is subscribed on. - RCLCPP_PUBLIC - const char * - get_topic_name() const; - - RCLCPP_PUBLIC - rcl_subscription_t * - get_subscription_handle(); - - RCLCPP_PUBLIC - const rcl_subscription_t * - get_subscription_handle() const; - - RCLCPP_PUBLIC - virtual const rcl_subscription_t * - get_intra_process_subscription_handle() const; - - /// Borrow a new message. - /** \return Shared pointer to the fresh message. */ - virtual std::shared_ptr - create_message() = 0; - /// Check if we need to handle the message, and execute the callback if we do. - /** - * \param[in] message Shared pointer to the message to handle. - * \param[in] message_info Metadata associated with this message. - */ - virtual void - handle_message(std::shared_ptr & message, const rmw_message_info_t & message_info) = 0; + friend class rclcpp::node_interfaces::NodeTopicsInterface; - /// Return the message borrowed in create_message. - /** \param[in] message Shared pointer to the returned message. */ - virtual void - return_message(std::shared_ptr & message) = 0; +public: + // Redeclare these here to use outside of the class. + using SubscribedType = SubscribedT; + using ROSMessageType = ROSMessageT; + using MessageMemoryStrategyType = MessageMemoryStrategyT; - virtual void - handle_intra_process_message( - rcl_interfaces::msg::IntraProcessMessage & ipm, - const rmw_message_info_t & message_info) = 0; + using SubscribedTypeAllocatorTraits = allocator::AllocRebind; + using SubscribedTypeAllocator = typename SubscribedTypeAllocatorTraits::allocator_type; + using SubscribedTypeDeleter = allocator::Deleter; -protected: - rcl_subscription_t intra_process_subscription_handle_ = rcl_get_zero_initialized_subscription(); - rcl_subscription_t subscription_handle_ = rcl_get_zero_initialized_subscription(); - std::shared_ptr node_handle_; + using ROSMessageTypeAllocatorTraits = allocator::AllocRebind; + using ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type; + using ROSMessageTypeDeleter = allocator::Deleter; private: - RCLCPP_DISABLE_COPY(SubscriptionBase) -}; - -/// Subscription implementation, templated on the type of message this subscription receives. -template> -class Subscription : public SubscriptionBase -{ - friend class rclcpp::node_interfaces::NodeTopicsInterface; + using SubscriptionTopicStatisticsSharedPtr = + std::shared_ptr; public: - using MessageAllocTraits = allocator::AllocRebind; - using MessageAlloc = typename MessageAllocTraits::allocator_type; - using MessageDeleter = allocator::Deleter; - using MessageUniquePtr = std::unique_ptr; - RCLCPP_SMART_PTR_DEFINITIONS(Subscription) /// Default constructor. /** - * The constructor for a subscription is almost never called directly. Instead, subscriptions - * should be instantiated through Node::create_subscription. - * \param[in] node_handle rcl representation of the node that owns this subscription. + * The constructor for a subscription is almost never called directly. + * Instead, subscriptions should be instantiated through the function + * rclcpp::create_subscription(). + * + * \param[in] node_base NodeBaseInterface pointer that is used in part of the setup. + * \param[in] type_support_handle rosidl type support struct, for the Message type of the topic. * \param[in] topic_name Name of the topic to subscribe to. - * \param[in] subscription_options options for the subscription. + * \param[in] qos QoS profile for Subcription. * \param[in] callback User defined callback to call when a message is received. - * \param[in] memory_strategy The memory strategy to be used for managing message memory. + * \param[in] options Options for the subscription. + * \param[in] message_memory_strategy The memory strategy to be used for managing message memory. + * \param[in] subscription_topic_statistics Optional pointer to a topic statistics subcription. + * \throws std::invalid_argument if the QoS is uncompatible with intra-process (if one + * of the following conditions are true: qos_profile.history == RMW_QOS_POLICY_HISTORY_KEEP_ALL, + * qos_profile.depth == 0 or qos_profile.durability != RMW_QOS_POLICY_DURABILITY_VOLATILE). */ + // *INDENT-OFF* Subscription( - std::shared_ptr node_handle, + rclcpp::node_interfaces::NodeBaseInterface * node_base, + const rosidl_message_type_support_t & type_support_handle, const std::string & topic_name, - const rcl_subscription_options_t & subscription_options, - AnySubscriptionCallback callback, - typename message_memory_strategy::MessageMemoryStrategy::SharedPtr - memory_strategy = message_memory_strategy::MessageMemoryStrategy::create_default()) + const rclcpp::QoS & qos, + AnySubscriptionCallback callback, + const rclcpp::SubscriptionOptionsWithAllocator & options, + typename MessageMemoryStrategyT::SharedPtr message_memory_strategy, + SubscriptionTopicStatisticsSharedPtr subscription_topic_statistics = nullptr) : SubscriptionBase( - node_handle, - *rosidl_typesupport_cpp::get_message_type_support_handle(), + node_base, + type_support_handle, topic_name, - subscription_options), + options.to_rcl_subscription_options(qos), + // NOTE(methylDragon): Passing these args separately is necessary for event binding + options.event_callbacks, + options.use_default_callbacks, + callback.is_serialized_message_callback() ? DeliveredMessageKind::SERIALIZED_MESSAGE : DeliveredMessageKind::ROS_MESSAGE), // NOLINT any_callback_(callback), - message_memory_strategy_(memory_strategy), - get_intra_process_message_callback_(nullptr), - matches_any_intra_process_publishers_(nullptr) - {} + options_(options), + message_memory_strategy_(message_memory_strategy) + // *INDENT-ON* + { + // Setup intra process publishing if requested. + if (rclcpp::detail::resolve_use_intra_process(options_, *node_base)) { + using rclcpp::detail::resolve_intra_process_buffer_type; + + // Check if the QoS is compatible with intra-process. + auto qos_profile = get_actual_qos(); + if (qos_profile.history() != rclcpp::HistoryPolicy::KeepLast) { + throw std::invalid_argument( + "intraprocess communication allowed only with keep last history qos policy"); + } + if (qos_profile.depth() == 0) { + throw std::invalid_argument( + "intraprocess communication is not allowed with 0 depth qos policy"); + } + + using SubscriptionIntraProcessT = rclcpp::experimental::SubscriptionIntraProcess< + MessageT, + SubscribedType, + SubscribedTypeAllocator, + SubscribedTypeDeleter, + ROSMessageT, + AllocatorT>; + + // First create a SubscriptionIntraProcess which will be given to the intra-process manager. + auto context = node_base->get_context(); + subscription_intra_process_ = std::make_shared( + callback, + options_.get_allocator(), + context, + this->get_topic_name(), // important to get like this, as it has the fully-qualified name + qos_profile, + resolve_intra_process_buffer_type(options_.intra_process_buffer_type, callback)); + TRACETOOLS_TRACEPOINT( + rclcpp_subscription_init, + static_cast(get_subscription_handle().get()), + static_cast(subscription_intra_process_.get())); + + // Add it to the intra process manager. + using rclcpp::experimental::IntraProcessManager; + auto ipm = context->get_sub_context(); + uint64_t intra_process_subscription_id = ipm->template add_subscription< + ROSMessageType, ROSMessageTypeAllocator>(subscription_intra_process_); + this->setup_intra_process(intra_process_subscription_id, ipm); + } + + if (subscription_topic_statistics != nullptr) { + this->subscription_topic_statistics_ = std::move(subscription_topic_statistics); + } + + TRACETOOLS_TRACEPOINT( + rclcpp_subscription_init, + static_cast(get_subscription_handle().get()), + static_cast(this)); + TRACETOOLS_TRACEPOINT( + rclcpp_subscription_callback_added, + static_cast(this), + static_cast(&any_callback_)); + // The callback object gets copied, so if registration is done too early/before this point + // (e.g. in `AnySubscriptionCallback::set()`), its address won't match any address used later + // in subsequent tracepoints. +#ifndef TRACETOOLS_DISABLED + any_callback_.register_callback_for_tracing(); +#endif + } + + /// Called after construction to continue setup that requires shared_from_this(). + void + post_init_setup( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + const rclcpp::QoS & qos, + const rclcpp::SubscriptionOptionsWithAllocator & options) + { + (void)node_base; + (void)qos; + (void)options; + } + + /// Take the next message from the inter-process subscription. + /** + * Data may be taken (written) into the message_out and message_info_out even + * if false is returned. + * Specifically in the case of dropping redundant intra-process data, where + * data is received via both intra-process and inter-process (due to the + * underlying middleware being unabled to avoid this duplicate delivery) and + * so inter-process data from those intra-process publishers is ignored, but + * it has to be taken to know if it came from an intra-process publisher or + * not, and therefore could be dropped. + * + * \sa SubscriptionBase::take_type_erased() + * + * \param[out] message_out The message into which take will copy the data. + * \param[out] message_info_out The message info for the taken message. + * \returns true if data was taken and is valid, otherwise false + * \throws any rcl errors from rcl_take, \sa rclcpp::exceptions::throw_from_rcl_error() + */ + bool + take(ROSMessageType & message_out, rclcpp::MessageInfo & message_info_out) + { + return this->take_type_erased(static_cast(&message_out), message_info_out); + } - /// Support dynamically setting the message memory strategy. + /// Take the next message from the inter-process subscription. /** - * Behavior may be undefined if called while the subscription could be executing. - * \param[in] message_memory_strategy Shared pointer to the memory strategy to set. + * This version takes a SubscribedType which is different from the + * ROSMessageType when a rclcpp::TypeAdapter is in used. + * + * \sa take(ROSMessageType &, rclcpp::MessageInfo &) */ - void set_message_memory_strategy( - typename message_memory_strategy::MessageMemoryStrategy::SharedPtr message_memory_strategy) + template + std::enable_if_t< + !rosidl_generator_traits::is_message::value && + std::is_same_v, + bool + > + take(TakeT & message_out, rclcpp::MessageInfo & message_info_out) { - message_memory_strategy_ = message_memory_strategy; + ROSMessageType local_message; + bool taken = this->take_type_erased(static_cast(&local_message), message_info_out); + if (taken) { + rclcpp::TypeAdapter::convert_to_custom(local_message, message_out); + } + return taken; } - std::shared_ptr create_message() + + std::shared_ptr + create_message() override { /* The default message memory strategy provides a dynamically allocated message on each call to * create_message, though alternative memory strategies that re-use a preallocated message may be @@ -181,110 +273,186 @@ class Subscription : public SubscriptionBase return message_memory_strategy_->borrow_message(); } - void handle_message(std::shared_ptr & message, const rmw_message_info_t & message_info) + std::shared_ptr + create_serialized_message() override { - if (matches_any_intra_process_publishers_) { - if (matches_any_intra_process_publishers_(&message_info.publisher_gid)) { - // In this case, the message will be delivered via intra process and - // we should ignore this copy of the message. - return; - } + return message_memory_strategy_->borrow_serialized_message(); + } + + void + handle_message( + std::shared_ptr & message, + const rclcpp::MessageInfo & message_info) override + { + if (matches_any_intra_process_publishers(&message_info.get_rmw_message_info().publisher_gid)) { + // In this case, the message will be delivered via intra process and + // we should ignore this copy of the message. + return; + } + auto typed_message = std::static_pointer_cast(message); + + std::chrono::time_point now; + if (subscription_topic_statistics_) { + // get current time before executing callback to + // exclude callback duration from topic statistics result. + now = std::chrono::system_clock::now(); } - auto typed_message = std::static_pointer_cast(message); + any_callback_.dispatch(typed_message, message_info); + + if (subscription_topic_statistics_) { + const auto nanos = std::chrono::time_point_cast(now); + const auto time = rclcpp::Time(nanos.time_since_epoch().count()); + subscription_topic_statistics_->handle_message(message_info.get_rmw_message_info(), time); + } } - /// Return the loaned message. - /** \param message message to be returned */ - void return_message(std::shared_ptr & message) + void + handle_serialized_message( + const std::shared_ptr & serialized_message, + const rclcpp::MessageInfo & message_info) override { - auto typed_message = std::static_pointer_cast(message); - message_memory_strategy_->return_message(typed_message); + std::chrono::time_point now; + if (subscription_topic_statistics_) { + // get current time before executing callback to + // exclude callback duration from topic statistics result. + now = std::chrono::system_clock::now(); + } + + any_callback_.dispatch(serialized_message, message_info); + + if (subscription_topic_statistics_) { + const auto nanos = std::chrono::time_point_cast(now); + const auto time = rclcpp::Time(nanos.time_since_epoch().count()); + subscription_topic_statistics_->handle_message(message_info.get_rmw_message_info(), time); + } } - void handle_intra_process_message( - rcl_interfaces::msg::IntraProcessMessage & ipm, - const rmw_message_info_t & message_info) + void + handle_loaned_message( + void * loaned_message, + const rclcpp::MessageInfo & message_info) override { - if (!get_intra_process_message_callback_) { - // throw std::runtime_error( - // "handle_intra_process_message called before setup_intra_process"); - // TODO(wjwwood): for now, this could mean that intra process was just not enabled. - // However, this can only really happen if this node has it disabled, but the other doesn't. + if (matches_any_intra_process_publishers(&message_info.get_rmw_message_info().publisher_gid)) { + // In this case, the message will be delivered via intra process and + // we should ignore this copy of the message. return; } - MessageUniquePtr msg; - get_intra_process_message_callback_( - ipm.publisher_id, - ipm.message_sequence, - intra_process_subscription_id_, - msg); - if (!msg) { - // This either occurred because the publisher no longer exists or the - // message requested is no longer being stored. - // TODO(wjwwood): should we notify someone of this? log error, log warning? - return; + + auto typed_message = static_cast(loaned_message); + // message is loaned, so we have to make sure that the deleter does not deallocate the message + auto sptr = std::shared_ptr( + typed_message, [](ROSMessageType * msg) {(void) msg;}); + + std::chrono::time_point now; + if (subscription_topic_statistics_) { + // get current time before executing callback to + // exclude callback duration from topic statistics result. + now = std::chrono::system_clock::now(); + } + + any_callback_.dispatch(sptr, message_info); + + if (subscription_topic_statistics_) { + const auto nanos = std::chrono::time_point_cast(now); + const auto time = rclcpp::Time(nanos.time_since_epoch().count()); + subscription_topic_statistics_->handle_message(message_info.get_rmw_message_info(), time); } - any_callback_.dispatch_intra_process(msg, message_info); } - using GetMessageCallbackType = - std::function; - using MatchesAnyPublishersCallbackType = std::function; + /// Return the borrowed message. + /** + * \param[inout] message message to be returned + */ + void + return_message(std::shared_ptr & message) override + { + auto typed_message = std::static_pointer_cast(message); + message_memory_strategy_->return_message(typed_message); + } + + /// Return the borrowed serialized message. + /** + * \param[inout] message serialized message to be returned + */ + void + return_serialized_message(std::shared_ptr & message) override + { + message_memory_strategy_->return_serialized_message(message); + } + + bool + use_take_shared_method() const + { + return any_callback_.use_take_shared_method(); + } - /// Implemenation detail. - void setup_intra_process( - uint64_t intra_process_subscription_id, - GetMessageCallbackType get_message_callback, - MatchesAnyPublishersCallbackType matches_any_publisher_callback, - const rcl_subscription_options_t & intra_process_options) + // DYNAMIC TYPE ================================================================================== + // TODO(methylDragon): Reorder later + // TODO(methylDragon): Implement later... + rclcpp::dynamic_typesupport::DynamicMessageType::SharedPtr + get_shared_dynamic_message_type() override { - std::string intra_process_topic_name = std::string(get_topic_name()) + "/_intra"; - rcl_ret_t ret = rcl_subscription_init( - &intra_process_subscription_handle_, - node_handle_.get(), - rclcpp::type_support::get_intra_process_message_msg_type_support(), - intra_process_topic_name.c_str(), - &intra_process_options); - if (ret != RCL_RET_OK) { - if (ret == RCL_RET_TOPIC_NAME_INVALID) { - auto rcl_node_handle = node_handle_.get(); - // this will throw on any validation problem - rcl_reset_error(); - expand_topic_or_service_name( - intra_process_topic_name, - rcl_node_get_name(rcl_node_handle), - rcl_node_get_namespace(rcl_node_handle)); - } + throw rclcpp::exceptions::UnimplementedError( + "get_shared_dynamic_message_type is not implemented for Subscription"); + } - rclcpp::exceptions::throw_from_rcl_error(ret, "could not create intra process subscription"); - } + rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr + get_shared_dynamic_message() override + { + throw rclcpp::exceptions::UnimplementedError( + "get_shared_dynamic_message is not implemented for Subscription"); + } - intra_process_subscription_id_ = intra_process_subscription_id; - get_intra_process_message_callback_ = get_message_callback; - matches_any_intra_process_publishers_ = matches_any_publisher_callback; + rclcpp::dynamic_typesupport::DynamicSerializationSupport::SharedPtr + get_shared_dynamic_serialization_support() override + { + throw rclcpp::exceptions::UnimplementedError( + "get_shared_dynamic_serialization_support is not implemented for Subscription"); } - /// Implemenation detail. - const rcl_subscription_t * - get_intra_process_subscription_handle() const + rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr + create_dynamic_message() override { - if (!get_intra_process_message_callback_) { - return nullptr; - } - return &intra_process_subscription_handle_; + throw rclcpp::exceptions::UnimplementedError( + "create_dynamic_message is not implemented for Subscription"); + } + + void + return_dynamic_message( + rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message) override + { + (void) message; + throw rclcpp::exceptions::UnimplementedError( + "return_dynamic_message is not implemented for Subscription"); + } + + void + handle_dynamic_message( + const rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message, + const rclcpp::MessageInfo & message_info) override + { + (void) message; + (void) message_info; + throw rclcpp::exceptions::UnimplementedError( + "handle_dynamic_message is not implemented for Subscription"); } private: RCLCPP_DISABLE_COPY(Subscription) - AnySubscriptionCallback any_callback_; - typename message_memory_strategy::MessageMemoryStrategy::SharedPtr + AnySubscriptionCallback any_callback_; + /// Copy of original options passed during construction. + /** + * It is important to save a copy of this so that the rmw payload which it + * may contain is kept alive for the duration of the subscription. + */ + const rclcpp::SubscriptionOptionsWithAllocator options_; + typename message_memory_strategy::MessageMemoryStrategy::SharedPtr message_memory_strategy_; - GetMessageCallbackType get_intra_process_message_callback_; - MatchesAnyPublishersCallbackType matches_any_intra_process_publishers_; - uint64_t intra_process_subscription_id_; + /// Component which computes and publishes topic statistics for this subscriber + SubscriptionTopicStatisticsSharedPtr subscription_topic_statistics_{nullptr}; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp new file mode 100644 index 0000000000..615f3852b6 --- /dev/null +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -0,0 +1,684 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__SUBSCRIPTION_BASE_HPP_ +#define RCLCPP__SUBSCRIPTION_BASE_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "rcl/event_callback.h" +#include "rcl/subscription.h" + +#include "rmw/impl/cpp/demangle.hpp" +#include "rmw/rmw.h" + +#include "rclcpp/any_subscription_callback.hpp" +#include "rclcpp/detail/cpp_callback_trampoline.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_message.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_message_type.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp" +#include "rclcpp/experimental/intra_process_manager.hpp" +#include "rclcpp/experimental/subscription_intra_process_base.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/message_info.hpp" +#include "rclcpp/network_flow_endpoint.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/event_handler.hpp" +#include "rclcpp/serialized_message.hpp" +#include "rclcpp/subscription_content_filter_options.hpp" +#include "rclcpp/type_support_decl.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ + +namespace node_interfaces +{ +class NodeBaseInterface; +} // namespace node_interfaces + +namespace experimental +{ +/** + * IntraProcessManager is forward declared here, avoiding a circular inclusion between + * `intra_process_manager.hpp` and `subscription_base.hpp`. + */ +class IntraProcessManager; +} // namespace experimental + +/// The kind of message that the subscription delivers in its callback, used by the executor +/** + * This enum needs to exist because the callback handle is not accessible to the executor's scope. + * + * "Kind" is used since what is being delivered is a category of messages, for example, there are + * different ROS message types that can be delivered, but they're all ROS messages. + * + * As a concrete example, all of the following callbacks will be considered ROS_MESSAGE for + * DeliveredMessageKind: + * - void callback(const std_msgs::msg::String &) + * - void callback(const std::string &) // type adaption + * - void callback(std::unique_ptr) + */ +enum class DeliveredMessageKind : uint8_t +{ + INVALID = 0, + ROS_MESSAGE = 1, // The subscription delivers a ROS message to its callback + SERIALIZED_MESSAGE = 2, // The subscription delivers a serialized message to its callback + DYNAMIC_MESSAGE = 3, // The subscription delivers a dynamic message to its callback +}; + +/// Virtual base class for subscriptions. This pattern allows us to iterate over different template +/// specializations of Subscription, among other things. +class SubscriptionBase : public std::enable_shared_from_this +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(SubscriptionBase) + + /// Constructor. + /** + * This accepts rcl_subscription_options_t instead of rclcpp::SubscriptionOptions because + * rclcpp::SubscriptionOptions::to_rcl_subscription_options depends on the message type. + * + * \param[in] node_base NodeBaseInterface pointer used in parts of the setup. + * \param[in] type_support_handle rosidl type support struct, for the Message type of the topic. + * \param[in] topic_name Name of the topic to subscribe to. + * \param[in] subscription_options Options for the subscription. + * \param[in] delivered_message_kind Enum flag to change how the message will be received and + * delivered + */ + RCLCPP_PUBLIC + SubscriptionBase( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + const rosidl_message_type_support_t & type_support_handle, + const std::string & topic_name, + const rcl_subscription_options_t & subscription_options, + const SubscriptionEventCallbacks & event_callbacks, + bool use_default_callbacks, + DeliveredMessageKind delivered_message_kind = DeliveredMessageKind::ROS_MESSAGE); + + /// Destructor. + RCLCPP_PUBLIC + virtual ~SubscriptionBase(); + + /// Add event handlers for passed in event_callbacks. + RCLCPP_PUBLIC + void + bind_event_callbacks( + const SubscriptionEventCallbacks & event_callbacks, bool use_default_callbacks); + + /// Get the topic that this subscription is subscribed on. + RCLCPP_PUBLIC + const char * + get_topic_name() const; + + RCLCPP_PUBLIC + std::shared_ptr + get_subscription_handle(); + + RCLCPP_PUBLIC + std::shared_ptr + get_subscription_handle() const; + + /// Get all the QoS event handlers associated with this subscription. + /** \return The map of QoS event handlers. */ + RCLCPP_PUBLIC + const + std::unordered_map> & + get_event_handlers() const; + + /// Get the actual QoS settings, after the defaults have been determined. + /** + * The actual configuration applied when using RMW_QOS_POLICY_*_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_QOS_POLICY_*_UNKNOWN. + * May throw runtime_error when an unexpected error occurs. + * + * \return The actual qos settings. + * \throws std::runtime_error if failed to get qos settings + */ + RCLCPP_PUBLIC + rclcpp::QoS + get_actual_qos() const; + + /// Take the next inter-process message from the subscription as a type erased pointer. + /** + * \sa Subscription::take() for details on how this function works. + * + * The only difference is that it takes a type erased pointer rather than a + * reference to the exact message type. + * + * This type erased version facilitates using the subscriptions in a type + * agnostic way using SubscriptionBase::create_message() and + * SubscriptionBase::handle_message(). + * + * \param[out] message_out The type erased message pointer into which take + * will copy the data. + * \param[out] message_info_out The message info for the taken message. + * \returns true if data was taken and is valid, otherwise false + * \throws any rcl errors from rcl_take, \sa rclcpp::exceptions::throw_from_rcl_error() + */ + RCLCPP_PUBLIC + bool + take_type_erased(void * message_out, rclcpp::MessageInfo & message_info_out); + + /// Take the next inter-process message, in its serialized form, from the subscription. + /** + * For now, if data is taken (written) into the message_out and + * message_info_out then true will be returned. + * Unlike Subscription::take(), taking data serialized is not possible via + * intra-process for the time being, so it will not need to de-duplicate + * data in any case. + * + * \param[out] message_out The serialized message data structure used to + * store the taken message. + * \param[out] message_info_out The message info for the taken message. + * \returns true if data was taken and is valid, otherwise false + * \throws any rcl errors from rcl_take, \sa rclcpp::exceptions::throw_from_rcl_error() + */ + RCLCPP_PUBLIC + bool + take_serialized(rclcpp::SerializedMessage & message_out, rclcpp::MessageInfo & message_info_out); + + /// Borrow a new message. + /** \return Shared pointer to the fresh message. */ + RCLCPP_PUBLIC + virtual + std::shared_ptr + create_message() = 0; + + /// Borrow a new serialized message + /** \return Shared pointer to a rcl_message_serialized_t. */ + RCLCPP_PUBLIC + virtual + std::shared_ptr + create_serialized_message() = 0; + + /// Check if we need to handle the message, and execute the callback if we do. + /** + * \param[in] message Shared pointer to the message to handle. + * \param[in] message_info Metadata associated with this message. + */ + RCLCPP_PUBLIC + virtual + void + handle_message(std::shared_ptr & message, const rclcpp::MessageInfo & message_info) = 0; + + RCLCPP_PUBLIC + virtual + void + handle_serialized_message( + const std::shared_ptr & serialized_message, + const rclcpp::MessageInfo & message_info) = 0; + + RCLCPP_PUBLIC + virtual + void + handle_loaned_message(void * loaned_message, const rclcpp::MessageInfo & message_info) = 0; + + /// Return the message borrowed in create_message. + /** \param[in] message Shared pointer to the returned message. */ + RCLCPP_PUBLIC + virtual + void + return_message(std::shared_ptr & message) = 0; + + /// Return the message borrowed in create_serialized_message. + /** \param[in] message Shared pointer to the returned message. */ + RCLCPP_PUBLIC + virtual + void + return_serialized_message(std::shared_ptr & message) = 0; + + RCLCPP_PUBLIC + const rosidl_message_type_support_t & + get_message_type_support_handle() const; + + /// Return if the subscription is serialized + /** + * \return `true` if the subscription is serialized, `false` otherwise + */ + RCLCPP_PUBLIC + bool + is_serialized() const; + + /// Return the delivered message kind. + /** + * \return `DeliveredMessageKind`, which adjusts how messages are received and delivered. + */ + RCLCPP_PUBLIC + DeliveredMessageKind + get_delivered_message_kind() const; + + /// Get matching publisher count. + /** \return The number of publishers on this topic. */ + RCLCPP_PUBLIC + size_t + get_publisher_count() const; + + /// 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. + * + * \return boolean flag indicating if middleware can loan messages. + */ + RCLCPP_PUBLIC + bool + can_loan_messages() const; + + using IntraProcessManagerWeakPtr = + std::weak_ptr; + + /// Implemenation detail. + RCLCPP_PUBLIC + void + setup_intra_process( + uint64_t intra_process_subscription_id, + IntraProcessManagerWeakPtr weak_ipm); + + /// Return the waitable for intra-process + /** + * \return the waitable sharedpointer for intra-process, or nullptr if intra-process is not setup. + * \throws std::runtime_error if the intra process manager is destroyed + */ + RCLCPP_PUBLIC + rclcpp::Waitable::SharedPtr + get_intra_process_waitable() const; + + /// Exchange state of whether or not a part of the subscription is used by a wait set. + /** + * Used to ensure parts of the subscription are not used with multiple wait + * sets simultaneously. + * + * \param[in] pointer_to_subscription_part address of a subscription part + * \param[in] in_use_state the new state to exchange, true means "now in use", + * and false means "no longer in use". + * \returns the current "in use" state. + * \throws std::invalid_argument If pointer_to_subscription_part is nullptr. + * \throws std::runtime_error If the pointer given is not a pointer to one of + * the parts of the subscription which can be used with a wait set. + */ + RCLCPP_PUBLIC + bool + exchange_in_use_by_wait_set_state(void * pointer_to_subscription_part, bool in_use_state); + + /// Get network flow endpoints + /** + * Describes network flow endpoints that this subscription is receiving messages on + * \return vector of NetworkFlowEndpoint + */ + RCLCPP_PUBLIC + std::vector + get_network_flow_endpoints() const; + + /// Set a callback to be called when each new message is received. + /** + * The callback receives a size_t which is the number of messages received + * since the last time this callback was called. + * Normally this is 1, but can be > 1 if messages were received before any + * callback was set. + * + * Since this callback is called from the middleware, you should aim to make + * it fast and not blocking. + * If you need to do a lot of work or wait for some other event, you should + * spin it off to another thread, otherwise you risk blocking the middleware. + * + * Calling it again will clear any previously set callback. + * + * This function is thread-safe. + * + * If you want more information available in the callback, like the subscription + * or other information, you may use a lambda with captures or std::bind. + * + * \sa rmw_subscription_set_on_new_message_callback + * \sa rcl_subscription_set_on_new_message_callback + * + * \param[in] callback functor to be called when a new message is received + */ + void + set_on_new_message_callback(std::function callback) + { + if (!callback) { + throw std::invalid_argument( + "The callback passed to set_on_new_message_callback " + "is not callable."); + } + + auto new_callback = + [callback, this](size_t number_of_messages) { + try { + callback(number_of_messages); + } catch (const std::exception & exception) { + RCLCPP_ERROR_STREAM( + node_logger_, + "rclcpp::SubscriptionBase@" << this << + " caught " << rmw::impl::cpp::demangle(exception) << + " exception in user-provided callback for the 'on new message' callback: " << + exception.what()); + } catch (...) { + RCLCPP_ERROR_STREAM( + node_logger_, + "rclcpp::SubscriptionBase@" << this << + " caught unhandled exception in user-provided callback " << + "for the 'on new message' callback"); + } + }; + + std::lock_guard lock(callback_mutex_); + + // Set it temporarily to the new callback, while we replace the old one. + // This two-step setting, prevents a gap where the old std::function has + // been replaced but the middleware hasn't been told about the new one yet. + set_on_new_message_callback( + rclcpp::detail::cpp_callback_trampoline, + static_cast(&new_callback)); + + // Store the std::function to keep it in scope, also overwrites the existing one. + on_new_message_callback_ = new_callback; + + // Set it again, now using the permanent storage. + set_on_new_message_callback( + rclcpp::detail::cpp_callback_trampoline< + decltype(on_new_message_callback_), const void *, size_t>, + static_cast(&on_new_message_callback_)); + } + + /// Unset the callback registered for new messages, if any. + void + clear_on_new_message_callback() + { + std::lock_guard lock(callback_mutex_); + + if (on_new_message_callback_) { + set_on_new_message_callback(nullptr, nullptr); + on_new_message_callback_ = nullptr; + } + } + + /// Set a callback to be called when each new intra-process message is received. + /** + * The callback receives a size_t which is the number of messages received + * since the last time this callback was called. + * Normally this is 1, but can be > 1 if messages were received before any + * callback was set. + * + * Calling it again will clear any previously set callback. + * + * This function is thread-safe. + * + * If you want more information available in the callback, like the subscription + * or other information, you may use a lambda with captures or std::bind. + * + * \sa rclcpp::SubscriptionIntraProcessBase::set_on_ready_callback + * + * \param[in] callback functor to be called when a new message is received + */ + void + set_on_new_intra_process_message_callback(std::function callback) + { + if (!use_intra_process_) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Calling set_on_new_intra_process_message_callback for subscription with IPC disabled"); + return; + } + + if (!callback) { + throw std::invalid_argument( + "The callback passed to set_on_new_intra_process_message_callback " + "is not callable."); + } + + // The on_ready_callback signature has an extra `int` argument used to disambiguate between + // possible different entities within a generic waitable. + // We hide that detail to users of this method. + std::function new_callback = std::bind(callback, std::placeholders::_1); + subscription_intra_process_->set_on_ready_callback(new_callback); + } + + /// Unset the callback registered for new intra-process messages, if any. + void + clear_on_new_intra_process_message_callback() + { + if (!use_intra_process_) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Calling clear_on_new_intra_process_message_callback for subscription with IPC disabled"); + return; + } + + subscription_intra_process_->clear_on_ready_callback(); + } + + /// Set a callback to be called when each new qos event instance occurs. + /** + * The callback receives a size_t which is the number of events that occurred + * since the last time this callback was called. + * Normally this is 1, but can be > 1 if events occurred before any + * callback was set. + * + * Since this callback is called from the middleware, you should aim to make + * it fast and not blocking. + * If you need to do a lot of work or wait for some other event, you should + * spin it off to another thread, otherwise you risk blocking the middleware. + * + * Calling it again will clear any previously set callback. + * + * An exception will be thrown if the callback is not callable. + * + * This function is thread-safe. + * + * If you want more information available in the callback, like the qos event + * or other information, you may use a lambda with captures or std::bind. + * + * \sa rclcpp::EventHandlerBase::set_on_ready_callback + * + * \param[in] callback functor to be called when a new event occurs + * \param[in] event_type identifier for the qos event we want to attach the callback to + */ + void + set_on_new_qos_event_callback( + std::function callback, + rcl_subscription_event_type_t event_type) + { + if (event_handlers_.count(event_type) == 0) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Calling set_on_new_qos_event_callback for non registered subscription event_type"); + return; + } + + if (!callback) { + throw std::invalid_argument( + "The callback passed to set_on_new_qos_event_callback " + "is not callable."); + } + + // The on_ready_callback signature has an extra `int` argument used to disambiguate between + // possible different entities within a generic waitable. + // We hide that detail to users of this method. + std::function new_callback = std::bind(callback, std::placeholders::_1); + event_handlers_[event_type]->set_on_ready_callback(new_callback); + } + + /// Unset the callback registered for new qos events, if any. + void + clear_on_new_qos_event_callback(rcl_subscription_event_type_t event_type) + { + if (event_handlers_.count(event_type) == 0) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Calling clear_on_new_qos_event_callback for non registered event_type"); + return; + } + + event_handlers_[event_type]->clear_on_ready_callback(); + } + + /// Check if content filtered topic feature of the subscription instance is enabled. + /** + * \return boolean flag indicating if the content filtered topic of this subscription is enabled. + */ + RCLCPP_PUBLIC + bool + is_cft_enabled() const; + + /// Set the filter expression and expression parameters for the subscription. + /** + * \param[in] filter_expression A filter expression to set. + * \sa ContentFilterOptions::filter_expression + * An empty string ("") will clear the content filter setting of the subscription. + * \param[in] expression_parameters Array of expression parameters to set. + * \sa ContentFilterOptions::expression_parameters + * \throws RCLBadAlloc if memory cannot be allocated + * \throws RCLError if an unexpect error occurs + */ + RCLCPP_PUBLIC + void + set_content_filter( + const std::string & filter_expression, + const std::vector & expression_parameters = {}); + + /// Get the filter expression and expression parameters for the subscription. + /** + * \return rclcpp::ContentFilterOptions The content filter options to get. + * \throws RCLBadAlloc if memory cannot be allocated + * \throws RCLError if an unexpect error occurs + */ + RCLCPP_PUBLIC + rclcpp::ContentFilterOptions + get_content_filter() const; + + // DYNAMIC TYPE ================================================================================== + // TODO(methylDragon): Reorder later + RCLCPP_PUBLIC + virtual + rclcpp::dynamic_typesupport::DynamicMessageType::SharedPtr + get_shared_dynamic_message_type() = 0; + + RCLCPP_PUBLIC + virtual + rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr + get_shared_dynamic_message() = 0; + + RCLCPP_PUBLIC + virtual + rclcpp::dynamic_typesupport::DynamicSerializationSupport::SharedPtr + get_shared_dynamic_serialization_support() = 0; + + /// Borrow a new serialized message (this clones!) + /** \return Shared pointer to a rclcpp::dynamic_typesupport::DynamicMessage. */ + RCLCPP_PUBLIC + virtual + rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr + create_dynamic_message() = 0; + + RCLCPP_PUBLIC + virtual + void + return_dynamic_message(rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message) = 0; + + RCLCPP_PUBLIC + virtual + void + handle_dynamic_message( + const rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message, + const rclcpp::MessageInfo & message_info) = 0; + + RCLCPP_PUBLIC + bool + take_dynamic_message( + rclcpp::dynamic_typesupport::DynamicMessage & message_out, + rclcpp::MessageInfo & message_info_out); + // =============================================================================================== + +protected: + template + void + add_event_handler( + const EventCallbackT & callback, + const rcl_subscription_event_type_t event_type) + { + auto handler = std::make_shared>>( + callback, + rcl_subscription_event_init, + get_subscription_handle(), + event_type); + qos_events_in_use_by_wait_set_.insert(std::make_pair(handler.get(), false)); + event_handlers_.insert(std::make_pair(event_type, handler)); + } + + RCLCPP_PUBLIC + void default_incompatible_qos_callback(QOSRequestedIncompatibleQoSInfo & info) const; + + RCLCPP_PUBLIC + void default_incompatible_type_callback(IncompatibleTypeInfo & info) const; + + RCLCPP_PUBLIC + bool + matches_any_intra_process_publishers(const rmw_gid_t * sender_gid) const; + + RCLCPP_PUBLIC + void + set_on_new_message_callback(rcl_event_callback_t callback, const void * user_data); + + rclcpp::node_interfaces::NodeBaseInterface * const node_base_; + + std::shared_ptr node_handle_; + + std::recursive_mutex callback_mutex_; + // It is important to declare on_new_message_callback_ before + // subscription_handle_, so on destruction the subscription is + // destroyed first. Otherwise, the rmw subscription callback + // would point briefly to a destroyed function. + std::function on_new_message_callback_{nullptr}; + // Declare subscription_handle_ after callback + std::shared_ptr subscription_handle_; + std::shared_ptr intra_process_subscription_handle_; + rclcpp::Logger node_logger_; + + std::unordered_map> event_handlers_; + + bool use_intra_process_; + IntraProcessManagerWeakPtr weak_ipm_; + uint64_t intra_process_subscription_id_; + std::shared_ptr subscription_intra_process_; + + const SubscriptionEventCallbacks event_callbacks_; + +private: + RCLCPP_DISABLE_COPY(SubscriptionBase) + + rosidl_message_type_support_t type_support_; + DeliveredMessageKind delivered_message_kind_; + + std::atomic subscription_in_use_by_wait_set_{false}; + std::atomic intra_process_subscription_waitable_in_use_by_wait_set_{false}; + std::unordered_map> qos_events_in_use_by_wait_set_; +}; + +} // namespace rclcpp + +#endif // RCLCPP__SUBSCRIPTION_BASE_HPP_ diff --git a/rclcpp/include/rclcpp/subscription_content_filter_options.hpp b/rclcpp/include/rclcpp/subscription_content_filter_options.hpp new file mode 100644 index 0000000000..8de034ddf5 --- /dev/null +++ b/rclcpp/include/rclcpp/subscription_content_filter_options.hpp @@ -0,0 +1,38 @@ +// Copyright 2022 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__SUBSCRIPTION_CONTENT_FILTER_OPTIONS_HPP_ +#define RCLCPP__SUBSCRIPTION_CONTENT_FILTER_OPTIONS_HPP_ + +#include +#include + +namespace rclcpp +{ + +/// Options to configure content filtered topic in the subscription. +struct ContentFilterOptions +{ + /// Filter expression is similar to the WHERE part of an SQL clause. + std::string filter_expression; + /** + * Expression parameters is the tokens placeholder ‘parameters’ (i.e., "%n" tokens begin from 0) + * in the filter_expression. The maximum expression_parameters size is 100. + */ + std::vector expression_parameters; +}; + +} // namespace rclcpp + +#endif // RCLCPP__SUBSCRIPTION_CONTENT_FILTER_OPTIONS_HPP_ diff --git a/rclcpp/include/rclcpp/subscription_factory.hpp b/rclcpp/include/rclcpp/subscription_factory.hpp index ad40f8a3a4..0e9d9fefe5 100644 --- a/rclcpp/include/rclcpp/subscription_factory.hpp +++ b/rclcpp/include/rclcpp/subscription_factory.hpp @@ -24,144 +24,102 @@ #include "rosidl_typesupport_cpp/message_type_support.hpp" -#include "rclcpp/subscription.hpp" -#include "rclcpp/intra_process_manager.hpp" +#include "rclcpp/any_subscription_callback.hpp" +#include "rclcpp/get_message_type_support_handle.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/subscription.hpp" +#include "rclcpp/subscription_options.hpp" +#include "rclcpp/subscription_traits.hpp" +#include "rclcpp/topic_statistics/subscription_topic_statistics.hpp" #include "rclcpp/visibility_control.hpp" namespace rclcpp { -/// Factory with functions used to create a Subscription. +/// Factory containing a function used to create a Subscription. /** - * This factory class is used to encapsulate the template generated functions - * which are used during the creation of a Message type specific subscription + * This factory class is used to encapsulate the template generated function + * which is used during the creation of a Message type specific subscription * within a non-templated class. * * It is created using the create_subscription_factory function, which is * usually called from a templated "create_subscription" method of the Node * class, and is passed to the non-templated "create_subscription" method of * the NodeTopics class where it is used to create and setup the Subscription. + * + * It also handles the two step construction of Subscriptions, first calling + * the constructor and then the post_init_setup() method. */ struct SubscriptionFactory { // Creates a Subscription object and returns it as a SubscriptionBase. using SubscriptionFactoryFunction = std::function< - rclcpp::SubscriptionBase::SharedPtr( - rclcpp::node_interfaces::NodeBaseInterface * node_base, - const std::string & topic_name, - rcl_subscription_options_t & subscription_options)>; - - SubscriptionFactoryFunction create_typed_subscription; + rclcpp::SubscriptionBase::SharedPtr( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + const std::string & topic_name, + const rclcpp::QoS & qos)>; - // Function that takes a MessageT from the intra process manager - using SetupIntraProcessFunction = std::function< - void( - rclcpp::intra_process_manager::IntraProcessManager::SharedPtr ipm, - rclcpp::SubscriptionBase::SharedPtr subscription, - const rcl_subscription_options_t & subscription_options)>; - - SetupIntraProcessFunction setup_intra_process; + const SubscriptionFactoryFunction create_typed_subscription; }; -/// Return a SubscriptionFactory with functions for creating a SubscriptionT. -template +/// Return a SubscriptionFactory setup to create a SubscriptionT. +/** + * \param[in] callback The user-defined callback function to receive a message + * \param[in] options Additional options for the creation of the Subscription. + * \param[in] msg_mem_strat The message memory strategy to use for allocating messages. + * \param[in] subscription_topic_stats Optional stats callback for topic_statistics + */ +template< + typename MessageT, + typename CallbackT, + typename AllocatorT, + typename SubscriptionT = rclcpp::Subscription, + typename MessageMemoryStrategyT = typename SubscriptionT::MessageMemoryStrategyType +> SubscriptionFactory create_subscription_factory( CallbackT && callback, - typename rclcpp::message_memory_strategy::MessageMemoryStrategy::SharedPtr - msg_mem_strat, - std::shared_ptr allocator) + const rclcpp::SubscriptionOptionsWithAllocator & options, + typename MessageMemoryStrategyT::SharedPtr msg_mem_strat, + std::shared_ptr + subscription_topic_stats = nullptr +) { - SubscriptionFactory factory; + auto allocator = options.get_allocator(); using rclcpp::AnySubscriptionCallback; - AnySubscriptionCallback any_subscription_callback(allocator); + AnySubscriptionCallback any_subscription_callback(*allocator); any_subscription_callback.set(std::forward(callback)); - auto message_alloc = - std::make_shared::MessageAlloc>(); - - // factory function that creates a MessageT specific SubscriptionT - factory.create_typed_subscription = - [allocator, msg_mem_strat, any_subscription_callback, message_alloc]( - rclcpp::node_interfaces::NodeBaseInterface * node_base, - const std::string & topic_name, - rcl_subscription_options_t & subscription_options + SubscriptionFactory factory { + // factory function that creates a MessageT specific SubscriptionT + [options, msg_mem_strat, any_subscription_callback, subscription_topic_stats]( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + const std::string & topic_name, + const rclcpp::QoS & qos ) -> rclcpp::SubscriptionBase::SharedPtr { - subscription_options.allocator = - rclcpp::allocator::get_rcl_allocator(*message_alloc.get()); - using rclcpp::Subscription; using rclcpp::SubscriptionBase; - auto sub = Subscription::make_shared( - node_base->get_shared_rcl_node_handle(), + auto sub = Subscription::make_shared( + node_base, + rclcpp::get_message_type_support_handle(), topic_name, - subscription_options, + qos, any_subscription_callback, - msg_mem_strat); + options, + msg_mem_strat, + subscription_topic_stats); + // This is used for setting up things like intra process comms which + // require this->shared_from_this() which cannot be called from + // the constructor. + sub->post_init_setup(node_base, qos, options); auto sub_base_ptr = std::dynamic_pointer_cast(sub); return sub_base_ptr; - }; - - // function that will setup intra process communications for the subscription - factory.setup_intra_process = - [message_alloc]( - rclcpp::intra_process_manager::IntraProcessManager::SharedPtr ipm, - rclcpp::SubscriptionBase::SharedPtr subscription, - const rcl_subscription_options_t & subscription_options) - { - rclcpp::intra_process_manager::IntraProcessManager::WeakPtr weak_ipm = ipm; - uint64_t intra_process_subscription_id = ipm->add_subscription(subscription); - - auto intra_process_options = rcl_subscription_get_default_options(); - intra_process_options.allocator = rclcpp::allocator::get_rcl_allocator( - *message_alloc.get()); - intra_process_options.qos = subscription_options.qos; - intra_process_options.ignore_local_publications = false; - - // function that will be called to take a MessageT from the intra process manager - auto take_intra_process_message_func = - [weak_ipm]( - uint64_t publisher_id, - uint64_t message_sequence, - uint64_t subscription_id, - typename rclcpp::Subscription::MessageUniquePtr & message) - { - auto ipm = weak_ipm.lock(); - if (!ipm) { - // TODO(wjwwood): should this just return silently? Or return with a logged warning? - throw std::runtime_error( - "intra process take called after destruction of intra process manager"); - } - ipm->take_intra_process_message( - publisher_id, message_sequence, subscription_id, message); - }; - - // function that is called to see if the publisher id matches any local publishers - auto matches_any_publisher_func = - [weak_ipm](const rmw_gid_t * sender_gid) -> bool - { - auto ipm = weak_ipm.lock(); - if (!ipm) { - throw std::runtime_error( - "intra process publisher check called " - "after destruction of intra process manager"); - } - return ipm->matches_any_publishers(sender_gid); - }; - - auto typed_sub_ptr = std::dynamic_pointer_cast(subscription); - typed_sub_ptr->setup_intra_process( - intra_process_subscription_id, - take_intra_process_message_func, - matches_any_publisher_func, - intra_process_options - ); - }; - // end definition of factory function to setup intra process + } + }; // return the factory now that it is populated return factory; diff --git a/rclcpp/include/rclcpp/subscription_options.hpp b/rclcpp/include/rclcpp/subscription_options.hpp new file mode 100644 index 0000000000..0dd738ee60 --- /dev/null +++ b/rclcpp/include/rclcpp/subscription_options.hpp @@ -0,0 +1,189 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__SUBSCRIPTION_OPTIONS_HPP_ +#define RCLCPP__SUBSCRIPTION_OPTIONS_HPP_ + +#include +#include +#include +#include +#include + +#include "rclcpp/callback_group.hpp" +#include "rclcpp/detail/rmw_implementation_specific_subscription_payload.hpp" +#include "rclcpp/intra_process_buffer_type.hpp" +#include "rclcpp/intra_process_setting.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/event_handler.hpp" +#include "rclcpp/qos_overriding_options.hpp" +#include "rclcpp/subscription_content_filter_options.hpp" +#include "rclcpp/topic_statistics_state.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ + +/// Non-template base class for subscription options. +struct SubscriptionOptionsBase +{ + /// Callbacks for events related to this subscription. + SubscriptionEventCallbacks event_callbacks; + + /// Whether or not to use default callbacks when user doesn't supply any in event_callbacks + bool use_default_callbacks = true; + + /// True to ignore local publications. + bool ignore_local_publications = false; + + /// Require middleware to generate unique network flow endpoints + /// Disabled by default + rmw_unique_network_flow_endpoints_requirement_t require_unique_network_flow_endpoints = + RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_NOT_REQUIRED; + + /// The callback group for this subscription. NULL to use the default callback group. + rclcpp::CallbackGroup::SharedPtr callback_group = nullptr; + + /// Setting to explicitly set intraprocess communications. + IntraProcessSetting use_intra_process_comm = IntraProcessSetting::NodeDefault; + + /// Setting the data-type stored in the intraprocess buffer + IntraProcessBufferType intra_process_buffer_type = IntraProcessBufferType::CallbackDefault; + + /// Optional RMW implementation specific payload to be used during creation of the subscription. + std::shared_ptr + rmw_implementation_payload = nullptr; + + // Options to configure topic statistics collector in the subscription. + struct TopicStatisticsOptions + { + // Enable and disable topic statistics calculation and publication. Defaults to disabled. + TopicStatisticsState state = TopicStatisticsState::NodeDefault; + + // Topic to which topic statistics get published when enabled. Defaults to /statistics. + std::string publish_topic = "/statistics"; + + // Topic statistics publication period in ms. Defaults to one second. + // Only values greater than zero are allowed. + std::chrono::milliseconds publish_period{std::chrono::seconds(1)}; + + // An optional QoS which can provide topic_statistics with a stable QoS separate from + // the subscription's current QoS settings which could be unstable. + // Explicitly set the enough depth to avoid missing the statistics messages. + rclcpp::QoS qos = SystemDefaultsQoS().keep_last(10); + }; + + TopicStatisticsOptions topic_stats_options; + + QosOverridingOptions qos_overriding_options; + + ContentFilterOptions content_filter_options; +}; + +/// Structure containing optional configuration for Subscriptions. +template +struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase +{ + static_assert( + std::is_void_v::value_type>, + "Subscription allocator value type must be void"); + + /// Optional custom allocator. + std::shared_ptr allocator = nullptr; + + SubscriptionOptionsWithAllocator() {} + + /// Constructor using base class as input. + explicit SubscriptionOptionsWithAllocator( + const SubscriptionOptionsBase & subscription_options_base) + : SubscriptionOptionsBase(subscription_options_base) + {} + + /// Convert this class, with a rclcpp::QoS, into an rcl_subscription_options_t. + /** + * \param qos QoS profile for subcription. + * \return rcl_subscription_options_t structure based on the rclcpp::QoS + */ + rcl_subscription_options_t + to_rcl_subscription_options(const rclcpp::QoS & qos) const + { + rcl_subscription_options_t result = rcl_subscription_get_default_options(); + result.allocator = this->get_rcl_allocator(); + result.qos = qos.get_rmw_qos_profile(); + result.rmw_subscription_options.ignore_local_publications = this->ignore_local_publications; + result.rmw_subscription_options.require_unique_network_flow_endpoints = + this->require_unique_network_flow_endpoints; + + // Apply payload to rcl_subscription_options if necessary. + if (rmw_implementation_payload && rmw_implementation_payload->has_been_customized()) { + rmw_implementation_payload->modify_rmw_subscription_options(result.rmw_subscription_options); + } + + // Copy content_filter_options into rcl_subscription_options. + if (!content_filter_options.filter_expression.empty()) { + std::vector cstrings = + get_c_vector_string(content_filter_options.expression_parameters); + rcl_ret_t ret = rcl_subscription_options_set_content_filter_options( + get_c_string(content_filter_options.filter_expression), + cstrings.size(), + cstrings.data(), + &result); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "failed to set content_filter_options"); + } + } + + return result; + } + + std::shared_ptr + get_allocator() const + { + if (!this->allocator) { + if (!allocator_storage_) { + allocator_storage_ = std::make_shared(); + } + return allocator_storage_; + } + return this->allocator; + } + +private: + using PlainAllocator = + typename std::allocator_traits::template rebind_alloc; + + rcl_allocator_t + get_rcl_allocator() const + { + if (!plain_allocator_storage_) { + plain_allocator_storage_ = + std::make_shared(*this->get_allocator()); + } + return rclcpp::allocator::get_rcl_allocator(*plain_allocator_storage_); + } + + // This is a temporal workaround, to make sure that get_allocator() + // always returns a copy of the same allocator. + mutable std::shared_ptr allocator_storage_; + + // This is a temporal workaround, to keep the plain allocator that backs + // up the rcl allocator returned in rcl_subscription_options_t alive. + mutable std::shared_ptr plain_allocator_storage_; +}; + +using SubscriptionOptions = SubscriptionOptionsWithAllocator>; +} // namespace rclcpp + +#endif // RCLCPP__SUBSCRIPTION_OPTIONS_HPP_ diff --git a/rclcpp/include/rclcpp/subscription_traits.hpp b/rclcpp/include/rclcpp/subscription_traits.hpp new file mode 100644 index 0000000000..34f2cc9219 --- /dev/null +++ b/rclcpp/include/rclcpp/subscription_traits.hpp @@ -0,0 +1,104 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__SUBSCRIPTION_TRAITS_HPP_ +#define RCLCPP__SUBSCRIPTION_TRAITS_HPP_ + +#include + +#include "rclcpp/function_traits.hpp" +#include "rclcpp/serialized_message.hpp" +#include "rclcpp/subscription_options.hpp" +#include "rcl/types.h" + +namespace rclcpp +{ + +class QoS; + +namespace subscription_traits +{ + +/* + * The current version of uncrustify has a misinterpretion here + * between `:` used for inheritance vs for initializer list + * The result is that whenever a templated struct is used, + * the colon has to be without any whitespace next to it whereas + * when no template is used, the colon has to be separated by a space. + * Cheers! + */ +template +struct is_serialized_subscription_argument : std::false_type +{}; + +template<> +struct is_serialized_subscription_argument: std::true_type +{}; + +template<> +struct is_serialized_subscription_argument> + : std::true_type +{}; + +template +struct is_serialized_subscription : is_serialized_subscription_argument +{}; + +template +struct is_serialized_callback + : is_serialized_subscription_argument< + typename rclcpp::function_traits::function_traits::template argument_type<0>> +{}; + +template +struct extract_message_type +{ + using type = typename std::remove_cv_t>; +}; + +template +struct extract_message_type>: extract_message_type +{}; + +template +struct extract_message_type>: extract_message_type +{}; + +template< + typename CallbackT, + typename AllocatorT = std::allocator, + // Do not attempt if CallbackT is an integer (mistaken for depth) + typename = std::enable_if_t>>::value>, + // Do not attempt if CallbackT is a QoS (mistaken for qos) + typename = std::enable_if_t>>::value>, + // Do not attempt if CallbackT is a rmw_qos_profile_t (mistaken for qos profile) + typename = std::enable_if_t>>::value>, + // Do not attempt if CallbackT is a rclcpp::SubscriptionOptionsWithAllocator + typename = std::enable_if_t, + std::remove_cv_t>>::value> +> +struct has_message_type : extract_message_type< + typename rclcpp::function_traits::function_traits::template argument_type<0>> +{}; + +} // namespace subscription_traits +} // namespace rclcpp + +#endif // RCLCPP__SUBSCRIPTION_TRAITS_HPP_ diff --git a/rclcpp/include/rclcpp/subscription_wait_set_mask.hpp b/rclcpp/include/rclcpp/subscription_wait_set_mask.hpp new file mode 100644 index 0000000000..597dd81d3e --- /dev/null +++ b/rclcpp/include/rclcpp/subscription_wait_set_mask.hpp @@ -0,0 +1,37 @@ +// 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 RCLCPP__SUBSCRIPTION_WAIT_SET_MASK_HPP_ +#define RCLCPP__SUBSCRIPTION_WAIT_SET_MASK_HPP_ + +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ + +/// Options used to determine what parts of a subscription get added to or removed from a wait set. +class RCLCPP_PUBLIC SubscriptionWaitSetMask +{ +public: + /// If true, include the actual subscription. + bool include_subscription = true; + /// If true, include any events attached to the subscription. + bool include_events = true; + /// If true, include the waitable used to handle intra process communication. + bool include_intra_process_waitable = true; +}; + +} // namespace rclcpp + +#endif // RCLCPP__SUBSCRIPTION_WAIT_SET_MASK_HPP_ diff --git a/rclcpp/include/rclcpp/time.hpp b/rclcpp/include/rclcpp/time.hpp index 22e0335da8..7ebf9f21b8 100644 --- a/rclcpp/include/rclcpp/time.hpp +++ b/rclcpp/include/rclcpp/time.hpp @@ -31,37 +31,91 @@ class Clock; class Time { public: + /// Time constructor + /** + * Initializes the time values for seconds and nanoseconds individually. + * Large values for nanoseconds are wrapped automatically with the remainder added to seconds. + * Both inputs must be integers. + * + * \param seconds part of the time in seconds since time epoch + * \param nanoseconds part of the time in nanoseconds since time epoch + * \param clock_type clock type + * \throws std::runtime_error if seconds are negative + */ RCLCPP_PUBLIC Time(int32_t seconds, uint32_t nanoseconds, rcl_clock_type_t clock_type = RCL_SYSTEM_TIME); + /// Time constructor + /** + * \param nanoseconds since time epoch + * \param clock_type clock type + * \throws std::runtime_error if nanoseconds are negative + */ RCLCPP_PUBLIC - explicit Time(int64_t nanoseconds = 0, rcl_clock_type_t clock = RCL_SYSTEM_TIME); + explicit Time(int64_t nanoseconds = 0, rcl_clock_type_t clock_type = RCL_SYSTEM_TIME); + /// Copy constructor RCLCPP_PUBLIC Time(const Time & rhs); + /// Move constructor + RCLCPP_PUBLIC + Time(Time && rhs) noexcept; + + /// Time constructor + /** + * \param time_msg builtin_interfaces time message to copy + * \param clock_type clock type + * \throws std::runtime_error if seconds are negative + */ RCLCPP_PUBLIC Time( const builtin_interfaces::msg::Time & time_msg, - rcl_clock_type_t ros_time = RCL_ROS_TIME); + rcl_clock_type_t clock_type = RCL_ROS_TIME); + /// Time constructor + /** + * \param time_point rcl_time_point_t structure to copy + */ RCLCPP_PUBLIC explicit Time(const rcl_time_point_t & time_point); + /// Time destructor RCLCPP_PUBLIC virtual ~Time(); + /// Return a builtin_interfaces::msg::Time object based RCLCPP_PUBLIC operator builtin_interfaces::msg::Time() const; + /** + * Copy assignment operator + * \throws std::runtime_error if seconds are negative + */ RCLCPP_PUBLIC Time & operator=(const Time & rhs); + /** + * Assign Time from a builtin_interfaces::msg::Time instance. + * The clock_type will be reset to RCL_ROS_TIME. + * Equivalent to *this = Time(time_msg, RCL_ROS_TIME). + * \throws std::runtime_error if seconds are negative + */ RCLCPP_PUBLIC Time & operator=(const builtin_interfaces::msg::Time & time_msg); + /** + * Move assignment operator + */ + RCLCPP_PUBLIC + Time & + operator=(Time && rhs) noexcept; + + /** + * \throws std::runtime_error if the time sources are different + */ RCLCPP_PUBLIC bool operator==(const rclcpp::Time & rhs) const; @@ -70,38 +124,101 @@ class Time bool operator!=(const rclcpp::Time & rhs) const; + /** + * \throws std::runtime_error if the time sources are different + */ RCLCPP_PUBLIC bool operator<(const rclcpp::Time & rhs) const; + /** + * \throws std::runtime_error if the time sources are different + */ RCLCPP_PUBLIC bool operator<=(const rclcpp::Time & rhs) const; + /** + * \throws std::runtime_error if the time sources are different + */ RCLCPP_PUBLIC bool operator>=(const rclcpp::Time & rhs) const; + /** + * \throws std::runtime_error if the time sources are different + */ RCLCPP_PUBLIC bool operator>(const rclcpp::Time & rhs) const; + /** + * \throws std::overflow_error if addition leads to overflow + */ RCLCPP_PUBLIC Time operator+(const rclcpp::Duration & rhs) const; + /** + * \throws std::runtime_error if the time sources are different + * \throws std::overflow_error if addition leads to overflow + */ RCLCPP_PUBLIC Duration operator-(const rclcpp::Time & rhs) const; + /** + * \throws std::overflow_error if addition leads to overflow + */ RCLCPP_PUBLIC Time operator-(const rclcpp::Duration & rhs) const; + /** + * \throws std::overflow_error if addition leads to overflow + */ + RCLCPP_PUBLIC + Time & + operator+=(const rclcpp::Duration & rhs); + + /** + * \throws std::overflow_error if addition leads to overflow + */ + RCLCPP_PUBLIC + Time & + operator-=(const rclcpp::Duration & rhs); + + /// Get the nanoseconds since epoch + /** + * \return the nanoseconds since epoch as a rcl_time_point_value_t structure. + */ RCLCPP_PUBLIC rcl_time_point_value_t nanoseconds() const; + /// Get the maximum representable value. + /** + * \return the maximum representable value + */ + RCLCPP_PUBLIC + static Time + max(rcl_clock_type_t clock_type = RCL_SYSTEM_TIME); // NOLINT + + /// Get the seconds since epoch + /** + * \warning Depending on sizeof(double) there could be significant precision loss. + * When an exact time is required use nanoseconds() instead. + * + * \return the seconds since epoch as a floating point number. + */ + RCLCPP_PUBLIC + double + seconds() const; + + /// Get the clock type + /** + * \return the clock type + */ RCLCPP_PUBLIC rcl_clock_type_t get_clock_type() const; @@ -111,9 +228,22 @@ class Time friend Clock; // Allow clock to manipulate internal data }; +/** + * \throws std::overflow_error if addition leads to overflow + */ +RCLCPP_PUBLIC Time operator+(const rclcpp::Duration & lhs, const rclcpp::Time & rhs); +/// Convert rcl_time_point_value_t to builtin_interfaces::msg::Time +/** + * \param[in] time_point is a rcl_time_point_value_t + * \return the builtin_interfaces::msg::Time from the time_point + */ +RCLCPP_PUBLIC +builtin_interfaces::msg::Time +convert_rcl_time_to_sec_nanos(const rcl_time_point_value_t & time_point); + } // namespace rclcpp #endif // RCLCPP__TIME_HPP_ diff --git a/rclcpp/include/rclcpp/time_source.hpp b/rclcpp/include/rclcpp/time_source.hpp index 0d6f8dad30..0de9b368e8 100644 --- a/rclcpp/include/rclcpp/time_source.hpp +++ b/rclcpp/include/rclcpp/time_source.hpp @@ -21,106 +21,138 @@ #include "rcl/time.h" #include "builtin_interfaces/msg/time.hpp" +#include "rosgraph_msgs/msg/clock.hpp" #include "rcl_interfaces/msg/parameter_event.hpp" #include "rclcpp/node.hpp" -#include "rclcpp/parameter_client.hpp" -#include "rclcpp/parameter_events_filter.hpp" +#include "rclcpp/executors.hpp" +#include "rclcpp/node_interfaces/node_parameters_interface.hpp" namespace rclcpp { class Clock; +/** + * Time source that will drive the attached clocks. + * + * If the attached node `use_sim_time` parameter is `true`, the attached clocks will + * be updated based on messages received. + * + * The subscription to the clock topic created by the time source can have it's qos reconfigured + * using parameter overrides, particularly the following ones are accepted: + * + * - qos_overrides./clock.depth + * - qos_overrides./clock.durability + * - qos_overrides./clock.history + * - qos_overrides./clock.reliability + */ class TimeSource { public: + /// Constructor + /** + * The node will be attached to the time source. + * + * \param node std::shared pointer to a initialized node + * \param qos QoS that will be used when creating a `/clock` subscription. + * \param use_clock_thread whether to spin the attached node in a separate thread + */ RCLCPP_PUBLIC - explicit TimeSource(rclcpp::Node::SharedPtr node); + explicit TimeSource( + rclcpp::Node::SharedPtr node, + const rclcpp::QoS & qos = rclcpp::ClockQoS(), + bool use_clock_thread = true); + /// Empty constructor + /** + * An Empty TimeSource class + * + * \param qos QoS that will be used when creating a `/clock` subscription. + * \param use_clock_thread whether to spin the attached node in a separate thread. + */ RCLCPP_PUBLIC - TimeSource(); + explicit TimeSource( + const rclcpp::QoS & qos = rclcpp::ClockQoS(), + bool use_clock_thread = true); + + // The TimeSource is uncopyable + TimeSource(const TimeSource &) = delete; + TimeSource & operator=(const TimeSource &) = delete; + // The TimeSource is moveable + TimeSource(TimeSource &&) = default; + TimeSource & operator=(TimeSource &&) = default; + + /// Attach node to the time source. + /** + * \param node std::shared pointer to a initialized node + */ RCLCPP_PUBLIC void attachNode(rclcpp::Node::SharedPtr node); + /// Attach node to the time source. + /** + * If the parameter `use_sim_time` is `true` then the source time is the simulation time, + * otherwise the source time is defined by the system. + * + * \param node_base_interface Node base interface. + * \param node_topics_interface Node topic base interface. + * \param node_graph_interface Node graph interface. + * \param node_services_interface Node service interface. + * \param node_logging_interface Node logging interface. + * \param node_clock_interface Node clock interface. + * \param node_parameters_interface Node parameters interface. + */ RCLCPP_PUBLIC void attachNode( - const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, - const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface, - const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface, - const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface); - + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface, + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface, + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_interface, + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_interface); + + /// Detach the node from the time source RCLCPP_PUBLIC void detachNode(); /// Attach a clock to the time source to be updated /** - * \throws std::invalid_argument if node is nullptr + * \param[in] clock to attach to the time source + * \throws std::invalid_argument the time source must be a RCL_ROS_TIME otherwise throws an exception */ RCLCPP_PUBLIC void attachClock(rclcpp::Clock::SharedPtr clock); + /// Detach a clock from the time source RCLCPP_PUBLIC void detachClock(rclcpp::Clock::SharedPtr clock); + /// Get whether a separate clock thread is used or not + RCLCPP_PUBLIC + bool get_use_clock_thread(); + + /// Set whether to use a separate clock thread or not + RCLCPP_PUBLIC + void set_use_clock_thread(bool use_clock_thread); + + /// Check if the clock thread is joinable + RCLCPP_PUBLIC + bool clock_thread_is_joinable(); + + /// TimeSource Destructor RCLCPP_PUBLIC ~TimeSource(); private: - // Preserve the node reference - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; - rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_; - rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_; - rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_; - - // The subscription for the clock callback - using MessageT = builtin_interfaces::msg::Time; - using Alloc = std::allocator; - using SubscriptionT = rclcpp::Subscription; - std::shared_ptr clock_subscription_; - - // The clock callback itself - void clock_cb(const builtin_interfaces::msg::Time::SharedPtr msg); - - // Parameter Client pointer - std::shared_ptr parameter_client_; - - // Parameter Event subscription - using ParamMessageT = rcl_interfaces::msg::ParameterEvent; - using ParamSubscriptionT = rclcpp::Subscription; - std::shared_ptr parameter_subscription_; - - // Callback for parameter updates - void on_parameter_event(const rcl_interfaces::msg::ParameterEvent::SharedPtr event); - - // An enum to hold the parameter state - enum UseSimTimeParameterState {UNSET, SET_TRUE, SET_FALSE}; - UseSimTimeParameterState parameter_state_; - - // An internal method to use in the clock callback that iterates and enables all clocks - void enable_ros_time(); - // An internal method to use in the clock callback that iterates and disables all clocks - void disable_ros_time(); - - // Internal helper functions used inside iterators - static void enable_ros_time(rclcpp::Clock::SharedPtr clock); - static void disable_ros_time(rclcpp::Clock::SharedPtr clock); - static void set_clock( - const builtin_interfaces::msg::Time::SharedPtr msg, - bool set_ros_time_enabled, - rclcpp::Clock::SharedPtr clock); - - // Local storage of validity of ROS time - // This is needed when new clocks are added. - bool ros_time_active_; - // Last set message to be passed to newly registered clocks - builtin_interfaces::msg::Time::SharedPtr last_msg_set_; - - // A lock to protect iterating the associated_clocks_ field. - std::mutex clock_list_lock_; - // A vector to store references to associated clocks. - std::vector associated_clocks_; + class NodeState; + std::shared_ptr node_state_; + + // Preserve the arguments received by the constructor for reuse at runtime + bool constructed_use_clock_thread_; + rclcpp::QoS constructed_qos_; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp index 94cd537a39..0ed62007d4 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/rclcpp/include/rclcpp/timer.hpp @@ -15,19 +15,25 @@ #ifndef RCLCPP__TIMER_HPP_ #define RCLCPP__TIMER_HPP_ +#include #include #include +#include #include #include #include #include #include +#include "rclcpp/clock.hpp" +#include "rclcpp/context.hpp" #include "rclcpp/function_traits.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/rate.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp/visibility_control.hpp" +#include "tracetools/tracetools.h" +#include "tracetools/utils.hpp" #include "rcl/error_handling.h" #include "rcl/timer.h" @@ -38,35 +44,95 @@ namespace rclcpp { +struct TimerInfo +{ + Time expected_call_time; + Time actual_call_time; +}; + class TimerBase { public: RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(TimerBase) + /// TimerBase constructor + /** + * \param clock A clock to use for time and sleeping + * \param period The interval at which the timer fires + * \param context node context + * \param autostart timer state on initialization + * + * In order to activate a timer that is not started on initialization, + * user should call the reset() method. + */ RCLCPP_PUBLIC - explicit TimerBase(std::chrono::nanoseconds period); + explicit TimerBase( + Clock::SharedPtr clock, + std::chrono::nanoseconds period, + rclcpp::Context::SharedPtr context, + bool autostart = true); + /// TimerBase destructor RCLCPP_PUBLIC + virtual ~TimerBase(); + /// Cancel the timer. + /** + * \throws std::runtime_error if the rcl_timer_cancel returns a failure + */ RCLCPP_PUBLIC void cancel(); + /// Return the timer cancellation state. + /** + * \return true if the timer has been cancelled, false otherwise + * \throws std::runtime_error if the rcl_get_error_state returns 0 + * \throws rclcpp::exceptions::RCLError some child class exception based on ret + */ + RCLCPP_PUBLIC + bool + is_canceled(); + + /// Reset the timer. + /** + * \throws std::runtime_error if the rcl_timer_reset returns a failure + */ RCLCPP_PUBLIC void reset(); + /// Indicate that we're about to execute the callback. + /** + * The multithreaded executor takes advantage of this to avoid scheduling + * the callback multiple times. + * + * \return a valid shared_ptr if the callback should be executed, + * an invalid shared_ptr (nullptr) if the timer was canceled. + */ + RCLCPP_PUBLIC + virtual std::shared_ptr + call() = 0; + + /// Call the callback function when the timer signal is emitted. + /** + * \param[in] data the pointer returned by the call function + */ RCLCPP_PUBLIC virtual void - execute_callback() = 0; + execute_callback(const std::shared_ptr & data) = 0; RCLCPP_PUBLIC - const rcl_timer_t * + std::shared_ptr get_timer_handle(); /// Check how long the timer has until its next scheduled callback. - /** \return A std::chrono::duration representing the relative time until the next callback. */ + /** + * \return A std::chrono::duration representing the relative time until the next callback + * or std::chrono::nanoseconds::max() if the timer is canceled. + * \throws std::runtime_error if the rcl_timer_get_time_until_next_call returns a failure + */ RCLCPP_PUBLIC std::chrono::nanoseconds time_until_trigger(); @@ -80,26 +146,80 @@ class TimerBase * This function expects its caller to immediately trigger the callback after this function, * since it maintains the last time the callback was triggered. * \return True if the timer needs to trigger. + * \throws std::runtime_error if it failed to check timer */ RCLCPP_PUBLIC bool is_ready(); + /// Exchange the "in use by wait set" state for this timer. + /** + * This is used to ensure this timer is not used by multiple + * wait sets at the same time. + * + * \param[in] in_use_state the new state to exchange into the state, true + * indicates it is now in use by a wait set, and false is that it is no + * longer in use by a wait set. + * \returns the previous state. + */ + RCLCPP_PUBLIC + bool + exchange_in_use_by_wait_set_state(bool in_use_state); + + /// Set a callback to be called when the timer is reset + /** + * You should aim to make this callback fast and not blocking. + * If you need to do a lot of work or wait for some other event, you should + * spin it off to another thread. + * + * Calling it again will override any previously set callback. + * An exception will be thrown if the callback is not callable. + * + * This function is thread-safe. + * + * If you want more information available in the callback, + * you may use a lambda with captures or std::bind. + * + * \param[in] callback functor to be called whenever timer is reset + */ + RCLCPP_PUBLIC + void + set_on_reset_callback(std::function callback); + + /// Unset the callback registered for reset timer + RCLCPP_PUBLIC + void + clear_on_reset_callback(); + protected: - rcl_timer_t timer_handle_ = rcl_get_zero_initialized_timer(); -}; + std::recursive_mutex callback_mutex_; + // Declare callback before timer_handle_, so on destruction + // the callback is destroyed last. Otherwise, the rcl timer + // callback would point briefly to a destroyed function. + // Clearing the callback on timer destructor also makes sure + // the rcl callback is cleared before on_reset_callback_. + std::function on_reset_callback_{nullptr}; + Clock::SharedPtr clock_; + std::shared_ptr timer_handle_; -using VoidCallbackType = std::function; -using TimerCallbackType = std::function; + std::atomic in_use_by_wait_set_{false}; + + RCLCPP_PUBLIC + void + set_on_reset_callback(rcl_event_callback_t callback, const void * user_data); +}; -/// Generic timer templated on the clock type. Periodically executes a user-specified callback. +using VoidCallbackType = std::function; +using TimerCallbackType = std::function; +using TimerInfoCallbackType = std::function; + +/// Generic timer. Periodically executes a user-specified callback. template< typename FunctorT, - class Clock, typename std::enable_if< - (rclcpp::function_traits::same_arguments::value || - rclcpp::function_traits::same_arguments::value) && - Clock::is_steady + rclcpp::function_traits::same_arguments::value || + rclcpp::function_traits::same_arguments::value || + rclcpp::function_traits::same_arguments::value >::type * = nullptr > class GenericTimer : public TimerBase @@ -109,12 +229,32 @@ class GenericTimer : public TimerBase /// Default constructor. /** + * \param[in] clock The clock providing the current time. * \param[in] period The interval at which the timer fires. * \param[in] callback User-specified callback function. + * \param[in] context custom context to be used. + * \param autostart timer state on initialization */ - GenericTimer(std::chrono::nanoseconds period, FunctorT && callback) - : TimerBase(period), callback_(std::forward(callback)) + explicit GenericTimer( + Clock::SharedPtr clock, std::chrono::nanoseconds period, FunctorT && callback, + rclcpp::Context::SharedPtr context, bool autostart = true + ) + : TimerBase(clock, period, context, autostart), callback_(std::forward(callback)) { + TRACETOOLS_TRACEPOINT( + rclcpp_timer_callback_added, + static_cast(get_timer_handle().get()), + reinterpret_cast(&callback_)); +#ifndef TRACETOOLS_DISABLED + if (TRACETOOLS_TRACEPOINT_ENABLED(rclcpp_callback_register)) { + char * symbol = tracetools::get_symbol(callback_); + TRACETOOLS_DO_TRACEPOINT( + rclcpp_callback_register, + reinterpret_cast(&callback_), + symbol); + std::free(symbol); + } +#endif } /// Default destructor. @@ -122,22 +262,35 @@ class GenericTimer : public TimerBase { // Stop the timer from running. cancel(); - if (rcl_timer_fini(&timer_handle_) != RCL_RET_OK) { - fprintf(stderr, "Failed to clean up rcl timer handle: %s\n", rcl_get_error_string_safe()); - } } - void - execute_callback() + /** + * \sa rclcpp::TimerBase::call + * \throws std::runtime_error if it failed to notify timer that callback will occurr + */ + std::shared_ptr + call() override { - rcl_ret_t ret = rcl_timer_call(&timer_handle_); + auto timer_call_info_ = std::make_shared(); + rcl_ret_t ret = rcl_timer_call_with_info(timer_handle_.get(), timer_call_info_.get()); if (ret == RCL_RET_TIMER_CANCELED) { - return; + return nullptr; } if (ret != RCL_RET_OK) { throw std::runtime_error("Failed to notify timer that callback occurred"); } - execute_callback_delegate<>(); + return timer_call_info_; + } + + /** + * \sa rclcpp::TimerBase::execute_callback + */ + void + execute_callback(const std::shared_ptr & data) override + { + TRACETOOLS_TRACEPOINT(callback_start, reinterpret_cast(&callback_), false); + execute_callback_delegate<>(*static_cast(data.get())); + TRACETOOLS_TRACEPOINT(callback_end, reinterpret_cast(&callback_)); } // void specialization @@ -148,7 +301,7 @@ class GenericTimer : public TimerBase >::type * = nullptr > void - execute_callback_delegate() + execute_callback_delegate(const rcl_timer_call_info_t &) { callback_(); } @@ -160,15 +313,32 @@ class GenericTimer : public TimerBase >::type * = nullptr > void - execute_callback_delegate() + execute_callback_delegate(const rcl_timer_call_info_t &) { callback_(*this); } - virtual bool - is_steady() + + template< + typename CallbackT = FunctorT, + typename std::enable_if< + rclcpp::function_traits::same_arguments::value + >::type * = nullptr + > + void + execute_callback_delegate(const rcl_timer_call_info_t & timer_call_info) { - return Clock::is_steady; + const TimerInfo info{Time{timer_call_info.expected_call_time, clock_->get_clock_type()}, + Time{timer_call_info.actual_call_time, clock_->get_clock_type()}}; + callback_(info); + } + + /// Is the clock steady (i.e. is the time between ticks constant?) + /** \return True if the clock used by this timer is steady. */ + bool + is_steady() override + { + return clock_->get_clock_type() == RCL_STEADY_TIME; } protected: @@ -177,8 +347,38 @@ class GenericTimer : public TimerBase FunctorT callback_; }; -template -using WallTimer = GenericTimer; +template< + typename FunctorT, + typename std::enable_if< + rclcpp::function_traits::same_arguments::value || + rclcpp::function_traits::same_arguments::value || + rclcpp::function_traits::same_arguments::value + >::type * = nullptr +> +class WallTimer : public GenericTimer +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(WallTimer) + + /// Wall timer constructor + /** + * \param period The interval at which the timer fires + * \param callback The callback function to execute every interval + * \param context node context + * \param autostart timer state on initialization + */ + WallTimer( + std::chrono::nanoseconds period, + FunctorT && callback, + rclcpp::Context::SharedPtr context, + bool autostart = true) + : GenericTimer( + std::make_shared(RCL_STEADY_TIME), period, std::move(callback), context, autostart) + {} + +protected: + RCLCPP_DISABLE_COPY(WallTimer) +}; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp b/rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp new file mode 100644 index 0000000000..7a6db13502 --- /dev/null +++ b/rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp @@ -0,0 +1,235 @@ +// Copyright 2020 Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__TOPIC_STATISTICS__SUBSCRIPTION_TOPIC_STATISTICS_HPP_ +#define RCLCPP__TOPIC_STATISTICS__SUBSCRIPTION_TOPIC_STATISTICS_HPP_ + +#include +#include +#include +#include + +#include "libstatistics_collector/collector/generate_statistics_message.hpp" +#include "libstatistics_collector/moving_average_statistics/types.hpp" +#include "libstatistics_collector/topic_statistics_collector/constants.hpp" +#include "libstatistics_collector/topic_statistics_collector/received_message_age.hpp" +#include "libstatistics_collector/topic_statistics_collector/received_message_period.hpp" + +#include "rcl/time.h" +#include "rclcpp/time.hpp" +#include "rclcpp/publisher.hpp" +#include "rclcpp/timer.hpp" + +#include "statistics_msgs/msg/metrics_message.hpp" + +namespace rclcpp +{ +namespace topic_statistics +{ + +constexpr const char kDefaultPublishTopicName[]{"/statistics"}; +constexpr const std::chrono::milliseconds kDefaultPublishingPeriod{std::chrono::seconds(1)}; + +using libstatistics_collector::collector::GenerateStatisticMessage; +using statistics_msgs::msg::MetricsMessage; +using libstatistics_collector::moving_average_statistics::StatisticData; + +/** + * Class used to collect, measure, and publish topic statistics data. Current statistics + * supported for subscribers are received message age and received message period. + */ +class SubscriptionTopicStatistics +{ + using TopicStatsCollector = libstatistics_collector::TopicStatisticsCollector; + using ReceivedMessageAge = libstatistics_collector::ReceivedMessageAgeCollector; + using ReceivedMessagePeriod = libstatistics_collector::ReceivedMessagePeriodCollector; + +public: + /// Construct a SubscriptionTopicStatistics object. + /** + * This object wraps utilities, defined in libstatistics_collector, to collect, + * measure, and publish topic statistics data. This throws an invalid_argument + * if the input publisher is null. + * + * \param node_name the name of the node, which created this instance, in order to denote + * topic source + * \param publisher instance constructed by the node in order to publish statistics data. + * This class owns the publisher. + * \throws std::invalid_argument if publisher pointer is nullptr + */ + SubscriptionTopicStatistics( + const std::string & node_name, + rclcpp::Publisher::SharedPtr publisher) + : node_name_(node_name), + publisher_(std::move(publisher)) + { + // TODO(dbbonnie): ros-tooling/aws-roadmap/issues/226, received message age + + if (nullptr == publisher_) { + throw std::invalid_argument("publisher pointer is nullptr"); + } + + bring_up(); + } + + virtual ~SubscriptionTopicStatistics() + { + tear_down(); + } + + /// Handle a message received by the subscription to collect statistics. + /** + * This method acquires a lock to prevent race conditions to collectors list. + * + * \param message_info the message info corresponding to the received message + * \param now_nanoseconds current time in nanoseconds + */ + virtual void handle_message( + const rmw_message_info_t & message_info, + const rclcpp::Time now_nanoseconds) const + { + std::lock_guard lock(mutex_); + for (const auto & collector : subscriber_statistics_collectors_) { + collector->OnMessageReceived(message_info, now_nanoseconds.nanoseconds()); + } + } + + /// Set the timer used to publish statistics messages. + /** + * \param publisher_timer the timer to fire the publisher, created by the node + */ + void set_publisher_timer(rclcpp::TimerBase::SharedPtr publisher_timer) + { + publisher_timer_ = publisher_timer; + } + + /// Publish a populated MetricsStatisticsMessage. + /** + * This method acquires a lock to prevent race conditions to collectors list. + */ + virtual void publish_message_and_reset_measurements() + { + std::vector msgs; + rclcpp::Time window_end{get_current_nanoseconds_since_epoch()}; + + { + std::lock_guard lock(mutex_); + for (auto & collector : subscriber_statistics_collectors_) { + const auto collected_stats = collector->GetStatisticsResults(); + collector->ClearCurrentMeasurements(); + + auto message = libstatistics_collector::collector::GenerateStatisticMessage( + node_name_, + collector->GetMetricName(), + collector->GetMetricUnit(), + window_start_, + window_end, + collected_stats); + msgs.push_back(message); + } + } + + for (auto & msg : msgs) { + publisher_->publish(msg); + } + window_start_ = window_end; + } + +protected: + /// Return a vector of all the currently collected data. + /** + * This method acquires a lock to prevent race conditions to collectors list. + * + * \return a vector of all the collected data + */ + std::vector get_current_collector_data() const + { + std::vector data; + std::lock_guard lock(mutex_); + for (const auto & collector : subscriber_statistics_collectors_) { + data.push_back(collector->GetStatisticsResults()); + } + return data; + } + +private: + /// Construct and start all collectors and set window_start_. + /** + * This method acquires a lock to prevent race conditions to collectors list. + */ + void bring_up() + { + auto received_message_age = std::make_unique(); + received_message_age->Start(); + auto received_message_period = std::make_unique(); + received_message_period->Start(); + { + std::lock_guard lock(mutex_); + subscriber_statistics_collectors_.emplace_back(std::move(received_message_age)); + subscriber_statistics_collectors_.emplace_back(std::move(received_message_period)); + } + + window_start_ = rclcpp::Time(get_current_nanoseconds_since_epoch()); + } + + /// Stop all collectors, clear measurements, stop publishing timer, and reset publisher. + /** + * This method acquires a lock to prevent race conditions to collectors list. + */ + void tear_down() + { + { + std::lock_guard lock(mutex_); + for (auto & collector : subscriber_statistics_collectors_) { + collector->Stop(); + } + + subscriber_statistics_collectors_.clear(); + } + + if (publisher_timer_) { + publisher_timer_->cancel(); + publisher_timer_.reset(); + } + + publisher_.reset(); + } + + /// Return the current nanoseconds (count) since epoch. + /** + * \return the current nanoseconds (count) since epoch + */ + int64_t get_current_nanoseconds_since_epoch() const + { + const auto now = std::chrono::system_clock::now(); + return std::chrono::duration_cast(now.time_since_epoch()).count(); + } + + /// Mutex to protect the subsequence vectors + mutable std::mutex mutex_; + /// Collection of statistics collectors + std::vector> subscriber_statistics_collectors_{}; + /// Node name used to generate topic statistics messages to be published + const std::string node_name_; + /// Publisher, created by the node, used to publish topic statistics messages + rclcpp::Publisher::SharedPtr publisher_; + /// Timer which fires the publisher + rclcpp::TimerBase::SharedPtr publisher_timer_; + /// The start of the collection window, used in the published topic statistics message + rclcpp::Time window_start_; +}; +} // namespace topic_statistics +} // namespace rclcpp + +#endif // RCLCPP__TOPIC_STATISTICS__SUBSCRIPTION_TOPIC_STATISTICS_HPP_ diff --git a/rclcpp/include/rclcpp/topic_statistics_state.hpp b/rclcpp/include/rclcpp/topic_statistics_state.hpp new file mode 100644 index 0000000000..b407622a4e --- /dev/null +++ b/rclcpp/include/rclcpp/topic_statistics_state.hpp @@ -0,0 +1,35 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__TOPIC_STATISTICS_STATE_HPP_ +#define RCLCPP__TOPIC_STATISTICS_STATE_HPP_ + +namespace rclcpp +{ + +/// Represent the state of topic statistics collector. +/// Used as argument in create_subscriber. +enum class TopicStatisticsState +{ + /// Explicitly enable topic statistics at subscription level. + Enable, + /// Explicitly disable topic statistics at subscription level. + Disable, + /// Take topic statistics state from the node. + NodeDefault +}; + +} // namespace rclcpp + +#endif // RCLCPP__TOPIC_STATISTICS_STATE_HPP_ diff --git a/rclcpp/include/rclcpp/type_adapter.hpp b/rclcpp/include/rclcpp/type_adapter.hpp new file mode 100644 index 0000000000..d6834ccc22 --- /dev/null +++ b/rclcpp/include/rclcpp/type_adapter.hpp @@ -0,0 +1,201 @@ +// 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 RCLCPP__TYPE_ADAPTER_HPP_ +#define RCLCPP__TYPE_ADAPTER_HPP_ + +#include + +namespace rclcpp +{ + +/// Template structure used to adapt custom, user-defined types to ROS types. +/** + * Adapting a custom, user-defined type to a ROS type allows that custom type + * to be used when publishing and subscribing in ROS. + * + * In order to adapt a custom type to a ROS type, the user must create a + * template specialization of this structure for the custom type. + * In that specialization they must: + * + * - change `is_specialized` to `std::true_type`, + * - specify the custom type with `using custom_type = ...`, + * - specify the ROS type with `using ros_message_type = ...`, + * - provide static convert functions with the signatures: + * - static void convert_to_ros(const custom_type &, ros_message_type &) + * - static void convert_to_custom(const ros_message_type &, custom_type &) + * + * The convert functions must convert from one type to the other. + * + * For example, here is a theoretical example for adapting `std::string` to the + * `std_msgs::msg::String` ROS message type: + * + * template<> + * struct rclcpp::TypeAdapter + * { + * using is_specialized = std::true_type; + * using custom_type = std::string; + * using ros_message_type = std_msgs::msg::String; + * + * static + * void + * convert_to_ros_message( + * const custom_type & source, + * ros_message_type & destination) + * { + * destination.data = source; + * } + * + * static + * void + * convert_to_custom( + * const ros_message_type & source, + * custom_type & destination) + * { + * destination = source.data; + * } + * }; + * + * The adapter can then be used when creating a publisher or subscription, + * e.g.: + * + * using MyAdaptedType = TypeAdapter; + * auto pub = node->create_publisher("topic", 10); + * auto sub = node->create_subscription( + * "topic", + * 10, + * [](const std::string & msg) {...}); + * + * You can also be more declarative by using the adapt_type::as metafunctions, + * which are a bit less ambiguous to read: + * + * using AdaptedType = rclcpp::adapt_type::as; + * auto pub = node->create_publisher(...); + * + * If you wish, you may associate a custom type with a single ROS message type, + * allowing you to be a bit more brief when creating entities, e.g.: + * + * // First you must declare the association, this is similar to how you + * // would avoid using the namespace in C++ by doing `using std::vector;`. + * RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE(std::string, std_msgs::msg::String); + * + * // Then you can create things with just the custom type, and the ROS + * // message type is implied based on the previous statement. + * auto pub = node->create_publisher(...); + */ +template +struct TypeAdapter +{ + using is_specialized = std::false_type; + using custom_type = CustomType; + // In this case, the CustomType is the only thing given, or there is no specialization. + // Assign ros_message_type to CustomType for the former case. + using ros_message_type = CustomType; +}; + +/// Helper template to determine if a type is a TypeAdapter, false specialization. +template +struct is_type_adapter : std::false_type {}; + +/// Helper template to determine if a type is a TypeAdapter, true specialization. +template +struct is_type_adapter>: std::true_type {}; + +/// Identity specialization for TypeAdapter. +template +struct TypeAdapter::value>>: T {}; + +namespace detail +{ + +template +struct assert_type_pair_is_specialized_type_adapter +{ + using type_adapter = TypeAdapter; + static_assert( + type_adapter::is_specialized::value, + "No type adapter for this custom type/ros message type pair"); +}; + +} // namespace detail + +/// Template metafunction that can make the type being adapted explicit. +template +struct adapt_type +{ + template + using as = typename ::rclcpp::detail::assert_type_pair_is_specialized_type_adapter< + CustomType, + ROSMessageType + >::type_adapter; +}; + +/// Implicit type adapter used as a short hand way to create something with just the custom type. +/** + * This is used when creating a publisher or subscription using just the custom + * type in conjunction with RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE(). + * For example: + * + * #include "type_adapter_for_std_string_to_std_msgs_String.hpp" + * + * RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE(std::string, std_msgs::msg::String); + * + * int main(...) { + * // ... + * auto pub = node->create_publisher(...); + * } + * + * \sa TypeAdapter for more examples. + */ +template +struct ImplicitTypeAdapter +{ + using is_specialized = std::false_type; +}; + +/// Specialization of TypeAdapter for ImplicitTypeAdapter. +/** + * This allows for things like this: + * + * RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE(std::string, std_msgs::msg::String); + * auto pub = node->create_publisher("topic", 10); + * + */ +template +struct TypeAdapter::is_specialized::value>> + : ImplicitTypeAdapter +{}; + +/// Assigns the custom type implicitly to the given custom type/ros message type pair. +/** + * Note: this macro needs to be used in the root namespace. + * We cannot use ::rclcpp to protect against this, due to how GCC interprets the + * syntax, see: https://stackoverflow.com/a/2781537 + * + * \sa TypeAdapter + * \sa ImplicitTypeAdapter + */ +#define RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE(CustomType, ROSMessageType) \ + template<> \ + struct rclcpp::ImplicitTypeAdapter \ + : public rclcpp::TypeAdapter \ + { \ + static_assert( \ + is_specialized::value, \ + "Cannot use custom type as ros type when there is no TypeAdapter for that pair"); \ + } + +} // namespace rclcpp + +#endif // RCLCPP__TYPE_ADAPTER_HPP_ diff --git a/rclcpp/include/rclcpp/type_support_decl.hpp b/rclcpp/include/rclcpp/type_support_decl.hpp index cda0bae730..cf08ebf5b1 100644 --- a/rclcpp/include/rclcpp/type_support_decl.hpp +++ b/rclcpp/include/rclcpp/type_support_decl.hpp @@ -15,8 +15,8 @@ #ifndef RCLCPP__TYPE_SUPPORT_DECL_HPP_ #define RCLCPP__TYPE_SUPPORT_DECL_HPP_ -#include "rosidl_generator_cpp/message_type_support_decl.hpp" -#include "rosidl_generator_cpp/service_type_support_decl.hpp" +#include "rosidl_runtime_cpp/message_type_support_decl.hpp" +#include "rosidl_runtime_cpp/service_type_support_decl.hpp" #include "rosidl_typesupport_cpp/message_type_support.hpp" #include "rosidl_typesupport_cpp/service_type_support.hpp" diff --git a/rclcpp/include/rclcpp/typesupport_helpers.hpp b/rclcpp/include/rclcpp/typesupport_helpers.hpp new file mode 100644 index 0000000000..7f75a8052b --- /dev/null +++ b/rclcpp/include/rclcpp/typesupport_helpers.hpp @@ -0,0 +1,77 @@ +// Copyright 2018, Bosch Software Innovations GmbH. +// Copyright 2021, Apex.AI 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 RCLCPP__TYPESUPPORT_HELPERS_HPP_ +#define RCLCPP__TYPESUPPORT_HELPERS_HPP_ + +#include +#include +#include + +#include "rcpputils/shared_library.hpp" +#include "rosidl_runtime_cpp/message_type_support_decl.hpp" +#include "rosidl_runtime_cpp/service_type_support_decl.hpp" + +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +/// Load the type support library for the given type. +/** + * \param[in] type The topic type, e.g. "std_msgs/msg/String" + * \param[in] typesupport_identifier Type support identifier, typically "rosidl_typesupport_cpp" + * \return A shared library + */ +RCLCPP_PUBLIC +std::shared_ptr +get_typesupport_library(const std::string & type, const std::string & typesupport_identifier); + +/// Extract the message type support handle from the library. +/** + * The library needs to match the topic type. The shared library must stay loaded for the lifetime of the result. + * + * \param[in] type The topic type, e.g. "std_msgs/msg/String" + * \param[in] typesupport_identifier Type support identifier, typically "rosidl_typesupport_cpp" + * \param[in] library The shared type support library + * \throws std::runtime_error if the symbol of type not found in the library. + * \return A message type support handle + */ +RCLCPP_PUBLIC +const rosidl_message_type_support_t * +get_message_typesupport_handle( + const std::string & type, + const std::string & typesupport_identifier, + rcpputils::SharedLibrary & library); + +/// Extract the service type support handle from the library. +/** + * The library needs to match the topic type. The shared library must stay loaded for the lifetime of the result. + * + * \param[in] type The service type, e.g. "std_srvs/srv/Empty" + * \param[in] typesupport_identifier Type support identifier, typically "rosidl_typesupport_cpp" + * \param[in] library The shared type support library + * \throws std::runtime_error if the symbol of type not found in the library. + * \return A service type support handle + */ +RCLCPP_PUBLIC +const rosidl_service_type_support_t * +get_service_typesupport_handle( + const std::string & type, + const std::string & typesupport_identifier, + rcpputils::SharedLibrary & library); + +} // namespace rclcpp + +#endif // RCLCPP__TYPESUPPORT_HELPERS_HPP_ diff --git a/rclcpp/include/rclcpp/utilities.hpp b/rclcpp/include/rclcpp/utilities.hpp index 987d1347ec..5b4ea86a6d 100644 --- a/rclcpp/include/rclcpp/utilities.hpp +++ b/rclcpp/include/rclcpp/utilities.hpp @@ -18,17 +18,14 @@ #include #include #include +#include +#include +#include "rclcpp/context.hpp" +#include "rclcpp/init_options.hpp" #include "rclcpp/visibility_control.hpp" -#include "rcl/guard_condition.h" -#include "rcl/wait.h" - -#include "rmw/macros.h" -#include "rmw/rmw.h" - #ifdef ANDROID -#include #include namespace std @@ -46,69 +43,195 @@ std::string to_string(T value) namespace rclcpp { +/// Option to indicate which signal handlers rclcpp should install. +enum class SignalHandlerOptions +{ + /// Install both sigint and sigterm, this is the default behavior. + All, + /// Install only a sigint handler. + SigInt, + /// Install only a sigterm handler. + SigTerm, + /// Do not install any signal handler. + None, +}; + /// Initialize communications via the rmw implementation and set up a global signal handler. /** - * \param[in] argc Number of arguments. - * \param[in] argv Argument vector. Will eventually be used for passing options to rclcpp. + * Initializes the global context which is accessible via the function + * rclcpp::contexts::get_global_default_context(). + * Also, installs the global signal handlers with the function + * rclcpp::install_signal_handlers(). + * + * \sa rclcpp::Context::init() for more details on arguments and possible exceptions + * + * \param[in] argc number of command-line arguments to parse. + * \param[in] argv array of command-line arguments to parse. + * \param[in] init_options initialization options to apply. + * \param[in] signal_handler_options option to indicate which signal handlers should be installed. */ RCLCPP_PUBLIC void -init(int argc, char * argv[]); +init( + int argc, + char const * const * argv, + const InitOptions & init_options = InitOptions(), + SignalHandlerOptions signal_handler_options = SignalHandlerOptions::All); -/// Check rclcpp's status. -/** \return True if SIGINT hasn't fired yet, false otherwise. */ +/// Install the global signal handler for rclcpp. +/** + * This function should only need to be run one time per process. + * It is implicitly run by rclcpp::init(), and therefore this function does not + * need to be run manually if rclcpp::init() has already been run. + * + * The signal handler will shutdown all initialized context. + * It will also interrupt any blocking functions in ROS allowing them react to + * any changes in the state of the system (like shutdown). + * + * This function is thread-safe. + * + * \param[in] signal_handler_options option to indicate which signal handlers should be installed. + * \return true if signal handler was installed by this function, false if already installed. + */ RCLCPP_PUBLIC bool -ok(); +install_signal_handlers(SignalHandlerOptions signal_handler_options = SignalHandlerOptions::All); -/// Notify the signal handler and rmw that rclcpp is shutting down. +/// Return true if the signal handlers are installed, otherwise false. RCLCPP_PUBLIC -void -shutdown(); +bool +signal_handlers_installed(); -/// Register a function to be called when shutdown is called. -/** Calling the callbacks is the last thing shutdown() does. */ +/// Get the current signal handler options. +/** + * If no signal handler is installed, SignalHandlerOptions::None is returned. + */ RCLCPP_PUBLIC -void -on_shutdown(std::function callback); +SignalHandlerOptions +get_current_signal_handler_options(); + +/// Uninstall the global signal handler for rclcpp. +/** + * This function does not necessarily need to be called, but can be used to + * undo what rclcpp::install_signal_handlers() or rclcpp::init() do with + * respect to signal handling. + * If you choose to use it, this function only needs to be run one time. + * It is implicitly run by rclcpp::shutdown(), and therefore this function does + * not need to be run manually if rclcpp::shutdown() has already been run. + * + * This function is thread-safe. + * + * \return true if signal handler was uninstalled by this function, false if was not installed. + */ +RCLCPP_PUBLIC +bool +uninstall_signal_handlers(); + +/// Initialize communications via the rmw implementation and set up a global signal handler. +/** + * Additionally removes ROS-specific arguments from the argument vector. + * + * \sa rclcpp::Context::init() for more details on arguments and possible exceptions + * \returns Members of the argument vector that are not ROS arguments. + * \throws anything remove_ros_arguments can throw + */ +RCLCPP_PUBLIC +std::vector +init_and_remove_ros_arguments( + int argc, + char const * const * argv, + const InitOptions & init_options = InitOptions()); + +/// Remove ROS-specific arguments from argument vector. +/** + * Some arguments may not have been intended as ROS arguments. + * This function populates the arguments in a vector. + * Since the first argument is always assumed to be a process name, the vector + * will always contain the process name. + * + * \param[in] argc Number of arguments. + * \param[in] argv Argument vector. + * \returns Members of the argument vector that are not ROS arguments. + * \throws anything throw_from_rcl_error can throw + * \throws rclcpp::exceptions::RCLError if the parsing fails + */ +RCLCPP_PUBLIC +std::vector +remove_ros_arguments(int argc, char const * const * argv); -/// Get a handle to the rmw guard condition that manages the signal handler. +/// Check rclcpp's status. /** - * The first time that this function is called for a given wait set a new guard - * condition will be created and returned; thereafter the same guard condition - * will be returned for the same wait set. This mechanism is designed to ensure - * that the same guard condition is not reused across wait sets (e.g., when - * using multiple executors in the same process). Will throw an exception if - * initialization of the guard condition fails. - * \param wait_set Pointer to the rcl_wait_set_t that will be using the - * resulting guard condition. - * \return Pointer to the guard condition. + * This may return false for a context which has been shutdown, or for a + * context that was shutdown due to SIGINT being received by the rclcpp signal + * handler. + * + * If nullptr is given for the context, then the global context is used, i.e. + * the context initialized by rclcpp::init(). + * + * \param[in] context Optional check for shutdown of this Context. + * \return false if shutdown has been called, true otherwise */ RCLCPP_PUBLIC -rcl_guard_condition_t * -get_sigint_guard_condition(rcl_wait_set_t * wait_set); +bool +ok(rclcpp::Context::SharedPtr context = nullptr); + +/// Shutdown rclcpp context, invalidating it for derived entities. +/** + * If nullptr is given for the context, then the global context is used, i.e. + * the context initialized by rclcpp::init(). + * + * If the global context is used, then the signal handlers are also uninstalled. + * + * This will also cause the "on_shutdown" callbacks to be called. + * + * \sa rclcpp::Context::shutdown() + * \param[in] context Optional to be shutdown + * \param[in] reason Optional string passed to the context shutdown method + * \return true if shutdown was successful, false if context was already shutdown + */ +RCLCPP_PUBLIC +bool +shutdown( + rclcpp::Context::SharedPtr context = nullptr, + const std::string & reason = "user called rclcpp::shutdown()"); -/// Release the previously allocated guard condition that manages the signal handler. +/// Register a function to be called when shutdown is called on the context. /** - * If you previously called get_sigint_guard_condition() for a given wait set - * to get a sigint guard condition, then you should call release_sigint_guard_condition() - * when you're done, to free that condition. Will throw an exception if - * get_sigint_guard_condition() wasn't previously called for the given wait set. - * \param wait_set Pointer to the rcl_wait_set_t that was using the - * resulting guard condition. + * If nullptr is given for the context, then the global context is used, i.e. + * the context initialized by rclcpp::init(). + * + * These callbacks are called when the associated Context is shutdown with the + * Context::shutdown() method. + * When shutdown by the SIGINT handler, shutdown, and therefore these callbacks, + * is called asynchronously from the dedicated signal handling thread, at some + * point after the SIGINT signal is received. + * + * \sa rclcpp::Context::on_shutdown() + * \param[in] callback to be called when the given context is shutdown + * \param[in] context with which to associate the context */ RCLCPP_PUBLIC void -release_sigint_guard_condition(rcl_wait_set_t * wait_set); +on_shutdown(std::function callback, rclcpp::Context::SharedPtr context = nullptr); /// Use the global condition variable to block for the specified amount of time. /** + * This function can be interrupted early if the associated context becomes + * invalid due to shutdown() or the signal handler. + * \sa rclcpp::Context::sleep_for + * + * If nullptr is given for the context, then the global context is used, i.e. + * the context initialized by rclcpp::init(). + * * \param[in] nanoseconds A std::chrono::duration representing how long to sleep for. - * \return True if the condition variable did not timeout. + * \param[in] context Optional which may interrupt this sleep + * \return true if the condition variable did not timeout. */ RCLCPP_PUBLIC bool -sleep_for(const std::chrono::nanoseconds & nanoseconds); +sleep_for( + const std::chrono::nanoseconds & nanoseconds, + rclcpp::Context::SharedPtr context = nullptr); /// Safely check if addition will overflow. /** @@ -178,6 +301,35 @@ sub_will_underflow(const T x, const T y) return (y > 0) && (x < (std::numeric_limits::min() + y)); } +/// Return the given string. +/** + * This function is overloaded to transform any string to C-style string. + * + * \param[in] string_in is the string to be returned + * \return the given string + */ +RCLCPP_PUBLIC +const char * +get_c_string(const char * string_in); + +/// Return the C string from the given std::string. +/** + * \param[in] string_in is a std::string + * \return the C string from the std::string + */ +RCLCPP_PUBLIC +const char * +get_c_string(const std::string & string_in); + +/// Return the std::vector of C string from the given std::vector. +/** + * \param[in] strings_in is a std::vector of std::string + * \return the std::vector of C string from the std::vector + */ +RCLCPP_PUBLIC +std::vector +get_c_vector_string(const std::vector & strings_in); + } // namespace rclcpp #endif // RCLCPP__UTILITIES_HPP_ diff --git a/rclcpp/include/rclcpp/wait_for_message.hpp b/rclcpp/include/rclcpp/wait_for_message.hpp new file mode 100644 index 0000000000..25c45ad782 --- /dev/null +++ b/rclcpp/include/rclcpp/wait_for_message.hpp @@ -0,0 +1,103 @@ +// 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 RCLCPP__WAIT_FOR_MESSAGE_HPP_ +#define RCLCPP__WAIT_FOR_MESSAGE_HPP_ + +#include +#include + +#include "rcpputils/scope_exit.hpp" + +#include "rclcpp/node.hpp" +#include "rclcpp/visibility_control.hpp" +#include "rclcpp/wait_set.hpp" + +namespace rclcpp +{ +/// Wait for the next incoming message. +/** + * Given an already initialized subscription, + * wait for the next incoming message to arrive before the specified timeout. + * + * \param[out] out is the message to be filled when a new message is arriving. + * \param[in] subscription shared pointer to a previously initialized subscription. + * \param[in] context shared pointer to a context to watch for SIGINT requests. + * \param[in] time_to_wait parameter specifying the timeout before returning. + * \return true if a message was successfully received, false if message could not + * be obtained or shutdown was triggered asynchronously on the context. + */ +template +bool wait_for_message( + MsgT & out, + std::shared_ptr> subscription, + std::shared_ptr context, + std::chrono::duration time_to_wait = std::chrono::duration(-1)) +{ + auto gc = std::make_shared(context); + auto shutdown_callback_handle = context->add_on_shutdown_callback( + [weak_gc = std::weak_ptr{gc}]() { + auto strong_gc = weak_gc.lock(); + if (strong_gc) { + strong_gc->trigger(); + } + }); + + rclcpp::WaitSet wait_set; + wait_set.add_subscription(subscription); + RCPPUTILS_SCOPE_EXIT(wait_set.remove_subscription(subscription); ); + wait_set.add_guard_condition(gc); + auto ret = wait_set.wait(time_to_wait); + if (ret.kind() != rclcpp::WaitResultKind::Ready) { + return false; + } + + if (wait_set.get_rcl_wait_set().guard_conditions[0]) { + return false; + } + + rclcpp::MessageInfo info; + if (!subscription->take(out, info)) { + return false; + } + + return true; +} + +/// Wait for the next incoming message. +/** + * Wait for the next incoming message to arrive on a specified topic before the specified timeout. + * + * \param[out] out is the message to be filled when a new message is arriving. + * \param[in] node the node pointer to initialize the subscription on. + * \param[in] topic the topic to wait for messages. + * \param[in] time_to_wait parameter specifying the timeout before returning. + * \return true if a message was successfully received, false if message could not + * be obtained or shutdown was triggered asynchronously on the context. + */ +template +bool wait_for_message( + MsgT & out, + rclcpp::Node::SharedPtr node, + const std::string & topic, + std::chrono::duration time_to_wait = std::chrono::duration(-1)) +{ + auto sub = node->create_subscription(topic, 1, [](const std::shared_ptr) {}); + return wait_for_message( + out, sub, node->get_node_options().context(), time_to_wait); +} + +} // namespace rclcpp + +#endif // RCLCPP__WAIT_FOR_MESSAGE_HPP_ diff --git a/rclcpp/include/rclcpp/wait_result.hpp b/rclcpp/include/rclcpp/wait_result.hpp new file mode 100644 index 0000000000..3384a7846a --- /dev/null +++ b/rclcpp/include/rclcpp/wait_result.hpp @@ -0,0 +1,330 @@ +// 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 RCLCPP__WAIT_RESULT_HPP_ +#define RCLCPP__WAIT_RESULT_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "rcl/wait.h" + +#include "rclcpp/macros.hpp" +#include "rclcpp/wait_result_kind.hpp" + +#include "rclcpp/client.hpp" +#include "rclcpp/service.hpp" +#include "rclcpp/subscription_base.hpp" +#include "rclcpp/timer.hpp" + +namespace rclcpp +{ + +// TODO(wjwwood): the union-like design of this class could be replaced with +// std::variant, when we have access to that... +/// Interface for introspecting a wait set after waiting on it. +/** + * This class: + * + * - provides the result of waiting, i.e. ready, timeout, or empty, and + * - holds the ownership of the entities of the wait set, if needed, and + * - provides the necessary information for iterating over the wait set. + * + * This class is only valid as long as the wait set which created it is valid, + * and it must be deleted before the wait set is deleted, as it contains a + * back reference to the wait set. + * + * An instance of this, which is returned from rclcpp::WaitSetTemplate::wait(), + * will cause the wait set to keep ownership of the entities because it only + * holds a reference to the sequences of them, rather than taking a copy. + * Also, in the thread-safe case, an instance of this will cause the wait set, + * to block calls which modify the sequences of the entities, e.g. add/remove + * guard condition or subscription methods. + * + * \tparam WaitSetT The wait set type which created this class. + */ +template +class WaitResult final +{ +public: + /// Create WaitResult from a "ready" result. + /** + * \param[in] wait_set A reference to the wait set, which this class + * will keep for the duration of its lifetime. + * \return a WaitResult from a "ready" result. + */ + static + WaitResult + from_ready_wait_result_kind(WaitSetT & wait_set) + { + return WaitResult(WaitResultKind::Ready, wait_set); + } + + /// Create WaitResult from a "timeout" result. + static + WaitResult + from_timeout_wait_result_kind() + { + return WaitResult(WaitResultKind::Timeout); + } + + /// Create WaitResult from a "empty" result. + static + WaitResult + from_empty_wait_result_kind() + { + return WaitResult(WaitResultKind::Empty); + } + + /// Return the kind of the WaitResult. + WaitResultKind + kind() const + { + return wait_result_kind_; + } + + /// Return the rcl wait set. + /** + * \return const rcl wait set. + * \throws std::runtime_error if the class cannot access wait set when the result was not ready + */ + const WaitSetT & + get_wait_set() const + { + if (this->kind() != WaitResultKind::Ready) { + throw std::runtime_error("cannot access wait set when the result was not ready"); + } + // This should never happen, defensive (and debug mode) check only. + assert(wait_set_pointer_); + return *wait_set_pointer_; + } + + /// Return the rcl wait set. + /** + * \return rcl wait set. + * \throws std::runtime_error if the class cannot access wait set when the result was not ready + */ + WaitSetT & + get_wait_set() + { + if (this->kind() != WaitResultKind::Ready) { + throw std::runtime_error("cannot access wait set when the result was not ready"); + } + // This should never happen, defensive (and debug mode) check only. + assert(wait_set_pointer_); + return *wait_set_pointer_; + } + + WaitResult(WaitResult && other) noexcept + : wait_result_kind_(other.wait_result_kind_), + wait_set_pointer_(std::exchange(other.wait_set_pointer_, nullptr)) + {} + + ~WaitResult() + { + if (wait_set_pointer_) { + wait_set_pointer_->wait_result_release(); + } + } + + /// Get the next ready timer and its index in the wait result, but do not clear it. + /** + * The returned timer is not cleared automatically, as it the case with the + * other next_ready_*()-like functions. + * Instead, this function returns the timer and the index that identifies it + * in the wait result, so that it can be cleared (marked as taken or used) + * in a separate step with clear_timer_with_index(). + * This is necessary in some multi-threaded executor implementations. + * + * If the timer is not cleared using the index, subsequent calls to this + * function will return the same timer. + * + * If there is no ready timer, then nullptr will be returned and the index + * will be invalid and should not be used. + * + * \param[in] start_index index at which to start searching for the next ready + * timer in the wait result. If the start_index is out of bounds for the + * list of timers in the wait result, then {nullptr, start_index} will be + * returned. Defaults to 0. + * \return next ready timer pointer and its index in the wait result, or + * {nullptr, start_index} if none was found. + */ + std::pair, size_t> + peek_next_ready_timer(size_t start_index = 0) + { + check_wait_result_dirty(); + auto ret = std::shared_ptr{nullptr}; + size_t ii = start_index; + if (this->kind() == WaitResultKind::Ready) { + auto & wait_set = this->get_wait_set(); + auto & rcl_wait_set = wait_set.storage_get_rcl_wait_set(); + for (; ii < wait_set.size_of_timers(); ++ii) { + if (rcl_wait_set.timers[ii] != nullptr) { + ret = wait_set.timers(ii); + break; + } + } + } + return {ret, ii}; + } + + /// Clear the timer at the given index. + /** + * Clearing a timer from the wait result prevents it from being returned by + * the peek_next_ready_timer() on subsequent calls. + * + * The index should come from the peek_next_ready_timer() function, and + * should only be used with this function if the timer pointer was valid. + * + * \throws std::out_of_range if the given index is out of range + */ + void + clear_timer_with_index(size_t index) + { + auto & wait_set = this->get_wait_set(); + auto & rcl_wait_set = wait_set.storage_get_rcl_wait_set(); + if (index >= wait_set.size_of_timers()) { + throw std::out_of_range("given timer index is out of range"); + } + rcl_wait_set.timers[index] = nullptr; + } + + /// Get the next ready subscription, clearing it from the wait result. + std::shared_ptr + next_ready_subscription() + { + check_wait_result_dirty(); + auto ret = std::shared_ptr{nullptr}; + if (this->kind() == WaitResultKind::Ready) { + auto & wait_set = this->get_wait_set(); + auto & rcl_wait_set = wait_set.storage_get_rcl_wait_set(); + for (size_t ii = 0; ii < wait_set.size_of_subscriptions(); ++ii) { + if (rcl_wait_set.subscriptions[ii] != nullptr) { + ret = wait_set.subscriptions(ii); + rcl_wait_set.subscriptions[ii] = nullptr; + break; + } + } + } + return ret; + } + + /// Get the next ready service, clearing it from the wait result. + std::shared_ptr + next_ready_service() + { + check_wait_result_dirty(); + auto ret = std::shared_ptr{nullptr}; + if (this->kind() == WaitResultKind::Ready) { + auto & wait_set = this->get_wait_set(); + auto & rcl_wait_set = wait_set.storage_get_rcl_wait_set(); + for (size_t ii = 0; ii < wait_set.size_of_services(); ++ii) { + if (rcl_wait_set.services[ii] != nullptr) { + ret = wait_set.services(ii); + rcl_wait_set.services[ii] = nullptr; + break; + } + } + } + return ret; + } + + /// Get the next ready client, clearing it from the wait result. + std::shared_ptr + next_ready_client() + { + check_wait_result_dirty(); + auto ret = std::shared_ptr{nullptr}; + if (this->kind() == WaitResultKind::Ready) { + auto & wait_set = this->get_wait_set(); + auto & rcl_wait_set = wait_set.storage_get_rcl_wait_set(); + for (size_t ii = 0; ii < wait_set.size_of_clients(); ++ii) { + if (rcl_wait_set.clients[ii] != nullptr) { + ret = wait_set.clients(ii); + rcl_wait_set.clients[ii] = nullptr; + break; + } + } + } + return ret; + } + + /// Get the next ready waitable, clearing it from the wait result. + std::shared_ptr + next_ready_waitable() + { + check_wait_result_dirty(); + auto waitable = std::shared_ptr{nullptr}; + auto data = std::shared_ptr{nullptr}; + + if (this->kind() == WaitResultKind::Ready) { + auto & wait_set = this->get_wait_set(); + auto & rcl_wait_set = wait_set.get_rcl_wait_set(); + while (next_waitable_index_ < wait_set.size_of_waitables()) { + auto cur_waitable = wait_set.waitables(next_waitable_index_++); + if (cur_waitable != nullptr && cur_waitable->is_ready(rcl_wait_set)) { + waitable = cur_waitable; + break; + } + } + } + + return waitable; + } + +private: + RCLCPP_DISABLE_COPY(WaitResult) + + explicit WaitResult(WaitResultKind wait_result_kind) + : wait_result_kind_(wait_result_kind) + { + // Should be enforced by the static factory methods on this class. + assert(WaitResultKind::Ready != wait_result_kind); + } + + WaitResult(WaitResultKind wait_result_kind, WaitSetT & wait_set) + : wait_result_kind_(wait_result_kind), + wait_set_pointer_(&wait_set) + { + // Should be enforced by the static factory methods on this class. + assert(WaitResultKind::Ready == wait_result_kind); + // Secure thread-safety (if provided) and shared ownership (if needed). + this->get_wait_set().wait_result_acquire(); + } + + /// Check if the wait result is invalid because the wait set was modified. + void + check_wait_result_dirty() + { + // In the case that the wait set was modified while the result was out, + // we must mark the wait result as no longer valid + if (wait_set_pointer_ && this->get_wait_set().wait_result_dirty_) { + this->wait_result_kind_ = WaitResultKind::Invalid; + } + } + + WaitResultKind wait_result_kind_; + + WaitSetT * wait_set_pointer_ = nullptr; + + size_t next_waitable_index_ = 0; +}; + +} // namespace rclcpp + +#endif // RCLCPP__WAIT_RESULT_HPP_ diff --git a/rclcpp/include/rclcpp/wait_result_kind.hpp b/rclcpp/include/rclcpp/wait_result_kind.hpp new file mode 100644 index 0000000000..7980d1d127 --- /dev/null +++ b/rclcpp/include/rclcpp/wait_result_kind.hpp @@ -0,0 +1,34 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__WAIT_RESULT_KIND_HPP_ +#define RCLCPP__WAIT_RESULT_KIND_HPP_ + +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ + +/// Represents the various kinds of results from waiting on a wait set. +enum RCLCPP_PUBLIC WaitResultKind +{ + Ready, // + +#include "rcl/wait.h" + +#include "rclcpp/guard_condition.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" +#include "rclcpp/wait_set_policies/dynamic_storage.hpp" +#include "rclcpp/wait_set_policies/sequential_synchronization.hpp" +#include "rclcpp/wait_set_policies/static_storage.hpp" +#include "rclcpp/wait_set_policies/thread_safe_synchronization.hpp" +#include "rclcpp/wait_set_template.hpp" + +namespace rclcpp +{ + +/// Most common user configuration of a WaitSet, which is dynamic but not thread-safe. +/** + * This wait set allows you to add and remove items dynamically, and it will + * automatically remove items that are let out of scope each time wait() or + * prune_destroyed_entities() is called. + * + * It will not, however, provide thread-safety for adding and removing entities + * while waiting. + * + * \sa rclcpp::WaitSetTemplate for API documentation + */ +using WaitSet = rclcpp::WaitSetTemplate< + rclcpp::wait_set_policies::SequentialSynchronization, + rclcpp::wait_set_policies::DynamicStorage +>; + +/// WaitSet configuration which does not allow changes after construction. +/** + * This wait set requires that you specify all entities at construction, and + * prevents you from calling the typical add and remove functions. + * It also requires that you specify how many of each item there will be as a + * template argument. + * + * It will share ownership of the entities until destroyed, therefore it will + * prevent the destruction of entities so long as the wait set exists, even if + * the user lets their copy of the shared pointer to the entity go out of scope. + * + * Since the wait set cannot be mutated, it does not need to be thread-safe. + * + * \sa rclcpp::WaitSetTemplate for API documentation + */ +template< + std::size_t NumberOfSubscriptions, + std::size_t NumberOfGuardCondtions, + std::size_t NumberOfTimers, + std::size_t NumberOfClients, + std::size_t NumberOfServices, + std::size_t NumberOfWaitables +> +using StaticWaitSet = rclcpp::WaitSetTemplate< + rclcpp::wait_set_policies::SequentialSynchronization, + rclcpp::wait_set_policies::StaticStorage< + NumberOfSubscriptions, + NumberOfGuardCondtions, + NumberOfTimers, + NumberOfClients, + NumberOfServices, + NumberOfWaitables + > +>; + +/// Like WaitSet, this configuration is dynamic, but is also thread-safe. +/** + * This wait set allows you to add and remove items dynamically, and it will + * automatically remove items that are let out of scope each time wait() or + * prune_destroyed_entities() is called. + * + * It will also ensure that adding and removing items explicitly from the + * wait set is done in a thread-safe way, protecting against concurrent add and + * deletes, as well as add and deletes during a wait(). + * This thread-safety comes at some overhead and the use of thread + * synchronization primitives. + * + * \sa rclcpp::WaitSetTemplate for API documentation + */ +using ThreadSafeWaitSet = rclcpp::WaitSetTemplate< + rclcpp::wait_set_policies::ThreadSafeSynchronization, + rclcpp::wait_set_policies::DynamicStorage +>; + +} // namespace rclcpp + +#endif // RCLCPP__WAIT_SET_HPP_ diff --git a/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp b/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp new file mode 100644 index 0000000000..99050dfeba --- /dev/null +++ b/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp @@ -0,0 +1,437 @@ +// 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 RCLCPP__WAIT_SET_POLICIES__DETAIL__STORAGE_POLICY_COMMON_HPP_ +#define RCLCPP__WAIT_SET_POLICIES__DETAIL__STORAGE_POLICY_COMMON_HPP_ + +#include +#include +#include + +#include "rcl/wait.h" + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" +#include "rclcpp/waitable.hpp" + +namespace rclcpp +{ +namespace wait_set_policies +{ +namespace detail +{ + +/// Common structure for storage policies, which provides rcl wait set access. +template +class StoragePolicyCommon +{ +protected: + template< + class SubscriptionsIterable, + class GuardConditionsIterable, + class ExtraGuardConditionsIterable, + class TimersIterable, + class ClientsIterable, + class ServicesIterable, + class WaitablesIterable + > + explicit + StoragePolicyCommon( + const SubscriptionsIterable & subscriptions, + const GuardConditionsIterable & guard_conditions, + const ExtraGuardConditionsIterable & extra_guard_conditions, + const TimersIterable & timers, + const ClientsIterable & clients, + const ServicesIterable & services, + const WaitablesIterable & waitables, + rclcpp::Context::SharedPtr context + ) + : rcl_wait_set_(rcl_get_zero_initialized_wait_set()), context_(context) + { + // Check context is not nullptr. + if (nullptr == context) { + throw std::invalid_argument("context is nullptr"); + } + // Accumulate total contributions from waitables. + size_t subscriptions_from_waitables = 0; + size_t guard_conditions_from_waitables = 0; + size_t timers_from_waitables = 0; + size_t clients_from_waitables = 0; + size_t services_from_waitables = 0; + size_t events_from_waitables = 0; + for (const auto & waitable_entry : waitables) { + auto waitable_ptr_pair = get_raw_pointer_from_smart_pointer(waitable_entry.waitable); + if (nullptr == waitable_ptr_pair.second) { + if (HasStrongOwnership) { + throw std::runtime_error("unexpected condition, fixed storage policy needs pruning"); + } + // Flag for pruning. + needs_pruning_ = true; + continue; + } + + rclcpp::Waitable & waitable = *waitable_ptr_pair.second; + subscriptions_from_waitables += waitable.get_number_of_ready_subscriptions(); + guard_conditions_from_waitables += waitable.get_number_of_ready_guard_conditions(); + timers_from_waitables += waitable.get_number_of_ready_timers(); + clients_from_waitables += waitable.get_number_of_ready_clients(); + services_from_waitables += waitable.get_number_of_ready_services(); + events_from_waitables += waitable.get_number_of_ready_events(); + } + // Initialize wait set using initial inputs. + rcl_ret_t ret = rcl_wait_set_init( + &rcl_wait_set_, + subscriptions.size() + subscriptions_from_waitables, + guard_conditions.size() + extra_guard_conditions.size() + guard_conditions_from_waitables, + timers.size() + timers_from_waitables, + clients.size() + clients_from_waitables, + services.size() + services_from_waitables, + events_from_waitables, + context_->get_rcl_context().get(), + // TODO(wjwwood): support custom allocator, maybe restrict to polymorphic allocator + rcl_get_default_allocator()); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to create wait set"); + } + + // (Re)build the wait set for the first time. + this->storage_rebuild_rcl_wait_set_with_sets( + subscriptions, + guard_conditions, + extra_guard_conditions, + timers, + clients, + services, + waitables); + } + + ~StoragePolicyCommon() + { + rcl_ret_t ret = rcl_wait_set_fini(&rcl_wait_set_); + if (RCL_RET_OK != ret) { + try { + rclcpp::exceptions::throw_from_rcl_error(ret); + } catch (const std::exception & exception) { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), + "Error in destruction of rcl wait set: %s", exception.what()); + } + } + } + + template + std::pair + get_raw_pointer_from_smart_pointer(const std::shared_ptr & shared_pointer) + { + return {nullptr, shared_pointer.get()}; + } + + template + std::pair, EntityT *> + get_raw_pointer_from_smart_pointer(const std::weak_ptr & weak_pointer) + { + auto shared_pointer = weak_pointer.lock(); + return {shared_pointer, shared_pointer.get()}; + } + + /// Rebuild the wait set, preparing it for the next wait call. + /** + * The wait set is rebuilt by: + * + * - resizing the wait set if needed, + * - clearing the wait set if not already done by resizing, and + * - re-adding the entities. + */ + template< + class SubscriptionsIterable, + class GuardConditionsIterable, + class ExtraGuardConditionsIterable, + class TimersIterable, + class ClientsIterable, + class ServicesIterable, + class WaitablesIterable + > + void + storage_rebuild_rcl_wait_set_with_sets( + const SubscriptionsIterable & subscriptions, + const GuardConditionsIterable & guard_conditions, + const ExtraGuardConditionsIterable & extra_guard_conditions, + const TimersIterable & timers, + const ClientsIterable & clients, + const ServicesIterable & services, + const WaitablesIterable & waitables + ) + { + bool was_resized = false; + // Resize the wait set, but only if it needs to be. + if (needs_resize_) { + // Resizing with rcl_wait_set_resize() is a no-op if nothing has changed, + // but tracking the need to resize in this class avoids an unnecessary + // library call (rcl is most likely a separate shared library) each wait + // loop. + // Also, since static storage wait sets will never need resizing, so it + // avoids completely redundant calls to this function in that case. + // Accumulate total contributions from waitables. + size_t subscriptions_from_waitables = 0; + size_t guard_conditions_from_waitables = 0; + size_t timers_from_waitables = 0; + size_t clients_from_waitables = 0; + size_t services_from_waitables = 0; + size_t events_from_waitables = 0; + for (const auto & waitable_entry : waitables) { + if (!waitable_entry.waitable) { + // In this case it was probably stored as a weak_ptr, but is now locking to nullptr. + if (HasStrongOwnership) { + // This will not happen in fixed sized storage, as it holds + // shared ownership the whole time and is never in need of pruning. + throw std::runtime_error("unexpected condition, fixed storage policy needs pruning"); + } + // Flag for pruning. + needs_pruning_ = true; + continue; + } + auto & waitable = *waitable_entry.waitable; + subscriptions_from_waitables += waitable.get_number_of_ready_subscriptions(); + guard_conditions_from_waitables += waitable.get_number_of_ready_guard_conditions(); + timers_from_waitables += waitable.get_number_of_ready_timers(); + clients_from_waitables += waitable.get_number_of_ready_clients(); + services_from_waitables += waitable.get_number_of_ready_services(); + events_from_waitables += waitable.get_number_of_ready_events(); + } + rcl_ret_t ret = rcl_wait_set_resize( + &rcl_wait_set_, + subscriptions.size() + subscriptions_from_waitables, + guard_conditions.size() + extra_guard_conditions.size() + guard_conditions_from_waitables, + timers.size() + timers_from_waitables, + clients.size() + clients_from_waitables, + services.size() + services_from_waitables, + events_from_waitables + ); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't resize the wait set"); + } + was_resized = true; + // Assumption: the calling code ensures this function is not called + // concurrently with functions that set this variable to true, either + // with documentation (as is the case for the SequentialSychronization + // policy), or with synchronization primitives (as is the case with + // the ThreadSafeSynchronization policy). + needs_resize_ = false; + } + + // Now clear the wait set, but only if it was not resized, as resizing also + // clears the wait set. + if (!was_resized) { + rcl_ret_t ret = rcl_wait_set_clear(&rcl_wait_set_); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't clear the wait set"); + } + } + + // Add subscriptions. + for (const auto & subscription_entry : subscriptions) { + if (!subscription_entry.subscription) { + // In this case it was probably stored as a weak_ptr, but is now locking to nullptr. + if (HasStrongOwnership) { + // This will not happen in fixed sized storage, as it holds + // shared ownership the whole time and is never in need of pruning. + throw std::runtime_error("unexpected condition, fixed storage policy needs pruning"); + } + // Flag for pruning. + needs_pruning_ = true; + continue; + } + + rcl_ret_t ret = rcl_wait_set_add_subscription( + &rcl_wait_set_, + subscription_entry.subscription->get_subscription_handle().get(), + nullptr); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't fill wait set"); + } + } + + // Setup common code to add guard_conditions. + auto add_guard_conditions = + [this](const auto & inner_guard_conditions) + { + for (const auto & guard_condition : inner_guard_conditions) { + if (!guard_condition) { + // In this case it was probably stored as a weak_ptr, but is now locking to nullptr. + if (HasStrongOwnership) { + // This will not happen in fixed sized storage, as it holds + // shared ownership the whole time and is never in need of pruning. + throw std::runtime_error("unexpected condition, fixed storage policy needs pruning"); + } + // Flag for pruning. + needs_pruning_ = true; + continue; + } + rcl_ret_t ret = rcl_wait_set_add_guard_condition( + &rcl_wait_set_, + &guard_condition->get_rcl_guard_condition(), + nullptr); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't fill wait set"); + } + } + }; + + // Add guard conditions. + add_guard_conditions(guard_conditions); + + // Add extra guard conditions. + add_guard_conditions(extra_guard_conditions); + + // Add timers. + for (const auto & timer : timers) { + if (!timer) { + // In this case it was probably stored as a weak_ptr, but is now locking to nullptr. + if (HasStrongOwnership) { + // This will not happen in fixed sized storage, as it holds + // shared ownership the whole time and is never in need of pruning. + throw std::runtime_error("unexpected condition, fixed storage policy needs pruning"); + } + // Flag for pruning. + needs_pruning_ = true; + continue; + } + rcl_ret_t ret = rcl_wait_set_add_timer( + &rcl_wait_set_, + timer->get_timer_handle().get(), + nullptr); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't fill wait set"); + } + } + + // Add clients. + for (const auto & client : clients) { + if (!client) { + // In this case it was probably stored as a weak_ptr, but is now locking to nullptr. + if (HasStrongOwnership) { + // This will not happen in fixed sized storage, as it holds + // shared ownership the whole time and is never in need of pruning. + throw std::runtime_error("unexpected condition, fixed storage policy needs pruning"); + } + // Flag for pruning. + needs_pruning_ = true; + continue; + } + rcl_ret_t ret = rcl_wait_set_add_client( + &rcl_wait_set_, + client->get_client_handle().get(), + nullptr); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + } + + // Add services. + for (const auto & service : services) { + if (!service) { + // In this case it was probably stored as a weak_ptr, but is now locking to nullptr. + if (HasStrongOwnership) { + // This will not happen in fixed sized storage, as it holds + // shared ownership the whole time and is never in need of pruning. + throw std::runtime_error("unexpected condition, fixed storage policy needs pruning"); + } + // Flag for pruning. + needs_pruning_ = true; + continue; + } + rcl_ret_t ret = rcl_wait_set_add_service( + &rcl_wait_set_, + service->get_service_handle().get(), + nullptr); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't fill wait set"); + } + } + + // Add waitables. + for (auto & waitable_entry : waitables) { + if (!waitable_entry.waitable) { + // In this case it was probably stored as a weak_ptr, but is now locking to nullptr. + if (HasStrongOwnership) { + // This will not happen in fixed sized storage, as it holds + // shared ownership the whole time and is never in need of pruning. + throw std::runtime_error("unexpected condition, fixed storage policy needs pruning"); + } + // Flag for pruning. + needs_pruning_ = true; + continue; + } + waitable_entry.waitable->add_to_wait_set(rcl_wait_set_); + } + } + + const rcl_wait_set_t & + storage_get_rcl_wait_set() const + { + return rcl_wait_set_; + } + + rcl_wait_set_t & + storage_get_rcl_wait_set() + { + return rcl_wait_set_; + } + + void + storage_flag_for_resize() + { + needs_resize_ = true; + } + + size_t size_of_subscriptions() const {return 0;} + size_t size_of_timers() const {return 0;} + size_t size_of_clients() const {return 0;} + size_t size_of_services() const {return 0;} + size_t size_of_waitables() const {return 0;} + + template + typename SubscriptionsIterable::value_type + subscriptions(size_t) const {return nullptr;} + + template + typename TimersIterable::value_type + timers(size_t) const {return nullptr;} + + template + typename ClientsIterable::value_type + clients(size_t) const {return nullptr;} + + template + typename ServicesIterable::value_type + services(size_t) const {return nullptr;} + + template + typename WaitablesIterable::value_type + waitables(size_t) const {return nullptr;} + + rcl_wait_set_t rcl_wait_set_; + rclcpp::Context::SharedPtr context_; + + bool needs_pruning_ = false; + bool needs_resize_ = false; +}; + +} // namespace detail +} // namespace wait_set_policies +} // namespace rclcpp + +#endif // RCLCPP__WAIT_SET_POLICIES__DETAIL__STORAGE_POLICY_COMMON_HPP_ diff --git a/rclcpp/include/rclcpp/wait_set_policies/detail/synchronization_policy_common.hpp b/rclcpp/include/rclcpp/wait_set_policies/detail/synchronization_policy_common.hpp new file mode 100644 index 0000000000..2856ec7275 --- /dev/null +++ b/rclcpp/include/rclcpp/wait_set_policies/detail/synchronization_policy_common.hpp @@ -0,0 +1,72 @@ +// 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 RCLCPP__WAIT_SET_POLICIES__DETAIL__SYNCHRONIZATION_POLICY_COMMON_HPP_ +#define RCLCPP__WAIT_SET_POLICIES__DETAIL__SYNCHRONIZATION_POLICY_COMMON_HPP_ + +#include +#include + +namespace rclcpp +{ +namespace wait_set_policies +{ +namespace detail +{ + +/// Common structure for synchronization policies. +class SynchronizationPolicyCommon +{ +protected: + SynchronizationPolicyCommon() = default; + ~SynchronizationPolicyCommon() = default; + + std::function + create_loop_predicate( + std::chrono::nanoseconds time_to_wait_ns, + std::chrono::steady_clock::time_point start) + { + if (time_to_wait_ns >= std::chrono::nanoseconds(0)) { + // If time_to_wait_ns is >= 0 schedule against a deadline. + auto deadline = start + time_to_wait_ns; + return [deadline]() -> bool {return std::chrono::steady_clock::now() < deadline;}; + } else { + // In the case of time_to_wait_ns < 0, just always return true to loop forever. + return []() -> bool {return true;}; + } + } + + std::chrono::nanoseconds + calculate_time_left_to_wait( + std::chrono::nanoseconds original_time_to_wait_ns, + std::chrono::steady_clock::time_point start) + { + std::chrono::nanoseconds time_left_to_wait; + if (original_time_to_wait_ns < std::chrono::nanoseconds(0)) { + time_left_to_wait = original_time_to_wait_ns; + } else { + time_left_to_wait = original_time_to_wait_ns - (std::chrono::steady_clock::now() - start); + if (time_left_to_wait < std::chrono::nanoseconds(0)) { + time_left_to_wait = std::chrono::nanoseconds(0); + } + } + return time_left_to_wait; + } +}; + +} // namespace detail +} // namespace wait_set_policies +} // namespace rclcpp + +#endif // RCLCPP__WAIT_SET_POLICIES__DETAIL__SYNCHRONIZATION_POLICY_COMMON_HPP_ diff --git a/rclcpp/include/rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.hpp b/rclcpp/include/rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.hpp new file mode 100644 index 0000000000..933bbf6895 --- /dev/null +++ b/rclcpp/include/rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.hpp @@ -0,0 +1,243 @@ +// 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 RCLCPP__WAIT_SET_POLICIES__DETAIL__WRITE_PREFERRING_READ_WRITE_LOCK_HPP_ +#define RCLCPP__WAIT_SET_POLICIES__DETAIL__WRITE_PREFERRING_READ_WRITE_LOCK_HPP_ + +#include +#include +#include + +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace wait_set_policies +{ +namespace detail +{ + +/// Writer-perferring read-write lock. +/** + * This class is based on an implementation of a "write-preferring RW lock" as described in this + * wikipedia page: + * + * https://en.wikipedia.org/wiki/Readers%E2%80%93writer_lock#Using_a_condition_variable_and_a_mutex + * + * Copying here for posterity: + * + * \verbatim + * For a write-preferring RW lock one can use two integer counters and one boolean flag: + * + * num_readers_active: the number of readers that have acquired the lock (integer) + * num_writers_waiting: the number of writers waiting for access (integer) + * writer_active: whether a writer has acquired the lock (boolean). + * + * Initially num_readers_active and num_writers_waiting are zero and writer_active is false. + * + * The lock and release operations can be implemented as + * + * Begin Read + * + * Lock g + * While num_writers_waiting > 0 or writer_active: + * wait cond, g[a] + * Increment num_readers_active + * Unlock g. + * + * End Read + * + * Lock g + * Decrement num_readers_active + * If num_readers_active = 0: + * Notify cond (broadcast) + * Unlock g. + * + * Begin Write + * + * Lock g + * Increment num_writers_waiting + * While num_readers_active > 0 or writer_active is true: + * wait cond, g + * Decrement num_writers_waiting + * Set writer_active to true + * Unlock g. + * + * End Write + * + * Lock g + * Set writer_active to false + * Notify cond (broadcast) + * Unlock g. + * \endverbatim + * + * It will prefer any waiting write calls to any waiting read calls, meaning + * that excessive write calls can starve read calls. + * + * This class diverges from that design in two important ways. + * First, it is a single reader, single writer version. + * Second, it allows for user defined code to be run after a writer enters the + * waiting state, and the purpose of this feature is to allow the user to + * interrupt any potentially long blocking read activities. + * + * Together these two features allow new waiting writers to not only ensure + * they get the lock before any queued readers, but also that it can safely + * interrupt read activities if needed, without allowing new read activities to + * start before it gains the lock. + * + * The first difference prevents the case that a multiple read activities occur + * at the same time but the writer can only reliably interrupt one of them. + * By preventing multiple read activities concurrently, this case is avoided. + * The second difference allows the user to define how to interrupt read + * activity that could be blocking the write activities that need to happen + * as soon as possible. + * + * To implement the differences, this class replaces the "num_readers_active" + * counter with a "reader_active" boolean. + * It also changes the "Begin Read" section from above, like this: + * + * \verbatim + * Begin Read + * + * Lock g + * While num_writers_waiting > 0 or writer_active or reader_active: // changed + * wait cond, g[a] + * Set reader_active to true // changed + * Unlock g. + * \endverbatim + * + * And changes the "End Read" section from above, like this: + * + * \verbatim + * End Read + * + * Lock g + * Set reader_active to false // changed + * Notify cond (broadcast) // changed, now unconditional + * Unlock g. + * \endverbatim + * + * The "Begin Write" section is also updated as follows: + * + * \verbatim + * Begin Write + * + * Lock g + * Increment num_writers_waiting + * Call user defined enter_waiting function // new + * While reader_active is true or writer_active is true: // changed + * wait cond, g + * Decrement num_writers_waiting + * Set writer_active to true + * Unlock g. + * \endverbatim + * + * The implementation uses a single condition variable, single lock, and several + * state variables. + * + * The typical use of this class is as follows: + * + * class MyClass + * { + * WritePreferringReadWriteLock wprw_lock_; + * public: + * MyClass() {} + * void do_some_reading() + * { + * using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock; + * std::lock_guard lock(wprw_lock_.get_read_mutex()); + * // Do reading... + * } + * void do_some_writing() + * { + * using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock; + * std::lock_guard lock(wprw_lock_.get_write_mutex()); + * // Do writing... + * } + * }; + */ +class WritePreferringReadWriteLock final +{ +public: + RCLCPP_PUBLIC + explicit WritePreferringReadWriteLock(std::function enter_waiting_function = nullptr); + + /// Read mutex for the WritePreferringReadWriteLock. + /** + * Implements the "C++ named requirements: BasicLockable". + */ + class RCLCPP_PUBLIC ReadMutex + { +public: + void + lock(); + + void + unlock(); + +protected: + explicit ReadMutex(WritePreferringReadWriteLock & parent_lock); + + WritePreferringReadWriteLock & parent_lock_; + + friend class WritePreferringReadWriteLock; + }; + + /// Write mutex for the WritePreferringReadWriteLock. + /** + * Implements the "C++ named requirements: BasicLockable". + */ + class RCLCPP_PUBLIC WriteMutex + { +public: + void + lock(); + + void + unlock(); + +protected: + explicit WriteMutex(WritePreferringReadWriteLock & parent_lock); + + WritePreferringReadWriteLock & parent_lock_; + + friend class WritePreferringReadWriteLock; + }; + + /// Return read mutex which can be used with standard constructs like std::lock_guard. + RCLCPP_PUBLIC + ReadMutex & + get_read_mutex(); + + /// Return write mutex which can be used with standard constructs like std::lock_guard. + RCLCPP_PUBLIC + WriteMutex & + get_write_mutex(); + +protected: + bool reader_active_ = false; + std::size_t number_of_writers_waiting_ = 0; + bool writer_active_ = false; + std::mutex mutex_; + std::condition_variable condition_variable_; + ReadMutex read_mutex_; + WriteMutex write_mutex_; + std::function enter_waiting_function_; +}; + +} // namespace detail +} // namespace wait_set_policies +} // namespace rclcpp + +#endif // RCLCPP__WAIT_SET_POLICIES__DETAIL__WRITE_PREFERRING_READ_WRITE_LOCK_HPP_ diff --git a/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp b/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp new file mode 100644 index 0000000000..8f97596218 --- /dev/null +++ b/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp @@ -0,0 +1,535 @@ +// 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 RCLCPP__WAIT_SET_POLICIES__DYNAMIC_STORAGE_HPP_ +#define RCLCPP__WAIT_SET_POLICIES__DYNAMIC_STORAGE_HPP_ + +#include +#include +#include +#include + +#include "rclcpp/client.hpp" +#include "rclcpp/guard_condition.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/service.hpp" +#include "rclcpp/subscription_base.hpp" +#include "rclcpp/subscription_wait_set_mask.hpp" +#include "rclcpp/timer.hpp" +#include "rclcpp/visibility_control.hpp" +#include "rclcpp/wait_set_policies/detail/storage_policy_common.hpp" +#include "rclcpp/waitable.hpp" + +namespace rclcpp +{ +namespace wait_set_policies +{ + +/// WaitSet policy that provides dynamically sized storage. +class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCommon +{ +protected: + using is_mutable = std::true_type; + + class SubscriptionEntry + { + // (wjwwood): indent of 'public:' is weird, I know. uncrustify is dumb. + +public: + std::shared_ptr subscription; + rclcpp::SubscriptionWaitSetMask mask; + + /// Conversion constructor, which is intentionally not marked explicit. + SubscriptionEntry( + std::shared_ptr subscription_in = nullptr, + const rclcpp::SubscriptionWaitSetMask & mask_in = {}) + : subscription(std::move(subscription_in)), + mask(mask_in) + {} + + void + reset() noexcept + { + subscription.reset(); + } + }; + class WeakSubscriptionEntry + { +public: + std::weak_ptr subscription; + rclcpp::SubscriptionWaitSetMask mask; + + explicit WeakSubscriptionEntry( + const std::shared_ptr & subscription_in, + const rclcpp::SubscriptionWaitSetMask & mask_in) noexcept + : subscription(subscription_in), + mask(mask_in) + {} + + explicit WeakSubscriptionEntry(const SubscriptionEntry & other) + : subscription(other.subscription), + mask(other.mask) + {} + + std::shared_ptr + lock() const + { + return subscription.lock(); + } + + bool + expired() const noexcept + { + return subscription.expired(); + } + }; + using SequenceOfWeakSubscriptions = std::vector; + using SubscriptionsIterable = std::vector; + + using SequenceOfWeakGuardConditions = std::vector>; + using GuardConditionsIterable = std::vector>; + + using SequenceOfWeakTimers = std::vector>; + using TimersIterable = std::vector>; + + using SequenceOfWeakClients = std::vector>; + using ClientsIterable = std::vector>; + + using SequenceOfWeakServices = std::vector>; + using ServicesIterable = std::vector>; + + class WaitableEntry + { +public: + std::shared_ptr waitable; + std::shared_ptr associated_entity; + + /// Conversion constructor, which is intentionally not marked explicit. + WaitableEntry( + std::shared_ptr waitable_in = nullptr, + std::shared_ptr associated_entity_in = nullptr) noexcept + : waitable(std::move(waitable_in)), + associated_entity(std::move(associated_entity_in)) + {} + + void + reset() noexcept + { + waitable.reset(); + associated_entity.reset(); + } + }; + class WeakWaitableEntry + { +public: + std::weak_ptr waitable; + std::weak_ptr associated_entity; + + explicit WeakWaitableEntry( + const std::shared_ptr & waitable_in, + const std::shared_ptr & associated_entity_in) noexcept + : waitable(waitable_in), + associated_entity(associated_entity_in) + {} + + explicit WeakWaitableEntry(const WaitableEntry & other) + : waitable(other.waitable), + associated_entity(other.associated_entity) + {} + + std::shared_ptr + lock() const + { + return waitable.lock(); + } + + bool + expired() const noexcept + { + return waitable.expired(); + } + }; + using SequenceOfWeakWaitables = std::vector; + using WaitablesIterable = std::vector; + + template + explicit + DynamicStorage( + const SubscriptionsIterable & subscriptions, + const GuardConditionsIterable & guard_conditions, + const ArrayOfExtraGuardConditions & extra_guard_conditions, + const TimersIterable & timers, + const ClientsIterable & clients, + const ServicesIterable & services, + const WaitablesIterable & waitables, + rclcpp::Context::SharedPtr context + ) + : StoragePolicyCommon( + subscriptions, + guard_conditions, + extra_guard_conditions, + timers, + clients, + services, + waitables, + context), + subscriptions_(subscriptions.cbegin(), subscriptions.cend()), + shared_subscriptions_(subscriptions_.size()), + guard_conditions_(guard_conditions.cbegin(), guard_conditions.cend()), + shared_guard_conditions_(guard_conditions_.size()), + timers_(timers.cbegin(), timers.cend()), + shared_timers_(timers_.size()), + clients_(clients.cbegin(), clients.cend()), + shared_clients_(clients_.size()), + services_(services.cbegin(), services.cend()), + shared_services_(services_.size()), + waitables_(waitables.cbegin(), waitables.cend()), + shared_waitables_(waitables_.size()) + {} + + ~DynamicStorage() = default; + + template + void + storage_rebuild_rcl_wait_set(const ArrayOfExtraGuardConditions & extra_guard_conditions) + { + this->storage_acquire_ownerships(); + + this->storage_rebuild_rcl_wait_set_with_sets( + shared_subscriptions_, + shared_guard_conditions_, + extra_guard_conditions, + shared_timers_, + shared_clients_, + shared_services_, + shared_waitables_ + ); + + this->storage_release_ownerships(); + } + + template + static + bool + storage_has_entity(const EntityT & entity, const SequenceOfEntitiesT & entities) + { + return std::any_of( + entities.cbegin(), + entities.cend(), + [&entity](const auto & inner) {return &entity == inner.lock().get();}); + } + + template + static + auto + storage_find_entity(const EntityT & entity, const SequenceOfEntitiesT & entities) + { + return std::find_if( + entities.cbegin(), + entities.cend(), + [&entity](const auto & inner) {return &entity == inner.lock().get();}); + } + + void + storage_add_subscription(std::shared_ptr && subscription) + { + if (this->storage_has_entity(*subscription, subscriptions_)) { + throw std::runtime_error("subscription already in wait set"); + } + WeakSubscriptionEntry weak_entry{std::move(subscription), {}}; + subscriptions_.push_back(std::move(weak_entry)); + this->storage_flag_for_resize(); + } + + void + storage_remove_subscription(std::shared_ptr && subscription) + { + auto it = this->storage_find_entity(*subscription, subscriptions_); + if (subscriptions_.cend() == it) { + throw std::runtime_error("subscription not in wait set"); + } + subscriptions_.erase(it); + this->storage_flag_for_resize(); + } + + void + storage_add_guard_condition(std::shared_ptr && guard_condition) + { + if (this->storage_has_entity(*guard_condition, guard_conditions_)) { + throw std::runtime_error("guard_condition already in wait set"); + } + guard_conditions_.push_back(std::move(guard_condition)); + this->storage_flag_for_resize(); + } + + void + storage_remove_guard_condition(std::shared_ptr && guard_condition) + { + auto it = this->storage_find_entity(*guard_condition, guard_conditions_); + if (guard_conditions_.cend() == it) { + throw std::runtime_error("guard_condition not in wait set"); + } + guard_conditions_.erase(it); + this->storage_flag_for_resize(); + } + + void + storage_add_timer(std::shared_ptr && timer) + { + if (this->storage_has_entity(*timer, timers_)) { + throw std::runtime_error("timer already in wait set"); + } + timers_.push_back(std::move(timer)); + this->storage_flag_for_resize(); + } + + void + storage_remove_timer(std::shared_ptr && timer) + { + auto it = this->storage_find_entity(*timer, timers_); + if (timers_.cend() == it) { + throw std::runtime_error("timer not in wait set"); + } + timers_.erase(it); + this->storage_flag_for_resize(); + } + + void + storage_add_client(std::shared_ptr && client) + { + if (this->storage_has_entity(*client, clients_)) { + throw std::runtime_error("client already in wait set"); + } + clients_.push_back(std::move(client)); + this->storage_flag_for_resize(); + } + + void + storage_remove_client(std::shared_ptr && client) + { + auto it = this->storage_find_entity(*client, clients_); + if (clients_.cend() == it) { + throw std::runtime_error("client not in wait set"); + } + clients_.erase(it); + this->storage_flag_for_resize(); + } + + void + storage_add_service(std::shared_ptr && service) + { + if (this->storage_has_entity(*service, services_)) { + throw std::runtime_error("service already in wait set"); + } + services_.push_back(std::move(service)); + this->storage_flag_for_resize(); + } + + void + storage_remove_service(std::shared_ptr && service) + { + auto it = this->storage_find_entity(*service, services_); + if (services_.cend() == it) { + throw std::runtime_error("service not in wait set"); + } + services_.erase(it); + this->storage_flag_for_resize(); + } + + void + storage_add_waitable( + std::shared_ptr && waitable, + std::shared_ptr && associated_entity) + { + if (this->storage_has_entity(*waitable, waitables_)) { + throw std::runtime_error("waitable already in wait set"); + } + WeakWaitableEntry weak_entry(std::move(waitable), std::move(associated_entity)); + waitables_.push_back(std::move(weak_entry)); + this->storage_flag_for_resize(); + } + + void + storage_remove_waitable(std::shared_ptr && waitable) + { + auto it = this->storage_find_entity(*waitable, waitables_); + if (waitables_.cend() == it) { + throw std::runtime_error("waitable not in wait set"); + } + waitables_.erase(it); + this->storage_flag_for_resize(); + } + + // this is noexcept because: + // - std::weak_ptr::expired is noexcept + // - the erase-remove idiom is noexcept, since we're not using the ExecutionPolicy version + // - std::vector::erase is noexcept if the assignment operator of T is also + // - and, the operator= for std::weak_ptr is noexcept + void + storage_prune_deleted_entities() noexcept + { + // reusable (templated) lambda for removal predicate + auto p = + [](const auto & weak_ptr) { + // remove entries which have expired + return weak_ptr.expired(); + }; + // remove guard conditions which have been deleted + subscriptions_.erase( + std::remove_if(subscriptions_.begin(), subscriptions_.end(), p), subscriptions_.end()); + guard_conditions_.erase( + std::remove_if(guard_conditions_.begin(), guard_conditions_.end(), p), + guard_conditions_.end()); + timers_.erase(std::remove_if(timers_.begin(), timers_.end(), p), timers_.end()); + clients_.erase(std::remove_if(clients_.begin(), clients_.end(), p), clients_.end()); + services_.erase(std::remove_if(services_.begin(), services_.end(), p), services_.end()); + waitables_.erase(std::remove_if(waitables_.begin(), waitables_.end(), p), waitables_.end()); + } + + void + storage_acquire_ownerships() + { + if (++ownership_reference_counter_ > 1) { + // Avoid redundant locking. + return; + } + // Setup common locking function. + auto lock_all = [](const auto & weak_ptrs, auto & shared_ptrs) { + shared_ptrs.resize(weak_ptrs.size()); + size_t index = 0; + for (const auto & weak_ptr : weak_ptrs) { + shared_ptrs[index++] = weak_ptr.lock(); + } + }; + // Lock all the weak pointers and hold them until released. + lock_all(subscriptions_, shared_subscriptions_); + lock_all(guard_conditions_, shared_guard_conditions_); + lock_all(timers_, shared_timers_); + lock_all(clients_, shared_clients_); + lock_all(services_, shared_services_); + + // We need a specialized version of this for waitables. + auto lock_all_waitables = [](const auto & weak_ptrs, auto & shared_ptrs) { + shared_ptrs.resize(weak_ptrs.size()); + size_t index = 0; + for (const auto & weak_ptr : weak_ptrs) { + shared_ptrs[index++] = WaitableEntry{ + weak_ptr.waitable.lock(), + weak_ptr.associated_entity.lock()}; + } + }; + lock_all_waitables(waitables_, shared_waitables_); + } + + void + storage_release_ownerships() + { + if (--ownership_reference_counter_ > 0) { + // Avoid releasing ownership until reference count is 0. + return; + } + // "Unlock" all shared pointers by resetting them. + auto reset_all = [](auto & shared_ptrs) { + for (auto & shared_ptr : shared_ptrs) { + shared_ptr.reset(); + } + }; + reset_all(shared_subscriptions_); + reset_all(shared_guard_conditions_); + reset_all(shared_timers_); + reset_all(shared_clients_); + reset_all(shared_services_); + reset_all(shared_waitables_); + } + + size_t size_of_subscriptions() const + { + return shared_subscriptions_.size(); + } + + size_t size_of_timers() const + { + return shared_timers_.size(); + } + + size_t size_of_clients() const + { + return shared_clients_.size(); + } + + size_t size_of_services() const + { + return shared_services_.size(); + } + + size_t size_of_waitables() const + { + return shared_waitables_.size(); + } + + std::shared_ptr + subscriptions(size_t ii) const + { + return shared_subscriptions_[ii].subscription; + } + + std::shared_ptr + timers(size_t ii) const + { + return shared_timers_[ii]; + } + + std::shared_ptr + clients(size_t ii) const + { + return shared_clients_[ii]; + } + + std::shared_ptr + services(size_t ii) const + { + return shared_services_[ii]; + } + + std::shared_ptr + waitables(size_t ii) const + { + return shared_waitables_[ii].waitable; + } + + size_t ownership_reference_counter_ = 0; + + SequenceOfWeakSubscriptions subscriptions_; + SubscriptionsIterable shared_subscriptions_; + + SequenceOfWeakGuardConditions guard_conditions_; + GuardConditionsIterable shared_guard_conditions_; + + SequenceOfWeakTimers timers_; + TimersIterable shared_timers_; + + SequenceOfWeakClients clients_; + ClientsIterable shared_clients_; + + SequenceOfWeakServices services_; + ServicesIterable shared_services_; + + SequenceOfWeakWaitables waitables_; + WaitablesIterable shared_waitables_; +}; + +} // namespace wait_set_policies +} // namespace rclcpp + +#endif // RCLCPP__WAIT_SET_POLICIES__DYNAMIC_STORAGE_HPP_ diff --git a/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp b/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp new file mode 100644 index 0000000000..4afc2a1b27 --- /dev/null +++ b/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp @@ -0,0 +1,317 @@ +// 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 RCLCPP__WAIT_SET_POLICIES__SEQUENTIAL_SYNCHRONIZATION_HPP_ +#define RCLCPP__WAIT_SET_POLICIES__SEQUENTIAL_SYNCHRONIZATION_HPP_ + +#include +#include +#include +#include + +#include "rclcpp/client.hpp" +#include "rclcpp/exceptions.hpp" +#include "rclcpp/guard_condition.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/service.hpp" +#include "rclcpp/subscription_base.hpp" +#include "rclcpp/subscription_wait_set_mask.hpp" +#include "rclcpp/timer.hpp" +#include "rclcpp/visibility_control.hpp" +#include "rclcpp/wait_result.hpp" +#include "rclcpp/wait_result_kind.hpp" +#include "rclcpp/wait_set_policies/detail/synchronization_policy_common.hpp" +#include "rclcpp/waitable.hpp" + +namespace rclcpp +{ +namespace wait_set_policies +{ + +/// WaitSet policy that explicitly provides no thread synchronization. +class SequentialSynchronization : public detail::SynchronizationPolicyCommon +{ +protected: + explicit SequentialSynchronization(rclcpp::Context::SharedPtr) {} + ~SequentialSynchronization() = default; + + /// Return any "extra" guard conditions needed to implement the synchronization policy. + /** + * Since this policy provides no thread-safety, it also needs no extra guard + * conditions to implement it. + */ + const std::array, 0> & + get_extra_guard_conditions() + { + static const std::array, 0> empty{}; + return empty; + } + + /// Add subscription without thread-safety. + /** + * Does not throw, but storage function may throw. + */ + void + sync_add_subscription( + std::shared_ptr && subscription, + const rclcpp::SubscriptionWaitSetMask & mask, + std::function< + void(std::shared_ptr&&, const rclcpp::SubscriptionWaitSetMask &) + > add_subscription_function) + { + // Explicitly no thread synchronization. + add_subscription_function(std::move(subscription), mask); + } + + /// Remove guard condition without thread-safety. + /** + * Does not throw, but storage function may throw. + */ + void + sync_remove_subscription( + std::shared_ptr && subscription, + const rclcpp::SubscriptionWaitSetMask & mask, + std::function< + void(std::shared_ptr&&, const rclcpp::SubscriptionWaitSetMask &) + > remove_subscription_function) + { + // Explicitly no thread synchronization. + remove_subscription_function(std::move(subscription), mask); + } + + /// Add guard condition without thread-safety. + /** + * Does not throw, but storage function may throw. + */ + void + sync_add_guard_condition( + std::shared_ptr && guard_condition, + std::function&&)> add_guard_condition_function) + { + // Explicitly no thread synchronization. + add_guard_condition_function(std::move(guard_condition)); + } + + /// Remove guard condition without thread-safety. + /** + * Does not throw, but storage function may throw. + */ + void + sync_remove_guard_condition( + std::shared_ptr && guard_condition, + std::function&&)> remove_guard_condition_function) + { + // Explicitly no thread synchronization. + remove_guard_condition_function(std::move(guard_condition)); + } + + /// Add timer without thread-safety. + /** + * Does not throw, but storage function may throw. + */ + void + sync_add_timer( + std::shared_ptr && timer, + std::function&&)> add_timer_function) + { + // Explicitly no thread synchronization. + add_timer_function(std::move(timer)); + } + + /// Remove timer without thread-safety. + /** + * Does not throw, but storage function may throw. + */ + void + sync_remove_timer( + std::shared_ptr && timer, + std::function&&)> remove_timer_function) + { + // Explicitly no thread synchronization. + remove_timer_function(std::move(timer)); + } + + /// Add client without thread-safety. + /** + * Does not throw, but storage function may throw. + */ + void + sync_add_client( + std::shared_ptr && client, + std::function&&)> add_client_function) + { + // Explicitly no thread synchronization. + add_client_function(std::move(client)); + } + + /// Remove client without thread-safety. + /** + * Does not throw, but storage function may throw. + */ + void + sync_remove_client( + std::shared_ptr && client, + std::function&&)> remove_client_function) + { + // Explicitly no thread synchronization. + remove_client_function(std::move(client)); + } + + /// Add service without thread-safety. + /** + * Does not throw, but storage function may throw. + */ + void + sync_add_service( + std::shared_ptr && service, + std::function&&)> add_service_function) + { + // Explicitly no thread synchronization. + add_service_function(std::move(service)); + } + + /// Remove service without thread-safety. + /** + * Does not throw, but storage function may throw. + */ + void + sync_remove_service( + std::shared_ptr && service, + std::function&&)> remove_service_function) + { + // Explicitly no thread synchronization. + remove_service_function(std::move(service)); + } + + /// Add waitable without thread-safety. + /** + * Does not throw, but storage function may throw. + */ + void + sync_add_waitable( + std::shared_ptr && waitable, + std::shared_ptr && associated_entity, + std::function< + void(std::shared_ptr&&, std::shared_ptr &&) + > add_waitable_function) + { + // Explicitly no thread synchronization. + add_waitable_function(std::move(waitable), std::move(associated_entity)); + } + + /// Remove waitable without thread-safety. + /** + * Does not throw, but storage function may throw. + */ + void + sync_remove_waitable( + std::shared_ptr && waitable, + std::function&&)> remove_waitable_function) + { + // Explicitly no thread synchronization. + remove_waitable_function(std::move(waitable)); + } + + /// Prune deleted entities without thread-safety. + /** + * Does not throw, but storage function may throw. + */ + void + sync_prune_deleted_entities(std::function prune_deleted_entities_function) + { + // Explicitly no thread synchronization. + prune_deleted_entities_function(); + } + + /// Implements wait without any thread-safety. + template + WaitResultT + sync_wait( + std::chrono::nanoseconds time_to_wait_ns, + std::function rebuild_rcl_wait_set, + std::function get_rcl_wait_set, + std::function create_wait_result) + { + // Assumption: this function assumes that some measure has been taken to + // ensure none of the entities being waited on by the wait set are allowed + // to go out of scope and therefore be deleted. + // In the case of the StaticStorage policy, this is ensured because it + // retains shared ownership of all entites for the duration of its own life. + // In the case of the DynamicStorage policy, this is ensured by the function + // which calls this function, by acquiring shared ownership of the entites + // for the duration of this function. + + // Setup looping predicate. + auto start = std::chrono::steady_clock::now(); + std::function should_loop = this->create_loop_predicate(time_to_wait_ns, start); + + // Wait until exit condition is met. + do { + // Rebuild the wait set. + // This will resize the wait set if needed, due to e.g. adding or removing + // entities since the last wait, but this should never occur in static + // storage wait sets since they cannot be changed after construction. + // This will also clear the wait set and re-add all the entities, which + // prepares it to be waited on again. + rebuild_rcl_wait_set(); + + rcl_wait_set_t & rcl_wait_set = get_rcl_wait_set(); + + // Wait unconditionally until timeout condition occurs since we assume + // there are no conditions that would require the wait to stop and reset, + // like asynchronously adding or removing an entity, i.e. explicitly + // providing no thread-safety. + + // Calculate how much time there is left to wait, unless blocking indefinitely. + auto time_left_to_wait_ns = this->calculate_time_left_to_wait(time_to_wait_ns, start); + + // Then wait for entities to become ready. + rcl_ret_t ret = rcl_wait(&rcl_wait_set, time_left_to_wait_ns.count()); + if (RCL_RET_OK == ret) { + // Something has become ready in the wait set, and since this class + // did not add anything to it, it is a user entity that is ready. + return create_wait_result(WaitResultKind::Ready); + } else if (RCL_RET_TIMEOUT == ret) { + // The wait set timed out, exit the loop. + break; + } else if (RCL_RET_WAIT_SET_EMPTY == ret) { + // Wait set was empty, return Empty. + return create_wait_result(WaitResultKind::Empty); + } else { + // Some other error case, throw. + rclcpp::exceptions::throw_from_rcl_error(ret, "rcl_wait() failed"); + } + } while (should_loop()); + + // Wait did not result in ready items, return timeout. + return create_wait_result(WaitResultKind::Timeout); + } + + void + sync_wait_result_acquire() + { + // Explicitly do nothing. + } + + void + sync_wait_result_release() + { + // Explicitly do nothing. + } +}; + +} // namespace wait_set_policies +} // namespace rclcpp + +#endif // RCLCPP__WAIT_SET_POLICIES__SEQUENTIAL_SYNCHRONIZATION_HPP_ diff --git a/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp b/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp new file mode 100644 index 0000000000..7f5cad74ad --- /dev/null +++ b/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp @@ -0,0 +1,257 @@ +// 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 RCLCPP__WAIT_SET_POLICIES__STATIC_STORAGE_HPP_ +#define RCLCPP__WAIT_SET_POLICIES__STATIC_STORAGE_HPP_ + +#include +#include +#include + +#include "rclcpp/client.hpp" +#include "rclcpp/guard_condition.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/service.hpp" +#include "rclcpp/subscription_base.hpp" +#include "rclcpp/subscription_wait_set_mask.hpp" +#include "rclcpp/timer.hpp" +#include "rclcpp/visibility_control.hpp" +#include "rclcpp/wait_set_policies/detail/storage_policy_common.hpp" +#include "rclcpp/waitable.hpp" + +namespace rclcpp +{ +namespace wait_set_policies +{ + +/// WaitSet policy that explicitly provides fixed sized storage only. +/** + * Note the underlying rcl_wait_set_t is still dynamically allocated, but only + * once during construction, and deallocated once during destruction. + */ +template< + std::size_t NumberOfSubscriptions, + std::size_t NumberOfGuardCondtions, + std::size_t NumberOfTimers, + std::size_t NumberOfClients, + std::size_t NumberOfServices, + std::size_t NumberOfWaitables +> +class StaticStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCommon +{ +protected: + using is_mutable = std::false_type; + + class SubscriptionEntry + { +public: + std::shared_ptr subscription; + rclcpp::SubscriptionWaitSetMask mask; + + /// Conversion constructor, which is intentionally not marked explicit. + SubscriptionEntry( + std::shared_ptr subscription_in = nullptr, + rclcpp::SubscriptionWaitSetMask mask_in = {}) + : subscription(std::move(subscription_in)), + mask(mask_in) + {} + }; + using ArrayOfSubscriptions = std::array< + SubscriptionEntry, + NumberOfSubscriptions + >; + using SubscriptionsIterable = ArrayOfSubscriptions; + + using ArrayOfGuardConditions = std::array< + std::shared_ptr, + NumberOfGuardCondtions + >; + using GuardConditionsIterable = ArrayOfGuardConditions; + + using ArrayOfTimers = std::array< + std::shared_ptr, + NumberOfTimers + >; + using TimersIterable = ArrayOfTimers; + + using ArrayOfClients = std::array< + std::shared_ptr, + NumberOfClients + >; + using ClientsIterable = ArrayOfClients; + + using ArrayOfServices = std::array< + std::shared_ptr, + NumberOfServices + >; + using ServicesIterable = ArrayOfServices; + + struct WaitableEntry + { + /// Conversion constructor, which is intentionally not marked explicit. + WaitableEntry( + std::shared_ptr waitable_in = nullptr, + std::shared_ptr associated_entity_in = nullptr) noexcept + : waitable(std::move(waitable_in)), + associated_entity(std::move(associated_entity_in)) + {} + + std::shared_ptr waitable; + std::shared_ptr associated_entity; + }; + using ArrayOfWaitables = std::array< + WaitableEntry, + NumberOfWaitables + >; + using WaitablesIterable = ArrayOfWaitables; + + template + explicit + StaticStorage( + const ArrayOfSubscriptions & subscriptions, + const ArrayOfGuardConditions & guard_conditions, + const ArrayOfExtraGuardConditions & extra_guard_conditions, + const ArrayOfTimers & timers, + const ArrayOfClients & clients, + const ArrayOfServices & services, + const ArrayOfWaitables & waitables, + rclcpp::Context::SharedPtr context + ) + : StoragePolicyCommon( + subscriptions, + guard_conditions, + extra_guard_conditions, + timers, + clients, + services, + waitables, + context), + subscriptions_(subscriptions), + guard_conditions_(guard_conditions), + timers_(timers), + clients_(clients), + services_(services), + waitables_(waitables) + {} + + ~StaticStorage() = default; + + template + void + storage_rebuild_rcl_wait_set(const ArrayOfExtraGuardConditions & extra_guard_conditions) + { + this->storage_rebuild_rcl_wait_set_with_sets( + subscriptions_, + guard_conditions_, + extra_guard_conditions, + timers_, + clients_, + services_, + waitables_ + ); + } + + // storage_add_subscription() explicitly not declared here + // storage_remove_subscription() explicitly not declared here + // storage_add_guard_condition() explicitly not declared here + // storage_remove_guard_condition() explicitly not declared here + // storage_add_timer() explicitly not declared here + // storage_remove_timer() explicitly not declared here + // storage_add_client() explicitly not declared here + // storage_remove_client() explicitly not declared here + // storage_add_service() explicitly not declared here + // storage_remove_service() explicitly not declared here + // storage_add_waitable() explicitly not declared here + // storage_remove_waitable() explicitly not declared here + // storage_prune_deleted_entities() explicitly not declared here + + void + storage_acquire_ownerships() + { + // Explicitly do nothing. + } + + void + storage_release_ownerships() + { + // Explicitly do nothing. + } + + size_t size_of_subscriptions() const + { + return subscriptions_.size(); + } + + size_t size_of_timers() const + { + return timers_.size(); + } + + size_t size_of_clients() const + { + return clients_.size(); + } + + size_t size_of_services() const + { + return services_.size(); + } + + size_t size_of_waitables() const + { + return waitables_.size(); + } + + typename ArrayOfSubscriptions::value_type + subscriptions(size_t ii) const + { + return subscriptions_[ii]; + } + + typename ArrayOfTimers::value_type + timers(size_t ii) const + { + return timers_[ii]; + } + + typename ArrayOfClients::value_type + clients(size_t ii) const + { + return clients_[ii]; + } + + typename ArrayOfServices::value_type + services(size_t ii) const + { + return services_[ii]; + } + + typename ArrayOfWaitables::value_type + waitables(size_t ii) const + { + return waitables_[ii]; + } + + const ArrayOfSubscriptions subscriptions_; + const ArrayOfGuardConditions guard_conditions_; + const ArrayOfTimers timers_; + const ArrayOfClients clients_; + const ArrayOfServices services_; + const ArrayOfWaitables waitables_; +}; + +} // namespace wait_set_policies +} // namespace rclcpp + +#endif // RCLCPP__WAIT_SET_POLICIES__STATIC_STORAGE_HPP_ diff --git a/rclcpp/include/rclcpp/wait_set_policies/thread_safe_synchronization.hpp b/rclcpp/include/rclcpp/wait_set_policies/thread_safe_synchronization.hpp new file mode 100644 index 0000000000..4a4fb16547 --- /dev/null +++ b/rclcpp/include/rclcpp/wait_set_policies/thread_safe_synchronization.hpp @@ -0,0 +1,379 @@ +// 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 RCLCPP__WAIT_SET_POLICIES__THREAD_SAFE_SYNCHRONIZATION_HPP_ +#define RCLCPP__WAIT_SET_POLICIES__THREAD_SAFE_SYNCHRONIZATION_HPP_ + +#include +#include +#include +#include + +#include "rclcpp/client.hpp" +#include "rclcpp/exceptions.hpp" +#include "rclcpp/guard_condition.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/service.hpp" +#include "rclcpp/subscription_base.hpp" +#include "rclcpp/subscription_wait_set_mask.hpp" +#include "rclcpp/timer.hpp" +#include "rclcpp/visibility_control.hpp" +#include "rclcpp/wait_result.hpp" +#include "rclcpp/wait_result_kind.hpp" +#include "rclcpp/wait_set_policies/detail/synchronization_policy_common.hpp" +#include "rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.hpp" +#include "rclcpp/waitable.hpp" + +namespace rclcpp +{ +namespace wait_set_policies +{ + +/// WaitSet policy that provides thread-safe synchronization for the wait set. +/** + * This class uses a "write-preferring RW lock" so that adding items to, and + * removing items from, the wait set will take priority over reading, i.e. + * waiting. + * This is done since add and remove calls will interrupt the wait set anyways + * so it is wasteful to do "fair" locking when there are many add/remove + * operations queued up. + * + * There are some things to consider about the thread-safety provided by this + * policy. + * There are two categories of activities, reading and writing activities. + * The writing activities include all of the add and remove methods, as well as + * the prune_deleted_entities() method. + * The reading methods include the wait() method and keeping a WaitResult in + * scope. + * The reading and writing activities will not be run at the same time, and one + * will block the other. + * Therefore, if you are holding a WaitResult in scope, and try to add or + * remove an entity at the same time, they will block each other. + * The write activities will try to interrupt the wait() method by triggering + * a guard condition, but they have no way of causing the WaitResult to release + * its lock. + */ +class ThreadSafeSynchronization : public detail::SynchronizationPolicyCommon +{ +protected: + explicit ThreadSafeSynchronization(rclcpp::Context::SharedPtr context) + : extra_guard_conditions_{{std::make_shared(context)}}, + wprw_lock_([this]() {this->interrupt_waiting_wait_set();}) + {} + ~ThreadSafeSynchronization() = default; + + /// Return any "extra" guard conditions needed to implement the synchronization policy. + /** + * This policy has one guard condition which is used to interrupt the wait + * set when adding and removing entities. + */ + const std::array, 1> & + get_extra_guard_conditions() + { + return extra_guard_conditions_; + } + + /// Interrupt any waiting wait set. + /** + * Used to interrupt the wait set when adding or removing items. + */ + void + interrupt_waiting_wait_set() + { + extra_guard_conditions_[0]->trigger(); + } + + /// Add subscription. + void + sync_add_subscription( + std::shared_ptr && subscription, + const rclcpp::SubscriptionWaitSetMask & mask, + std::function< + void(std::shared_ptr&&, const rclcpp::SubscriptionWaitSetMask &) + > add_subscription_function) + { + using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock; + std::lock_guard lock(wprw_lock_.get_write_mutex()); + add_subscription_function(std::move(subscription), mask); + } + + /// Remove guard condition. + void + sync_remove_subscription( + std::shared_ptr && subscription, + const rclcpp::SubscriptionWaitSetMask & mask, + std::function< + void(std::shared_ptr&&, const rclcpp::SubscriptionWaitSetMask &) + > remove_subscription_function) + { + using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock; + std::lock_guard lock(wprw_lock_.get_write_mutex()); + remove_subscription_function(std::move(subscription), mask); + } + + /// Add guard condition. + void + sync_add_guard_condition( + std::shared_ptr && guard_condition, + std::function&&)> add_guard_condition_function) + { + using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock; + std::lock_guard lock(wprw_lock_.get_write_mutex()); + add_guard_condition_function(std::move(guard_condition)); + } + + /// Remove guard condition. + void + sync_remove_guard_condition( + std::shared_ptr && guard_condition, + std::function&&)> remove_guard_condition_function) + { + using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock; + std::lock_guard lock(wprw_lock_.get_write_mutex()); + remove_guard_condition_function(std::move(guard_condition)); + } + + /// Add timer. + void + sync_add_timer( + std::shared_ptr && timer, + std::function&&)> add_timer_function) + { + using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock; + std::lock_guard lock(wprw_lock_.get_write_mutex()); + add_timer_function(std::move(timer)); + } + + /// Remove timer. + void + sync_remove_timer( + std::shared_ptr && timer, + std::function&&)> remove_timer_function) + { + using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock; + std::lock_guard lock(wprw_lock_.get_write_mutex()); + remove_timer_function(std::move(timer)); + } + + /// Add client. + void + sync_add_client( + std::shared_ptr && client, + std::function&&)> add_client_function) + { + using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock; + std::lock_guard lock(wprw_lock_.get_write_mutex()); + add_client_function(std::move(client)); + } + + /// Remove client. + void + sync_remove_client( + std::shared_ptr && client, + std::function&&)> remove_client_function) + { + using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock; + std::lock_guard lock(wprw_lock_.get_write_mutex()); + remove_client_function(std::move(client)); + } + + /// Add service. + void + sync_add_service( + std::shared_ptr && service, + std::function&&)> add_service_function) + { + using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock; + std::lock_guard lock(wprw_lock_.get_write_mutex()); + add_service_function(std::move(service)); + } + + /// Remove service. + void + sync_remove_service( + std::shared_ptr && service, + std::function&&)> remove_service_function) + { + using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock; + std::lock_guard lock(wprw_lock_.get_write_mutex()); + remove_service_function(std::move(service)); + } + + /// Add waitable. + void + sync_add_waitable( + std::shared_ptr && waitable, + std::shared_ptr && associated_entity, + std::function< + void(std::shared_ptr&&, std::shared_ptr &&) + > add_waitable_function) + { + using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock; + std::lock_guard lock(wprw_lock_.get_write_mutex()); + add_waitable_function(std::move(waitable), std::move(associated_entity)); + } + + /// Remove waitable. + void + sync_remove_waitable( + std::shared_ptr && waitable, + std::function&&)> remove_waitable_function) + { + using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock; + std::lock_guard lock(wprw_lock_.get_write_mutex()); + remove_waitable_function(std::move(waitable)); + } + + /// Prune deleted entities. + void + sync_prune_deleted_entities(std::function prune_deleted_entities_function) + { + using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock; + std::lock_guard lock(wprw_lock_.get_write_mutex()); + prune_deleted_entities_function(); + } + + /// Implements wait. + template + WaitResultT + sync_wait( + std::chrono::nanoseconds time_to_wait_ns, + std::function rebuild_rcl_wait_set, + std::function get_rcl_wait_set, + std::function create_wait_result) + { + // Assumption: this function assumes that some measure has been taken to + // ensure none of the entities being waited on by the wait set are allowed + // to go out of scope and therefore be deleted. + // In the case of the StaticStorage policy, this is ensured because it + // retains shared ownership of all entites for the duration of its own life. + // In the case of the DynamicStorage policy, this is ensured by the function + // which calls this function, by acquiring shared ownership of the entites + // for the duration of this function. + + // Setup looping predicate. + auto start = std::chrono::steady_clock::now(); + std::function should_loop = this->create_loop_predicate(time_to_wait_ns, start); + + // Wait until exit condition is met. + do { + { + // We have to prevent the entity sets from being mutated while building + // the rcl wait set. + using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock; + std::lock_guard lock(wprw_lock_.get_read_mutex()); + // Rebuild the wait set. + // This will resize the wait set if needed, due to e.g. adding or removing + // entities since the last wait, but this should never occur in static + // storage wait sets since they cannot be changed after construction. + // This will also clear the wait set and re-add all the entities, which + // prepares it to be waited on again. + rebuild_rcl_wait_set(); + } + + rcl_wait_set_t & rcl_wait_set = get_rcl_wait_set(); + + // Wait unconditionally until timeout condition occurs since we assume + // there are no conditions that would require the wait to stop and reset, + // like asynchronously adding or removing an entity, i.e. explicitly + // providing no thread-safety. + + // Calculate how much time there is left to wait, unless blocking indefinitely. + auto time_left_to_wait_ns = this->calculate_time_left_to_wait(time_to_wait_ns, start); + + // Then wait for entities to become ready. + + // It is ok to wait while not having the lock acquired, because the state + // in the rcl wait set will not be updated until this method calls + // rebuild_rcl_wait_set(). + rcl_ret_t ret = rcl_wait(&rcl_wait_set, time_left_to_wait_ns.count()); + if (RCL_RET_OK == ret) { + // Something has become ready in the wait set, first check if it was + // the guard condition added by this class and/or a user defined guard condition. + const rcl_guard_condition_t * interrupt_guard_condition_ptr = + &(extra_guard_conditions_[0]->get_rcl_guard_condition()); + bool was_interrupted_by_this_class = false; + bool any_user_guard_conditions_triggered = false; + for (size_t index = 0; index < rcl_wait_set.size_of_guard_conditions; ++index) { + const rcl_guard_condition_t * current = rcl_wait_set.guard_conditions[index]; + if (nullptr != current) { + // Something is ready. + if (rcl_wait_set.guard_conditions[index] == interrupt_guard_condition_ptr) { + // This means that this class triggered a guard condition to interrupt this wait. + was_interrupted_by_this_class = true; + } else { + // This means it was a user guard condition. + any_user_guard_conditions_triggered = true; + } + } + } + + if (!was_interrupted_by_this_class || any_user_guard_conditions_triggered) { + // In this case we know: + // - something was ready + // - it was either: + // - not interrupted by this class, or + // - maybe it was, but there were also user defined guard conditions. + // + // We cannot ignore user defined guard conditions, but we can ignore + // other kinds of user defined entities, because they will still be + // ready next time we wait, whereas guard conditions are cleared. + // Therefore we need to create a WaitResult and return it. + + // The WaitResult will call sync_wait_result_acquire() and + // sync_wait_result_release() to ensure thread-safety by preventing + // the mutation of the entity sets while introspecting after waiting. + return create_wait_result(WaitResultKind::Ready); + } + // If we get here the we interrupted the wait set and there were no user + // guard conditions that needed to be handled. + // So we will loop and it will re-acquire the lock and rebuild the + // rcl wait set. + } else if (RCL_RET_TIMEOUT == ret) { + // The wait set timed out, exit the loop. + break; + } else if (RCL_RET_WAIT_SET_EMPTY == ret) { + // Wait set was empty, return Empty. + return create_wait_result(WaitResultKind::Empty); + } else { + // Some other error case, throw. + rclcpp::exceptions::throw_from_rcl_error(ret); + } + } while (should_loop()); + + // Wait did not result in ready items, return timeout. + return create_wait_result(WaitResultKind::Timeout); + } + + void + sync_wait_result_acquire() + { + wprw_lock_.get_read_mutex().lock(); + } + + void + sync_wait_result_release() + { + wprw_lock_.get_read_mutex().unlock(); + } + +protected: + std::array, 1> extra_guard_conditions_; + rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock wprw_lock_; +}; + +} // namespace wait_set_policies +} // namespace rclcpp + +#endif // RCLCPP__WAIT_SET_POLICIES__THREAD_SAFE_SYNCHRONIZATION_HPP_ diff --git a/rclcpp/include/rclcpp/wait_set_template.hpp b/rclcpp/include/rclcpp/wait_set_template.hpp new file mode 100644 index 0000000000..ce69da6bf2 --- /dev/null +++ b/rclcpp/include/rclcpp/wait_set_template.hpp @@ -0,0 +1,767 @@ +// 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 RCLCPP__WAIT_SET_TEMPLATE_HPP_ +#define RCLCPP__WAIT_SET_TEMPLATE_HPP_ + +#include +#include +#include + +#include "rcl/wait.h" +#include "rcpputils/scope_exit.hpp" + +#include "rclcpp/client.hpp" +#include "rclcpp/context.hpp" +#include "rclcpp/contexts/default_context.hpp" +#include "rclcpp/guard_condition.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/service.hpp" +#include "rclcpp/subscription_base.hpp" +#include "rclcpp/subscription_wait_set_mask.hpp" +#include "rclcpp/timer.hpp" +#include "rclcpp/visibility_control.hpp" +#include "rclcpp/wait_result.hpp" +#include "rclcpp/waitable.hpp" + +namespace rclcpp +{ + +/// Encapsulates sets of waitable items which can be waited on as a group. +/** + * This class uses the rcl_wait_set_t as storage, but it also helps manage the + * ownership of associated rclcpp types. + */ +template +class WaitSetTemplate final : private SynchronizationPolicy, private StoragePolicy +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(WaitSetTemplate) + + using typename StoragePolicy::SubscriptionEntry; + using typename StoragePolicy::WaitableEntry; + + /// Construct a wait set with optional initial waitable entities and optional custom context. + /** + * For the waitables, they have additionally an "associated" entity, which + * you can read more about in the add and remove functions for those types + * in this class. + * + * \param[in] subscriptions Vector of subscriptions to be added. + * \param[in] guard_conditions Vector of guard conditions to be added. + * \param[in] timers Vector of timers to be added. + * \param[in] clients Vector of clients and their associated entity to be added. + * \param[in] services Vector of services and their associated entity to be added. + * \param[in] waitables Vector of waitables and their associated entity to be added. + * \param[in] context Custom context to be used, defaults to global default. + * \throws std::invalid_argument If context is nullptr. + */ + explicit + WaitSetTemplate( + const typename StoragePolicy::SubscriptionsIterable & subscriptions = {}, + const typename StoragePolicy::GuardConditionsIterable & guard_conditions = {}, + const typename StoragePolicy::TimersIterable & timers = {}, + const typename StoragePolicy::ClientsIterable & clients = {}, + const typename StoragePolicy::ServicesIterable & services = {}, + const typename StoragePolicy::WaitablesIterable & waitables = {}, + rclcpp::Context::SharedPtr context = + rclcpp::contexts::get_global_default_context()) + : SynchronizationPolicy(context), + StoragePolicy( + subscriptions, + guard_conditions, + // this method comes from the SynchronizationPolicy + this->get_extra_guard_conditions(), + timers, + clients, + services, + waitables, + context) + {} + + /// Return the internal rcl wait set object. + /** + * This method provides no thread-safety when accessing this structure. + * The state of this structure can be updated at anytime by methods like + * wait(), add_*(), remove_*(), etc. + */ + const rcl_wait_set_t & + get_rcl_wait_set() const + { + // this method comes from the StoragePolicy + return this->storage_get_rcl_wait_set(); + } + + /// Add a subscription to this wait set. + /** + * \sa add_guard_condition() for details of how this method works. + * + * Additionally to the documentation for add_guard_condition, this method + * has a mask parameter which allows you to control which parts of the + * subscription is added to the wait set with this call. + * For example, you might want to include the actual subscription to this + * wait set, but add the intra-process waitable to another wait set. + * If intra-process is disabled, no error will occur, it will just be skipped. + * + * When introspecting after waiting, this subscription's shared pointer will + * be the Waitable's (intra-process or the QoS Events) "associated entity" + * pointer, for more easily figuring out which subscription which waitable + * goes with afterwards. + * + * \param[in] subscription Subscription to be added. + * \param[in] mask A class which controls which parts of the subscription to add. + * \throws std::invalid_argument if subscription is nullptr. + * \throws std::runtime_error if subscription has already been added or is + * associated with another wait set. + * \throws exceptions based on the policies used. + */ + void + add_subscription( + std::shared_ptr subscription, + rclcpp::SubscriptionWaitSetMask mask = {}) + { + if (nullptr == subscription) { + throw std::invalid_argument("subscription is nullptr"); + } + // this method comes from the SynchronizationPolicy + this->sync_add_subscription( + std::move(subscription), + mask, + [this]( + std::shared_ptr && inner_subscription, + const rclcpp::SubscriptionWaitSetMask & mask) + { + // These methods comes from the StoragePolicy, and may not exist for + // fixed sized storage policies. + // It will throw if the subscription has already been added. + if (mask.include_subscription) { + auto local_subscription = inner_subscription; + bool already_in_use = + local_subscription->exchange_in_use_by_wait_set_state(local_subscription.get(), true); + if (already_in_use) { + throw std::runtime_error("subscription already associated with a wait set"); + } + this->storage_add_subscription(std::move(local_subscription)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} + } + if (mask.include_events) { + for (auto key_event_pair : inner_subscription->get_event_handlers()) { + auto event = key_event_pair.second; + auto local_subscription = inner_subscription; + bool already_in_use = + local_subscription->exchange_in_use_by_wait_set_state(event.get(), true); + if (already_in_use) { + throw std::runtime_error("subscription event already associated with a wait set"); + } + this->storage_add_waitable(std::move(event), std::move(local_subscription)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} + } + } + if (mask.include_intra_process_waitable) { + auto local_subscription = inner_subscription; + auto waitable = inner_subscription->get_intra_process_waitable(); + if (nullptr != waitable) { + bool already_in_use = local_subscription->exchange_in_use_by_wait_set_state( + waitable.get(), + true); + if (already_in_use) { + throw std::runtime_error( + "subscription intra-process waitable already associated with a wait set"); + } + this->storage_add_waitable( + std::move(inner_subscription->get_intra_process_waitable()), + std::move(local_subscription)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} + } + } + }); + } + + /// Remove a subscription from this wait set. + /** + * \sa remove_guard_condition() for details of how this method works. + * + * Additionally to the documentation for add_guard_condition, this method + * has a mask parameter which allows you to control which parts of the + * subscription is added to the wait set with this call. + * You may remove items selectively from the wait set in a different order + * than they were added. + * + * \param[in] subscription Subscription to be removed. + * \param[in] mask A class which controls which parts of the subscription to remove. + * \throws std::invalid_argument if subscription is nullptr. + * \throws std::runtime_error if subscription is not part of the wait set. + * \throws exceptions based on the policies used. + */ + void + remove_subscription( + std::shared_ptr subscription, + rclcpp::SubscriptionWaitSetMask mask = {}) + { + if (nullptr == subscription) { + throw std::invalid_argument("subscription is nullptr"); + } + // this method comes from the SynchronizationPolicy + this->sync_remove_subscription( + std::move(subscription), + mask, + [this]( + std::shared_ptr && inner_subscription, + const rclcpp::SubscriptionWaitSetMask & mask) + { + // This method comes from the StoragePolicy, and it may not exist for + // fixed sized storage policies. + // It will throw if the subscription is not in the wait set. + if (mask.include_subscription) { + auto local_subscription = inner_subscription; + local_subscription->exchange_in_use_by_wait_set_state(local_subscription.get(), false); + this->storage_remove_subscription(std::move(local_subscription)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} + } + if (mask.include_events) { + for (auto key_event_pair : inner_subscription->get_event_handlers()) { + auto event = key_event_pair.second; + auto local_subscription = inner_subscription; + local_subscription->exchange_in_use_by_wait_set_state(event.get(), false); + this->storage_remove_waitable(std::move(event)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} + } + } + if (mask.include_intra_process_waitable) { + auto local_waitable = inner_subscription->get_intra_process_waitable(); + if (nullptr != local_waitable) { + // This is the case when intra process is enabled for the subscription. + inner_subscription->exchange_in_use_by_wait_set_state(local_waitable.get(), false); + this->storage_remove_waitable(std::move(local_waitable)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} + } + } + }); + } + + /// Add a guard condition to this wait set. + /** + * Guard condition is added to the wait set, and shared ownership is held + * while waiting. + * However, if between calls to wait() the guard condition's reference count + * goes to zero, it will be implicitly removed on the next call to wait(). + * + * Except in the case of a fixed sized storage, where changes to the wait set + * cannot occur after construction, in which case it holds shared ownership + * at all times until the wait set is destroy, but this method also does not + * exist on a fixed sized wait set. + * + * This function may be thread-safe depending on the SynchronizationPolicy + * used with this class. + * Using the ThreadSafeWaitSetPolicy will ensure that wait() is interrupted + * and returns before this function adds the guard condition. + * Otherwise, it is not safe to call this function concurrently with wait(). + * + * This function will not be enabled (will not be available) if the + * StoragePolicy does not allow editing of the wait set after initialization. + * + * \param[in] guard_condition Guard condition to be added. + * \throws std::invalid_argument if guard_condition is nullptr. + * \throws std::runtime_error if guard_condition has already been added or is + * associated with another wait set. + * \throws exceptions based on the policies used. + */ + void + add_guard_condition(std::shared_ptr guard_condition) + { + if (nullptr == guard_condition) { + throw std::invalid_argument("guard_condition is nullptr"); + } + // this method comes from the SynchronizationPolicy + this->sync_add_guard_condition( + std::move(guard_condition), + [this](std::shared_ptr && inner_guard_condition) { + bool already_in_use = inner_guard_condition->exchange_in_use_by_wait_set_state(true); + if (already_in_use) { + throw std::runtime_error("guard condition already in use by another wait set"); + } + // This method comes from the StoragePolicy, and it may not exist for + // fixed sized storage policies. + // It will throw if the guard condition has already been added. + this->storage_add_guard_condition(std::move(inner_guard_condition)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} + }); + } + + /// Remove a guard condition from this wait set. + /** + * Guard condition is removed from the wait set, and if needed the shared + * ownership is released. + * + * This function may be thread-safe depending on the SynchronizationPolicy + * used with this class. + * Using the ThreadSafeWaitSetPolicy will ensure that wait() is interrupted + * and returns before this function removes the guard condition. + * Otherwise, it is not safe to call this function concurrently with wait(). + * + * This function will not be enabled (will not be available) if the + * StoragePolicy does not allow editing of the wait set after initialization. + * + * \param[in] guard_condition Guard condition to be removed. + * \throws std::invalid_argument if guard_condition is nullptr. + * \throws std::runtime_error if guard_condition is not part of the wait set. + * \throws exceptions based on the policies used. + */ + void + remove_guard_condition(std::shared_ptr guard_condition) + { + if (nullptr == guard_condition) { + throw std::invalid_argument("guard_condition is nullptr"); + } + // this method comes from the SynchronizationPolicy + this->sync_remove_guard_condition( + std::move(guard_condition), + [this](std::shared_ptr && inner_guard_condition) { + inner_guard_condition->exchange_in_use_by_wait_set_state(false); + // This method comes from the StoragePolicy, and it may not exist for + // fixed sized storage policies. + // It will throw if the guard condition is not in the wait set. + this->storage_remove_guard_condition(std::move(inner_guard_condition)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} + }); + } + + /// Add a timer to this wait set. + /** + * \sa add_guard_condition() for details of how this method works. + * + * \param[in] timer Timer to be added. + * \throws std::invalid_argument if timer is nullptr. + * \throws std::runtime_error if timer has already been added or is + * associated with another wait set. + * \throws exceptions based on the policies used. + */ + void + add_timer(std::shared_ptr timer) + { + if (nullptr == timer) { + throw std::invalid_argument("timer is nullptr"); + } + // this method comes from the SynchronizationPolicy + this->sync_add_timer( + std::move(timer), + [this](std::shared_ptr && inner_timer) { + bool already_in_use = inner_timer->exchange_in_use_by_wait_set_state(true); + if (already_in_use) { + throw std::runtime_error("timer already in use by another wait set"); + } + // This method comes from the StoragePolicy, and it may not exist for + // fixed sized storage policies. + // It will throw if the timer has already been added. + this->storage_add_timer(std::move(inner_timer)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} + }); + } + + /// Remove a timer from this wait set. + /** + * \sa remove_guard_condition() for details of how this method works. + * + * \param[in] timer Timer to be removed. + * \throws std::invalid_argument if timer is nullptr. + * \throws std::runtime_error if timer is not part of the wait set. + * \throws exceptions based on the policies used. + */ + void + remove_timer(std::shared_ptr timer) + { + if (nullptr == timer) { + throw std::invalid_argument("timer is nullptr"); + } + // this method comes from the SynchronizationPolicy + this->sync_remove_timer( + std::move(timer), + [this](std::shared_ptr && inner_timer) { + inner_timer->exchange_in_use_by_wait_set_state(false); + // This method comes from the StoragePolicy, and it may not exist for + // fixed sized storage policies. + // It will throw if the timer is not in the wait set. + this->storage_remove_timer(std::move(inner_timer)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} + }); + } + + /// Add a client to this wait set. + /** + * \sa add_guard_condition() for details of how this method works. + * + * \param[in] client Client to be added. + * \throws std::invalid_argument if client is nullptr. + * \throws std::runtime_error if client has already been added or is + * associated with another wait set. + * \throws exceptions based on the policies used. + */ + void + add_client(std::shared_ptr client) + { + if (nullptr == client) { + throw std::invalid_argument("client is nullptr"); + } + // this method comes from the SynchronizationPolicy + this->sync_add_client( + std::move(client), + [this](std::shared_ptr && inner_client) { + bool already_in_use = inner_client->exchange_in_use_by_wait_set_state(true); + if (already_in_use) { + throw std::runtime_error("client already in use by another wait set"); + } + // This method comes from the StoragePolicy, and it may not exist for + // fixed sized storage policies. + // It will throw if the client has already been added. + this->storage_add_client(std::move(inner_client)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} + }); + } + + /// Remove a client from this wait set. + /** + * \sa remove_guard_condition() for details of how this method works. + * + * \param[in] client Client to be removed. + * \throws std::invalid_argument if client is nullptr. + * \throws std::runtime_error if client is not part of the wait set. + * \throws exceptions based on the policies used. + */ + void + remove_client(std::shared_ptr client) + { + if (nullptr == client) { + throw std::invalid_argument("client is nullptr"); + } + // this method comes from the SynchronizationPolicy + this->sync_remove_client( + std::move(client), + [this](std::shared_ptr && inner_client) { + inner_client->exchange_in_use_by_wait_set_state(false); + // This method comes from the StoragePolicy, and it may not exist for + // fixed sized storage policies. + // It will throw if the client is not in the wait set. + this->storage_remove_client(std::move(inner_client)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} + }); + } + + /// Add a service to this wait set. + /** + * \sa add_guard_condition() for details of how this method works. + * + * \param[in] service Service to be added. + * \throws std::invalid_argument if service is nullptr. + * \throws std::runtime_error if service has already been added or is + * associated with another wait set. + * \throws exceptions based on the policies used. + */ + void + add_service(std::shared_ptr service) + { + if (nullptr == service) { + throw std::invalid_argument("service is nullptr"); + } + // this method comes from the SynchronizationPolicy + this->sync_add_service( + std::move(service), + [this](std::shared_ptr && inner_service) { + bool already_in_use = inner_service->exchange_in_use_by_wait_set_state(true); + if (already_in_use) { + throw std::runtime_error("service already in use by another wait set"); + } + // This method comes from the StoragePolicy, and it may not exist for + // fixed sized storage policies. + // It will throw if the service has already been added. + this->storage_add_service(std::move(inner_service)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} + }); + } + + /// Remove a service from this wait set. + /** + * \sa remove_guard_condition() for details of how this method works. + * + * \param[in] service Service to be removed. + * \throws std::invalid_argument if service is nullptr. + * \throws std::runtime_error if service is not part of the wait set. + * \throws exceptions based on the policies used. + */ + void + remove_service(std::shared_ptr service) + { + if (nullptr == service) { + throw std::invalid_argument("service is nullptr"); + } + // this method comes from the SynchronizationPolicy + this->sync_remove_service( + std::move(service), + [this](std::shared_ptr && inner_service) { + inner_service->exchange_in_use_by_wait_set_state(false); + // This method comes from the StoragePolicy, and it may not exist for + // fixed sized storage policies. + // It will throw if the service is not in the wait set. + this->storage_remove_service(std::move(inner_service)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} + }); + } + + /// Add a waitable to this wait set. + /** + * \sa add_guard_condition() for details of how this method works. + * + * Additionally, this function has an optional parameter which can be used to + * more quickly associate this waitable with an entity when it is ready, and + * so that the ownership maybe held in order to keep the waitable's parent in + * scope while waiting. + * If it is set to nullptr it will be ignored. + * The destruction of the associated entity's shared pointer will not cause + * the waitable to be removed, but it will cause the associated entity pointer + * to be nullptr when introspecting this waitable after waiting. + * + * Note that rclcpp::EventHandlerBase is just a special case of + * rclcpp::Waitable and can be added with this function. + * + * \param[in] waitable Waitable to be added. + * \param[in] associated_entity Type erased shared pointer associated with the waitable. + * This may be nullptr. + * \throws std::invalid_argument if waitable is nullptr. + * \throws std::runtime_error if waitable has already been added or is + * associated with another wait set. + * \throws exceptions based on the policies used. + */ + void + add_waitable( + std::shared_ptr waitable, + std::shared_ptr associated_entity = nullptr) + { + if (nullptr == waitable) { + throw std::invalid_argument("waitable is nullptr"); + } + // this method comes from the SynchronizationPolicy + this->sync_add_waitable( + std::move(waitable), + std::move(associated_entity), + [this]( + std::shared_ptr && inner_waitable, + std::shared_ptr && associated_entity) + { + bool already_in_use = inner_waitable->exchange_in_use_by_wait_set_state(true); + if (already_in_use) { + throw std::runtime_error("waitable already in use by another wait set"); + } + // This method comes from the StoragePolicy, and it may not exist for + // fixed sized storage policies. + // It will throw if the waitable has already been added. + this->storage_add_waitable(std::move(inner_waitable), std::move(associated_entity)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} + }); + } + + /// Remove a waitable from this wait set. + /** + * \sa remove_guard_condition() for details of how this method works. + * + * \param[in] waitable Waitable to be removed. + * \throws std::invalid_argument if waitable is nullptr. + * \throws std::runtime_error if waitable is not part of the wait set. + * \throws exceptions based on the policies used. + */ + void + remove_waitable(std::shared_ptr waitable) + { + if (nullptr == waitable) { + throw std::invalid_argument("waitable is nullptr"); + } + // this method comes from the SynchronizationPolicy + this->sync_remove_waitable( + std::move(waitable), + [this](std::shared_ptr && inner_waitable) { + inner_waitable->exchange_in_use_by_wait_set_state(false); + // This method comes from the StoragePolicy, and it may not exist for + // fixed sized storage policies. + // It will throw if the waitable is not in the wait set. + this->storage_remove_waitable(std::move(inner_waitable)); + if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} + }); + } + + /// Remove any destroyed entities from the wait set. + /** + * When the storage policy does not maintain shared ownership for the life + * of the wait set, e.g. the DynamicStorage policy, it is possible for an + * entity to go out of scope and be deleted without this wait set noticing. + * Therefore there are weak references in this wait set which need to be + * periodically cleared. + * This function performs that clean up. + * + * Since this involves removing entities from the wait set, and is only + * needed if the wait set does not keep ownership of the added entities, the + * storage policies which are static will not need this function and therefore + * do not provide this function. + * + * \throws exceptions based on the policies used. + */ + void + prune_deleted_entities() + { + // this method comes from the SynchronizationPolicy + this->sync_prune_deleted_entities( + [this]() { + // This method comes from the StoragePolicy, and it may not exist for + // fixed sized storage policies. + this->storage_prune_deleted_entities(); + }); + } + + /// Wait for any of the entities in the wait set to be ready, or a period of time to pass. + /** + * This function will return when either one of the entities within this wait + * set is ready, or a period of time has passed, which ever is first. + * The term "ready" means different things for different entities, but + * generally it means some condition is met asynchronously for which this + * function waits. + * + * This function can either wait for a period of time, do no waiting + * (non-blocking), or wait indefinitely, all based on the value of the + * time_to_wait parameter. + * Waiting is always measured against the std::chrono::steady_clock. + * If waiting indefinitely, the Timeout result is not possible. + * There is no "cancel wait" function on this class, but if you want to wait + * indefinitely but have a way to asynchronously interrupt this method, then + * you can use a dedicated rclcpp::GuardCondition for that purpose. + * + * This function will modify the internal rcl_wait_set_t, so introspecting + * the wait set during a call to wait is never safe. + * You should always wait, then introspect, and then, only when done + * introspecting, wait again. + * + * It may be thread-safe to add and remove entities to the wait set + * concurrently with this function, depending on the SynchronizationPolicy + * that is used. + * With the rclcpp::wait_set_policies::ThreadSafeSynchronization policy this + * function will stop waiting to allow add or remove of an entity, and then + * resume waiting, so long as the timeout has not been reached. + * + * \param[in] time_to_wait If > 0, time to wait for entities to be ready, + * if == 0, check if anything is ready without blocking, or + * if < 0, wait indefinitely until one of the items is ready. + * Default is -1, so wait indefinitely. + * \returns Ready when one of the entities is ready, or + * \returns Timeout when the given time to wait is exceeded, not possible + * when time_to_wait is < 0, or + * \returns Empty if the wait set is empty, avoiding the possibility of + * waiting indefinitely on an empty wait set. + * \throws rclcpp::exceptions::RCLError on unhandled rcl errors or, + * \throws std::runtime_error if unknown WaitResultKind + */ + template + RCUTILS_WARN_UNUSED + WaitResult + wait(std::chrono::duration time_to_wait = std::chrono::duration(-1)) + { + auto time_to_wait_ns = std::chrono::duration_cast(time_to_wait); + + // ensure the ownership of the entities in the wait set is shared for the duration of wait + this->storage_acquire_ownerships(); + RCPPUTILS_SCOPE_EXIT({this->storage_release_ownerships();}); + + // this method comes from the SynchronizationPolicy + return this->template sync_wait>( + // pass along the time_to_wait duration as nanoseconds + time_to_wait_ns, + // this method provides the ability to rebuild the wait set, if needed + [this]() { + // This method comes from the StoragePolicy + this->storage_rebuild_rcl_wait_set( + // This method comes from the SynchronizationPolicy + this->get_extra_guard_conditions() + ); + }, + // this method provides access to the rcl wait set + [this]() -> rcl_wait_set_t & { + // This method comes from the StoragePolicy + return this->storage_get_rcl_wait_set(); + }, + // this method provides a way to create the WaitResult + [this](WaitResultKind wait_result_kind) -> WaitResult { + // convert the result into a WaitResult + switch (wait_result_kind) { + case WaitResultKind::Ready: + return WaitResult::from_ready_wait_result_kind(*this); + case WaitResultKind::Timeout: + return WaitResult::from_timeout_wait_result_kind(); + case WaitResultKind::Empty: + return WaitResult::from_empty_wait_result_kind(); + default: + auto msg = "unknown WaitResultKind with value: " + std::to_string(wait_result_kind); + throw std::runtime_error(msg); + } + } + ); + } + +private: + // Add WaitResult type as a friend so it can call private methods for + // acquiring and releasing resources as the WaitResult is initialized and + // destructed, respectively. + friend WaitResult; + + /// Called by the WaitResult's constructor to place a hold on ownership and thread-safety. + /** + * Should only be called in pairs with wait_result_release(). + * + * \throws std::runtime_error If called twice before wait_result_release(). + */ + void + wait_result_acquire() + { + if (wait_result_holding_) { + throw std::runtime_error("wait_result_acquire() called while already holding"); + } + wait_result_holding_ = true; + wait_result_dirty_ = false; + // this method comes from the SynchronizationPolicy + this->sync_wait_result_acquire(); + // this method comes from the StoragePolicy + this->storage_acquire_ownerships(); + } + + /// Called by the WaitResult's destructor to release resources. + /** + * Should only be called if wait_result_acquire() has been called. + * + * \throws std::runtime_error If called before wait_result_acquire(). + */ + void + wait_result_release() + { + if (!wait_result_holding_) { + throw std::runtime_error("wait_result_release() called while not holding"); + } + wait_result_holding_ = false; + wait_result_dirty_ = false; + // this method comes from the StoragePolicy + this->storage_release_ownerships(); + // this method comes from the SynchronizationPolicy + this->sync_wait_result_release(); + } + + bool wait_result_holding_ = false; + bool wait_result_dirty_ = false; +}; + +} // namespace rclcpp + +#endif // RCLCPP__WAIT_SET_TEMPLATE_HPP_ diff --git a/rclcpp/include/rclcpp/waitable.hpp b/rclcpp/include/rclcpp/waitable.hpp new file mode 100644 index 0000000000..c803629dcd --- /dev/null +++ b/rclcpp/include/rclcpp/waitable.hpp @@ -0,0 +1,267 @@ +// 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 RCLCPP__WAITABLE_HPP_ +#define RCLCPP__WAITABLE_HPP_ + +#include +#include +#include + +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" + +#include "rcl/wait.h" + +namespace rclcpp +{ + +class Waitable +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Waitable) + + RCLCPP_PUBLIC + virtual ~Waitable() = default; + + /// Get the number of ready subscriptions + /** + * Returns a value of 0 by default. + * This should be overridden if the Waitable contains one or more subscriptions. + * \return The number of subscriptions associated with the Waitable. + */ + RCLCPP_PUBLIC + virtual + size_t + get_number_of_ready_subscriptions(); + + /// Get the number of ready timers + /** + * Returns a value of 0 by default. + * This should be overridden if the Waitable contains one or more timers. + * \return The number of timers associated with the Waitable. + */ + RCLCPP_PUBLIC + virtual + size_t + get_number_of_ready_timers(); + + /// Get the number of ready clients + /** + * Returns a value of 0 by default. + * This should be overridden if the Waitable contains one or more clients. + * \return The number of clients associated with the Waitable. + */ + RCLCPP_PUBLIC + virtual + size_t + get_number_of_ready_clients(); + + /// Get the number of ready events + /** + * Returns a value of 0 by default. + * This should be overridden if the Waitable contains one or more events. + * \return The number of events associated with the Waitable. + */ + RCLCPP_PUBLIC + virtual + size_t + get_number_of_ready_events(); + + /// Get the number of ready services + /** + * Returns a value of 0 by default. + * This should be overridden if the Waitable contains one or more services. + * \return The number of services associated with the Waitable. + */ + RCLCPP_PUBLIC + virtual + size_t + get_number_of_ready_services(); + + /// Get the number of ready guard_conditions + /** + * Returns a value of 0 by default. + * This should be overridden if the Waitable contains one or more guard_conditions. + * \return The number of guard_conditions associated with the Waitable. + */ + RCLCPP_PUBLIC + virtual + size_t + get_number_of_ready_guard_conditions(); + + /// Add the Waitable to a wait set. + /** + * \param[in] wait_set A handle to the wait set to add the Waitable to. + * \throws rclcpp::execptions::RCLError from rcl_wait_set_add_*() + */ + RCLCPP_PUBLIC + virtual + void + add_to_wait_set(rcl_wait_set_t & wait_set) = 0; + + /// Check if the Waitable is ready. + /** + * The input wait set should be the same that was used in a previously call to + * `add_to_wait_set()`. + * The wait set should also have been previously waited on with `rcl_wait()`. + * + * \param[in] wait_set A handle to the wait set the Waitable was previously added to + * and that has been waited on. + * \return `true` if the Waitable is ready, `false` otherwise. + */ + RCLCPP_PUBLIC + virtual + bool + is_ready(const rcl_wait_set_t & wait_set) = 0; + + /// Take the data so that it can be consumed with `execute`. + /** + * NOTE: take_data is a partial fix to a larger design issue with the + * multithreaded executor. This method is likely to be removed when + * a more permanent fix is implemented. A longterm fix is currently + * being discussed here: https://github.com/ros2/rclcpp/pull/1276 + * + * This method takes the data from the underlying data structure and + * writes it to the void shared pointer `data` that is passed into the + * method. The `data` can then be executed with the `execute` method. + * + * Before calling this method, the Waitable should be added to a wait set, + * waited on, and then updated. + * + * Example usage: + * + * ```cpp + * // ... create a wait set and a Waitable + * // Add the Waitable to the wait set + * waitable.add_to_wait_set(wait_set); + * // Wait + * rcl_ret_t wait_ret = rcl_wait(wait_set); + * // ... error handling + * // Update the Waitable + * waitable.update(wait_set); + * // Execute any entities of the Waitable that may be ready + * std::shared_ptr data = waitable.take_data(); + * ``` + */ + RCLCPP_PUBLIC + virtual + std::shared_ptr + take_data() = 0; + + /// Take the data so that it can be consumed with `execute`. + /** + * This function allows to specify an entity ID to take the data from. + * Entity IDs are identifiers that can be defined by waitable-derived + * classes that are composed of several distinct entities. + * The main use-case is in conjunction with the listener APIs. + * + * \param[in] id the id of the entity from which to take + * \returns the type-erased data taken from entity specified + * + * \sa rclcpp::Waitable::take_data + * \sa rclcpp::Waitable::set_on_ready_callback + */ + RCLCPP_PUBLIC + virtual + std::shared_ptr + take_data_by_entity_id(size_t id) = 0; + + /// Execute data that is passed in. + /** + * Before calling this method, the Waitable should be added to a wait set, + * waited on, and then updated - and the `take_data` method should be + * called. + * + * Example usage: + * + * ```cpp + * // ... create a wait set and a Waitable + * // Add the Waitable to the wait set + * waitable.add_to_wait_set(wait_set); + * // Wait + * rcl_ret_t wait_ret = rcl_wait(wait_set); + * // ... error handling + * // Update the Waitable + * waitable.update(wait_set); + * // Execute any entities of the Waitable that may be ready + * std::shared_ptr data = waitable.take_data(); + * waitable.execute(data); + * ``` + */ + RCLCPP_PUBLIC + virtual + void + execute(const std::shared_ptr & data) = 0; + + /// Exchange the "in use by wait set" state for this timer. + /** + * This is used to ensure this timer is not used by multiple + * wait sets at the same time. + * + * \param[in] in_use_state the new state to exchange into the state, true + * indicates it is now in use by a wait set, and false is that it is no + * longer in use by a wait set. + * \returns the previous state. + */ + RCLCPP_PUBLIC + bool + exchange_in_use_by_wait_set_state(bool in_use_state); + + /// Set a callback to be called whenever the waitable becomes ready. + /** + * The callback receives a size_t which is the number of times the waitable was ready + * since the last time this callback was called. + * Normally this is 1, but can be > 1 if waitable was triggered before any + * callback was set. + * + * The callback also receives an int identifier argument. + * This is needed because a Waitable may be composed of several distinct entities, + * such as subscriptions, services, etc. + * The application should provide a generic callback function that will be then + * forwarded by the waitable to all of its entities. + * Before forwarding, a different value for the identifier argument will be + * bond to the function. + * This implies that the provided callback can use the identifier to behave + * differently depending on which entity triggered the waitable to become ready. + * + * Note: this function must be overridden with a proper implementation + * by the custom classes who inherit from rclcpp::Waitable if they want to use it. + * + * \sa rclcpp::Waitable::clear_on_ready_callback + * + * \param[in] callback functor to be called when the waitable becomes ready + */ + RCLCPP_PUBLIC + virtual + void + set_on_ready_callback(std::function callback) = 0; + + /// Unset any callback registered via set_on_ready_callback. + /** + * Note: this function must be overridden with a proper implementation + * by the custom classes who inherit from rclcpp::Waitable if they want to use it. + */ + RCLCPP_PUBLIC + virtual + void + clear_on_ready_callback() = 0; + +private: + std::atomic in_use_by_wait_set_{false}; +}; // class Waitable + +} // namespace rclcpp + +#endif // RCLCPP__WAITABLE_HPP_ diff --git a/rclcpp/package.xml b/rclcpp/package.xml index db0b8a4804..abbd14f8dc 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,37 +2,61 @@ rclcpp - 0.4.0 + 29.2.0 The ROS client library in C++. - Dirk Thomas + + Ivan Paunovic + Michel Hidalgo + William Woodall + Apache License 2.0 - ament_cmake + Dirk Thomas + Jacob Perron - rmw + ament_cmake_ros + ament_cmake_gen_version_h + python3 + ament_index_cpp builtin_interfaces rcl_interfaces - rosidl_generator_cpp + rosgraph_msgs + rosidl_runtime_c + rosidl_runtime_cpp rosidl_typesupport_c rosidl_typesupport_cpp + ament_index_cpp builtin_interfaces rcl_interfaces - rosidl_generator_cpp + rosgraph_msgs + rosidl_runtime_c + rosidl_runtime_cpp rosidl_typesupport_c rosidl_typesupport_cpp + libstatistics_collector rcl - rmw_implementation - - ament_cmake + rcl_logging_interface + rcl_yaml_param_parser + rcpputils + rcutils + rmw + rosidl_dynamic_typesupport + statistics_msgs + tracetools ament_cmake_gmock + ament_cmake_google_benchmark ament_cmake_gtest ament_lint_auto ament_lint_common + mimick_vendor + performance_test_fixture rmw rmw_implementation_cmake + rosidl_default_generators + test_msgs ament_cmake diff --git a/rclcpp/rclcpp-extras.cmake b/rclcpp/rclcpp-extras.cmake deleted file mode 100644 index 7d3a1429fa..0000000000 --- a/rclcpp/rclcpp-extras.cmake +++ /dev/null @@ -1,29 +0,0 @@ -# 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. - -# copied from rclcpp/rclcpp-extras.cmake - -# register ament_package() hook for node plugins once -macro(_rclcpp_register_package_hook) - if(NOT DEFINED _RCLCPP_PACKAGE_HOOK_REGISTERED) - set(_RCLCPP_PACKAGE_HOOK_REGISTERED TRUE) - - find_package(ament_cmake_core QUIET REQUIRED) - ament_register_extension("ament_package" "rclcpp" - "rclcpp_package_hook.cmake") - endif() -endmacro() - -include("${rclcpp_DIR}/rclcpp_create_node_main.cmake") -include("${rclcpp_DIR}/rclcpp_register_node_plugins.cmake") diff --git a/rclcpp/resource/get_interface.hpp.em b/rclcpp/resource/get_interface.hpp.em new file mode 100644 index 0000000000..879b405f7f --- /dev/null +++ b/rclcpp/resource/get_interface.hpp.em @@ -0,0 +1,111 @@ +// 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. + +@{ +uppercase_interface_name = interface_name.upper() +}@ + +#ifndef RCLCPP__NODE_INTERFACES__GET_@(uppercase_interface_name)_HPP_ +#define RCLCPP__NODE_INTERFACES__GET_@(uppercase_interface_name)_HPP_ + +#include +#include +#include + +#include "rcpputils/pointer_traits.hpp" + +#include "rclcpp/node_interfaces/@(interface_name).hpp" +#include "rclcpp/node_interfaces/@(interface_name)_traits.hpp" + +@{ +interface_typename = ''.join([part.capitalize() for part in interface_name.split('_')]) +}@ + +/// This header provides the get_@(interface_name)() template function. +/** + * This function is useful for getting the @(interface_typename) pointer from + * various kinds of Node-like classes. + * + * It's able to get a std::shared_ptr to a @(interface_typename) so long as the class + * has a method called ``get_@(interface_name)()`` which returns one. + */ + +namespace rclcpp +{ +namespace node_interfaces +{ +namespace detail +{ + +// If NodeType has a method called get_@(interface_name)() which returns a shared pointer. +template< + typename NodeType, + typename std::enable_if::type + >::value, int>::type = 0 +> +std::shared_ptr +get_@(interface_name)_from_pointer(NodeType node_pointer) +{ + if (!node_pointer) { + throw std::invalid_argument("node cannot be nullptr"); + } + return node_pointer->get_@(interface_name)(); +} + +} // namespace detail + +/// Get the @(interface_typename) as a shared pointer from a pointer to a "Node like" object. +template< + typename NodeType, + typename std::enable_if< + rcpputils::is_pointer::value, int + >::type = 0 +> +inline +std::shared_ptr +get_@(interface_name)(NodeType && node) +{ + // Forward pointers to detail implementation directly. + return detail::get_@(interface_name)_from_pointer(node); +} + +/// Get the @(interface_typename) as a shared pointer from a "Node like" object. +template< + typename NodeType, + typename std::enable_if< + !rcpputils::is_pointer::value, int + >::type = 0 +> +inline +std::shared_ptr +get_@(interface_name)(NodeType && node) +{ + // Forward references to detail implementation as a pointer. + return detail::get_@(interface_name)_from_pointer(&node); +} + +/// Keep the @(interface_typename) a shared pointer. +inline +std::shared_ptr +get_@(interface_name)( + std::shared_ptr & node_interface) +{ + return node_interface; +} + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__GET_@(uppercase_interface_name)_HPP_ diff --git a/rclcpp/resource/interface_traits.hpp.em b/rclcpp/resource/interface_traits.hpp.em new file mode 100644 index 0000000000..506d67fefd --- /dev/null +++ b/rclcpp/resource/interface_traits.hpp.em @@ -0,0 +1,47 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +@{ +uppercase_interface_name = interface_name.upper() +interface_typename = ''.join([part.capitalize() for part in interface_name.split('_')]) +}@ + +#ifndef RCLCPP__NODE_INTERFACES__@(uppercase_interface_name)_TRAITS_HPP_ +#define RCLCPP__NODE_INTERFACES__@(uppercase_interface_name)_TRAITS_HPP_ + +#include +#include + +#include "rclcpp/node_interfaces/@(interface_name).hpp" + +namespace rclcpp +{ +namespace node_interfaces +{ + +template +struct has_@(interface_name) : std::false_type +{}; + +template +struct has_@(interface_name)< + T, typename std::enable_if< + std::is_same< + std::shared_ptr, + decltype(std::declval().get_@(interface_name)())>::value>::type> : std::true_type +{}; + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__@(uppercase_interface_name)_TRAITS_HPP_ diff --git a/rclcpp/resource/logging.hpp.em b/rclcpp/resource/logging.hpp.em index 24753f8116..01087e5dd0 100644 --- a/rclcpp/resource/logging.hpp.em +++ b/rclcpp/resource/logging.hpp.em @@ -17,10 +17,12 @@ #ifndef RCLCPP__LOGGING_HPP_ #define RCLCPP__LOGGING_HPP_ +#include #include #include "rclcpp/logger.hpp" #include "rcutils/logging_macros.h" +#include "rclcpp/utilities.hpp" // These are used for compiling out logging macros lower than a minimum severity. #define RCLCPP_LOG_MIN_SEVERITY_DEBUG 0 @@ -30,6 +32,9 @@ #define RCLCPP_LOG_MIN_SEVERITY_FATAL 4 #define RCLCPP_LOG_MIN_SEVERITY_NONE 5 +#define RCLCPP_FIRST_ARG(N, ...) N +#define RCLCPP_ALL_BUT_FIRST_ARGS(N, ...) __VA_ARGS__ + /** * \def RCLCPP_LOG_MIN_SEVERITY * Define RCLCPP_LOG_MIN_SEVERITY=RCLCPP_LOG_MIN_SEVERITY_[DEBUG|INFO|WARN|ERROR|FATAL] @@ -41,16 +46,38 @@ #endif @{ +from collections import OrderedDict +from copy import deepcopy from rcutils.logging import feature_combinations -from rcutils.logging import get_macro_parameters from rcutils.logging import get_suffix_from_features from rcutils.logging import severities +from rcutils.logging import throttle_args +from rcutils.logging import throttle_params + +throttle_args['condition_before'] = 'RCUTILS_LOG_CONDITION_THROTTLE_BEFORE(clock, duration)' +del throttle_params['get_time_point_value'] +throttle_params['clock'] = 'rclcpp::Clock that will be used to get the time point.' +throttle_params.move_to_end('clock', last=False) -# TODO(dhood): Implement the throttle macro using time sources available in rclcpp -excluded_features = ['named', 'throttle'] -def is_supported_feature_combination(feature_combination): - is_excluded = any([ef in feature_combination for ef in excluded_features]) - return not is_excluded +rclcpp_feature_combinations = OrderedDict() +for combinations, feature in feature_combinations.items(): + # skip feature combinations using 'named' + if 'named' in combinations: + continue + rclcpp_feature_combinations[combinations] = feature +# add a stream variant for each available feature combination +stream_arg = 'stream_arg' +for combinations, feature in list(rclcpp_feature_combinations.items()): + combinations = ('stream', ) + combinations + feature = deepcopy(feature) + feature.params[stream_arg] = 'The argument << into a stringstream' + rclcpp_feature_combinations[combinations] = feature + +def get_rclcpp_suffix_from_features(features): + suffix = get_suffix_from_features(features) + if 'stream' in features: + suffix = '_STREAM' + suffix + return suffix }@ @[for severity in severities]@ /** @@name Logging macros for severity @(severity). @@ -58,43 +85,76 @@ def is_supported_feature_combination(feature_combination): ///@@{ #if (RCLCPP_LOG_MIN_SEVERITY > RCLCPP_LOG_MIN_SEVERITY_@(severity)) // empty logging macros for severity @(severity) when being disabled at compile time -@[ for feature_combination in [fc for fc in feature_combinations if is_supported_feature_combination(fc)]]@ -@{suffix = get_suffix_from_features(feature_combination)}@ +@[ for feature_combination in rclcpp_feature_combinations.keys()]@ +@{suffix = get_rclcpp_suffix_from_features(feature_combination)}@ /// Empty logging macro due to the preprocessor definition of RCLCPP_LOG_MIN_SEVERITY. #define RCLCPP_@(severity)@(suffix)(...) @[ end for]@ #else -@[ for feature_combination in [fc for fc in feature_combinations if is_supported_feature_combination(fc)]]@ -@{suffix = get_suffix_from_features(feature_combination)}@ +@[ for feature_combination in rclcpp_feature_combinations.keys()]@ +@{suffix = get_rclcpp_suffix_from_features(feature_combination)}@ +// The RCLCPP_@(severity)@(suffix) macro is surrounded by do { .. } while (0) +// to implement the standard C macro idiom to make the macro safe in all +// contexts; see http://c-faq.com/cpp/multistmt.html for more information. /** * \def RCLCPP_@(severity)@(suffix) * Log a message with severity @(severity)@ -@[ if feature_combinations[feature_combination].doc_lines]@ +@[ if rclcpp_feature_combinations[feature_combination].doc_lines]@ with the following conditions: @[ else]@ . @[ end if]@ -@[ for doc_line in feature_combinations[feature_combination].doc_lines]@ +@[ for doc_line in rclcpp_feature_combinations[feature_combination].doc_lines]@ * @(doc_line) @[ end for]@ * \param logger The `rclcpp::Logger` to use -@[ for param_name, doc_line in feature_combinations[feature_combination].params.items()]@ +@[ for param_name, doc_line in rclcpp_feature_combinations[feature_combination].params.items()]@ * \param @(param_name) @(doc_line) @[ end for]@ - * \param ... The format string, followed by the variable arguments for the format string +@[ if 'stream' not in feature_combination]@ + * \param ... The format string, followed by the variable arguments for the format string. +@[ end if]@ */ -#define RCLCPP_@(severity)@(suffix)(logger, @(''.join([p + ', ' for p in get_macro_parameters(feature_combination).keys()]))...) \ - static_assert( \ - ::std::is_same::type, ::rclcpp::Logger>::value, \ - "First argument to logging macros must be an rclcpp::Logger"); \ - RCUTILS_LOG_@(severity)@(suffix)_NAMED( \ -@{params = get_macro_parameters(feature_combination).keys()}@ +@{params = rclcpp_feature_combinations[feature_combination].params.keys()}@ +#define RCLCPP_@(severity)@(suffix)(logger@(''.join([', ' + p for p in params]))@ +@[ if 'stream' not in feature_combination]@ +, ...@ +@[ end if]@ +) \ + do { \ + static_assert( \ + ::std::is_same>, \ + typename ::rclcpp::Logger>::value, \ + "First argument to logging macros must be an rclcpp::Logger"); \ +@[ if 'throttle' in feature_combination]@ \ + auto get_time_point = [&c=clock](rcutils_time_point_value_t * time_point) -> rcutils_ret_t { \ + try { \ + *time_point = c.now().nanoseconds(); \ + } catch (...) { \ + RCUTILS_SAFE_FWRITE_TO_STDERR( \ + "[rclcpp|logging.hpp] RCLCPP_@(severity)@(suffix) could not get current time stamp\n"); \ + return RCUTILS_RET_ERROR; \ + } \ + return RCUTILS_RET_OK; \ + }; \ +@[ end if] \ +@[ if 'stream' in feature_combination]@ + std::stringstream rclcpp_stream_ss_; \ + rclcpp_stream_ss_ << @(stream_arg); \ +@[ end if]@ + RCUTILS_LOG_@(severity)@(get_suffix_from_features(feature_combination))_NAMED( \ +@{params = ['get_time_point' if p == 'clock' and 'throttle' in feature_combination else p for p in params]}@ @[ if params]@ -@(''.join([' ' + p + ', \\\n' for p in params]))@ +@(''.join([' ' + p + ', \\\n' for p in params if p != stream_arg]))@ +@[ end if]@ + (logger).get_name(), \ +@[ if 'stream' not in feature_combination]@ + __VA_ARGS__); \ +@[ else]@ + "%s", rclcpp_stream_ss_.str().c_str()); \ @[ end if]@ - logger.get_name(), \ - __VA_ARGS__) + } while (0) @[ end for]@ #endif diff --git a/rclcpp/src/rclcpp/any_executable.cpp b/rclcpp/src/rclcpp/any_executable.cpp index 256033bbad..2449cbe1f7 100644 --- a/rclcpp/src/rclcpp/any_executable.cpp +++ b/rclcpp/src/rclcpp/any_executable.cpp @@ -14,14 +14,14 @@ #include "rclcpp/any_executable.hpp" -using rclcpp::executor::AnyExecutable; +using rclcpp::AnyExecutable; AnyExecutable::AnyExecutable() : subscription(nullptr), - subscription_intra_process(nullptr), timer(nullptr), service(nullptr), client(nullptr), + waitable(nullptr), callback_group(nullptr), node_base(nullptr) {} diff --git a/rclcpp/src/rclcpp/callback_group.cpp b/rclcpp/src/rclcpp/callback_group.cpp index 52558ee2fd..bcacaabebe 100644 --- a/rclcpp/src/rclcpp/callback_group.cpp +++ b/rclcpp/src/rclcpp/callback_group.cpp @@ -12,55 +12,139 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "rclcpp/callback_group.hpp" +#include +#include +#include +#include +#include +#include -#include +#include "rclcpp/callback_group.hpp" +#include "rclcpp/client.hpp" +#include "rclcpp/service.hpp" +#include "rclcpp/subscription_base.hpp" +#include "rclcpp/timer.hpp" +#include "rclcpp/waitable.hpp" -using rclcpp::callback_group::CallbackGroup; -using rclcpp::callback_group::CallbackGroupType; +using rclcpp::CallbackGroup; +using rclcpp::CallbackGroupType; -CallbackGroup::CallbackGroup(CallbackGroupType group_type) -: type_(group_type), can_be_taken_from_(true) +CallbackGroup::CallbackGroup( + CallbackGroupType group_type, + rclcpp::Context::WeakPtr context, + bool automatically_add_to_executor_with_node) +: type_(group_type), associated_with_executor_(false), + can_be_taken_from_(true), + automatically_add_to_executor_with_node_(automatically_add_to_executor_with_node), + context_(context) {} -const std::vector & -CallbackGroup::get_subscription_ptrs() const +CallbackGroup::~CallbackGroup() { - std::lock_guard lock(mutex_); - return subscription_ptrs_; + trigger_notify_guard_condition(); } -const std::vector & -CallbackGroup::get_timer_ptrs() const +std::atomic_bool & +CallbackGroup::can_be_taken_from() { - std::lock_guard lock(mutex_); - return timer_ptrs_; + return can_be_taken_from_; } -const std::vector & -CallbackGroup::get_service_ptrs() const +const CallbackGroupType & +CallbackGroup::type() const { - std::lock_guard lock(mutex_); - return service_ptrs_; + return type_; } -const std::vector & -CallbackGroup::get_client_ptrs() const +size_t +CallbackGroup::size() const +{ + return + subscription_ptrs_.size() + + service_ptrs_.size() + + client_ptrs_.size() + + timer_ptrs_.size() + + waitable_ptrs_.size(); +} + +void CallbackGroup::collect_all_ptrs( + std::function sub_func, + std::function service_func, + std::function client_func, + std::function timer_func, + std::function waitable_func) const { std::lock_guard lock(mutex_); - return client_ptrs_; + + for (const rclcpp::SubscriptionBase::WeakPtr & weak_ptr : subscription_ptrs_) { + rclcpp::SubscriptionBase::SharedPtr ref_ptr = weak_ptr.lock(); + if (ref_ptr) { + sub_func(ref_ptr); + } + } + + for (const rclcpp::ServiceBase::WeakPtr & weak_ptr : service_ptrs_) { + rclcpp::ServiceBase::SharedPtr ref_ptr = weak_ptr.lock(); + if (ref_ptr) { + service_func(ref_ptr); + } + } + + for (const rclcpp::ClientBase::WeakPtr & weak_ptr : client_ptrs_) { + rclcpp::ClientBase::SharedPtr ref_ptr = weak_ptr.lock(); + if (ref_ptr) { + client_func(ref_ptr); + } + } + + for (const rclcpp::TimerBase::WeakPtr & weak_ptr : timer_ptrs_) { + rclcpp::TimerBase::SharedPtr ref_ptr = weak_ptr.lock(); + if (ref_ptr) { + timer_func(ref_ptr); + } + } + + for (const rclcpp::Waitable::WeakPtr & weak_ptr : waitable_ptrs_) { + rclcpp::Waitable::SharedPtr ref_ptr = weak_ptr.lock(); + if (ref_ptr) { + waitable_func(ref_ptr); + } + } } std::atomic_bool & -CallbackGroup::can_be_taken_from() +CallbackGroup::get_associated_with_executor_atomic() { - return can_be_taken_from_; + return associated_with_executor_; } -const CallbackGroupType & -CallbackGroup::type() const +bool +CallbackGroup::automatically_add_to_executor_with_node() const { - return type_; + return automatically_add_to_executor_with_node_; +} + +rclcpp::GuardCondition::SharedPtr +CallbackGroup::get_notify_guard_condition() +{ + std::lock_guard lock(notify_guard_condition_mutex_); + rclcpp::Context::SharedPtr context_ptr = context_.lock(); + if (context_ptr && context_ptr->is_valid()) { + if (!notify_guard_condition_) { + notify_guard_condition_ = std::make_shared(context_ptr); + } + return notify_guard_condition_; + } + return nullptr; +} + +void +CallbackGroup::trigger_notify_guard_condition() +{ + std::lock_guard lock(notify_guard_condition_mutex_); + if (notify_guard_condition_) { + notify_guard_condition_->trigger(); + } } void @@ -69,6 +153,12 @@ CallbackGroup::add_subscription( { std::lock_guard lock(mutex_); subscription_ptrs_.push_back(subscription_ptr); + subscription_ptrs_.erase( + std::remove_if( + subscription_ptrs_.begin(), + subscription_ptrs_.end(), + [](rclcpp::SubscriptionBase::WeakPtr x) {return x.expired();}), + subscription_ptrs_.end()); } void @@ -76,6 +166,12 @@ CallbackGroup::add_timer(const rclcpp::TimerBase::SharedPtr timer_ptr) { std::lock_guard lock(mutex_); timer_ptrs_.push_back(timer_ptr); + timer_ptrs_.erase( + std::remove_if( + timer_ptrs_.begin(), + timer_ptrs_.end(), + [](rclcpp::TimerBase::WeakPtr x) {return x.expired();}), + timer_ptrs_.end()); } void @@ -83,6 +179,12 @@ CallbackGroup::add_service(const rclcpp::ServiceBase::SharedPtr service_ptr) { std::lock_guard lock(mutex_); service_ptrs_.push_back(service_ptr); + service_ptrs_.erase( + std::remove_if( + service_ptrs_.begin(), + service_ptrs_.end(), + [](rclcpp::ServiceBase::WeakPtr x) {return x.expired();}), + service_ptrs_.end()); } void @@ -90,4 +192,36 @@ CallbackGroup::add_client(const rclcpp::ClientBase::SharedPtr client_ptr) { std::lock_guard lock(mutex_); client_ptrs_.push_back(client_ptr); + client_ptrs_.erase( + std::remove_if( + client_ptrs_.begin(), + client_ptrs_.end(), + [](rclcpp::ClientBase::WeakPtr x) {return x.expired();}), + client_ptrs_.end()); +} + +void +CallbackGroup::add_waitable(const rclcpp::Waitable::SharedPtr waitable_ptr) +{ + std::lock_guard lock(mutex_); + waitable_ptrs_.push_back(waitable_ptr); + waitable_ptrs_.erase( + std::remove_if( + waitable_ptrs_.begin(), + waitable_ptrs_.end(), + [](rclcpp::Waitable::WeakPtr x) {return x.expired();}), + waitable_ptrs_.end()); +} + +void +CallbackGroup::remove_waitable(const rclcpp::Waitable::SharedPtr waitable_ptr) noexcept +{ + std::lock_guard lock(mutex_); + for (auto iter = waitable_ptrs_.begin(); iter != waitable_ptrs_.end(); ++iter) { + const auto shared_ptr = iter->lock(); + if (shared_ptr.get() == waitable_ptr.get()) { + waitable_ptrs_.erase(iter); + break; + } + } } diff --git a/rclcpp/src/rclcpp/client.cpp b/rclcpp/src/rclcpp/client.cpp index d646005b1f..8388ee1888 100644 --- a/rclcpp/src/rclcpp/client.cpp +++ b/rclcpp/src/rclcpp/client.cpp @@ -14,6 +14,7 @@ #include "rclcpp/client.hpp" +#include #include #include #include @@ -22,10 +23,13 @@ #include "rcl/graph.h" #include "rcl/node.h" #include "rcl/wait.h" + #include "rclcpp/exceptions.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/node_interfaces/node_graph_interface.hpp" +#include "rclcpp/qos.hpp" #include "rclcpp/utilities.hpp" +#include "rclcpp/logging.hpp" using rclcpp::ClientBase; using rclcpp::exceptions::InvalidNodeError; @@ -33,39 +37,84 @@ using rclcpp::exceptions::throw_from_rcl_error; ClientBase::ClientBase( rclcpp::node_interfaces::NodeBaseInterface * node_base, - rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, - const std::string & service_name) + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph) : node_graph_(node_graph), node_handle_(node_base->get_shared_rcl_node_handle()), - service_name_(service_name) -{} + context_(node_base->get_context()), + node_logger_(rclcpp::get_node_logger(node_handle_.get())) +{ + std::weak_ptr weak_node_handle(node_handle_); + rcl_client_t * new_rcl_client = new rcl_client_t; + *new_rcl_client = rcl_get_zero_initialized_client(); + client_handle_.reset( + new_rcl_client, [weak_node_handle](rcl_client_t * client) + { + auto handle = weak_node_handle.lock(); + if (handle) { + if (rcl_client_fini(client, handle.get()) != RCL_RET_OK) { + RCLCPP_ERROR( + rclcpp::get_node_logger(handle.get()).get_child("rclcpp"), + "Error in destruction of rcl client handle: %s", rcl_get_error_string().str); + rcl_reset_error(); + } + } else { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), + "Error in destruction of rcl client handle: " + "the Node Handle was destructed too early. You will leak memory"); + } + delete client; + }); +} -ClientBase::~ClientBase() {} +bool +ClientBase::take_type_erased_response(void * response_out, rmw_request_id_t & request_header_out) +{ + rcl_ret_t ret = rcl_take_response( + this->get_client_handle().get(), + &request_header_out, + response_out); + if (RCL_RET_CLIENT_TAKE_FAILED == ret) { + return false; + } else if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + return true; +} -const std::string & +const char * ClientBase::get_service_name() const { - return this->service_name_; + return rcl_client_get_service_name(this->get_client_handle().get()); } -rcl_client_t * +std::shared_ptr ClientBase::get_client_handle() { - return &client_handle_; + return client_handle_; } -const rcl_client_t * +std::shared_ptr ClientBase::get_client_handle() const { - return &client_handle_; + return client_handle_; } bool ClientBase::service_is_ready() const { bool is_ready; - rcl_ret_t ret = - rcl_service_server_is_available(this->get_rcl_node_handle(), &client_handle_, &is_ready); + rcl_ret_t ret = rcl_service_server_is_available( + this->get_rcl_node_handle(), + this->get_client_handle().get(), + &is_ready); + if (RCL_RET_NODE_INVALID == ret) { + const rcl_node_t * node_handle = this->get_rcl_node_handle(); + if (node_handle && !rcl_context_is_valid(node_handle->context)) { + // context is shutdown, do a soft failure + return false; + } + } if (ret != RCL_RET_OK) { throw_from_rcl_error(ret, "rcl_service_server_is_available failed"); } @@ -76,12 +125,10 @@ bool ClientBase::wait_for_service_nanoseconds(std::chrono::nanoseconds timeout) { auto start = std::chrono::steady_clock::now(); - // make an event to reuse, rather than create a new one each time auto node_ptr = node_graph_.lock(); if (!node_ptr) { throw InvalidNodeError(); } - auto event = node_ptr->get_graph_event(); // check to see if the server is ready immediately if (this->service_is_ready()) { return true; @@ -90,31 +137,44 @@ ClientBase::wait_for_service_nanoseconds(std::chrono::nanoseconds timeout) // check was non-blocking, return immediately return false; } + // make an event to reuse, rather than create a new one each time + auto event = node_ptr->get_graph_event(); // update the time even on the first loop to account for time spent in the first call // to this->server_is_ready() - std::chrono::nanoseconds time_to_wait = timeout - (std::chrono::steady_clock::now() - start); - if (timeout > std::chrono::nanoseconds(0) && time_to_wait < std::chrono::nanoseconds(0)) { + std::chrono::nanoseconds time_to_wait = + timeout > std::chrono::nanoseconds(0) ? + timeout - (std::chrono::steady_clock::now() - start) : + std::chrono::nanoseconds::max(); + if (time_to_wait < std::chrono::nanoseconds(0)) { // Do not allow the time_to_wait to become negative when timeout was originally positive. // Setting time_to_wait to 0 will allow one non-blocking wait because of the do-while. time_to_wait = std::chrono::nanoseconds(0); } - // continue forever if timeout is negative, otherwise continue until out of time_to_wait do { - if (!rclcpp::ok()) { + if (!rclcpp::ok(this->context_)) { return false; } - node_ptr->wait_for_graph_change(event, time_to_wait); - event->check_and_clear(); // reset the event - - // always check if the service is ready, even if the graph event wasn't triggered - // this is needed to avoid a race condition that is specific to the Connext RMW implementation + // Limit each wait to 100ms to workaround an issue specific to the Connext RMW implementation. + // A race condition means that graph changes for services becoming available may trigger the + // wait set to wake up, but then not be reported as ready immediately after the wake up // (see https://github.com/ros2/rmw_connext/issues/201) + // If no other graph events occur, the wait set will not be triggered again until the timeout + // has been reached, despite the service being available, so we artificially limit the wait + // time to limit the delay. + node_ptr->wait_for_graph_change( + event, std::min(time_to_wait, std::chrono::nanoseconds(RCL_MS_TO_NS(100)))); + // Because of the aforementioned race condition, we check if the service is ready even if the + // graph event wasn't triggered. + event->check_and_clear(); if (this->service_is_ready()) { return true; } // server is not ready, loop if there is time left - time_to_wait = timeout - (std::chrono::steady_clock::now() - start); - } while (time_to_wait > std::chrono::nanoseconds(0) || timeout < std::chrono::nanoseconds(0)); + if (timeout > std::chrono::nanoseconds(0)) { + time_to_wait = timeout - (std::chrono::steady_clock::now() - start); + } + // if timeout is negative, time_to_wait will never reach zero + } while (time_to_wait > std::chrono::nanoseconds(0)); return false; // timeout exceeded while waiting for the server to be ready } @@ -129,3 +189,60 @@ ClientBase::get_rcl_node_handle() const { return node_handle_.get(); } + +bool +ClientBase::exchange_in_use_by_wait_set_state(bool in_use_state) +{ + return in_use_by_wait_set_.exchange(in_use_state); +} + +rclcpp::QoS +ClientBase::get_request_publisher_actual_qos() const +{ + const rmw_qos_profile_t * qos = + rcl_client_request_publisher_get_actual_qos(client_handle_.get()); + if (!qos) { + auto msg = + std::string("failed to get client's request publisher qos settings: ") + + rcl_get_error_string().str; + rcl_reset_error(); + throw std::runtime_error(msg); + } + + rclcpp::QoS request_publisher_qos = + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(*qos), *qos); + + return request_publisher_qos; +} + +rclcpp::QoS +ClientBase::get_response_subscription_actual_qos() const +{ + const rmw_qos_profile_t * qos = + rcl_client_response_subscription_get_actual_qos(client_handle_.get()); + if (!qos) { + auto msg = + std::string("failed to get client's response subscription qos settings: ") + + rcl_get_error_string().str; + rcl_reset_error(); + throw std::runtime_error(msg); + } + + rclcpp::QoS response_subscription_qos = + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(*qos), *qos); + + return response_subscription_qos; +} + +void +ClientBase::set_on_new_response_callback(rcl_event_callback_t callback, const void * user_data) +{ + rcl_ret_t ret = rcl_client_set_on_new_response_callback( + client_handle_.get(), + callback, + user_data); + + if (RCL_RET_OK != ret) { + throw_from_rcl_error(ret, "failed to set the on new response callback for client"); + } +} diff --git a/rclcpp/src/rclcpp/clock.cpp b/rclcpp/src/rclcpp/clock.cpp index 1ca8708475..5c13f19d13 100644 --- a/rclcpp/src/rclcpp/clock.cpp +++ b/rclcpp/src/rclcpp/clock.cpp @@ -14,157 +14,357 @@ #include "rclcpp/clock.hpp" +#include #include -#include -#include - -#include "builtin_interfaces/msg/time.hpp" - -#include "rcl/time.h" +#include #include "rclcpp/exceptions.hpp" +#include "rclcpp/utilities.hpp" +#include "rcpputils/scope_exit.hpp" #include "rcutils/logging_macros.h" namespace rclcpp { -bool -JumpThreshold::is_exceeded(const TimeJump & jump) + +class Clock::Impl { - if (on_clock_change_ && - (jump.jump_type_ == TimeJump::ClockChange_t::ROS_TIME_ACTIVATED || - jump.jump_type_ == TimeJump::ClockChange_t::ROS_TIME_DEACTIVATED)) +public: + explicit Impl(rcl_clock_type_t clock_type) + : allocator_{rcl_get_default_allocator()} { - return true; + rcl_ret_t ret = rcl_clock_init(clock_type, &rcl_clock_, &allocator_); + if (ret != RCL_RET_OK) { + exceptions::throw_from_rcl_error(ret, "failed to initialize rcl clock"); + } } - if ((uint64_t)jump.delta_.nanoseconds > min_forward_ || - (uint64_t)jump.delta_.nanoseconds < min_backward_) + + ~Impl() { - return true; + rcl_ret_t ret = rcl_clock_fini(&rcl_clock_); + if (ret != RCL_RET_OK) { + RCUTILS_LOG_ERROR("Failed to fini rcl clock."); + } } - return false; -} + + rcl_clock_t rcl_clock_; + rcl_allocator_t allocator_; + bool stop_sleeping_ = false; + bool shutdown_ = false; + std::condition_variable cv_; + std::mutex wait_mutex_; + std::mutex clock_mutex_; +}; JumpHandler::JumpHandler( - std::function pre_callback, - std::function post_callback, - JumpThreshold & threshold) + pre_callback_t pre_callback, + post_callback_t post_callback, + const rcl_jump_threshold_t & threshold) : pre_callback(pre_callback), post_callback(post_callback), notice_threshold(threshold) {} Clock::Clock(rcl_clock_type_t clock_type) +: impl_(new Clock::Impl(clock_type)) {} + +Clock::~Clock() {} + +Time +Clock::now() const { - allocator_ = rcl_get_default_allocator(); - auto ret = rcl_clock_init(clock_type, &rcl_clock_, &allocator_); + Time now(0, 0, impl_->rcl_clock_.type); + + auto ret = rcl_clock_get_now(&impl_->rcl_clock_, &now.rcl_time_.nanoseconds); if (ret != RCL_RET_OK) { - rclcpp::exceptions::throw_from_rcl_error( - ret, "could not get current time stamp"); + exceptions::throw_from_rcl_error(ret, "could not get current time stamp"); } + + return now; } -Clock::~Clock() +void +Clock::cancel_sleep_or_wait() { - auto ret = rcl_clock_fini(&rcl_clock_); - if (ret != RCL_RET_OK) { - RCUTILS_LOG_ERROR("Failed to fini rcl clock."); + { + std::unique_lock lock(impl_->wait_mutex_); + impl_->stop_sleeping_ = true; } + impl_->cv_.notify_one(); } -Time -Clock::now() +bool +Clock::sleep_until( + Time until, + Context::SharedPtr context) { - Time now(0, 0, rcl_clock_.type); + if (!context || !context->is_valid()) { + throw std::runtime_error("context cannot be slept with because it's invalid"); + } + const auto this_clock_type = get_clock_type(); + if (until.get_clock_type() != this_clock_type) { + throw std::runtime_error("until's clock type does not match this clock's type"); + } + bool time_source_changed = false; - auto ret = rcl_clock_get_now(&rcl_clock_, &now.rcl_time_); - if (ret != RCL_RET_OK) { - rclcpp::exceptions::throw_from_rcl_error( - ret, "could not get current time stamp"); + // Wake this thread if the context is shutdown + rclcpp::OnShutdownCallbackHandle shutdown_cb_handle = context->add_on_shutdown_callback( + [this]() { + { + std::unique_lock lock(impl_->wait_mutex_); + impl_->shutdown_ = true; + } + impl_->cv_.notify_one(); + }); + // No longer need the shutdown callback when this function exits + auto callback_remover = rcpputils::scope_exit( + [context, &shutdown_cb_handle]() { + context->remove_on_shutdown_callback(shutdown_cb_handle); + }); + + if (this_clock_type == RCL_STEADY_TIME) { + // Synchronize because RCL steady clock epoch might differ from chrono::steady_clock epoch + const Time rcl_entry = now(); + const std::chrono::steady_clock::time_point chrono_entry = std::chrono::steady_clock::now(); + const Duration delta_t = until - rcl_entry; + const std::chrono::steady_clock::time_point chrono_until = + chrono_entry + std::chrono::nanoseconds(delta_t.nanoseconds()); + + // loop over spurious wakeups but notice shutdown or stop of sleep + std::unique_lock lock(impl_->wait_mutex_); + while (now() < until && !impl_->stop_sleeping_ && !impl_->shutdown_ && context->is_valid()) { + impl_->cv_.wait_until(lock, chrono_until); + } + impl_->stop_sleeping_ = false; + } else if (this_clock_type == RCL_SYSTEM_TIME) { + auto system_time = std::chrono::system_clock::time_point( + // Cast because system clock resolution is too big for nanoseconds on some systems + std::chrono::duration_cast( + std::chrono::nanoseconds(until.nanoseconds()))); + + // loop over spurious wakeups but notice shutdown or stop of sleep + std::unique_lock lock(impl_->wait_mutex_); + while (now() < until && !impl_->stop_sleeping_ && !impl_->shutdown_ && context->is_valid()) { + impl_->cv_.wait_until(lock, system_time); + } + impl_->stop_sleeping_ = false; + } else if (this_clock_type == RCL_ROS_TIME) { + // Install jump handler for any amount of time change, for two purposes: + // - if ROS time is active, check if time reached on each new clock sample + // - Trigger via on_clock_change to detect if time source changes, to invalidate sleep + rcl_jump_threshold_t threshold; + threshold.on_clock_change = true; + // 0 is disable, so -1 and 1 are smallest possible time changes + threshold.min_backward.nanoseconds = -1; + threshold.min_forward.nanoseconds = 1; + auto clock_handler = create_jump_callback( + nullptr, + [this, &time_source_changed](const rcl_time_jump_t & jump) { + if (jump.clock_change != RCL_ROS_TIME_NO_CHANGE) { + std::lock_guard lk(impl_->wait_mutex_); + time_source_changed = true; + } + impl_->cv_.notify_one(); + }, + threshold); + + if (!ros_time_is_active()) { + auto system_time = std::chrono::system_clock::time_point( + // Cast because system clock resolution is too big for nanoseconds on some systems + std::chrono::duration_cast( + std::chrono::nanoseconds(until.nanoseconds()))); + + // loop over spurious wakeups but notice shutdown, stop of sleep or time source change + std::unique_lock lock(impl_->wait_mutex_); + while (now() < until && !impl_->stop_sleeping_ && !impl_->shutdown_ && context->is_valid() && + !time_source_changed) + { + impl_->cv_.wait_until(lock, system_time); + } + impl_->stop_sleeping_ = false; + } else { + // RCL_ROS_TIME with ros_time_is_active. + // Just wait without "until" because installed + // jump callbacks wake the cv on every new sample. + std::unique_lock lock(impl_->wait_mutex_); + while (now() < until && !impl_->stop_sleeping_ && !impl_->shutdown_ && context->is_valid() && + !time_source_changed) + { + impl_->cv_.wait(lock); + } + impl_->stop_sleeping_ = false; + } } - return now; + if (!context->is_valid() || time_source_changed) { + return false; + } + + return now() >= until; } +bool +Clock::sleep_for(Duration rel_time, Context::SharedPtr context) +{ + return sleep_until(now() + rel_time, context); +} + +bool +Clock::started() +{ + if (!rcl_clock_valid(get_clock_handle())) { + throw std::runtime_error("clock is not rcl_clock_valid"); + } + return rcl_clock_time_started(get_clock_handle()); +} + +bool +Clock::wait_until_started(Context::SharedPtr context) +{ + if (!context || !context->is_valid()) { + throw std::runtime_error("context cannot be slept with because it's invalid"); + } + if (!rcl_clock_valid(get_clock_handle())) { + throw std::runtime_error("clock cannot be waited on as it is not rcl_clock_valid"); + } + + if (started()) { + return true; + } else { + // Wait until the first non-zero time + return sleep_until(rclcpp::Time(0, 1, get_clock_type()), context); + } +} + +bool +Clock::wait_until_started( + const Duration & timeout, + Context::SharedPtr context, + const Duration & wait_tick_ns) +{ + if (!context || !context->is_valid()) { + throw std::runtime_error("context cannot be slept with because it's invalid"); + } + if (!rcl_clock_valid(get_clock_handle())) { + throw std::runtime_error("clock cannot be waited on as it is not rcl_clock_valid"); + } + + Clock timeout_clock = Clock(RCL_STEADY_TIME); + Time start = timeout_clock.now(); + + // Check if the clock has started every wait_tick_ns nanoseconds + // Context check checks for rclcpp::shutdown() + while (!started() && context->is_valid()) { + if (timeout < wait_tick_ns) { + timeout_clock.sleep_for(timeout); + } else { + Duration time_left = start + timeout - timeout_clock.now(); + if (time_left > wait_tick_ns) { + timeout_clock.sleep_for(Duration(wait_tick_ns)); + } else { + timeout_clock.sleep_for(time_left); + } + } + + if (timeout_clock.now() - start > timeout) { + return started(); + } + } + return started(); +} + + bool Clock::ros_time_is_active() { - if (!rcl_clock_valid(&rcl_clock_)) { + if (!rcl_clock_valid(&impl_->rcl_clock_)) { RCUTILS_LOG_ERROR("ROS time not valid!"); return false; } - bool is_enabled; - auto ret = rcl_is_enabled_ros_time_override(&rcl_clock_, &is_enabled); + bool is_enabled = false; + auto ret = rcl_is_enabled_ros_time_override(&impl_->rcl_clock_, &is_enabled); if (ret != RCL_RET_OK) { - rclcpp::exceptions::throw_from_rcl_error( + exceptions::throw_from_rcl_error( ret, "Failed to check ros_time_override_status"); } return is_enabled; } -rcl_clock_type_t -Clock::get_clock_type() +rcl_clock_t * +Clock::get_clock_handle() noexcept { - return rcl_clock_.type; + return &impl_->rcl_clock_; } -rclcpp::JumpHandler::SharedPtr -Clock::create_jump_callback( - std::function pre_callback, - std::function post_callback, - JumpThreshold & threshold) +rcl_clock_type_t +Clock::get_clock_type() const noexcept { - // JumpHandler jump_callback; - auto jump_callback = - std::make_shared(pre_callback, post_callback, threshold); - { - std::lock_guard guard(callback_list_mutex_); - active_jump_handlers_.push_back(jump_callback); - } - return jump_callback; + return impl_->rcl_clock_.type; } -std::vector -Clock::get_triggered_callback_handlers(const TimeJump & jump) +std::mutex & +Clock::get_clock_mutex() noexcept { - std::vector callbacks; - std::lock_guard guard(callback_list_mutex_); - active_jump_handlers_.erase( - std::remove_if( - active_jump_handlers_.begin(), - active_jump_handlers_.end(), - [&callbacks, &jump](const std::weak_ptr & wjcb) { - if (auto jcb = wjcb.lock()) { - if (jcb->notice_threshold.is_exceeded(jump)) { - callbacks.push_back(jcb); - } - return false; - } - // Lock failed so clear the weak pointer. - return true; - }), - active_jump_handlers_.end()); - return callbacks; + return impl_->clock_mutex_; } void -Clock::invoke_prejump_callbacks( - const std::vector & callbacks) +Clock::on_time_jump( + const rcl_time_jump_t * time_jump, + bool before_jump, + void * user_data) { - for (const auto cb : callbacks) { - cb->pre_callback(); + const auto * handler = static_cast(user_data); + if (nullptr == handler) { + return; + } + if (before_jump && handler->pre_callback) { + handler->pre_callback(); + } else if (!before_jump && handler->post_callback) { + handler->post_callback(*time_jump); } } -void -Clock::invoke_postjump_callbacks( - const std::vector & callbacks, - const TimeJump & jump) +JumpHandler::SharedPtr +Clock::create_jump_callback( + JumpHandler::pre_callback_t pre_callback, + JumpHandler::post_callback_t post_callback, + const rcl_jump_threshold_t & threshold) { - for (auto cb : callbacks) { - cb->post_callback(jump); + // Allocate a new jump handler + JumpHandler::UniquePtr handler(new JumpHandler(pre_callback, post_callback, threshold)); + if (nullptr == handler) { + throw std::bad_alloc{}; } + + { + std::lock_guard clock_guard(impl_->clock_mutex_); + // Try to add the jump callback to the clock + rcl_ret_t ret = rcl_clock_add_jump_callback( + &impl_->rcl_clock_, threshold, Clock::on_time_jump, + handler.get()); + if (RCL_RET_OK != ret) { + exceptions::throw_from_rcl_error(ret, "Failed to add time jump callback"); + } + } + + std::weak_ptr weak_impl = impl_; + // *INDENT-OFF* + // create shared_ptr that removes the callback automatically when all copies are destructed + return JumpHandler::SharedPtr(handler.release(), [weak_impl](JumpHandler * handler) noexcept { + auto shared_impl = weak_impl.lock(); + if (shared_impl) { + std::lock_guard clock_guard(shared_impl->clock_mutex_); + rcl_ret_t ret = rcl_clock_remove_jump_callback(&shared_impl->rcl_clock_, + Clock::on_time_jump, handler); + if (RCL_RET_OK != ret) { + RCUTILS_LOG_ERROR("Failed to remove time jump callback"); + } + } + delete handler; + }); + // *INDENT-ON* } } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/context.cpp b/rclcpp/src/rclcpp/context.cpp index fa481d796b..a9b40b733e 100644 --- a/rclcpp/src/rclcpp/context.cpp +++ b/rclcpp/src/rclcpp/context.cpp @@ -1,4 +1,4 @@ -// Copyright 2015 Open Source Robotics Foundation, Inc. +// Copyright 2015-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. @@ -14,6 +14,512 @@ #include "rclcpp/context.hpp" +#include +#include +#include +#include +#include +#include +#include + +#include "rcl/init.h" +#include "rcl/logging.h" + +#include "rclcpp/detail/utilities.hpp" +#include "rclcpp/exceptions.hpp" +#include "rclcpp/logging.hpp" + +#include "rcutils/error_handling.h" +#include "rcutils/macros.h" + +#include "./logging_mutex.hpp" + using rclcpp::Context; -Context::Context() {} +namespace rclcpp +{ +/// Class to manage vector of weak pointers to all created contexts +class WeakContextsWrapper +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(WeakContextsWrapper) + + void + add_context(const Context::SharedPtr & context) + { + std::lock_guard guard(mutex_); + weak_contexts_.push_back(context); + } + + void + remove_context(const Context * context) + { + std::lock_guard guard(mutex_); + weak_contexts_.erase( + std::remove_if( + weak_contexts_.begin(), + weak_contexts_.end(), + [context](const Context::WeakPtr weak_context) { + auto locked_context = weak_context.lock(); + if (!locked_context) { + // take advantage and removed expired contexts + return true; + } + return locked_context.get() == context; + } + ), + weak_contexts_.end()); + } + + std::vector + get_contexts() + { + std::lock_guard lock(mutex_); + std::vector shared_contexts; + for (auto it = weak_contexts_.begin(); it != weak_contexts_.end(); /* noop */) { + auto context_ptr = it->lock(); + if (!context_ptr) { + // remove invalid weak context pointers + it = weak_contexts_.erase(it); + } else { + ++it; + shared_contexts.push_back(context_ptr); + } + } + return shared_contexts; + } + +private: + std::vector> weak_contexts_; + std::mutex mutex_; +}; +} // namespace rclcpp + +using rclcpp::WeakContextsWrapper; + +/// Global vector of weak pointers to all contexts +static +WeakContextsWrapper::SharedPtr +get_weak_contexts() +{ + static WeakContextsWrapper::SharedPtr weak_contexts = WeakContextsWrapper::make_shared(); + if (!weak_contexts) { + throw std::runtime_error("weak contexts vector is not valid"); + } + return weak_contexts; +} + +/// Count of contexts that wanted to initialize the logging system. +static +size_t & +get_logging_reference_count() +{ + static size_t ref_count = 0; + return ref_count; +} + +extern "C" +{ +static +void +rclcpp_logging_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) +{ + try { + std::shared_ptr logging_mutex; + logging_mutex = get_global_logging_mutex(); + std::lock_guard guard(*logging_mutex); + return rcl_logging_multiple_output_handler( + location, severity, name, timestamp, format, args); + } catch (std::exception & ex) { + RCUTILS_SAFE_FWRITE_TO_STDERR(ex.what()); + RCUTILS_SAFE_FWRITE_TO_STDERR("\n"); + } catch (...) { + RCUTILS_SAFE_FWRITE_TO_STDERR("failed to take global rclcpp logging mutex\n"); + } +} +} // extern "C" + +Context::Context() +: rcl_context_(nullptr), + shutdown_reason_(""), + logging_mutex_(nullptr) +{} + +Context::~Context() +{ + // acquire the init lock to prevent race conditions with init and shutdown + // this will not prevent errors, but will maybe make them easier to reproduce + std::lock_guard lock(init_mutex_); + try { + // Cannot rely on virtual dispatch in a destructor, so explicitly use the + // shutdown() provided by this base class. + Context::shutdown("context destructor was called while still not shutdown"); + // at this point it is shutdown and cannot reinit + // clean_up will finalize the rcl context + this->clean_up(); + } catch (const std::exception & exc) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "unhandled exception in ~Context(): %s", exc.what()); + } catch (...) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "unhandled exception in ~Context()"); + } +} + +RCLCPP_LOCAL +void +__delete_context(rcl_context_t * context) +{ + if (context) { + if (rcl_context_is_valid(context)) { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), "rcl context unexpectedly not shutdown during cleanup"); + } else { + // if context pointer is not null and is shutdown, then it's ready for fini + rcl_ret_t ret = rcl_context_fini(context); + if (RCL_RET_OK != ret) { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), + "failed to finalize context: %s", rcl_get_error_string().str); + rcl_reset_error(); + } + } + delete context; + } +} + +void +Context::init( + int argc, + char const * const * argv, + const rclcpp::InitOptions & init_options) +{ + std::lock_guard init_lock(init_mutex_); + if (this->is_valid()) { + throw rclcpp::ContextAlreadyInitialized(); + } + this->clean_up(); + rcl_context_t * context = new rcl_context_t; + if (!context) { + throw std::runtime_error("failed to allocate memory for rcl context"); + } + *context = rcl_get_zero_initialized_context(); + rcl_ret_t ret = rcl_init(argc, argv, init_options.get_rcl_init_options(), context); + if (RCL_RET_OK != ret) { + delete context; + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to initialize rcl"); + } + rcl_context_.reset(context, __delete_context); + + try { + if (init_options.auto_initialize_logging()) { + logging_mutex_ = get_global_logging_mutex(); + std::lock_guard guard(*logging_mutex_); + size_t & count = get_logging_reference_count(); + if (0u == count) { + ret = rcl_logging_configure_with_output_handler( + &rcl_context_->global_arguments, + rcl_init_options_get_allocator(init_options.get_rcl_init_options()), + rclcpp_logging_output_handler); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to configure logging"); + } + } else { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "logging was initialized more than once"); + } + ++count; + } + + std::vector unparsed_ros_arguments = detail::get_unparsed_ros_arguments( + argc, argv, &(rcl_context_->global_arguments), rcl_get_default_allocator()); + if (!unparsed_ros_arguments.empty()) { + throw exceptions::UnknownROSArgsError(std::move(unparsed_ros_arguments)); + } + + init_options_ = init_options; + + weak_contexts_ = get_weak_contexts(); + weak_contexts_->add_context(this->shared_from_this()); + } catch (const std::exception & e) { + ret = rcl_shutdown(rcl_context_.get()); + rcl_context_.reset(); + if (RCL_RET_OK != ret) { + std::ostringstream oss; + oss << "While handling: " << e.what() << std::endl << + " another exception was thrown"; + rclcpp::exceptions::throw_from_rcl_error(ret, oss.str()); + } + throw; + } +} + +bool +Context::is_valid() const +{ + // Take a local copy of the shared pointer to avoid it getting nulled under our feet. + auto local_rcl_context = rcl_context_; + if (!local_rcl_context) { + return false; + } + return rcl_context_is_valid(local_rcl_context.get()); +} + +const rclcpp::InitOptions & +Context::get_init_options() const +{ + return init_options_; +} + +rclcpp::InitOptions +Context::get_init_options() +{ + return init_options_; +} + +size_t +Context::get_domain_id() const +{ + size_t domain_id; + rcl_ret_t ret = rcl_context_get_domain_id(rcl_context_.get(), &domain_id); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to get domain id from context"); + } + return domain_id; +} + +std::string +Context::shutdown_reason() const +{ + std::lock_guard lock(init_mutex_); + return shutdown_reason_; +} + +bool +Context::shutdown(const std::string & reason) +{ + // prevent races + std::lock_guard init_lock(init_mutex_); + // ensure validity + if (!this->is_valid()) { + // if it is not valid, then it cannot be shutdown + return false; + } + + // call each pre-shutdown callback + { + std::lock_guard lock{pre_shutdown_callbacks_mutex_}; + for (const auto & callback : pre_shutdown_callbacks_) { + (*callback)(); + } + } + + // rcl shutdown + rcl_ret_t ret = rcl_shutdown(rcl_context_.get()); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + // set shutdown reason + shutdown_reason_ = reason; + // call each shutdown callback + { + std::lock_guard lock(on_shutdown_callbacks_mutex_); + for (const auto & callback : on_shutdown_callbacks_) { + (*callback)(); + } + } + + // interrupt all blocking sleep_for() and all blocking executors or wait sets + this->interrupt_all_sleep_for(); + // remove self from the global contexts + weak_contexts_->remove_context(this); + // shutdown logger + if (logging_mutex_) { + // logging was initialized by this context + std::lock_guard guard(*logging_mutex_); + size_t & count = get_logging_reference_count(); + if (0u == --count) { + rcl_ret_t rcl_ret = rcl_logging_fini(); + if (RCL_RET_OK != rcl_ret) { + RCUTILS_SAFE_FWRITE_TO_STDERR( + RCUTILS_STRINGIFY(__file__) ":" + RCUTILS_STRINGIFY(__LINE__) + " failed to fini logging"); + rcl_reset_error(); + } + } + } + return true; +} + +rclcpp::Context::OnShutdownCallback +Context::on_shutdown(OnShutdownCallback callback) +{ + add_on_shutdown_callback(callback); + return callback; +} + +rclcpp::OnShutdownCallbackHandle +Context::add_on_shutdown_callback(OnShutdownCallback callback) +{ + return add_shutdown_callback(callback); +} + +bool +Context::remove_on_shutdown_callback(const OnShutdownCallbackHandle & callback_handle) +{ + return remove_shutdown_callback(callback_handle); +} + +rclcpp::PreShutdownCallbackHandle +Context::add_pre_shutdown_callback(PreShutdownCallback callback) +{ + return add_shutdown_callback(callback); +} + +bool +Context::remove_pre_shutdown_callback( + const PreShutdownCallbackHandle & callback_handle) +{ + return remove_shutdown_callback(callback_handle); +} + +template +rclcpp::ShutdownCallbackHandle +Context::add_shutdown_callback( + ShutdownCallback callback) +{ + auto callback_shared_ptr = + std::make_shared(callback); + + static_assert( + shutdown_type == ShutdownType::pre_shutdown || shutdown_type == ShutdownType::on_shutdown); + + if constexpr (shutdown_type == ShutdownType::pre_shutdown) { + std::lock_guard lock(pre_shutdown_callbacks_mutex_); + pre_shutdown_callbacks_.emplace_back(callback_shared_ptr); + } else { + std::lock_guard lock(on_shutdown_callbacks_mutex_); + on_shutdown_callbacks_.emplace_back(callback_shared_ptr); + } + + ShutdownCallbackHandle callback_handle; + callback_handle.callback = callback_shared_ptr; + return callback_handle; +} + +template +bool +Context::remove_shutdown_callback( + const ShutdownCallbackHandle & callback_handle) +{ + const auto callback_shared_ptr = callback_handle.callback.lock(); + if (callback_shared_ptr == nullptr) { + return false; + } + + const auto remove_callback = [&callback_shared_ptr](auto & mutex, auto & callback_vector) { + const std::lock_guard lock(mutex); + auto iter = callback_vector.begin(); + for (; iter != callback_vector.end(); iter++) { + if ((*iter).get() == callback_shared_ptr.get()) { + break; + } + } + if (iter == callback_vector.end()) { + return false; + } + callback_vector.erase(iter); + return true; + }; + + static_assert( + shutdown_type == ShutdownType::pre_shutdown || shutdown_type == ShutdownType::on_shutdown); + + if constexpr (shutdown_type == ShutdownType::pre_shutdown) { + return remove_callback(pre_shutdown_callbacks_mutex_, pre_shutdown_callbacks_); + } else { + return remove_callback(on_shutdown_callbacks_mutex_, on_shutdown_callbacks_); + } +} + +std::vector +Context::get_on_shutdown_callbacks() const +{ + return get_shutdown_callback(); +} + +std::vector +Context::get_pre_shutdown_callbacks() const +{ + return get_shutdown_callback(); +} + +template +std::vector +Context::get_shutdown_callback() const +{ + const auto get_callback_vector = [](auto & mutex, auto & callback_set) { + const std::lock_guard lock(mutex); + std::vector callbacks; + for (auto & callback : callback_set) { + callbacks.push_back(*callback); + } + return callbacks; + }; + + static_assert( + shutdown_type == ShutdownType::pre_shutdown || shutdown_type == ShutdownType::on_shutdown); + + if constexpr (shutdown_type == ShutdownType::pre_shutdown) { + return get_callback_vector(pre_shutdown_callbacks_mutex_, pre_shutdown_callbacks_); + } else { + return get_callback_vector(on_shutdown_callbacks_mutex_, on_shutdown_callbacks_); + } +} + +std::shared_ptr +Context::get_rcl_context() +{ + return rcl_context_; +} + +bool +Context::sleep_for(const std::chrono::nanoseconds & nanoseconds) +{ + std::chrono::nanoseconds time_left = nanoseconds; + do { + { + std::unique_lock lock(interrupt_mutex_); + auto start = std::chrono::steady_clock::now(); + // this will release the lock while waiting + interrupt_condition_variable_.wait_for(lock, time_left); + time_left -= std::chrono::steady_clock::now() - start; + } + } while (time_left > std::chrono::nanoseconds::zero() && this->is_valid()); + // Return true if the timeout elapsed successfully, otherwise false. + return this->is_valid(); +} + +void +Context::interrupt_all_sleep_for() +{ + interrupt_condition_variable_.notify_all(); +} + +void +Context::clean_up() +{ + shutdown_reason_ = ""; + rcl_context_.reset(); + sub_contexts_.clear(); +} + +std::vector +rclcpp::get_contexts() +{ + WeakContextsWrapper::SharedPtr weak_contexts = get_weak_contexts(); + return weak_contexts->get_contexts(); +} diff --git a/rclcpp/src/rclcpp/contexts/default_context.cpp b/rclcpp/src/rclcpp/contexts/default_context.cpp index 53c6c9f3f2..3ece873db9 100644 --- a/rclcpp/src/rclcpp/contexts/default_context.cpp +++ b/rclcpp/src/rclcpp/contexts/default_context.cpp @@ -14,13 +14,13 @@ #include "rclcpp/contexts/default_context.hpp" -using rclcpp::contexts::default_context::DefaultContext; +using rclcpp::contexts::DefaultContext; DefaultContext::DefaultContext() {} DefaultContext::SharedPtr -rclcpp::contexts::default_context::get_global_default_context() +rclcpp::contexts::get_global_default_context() { static DefaultContext::SharedPtr default_context = DefaultContext::make_shared(); return default_context; diff --git a/rclcpp/src/rclcpp/create_generic_client.cpp b/rclcpp/src/rclcpp/create_generic_client.cpp new file mode 100644 index 0000000000..4b3b7ddc35 --- /dev/null +++ b/rclcpp/src/rclcpp/create_generic_client.cpp @@ -0,0 +1,44 @@ +// Copyright 2023 Sony Group Corporation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT 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 "rclcpp/create_generic_client.hpp" +#include "rclcpp/generic_client.hpp" + +namespace rclcpp +{ +rclcpp::GenericClient::SharedPtr +create_generic_client( + std::shared_ptr node_base, + std::shared_ptr node_graph, + std::shared_ptr node_services, + const std::string & service_name, + const std::string & service_type, + const rclcpp::QoS & qos, + rclcpp::CallbackGroup::SharedPtr group) +{ + rcl_client_options_t options = rcl_client_get_default_options(); + options.qos = qos.get_rmw_qos_profile(); + + auto cli = rclcpp::GenericClient::make_shared( + node_base.get(), + node_graph, + service_name, + service_type, + options); + + auto cli_base_ptr = std::dynamic_pointer_cast(cli); + node_services->add_client(cli_base_ptr, group); + return cli; +} +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/create_generic_service.cpp b/rclcpp/src/rclcpp/create_generic_service.cpp new file mode 100644 index 0000000000..492635beb2 --- /dev/null +++ b/rclcpp/src/rclcpp/create_generic_service.cpp @@ -0,0 +1,49 @@ +// Copyright 2024 Sony Group Corporation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT 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 "rclcpp/create_generic_service.hpp" +#include "rclcpp/generic_service.hpp" + +namespace rclcpp +{ +rclcpp::GenericService::SharedPtr +create_generic_service( + std::shared_ptr node_base, + std::shared_ptr node_graph, + std::shared_ptr node_services, + const std::string & service_name, + const std::string & service_type, + GenericServiceCallback any_callback, + const rclcpp::QoS & qos, + rclcpp::CallbackGroup::SharedPtr group) +{ + rcl_service_options_t options = rcl_service_get_default_options(); + options.qos = qos.get_rmw_qos_profile(); + + auto srv = rclcpp::GenericService::make_shared( + node_base.get(), + node_graph, + service_name, + service_type, + any_callback, + options); + + auto srv_base_ptr = std::dynamic_pointer_cast(srv); + node_services->add_service(srv_base_ptr, group); + return srv; +} +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/detail/add_guard_condition_to_rcl_wait_set.cpp b/rclcpp/src/rclcpp/detail/add_guard_condition_to_rcl_wait_set.cpp new file mode 100644 index 0000000000..85b4c6594a --- /dev/null +++ b/rclcpp/src/rclcpp/detail/add_guard_condition_to_rcl_wait_set.cpp @@ -0,0 +1,39 @@ +// 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 "rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp" +#include "rclcpp/exceptions.hpp" + +namespace rclcpp +{ +namespace detail +{ + +void +add_guard_condition_to_rcl_wait_set( + rcl_wait_set_t & wait_set, + const rclcpp::GuardCondition & guard_condition) +{ + const auto & gc = guard_condition.get_rcl_guard_condition(); + + rcl_ret_t ret = rcl_wait_set_add_guard_condition(&wait_set, &gc, NULL); + + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "failed to add guard condition to wait set"); + } +} + +} // namespace detail +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/detail/resolve_intra_process_buffer_type.cpp b/rclcpp/src/rclcpp/detail/resolve_intra_process_buffer_type.cpp new file mode 100644 index 0000000000..1ca9892ac4 --- /dev/null +++ b/rclcpp/src/rclcpp/detail/resolve_intra_process_buffer_type.cpp @@ -0,0 +1,37 @@ +// Copyright 2023 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 + +namespace rclcpp +{ + +namespace detail +{ +rclcpp::IntraProcessBufferType +resolve_intra_process_buffer_type( + const rclcpp::IntraProcessBufferType buffer_type) +{ + if (buffer_type == IntraProcessBufferType::CallbackDefault) { + throw std::invalid_argument( + "IntraProcessBufferType::CallbackDefault is not allowed " + "when there is no callback function"); + } + + return buffer_type; +} + +} // namespace detail + +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.cpp b/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.cpp new file mode 100644 index 0000000000..3959e64882 --- /dev/null +++ b/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.cpp @@ -0,0 +1,72 @@ +// 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 "resolve_parameter_overrides.hpp" + +#include +#include +#include + +#include "rcl_yaml_param_parser/parser.h" +#include "rcpputils/scope_exit.hpp" + +#include "rclcpp/parameter_map.hpp" + +std::map +rclcpp::detail::resolve_parameter_overrides( + const std::string & node_fqn, + const std::vector & parameter_overrides, + const rcl_arguments_t * local_args, + const rcl_arguments_t * global_args) +{ + std::map result; + + // global before local so that local overwrites global + std::array argument_sources = {global_args, local_args}; + + // Get fully qualified node name post-remapping to use to find node's params in yaml files + + for (const rcl_arguments_t * source : argument_sources) { + if (!source) { + continue; + } + rcl_params_t * params = NULL; + rcl_ret_t ret = rcl_arguments_get_param_overrides(source, ¶ms); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + if (params) { + auto cleanup_params = rcpputils::make_scope_exit( + [params]() { + rcl_yaml_node_struct_fini(params); + }); + rclcpp::ParameterMap initial_map = rclcpp::parameter_map_from(params, node_fqn.c_str()); + + if (initial_map.count(node_fqn) > 0) { + // Combine parameter yaml files, overwriting values in older ones + for (const rclcpp::Parameter & param : initial_map.at(node_fqn)) { + result[param.get_name()] = + rclcpp::ParameterValue(param.get_value_message()); + } + } + } + } + + // parameter overrides passed to constructor will overwrite overrides from yaml file sources + for (auto & param : parameter_overrides) { + result[param.get_name()] = + rclcpp::ParameterValue(param.get_value_message()); + } + return result; +} diff --git a/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.hpp b/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.hpp new file mode 100644 index 0000000000..5eaeb0a75c --- /dev/null +++ b/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.hpp @@ -0,0 +1,44 @@ +// 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 RCLCPP__DETAIL__RESOLVE_PARAMETER_OVERRIDES_HPP_ +#define RCLCPP__DETAIL__RESOLVE_PARAMETER_OVERRIDES_HPP_ + +#include +#include +#include + +#include "rcl/arguments.h" + +#include "rclcpp/parameter.hpp" +#include "rclcpp/parameter_value.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace detail +{ +/// \internal Get the parameter overrides from the arguments. +RCLCPP_LOCAL +std::map +resolve_parameter_overrides( + const std::string & node_name, + const std::vector & parameter_overrides, + const rcl_arguments_t * local_args, + const rcl_arguments_t * global_args); + +} // namespace detail +} // namespace rclcpp + +#endif // RCLCPP__DETAIL__RESOLVE_PARAMETER_OVERRIDES_HPP_ diff --git a/rclcpp/src/rclcpp/detail/rmw_implementation_specific_payload.cpp b/rclcpp/src/rclcpp/detail/rmw_implementation_specific_payload.cpp new file mode 100644 index 0000000000..be6ec91c9b --- /dev/null +++ b/rclcpp/src/rclcpp/detail/rmw_implementation_specific_payload.cpp @@ -0,0 +1,35 @@ +// 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 + +namespace rclcpp +{ +namespace detail +{ + +bool +RMWImplementationSpecificPayload::has_been_customized() const +{ + return nullptr != this->get_implementation_identifier(); +} + +const char * +RMWImplementationSpecificPayload::get_implementation_identifier() const +{ + return nullptr; +} + +} // namespace detail +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/detail/rmw_implementation_specific_publisher_payload.cpp b/rclcpp/src/rclcpp/detail/rmw_implementation_specific_publisher_payload.cpp new file mode 100644 index 0000000000..cd7580d493 --- /dev/null +++ b/rclcpp/src/rclcpp/detail/rmw_implementation_specific_publisher_payload.cpp @@ -0,0 +1,33 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "rcl/publisher.h" + +namespace rclcpp +{ +namespace detail +{ + +void +RMWImplementationSpecificPublisherPayload::modify_rmw_publisher_options( + rmw_publisher_options_t & rmw_publisher_options) const +{ + // By default, do not mutate the rmw publisher options. + (void)rmw_publisher_options; +} + +} // namespace detail +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/detail/rmw_implementation_specific_subscription_payload.cpp b/rclcpp/src/rclcpp/detail/rmw_implementation_specific_subscription_payload.cpp new file mode 100644 index 0000000000..c84c3d4b03 --- /dev/null +++ b/rclcpp/src/rclcpp/detail/rmw_implementation_specific_subscription_payload.cpp @@ -0,0 +1,33 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "rcl/subscription.h" + +namespace rclcpp +{ +namespace detail +{ + +void +RMWImplementationSpecificSubscriptionPayload::modify_rmw_subscription_options( + rmw_subscription_options_t & rmw_subscription_options) const +{ + // By default, do not mutate the rmw subscription options. + (void)rmw_subscription_options; +} + +} // namespace detail +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/detail/utilities.cpp b/rclcpp/src/rclcpp/detail/utilities.cpp new file mode 100644 index 0000000000..0166f2dceb --- /dev/null +++ b/rclcpp/src/rclcpp/detail/utilities.cpp @@ -0,0 +1,64 @@ +// 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 "rclcpp/detail/utilities.hpp" + +#include +#include +#include +#include + +#include "rclcpp/exceptions.hpp" + +#include "rcl/allocator.h" +#include "rcl/arguments.h" + +namespace rclcpp +{ +namespace detail +{ + +std::vector +get_unparsed_ros_arguments( + int argc, char const * const * argv, + rcl_arguments_t * arguments, + rcl_allocator_t allocator) +{ + (void)argc; + std::vector unparsed_ros_arguments; + int unparsed_ros_args_count = rcl_arguments_get_count_unparsed_ros(arguments); + if (unparsed_ros_args_count > 0) { + int * unparsed_ros_args_indices = nullptr; + rcl_ret_t ret = + rcl_arguments_get_unparsed_ros(arguments, allocator, &unparsed_ros_args_indices); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to get unparsed ROS arguments"); + } + try { + for (int i = 0; i < unparsed_ros_args_count; ++i) { + assert(unparsed_ros_args_indices[i] >= 0); + assert(unparsed_ros_args_indices[i] < argc); + unparsed_ros_arguments.push_back(argv[unparsed_ros_args_indices[i]]); + } + allocator.deallocate(unparsed_ros_args_indices, allocator.state); + } catch (...) { + allocator.deallocate(unparsed_ros_args_indices, allocator.state); + throw; + } + } + return unparsed_ros_arguments; +} + +} // namespace detail +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/duration.cpp b/rclcpp/src/rclcpp/duration.cpp index b2338519dc..7cb678456e 100644 --- a/rclcpp/src/rclcpp/duration.cpp +++ b/rclcpp/src/rclcpp/duration.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include #include @@ -27,6 +28,8 @@ #include "rcutils/logging_macros.h" +#include "rclcpp/utilities.hpp" + namespace rclcpp { @@ -36,26 +39,19 @@ Duration::Duration(int32_t seconds, uint32_t nanoseconds) rcl_duration_.nanoseconds += nanoseconds; } -Duration::Duration(int64_t nanoseconds) -{ - rcl_duration_.nanoseconds = nanoseconds; -} - Duration::Duration(std::chrono::nanoseconds nanoseconds) { rcl_duration_.nanoseconds = nanoseconds.count(); } -Duration::Duration(const Duration & rhs) -{ - rcl_duration_.nanoseconds = rhs.rcl_duration_.nanoseconds; -} +Duration::Duration(const Duration & rhs) = default; Duration::Duration( const builtin_interfaces::msg::Duration & duration_msg) { - rcl_duration_.nanoseconds = RCL_S_TO_NS(static_cast(duration_msg.sec)); - rcl_duration_.nanoseconds += duration_msg.nanosec; + rcl_duration_.nanoseconds = + RCL_S_TO_NS(static_cast(duration_msg.sec)); + rcl_duration_.nanoseconds += static_cast(duration_msg.nanosec); } Duration::Duration(const rcl_duration_t & duration) @@ -64,34 +60,42 @@ Duration::Duration(const rcl_duration_t & duration) // noop } -Duration::~Duration() -{ -} - Duration::operator builtin_interfaces::msg::Duration() const { builtin_interfaces::msg::Duration msg_duration; - msg_duration.sec = static_cast(RCL_NS_TO_S(rcl_duration_.nanoseconds)); - msg_duration.nanosec = - static_cast(rcl_duration_.nanoseconds % (1000 * 1000 * 1000)); + constexpr rcl_duration_value_t kDivisor = RCL_S_TO_NS(1); + constexpr int32_t max_s = std::numeric_limits::max(); + constexpr int32_t min_s = std::numeric_limits::min(); + constexpr uint32_t max_ns = std::numeric_limits::max(); + const auto result = std::div(rcl_duration_.nanoseconds, kDivisor); + if (result.rem >= 0) { + // saturate if we will overflow + if (result.quot > max_s) { + msg_duration.sec = max_s; + msg_duration.nanosec = max_ns; + } else { + msg_duration.sec = static_cast(result.quot); + msg_duration.nanosec = static_cast(result.rem); + } + } else { + if (result.quot <= min_s) { + msg_duration.sec = min_s; + msg_duration.nanosec = 0u; + } else { + msg_duration.sec = static_cast(result.quot - 1); + msg_duration.nanosec = static_cast(kDivisor + result.rem); + } + } return msg_duration; } Duration & -Duration::operator=(const Duration & rhs) -{ - rcl_duration_.nanoseconds = rhs.rcl_duration_.nanoseconds; - return *this; -} +Duration::operator=(const Duration & rhs) = default; Duration & Duration::operator=(const builtin_interfaces::msg::Duration & duration_msg) { - if (duration_msg.sec < 0) { - throw std::runtime_error("cannot store a negative duration point in rclcpp::Duration"); - } - rcl_duration_.nanoseconds = RCL_S_TO_NS(static_cast(duration_msg.sec)); - rcl_duration_.nanoseconds += duration_msg.nanosec; + *this = Duration(duration_msg); return *this; } @@ -101,6 +105,12 @@ Duration::operator==(const rclcpp::Duration & rhs) const return rcl_duration_.nanoseconds == rhs.rcl_duration_.nanoseconds; } +bool +Duration::operator!=(const rclcpp::Duration & rhs) const +{ + return rcl_duration_.nanoseconds != rhs.rcl_duration_.nanoseconds; +} + bool Duration::operator<(const rclcpp::Duration & rhs) const { @@ -128,15 +138,15 @@ Duration::operator>(const rclcpp::Duration & rhs) const void bounds_check_duration_sum(int64_t lhsns, int64_t rhsns, uint64_t max) { - auto abs_lhs = (uint64_t)std::abs(lhsns); - auto abs_rhs = (uint64_t)std::abs(rhsns); + auto abs_lhs = static_cast(std::abs(lhsns)); + auto abs_rhs = static_cast(std::abs(rhsns)); if (lhsns > 0 && rhsns > 0) { - if (abs_lhs + abs_rhs > (uint64_t) max) { + if (abs_lhs + abs_rhs > max) { throw std::overflow_error("addition leads to int64_t overflow"); } } else if (lhsns < 0 && rhsns < 0) { - if (abs_lhs + abs_rhs > (uint64_t) max) { + if (abs_lhs + abs_rhs > max) { throw std::underflow_error("addition leads to int64_t underflow"); } } @@ -149,22 +159,29 @@ Duration::operator+(const rclcpp::Duration & rhs) const this->rcl_duration_.nanoseconds, rhs.rcl_duration_.nanoseconds, std::numeric_limits::max()); - return Duration( + return Duration::from_nanoseconds( rcl_duration_.nanoseconds + rhs.rcl_duration_.nanoseconds); } +Duration & +Duration::operator+=(const rclcpp::Duration & rhs) +{ + *this = *this + rhs; + return *this; +} + void bounds_check_duration_difference(int64_t lhsns, int64_t rhsns, uint64_t max) { - auto abs_lhs = (uint64_t)std::abs(lhsns); - auto abs_rhs = (uint64_t)std::abs(rhsns); + auto abs_lhs = static_cast(std::abs(lhsns)); + auto abs_rhs = static_cast(std::abs(rhsns)); if (lhsns > 0 && rhsns < 0) { - if (abs_lhs + abs_rhs > (uint64_t) max) { + if (abs_lhs + abs_rhs > max) { throw std::overflow_error("duration subtraction leads to int64_t overflow"); } } else if (lhsns < 0 && rhsns > 0) { - if (abs_lhs + abs_rhs > (uint64_t) max) { + if (abs_lhs + abs_rhs > max) { throw std::underflow_error("duration subtraction leads to int64_t underflow"); } } @@ -178,14 +195,173 @@ Duration::operator-(const rclcpp::Duration & rhs) const rhs.rcl_duration_.nanoseconds, std::numeric_limits::max()); - return Duration( + return Duration::from_nanoseconds( rcl_duration_.nanoseconds - rhs.rcl_duration_.nanoseconds); } +Duration & +Duration::operator-=(const rclcpp::Duration & rhs) +{ + *this = *this - rhs; + return *this; +} + +void +bounds_check_duration_scale(int64_t dns, double scale, uint64_t max) +{ + auto abs_dns = static_cast(std::abs(dns)); + auto abs_scale = std::abs(scale); + if (abs_scale > 1.0 && abs_dns > + static_cast(static_cast(max) / static_cast(abs_scale))) + { + if ((dns > 0 && scale > 0) || (dns < 0 && scale < 0)) { + throw std::overflow_error("duration scaling leads to int64_t overflow"); + } else { + throw std::underflow_error("duration scaling leads to int64_t underflow"); + } + } +} + +Duration +Duration::operator*(double scale) const +{ + if (!std::isfinite(scale)) { + throw std::runtime_error("abnormal scale in rclcpp::Duration"); + } + bounds_check_duration_scale( + this->rcl_duration_.nanoseconds, + scale, + std::numeric_limits::max()); + long double scale_ld = static_cast(scale); + return Duration::from_nanoseconds( + static_cast( + static_cast(rcl_duration_.nanoseconds) * scale_ld)); +} + +Duration & +Duration::operator*=(double scale) +{ + *this = *this * scale; + return *this; +} + rcl_duration_value_t Duration::nanoseconds() const { return rcl_duration_.nanoseconds; } +Duration +Duration::max() +{ + return Duration(std::numeric_limits::max(), 999999999); +} + +double +Duration::seconds() const +{ + return std::chrono::duration(std::chrono::nanoseconds(rcl_duration_.nanoseconds)).count(); +} + +rmw_time_t +Duration::to_rmw_time() const +{ + if (rcl_duration_.nanoseconds < 0) { + throw std::runtime_error("rmw_time_t cannot be negative"); + } + + // Purposefully avoid creating from builtin_interfaces::msg::Duration + // to avoid possible overflow converting from int64_t to int32_t, then back to uint64_t + rmw_time_t result; + constexpr rcl_duration_value_t kDivisor = RCL_S_TO_NS(1); + const auto div_result = std::div(rcl_duration_.nanoseconds, kDivisor); + result.sec = static_cast(div_result.quot); + result.nsec = static_cast(div_result.rem); + + return result; +} + +Duration +Duration::from_rmw_time(rmw_time_t duration) +{ + Duration ret; + constexpr rcl_duration_value_t limit_ns = std::numeric_limits::max(); + constexpr rcl_duration_value_t limit_sec = RCL_NS_TO_S(limit_ns); + if (duration.sec > limit_sec || duration.nsec > limit_ns) { + // saturate if will overflow + ret.rcl_duration_.nanoseconds = limit_ns; + return ret; + } + uint64_t total_ns = RCL_S_TO_NS(duration.sec) + duration.nsec; + if (total_ns > limit_ns) { + // saturate if will overflow + ret.rcl_duration_.nanoseconds = limit_ns; + return ret; + } + ret.rcl_duration_.nanoseconds = static_cast(total_ns); + return ret; +} + +Duration +Duration::from_seconds(double seconds) +{ + Duration ret; + ret.rcl_duration_.nanoseconds = static_cast(RCL_S_TO_NS(seconds)); + return ret; +} + +Duration +Duration::from_nanoseconds(rcl_duration_value_t nanoseconds) +{ + Duration ret; + ret.rcl_duration_.nanoseconds = nanoseconds; + return ret; +} + +builtin_interfaces::msg::Time +operator+(const builtin_interfaces::msg::Time & lhs, const rclcpp::Duration & rhs) +{ + if (lhs.sec < 0) { + throw std::runtime_error("message time is negative"); + } + + rcl_time_point_value_t rcl_time; + rcl_time = RCL_S_TO_NS(static_cast(lhs.sec)); + rcl_time += lhs.nanosec; + + if (rclcpp::add_will_overflow(rcl_time, rhs.nanoseconds())) { + throw std::overflow_error("addition leads to int64_t overflow"); + } + if (rclcpp::add_will_underflow(rcl_time, rhs.nanoseconds())) { + throw std::underflow_error("addition leads to int64_t underflow"); + } + + rcl_time += rhs.nanoseconds(); + + return convert_rcl_time_to_sec_nanos(rcl_time); +} + +builtin_interfaces::msg::Time +operator-(const builtin_interfaces::msg::Time & lhs, const rclcpp::Duration & rhs) +{ + if (lhs.sec < 0) { + throw std::runtime_error("message time is negative"); + } + + rcl_time_point_value_t rcl_time; + rcl_time = RCL_S_TO_NS(static_cast(lhs.sec)); + rcl_time += lhs.nanosec; + + if (rclcpp::sub_will_overflow(rcl_time, rhs.nanoseconds())) { + throw std::overflow_error("addition leads to int64_t overflow"); + } + if (rclcpp::sub_will_underflow(rcl_time, rhs.nanoseconds())) { + throw std::underflow_error("addition leads to int64_t underflow"); + } + + rcl_time -= rhs.nanoseconds(); + + return convert_rcl_time_to_sec_nanos(rcl_time); +} + } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message.cpp b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message.cpp new file mode 100644 index 0000000000..c340b107b6 --- /dev/null +++ b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message.cpp @@ -0,0 +1,40 @@ +// Copyright 2023 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 "rcl/allocator.h" +#include "rcl/types.h" +#include "rcutils/logging_macros.h" + +#include "rclcpp/dynamic_typesupport/dynamic_message.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_message_type.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_message_type_builder.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp" +#include "rclcpp/exceptions.hpp" + + +using rclcpp::dynamic_typesupport::DynamicMessage; +using rclcpp::dynamic_typesupport::DynamicMessageType; +using rclcpp::dynamic_typesupport::DynamicMessageTypeBuilder; +using rclcpp::dynamic_typesupport::DynamicSerializationSupport; + +DynamicMessage::~DynamicMessage() +{} // STUBBED diff --git a/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type.cpp b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type.cpp new file mode 100644 index 0000000000..069aff03aa --- /dev/null +++ b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type.cpp @@ -0,0 +1,38 @@ +// Copyright 2023 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 "rcutils/logging_macros.h" + +#include "rclcpp/dynamic_typesupport/dynamic_message.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_message_type.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_message_type_builder.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp" +#include "rclcpp/exceptions.hpp" + + +using rclcpp::dynamic_typesupport::DynamicMessage; +using rclcpp::dynamic_typesupport::DynamicMessageType; +using rclcpp::dynamic_typesupport::DynamicMessageTypeBuilder; +using rclcpp::dynamic_typesupport::DynamicSerializationSupport; + +DynamicMessageType::~DynamicMessageType() +{} // STUBBED diff --git a/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type_builder.cpp b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type_builder.cpp new file mode 100644 index 0000000000..1c5d3708a1 --- /dev/null +++ b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type_builder.cpp @@ -0,0 +1,37 @@ +// Copyright 2023 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 "rcutils/logging_macros.h" + +#include "rclcpp/dynamic_typesupport/dynamic_message.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_message_type.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_message_type_builder.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp" +#include "rclcpp/exceptions.hpp" + +using rclcpp::dynamic_typesupport::DynamicMessage; +using rclcpp::dynamic_typesupport::DynamicMessageType; +using rclcpp::dynamic_typesupport::DynamicMessageTypeBuilder; +using rclcpp::dynamic_typesupport::DynamicSerializationSupport; + +DynamicMessageTypeBuilder::~DynamicMessageTypeBuilder() +{} // STUBBED diff --git a/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type_support.cpp b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type_support.cpp new file mode 100644 index 0000000000..3a920c2805 --- /dev/null +++ b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type_support.cpp @@ -0,0 +1,49 @@ +// Copyright 2023 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 + +#include "rcl/allocator.h" +#include "rcl/dynamic_message_type_support.h" +#include "rcl/type_hash.h" +#include "rcl/types.h" +#include "rcutils/logging_macros.h" +#include "rcutils/types/rcutils_ret.h" +#include "rmw/dynamic_message_type_support.h" + +#include "rclcpp/dynamic_typesupport/dynamic_message.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_message_type.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_message_type_support.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp" +#include "rclcpp/exceptions.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" + +using rclcpp::dynamic_typesupport::DynamicMessage; +using rclcpp::dynamic_typesupport::DynamicMessageType; +using rclcpp::dynamic_typesupport::DynamicMessageTypeSupport; +using rclcpp::dynamic_typesupport::DynamicSerializationSupport; + +DynamicMessageTypeSupport::~DynamicMessageTypeSupport() +{} // STUBBED diff --git a/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_serialization_support.cpp b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_serialization_support.cpp new file mode 100644 index 0000000000..769f5fb8cf --- /dev/null +++ b/rclcpp/src/rclcpp/dynamic_typesupport/dynamic_serialization_support.cpp @@ -0,0 +1,46 @@ +// Copyright 2023 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 "rclcpp/dynamic_typesupport/dynamic_serialization_support.hpp" +#include "rclcpp/exceptions.hpp" + +using rclcpp::dynamic_typesupport::DynamicSerializationSupport; + +// CONSTRUCTION ==================================================================================== +DynamicSerializationSupport::DynamicSerializationSupport(rcl_allocator_t allocator) +: DynamicSerializationSupport::DynamicSerializationSupport("", allocator) +{ + throw std::runtime_error("Unimplemented"); +} + +DynamicSerializationSupport::DynamicSerializationSupport( + const std::string & /*serialization_library_name*/, + rcl_allocator_t /*allocator*/) +: rosidl_serialization_support_( + rosidl_dynamic_typesupport_get_zero_initialized_serialization_support()) +{ + throw std::runtime_error("Unimplemented"); +} + +DynamicSerializationSupport::~DynamicSerializationSupport() +{} // STUBBED diff --git a/rclcpp/src/rclcpp/event_handler.cpp b/rclcpp/src/rclcpp/event_handler.cpp new file mode 100644 index 0000000000..630bc26d33 --- /dev/null +++ b/rclcpp/src/rclcpp/event_handler.cpp @@ -0,0 +1,91 @@ +// 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 "rcl/error_handling.h" +#include "rcl/event.h" + +#include "rclcpp/event_handler.hpp" +#include "rclcpp/exceptions/exceptions.hpp" + +namespace rclcpp +{ + +UnsupportedEventTypeException::UnsupportedEventTypeException( + rcl_ret_t ret, + const rcl_error_state_t * error_state, + const std::string & prefix) +: UnsupportedEventTypeException(exceptions::RCLErrorBase(ret, error_state), prefix) +{} + +UnsupportedEventTypeException::UnsupportedEventTypeException( + const exceptions::RCLErrorBase & base_exc, + const std::string & prefix) +: exceptions::RCLErrorBase(base_exc), + std::runtime_error(prefix + (prefix.empty() ? "" : ": ") + base_exc.formatted_message) +{} + +EventHandlerBase::~EventHandlerBase() +{ + if (rcl_event_fini(&event_handle_) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Error in destruction of rcl event handle: %s", rcl_get_error_string().str); + rcl_reset_error(); + } +} + +/// Get the number of ready events. +size_t +EventHandlerBase::get_number_of_ready_events() +{ + return 1; +} + +/// Add the Waitable to a wait set. +void +EventHandlerBase::add_to_wait_set(rcl_wait_set_t & wait_set) +{ + rcl_ret_t ret = rcl_wait_set_add_event(&wait_set, &event_handle_, &wait_set_event_index_); + if (RCL_RET_OK != ret) { + exceptions::throw_from_rcl_error(ret, "Couldn't add event to wait set"); + } +} + +/// Check if the Waitable is ready. +bool +EventHandlerBase::is_ready(const rcl_wait_set_t & wait_set) +{ + return wait_set.events[wait_set_event_index_] == &event_handle_; +} + +void +EventHandlerBase::set_on_new_event_callback( + rcl_event_callback_t callback, + const void * user_data) +{ + rcl_ret_t ret = rcl_event_set_callback( + &event_handle_, + callback, + user_data); + + if (RCL_RET_OK != ret) { + using rclcpp::exceptions::throw_from_rcl_error; + throw_from_rcl_error(ret, "failed to set the on new message callback for Event"); + } +} + +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/exceptions.cpp b/rclcpp/src/rclcpp/exceptions/exceptions.cpp similarity index 68% rename from rclcpp/src/rclcpp/exceptions.cpp rename to rclcpp/src/rclcpp/exceptions/exceptions.cpp index bbaa83b17c..4371066c84 100644 --- a/rclcpp/src/rclcpp/exceptions.cpp +++ b/rclcpp/src/rclcpp/exceptions/exceptions.cpp @@ -17,6 +17,7 @@ #include #include #include +#include using namespace std::string_literals; @@ -39,12 +40,12 @@ NameValidationError::format_error( return msg; } -void -throw_from_rcl_error( +std::exception_ptr +from_rcl_error( rcl_ret_t ret, const std::string & prefix, const rcl_error_state_t * error_state, - void (*reset_error)()) + void (* reset_error)()) { if (RCL_RET_OK == ret) { throw std::invalid_argument("ret is RCL_RET_OK"); @@ -55,9 +56,9 @@ throw_from_rcl_error( if (!error_state) { throw std::runtime_error("rcl error state is not set"); } - std::string formated_prefix = prefix; + std::string formatted_prefix = prefix; if (!prefix.empty()) { - formated_prefix += ": "; + formatted_prefix += ": "; } RCLErrorBase base_exc(ret, error_state); if (reset_error) { @@ -65,17 +66,33 @@ throw_from_rcl_error( } switch (ret) { case RCL_RET_BAD_ALLOC: - throw RCLBadAlloc(base_exc); + return std::make_exception_ptr(RCLBadAlloc(base_exc)); case RCL_RET_INVALID_ARGUMENT: - throw RCLInvalidArgument(base_exc, formated_prefix); + return std::make_exception_ptr(RCLInvalidArgument(base_exc, formatted_prefix)); + case RCL_RET_INVALID_ROS_ARGS: + return std::make_exception_ptr(RCLInvalidROSArgsError(base_exc, formatted_prefix)); default: - throw RCLError(base_exc, formated_prefix); + return std::make_exception_ptr(RCLError(base_exc, formatted_prefix)); } } +void +throw_from_rcl_error( + rcl_ret_t ret, + const std::string & prefix, + const rcl_error_state_t * error_state, + void (* reset_error)()) +{ + // We expect this to either throw a standard error, + // or to generate an error pointer (which is caught + // in err, and immediately thrown) + auto err = from_rcl_error(ret, prefix, error_state, reset_error); + std::rethrow_exception(err); +} + RCLErrorBase::RCLErrorBase(rcl_ret_t ret, const rcl_error_state_t * error_state) : ret(ret), message(error_state->message), file(error_state->file), line(error_state->line_number), - formatted_message(rcl_get_error_string_safe()) + formatted_message(rcl_get_error_string().str) {} RCLError::RCLError( @@ -112,5 +129,18 @@ RCLInvalidArgument::RCLInvalidArgument( : RCLErrorBase(base_exc), std::invalid_argument(prefix + base_exc.formatted_message) {} +RCLInvalidROSArgsError::RCLInvalidROSArgsError( + rcl_ret_t ret, + const rcl_error_state_t * error_state, + const std::string & prefix) +: RCLInvalidROSArgsError(RCLErrorBase(ret, error_state), prefix) +{} + +RCLInvalidROSArgsError::RCLInvalidROSArgsError( + const RCLErrorBase & base_exc, + const std::string & prefix) +: RCLErrorBase(base_exc), std::runtime_error(prefix + base_exc.formatted_message) +{} + } // namespace exceptions } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/executable_list.cpp b/rclcpp/src/rclcpp/executable_list.cpp new file mode 100644 index 0000000000..07edbc9586 --- /dev/null +++ b/rclcpp/src/rclcpp/executable_list.cpp @@ -0,0 +1,84 @@ +// Copyright 2019 Nobleo Technology +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT 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 "rclcpp/experimental/executable_list.hpp" + +using rclcpp::experimental::ExecutableList; + +ExecutableList::ExecutableList() +: number_of_subscriptions(0), + number_of_timers(0), + number_of_services(0), + number_of_clients(0), + number_of_waitables(0) +{} + +ExecutableList::~ExecutableList() +{} + +void +ExecutableList::clear() +{ + this->timer.clear(); + this->number_of_timers = 0; + + this->subscription.clear(); + this->number_of_subscriptions = 0; + + this->service.clear(); + this->number_of_services = 0; + + this->client.clear(); + this->number_of_clients = 0; + + this->waitable.clear(); + this->number_of_waitables = 0; +} + +void +ExecutableList::add_subscription(rclcpp::SubscriptionBase::SharedPtr subscription) +{ + this->subscription.push_back(std::move(subscription)); + this->number_of_subscriptions++; +} + +void +ExecutableList::add_timer(rclcpp::TimerBase::SharedPtr timer) +{ + this->timer.push_back(std::move(timer)); + this->number_of_timers++; +} + +void +ExecutableList::add_service(rclcpp::ServiceBase::SharedPtr service) +{ + this->service.push_back(std::move(service)); + this->number_of_services++; +} + +void +ExecutableList::add_client(rclcpp::ClientBase::SharedPtr client) +{ + this->client.push_back(std::move(client)); + this->number_of_clients++; +} + +void +ExecutableList::add_waitable(rclcpp::Waitable::SharedPtr waitable) +{ + this->waitable.push_back(std::move(waitable)); + this->number_of_waitables++; +} diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index eb31da6692..69131cc111 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -13,119 +13,203 @@ // limitations under the License. #include +#include +#include +#include #include +#include #include #include +#include +#include #include "rcl/allocator.h" #include "rcl/error_handling.h" +#include "rclcpp/executors/executor_notify_waitable.hpp" +#include "rclcpp/subscription_wait_set_mask.hpp" +#include "rcpputils/scope_exit.hpp" +#include "rclcpp/dynamic_typesupport/dynamic_message.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp/executor.hpp" +#include "rclcpp/guard_condition.hpp" #include "rclcpp/node.hpp" -#include "rclcpp/scope_exit.hpp" #include "rclcpp/utilities.hpp" -#include "rcl_interfaces/msg/intra_process_message.hpp" +#include "rcutils/logging_macros.h" -using rclcpp::executor::AnyExecutable; -using rclcpp::executor::Executor; -using rclcpp::executor::ExecutorArgs; -using rclcpp::executor::FutureReturnCode; +#include "tracetools/tracetools.h" -Executor::Executor(const ExecutorArgs & args) +using namespace std::chrono_literals; + +using rclcpp::Executor; + +/// Mask to indicate to the waitset to only add the subscription. +/// The events and intraprocess waitable are already added via the callback group. +static constexpr rclcpp::SubscriptionWaitSetMask kDefaultSubscriptionMask = {true, false, false}; + +class rclcpp::ExecutorImplementation {}; + +Executor::Executor(const std::shared_ptr & context) : spinning(false), - memory_strategy_(args.memory_strategy) + entities_need_rebuild_(true), + collector_(nullptr), + wait_set_({}, {}, {}, {}, {}, {}, context) { - rcl_guard_condition_options_t guard_condition_options = rcl_guard_condition_get_default_options(); - if (rcl_guard_condition_init( - &interrupt_guard_condition_, guard_condition_options) != RCL_RET_OK) - { - throw std::runtime_error( - std::string("Failed to create interrupt guard condition in Executor constructor: ") + - rcl_get_error_string_safe()); - } +} - // The number of guard conditions is always at least 2: 1 for the ctrl-c guard cond, - // and one for the executor's guard cond (interrupt_guard_condition_) +Executor::Executor(const rclcpp::ExecutorOptions & options) +: spinning(false), + interrupt_guard_condition_(std::make_shared(options.context)), + shutdown_guard_condition_(std::make_shared(options.context)), + context_(options.context), + notify_waitable_(std::make_shared( + [this]() { + this->entities_need_rebuild_.store(true); + })), + entities_need_rebuild_(true), + collector_(notify_waitable_), + wait_set_({}, {}, {}, {}, {}, {}, options.context), + current_notify_waitable_(notify_waitable_), + impl_(std::make_unique()) +{ + shutdown_callback_handle_ = context_->add_on_shutdown_callback( + [weak_gc = std::weak_ptr{shutdown_guard_condition_}]() { + auto strong_gc = weak_gc.lock(); + if (strong_gc) { + strong_gc->trigger(); + } + }); - // Put the global ctrl-c guard condition in - memory_strategy_->add_guard_condition(rclcpp::get_sigint_guard_condition(&wait_set_)); + notify_waitable_->add_guard_condition(interrupt_guard_condition_); + notify_waitable_->add_guard_condition(shutdown_guard_condition_); - // Put the executor's guard condition in - memory_strategy_->add_guard_condition(&interrupt_guard_condition_); - rcl_allocator_t allocator = memory_strategy_->get_allocator(); + wait_set_.add_waitable(notify_waitable_); +} - if (rcl_wait_set_init( - &wait_set_, 0, 2, 0, 0, 0, allocator) != RCL_RET_OK) - { - fprintf(stderr, - "[rclcpp::error] failed to create wait set: %s\n", rcl_get_error_string_safe()); +Executor::~Executor() +{ + std::lock_guard guard(mutex_); + + notify_waitable_->remove_guard_condition(interrupt_guard_condition_); + notify_waitable_->remove_guard_condition(shutdown_guard_condition_); + current_collection_.timers.update( + {}, {}, + [this](auto timer) {wait_set_.remove_timer(timer);}); + + current_collection_.subscriptions.update( + {}, {}, + [this](auto subscription) { + wait_set_.remove_subscription(subscription, kDefaultSubscriptionMask); + }); + + current_collection_.clients.update( + {}, {}, + [this](auto client) {wait_set_.remove_client(client);}); + + current_collection_.services.update( + {}, {}, + [this](auto service) {wait_set_.remove_service(service);}); + + current_collection_.guard_conditions.update( + {}, {}, + [this](auto guard_condition) {wait_set_.remove_guard_condition(guard_condition);}); + + current_collection_.waitables.update( + {}, {}, + [this](auto waitable) {wait_set_.remove_waitable(waitable);}); + + // Remove shutdown callback handle registered to Context + if (!context_->remove_on_shutdown_callback(shutdown_callback_handle_)) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "failed to remove registered on_shutdown callback"); rcl_reset_error(); - if (rcl_guard_condition_fini(&interrupt_guard_condition_) != RCL_RET_OK) { - fprintf(stderr, - "[rclcpp::error] failed to destroy guard condition: %s\n", rcl_get_error_string_safe()); - rcl_reset_error(); - } - throw std::runtime_error("Failed to create wait set in Executor constructor"); } } -Executor::~Executor() +void +Executor::handle_updated_entities(bool notify) { - // Disassocate all nodes - for (auto & weak_node : weak_nodes_) { - auto node = weak_node.lock(); - if (node) { - std::atomic_bool & has_executor = node->get_associated_with_executor_atomic(); - has_executor.store(false); - } + this->entities_need_rebuild_.store(true); + + if (!spinning.load() && entities_need_rebuild_.exchange(false)) { + std::lock_guard guard(mutex_); + this->collect_entities(); } - weak_nodes_.clear(); - // Finalize the wait set. - if (rcl_wait_set_fini(&wait_set_) != RCL_RET_OK) { - fprintf(stderr, - "[rclcpp::error] failed to destroy wait set: %s\n", rcl_get_error_string_safe()); - rcl_reset_error(); + if (notify) { + interrupt_guard_condition_->trigger(); } - // Finalize the interrupt guard condition. - if (rcl_guard_condition_fini(&interrupt_guard_condition_) != RCL_RET_OK) { - fprintf(stderr, - "[rclcpp::error] failed to destroy guard condition: %s\n", rcl_get_error_string_safe()); - rcl_reset_error(); +} + +std::vector +Executor::get_all_callback_groups() +{ + this->collector_.update_collections(); + return this->collector_.get_all_callback_groups(); +} + +std::vector +Executor::get_manually_added_callback_groups() +{ + this->collector_.update_collections(); + return this->collector_.get_manually_added_callback_groups(); +} + +std::vector +Executor::get_automatically_added_callback_groups_from_nodes() +{ + this->collector_.update_collections(); + return this->collector_.get_automatically_added_callback_groups(); +} + +void +Executor::add_callback_group( + rclcpp::CallbackGroup::SharedPtr group_ptr, + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, + bool notify) +{ + (void) node_ptr; + this->collector_.add_callback_group(group_ptr); + + try { + this->handle_updated_entities(notify); + } catch (const rclcpp::exceptions::RCLError & ex) { + throw std::runtime_error( + std::string( + "Failed to handle entities update on callback group add: ") + ex.what()); } - // Remove and release the sigint guard condition - memory_strategy_->remove_guard_condition( - rclcpp::get_sigint_guard_condition(&wait_set_)); - rclcpp::release_sigint_guard_condition(&wait_set_); } void Executor::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify) { - // If the node already has an executor - std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic(); - if (has_executor.exchange(true)) { - throw std::runtime_error("Node has already been added to an executor."); - } - // Check to ensure node not already added - for (auto & weak_node : weak_nodes_) { - auto node = weak_node.lock(); - if (node == node_ptr) { - // TODO(jacquelinekay): Use a different error here? - throw std::runtime_error("Cannot add node to executor, node already added."); - } + this->collector_.add_node(node_ptr); + + try { + this->handle_updated_entities(notify); + } catch (const rclcpp::exceptions::RCLError & ex) { + throw std::runtime_error( + std::string( + "Failed to handle entities update on node add: ") + ex.what()); } - weak_nodes_.push_back(node_ptr); - if (notify) { - // Interrupt waiting to handle new node - if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) { - throw std::runtime_error(rcl_get_error_string_safe()); - } +} + +void +Executor::remove_callback_group( + rclcpp::CallbackGroup::SharedPtr group_ptr, + bool notify) +{ + this->collector_.remove_callback_group(group_ptr); + + try { + this->handle_updated_entities(notify); + } catch (const rclcpp::exceptions::RCLError & ex) { + throw std::runtime_error( + std::string( + "Failed to handle entities update on callback group remove: ") + ex.what()); } - // Add the node's notify condition to the guard condition handles - memory_strategy_->add_guard_condition(node_ptr->get_notify_guard_condition()); } void @@ -137,29 +221,15 @@ Executor::add_node(std::shared_ptr node_ptr, bool notify) void Executor::remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify) { - bool node_removed = false; - weak_nodes_.erase( - std::remove_if( - weak_nodes_.begin(), weak_nodes_.end(), - [&](rclcpp::node_interfaces::NodeBaseInterface::WeakPtr & i) - { - bool matched = (i.lock() == node_ptr); - node_removed |= matched; - return matched; - } - ) - ); - std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic(); - has_executor.store(false); - if (notify) { - // If the node was matched and removed, interrupt waiting - if (node_removed) { - if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) { - throw std::runtime_error(rcl_get_error_string_safe()); - } - } + this->collector_.remove_node(node_ptr); + + try { + this->handle_updated_entities(notify); + } catch (const rclcpp::exceptions::RCLError & ex) { + throw std::runtime_error( + std::string( + "Failed to handle entities update on node remove: ") + ex.what()); } - memory_strategy_->remove_guard_condition(node_ptr->get_notify_guard_condition()); } void @@ -179,6 +249,59 @@ Executor::spin_node_once_nanoseconds( this->remove_node(node, false); } +rclcpp::FutureReturnCode +Executor::spin_until_future_complete_impl( + std::chrono::nanoseconds timeout, + const std::function & wait_for_future) +{ + // TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete + // inside a callback executed by an executor. + + // Check the future before entering the while loop. + // If the future is already complete, don't try to spin. + std::future_status status = wait_for_future(std::chrono::seconds(0)); + if (status == std::future_status::ready) { + return FutureReturnCode::SUCCESS; + } + + auto end_time = std::chrono::steady_clock::now(); + std::chrono::nanoseconds timeout_ns = std::chrono::duration_cast( + timeout); + if (timeout_ns > std::chrono::nanoseconds::zero()) { + end_time += timeout_ns; + } + std::chrono::nanoseconds timeout_left = timeout_ns; + + if (spinning.exchange(true)) { + throw std::runtime_error("spin_until_future_complete() called while already spinning"); + } + RCPPUTILS_SCOPE_EXIT(wait_result_.reset();this->spinning.store(false);); + while (rclcpp::ok(this->context_) && spinning.load()) { + // Do one item of work. + spin_once_impl(timeout_left); + + // Check if the future is set, return SUCCESS if it is. + status = wait_for_future(std::chrono::seconds(0)); + if (status == std::future_status::ready) { + return FutureReturnCode::SUCCESS; + } + // If the original timeout is < 0, then this is blocking, never TIMEOUT. + if (timeout_ns < std::chrono::nanoseconds::zero()) { + continue; + } + // Otherwise check if we still have time to wait, return TIMEOUT if not. + auto now = std::chrono::steady_clock::now(); + if (now >= end_time) { + return FutureReturnCode::TIMEOUT; + } + // Subtract the elapsed time from the original timeout. + timeout_left = std::chrono::duration_cast(end_time - now); + } + + // The future did not complete before ok() returned false, return INTERRUPTED. + return FutureReturnCode::INTERRUPTED; +} + void Executor::spin_node_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node) { @@ -193,419 +316,585 @@ Executor::spin_node_some(std::shared_ptr node) this->spin_node_some(node->get_node_base_interface()); } +void Executor::spin_some(std::chrono::nanoseconds max_duration) +{ + return this->spin_some_impl(max_duration, false); +} + void -Executor::spin_some() +Executor::spin_node_all( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node, + std::chrono::nanoseconds max_duration) { + this->add_node(node, false); + spin_all(max_duration); + this->remove_node(node, false); +} + +void +Executor::spin_node_all(std::shared_ptr node, std::chrono::nanoseconds max_duration) +{ + this->spin_node_all(node->get_node_base_interface(), max_duration); +} + +void Executor::spin_all(std::chrono::nanoseconds max_duration) +{ + if (max_duration < 0ns) { + throw std::invalid_argument("max_duration must be greater than or equal to 0"); + } + return this->spin_some_impl(max_duration, true); +} + +void +Executor::spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive) +{ + auto start = std::chrono::steady_clock::now(); + auto max_duration_not_elapsed = [max_duration, start]() { + if (std::chrono::nanoseconds(0) == max_duration) { + // told to spin forever if need be + return true; + } else if (std::chrono::steady_clock::now() - start < max_duration) { + // told to spin only for some maximum amount of time + return true; + } + // spun too long + return false; + }; + if (spinning.exchange(true)) { throw std::runtime_error("spin_some() called while already spinning"); } - RCLCPP_SCOPE_EXIT(this->spinning.store(false); ); - AnyExecutable::SharedPtr any_exec; - while ((any_exec = get_next_executable(std::chrono::milliseconds::zero())) && spinning.load()) { - execute_any_executable(any_exec); + RCPPUTILS_SCOPE_EXIT(wait_result_.reset();this->spinning.store(false);); + + // clear the wait result and wait for work without blocking to collect the work + // for the first time + // both spin_some and spin_all wait for work at the beginning + wait_result_.reset(); + wait_for_work(std::chrono::milliseconds(0)); + bool just_waited = true; + + // The logic of this while loop is as follows: + // + // - while not shutdown, and spinning (not canceled), and not max duration reached... + // - try to get an executable item to execute, and execute it if available + // - otherwise, reset the wait result, and ... + // - if there was no work available just after waiting, break the loop unconditionally + // - this is appropriate for both spin_some and spin_all which use this function + // - else if exhaustive = true, then wait for work again + // - this is only used for spin_all and not spin_some + // - else break + // - this only occurs with spin_some + // + // The logic of this loop is subtle and should be carefully changed if at all. + // See also: + // https://github.com/ros2/rclcpp/issues/2508 + // https://github.com/ros2/rclcpp/pull/2517 + while (rclcpp::ok(context_) && spinning.load() && max_duration_not_elapsed()) { + AnyExecutable any_exec; + if (get_next_ready_executable(any_exec)) { + execute_any_executable(any_exec); + just_waited = false; + } else { + // if nothing is ready, reset the result to clear it + wait_result_.reset(); + + if (just_waited) { + // there was no work after just waiting, always exit in this case + // before the exhaustive condition can be checked + break; + } + + if (exhaustive) { + // if exhaustive, wait for work again + // this only happens for spin_all; spin_some only waits at the start + wait_for_work(std::chrono::milliseconds(0)); + just_waited = true; + } else { + break; + } + } } } void -Executor::spin_once(std::chrono::nanoseconds timeout) +Executor::spin_once_impl(std::chrono::nanoseconds timeout) { - if (spinning.exchange(true)) { - throw std::runtime_error("spin_once() called while already spinning"); - } - RCLCPP_SCOPE_EXIT(this->spinning.store(false); ); - auto any_exec = get_next_executable(timeout); - if (any_exec) { + AnyExecutable any_exec; + if (get_next_executable(any_exec, timeout)) { execute_any_executable(any_exec); } } void -Executor::cancel() +Executor::spin_once(std::chrono::nanoseconds timeout) { - spinning.store(false); - if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) { - throw std::runtime_error(rcl_get_error_string_safe()); + if (spinning.exchange(true)) { + throw std::runtime_error("spin_once() called while already spinning"); } + RCPPUTILS_SCOPE_EXIT(wait_result_.reset();this->spinning.store(false);); + spin_once_impl(timeout); } void -Executor::set_memory_strategy(rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy) +Executor::cancel() { - if (memory_strategy == nullptr) { - throw std::runtime_error("Received NULL memory strategy in executor."); + spinning.store(false); + try { + interrupt_guard_condition_->trigger(); + } catch (const rclcpp::exceptions::RCLError & ex) { + throw std::runtime_error( + std::string("Failed to trigger guard condition in cancel: ") + ex.what()); } - memory_strategy_ = memory_strategy; } void -Executor::execute_any_executable(AnyExecutable::SharedPtr any_exec) +Executor::execute_any_executable(AnyExecutable & any_exec) { - if (!any_exec || !spinning.load()) { + if (!spinning.load()) { return; } - if (any_exec->timer) { - execute_timer(any_exec->timer); + + assert( + (void("cannot execute an AnyExecutable without a valid callback group"), + any_exec.callback_group)); + + if (any_exec.timer) { + TRACETOOLS_TRACEPOINT( + rclcpp_executor_execute, + static_cast(any_exec.timer->get_timer_handle().get())); + execute_timer(any_exec.timer, any_exec.data); } - if (any_exec->subscription) { - execute_subscription(any_exec->subscription); + if (any_exec.subscription) { + TRACETOOLS_TRACEPOINT( + rclcpp_executor_execute, + static_cast(any_exec.subscription->get_subscription_handle().get())); + execute_subscription(any_exec.subscription); } - if (any_exec->subscription_intra_process) { - execute_intra_process_subscription(any_exec->subscription_intra_process); + if (any_exec.service) { + execute_service(any_exec.service); } - if (any_exec->service) { - execute_service(any_exec->service); + if (any_exec.client) { + execute_client(any_exec.client); } - if (any_exec->client) { - execute_client(any_exec->client); + if (any_exec.waitable) { + const std::shared_ptr & const_data = any_exec.data; + any_exec.waitable->execute(const_data); } + // Reset the callback_group, regardless of type - any_exec->callback_group->can_be_taken_from().store(true); - // Wake the wait, because it may need to be recalculated or work that - // was previously blocked is now available. - if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) { - throw std::runtime_error(rcl_get_error_string_safe()); - } + any_exec.callback_group->can_be_taken_from().store(true); } +template +static void -Executor::execute_subscription( - rclcpp::SubscriptionBase::SharedPtr subscription) -{ - std::shared_ptr message = subscription->create_message(); - rmw_message_info_t message_info; - - auto ret = rcl_take(subscription->get_subscription_handle(), - message.get(), &message_info); - if (ret == RCL_RET_OK) { - message_info.from_intra_process = false; - subscription->handle_message(message, message_info); - } else if (ret != RCL_RET_SUBSCRIPTION_TAKE_FAILED) { - fprintf(stderr, - "[rclcpp::error] take failed for subscription on topic '%s': %s\n", - subscription->get_topic_name(), rcl_get_error_string_safe()); - rcl_reset_error(); +take_and_do_error_handling( + const char * action_description, + const char * topic_or_service_name, + Taker take_action, + Handler handle_action) +{ + bool taken = false; + try { + taken = take_action(); + } catch (const rclcpp::exceptions::RCLError & rcl_error) { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), + "executor %s '%s' unexpectedly failed: %s", + action_description, + topic_or_service_name, + rcl_error.what()); + } + if (taken) { + handle_action(); + } else { + // Message or Service was not taken for some reason. + // Note that this can be normal, if the underlying middleware needs to + // interrupt wait spuriously it is allowed. + // So in that case the executor cannot tell the difference in a + // spurious wake up and an entity actually having data until trying + // to take the data. + RCLCPP_DEBUG( + rclcpp::get_logger("rclcpp"), + "executor %s '%s' failed to take anything", + action_description, + topic_or_service_name); } - subscription->return_message(message); } void -Executor::execute_intra_process_subscription( - rclcpp::SubscriptionBase::SharedPtr subscription) -{ - rcl_interfaces::msg::IntraProcessMessage ipm; - rmw_message_info_t message_info; - rcl_ret_t status = rcl_take( - subscription->get_intra_process_subscription_handle(), - &ipm, - &message_info); - - if (status == RCL_RET_OK) { - message_info.from_intra_process = true; - subscription->handle_intra_process_message(ipm, message_info); - } else if (status != RCL_RET_SUBSCRIPTION_TAKE_FAILED) { - fprintf(stderr, - "[rclcpp::error] take failed for intra process subscription on topic '%s': %s\n", - subscription->get_topic_name(), rcl_get_error_string_safe()); - rcl_reset_error(); +Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription) +{ + using rclcpp::dynamic_typesupport::DynamicMessage; + + rclcpp::MessageInfo message_info; + message_info.get_rmw_message_info().from_intra_process = false; + + switch (subscription->get_delivered_message_kind()) { + // Deliver ROS message + case rclcpp::DeliveredMessageKind::ROS_MESSAGE: + { + if (subscription->can_loan_messages()) { + // This is the case where a loaned message is taken from the middleware via + // inter-process communication, given to the user for their callback, + // and then returned. + void * loaned_msg = nullptr; + // TODO(wjwwood): refactor this into methods on subscription when LoanedMessage + // is extened to support subscriptions as well. + take_and_do_error_handling( + "taking a loaned message from topic", + subscription->get_topic_name(), + [&]() + { + rcl_ret_t ret = rcl_take_loaned_message( + subscription->get_subscription_handle().get(), + &loaned_msg, + &message_info.get_rmw_message_info(), + nullptr); + if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret) { + return false; + } else if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + return true; + }, + [&]() {subscription->handle_loaned_message(loaned_msg, message_info);}); + if (nullptr != loaned_msg) { + rcl_ret_t ret = rcl_return_loaned_message_from_subscription( + subscription->get_subscription_handle().get(), loaned_msg); + if (RCL_RET_OK != ret) { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), + "rcl_return_loaned_message_from_subscription() failed for subscription on topic " + "'%s': %s", + subscription->get_topic_name(), rcl_get_error_string().str); + } + loaned_msg = nullptr; + } + } else { + // This case is taking a copy of the message data from the middleware via + // inter-process communication. + std::shared_ptr message = subscription->create_message(); + take_and_do_error_handling( + "taking a message from topic", + subscription->get_topic_name(), + [&]() {return subscription->take_type_erased(message.get(), message_info);}, + [&]() {subscription->handle_message(message, message_info);}); + // TODO(clalancette): In the case that the user is using the MessageMemoryPool, + // and they take a shared_ptr reference to the message in the callback, this can + // inadvertently return the message to the pool when the user is still using it. + // This is a bug that needs to be fixed in the pool, and we should probably have + // a custom deleter for the message that actually does the return_message(). + subscription->return_message(message); + } + break; + } + + // Deliver serialized message + case rclcpp::DeliveredMessageKind::SERIALIZED_MESSAGE: + { + // This is the case where a copy of the serialized message is taken from + // the middleware via inter-process communication. + std::shared_ptr serialized_msg = + subscription->create_serialized_message(); + take_and_do_error_handling( + "taking a serialized message from topic", + subscription->get_topic_name(), + [&]() {return subscription->take_serialized(*serialized_msg.get(), message_info);}, + [&]() + { + subscription->handle_serialized_message(serialized_msg, message_info); + }); + subscription->return_serialized_message(serialized_msg); + break; + } + + // DYNAMIC SUBSCRIPTION ======================================================================== + // Deliver dynamic message + case rclcpp::DeliveredMessageKind::DYNAMIC_MESSAGE: + { + throw std::runtime_error("Unimplemented"); + } + + default: + { + throw std::runtime_error("Delivered message kind is not supported"); + } } } void -Executor::execute_timer( - rclcpp::TimerBase::SharedPtr timer) +Executor::execute_timer(rclcpp::TimerBase::SharedPtr timer, const std::shared_ptr & data_ptr) { - timer->execute_callback(); + timer->execute_callback(data_ptr); } void -Executor::execute_service( - rclcpp::ServiceBase::SharedPtr service) +Executor::execute_service(rclcpp::ServiceBase::SharedPtr service) { auto request_header = service->create_request_header(); std::shared_ptr request = service->create_request(); - rcl_ret_t status = rcl_take_request( - service->get_service_handle(), - request_header.get(), - request.get()); - if (status == RCL_RET_OK) { - service->handle_request(request_header, request); - } else if (status != RCL_RET_SERVICE_TAKE_FAILED) { - fprintf(stderr, - "[rclcpp::error] take request failed for server of service '%s': %s\n", - service->get_service_name().c_str(), rcl_get_error_string_safe()); - rcl_reset_error(); - } + take_and_do_error_handling( + "taking a service server request from service", + service->get_service_name(), + [&]() {return service->take_type_erased_request(request.get(), *request_header);}, + [&]() {service->handle_request(request_header, request);}); } void -Executor::execute_client( - rclcpp::ClientBase::SharedPtr client) +Executor::execute_client(rclcpp::ClientBase::SharedPtr client) { auto request_header = client->create_request_header(); std::shared_ptr response = client->create_response(); - rcl_ret_t status = rcl_take_response( - client->get_client_handle(), - request_header.get(), - response.get()); - if (status == RCL_RET_OK) { - client->handle_response(request_header, response); - } else if (status != RCL_RET_CLIENT_TAKE_FAILED) { - fprintf(stderr, - "[rclcpp::error] take response failed for client of service '%s': %s\n", - client->get_service_name().c_str(), rcl_get_error_string_safe()); - rcl_reset_error(); - } + take_and_do_error_handling( + "taking a service client response from service", + client->get_service_name(), + [&]() {return client->take_type_erased_response(response.get(), *request_header);}, + [&]() {client->handle_response(request_header, response);}); +} + +void +Executor::collect_entities() +{ + // Updating the entity collection and waitset expires any active result + this->wait_result_.reset(); + + // Get the current list of available waitables from the collector. + rclcpp::executors::ExecutorEntitiesCollection collection; + this->collector_.update_collections(); + auto callback_groups = this->collector_.get_all_callback_groups(); + rclcpp::executors::build_entities_collection(callback_groups, collection); + + // Make a copy of notify waitable so we can continue to mutate the original + // one outside of the execute loop. + // This prevents the collection of guard conditions in the waitable from changing + // while we are waiting on it. + if (notify_waitable_) { + current_notify_waitable_ = std::make_shared( + *notify_waitable_); + auto notify_waitable = std::static_pointer_cast(current_notify_waitable_); + collection.waitables.insert({notify_waitable.get(), {notify_waitable, {}}}); + } + + // We must remove expired entities here, so that we don't continue to use older entities. + // See https://github.com/ros2/rclcpp/issues/2180 for more information. + current_collection_.remove_expired_entities(); + + // Update each of the groups of entities in the current collection, adding or removing + // from the wait set as necessary. + current_collection_.timers.update( + collection.timers, + [this](auto timer) {wait_set_.add_timer(timer);}, + [this](auto timer) {wait_set_.remove_timer(timer);}); + + current_collection_.subscriptions.update( + collection.subscriptions, + [this](auto subscription) { + wait_set_.add_subscription(subscription, kDefaultSubscriptionMask); + }, + [this](auto subscription) { + wait_set_.remove_subscription(subscription, kDefaultSubscriptionMask); + }); + + current_collection_.clients.update( + collection.clients, + [this](auto client) {wait_set_.add_client(client);}, + [this](auto client) {wait_set_.remove_client(client);}); + + current_collection_.services.update( + collection.services, + [this](auto service) {wait_set_.add_service(service);}, + [this](auto service) {wait_set_.remove_service(service);}); + + current_collection_.guard_conditions.update( + collection.guard_conditions, + [this](auto guard_condition) {wait_set_.add_guard_condition(guard_condition);}, + [this](auto guard_condition) {wait_set_.remove_guard_condition(guard_condition);}); + + current_collection_.waitables.update( + collection.waitables, + [this](auto waitable) {wait_set_.add_waitable(waitable);}, + [this](auto waitable) {wait_set_.remove_waitable(waitable);}); + + // In the case that an entity already has an expired weak pointer + // before being removed from the waitset, additionally prune the waitset. + this->wait_set_.prune_deleted_entities(); } void Executor::wait_for_work(std::chrono::nanoseconds timeout) { - // Collect the subscriptions and timers to be waited on - memory_strategy_->clear_handles(); - bool has_invalid_weak_nodes = memory_strategy_->collect_entities(weak_nodes_); - - // Clean up any invalid nodes, if they were detected - if (has_invalid_weak_nodes) { - weak_nodes_.erase( - remove_if( - weak_nodes_.begin(), weak_nodes_.end(), - [](rclcpp::node_interfaces::NodeBaseInterface::WeakPtr i) - { - return i.expired(); - } - ) - ); - } - // clear wait set - if (rcl_wait_set_clear_subscriptions(&wait_set_) != RCL_RET_OK) { - throw std::runtime_error("Couldn't clear subscriptions from wait set"); - } - if (rcl_wait_set_clear_services(&wait_set_) != RCL_RET_OK) { - throw std::runtime_error("Couldn't clear servicess from wait set"); - } - if (rcl_wait_set_clear_clients(&wait_set_) != RCL_RET_OK) { - throw std::runtime_error("Couldn't clear clients from wait set"); - } - if (rcl_wait_set_clear_guard_conditions(&wait_set_) != RCL_RET_OK) { - throw std::runtime_error("Couldn't clear guard conditions from wait set"); - } - if (rcl_wait_set_clear_timers(&wait_set_) != RCL_RET_OK) { - throw std::runtime_error("Couldn't clear timers from wait set"); - } + TRACETOOLS_TRACEPOINT(rclcpp_executor_wait_for_work, timeout.count()); - if (rcl_wait_set_resize_subscriptions( - &wait_set_, memory_strategy_->number_of_ready_subscriptions()) != RCL_RET_OK) - { - throw std::runtime_error( - std::string("Couldn't resize the number of subscriptions in wait set : ") + - rcl_get_error_string_safe()); - } + // Clear any previous wait result + this->wait_result_.reset(); - if (rcl_wait_set_resize_services( - &wait_set_, memory_strategy_->number_of_ready_services()) != RCL_RET_OK) { - throw std::runtime_error( - std::string("Couldn't resize the number of services in wait set : ") + - rcl_get_error_string_safe()); + std::lock_guard guard(mutex_); + if (this->entities_need_rebuild_.exchange(false) || current_collection_.empty()) { + this->collect_entities(); + } } - - if (rcl_wait_set_resize_clients( - &wait_set_, memory_strategy_->number_of_ready_clients()) != RCL_RET_OK) - { - throw std::runtime_error( - std::string("Couldn't resize the number of clients in wait set : ") + - rcl_get_error_string_safe()); + this->wait_result_.emplace(wait_set_.wait(timeout)); + if (!this->wait_result_ || this->wait_result_->kind() == WaitResultKind::Empty) { + RCUTILS_LOG_WARN_NAMED( + "rclcpp", + "empty wait set received in wait(). This should never happen."); + } else { + if (this->wait_result_->kind() == WaitResultKind::Ready && current_notify_waitable_) { + auto & rcl_wait_set = this->wait_result_->get_wait_set().get_rcl_wait_set(); + if (current_notify_waitable_->is_ready(rcl_wait_set)) { + current_notify_waitable_->execute(current_notify_waitable_->take_data()); + } + } } +} - if (rcl_wait_set_resize_guard_conditions( - &wait_set_, memory_strategy_->number_of_guard_conditions()) != RCL_RET_OK) - { - throw std::runtime_error( - std::string("Couldn't resize the number of guard_conditions in wait set : ") + - rcl_get_error_string_safe()); - } +bool +Executor::get_next_ready_executable(AnyExecutable & any_executable) +{ + TRACETOOLS_TRACEPOINT(rclcpp_executor_get_next_ready); - if (rcl_wait_set_resize_timers( - &wait_set_, memory_strategy_->number_of_ready_timers()) != RCL_RET_OK) - { - throw std::runtime_error( - std::string("Couldn't resize the number of timers in wait set : ") + - rcl_get_error_string_safe()); - } + bool valid_executable = false; - if (!memory_strategy_->add_handles_to_wait_set(&wait_set_)) { - throw std::runtime_error("Couldn't fill wait set"); - } - rcl_ret_t status = - rcl_wait(&wait_set_, std::chrono::duration_cast(timeout).count()); - if (status == RCL_RET_WAIT_SET_EMPTY) { - fprintf(stderr, "Warning: empty wait set received in rcl_wait(). This should never happen.\n"); - } else if (status != RCL_RET_OK && status != RCL_RET_TIMEOUT) { - using rclcpp::exceptions::throw_from_rcl_error; - throw_from_rcl_error(status, "rcl_wait() failed"); + if (!wait_result_.has_value() || wait_result_->kind() != rclcpp::WaitResultKind::Ready) { + return false; } - // check the null handles in the wait set and remove them from the handles in memory strategy - // for callback-based entities - memory_strategy_->remove_null_handles(&wait_set_); -} - -rclcpp::node_interfaces::NodeBaseInterface::SharedPtr -Executor::get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group) -{ - if (!group) { - return nullptr; - } - for (auto & weak_node : weak_nodes_) { - auto node = weak_node.lock(); - if (!node) { - continue; - } - for (auto & weak_group : node->get_callback_groups()) { - auto callback_group = weak_group.lock(); - if (callback_group == group) { - return node; + if (!valid_executable) { + size_t current_timer_index = 0; + while (true) { + auto [timer, timer_index] = wait_result_->peek_next_ready_timer(current_timer_index); + if (nullptr == timer) { + break; } + current_timer_index = timer_index; + auto entity_iter = current_collection_.timers.find(timer->get_timer_handle().get()); + if (entity_iter != current_collection_.timers.end()) { + auto callback_group = entity_iter->second.callback_group.lock(); + if (!callback_group || !callback_group->can_be_taken_from()) { + current_timer_index++; + continue; + } + // At this point the timer is either ready for execution or was perhaps + // it was canceled, based on the result of call(), but either way it + // should not be checked again from peek_next_ready_timer(), so clear + // it from the wait result. + wait_result_->clear_timer_with_index(current_timer_index); + // Check that the timer should be called still, i.e. it wasn't canceled. + any_executable.data = timer->call(); + if (!any_executable.data) { + current_timer_index++; + continue; + } + any_executable.timer = timer; + any_executable.callback_group = callback_group; + valid_executable = true; + break; + } + current_timer_index++; } } - return nullptr; -} -rclcpp::callback_group::CallbackGroup::SharedPtr -Executor::get_group_by_timer(rclcpp::TimerBase::SharedPtr timer) -{ - for (auto & weak_node : weak_nodes_) { - auto node = weak_node.lock(); - if (!node) { - continue; - } - for (auto & weak_group : node->get_callback_groups()) { - auto group = weak_group.lock(); - if (!group) { - continue; - } - for (auto & weak_timer : group->get_timer_ptrs()) { - auto t = weak_timer.lock(); - if (t == timer) { - return group; + if (!valid_executable) { + while (auto subscription = wait_result_->next_ready_subscription()) { + auto entity_iter = current_collection_.subscriptions.find( + subscription->get_subscription_handle().get()); + if (entity_iter != current_collection_.subscriptions.end()) { + auto callback_group = entity_iter->second.callback_group.lock(); + if (!callback_group || !callback_group->can_be_taken_from()) { + continue; } + any_executable.subscription = subscription; + any_executable.callback_group = callback_group; + valid_executable = true; + break; } } } - return rclcpp::callback_group::CallbackGroup::SharedPtr(); -} -void -Executor::get_next_timer(AnyExecutable::SharedPtr any_exec) -{ - for (auto & weak_node : weak_nodes_) { - auto node = weak_node.lock(); - if (!node) { - continue; - } - for (auto & weak_group : node->get_callback_groups()) { - auto group = weak_group.lock(); - if (!group || !group->can_be_taken_from().load()) { - continue; - } - for (auto & timer_ref : group->get_timer_ptrs()) { - auto timer = timer_ref.lock(); - if (timer && timer->is_ready()) { - any_exec->timer = timer; - any_exec->callback_group = group; - node = get_node_by_group(group); - return; + if (!valid_executable) { + while (auto service = wait_result_->next_ready_service()) { + auto entity_iter = current_collection_.services.find(service->get_service_handle().get()); + if (entity_iter != current_collection_.services.end()) { + auto callback_group = entity_iter->second.callback_group.lock(); + if (!callback_group || !callback_group->can_be_taken_from()) { + continue; } + any_executable.service = service; + any_executable.callback_group = callback_group; + valid_executable = true; + break; } } } -} -AnyExecutable::SharedPtr -Executor::get_next_ready_executable() -{ - auto any_exec = memory_strategy_->instantiate_next_executable(); - // Check the timers to see if there are any that are ready, if so return - get_next_timer(any_exec); - if (any_exec->timer) { - return any_exec; - } - // Check the subscriptions to see if there are any that are ready - memory_strategy_->get_next_subscription(any_exec, weak_nodes_); - if (any_exec->subscription || any_exec->subscription_intra_process) { - return any_exec; + if (!valid_executable) { + while (auto client = wait_result_->next_ready_client()) { + auto entity_iter = current_collection_.clients.find(client->get_client_handle().get()); + if (entity_iter != current_collection_.clients.end()) { + auto callback_group = entity_iter->second.callback_group.lock(); + if (!callback_group || !callback_group->can_be_taken_from()) { + continue; + } + any_executable.client = client; + any_executable.callback_group = callback_group; + valid_executable = true; + break; + } + } } - // Check the services to see if there are any that are ready - memory_strategy_->get_next_service(any_exec, weak_nodes_); - if (any_exec->service) { - return any_exec; + + if (!valid_executable) { + while (auto waitable = wait_result_->next_ready_waitable()) { + auto entity_iter = current_collection_.waitables.find(waitable.get()); + if (entity_iter != current_collection_.waitables.end()) { + auto callback_group = entity_iter->second.callback_group.lock(); + if (!callback_group || !callback_group->can_be_taken_from()) { + continue; + } + any_executable.waitable = waitable; + any_executable.callback_group = callback_group; + any_executable.data = waitable->take_data(); + valid_executable = true; + break; + } + } } - // Check the clients to see if there are any that are ready - memory_strategy_->get_next_client(any_exec, weak_nodes_); - if (any_exec->client) { - return any_exec; + + if (any_executable.callback_group) { + if (any_executable.callback_group->type() == CallbackGroupType::MutuallyExclusive) { + assert(any_executable.callback_group->can_be_taken_from().load()); + any_executable.callback_group->can_be_taken_from().store(false); + } } - // If there is no ready executable, return a null ptr - return nullptr; + + + return valid_executable; } -AnyExecutable::SharedPtr -Executor::get_next_executable(std::chrono::nanoseconds timeout) +bool +Executor::get_next_executable(AnyExecutable & any_executable, std::chrono::nanoseconds timeout) { + bool success = false; // Check to see if there are any subscriptions or timers needing service // TODO(wjwwood): improve run to run efficiency of this function - auto any_exec = get_next_ready_executable(); + success = get_next_ready_executable(any_executable); // If there are none - if (!any_exec) { + if (!success) { // Wait for subscriptions or timers to work on wait_for_work(timeout); if (!spinning.load()) { - return nullptr; + return false; } // Try again - any_exec = get_next_ready_executable(); - } - // At this point any_exec should be valid with either a valid subscription - // or a valid timer, or it should be a null shared_ptr - if (any_exec) { - // If it is valid, check to see if the group is mutually exclusive or - // not, then mark it accordingly - if (any_exec->callback_group && any_exec->callback_group->type() == \ - callback_group::CallbackGroupType::MutuallyExclusive) - { - // It should not have been taken otherwise - assert(any_exec->callback_group->can_be_taken_from().load()); - // Set to false to indicate something is being run from this group - // This is reset to true either when the any_exec is executed or when the - // any_exec is destructued - any_exec->callback_group->can_be_taken_from().store(false); - } + success = get_next_ready_executable(any_executable); } - return any_exec; -} - -std::ostream & -rclcpp::executor::operator<<(std::ostream & os, const FutureReturnCode & future_return_code) -{ - return os << to_string(future_return_code); + return success; } -std::string -rclcpp::executor::to_string(const FutureReturnCode & future_return_code) +bool +Executor::is_spinning() { - using enum_type = std::underlying_type::type; - std::string prefix = "Unknown enum value ("; - std::string ret_as_string = std::to_string(static_cast(future_return_code)); - switch (future_return_code) { - case FutureReturnCode::SUCCESS: - prefix = "SUCCESS ("; - break; - case FutureReturnCode::INTERRUPTED: - prefix = "INTERRUPTED ("; - break; - case FutureReturnCode::TIMEOUT: - prefix = "TIMEOUT ("; - break; - } - return prefix + ret_as_string + ")"; + return spinning; } diff --git a/rclcpp/src/rclcpp/executor_options.cpp b/rclcpp/src/rclcpp/executor_options.cpp new file mode 100644 index 0000000000..018d1261ae --- /dev/null +++ b/rclcpp/src/rclcpp/executor_options.cpp @@ -0,0 +1,55 @@ +// Copyright 2024 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 "rclcpp/executor_options.hpp" + +using rclcpp::ExecutorOptions; + +namespace rclcpp +{ + +class ExecutorOptionsImplementation {}; + +} // namespace rclcpp + +ExecutorOptions::ExecutorOptions() +: memory_strategy(rclcpp::memory_strategies::create_default_strategy()), + context(rclcpp::contexts::get_global_default_context()), + max_conditions(0), + impl_(nullptr) +{} + +ExecutorOptions::~ExecutorOptions() +{} + +ExecutorOptions::ExecutorOptions(const ExecutorOptions & other) +{ + *this = other; +} + +ExecutorOptions & ExecutorOptions::operator=(const ExecutorOptions & other) +{ + if (this == &other) { + return *this; + } + + this->memory_strategy = other.memory_strategy; + this->context = other.context; + this->max_conditions = other.max_conditions; + if (nullptr != other.impl_) { + this->impl_ = std::make_unique(*other.impl_); + } + + return *this; +} diff --git a/rclcpp/src/rclcpp/executors.cpp b/rclcpp/src/rclcpp/executors.cpp index 0a900c07da..94137d50bc 100644 --- a/rclcpp/src/rclcpp/executors.cpp +++ b/rclcpp/src/rclcpp/executors.cpp @@ -14,10 +14,29 @@ #include "rclcpp/executors.hpp" +void +rclcpp::spin_all( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, + std::chrono::nanoseconds max_duration) +{ + rclcpp::ExecutorOptions options; + options.context = node_ptr->get_context(); + rclcpp::executors::SingleThreadedExecutor exec(options); + exec.spin_node_all(node_ptr, max_duration); +} + +void +rclcpp::spin_all(rclcpp::Node::SharedPtr node_ptr, std::chrono::nanoseconds max_duration) +{ + rclcpp::spin_all(node_ptr->get_node_base_interface(), max_duration); +} + void rclcpp::spin_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr) { - rclcpp::executors::SingleThreadedExecutor exec; + rclcpp::ExecutorOptions options; + options.context = node_ptr->get_context(); + rclcpp::executors::SingleThreadedExecutor exec(options); exec.spin_node_some(node_ptr); } @@ -30,7 +49,9 @@ rclcpp::spin_some(rclcpp::Node::SharedPtr node_ptr) void rclcpp::spin(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr) { - rclcpp::executors::SingleThreadedExecutor exec; + rclcpp::ExecutorOptions options; + options.context = node_ptr->get_context(); + rclcpp::executors::SingleThreadedExecutor exec(options); exec.add_node(node_ptr); exec.spin(); exec.remove_node(node_ptr); diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp new file mode 100644 index 0000000000..68ac56b656 --- /dev/null +++ b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp @@ -0,0 +1,251 @@ +// Copyright 2023 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 "rclcpp/executors/executor_entities_collection.hpp" + +namespace rclcpp +{ +namespace executors +{ +bool ExecutorEntitiesCollection::empty() const +{ + return + subscriptions.empty() && + timers.empty() && + guard_conditions.empty() && + clients.empty() && + services.empty() && + waitables.empty(); +} + +void ExecutorEntitiesCollection::clear() +{ + subscriptions.clear(); + timers.clear(); + guard_conditions.clear(); + clients.clear(); + services.clear(); + waitables.clear(); +} + +size_t ExecutorEntitiesCollection::remove_expired_entities() +{ + auto remove_entities = [](auto & collection) -> size_t { + size_t removed = 0; + for (auto it = collection.begin(); it != collection.end(); ) { + if (it->second.entity.expired()) { + ++removed; + it = collection.erase(it); + } else { + ++it; + } + } + return removed; + }; + + return + remove_entities(subscriptions) + + remove_entities(timers) + + remove_entities(guard_conditions) + + remove_entities(clients) + + remove_entities(services) + + remove_entities(waitables); +} + +void +build_entities_collection( + const std::vector & callback_groups, + ExecutorEntitiesCollection & collection) +{ + collection.clear(); + + for (auto weak_group_ptr : callback_groups) { + auto group_ptr = weak_group_ptr.lock(); + if (!group_ptr) { + continue; + } + + if (group_ptr->can_be_taken_from().load()) { + group_ptr->collect_all_ptrs( + [&collection, weak_group_ptr](const rclcpp::SubscriptionBase::SharedPtr & subscription) { + collection.subscriptions.insert( + { + subscription->get_subscription_handle().get(), + {subscription, weak_group_ptr} + }); + }, + [&collection, weak_group_ptr](const rclcpp::ServiceBase::SharedPtr & service) { + collection.services.insert( + { + service->get_service_handle().get(), + {service, weak_group_ptr} + }); + }, + [&collection, weak_group_ptr](const rclcpp::ClientBase::SharedPtr & client) { + collection.clients.insert( + { + client->get_client_handle().get(), + {client, weak_group_ptr} + }); + }, + [&collection, weak_group_ptr](const rclcpp::TimerBase::SharedPtr & timer) { + collection.timers.insert( + { + timer->get_timer_handle().get(), + {timer, weak_group_ptr} + }); + }, + [&collection, weak_group_ptr](const rclcpp::Waitable::SharedPtr & waitable) { + collection.waitables.insert( + { + waitable.get(), + {waitable, weak_group_ptr} + }); + } + ); + } + } +} + +size_t +ready_executables( + const ExecutorEntitiesCollection & collection, + rclcpp::WaitResult & wait_result, + std::deque & executables +) +{ + size_t added = 0; + if (wait_result.kind() != rclcpp::WaitResultKind::Ready) { + return added; + } + auto rcl_wait_set = wait_result.get_wait_set().get_rcl_wait_set(); + + // Cache shared pointers to groups to avoid extra work re-locking them + std::map> group_map; + + auto group_cache = [&group_map](const rclcpp::CallbackGroup::WeakPtr & weak_cbg_ptr) + { + if (group_map.count(weak_cbg_ptr) == 0) { + group_map.insert({weak_cbg_ptr, weak_cbg_ptr.lock()}); + } + return group_map.find(weak_cbg_ptr)->second; + }; + + for (size_t ii = 0; ii < rcl_wait_set.size_of_timers; ++ii) { + if (nullptr == rcl_wait_set.timers[ii]) {continue;} + auto entity_iter = collection.timers.find(rcl_wait_set.timers[ii]); + if (entity_iter != collection.timers.end()) { + auto entity = entity_iter->second.entity.lock(); + if (!entity) { + continue; + } + auto group_info = group_cache(entity_iter->second.callback_group); + if (!group_info || !group_info->can_be_taken_from().load()) { + continue; + } + if (!entity->call()) { + continue; + } + rclcpp::AnyExecutable exec; + exec.timer = entity; + exec.callback_group = group_info; + executables.push_back(exec); + added++; + } + } + + for (size_t ii = 0; ii < rcl_wait_set.size_of_subscriptions; ++ii) { + if (nullptr == rcl_wait_set.subscriptions[ii]) {continue;} + auto entity_iter = collection.subscriptions.find(rcl_wait_set.subscriptions[ii]); + if (entity_iter != collection.subscriptions.end()) { + auto entity = entity_iter->second.entity.lock(); + if (!entity) { + continue; + } + auto group_info = group_cache(entity_iter->second.callback_group); + if (!group_info || !group_info->can_be_taken_from().load()) { + continue; + } + rclcpp::AnyExecutable exec; + exec.subscription = entity; + exec.callback_group = group_info; + executables.push_back(exec); + added++; + } + } + + for (size_t ii = 0; ii < rcl_wait_set.size_of_services; ++ii) { + if (nullptr == rcl_wait_set.services[ii]) {continue;} + auto entity_iter = collection.services.find(rcl_wait_set.services[ii]); + if (entity_iter != collection.services.end()) { + auto entity = entity_iter->second.entity.lock(); + if (!entity) { + continue; + } + auto group_info = group_cache(entity_iter->second.callback_group); + if (!group_info || !group_info->can_be_taken_from().load()) { + continue; + } + rclcpp::AnyExecutable exec; + exec.service = entity; + exec.callback_group = group_info; + executables.push_back(exec); + added++; + } + } + + for (size_t ii = 0; ii < rcl_wait_set.size_of_clients; ++ii) { + if (nullptr == rcl_wait_set.clients[ii]) {continue;} + auto entity_iter = collection.clients.find(rcl_wait_set.clients[ii]); + if (entity_iter != collection.clients.end()) { + auto entity = entity_iter->second.entity.lock(); + if (!entity) { + continue; + } + auto group_info = group_cache(entity_iter->second.callback_group); + if (!group_info || !group_info->can_be_taken_from().load()) { + continue; + } + rclcpp::AnyExecutable exec; + exec.client = entity; + exec.callback_group = group_info; + executables.push_back(exec); + added++; + } + } + + for (const auto & [handle, entry] : collection.waitables) { + auto waitable = entry.entity.lock(); + if (!waitable) { + continue; + } + if (!waitable->is_ready(rcl_wait_set)) { + continue; + } + auto group_info = group_cache(entry.callback_group); + if (!group_info || !group_info->can_be_taken_from().load()) { + continue; + } + rclcpp::AnyExecutable exec; + exec.waitable = waitable; + exec.callback_group = group_info; + executables.push_back(exec); + added++; + } + return added; +} +} // namespace executors +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp new file mode 100644 index 0000000000..702716a758 --- /dev/null +++ b/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp @@ -0,0 +1,438 @@ +// Copyright 2023 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 "rclcpp/executors/executor_entities_collector.hpp" +#include "rclcpp/executors/executor_notify_waitable.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" + +namespace rclcpp +{ +namespace executors +{ + +ExecutorEntitiesCollector::ExecutorEntitiesCollector( + std::shared_ptr notify_waitable) +: notify_waitable_(notify_waitable) +{ +} + +ExecutorEntitiesCollector::~ExecutorEntitiesCollector() +{ + for (auto weak_node_it = weak_nodes_.begin(); weak_node_it != weak_nodes_.end(); ) { + weak_node_it = remove_weak_node(weak_node_it); + } + + for (auto weak_group_it = automatically_added_groups_.begin(); + weak_group_it != automatically_added_groups_.end(); ) + { + weak_group_it = remove_weak_callback_group(weak_group_it, automatically_added_groups_); + } + + for (auto weak_group_it = manually_added_groups_.begin(); + weak_group_it != manually_added_groups_.end(); ) + { + weak_group_it = remove_weak_callback_group(weak_group_it, manually_added_groups_); + } + + for (auto weak_node_ptr : pending_added_nodes_) { + auto node_ptr = weak_node_ptr.lock(); + if (node_ptr) { + node_ptr->get_associated_with_executor_atomic().store(false); + } + } + pending_added_nodes_.clear(); + pending_removed_nodes_.clear(); + + for (auto weak_group_ptr : pending_manually_added_groups_) { + auto group_ptr = weak_group_ptr.lock(); + if (group_ptr) { + group_ptr->get_associated_with_executor_atomic().store(false); + } + // Disassociate the guard condition from the executor notify waitable + auto guard_condition_it = weak_groups_to_guard_conditions_.find(weak_group_ptr); + if (guard_condition_it != weak_groups_to_guard_conditions_.end()) { + this->notify_waitable_->remove_guard_condition(guard_condition_it->second); + weak_groups_to_guard_conditions_.erase(guard_condition_it); + } + } + pending_manually_added_groups_.clear(); + pending_manually_removed_groups_.clear(); +} + +bool +ExecutorEntitiesCollector::has_pending() const +{ + std::lock_guard lock(mutex_); + return pending_manually_added_groups_.size() != 0 || + pending_manually_removed_groups_.size() != 0 || + pending_added_nodes_.size() != 0 || + pending_removed_nodes_.size() != 0; +} + +void +ExecutorEntitiesCollector::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr) +{ + // If the node already has an executor + std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic(); + if (has_executor.exchange(true)) { + throw std::runtime_error( + std::string("Node '") + node_ptr->get_fully_qualified_name() + + "' has already been added to an executor."); + } + + std::lock_guard lock(mutex_); + bool associated = weak_nodes_.count(node_ptr) != 0; + bool add_queued = pending_added_nodes_.count(node_ptr) != 0; + bool remove_queued = pending_removed_nodes_.count(node_ptr) != 0; + + if ((associated || add_queued) && !remove_queued) { + throw std::runtime_error( + std::string("Node '") + node_ptr->get_fully_qualified_name() + + "' has already been added to this executor."); + } + + this->pending_added_nodes_.insert(node_ptr); +} + +void +ExecutorEntitiesCollector::remove_node( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr) +{ + std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic(); + if (!has_executor.exchange(false)) { + throw std::runtime_error( + std::string("Node '") + node_ptr->get_fully_qualified_name() + + "' needs to be associated with an executor."); + } + + std::lock_guard lock(mutex_); + bool associated = weak_nodes_.count(node_ptr) != 0; + bool add_queued = pending_added_nodes_.count(node_ptr) != 0; + bool remove_queued = pending_removed_nodes_.count(node_ptr) != 0; + + if (!(associated || add_queued) || remove_queued) { + throw std::runtime_error( + std::string("Node '") + node_ptr->get_fully_qualified_name() + + "' needs to be associated with this executor."); + } + + this->pending_removed_nodes_.insert(node_ptr); +} + +void +ExecutorEntitiesCollector::add_callback_group(rclcpp::CallbackGroup::SharedPtr group_ptr) +{ + std::atomic_bool & has_executor = group_ptr->get_associated_with_executor_atomic(); + if (has_executor.exchange(true)) { + throw std::runtime_error("Callback group has already been added to an executor."); + } + + std::lock_guard lock(mutex_); + bool associated = manually_added_groups_.count(group_ptr) != 0; + bool add_queued = pending_manually_added_groups_.count(group_ptr) != 0; + bool remove_queued = pending_manually_removed_groups_.count(group_ptr) != 0; + + if ((associated || add_queued) && !remove_queued) { + throw std::runtime_error("Callback group has already been added to this executor."); + } + + this->pending_manually_added_groups_.insert(group_ptr); + + // Store callback group notify guard condition in map and add it to the notify waitable + auto group_guard_condition = group_ptr->get_notify_guard_condition(); + weak_groups_to_guard_conditions_.insert({group_ptr, group_guard_condition}); + this->notify_waitable_->add_guard_condition(group_guard_condition); +} + +void +ExecutorEntitiesCollector::remove_callback_group(rclcpp::CallbackGroup::SharedPtr group_ptr) +{ + if (!group_ptr->get_associated_with_executor_atomic().load()) { + throw std::runtime_error("Callback group needs to be associated with an executor."); + } + /** + * TODO(mjcarroll): The callback groups, being created by a node, should never outlive + * the node. Since we haven't historically enforced this, turning this on may cause + * previously-functional code to fail. + * Consider re-enablng this check (along with corresponding CallbackGroup::has_valid_node), + * when we can guarantee node/group lifetimes. + if (!group_ptr->has_valid_node()) { + throw std::runtime_error("Node must not be deleted before its callback group(s)."); + } + */ + auto weak_group_ptr = rclcpp::CallbackGroup::WeakPtr(group_ptr); + std::lock_guard lock(mutex_); + bool associated = manually_added_groups_.count(group_ptr) != 0; + bool add_queued = pending_manually_added_groups_.count(group_ptr) != 0; + bool remove_queued = pending_manually_removed_groups_.count(group_ptr) != 0; + + if (!(associated || add_queued) || remove_queued) { + throw std::runtime_error("Callback group needs to be associated with this executor."); + } + + this->pending_manually_removed_groups_.insert(group_ptr); +} + +std::vector +ExecutorEntitiesCollector::get_all_callback_groups() const +{ + std::vector groups; + std::lock_guard lock(mutex_); + for (const auto & group_ptr : manually_added_groups_) { + groups.push_back(group_ptr); + } + for (auto const & group_ptr : automatically_added_groups_) { + groups.push_back(group_ptr); + } + return groups; +} + +std::vector +ExecutorEntitiesCollector::get_manually_added_callback_groups() const +{ + std::vector groups; + std::lock_guard lock(mutex_); + for (const auto & group_ptr : manually_added_groups_) { + groups.push_back(group_ptr); + } + return groups; +} + +std::vector +ExecutorEntitiesCollector::get_automatically_added_callback_groups() const +{ + std::vector groups; + std::lock_guard lock(mutex_); + for (auto const & group_ptr : automatically_added_groups_) { + groups.push_back(group_ptr); + } + return groups; +} + +void +ExecutorEntitiesCollector::update_collections() +{ + std::lock_guard lock(mutex_); + this->process_queues(); + this->add_automatically_associated_callback_groups(this->weak_nodes_); + this->prune_invalid_nodes_and_groups(); +} + +ExecutorEntitiesCollector::NodeCollection::iterator +ExecutorEntitiesCollector::remove_weak_node(NodeCollection::iterator weak_node) +{ + // Disassociate the guard condition from the executor notify waitable + auto guard_condition_it = weak_nodes_to_guard_conditions_.find(*weak_node); + if (guard_condition_it != weak_nodes_to_guard_conditions_.end()) { + this->notify_waitable_->remove_guard_condition(guard_condition_it->second); + weak_nodes_to_guard_conditions_.erase(guard_condition_it); + } + + // Mark the node as disassociated (if the node is still valid) + auto node_ptr = weak_node->lock(); + if (node_ptr) { + std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic(); + has_executor.store(false); + } + + // Remove the node from tracked nodes + return weak_nodes_.erase(weak_node); +} + +ExecutorEntitiesCollector::CallbackGroupCollection::iterator +ExecutorEntitiesCollector::remove_weak_callback_group( + CallbackGroupCollection::iterator weak_group_it, + CallbackGroupCollection & collection +) +{ + // Disassociate the guard condition from the executor notify waitable + auto guard_condition_it = weak_groups_to_guard_conditions_.find(*weak_group_it); + if (guard_condition_it != weak_groups_to_guard_conditions_.end()) { + this->notify_waitable_->remove_guard_condition(guard_condition_it->second); + weak_groups_to_guard_conditions_.erase(guard_condition_it); + } + + // Mark the node as disassociated (if the group is still valid) + auto group_ptr = weak_group_it->lock(); + if (group_ptr) { + /** + * TODO(mjcarroll): The callback groups, being created by a node, should never outlive + * the node. Since we haven't historically enforced this, turning this on may cause + * previously-functional code to fail. + * Consider re-enablng this check (along with corresponding CallbackGroup::has_valid_node), + * when we can guarantee node/group lifetimes. + if (!group_ptr->has_valid_node()) { + throw std::runtime_error("Node must not be deleted before its callback group(s)."); + } + */ + std::atomic_bool & has_executor = group_ptr->get_associated_with_executor_atomic(); + has_executor.store(false); + } + + // Remove the node from tracked nodes + return collection.erase(weak_group_it); +} + +void +ExecutorEntitiesCollector::add_callback_group_to_collection( + rclcpp::CallbackGroup::SharedPtr group_ptr, + CallbackGroupCollection & collection) +{ + auto iter = collection.insert(group_ptr); + if (iter.second == false) { + throw std::runtime_error("Callback group has already been added to this executor."); + } + + // Store node guard condition in map and add it to the notify waitable + auto group_guard_condition = group_ptr->get_notify_guard_condition(); + weak_groups_to_guard_conditions_.insert({group_ptr, group_guard_condition}); + this->notify_waitable_->add_guard_condition(group_guard_condition); +} + +void +ExecutorEntitiesCollector::process_queues() +{ + for (auto weak_node_ptr : pending_added_nodes_) { + auto node_ptr = weak_node_ptr.lock(); + if (!node_ptr) { + continue; + } + weak_nodes_.insert(weak_node_ptr); + this->add_automatically_associated_callback_groups({weak_node_ptr}); + + // Store node guard condition in map and add it to the notify waitable + auto node_guard_condition = node_ptr->get_shared_notify_guard_condition(); + weak_nodes_to_guard_conditions_.insert({weak_node_ptr, node_guard_condition}); + this->notify_waitable_->add_guard_condition(node_guard_condition); + } + pending_added_nodes_.clear(); + + for (auto weak_node_ptr : pending_removed_nodes_) { + auto node_it = weak_nodes_.find(weak_node_ptr); + if (node_it != weak_nodes_.end()) { + remove_weak_node(node_it); + } else { + // The node may have been destroyed and removed from the colletion before + // we processed the queues. Don't throw if the pointer is already expired. + if (!weak_node_ptr.expired()) { + throw std::runtime_error("Node needs to be associated with this executor."); + } + } + + auto node_ptr = weak_node_ptr.lock(); + if (node_ptr) { + for (auto group_it = automatically_added_groups_.begin(); + group_it != automatically_added_groups_.end(); ) + { + auto group_ptr = group_it->lock(); + if (node_ptr->callback_group_in_node(group_ptr)) { + group_it = remove_weak_callback_group(group_it, automatically_added_groups_); + } else { + ++group_it; + } + } + } + } + pending_removed_nodes_.clear(); + + for (auto weak_group_ptr : pending_manually_added_groups_) { + auto group_ptr = weak_group_ptr.lock(); + if (group_ptr) { + this->add_callback_group_to_collection(group_ptr, manually_added_groups_); + } else { + // Disassociate the guard condition from the executor notify waitable + auto guard_condition_it = weak_groups_to_guard_conditions_.find(weak_group_ptr); + if (guard_condition_it != weak_groups_to_guard_conditions_.end()) { + this->notify_waitable_->remove_guard_condition(guard_condition_it->second); + weak_groups_to_guard_conditions_.erase(guard_condition_it); + } + } + } + pending_manually_added_groups_.clear(); + + for (auto weak_group_ptr : pending_manually_removed_groups_) { + auto group_ptr = weak_group_ptr.lock(); + if (group_ptr) { + auto group_it = manually_added_groups_.find(group_ptr); + if (group_it != manually_added_groups_.end()) { + remove_weak_callback_group(group_it, manually_added_groups_); + } else { + throw std::runtime_error( + "Attempting to remove a callback group not added to this executor."); + } + } + } + pending_manually_removed_groups_.clear(); +} + +void +ExecutorEntitiesCollector::add_automatically_associated_callback_groups( + const NodeCollection & nodes_to_check) +{ + for (auto & weak_node : nodes_to_check) { + auto node = weak_node.lock(); + if (node) { + node->for_each_callback_group( + [this, node](rclcpp::CallbackGroup::SharedPtr group_ptr) + { + if (!group_ptr->get_associated_with_executor_atomic().load() && + group_ptr->automatically_add_to_executor_with_node()) + { + std::atomic_bool & has_executor = group_ptr->get_associated_with_executor_atomic(); + if (has_executor.exchange(true)) { + throw std::runtime_error("Callback group has already been added to an executor."); + } + this->add_callback_group_to_collection(group_ptr, this->automatically_added_groups_); + } + }); + } + } +} + +void +ExecutorEntitiesCollector::prune_invalid_nodes_and_groups() +{ + for (auto node_it = weak_nodes_.begin(); + node_it != weak_nodes_.end(); ) + { + if (node_it->expired()) { + node_it = remove_weak_node(node_it); + } else { + node_it++; + } + } + for (auto group_it = automatically_added_groups_.begin(); + group_it != automatically_added_groups_.end(); ) + { + if (group_it->expired()) { + group_it = remove_weak_callback_group(group_it, automatically_added_groups_); + } else { + group_it++; + } + } + for (auto group_it = manually_added_groups_.begin(); + group_it != manually_added_groups_.end(); ) + { + if (group_it->expired()) { + group_it = remove_weak_callback_group(group_it, manually_added_groups_); + } else { + group_it++; + } + } +} + +} // namespace executors +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp b/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp new file mode 100644 index 0000000000..2e62f9dd1a --- /dev/null +++ b/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp @@ -0,0 +1,193 @@ +// Copyright 2023 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 "rclcpp/exceptions.hpp" +#include "rclcpp/executors/executor_notify_waitable.hpp" + +namespace rclcpp +{ +namespace executors +{ + +ExecutorNotifyWaitable::ExecutorNotifyWaitable(std::function on_execute_callback) +: execute_callback_(on_execute_callback) +{ +} + +ExecutorNotifyWaitable::ExecutorNotifyWaitable(ExecutorNotifyWaitable & other) +{ + std::lock_guard lock(other.guard_condition_mutex_); + this->execute_callback_ = other.execute_callback_; + this->notify_guard_conditions_ = other.notify_guard_conditions_; +} + +ExecutorNotifyWaitable & ExecutorNotifyWaitable::operator=(ExecutorNotifyWaitable & other) +{ + if (this != &other) { + std::lock_guard lock(other.guard_condition_mutex_); + this->execute_callback_ = other.execute_callback_; + this->notify_guard_conditions_ = other.notify_guard_conditions_; + } + return *this; +} + +void +ExecutorNotifyWaitable::add_to_wait_set(rcl_wait_set_t & wait_set) +{ + std::lock_guard lock(guard_condition_mutex_); + + // Note: no guard conditions need to be re-triggered, since the guard + // conditions in this class are not tracking a stateful condition, but instead + // only serve to interrupt the wait set when new information is available to + // consider. + for (auto weak_guard_condition : this->notify_guard_conditions_) { + auto guard_condition = weak_guard_condition.lock(); + if (!guard_condition) {continue;} + + rcl_guard_condition_t * cond = &guard_condition->get_rcl_guard_condition(); + rcl_ret_t ret = rcl_wait_set_add_guard_condition(&wait_set, cond, NULL); + + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "failed to add guard condition to wait set"); + } + } +} + +bool +ExecutorNotifyWaitable::is_ready(const rcl_wait_set_t & wait_set) +{ + std::lock_guard lock(guard_condition_mutex_); + + bool any_ready = false; + for (size_t ii = 0; ii < wait_set.size_of_guard_conditions; ++ii) { + const auto * rcl_guard_condition = wait_set.guard_conditions[ii]; + + if (nullptr == rcl_guard_condition) { + continue; + } + for (const auto & weak_guard_condition : this->notify_guard_conditions_) { + auto guard_condition = weak_guard_condition.lock(); + if (guard_condition && &guard_condition->get_rcl_guard_condition() == rcl_guard_condition) { + any_ready = true; + break; + } + } + } + return any_ready; +} + +void +ExecutorNotifyWaitable::execute(const std::shared_ptr & /*data*/) +{ + std::lock_guard lock(execute_mutex_); + this->execute_callback_(); +} + +std::shared_ptr +ExecutorNotifyWaitable::take_data() +{ + return nullptr; +} + +std::shared_ptr +ExecutorNotifyWaitable::take_data_by_entity_id(size_t id) +{ + (void) id; + return nullptr; +} + +void +ExecutorNotifyWaitable::set_on_ready_callback(std::function callback) +{ + // The second argument of the callback could be used to identify which guard condition + // triggered the event. + // We could indicate which of the guard conditions was triggered, but the executor + // is already going to check that. + auto gc_callback = [callback](size_t count) { + callback(count, 0); + }; + + std::lock_guard lock(guard_condition_mutex_); + + on_ready_callback_ = gc_callback; + for (auto weak_gc : notify_guard_conditions_) { + auto gc = weak_gc.lock(); + if (!gc) { + continue; + } + gc->set_on_trigger_callback(on_ready_callback_); + } +} + +RCLCPP_PUBLIC +void +ExecutorNotifyWaitable::clear_on_ready_callback() +{ + std::lock_guard lock(guard_condition_mutex_); + + on_ready_callback_ = nullptr; + for (auto weak_gc : notify_guard_conditions_) { + auto gc = weak_gc.lock(); + if (!gc) { + continue; + } + gc->set_on_trigger_callback(nullptr); + } +} + +RCLCPP_PUBLIC +void +ExecutorNotifyWaitable::set_execute_callback(std::function on_execute_callback) +{ + std::lock_guard lock(execute_mutex_); + execute_callback_ = on_execute_callback; +} + +void +ExecutorNotifyWaitable::add_guard_condition(rclcpp::GuardCondition::WeakPtr weak_guard_condition) +{ + std::lock_guard lock(guard_condition_mutex_); + auto guard_condition = weak_guard_condition.lock(); + if (guard_condition && notify_guard_conditions_.count(weak_guard_condition) == 0) { + notify_guard_conditions_.insert(weak_guard_condition); + if (on_ready_callback_) { + guard_condition->set_on_trigger_callback(on_ready_callback_); + } + } +} + +void +ExecutorNotifyWaitable::remove_guard_condition(rclcpp::GuardCondition::WeakPtr weak_guard_condition) +{ + std::lock_guard lock(guard_condition_mutex_); + if (notify_guard_conditions_.count(weak_guard_condition) != 0) { + notify_guard_conditions_.erase(weak_guard_condition); + auto guard_condition = weak_guard_condition.lock(); + // If this notify waitable doesn't have an on_ready_callback, then there's nothing to unset + if (guard_condition && on_ready_callback_) { + guard_condition->set_on_trigger_callback(nullptr); + } + } +} + +size_t +ExecutorNotifyWaitable::get_number_of_ready_guard_conditions() +{ + std::lock_guard lock(guard_condition_mutex_); + return notify_guard_conditions_.size(); +} + +} // namespace executors +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp index b31fbe7e0c..130b4d953f 100644 --- a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp @@ -16,19 +16,34 @@ #include #include +#include #include +#include "rcpputils/scope_exit.hpp" + +#include "rclcpp/logging.hpp" #include "rclcpp/utilities.hpp" -#include "rclcpp/scope_exit.hpp" using rclcpp::executors::MultiThreadedExecutor; -MultiThreadedExecutor::MultiThreadedExecutor(const rclcpp::executor::ExecutorArgs & args) -: executor::Executor(args) +MultiThreadedExecutor::MultiThreadedExecutor( + const rclcpp::ExecutorOptions & options, + size_t number_of_threads, + bool yield_before_execute, + std::chrono::nanoseconds next_exec_timeout) +: rclcpp::Executor(options), + yield_before_execute_(yield_before_execute), + next_exec_timeout_(next_exec_timeout) { - number_of_threads_ = std::thread::hardware_concurrency(); - if (number_of_threads_ == 0) { - number_of_threads_ = 1; + number_of_threads_ = number_of_threads > 0 ? + number_of_threads : + std::max(std::thread::hardware_concurrency(), 2U); + + if (number_of_threads_ == 1) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "MultiThreadedExecutor is used with a single thread.\n" + "Use the SingleThreadedExecutor instead."); } } @@ -40,11 +55,11 @@ MultiThreadedExecutor::spin() if (spinning.exchange(true)) { throw std::runtime_error("spin() called while already spinning"); } - RCLCPP_SCOPE_EXIT(this->spinning.store(false); ); + RCPPUTILS_SCOPE_EXIT(wait_result_.reset();this->spinning.store(false);); std::vector threads; size_t thread_id = 0; { - std::lock_guard wait_lock(wait_mutex_); + std::lock_guard wait_lock{wait_mutex_}; for (; thread_id < number_of_threads_ - 1; ++thread_id) { auto func = std::bind(&MultiThreadedExecutor::run, this, thread_id); threads.emplace_back(func); @@ -64,17 +79,40 @@ MultiThreadedExecutor::get_number_of_threads() } void -MultiThreadedExecutor::run(size_t) +MultiThreadedExecutor::run(size_t this_thread_number) { - while (rclcpp::ok() && spinning.load()) { - executor::AnyExecutable::SharedPtr any_exec; + (void)this_thread_number; + while (rclcpp::ok(this->context_) && spinning.load()) { + rclcpp::AnyExecutable any_exec; { - std::lock_guard wait_lock(wait_mutex_); - if (!rclcpp::ok() || !spinning.load()) { + std::lock_guard wait_lock{wait_mutex_}; + if (!rclcpp::ok(this->context_) || !spinning.load()) { return; } - any_exec = get_next_executable(); + if (!get_next_executable(any_exec, next_exec_timeout_)) { + continue; + } } + if (yield_before_execute_) { + std::this_thread::yield(); + } + execute_any_executable(any_exec); + + if (any_exec.callback_group && + any_exec.callback_group->type() == CallbackGroupType::MutuallyExclusive) + { + try { + interrupt_guard_condition_->trigger(); + } catch (const rclcpp::exceptions::RCLError & ex) { + throw std::runtime_error( + std::string( + "Failed to trigger guard condition on callback group change: ") + ex.what()); + } + } + + // Clear the callback_group to prevent the AnyExecutable destructor from + // resetting the callback group `can_be_taken_from` + any_exec.callback_group.reset(); } } diff --git a/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp index 291560d13c..689bbae398 100644 --- a/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp @@ -12,13 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "rcpputils/scope_exit.hpp" + #include "rclcpp/executors/single_threaded_executor.hpp" -#include "rclcpp/scope_exit.hpp" +#include "rclcpp/any_executable.hpp" using rclcpp::executors::SingleThreadedExecutor; -SingleThreadedExecutor::SingleThreadedExecutor(const rclcpp::executor::ExecutorArgs & args) -: executor::Executor(args) {} +SingleThreadedExecutor::SingleThreadedExecutor(const rclcpp::ExecutorOptions & options) +: rclcpp::Executor(options) {} SingleThreadedExecutor::~SingleThreadedExecutor() {} @@ -28,9 +30,16 @@ SingleThreadedExecutor::spin() if (spinning.exchange(true)) { throw std::runtime_error("spin() called while already spinning"); } - RCLCPP_SCOPE_EXIT(this->spinning.store(false); ); - while (rclcpp::ok() && spinning.load()) { - auto any_exec = get_next_executable(); - execute_any_executable(any_exec); + RCPPUTILS_SCOPE_EXIT(wait_result_.reset();this->spinning.store(false);); + + // Clear any previous result and rebuild the waitset + this->wait_result_.reset(); + this->entities_need_rebuild_ = true; + + while (rclcpp::ok(this->context_) && spinning.load()) { + rclcpp::AnyExecutable any_executable; + if (get_next_executable(any_executable)) { + execute_any_executable(any_executable); + } } } diff --git a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp new file mode 100644 index 0000000000..d517ccafd0 --- /dev/null +++ b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp @@ -0,0 +1,204 @@ +// Copyright 2019 Nobleo Technology +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT 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 "rclcpp/executors/executor_entities_collection.hpp" +#include "rcpputils/scope_exit.hpp" + +#include "rclcpp/executors/static_single_threaded_executor.hpp" +#include "rclcpp/any_executable.hpp" + +using rclcpp::executors::StaticSingleThreadedExecutor; + +StaticSingleThreadedExecutor::StaticSingleThreadedExecutor(const rclcpp::ExecutorOptions & options) +: rclcpp::Executor(options) +{ +} + +StaticSingleThreadedExecutor::~StaticSingleThreadedExecutor() {} + +void +StaticSingleThreadedExecutor::spin() +{ + if (spinning.exchange(true)) { + throw std::runtime_error("spin() called while already spinning"); + } + RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); + + // This is essentially the contents of the rclcpp::Executor::wait_for_work method, + // except we need to keep the wait result to reproduce the StaticSingleThreadedExecutor + // behavior. + while (rclcpp::ok(this->context_) && spinning.load()) { + this->spin_once_impl(std::chrono::nanoseconds(-1)); + } +} + +void +StaticSingleThreadedExecutor::spin_some(std::chrono::nanoseconds max_duration) +{ + // In this context a 0 input max_duration means no duration limit + if (std::chrono::nanoseconds(0) == max_duration) { + max_duration = std::chrono::nanoseconds::max(); + } + return this->spin_some_impl(max_duration, false); +} + +void +StaticSingleThreadedExecutor::spin_all(std::chrono::nanoseconds max_duration) +{ + if (max_duration < std::chrono::nanoseconds(0)) { + throw std::invalid_argument("max_duration must be greater than or equal to 0"); + } + return this->spin_some_impl(max_duration, true); +} + +void +StaticSingleThreadedExecutor::spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive) +{ + auto start = std::chrono::steady_clock::now(); + auto max_duration_not_elapsed = [max_duration, start]() { + const auto spin_forever = std::chrono::nanoseconds(0) == max_duration; + const auto cur_duration = std::chrono::steady_clock::now() - start; + return spin_forever || (cur_duration < max_duration); + }; + + if (spinning.exchange(true)) { + throw std::runtime_error("spin_some() called while already spinning"); + } + RCPPUTILS_SCOPE_EXIT(this->spinning.store(false);); + + while (rclcpp::ok(context_) && spinning.load() && max_duration_not_elapsed()) { + // Get executables that are ready now + std::lock_guard guard(mutex_); + + auto wait_result = this->collect_and_wait(std::chrono::nanoseconds(0)); + if (wait_result.has_value()) { + // Execute ready executables + bool work_available = this->execute_ready_executables( + current_collection_, + wait_result.value(), + false); + if (!work_available || !exhaustive) { + break; + } + } + } +} + +void +StaticSingleThreadedExecutor::spin_once_impl(std::chrono::nanoseconds timeout) +{ + if (rclcpp::ok(context_) && spinning.load()) { + std::lock_guard guard(mutex_); + auto wait_result = this->collect_and_wait(timeout); + if (wait_result.has_value()) { + this->execute_ready_executables(current_collection_, wait_result.value(), true); + } + } +} + +std::optional> +StaticSingleThreadedExecutor::collect_and_wait(std::chrono::nanoseconds timeout) +{ + if (this->entities_need_rebuild_.exchange(false) || current_collection_.empty()) { + this->collect_entities(); + } + auto wait_result = wait_set_.wait(std::chrono::nanoseconds(timeout)); + if (wait_result.kind() == WaitResultKind::Empty) { + RCUTILS_LOG_WARN_NAMED( + "rclcpp", + "empty wait set received in wait(). This should never happen."); + return {}; + } else { + if (wait_result.kind() == WaitResultKind::Ready && current_notify_waitable_) { + auto & rcl_wait_set = wait_result.get_wait_set().get_rcl_wait_set(); + if (current_notify_waitable_->is_ready(rcl_wait_set)) { + current_notify_waitable_->execute(current_notify_waitable_->take_data()); + } + } + } + return wait_result; +} + +// This preserves the "scheduling semantics" of the StaticSingleThreadedExecutor +// from the original implementation. +bool StaticSingleThreadedExecutor::execute_ready_executables( + const rclcpp::executors::ExecutorEntitiesCollection & collection, + rclcpp::WaitResult & wait_result, + bool spin_once) +{ + bool any_ready_executable = false; + if (wait_result.kind() != rclcpp::WaitResultKind::Ready) { + return any_ready_executable; + } + + while (auto subscription = wait_result.next_ready_subscription()) { + auto entity_iter = collection.subscriptions.find(subscription->get_subscription_handle().get()); + if (entity_iter != collection.subscriptions.end()) { + execute_subscription(subscription); + any_ready_executable = true; + if (spin_once) {return any_ready_executable;} + } + } + + size_t current_timer_index = 0; + while (true) { + auto [timer, timer_index] = wait_result.peek_next_ready_timer(current_timer_index); + if (nullptr == timer) { + break; + } + current_timer_index = timer_index; + auto entity_iter = collection.timers.find(timer->get_timer_handle().get()); + if (entity_iter != collection.timers.end()) { + wait_result.clear_timer_with_index(current_timer_index); + auto data = timer->call(); + if (!data) { + // someone canceled the timer between is_ready and call + continue; + } + + execute_timer(std::move(timer), data); + any_ready_executable = true; + if (spin_once) {return any_ready_executable;} + } + } + + while (auto client = wait_result.next_ready_client()) { + auto entity_iter = collection.clients.find(client->get_client_handle().get()); + if (entity_iter != collection.clients.end()) { + execute_client(client); + any_ready_executable = true; + if (spin_once) {return any_ready_executable;} + } + } + + while (auto service = wait_result.next_ready_service()) { + auto entity_iter = collection.services.find(service->get_service_handle().get()); + if (entity_iter != collection.services.end()) { + execute_service(service); + any_ready_executable = true; + if (spin_once) {return any_ready_executable;} + } + } + + while (auto waitable = wait_result.next_ready_waitable()) { + auto entity_iter = collection.waitables.find(waitable.get()); + if (entity_iter != collection.waitables.end()) { + const auto data = waitable->take_data(); + waitable->execute(data); + any_ready_executable = true; + if (spin_once) {return any_ready_executable;} + } + } + return any_ready_executable; +} diff --git a/rclcpp/src/rclcpp/expand_topic_or_service_name.cpp b/rclcpp/src/rclcpp/expand_topic_or_service_name.cpp index ca2787ca0e..79aecdad42 100644 --- a/rclcpp/src/rclcpp/expand_topic_or_service_name.cpp +++ b/rclcpp/src/rclcpp/expand_topic_or_service_name.cpp @@ -19,7 +19,7 @@ #include "rcl/expand_topic_name.h" #include "rcl/validate_topic_name.h" #include "rclcpp/exceptions.hpp" -#include "rclcpp/scope_exit.hpp" +#include "rcutils/logging_macros.h" #include "rcutils/types/string_map.h" #include "rmw/error_handling.h" #include "rmw/validate_namespace.h" @@ -50,25 +50,18 @@ rclcpp::expand_topic_or_service_name( } rcl_ret_t ret = rcl_get_default_topic_name_substitutions(&substitutions_map); if (ret != RCL_RET_OK) { - rcutils_error_state_t error_state; - if (rcutils_error_state_copy(rcl_get_error_state(), &error_state) != RCUTILS_RET_OK) { - throw std::bad_alloc(); - } - auto error_state_scope_exit = rclcpp::make_scope_exit( - [&error_state]() { - rcutils_error_state_fini(&error_state); - }); + const rcutils_error_state_t * error_state = rcl_get_error_state(); // finalize the string map before throwing rcutils_ret = rcutils_string_map_fini(&substitutions_map); if (rcutils_ret != RCUTILS_RET_OK) { - fprintf(stderr, - "[rclcpp|" RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__LINE__) "]: " - "failed to fini string_map (%d) during error handling: %s\n", + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "failed to fini string_map (%d) during error handling: %s", rcutils_ret, - rcutils_get_error_string_safe()); + rcutils_get_error_string().str); rcutils_reset_error(); } - throw_from_rcl_error(ret, "", &error_state); + throw_from_rcl_error(ret, "", error_state); } ret = rcl_expand_topic_name( diff --git a/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp b/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp new file mode 100644 index 0000000000..e8a6e1889c --- /dev/null +++ b/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp @@ -0,0 +1,450 @@ +// Copyright 2023 iRobot Corporation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT 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 "rclcpp/experimental/executors/events_executor/events_executor.hpp" + +#include +#include +#include + +#include "rcpputils/scope_exit.hpp" + +using namespace std::chrono_literals; + +using rclcpp::experimental::executors::EventsExecutor; + +EventsExecutor::EventsExecutor( + rclcpp::experimental::executors::EventsQueue::UniquePtr events_queue, + bool execute_timers_separate_thread, + const rclcpp::ExecutorOptions & options) +: rclcpp::Executor(options) +{ + // Get ownership of the queue used to store events. + if (!events_queue) { + throw std::invalid_argument("events_queue can't be a null pointer"); + } + events_queue_ = std::move(events_queue); + + // Create timers manager + // The timers manager can be used either to only track timers (in this case an expired + // timer will generate an executor event and then it will be executed by the executor thread) + // or it can also take care of executing expired timers in its dedicated thread. + std::function &)> timer_on_ready_cb = nullptr; + if (!execute_timers_separate_thread) { + timer_on_ready_cb = + [this](const rclcpp::TimerBase * timer_id, const std::shared_ptr & data) { + ExecutorEvent event = {timer_id, data, -1, ExecutorEventType::TIMER_EVENT, 1}; + this->events_queue_->enqueue(event); + }; + } + timers_manager_ = + std::make_shared(context_, timer_on_ready_cb); + + entities_need_rebuild_ = false; + + this->setup_notify_waitable(); + + // Ensure that the entities collection is empty (the base class may have added elements + // that we are not interested in) + this->current_collection_.clear(); + + // Make sure that the notify waitable is immediately added to the collection + // to avoid missing events + this->add_notify_waitable_to_collection(current_collection_.waitables); +} + +void +EventsExecutor::setup_notify_waitable() +{ + // The base class already created this object but the events-executor + // needs different callbacks. + assert(notify_waitable_ && "The notify waitable should have already been constructed"); + + notify_waitable_->set_execute_callback( + [this]() { + // This callback is invoked when: + // - the interrupt or shutdown guard condition is triggered: + // ---> we need to wake up the executor so that it can terminate + // - a node or callback group guard condition is triggered: + // ---> the entities collection is changed, we need to update callbacks + this->handle_updated_entities(false); + }); + + auto notify_waitable_entity_id = notify_waitable_.get(); + notify_waitable_->set_on_ready_callback( + [this, notify_waitable_entity_id](size_t num_events, int waitable_data) { + // The notify waitable has a special callback. + // We don't care about how many events as when we wake up the executor we are going to + // process everything regardless. + // For the same reason, if an event of this type has already been pushed but it has not been + // processed yet, we avoid pushing additional events. + (void)num_events; + if (entities_need_rebuild_.exchange(true)) { + return; + } + + ExecutorEvent event = + {notify_waitable_entity_id, nullptr, waitable_data, ExecutorEventType::WAITABLE_EVENT, 1}; + this->events_queue_->enqueue(event); + }); +} + +EventsExecutor::~EventsExecutor() +{ + spinning.store(false); + notify_waitable_->clear_on_ready_callback(); + this->refresh_current_collection({}); +} + +void +EventsExecutor::spin() +{ + if (spinning.exchange(true)) { + throw std::runtime_error("spin() called while already spinning"); + } + RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); + + timers_manager_->start(); + RCPPUTILS_SCOPE_EXIT(timers_manager_->stop(); ); + + while (rclcpp::ok(context_) && spinning.load()) { + // Wait until we get an event + ExecutorEvent event; + bool has_event = events_queue_->dequeue(event); + if (has_event) { + this->execute_event(event); + } + } +} + +void +EventsExecutor::spin_some(std::chrono::nanoseconds max_duration) +{ + return this->spin_some_impl(max_duration, false); +} + +void +EventsExecutor::spin_all(std::chrono::nanoseconds max_duration) +{ + if (max_duration <= 0ns) { + throw std::invalid_argument("max_duration must be positive"); + } + return this->spin_some_impl(max_duration, true); +} + +void +EventsExecutor::spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive) +{ + if (spinning.exchange(true)) { + throw std::runtime_error("spin_some() called while already spinning"); + } + + RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); + + auto start = std::chrono::steady_clock::now(); + + auto max_duration_not_elapsed = [max_duration, start]() { + if (std::chrono::nanoseconds(0) == max_duration) { + // told to spin forever if need be + return true; + } else if (std::chrono::steady_clock::now() - start < max_duration) { + // told to spin only for some maximum amount of time + return true; + } + // spun too long + return false; + }; + + // If this spin is not exhaustive (e.g. spin_some), we need to explicitly check + // if entities need to be rebuilt here rather than letting the notify waitable event do it. + // A non-exhaustive spin would not check for work a second time, thus delaying the execution + // of some entities to the next invocation of spin. + if (!exhaustive) { + this->handle_updated_entities(false); + } + + // Get the number of events and timers ready at start + const size_t ready_events_at_start = events_queue_->size(); + size_t executed_events = 0; + const size_t ready_timers_at_start = timers_manager_->get_number_ready_timers(); + size_t executed_timers = 0; + + while (rclcpp::ok(context_) && spinning.load() && max_duration_not_elapsed()) { + // Execute first ready event from queue if exists + if (exhaustive || (executed_events < ready_events_at_start)) { + bool has_event = !events_queue_->empty(); + + if (has_event) { + ExecutorEvent event; + bool ret = events_queue_->dequeue(event, std::chrono::nanoseconds(0)); + if (ret) { + this->execute_event(event); + executed_events++; + continue; + } + } + } + + // Execute first timer if it is ready + if (exhaustive || (executed_timers < ready_timers_at_start)) { + bool timer_executed = timers_manager_->execute_head_timer(); + if (timer_executed) { + executed_timers++; + continue; + } + } + + // If there's no more work available, exit + break; + } +} + +void +EventsExecutor::spin_once_impl(std::chrono::nanoseconds timeout) +{ + // In this context a negative input timeout means no timeout + if (timeout < 0ns) { + timeout = std::chrono::nanoseconds::max(); + } + + // Select the smallest between input timeout and timer timeout. + // Cancelled timers are not considered. + bool is_timer_timeout = false; + auto next_timer_timeout = timers_manager_->get_head_timeout(); + if (next_timer_timeout.has_value() && next_timer_timeout.value() < timeout) { + timeout = next_timer_timeout.value(); + is_timer_timeout = true; + } + + ExecutorEvent event; + bool has_event = events_queue_->dequeue(event, timeout); + + // If we wake up from the wait with an event, it means that it + // arrived before any of the timers expired. + if (has_event) { + this->execute_event(event); + } else if (is_timer_timeout) { + timers_manager_->execute_head_timer(); + } +} + + +void +EventsExecutor::execute_event(const ExecutorEvent & event) +{ + switch (event.type) { + case ExecutorEventType::CLIENT_EVENT: + { + rclcpp::ClientBase::SharedPtr client; + { + client = this->retrieve_entity( + static_cast(event.entity_key), + current_collection_.clients); + } + if (client) { + for (size_t i = 0; i < event.num_events; i++) { + execute_client(client); + } + } + + break; + } + case ExecutorEventType::SUBSCRIPTION_EVENT: + { + rclcpp::SubscriptionBase::SharedPtr subscription; + { + subscription = this->retrieve_entity( + static_cast(event.entity_key), + current_collection_.subscriptions); + } + if (subscription) { + for (size_t i = 0; i < event.num_events; i++) { + execute_subscription(subscription); + } + } + break; + } + case ExecutorEventType::SERVICE_EVENT: + { + rclcpp::ServiceBase::SharedPtr service; + { + service = this->retrieve_entity( + static_cast(event.entity_key), + current_collection_.services); + } + if (service) { + for (size_t i = 0; i < event.num_events; i++) { + execute_service(service); + } + } + + break; + } + case ExecutorEventType::TIMER_EVENT: + { + timers_manager_->execute_ready_timer( + static_cast(event.entity_key), event.data); + break; + } + case ExecutorEventType::WAITABLE_EVENT: + { + rclcpp::Waitable::SharedPtr waitable; + { + waitable = this->retrieve_entity( + static_cast(event.entity_key), + current_collection_.waitables); + } + if (waitable) { + for (size_t i = 0; i < event.num_events; i++) { + const auto data = waitable->take_data_by_entity_id(event.waitable_data); + waitable->execute(data); + } + } + break; + } + } +} + +void +EventsExecutor::handle_updated_entities(bool notify) +{ + (void)notify; + + // Do not rebuild if we don't need to. + // A rebuild event could be generated, but then + // this function could end up being called from somewhere else + // before that event gets processed, for example if + // a node or callback group is manually added to the executor. + const bool notify_waitable_triggered = entities_need_rebuild_.exchange(false); + if (!notify_waitable_triggered && !this->collector_.has_pending()) { + return; + } + + // Build the new collection + this->collector_.update_collections(); + auto callback_groups = this->collector_.get_all_callback_groups(); + rclcpp::executors::ExecutorEntitiesCollection new_collection; + rclcpp::executors::build_entities_collection(callback_groups, new_collection); + + // TODO(alsora): this may be implemented in a better way. + // We need the notify waitable to be included in the executor "current_collection" + // because we need to be able to retrieve events for it. + // We could explicitly check for the notify waitable ID when we receive a waitable event + // but I think that it's better if the waitable was in the collection and it could be + // retrieved in the "standard" way. + // To do it, we need to add the notify waitable as an entry in the new collection + // such that it's neither added or removed (it should have already been added + // to the current collection in the constructor) + this->add_notify_waitable_to_collection(new_collection.waitables); + + this->refresh_current_collection(new_collection); +} + +void +EventsExecutor::refresh_current_collection( + const rclcpp::executors::ExecutorEntitiesCollection & new_collection) +{ + // Acquire lock before modifying the current collection + std::lock_guard guard(mutex_); + + // Remove expired entities to ensure re-initialized objects + // are updated. This fixes issues with stale state entities. + // See: https://github.com/ros2/rclcpp/pull/2586 + current_collection_.remove_expired_entities(); + + current_collection_.timers.update( + new_collection.timers, + [this](rclcpp::TimerBase::SharedPtr timer) {timers_manager_->add_timer(timer);}, + [this](rclcpp::TimerBase::SharedPtr timer) {timers_manager_->remove_timer(timer);}); + + current_collection_.subscriptions.update( + new_collection.subscriptions, + [this](auto subscription) { + subscription->set_on_new_message_callback( + this->create_entity_callback( + subscription->get_subscription_handle().get(), ExecutorEventType::SUBSCRIPTION_EVENT)); + }, + [](auto subscription) {subscription->clear_on_new_message_callback();}); + + current_collection_.clients.update( + new_collection.clients, + [this](auto client) { + client->set_on_new_response_callback( + this->create_entity_callback( + client->get_client_handle().get(), ExecutorEventType::CLIENT_EVENT)); + }, + [](auto client) {client->clear_on_new_response_callback();}); + + current_collection_.services.update( + new_collection.services, + [this](auto service) { + service->set_on_new_request_callback( + this->create_entity_callback( + service->get_service_handle().get(), ExecutorEventType::SERVICE_EVENT)); + }, + [](auto service) {service->clear_on_new_request_callback();}); + + // DO WE NEED THIS? WE ARE NOT DOING ANYTHING WITH GUARD CONDITIONS + /* + current_collection_.guard_conditions.update(new_collection.guard_conditions, + [](auto guard_condition) {(void)guard_condition;}, + [](auto guard_condition) {guard_condition->set_on_trigger_callback(nullptr);}); + */ + + current_collection_.waitables.update( + new_collection.waitables, + [this](auto waitable) { + waitable->set_on_ready_callback( + this->create_waitable_callback(waitable.get())); + }, + [](auto waitable) {waitable->clear_on_ready_callback();}); +} + +std::function +EventsExecutor::create_entity_callback( + void * entity_key, ExecutorEventType event_type) +{ + std::function + callback = [this, entity_key, event_type](size_t num_events) { + ExecutorEvent event = {entity_key, nullptr, -1, event_type, num_events}; + this->events_queue_->enqueue(event); + }; + return callback; +} + +std::function +EventsExecutor::create_waitable_callback(const rclcpp::Waitable * entity_key) +{ + std::function + callback = [this, entity_key](size_t num_events, int waitable_data) { + ExecutorEvent event = + {entity_key, nullptr, waitable_data, ExecutorEventType::WAITABLE_EVENT, num_events}; + this->events_queue_->enqueue(event); + }; + return callback; +} + +void +EventsExecutor::add_notify_waitable_to_collection( + rclcpp::executors::ExecutorEntitiesCollection::WaitableCollection & collection) +{ + // The notify waitable is not associated to any group, so use an invalid one + rclcpp::CallbackGroup::WeakPtr weak_group_ptr; + collection.insert( + { + this->notify_waitable_.get(), + {this->notify_waitable_, weak_group_ptr} + }); +} diff --git a/rclcpp/src/rclcpp/experimental/timers_manager.cpp b/rclcpp/src/rclcpp/experimental/timers_manager.cpp new file mode 100644 index 0000000000..2caa0a6b15 --- /dev/null +++ b/rclcpp/src/rclcpp/experimental/timers_manager.cpp @@ -0,0 +1,329 @@ +// Copyright 2023 iRobot Corporation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT 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 "rclcpp/experimental/timers_manager.hpp" + +#include + +#include +#include +#include +#include + +#include "rcpputils/scope_exit.hpp" + +using rclcpp::experimental::TimersManager; + +TimersManager::TimersManager( + std::shared_ptr context, + std::function &)> on_ready_callback) +: on_ready_callback_(on_ready_callback), + context_(context) +{ +} + +TimersManager::~TimersManager() +{ + // Remove all timers + this->clear(); + + // Make sure timers thread is stopped before destroying this object + this->stop(); +} + +void TimersManager::add_timer(rclcpp::TimerBase::SharedPtr timer) +{ + if (!timer) { + throw std::invalid_argument("TimersManager::add_timer() trying to add nullptr timer"); + } + + bool added = false; + { + std::unique_lock lock(timers_mutex_); + added = weak_timers_heap_.add_timer(timer); + timers_updated_ = timers_updated_ || added; + } + + timer->set_on_reset_callback( + [this](size_t arg) { + { + (void)arg; + std::unique_lock lock(timers_mutex_); + timers_updated_ = true; + } + timers_cv_.notify_one(); + }); + + if (added) { + // Notify that a timer has been added + timers_cv_.notify_one(); + } +} + +void TimersManager::start() +{ + // Make sure that the thread is not already running + if (running_.exchange(true)) { + throw std::runtime_error("TimersManager::start() can't start timers thread as already running"); + } + + timers_thread_ = std::thread(&TimersManager::run_timers, this); +} + +void TimersManager::stop() +{ + // Lock stop() function to prevent race condition in destructor + std::unique_lock lock(stop_mutex_); + running_ = false; + + // Notify the timers manager thread to wake up + { + std::unique_lock lock(timers_mutex_); + timers_updated_ = true; + } + timers_cv_.notify_one(); + + // Join timers thread if it's running + if (timers_thread_.joinable()) { + timers_thread_.join(); + } +} + +std::optional TimersManager::get_head_timeout() +{ + // Do not allow to interfere with the thread running + if (running_) { + throw std::runtime_error( + "get_head_timeout() can't be used while timers thread is running"); + } + + std::unique_lock lock(timers_mutex_); + return this->get_head_timeout_unsafe(); +} + +size_t TimersManager::get_number_ready_timers() +{ + // Do not allow to interfere with the thread running + if (running_) { + throw std::runtime_error( + "get_number_ready_timers() can't be used while timers thread is running"); + } + + std::unique_lock lock(timers_mutex_); + TimersHeap locked_heap = weak_timers_heap_.validate_and_lock(); + return locked_heap.get_number_ready_timers(); +} + +bool TimersManager::execute_head_timer() +{ + // Do not allow to interfere with the thread running + if (running_) { + throw std::runtime_error( + "execute_head_timer() can't be used while timers thread is running"); + } + + std::unique_lock lock(timers_mutex_); + + TimersHeap timers_heap = weak_timers_heap_.validate_and_lock(); + + // Nothing to do if we don't have any timer + if (timers_heap.empty()) { + return false; + } + + TimerPtr head_timer = timers_heap.front(); + + const bool timer_ready = head_timer->is_ready(); + if (timer_ready) { + // NOTE: here we always execute the timer, regardless of whether the + // on_ready_callback is set or not. + auto data = head_timer->call(); + if (!data) { + // someone canceled the timer between is_ready and call + return false; + } + head_timer->execute_callback(data); + timers_heap.heapify_root(); + weak_timers_heap_.store(timers_heap); + } + + return timer_ready; +} + +void TimersManager::execute_ready_timer( + const rclcpp::TimerBase * timer_id, + const std::shared_ptr & data) +{ + TimerPtr ready_timer; + { + std::unique_lock lock(timers_mutex_); + ready_timer = weak_timers_heap_.get_timer(timer_id); + } + if (ready_timer) { + ready_timer->execute_callback(data); + } +} + +std::optional TimersManager::get_head_timeout_unsafe() +{ + // If we don't have any weak pointer, then we just return maximum timeout + if (weak_timers_heap_.empty()) { + return std::chrono::nanoseconds::max(); + } + // Weak heap is not empty, so try to lock the first element. + // If it is still a valid pointer, it is guaranteed to be the correct head + TimerPtr head_timer = weak_timers_heap_.front().lock(); + + if (!head_timer) { + // The first element has expired, we can't make other assumptions on the heap + // and we need to entirely validate it. + TimersHeap locked_heap = weak_timers_heap_.validate_and_lock(); + // NOTE: the following operations will not modify any element in the heap, so we + // don't have to call `weak_timers_heap_.store(locked_heap)` at the end. + + if (locked_heap.empty()) { + return std::chrono::nanoseconds::max(); + } + head_timer = locked_heap.front(); + } + if (head_timer->is_canceled()) { + return std::nullopt; + } + return head_timer->time_until_trigger(); +} + +void TimersManager::execute_ready_timers_unsafe() +{ + // We start by locking the timers + TimersHeap locked_heap = weak_timers_heap_.validate_and_lock(); + + // Nothing to do if we don't have any timer + if (locked_heap.empty()) { + return; + } + + // Keep executing timers until they are ready and they were already ready when we started. + // The two checks prevent this function from blocking indefinitely if the + // time required for executing the timers is longer than their period. + + TimerPtr head_timer = locked_heap.front(); + const size_t number_ready_timers = locked_heap.get_number_ready_timers(); + size_t executed_timers = 0; + while (executed_timers < number_ready_timers && head_timer->is_ready()) { + auto data = head_timer->call(); + if (data) { + if (on_ready_callback_) { + on_ready_callback_(head_timer.get(), data); + } else { + head_timer->execute_callback(data); + } + } else { + // someone canceled the timer between is_ready and call + // we don't do anything, as the timer is now 'processed' + } + + executed_timers++; + // Executing a timer will result in updating its time_until_trigger, so re-heapify + locked_heap.heapify_root(); + // Get new head timer + head_timer = locked_heap.front(); + } + + // After having performed work on the locked heap we reflect the changes to weak one. + // Timers will be already sorted the next time we need them if none went out of scope. + weak_timers_heap_.store(locked_heap); +} + +void TimersManager::run_timers() +{ + // Make sure the running flag is set to false when we exit from this function + // to allow restarting the timers thread. + RCPPUTILS_SCOPE_EXIT(this->running_.store(false); ); + + while (rclcpp::ok(context_) && running_) { + // Lock mutex + std::unique_lock lock(timers_mutex_); + + std::optional time_to_sleep = get_head_timeout_unsafe(); + + // If head timer was cancelled, try to reheap and get a new head. + // This avoids an edge condition where head timer is cancelled, but other + // valid timers remain in the heap. + if (!time_to_sleep.has_value()) { + // Re-heap to (possibly) move cancelled timer from head of heap. If + // entire heap is cancelled, this will still result in a nullopt. + TimersHeap locked_heap = weak_timers_heap_.validate_and_lock(); + locked_heap.heapify(); + weak_timers_heap_.store(locked_heap); + time_to_sleep = get_head_timeout_unsafe(); + } + + // If no timers, or all timers cancelled, wait for an update. + if (!time_to_sleep.has_value() || (time_to_sleep.value() == std::chrono::nanoseconds::max()) ) { + // Wait until notification that timers have been updated + timers_cv_.wait(lock, [this]() {return timers_updated_;}); + + // Re-heap in case ordering changed due to a cancelled timer + // re-activating. + TimersHeap locked_heap = weak_timers_heap_.validate_and_lock(); + locked_heap.heapify(); + weak_timers_heap_.store(locked_heap); + } else if (time_to_sleep.value() != std::chrono::nanoseconds::zero()) { + // If time_to_sleep is zero, we immediately execute. Otherwise, wait + // until timeout or notification that timers have been updated + timers_cv_.wait_for(lock, time_to_sleep.value(), [this]() {return timers_updated_;}); + } + + // Reset timers updated flag + timers_updated_ = false; + + // Execute timers + this->execute_ready_timers_unsafe(); + } +} + +void TimersManager::clear() +{ + { + // Lock mutex and then clear all data structures + std::unique_lock lock(timers_mutex_); + + TimersHeap locked_heap = weak_timers_heap_.validate_and_lock(); + locked_heap.clear_timers_on_reset_callbacks(); + + weak_timers_heap_.clear(); + + timers_updated_ = true; + } + + // Notify timers thread such that it can re-compute its timeout + timers_cv_.notify_one(); +} + +void TimersManager::remove_timer(TimerPtr timer) +{ + bool removed = false; + { + std::unique_lock lock(timers_mutex_); + removed = weak_timers_heap_.remove_timer(timer); + + timers_updated_ = timers_updated_ || removed; + } + + if (removed) { + // Notify timers thread such that it can re-compute its timeout + timers_cv_.notify_one(); + timer->clear_on_reset_callback(); + } +} diff --git a/rclcpp/src/rclcpp/future_return_code.cpp b/rclcpp/src/rclcpp/future_return_code.cpp new file mode 100644 index 0000000000..61e167dfb9 --- /dev/null +++ b/rclcpp/src/rclcpp/future_return_code.cpp @@ -0,0 +1,50 @@ +// 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 "rclcpp/future_return_code.hpp" + +namespace rclcpp +{ + +std::ostream & +operator<<(std::ostream & os, const rclcpp::FutureReturnCode & future_return_code) +{ + return os << to_string(future_return_code); +} + +std::string +to_string(const rclcpp::FutureReturnCode & future_return_code) +{ + using enum_type = std::underlying_type::type; + std::string prefix = "Unknown enum value ("; + std::string ret_as_string = std::to_string(static_cast(future_return_code)); + switch (future_return_code) { + case FutureReturnCode::SUCCESS: + prefix = "SUCCESS ("; + break; + case FutureReturnCode::INTERRUPTED: + prefix = "INTERRUPTED ("; + break; + case FutureReturnCode::TIMEOUT: + prefix = "TIMEOUT ("; + break; + } + return prefix + ret_as_string + ")"; +} + +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/generic_client.cpp b/rclcpp/src/rclcpp/generic_client.cpp new file mode 100644 index 0000000000..0ac9a86e15 --- /dev/null +++ b/rclcpp/src/rclcpp/generic_client.cpp @@ -0,0 +1,192 @@ +// Copyright 2023 Sony Group Corporation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT 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 "rclcpp/generic_client.hpp" +#include "rclcpp/typesupport_helpers.hpp" + +#include "rosidl_runtime_c/service_type_support_struct.h" +#include "rosidl_typesupport_introspection_cpp/identifier.hpp" +#include "rosidl_typesupport_introspection_cpp/service_introspection.hpp" + +namespace rclcpp +{ +GenericClient::GenericClient( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, + const std::string & service_name, + const std::string & service_type, + rcl_client_options_t & client_options) +: ClientBase(node_base, node_graph) +{ + const rosidl_service_type_support_t * service_ts; + try { + ts_lib_ = get_typesupport_library( + service_type, "rosidl_typesupport_cpp"); + + service_ts = get_service_typesupport_handle( + service_type, "rosidl_typesupport_cpp", *ts_lib_); + + auto response_type_support_intro = get_message_typesupport_handle( + service_ts->response_typesupport, + rosidl_typesupport_introspection_cpp::typesupport_identifier); + response_members_ = static_cast( + response_type_support_intro->data); + } catch (std::runtime_error & err) { + RCLCPP_ERROR( + rclcpp::get_node_logger(node_handle_.get()).get_child("rclcpp"), + "Invalid service type: %s", + err.what()); + throw rclcpp::exceptions::InvalidServiceTypeError(err.what()); + } + + rcl_ret_t ret = rcl_client_init( + this->get_client_handle().get(), + this->get_rcl_node_handle(), + service_ts, + service_name.c_str(), + &client_options); + if (ret != RCL_RET_OK) { + if (ret == RCL_RET_SERVICE_NAME_INVALID) { + auto rcl_node_handle = this->get_rcl_node_handle(); + // this will throw on any validation problem + rcl_reset_error(); + expand_topic_or_service_name( + service_name, + rcl_node_get_name(rcl_node_handle), + rcl_node_get_namespace(rcl_node_handle), + true); + } + rclcpp::exceptions::throw_from_rcl_error(ret, "could not create generic client"); + } +} + +std::shared_ptr +GenericClient::create_response() +{ + void * response = new uint8_t[response_members_->size_of_]; + response_members_->init_function(response, rosidl_runtime_cpp::MessageInitialization::ZERO); + return std::shared_ptr( + response, + [this](void * p) + { + response_members_->fini_function(p); + delete[] reinterpret_cast(p); + }); +} + +std::shared_ptr +GenericClient::create_request_header() +{ + // TODO(wjwwood): This should probably use rmw_request_id's allocator. + // (since it is a C type) + return std::shared_ptr(new rmw_request_id_t); +} + +void +GenericClient::handle_response( + std::shared_ptr request_header, + std::shared_ptr response) +{ + auto optional_pending_request = + this->get_and_erase_pending_request(request_header->sequence_number); + if (!optional_pending_request) { + return; + } + auto & value = *optional_pending_request; + if (std::holds_alternative(value)) { + auto & promise = std::get(value); + promise.set_value(std::move(response)); + } else if (std::holds_alternative(value)) { + auto & inner = std::get(value); + const auto & callback = std::get(inner); + auto & promise = std::get(inner); + auto & future = std::get(inner); + promise.set_value(std::move(response)); + callback(std::move(future)); + } +} + +size_t +GenericClient::prune_pending_requests() +{ + std::lock_guard guard(pending_requests_mutex_); + auto ret = pending_requests_.size(); + pending_requests_.clear(); + return ret; +} + +bool +GenericClient::remove_pending_request(int64_t request_id) +{ + std::lock_guard guard(pending_requests_mutex_); + return pending_requests_.erase(request_id) != 0u; +} + +bool +GenericClient::remove_pending_request(const FutureAndRequestId & future) +{ + return this->remove_pending_request(future.request_id); +} + +bool +GenericClient::remove_pending_request(const SharedFutureAndRequestId & future) +{ + return this->remove_pending_request(future.request_id); +} + +std::optional +GenericClient::get_and_erase_pending_request(int64_t request_number) +{ + std::unique_lock lock(pending_requests_mutex_); + auto it = pending_requests_.find(request_number); + if (it == pending_requests_.end()) { + RCUTILS_LOG_DEBUG_NAMED( + "rclcpp", + "Received invalid sequence number. Ignoring..."); + return std::nullopt; + } + auto value = std::move(it->second.second); + pending_requests_.erase(request_number); + return value; +} + +GenericClient::FutureAndRequestId +GenericClient::async_send_request(const Request request) +{ + Promise promise; + auto future = promise.get_future(); + auto req_id = async_send_request_impl( + request, + std::move(promise)); + return FutureAndRequestId(std::move(future), req_id); +} + +int64_t +GenericClient::async_send_request_impl(const Request request, CallbackInfoVariant value) +{ + int64_t sequence_number; + std::lock_guard lock(pending_requests_mutex_); + rcl_ret_t ret = rcl_send_request(get_client_handle().get(), request, &sequence_number); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send request"); + } + pending_requests_.try_emplace( + sequence_number, + std::make_pair(std::chrono::system_clock::now(), std::move(value))); + return sequence_number; +} + +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/generic_publisher.cpp b/rclcpp/src/rclcpp/generic_publisher.cpp new file mode 100644 index 0000000000..d526a6fac1 --- /dev/null +++ b/rclcpp/src/rclcpp/generic_publisher.cpp @@ -0,0 +1,84 @@ +// Copyright 2018, Bosch Software Innovations GmbH. +// Copyright 2021, Apex.AI 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 "rclcpp/generic_publisher.hpp" + +#include +#include + +namespace rclcpp +{ + +void GenericPublisher::publish(const rclcpp::SerializedMessage & message) +{ + TRACETOOLS_TRACEPOINT( + rclcpp_publish, + nullptr, + static_cast(&message.get_rcl_serialized_message())); + auto return_code = rcl_publish_serialized_message( + get_publisher_handle().get(), &message.get_rcl_serialized_message(), NULL); + + if (return_code != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(return_code, "failed to publish serialized message"); + } +} + +void GenericPublisher::publish_as_loaned_msg(const rclcpp::SerializedMessage & message) +{ + auto loaned_message = borrow_loaned_message(); + deserialize_message(message.get_rcl_serialized_message(), loaned_message); + publish_loaned_message(loaned_message); +} + +void * GenericPublisher::borrow_loaned_message() +{ + void * loaned_message = nullptr; + auto return_code = rcl_borrow_loaned_message( + get_publisher_handle().get(), &type_support_, &loaned_message); + + if (return_code != RMW_RET_OK) { + if (return_code == RCL_RET_UNSUPPORTED) { + rclcpp::exceptions::throw_from_rcl_error( + return_code, + "current middleware cannot support loan messages"); + } else { + rclcpp::exceptions::throw_from_rcl_error(return_code, "failed to borrow loaned msg"); + } + } + return loaned_message; +} + +void GenericPublisher::deserialize_message( + const rmw_serialized_message_t & serialized_message, + void * deserialized_msg) +{ + auto return_code = rmw_deserialize(&serialized_message, &type_support_, deserialized_msg); + if (return_code != RMW_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(return_code, "failed to deserialize msg"); + } +} + +void GenericPublisher::publish_loaned_message(void * loaned_message) +{ + TRACETOOLS_TRACEPOINT(rclcpp_publish, nullptr, static_cast(loaned_message)); + auto return_code = rcl_publish_loaned_message( + get_publisher_handle().get(), loaned_message, NULL); + + if (return_code != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(return_code, "failed to publish loaned message"); + } +} + +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/generic_service.cpp b/rclcpp/src/rclcpp/generic_service.cpp new file mode 100644 index 0000000000..75b34993bf --- /dev/null +++ b/rclcpp/src/rclcpp/generic_service.cpp @@ -0,0 +1,172 @@ +// Copyright 2024 Sony Group Corporation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT 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 "rclcpp/generic_service.hpp" + +namespace rclcpp +{ +GenericService::GenericService( + std::shared_ptr node_handle, + const std::string & service_name, + const std::string & service_type, + GenericServiceCallback any_callback, + rcl_service_options_t & service_options) +: ServiceBase(node_handle), + any_callback_(any_callback) +{ + const rosidl_service_type_support_t * service_ts; + try { + ts_lib_ = get_typesupport_library( + service_type, "rosidl_typesupport_cpp"); + + service_ts = get_service_typesupport_handle( + service_type, "rosidl_typesupport_cpp", *ts_lib_); + + auto request_type_support_intro = get_message_typesupport_handle( + service_ts->request_typesupport, + rosidl_typesupport_introspection_cpp::typesupport_identifier); + request_members_ = static_cast( + request_type_support_intro->data); + + auto response_type_support_intro = get_message_typesupport_handle( + service_ts->response_typesupport, + rosidl_typesupport_introspection_cpp::typesupport_identifier); + response_members_ = static_cast( + response_type_support_intro->data); + } catch (std::runtime_error & err) { + RCLCPP_ERROR( + rclcpp::get_node_logger(node_handle_.get()).get_child("rclcpp"), + "Invalid service type: %s", + err.what()); + throw rclcpp::exceptions::InvalidServiceTypeError(err.what()); + } + + // rcl does the static memory allocation here + service_handle_ = std::shared_ptr( + new rcl_service_t, [handle = node_handle_, service_name](rcl_service_t * service) + { + if (rcl_service_fini(service, handle.get()) != RCL_RET_OK) { + RCLCPP_ERROR( + rclcpp::get_node_logger(handle.get()).get_child("rclcpp"), + "Error in destruction of rcl service handle: %s", + rcl_get_error_string().str); + rcl_reset_error(); + } + delete service; + }); + *service_handle_.get() = rcl_get_zero_initialized_service(); + + rcl_ret_t ret = rcl_service_init( + service_handle_.get(), + node_handle.get(), + service_ts, + service_name.c_str(), + &service_options); + if (ret != RCL_RET_OK) { + if (ret == RCL_RET_SERVICE_NAME_INVALID) { + auto rcl_node_handle = get_rcl_node_handle(); + // this will throw on any validation problem + rcl_reset_error(); + expand_topic_or_service_name( + service_name, + rcl_node_get_name(rcl_node_handle), + rcl_node_get_namespace(rcl_node_handle), + true); + } + + rclcpp::exceptions::throw_from_rcl_error(ret, "could not create service"); + } + TRACETOOLS_TRACEPOINT( + rclcpp_service_callback_added, + static_cast(get_service_handle().get()), + static_cast(&any_callback_)); +#ifndef TRACETOOLS_DISABLED + any_callback_.register_callback_for_tracing(); +#endif +} + +bool +GenericService::take_request( + SharedRequest request_out, + rmw_request_id_t & request_id_out) +{ + request_out = create_request(); + return this->take_type_erased_request(request_out.get(), request_id_out); +} + +std::shared_ptr +GenericService::create_request() +{ + Request request = new uint8_t[request_members_->size_of_]; + request_members_->init_function(request, rosidl_runtime_cpp::MessageInitialization::ZERO); + return std::shared_ptr( + request, + [this](void * p) + { + request_members_->fini_function(p); + delete[] reinterpret_cast(p); + }); +} + +std::shared_ptr +GenericService::create_response() +{ + Response response = new uint8_t[response_members_->size_of_]; + response_members_->init_function(response, rosidl_runtime_cpp::MessageInitialization::ZERO); + return std::shared_ptr( + response, + [this](void * p) + { + response_members_->fini_function(p); + delete[] reinterpret_cast(p); + }); +} + +std::shared_ptr +GenericService::create_request_header() +{ + return std::make_shared(); +} + +void +GenericService::handle_request( + std::shared_ptr request_header, + std::shared_ptr request) +{ + auto response = any_callback_.dispatch( + this->shared_from_this(), request_header, request, create_response()); + if (response) { + send_response(*request_header, response); + } +} + +void +GenericService::send_response(rmw_request_id_t & req_id, SharedResponse & response) +{ + rcl_ret_t ret = rcl_send_response(get_service_handle().get(), &req_id, response.get()); + + if (ret == RCL_RET_TIMEOUT) { + RCLCPP_WARN( + node_logger_.get_child("rclcpp"), + "failed to send response to %s (timeout): %s", + this->get_service_name(), rcl_get_error_string().str); + rcl_reset_error(); + return; + } + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send response"); + } +} + +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/generic_subscription.cpp b/rclcpp/src/rclcpp/generic_subscription.cpp new file mode 100644 index 0000000000..ae28354b98 --- /dev/null +++ b/rclcpp/src/rclcpp/generic_subscription.cpp @@ -0,0 +1,132 @@ +// Copyright 2018, Bosch Software Innovations GmbH. +// Copyright 2021, Apex.AI 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 "rclcpp/generic_subscription.hpp" + +#include +#include + +#include "rcl/subscription.h" + +#include "rclcpp/exceptions.hpp" + +namespace rclcpp +{ + +std::shared_ptr +GenericSubscription::create_message() +{ + return create_serialized_message(); +} + +std::shared_ptr +GenericSubscription::create_serialized_message() +{ + return std::make_shared(0); +} + +void +GenericSubscription::handle_message( + std::shared_ptr &, + const rclcpp::MessageInfo &) +{ + throw rclcpp::exceptions::UnimplementedError( + "handle_message is not implemented for GenericSubscription"); +} + +void +GenericSubscription::handle_serialized_message( + const std::shared_ptr & message, + const rclcpp::MessageInfo & message_info) +{ + any_callback_.dispatch(message, message_info); +} + +void +GenericSubscription::handle_loaned_message( + void * message, const rclcpp::MessageInfo & message_info) +{ + (void) message; + (void) message_info; + throw rclcpp::exceptions::UnimplementedError( + "handle_loaned_message is not implemented for GenericSubscription"); +} + +void +GenericSubscription::return_message(std::shared_ptr & message) +{ + auto typed_message = std::static_pointer_cast(message); + return_serialized_message(typed_message); +} + +void +GenericSubscription::return_serialized_message( + std::shared_ptr & message) +{ + message.reset(); +} + + +// DYNAMIC TYPE ==================================================================================== +// TODO(methylDragon): Reorder later +rclcpp::dynamic_typesupport::DynamicMessageType::SharedPtr +GenericSubscription::get_shared_dynamic_message_type() +{ + throw rclcpp::exceptions::UnimplementedError( + "get_shared_dynamic_message_type is not implemented for GenericSubscription"); +} + +rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr +GenericSubscription::get_shared_dynamic_message() +{ + throw rclcpp::exceptions::UnimplementedError( + "get_shared_dynamic_message is not implemented for GenericSubscription"); +} + +rclcpp::dynamic_typesupport::DynamicSerializationSupport::SharedPtr +GenericSubscription::get_shared_dynamic_serialization_support() +{ + throw rclcpp::exceptions::UnimplementedError( + "get_shared_dynamic_serialization_support is not implemented for GenericSubscription"); +} + +rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr +GenericSubscription::create_dynamic_message() +{ + throw rclcpp::exceptions::UnimplementedError( + "create_dynamic_message is not implemented for GenericSubscription"); +} + +void +GenericSubscription::return_dynamic_message( + rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message) +{ + (void) message; + throw rclcpp::exceptions::UnimplementedError( + "return_dynamic_message is not implemented for GenericSubscription"); +} + +void +GenericSubscription::handle_dynamic_message( + const rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message, + const rclcpp::MessageInfo & message_info) +{ + (void) message; + (void) message_info; + throw rclcpp::exceptions::UnimplementedError( + "handle_dynamic_message is not implemented for GenericSubscription"); +} + +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/graph_listener.cpp b/rclcpp/src/rclcpp/graph_listener.cpp index 26c9c58ef0..d7ea42117a 100644 --- a/rclcpp/src/rclcpp/graph_listener.cpp +++ b/rclcpp/src/rclcpp/graph_listener.cpp @@ -22,10 +22,14 @@ #include "rcl/error_handling.h" #include "rcl/types.h" +#include "rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp" #include "rclcpp/exceptions.hpp" +#include "rclcpp/logging.hpp" #include "rclcpp/node.hpp" #include "rmw/impl/cpp/demangle.hpp" +#include "rcutils/logging_macros.h" + using rclcpp::exceptions::throw_from_rcl_error; namespace rclcpp @@ -33,22 +37,35 @@ namespace rclcpp namespace graph_listener { -GraphListener::GraphListener() -: is_started_(false), is_shutdown_(false), shutdown_guard_condition_(nullptr) +GraphListener::GraphListener(const std::shared_ptr & parent_context) +: weak_parent_context_(parent_context), + rcl_parent_context_(parent_context->get_rcl_context()), + is_started_(false), + is_shutdown_(false), + interrupt_guard_condition_(parent_context) { - rcl_ret_t ret = rcl_guard_condition_init( - &interrupt_guard_condition_, - rcl_guard_condition_get_default_options()); - if (RCL_RET_OK != ret) { - throw_from_rcl_error(ret, "failed to create interrupt guard condition"); - } - - shutdown_guard_condition_ = rclcpp::get_sigint_guard_condition(&wait_set_); } GraphListener::~GraphListener() { - this->shutdown(); + GraphListener::shutdown(std::nothrow); +} + +void GraphListener::init_wait_set() +{ + rcl_ret_t ret = rcl_wait_set_init( + &wait_set_, + 0, // number_of_subscriptions + 2, // number_of_guard_conditions + 0, // number_of_timers + 0, // number_of_clients + 0, // number_of_services + 0, // number_of_events + rcl_parent_context_.get(), + rcl_get_default_allocator()); + if (RCL_RET_OK != ret) { + throw_from_rcl_error(ret, "failed to initialize wait set"); + } } void @@ -58,30 +75,22 @@ GraphListener::start_if_not_started() if (is_shutdown_.load()) { throw GraphListenerShutdownError(); } - if (!is_started_) { - // Initialize the wait set before starting. - rcl_ret_t ret = rcl_wait_set_init( - &wait_set_, - 0, // number_of_subscriptions - 2, // number_of_guard_conditions - 0, // number_of_timers - 0, // number_of_clients - 0, // number_of_services - rcl_get_default_allocator()); - if (RCL_RET_OK != ret) { - throw_from_rcl_error(ret, "failed to initialize wait set"); - } + auto parent_context = weak_parent_context_.lock(); + if (!is_started_ && parent_context) { // Register an on_shutdown hook to shtudown the graph listener. // This is important to ensure that the wait set is finalized before // destruction of static objects occurs. std::weak_ptr weak_this = shared_from_this(); - rclcpp::on_shutdown( + parent_context->on_shutdown( [weak_this]() { auto shared_this = weak_this.lock(); if (shared_this) { - shared_this->shutdown(); + // should not throw from on_shutdown if it can be avoided + shared_this->shutdown(std::nothrow); } }); + // Initialize the wait set before starting. + init_wait_set(); // Start the listener thread. listener_thread_ = std::thread(&GraphListener::run, this); is_started_ = true; @@ -94,13 +103,16 @@ GraphListener::run() try { run_loop(); } catch (const std::exception & exc) { - fprintf(stderr, - "[rclcpp] caught %s exception in GraphListener thread: %s\n", + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "caught %s exception in GraphListener thread: %s", rmw::impl::cpp::demangle(exc).c_str(), exc.what()); std::rethrow_exception(std::current_exception()); } catch (...) { - fprintf(stderr, "[rclcpp] unknown error in GraphListener thread\n"); + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "unknown error in GraphListener thread"); std::rethrow_exception(std::current_exception()); } } @@ -123,31 +135,27 @@ GraphListener::run_loop() } // This lock is released when the loop continues or exits. std::lock_guard nodes_lock(node_graph_interfaces_mutex_, std::adopt_lock); - // Resize the wait set if necessary. - if (wait_set_.size_of_guard_conditions < (node_graph_interfaces_.size() + 2)) { - ret = rcl_wait_set_resize_guard_conditions(&wait_set_, node_graph_interfaces_.size() + 2); + const size_t node_graph_interfaces_size = node_graph_interfaces_.size(); + // Add 2 for the interrupt and shutdown guard conditions + if (wait_set_.size_of_guard_conditions < (node_graph_interfaces_size + 2)) { + ret = rcl_wait_set_resize(&wait_set_, 0, node_graph_interfaces_size + 2, 0, 0, 0, 0); if (RCL_RET_OK != ret) { throw_from_rcl_error(ret, "failed to resize wait set"); } } - // Clear the wait set's guard conditions. - ret = rcl_wait_set_clear_guard_conditions(&wait_set_); + // Clear the wait set. + ret = rcl_wait_set_clear(&wait_set_); if (RCL_RET_OK != ret) { throw_from_rcl_error(ret, "failed to clear wait set"); } // Put the interrupt guard condition in the wait set. - ret = rcl_wait_set_add_guard_condition(&wait_set_, &interrupt_guard_condition_); - if (RCL_RET_OK != ret) { - throw_from_rcl_error(ret, "failed to add interrupt guard condition to wait set"); - } - // Put the shutdown guard condition in the wait set. - ret = rcl_wait_set_add_guard_condition(&wait_set_, shutdown_guard_condition_); - if (RCL_RET_OK != ret) { - throw_from_rcl_error(ret, "failed to add shutdown guard condition to wait set"); - } + detail::add_guard_condition_to_rcl_wait_set(wait_set_, interrupt_guard_condition_); + // Put graph guard conditions for each node into the wait set. - for (const auto node_ptr : node_graph_interfaces_) { + std::vector graph_gc_indexes(node_graph_interfaces_size, 0u); + for (size_t i = 0u; i < node_graph_interfaces_size; ++i) { + auto node_ptr = node_graph_interfaces_[i]; // Only wait on graph changes if some user of the node is watching. if (node_ptr->count_graph_users() == 0) { continue; @@ -157,7 +165,7 @@ GraphListener::run_loop() if (!graph_gc) { throw_from_rcl_error(RCL_RET_ERROR, "failed to get graph guard condition"); } - ret = rcl_wait_set_add_guard_condition(&wait_set_, graph_gc); + ret = rcl_wait_set_add_guard_condition(&wait_set_, graph_gc, &graph_gc_indexes[i]); if (RCL_RET_OK != ret) { throw_from_rcl_error(ret, "failed to add graph guard condition to wait set"); } @@ -172,25 +180,17 @@ GraphListener::run_loop() throw_from_rcl_error(ret, "failed to wait on wait set"); } - bool shutdown_guard_condition_triggered = false; - // Check to see if the shutdown guard condition has been triggered. - for (size_t i = 0; i < wait_set_.size_of_guard_conditions; ++i) { - if (shutdown_guard_condition_ == wait_set_.guard_conditions[i]) { - shutdown_guard_condition_triggered = true; - } - } // Notify nodes who's guard conditions are set (triggered). - for (const auto node_ptr : node_graph_interfaces_) { + for (size_t i = 0u; i < node_graph_interfaces_size; ++i) { + const auto node_ptr = node_graph_interfaces_[i]; auto graph_gc = node_ptr->get_graph_guard_condition(); if (!graph_gc) { throw_from_rcl_error(RCL_RET_ERROR, "failed to get graph guard condition"); } - for (size_t i = 0; i < wait_set_.size_of_guard_conditions; ++i) { - if (graph_gc == wait_set_.guard_conditions[i]) { - node_ptr->notify_graph_change(); - } + if (graph_gc == wait_set_.guard_conditions[graph_gc_indexes[i]]) { + node_ptr->notify_graph_change(); } - if (shutdown_guard_condition_triggered) { + if (is_shutdown_) { // If shutdown, then notify the node of this as well. node_ptr->notify_shutdown(); } @@ -199,19 +199,16 @@ GraphListener::run_loop() } static void -interrupt_(rcl_guard_condition_t * interrupt_guard_condition) +interrupt_(GuardCondition * interrupt_guard_condition) { - rcl_ret_t ret = rcl_trigger_guard_condition(interrupt_guard_condition); - if (RCL_RET_OK != ret) { - throw_from_rcl_error(ret, "failed to trigger the interrupt guard condition"); - } + interrupt_guard_condition->trigger(); } static void acquire_nodes_lock_( std::mutex * node_graph_interfaces_barrier_mutex, std::mutex * node_graph_interfaces_mutex, - rcl_guard_condition_t * interrupt_guard_condition) + GuardCondition * interrupt_guard_condition) { { // Acquire this lock to prevent the run loop from re-locking the @@ -322,7 +319,16 @@ GraphListener::remove_node(rclcpp::node_interfaces::NodeGraphInterface * node_gr } void -GraphListener::shutdown() +GraphListener::cleanup_wait_set() +{ + rcl_ret_t ret = rcl_wait_set_fini(&wait_set_); + if (RCL_RET_OK != ret) { + throw_from_rcl_error(ret, "failed to finalize wait set"); + } +} + +void +GraphListener::__shutdown() { std::lock_guard shutdown_lock(shutdown_mutex_); if (!is_shutdown_.exchange(true)) { @@ -330,23 +336,35 @@ GraphListener::shutdown() interrupt_(&interrupt_guard_condition_); listener_thread_.join(); } - rcl_ret_t ret = rcl_guard_condition_fini(&interrupt_guard_condition_); - if (RCL_RET_OK != ret) { - throw_from_rcl_error(ret, "failed to finalize interrupt guard condition"); - } - if (shutdown_guard_condition_) { - rclcpp::release_sigint_guard_condition(&wait_set_); - shutdown_guard_condition_ = nullptr; - } if (is_started_) { - ret = rcl_wait_set_fini(&wait_set_); - if (RCL_RET_OK != ret) { - throw_from_rcl_error(ret, "failed to finalize wait set"); - } + cleanup_wait_set(); } } } +void +GraphListener::shutdown() +{ + this->__shutdown(); +} + +void +GraphListener::shutdown(const std::nothrow_t &) noexcept +{ + try { + this->__shutdown(); + } catch (const std::exception & exc) { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), + "caught %s exception when shutting down GraphListener: %s", + rmw::impl::cpp::demangle(exc).c_str(), exc.what()); + } catch (...) { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), + "caught unknown exception when shutting down GraphListener"); + } +} + bool GraphListener::is_shutdown() { diff --git a/rclcpp/src/rclcpp/guard_condition.cpp b/rclcpp/src/rclcpp/guard_condition.cpp new file mode 100644 index 0000000000..700985f620 --- /dev/null +++ b/rclcpp/src/rclcpp/guard_condition.cpp @@ -0,0 +1,131 @@ +// 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 "rclcpp/guard_condition.hpp" + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/logging.hpp" + +namespace rclcpp +{ + +GuardCondition::GuardCondition( + const rclcpp::Context::SharedPtr & context, + rcl_guard_condition_options_t guard_condition_options) +: rcl_guard_condition_{rcl_get_zero_initialized_guard_condition()} +{ + if (!context) { + throw std::invalid_argument("context argument unexpectedly nullptr"); + } + + rcl_ret_t ret = rcl_guard_condition_init( + &this->rcl_guard_condition_, + context->get_rcl_context().get(), + guard_condition_options); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to create guard condition"); + } +} + +GuardCondition::~GuardCondition() +{ + rcl_ret_t ret = rcl_guard_condition_fini(&this->rcl_guard_condition_); + if (RCL_RET_OK != ret) { + try { + rclcpp::exceptions::throw_from_rcl_error(ret); + } catch (const std::exception & exception) { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), + "failed to finalize guard condition: %s", exception.what()); + } + } +} + +rcl_guard_condition_t & +GuardCondition::get_rcl_guard_condition() +{ + return rcl_guard_condition_; +} + +const rcl_guard_condition_t & +GuardCondition::get_rcl_guard_condition() const +{ + return rcl_guard_condition_; +} + +void +GuardCondition::trigger() +{ + rcl_ret_t ret = rcl_trigger_guard_condition(&rcl_guard_condition_); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + + { + std::lock_guard lock(reentrant_mutex_); + + if (on_trigger_callback_) { + on_trigger_callback_(1); + } else { + unread_count_++; + } + } +} + +bool +GuardCondition::exchange_in_use_by_wait_set_state(bool in_use_state) +{ + return in_use_by_wait_set_.exchange(in_use_state); +} + +void +GuardCondition::add_to_wait_set(rcl_wait_set_t & wait_set) +{ + std::lock_guard lock(reentrant_mutex_); + + if (exchange_in_use_by_wait_set_state(true)) { + if (&wait_set != wait_set_) { + throw std::runtime_error("guard condition has already been added to a wait set."); + } + } else { + wait_set_ = &wait_set; + } + + rcl_ret_t ret = rcl_wait_set_add_guard_condition(&wait_set, &this->rcl_guard_condition_, NULL); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "failed to add guard condition to wait set"); + } +} + +void +GuardCondition::set_on_trigger_callback(std::function callback) +{ + std::lock_guard lock(reentrant_mutex_); + + if (callback) { + on_trigger_callback_ = callback; + + if (unread_count_) { + callback(unread_count_); + unread_count_ = 0; + } + } else { + on_trigger_callback_ = nullptr; + } +} + +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/init_options.cpp b/rclcpp/src/rclcpp/init_options.cpp new file mode 100644 index 0000000000..5b8452c09a --- /dev/null +++ b/rclcpp/src/rclcpp/init_options.cpp @@ -0,0 +1,146 @@ +// 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 "rclcpp/init_options.hpp" + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/logging.hpp" + +namespace rclcpp +{ + +InitOptions::InitOptions(rcl_allocator_t allocator) +: init_options_(new rcl_init_options_t) +{ + *init_options_ = rcl_get_zero_initialized_init_options(); + rcl_ret_t ret = rcl_init_options_init(init_options_.get(), allocator); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to initialize rcl init options"); + } +} + +InitOptions::InitOptions(const rcl_init_options_t & init_options) +: init_options_(new rcl_init_options_t) +{ + *init_options_ = rcl_get_zero_initialized_init_options(); + rcl_ret_t ret = rcl_init_options_copy(&init_options, init_options_.get()); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to copy rcl init options"); + } +} + +InitOptions::InitOptions(const InitOptions & other) +: InitOptions(*other.get_rcl_init_options()) +{ + shutdown_on_signal = other.shutdown_on_signal; + initialize_logging_ = other.initialize_logging_; +} + +bool +InitOptions::auto_initialize_logging() const +{ + return initialize_logging_; +} + +InitOptions & +InitOptions::auto_initialize_logging(bool initialize_logging) +{ + initialize_logging_ = initialize_logging; + return *this; +} + +InitOptions & +InitOptions::operator=(const InitOptions & other) +{ + if (this != &other) { + std::lock_guard init_options_lock(init_options_mutex_); + this->finalize_init_options_impl(); + rcl_ret_t ret = rcl_init_options_copy(other.get_rcl_init_options(), init_options_.get()); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to copy rcl init options"); + } + this->shutdown_on_signal = other.shutdown_on_signal; + this->initialize_logging_ = other.initialize_logging_; + } + return *this; +} + +InitOptions::~InitOptions() +{ + this->finalize_init_options(); +} + +void +InitOptions::finalize_init_options() +{ + std::lock_guard init_options_lock(init_options_mutex_); + this->finalize_init_options_impl(); +} + +void +InitOptions::finalize_init_options_impl() +{ + if (init_options_) { + rcl_ret_t ret = rcl_init_options_fini(init_options_.get()); + if (RCL_RET_OK != ret) { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), + "failed to finalize rcl init options: %s", rcl_get_error_string().str); + rcl_reset_error(); + } + *init_options_ = rcl_get_zero_initialized_init_options(); + } +} + +const rcl_init_options_t * +InitOptions::get_rcl_init_options() const +{ + return init_options_.get(); +} + +void +InitOptions::use_default_domain_id() +{ + size_t domain_id = RCL_DEFAULT_DOMAIN_ID; + rcl_ret_t ret = rcl_get_default_domain_id(&domain_id); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to get default domain id"); + } + set_domain_id(domain_id); +} + +void +InitOptions::set_domain_id(size_t domain_id) +{ + std::lock_guard init_options_lock(init_options_mutex_); + rcl_ret_t ret = rcl_init_options_set_domain_id(init_options_.get(), domain_id); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to set domain id to rcl init options"); + } +} + +size_t +InitOptions::get_domain_id() const +{ + std::lock_guard init_options_lock(init_options_mutex_); + size_t domain_id; + rcl_ret_t ret = rcl_init_options_get_domain_id(init_options_.get(), &domain_id); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to get domain id from rcl init options"); + } + + return domain_id; +} + +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/intra_process_manager.cpp b/rclcpp/src/rclcpp/intra_process_manager.cpp index 723c43fd8d..831ffdf2ca 100644 --- a/rclcpp/src/rclcpp/intra_process_manager.cpp +++ b/rclcpp/src/rclcpp/intra_process_manager.cpp @@ -12,48 +12,152 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "rclcpp/intra_process_manager.hpp" +#include "rclcpp/experimental/intra_process_manager.hpp" + +#include +#include +#include namespace rclcpp { -namespace intra_process_manager +namespace experimental { static std::atomic _next_unique_id {1}; -IntraProcessManager::IntraProcessManager( - rclcpp::intra_process_manager::IntraProcessManagerImplBase::SharedPtr impl) -: impl_(impl) +IntraProcessManager::IntraProcessManager() {} IntraProcessManager::~IntraProcessManager() {} uint64_t -IntraProcessManager::add_subscription( - rclcpp::SubscriptionBase::SharedPtr subscription) +IntraProcessManager::add_publisher( + rclcpp::PublisherBase::SharedPtr publisher, + rclcpp::experimental::buffers::IntraProcessBufferBase::SharedPtr buffer) { - auto id = IntraProcessManager::get_next_unique_id(); - impl_->add_subscription(id, subscription); - return id; + std::unique_lock lock(mutex_); + + uint64_t pub_id = IntraProcessManager::get_next_unique_id(); + + publishers_[pub_id] = publisher; + if (publisher->is_durability_transient_local()) { + if (buffer) { + publisher_buffers_[pub_id] = buffer; + } else { + throw std::runtime_error( + "transient_local publisher needs to pass" + "a valid publisher buffer ptr when calling add_publisher()"); + } + } + + // Initialize the subscriptions storage for this publisher. + pub_to_subs_[pub_id] = SplittedSubscriptions(); + + // create an entry for the publisher id and populate with already existing subscriptions + for (auto & pair : subscriptions_) { + auto subscription = pair.second.lock(); + if (!subscription) { + continue; + } + if (can_communicate(publisher, subscription)) { + uint64_t sub_id = pair.first; + insert_sub_id_for_pub(sub_id, pub_id, subscription->use_take_shared_method()); + } + } + + return pub_id; } void IntraProcessManager::remove_subscription(uint64_t intra_process_subscription_id) { - impl_->remove_subscription(intra_process_subscription_id); + std::unique_lock lock(mutex_); + + subscriptions_.erase(intra_process_subscription_id); + + for (auto & pair : pub_to_subs_) { + pair.second.take_shared_subscriptions.erase( + std::remove( + pair.second.take_shared_subscriptions.begin(), + pair.second.take_shared_subscriptions.end(), + intra_process_subscription_id), + pair.second.take_shared_subscriptions.end()); + + pair.second.take_ownership_subscriptions.erase( + std::remove( + pair.second.take_ownership_subscriptions.begin(), + pair.second.take_ownership_subscriptions.end(), + intra_process_subscription_id), + pair.second.take_ownership_subscriptions.end()); + } } void IntraProcessManager::remove_publisher(uint64_t intra_process_publisher_id) { - impl_->remove_publisher(intra_process_publisher_id); + std::unique_lock lock(mutex_); + + publishers_.erase(intra_process_publisher_id); + publisher_buffers_.erase(intra_process_publisher_id); + pub_to_subs_.erase(intra_process_publisher_id); } bool IntraProcessManager::matches_any_publishers(const rmw_gid_t * id) const { - return impl_->matches_any_publishers(id); + std::shared_lock lock(mutex_); + + for (auto & publisher_pair : publishers_) { + auto publisher = publisher_pair.second.lock(); + if (!publisher) { + continue; + } + if (*publisher.get() == id) { + return true; + } + } + return false; +} + +size_t +IntraProcessManager::get_subscription_count(uint64_t intra_process_publisher_id) const +{ + std::shared_lock lock(mutex_); + + auto publisher_it = pub_to_subs_.find(intra_process_publisher_id); + if (publisher_it == pub_to_subs_.end()) { + // Publisher is either invalid or no longer exists. + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Calling get_subscription_count for invalid or no longer existing publisher id"); + return 0; + } + + auto count = + publisher_it->second.take_shared_subscriptions.size() + + publisher_it->second.take_ownership_subscriptions.size(); + + return count; +} + +SubscriptionIntraProcessBase::SharedPtr +IntraProcessManager::get_subscription_intra_process(uint64_t intra_process_subscription_id) +{ + std::shared_lock lock(mutex_); + + auto subscription_it = subscriptions_.find(intra_process_subscription_id); + if (subscription_it == subscriptions_.end()) { + return nullptr; + } else { + auto subscription = subscription_it->second.lock(); + if (subscription) { + return subscription; + } else { + subscriptions_.erase(subscription_it); + return nullptr; + } + } } uint64_t @@ -78,5 +182,83 @@ IntraProcessManager::get_next_unique_id() return next_id; } -} // namespace intra_process_manager +void +IntraProcessManager::insert_sub_id_for_pub( + uint64_t sub_id, + uint64_t pub_id, + bool use_take_shared_method) +{ + if (use_take_shared_method) { + pub_to_subs_[pub_id].take_shared_subscriptions.push_back(sub_id); + } else { + pub_to_subs_[pub_id].take_ownership_subscriptions.push_back(sub_id); + } +} + +bool +IntraProcessManager::can_communicate( + rclcpp::PublisherBase::SharedPtr pub, + rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr sub) const +{ + // publisher and subscription must be on the same topic + if (strcmp(pub->get_topic_name(), sub->get_topic_name()) != 0) { + return false; + } + + auto check_result = rclcpp::qos_check_compatible(pub->get_actual_qos(), sub->get_actual_qos()); + if (check_result.compatibility == rclcpp::QoSCompatibility::Error) { + return false; + } + + return true; +} + +size_t +IntraProcessManager::lowest_available_capacity(const uint64_t intra_process_publisher_id) const +{ + size_t capacity = std::numeric_limits::max(); + + auto publisher_it = pub_to_subs_.find(intra_process_publisher_id); + if (publisher_it == pub_to_subs_.end()) { + // Publisher is either invalid or no longer exists. + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Calling lowest_available_capacity for invalid or no longer existing publisher id"); + return 0u; + } + + if (publisher_it->second.take_shared_subscriptions.empty() && + publisher_it->second.take_ownership_subscriptions.empty()) + { + // no subscriptions available + return 0u; + } + + auto available_capacity = [this, &capacity](const uint64_t intra_process_subscription_id) + { + auto subscription_it = subscriptions_.find(intra_process_subscription_id); + if (subscription_it != subscriptions_.end()) { + auto subscription = subscription_it->second.lock(); + if (subscription) { + capacity = std::min(capacity, subscription->available_capacity()); + } + } else { + // Subscription is either invalid or no longer exists. + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Calling available_capacity for invalid or no longer existing subscription id"); + } + }; + + for (const auto sub_id : publisher_it->second.take_shared_subscriptions) { + available_capacity(sub_id); + } + + for (const auto sub_id : publisher_it->second.take_ownership_subscriptions) { + available_capacity(sub_id); + } + + return capacity; +} +} // namespace experimental } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/logger.cpp b/rclcpp/src/rclcpp/logger.cpp index 03b5e0c332..788ae31d31 100644 --- a/rclcpp/src/rclcpp/logger.cpp +++ b/rclcpp/src/rclcpp/logger.cpp @@ -12,14 +12,25 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include #include +#include +#include "rcl_logging_interface/rcl_logging_interface.h" +#include "rcl/error_handling.h" +#include "rcl/logging_rosout.h" + +#include "rclcpp/exceptions.hpp" #include "rclcpp/logger.hpp" +#include "rclcpp/logging.hpp" + +#include "./logging_mutex.hpp" namespace rclcpp { -rclcpp::Logger +Logger get_logger(const std::string & name) { #if RCLCPP_LOGGING_ENABLED @@ -30,4 +41,132 @@ get_logger(const std::string & name) #endif } +Logger +get_node_logger(const rcl_node_t * node) +{ + const char * logger_name = rcl_node_get_logger_name(node); + if (nullptr == logger_name) { + auto logger = rclcpp::get_logger("rclcpp"); + RCLCPP_ERROR( + logger, "failed to get logger name from node at address %p", + static_cast(const_cast(node))); + return logger; + } + return rclcpp::get_logger(logger_name); +} + +// TODO(ahcorde): Remove deprecated class on the next release (in Rolling after Kilted). +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif +rcpputils::fs::path +get_logging_directory() +{ + char * log_dir = NULL; + auto allocator = rcutils_get_default_allocator(); + rcl_logging_ret_t ret = rcl_logging_get_logging_directory(allocator, &log_dir); + if (RCL_LOGGING_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + std::string path{log_dir}; + allocator.deallocate(log_dir, allocator.state); + return path; +} +// remove warning suppression +#if !defined(_WIN32) +# pragma GCC diagnostic pop +#else // !defined(_WIN32) +# pragma warning(pop) +#endif + +std::filesystem::path +get_log_directory() +{ + char * log_dir = NULL; + auto allocator = rcutils_get_default_allocator(); + rcl_logging_ret_t ret = rcl_logging_get_logging_directory(allocator, &log_dir); + if (RCL_LOGGING_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + std::string path{log_dir}; + allocator.deallocate(log_dir, allocator.state); + return path; +} + +Logger +Logger::get_child(const std::string & suffix) +{ + if (!name_) { + return Logger(); + } + + rcl_ret_t rcl_ret = RCL_RET_OK; + std::shared_ptr logging_mutex; + logging_mutex = get_global_logging_mutex(); + { + std::lock_guard guard(*logging_mutex); + rcl_ret = rcl_logging_rosout_add_sublogger((*name_).c_str(), suffix.c_str()); + if (RCL_RET_NOT_FOUND == rcl_ret) { + rcl_reset_error(); + } else if (RCL_RET_OK != rcl_ret) { + exceptions::throw_from_rcl_error( + rcl_ret, "failed to call rcl_logging_rosout_add_sublogger", + rcl_get_error_state(), rcl_reset_error); + } + } + + Logger logger(*name_ + RCUTILS_LOGGING_SEPARATOR_STRING + suffix); + if (RCL_RET_OK == rcl_ret) { + logger.logger_sublogger_pairname_.reset( + new std::pair({*name_, suffix}), + [logging_mutex](std::pair * logger_sublogger_pairname_ptr) { + std::lock_guard guard(*logging_mutex); + rcl_ret_t rcl_ret = rcl_logging_rosout_remove_sublogger( + logger_sublogger_pairname_ptr->first.c_str(), + logger_sublogger_pairname_ptr->second.c_str()); + delete logger_sublogger_pairname_ptr; + if (RCL_RET_OK != rcl_ret) { + rcl_reset_error(); + } + }); + } + return logger; +} + +void +Logger::set_level(Level level) +{ + rcutils_ret_t rcutils_ret = rcutils_logging_set_logger_level( + get_name(), + static_cast(level)); + if (rcutils_ret != RCUTILS_RET_OK) { + if (rcutils_ret == RCUTILS_RET_INVALID_ARGUMENT) { + exceptions::throw_from_rcl_error( + RCL_RET_INVALID_ARGUMENT, "Invalid parameter", + rcutils_get_error_state(), rcutils_reset_error); + } + exceptions::throw_from_rcl_error( + RCL_RET_ERROR, "Couldn't set logger level", + rcutils_get_error_state(), rcutils_reset_error); + } +} + +Logger::Level +Logger::get_effective_level() const +{ + int logger_level = rcutils_logging_get_logger_effective_level(get_name()); + + if (logger_level < 0) { + exceptions::throw_from_rcl_error( + RCL_RET_ERROR, "Couldn't get logger level", + rcutils_get_error_state(), rcutils_reset_error); + } + + return static_cast(logger_level); +} + } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/logging_mutex.cpp b/rclcpp/src/rclcpp/logging_mutex.cpp new file mode 100644 index 0000000000..bbbe9bbeed --- /dev/null +++ b/rclcpp/src/rclcpp/logging_mutex.cpp @@ -0,0 +1,31 @@ +// 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/macros.h" + +#include "./logging_mutex.hpp" + +std::shared_ptr +get_global_logging_mutex() +{ + static auto mutex = std::make_shared(); + if (RCUTILS_UNLIKELY(!mutex)) { + throw std::runtime_error("rclcpp global logging mutex is a nullptr"); + } + return mutex; +} diff --git a/rclcpp/src/rclcpp/logging_mutex.hpp b/rclcpp/src/rclcpp/logging_mutex.hpp new file mode 100644 index 0000000000..0e7be39237 --- /dev/null +++ b/rclcpp/src/rclcpp/logging_mutex.hpp @@ -0,0 +1,39 @@ +// 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 RCLCPP__LOGGING_MUTEX_HPP_ +#define RCLCPP__LOGGING_MUTEX_HPP_ + +#include +#include + +#include "rclcpp/visibility_control.hpp" + +/// Global logging mutex +/** + * This mutex is locked in the following situations: + * - In initialization/destruction of contexts. + * - In initialization/destruction of nodes. + * - In the rcl logging output handler installed by rclcpp, + * i.e.: in all calls to the logger macros, including RCUTILS_* ones. + */ +// Implementation detail: +// A shared pointer to the mutex is used, so that objects that need to use +// it at destruction time can hold it alive. +// In that way, a destruction ordering problem between static objects is avoided. +RCLCPP_LOCAL +std::shared_ptr +get_global_logging_mutex(); + +#endif // RCLCPP__LOGGING_MUTEX_HPP_ diff --git a/rclcpp/src/rclcpp/memory_strategy.cpp b/rclcpp/src/rclcpp/memory_strategy.cpp index f4efb3f1b2..cb69dc0d26 100644 --- a/rclcpp/src/rclcpp/memory_strategy.cpp +++ b/rclcpp/src/rclcpp/memory_strategy.cpp @@ -13,34 +13,26 @@ // limitations under the License. #include "rclcpp/memory_strategy.hpp" +#include using rclcpp::memory_strategy::MemoryStrategy; rclcpp::SubscriptionBase::SharedPtr MemoryStrategy::get_subscription_by_handle( - const rcl_subscription_t * subscriber_handle, const WeakNodeVector & weak_nodes) + const std::shared_ptr & subscriber_handle, + const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { - for (auto & weak_node : weak_nodes) { - auto node = weak_node.lock(); - if (!node) { + for (const auto & pair : weak_groups_to_nodes) { + auto group = pair.first.lock(); + if (!group) { continue; } - for (auto & weak_group : node->get_callback_groups()) { - auto group = weak_group.lock(); - if (!group) { - continue; - } - for (auto & weak_subscription : group->get_subscription_ptrs()) { - auto subscription = weak_subscription.lock(); - if (subscription) { - if (subscription->get_subscription_handle() == subscriber_handle) { - return subscription; - } - if (subscription->get_intra_process_subscription_handle() == subscriber_handle) { - return subscription; - } - } - } + auto match_subscription = group->find_subscription_ptrs_if( + [&subscriber_handle](const rclcpp::SubscriptionBase::SharedPtr & subscription) -> bool { + return subscription->get_subscription_handle() == subscriber_handle; + }); + if (match_subscription) { + return match_subscription; } } return nullptr; @@ -48,25 +40,20 @@ MemoryStrategy::get_subscription_by_handle( rclcpp::ServiceBase::SharedPtr MemoryStrategy::get_service_by_handle( - const rcl_service_t * service_handle, - const WeakNodeVector & weak_nodes) + const std::shared_ptr & service_handle, + const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { - for (auto & weak_node : weak_nodes) { - auto node = weak_node.lock(); - if (!node) { + for (const auto & pair : weak_groups_to_nodes) { + auto group = pair.first.lock(); + if (!group) { continue; } - for (auto & weak_group : node->get_callback_groups()) { - auto group = weak_group.lock(); - if (!group) { - continue; - } - for (auto & weak_service : group->get_service_ptrs()) { - auto service = weak_service.lock(); - if (service && service->get_service_handle() == service_handle) { - return service; - } - } + auto service_ref = group->find_service_ptrs_if( + [&service_handle](const rclcpp::ServiceBase::SharedPtr & service) -> bool { + return service->get_service_handle() == service_handle; + }); + if (service_ref) { + return service_ref; } } return nullptr; @@ -74,25 +61,41 @@ MemoryStrategy::get_service_by_handle( rclcpp::ClientBase::SharedPtr MemoryStrategy::get_client_by_handle( - const rcl_client_t * client_handle, - const WeakNodeVector & weak_nodes) + const std::shared_ptr & client_handle, + const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { - for (auto & weak_node : weak_nodes) { - auto node = weak_node.lock(); - if (!node) { + for (const auto & pair : weak_groups_to_nodes) { + auto group = pair.first.lock(); + if (!group) { continue; } - for (auto & weak_group : node->get_callback_groups()) { - auto group = weak_group.lock(); - if (!group) { - continue; - } - for (auto & weak_client : group->get_client_ptrs()) { - auto client = weak_client.lock(); - if (client && client->get_client_handle() == client_handle) { - return client; - } - } + auto client_ref = group->find_client_ptrs_if( + [&client_handle](const rclcpp::ClientBase::SharedPtr & client) -> bool { + return client->get_client_handle() == client_handle; + }); + if (client_ref) { + return client_ref; + } + } + return nullptr; +} + +rclcpp::TimerBase::SharedPtr +MemoryStrategy::get_timer_by_handle( + const std::shared_ptr & timer_handle, + const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) +{ + for (const auto & pair : weak_groups_to_nodes) { + auto group = pair.first.lock(); + if (!group) { + continue; + } + auto timer_ref = group->find_timer_ptrs_if( + [&timer_handle](const rclcpp::TimerBase::SharedPtr & timer) -> bool { + return timer->get_timer_handle() == timer_handle; + }); + if (timer_ref) { + return timer_ref; } } return nullptr; @@ -100,100 +103,127 @@ MemoryStrategy::get_client_by_handle( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr MemoryStrategy::get_node_by_group( - rclcpp::callback_group::CallbackGroup::SharedPtr group, - const WeakNodeVector & weak_nodes) + const rclcpp::CallbackGroup::SharedPtr & group, + const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { if (!group) { return nullptr; } - for (auto & weak_node : weak_nodes) { - auto node = weak_node.lock(); - if (!node) { - continue; - } - for (auto & weak_group : node->get_callback_groups()) { - auto callback_group = weak_group.lock(); - if (callback_group == group) { - return node; - } - } + + rclcpp::CallbackGroup::WeakPtr weak_group_ptr(group); + const auto finder = weak_groups_to_nodes.find(weak_group_ptr); + if (finder != weak_groups_to_nodes.end()) { + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr = finder->second.lock(); + return node_ptr; } return nullptr; } -rclcpp::callback_group::CallbackGroup::SharedPtr +rclcpp::CallbackGroup::SharedPtr MemoryStrategy::get_group_by_subscription( - rclcpp::SubscriptionBase::SharedPtr subscription, - const WeakNodeVector & weak_nodes) + const rclcpp::SubscriptionBase::SharedPtr & subscription, + const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { - for (auto & weak_node : weak_nodes) { - auto node = weak_node.lock(); - if (!node) { + for (const auto & pair : weak_groups_to_nodes) { + auto group = pair.first.lock(); + auto node = pair.second.lock(); + if (!group || !node) { continue; } - for (auto & weak_group : node->get_callback_groups()) { - auto group = weak_group.lock(); - if (!group) { - continue; - } - for (auto & weak_sub : group->get_subscription_ptrs()) { - auto sub = weak_sub.lock(); - if (sub == subscription) { - return group; - } - } + auto match_sub = group->find_subscription_ptrs_if( + [&subscription](const rclcpp::SubscriptionBase::SharedPtr & sub) -> bool { + return sub == subscription; + }); + if (match_sub) { + return group; } } return nullptr; } -rclcpp::callback_group::CallbackGroup::SharedPtr +rclcpp::CallbackGroup::SharedPtr MemoryStrategy::get_group_by_service( - rclcpp::ServiceBase::SharedPtr service, - const WeakNodeVector & weak_nodes) + const rclcpp::ServiceBase::SharedPtr & service, + const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { - for (auto & weak_node : weak_nodes) { - auto node = weak_node.lock(); - if (!node) { + for (const auto & pair : weak_groups_to_nodes) { + auto group = pair.first.lock(); + auto node = pair.second.lock(); + if (!group || !node) { continue; } - for (auto & weak_group : node->get_callback_groups()) { - auto group = weak_group.lock(); - if (!group) { - continue; - } - for (auto & weak_serv : group->get_service_ptrs()) { - auto serv = weak_serv.lock(); - if (serv && serv == service) { - return group; - } - } + auto service_ref = group->find_service_ptrs_if( + [&service](const rclcpp::ServiceBase::SharedPtr & serv) -> bool { + return serv == service; + }); + if (service_ref) { + return group; } } return nullptr; } -rclcpp::callback_group::CallbackGroup::SharedPtr +rclcpp::CallbackGroup::SharedPtr MemoryStrategy::get_group_by_client( - rclcpp::ClientBase::SharedPtr client, - const WeakNodeVector & weak_nodes) + const rclcpp::ClientBase::SharedPtr & client, + const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) +{ + for (const auto & pair : weak_groups_to_nodes) { + auto group = pair.first.lock(); + auto node = pair.second.lock(); + if (!group || !node) { + continue; + } + auto client_ref = group->find_client_ptrs_if( + [&client](const rclcpp::ClientBase::SharedPtr & cli) -> bool { + return cli == client; + }); + if (client_ref) { + return group; + } + } + return nullptr; +} + +rclcpp::CallbackGroup::SharedPtr +MemoryStrategy::get_group_by_timer( + const rclcpp::TimerBase::SharedPtr & timer, + const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) +{ + for (const auto & pair : weak_groups_to_nodes) { + auto group = pair.first.lock(); + auto node = pair.second.lock(); + if (!group || !node) { + continue; + } + auto timer_ref = group->find_timer_ptrs_if( + [&timer](const rclcpp::TimerBase::SharedPtr & time) -> bool { + return time == timer; + }); + if (timer_ref) { + return group; + } + } + return nullptr; +} + +rclcpp::CallbackGroup::SharedPtr +MemoryStrategy::get_group_by_waitable( + const rclcpp::Waitable::SharedPtr & waitable, + const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { - for (auto & weak_node : weak_nodes) { - auto node = weak_node.lock(); - if (!node) { + for (const auto & pair : weak_groups_to_nodes) { + auto group = pair.first.lock(); + auto node = pair.second.lock(); + if (!group || !node) { continue; } - for (auto & weak_group : node->get_callback_groups()) { - auto group = weak_group.lock(); - if (!group) { - continue; - } - for (auto & weak_client : group->get_client_ptrs()) { - auto cli = weak_client.lock(); - if (cli && cli == client) { - return group; - } - } + auto waitable_ref = group->find_waitable_ptrs_if( + [&waitable](const rclcpp::Waitable::SharedPtr & group_waitable) -> bool { + return group_waitable == waitable; + }); + if (waitable_ref) { + return group; } } return nullptr; diff --git a/rclcpp/src/rclcpp/message_info.cpp b/rclcpp/src/rclcpp/message_info.cpp new file mode 100644 index 0000000000..0c2eab81fa --- /dev/null +++ b/rclcpp/src/rclcpp/message_info.cpp @@ -0,0 +1,39 @@ +// 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 "rclcpp/message_info.hpp" + +namespace rclcpp +{ + +MessageInfo::MessageInfo(const rmw_message_info_t & rmw_message_info) +: rmw_message_info_(rmw_message_info) +{} + +MessageInfo::~MessageInfo() +{} + +const rmw_message_info_t & +MessageInfo::get_rmw_message_info() const +{ + return rmw_message_info_; +} + +rmw_message_info_t & +MessageInfo::get_rmw_message_info() +{ + return rmw_message_info_; +} + +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/network_flow_endpoint.cpp b/rclcpp/src/rclcpp/network_flow_endpoint.cpp new file mode 100644 index 0000000000..2b85b17123 --- /dev/null +++ b/rclcpp/src/rclcpp/network_flow_endpoint.cpp @@ -0,0 +1,84 @@ +// 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 "rclcpp/network_flow_endpoint.hpp" + +namespace rclcpp +{ + +const std::string & +NetworkFlowEndpoint::transport_protocol() const +{ + return transport_protocol_; +} + +const std::string & +NetworkFlowEndpoint::internet_protocol() const +{ + return internet_protocol_; +} + +uint16_t NetworkFlowEndpoint::transport_port() const +{ + return transport_port_; +} + +uint32_t NetworkFlowEndpoint::flow_label() const +{ + return flow_label_; +} + +uint8_t NetworkFlowEndpoint::dscp() const +{ + return dscp_; +} + +const std::string & +NetworkFlowEndpoint::internet_address() const +{ + return internet_address_; +} + +bool operator==(const NetworkFlowEndpoint & left, const NetworkFlowEndpoint & right) +{ + return left.transport_protocol_ == right.transport_protocol_ && + left.internet_protocol_ == right.internet_protocol_ && + left.transport_port_ == right.transport_port_ && + left.flow_label_ == right.flow_label_ && + left.dscp_ == right.dscp_ && + left.internet_address_ == right.internet_address_; +} + +bool operator!=(const NetworkFlowEndpoint & left, const NetworkFlowEndpoint & right) +{ + return !(left == right); +} + +std::ostream & operator<<(std::ostream & os, const NetworkFlowEndpoint & network_flow_endpoint) +{ + // Stream out in JSON-like format + os << "{" << + "\"transportProtocol\": \"" << network_flow_endpoint.transport_protocol_ << "\", " << + "\"internetProtocol\": \"" << network_flow_endpoint.internet_protocol_ << "\", " << + "\"transportPort\": \"" << network_flow_endpoint.transport_port_ << "\", " << + "\"flowLabel\": \"" << std::to_string(network_flow_endpoint.flow_label_) << "\", " << + "\"dscp\": \"" << std::to_string(network_flow_endpoint.dscp_) << "\", " << + "\"internetAddress\": \"" << network_flow_endpoint.internet_address_ << "\"" << + "}"; + return os; +} + +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index 4d55972749..1a68c7f108 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #include #include #include @@ -20,6 +21,10 @@ #include #include +#include "rcl/arguments.h" + +#include "rclcpp/create_generic_client.hpp" +#include "rclcpp/detail/qos_parameters.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp/graph_listener.hpp" #include "rclcpp/node.hpp" @@ -29,50 +34,279 @@ #include "rclcpp/node_interfaces/node_logging.hpp" #include "rclcpp/node_interfaces/node_parameters.hpp" #include "rclcpp/node_interfaces/node_services.hpp" +#include "rclcpp/node_interfaces/node_time_source.hpp" #include "rclcpp/node_interfaces/node_timers.hpp" #include "rclcpp/node_interfaces/node_topics.hpp" +#include "rclcpp/node_interfaces/node_type_descriptions.hpp" +#include "rclcpp/node_interfaces/node_waitables.hpp" +#include "rclcpp/qos_overriding_options.hpp" + +#include "rmw/validate_namespace.h" + +#include "./detail/resolve_parameter_overrides.hpp" using rclcpp::Node; +using rclcpp::NodeOptions; using rclcpp::exceptions::throw_from_rcl_error; +namespace +{ + +RCLCPP_LOCAL +std::string +extend_sub_namespace(const std::string & existing_sub_namespace, const std::string & extension) +{ + // Assumption is that the existing_sub_namespace does not need checking + // because it would be checked already when it was set with this function. + + if (extension.empty()) { + throw rclcpp::exceptions::NameValidationError( + "sub_namespace", + extension.c_str(), + "sub-nodes should not extend nodes by an empty sub-namespace", + 0); + } else if (extension.front() == '/') { + // check if the new sub-namespace extension is absolute + throw rclcpp::exceptions::NameValidationError( + "sub_namespace", + extension.c_str(), + "a sub-namespace should not have a leading /", + 0); + } + + std::string new_sub_namespace; + if (existing_sub_namespace.empty()) { + new_sub_namespace = extension; + } else { + new_sub_namespace = existing_sub_namespace + "/" + extension; + } + + // remove any trailing `/` so that new extensions do not result in `//` + if (new_sub_namespace.back() == '/') { + new_sub_namespace = new_sub_namespace.substr(0, new_sub_namespace.size() - 1); + } + + return new_sub_namespace; +} + +RCLCPP_LOCAL +std::string +create_effective_namespace(const std::string & node_namespace, const std::string & sub_namespace) +{ + // Assumption is that both the node_namespace and sub_namespace are conforming + // and do not need trimming of `/` and other things, as they were validated + // in other functions already. + + // A node may not have a sub_namespace if it is no sub_node. In this case, + // just return the original namespace + if (sub_namespace.empty()) { + return node_namespace; + } else if (node_namespace.back() == '/') { + // this is the special case where node_namespace is just `/` + return node_namespace + sub_namespace; + } else { + return node_namespace + "/" + sub_namespace; + } +} + +} // namespace + +/// Internal implementation to provide hidden and API/ABI stable changes to the Node. +/** + * This class is intended to be an "escape hatch" within a stable distribution, so that certain + * smaller features and bugfixes can be backported, having a place to put new members, while + * maintaining the ABI. + * + * This is not intended to be a parking place for new features, it should be used for backports + * only, left empty and unallocated in Rolling. + */ +class Node::NodeImpl +{ +public: + NodeImpl() = default; + ~NodeImpl() = default; +}; + Node::Node( const std::string & node_name, - const std::string & namespace_, - bool use_intra_process_comms) -: Node( - node_name, - namespace_, - rclcpp::contexts::default_context::get_global_default_context(), - use_intra_process_comms) -{} + const NodeOptions & options) +: Node(node_name, "", options) +{ +} + +static +rclcpp::QoS +get_parameter_events_qos( + rclcpp::node_interfaces::NodeBaseInterface & node_base, + const rclcpp::NodeOptions & options) +{ + auto final_qos = options.parameter_event_qos(); + const rcl_arguments_t * global_args = nullptr; + auto * rcl_options = options.get_rcl_node_options(); + if (rcl_options->use_global_arguments) { + auto context_ptr = node_base.get_context()->get_rcl_context(); + global_args = &(context_ptr->global_arguments); + } + + auto parameter_overrides = rclcpp::detail::resolve_parameter_overrides( + node_base.get_fully_qualified_name(), + options.parameter_overrides(), + &rcl_options->arguments, + global_args); + + auto final_topic_name = node_base.resolve_topic_or_service_name("/parameter_events", false); + auto prefix = "qos_overrides." + final_topic_name + "."; + std::array policies = { + rclcpp::QosPolicyKind::Depth, + rclcpp::QosPolicyKind::Durability, + rclcpp::QosPolicyKind::History, + rclcpp::QosPolicyKind::Reliability, + }; + for (const auto & policy : policies) { + auto param_name = prefix + rclcpp::qos_policy_kind_to_cstr(policy); + auto it = parameter_overrides.find(param_name); + auto value = it != parameter_overrides.end() ? + it->second : + rclcpp::detail::get_default_qos_param_value(policy, options.parameter_event_qos()); + rclcpp::detail::apply_qos_override(policy, value, final_qos); + } + return final_qos; +} Node::Node( const std::string & node_name, const std::string & namespace_, - rclcpp::Context::SharedPtr context, - bool use_intra_process_comms) -: node_base_(new rclcpp::node_interfaces::NodeBase(node_name, namespace_, context)), + const NodeOptions & options) +: node_base_(new rclcpp::node_interfaces::NodeBase( + node_name, + namespace_, + options.context(), + *(options.get_rcl_node_options()), + options.use_intra_process_comms(), + options.enable_topic_statistics())), node_graph_(new rclcpp::node_interfaces::NodeGraph(node_base_.get())), - node_logging_(new rclcpp::node_interfaces::NodeLogging(node_base_.get())), + node_logging_(new rclcpp::node_interfaces::NodeLogging(node_base_)), node_timers_(new rclcpp::node_interfaces::NodeTimers(node_base_.get())), - node_topics_(new rclcpp::node_interfaces::NodeTopics(node_base_.get())), + node_topics_(new rclcpp::node_interfaces::NodeTopics(node_base_.get(), node_timers_.get())), node_services_(new rclcpp::node_interfaces::NodeServices(node_base_.get())), + node_clock_(new rclcpp::node_interfaces::NodeClock( + node_base_, + node_topics_, + node_graph_, + node_services_, + node_logging_, + options.clock_type() + )), node_parameters_(new rclcpp::node_interfaces::NodeParameters( - node_topics_.get(), - use_intra_process_comms + node_base_, + node_logging_, + node_topics_, + node_services_, + node_clock_, + options.parameter_overrides(), + options.start_parameter_services(), + options.start_parameter_event_publisher(), + // This is needed in order to apply parameter overrides to the qos profile provided in + // options. + get_parameter_events_qos(*node_base_, options), + options.parameter_event_publisher_options(), + options.allow_undeclared_parameters(), + options.automatically_declare_parameters_from_overrides() )), - node_clock_(new rclcpp::node_interfaces::NodeClock( + node_time_source_(new rclcpp::node_interfaces::NodeTimeSource( node_base_, node_topics_, node_graph_, + node_services_, + node_logging_, + node_clock_, + node_parameters_, + options.clock_qos(), + options.use_clock_thread() + )), + node_type_descriptions_(new rclcpp::node_interfaces::NodeTypeDescriptions( + node_base_, + node_logging_, + node_parameters_, node_services_ )), - use_intra_process_comms_(use_intra_process_comms) + node_waitables_(new rclcpp::node_interfaces::NodeWaitables(node_base_.get())), + node_options_(options), + sub_namespace_(""), + effective_namespace_(create_effective_namespace(this->get_namespace(), sub_namespace_)) { + // we have got what we wanted directly from the overrides, + // but declare the parameters anyway so they are visible. + rclcpp::detail::declare_qos_parameters( + rclcpp::QosOverridingOptions + { + QosPolicyKind::Depth, + QosPolicyKind::Durability, + QosPolicyKind::History, + QosPolicyKind::Reliability, + }, + node_parameters_, + node_topics_->resolve_topic_name("/parameter_events"), + options.parameter_event_qos(), + rclcpp::detail::PublisherQosParametersTraits{}); + + if (options.enable_logger_service()) { + node_logging_->create_logger_services(node_services_); + } +} + +Node::Node( + const Node & other, + const std::string & sub_namespace) +: node_base_(other.node_base_), + node_graph_(other.node_graph_), + node_logging_(other.node_logging_), + node_timers_(other.node_timers_), + node_topics_(other.node_topics_), + node_services_(other.node_services_), + node_clock_(other.node_clock_), + node_parameters_(other.node_parameters_), + node_time_source_(other.node_time_source_), + node_waitables_(other.node_waitables_), + node_options_(other.node_options_), + sub_namespace_(extend_sub_namespace(other.get_sub_namespace(), sub_namespace)), + effective_namespace_(create_effective_namespace(other.get_namespace(), sub_namespace_)), + hidden_impl_(other.hidden_impl_) +{ + // Validate new effective namespace. + int validation_result; + size_t invalid_index; + rmw_ret_t rmw_ret = + rmw_validate_namespace(effective_namespace_.c_str(), &validation_result, &invalid_index); + + if (rmw_ret != RMW_RET_OK) { + if (rmw_ret == RMW_RET_INVALID_ARGUMENT) { + throw_from_rcl_error(RCL_RET_INVALID_ARGUMENT, "failed to validate subnode namespace"); + } + throw_from_rcl_error(RCL_RET_ERROR, "failed to validate subnode namespace"); + } + + if (validation_result != RMW_NAMESPACE_VALID) { + throw rclcpp::exceptions::InvalidNamespaceError( + effective_namespace_.c_str(), + rmw_namespace_validation_result_string(validation_result), + invalid_index); + } } Node::~Node() -{} +{ + // release sub-interfaces in an order that allows them to consult with node_base during tear-down + node_waitables_.reset(); + node_time_source_.reset(); + node_parameters_.reset(); + node_clock_.reset(); + node_services_.reset(); + node_topics_.reset(); + node_timers_.reset(); + node_logging_.reset(); + node_graph_.reset(); +} const char * Node::get_name() const @@ -86,80 +320,176 @@ Node::get_namespace() const return node_base_->get_namespace(); } +const char * +Node::get_fully_qualified_name() const +{ + return node_base_->get_fully_qualified_name(); +} + rclcpp::Logger Node::get_logger() const { return node_logging_->get_logger(); } -rclcpp::callback_group::CallbackGroup::SharedPtr +rclcpp::CallbackGroup::SharedPtr Node::create_callback_group( - rclcpp::callback_group::CallbackGroupType group_type) + rclcpp::CallbackGroupType group_type, + bool automatically_add_to_executor_with_node) +{ + return node_base_->create_callback_group(group_type, automatically_add_to_executor_with_node); +} + +const rclcpp::ParameterValue & +Node::declare_parameter( + const std::string & name, + const rclcpp::ParameterValue & default_value, + const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor, + bool ignore_override) { - return node_base_->create_callback_group(group_type); + return this->node_parameters_->declare_parameter( + name, + default_value, + parameter_descriptor, + ignore_override); +} + +const rclcpp::ParameterValue & +Node::declare_parameter( + const std::string & name, + rclcpp::ParameterType type, + const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor, + bool ignore_override) +{ + return this->node_parameters_->declare_parameter( + name, + type, + parameter_descriptor, + ignore_override); +} + +void +Node::undeclare_parameter(const std::string & name) +{ + this->node_parameters_->undeclare_parameter(name); } bool -Node::group_in_node(rclcpp::callback_group::CallbackGroup::SharedPtr group) +Node::has_parameter(const std::string & name) const { - return node_base_->callback_group_in_node(group); + return this->node_parameters_->has_parameter(name); +} + +rcl_interfaces::msg::SetParametersResult +Node::set_parameter(const rclcpp::Parameter & parameter) +{ + return node_parameters_->set_parameters_atomically({parameter}); } std::vector -Node::set_parameters( - const std::vector & parameters) +Node::set_parameters(const std::vector & parameters) { return node_parameters_->set_parameters(parameters); } rcl_interfaces::msg::SetParametersResult -Node::set_parameters_atomically( - const std::vector & parameters) +Node::set_parameters_atomically(const std::vector & parameters) { return node_parameters_->set_parameters_atomically(parameters); } -std::vector -Node::get_parameters( - const std::vector & names) const -{ - return node_parameters_->get_parameters(names); -} - -rclcpp::parameter::ParameterVariant +rclcpp::Parameter Node::get_parameter(const std::string & name) const { return node_parameters_->get_parameter(name); } -bool Node::get_parameter( - const std::string & name, - rclcpp::parameter::ParameterVariant & parameter) const +bool +Node::get_parameter(const std::string & name, rclcpp::Parameter & parameter) const { return node_parameters_->get_parameter(name, parameter); } -std::vector -Node::describe_parameters( +std::vector +Node::get_parameters( const std::vector & names) const +{ + return node_parameters_->get_parameters(names); +} + +rcl_interfaces::msg::ParameterDescriptor +Node::describe_parameter(const std::string & name) const +{ + auto result = node_parameters_->describe_parameters({name}); + if (0 == result.size()) { + throw rclcpp::exceptions::ParameterNotDeclaredException(name); + } + if (result.size() > 1) { + throw std::runtime_error("number of described parameters unexpectedly more than one"); + } + return result.front(); +} + +std::vector +Node::describe_parameters(const std::vector & names) const { return node_parameters_->describe_parameters(names); } std::vector -Node::get_parameter_types( - const std::vector & names) const +Node::get_parameter_types(const std::vector & names) const { return node_parameters_->get_parameter_types(names); } rcl_interfaces::msg::ListParametersResult -Node::list_parameters( - const std::vector & prefixes, uint64_t depth) const +Node::list_parameters(const std::vector & prefixes, uint64_t depth) const { return node_parameters_->list_parameters(prefixes, depth); } +rclcpp::Node::PreSetParametersCallbackHandle::SharedPtr +Node::add_pre_set_parameters_callback(PreSetParametersCallbackType callback) +{ + return node_parameters_->add_pre_set_parameters_callback(callback); +} + +rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr +Node::add_on_set_parameters_callback(OnSetParametersCallbackType callback) +{ + return node_parameters_->add_on_set_parameters_callback(callback); +} + +rclcpp::Node::PostSetParametersCallbackHandle::SharedPtr +Node::add_post_set_parameters_callback(PostSetParametersCallbackType callback) +{ + return node_parameters_->add_post_set_parameters_callback(callback); +} + +void +Node::remove_pre_set_parameters_callback(const PreSetParametersCallbackHandle * const handler) +{ + node_parameters_->remove_pre_set_parameters_callback(handler); +} + +void +Node::remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const handler) +{ + node_parameters_->remove_on_set_parameters_callback(handler); +} + +void +Node::remove_post_set_parameters_callback(const PostSetParametersCallbackHandle * const handler) +{ + node_parameters_->remove_post_set_parameters_callback(handler); +} + +std::vector +Node::get_node_names() const +{ + return node_graph_->get_node_names(); +} + std::map> Node::get_topic_names_and_types() const { @@ -172,6 +502,15 @@ Node::get_service_names_and_types() const return node_graph_->get_service_names_and_types(); } +std::map> +Node::get_service_names_and_types_by_node( + const std::string & node_name, + const std::string & namespace_) const +{ + return node_graph_->get_service_names_and_types_by_node( + node_name, namespace_); +} + size_t Node::count_publishers(const std::string & topic_name) const { @@ -184,10 +523,35 @@ Node::count_subscribers(const std::string & topic_name) const return node_graph_->count_subscribers(topic_name); } -const std::vector & -Node::get_callback_groups() const +size_t +Node::count_clients(const std::string & service_name) const +{ + return node_graph_->count_clients(service_name); +} + +size_t +Node::count_services(const std::string & service_name) const +{ + return node_graph_->count_services(service_name); +} + +std::vector +Node::get_publishers_info_by_topic(const std::string & topic_name, bool no_mangle) const +{ + return node_graph_->get_publishers_info_by_topic(topic_name, no_mangle); +} + +std::vector +Node::get_subscriptions_info_by_topic(const std::string & topic_name, bool no_mangle) const +{ + return node_graph_->get_subscriptions_info_by_topic(topic_name, no_mangle); +} + +void +Node::for_each_callback_group( + const node_interfaces::NodeBaseInterface::CallbackGroupFunction & func) { - return node_base_->get_callback_groups(); + node_base_->for_each_callback_group(func); } rclcpp::Event::SharedPtr @@ -210,8 +574,14 @@ Node::get_clock() return node_clock_->get_clock(); } +rclcpp::Clock::ConstSharedPtr +Node::get_clock() const +{ + return node_clock_->get_clock(); +} + rclcpp::Time -Node::now() +Node::now() const { return node_clock_->get_clock()->now(); } @@ -234,6 +604,18 @@ Node::get_node_graph_interface() return node_graph_; } +rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr +Node::get_node_logging_interface() +{ + return node_logging_; +} + +rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr +Node::get_node_time_source_interface() +{ + return node_time_source_; +} + rclcpp::node_interfaces::NodeTimersInterface::SharedPtr Node::get_node_timers_interface() { @@ -246,6 +628,12 @@ Node::get_node_topics_interface() return node_topics_; } +rclcpp::node_interfaces::NodeTypeDescriptionsInterface::SharedPtr +Node::get_node_type_descriptions_interface() +{ + return node_type_descriptions_; +} + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr Node::get_node_services_interface() { @@ -257,3 +645,52 @@ Node::get_node_parameters_interface() { return node_parameters_; } + +rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr +Node::get_node_waitables_interface() +{ + return node_waitables_; +} + +const std::string & +Node::get_sub_namespace() const +{ + return this->sub_namespace_; +} + +const std::string & +Node::get_effective_namespace() const +{ + return this->effective_namespace_; +} + +Node::SharedPtr +Node::create_sub_node(const std::string & sub_namespace) +{ + // Cannot use make_shared() here as it requires the constructor to be + // public, and this constructor is intentionally protected instead. + return std::shared_ptr(new Node(*this, sub_namespace)); +} + +const NodeOptions & +Node::get_node_options() const +{ + return this->node_options_; +} + +rclcpp::GenericClient::SharedPtr +Node::create_generic_client( + const std::string & service_name, + const std::string & service_type, + const rclcpp::QoS & qos, + rclcpp::CallbackGroup::SharedPtr group) +{ + return rclcpp::create_generic_client( + node_base_, + node_graph_, + node_services_, + service_name, + service_type, + qos, + group); +} diff --git a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp index ff05e6980c..5648290654 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp @@ -19,9 +19,16 @@ #include "rclcpp/node_interfaces/node_base.hpp" +#include "rcl/arguments.h" +#include "rcl/node_type_cache.h" +#include "rcl/logging.h" +#include "rcl/logging_rosout.h" #include "rclcpp/exceptions.hpp" -#include "rmw/validate_node_name.h" +#include "rcutils/logging_macros.h" #include "rmw/validate_namespace.h" +#include "rmw/validate_node_name.h" + +#include "../logging_mutex.hpp" using rclcpp::exceptions::throw_from_rcl_error; @@ -30,70 +37,33 @@ using rclcpp::node_interfaces::NodeBase; NodeBase::NodeBase( const std::string & node_name, const std::string & namespace_, - rclcpp::Context::SharedPtr context) + rclcpp::Context::SharedPtr context, + const rcl_node_options_t & rcl_node_options, + bool use_intra_process_default, + bool enable_topic_statistics_default, + rclcpp::CallbackGroup::SharedPtr default_callback_group) : context_(context), + use_intra_process_default_(use_intra_process_default), + enable_topic_statistics_default_(enable_topic_statistics_default), node_handle_(nullptr), - default_callback_group_(nullptr), + default_callback_group_(default_callback_group), associated_with_executor_(false), + notify_guard_condition_(std::make_shared(context)), notify_guard_condition_is_valid_(false) { - // Setup the guard condition that is notified when changes occur in the graph. - rcl_guard_condition_options_t guard_condition_options = rcl_guard_condition_get_default_options(); - rcl_ret_t ret = rcl_guard_condition_init(¬ify_guard_condition_, guard_condition_options); - if (ret != RCL_RET_OK) { - throw_from_rcl_error(ret, "failed to create interrupt guard condition"); - } - - // Setup a safe exit lamda to clean up the guard condition in case of an error here. - auto finalize_notify_guard_condition = [this]() { - // Finalize the interrupt guard condition. - if (rcl_guard_condition_fini(¬ify_guard_condition_) != RCL_RET_OK) { - fprintf(stderr, - "[rclcpp::error] failed to destroy guard condition: %s\n", rcl_get_error_string_safe()); - } - }; - - // Determine the domain id based on the options and the ROS_DOMAIN_ID env variable. - size_t domain_id = 0; - char * ros_domain_id = nullptr; - const char * env_var = "ROS_DOMAIN_ID"; -#ifndef _WIN32 - ros_domain_id = getenv(env_var); -#else - size_t ros_domain_id_size; - _dupenv_s(&ros_domain_id, &ros_domain_id_size, env_var); -#endif - if (ros_domain_id) { - uint32_t number = strtoul(ros_domain_id, NULL, 0); - if (number == (std::numeric_limits::max)()) { - // Finalize the interrupt guard condition. - finalize_notify_guard_condition(); -#ifdef _WIN32 - // free the ros_domain_id before throwing, if getenv was used on Windows - free(ros_domain_id); -#endif - - throw std::runtime_error("failed to interpret ROS_DOMAIN_ID as integral number"); - } - domain_id = static_cast(number); -#ifdef _WIN32 - free(ros_domain_id); -#endif - } - // Create the rcl node and store it in a shared_ptr with a custom destructor. - rcl_node_t * rcl_node = new rcl_node_t(rcl_get_zero_initialized_node()); + std::unique_ptr rcl_node(new rcl_node_t(rcl_get_zero_initialized_node())); - rcl_node_options_t options = rcl_node_get_default_options(); - // TODO(wjwwood): pass the Allocator to the options - options.domain_id = domain_id; - ret = rcl_node_init(rcl_node, node_name.c_str(), namespace_.c_str(), &options); - if (ret != RCL_RET_OK) { - // Finalize the interrupt guard condition. - finalize_notify_guard_condition(); + std::shared_ptr logging_mutex = get_global_logging_mutex(); - delete rcl_node; + rcl_ret_t ret; + // TODO(ivanpauno): /rosout Qos should be reconfigurable. + ret = rcl_node_init( + rcl_node.get(), + node_name.c_str(), namespace_.c_str(), + context_->get_rcl_context().get(), &rcl_node_options); + if (ret != RCL_RET_OK) { if (ret == RCL_RET_NODE_INVALID_NAME) { rcl_reset_error(); // discard rcl_node_init error int validation_result; @@ -142,19 +112,45 @@ NodeBase::NodeBase( throw_from_rcl_error(ret, "failed to initialize rcl node"); } + // The initialization for the rosout publisher + if (rcl_logging_rosout_enabled() && rcl_node_options.enable_rosout) { + std::lock_guard guard(*logging_mutex); + ret = rcl_logging_rosout_init_publisher_for_node(rcl_node.get()); + if (ret != RCL_RET_OK) { + throw_from_rcl_error(ret, "failed to initialize rosout publisher"); + } + } + node_handle_.reset( - rcl_node, - [](rcl_node_t * node) -> void { + rcl_node.release(), + [logging_mutex, rcl_node_options](rcl_node_t * node) -> void { + { + std::lock_guard guard(*logging_mutex); + if (rcl_logging_rosout_enabled() && rcl_node_options.enable_rosout) { + rcl_ret_t ret = rcl_logging_rosout_fini_publisher_for_node(node); + if (ret != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Error in destruction of rosout publisher: %s", rcl_get_error_string().str); + } + } + } if (rcl_node_fini(node) != RCL_RET_OK) { - fprintf( - stderr, "Error in destruction of rcl node handle: %s\n", rcl_get_error_string_safe()); + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Error in destruction of rcl node handle: %s", rcl_get_error_string().str); } delete node; }); - // Create the default callback group. - using rclcpp::callback_group::CallbackGroupType; - default_callback_group_ = create_callback_group(CallbackGroupType::MutuallyExclusive); + // Create the default callback group, if needed. + if (nullptr == default_callback_group_) { + using rclcpp::CallbackGroupType; + // Default callback group is mutually exclusive and automatically associated with + // any executors that this node is added to. + default_callback_group_ = + NodeBase::create_callback_group(CallbackGroupType::MutuallyExclusive, true); + } // Indicate the notify_guard_condition is now valid. notify_guard_condition_is_valid_ = true; @@ -166,10 +162,6 @@ NodeBase::~NodeBase() { std::lock_guard notify_condition_lock(notify_guard_condition_mutex_); notify_guard_condition_is_valid_ = false; - if (rcl_guard_condition_fini(¬ify_guard_condition_) != RCL_RET_OK) { - fprintf(stderr, - "[rclcpp::error] failed to destroy guard condition: %s\n", rcl_get_error_string_safe()); - } } } @@ -185,6 +177,12 @@ NodeBase::get_namespace() const return rcl_node_get_namespace(node_handle_.get()); } +const char * +NodeBase::get_fully_qualified_name() const +{ + return rcl_node_get_fully_qualified_name(node_handle_.get()); +} + rclcpp::Context::SharedPtr NodeBase::get_context() { @@ -206,48 +204,67 @@ NodeBase::get_rcl_node_handle() const std::shared_ptr NodeBase::get_shared_rcl_node_handle() { - return node_handle_; + return std::shared_ptr(shared_from_this(), node_handle_.get()); } std::shared_ptr NodeBase::get_shared_rcl_node_handle() const { - return node_handle_; + return std::shared_ptr(shared_from_this(), node_handle_.get()); } -rclcpp::callback_group::CallbackGroup::SharedPtr -NodeBase::create_callback_group(rclcpp::callback_group::CallbackGroupType group_type) +rclcpp::CallbackGroup::SharedPtr +NodeBase::create_callback_group( + rclcpp::CallbackGroupType group_type, + bool automatically_add_to_executor_with_node) { - using rclcpp::callback_group::CallbackGroup; - using rclcpp::callback_group::CallbackGroupType; - auto group = CallbackGroup::SharedPtr(new CallbackGroup(group_type)); + auto group = std::make_shared( + group_type, + context_->weak_from_this(), + automatically_add_to_executor_with_node); + std::lock_guard lock(callback_groups_mutex_); callback_groups_.push_back(group); + + // This guard condition is generally used to signal to this node's executor that a callback + // group has been added that should be considered for new entities. + // If this is creating the default callback group, then the notify guard condition won't be + // ready or needed yet, as the node is not done being constructed and therefore cannot be added. + // If the callback group is not automatically associated with this node's executors, then + // triggering the guard condition is also unnecessary, it will be manually added to an exector. + if (notify_guard_condition_is_valid_ && automatically_add_to_executor_with_node) { + this->trigger_notify_guard_condition(); + } return group; } -rclcpp::callback_group::CallbackGroup::SharedPtr +rclcpp::CallbackGroup::SharedPtr NodeBase::get_default_callback_group() { return default_callback_group_; } bool -NodeBase::callback_group_in_node(rclcpp::callback_group::CallbackGroup::SharedPtr group) +NodeBase::callback_group_in_node(rclcpp::CallbackGroup::SharedPtr group) { - bool group_belongs_to_this_node = false; + std::lock_guard lock(callback_groups_mutex_); for (auto & weak_group : this->callback_groups_) { auto cur_group = weak_group.lock(); if (cur_group && (cur_group == group)) { - group_belongs_to_this_node = true; + return true; } } - return group_belongs_to_this_node; + return false; } -const std::vector & -NodeBase::get_callback_groups() const +void NodeBase::for_each_callback_group(const CallbackGroupFunction & func) { - return callback_groups_; + std::lock_guard lock(callback_groups_mutex_); + for (rclcpp::CallbackGroup::WeakPtr & weak_group : this->callback_groups_) { + rclcpp::CallbackGroup::SharedPtr group = weak_group.lock(); + if (group) { + func(group); + } + } } std::atomic_bool & @@ -256,18 +273,65 @@ NodeBase::get_associated_with_executor_atomic() return associated_with_executor_; } -rcl_guard_condition_t * +rclcpp::GuardCondition & NodeBase::get_notify_guard_condition() +{ + std::lock_guard notify_condition_lock(notify_guard_condition_mutex_); + if (!notify_guard_condition_is_valid_) { + throw std::runtime_error("failed to get notify guard condition because it is invalid"); + } + return *notify_guard_condition_; +} + +rclcpp::GuardCondition::SharedPtr +NodeBase::get_shared_notify_guard_condition() { std::lock_guard notify_condition_lock(notify_guard_condition_mutex_); if (!notify_guard_condition_is_valid_) { return nullptr; } - return ¬ify_guard_condition_; + return notify_guard_condition_; +} + +void +NodeBase::trigger_notify_guard_condition() +{ + std::lock_guard notify_condition_lock(notify_guard_condition_mutex_); + if (!notify_guard_condition_is_valid_) { + throw std::runtime_error("failed to trigger notify guard condition because it is invalid"); + } + notify_guard_condition_->trigger(); +} + +bool +NodeBase::get_use_intra_process_default() const +{ + return use_intra_process_default_; +} + +bool +NodeBase::get_enable_topic_statistics_default() const +{ + return enable_topic_statistics_default_; } -std::unique_lock -NodeBase::acquire_notify_guard_condition_lock() const +std::string +NodeBase::resolve_topic_or_service_name( + const std::string & name, bool is_service, bool only_expand) const { - return std::unique_lock(notify_guard_condition_mutex_); + char * output_cstr = NULL; + auto allocator = rcl_get_default_allocator(); + rcl_ret_t ret = rcl_node_resolve_name( + node_handle_.get(), + name.c_str(), + allocator, + is_service, + only_expand, + &output_cstr); + if (RCL_RET_OK != ret) { + throw_from_rcl_error(ret, "failed to resolve name", rcl_get_error_state()); + } + std::string output{output_cstr}; + allocator.deallocate(output_cstr, allocator.state); + return output; } diff --git a/rclcpp/src/rclcpp/node_interfaces/node_clock.cpp b/rclcpp/src/rclcpp/node_interfaces/node_clock.cpp index c4d78ca455..a37c65b71b 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_clock.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_clock.cpp @@ -23,26 +23,28 @@ NodeClock::NodeClock( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics, rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, - rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services) + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + rcl_clock_type_t clock_type) : node_base_(node_base), node_topics_(node_topics), node_graph_(node_graph), node_services_(node_services), - ros_clock_(std::make_shared(RCL_ROS_TIME)) -{ - time_source_.attachNode( - node_base_, - node_topics_, - node_graph_, - node_services_); - time_source_.attachClock(ros_clock_); -} + node_logging_(node_logging), + clock_(std::make_shared(clock_type)) +{} NodeClock::~NodeClock() {} -std::shared_ptr +rclcpp::Clock::SharedPtr NodeClock::get_clock() { - return ros_clock_; + return clock_; +} + +rclcpp::Clock::ConstSharedPtr +NodeClock::get_clock() const +{ + return clock_; } diff --git a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp index 2feb8f03f2..f6e9e1fda0 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp @@ -14,14 +14,21 @@ #include "rclcpp/node_interfaces/node_graph.hpp" +#include #include #include +#include +#include #include #include "rcl/graph.h" -#include "rclcpp/exceptions.hpp" +#include "rcl/remap.h" #include "rclcpp/event.hpp" +#include "rclcpp/exceptions.hpp" +#include "rclcpp/expand_topic_or_service_name.hpp" #include "rclcpp/graph_listener.hpp" +#include "rclcpp/node_interfaces/node_graph_interface.hpp" +#include "rcpputils/scope_exit.hpp" using rclcpp::node_interfaces::NodeGraph; using rclcpp::exceptions::throw_from_rcl_error; @@ -29,7 +36,9 @@ using rclcpp::graph_listener::GraphListener; NodeGraph::NodeGraph(rclcpp::node_interfaces::NodeBaseInterface * node_base) : node_base_(node_base), - graph_listener_(node_base->get_context()->get_sub_context()), + graph_listener_( + node_base->get_context()->get_sub_context(node_base->get_context()) + ), should_add_to_graph_listener_(true), graph_users_count_(0) {} @@ -58,13 +67,14 @@ NodeGraph::get_topic_names_and_types(bool no_demangle) const &topic_names_and_types); if (ret != RCL_RET_OK) { auto error_msg = std::string("failed to get topic names and types: ") + - rcl_get_error_string_safe(); + rcl_get_error_string().str; rcl_reset_error(); if (rcl_names_and_types_fini(&topic_names_and_types) != RCL_RET_OK) { error_msg += std::string(", failed also to cleanup topic names and types, leaking memory: ") + - rcl_get_error_string_safe(); + rcl_get_error_string().str; + rcl_reset_error(); } - throw std::runtime_error(error_msg + rcl_get_error_string_safe()); + throw std::runtime_error(error_msg); } std::map> topics_and_types; @@ -79,7 +89,7 @@ NodeGraph::get_topic_names_and_types(bool no_demangle) const if (ret != RCL_RET_OK) { // *INDENT-OFF* throw std::runtime_error( - std::string("could not destroy topic names and types: ") + rcl_get_error_string_safe()); + std::string("could not destroy topic names and types: ") + rcl_get_error_string().str); // *INDENT-ON* } @@ -98,14 +108,15 @@ NodeGraph::get_service_names_and_types() const &service_names_and_types); if (ret != RCL_RET_OK) { auto error_msg = std::string("failed to get service names and types: ") + - rcl_get_error_string_safe(); + rcl_get_error_string().str; rcl_reset_error(); if (rcl_names_and_types_fini(&service_names_and_types) != RCL_RET_OK) { error_msg += std::string(", failed also to cleanup service names and types, leaking memory: ") + - rcl_get_error_string_safe(); + rcl_get_error_string().str; + rcl_reset_error(); } - throw std::runtime_error(error_msg + rcl_get_error_string_safe()); + throw std::runtime_error(error_msg); } std::map> services_and_types; @@ -120,46 +131,326 @@ NodeGraph::get_service_names_and_types() const if (ret != RCL_RET_OK) { // *INDENT-OFF* throw std::runtime_error( - std::string("could not destroy service names and types: ") + rcl_get_error_string_safe()); + std::string("could not destroy service names and types: ") + rcl_get_error_string().str); // *INDENT-ON* } return services_and_types; } +std::map> +NodeGraph::get_service_names_and_types_by_node( + const std::string & node_name, + const std::string & namespace_) const +{ + rcl_names_and_types_t service_names_and_types = rcl_get_zero_initialized_names_and_types(); + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_ret_t ret = rcl_get_service_names_and_types_by_node( + node_base_->get_rcl_node_handle(), + &allocator, + node_name.c_str(), + namespace_.c_str(), + &service_names_and_types); + if (ret != RCL_RET_OK) { + auto error_msg = std::string("failed to get service names and types by node: ") + + rcl_get_error_string().str; + rcl_reset_error(); + if (rcl_names_and_types_fini(&service_names_and_types) != RCL_RET_OK) { + error_msg += + std::string(", failed also to cleanup service names and types, leaking memory: ") + + rcl_get_error_string().str; + rcl_reset_error(); + } + throw std::runtime_error(error_msg); + } + + std::map> services_and_types; + for (size_t i = 0; i < service_names_and_types.names.size; ++i) { + std::string service_name = service_names_and_types.names.data[i]; + for (size_t j = 0; j < service_names_and_types.types[i].size; ++j) { + services_and_types[service_name].emplace_back(service_names_and_types.types[i].data[j]); + } + } + + ret = rcl_names_and_types_fini(&service_names_and_types); + if (ret != RCL_RET_OK) { + throw_from_rcl_error(ret, "could not destroy service names and types"); + } + + return services_and_types; +} + +std::map> +NodeGraph::get_client_names_and_types_by_node( + const std::string & node_name, + const std::string & namespace_) const +{ + rcl_names_and_types_t service_names_and_types = rcl_get_zero_initialized_names_and_types(); + auto service_names_and_types_finalizer = rcpputils::make_scope_exit( + [&service_names_and_types]() { + if (rcl_names_and_types_fini(&service_names_and_types) != RCL_RET_OK) { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), "could not destroy service names and types"); + } + }); + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_ret_t ret = rcl_get_client_names_and_types_by_node( + node_base_->get_rcl_node_handle(), + &allocator, + node_name.c_str(), + namespace_.c_str(), + &service_names_and_types); + if (ret != RCL_RET_OK) { + throw_from_rcl_error(ret, "failed to get service names and types by node"); + } + + std::map> services_and_types; + for (size_t i = 0; i < service_names_and_types.names.size; ++i) { + std::string service_name = service_names_and_types.names.data[i]; + for (size_t j = 0; j < service_names_and_types.types[i].size; ++j) { + services_and_types[service_name].emplace_back(service_names_and_types.types[i].data[j]); + } + } + + return services_and_types; +} + +std::map> +NodeGraph::get_publisher_names_and_types_by_node( + const std::string & node_name, + const std::string & namespace_, + bool no_demangle) const +{ + rcl_names_and_types_t topic_names_and_types = rcl_get_zero_initialized_names_and_types(); + auto topic_names_and_types_finalizer = rcpputils::make_scope_exit( + [&topic_names_and_types]() { + if (rcl_names_and_types_fini(&topic_names_and_types) != RCL_RET_OK) { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), "could not destroy topic names and types"); + } + }); + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_ret_t ret = rcl_get_publisher_names_and_types_by_node( + node_base_->get_rcl_node_handle(), + &allocator, + no_demangle, + node_name.c_str(), + namespace_.c_str(), + &topic_names_and_types); + if (ret != RCL_RET_OK) { + throw_from_rcl_error(ret, "failed to get topic names and types by node"); + } + + std::map> topics_and_types; + for (size_t i = 0; i < topic_names_and_types.names.size; ++i) { + std::string topic_name = topic_names_and_types.names.data[i]; + for (size_t j = 0; j < topic_names_and_types.types[i].size; ++j) { + topics_and_types[topic_name].emplace_back(topic_names_and_types.types[i].data[j]); + } + } + + return topics_and_types; +} + +std::map> +NodeGraph::get_subscriber_names_and_types_by_node( + const std::string & node_name, + const std::string & namespace_, + bool no_demangle) const +{ + rcl_names_and_types_t topic_names_and_types = rcl_get_zero_initialized_names_and_types(); + auto topic_names_and_types_finalizer = rcpputils::make_scope_exit( + [&topic_names_and_types]() { + if (rcl_names_and_types_fini(&topic_names_and_types) != RCL_RET_OK) { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), "could not destroy topic names and types"); + } + }); + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_ret_t ret = rcl_get_subscriber_names_and_types_by_node( + node_base_->get_rcl_node_handle(), + &allocator, + no_demangle, + node_name.c_str(), + namespace_.c_str(), + &topic_names_and_types); + if (ret != RCL_RET_OK) { + throw_from_rcl_error(ret, "failed to get topic names and types by node"); + } + + std::map> topics_and_types; + for (size_t i = 0; i < topic_names_and_types.names.size; ++i) { + std::string topic_name = topic_names_and_types.names.data[i]; + for (size_t j = 0; j < topic_names_and_types.types[i].size; ++j) { + topics_and_types[topic_name].emplace_back(topic_names_and_types.types[i].data[j]); + } + } + + return topics_and_types; +} + std::vector NodeGraph::get_node_names() const +{ + std::vector nodes; + auto names_and_namespaces = get_node_names_and_namespaces(); + + std::transform( + names_and_namespaces.begin(), + names_and_namespaces.end(), + std::back_inserter(nodes), + [](std::pair nns) { + std::string return_string; + if (nns.second.back() == '/') { + return_string = nns.second + nns.first; + } else { + return_string = nns.second + '/' + nns.first; + } + // Quick check to make sure that we start with a slash + // Since fully-qualified strings need to + if (return_string.front() != '/') { + return_string = "/" + return_string; + } + return return_string; + } + ); + return nodes; +} + +std::vector> +NodeGraph::get_node_names_with_enclaves() const +{ + rcutils_string_array_t node_names_c = + rcutils_get_zero_initialized_string_array(); + rcutils_string_array_t node_namespaces_c = + rcutils_get_zero_initialized_string_array(); + rcutils_string_array_t node_enclaves_c = + rcutils_get_zero_initialized_string_array(); + + auto allocator = rcl_get_default_allocator(); + auto ret = rcl_get_node_names_with_enclaves( + node_base_->get_rcl_node_handle(), + allocator, + &node_names_c, + &node_namespaces_c, + &node_enclaves_c); + if (ret != RCL_RET_OK) { + auto error_msg = + std::string("failed to get node names with enclaves: ") + rcl_get_error_string().str; + rcl_reset_error(); + if (rcutils_string_array_fini(&node_names_c) != RCUTILS_RET_OK) { + error_msg += std::string(", failed also to cleanup node names, leaking memory: ") + + rcl_get_error_string().str; + rcl_reset_error(); + } + if (rcutils_string_array_fini(&node_namespaces_c) != RCUTILS_RET_OK) { + error_msg += std::string(", failed also to cleanup node namespaces, leaking memory: ") + + rcl_get_error_string().str; + rcl_reset_error(); + } + if (rcutils_string_array_fini(&node_enclaves_c) != RCUTILS_RET_OK) { + error_msg += std::string(", failed also to cleanup node enclaves, leaking memory: ") + + rcl_get_error_string().str; + rcl_reset_error(); + } + throw std::runtime_error(error_msg); + } + + std::vector> node_tuples; + for (size_t i = 0; i < node_names_c.size; ++i) { + if (node_names_c.data[i] && node_namespaces_c.data[i] && node_enclaves_c.data[i]) { + node_tuples.emplace_back( + std::make_tuple(node_names_c.data[i], node_namespaces_c.data[i], node_enclaves_c.data[i])); + } + } + + std::string error("failed to finalize array"); + rcl_ret_t ret_names = rcutils_string_array_fini(&node_names_c); + if (ret_names != RCUTILS_RET_OK) { + error += std::string(", could not destroy node names, leaking memory: ") + + rcl_get_error_string().str; + rcl_reset_error(); + } + rcl_ret_t ret_ns = rcutils_string_array_fini(&node_namespaces_c); + if (ret_ns != RCUTILS_RET_OK) { + error += std::string(", could not destroy node namespaces, leaking memory: ") + + rcl_get_error_string().str; + rcl_reset_error(); + } + + rcl_ret_t ret_ecv = rcutils_string_array_fini(&node_enclaves_c); + if (ret_ecv != RCUTILS_RET_OK) { + error += std::string(", could not destroy node enclaves, leaking memory: ") + + rcl_get_error_string().str; + rcl_reset_error(); + } + + if (ret_names != RCUTILS_RET_OK || ret_ns != RCUTILS_RET_OK || ret_ecv != RCUTILS_RET_OK) { + throw std::runtime_error(error); + } + + return node_tuples; +} + +std::vector> +NodeGraph::get_node_names_and_namespaces() const { rcutils_string_array_t node_names_c = rcutils_get_zero_initialized_string_array(); + rcutils_string_array_t node_namespaces_c = + rcutils_get_zero_initialized_string_array(); auto allocator = rcl_get_default_allocator(); auto ret = rcl_get_node_names( node_base_->get_rcl_node_handle(), allocator, - &node_names_c); + &node_names_c, + &node_namespaces_c); if (ret != RCL_RET_OK) { - auto error_msg = std::string("failed to get node names: ") + rcl_get_error_string_safe(); + auto error_msg = std::string("failed to get node names: ") + rcl_get_error_string().str; rcl_reset_error(); if (rcutils_string_array_fini(&node_names_c) != RCUTILS_RET_OK) { error_msg += std::string(", failed also to cleanup node names, leaking memory: ") + - rcl_get_error_string_safe(); + rcl_get_error_string().str; + rcl_reset_error(); + } + if (rcutils_string_array_fini(&node_namespaces_c) != RCUTILS_RET_OK) { + error_msg += std::string(", failed also to cleanup node namespaces, leaking memory: ") + + rcl_get_error_string().str; + rcl_reset_error(); } // TODO(karsten1987): Append rcutils_error_message once it's in master throw std::runtime_error(error_msg); } - std::vector node_names(&node_names_c.data[0], - &node_names_c.data[0 + node_names_c.size]); - ret = rcutils_string_array_fini(&node_names_c); - if (ret != RCUTILS_RET_OK) { + std::vector> node_names; + node_names.reserve(node_names_c.size); + for (size_t i = 0; i < node_names_c.size; ++i) { + if (node_names_c.data[i] && node_namespaces_c.data[i]) { + node_names.emplace_back(node_names_c.data[i], node_namespaces_c.data[i]); + } + } + + std::string error; + rcl_ret_t ret_names = rcutils_string_array_fini(&node_names_c); + if (ret_names != RCUTILS_RET_OK) { // *INDENT-OFF* // TODO(karsten1987): Append rcutils_error_message once it's in master - throw std::runtime_error( - std::string("could not destroy node names: ")); + error = "could not destroy node names"; + // *INDENT-ON* + } + rcl_ret_t ret_ns = rcutils_string_array_fini(&node_namespaces_c); + if (ret_ns != RCUTILS_RET_OK) { + // *INDENT-OFF* + // TODO(karsten1987): Append rcutils_error_message once it's in master + error += ", could not destroy node namespaces"; // *INDENT-ON* } + if (ret_names != RCUTILS_RET_OK || ret_ns != RCUTILS_RET_OK) { + throw std::runtime_error(error); + } + return node_names; } @@ -179,7 +470,7 @@ NodeGraph::count_publishers(const std::string & topic_name) const if (ret != RMW_RET_OK) { // *INDENT-OFF* throw std::runtime_error( - std::string("could not count publishers: ") + rmw_get_error_string_safe()); + std::string("could not count publishers: ") + rmw_get_error_string().str); // *INDENT-ON* } return count; @@ -201,7 +492,51 @@ NodeGraph::count_subscribers(const std::string & topic_name) const if (ret != RMW_RET_OK) { // *INDENT-OFF* throw std::runtime_error( - std::string("could not count subscribers: ") + rmw_get_error_string_safe()); + std::string("could not count subscribers: ") + rmw_get_error_string().str); + // *INDENT-ON* + } + return count; +} + +size_t +NodeGraph::count_clients(const std::string & service_name) const +{ + auto rcl_node_handle = node_base_->get_rcl_node_handle(); + + auto fqdn = rclcpp::expand_topic_or_service_name( + service_name, + rcl_node_get_name(rcl_node_handle), + rcl_node_get_namespace(rcl_node_handle), + true); + + size_t count; + auto ret = rcl_count_clients(rcl_node_handle, fqdn.c_str(), &count); + if (ret != RMW_RET_OK) { + // *INDENT-OFF* + throw std::runtime_error( + std::string("could not count clients: ") + rmw_get_error_string().str); + // *INDENT-ON* + } + return count; +} + +size_t +NodeGraph::count_services(const std::string & service_name) const +{ + auto rcl_node_handle = node_base_->get_rcl_node_handle(); + + auto fqdn = rclcpp::expand_topic_or_service_name( + service_name, + rcl_node_get_name(rcl_node_handle), + rcl_node_get_namespace(rcl_node_handle), + true); + + size_t count; + auto ret = rcl_count_services(rcl_node_handle, fqdn.c_str(), &count); + if (ret != RMW_RET_OK) { + // *INDENT-OFF* + throw std::runtime_error( + std::string("could not count services: ") + rmw_get_error_string().str); // *INDENT-ON* } return count; @@ -242,12 +577,11 @@ NodeGraph::notify_graph_change() } } graph_cv_.notify_all(); - { - auto notify_condition_lock = node_base_->acquire_notify_guard_condition_lock(); - rcl_ret_t ret = rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()); - if (RCL_RET_OK != ret) { - throw_from_rcl_error(ret, "failed to trigger notify guard condition"); - } + try { + node_base_->trigger_notify_guard_condition(); + } catch (const rclcpp::exceptions::RCLError & ex) { + throw std::runtime_error( + std::string("failed to notify wait set on graph change: ") + ex.what()); } } @@ -262,9 +596,11 @@ rclcpp::Event::SharedPtr NodeGraph::get_graph_event() { auto event = rclcpp::Event::make_shared(); - std::lock_guard graph_changed_lock(graph_mutex_); - graph_events_.push_back(event); - graph_users_count_++; + { + std::lock_guard graph_changed_lock(graph_mutex_); + graph_events_.push_back(event); + graph_users_count_++; + } // on first call, add node to graph_listener_ if (should_add_to_graph_listener_.exchange(false)) { graph_listener_->add_node(this); @@ -296,8 +632,8 @@ NodeGraph::wait_for_graph_change( throw EventNotRegisteredError(); } } - auto pred = [&event]() { - return event->check() || !rclcpp::ok(); + auto pred = [&event, context = node_base_->get_context()]() { + return event->check() || !rclcpp::ok(context); }; std::unique_lock graph_lock(graph_mutex_); if (!pred()) { @@ -306,7 +642,205 @@ NodeGraph::wait_for_graph_change( } size_t -NodeGraph::count_graph_users() +NodeGraph::count_graph_users() const { return graph_users_count_.load(); } + +static +std::vector +convert_to_topic_info_list(const rcl_topic_endpoint_info_array_t & info_array) +{ + std::vector topic_info_list; + for (size_t i = 0; i < info_array.size; ++i) { + topic_info_list.push_back(rclcpp::TopicEndpointInfo(info_array.info_array[i])); + } + return topic_info_list; +} + +template +static std::vector +get_info_by_topic( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + const std::string & topic_name, + bool no_mangle, + FunctionT rcl_get_info_by_topic) +{ + std::string fqdn; + auto rcl_node_handle = node_base->get_rcl_node_handle(); + + if (no_mangle) { + fqdn = topic_name; + } else { + fqdn = rclcpp::expand_topic_or_service_name( + topic_name, + rcl_node_get_name(rcl_node_handle), + rcl_node_get_namespace(rcl_node_handle), + false); // false = not a service + + // Get the node options + const rcl_node_options_t * node_options = rcl_node_get_options(rcl_node_handle); + if (nullptr == node_options) { + throw std::runtime_error("Need valid node options in get_info_by_topic()"); + } + const rcl_arguments_t * global_args = nullptr; + if (node_options->use_global_arguments) { + global_args = &(rcl_node_handle->context->global_arguments); + } + + char * remapped_topic_name = nullptr; + rcl_ret_t ret = rcl_remap_topic_name( + &(node_options->arguments), + global_args, + fqdn.c_str(), + rcl_node_get_name(rcl_node_handle), + rcl_node_get_namespace(rcl_node_handle), + node_options->allocator, + &remapped_topic_name); + if (RCL_RET_OK != ret) { + throw_from_rcl_error(ret, std::string("Failed to remap topic name ") + fqdn); + } else if (nullptr != remapped_topic_name) { + fqdn = remapped_topic_name; + node_options->allocator.deallocate(remapped_topic_name, node_options->allocator.state); + } + } + + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + rcl_topic_endpoint_info_array_t info_array = rcl_get_zero_initialized_topic_endpoint_info_array(); + rcl_ret_t ret = + rcl_get_info_by_topic(rcl_node_handle, &allocator, fqdn.c_str(), no_mangle, &info_array); + if (RCL_RET_OK != ret) { + auto error_msg = + std::string("Failed to get information by topic for ") + EndpointType + std::string(":"); + if (RCL_RET_UNSUPPORTED == ret) { + error_msg += std::string("function not supported by RMW_IMPLEMENTATION"); + } else { + error_msg += rcl_get_error_string().str; + } + rcl_reset_error(); + if (RCL_RET_OK != rcl_topic_endpoint_info_array_fini(&info_array, &allocator)) { + error_msg += std::string(", failed also to cleanup topic info array, leaking memory: ") + + rcl_get_error_string().str; + rcl_reset_error(); + } + throw_from_rcl_error(ret, error_msg); + } + + std::vector topic_info_list = convert_to_topic_info_list(info_array); + ret = rcl_topic_endpoint_info_array_fini(&info_array, &allocator); + if (RCL_RET_OK != ret) { + throw_from_rcl_error(ret, "rcl_topic_info_array_fini failed."); + } + + return topic_info_list; +} + +static constexpr char kPublisherEndpointTypeName[] = "publishers"; +std::vector +NodeGraph::get_publishers_info_by_topic( + const std::string & topic_name, + bool no_mangle) const +{ + return get_info_by_topic( + node_base_, + topic_name, + no_mangle, + rcl_get_publishers_info_by_topic); +} + +static constexpr char kSubscriptionEndpointTypeName[] = "subscriptions"; +std::vector +NodeGraph::get_subscriptions_info_by_topic( + const std::string & topic_name, + bool no_mangle) const +{ + return get_info_by_topic( + node_base_, + topic_name, + no_mangle, + rcl_get_subscriptions_info_by_topic); +} + +std::string & +rclcpp::TopicEndpointInfo::node_name() +{ + return node_name_; +} + +const std::string & +rclcpp::TopicEndpointInfo::node_name() const +{ + return node_name_; +} + +std::string & +rclcpp::TopicEndpointInfo::node_namespace() +{ + return node_namespace_; +} + +const std::string & +rclcpp::TopicEndpointInfo::node_namespace() const +{ + return node_namespace_; +} + +std::string & +rclcpp::TopicEndpointInfo::topic_type() +{ + return topic_type_; +} + +const std::string & +rclcpp::TopicEndpointInfo::topic_type() const +{ + return topic_type_; +} + +rclcpp::EndpointType & +rclcpp::TopicEndpointInfo::endpoint_type() +{ + return endpoint_type_; +} + +const rclcpp::EndpointType & +rclcpp::TopicEndpointInfo::endpoint_type() const +{ + return endpoint_type_; +} + +std::array & +rclcpp::TopicEndpointInfo::endpoint_gid() +{ + return endpoint_gid_; +} + +const std::array & +rclcpp::TopicEndpointInfo::endpoint_gid() const +{ + return endpoint_gid_; +} + +rclcpp::QoS & +rclcpp::TopicEndpointInfo::qos_profile() +{ + return qos_profile_; +} + +const rclcpp::QoS & +rclcpp::TopicEndpointInfo::qos_profile() const +{ + return qos_profile_; +} + +rosidl_type_hash_t & +rclcpp::TopicEndpointInfo::topic_type_hash() +{ + return topic_type_hash_; +} + +const rosidl_type_hash_t & +rclcpp::TopicEndpointInfo::topic_type_hash() const +{ + return topic_type_hash_; +} diff --git a/rclcpp/src/rclcpp/node_interfaces/node_logging.cpp b/rclcpp/src/rclcpp/node_interfaces/node_logging.cpp index bba4d49cfd..4f7476a09e 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_logging.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_logging.cpp @@ -12,15 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "rclcpp/node_impl.hpp" #include "rclcpp/node_interfaces/node_logging.hpp" +#include "rclcpp/node_interfaces/node_services_interface.hpp" using rclcpp::node_interfaces::NodeLogging; -NodeLogging::NodeLogging(rclcpp::node_interfaces::NodeBaseInterface * node_base) +NodeLogging::NodeLogging(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base) : node_base_(node_base) { - // TODO(dhood): use the namespace (slashes converted to dots) - logger_ = rclcpp::get_logger(node_base_->get_name()); + logger_ = rclcpp::get_logger(NodeLogging::get_logger_name()); } NodeLogging::~NodeLogging() @@ -32,3 +33,61 @@ NodeLogging::get_logger() const { return logger_; } + +const char * +NodeLogging::get_logger_name() const +{ + return rcl_node_get_logger_name(node_base_->get_rcl_node_handle()); +} + +void NodeLogging::create_logger_services( + node_interfaces::NodeServicesInterface::SharedPtr node_services) +{ + rclcpp::ServicesQoS qos_profile; + const std::string node_name = node_base_->get_name(); + auto callback_group = node_base_->get_default_callback_group(); + + get_loggers_service_ = rclcpp::create_service( + node_base_, node_services, + node_name + "/get_logger_levels", + []( + const std::shared_ptr, + const std::shared_ptr request, + std::shared_ptr response) + { + for (auto & name : request->names) { + rcl_interfaces::msg::LoggerLevel logger_level; + logger_level.name = name; + auto ret = rcutils_logging_get_logger_level(name.c_str()); + if (ret < 0) { + logger_level.level = 0; + } else { + logger_level.level = static_cast(ret); + } + response->levels.push_back(std::move(logger_level)); + } + }, + qos_profile, callback_group); + + set_loggers_service_ = rclcpp::create_service( + node_base_, node_services, + node_name + "/set_logger_levels", + []( + const std::shared_ptr, + const std::shared_ptr request, + std::shared_ptr response) + { + rcl_interfaces::msg::SetLoggerLevelsResult result; + for (auto & level : request->levels) { + auto ret = rcutils_logging_set_logger_level(level.name.c_str(), level.level); + if (ret != RCUTILS_RET_OK) { + result.successful = false; + result.reason = rcutils_get_error_string().str; + } else { + result.successful = true; + } + response->results.push_back(std::move(result)); + } + }, + qos_profile, callback_group); +} diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index c58d1064f4..922ce9e4d1 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -14,232 +14,1180 @@ #include "rclcpp/node_interfaces/node_parameters.hpp" +#include + +#include +#include +#include +#include +#include +#include #include #include +#include #include #include #include #include "rcl_interfaces/srv/list_parameters.hpp" #include "rclcpp/create_publisher.hpp" +#include "rclcpp/parameter_map.hpp" #include "rcutils/logging_macros.h" #include "rmw/qos_profiles.h" +#include "../detail/resolve_parameter_overrides.hpp" + using rclcpp::node_interfaces::NodeParameters; +RCLCPP_LOCAL +void +local_perform_automatically_declare_parameters_from_overrides( + const std::map & parameter_overrides, + std::function has_parameter, + std::function + declare_parameter) +{ + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.dynamic_typing = true; + for (const auto & pair : parameter_overrides) { + if (!has_parameter(pair.first)) { + declare_parameter( + pair.first, + pair.second, + descriptor, + true); + } + } +} + NodeParameters::NodeParameters( - rclcpp::node_interfaces::NodeTopicsInterface * node_topics, - bool use_intra_process) -: node_topics_(node_topics) + const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics, + const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services, + const rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock, + const std::vector & parameter_overrides, + bool start_parameter_services, + bool start_parameter_event_publisher, + const rclcpp::QoS & parameter_event_qos, + const rclcpp::PublisherOptionsBase & parameter_event_publisher_options, + bool allow_undeclared_parameters, + bool automatically_declare_parameters_from_overrides) +: allow_undeclared_(allow_undeclared_parameters), + events_publisher_(nullptr), + node_logging_(node_logging), + node_clock_(node_clock) { using MessageT = rcl_interfaces::msg::ParameterEvent; using PublisherT = rclcpp::Publisher; using AllocatorT = std::allocator; // TODO(wjwwood): expose this allocator through the Parameter interface. - auto allocator = std::make_shared(); + rclcpp::PublisherOptionsWithAllocator publisher_options( + parameter_event_publisher_options); + publisher_options.allocator = std::make_shared(); + + if (start_parameter_services) { + parameter_service_ = std::make_shared(node_base, node_services, this); + } + + if (start_parameter_event_publisher) { + // TODO(ivanpauno): Qos of the `/parameters_event` topic should be somehow overridable. + events_publisher_ = rclcpp::create_publisher( + node_topics, + "/parameter_events", + parameter_event_qos, + publisher_options); + } + + // Get the node options + const rcl_node_t * node = node_base->get_rcl_node_handle(); + if (nullptr == node) { + throw std::runtime_error("Need valid node handle in NodeParameters"); + } + const rcl_node_options_t * options = rcl_node_get_options(node); + if (nullptr == options) { + throw std::runtime_error("Need valid node options in NodeParameters"); + } + + const rcl_arguments_t * global_args = nullptr; + if (options->use_global_arguments) { + auto context_ptr = node_base->get_context()->get_rcl_context(); + global_args = &(context_ptr->global_arguments); + } + combined_name_ = node_base->get_fully_qualified_name(); + + parameter_overrides_ = rclcpp::detail::resolve_parameter_overrides( + combined_name_, parameter_overrides, &options->arguments, global_args); + + // If asked, initialize any parameters that ended up in the initial parameter values, + // but did not get declared explcitily by this point. + if (automatically_declare_parameters_from_overrides) { + using namespace std::placeholders; + local_perform_automatically_declare_parameters_from_overrides( + this->get_parameter_overrides(), + std::bind(&NodeParameters::has_parameter, this, _1), + [this]( + const std::string & name, + const rclcpp::ParameterValue & default_value, + const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor, + bool ignore_override) + { + NodeParameters::declare_parameter( + name, default_value, parameter_descriptor, ignore_override); + } + ); + } +} - events_publisher_ = rclcpp::create_publisher( - node_topics_, - "parameter_events", - rmw_qos_profile_parameter_events, - use_intra_process, - allocator); +void +NodeParameters::perform_automatically_declare_parameters_from_overrides() +{ + local_perform_automatically_declare_parameters_from_overrides( + this->get_parameter_overrides(), + [this](const std::string & name) { + return this->has_parameter(name); + }, + [this]( + const std::string & name, + const rclcpp::ParameterValue & default_value, + const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor, + bool ignore_override) + { + this->declare_parameter( + name, default_value, parameter_descriptor, ignore_override); + } + ); } NodeParameters::~NodeParameters() {} +RCLCPP_LOCAL +bool +__lockless_has_parameter( + const std::map & parameters, + const std::string & name) +{ + return parameters.find(name) != parameters.end(); +} + +// see https://en.cppreference.com/w/cpp/types/numeric_limits/epsilon +RCLCPP_LOCAL +bool +__are_doubles_equal(double x, double y, double ulp = 100.0) +{ + return std::abs(x - y) <= std::numeric_limits::epsilon() * std::abs(x + y) * ulp; +} + +static +std::string +format_range_reason(const std::string & name, const char * range_type) +{ + std::ostringstream ss; + ss << "Parameter {" << name << "} doesn't comply with " << range_type << " range."; + return ss.str(); +} + +RCLCPP_LOCAL +rcl_interfaces::msg::SetParametersResult +__check_parameter_value_in_range( + const rcl_interfaces::msg::ParameterDescriptor & descriptor, + const rclcpp::ParameterValue & value) +{ + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + if (!descriptor.integer_range.empty() && value.get_type() == rclcpp::PARAMETER_INTEGER) { + int64_t v = value.get(); + auto integer_range = descriptor.integer_range.at(0); + if (v == integer_range.from_value || v == integer_range.to_value) { + return result; + } + if ((v < integer_range.from_value) || (v > integer_range.to_value)) { + result.successful = false; + result.reason = format_range_reason(descriptor.name, "integer"); + return result; + } + if (integer_range.step == 0) { + return result; + } + if (((v - integer_range.from_value) % integer_range.step) == 0) { + return result; + } + result.successful = false; + result.reason = format_range_reason(descriptor.name, "integer"); + return result; + } + + if (!descriptor.floating_point_range.empty() && value.get_type() == rclcpp::PARAMETER_DOUBLE) { + double v = value.get(); + auto fp_range = descriptor.floating_point_range.at(0); + if (__are_doubles_equal(v, fp_range.from_value) || __are_doubles_equal(v, fp_range.to_value)) { + return result; + } + if ((v < fp_range.from_value) || (v > fp_range.to_value)) { + result.successful = false; + result.reason = format_range_reason(descriptor.name, "floating point"); + return result; + } + if (fp_range.step == 0.0) { + return result; + } + double rounded_div = std::round((v - fp_range.from_value) / fp_range.step); + if (__are_doubles_equal(v, fp_range.from_value + rounded_div * fp_range.step)) { + return result; + } + result.successful = false; + result.reason = format_range_reason(descriptor.name, "floating point"); + return result; + } + return result; +} + +static +std::string +format_type_reason( + const std::string & name, const std::string & old_type, const std::string & new_type) +{ + std::ostringstream ss; + // WARN: A condition later depends on this message starting with "Wrong parameter type", + // check `declare_parameter` if you modify this! + ss << "Wrong parameter type, parameter {" << name << "} is of type {" << old_type << + "}, setting it to {" << new_type << "} is not allowed."; + return ss.str(); +} + +// Return true if parameter values comply with the descriptors in parameter_infos. +RCLCPP_LOCAL +rcl_interfaces::msg::SetParametersResult +__check_parameters( + std::map & parameter_infos, + const std::vector & parameters, + bool allow_undeclared) +{ + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + for (const rclcpp::Parameter & parameter : parameters) { + std::string name = parameter.get_name(); + rcl_interfaces::msg::ParameterDescriptor descriptor; + if (allow_undeclared) { + auto it = parameter_infos.find(name); + if (it != parameter_infos.cend()) { + descriptor = it->second.descriptor; + } else { + // implicitly declared parameters are dinamically typed! + descriptor.dynamic_typing = true; + } + } else { + descriptor = parameter_infos[name].descriptor; + } + if (descriptor.name.empty()) { + descriptor.name = name; + } + const auto new_type = parameter.get_type(); + const auto specified_type = static_cast(descriptor.type); + result.successful = descriptor.dynamic_typing || specified_type == new_type; + if (!result.successful) { + result.reason = format_type_reason( + name, rclcpp::to_string(specified_type), rclcpp::to_string(new_type)); + return result; + } + result = __check_parameter_value_in_range( + descriptor, + parameter.get_parameter_value()); + if (!result.successful) { + return result; + } + } + return result; +} + +using PreSetParametersCallbackType = + rclcpp::node_interfaces::NodeParametersInterface::PreSetParametersCallbackType; +using PreSetParametersCallbackHandle = + rclcpp::node_interfaces::PreSetParametersCallbackHandle; +using PreSetCallbacksHandleContainer = + rclcpp::node_interfaces::NodeParameters::PreSetCallbacksHandleContainer; + +using OnSetParametersCallbackType = + rclcpp::node_interfaces::NodeParametersInterface::OnSetParametersCallbackType; +using OnSetParametersCallbackHandle = + rclcpp::node_interfaces::OnSetParametersCallbackHandle; +using OnSetCallbacksHandleContainer = + rclcpp::node_interfaces::NodeParameters::OnSetCallbacksHandleContainer; + +using PostSetParametersCallbackType = + rclcpp::node_interfaces::NodeParametersInterface::PostSetParametersCallbackType; +using PostSetParametersCallbackHandle = + rclcpp::node_interfaces::PostSetParametersCallbackHandle; +using PostSetCallbacksHandleContainer = + rclcpp::node_interfaces::NodeParameters::PostSetCallbacksHandleContainer; + +RCLCPP_LOCAL +void +__call_pre_set_parameters_callbacks( + std::vector & parameters, + PreSetCallbacksHandleContainer & callback_container) +{ + if (callback_container.empty()) { + return; + } + + auto it = callback_container.begin(); + while (it != callback_container.end()) { + auto shared_handle = it->lock(); + if (nullptr != shared_handle) { + shared_handle->callback(parameters); + it++; + } else { + it = callback_container.erase(it); + } + } +} + +RCLCPP_LOCAL +rcl_interfaces::msg::SetParametersResult +__call_on_set_parameters_callbacks( + const std::vector & parameters, + OnSetCallbacksHandleContainer & callback_container) +{ + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + auto it = callback_container.begin(); + while (it != callback_container.end()) { + auto shared_handle = it->lock(); + if (nullptr != shared_handle) { + result = shared_handle->callback(parameters); + if (!result.successful) { + return result; + } + it++; + } else { + it = callback_container.erase(it); + } + } + return result; +} + +RCLCPP_LOCAL +void __call_post_set_parameters_callbacks( + const std::vector & parameters, + PostSetCallbacksHandleContainer & callback_container) +{ + if (callback_container.empty()) { + return; + } + + auto it = callback_container.begin(); + while (it != callback_container.end()) { + auto shared_handle = it->lock(); + if (nullptr != shared_handle) { + shared_handle->callback(parameters); + it++; + } else { + it = callback_container.erase(it); + } + } +} + +RCLCPP_LOCAL +rcl_interfaces::msg::SetParametersResult +__set_parameters_atomically_common( + const std::vector & parameters, + std::map & parameter_infos, + OnSetCallbacksHandleContainer & on_set_callback_container, + PostSetCallbacksHandleContainer & post_set_callback_container, + bool allow_undeclared = false) +{ + // Check if the value being set complies with the descriptor. + rcl_interfaces::msg::SetParametersResult result = __check_parameters( + parameter_infos, parameters, allow_undeclared); + if (!result.successful) { + return result; + } + // Call the user callbacks to see if the new value(s) are allowed. + result = + __call_on_set_parameters_callbacks(parameters, on_set_callback_container); + if (!result.successful) { + return result; + } + // If accepted, actually set the values. + if (result.successful) { + for (size_t i = 0; i < parameters.size(); ++i) { + const std::string & name = parameters[i].get_name(); + parameter_infos[name].descriptor.name = parameters[i].get_name(); + parameter_infos[name].descriptor.type = parameters[i].get_type(); + parameter_infos[name].value = parameters[i].get_parameter_value(); + } + // Call the user post set parameter callback + __call_post_set_parameters_callbacks(parameters, post_set_callback_container); + } + + // Either way, return the result. + return result; +} + +RCLCPP_LOCAL +rcl_interfaces::msg::SetParametersResult +__declare_parameter_common( + const std::string & name, + const rclcpp::ParameterValue & default_value, + const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor, + std::map & parameters_out, + const std::map & overrides, + OnSetCallbacksHandleContainer & on_set_callback_container, + PostSetCallbacksHandleContainer & post_set_callback_container, + rcl_interfaces::msg::ParameterEvent * parameter_event_out, + bool ignore_override = false) +{ + using rclcpp::node_interfaces::ParameterInfo; + std::map parameter_infos {{name, ParameterInfo()}}; + parameter_infos.at(name).descriptor = parameter_descriptor; + + // Use the value from the overrides if available, otherwise use the default. + const rclcpp::ParameterValue * initial_value = &default_value; + auto overrides_it = overrides.find(name); + if (!ignore_override && overrides_it != overrides.end()) { + initial_value = &overrides_it->second; + } + + // If there is no initial value, then skip initialization + if (initial_value->get_type() == rclcpp::PARAMETER_NOT_SET) { + // Add declared parameters to storage (without a value) + parameter_infos[name].descriptor.name = name; + if (parameter_descriptor.dynamic_typing) { + parameter_infos[name].descriptor.type = rclcpp::PARAMETER_NOT_SET; + } else { + parameter_infos[name].descriptor.type = parameter_descriptor.type; + } + parameters_out[name] = parameter_infos.at(name); + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + return result; + } + + // Check with the user's callbacks to see if the initial value can be set. + std::vector parameter_wrappers {rclcpp::Parameter(name, *initial_value)}; + // This function also takes care of default vs initial value. + auto result = __set_parameters_atomically_common( + parameter_wrappers, + parameter_infos, + on_set_callback_container, + post_set_callback_container + ); + + if (!result.successful) { + return result; + } + + // Add declared parameters to storage. + parameters_out[name] = parameter_infos.at(name); + + // Extend the given parameter event, if valid. + if (parameter_event_out) { + parameter_event_out->new_parameters.push_back(parameter_wrappers[0].to_parameter_msg()); + } + + return result; +} + +static +const rclcpp::ParameterValue & +declare_parameter_helper( + const std::string & name, + rclcpp::ParameterType type, + const rclcpp::ParameterValue & default_value, + rcl_interfaces::msg::ParameterDescriptor parameter_descriptor, + bool ignore_override, + std::map & parameters, + const std::map & overrides, + OnSetCallbacksHandleContainer & on_set_callback_container, + PostSetCallbacksHandleContainer & post_set_callback_container, + rclcpp::Publisher * events_publisher, + const std::string & combined_name, + rclcpp::node_interfaces::NodeClockInterface & node_clock) +{ + // TODO(sloretz) parameter name validation + if (name.empty()) { + throw rclcpp::exceptions::InvalidParametersException("parameter name must not be empty"); + } + + // Error if this parameter has already been declared and is different + if (__lockless_has_parameter(parameters, name)) { + throw rclcpp::exceptions::ParameterAlreadyDeclaredException( + "parameter '" + name + "' has already been declared"); + } + + if (!parameter_descriptor.dynamic_typing) { + if (rclcpp::PARAMETER_NOT_SET == type) { + type = default_value.get_type(); + } + if (rclcpp::PARAMETER_NOT_SET == type) { + throw rclcpp::exceptions::InvalidParameterTypeException{ + name, + "cannot declare a statically typed parameter with an uninitialized value" + }; + } + parameter_descriptor.type = static_cast(type); + } + + rcl_interfaces::msg::ParameterEvent parameter_event; + auto result = __declare_parameter_common( + name, + default_value, + parameter_descriptor, + parameters, + overrides, + on_set_callback_container, + post_set_callback_container, + ¶meter_event, + ignore_override); + + // If it failed to be set, then throw an exception. + if (!result.successful) { + constexpr const char type_error_msg_start[] = "Wrong parameter type"; + if ( + 0u == std::strncmp( + result.reason.c_str(), type_error_msg_start, sizeof(type_error_msg_start) - 1)) + { + // TODO(ivanpauno): Refactor the logic so we don't need the above `strncmp` and we can + // detect between both exceptions more elegantly. + throw rclcpp::exceptions::InvalidParameterTypeException(name, result.reason); + } + throw rclcpp::exceptions::InvalidParameterValueException( + "parameter '" + name + "' could not be set: " + result.reason); + } + + // Publish if events_publisher_ is not nullptr, which may be if disabled in the constructor. + if (nullptr != events_publisher) { + parameter_event.node = combined_name; + parameter_event.stamp = node_clock.get_clock()->now(); + events_publisher->publish(parameter_event); + } + + return parameters.at(name).value; +} + +const rclcpp::ParameterValue & +NodeParameters::declare_parameter( + const std::string & name, + const rclcpp::ParameterValue & default_value, + const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor, + bool ignore_override) +{ + std::lock_guard lock(mutex_); + ParameterMutationRecursionGuard guard(parameter_modification_enabled_); + + return declare_parameter_helper( + name, + rclcpp::PARAMETER_NOT_SET, + default_value, + parameter_descriptor, + ignore_override, + parameters_, + parameter_overrides_, + on_set_parameters_callback_container_, + post_set_parameters_callback_container_, + events_publisher_.get(), + combined_name_, + *node_clock_); +} + +const rclcpp::ParameterValue & +NodeParameters::declare_parameter( + const std::string & name, + rclcpp::ParameterType type, + const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor, + bool ignore_override) +{ + std::lock_guard lock(mutex_); + ParameterMutationRecursionGuard guard(parameter_modification_enabled_); + + if (rclcpp::PARAMETER_NOT_SET == type) { + throw std::invalid_argument{ + "declare_parameter(): the provided parameter type cannot be rclcpp::PARAMETER_NOT_SET"}; + } + + if (parameter_descriptor.dynamic_typing == true) { + throw std::invalid_argument{ + "declare_parameter(): cannot declare parameter of specific type and pass descriptor" + "with `dynamic_typing=true`"}; + } + + return declare_parameter_helper( + name, + type, + rclcpp::ParameterValue{}, + parameter_descriptor, + ignore_override, + parameters_, + parameter_overrides_, + on_set_parameters_callback_container_, + post_set_parameters_callback_container_, + events_publisher_.get(), + combined_name_, + *node_clock_); +} + +void +NodeParameters::undeclare_parameter(const std::string & name) +{ + std::lock_guard lock(mutex_); + + ParameterMutationRecursionGuard guard(parameter_modification_enabled_); + + auto parameter_info = parameters_.find(name); + if (parameter_info == parameters_.end()) { + throw rclcpp::exceptions::ParameterNotDeclaredException( + "cannot undeclare parameter '" + name + "' which has not yet been declared"); + } + + if (parameter_info->second.descriptor.read_only) { + throw rclcpp::exceptions::ParameterImmutableException( + "cannot undeclare parameter '" + name + "' because it is read-only"); + } + if (!parameter_info->second.descriptor.dynamic_typing) { + throw rclcpp::exceptions::InvalidParameterTypeException{ + name, "cannot undeclare a statically typed parameter"}; + } + + parameters_.erase(parameter_info); +} + +bool +NodeParameters::has_parameter(const std::string & name) const +{ + std::lock_guard lock(mutex_); + + return __lockless_has_parameter(parameters_, name); +} + std::vector -NodeParameters::set_parameters( - const std::vector & parameters) +NodeParameters::set_parameters(const std::vector & parameters) { std::vector results; - for (auto p : parameters) { + results.reserve(parameters.size()); + + for (const auto & p : parameters) { auto result = set_parameters_atomically({{p}}); results.push_back(result); } + return results; } +template +auto +__find_parameter_by_name( + ParameterVectorType & parameters, + const std::string & name) +{ + return std::find_if( + parameters.begin(), + parameters.end(), + [&](auto parameter) {return parameter.get_name() == name;}); +} + rcl_interfaces::msg::SetParametersResult -NodeParameters::set_parameters_atomically( - const std::vector & parameters) +NodeParameters::set_parameters_atomically(const std::vector & parameters) { - std::lock_guard lock(mutex_); - std::map tmp_map; - auto parameter_event = std::make_shared(); + std::lock_guard lock(mutex_); + + ParameterMutationRecursionGuard guard(parameter_modification_enabled_); - // TODO(jacquelinekay): handle parameter constraints rcl_interfaces::msg::SetParametersResult result; - if (parameters_callback_) { - result = parameters_callback_(parameters); - } else { - result.successful = true; + + // call any user registered pre set parameter callbacks + // this callback can make changes to the original parameters list + // also check if the changed parameter list is empty or not, if empty return + std::vector parameters_after_pre_set_callback(parameters); + __call_pre_set_parameters_callbacks( + parameters_after_pre_set_callback, + pre_set_parameters_callback_container_); + + if (parameters_after_pre_set_callback.empty()) { + result.successful = false; + result.reason = "parameter list cannot be empty, this might be due to " + "pre_set_parameters_callback modifying the original parameters list."; + return result; + } + + // Check if any of the parameters are read-only, or if any parameters are not + // declared. + // If not declared, keep track of them in order to declare them later, when + // undeclared parameters are allowed, and if they're not allowed, fail. + std::vector parameters_to_be_declared; + for (const auto & parameter : parameters_after_pre_set_callback) { + const std::string & name = parameter.get_name(); + + // Check to make sure the parameter name is valid. + if (name.empty()) { + throw rclcpp::exceptions::InvalidParametersException("parameter name must not be empty"); + } + + // Check to see if it is declared. + auto parameter_info = parameters_.find(name); + if (parameter_info == parameters_.end()) { + // If not check to see if undeclared paramaters are allowed, ... + if (allow_undeclared_) { + // If so, mark the parameter to be declared for the user implicitly. + parameters_to_be_declared.push_back(¶meter); + // continue as it cannot be read-only, and because the declare will + // implicitly set the parameter and parameter_infos is for setting only. + continue; + } else { + // If not, then throw the exception as documented. + throw rclcpp::exceptions::ParameterNotDeclaredException( + "parameter '" + name + "' cannot be set because it was not declared"); + } + } + + // Check to see if it is read-only. + if (parameter_info->second.descriptor.read_only) { + result.successful = false; + result.reason = "parameter '" + name + "' cannot be set because it is read-only"; + return result; + } + } + + // Declare parameters into a temporary "staging area", incase one of the declares fail. + // We will use the staged changes as input to the "set atomically" action. + // We explicitly avoid calling the user callbacks here, so that it may be called once, with + // all the other parameters to be set (already declared parameters). + std::map staged_parameter_changes; + rcl_interfaces::msg::ParameterEvent parameter_event_msg; + parameter_event_msg.node = combined_name_; + OnSetCallbacksHandleContainer empty_on_set_callback_container; + PostSetCallbacksHandleContainer empty_post_set_callback_container; + + // Implicit declare uses dynamic type descriptor. + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.dynamic_typing = true; + for (auto parameter_to_be_declared : parameters_to_be_declared) { + // This should not throw, because we validated the name and checked that + // the parameter was not already declared. + result = __declare_parameter_common( + parameter_to_be_declared->get_name(), + parameter_to_be_declared->get_parameter_value(), + descriptor, + staged_parameter_changes, + parameter_overrides_, + // Only call callbacks once below + empty_on_set_callback_container, // callback_container is explicitly empty + empty_post_set_callback_container, // callback_container is explicitly empty + ¶meter_event_msg, + true); + if (!result.successful) { + // Declare failed, return knowing that nothing was changed because the + // staged changes were not applied. + return result; + } + } + + // If there were implicitly declared parameters, then we may need to copy the input parameters + // and then assign the value that was selected after the declare (could be affected by the + // initial parameter values). + const std::vector * parameters_to_be_set = ¶meters_after_pre_set_callback; + std::vector parameters_copy; + if (0 != staged_parameter_changes.size()) { // If there were any implicitly declared parameters. + bool any_initial_values_used = false; + for (const auto & staged_parameter_change : staged_parameter_changes) { + auto it = __find_parameter_by_name( + parameters_after_pre_set_callback, + staged_parameter_change.first); + if (it->get_parameter_value() != staged_parameter_change.second.value) { + // In this case, the value of the staged parameter differs from the + // input from the user, and therefore we need to update things before setting. + any_initial_values_used = true; + // No need to search further since at least one initial value needs to be used. + break; + } + } + if (any_initial_values_used) { + parameters_copy = parameters_after_pre_set_callback; + for (const auto & staged_parameter_change : staged_parameter_changes) { + auto it = __find_parameter_by_name(parameters_copy, staged_parameter_change.first); + *it = Parameter(staged_parameter_change.first, staged_parameter_change.second.value); + } + parameters_to_be_set = ¶meters_copy; + } + } + + // Collect parameters who will have had their type changed to + // rclcpp::PARAMETER_NOT_SET so they can later be implicitly undeclared. + std::vector parameters_to_be_undeclared; + for (const auto & parameter : *parameters_to_be_set) { + if (rclcpp::PARAMETER_NOT_SET == parameter.get_type()) { + auto it = parameters_.find(parameter.get_name()); + if (it != parameters_.end() && rclcpp::PARAMETER_NOT_SET != it->second.value.get_type()) { + if (!it->second.descriptor.dynamic_typing) { + result.reason = "cannot undeclare a statically typed parameter"; + result.successful = false; + return result; + } + parameters_to_be_undeclared.push_back(¶meter); + } + } } + // Set all of the parameters including the ones declared implicitly above. + result = __set_parameters_atomically_common( + // either the original parameters given by the user, or ones updated with initial values + *parameters_to_be_set, + // they are actually set on the official parameter storage + parameters_, + // These callbacks are called once. When a callback returns an unsuccessful result, + // the remaining aren't called + on_set_parameters_callback_container_, + post_set_parameters_callback_container_, + allow_undeclared_); // allow undeclared + + // If not successful, then stop here. if (!result.successful) { return result; } - for (auto p : parameters) { - if (parameters_.find(p.get_name()) == parameters_.end()) { - if (p.get_type() != rclcpp::parameter::ParameterType::PARAMETER_NOT_SET) { - // case: parameter not set before, and input is something other than "NOT_SET" - parameter_event->new_parameters.push_back(p.to_parameter()); - } - } else if (p.get_type() != rclcpp::parameter::ParameterType::PARAMETER_NOT_SET) { - // case: parameter was set before, and input is something other than "NOT_SET" - parameter_event->changed_parameters.push_back(p.to_parameter()); - } else { - // case: parameter was set before, and input is "NOT_SET" - // therefore we will "unset" the previously set parameter - // it is not necessary to erase the parameter from parameters_ - // because the new value for this key (p.get_name()) will be a - // ParameterVariant with type "NOT_SET" - parameter_event->deleted_parameters.push_back(p.to_parameter()); + // If successful, then update the parameter infos from the implicitly declared parameter's. + for (const auto & kv_pair : staged_parameter_changes) { + // assumption: the parameter is already present in parameters_ due to the above "set" + assert(__lockless_has_parameter(parameters_, kv_pair.first)); + // assumption: the value in parameters_ is the same as the value resulting from the declare + assert(parameters_[kv_pair.first].value == kv_pair.second.value); + // This assignment should not change the name, type, or value, but may + // change other things from the ParameterInfo. + parameters_[kv_pair.first] = kv_pair.second; + } + + // Undeclare parameters that need to be. + for (auto parameter_to_undeclare : parameters_to_be_undeclared) { + auto it = parameters_.find(parameter_to_undeclare->get_name()); + // assumption: the parameter to be undeclared should be in the parameter infos map + assert(it != parameters_.end()); + if (it != parameters_.end()) { + // Update the parameter event message and remove it. + parameter_event_msg.deleted_parameters.push_back( + rclcpp::Parameter(it->first, it->second.value).to_parameter_msg()); + parameters_.erase(it); } - tmp_map[p.get_name()] = p; } - // std::map::insert will not overwrite elements, so we'll keep the new - // ones and add only those that already exist in the Node's internal map - tmp_map.insert(parameters_.begin(), parameters_.end()); - std::swap(tmp_map, parameters_); - events_publisher_->publish(parameter_event); + // Update the parameter event message for any parameters which were only set, + // and not either declared or undeclared. + for (const auto & parameter : *parameters_to_be_set) { + if (staged_parameter_changes.find(parameter.get_name()) != staged_parameter_changes.end()) { + // This parameter was declared. + continue; + } + auto it = std::find_if( + parameters_to_be_undeclared.begin(), + parameters_to_be_undeclared.end(), + [¶meter](const auto & p) {return p->get_name() == parameter.get_name();}); + if (it != parameters_to_be_undeclared.end()) { + // This parameter was undeclared (deleted). + continue; + } + // This parameter was neither declared nor undeclared. + parameter_event_msg.changed_parameters.push_back(parameter.to_parameter_msg()); + } + // Publish if events_publisher_ is not nullptr, which may be if disabled in the constructor. + if (nullptr != events_publisher_) { + parameter_event_msg.stamp = node_clock_->get_clock()->now(); + events_publisher_->publish(parameter_event_msg); + } return result; } -std::vector +std::vector NodeParameters::get_parameters(const std::vector & names) const { - std::lock_guard lock(mutex_); - std::vector results; + std::vector results; + results.reserve(names.size()); + std::lock_guard lock(mutex_); for (auto & name : names) { - if (std::any_of(parameters_.cbegin(), parameters_.cend(), - [&name](const std::pair & kv) { - return name == kv.first; - })) - { - results.push_back(parameters_.at(name)); - } + results.emplace_back(this->get_parameter(name)); } return results; } -rclcpp::parameter::ParameterVariant +rclcpp::Parameter NodeParameters::get_parameter(const std::string & name) const { - rclcpp::parameter::ParameterVariant parameter; + std::lock_guard lock(mutex_); - if (get_parameter(name, parameter)) { - return parameter; + auto param_iter = parameters_.find(name); + if (parameters_.end() != param_iter) { + if ( + param_iter->second.value.get_type() != rclcpp::ParameterType::PARAMETER_NOT_SET || + param_iter->second.descriptor.dynamic_typing) + { + return rclcpp::Parameter{name, param_iter->second.value}; + } + throw rclcpp::exceptions::ParameterUninitializedException(name); + } else if (this->allow_undeclared_) { + return rclcpp::Parameter{name}; } else { - throw std::out_of_range("Parameter '" + name + "' not set"); + throw rclcpp::exceptions::ParameterNotDeclaredException(name); } } bool NodeParameters::get_parameter( const std::string & name, - rclcpp::parameter::ParameterVariant & parameter) const + rclcpp::Parameter & parameter) const { - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); - if (parameters_.count(name)) { - parameter = parameters_.at(name); + auto param_iter = parameters_.find(name); + if ( + parameters_.end() != param_iter && + param_iter->second.value.get_type() != rclcpp::ParameterType::PARAMETER_NOT_SET) + { + parameter = {name, param_iter->second.value}; return true; } else { return false; } } +bool +NodeParameters::get_parameters_by_prefix( + const std::string & prefix, + std::map & parameters) const +{ + std::lock_guard lock(mutex_); + + std::string prefix_with_dot = prefix.empty() ? prefix : prefix + "."; + bool ret = false; + + for (const auto & param : parameters_) { + if (param.first.find(prefix_with_dot) == 0 && param.first.length() > prefix_with_dot.length()) { + // Found one! + parameters[param.first.substr(prefix_with_dot.length())] = rclcpp::Parameter(param.second); + ret = true; + } + } + + return ret; +} + std::vector NodeParameters::describe_parameters(const std::vector & names) const { - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); std::vector results; - for (auto & kv : parameters_) { - if (std::any_of(names.cbegin(), names.cend(), [&kv](const std::string & name) { - return name == kv.first; - })) - { - rcl_interfaces::msg::ParameterDescriptor parameter_descriptor; - parameter_descriptor.name = kv.first; - parameter_descriptor.type = kv.second.get_type(); - results.push_back(parameter_descriptor); + results.reserve(names.size()); + + for (const auto & name : names) { + auto it = parameters_.find(name); + if (it != parameters_.cend()) { + results.push_back(it->second.descriptor); + } else if (allow_undeclared_) { + // parameter not found, but undeclared allowed, so return empty + rcl_interfaces::msg::ParameterDescriptor default_description; + default_description.name = name; + results.push_back(default_description); + } else { + throw rclcpp::exceptions::ParameterNotDeclaredException(name); } } + + if (results.size() != names.size()) { + throw std::runtime_error("results and names unexpectedly different sizes"); + } + return results; } std::vector NodeParameters::get_parameter_types(const std::vector & names) const { - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); std::vector results; - for (auto & kv : parameters_) { - if (std::any_of(names.cbegin(), names.cend(), [&kv](const std::string & name) { - return name == kv.first; - })) - { - results.push_back(kv.second.get_type()); - } else { + results.reserve(names.size()); + + for (const auto & name : names) { + auto it = parameters_.find(name); + if (it != parameters_.cend()) { + results.push_back(it->second.value.get_type()); + } else if (allow_undeclared_) { + // parameter not found, but undeclared allowed, so return not set results.push_back(rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET); + } else { + throw rclcpp::exceptions::ParameterNotDeclaredException(name); } } + + if (results.size() != names.size()) { + throw std::runtime_error("results and names unexpectedly different sizes"); + } + return results; } rcl_interfaces::msg::ListParametersResult NodeParameters::list_parameters(const std::vector & prefixes, uint64_t depth) const { - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); rcl_interfaces::msg::ListParametersResult result; // TODO(mikaelarguedas) define parameter separator different from "/" to avoid ambiguity // using "." for now const char * separator = "."; - for (auto & kv : parameters_) { - bool get_all = (prefixes.size() == 0) && - ((depth == rcl_interfaces::srv::ListParameters::Request::DEPTH_RECURSIVE) || - (static_cast(std::count(kv.first.begin(), kv.first.end(), *separator)) < depth)); - bool prefix_matches = std::any_of(prefixes.cbegin(), prefixes.cend(), - [&kv, &depth, &separator](const std::string & prefix) { - if (kv.first == prefix) { - return true; - } else if (kv.first.find(prefix + separator) == 0) { - size_t length = prefix.length(); - std::string substr = kv.first.substr(length); - // Cast as unsigned integer to avoid warning - return (depth == rcl_interfaces::srv::ListParameters::Request::DEPTH_RECURSIVE) || - (static_cast(std::count(substr.begin(), substr.end(), *separator)) < depth); - } - return false; - }); - if (get_all || prefix_matches) { - result.names.push_back(kv.first); - size_t last_separator = kv.first.find_last_of(separator); - if (std::string::npos != last_separator) { - std::string prefix = kv.first.substr(0, last_separator); - if (std::find(result.prefixes.cbegin(), result.prefixes.cend(), prefix) == - result.prefixes.cend()) - { - result.prefixes.push_back(prefix); + + auto separators_less_than_depth = [&depth, &separator](const std::string & str) -> bool { + return static_cast(std::count(str.begin(), str.end(), *separator)) < depth; + }; + + bool recursive = (prefixes.size() == 0) && + (depth == rcl_interfaces::srv::ListParameters::Request::DEPTH_RECURSIVE); + + for (const std::pair & kv : parameters_) { + if (!recursive) { + bool get_all = (prefixes.size() == 0) && separators_less_than_depth(kv.first); + if (!get_all) { + bool prefix_matches = std::any_of( + prefixes.cbegin(), prefixes.cend(), + [&kv, &depth, &separator, &separators_less_than_depth](const std::string & prefix) { + if (kv.first == prefix) { + return true; + } else if (kv.first.find(prefix + separator) == 0) { + if (depth == rcl_interfaces::srv::ListParameters::Request::DEPTH_RECURSIVE) { + return true; + } + std::string substr = kv.first.substr(prefix.length() + 1); + return separators_less_than_depth(substr); + } + return false; + }); + + if (!prefix_matches) { + continue; } } } + + result.names.push_back(kv.first); + size_t last_separator = kv.first.find_last_of(separator); + if (std::string::npos != last_separator) { + std::string prefix = kv.first.substr(0, last_separator); + if ( + std::find(result.prefixes.cbegin(), result.prefixes.cend(), prefix) == + result.prefixes.cend()) + { + result.prefixes.push_back(prefix); + } + } } return result; } void -NodeParameters::register_param_change_callback(ParametersCallbackFunction callback) +NodeParameters::remove_pre_set_parameters_callback( + const PreSetParametersCallbackHandle * const handle) { - if (parameters_callback_) { - RCUTILS_LOG_WARN("param_change_callback already registered, " - "overwriting previous callback") + std::lock_guard lock(mutex_); + ParameterMutationRecursionGuard guard(parameter_modification_enabled_); + + auto it = std::find_if( + pre_set_parameters_callback_container_.begin(), + pre_set_parameters_callback_container_.end(), + [handle](const auto & weak_handle) { + return handle == weak_handle.lock().get(); + }); + if (it != pre_set_parameters_callback_container_.end()) { + pre_set_parameters_callback_container_.erase(it); + } else { + throw std::runtime_error("Pre set parameter callback doesn't exist"); } - parameters_callback_ = callback; +} + +void +NodeParameters::remove_on_set_parameters_callback( + const OnSetParametersCallbackHandle * const handle) +{ + std::lock_guard lock(mutex_); + ParameterMutationRecursionGuard guard(parameter_modification_enabled_); + + auto it = std::find_if( + on_set_parameters_callback_container_.begin(), + on_set_parameters_callback_container_.end(), + [handle](const auto & weak_handle) { + return handle == weak_handle.lock().get(); + }); + if (it != on_set_parameters_callback_container_.end()) { + on_set_parameters_callback_container_.erase(it); + } else { + throw std::runtime_error("On set parameter callback doesn't exist"); + } +} + +void +NodeParameters::remove_post_set_parameters_callback( + const PostSetParametersCallbackHandle * const handle) +{ + std::lock_guard lock(mutex_); + ParameterMutationRecursionGuard guard(parameter_modification_enabled_); + + auto it = std::find_if( + post_set_parameters_callback_container_.begin(), + post_set_parameters_callback_container_.end(), + [handle](const auto & weak_handle) { + return handle == weak_handle.lock().get(); + }); + if (it != post_set_parameters_callback_container_.end()) { + post_set_parameters_callback_container_.erase(it); + } else { + throw std::runtime_error("Post set parameter callback doesn't exist"); + } +} + +PreSetParametersCallbackHandle::SharedPtr +NodeParameters::add_pre_set_parameters_callback(PreSetParametersCallbackType callback) +{ + std::lock_guard lock(mutex_); + ParameterMutationRecursionGuard guard(parameter_modification_enabled_); + + auto handle = std::make_shared(); + handle->callback = callback; + // the last callback registered is executed first. + pre_set_parameters_callback_container_.emplace_front(handle); + return handle; +} + +OnSetParametersCallbackHandle::SharedPtr +NodeParameters::add_on_set_parameters_callback(OnSetParametersCallbackType callback) +{ + std::lock_guard lock(mutex_); + ParameterMutationRecursionGuard guard(parameter_modification_enabled_); + + auto handle = std::make_shared(); + handle->callback = callback; + // the last callback registered is executed first. + on_set_parameters_callback_container_.emplace_front(handle); + return handle; +} + +PostSetParametersCallbackHandle::SharedPtr +NodeParameters::add_post_set_parameters_callback( + PostSetParametersCallbackType callback) +{ + std::lock_guard lock(mutex_); + ParameterMutationRecursionGuard guard(parameter_modification_enabled_); + + auto handle = std::make_shared(); + handle->callback = callback; + // the last callback registered is executed first. + post_set_parameters_callback_container_.emplace_front(handle); + return handle; +} + +const std::map & +NodeParameters::get_parameter_overrides() const +{ + return parameter_overrides_; } diff --git a/rclcpp/src/rclcpp/node_interfaces/node_services.cpp b/rclcpp/src/rclcpp/node_interfaces/node_services.cpp index 7c13438028..fdd4e83780 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_services.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_services.cpp @@ -28,52 +28,55 @@ NodeServices::~NodeServices() void NodeServices::add_service( rclcpp::ServiceBase::SharedPtr service_base_ptr, - rclcpp::callback_group::CallbackGroup::SharedPtr group) + rclcpp::CallbackGroup::SharedPtr group) { if (group) { if (!node_base_->callback_group_in_node(group)) { - // TODO(jacquelinekay): use custom exception - throw std::runtime_error("Cannot create service, group not in node."); + throw rclcpp::exceptions::MissingGroupNodeException("service"); } - group->add_service(service_base_ptr); } else { - node_base_->get_default_callback_group()->add_service(service_base_ptr); + group = node_base_->get_default_callback_group(); } + group->add_service(service_base_ptr); + // Notify the executor that a new service was created using the parent Node. - { - auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock(); - if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) { - throw std::runtime_error( - std::string( - "Failed to notify wait set on service creation: ") + rmw_get_error_string() - ); - } + try { + node_base_->trigger_notify_guard_condition(); + group->trigger_notify_guard_condition(); + } catch (const rclcpp::exceptions::RCLError & ex) { + throw std::runtime_error( + std::string("failed to notify wait set on service creation: ") + ex.what()); } } void NodeServices::add_client( rclcpp::ClientBase::SharedPtr client_base_ptr, - rclcpp::callback_group::CallbackGroup::SharedPtr group) + rclcpp::CallbackGroup::SharedPtr group) { if (group) { if (!node_base_->callback_group_in_node(group)) { - // TODO(jacquelinekay): use custom exception - throw std::runtime_error("Cannot create client, group not in node."); + throw rclcpp::exceptions::MissingGroupNodeException("client"); } - group->add_client(client_base_ptr); } else { - node_base_->get_default_callback_group()->add_client(client_base_ptr); + group = node_base_->get_default_callback_group(); } + group->add_client(client_base_ptr); + // Notify the executor that a new client was created using the parent Node. - { - auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock(); - if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) { - throw std::runtime_error( - std::string("Failed to notify wait set on client creation: ") + rmw_get_error_string() - ); - } + try { + node_base_->trigger_notify_guard_condition(); + group->trigger_notify_guard_condition(); + } catch (const rclcpp::exceptions::RCLError & ex) { + throw std::runtime_error( + std::string("failed to notify wait set on client creation: ") + ex.what()); } } + +std::string +NodeServices::resolve_service_name(const std::string & name, bool only_expand) const +{ + return node_base_->resolve_topic_or_service_name(name, true, only_expand); +} diff --git a/rclcpp/src/rclcpp/node_interfaces/node_time_source.cpp b/rclcpp/src/rclcpp/node_interfaces/node_time_source.cpp new file mode 100644 index 0000000000..2bd3a098b6 --- /dev/null +++ b/rclcpp/src/rclcpp/node_interfaces/node_time_source.cpp @@ -0,0 +1,53 @@ +// 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 "rclcpp/node_interfaces/node_time_source.hpp" + +#include +#include + +using rclcpp::node_interfaces::NodeTimeSource; + +NodeTimeSource::NodeTimeSource( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics, + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock, + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters, + const rclcpp::QoS & qos, + bool use_clock_thread) +: node_base_(node_base), + node_topics_(node_topics), + node_graph_(node_graph), + node_services_(node_services), + node_logging_(node_logging), + node_clock_(node_clock), + node_parameters_(node_parameters), + time_source_(qos, use_clock_thread) +{ + time_source_.attachNode( + node_base_, + node_topics_, + node_graph_, + node_services_, + node_logging_, + node_clock_, + node_parameters_); + time_source_.attachClock(node_clock_->get_clock()); +} + +NodeTimeSource::~NodeTimeSource() +{} diff --git a/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp b/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp index 70073d2c60..29d3125e1f 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp @@ -16,6 +16,8 @@ #include +#include "tracetools/tracetools.h" + using rclcpp::node_interfaces::NodeTimers; NodeTimers::NodeTimers(rclcpp::node_interfaces::NodeBaseInterface * node_base) @@ -28,20 +30,27 @@ NodeTimers::~NodeTimers() void NodeTimers::add_timer( rclcpp::TimerBase::SharedPtr timer, - rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) + rclcpp::CallbackGroup::SharedPtr callback_group) { if (callback_group) { if (!node_base_->callback_group_in_node(callback_group)) { - // TODO(jacquelinekay): use custom exception - throw std::runtime_error("Cannot create timer, group not in node."); + throw rclcpp::exceptions::MissingGroupNodeException("timer"); } - callback_group->add_timer(timer); } else { - node_base_->get_default_callback_group()->add_timer(timer); + callback_group = node_base_->get_default_callback_group(); } - if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) { + callback_group->add_timer(timer); + + try { + node_base_->trigger_notify_guard_condition(); + callback_group->trigger_notify_guard_condition(); + } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( - std::string( - "Failed to notify wait set on timer creation: ") + rmw_get_error_string()); + std::string("failed to notify wait set on timer creation: ") + ex.what()); } + + TRACETOOLS_TRACEPOINT( + rclcpp_timer_link_node, + static_cast(timer->get_timer_handle().get()), + static_cast(node_base_->get_rcl_node_handle())); } diff --git a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp index 882ac1f1b0..ce71036b93 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp @@ -14,17 +14,27 @@ #include "rclcpp/node_interfaces/node_topics.hpp" +#include #include -#include "rclcpp/intra_process_manager.hpp" +#include "rclcpp/callback_group.hpp" #include "rclcpp/exceptions.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_timers_interface.hpp" +#include "rclcpp/publisher_base.hpp" +#include "rclcpp/publisher_factory.hpp" +#include "rclcpp/subscription_base.hpp" +#include "rclcpp/subscription_factory.hpp" +#include "rclcpp/qos.hpp" using rclcpp::exceptions::throw_from_rcl_error; using rclcpp::node_interfaces::NodeTopics; -NodeTopics::NodeTopics(rclcpp::node_interfaces::NodeBaseInterface * node_base) -: node_base_(node_base) +NodeTopics::NodeTopics( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + rclcpp::node_interfaces::NodeTimersInterface * node_timers) +: node_base_(node_base), node_timers_(node_timers) {} NodeTopics::~NodeTopics() @@ -34,48 +44,38 @@ rclcpp::PublisherBase::SharedPtr NodeTopics::create_publisher( const std::string & topic_name, const rclcpp::PublisherFactory & publisher_factory, - rcl_publisher_options_t & publisher_options, - bool use_intra_process) + const rclcpp::QoS & qos) { - // Create the MessageT specific Publisher using the factory, but store it as PublisherBase. - auto publisher = publisher_factory.create_typed_publisher( - node_base_, topic_name, publisher_options); - - // Setup intra process publishing if requested. - if (use_intra_process) { - auto context = node_base_->get_context(); - // Get the intra process manager instance for this context. - auto ipm = context->get_sub_context(); - // Register the publisher with the intra process manager. - uint64_t intra_process_publisher_id = - publisher_factory.add_publisher_to_intra_process_manager(ipm.get(), publisher); - // Create a function to be called when publisher to do the intra process publish. - auto shared_publish_callback = publisher_factory.create_shared_publish_callback(ipm); - publisher->setup_intra_process( - intra_process_publisher_id, - shared_publish_callback, - publisher_options); - } - - // Return the completed publisher. - return publisher; + // Create the MessageT specific Publisher using the factory, but return it as PublisherBase. + return publisher_factory.create_typed_publisher(node_base_, topic_name, qos); } void NodeTopics::add_publisher( - rclcpp::PublisherBase::SharedPtr publisher) + rclcpp::PublisherBase::SharedPtr publisher, + rclcpp::CallbackGroup::SharedPtr callback_group) { - // The publisher is not added to a callback group or anthing like that for now. - // It may be stored within the NodeTopics class or the NodeBase class in the future. - (void)publisher; - // Notify the executor that a new publisher was created using the parent Node. - { - auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock(); - if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) { - throw std::runtime_error( - std::string( - "Failed to notify wait set on publisher creation: ") + rmw_get_error_string()); + // Assign to a group. + if (callback_group) { + if (!node_base_->callback_group_in_node(callback_group)) { + throw rclcpp::exceptions::MissingGroupNodeException("publisher"); } + } else { + callback_group = node_base_->get_default_callback_group(); + } + + for (auto & key_event_pair : publisher->get_event_handlers()) { + auto publisher_event = key_event_pair.second; + callback_group->add_waitable(publisher_event); + } + + // Notify the executor that a new publisher was created using the parent Node. + try { + node_base_->trigger_notify_guard_condition(); + callback_group->trigger_notify_guard_condition(); + } catch (const rclcpp::exceptions::RCLError & ex) { + throw std::runtime_error( + std::string("failed to notify wait set on publisher creation: ") + ex.what()); } } @@ -83,49 +83,63 @@ rclcpp::SubscriptionBase::SharedPtr NodeTopics::create_subscription( const std::string & topic_name, const rclcpp::SubscriptionFactory & subscription_factory, - rcl_subscription_options_t & subscription_options, - bool use_intra_process) + const rclcpp::QoS & qos) { - auto subscription = subscription_factory.create_typed_subscription( - node_base_, topic_name, subscription_options); - - // Setup intra process publishing if requested. - if (use_intra_process) { - auto context = node_base_->get_context(); - auto intra_process_manager = - context->get_sub_context(); - subscription_factory.setup_intra_process( - intra_process_manager, subscription, subscription_options); - } - - // Return the completed subscription. - return subscription; + // Create the MessageT specific Subscription using the factory, but return a SubscriptionBase. + return subscription_factory.create_typed_subscription(node_base_, topic_name, qos); } void NodeTopics::add_subscription( rclcpp::SubscriptionBase::SharedPtr subscription, - rclcpp::callback_group::CallbackGroup::SharedPtr callback_group) + rclcpp::CallbackGroup::SharedPtr callback_group) { // Assign to a group. if (callback_group) { if (!node_base_->callback_group_in_node(callback_group)) { - // TODO(jacquelinekay): use custom exception - throw std::runtime_error("Cannot create subscription, callback group not in node."); + throw rclcpp::exceptions::MissingGroupNodeException("subscription"); } - callback_group->add_subscription(subscription); } else { - node_base_->get_default_callback_group()->add_subscription(subscription); + callback_group = node_base_->get_default_callback_group(); + } + + callback_group->add_subscription(subscription); + + for (auto & key_event_pair : subscription->get_event_handlers()) { + auto subscription_event = key_event_pair.second; + callback_group->add_waitable(subscription_event); + } + + auto intra_process_waitable = subscription->get_intra_process_waitable(); + if (nullptr != intra_process_waitable) { + // Add to the callback group to be notified about intra-process msgs. + callback_group->add_waitable(intra_process_waitable); } // Notify the executor that a new subscription was created using the parent Node. - { - auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock(); - if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) { - throw std::runtime_error( - std::string( - "Failed to notify wait set on subscription creation: ") + rmw_get_error_string() - ); - } + try { + node_base_->trigger_notify_guard_condition(); + callback_group->trigger_notify_guard_condition(); + } catch (const rclcpp::exceptions::RCLError & ex) { + throw std::runtime_error( + std::string("failed to notify wait set on subscription creation: ") + ex.what()); } } + +rclcpp::node_interfaces::NodeBaseInterface * +NodeTopics::get_node_base_interface() const +{ + return node_base_; +} + +rclcpp::node_interfaces::NodeTimersInterface * +NodeTopics::get_node_timers_interface() const +{ + return node_timers_; +} + +std::string +NodeTopics::resolve_topic_name(const std::string & name, bool only_expand) const +{ + return node_base_->resolve_topic_or_service_name(name, false, only_expand); +} diff --git a/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp b/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp new file mode 100644 index 0000000000..fdac4652e0 --- /dev/null +++ b/rclcpp/src/rclcpp/node_interfaces/node_type_descriptions.cpp @@ -0,0 +1,153 @@ +// Copyright 2023 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 "rclcpp/node_interfaces/node_type_descriptions.hpp" +#include "rclcpp/parameter_client.hpp" + +#include "type_description_interfaces/srv/get_type_description.h" + +namespace +{ +// Helper wrapper for rclcpp::Service to access ::Request and ::Response types for allocation. +struct GetTypeDescription__C +{ + using Request = type_description_interfaces__srv__GetTypeDescription_Request; + using Response = type_description_interfaces__srv__GetTypeDescription_Response; + using Event = type_description_interfaces__srv__GetTypeDescription_Event; +}; +} // namespace + +// Helper function for C typesupport. +namespace rosidl_typesupport_cpp +{ +template<> +rosidl_service_type_support_t const * +get_service_type_support_handle() +{ + return ROSIDL_GET_SRV_TYPE_SUPPORT(type_description_interfaces, srv, GetTypeDescription); +} +} // namespace rosidl_typesupport_cpp + +namespace rclcpp +{ +namespace node_interfaces +{ + +class NodeTypeDescriptions::NodeTypeDescriptionsImpl +{ +public: + using ServiceT = GetTypeDescription__C; + + rclcpp::Logger logger_; + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; + rclcpp::Service::SharedPtr type_description_srv_; + + NodeTypeDescriptionsImpl( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters, + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services) + : logger_(node_logging->get_logger()), + node_base_(node_base) + { + const std::string enable_param_name = "start_type_description_service"; + + bool enabled = false; + try { + auto enable_param = node_parameters->declare_parameter( + enable_param_name, + rclcpp::ParameterValue(true), + rcl_interfaces::msg::ParameterDescriptor() + .set__name(enable_param_name) + .set__type(rclcpp::PARAMETER_BOOL) + .set__description("Start the ~/get_type_description service for this node.") + .set__read_only(true)); + enabled = enable_param.get(); + } catch (const rclcpp::exceptions::InvalidParameterTypeException & exc) { + RCLCPP_ERROR(logger_, "%s", exc.what()); + throw; + } + + if (enabled) { + auto * rcl_node = node_base->get_rcl_node_handle(); + std::shared_ptr rcl_srv( + new rcl_service_t, + [rcl_node, logger = this->logger_](rcl_service_t * service) + { + if (rcl_service_fini(service, rcl_node) != RCL_RET_OK) { + RCLCPP_ERROR( + logger, + "Error in destruction of rcl service handle [~/get_type_description]: %s", + rcl_get_error_string().str); + rcl_reset_error(); + } + delete service; + }); + *rcl_srv = rcl_get_zero_initialized_service(); + rcl_ret_t rcl_ret = rcl_node_type_description_service_init(rcl_srv.get(), rcl_node); + + if (rcl_ret != RCL_RET_OK) { + RCLCPP_ERROR( + logger_, "Failed to initialize ~/get_type_description service: %s", + rcl_get_error_string().str); + throw std::runtime_error( + "Failed to initialize ~/get_type_description service."); + } + + rclcpp::AnyServiceCallback cb; + cb.set( + [this]( + std::shared_ptr header, + std::shared_ptr request, + std::shared_ptr response + ) { + rcl_node_type_description_service_handle_request( + node_base_->get_rcl_node_handle(), + header.get(), + request.get(), + response.get()); + }); + + type_description_srv_ = std::make_shared>( + node_base_->get_shared_rcl_node_handle(), + rcl_srv, + cb); + node_services->add_service( + std::dynamic_pointer_cast(type_description_srv_), + nullptr); + } + } +}; + +NodeTypeDescriptions::NodeTypeDescriptions( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters, + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services) +: impl_(new NodeTypeDescriptionsImpl( + node_base, + node_logging, + node_parameters, + node_services)) +{} + +NodeTypeDescriptions::~NodeTypeDescriptions() +{} + +} // namespace node_interfaces +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp b/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp new file mode 100644 index 0000000000..96eb8df9cf --- /dev/null +++ b/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp @@ -0,0 +1,66 @@ +// 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 "rclcpp/node_interfaces/node_waitables.hpp" + +#include + +using rclcpp::node_interfaces::NodeWaitables; + +NodeWaitables::NodeWaitables(rclcpp::node_interfaces::NodeBaseInterface * node_base) +: node_base_(node_base) +{} + +NodeWaitables::~NodeWaitables() +{} + +void +NodeWaitables::add_waitable( + rclcpp::Waitable::SharedPtr waitable_ptr, + rclcpp::CallbackGroup::SharedPtr group) +{ + if (group) { + if (!node_base_->callback_group_in_node(group)) { + throw rclcpp::exceptions::MissingGroupNodeException("waitable"); + } + } else { + group = node_base_->get_default_callback_group(); + } + + group->add_waitable(waitable_ptr); + + // Notify the executor that a new waitable was created using the parent Node. + try { + node_base_->trigger_notify_guard_condition(); + group->trigger_notify_guard_condition(); + } catch (const rclcpp::exceptions::RCLError & ex) { + throw std::runtime_error( + std::string("failed to notify wait set on waitable creation: ") + ex.what()); + } +} + +void +NodeWaitables::remove_waitable( + rclcpp::Waitable::SharedPtr waitable_ptr, + rclcpp::CallbackGroup::SharedPtr group) noexcept +{ + if (group) { + if (!node_base_->callback_group_in_node(group)) { + return; + } + group->remove_waitable(waitable_ptr); + } else { + node_base_->get_default_callback_group()->remove_waitable(waitable_ptr); + } +} diff --git a/rclcpp/src/rclcpp/node_options.cpp b/rclcpp/src/rclcpp/node_options.cpp new file mode 100644 index 0000000000..ca58154eb4 --- /dev/null +++ b/rclcpp/src/rclcpp/node_options.cpp @@ -0,0 +1,400 @@ +// 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 "rclcpp/node_options.hpp" + +#include +#include +#include +#include +#include + +#include "rclcpp/detail/utilities.hpp" +#include "rclcpp/exceptions.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp/publisher_options.hpp" +#include "rclcpp/qos.hpp" + +using rclcpp::exceptions::throw_from_rcl_error; + +namespace rclcpp +{ + +namespace detail +{ +static +void +rcl_node_options_t_destructor(rcl_node_options_t * node_options) +{ + if (node_options) { + rcl_ret_t ret = rcl_node_options_fini(node_options); + if (RCL_RET_OK != ret) { + // Cannot throw here, as it may be called in the destructor. + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), + "failed to finalize rcl node options: %s", rcl_get_error_string().str); + rcl_reset_error(); + } + + delete node_options; + node_options = nullptr; + } +} +} // namespace detail + +NodeOptions::NodeOptions(rcl_allocator_t allocator) +: node_options_(nullptr, detail::rcl_node_options_t_destructor), allocator_(allocator) +{} + +NodeOptions::NodeOptions(const NodeOptions & other) +: node_options_(nullptr, detail::rcl_node_options_t_destructor) +{ + *this = other; +} + +NodeOptions & +NodeOptions::operator=(const NodeOptions & other) +{ + if (this != &other) { + this->node_options_.reset(); + this->context_ = other.context_; + this->arguments_ = other.arguments_; + this->parameter_overrides_ = other.parameter_overrides_; + this->use_global_arguments_ = other.use_global_arguments_; + this->enable_rosout_ = other.enable_rosout_; + this->use_intra_process_comms_ = other.use_intra_process_comms_; + this->enable_topic_statistics_ = other.enable_topic_statistics_; + this->start_parameter_services_ = other.start_parameter_services_; + this->start_parameter_event_publisher_ = other.start_parameter_event_publisher_; + this->clock_type_ = other.clock_type_; + this->clock_qos_ = other.clock_qos_; + this->use_clock_thread_ = other.use_clock_thread_; + this->enable_logger_service_ = other.enable_logger_service_; + this->parameter_event_qos_ = other.parameter_event_qos_; + this->rosout_qos_ = other.rosout_qos_; + this->parameter_event_publisher_options_ = other.parameter_event_publisher_options_; + this->allow_undeclared_parameters_ = other.allow_undeclared_parameters_; + this->automatically_declare_parameters_from_overrides_ = + other.automatically_declare_parameters_from_overrides_; + this->allocator_ = other.allocator_; + } + return *this; +} + +const rcl_node_options_t * +NodeOptions::get_rcl_node_options() const +{ + // If it is nullptr, create it on demand. + if (!node_options_) { + node_options_.reset(new rcl_node_options_t); + *node_options_ = rcl_node_get_default_options(); + node_options_->allocator = this->allocator_; + node_options_->use_global_arguments = this->use_global_arguments_; + node_options_->enable_rosout = this->enable_rosout_; + node_options_->rosout_qos = this->rosout_qos_.get_rmw_qos_profile(); + + int c_argc = 0; + std::unique_ptr c_argv; + if (!this->arguments_.empty()) { + if (this->arguments_.size() > static_cast(std::numeric_limits::max())) { + throw_from_rcl_error(RCL_RET_INVALID_ARGUMENT, "Too many args"); + } + + c_argc = static_cast(this->arguments_.size()); + c_argv.reset(new const char *[c_argc]); + + for (std::size_t i = 0; i < this->arguments_.size(); ++i) { + c_argv[i] = this->arguments_[i].c_str(); + } + } + + rcl_ret_t ret = rcl_parse_arguments( + c_argc, c_argv.get(), this->allocator_, &(node_options_->arguments)); + + if (RCL_RET_OK != ret) { + throw_from_rcl_error(ret, "failed to parse arguments"); + } + + std::vector unparsed_ros_arguments = detail::get_unparsed_ros_arguments( + c_argc, c_argv.get(), &(node_options_->arguments), this->allocator_); + if (!unparsed_ros_arguments.empty()) { + throw exceptions::UnknownROSArgsError(std::move(unparsed_ros_arguments)); + } + } + + return node_options_.get(); +} + +rclcpp::Context::SharedPtr +NodeOptions::context() const +{ + return this->context_; +} + +NodeOptions & +NodeOptions::context(rclcpp::Context::SharedPtr context) +{ + this->context_ = context; + return *this; +} + +const std::vector & +NodeOptions::arguments() const +{ + return this->arguments_; +} + +NodeOptions & +NodeOptions::arguments(const std::vector & arguments) +{ + this->node_options_.reset(); // reset node options to make it be recreated on next access. + this->arguments_ = arguments; + return *this; +} + +std::vector & +NodeOptions::parameter_overrides() +{ + return this->parameter_overrides_; +} + +const std::vector & +NodeOptions::parameter_overrides() const +{ + return this->parameter_overrides_; +} + +NodeOptions & +NodeOptions::parameter_overrides(const std::vector & parameter_overrides) +{ + this->parameter_overrides_ = parameter_overrides; + return *this; +} + +bool +NodeOptions::use_global_arguments() const +{ + return this->use_global_arguments_; +} + +NodeOptions & +NodeOptions::use_global_arguments(bool use_global_arguments) +{ + this->node_options_.reset(); // reset node options to make it be recreated on next access. + this->use_global_arguments_ = use_global_arguments; + return *this; +} + +bool +NodeOptions::enable_rosout() const +{ + return this->enable_rosout_; +} + +NodeOptions & +NodeOptions::enable_rosout(bool enable_rosout) +{ + this->node_options_.reset(); // reset node options to make it be recreated on next access. + this->enable_rosout_ = enable_rosout; + return *this; +} + +bool +NodeOptions::use_intra_process_comms() const +{ + return this->use_intra_process_comms_; +} + +NodeOptions & +NodeOptions::use_intra_process_comms(bool use_intra_process_comms) +{ + this->use_intra_process_comms_ = use_intra_process_comms; + return *this; +} + +bool +NodeOptions::enable_topic_statistics() const +{ + return this->enable_topic_statistics_; +} + +NodeOptions & +NodeOptions::enable_topic_statistics(bool enable_topic_statistics) +{ + this->enable_topic_statistics_ = enable_topic_statistics; + return *this; +} + +bool +NodeOptions::start_parameter_services() const +{ + return this->start_parameter_services_; +} + +NodeOptions & +NodeOptions::start_parameter_services(bool start_parameter_services) +{ + this->start_parameter_services_ = start_parameter_services; + return *this; +} + +bool +NodeOptions::enable_logger_service() const +{ + return this->enable_logger_service_; +} + +NodeOptions & +NodeOptions::enable_logger_service(bool enable_logger_service) +{ + this->enable_logger_service_ = enable_logger_service; + return *this; +} + +bool +NodeOptions::start_parameter_event_publisher() const +{ + return this->start_parameter_event_publisher_; +} + +NodeOptions & +NodeOptions::start_parameter_event_publisher(bool start_parameter_event_publisher) +{ + this->start_parameter_event_publisher_ = start_parameter_event_publisher; + return *this; +} + +const rcl_clock_type_t & +NodeOptions::clock_type() const +{ + return this->clock_type_; +} + +NodeOptions & +NodeOptions::clock_type(const rcl_clock_type_t & clock_type) +{ + this->clock_type_ = clock_type; + return *this; +} + +const rclcpp::QoS & +NodeOptions::clock_qos() const +{ + return this->clock_qos_; +} + +NodeOptions & +NodeOptions::clock_qos(const rclcpp::QoS & clock_qos) +{ + this->clock_qos_ = clock_qos; + return *this; +} + +bool +NodeOptions::use_clock_thread() const +{ + return this->use_clock_thread_; +} + +NodeOptions & +NodeOptions::use_clock_thread(bool use_clock_thread) +{ + this->use_clock_thread_ = use_clock_thread; + return *this; +} + +const rclcpp::QoS & +NodeOptions::parameter_event_qos() const +{ + return this->parameter_event_qos_; +} + +NodeOptions & +NodeOptions::parameter_event_qos(const rclcpp::QoS & parameter_event_qos) +{ + this->parameter_event_qos_ = parameter_event_qos; + return *this; +} + +const rclcpp::QoS & +NodeOptions::rosout_qos() const +{ + return this->rosout_qos_; +} + +NodeOptions & +NodeOptions::rosout_qos(const rclcpp::QoS & rosout_qos) +{ + this->node_options_.reset(); + this->rosout_qos_ = rosout_qos; + return *this; +} + +const rclcpp::PublisherOptionsBase & +NodeOptions::parameter_event_publisher_options() const +{ + return parameter_event_publisher_options_; +} + +NodeOptions & +NodeOptions::parameter_event_publisher_options( + const rclcpp::PublisherOptionsBase & parameter_event_publisher_options) +{ + parameter_event_publisher_options_ = parameter_event_publisher_options; + return *this; +} + +bool +NodeOptions::allow_undeclared_parameters() const +{ + return this->allow_undeclared_parameters_; +} + +NodeOptions & +NodeOptions::allow_undeclared_parameters(bool allow_undeclared_parameters) +{ + this->allow_undeclared_parameters_ = allow_undeclared_parameters; + return *this; +} + +bool +NodeOptions::automatically_declare_parameters_from_overrides() const +{ + return this->automatically_declare_parameters_from_overrides_; +} + +NodeOptions & +NodeOptions::automatically_declare_parameters_from_overrides( + bool automatically_declare_parameters_from_overrides) +{ + this->automatically_declare_parameters_from_overrides_ = + automatically_declare_parameters_from_overrides; + return *this; +} + +const rcl_allocator_t & +NodeOptions::allocator() const +{ + return this->allocator_; +} + +NodeOptions & +NodeOptions::allocator(rcl_allocator_t allocator) +{ + this->node_options_.reset(); // reset node options to make it be recreated on next access. + this->allocator_ = allocator; + return *this; +} + +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/parameter.cpp b/rclcpp/src/rclcpp/parameter.cpp index 7e0d289f74..673e06ab9b 100644 --- a/rclcpp/src/rclcpp/parameter.cpp +++ b/rclcpp/src/rclcpp/parameter.cpp @@ -12,222 +12,158 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "rclcpp/parameter.hpp" + #include #include #include #include -#include "rclcpp/parameter.hpp" +#include "rclcpp/node_interfaces/node_parameters.hpp" #include "rclcpp/utilities.hpp" -using rclcpp::parameter::ParameterType; -using rclcpp::parameter::ParameterVariant; +using rclcpp::ParameterType; +using rclcpp::Parameter; -ParameterVariant::ParameterVariant() +Parameter::Parameter() : name_("") { - value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET; -} - -ParameterVariant::ParameterVariant(const std::string & name, const bool bool_value) -: name_(name) -{ - value_.bool_value = bool_value; - value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; } -ParameterVariant::ParameterVariant(const std::string & name, const int int_value) -: name_(name) +Parameter::Parameter(const std::string & name) +: name_(name), value_() { - value_.integer_value = int_value; - value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; } -ParameterVariant::ParameterVariant(const std::string & name, const int64_t int_value) -: name_(name) +Parameter::Parameter(const std::string & name, const rclcpp::ParameterValue & value) +: name_(name), value_(value) { - value_.integer_value = int_value; - value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; } -ParameterVariant::ParameterVariant(const std::string & name, const float double_value) -: name_(name) +Parameter::Parameter(const rclcpp::node_interfaces::ParameterInfo & parameter_info) +: Parameter(parameter_info.descriptor.name, parameter_info.value) { - value_.double_value = double_value; - value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; } -ParameterVariant::ParameterVariant(const std::string & name, const double double_value) -: name_(name) -{ - value_.double_value = double_value; - value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; -} - -ParameterVariant::ParameterVariant(const std::string & name, const std::string & string_value) -: name_(name) +bool +Parameter::operator==(const Parameter & rhs) const { - value_.string_value = string_value; - value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + return this->name_ == rhs.name_ && this->value_ == rhs.value_; } -ParameterVariant::ParameterVariant(const std::string & name, const char * string_value) -: ParameterVariant(name, std::string(string_value)) -{} - -ParameterVariant::ParameterVariant( - const std::string & name, const std::vector & byte_array_value) -: name_(name) +bool +Parameter::operator!=(const Parameter & rhs) const { - value_.byte_array_value = byte_array_value; - value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_BYTE_ARRAY; + return !(*this == rhs); } ParameterType -ParameterVariant::get_type() const +Parameter::get_type() const { - return static_cast(value_.type); + return value_.get_type(); } std::string -ParameterVariant::get_type_name() const +Parameter::get_type_name() const { - switch (get_type()) { - case rclcpp::parameter::ParameterType::PARAMETER_BOOL: - return "bool"; - case rclcpp::parameter::ParameterType::PARAMETER_INTEGER: - return "integer"; - case rclcpp::parameter::ParameterType::PARAMETER_DOUBLE: - return "double"; - case rclcpp::parameter::ParameterType::PARAMETER_STRING: - return "string"; - case rclcpp::parameter::ParameterType::PARAMETER_BYTE_ARRAY: - return "bytes"; - case rclcpp::parameter::ParameterType::PARAMETER_NOT_SET: - return "not set"; - default: - // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here) - throw std::runtime_error( - "Unexpected type from ParameterVariant: " + std::to_string(get_type())); - // *INDENT-ON* - } + return rclcpp::to_string(get_type()); } const std::string & -ParameterVariant::get_name() const +Parameter::get_name() const { return name_; } rcl_interfaces::msg::ParameterValue -ParameterVariant::get_parameter_value() const +Parameter::get_value_message() const +{ + return value_.to_value_msg(); +} + +const rclcpp::ParameterValue & +Parameter::get_parameter_value() const { return value_; } +bool +Parameter::as_bool() const +{ + return get_value(); +} + int64_t -ParameterVariant::as_int() const +Parameter::as_int() const { return get_value(); } double -ParameterVariant::as_double() const +Parameter::as_double() const { return get_value(); } const std::string & -ParameterVariant::as_string() const +Parameter::as_string() const { return get_value(); } -bool -ParameterVariant::as_bool() const +const std::vector & +Parameter::as_byte_array() const { - return get_value(); + return get_value(); } -const std::vector & -ParameterVariant::as_bytes() const +const std::vector & +Parameter::as_bool_array() const { - return get_value(); + return get_value(); } -ParameterVariant -ParameterVariant::from_parameter(const rcl_interfaces::msg::Parameter & parameter) +const std::vector & +Parameter::as_integer_array() const { - switch (parameter.value.type) { - case PARAMETER_BOOL: - return ParameterVariant(parameter.name, parameter.value.bool_value); - case PARAMETER_INTEGER: - return ParameterVariant(parameter.name, parameter.value.integer_value); - case PARAMETER_DOUBLE: - return ParameterVariant(parameter.name, parameter.value.double_value); - case PARAMETER_STRING: - return ParameterVariant(parameter.name, parameter.value.string_value); - case PARAMETER_BYTE_ARRAY: - return ParameterVariant(parameter.name, parameter.value.byte_array_value); - case PARAMETER_NOT_SET: - throw std::runtime_error("Type from ParameterValue is not set"); - default: - // TODO(wjwwood): use custom exception - // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here) - throw std::runtime_error( - "Unexpected type from ParameterVariant: " + std::to_string(parameter.value.type)); - // *INDENT-ON* - } + return get_value(); +} + +const std::vector & +Parameter::as_double_array() const +{ + return get_value(); +} + +const std::vector & +Parameter::as_string_array() const +{ + return get_value(); +} + +Parameter +Parameter::from_parameter_msg(const rcl_interfaces::msg::Parameter & parameter) +{ + return Parameter(parameter.name, parameter.value); } rcl_interfaces::msg::Parameter -ParameterVariant::to_parameter() +Parameter::to_parameter_msg() const { rcl_interfaces::msg::Parameter parameter; parameter.name = name_; - parameter.value = value_; + parameter.value = value_.to_value_msg(); return parameter; } std::string -ParameterVariant::value_to_string() const +Parameter::value_to_string() const { - switch (get_type()) { - case rclcpp::parameter::ParameterType::PARAMETER_BOOL: - return as_bool() ? "true" : "false"; - case rclcpp::parameter::ParameterType::PARAMETER_INTEGER: - return std::to_string(as_int()); - case rclcpp::parameter::ParameterType::PARAMETER_DOUBLE: - return std::to_string(as_double()); - case rclcpp::parameter::ParameterType::PARAMETER_STRING: - return as_string(); - case rclcpp::parameter::ParameterType::PARAMETER_BYTE_ARRAY: - { - std::stringstream bytes; - bool first_byte = true; - bytes << "[" << std::hex; - for (auto & byte : as_bytes()) { - bytes << "0x" << byte; - if (!first_byte) { - bytes << ", "; - } else { - first_byte = false; - } - } - return bytes.str(); - } - case rclcpp::parameter::ParameterType::PARAMETER_NOT_SET: - return "not set"; - default: - // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here) - throw std::runtime_error( - "Unexpected type from ParameterVariant: " + std::to_string(get_type())); - // *INDENT-ON* - } + return rclcpp::to_string(value_); } std::string -rclcpp::parameter::_to_json_dict_entry(const ParameterVariant & param) +rclcpp::_to_json_dict_entry(const Parameter & param) { std::stringstream ss; ss << "\"" << param.get_name() << "\": "; @@ -237,21 +173,21 @@ rclcpp::parameter::_to_json_dict_entry(const ParameterVariant & param) } std::ostream & -rclcpp::parameter::operator<<(std::ostream & os, const rclcpp::parameter::ParameterVariant & pv) +rclcpp::operator<<(std::ostream & os, const rclcpp::Parameter & pv) { os << std::to_string(pv); return os; } std::ostream & -rclcpp::parameter::operator<<(std::ostream & os, const std::vector & parameters) +rclcpp::operator<<(std::ostream & os, const std::vector & parameters) { os << std::to_string(parameters); return os; } std::string -std::to_string(const rclcpp::parameter::ParameterVariant & param) +std::to_string(const rclcpp::Parameter & param) { std::stringstream ss; ss << "{\"name\": \"" << param.get_name() << "\", "; @@ -261,7 +197,7 @@ std::to_string(const rclcpp::parameter::ParameterVariant & param) } std::string -std::to_string(const std::vector & parameters) +std::to_string(const std::vector & parameters) { std::stringstream ss; ss << "{"; @@ -272,7 +208,7 @@ std::to_string(const std::vector & paramete } else { first = false; } - ss << rclcpp::parameter::_to_json_dict_entry(pv); + ss << rclcpp::_to_json_dict_entry(pv); } ss << "}"; return ss.str(); diff --git a/rclcpp/src/rclcpp/parameter_client.cpp b/rclcpp/src/rclcpp/parameter_client.cpp index 18dad919b3..b60585ef00 100644 --- a/rclcpp/src/rclcpp/parameter_client.cpp +++ b/rclcpp/src/rclcpp/parameter_client.cpp @@ -15,7 +15,12 @@ #include "rclcpp/parameter_client.hpp" #include +#include +#include +#include +#include #include +#include #include #include @@ -30,17 +35,18 @@ AsyncParametersClient::AsyncParametersClient( const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface, const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface, const std::string & remote_node_name, - const rmw_qos_profile_t & qos_profile) + const rclcpp::QoS & qos_profile, + rclcpp::CallbackGroup::SharedPtr group) : node_topics_interface_(node_topics_interface) { if (remote_node_name != "") { remote_node_name_ = remote_node_name; } else { - remote_node_name_ = node_base_interface->get_name(); + remote_node_name_ = node_base_interface->get_fully_qualified_name(); } rcl_client_options_t options = rcl_client_get_default_options(); - options.qos = qos_profile; + options.qos = qos_profile.get_rmw_qos_profile(); using rclcpp::Client; using rclcpp::ClientBase; @@ -51,7 +57,7 @@ AsyncParametersClient::AsyncParametersClient( remote_node_name_ + "/" + parameter_service_names::get_parameters, options); auto get_parameters_base = std::dynamic_pointer_cast(get_parameters_client_); - node_services_interface->add_client(get_parameters_base, nullptr); + node_services_interface->add_client(get_parameters_base, group); get_parameter_types_client_ = Client::make_shared( node_base_interface.get(), @@ -60,7 +66,7 @@ AsyncParametersClient::AsyncParametersClient( options); auto get_parameter_types_base = std::dynamic_pointer_cast(get_parameter_types_client_); - node_services_interface->add_client(get_parameter_types_base, nullptr); + node_services_interface->add_client(get_parameter_types_base, group); set_parameters_client_ = Client::make_shared( node_base_interface.get(), @@ -68,7 +74,17 @@ AsyncParametersClient::AsyncParametersClient( remote_node_name_ + "/" + parameter_service_names::set_parameters, options); auto set_parameters_base = std::dynamic_pointer_cast(set_parameters_client_); - node_services_interface->add_client(set_parameters_base, nullptr); + node_services_interface->add_client(set_parameters_base, group); + + set_parameters_atomically_client_ = + Client::make_shared( + node_base_interface.get(), + node_graph_interface, + remote_node_name_ + "/" + parameter_service_names::set_parameters_atomically, + options); + auto set_parameters_atomically_base = std::dynamic_pointer_cast( + set_parameters_atomically_client_); + node_services_interface->add_client(set_parameters_atomically_base, group); list_parameters_client_ = Client::make_shared( node_base_interface.get(), @@ -76,7 +92,7 @@ AsyncParametersClient::AsyncParametersClient( remote_node_name_ + "/" + parameter_service_names::list_parameters, options); auto list_parameters_base = std::dynamic_pointer_cast(list_parameters_client_); - node_services_interface->add_client(list_parameters_base, nullptr); + node_services_interface->add_client(list_parameters_base, group); describe_parameters_client_ = Client::make_shared( node_base_interface.get(), @@ -85,44 +101,18 @@ AsyncParametersClient::AsyncParametersClient( options); auto describe_parameters_base = std::dynamic_pointer_cast(describe_parameters_client_); - node_services_interface->add_client(describe_parameters_base, nullptr); + node_services_interface->add_client(describe_parameters_base, group); } -AsyncParametersClient::AsyncParametersClient( - const rclcpp::Node::SharedPtr node, - const std::string & remote_node_name, - const rmw_qos_profile_t & qos_profile) -: AsyncParametersClient( - node->get_node_base_interface(), - node->get_node_topics_interface(), - node->get_node_graph_interface(), - node->get_node_services_interface(), - remote_node_name, - qos_profile) -{} - -AsyncParametersClient::AsyncParametersClient( - rclcpp::Node * node, - const std::string & remote_node_name, - const rmw_qos_profile_t & qos_profile) -: AsyncParametersClient( - node->get_node_base_interface(), - node->get_node_topics_interface(), - node->get_node_graph_interface(), - node->get_node_services_interface(), - remote_node_name, - qos_profile) -{} - -std::shared_future> +std::shared_future> AsyncParametersClient::get_parameters( const std::vector & names, std::function< - void(std::shared_future>) + void(std::shared_future>) > callback) { auto promise_result = - std::make_shared>>(); + std::make_shared>>(); auto future_result = promise_result->get_future().share(); auto request = std::make_shared(); @@ -133,19 +123,18 @@ AsyncParametersClient::get_parameters( [request, promise_result, future_result, callback]( rclcpp::Client::SharedFuture cb_f) { - std::vector parameter_variants; + std::vector parameters; auto & pvalues = cb_f.get()->values; for (auto & pvalue : pvalues) { - auto i = &pvalue - &pvalues[0]; + auto i = static_cast(&pvalue - &pvalues[0]); rcl_interfaces::msg::Parameter parameter; parameter.name = request->names[i]; parameter.value = pvalue; - parameter_variants.push_back(rclcpp::parameter::ParameterVariant::from_parameter( - parameter)); + parameters.push_back(rclcpp::Parameter::from_parameter_msg(parameter)); } - promise_result->set_value(parameter_variants); + promise_result->set_value(parameters); if (callback != nullptr) { callback(future_result); } @@ -155,15 +144,44 @@ AsyncParametersClient::get_parameters( return future_result; } -std::shared_future> +std::shared_future> +AsyncParametersClient::describe_parameters( + const std::vector & names, + std::function< + void(std::shared_future>) + > callback) +{ + auto promise_result = + std::make_shared>>(); + auto future_result = promise_result->get_future().share(); + + auto request = std::make_shared(); + request->names = names; + + describe_parameters_client_->async_send_request( + request, + [promise_result, future_result, callback]( + rclcpp::Client::SharedFuture cb_f) + { + promise_result->set_value(cb_f.get()->descriptors); + if (callback != nullptr) { + callback(future_result); + } + } + ); + + return future_result; +} + +std::shared_future> AsyncParametersClient::get_parameter_types( const std::vector & names, std::function< - void(std::shared_future>) + void(std::shared_future>) > callback) { auto promise_result = - std::make_shared>>(); + std::make_shared>>(); auto future_result = promise_result->get_future().share(); auto request = std::make_shared(); @@ -174,10 +192,10 @@ AsyncParametersClient::get_parameter_types( [promise_result, future_result, callback]( rclcpp::Client::SharedFuture cb_f) { - std::vector types; + std::vector types; auto & pts = cb_f.get()->types; for (auto & pt : pts) { - pts.push_back(static_cast(pt)); + types.push_back(static_cast(pt)); } promise_result->set_value(types); if (callback != nullptr) { @@ -191,7 +209,7 @@ AsyncParametersClient::get_parameter_types( std::shared_future> AsyncParametersClient::set_parameters( - const std::vector & parameters, + const std::vector & parameters, std::function< void(std::shared_future>) > callback) @@ -202,10 +220,9 @@ AsyncParametersClient::set_parameters( auto request = std::make_shared(); - std::transform(parameters.begin(), parameters.end(), std::back_inserter(request->parameters), - [](rclcpp::parameter::ParameterVariant p) { - return p.to_parameter(); - } + std::transform( + parameters.begin(), parameters.end(), std::back_inserter(request->parameters), + [](rclcpp::Parameter p) {return p.to_parameter_msg();} ); set_parameters_client_->async_send_request( @@ -225,7 +242,7 @@ AsyncParametersClient::set_parameters( std::shared_future AsyncParametersClient::set_parameters_atomically( - const std::vector & parameters, + const std::vector & parameters, std::function< void(std::shared_future) > callback) @@ -236,10 +253,9 @@ AsyncParametersClient::set_parameters_atomically( auto request = std::make_shared(); - std::transform(parameters.begin(), parameters.end(), std::back_inserter(request->parameters), - [](rclcpp::parameter::ParameterVariant p) { - return p.to_parameter(); - } + std::transform( + parameters.begin(), parameters.end(), std::back_inserter(request->parameters), + [](rclcpp::Parameter p) {return p.to_parameter_msg();} ); set_parameters_atomically_client_->async_send_request( @@ -257,6 +273,51 @@ AsyncParametersClient::set_parameters_atomically( return future_result; } +std::shared_future> +AsyncParametersClient::delete_parameters( + const std::vector & parameters_names) +{ + std::vector parameters; + for (const std::string & name : parameters_names) { + parameters.push_back(rclcpp::Parameter(name)); + } + auto future_result = set_parameters(parameters); + + return future_result; +} + +std::shared_future> +AsyncParametersClient::load_parameters( + const std::string & yaml_filename) +{ + rclcpp::ParameterMap parameter_map = + rclcpp::parameter_map_from_yaml_file(yaml_filename, remote_node_name_.c_str()); + + auto iter = parameter_map.find(remote_node_name_); + if (iter == parameter_map.end() || iter->second.size() == 0) { + throw rclcpp::exceptions::InvalidParametersException("No valid parameter"); + } + auto future_result = set_parameters(iter->second); + + return future_result; +} + +std::shared_future> +AsyncParametersClient::load_parameters( + const rclcpp::ParameterMap & parameter_map) +{ + std::vector parameters = + rclcpp::parameters_from_map(parameter_map, remote_node_name_.c_str()); + + if (parameters.size() == 0) { + throw rclcpp::exceptions::InvalidParametersException("No valid parameter"); + } + auto future_result = set_parameters(parameters); + + return future_result; +} + + std::shared_future AsyncParametersClient::list_parameters( const std::vector & prefixes, @@ -325,40 +386,22 @@ AsyncParametersClient::wait_for_service_nanoseconds(std::chrono::nanoseconds tim return true; } -SyncParametersClient::SyncParametersClient( - rclcpp::Node::SharedPtr node, - const std::string & remote_node_name, - const rmw_qos_profile_t & qos_profile) -: SyncParametersClient( - std::make_shared(), - node, - remote_node_name, - qos_profile) -{} - -SyncParametersClient::SyncParametersClient( - rclcpp::executor::Executor::SharedPtr executor, - rclcpp::Node::SharedPtr node, - const std::string & remote_node_name, - const rmw_qos_profile_t & qos_profile) -: executor_(executor), node_(node) -{ - async_parameters_client_ = - std::make_shared(node, remote_node_name, qos_profile); -} - -std::vector -SyncParametersClient::get_parameters(const std::vector & parameter_names) +std::vector +SyncParametersClient::get_parameters( + const std::vector & parameter_names, + std::chrono::nanoseconds timeout) { auto f = async_parameters_client_->get_parameters(parameter_names); using rclcpp::executors::spin_node_until_future_complete; - if (spin_node_until_future_complete(*executor_, node_->get_node_base_interface(), f) == - rclcpp::executor::FutureReturnCode::SUCCESS) + if ( + spin_node_until_future_complete( + *executor_, node_base_interface_, f, + timeout) == rclcpp::FutureReturnCode::SUCCESS) { return f.get(); } // Return an empty vector if unsuccessful - return std::vector(); + return std::vector(); } bool @@ -370,29 +413,88 @@ SyncParametersClient::has_parameter(const std::string & parameter_name) return vars.names.size() > 0; } -std::vector -SyncParametersClient::get_parameter_types(const std::vector & parameter_names) +std::vector +SyncParametersClient::describe_parameters( + const std::vector & parameter_names, + std::chrono::nanoseconds timeout) +{ + auto f = async_parameters_client_->describe_parameters(parameter_names); + + using rclcpp::executors::spin_node_until_future_complete; + rclcpp::FutureReturnCode future = + spin_node_until_future_complete(*executor_, node_base_interface_, f, timeout); + if (future == rclcpp::FutureReturnCode::SUCCESS) { + return f.get(); + } + return std::vector(); +} + +std::vector +SyncParametersClient::get_parameter_types( + const std::vector & parameter_names, + std::chrono::nanoseconds timeout) { auto f = async_parameters_client_->get_parameter_types(parameter_names); using rclcpp::executors::spin_node_until_future_complete; - if (spin_node_until_future_complete(*executor_, node_->get_node_base_interface(), f) == - rclcpp::executor::FutureReturnCode::SUCCESS) + if ( + spin_node_until_future_complete( + *executor_, node_base_interface_, f, + timeout) == rclcpp::FutureReturnCode::SUCCESS) { return f.get(); } - return std::vector(); + return std::vector(); } std::vector SyncParametersClient::set_parameters( - const std::vector & parameters) + const std::vector & parameters, + std::chrono::nanoseconds timeout) { auto f = async_parameters_client_->set_parameters(parameters); using rclcpp::executors::spin_node_until_future_complete; - if (spin_node_until_future_complete(*executor_, node_->get_node_base_interface(), f) == - rclcpp::executor::FutureReturnCode::SUCCESS) + if ( + spin_node_until_future_complete( + *executor_, node_base_interface_, f, + timeout) == rclcpp::FutureReturnCode::SUCCESS) + { + return f.get(); + } + return std::vector(); +} + +std::vector +SyncParametersClient::delete_parameters( + const std::vector & parameters_names, + std::chrono::nanoseconds timeout) +{ + auto f = async_parameters_client_->delete_parameters(parameters_names); + + using rclcpp::executors::spin_node_until_future_complete; + if ( + spin_node_until_future_complete( + *executor_, node_base_interface_, f, + timeout) == rclcpp::FutureReturnCode::SUCCESS) + { + return f.get(); + } + return std::vector(); +} + +std::vector +SyncParametersClient::load_parameters( + const std::string & yaml_filename, + std::chrono::nanoseconds timeout) +{ + auto f = async_parameters_client_->load_parameters(yaml_filename); + + using rclcpp::executors::spin_node_until_future_complete; + if ( + spin_node_until_future_complete( + *executor_, node_base_interface_, f, + timeout) == rclcpp::FutureReturnCode::SUCCESS) { return f.get(); } @@ -401,13 +503,16 @@ SyncParametersClient::set_parameters( rcl_interfaces::msg::SetParametersResult SyncParametersClient::set_parameters_atomically( - const std::vector & parameters) + const std::vector & parameters, + std::chrono::nanoseconds timeout) { auto f = async_parameters_client_->set_parameters_atomically(parameters); using rclcpp::executors::spin_node_until_future_complete; - if (spin_node_until_future_complete(*executor_, node_->get_node_base_interface(), f) == - rclcpp::executor::FutureReturnCode::SUCCESS) + if ( + spin_node_until_future_complete( + *executor_, node_base_interface_, f, + timeout) == rclcpp::FutureReturnCode::SUCCESS) { return f.get(); } @@ -418,13 +523,16 @@ SyncParametersClient::set_parameters_atomically( rcl_interfaces::msg::ListParametersResult SyncParametersClient::list_parameters( const std::vector & parameter_prefixes, - uint64_t depth) + uint64_t depth, + std::chrono::nanoseconds timeout) { auto f = async_parameters_client_->list_parameters(parameter_prefixes, depth); using rclcpp::executors::spin_node_until_future_complete; - if (spin_node_until_future_complete(*executor_, node_->get_node_base_interface(), f) == - rclcpp::executor::FutureReturnCode::SUCCESS) + if ( + spin_node_until_future_complete( + *executor_, node_base_interface_, f, + timeout) == rclcpp::FutureReturnCode::SUCCESS) { return f.get(); } diff --git a/rclcpp/src/rclcpp/parameter_event_handler.cpp b/rclcpp/src/rclcpp/parameter_event_handler.cpp new file mode 100644 index 0000000000..b1b36b663e --- /dev/null +++ b/rclcpp/src/rclcpp/parameter_event_handler.cpp @@ -0,0 +1,212 @@ +// Copyright 2019 Intel Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT 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 "rclcpp/parameter_event_handler.hpp" +#include "rcpputils/join.hpp" + +namespace rclcpp +{ + +ParameterEventCallbackHandle::SharedPtr +ParameterEventHandler::add_parameter_event_callback( + ParameterEventCallbackType callback) +{ + std::lock_guard lock(callbacks_->mutex_); + auto handle = std::make_shared(); + handle->callback = callback; + callbacks_->event_callbacks_.emplace_front(handle); + + return handle; +} + +void +ParameterEventHandler::remove_parameter_event_callback( + ParameterEventCallbackHandle::SharedPtr callback_handle) +{ + std::lock_guard lock(callbacks_->mutex_); + auto it = std::find_if( + callbacks_->event_callbacks_.begin(), + callbacks_->event_callbacks_.end(), + [callback_handle](const auto & weak_handle) { + return callback_handle.get() == weak_handle.lock().get(); + }); + if (it != callbacks_->event_callbacks_.end()) { + callbacks_->event_callbacks_.erase(it); + } else { + throw std::runtime_error("Callback doesn't exist"); + } +} + +ParameterCallbackHandle::SharedPtr +ParameterEventHandler::add_parameter_callback( + const std::string & parameter_name, + ParameterCallbackType callback, + const std::string & node_name) +{ + std::lock_guard lock(callbacks_->mutex_); + auto full_node_name = resolve_path(node_name); + + auto handle = std::make_shared(); + handle->callback = callback; + handle->parameter_name = parameter_name; + handle->node_name = full_node_name; + // the last callback registered is executed first. + callbacks_->parameter_callbacks_[{parameter_name, full_node_name}].emplace_front(handle); + + return handle; +} + +void +ParameterEventHandler::remove_parameter_callback( + ParameterCallbackHandle::SharedPtr callback_handle) +{ + std::lock_guard lock(callbacks_->mutex_); + auto handle = callback_handle.get(); + auto & container = callbacks_->parameter_callbacks_[{handle->parameter_name, handle->node_name}]; + auto it = std::find_if( + container.begin(), + container.end(), + [handle](const auto & weak_handle) { + return handle == weak_handle.lock().get(); + }); + if (it != container.end()) { + container.erase(it); + if (container.empty()) { + callbacks_->parameter_callbacks_.erase({handle->parameter_name, handle->node_name}); + } + } else { + throw std::runtime_error("Callback doesn't exist"); + } +} + +bool +ParameterEventHandler::get_parameter_from_event( + const rcl_interfaces::msg::ParameterEvent & event, + rclcpp::Parameter & parameter, + const std::string & parameter_name, + const std::string & node_name) +{ + if (event.node != node_name) { + return false; + } + + for (auto & new_parameter : event.new_parameters) { + if (new_parameter.name == parameter_name) { + parameter = rclcpp::Parameter::from_parameter_msg(new_parameter); + return true; + } + } + + for (auto & changed_parameter : event.changed_parameters) { + if (changed_parameter.name == parameter_name) { + parameter = rclcpp::Parameter::from_parameter_msg(changed_parameter); + return true; + } + } + + return false; +} + +rclcpp::Parameter +ParameterEventHandler::get_parameter_from_event( + const rcl_interfaces::msg::ParameterEvent & event, + const std::string & parameter_name, + const std::string & node_name) +{ + rclcpp::Parameter p; + if (!get_parameter_from_event(event, p, parameter_name, node_name)) { + if (event.node == node_name) { + return rclcpp::Parameter(parameter_name, rclcpp::PARAMETER_NOT_SET); + } else { + throw std::runtime_error( + "The node name '" + node_name + "' of parameter '" + parameter_name + + +"' doesn't match the node name '" + event.node + "' in parameter event"); + } + } + return p; +} + +std::vector +ParameterEventHandler::get_parameters_from_event( + const rcl_interfaces::msg::ParameterEvent & event) +{ + std::vector params; + + for (auto & new_parameter : event.new_parameters) { + params.push_back(rclcpp::Parameter::from_parameter_msg(new_parameter)); + } + + for (auto & changed_parameter : event.changed_parameters) { + params.push_back(rclcpp::Parameter::from_parameter_msg(changed_parameter)); + } + + return params; +} + +void +ParameterEventHandler::Callbacks::event_callback(const rcl_interfaces::msg::ParameterEvent & event) +{ + std::lock_guard lock(mutex_); + + for (auto it = parameter_callbacks_.begin(); it != parameter_callbacks_.end(); ++it) { + rclcpp::Parameter p; + if (get_parameter_from_event(event, p, it->first.first, it->first.second)) { + for (auto cb = it->second.begin(); cb != it->second.end(); ++cb) { + auto shared_handle = cb->lock(); + if (nullptr != shared_handle) { + shared_handle->callback(p); + } else { + cb = it->second.erase(cb); + } + } + } + } + + for (auto event_cb = event_callbacks_.begin(); event_cb != event_callbacks_.end(); ++event_cb) { + auto shared_event_handle = event_cb->lock(); + if (nullptr != shared_event_handle) { + shared_event_handle->callback(event); + } else { + event_cb = event_callbacks_.erase(event_cb); + } + } +} + +std::string +ParameterEventHandler::resolve_path(const std::string & path) +{ + std::string full_path; + + if (path == "") { + full_path = node_base_->get_fully_qualified_name(); + } else { + full_path = path; + if (*path.begin() != '/') { + auto ns = node_base_->get_namespace(); + const std::vector paths{ns, path}; + full_path = (ns == std::string("/")) ? ns + path : rcpputils::join(paths, "/"); + } + } + + return full_path; +} + +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/parameter_events_filter.cpp b/rclcpp/src/rclcpp/parameter_events_filter.cpp index 36ef708c35..be9882c85b 100644 --- a/rclcpp/src/rclcpp/parameter_events_filter.cpp +++ b/rclcpp/src/rclcpp/parameter_events_filter.cpp @@ -14,6 +14,7 @@ #include "rclcpp/parameter_events_filter.hpp" +#include #include #include @@ -22,7 +23,7 @@ using EventType = rclcpp::ParameterEventsFilter::EventType; using EventPair = rclcpp::ParameterEventsFilter::EventPair; ParameterEventsFilter::ParameterEventsFilter( - rcl_interfaces::msg::ParameterEvent::SharedPtr event, + std::shared_ptr event, const std::vector & names, const std::vector & types) : event_(event) diff --git a/rclcpp/src/rclcpp/parameter_map.cpp b/rclcpp/src/rclcpp/parameter_map.cpp new file mode 100644 index 0000000000..5ed67daae6 --- /dev/null +++ b/rclcpp/src/rclcpp/parameter_map.cpp @@ -0,0 +1,178 @@ +// 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 "rcpputils/find_and_replace.hpp" +#include "rcpputils/scope_exit.hpp" +#include "rclcpp/parameter_map.hpp" + +using rclcpp::exceptions::InvalidParametersException; +using rclcpp::exceptions::InvalidParameterValueException; +using rclcpp::ParameterMap; +using rclcpp::ParameterValue; + +static bool is_node_name_matched(const std::string & node_name, const char * node_fqn) +{ + // Update the regular expression ["/*" -> "(/\\w+)" and "/**" -> "(/\\w+)*"] + std::string regex = rcpputils::find_and_replace(node_name, "/*", "(/\\w+)"); + return std::regex_match(node_fqn, std::regex(regex)); +} + +ParameterMap +rclcpp::parameter_map_from(const rcl_params_t * const c_params, const char * node_fqn) +{ + if (NULL == c_params) { + throw InvalidParametersException("parameters struct is NULL"); + } else if (NULL == c_params->node_names) { + throw InvalidParametersException("node names array is NULL"); + } else if (NULL == c_params->params) { + throw InvalidParametersException("node params array is NULL"); + } + + // Convert c structs into a list of parameters to set + ParameterMap parameters; + for (size_t n = 0; n < c_params->num_nodes; ++n) { + const char * c_node_name = c_params->node_names[n]; + if (NULL == c_node_name) { + throw InvalidParametersException("Node name at index " + std::to_string(n) + " is NULL"); + } + + /// make sure there is a leading slash on the fully qualified node name + std::string node_name("/"); + if ('/' != c_node_name[0]) { + node_name += c_node_name; + } else { + node_name = c_node_name; + } + + if (node_fqn) { + if (!is_node_name_matched(node_name, node_fqn)) { + // No need to parse the items because the user just care about node_fqn + continue; + } + + node_name = node_fqn; + } + + const rcl_node_params_t * const c_params_node = &(c_params->params[n]); + + std::vector & params_node = parameters[node_name]; + params_node.reserve(c_params_node->num_params); + + for (size_t p = 0; p < c_params_node->num_params; ++p) { + const char * const c_param_name = c_params_node->parameter_names[p]; + if (NULL == c_param_name) { + std::string message( + "At node " + std::to_string(n) + " parameter " + std::to_string(p) + " name is NULL"); + throw InvalidParametersException(message); + } + const rcl_variant_t * const c_param_value = &(c_params_node->parameter_values[p]); + params_node.emplace_back(c_param_name, parameter_value_from(c_param_value)); + } + } + + return parameters; +} + +ParameterValue +rclcpp::parameter_value_from(const rcl_variant_t * const c_param_value) +{ + if (NULL == c_param_value) { + throw InvalidParameterValueException("Passed argument is NULL"); + } + if (c_param_value->bool_value) { + return ParameterValue(*(c_param_value->bool_value)); + } else if (c_param_value->integer_value) { + return ParameterValue(*(c_param_value->integer_value)); + } else if (c_param_value->double_value) { + return ParameterValue(*(c_param_value->double_value)); + } else if (c_param_value->string_value) { + return ParameterValue(std::string(c_param_value->string_value)); + } else if (c_param_value->byte_array_value) { + const rcl_byte_array_t * const byte_array = c_param_value->byte_array_value; + std::vector bytes; + bytes.reserve(byte_array->size); + for (size_t v = 0; v < byte_array->size; ++v) { + bytes.push_back(byte_array->values[v]); + } + return ParameterValue(bytes); + } else if (c_param_value->bool_array_value) { + const rcl_bool_array_t * const bool_array = c_param_value->bool_array_value; + std::vector bools; + bools.reserve(bool_array->size); + for (size_t v = 0; v < bool_array->size; ++v) { + bools.push_back(bool_array->values[v]); + } + return ParameterValue(bools); + } else if (c_param_value->integer_array_value) { + const rcl_int64_array_t * const int_array = c_param_value->integer_array_value; + std::vector integers; + integers.reserve(int_array->size); + for (size_t v = 0; v < int_array->size; ++v) { + integers.push_back(int_array->values[v]); + } + return ParameterValue(integers); + } else if (c_param_value->double_array_value) { + const rcl_double_array_t * const double_array = c_param_value->double_array_value; + std::vector doubles; + doubles.reserve(double_array->size); + for (size_t v = 0; v < double_array->size; ++v) { + doubles.push_back(double_array->values[v]); + } + return ParameterValue(doubles); + } else if (c_param_value->string_array_value) { + const rcutils_string_array_t * const string_array = c_param_value->string_array_value; + std::vector strings; + strings.reserve(string_array->size); + for (size_t v = 0; v < string_array->size; ++v) { + strings.emplace_back(string_array->data[v]); + } + return ParameterValue(strings); + } + + throw InvalidParameterValueException("No parameter value set"); +} + +ParameterMap +rclcpp::parameter_map_from_yaml_file(const std::string & yaml_filename, const char * node_fqn) +{ + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + rcl_params_t * rcl_parameters = rcl_yaml_node_struct_init(allocator); + RCPPUTILS_SCOPE_EXIT(rcl_yaml_node_struct_fini(rcl_parameters); ); + const char * path = yaml_filename.c_str(); + if (!rcl_parse_yaml_file(path, rcl_parameters)) { + rclcpp::exceptions::throw_from_rcl_error(RCL_RET_ERROR); + } + + return rclcpp::parameter_map_from(rcl_parameters, node_fqn); +} + +std::vector +rclcpp::parameters_from_map(const ParameterMap & parameter_map, const char * node_fqn) +{ + std::vector parameters; + std::string node_name_old; + for (auto & [node_name, node_parameters] : parameter_map) { + if (node_fqn && !is_node_name_matched(node_name, node_fqn)) { + // No need to parse the items because the user just care about node_fqn + continue; + } + parameters.insert(parameters.end(), node_parameters.begin(), node_parameters.end()); + } + + return parameters; +} diff --git a/rclcpp/src/rclcpp/parameter_service.cpp b/rclcpp/src/rclcpp/parameter_service.cpp index 86ca433ffd..0923798339 100644 --- a/rclcpp/src/rclcpp/parameter_service.cpp +++ b/rclcpp/src/rclcpp/parameter_service.cpp @@ -19,125 +19,141 @@ #include #include +#include "rclcpp/logging.hpp" + #include "./parameter_service_names.hpp" using rclcpp::ParameterService; ParameterService::ParameterService( - const rclcpp::Node::SharedPtr node, - const rmw_qos_profile_t & qos_profile) -: node_(node) + const std::shared_ptr node_base, + const std::shared_ptr node_services, + rclcpp::node_interfaces::NodeParametersInterface * node_params, + const rclcpp::QoS & qos_profile) { - std::weak_ptr captured_node = node_; - get_parameters_service_ = node_->create_service( - std::string(node_->get_name()) + "/" + parameter_service_names::get_parameters, - [captured_node]( + const std::string node_name = node_base->get_name(); + + get_parameters_service_ = create_service( + node_base, node_services, + node_name + "/" + parameter_service_names::get_parameters, + [node_params]( const std::shared_ptr, const std::shared_ptr request, std::shared_ptr response) { - auto node = captured_node.lock(); - if (!node) { - return; - } - auto values = node->get_parameters(request->names); - for (auto & pvariant : values) { - response->values.push_back(pvariant.get_parameter_value()); + try { + auto parameters = node_params->get_parameters(request->names); + for (const auto & param : parameters) { + response->values.push_back(param.get_value_message()); + } + } catch (const rclcpp::exceptions::ParameterNotDeclaredException & ex) { + RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Failed to get parameters: %s", ex.what()); + } catch (const rclcpp::exceptions::ParameterUninitializedException & ex) { + RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Failed to get parameters: %s", ex.what()); } }, - qos_profile); + qos_profile, nullptr); - get_parameter_types_service_ = node_->create_service( - std::string(node_->get_name()) + "/" + parameter_service_names::get_parameter_types, - [captured_node]( + get_parameter_types_service_ = create_service( + node_base, node_services, + node_name + "/" + parameter_service_names::get_parameter_types, + [node_params]( const std::shared_ptr, const std::shared_ptr request, std::shared_ptr response) { - auto node = captured_node.lock(); - if (!node) { - return; + try { + auto types = node_params->get_parameter_types(request->names); + std::transform( + types.cbegin(), types.cend(), + std::back_inserter(response->types), [](const uint8_t & type) { + return static_cast(type); + }); + } catch (const rclcpp::exceptions::ParameterNotDeclaredException & ex) { + RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Failed to get parameter types: %s", ex.what()); } - auto types = node->get_parameter_types(request->names); - std::transform(types.cbegin(), types.cend(), - std::back_inserter(response->types), [](const uint8_t & type) { - return static_cast(type); - }); }, - qos_profile); + qos_profile, nullptr); - set_parameters_service_ = node_->create_service( - std::string(node_->get_name()) + "/" + parameter_service_names::set_parameters, - [captured_node]( + set_parameters_service_ = create_service( + node_base, node_services, + node_name + "/" + parameter_service_names::set_parameters, + [node_params]( const std::shared_ptr, const std::shared_ptr request, std::shared_ptr response) { - auto node = captured_node.lock(); - if (!node) { - return; - } - std::vector pvariants; + // Set parameters one-by-one, since there's no way to return a partial result if + // set_parameters() fails. + auto result = rcl_interfaces::msg::SetParametersResult(); for (auto & p : request->parameters) { - pvariants.push_back(rclcpp::parameter::ParameterVariant::from_parameter(p)); + try { + result = node_params->set_parameters_atomically( + {rclcpp::Parameter::from_parameter_msg(p)}); + } catch (const rclcpp::exceptions::ParameterNotDeclaredException & ex) { + RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Failed to set parameter: %s", ex.what()); + result.successful = false; + result.reason = ex.what(); + } + response->results.push_back(result); } - auto results = node->set_parameters(pvariants); - response->results = results; }, - qos_profile); + qos_profile, nullptr); - set_parameters_atomically_service_ = - node_->create_service( - std::string(node_->get_name()) + "/" + parameter_service_names::set_parameters_atomically, - [captured_node]( + set_parameters_atomically_service_ = create_service( + node_base, node_services, + node_name + "/" + parameter_service_names::set_parameters_atomically, + [node_params]( const std::shared_ptr, const std::shared_ptr request, std::shared_ptr response) { - auto node = captured_node.lock(); - if (!node) { - return; + std::vector pvariants; + std::transform( + request->parameters.cbegin(), request->parameters.cend(), + std::back_inserter(pvariants), + [](const rcl_interfaces::msg::Parameter & p) { + return rclcpp::Parameter::from_parameter_msg(p); + }); + try { + auto result = node_params->set_parameters_atomically(pvariants); + response->result = result; + } catch (const rclcpp::exceptions::ParameterNotDeclaredException & ex) { + RCLCPP_DEBUG( + rclcpp::get_logger("rclcpp"), "Failed to set parameters atomically: %s", ex.what()); + response->result.successful = false; + response->result.reason = "One or more parameters were not declared before setting"; } - std::vector pvariants; - std::transform(request->parameters.cbegin(), request->parameters.cend(), - std::back_inserter(pvariants), - [](const rcl_interfaces::msg::Parameter & p) { - return rclcpp::parameter::ParameterVariant::from_parameter(p); - }); - auto result = node->set_parameters_atomically(pvariants); - response->result = result; }, - qos_profile); + qos_profile, nullptr); - describe_parameters_service_ = node_->create_service( - std::string(node_->get_name()) + "/" + parameter_service_names::describe_parameters, - [captured_node]( + describe_parameters_service_ = create_service( + node_base, node_services, + node_name + "/" + parameter_service_names::describe_parameters, + [node_params]( const std::shared_ptr, const std::shared_ptr request, std::shared_ptr response) { - auto node = captured_node.lock(); - if (!node) { - return; + try { + auto descriptors = node_params->describe_parameters(request->names); + response->descriptors = descriptors; + } catch (const rclcpp::exceptions::ParameterNotDeclaredException & ex) { + RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Failed to describe parameters: %s", ex.what()); } - auto descriptors = node->describe_parameters(request->names); - response->descriptors = descriptors; }, - qos_profile); + qos_profile, nullptr); - list_parameters_service_ = node_->create_service( - std::string(node_->get_name()) + "/" + parameter_service_names::list_parameters, - [captured_node]( + list_parameters_service_ = create_service( + node_base, node_services, + node_name + "/" + parameter_service_names::list_parameters, + [node_params]( const std::shared_ptr, const std::shared_ptr request, std::shared_ptr response) { - auto node = captured_node.lock(); - if (!node) { - return; - } - auto result = node->list_parameters(request->prefixes, request->depth); + auto result = node_params->list_parameters(request->prefixes, request->depth); response->result = result; }, - qos_profile); + qos_profile, nullptr); } diff --git a/rclcpp/src/rclcpp/parameter_value.cpp b/rclcpp/src/rclcpp/parameter_value.cpp new file mode 100644 index 0000000000..ee46e77673 --- /dev/null +++ b/rclcpp/src/rclcpp/parameter_value.cpp @@ -0,0 +1,240 @@ +// 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 "rclcpp/parameter_value.hpp" + +#include +#include + +using rclcpp::ParameterType; +using rclcpp::ParameterValue; + +std::string +rclcpp::to_string(const ParameterType type) +{ + switch (type) { + case ParameterType::PARAMETER_NOT_SET: + return "not set"; + case ParameterType::PARAMETER_BOOL: + return "bool"; + case ParameterType::PARAMETER_INTEGER: + return "integer"; + case ParameterType::PARAMETER_DOUBLE: + return "double"; + case ParameterType::PARAMETER_STRING: + return "string"; + case ParameterType::PARAMETER_BYTE_ARRAY: + return "byte_array"; + case ParameterType::PARAMETER_BOOL_ARRAY: + return "bool_array"; + case ParameterType::PARAMETER_INTEGER_ARRAY: + return "integer_array"; + case ParameterType::PARAMETER_DOUBLE_ARRAY: + return "double_array"; + case ParameterType::PARAMETER_STRING_ARRAY: + return "string_array"; + default: + return "unknown type"; + } +} + +std::ostream & +rclcpp::operator<<(std::ostream & os, const ParameterType type) +{ + os << rclcpp::to_string(type); + return os; +} + +template +std::string +array_to_string( + const std::vector & array, + const std::ios::fmtflags format_flags = std::ios::dec) +{ + std::stringstream type_array; + bool first_item = true; + type_array << "["; + type_array.setf(format_flags, std::ios_base::basefield | std::ios::boolalpha); + type_array << std::showbase; + for (const ValType & value : array) { + if (!first_item) { + type_array << ", "; + } else { + first_item = false; + } + type_array << static_cast(value); + } + type_array << "]"; + return type_array.str(); +} + +std::string +rclcpp::to_string(const ParameterValue & value) +{ + switch (value.get_type()) { + case ParameterType::PARAMETER_NOT_SET: + return "not set"; + case ParameterType::PARAMETER_BOOL: + return value.get() ? "true" : "false"; + case ParameterType::PARAMETER_INTEGER: + return std::to_string(value.get()); + case ParameterType::PARAMETER_DOUBLE: + return std::to_string(value.get()); + case ParameterType::PARAMETER_STRING: + return value.get(); + case ParameterType::PARAMETER_BYTE_ARRAY: + return array_to_string(value.get>(), std::ios::hex); + case ParameterType::PARAMETER_BOOL_ARRAY: + return array_to_string(value.get>(), std::ios::boolalpha); + case ParameterType::PARAMETER_INTEGER_ARRAY: + return array_to_string(value.get>()); + case ParameterType::PARAMETER_DOUBLE_ARRAY: + return array_to_string(value.get>()); + case ParameterType::PARAMETER_STRING_ARRAY: + return array_to_string(value.get>()); + default: + return "unknown type"; + } +} + +ParameterValue::ParameterValue() +{ + value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET; +} + +ParameterValue::ParameterValue(const rcl_interfaces::msg::ParameterValue & value) +{ + value_ = value; + switch (value.type) { + case PARAMETER_BOOL: + case PARAMETER_INTEGER: + case PARAMETER_DOUBLE: + case PARAMETER_STRING: + case PARAMETER_BYTE_ARRAY: + case PARAMETER_BOOL_ARRAY: + case PARAMETER_INTEGER_ARRAY: + case PARAMETER_DOUBLE_ARRAY: + case PARAMETER_STRING_ARRAY: + case PARAMETER_NOT_SET: + break; + default: + throw rclcpp::exceptions::UnknownTypeError(std::to_string(value.type)); + } +} + +ParameterValue::ParameterValue(const bool bool_value) +{ + value_.bool_value = bool_value; + value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; +} + +ParameterValue::ParameterValue(const int int_value) +{ + value_.integer_value = int_value; + value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; +} + +ParameterValue::ParameterValue(const int64_t int_value) +{ + value_.integer_value = int_value; + value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; +} + +ParameterValue::ParameterValue(const float double_value) +{ + value_.double_value = static_cast(double_value); + value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; +} + +ParameterValue::ParameterValue(const double double_value) +{ + value_.double_value = double_value; + value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; +} + +ParameterValue::ParameterValue(const std::string & string_value) +{ + value_.string_value = string_value; + value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; +} + +ParameterValue::ParameterValue(const char * string_value) +: ParameterValue(std::string(string_value)) +{} + +ParameterValue::ParameterValue(const std::vector & byte_array_value) +{ + value_.byte_array_value = byte_array_value; + value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_BYTE_ARRAY; +} + +ParameterValue::ParameterValue(const std::vector & bool_array_value) +{ + value_.bool_array_value = bool_array_value; + value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL_ARRAY; +} + +ParameterValue::ParameterValue(const std::vector & int_array_value) +{ + value_.integer_array_value.assign(int_array_value.cbegin(), int_array_value.cend()); + value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY; +} + +ParameterValue::ParameterValue(const std::vector & int_array_value) +{ + value_.integer_array_value = int_array_value; + value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY; +} + +ParameterValue::ParameterValue(const std::vector & float_array_value) +{ + value_.double_array_value.assign(float_array_value.cbegin(), float_array_value.cend()); + value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY; +} + +ParameterValue::ParameterValue(const std::vector & double_array_value) +{ + value_.double_array_value = double_array_value; + value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY; +} + +ParameterValue::ParameterValue(const std::vector & string_array_value) +{ + value_.string_array_value = string_array_value; + value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY; +} + +ParameterType +ParameterValue::get_type() const +{ + return static_cast(value_.type); +} + +rcl_interfaces::msg::ParameterValue +ParameterValue::to_value_msg() const +{ + return value_; +} + +bool +ParameterValue::operator==(const ParameterValue & rhs) const +{ + return this->value_ == rhs.value_; +} + +bool +ParameterValue::operator!=(const ParameterValue & rhs) const +{ + return this->value_ != rhs.value_; +} diff --git a/rclcpp/src/rclcpp/publisher.cpp b/rclcpp/src/rclcpp/publisher.cpp deleted file mode 100644 index 406f9273a8..0000000000 --- a/rclcpp/src/rclcpp/publisher.cpp +++ /dev/null @@ -1,218 +0,0 @@ -// 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 "rclcpp/publisher.hpp" - -#include -#include - -#include -#include -#include -#include -#include - -#include "rcl_interfaces/msg/intra_process_message.hpp" -#include "rmw/impl/cpp/demangle.hpp" - -#include "rclcpp/allocator/allocator_common.hpp" -#include "rclcpp/allocator/allocator_deleter.hpp" -#include "rclcpp/exceptions.hpp" -#include "rclcpp/macros.hpp" -#include "rclcpp/node.hpp" -#include "rclcpp/expand_topic_or_service_name.hpp" - -using rclcpp::PublisherBase; - -PublisherBase::PublisherBase( - rclcpp::node_interfaces::NodeBaseInterface * node_base, - const std::string & topic, - const rosidl_message_type_support_t & type_support, - const rcl_publisher_options_t & publisher_options) -: rcl_node_handle_(node_base->get_shared_rcl_node_handle()), - intra_process_publisher_id_(0), store_intra_process_message_(nullptr) -{ - rcl_ret_t ret = rcl_publisher_init( - &publisher_handle_, - rcl_node_handle_.get(), - &type_support, - topic.c_str(), - &publisher_options); - if (ret != RCL_RET_OK) { - if (ret == RCL_RET_TOPIC_NAME_INVALID) { - auto rcl_node_handle = rcl_node_handle_.get(); - // this will throw on any validation problem - rcl_reset_error(); - expand_topic_or_service_name( - topic, - rcl_node_get_name(rcl_node_handle), - rcl_node_get_namespace(rcl_node_handle)); - } - - rclcpp::exceptions::throw_from_rcl_error(ret, "could not create publisher"); - } - // Life time of this object is tied to the publisher handle. - rmw_publisher_t * publisher_rmw_handle = rcl_publisher_get_rmw_handle(&publisher_handle_); - if (!publisher_rmw_handle) { - auto msg = std::string("failed to get rmw handle: ") + rcl_get_error_string_safe(); - rcl_reset_error(); - throw std::runtime_error(msg); - } - if (rmw_get_gid_for_publisher(publisher_rmw_handle, &rmw_gid_) != RMW_RET_OK) { - auto msg = std::string("failed to get publisher gid: ") + rmw_get_error_string_safe(); - rmw_reset_error(); - throw std::runtime_error(msg); - } -} - -PublisherBase::~PublisherBase() -{ - if (rcl_publisher_fini(&intra_process_publisher_handle_, rcl_node_handle_.get()) != RCL_RET_OK) { - fprintf( - stderr, - "Error in destruction of intra process rcl publisher handle: %s\n", - rcl_get_error_string_safe()); - rcl_reset_error(); - } - - if (rcl_publisher_fini(&publisher_handle_, rcl_node_handle_.get()) != RCL_RET_OK) { - fprintf( - stderr, - "Error in destruction of rcl publisher handle: %s\n", - rcl_get_error_string_safe()); - rcl_reset_error(); - } -} - -const char * -PublisherBase::get_topic_name() const -{ - return rcl_publisher_get_topic_name(&publisher_handle_); -} - -size_t -PublisherBase::get_queue_size() const -{ - const rcl_publisher_options_t * publisher_options = rcl_publisher_get_options(&publisher_handle_); - if (!publisher_options) { - auto msg = std::string("failed to get publisher options: ") + rcl_get_error_string_safe(); - rcl_reset_error(); - throw std::runtime_error(msg); - } - return publisher_options->qos.depth; -} - -const rmw_gid_t & -PublisherBase::get_gid() const -{ - return rmw_gid_; -} - -const rmw_gid_t & -PublisherBase::get_intra_process_gid() const -{ - return intra_process_rmw_gid_; -} - -rcl_publisher_t * -PublisherBase::get_publisher_handle() -{ - return &publisher_handle_; -} - -const rcl_publisher_t * -PublisherBase::get_publisher_handle() const -{ - return &publisher_handle_; -} - -bool -PublisherBase::operator==(const rmw_gid_t & gid) const -{ - return *this == &gid; -} - -bool -PublisherBase::operator==(const rmw_gid_t * gid) const -{ - bool result = false; - auto ret = rmw_compare_gids_equal(gid, &this->get_gid(), &result); - if (ret != RMW_RET_OK) { - auto msg = std::string("failed to compare gids: ") + rmw_get_error_string_safe(); - rmw_reset_error(); - throw std::runtime_error(msg); - } - if (!result) { - ret = rmw_compare_gids_equal(gid, &this->get_intra_process_gid(), &result); - if (ret != RMW_RET_OK) { - auto msg = std::string("failed to compare gids: ") + rmw_get_error_string_safe(); - rmw_reset_error(); - throw std::runtime_error(msg); - } - } - return result; -} - -void -PublisherBase::setup_intra_process( - uint64_t intra_process_publisher_id, - StoreMessageCallbackT callback, - const rcl_publisher_options_t & intra_process_options) -{ - const char * topic_name = this->get_topic_name(); - if (!topic_name) { - throw std::runtime_error("failed to get topic name"); - } - - auto intra_process_topic_name = std::string(topic_name) + "/_intra"; - - rcl_ret_t ret = rcl_publisher_init( - &intra_process_publisher_handle_, - rcl_node_handle_.get(), - rclcpp::type_support::get_intra_process_message_msg_type_support(), - intra_process_topic_name.c_str(), - &intra_process_options); - if (ret != RCL_RET_OK) { - if (ret == RCL_RET_TOPIC_NAME_INVALID) { - auto rcl_node_handle = rcl_node_handle_.get(); - // this will throw on any validation problem - rcl_reset_error(); - expand_topic_or_service_name( - intra_process_topic_name, - rcl_node_get_name(rcl_node_handle), - rcl_node_get_namespace(rcl_node_handle)); - } - - rclcpp::exceptions::throw_from_rcl_error(ret, "could not create intra process publisher"); - } - - intra_process_publisher_id_ = intra_process_publisher_id; - store_intra_process_message_ = callback; - // Life time of this object is tied to the publisher handle. - rmw_publisher_t * publisher_rmw_handle = rcl_publisher_get_rmw_handle( - &intra_process_publisher_handle_); - if (publisher_rmw_handle == nullptr) { - auto msg = std::string("Failed to get rmw publisher handle") + rcl_get_error_string_safe(); - rcl_reset_error(); - throw std::runtime_error(msg); - } - auto rmw_ret = rmw_get_gid_for_publisher( - publisher_rmw_handle, &intra_process_rmw_gid_); - if (rmw_ret != RMW_RET_OK) { - auto msg = - std::string("failed to create intra process publisher gid: ") + rmw_get_error_string_safe(); - rmw_reset_error(); - throw std::runtime_error(msg); - } -} diff --git a/rclcpp/src/rclcpp/publisher_base.cpp b/rclcpp/src/rclcpp/publisher_base.cpp new file mode 100644 index 0000000000..0dc9b01a7d --- /dev/null +++ b/rclcpp/src/rclcpp/publisher_base.cpp @@ -0,0 +1,432 @@ +// 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 "rclcpp/publisher_base.hpp" + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "rcutils/logging_macros.h" +#include "rmw/impl/cpp/demangle.hpp" + +#include "rclcpp/allocator/allocator_common.hpp" +#include "rclcpp/allocator/allocator_deleter.hpp" +#include "rclcpp/exceptions.hpp" +#include "rclcpp/expand_topic_or_service_name.hpp" +#include "rclcpp/experimental/intra_process_manager.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/network_flow_endpoint.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/event_handler.hpp" + +using rclcpp::PublisherBase; + +PublisherBase::PublisherBase( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + const std::string & topic, + const rosidl_message_type_support_t & type_support, + const rcl_publisher_options_t & publisher_options, + const PublisherEventCallbacks & event_callbacks, + bool use_default_callbacks) +: rcl_node_handle_(node_base->get_shared_rcl_node_handle()), + intra_process_is_enabled_(false), + intra_process_publisher_id_(0), + type_support_(type_support), + event_callbacks_(event_callbacks) +{ + auto custom_deleter = [node_handle = this->rcl_node_handle_](rcl_publisher_t * rcl_pub) + { + if (rcl_publisher_fini(rcl_pub, node_handle.get()) != RCL_RET_OK) { + RCLCPP_ERROR( + rclcpp::get_node_logger(node_handle.get()).get_child("rclcpp"), + "Error in destruction of rcl publisher handle: %s", + rcl_get_error_string().str); + rcl_reset_error(); + } + delete rcl_pub; + }; + + publisher_handle_ = std::shared_ptr( + new rcl_publisher_t, custom_deleter); + *publisher_handle_.get() = rcl_get_zero_initialized_publisher(); + + rcl_ret_t ret = rcl_publisher_init( + publisher_handle_.get(), + rcl_node_handle_.get(), + &type_support, + topic.c_str(), + &publisher_options); + if (ret != RCL_RET_OK) { + if (ret == RCL_RET_TOPIC_NAME_INVALID) { + auto rcl_node_handle = rcl_node_handle_.get(); + // this will throw on any validation problem + rcl_reset_error(); + expand_topic_or_service_name( + topic, + rcl_node_get_name(rcl_node_handle), + rcl_node_get_namespace(rcl_node_handle)); + } + + rclcpp::exceptions::throw_from_rcl_error(ret, "could not create publisher"); + } + // Life time of this object is tied to the publisher handle. + rmw_publisher_t * publisher_rmw_handle = rcl_publisher_get_rmw_handle(publisher_handle_.get()); + if (!publisher_rmw_handle) { + auto msg = std::string("failed to get rmw handle: ") + rcl_get_error_string().str; + rcl_reset_error(); + throw std::runtime_error(msg); + } + if (rmw_get_gid_for_publisher(publisher_rmw_handle, &rmw_gid_) != RMW_RET_OK) { + auto msg = std::string("failed to get publisher gid: ") + rmw_get_error_string().str; + rmw_reset_error(); + throw std::runtime_error(msg); + } + + bind_event_callbacks(event_callbacks_, use_default_callbacks); +} + +PublisherBase::~PublisherBase() +{ + // must fini the events before fini-ing the publisher + event_handlers_.clear(); + + auto ipm = weak_ipm_.lock(); + + if (!intra_process_is_enabled_) { + return; + } + if (!ipm) { + // TODO(ivanpauno): should this raise an error? + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Intra process manager died before a publisher."); + return; + } + ipm->remove_publisher(intra_process_publisher_id_); +} + +const char * +PublisherBase::get_topic_name() const +{ + return rcl_publisher_get_topic_name(publisher_handle_.get()); +} + +void +PublisherBase::bind_event_callbacks( + const PublisherEventCallbacks & event_callbacks, bool use_default_callbacks) +{ + try { + if (event_callbacks.deadline_callback) { + this->add_event_handler( + event_callbacks.deadline_callback, + RCL_PUBLISHER_OFFERED_DEADLINE_MISSED); + } + } catch (const UnsupportedEventTypeException & /*exc*/) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Failed to add event handler for deadline; not supported"); + } + + try { + if (event_callbacks.liveliness_callback) { + this->add_event_handler( + event_callbacks.liveliness_callback, + RCL_PUBLISHER_LIVELINESS_LOST); + } + } catch (const UnsupportedEventTypeException & /*exc*/) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Failed to add event handler for liveliness; not supported"); + } + + QOSOfferedIncompatibleQoSCallbackType incompatible_qos_cb; + if (event_callbacks.incompatible_qos_callback) { + incompatible_qos_cb = event_callbacks.incompatible_qos_callback; + } else if (use_default_callbacks) { + // Register default callback when not specified + incompatible_qos_cb = [this](QOSOfferedIncompatibleQoSInfo & info) { + this->default_incompatible_qos_callback(info); + }; + } + try { + if (incompatible_qos_cb) { + this->add_event_handler(incompatible_qos_cb, RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS); + } + } catch (const UnsupportedEventTypeException & /*exc*/) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Failed to add event handler for incompatible qos; not supported"); + } + + IncompatibleTypeCallbackType incompatible_type_cb; + if (event_callbacks.incompatible_type_callback) { + incompatible_type_cb = event_callbacks.incompatible_type_callback; + } else if (use_default_callbacks) { + // Register default callback when not specified + incompatible_type_cb = [this](IncompatibleTypeInfo & info) { + this->default_incompatible_type_callback(info); + }; + } + try { + if (incompatible_type_cb) { + this->add_event_handler(incompatible_type_cb, RCL_PUBLISHER_INCOMPATIBLE_TYPE); + } + } catch (UnsupportedEventTypeException & /*exc*/) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Failed to add event handler for incompatible type; not supported"); + } + + try { + if (event_callbacks.matched_callback) { + this->add_event_handler( + event_callbacks.matched_callback, + RCL_PUBLISHER_MATCHED); + } + } catch (const UnsupportedEventTypeException & /*exc*/) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Failed to add event handler for matched; not supported"); + } +} + +size_t +PublisherBase::get_queue_size() const +{ + const rcl_publisher_options_t * publisher_options = rcl_publisher_get_options( + publisher_handle_.get()); + if (!publisher_options) { + auto msg = std::string("failed to get publisher options: ") + rcl_get_error_string().str; + rcl_reset_error(); + throw std::runtime_error(msg); + } + return publisher_options->qos.depth; +} + +const rmw_gid_t & +PublisherBase::get_gid() const +{ + return rmw_gid_; +} + +std::shared_ptr +PublisherBase::get_publisher_handle() +{ + return publisher_handle_; +} + +std::shared_ptr +PublisherBase::get_publisher_handle() const +{ + return publisher_handle_; +} + +const +std::unordered_map> & +PublisherBase::get_event_handlers() const +{ + return event_handlers_; +} + +size_t +PublisherBase::get_subscription_count() const +{ + size_t inter_process_subscription_count = 0; + + rcl_ret_t status = rcl_publisher_get_subscription_count( + publisher_handle_.get(), + &inter_process_subscription_count); + + if (RCL_RET_PUBLISHER_INVALID == status) { + rcl_reset_error(); /* next call will reset error message if not context */ + if (rcl_publisher_is_valid_except_context(publisher_handle_.get())) { + rcl_context_t * context = rcl_publisher_get_context(publisher_handle_.get()); + if (nullptr != context && !rcl_context_is_valid(context)) { + /* publisher is invalid due to context being shutdown */ + return 0; + } + } + } + if (RCL_RET_OK != status) { + rclcpp::exceptions::throw_from_rcl_error(status, "failed to get get subscription count"); + } + return inter_process_subscription_count; +} + +size_t +PublisherBase::get_intra_process_subscription_count() const +{ + auto ipm = weak_ipm_.lock(); + if (!intra_process_is_enabled_) { + return 0; + } + if (!ipm) { + // TODO(ivanpauno): should this just return silently? Or maybe return with a warning? + // Same as wjwwood comment in publisher_factory create_shared_publish_callback. + throw std::runtime_error( + "intra process subscriber count called after " + "destruction of intra process manager"); + } + return ipm->get_subscription_count(intra_process_publisher_id_); +} + +bool +PublisherBase::is_durability_transient_local() const +{ + return rcl_publisher_get_actual_qos(publisher_handle_.get())->durability == + RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; +} + +rclcpp::QoS +PublisherBase::get_actual_qos() const +{ + const rmw_qos_profile_t * qos = rcl_publisher_get_actual_qos(publisher_handle_.get()); + if (!qos) { + auto msg = std::string("failed to get qos settings: ") + rcl_get_error_string().str; + rcl_reset_error(); + throw std::runtime_error(msg); + } + + return rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(*qos), *qos); +} + +bool +PublisherBase::assert_liveliness() const +{ + return RCL_RET_OK == rcl_publisher_assert_liveliness(publisher_handle_.get()); +} + +bool +PublisherBase::can_loan_messages() const +{ + return !intra_process_is_enabled_ && rcl_publisher_can_loan_messages(publisher_handle_.get()); +} + +bool +PublisherBase::operator==(const rmw_gid_t & gid) const +{ + return *this == &gid; +} + +bool +PublisherBase::operator==(const rmw_gid_t * gid) const +{ + bool result = false; + auto ret = rmw_compare_gids_equal(gid, &this->get_gid(), &result); + if (ret != RMW_RET_OK) { + auto msg = std::string("failed to compare gids: ") + rmw_get_error_string().str; + rmw_reset_error(); + throw std::runtime_error(msg); + } + return result; +} + +void +PublisherBase::setup_intra_process( + uint64_t intra_process_publisher_id, + IntraProcessManagerSharedPtr ipm) +{ + intra_process_publisher_id_ = intra_process_publisher_id; + weak_ipm_ = ipm; + intra_process_is_enabled_ = true; +} + +void +PublisherBase::default_incompatible_qos_callback( + rclcpp::QOSOfferedIncompatibleQoSInfo & event) const +{ + std::string policy_name = qos_policy_name_from_kind(event.last_policy_kind); + RCLCPP_WARN( + rclcpp::get_logger(rcl_node_get_logger_name(rcl_node_handle_.get())), + "New subscription discovered on topic '%s', requesting incompatible QoS. " + "No messages will be sent to it. " + "Last incompatible policy: %s", + get_topic_name(), + policy_name.c_str()); +} + +void +PublisherBase::default_incompatible_type_callback( + rclcpp::IncompatibleTypeInfo & event) const +{ + (void)event; + + RCLCPP_WARN( + rclcpp::get_logger(rcl_node_get_logger_name(rcl_node_handle_.get())), + "Incompatible type on topic '%s', no messages will be sent to it.", get_topic_name()); +} + +std::vector PublisherBase::get_network_flow_endpoints() const +{ + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + rcl_network_flow_endpoint_array_t network_flow_endpoint_array = + rcl_get_zero_initialized_network_flow_endpoint_array(); + rcl_ret_t ret = rcl_publisher_get_network_flow_endpoints( + publisher_handle_.get(), &allocator, &network_flow_endpoint_array); + if (RCL_RET_OK != ret) { + auto error_msg = std::string("error obtaining network flows of publisher: ") + + rcl_get_error_string().str; + rcl_reset_error(); + if (RCL_RET_OK != + rcl_network_flow_endpoint_array_fini(&network_flow_endpoint_array)) + { + error_msg += std::string(", also error cleaning up network flow array: ") + + rcl_get_error_string().str; + rcl_reset_error(); + } + rclcpp::exceptions::throw_from_rcl_error(ret, error_msg); + } + + std::vector network_flow_endpoint_vector; + for (size_t i = 0; i < network_flow_endpoint_array.size; ++i) { + network_flow_endpoint_vector.push_back( + rclcpp::NetworkFlowEndpoint( + network_flow_endpoint_array.network_flow_endpoint[i])); + } + + ret = rcl_network_flow_endpoint_array_fini(&network_flow_endpoint_array); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "error cleaning up network flow array"); + } + + return network_flow_endpoint_vector; +} + +size_t PublisherBase::lowest_available_ipm_capacity() const +{ + if (!intra_process_is_enabled_) { + return 0u; + } + + auto ipm = weak_ipm_.lock(); + + if (!ipm) { + // TODO(ivanpauno): should this raise an error? + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Intra process manager died for a publisher."); + return 0u; + } + + return ipm->lowest_available_capacity(intra_process_publisher_id_); +} diff --git a/rclcpp/src/rclcpp/qos.cpp b/rclcpp/src/rclcpp/qos.cpp new file mode 100644 index 0000000000..2453149aa4 --- /dev/null +++ b/rclcpp/src/rclcpp/qos.cpp @@ -0,0 +1,421 @@ +// 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 "rclcpp/qos.hpp" + +#include + +#include "rclcpp/logging.hpp" + +#include "rmw/error_handling.h" +#include "rmw/types.h" +#include "rmw/qos_profiles.h" + +namespace rclcpp +{ + +std::string qos_policy_name_from_kind(rmw_qos_policy_kind_t policy_kind) +{ + switch (policy_kind) { + case RMW_QOS_POLICY_DURABILITY: + return "DURABILITY_QOS_POLICY"; + case RMW_QOS_POLICY_DEADLINE: + return "DEADLINE_QOS_POLICY"; + case RMW_QOS_POLICY_LIVELINESS: + return "LIVELINESS_QOS_POLICY"; + case RMW_QOS_POLICY_RELIABILITY: + return "RELIABILITY_QOS_POLICY"; + case RMW_QOS_POLICY_HISTORY: + return "HISTORY_QOS_POLICY"; + case RMW_QOS_POLICY_LIFESPAN: + return "LIFESPAN_QOS_POLICY"; + default: + return "INVALID_QOS_POLICY"; + } +} + +QoSInitialization::QoSInitialization( + rmw_qos_history_policy_t history_policy_arg, size_t depth_arg, + bool print_depth_warning) +: history_policy(history_policy_arg), depth(depth_arg) +{ + if (history_policy == RMW_QOS_POLICY_HISTORY_KEEP_LAST && depth == 0 && print_depth_warning) { + RCLCPP_WARN_ONCE( + rclcpp::get_logger( + "rclcpp"), + "A zero depth with KEEP_LAST doesn't make sense; no data could be stored. " + "This will be interpreted as SYSTEM_DEFAULT"); + } +} + +QoSInitialization +QoSInitialization::from_rmw(const rmw_qos_profile_t & rmw_qos) +{ + switch (rmw_qos.history) { + case RMW_QOS_POLICY_HISTORY_KEEP_ALL: + return KeepAll(); + case RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT: + return KeepLast(rmw_qos.depth, false); + case RMW_QOS_POLICY_HISTORY_KEEP_LAST: + case RMW_QOS_POLICY_HISTORY_UNKNOWN: + default: + return KeepLast(rmw_qos.depth); + } +} + +KeepAll::KeepAll() +: QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_ALL, 0) +{} + +KeepLast::KeepLast(size_t depth, bool print_depth_warning) +: QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_LAST, depth, print_depth_warning) +{ +} + +QoS::QoS( + const QoSInitialization & qos_initialization, + const rmw_qos_profile_t & initial_profile) +: rmw_qos_profile_(initial_profile) +{ + rmw_qos_profile_.history = qos_initialization.history_policy; + rmw_qos_profile_.depth = qos_initialization.depth; +} + +QoS::QoS(size_t history_depth) +: QoS(KeepLast(history_depth)) +{} + +rmw_qos_profile_t & +QoS::get_rmw_qos_profile() +{ + return rmw_qos_profile_; +} + +const rmw_qos_profile_t & +QoS::get_rmw_qos_profile() const +{ + return rmw_qos_profile_; +} + +QoS & +QoS::history(rmw_qos_history_policy_t history) +{ + rmw_qos_profile_.history = history; + return *this; +} + +QoS & +QoS::history(HistoryPolicy history) +{ + rmw_qos_profile_.history = static_cast(history); + return *this; +} + +QoS & +QoS::keep_last(size_t depth) +{ + if (depth == 0) { + RCLCPP_WARN_ONCE( + rclcpp::get_logger( + "rclcpp"), + "A zero depth with KEEP_LAST doesn't make sense; no data could be stored." + "This will be interpreted as SYSTEM_DEFAULT"); + } + + rmw_qos_profile_.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; + rmw_qos_profile_.depth = depth; + return *this; +} + +QoS & +QoS::keep_all() +{ + rmw_qos_profile_.history = RMW_QOS_POLICY_HISTORY_KEEP_ALL; + rmw_qos_profile_.depth = 0; + return *this; +} + +QoS & +QoS::reliability(rmw_qos_reliability_policy_t reliability) +{ + rmw_qos_profile_.reliability = reliability; + return *this; +} + +QoS & +QoS::reliability(ReliabilityPolicy reliability) +{ + rmw_qos_profile_.reliability = static_cast(reliability); + return *this; +} + +QoS & +QoS::reliable() +{ + return this->reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE); +} + +QoS & +QoS::best_effort() +{ + return this->reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT); +} + +QoS & +QoS::reliability_best_available() +{ + return this->reliability(RMW_QOS_POLICY_RELIABILITY_BEST_AVAILABLE); +} + +QoS & +QoS::durability(rmw_qos_durability_policy_t durability) +{ + rmw_qos_profile_.durability = durability; + return *this; +} + +QoS & +QoS::durability(DurabilityPolicy durability) +{ + rmw_qos_profile_.durability = static_cast(durability); + return *this; +} + +QoS & +QoS::durability_volatile() +{ + return this->durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); +} + +QoS & +QoS::transient_local() +{ + return this->durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); +} + +QoS & +QoS::durability_best_available() +{ + return this->durability(RMW_QOS_POLICY_DURABILITY_BEST_AVAILABLE); +} + +QoS & +QoS::deadline(rmw_time_t deadline) +{ + rmw_qos_profile_.deadline = deadline; + return *this; +} + +QoS & +QoS::deadline(const rclcpp::Duration & deadline) +{ + return this->deadline(deadline.to_rmw_time()); +} + +QoS & +QoS::lifespan(rmw_time_t lifespan) +{ + rmw_qos_profile_.lifespan = lifespan; + return *this; +} + +QoS & +QoS::lifespan(const rclcpp::Duration & lifespan) +{ + return this->lifespan(lifespan.to_rmw_time()); +} + +QoS & +QoS::liveliness(rmw_qos_liveliness_policy_t liveliness) +{ + rmw_qos_profile_.liveliness = liveliness; + return *this; +} + +QoS & +QoS::liveliness(LivelinessPolicy liveliness) +{ + rmw_qos_profile_.liveliness = static_cast(liveliness); + return *this; +} + + +QoS & +QoS::liveliness_lease_duration(rmw_time_t liveliness_lease_duration) +{ + rmw_qos_profile_.liveliness_lease_duration = liveliness_lease_duration; + return *this; +} + +QoS & +QoS::liveliness_lease_duration(const rclcpp::Duration & liveliness_lease_duration) +{ + return this->liveliness_lease_duration(liveliness_lease_duration.to_rmw_time()); +} + +QoS & +QoS::avoid_ros_namespace_conventions(bool avoid_ros_namespace_conventions) +{ + rmw_qos_profile_.avoid_ros_namespace_conventions = avoid_ros_namespace_conventions; + return *this; +} + +HistoryPolicy +QoS::history() const +{ + return static_cast(rmw_qos_profile_.history); +} + +size_t +QoS::depth() const {return rmw_qos_profile_.depth;} + +ReliabilityPolicy +QoS::reliability() const +{ + return static_cast(rmw_qos_profile_.reliability); +} + +DurabilityPolicy +QoS::durability() const +{ + return static_cast(rmw_qos_profile_.durability); +} + +Duration +QoS::deadline() const {return Duration::from_rmw_time(rmw_qos_profile_.deadline);} + +Duration +QoS::lifespan() const {return Duration::from_rmw_time(rmw_qos_profile_.lifespan);} + +LivelinessPolicy +QoS::liveliness() const +{ + return static_cast(rmw_qos_profile_.liveliness); +} + +Duration +QoS::liveliness_lease_duration() const +{ + return Duration::from_rmw_time(rmw_qos_profile_.liveliness_lease_duration); +} + +bool +QoS::avoid_ros_namespace_conventions() const +{ + return rmw_qos_profile_.avoid_ros_namespace_conventions; +} + +namespace +{ +/// Check if two rmw_time_t have the same values. +bool operator==(const rmw_time_t & left, const rmw_time_t & right) +{ + return left.sec == right.sec && left.nsec == right.nsec; +} +} // unnamed namespace + +bool operator==(const QoS & left, const QoS & right) +{ + const auto & pl = left.get_rmw_qos_profile(); + const auto & pr = right.get_rmw_qos_profile(); + return pl.history == pr.history && + pl.depth == pr.depth && + pl.reliability == pr.reliability && + pl.durability == pr.durability && + pl.deadline == pr.deadline && + pl.lifespan == pr.lifespan && + pl.liveliness == pr.liveliness && + pl.liveliness_lease_duration == pr.liveliness_lease_duration && + pl.avoid_ros_namespace_conventions == pr.avoid_ros_namespace_conventions; +} + +bool operator!=(const QoS & left, const QoS & right) +{ + return !(left == right); +} + +QoSCheckCompatibleResult +qos_check_compatible(const QoS & publisher_qos, const QoS & subscription_qos) +{ + rmw_qos_compatibility_type_t compatible; + const size_t reason_size = 2048u; + char reason_c_str[reason_size] = ""; + rmw_ret_t ret = rmw_qos_profile_check_compatible( + publisher_qos.get_rmw_qos_profile(), + subscription_qos.get_rmw_qos_profile(), + &compatible, + reason_c_str, + reason_size); + if (RMW_RET_OK != ret) { + std::string error_str(rmw_get_error_string().str); + rmw_reset_error(); + throw rclcpp::exceptions::QoSCheckCompatibleException{error_str}; + } + + QoSCheckCompatibleResult result; + result.reason = std::string(reason_c_str); + + switch (compatible) { + case RMW_QOS_COMPATIBILITY_OK: + result.compatibility = QoSCompatibility::Ok; + break; + case RMW_QOS_COMPATIBILITY_WARNING: + result.compatibility = QoSCompatibility::Warning; + break; + case RMW_QOS_COMPATIBILITY_ERROR: + result.compatibility = QoSCompatibility::Error; + break; + default: + throw rclcpp::exceptions::QoSCheckCompatibleException{ + "Unexpected compatibility value returned by rmw '" + std::to_string(compatible) + + "'"}; + } + return result; +} + +ClockQoS::ClockQoS(const QoSInitialization & qos_initialization) +// Using `rmw_qos_profile_sensor_data` intentionally. +// It's best effort and `qos_initialization` is overriding the depth to 1. +: QoS(qos_initialization, rmw_qos_profile_sensor_data) +{} + +SensorDataQoS::SensorDataQoS(const QoSInitialization & qos_initialization) +: QoS(qos_initialization, rmw_qos_profile_sensor_data) +{} + +ParametersQoS::ParametersQoS(const QoSInitialization & qos_initialization) +: QoS(qos_initialization, rmw_qos_profile_parameters) +{} + +ServicesQoS::ServicesQoS(const QoSInitialization & qos_initialization) +: QoS(qos_initialization, rmw_qos_profile_services_default) +{} + +ParameterEventsQoS::ParameterEventsQoS(const QoSInitialization & qos_initialization) +: QoS(qos_initialization, rmw_qos_profile_parameter_events) +{} + +RosoutQoS::RosoutQoS(const QoSInitialization & rosout_initialization) +: QoS(rosout_initialization, rcl_qos_profile_rosout_default) +{} + +SystemDefaultsQoS::SystemDefaultsQoS(const QoSInitialization & qos_initialization) +: QoS(qos_initialization, rmw_qos_profile_system_default) +{} + +BestAvailableQoS::BestAvailableQoS(const QoSInitialization & qos_initialization) +: QoS(qos_initialization, rmw_qos_profile_best_available) +{} + +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/qos_overriding_options.cpp b/rclcpp/src/rclcpp/qos_overriding_options.cpp new file mode 100644 index 0000000000..ab5d705bf3 --- /dev/null +++ b/rclcpp/src/rclcpp/qos_overriding_options.cpp @@ -0,0 +1,84 @@ +// 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 "rclcpp/qos_overriding_options.hpp" + +#include +#include +#include +#include +#include +#include + +#include "rmw/qos_policy_kind.h" +#include "rmw/qos_string_conversions.h" + +namespace rclcpp +{ + +const char * +qos_policy_kind_to_cstr(const QosPolicyKind & qpk) +{ + const char * ret = rmw_qos_policy_kind_to_str(static_cast(qpk)); + if (!ret) { + throw std::invalid_argument{"unknown QoS policy kind"}; + } + return ret; +} + +std::ostream & +operator<<(std::ostream & oss, const QosPolicyKind & qpk) +{ + return oss << qos_policy_kind_to_cstr(qpk); +} + +static std::initializer_list kDefaultPolicies = +{QosPolicyKind::History, QosPolicyKind::Depth, QosPolicyKind::Reliability}; + +QosOverridingOptions::QosOverridingOptions( + std::initializer_list policy_kinds, + QosCallback validation_callback, + std::string id) +: id_{std::move(id)}, + policy_kinds_{policy_kinds}, + validation_callback_{std::move(validation_callback)} +{} + +QosOverridingOptions +QosOverridingOptions::with_default_policies( + QosCallback validation_callback, + std::string id) +{ + return QosOverridingOptions{kDefaultPolicies, validation_callback, id}; +} + +const std::string & +QosOverridingOptions::get_id() const +{ + return id_; +} + +const std::vector & +QosOverridingOptions::get_policy_kinds() const +{ + return policy_kinds_; +} + +const QosCallback & +QosOverridingOptions::get_validation_callback() const +{ + return validation_callback_; +} + +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/rate.cpp b/rclcpp/src/rclcpp/rate.cpp new file mode 100644 index 0000000000..083bd223c5 --- /dev/null +++ b/rclcpp/src/rclcpp/rate.cpp @@ -0,0 +1,100 @@ +// Copyright 2023 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 "rclcpp/rate.hpp" + +#include +#include + +namespace rclcpp +{ + +Rate::Rate( + const double rate, Clock::SharedPtr clock) +: clock_(clock), period_(0, 0), last_interval_(clock_->now()) +{ + if (rate <= 0.0) { + throw std::invalid_argument{"rate must be greater than 0"}; + } + period_ = Duration::from_seconds(1.0 / rate); +} + +Rate::Rate( + const Duration & period, Clock::SharedPtr clock) +: clock_(clock), period_(period), last_interval_(clock_->now()) +{ + if (period <= Duration(0, 0)) { + throw std::invalid_argument{"period must be greater than 0"}; + } +} + +bool +Rate::sleep() +{ + // Time coming into sleep + auto now = clock_->now(); + // Time of next interval + auto next_interval = last_interval_ + period_; + // Detect backwards time flow + if (now < last_interval_) { + // Best thing to do is to set the next_interval to now + period + next_interval = now + period_; + } + // Update the interval + last_interval_ += period_; + // If the time_to_sleep is negative or zero, don't sleep + if (next_interval <= now) { + // If an entire cycle was missed then reset next interval. + // This might happen if the loop took more than a cycle. + // Or if time jumps forward. + if (now > next_interval + period_) { + last_interval_ = now + period_; + } + // Either way do not sleep and return false + return false; + } + // Calculate the time to sleep + auto time_to_sleep = next_interval - now; + // Sleep (will get interrupted by ctrl-c, may not sleep full time) + clock_->sleep_for(time_to_sleep); + return true; +} + +rcl_clock_type_t +Rate::get_type() const +{ + return clock_->get_clock_type(); +} + +void +Rate::reset() +{ + last_interval_ = clock_->now(); +} + +std::chrono::nanoseconds +Rate::period() const +{ + return std::chrono::nanoseconds(period_.nanoseconds()); +} + +WallRate::WallRate(const double rate) +: Rate(rate, std::make_shared(RCL_STEADY_TIME)) +{} + +WallRate::WallRate(const Duration & period) +: Rate(period, std::make_shared(RCL_STEADY_TIME)) +{} + +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/serialization.cpp b/rclcpp/src/rclcpp/serialization.cpp new file mode 100644 index 0000000000..bca341185f --- /dev/null +++ b/rclcpp/src/rclcpp/serialization.cpp @@ -0,0 +1,72 @@ +// 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 "rclcpp/serialization.hpp" + +#include +#include + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/serialized_message.hpp" + +#include "rcpputils/asserts.hpp" + +#include "rmw/rmw.h" + +namespace rclcpp +{ + +SerializationBase::SerializationBase(const rosidl_message_type_support_t * type_support) +: type_support_(type_support) +{ + rcpputils::check_true(nullptr != type_support, "Typesupport is nullpointer."); +} + +void SerializationBase::serialize_message( + const void * ros_message, SerializedMessage * serialized_message) const +{ + rcpputils::check_true(nullptr != type_support_, "Typesupport is nullpointer."); + rcpputils::check_true(nullptr != ros_message, "ROS message is nullpointer."); + rcpputils::check_true(nullptr != serialized_message, "Serialized message is nullpointer."); + + const auto ret = rmw_serialize( + ros_message, + type_support_, + &serialized_message->get_rcl_serialized_message()); + if (ret != RMW_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to serialize ROS message."); + } +} + +void SerializationBase::deserialize_message( + const SerializedMessage * serialized_message, void * ros_message) const +{ + rcpputils::check_true(nullptr != type_support_, "Typesupport is nullpointer."); + rcpputils::check_true(nullptr != serialized_message, "Serialized message is nullpointer."); + rcpputils::check_true( + 0u != serialized_message->capacity(), + "Wrongly initialized. Serialized message has a capacity of zero."); + rcpputils::check_true( + 0u != serialized_message->size(), + "Wrongly initialized. Serialized message has a size of zero."); + rcpputils::check_true(nullptr != ros_message, "ROS message is a nullpointer."); + + const auto ret = rmw_deserialize( + &serialized_message->get_rcl_serialized_message(), type_support_, ros_message); + if (ret != RMW_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to deserialize ROS message."); + } +} + +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/serialized_message.cpp b/rclcpp/src/rclcpp/serialized_message.cpp new file mode 100644 index 0000000000..2caaf90edb --- /dev/null +++ b/rclcpp/src/rclcpp/serialized_message.cpp @@ -0,0 +1,164 @@ +// 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 "rclcpp/serialized_message.hpp" + +#include + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/logging.hpp" + +#include "rmw/types.h" + +namespace rclcpp +{ + +inline void copy_rcl_message(const rcl_serialized_message_t & from, rcl_serialized_message_t & to) +{ + const auto ret = rmw_serialized_message_init( + &to, from.buffer_capacity, &from.allocator); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + + // do not call memcpy if the pointer is "static" + if (to.buffer != from.buffer) { + std::memcpy(to.buffer, from.buffer, from.buffer_length); + } + to.buffer_length = from.buffer_length; +} + +/// Object oriented version of rcl_serialized_message_t with destructor to avoid memory leaks +SerializedMessage::SerializedMessage(const rcl_allocator_t & allocator) +: SerializedMessage(0u, allocator) +{} + +SerializedMessage::SerializedMessage( + size_t initial_capacity, const rcl_allocator_t & allocator) +: serialized_message_(rmw_get_zero_initialized_serialized_message()) +{ + const auto ret = rmw_serialized_message_init( + &serialized_message_, initial_capacity, &allocator); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } +} + +SerializedMessage::SerializedMessage(const SerializedMessage & other) +: SerializedMessage(other.serialized_message_) +{} + +SerializedMessage::SerializedMessage(const rcl_serialized_message_t & other) +: serialized_message_(rmw_get_zero_initialized_serialized_message()) +{ + copy_rcl_message(other, serialized_message_); +} + +SerializedMessage::SerializedMessage(SerializedMessage && other) +: serialized_message_( + std::exchange(other.serialized_message_, rmw_get_zero_initialized_serialized_message())) +{} + +SerializedMessage::SerializedMessage(rcl_serialized_message_t && other) +: serialized_message_( + std::exchange(other, rmw_get_zero_initialized_serialized_message())) +{} + +SerializedMessage & SerializedMessage::operator=(const SerializedMessage & other) +{ + if (this != &other) { + serialized_message_ = rmw_get_zero_initialized_serialized_message(); + copy_rcl_message(other.serialized_message_, serialized_message_); + } + + return *this; +} + +SerializedMessage & SerializedMessage::operator=(const rcl_serialized_message_t & other) +{ + if (&serialized_message_ != &other) { + serialized_message_ = rmw_get_zero_initialized_serialized_message(); + copy_rcl_message(other, serialized_message_); + } + + return *this; +} + +SerializedMessage & SerializedMessage::operator=(SerializedMessage && other) +{ + if (this != &other) { + serialized_message_ = + std::exchange(other.serialized_message_, rmw_get_zero_initialized_serialized_message()); + } + + return *this; +} + +SerializedMessage & SerializedMessage::operator=(rcl_serialized_message_t && other) +{ + if (&serialized_message_ != &other) { + serialized_message_ = + std::exchange(other, rmw_get_zero_initialized_serialized_message()); + } + return *this; +} + +SerializedMessage::~SerializedMessage() +{ + if (nullptr != serialized_message_.buffer) { + const auto fini_ret = rmw_serialized_message_fini(&serialized_message_); + if (RCL_RET_OK != fini_ret) { + RCLCPP_ERROR( + get_logger("rclcpp"), + "Failed to destroy serialized message: %s", rcl_get_error_string().str); + } + } +} + +rcl_serialized_message_t & SerializedMessage::get_rcl_serialized_message() +{ + return serialized_message_; +} + +const rcl_serialized_message_t & SerializedMessage::get_rcl_serialized_message() const +{ + return serialized_message_; +} + +size_t SerializedMessage::size() const +{ + return serialized_message_.buffer_length; +} + +size_t SerializedMessage::capacity() const +{ + return serialized_message_.buffer_capacity; +} + +void SerializedMessage::reserve(size_t capacity) +{ + auto ret = rmw_serialized_message_resize(&serialized_message_, capacity); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } +} + +rcl_serialized_message_t SerializedMessage::release_rcl_serialized_message() +{ + auto ret = serialized_message_; + serialized_message_ = rmw_get_zero_initialized_serialized_message(); + + return ret; +} +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/service.cpp b/rclcpp/src/rclcpp/service.cpp index 441c2f67e6..9c246e4b6b 100644 --- a/rclcpp/src/rclcpp/service.cpp +++ b/rclcpp/src/rclcpp/service.cpp @@ -22,37 +22,46 @@ #include "rclcpp/any_service_callback.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/qos.hpp" #include "rmw/error_handling.h" #include "rmw/rmw.h" using rclcpp::ServiceBase; -ServiceBase::ServiceBase( - std::shared_ptr node_handle, - const std::string & service_name) -: node_handle_(node_handle), service_name_(service_name) -{} - ServiceBase::ServiceBase(std::shared_ptr node_handle) -: node_handle_(node_handle) +: node_handle_(node_handle), + node_logger_(rclcpp::get_node_logger(node_handle_.get())) {} -ServiceBase::~ServiceBase() -{} -std::string +bool +ServiceBase::take_type_erased_request(void * request_out, rmw_request_id_t & request_id_out) +{ + rcl_ret_t ret = rcl_take_request( + this->get_service_handle().get(), + &request_id_out, + request_out); + if (RCL_RET_SERVICE_TAKE_FAILED == ret) { + return false; + } else if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + return true; +} + +const char * ServiceBase::get_service_name() { - return this->service_name_; + return rcl_service_get_service_name(this->get_service_handle().get()); } -rcl_service_t * +std::shared_ptr ServiceBase::get_service_handle() { return service_handle_; } -const rcl_service_t * +std::shared_ptr ServiceBase::get_service_handle() const { return service_handle_; @@ -69,3 +78,61 @@ ServiceBase::get_rcl_node_handle() const { return node_handle_.get(); } + +bool +ServiceBase::exchange_in_use_by_wait_set_state(bool in_use_state) +{ + return in_use_by_wait_set_.exchange(in_use_state); +} + +rclcpp::QoS +ServiceBase::get_response_publisher_actual_qos() const +{ + const rmw_qos_profile_t * qos = + rcl_service_response_publisher_get_actual_qos(service_handle_.get()); + if (!qos) { + auto msg = + std::string("failed to get service's response publisher qos settings: ") + + rcl_get_error_string().str; + rcl_reset_error(); + throw std::runtime_error(msg); + } + + rclcpp::QoS response_publisher_qos = + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(*qos), *qos); + + return response_publisher_qos; +} + +rclcpp::QoS +ServiceBase::get_request_subscription_actual_qos() const +{ + const rmw_qos_profile_t * qos = + rcl_service_request_subscription_get_actual_qos(service_handle_.get()); + if (!qos) { + auto msg = + std::string("failed to get service's request subscription qos settings: ") + + rcl_get_error_string().str; + rcl_reset_error(); + throw std::runtime_error(msg); + } + + rclcpp::QoS request_subscription_qos = + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(*qos), *qos); + + return request_subscription_qos; +} + +void +ServiceBase::set_on_new_request_callback(rcl_event_callback_t callback, const void * user_data) +{ + rcl_ret_t ret = rcl_service_set_on_new_request_callback( + service_handle_.get(), + callback, + user_data); + + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "failed to set the on new request callback for service"); + } +} diff --git a/rclcpp/src/rclcpp/signal_handler.cpp b/rclcpp/src/rclcpp/signal_handler.cpp new file mode 100644 index 0000000000..cf26d06df4 --- /dev/null +++ b/rclcpp/src/rclcpp/signal_handler.cpp @@ -0,0 +1,390 @@ +// 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 "signal_handler.hpp" + +#include +#include +#include +#include +#include + +// includes for semaphore notification code +#if defined(_WIN32) +#include +#elif defined(__APPLE__) +#include +#else // posix +#include +#endif + +#include "rclcpp/logging.hpp" +#include "rclcpp/utilities.hpp" +#include "rcutils/strerror.h" +#include "rmw/impl/cpp/demangle.hpp" + +using rclcpp::SignalHandler; +using rclcpp::SignalHandlerOptions; + +SignalHandler::signal_handler_type +SignalHandler::set_signal_handler( + int signal_value, + const SignalHandler::signal_handler_type & signal_handler) +{ + bool signal_handler_install_failed; + SignalHandler::signal_handler_type old_signal_handler; +#if defined(RCLCPP_HAS_SIGACTION) + ssize_t ret = sigaction(signal_value, &signal_handler, &old_signal_handler); + signal_handler_install_failed = (ret == -1); +#else + old_signal_handler = std::signal(signal_value, signal_handler); + signal_handler_install_failed = (old_signal_handler == SIG_ERR); +#endif + if (signal_handler_install_failed) { + char error_string[1024]; + rcutils_strerror(error_string, sizeof(error_string)); + auto msg = + "Failed to set signal handler (" + std::to_string(errno) + "): " + error_string; + throw std::runtime_error(msg); + } + return old_signal_handler; +} + +// Unfortunately macros (or duplicated code) are needed here, +// as the signal handler must be a function pointer. +#if defined(RCLCPP_HAS_SIGACTION) +void +SignalHandler::signal_handler( + int signum, siginfo_t * siginfo, void * context) +{ + RCLCPP_INFO(SignalHandler::get_logger(), "signal_handler(signum=%d)", signum); + auto & instance = SignalHandler::get_global_signal_handler(); + + auto old_signal_handler = instance.get_old_signal_handler(signum); + if (old_signal_handler.sa_flags & SA_SIGINFO) { + if (old_signal_handler.sa_sigaction != NULL) { + old_signal_handler.sa_sigaction(signum, siginfo, context); + } + } else { + if ( + old_signal_handler.sa_handler != NULL && /* Is set */ + old_signal_handler.sa_handler != SIG_DFL && /* Is not default*/ + old_signal_handler.sa_handler != SIG_IGN) /* Is not ignored */ + { + old_signal_handler.sa_handler(signum); + } + } + instance.signal_handler_common(); +} +#else +void +SignalHandler::signal_handler(int signum) +{ + RCLCPP_INFO(SignalHandler::get_logger(), "signal_handler(signum=%d)", signum); + auto & instance = SignalHandler::get_global_signal_handler(); + auto old_signal_handler = instance.get_old_signal_handler(signum); + if ( + SIG_ERR != old_signal_handler && SIG_IGN != old_signal_handler && + SIG_DFL != old_signal_handler) + { + old_signal_handler(signum); + } + instance.signal_handler_common(); +} +#endif + +rclcpp::Logger & +SignalHandler::get_logger() +{ + return SignalHandler::get_global_signal_handler().logger_; +} + +SignalHandler & +SignalHandler::get_global_signal_handler() +{ + static SignalHandler & signal_handler = *new SignalHandler(); + return signal_handler; +} + +bool +SignalHandler::install(SignalHandlerOptions signal_handler_options) +{ + std::lock_guard lock(install_mutex_); + bool already_installed = installed_.exchange(true); + if (already_installed) { + return false; + } + if (signal_handler_options == SignalHandlerOptions::None) { + return true; + } + signal_handlers_options_ = signal_handler_options; + try { + setup_wait_for_signal(); + signal_received_.store(false); + + SignalHandler::signal_handler_type handler_argument; +#if defined(RCLCPP_HAS_SIGACTION) + memset(&handler_argument, 0, sizeof(handler_argument)); + sigemptyset(&handler_argument.sa_mask); + handler_argument.sa_sigaction = &this->signal_handler; + handler_argument.sa_flags = SA_SIGINFO; +#else + handler_argument = &this->signal_handler; +#endif + if ( + signal_handler_options == SignalHandlerOptions::SigInt || + signal_handler_options == SignalHandlerOptions::All) + { + old_sigint_handler_ = set_signal_handler(SIGINT, handler_argument); + } + + if ( + signal_handler_options == SignalHandlerOptions::SigTerm || + signal_handler_options == SignalHandlerOptions::All) + { + old_sigterm_handler_ = set_signal_handler(SIGTERM, handler_argument); + } + + signal_handler_thread_ = std::thread(&SignalHandler::deferred_signal_handler, this); + } catch (...) { + installed_.store(false); + throw; + } + RCLCPP_DEBUG(get_logger(), "signal handler installed"); + return true; +} + +bool +SignalHandler::uninstall() +{ + std::lock_guard lock(install_mutex_); + bool installed = installed_.exchange(false); + if (!installed) { + return false; + } + try { + // TODO(wjwwood): what happens if someone overrides our signal handler then calls uninstall? + // I think we need to assert that we're the current signal handler, and mitigate if not. + if ( + SignalHandlerOptions::SigInt == signal_handlers_options_ || + SignalHandlerOptions::All == signal_handlers_options_) + { + set_signal_handler(SIGINT, old_sigint_handler_); + } + if ( + SignalHandlerOptions::SigTerm == signal_handlers_options_ || + SignalHandlerOptions::All == signal_handlers_options_) + { + set_signal_handler(SIGTERM, old_sigterm_handler_); + } + signal_handlers_options_ = SignalHandlerOptions::None; + RCLCPP_DEBUG(get_logger(), "SignalHandler::uninstall(): notifying deferred signal handler"); + notify_signal_handler(); + if (signal_handler_thread_.joinable()) { + signal_handler_thread_.join(); + } + teardown_wait_for_signal(); + } catch (...) { + installed_.exchange(true); + throw; + } + RCLCPP_DEBUG(get_logger(), "signal handler uninstalled"); + return true; +} + +bool +SignalHandler::is_installed() +{ + return installed_.load(); +} + +SignalHandler::~SignalHandler() +{ + try { + uninstall(); + } catch (const std::exception & exc) { + RCLCPP_ERROR( + get_logger(), + "caught %s exception when uninstalling signal handlers in rclcpp::~SignalHandler: %s", + rmw::impl::cpp::demangle(exc).c_str(), exc.what()); + } catch (...) { + RCLCPP_ERROR( + get_logger(), + "caught unknown exception when uninstalling signal handlers in rclcpp::~SignalHandler"); + } +} + +SignalHandler::signal_handler_type +SignalHandler::get_old_signal_handler(int signum) +{ + if (SIGINT == signum) { + return old_sigint_handler_; + } else if (SIGTERM == signum) { + return old_sigterm_handler_; + } +#if defined(RCLCPP_HAS_SIGACTION) + SignalHandler::signal_handler_type ret; + memset(&ret, 0, sizeof(ret)); + sigemptyset(&ret.sa_mask); + ret.sa_handler = SIG_DFL; + return ret; +#else + return SIG_DFL; +#endif +} + +void +SignalHandler::signal_handler_common() +{ + auto & instance = SignalHandler::get_global_signal_handler(); + instance.signal_received_.store(true); + RCLCPP_DEBUG( + get_logger(), + "signal_handler(): notifying deferred signal handler"); + instance.notify_signal_handler(); +} + +void +SignalHandler::deferred_signal_handler() +{ + while (true) { + if (signal_received_.exchange(false)) { + RCLCPP_DEBUG(get_logger(), "deferred_signal_handler(): shutting down"); + for (auto context_ptr : rclcpp::get_contexts()) { + if (context_ptr->get_init_options().shutdown_on_signal) { + RCLCPP_DEBUG( + get_logger(), + "deferred_signal_handler(): " + "shutting down rclcpp::Context @ %p, because it had shutdown_on_signal == true", + static_cast(context_ptr.get())); + context_ptr->shutdown("signal handler"); + } + } + } + if (!is_installed()) { + RCLCPP_DEBUG(get_logger(), "deferred_signal_handler(): signal handling uninstalled"); + break; + } + RCLCPP_DEBUG( + get_logger(), "deferred_signal_handler(): waiting for SIGINT/SIGTERM or uninstall"); + wait_for_signal(); + RCLCPP_DEBUG( + get_logger(), "deferred_signal_handler(): woken up due to SIGINT/SIGTERM or uninstall"); + } +} + +void +SignalHandler::setup_wait_for_signal() +{ +#if defined(_WIN32) + signal_handler_sem_ = CreateSemaphore( + NULL, // default security attributes + 0, // initial semaphore count + 1, // maximum semaphore count + NULL); // unnamed semaphore + if (NULL == signal_handler_sem_) { + throw std::runtime_error("CreateSemaphore() failed in setup_wait_for_signal()"); + } +#elif defined(__APPLE__) + signal_handler_sem_ = dispatch_semaphore_create(0); +#else // posix + if (-1 == sem_init(&signal_handler_sem_, 0, 0)) { + throw std::runtime_error(std::string("sem_init() failed: ") + strerror(errno)); + } +#endif + wait_for_signal_is_setup_.store(true); +} + +void +SignalHandler::teardown_wait_for_signal() noexcept +{ + if (!wait_for_signal_is_setup_.exchange(false)) { + return; + } +#if defined(_WIN32) + CloseHandle(signal_handler_sem_); +#elif defined(__APPLE__) + dispatch_release(signal_handler_sem_); +#else // posix + if (-1 == sem_destroy(&signal_handler_sem_)) { + RCLCPP_ERROR(get_logger(), "invalid semaphore in teardown_wait_for_signal()"); + } +#endif +} + +void +SignalHandler::wait_for_signal() +{ + if (!wait_for_signal_is_setup_.load()) { + RCLCPP_ERROR(get_logger(), "called wait_for_signal() before setup_wait_for_signal()"); + return; + } +#if defined(_WIN32) + DWORD dw_wait_result = WaitForSingleObject(signal_handler_sem_, INFINITE); + switch (dw_wait_result) { + case WAIT_ABANDONED: + RCLCPP_ERROR( + get_logger(), "WaitForSingleObject() failed in wait_for_signal() with WAIT_ABANDONED: %s", + GetLastError()); + break; + case WAIT_OBJECT_0: + // successful + break; + case WAIT_TIMEOUT: + RCLCPP_ERROR(get_logger(), "WaitForSingleObject() timedout out in wait_for_signal()"); + break; + case WAIT_FAILED: + RCLCPP_ERROR( + get_logger(), "WaitForSingleObject() failed in wait_for_signal(): %s", GetLastError()); + break; + default: + RCLCPP_ERROR( + get_logger(), "WaitForSingleObject() gave unknown return in wait_for_signal(): %s", + GetLastError()); + } +#elif defined(__APPLE__) + dispatch_semaphore_wait(signal_handler_sem_, DISPATCH_TIME_FOREVER); +#else // posix + int s; + do { + s = sem_wait(&signal_handler_sem_); + } while (-1 == s && EINTR == errno); +#endif +} + +void +SignalHandler::notify_signal_handler() noexcept +{ + if (!wait_for_signal_is_setup_.load()) { + return; + } +#if defined(_WIN32) + if (!ReleaseSemaphore(signal_handler_sem_, 1, NULL)) { + RCLCPP_ERROR( + get_logger(), "ReleaseSemaphore() failed in notify_signal_handler(): %s", GetLastError()); + } +#elif defined(__APPLE__) + dispatch_semaphore_signal(signal_handler_sem_); +#else // posix + if (-1 == sem_post(&signal_handler_sem_)) { + RCLCPP_ERROR(get_logger(), "sem_post failed in notify_signal_handler()"); + } +#endif +} + +rclcpp::SignalHandlerOptions +SignalHandler::get_current_signal_handler_options() +{ + return signal_handlers_options_; +} diff --git a/rclcpp/src/rclcpp/signal_handler.hpp b/rclcpp/src/rclcpp/signal_handler.hpp new file mode 100644 index 0000000000..db608b0d10 --- /dev/null +++ b/rclcpp/src/rclcpp/signal_handler.hpp @@ -0,0 +1,214 @@ +// 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 RCLCPP__SIGNAL_HANDLER_HPP_ +#define RCLCPP__SIGNAL_HANDLER_HPP_ + +#include +#include +#include +#include + +#include "rclcpp/logging.hpp" +#include "rclcpp/utilities.hpp" + +// includes for semaphore notification code +#if defined(_WIN32) +#include +#elif defined(__APPLE__) +#include +#else // posix +#include +#endif + +// Determine if sigaction is available +#if __APPLE__ || _POSIX_C_SOURCE >= 1 || _XOPEN_SOURCE || _POSIX_SOURCE +#define RCLCPP_HAS_SIGACTION +#endif + +namespace rclcpp +{ + +/// Responsible for managing the SIGINT/SIGTERM signal handling. +/** + * This class is responsible for: + * + * - installing the signal handler for SIGINT/SIGTERM + * - uninstalling the signal handler for SIGINT/SIGTERM + * - creating a thread to execute "on signal" work outside of the signal handler + * - safely notifying the dedicated signal handling thread when receiving SIGINT/SIGTERM + * - implementation of all of the signal handling work, like shutting down contexts + * + * \internal + */ +class SignalHandler final +{ +public: + /// Return the global singleton of this class. + static + SignalHandler & + get_global_signal_handler(); + + /// Return a global singleton logger to avoid needing to create it everywhere. + static + rclcpp::Logger & + get_logger(); + + /// Install the signal handler for SIGINT/SIGTERM and start the dedicated signal handling thread. + /** + * Also stores the current signal handler to be called on signal and to + * restore when uninstalling this signal handler. + * + * \param signal_handler_options option to indicate which signal handlers should be installed. + */ + bool + install(SignalHandlerOptions signal_handler_options = SignalHandlerOptions::All); + + /// Uninstall the signal handler for SIGINT/SIGTERM and join the dedicated signal handling + /// thread. + /** + * Also restores the previous signal handler. + */ + bool + uninstall(); + + /// Return true if installed, false otherwise. + bool + is_installed(); + + /// Get the current signal handler options. + /** + * If no signal handler is installed, SignalHandlerOptions::None is returned. + */ + rclcpp::SignalHandlerOptions + get_current_signal_handler_options(); + +private: + /// Signal handler type, platform dependent. +#if defined(RCLCPP_HAS_SIGACTION) + using signal_handler_type = struct sigaction; +#else + using signal_handler_type = void (*)(int); +#endif + + + SignalHandler() = default; + + ~SignalHandler(); + + SignalHandler(const SignalHandler &) = delete; + SignalHandler(SignalHandler &&) = delete; + SignalHandler & + operator=(const SignalHandler &) = delete; + SignalHandler && + operator=(SignalHandler &&) = delete; + + /// Common signal handler code between sigaction and non-sigaction versions. + void + signal_handler_common(); + +#if defined(RCLCPP_HAS_SIGACTION) + /// Signal handler function. + static + void + signal_handler(int signal_value, siginfo_t * siginfo, void * context); +#else + /// Signal handler function. + static + void + signal_handler(int signal_value); +#endif + + /// Target of the dedicated signal handling thread. + void + deferred_signal_handler(); + + /// Setup anything that is necessary for wait_for_signal() or notify_signal_handler(). + /** + * This must be called before wait_for_signal() or notify_signal_handler(). + * This is not thread-safe. + */ + void + setup_wait_for_signal(); + + /// Undo all setup done in setup_wait_for_signal(). + /** + * Must not call wait_for_signal() or notify_signal_handler() after calling this. + * + * This is not thread-safe. + */ + void + teardown_wait_for_signal() noexcept; + + /// Wait for a notification from notify_signal_handler() in a signal safe way. + /** + * This static method may throw if posting the semaphore fails. + * + * This is not thread-safe. + */ + void + wait_for_signal(); + + /// Notify blocking wait_for_signal() calls in a signal safe way. + /** + * This is used to notify the deferred_signal_handler() thread to start work + * from the signal handler. + * + * This is thread-safe. + */ + void + notify_signal_handler() noexcept; + + static + signal_handler_type + set_signal_handler( + int signal_value, + const signal_handler_type & signal_handler); + + signal_handler_type + get_old_signal_handler(int signum); + + rclcpp::SignalHandlerOptions signal_handlers_options_ = rclcpp::SignalHandlerOptions::None; + + signal_handler_type old_sigint_handler_; + signal_handler_type old_sigterm_handler_; + + // logger instance + rclcpp::Logger logger_ = rclcpp::get_logger("rclcpp"); + + // Whether or not a signal has been received. + std::atomic_bool signal_received_ = false; + // A thread to which signal handling tasks are deferred. + std::thread signal_handler_thread_; + + // A mutex used to synchronize the install() and uninstall() methods. + std::mutex install_mutex_; + // Whether or not the signal handler has been installed. + std::atomic_bool installed_ = false; + + // Whether or not the semaphore for wait_for_signal is setup. + std::atomic_bool wait_for_signal_is_setup_; + // Storage for the wait_for_signal semaphore. +#if defined(_WIN32) + HANDLE signal_handler_sem_; +#elif defined(__APPLE__) + dispatch_semaphore_t signal_handler_sem_; +#else // posix + sem_t signal_handler_sem_; +#endif +}; + +} // namespace rclcpp + +#endif // RCLCPP__SIGNAL_HANDLER_HPP_ diff --git a/rclcpp/src/rclcpp/subscription.cpp b/rclcpp/src/rclcpp/subscription.cpp deleted file mode 100644 index 79170acdf8..0000000000 --- a/rclcpp/src/rclcpp/subscription.cpp +++ /dev/null @@ -1,98 +0,0 @@ -// 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 "rclcpp/subscription.hpp" - -#include -#include -#include - -#include "rclcpp/exceptions.hpp" -#include "rclcpp/expand_topic_or_service_name.hpp" - -#include "rmw/error_handling.h" -#include "rmw/rmw.h" - - -using rclcpp::SubscriptionBase; - -SubscriptionBase::SubscriptionBase( - std::shared_ptr node_handle, - const rosidl_message_type_support_t & type_support_handle, - const std::string & topic_name, - const rcl_subscription_options_t & subscription_options) -: node_handle_(node_handle) -{ - rcl_ret_t ret = rcl_subscription_init( - &subscription_handle_, - node_handle_.get(), - &type_support_handle, - topic_name.c_str(), - &subscription_options); - if (ret != RCL_RET_OK) { - if (ret == RCL_RET_TOPIC_NAME_INVALID) { - auto rcl_node_handle = node_handle_.get(); - // this will throw on any validation problem - rcl_reset_error(); - expand_topic_or_service_name( - topic_name, - rcl_node_get_name(rcl_node_handle), - rcl_node_get_namespace(rcl_node_handle)); - } - - rclcpp::exceptions::throw_from_rcl_error(ret, "could not create subscription"); - } -} - -SubscriptionBase::~SubscriptionBase() -{ - if (rcl_subscription_fini(&subscription_handle_, node_handle_.get()) != RCL_RET_OK) { - std::stringstream ss; - ss << "Error in destruction of rcl subscription handle: " << - rcl_get_error_string_safe() << '\n'; - (std::cerr << ss.str()).flush(); - } - if (rcl_subscription_fini( - &intra_process_subscription_handle_, node_handle_.get()) != RCL_RET_OK) - { - std::stringstream ss; - ss << "Error in destruction of rmw intra process subscription handle: " << - rcl_get_error_string_safe() << '\n'; - (std::cerr << ss.str()).flush(); - } -} - -const char * -SubscriptionBase::get_topic_name() const -{ - return rcl_subscription_get_topic_name(&subscription_handle_); -} - -rcl_subscription_t * -SubscriptionBase::get_subscription_handle() -{ - return &subscription_handle_; -} - -const rcl_subscription_t * -SubscriptionBase::get_subscription_handle() const -{ - return &subscription_handle_; -} - -const rcl_subscription_t * -SubscriptionBase::get_intra_process_subscription_handle() const -{ - return &intra_process_subscription_handle_; -} diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp new file mode 100644 index 0000000000..7dca16a1a9 --- /dev/null +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -0,0 +1,581 @@ +// 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 "rclcpp/subscription_base.hpp" + +#include +#include +#include +#include +#include + +#include "rcpputils/scope_exit.hpp" + +#include "rclcpp/dynamic_typesupport/dynamic_message.hpp" +#include "rclcpp/exceptions.hpp" +#include "rclcpp/expand_topic_or_service_name.hpp" +#include "rclcpp/experimental/intra_process_manager.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/event_handler.hpp" + +#include "rmw/error_handling.h" +#include "rmw/rmw.h" + +#include "rosidl_dynamic_typesupport/types.h" + +using rclcpp::SubscriptionBase; + +SubscriptionBase::SubscriptionBase( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + const rosidl_message_type_support_t & type_support_handle, + const std::string & topic_name, + const rcl_subscription_options_t & subscription_options, + const SubscriptionEventCallbacks & event_callbacks, + bool use_default_callbacks, + DeliveredMessageKind delivered_message_kind) +: node_base_(node_base), + node_handle_(node_base_->get_shared_rcl_node_handle()), + node_logger_(rclcpp::get_node_logger(node_handle_.get())), + use_intra_process_(false), + intra_process_subscription_id_(0), + event_callbacks_(event_callbacks), + type_support_(type_support_handle), + delivered_message_kind_(delivered_message_kind) +{ + auto custom_deletor = [node_handle = this->node_handle_](rcl_subscription_t * rcl_subs) + { + if (rcl_subscription_fini(rcl_subs, node_handle.get()) != RCL_RET_OK) { + RCLCPP_ERROR( + rclcpp::get_node_logger(node_handle.get()).get_child("rclcpp"), + "Error in destruction of rcl subscription handle: %s", + rcl_get_error_string().str); + rcl_reset_error(); + } + delete rcl_subs; + }; + + subscription_handle_ = std::shared_ptr( + new rcl_subscription_t, custom_deletor); + *subscription_handle_.get() = rcl_get_zero_initialized_subscription(); + + rcl_ret_t ret = rcl_subscription_init( + subscription_handle_.get(), + node_handle_.get(), + &type_support_handle, + topic_name.c_str(), + &subscription_options); + if (ret != RCL_RET_OK) { + if (ret == RCL_RET_TOPIC_NAME_INVALID) { + auto rcl_node_handle = node_handle_.get(); + // this will throw on any validation problem + rcl_reset_error(); + expand_topic_or_service_name( + topic_name, + rcl_node_get_name(rcl_node_handle), + rcl_node_get_namespace(rcl_node_handle)); + } + rclcpp::exceptions::throw_from_rcl_error(ret, "could not create subscription"); + } + + bind_event_callbacks(event_callbacks_, use_default_callbacks); +} + +SubscriptionBase::~SubscriptionBase() +{ + if (!use_intra_process_) { + return; + } + auto ipm = weak_ipm_.lock(); + if (!ipm) { + // TODO(ivanpauno): should this raise an error? + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Intra process manager died before than a subscription."); + return; + } + ipm->remove_subscription(intra_process_subscription_id_); +} + +void +SubscriptionBase::bind_event_callbacks( + const SubscriptionEventCallbacks & event_callbacks, bool use_default_callbacks) +{ + try { + if (event_callbacks.deadline_callback) { + this->add_event_handler( + event_callbacks.deadline_callback, + RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED); + } + } catch (const UnsupportedEventTypeException & /*exc*/) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Failed to add event handler for deadline; not supported"); + } + + try { + if (event_callbacks.liveliness_callback) { + this->add_event_handler( + event_callbacks.liveliness_callback, + RCL_SUBSCRIPTION_LIVELINESS_CHANGED); + } + } catch (const UnsupportedEventTypeException & /*exc*/) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Failed to add event handler for liveliness; not supported"); + } + + QOSRequestedIncompatibleQoSCallbackType incompatible_qos_cb; + if (event_callbacks.incompatible_qos_callback) { + incompatible_qos_cb = event_callbacks.incompatible_qos_callback; + } else if (use_default_callbacks) { + // Register default callback when not specified + incompatible_qos_cb = [this](QOSRequestedIncompatibleQoSInfo & info) { + this->default_incompatible_qos_callback(info); + }; + } + // Register default callback when not specified + try { + if (incompatible_qos_cb) { + this->add_event_handler(incompatible_qos_cb, RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS); + } + } catch (const UnsupportedEventTypeException & /*exc*/) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Failed to add event handler for incompatible qos; not supported"); + } + + IncompatibleTypeCallbackType incompatible_type_cb; + if (event_callbacks.incompatible_type_callback) { + incompatible_type_cb = event_callbacks.incompatible_type_callback; + } else if (use_default_callbacks) { + // Register default callback when not specified + incompatible_type_cb = [this](IncompatibleTypeInfo & info) { + this->default_incompatible_type_callback(info); + }; + } + try { + if (incompatible_type_cb) { + this->add_event_handler(incompatible_type_cb, RCL_SUBSCRIPTION_INCOMPATIBLE_TYPE); + } + } catch (UnsupportedEventTypeException & /*exc*/) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Failed to add event handler for incompatible type; not supported"); + } + + try { + if (event_callbacks.message_lost_callback) { + this->add_event_handler( + event_callbacks.message_lost_callback, + RCL_SUBSCRIPTION_MESSAGE_LOST); + } + } catch (const UnsupportedEventTypeException & /*exc*/) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Failed to add event handler for message lost; not supported"); + } + + try { + if (event_callbacks.matched_callback) { + this->add_event_handler( + event_callbacks.matched_callback, + RCL_SUBSCRIPTION_MATCHED); + } + } catch (const UnsupportedEventTypeException & /*exc*/) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Failed to add event handler for matched; not supported"); + } +} + +const char * +SubscriptionBase::get_topic_name() const +{ + return rcl_subscription_get_topic_name(subscription_handle_.get()); +} + +std::shared_ptr +SubscriptionBase::get_subscription_handle() +{ + return subscription_handle_; +} + +std::shared_ptr +SubscriptionBase::get_subscription_handle() const +{ + return subscription_handle_; +} + +const +std::unordered_map> & +SubscriptionBase::get_event_handlers() const +{ + return event_handlers_; +} + +rclcpp::QoS +SubscriptionBase::get_actual_qos() const +{ + const rmw_qos_profile_t * qos = rcl_subscription_get_actual_qos(subscription_handle_.get()); + if (!qos) { + auto msg = std::string("failed to get qos settings: ") + rcl_get_error_string().str; + rcl_reset_error(); + throw std::runtime_error(msg); + } + + return rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(*qos), *qos); +} + +bool +SubscriptionBase::take_type_erased(void * message_out, rclcpp::MessageInfo & message_info_out) +{ + rcl_ret_t ret = rcl_take( + this->get_subscription_handle().get(), + message_out, + &message_info_out.get_rmw_message_info(), + nullptr // rmw_subscription_allocation_t is unused here + ); + TRACETOOLS_TRACEPOINT(rclcpp_take, static_cast(message_out)); + if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret) { + return false; + } else if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + if ( + matches_any_intra_process_publishers(&message_info_out.get_rmw_message_info().publisher_gid)) + { + // In this case, the message will be delivered via intra-process and + // we should ignore this copy of the message. + return false; + } + return true; +} + +bool +SubscriptionBase::take_serialized( + rclcpp::SerializedMessage & message_out, + rclcpp::MessageInfo & message_info_out) +{ + rcl_ret_t ret = rcl_take_serialized_message( + this->get_subscription_handle().get(), + &message_out.get_rcl_serialized_message(), + &message_info_out.get_rmw_message_info(), + nullptr); + TRACETOOLS_TRACEPOINT( + rclcpp_take, + static_cast(&message_out.get_rcl_serialized_message())); + if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret) { + return false; + } else if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + return true; +} + +const rosidl_message_type_support_t & +SubscriptionBase::get_message_type_support_handle() const +{ + return type_support_; +} + +bool +SubscriptionBase::is_serialized() const +{ + return delivered_message_kind_ == rclcpp::DeliveredMessageKind::SERIALIZED_MESSAGE; +} + +rclcpp::DeliveredMessageKind +SubscriptionBase::get_delivered_message_kind() const +{ + return delivered_message_kind_; +} + +size_t +SubscriptionBase::get_publisher_count() const +{ + size_t inter_process_publisher_count = 0; + + rmw_ret_t status = rcl_subscription_get_publisher_count( + subscription_handle_.get(), + &inter_process_publisher_count); + + if (RCL_RET_OK != status) { + rclcpp::exceptions::throw_from_rcl_error(status, "failed to get get publisher count"); + } + return inter_process_publisher_count; +} + +void +SubscriptionBase::setup_intra_process( + uint64_t intra_process_subscription_id, + IntraProcessManagerWeakPtr weak_ipm) +{ + intra_process_subscription_id_ = intra_process_subscription_id; + weak_ipm_ = weak_ipm; + use_intra_process_ = true; +} + +bool +SubscriptionBase::can_loan_messages() const +{ + bool retval = rcl_subscription_can_loan_messages(subscription_handle_.get()); + if (retval) { + // TODO(clalancette): The loaned message interface is currently not safe to use with + // shared_ptr callbacks. If a user takes a copy of the shared_ptr, it can get freed from + // underneath them via rcl_return_loaned_message_from_subscription(). The correct solution is + // to return the loaned message in a custom deleter, but that needs to be carefully handled + // with locking. Warn the user about this until we fix it. + RCLCPP_WARN_ONCE( + this->node_logger_, + "Loaned messages are only safe with const ref subscription callbacks. " + "If you are using any other kind of subscriptions, " + "set the ROS_DISABLE_LOANED_MESSAGES environment variable to 1 (the default)."); + } + return retval; +} + +rclcpp::Waitable::SharedPtr +SubscriptionBase::get_intra_process_waitable() const +{ + // If not using intra process, shortcut to nullptr. + if (!use_intra_process_) { + return nullptr; + } + // Get the intra process manager. + auto ipm = weak_ipm_.lock(); + if (!ipm) { + throw std::runtime_error( + "SubscriptionBase::get_intra_process_waitable() called " + "after destruction of intra process manager"); + } + + // Use the id to retrieve the subscription intra-process from the intra-process manager. + return ipm->get_subscription_intra_process(intra_process_subscription_id_); +} + +void +SubscriptionBase::default_incompatible_qos_callback( + rclcpp::QOSRequestedIncompatibleQoSInfo & event) const +{ + std::string policy_name = qos_policy_name_from_kind(event.last_policy_kind); + RCLCPP_WARN( + rclcpp::get_logger(rcl_node_get_logger_name(node_handle_.get())), + "New publisher discovered on topic '%s', offering incompatible QoS. " + "No messages will be sent to it. " + "Last incompatible policy: %s", + get_topic_name(), + policy_name.c_str()); +} + +void +SubscriptionBase::default_incompatible_type_callback( + rclcpp::IncompatibleTypeInfo & event) const +{ + (void)event; + + RCLCPP_WARN( + rclcpp::get_logger(rcl_node_get_logger_name(node_handle_.get())), + "Incompatible type on topic '%s', no messages will be sent to it.", get_topic_name()); +} + +bool +SubscriptionBase::matches_any_intra_process_publishers(const rmw_gid_t * sender_gid) const +{ + if (!use_intra_process_) { + return false; + } + auto ipm = weak_ipm_.lock(); + if (!ipm) { + throw std::runtime_error( + "intra process publisher check called " + "after destruction of intra process manager"); + } + return ipm->matches_any_publishers(sender_gid); +} + +bool +SubscriptionBase::exchange_in_use_by_wait_set_state( + void * pointer_to_subscription_part, + bool in_use_state) +{ + if (nullptr == pointer_to_subscription_part) { + throw std::invalid_argument("pointer_to_subscription_part is unexpectedly nullptr"); + } + if (this == pointer_to_subscription_part) { + return subscription_in_use_by_wait_set_.exchange(in_use_state); + } + if (get_intra_process_waitable().get() == pointer_to_subscription_part) { + return intra_process_subscription_waitable_in_use_by_wait_set_.exchange(in_use_state); + } + for (const auto & key_event_pair : event_handlers_) { + auto qos_event = key_event_pair.second; + if (qos_event.get() == pointer_to_subscription_part) { + return qos_events_in_use_by_wait_set_[qos_event.get()].exchange(in_use_state); + } + } + throw std::runtime_error("given pointer_to_subscription_part does not match any part"); +} + +std::vector +SubscriptionBase::get_network_flow_endpoints() const +{ + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + rcl_network_flow_endpoint_array_t network_flow_endpoint_array = + rcl_get_zero_initialized_network_flow_endpoint_array(); + rcl_ret_t ret = rcl_subscription_get_network_flow_endpoints( + subscription_handle_.get(), &allocator, &network_flow_endpoint_array); + if (RCL_RET_OK != ret) { + auto error_msg = std::string("Error obtaining network flows of subscription: ") + + rcl_get_error_string().str; + rcl_reset_error(); + if (RCL_RET_OK != + rcl_network_flow_endpoint_array_fini(&network_flow_endpoint_array)) + { + error_msg += std::string(". Also error cleaning up network flow array: ") + + rcl_get_error_string().str; + rcl_reset_error(); + } + rclcpp::exceptions::throw_from_rcl_error(ret, error_msg); + } + + std::vector network_flow_endpoint_vector; + for (size_t i = 0; i < network_flow_endpoint_array.size; ++i) { + network_flow_endpoint_vector.push_back( + rclcpp::NetworkFlowEndpoint( + network_flow_endpoint_array. + network_flow_endpoint[i])); + } + + ret = rcl_network_flow_endpoint_array_fini(&network_flow_endpoint_array); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "error cleaning up network flow array"); + } + + return network_flow_endpoint_vector; +} + +void +SubscriptionBase::set_on_new_message_callback( + rcl_event_callback_t callback, + const void * user_data) +{ + rcl_ret_t ret = rcl_subscription_set_on_new_message_callback( + subscription_handle_.get(), + callback, + user_data); + + if (RCL_RET_OK != ret) { + using rclcpp::exceptions::throw_from_rcl_error; + throw_from_rcl_error(ret, "failed to set the on new message callback for subscription"); + } +} + +bool +SubscriptionBase::is_cft_enabled() const +{ + return rcl_subscription_is_cft_enabled(subscription_handle_.get()); +} + +void +SubscriptionBase::set_content_filter( + const std::string & filter_expression, + const std::vector & expression_parameters) +{ + rcl_subscription_content_filter_options_t options = + rcl_get_zero_initialized_subscription_content_filter_options(); + + std::vector cstrings = get_c_vector_string(expression_parameters); + rcl_ret_t ret = rcl_subscription_content_filter_options_init( + subscription_handle_.get(), + get_c_string(filter_expression), + cstrings.size(), + cstrings.data(), + &options); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "failed to init subscription content_filtered_topic option"); + } + RCPPUTILS_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_subscription_content_filter_options_fini( + subscription_handle_.get(), &options); + if (RCL_RET_OK != ret) { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), + "Failed to fini subscription content_filtered_topic option: %s", + rcl_get_error_string().str); + rcl_reset_error(); + } + }); + + ret = rcl_subscription_set_content_filter( + subscription_handle_.get(), + &options); + + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to set cft expression parameters"); + } +} + +rclcpp::ContentFilterOptions +SubscriptionBase::get_content_filter() const +{ + rclcpp::ContentFilterOptions ret_options; + rcl_subscription_content_filter_options_t options = + rcl_get_zero_initialized_subscription_content_filter_options(); + + rcl_ret_t ret = rcl_subscription_get_content_filter( + subscription_handle_.get(), + &options); + + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to get cft expression parameters"); + } + + RCPPUTILS_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_subscription_content_filter_options_fini( + subscription_handle_.get(), &options); + if (RCL_RET_OK != ret) { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), + "Failed to fini subscription content_filtered_topic option: %s", + rcl_get_error_string().str); + rcl_reset_error(); + } + }); + + rmw_subscription_content_filter_options_t & content_filter_options = + options.rmw_subscription_content_filter_options; + ret_options.filter_expression = content_filter_options.filter_expression; + + for (size_t i = 0; i < content_filter_options.expression_parameters.size; ++i) { + ret_options.expression_parameters.push_back( + content_filter_options.expression_parameters.data[i]); + } + + return ret_options; +} + + +// DYNAMIC TYPE ================================================================================== +bool +SubscriptionBase::take_dynamic_message( + rclcpp::dynamic_typesupport::DynamicMessage & /*message_out*/, + rclcpp::MessageInfo & /*message_info_out*/) +{ + throw std::runtime_error("Unimplemented"); + return false; +} diff --git a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp new file mode 100644 index 0000000000..f9d19da8c5 --- /dev/null +++ b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp @@ -0,0 +1,42 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp/experimental/subscription_intra_process_base.hpp" +#include "rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp" + +using rclcpp::experimental::SubscriptionIntraProcessBase; + +void +SubscriptionIntraProcessBase::add_to_wait_set(rcl_wait_set_t & wait_set) +{ + detail::add_guard_condition_to_rcl_wait_set(wait_set, gc_); +} + +const char * +SubscriptionIntraProcessBase::get_topic_name() const +{ + return topic_name_.c_str(); +} + +rclcpp::QoS +SubscriptionIntraProcessBase::get_actual_qos() const +{ + return qos_profile_; +} + +bool +SubscriptionIntraProcessBase::is_durability_transient_local() const +{ + return qos_profile_.durability() == rclcpp::DurabilityPolicy::TransientLocal; +} diff --git a/rclcpp/src/rclcpp/time.cpp b/rclcpp/src/rclcpp/time.cpp index c22ec50127..e2780c04d8 100644 --- a/rclcpp/src/rclcpp/time.cpp +++ b/rclcpp/src/rclcpp/time.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #include #include "rclcpp/clock.hpp" @@ -59,20 +60,22 @@ Time::Time(int32_t seconds, uint32_t nanoseconds, rcl_clock_type_t clock_type) Time::Time(int64_t nanoseconds, rcl_clock_type_t clock_type) : rcl_time_(init_time_point(clock_type)) { + if (nanoseconds < 0) { + throw std::runtime_error("cannot store a negative time point in rclcpp::Time"); + } + rcl_time_.nanoseconds = nanoseconds; } -Time::Time(const Time & rhs) -: rcl_time_(rhs.rcl_time_) -{ - rcl_time_.nanoseconds = rhs.rcl_time_.nanoseconds; -} +Time::Time(const Time & rhs) = default; + +Time::Time(Time && rhs) noexcept = default; Time::Time( const builtin_interfaces::msg::Time & time_msg, - rcl_clock_type_t ros_time) + rcl_clock_type_t clock_type) +: rcl_time_(init_time_point(clock_type)) { - rcl_time_ = init_time_point(ros_time); if (time_msg.sec < 0) { throw std::runtime_error("cannot store a negative time point in rclcpp::Time"); } @@ -87,41 +90,26 @@ Time::Time(const rcl_time_point_t & time_point) // noop } -Time::~Time() -{ -} +Time::~Time() = default; Time::operator builtin_interfaces::msg::Time() const { - builtin_interfaces::msg::Time msg_time; - msg_time.sec = static_cast(RCL_NS_TO_S(rcl_time_.nanoseconds)); - msg_time.nanosec = static_cast(rcl_time_.nanoseconds % (1000 * 1000 * 1000)); - return msg_time; + return convert_rcl_time_to_sec_nanos(rcl_time_.nanoseconds); } Time & -Time::operator=(const Time & rhs) -{ - rcl_time_ = rhs.rcl_time_; - return *this; -} +Time::operator=(const Time & rhs) = default; Time & Time::operator=(const builtin_interfaces::msg::Time & time_msg) { - if (time_msg.sec < 0) { - throw std::runtime_error("cannot store a negative time point in rclcpp::Time"); - } - - - rcl_clock_type_t ros_time = RCL_ROS_TIME; - rcl_time_ = init_time_point(ros_time); // TODO(tfoote) hard coded ROS here - - rcl_time_.nanoseconds = RCL_S_TO_NS(static_cast(time_msg.sec)); - rcl_time_.nanoseconds += time_msg.nanosec; + *this = Time(time_msg); return *this; } +Time & +Time::operator=(Time && rhs) noexcept = default; + bool Time::operator==(const rclcpp::Time & rhs) const { @@ -194,7 +182,10 @@ Duration Time::operator-(const rclcpp::Time & rhs) const { if (rcl_time_.clock_type != rhs.rcl_time_.clock_type) { - throw std::runtime_error("can't subtract times with different time sources"); + throw std::runtime_error( + std::string("can't subtract times with different time sources [") + + std::to_string(rcl_time_.clock_type) + " != " + + std::to_string(rhs.rcl_time_.clock_type) + "]"); } if (rclcpp::sub_will_overflow(rcl_time_.nanoseconds, rhs.rcl_time_.nanoseconds)) { @@ -205,7 +196,7 @@ Time::operator-(const rclcpp::Time & rhs) const throw std::underflow_error("time subtraction leads to int64_t underflow"); } - return Duration(rcl_time_.nanoseconds - rhs.rcl_time_.nanoseconds); + return Duration::from_nanoseconds(rcl_time_.nanoseconds - rhs.rcl_time_.nanoseconds); } Time @@ -227,6 +218,12 @@ Time::nanoseconds() const return rcl_time_.nanoseconds; } +double +Time::seconds() const +{ + return std::chrono::duration(std::chrono::nanoseconds(rcl_time_.nanoseconds)).count(); +} + rcl_clock_type_t Time::get_clock_type() const { @@ -245,5 +242,62 @@ operator+(const rclcpp::Duration & lhs, const rclcpp::Time & rhs) return Time(lhs.nanoseconds() + rhs.nanoseconds(), rhs.get_clock_type()); } +Time & +Time::operator+=(const rclcpp::Duration & rhs) +{ + if (rclcpp::add_will_overflow(rhs.nanoseconds(), this->nanoseconds())) { + throw std::overflow_error("addition leads to int64_t overflow"); + } + if (rclcpp::add_will_underflow(rhs.nanoseconds(), this->nanoseconds())) { + throw std::underflow_error("addition leads to int64_t underflow"); + } + + rcl_time_.nanoseconds += rhs.nanoseconds(); + if (rcl_time_.nanoseconds < 0) { + throw std::runtime_error("cannot store a negative time point in rclcpp::Time"); + } + + return *this; +} + +Time & +Time::operator-=(const rclcpp::Duration & rhs) +{ + if (rclcpp::sub_will_overflow(rcl_time_.nanoseconds, rhs.nanoseconds())) { + throw std::overflow_error("time subtraction leads to int64_t overflow"); + } + if (rclcpp::sub_will_underflow(rcl_time_.nanoseconds, rhs.nanoseconds())) { + throw std::underflow_error("time subtraction leads to int64_t underflow"); + } + + rcl_time_.nanoseconds -= rhs.nanoseconds(); + if (rcl_time_.nanoseconds < 0) { + throw std::runtime_error("cannot store a negative time point in rclcpp::Time"); + } + + return *this; +} + +Time +Time::max(rcl_clock_type_t clock_type) +{ + return Time(std::numeric_limits::max(), 999999999, clock_type); +} + +builtin_interfaces::msg::Time +convert_rcl_time_to_sec_nanos(const rcl_time_point_value_t & time_point) +{ + builtin_interfaces::msg::Time ret; + constexpr rcl_time_point_value_t kRemainder = RCL_S_TO_NS(1); + const auto result = std::div(time_point, kRemainder); + if (result.rem >= 0) { + ret.sec = static_cast(result.quot); + ret.nanosec = static_cast(result.rem); + } else { + ret.sec = static_cast(result.quot - 1); + ret.nanosec = static_cast(kRemainder + result.rem); + } + return ret; +} } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp index 54c72dbdc4..c6e39ace70 100644 --- a/rclcpp/src/rclcpp/time_source.cpp +++ b/rclcpp/src/rclcpp/time_source.cpp @@ -14,6 +14,7 @@ #include #include +#include #include #include @@ -21,276 +22,583 @@ #include "rcl/time.h" -#include "rcutils/logging_macros.h" - #include "rclcpp/clock.hpp" #include "rclcpp/exceptions.hpp" +#include "rclcpp/logging.hpp" #include "rclcpp/node.hpp" +#include "rclcpp/parameter_client.hpp" +#include "rclcpp/parameter_events_filter.hpp" #include "rclcpp/time.hpp" #include "rclcpp/time_source.hpp" - namespace rclcpp { -TimeSource::TimeSource(std::shared_ptr node) -: ros_time_active_(false) +class ClocksState final { - this->attachNode(node); -} +public: + ClocksState() + : logger_(rclcpp::get_logger("rclcpp")), + last_time_msg_(std::make_shared()) + { + } -TimeSource::TimeSource() -: ros_time_active_(false) -{ -} + // An internal method to use in the clock callback that iterates and enables all clocks + void enable_ros_time() + { + if (ros_time_active_) { + // already enabled no-op + return; + } -void TimeSource::attachNode(rclcpp::Node::SharedPtr node) -{ - attachNode( - node->get_node_base_interface(), - node->get_node_topics_interface(), - node->get_node_graph_interface(), - node->get_node_services_interface()); -} + // Local storage + ros_time_active_ = true; -void TimeSource::attachNode( - const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, - const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface, - const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface, - const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface) -{ - node_base_ = node_base_interface; - node_topics_ = node_topics_interface; - node_graph_ = node_graph_interface; - node_services_ = node_services_interface; - // TODO(tfoote): Update QOS - - const std::string & topic_name = "/clock"; - - rclcpp::callback_group::CallbackGroup::SharedPtr group; - using rclcpp::message_memory_strategy::MessageMemoryStrategy; - auto msg_mem_strat = MessageMemoryStrategy::create_default(); - auto allocator = std::make_shared(); - - auto cb = std::bind(&TimeSource::clock_cb, this, std::placeholders::_1); - - clock_subscription_ = rclcpp::create_subscription( - node_topics_.get(), - topic_name, - std::move(cb), - rmw_qos_profile_default, - group, - false, - false, - msg_mem_strat, - allocator); - - parameter_client_ = std::make_shared( - node_base_, - node_topics_, - node_graph_, - node_services_ - ); - parameter_subscription_ = - parameter_client_->on_parameter_event(std::bind(&TimeSource::on_parameter_event, - this, std::placeholders::_1)); -} + // Update all attached clocks to zero or last recorded time + set_all_clocks(last_time_msg_, true); + } -void TimeSource::detachNode() -{ - this->ros_time_active_ = false; - clock_subscription_.reset(); - parameter_client_.reset(); - node_base_.reset(); - node_topics_.reset(); - node_graph_.reset(); - node_services_.reset(); - disable_ros_time(); -} + // An internal method to use in the clock callback that iterates and disables all clocks + void disable_ros_time() + { + if (!ros_time_active_) { + // already disabled no-op + return; + } -void TimeSource::attachClock(std::shared_ptr clock) -{ - if (clock->get_clock_type() != RCL_ROS_TIME) { - throw std::invalid_argument("Cannot attach clock to a time source that's not a ROS clock"); + // Local storage + ros_time_active_ = false; + + // Update all attached clocks + auto msg = std::make_shared(); + set_all_clocks(msg, false); } - std::lock_guard guard(clock_list_lock_); - associated_clocks_.push_back(clock); - // Set the clock if there's already data for it - if (last_msg_set_) { - set_clock(last_msg_set_, ros_time_active_, clock); + // Check if ROS time is active + bool is_ros_time_active() const + { + return ros_time_active_; } -} -void TimeSource::detachClock(std::shared_ptr clock) -{ - std::lock_guard guard(clock_list_lock_); - auto result = std::find(associated_clocks_.begin(), associated_clocks_.end(), clock); - if (result != associated_clocks_.end()) { - associated_clocks_.erase(result); - } else { - RCUTILS_LOG_ERROR("Failed to remove clock"); + // Attach a clock + void attachClock(rclcpp::Clock::SharedPtr clock) + { + { + std::lock_guard clock_guard(clock->get_clock_mutex()); + if (clock->get_clock_type() != RCL_ROS_TIME && ros_time_active_) { + throw std::invalid_argument( + "ros_time_active_ can't be true while clock is not of RCL_ROS_TIME type"); + } + } + std::lock_guard guard(clock_list_lock_); + associated_clocks_.insert(clock); + // Set the clock to zero unless there's a recently received message + set_clock(last_time_msg_, ros_time_active_, clock); } -} -TimeSource::~TimeSource() -{ - if (node_base_ || node_topics_ || node_graph_ || node_services_) { - this->detachNode(); + // Detach a clock + void detachClock(rclcpp::Clock::SharedPtr clock) + { + std::lock_guard guard(clock_list_lock_); + auto removed = associated_clocks_.erase(clock); + if (removed == 0) { + RCLCPP_ERROR(logger_, "failed to remove clock"); + } } -} -void TimeSource::set_clock( - const builtin_interfaces::msg::Time::SharedPtr msg, bool set_ros_time_enabled, - std::shared_ptr clock) + // Internal helper function used inside iterators + static void set_clock( + const builtin_interfaces::msg::Time::SharedPtr msg, + bool set_ros_time_enabled, + rclcpp::Clock::SharedPtr clock) + { + std::lock_guard clock_guard(clock->get_clock_mutex()); + + if (clock->get_clock_type() == RCL_ROS_TIME) { + // Do change + if (!set_ros_time_enabled && clock->ros_time_is_active()) { + auto ret = rcl_disable_ros_time_override(clock->get_clock_handle()); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "Failed to disable ros_time_override_status"); + } + } else if (set_ros_time_enabled && !clock->ros_time_is_active()) { + auto ret = rcl_enable_ros_time_override(clock->get_clock_handle()); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "Failed to enable ros_time_override_status"); + } + } + + auto ret = rcl_set_ros_time_override( + clock->get_clock_handle(), + rclcpp::Time(*msg).nanoseconds()); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "Failed to set ros_time_override_status"); + } + } else if (set_ros_time_enabled) { + throw std::invalid_argument( + "set_ros_time_enabled can't be true while clock is not of RCL_ROS_TIME type"); + } + } + + // Internal helper function + void set_all_clocks( + const builtin_interfaces::msg::Time::SharedPtr msg, + bool set_ros_time_enabled) + { + std::lock_guard guard(clock_list_lock_); + for (auto it = associated_clocks_.begin(); it != associated_clocks_.end(); ++it) { + set_clock(msg, set_ros_time_enabled, *it); + } + } + + // Cache the last clock message received + void cache_last_msg(std::shared_ptr msg) + { + last_time_msg_ = std::make_shared(msg->clock); + } + + bool are_all_clocks_rcl_ros_time() + { + std::lock_guard guard(clock_list_lock_); + for (auto & clock : associated_clocks_) { + std::lock_guard clock_guard(clock->get_clock_mutex()); + if (clock->get_clock_type() != RCL_ROS_TIME) { + return false; + } + } + return true; + } + +private: + // Store (and update on node attach) logger for logging. + Logger logger_; + + // A lock to protect iterating the associated_clocks_ field. + std::mutex clock_list_lock_; + // An unordered_set to store references to associated clocks. + std::unordered_set associated_clocks_; + + // Local storage of validity of ROS time + // This is needed when new clocks are added. + bool ros_time_active_{false}; + // Last set message to be passed to newly registered clocks + std::shared_ptr last_time_msg_{nullptr}; +}; + +class TimeSource::NodeState final { - // Compute diff - rclcpp::Time msg_time = rclcpp::Time(*msg); - rclcpp::Time now = clock->now(); - auto diff = now - msg_time; - rclcpp::TimeJump jump; - jump.delta_.nanoseconds = diff.nanoseconds(); - - // Compute jump type - if (clock->ros_time_is_active()) { - if (set_ros_time_enabled) { - jump.jump_type_ = TimeJump::ClockChange_t::ROS_TIME_NO_CHANGE; +public: + NodeState(const rclcpp::QoS & qos, bool use_clock_thread) + : use_clock_thread_(use_clock_thread), + logger_(rclcpp::get_logger("rclcpp")), + qos_(qos) + { + } + + ~NodeState() + { + if ( + node_base_ || node_topics_ || node_graph_ || node_services_ || + node_logging_ || node_clock_ || node_parameters_) + { + detachNode(); + } + } + + // Check if a clock thread will be used + bool get_use_clock_thread() + { + return use_clock_thread_; + } + + // Set whether a clock thread will be used + void set_use_clock_thread(bool use_clock_thread) + { + use_clock_thread_ = use_clock_thread; + } + + // Check if the clock thread is joinable + bool clock_thread_is_joinable() + { + return clock_executor_thread_.joinable(); + } + + // Attach a node to this time source + void attachNode( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface, + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface, + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_interface, + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_interface) + { + std::lock_guard guard(node_base_lock_); + node_base_ = node_base_interface; + node_topics_ = node_topics_interface; + node_graph_ = node_graph_interface; + node_services_ = node_services_interface; + node_logging_ = node_logging_interface; + node_clock_ = node_clock_interface; + node_parameters_ = node_parameters_interface; + // TODO(tfoote): Update QOS + + logger_ = node_logging_->get_logger(); + + // Though this defaults to false, it can be overridden by initial parameter values for the + // node, which may be given by the user at the node's construction or even by command-line + // arguments. + rclcpp::ParameterValue use_sim_time_param; + const std::string use_sim_time_name = "use_sim_time"; + if (!node_parameters_->has_parameter(use_sim_time_name)) { + use_sim_time_param = node_parameters_->declare_parameter( + use_sim_time_name, + rclcpp::ParameterValue(false)); } else { - jump.jump_type_ = TimeJump::ClockChange_t::ROS_TIME_DEACTIVATED; + use_sim_time_param = node_parameters_->get_parameter(use_sim_time_name).get_parameter_value(); } - } else if (!clock->ros_time_is_active()) { - if (set_ros_time_enabled) { - jump.jump_type_ = TimeJump::ClockChange_t::ROS_TIME_ACTIVATED; + if (use_sim_time_param.get_type() == rclcpp::PARAMETER_BOOL) { + if (use_sim_time_param.get()) { + parameter_state_ = SET_TRUE; + clocks_state_.enable_ros_time(); + create_clock_sub(); + } } else { - jump.jump_type_ = TimeJump::ClockChange_t::SYSTEM_TIME_NO_CHANGE; + RCLCPP_ERROR( + logger_, "Invalid type '%s' for parameter 'use_sim_time', should be 'bool'", + rclcpp::to_string(use_sim_time_param.get_type()).c_str()); + throw std::invalid_argument("Invalid type for parameter 'use_sim_time', should be 'bool'"); } + + on_set_parameters_callback_ = node_parameters_->add_on_set_parameters_callback( + std::bind(&TimeSource::NodeState::on_set_parameters, this, std::placeholders::_1)); + + + // TODO(tfoote) use parameters interface not subscribe to events via topic ticketed #609 + parameter_subscription_ = rclcpp::AsyncParametersClient::on_parameter_event( + node_topics_, + [this](std::shared_ptr event) { + this->on_parameter_event(event); + }); } - if (jump.jump_type_ == TimeJump::ClockChange_t::SYSTEM_TIME_NO_CHANGE) { - // No change/no updates don't act. - return; + // Detach the attached node + void detachNode() + { + // destroy_clock_sub() *must* be first here, to ensure that the executor + // can't possibly call any of the callbacks as we are cleaning up. + destroy_clock_sub(); + std::lock_guard guard(node_base_lock_); + clocks_state_.disable_ros_time(); + if (on_set_parameters_callback_) { + node_parameters_->remove_on_set_parameters_callback(on_set_parameters_callback_.get()); + } + on_set_parameters_callback_.reset(); + parameter_subscription_.reset(); + node_base_.reset(); + node_topics_.reset(); + node_graph_.reset(); + node_services_.reset(); + node_logging_.reset(); + node_clock_.reset(); + node_parameters_.reset(); } - auto active_callbacks = clock->get_triggered_callback_handlers(jump); - clock->invoke_prejump_callbacks(active_callbacks); + void attachClock(std::shared_ptr clock) + { + clocks_state_.attachClock(std::move(clock)); + } - // Do change - if (jump.jump_type_ == TimeJump::ClockChange_t::ROS_TIME_DEACTIVATED) { - disable_ros_time(clock); - } else if (jump.jump_type_ == TimeJump::ClockChange_t::ROS_TIME_ACTIVATED) { - enable_ros_time(clock); + void detachClock(std::shared_ptr clock) + { + clocks_state_.detachClock(std::move(clock)); } - if (jump.jump_type_ == TimeJump::ClockChange_t::ROS_TIME_ACTIVATED || - jump.jump_type_ == TimeJump::ClockChange_t::ROS_TIME_NO_CHANGE) +private: + ClocksState clocks_state_; + + // Dedicated thread for clock subscription. + bool use_clock_thread_; + std::thread clock_executor_thread_; + + // Preserve the node reference + std::mutex node_base_lock_; + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_{nullptr}; + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_{nullptr}; + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_{nullptr}; + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_{nullptr}; + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_{nullptr}; + rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_{nullptr}; + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_{nullptr}; + + // Store (and update on node attach) logger for logging. + Logger logger_; + + // QoS of the clock subscription. + rclcpp::QoS qos_; + + // The subscription for the clock callback + using SubscriptionT = rclcpp::Subscription; + std::shared_ptr clock_subscription_{nullptr}; + std::mutex clock_sub_lock_; + rclcpp::CallbackGroup::SharedPtr clock_callback_group_; + rclcpp::executors::SingleThreadedExecutor::SharedPtr clock_executor_; + std::promise cancel_clock_executor_promise_; + + // The clock callback itself + void clock_cb(std::shared_ptr msg) { - auto ret = rcl_set_ros_time_override(&(clock->rcl_clock_), msg_time.nanoseconds()); - if (ret != RCL_RET_OK) { - rclcpp::exceptions::throw_from_rcl_error( - ret, "Failed to set ros_time_override_status"); + if (!clocks_state_.is_ros_time_active() && SET_TRUE == this->parameter_state_) { + clocks_state_.enable_ros_time(); } - } - // Post change callbacks - clock->invoke_postjump_callbacks(active_callbacks, jump); -} + // Cache the last message in case a new clock is attached. + clocks_state_.cache_last_msg(msg); + auto time_msg = std::make_shared(msg->clock); -void TimeSource::clock_cb(const builtin_interfaces::msg::Time::SharedPtr msg) -{ - if (!this->ros_time_active_) { - enable_ros_time(); + if (SET_TRUE == this->parameter_state_) { + clocks_state_.set_all_clocks(time_msg, true); + } } - // Cache the last message in case a new clock is attached. - last_msg_set_ = msg; - std::lock_guard guard(clock_list_lock_); - for (auto it = associated_clocks_.begin(); it != associated_clocks_.end(); ++it) { - set_clock(msg, true, *it); + // Create the subscription for the clock topic + void create_clock_sub() + { + std::lock_guard guard(clock_sub_lock_); + if (clock_subscription_) { + // Subscription already created. + return; + } + + rclcpp::SubscriptionOptions options; + options.qos_overriding_options = rclcpp::QosOverridingOptions( + { + rclcpp::QosPolicyKind::Depth, + rclcpp::QosPolicyKind::Durability, + rclcpp::QosPolicyKind::History, + rclcpp::QosPolicyKind::Reliability, + }); + + if (use_clock_thread_) { + clock_callback_group_ = node_base_->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + false + ); + options.callback_group = clock_callback_group_; + rclcpp::ExecutorOptions exec_options; + exec_options.context = node_base_->get_context(); + clock_executor_ = + std::make_shared(exec_options); + if (!clock_executor_thread_.joinable()) { + cancel_clock_executor_promise_ = std::promise{}; + clock_executor_thread_ = std::thread( + [this]() { + auto future = cancel_clock_executor_promise_.get_future(); + clock_executor_->add_callback_group(clock_callback_group_, node_base_); + clock_executor_->spin_until_future_complete(future); + } + ); + } + } + + clock_subscription_ = rclcpp::create_subscription( + node_parameters_, + node_topics_, + "/clock", + qos_, + [this](std::shared_ptr msg) { + bool execute_cb = false; + { + std::lock_guard guard(node_base_lock_); + // We are using node_base_ as an indication if there is a node attached. + // Only call the clock_cb if that is the case. + execute_cb = node_base_ != nullptr; + } + if (execute_cb) { + clock_cb(msg); + } + }, + options + ); } -} -void TimeSource::on_parameter_event(const rcl_interfaces::msg::ParameterEvent::SharedPtr event) -{ - // Filter for only 'use_sim_time' being added or changed. - rclcpp::ParameterEventsFilter filter(event, {"use_sim_time"}, - {rclcpp::ParameterEventsFilter::EventType::NEW, - rclcpp::ParameterEventsFilter::EventType::CHANGED}); - for (auto & it : filter.get_events()) { - if (it.second->value.type != parameter::ParameterType::PARAMETER_BOOL) { - RCUTILS_LOG_ERROR("use_sim_time parameter set to something besides a bool"); - continue; + // Destroy the subscription for the clock topic + void destroy_clock_sub() + { + std::lock_guard guard(clock_sub_lock_); + if (clock_executor_thread_.joinable()) { + cancel_clock_executor_promise_.set_value(); + clock_executor_->cancel(); + clock_executor_thread_.join(); + clock_executor_->remove_callback_group(clock_callback_group_); } - if (it.second->value.bool_value) { - parameter_state_ = SET_TRUE; - enable_ros_time(); - } else { - parameter_state_ = SET_FALSE; - disable_ros_time(); + clock_subscription_.reset(); + } + + // On set Parameters callback handle + node_interfaces::OnSetParametersCallbackHandle::SharedPtr on_set_parameters_callback_{nullptr}; + + // Parameter Event subscription + using ParamSubscriptionT = rclcpp::Subscription; + std::shared_ptr parameter_subscription_; + + // Callback for parameter settings + rcl_interfaces::msg::SetParametersResult on_set_parameters( + const std::vector & parameters) + { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + for (const auto & param : parameters) { + if (param.get_name() == "use_sim_time" && param.get_type() == rclcpp::PARAMETER_BOOL) { + if (param.as_bool() && !(clocks_state_.are_all_clocks_rcl_ros_time())) { + result.successful = false; + result.reason = + "use_sim_time parameter can't be true while clocks are not all of RCL_ROS_TIME type"; + RCLCPP_ERROR( + logger_, + "use_sim_time parameter can't be true while clocks are not all of RCL_ROS_TIME type"); + } + } } + return result; } - // Handle the case that use_sim_time was deleted. - rclcpp::ParameterEventsFilter deleted(event, {"use_sim_time"}, - {rclcpp::ParameterEventsFilter::EventType::DELETED}); - for (auto & it : deleted.get_events()) { - (void) it; // if there is a match it's already matched, don't bother reading it. - // If the parameter is deleted mark it as unset but dont' change state. - parameter_state_ = UNSET; + + // Callback for parameter updates + void on_parameter_event(std::shared_ptr event) + { + std::lock_guard guard(node_base_lock_); + + if (node_base_ == nullptr) { + // Do nothing if node_base_ is nullptr because it means the TimeSource is now + // without an attached node + return; + } + + // Filter out events on 'use_sim_time' parameter instances in other nodes. + if (event->node != node_base_->get_fully_qualified_name()) { + return; + } + // Filter for only 'use_sim_time' being added or changed. + rclcpp::ParameterEventsFilter filter(event, {"use_sim_time"}, + {rclcpp::ParameterEventsFilter::EventType::NEW, + rclcpp::ParameterEventsFilter::EventType::CHANGED}); + for (auto & it : filter.get_events()) { + if (it.second->value.type != ParameterType::PARAMETER_BOOL) { + RCLCPP_ERROR(logger_, "use_sim_time parameter cannot be set to anything but a bool"); + continue; + } + if (it.second->value.bool_value) { + parameter_state_ = SET_TRUE; + clocks_state_.enable_ros_time(); + create_clock_sub(); + } else { + parameter_state_ = SET_FALSE; + destroy_clock_sub(); + clocks_state_.disable_ros_time(); + } + } + // Handle the case that use_sim_time was deleted. + rclcpp::ParameterEventsFilter deleted(event, {"use_sim_time"}, + {rclcpp::ParameterEventsFilter::EventType::DELETED}); + for (auto & it : deleted.get_events()) { + (void) it; // if there is a match it's already matched, don't bother reading it. + // If the parameter is deleted mark it as unset but don't change state. + parameter_state_ = UNSET; + } } + + // An enum to hold the parameter state + enum UseSimTimeParameterState {UNSET, SET_TRUE, SET_FALSE}; + UseSimTimeParameterState parameter_state_; +}; + +TimeSource::TimeSource( + std::shared_ptr node, + const rclcpp::QoS & qos, + bool use_clock_thread) +: TimeSource(qos, use_clock_thread) +{ + attachNode(node); } -void TimeSource::enable_ros_time(std::shared_ptr clock) +TimeSource::TimeSource( + const rclcpp::QoS & qos, + bool use_clock_thread) +: constructed_use_clock_thread_(use_clock_thread), + constructed_qos_(qos) { - auto ret = rcl_enable_ros_time_override(&clock->rcl_clock_); - if (ret != RCL_RET_OK) { - rclcpp::exceptions::throw_from_rcl_error( - ret, "Failed to enable ros_time_override_status"); - } + node_state_ = std::make_shared(qos, use_clock_thread); } -void TimeSource::disable_ros_time(std::shared_ptr clock) +void TimeSource::attachNode(rclcpp::Node::SharedPtr node) { - auto ret = rcl_disable_ros_time_override(&clock->rcl_clock_); - if (ret != RCL_RET_OK) { - rclcpp::exceptions::throw_from_rcl_error( - ret, "Failed to enable ros_time_override_status"); - } + node_state_->set_use_clock_thread(node->get_node_options().use_clock_thread()); + attachNode( + node->get_node_base_interface(), + node->get_node_topics_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface(), + node->get_node_logging_interface(), + node->get_node_clock_interface(), + node->get_node_parameters_interface()); } -void TimeSource::enable_ros_time() +void TimeSource::attachNode( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface, + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface, + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_interface, + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_interface) { - if (ros_time_active_) { - // already enabled no-op - return; - } + node_state_->attachNode( + std::move(node_base_interface), + std::move(node_topics_interface), + std::move(node_graph_interface), + std::move(node_services_interface), + std::move(node_logging_interface), + std::move(node_clock_interface), + std::move(node_parameters_interface)); +} - // Local storage - ros_time_active_ = true; +void TimeSource::detachNode() +{ + node_state_.reset(); + node_state_ = std::make_shared( + constructed_qos_, + constructed_use_clock_thread_); +} - // Update all attached clocks - std::lock_guard guard(clock_list_lock_); - for (auto it = associated_clocks_.begin(); it != associated_clocks_.end(); ++it) { - auto msg = std::make_shared(); - msg->sec = 0; - msg->nanosec = 0; - set_clock(msg, true, *it); - } +void TimeSource::attachClock(std::shared_ptr clock) +{ + node_state_->attachClock(std::move(clock)); } -void TimeSource::disable_ros_time() +void TimeSource::detachClock(std::shared_ptr clock) { - if (!ros_time_active_) { - // already disabled no-op - return; - } + node_state_->detachClock(std::move(clock)); +} - // Local storage - ros_time_active_ = false; +bool TimeSource::get_use_clock_thread() +{ + return node_state_->get_use_clock_thread(); +} - // Update all attached clocks - std::lock_guard guard(clock_list_lock_); - for (auto it = associated_clocks_.begin(); it != associated_clocks_.end(); ++it) { - auto msg = std::make_shared(); - set_clock(msg, false, *it); - } +void TimeSource::set_use_clock_thread(bool use_clock_thread) +{ + node_state_->set_use_clock_thread(use_clock_thread); +} + +bool TimeSource::clock_thread_is_joinable() +{ + return node_state_->clock_thread_is_joinable(); +} + +TimeSource::~TimeSource() +{ } } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/timer.cpp b/rclcpp/src/rclcpp/timer.cpp index db03d6b69e..0dceb6b8d7 100644 --- a/rclcpp/src/rclcpp/timer.cpp +++ b/rclcpp/src/rclcpp/timer.cpp @@ -16,35 +16,99 @@ #include #include +#include +#include + +#include "rmw/impl/cpp/demangle.hpp" + +#include "rclcpp/contexts/default_context.hpp" +#include "rclcpp/detail/cpp_callback_trampoline.hpp" +#include "rclcpp/exceptions.hpp" +#include "rclcpp/logging.hpp" +#include "rcutils/logging_macros.h" using rclcpp::TimerBase; -TimerBase::TimerBase(std::chrono::nanoseconds period) +TimerBase::TimerBase( + rclcpp::Clock::SharedPtr clock, + std::chrono::nanoseconds period, + rclcpp::Context::SharedPtr context, + bool autostart) +: clock_(clock), timer_handle_(nullptr) { - if (rcl_timer_init( - &timer_handle_, period.count(), nullptr, - rcl_get_default_allocator()) != RCL_RET_OK) + if (nullptr == context) { + context = rclcpp::contexts::get_global_default_context(); + } + + auto rcl_context = context->get_rcl_context(); + + timer_handle_ = std::shared_ptr( + new rcl_timer_t, [ = ](rcl_timer_t * timer) mutable + { + { + std::lock_guard clock_guard(clock->get_clock_mutex()); + if (rcl_timer_fini(timer) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Failed to clean up rcl timer handle: %s", rcl_get_error_string().str); + rcl_reset_error(); + } + } + delete timer; + // Captured shared pointers by copy, reset to make sure timer is finalized before clock + clock.reset(); + rcl_context.reset(); + }); + + *timer_handle_.get() = rcl_get_zero_initialized_timer(); + + rcl_clock_t * clock_handle = clock_->get_clock_handle(); { - fprintf(stderr, "Couldn't initialize rcl timer handle: %s\n", rcl_get_error_string_safe()); + std::lock_guard clock_guard(clock_->get_clock_mutex()); + rcl_ret_t ret = rcl_timer_init2( + timer_handle_.get(), clock_handle, rcl_context.get(), period.count(), + nullptr, rcl_get_default_allocator(), autostart); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't initialize rcl timer handle"); + } } } TimerBase::~TimerBase() -{} +{ + clear_on_reset_callback(); +} void TimerBase::cancel() { - if (rcl_timer_cancel(&timer_handle_) != RCL_RET_OK) { - throw std::runtime_error(std::string("Couldn't cancel timer: ") + rcl_get_error_string_safe()); + rcl_ret_t ret = rcl_timer_cancel(timer_handle_.get()); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't cancel timer"); } } +bool +TimerBase::is_canceled() +{ + bool is_canceled = false; + rcl_ret_t ret = rcl_timer_is_canceled(timer_handle_.get(), &is_canceled); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't get timer cancelled state"); + } + return is_canceled; +} + void TimerBase::reset() { - if (rcl_timer_reset(&timer_handle_) != RCL_RET_OK) { - throw std::runtime_error(std::string("Couldn't reset timer: ") + rcl_get_error_string_safe()); + rcl_ret_t ret = RCL_RET_OK; + { + std::lock_guard lock(callback_mutex_); + ret = rcl_timer_reset(timer_handle_.get()); + } + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't reset timer"); } } @@ -52,8 +116,9 @@ bool TimerBase::is_ready() { bool ready = false; - if (rcl_timer_is_ready(&timer_handle_, &ready) != RCL_RET_OK) { - throw std::runtime_error(std::string("Failed to check timer: ") + rcl_get_error_string_safe()); + rcl_ret_t ret = rcl_timer_is_ready(timer_handle_.get(), &ready); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to check timer"); } return ready; } @@ -62,16 +127,94 @@ std::chrono::nanoseconds TimerBase::time_until_trigger() { int64_t time_until_next_call = 0; - if (rcl_timer_get_time_until_next_call(&timer_handle_, &time_until_next_call) != RCL_RET_OK) { - throw std::runtime_error( - std::string("Timer could not get time until next call: ") + - rcl_get_error_string_safe()); + rcl_ret_t ret = rcl_timer_get_time_until_next_call( + timer_handle_.get(), &time_until_next_call); + if (ret == RCL_RET_TIMER_CANCELED) { + return std::chrono::nanoseconds::max(); + } else if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Timer could not get time until next call"); } return std::chrono::nanoseconds(time_until_next_call); } -const rcl_timer_t * +std::shared_ptr TimerBase::get_timer_handle() { - return &timer_handle_; + return timer_handle_; +} + +bool +TimerBase::exchange_in_use_by_wait_set_state(bool in_use_state) +{ + return in_use_by_wait_set_.exchange(in_use_state); +} + +void +TimerBase::set_on_reset_callback(std::function callback) +{ + if (!callback) { + throw std::invalid_argument( + "The callback passed to set_on_reset_callback " + "is not callable."); + } + + auto new_callback = + [callback, this](size_t reset_calls) { + try { + callback(reset_calls); + } catch (const std::exception & exception) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("rclcpp"), + "rclcpp::TimerBase@" << this << + " caught " << rmw::impl::cpp::demangle(exception) << + " exception in user-provided callback for the 'on reset' callback: " << + exception.what()); + } catch (...) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("rclcpp"), + "rclcpp::TimerBase@" << this << + " caught unhandled exception in user-provided callback " << + "for the 'on reset' callback"); + } + }; + + std::lock_guard lock(callback_mutex_); + + // Set it temporarily to the new callback, while we replace the old one. + // This two-step setting, prevents a gap where the old std::function has + // been replaced but rcl hasn't been told about the new one yet. + set_on_reset_callback( + rclcpp::detail::cpp_callback_trampoline< + decltype(new_callback), const void *, size_t>, + static_cast(&new_callback)); + + // Store the std::function to keep it in scope, also overwrites the existing one. + on_reset_callback_ = new_callback; + + // Set it again, now using the permanent storage. + set_on_reset_callback( + rclcpp::detail::cpp_callback_trampoline< + decltype(on_reset_callback_), const void *, size_t>, + static_cast(&on_reset_callback_)); +} + +void +TimerBase::clear_on_reset_callback() +{ + std::lock_guard lock(callback_mutex_); + + if (on_reset_callback_) { + set_on_reset_callback(nullptr, nullptr); + on_reset_callback_ = nullptr; + } +} + +void +TimerBase::set_on_reset_callback(rcl_event_callback_t callback, const void * user_data) +{ + rcl_ret_t ret = rcl_timer_set_on_reset_callback(timer_handle_.get(), callback, user_data); + + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to set timer on reset callback"); + } } diff --git a/rclcpp/src/rclcpp/type_support.cpp b/rclcpp/src/rclcpp/type_support.cpp index 27dfccb98b..a77c4f8f19 100644 --- a/rclcpp/src/rclcpp/type_support.cpp +++ b/rclcpp/src/rclcpp/type_support.cpp @@ -15,7 +15,6 @@ #include "rclcpp/type_support_decl.hpp" #include "rclcpp/visibility_control.hpp" -#include "rcl_interfaces/msg/intra_process_message.hpp" #include "rcl_interfaces/msg/list_parameters_result.hpp" #include "rcl_interfaces/msg/parameter_descriptor.hpp" #include "rcl_interfaces/msg/parameter_event.hpp" @@ -27,14 +26,6 @@ #include "rcl_interfaces/srv/set_parameters.hpp" #include "rcl_interfaces/srv/set_parameters_atomically.hpp" -const rosidl_message_type_support_t * -rclcpp::type_support::get_intra_process_message_msg_type_support() -{ - return rosidl_typesupport_cpp::get_message_type_support_handle< - rcl_interfaces::msg::IntraProcessMessage - >(); -} - const rosidl_message_type_support_t * rclcpp::type_support::get_parameter_event_msg_type_support() { diff --git a/rclcpp/src/rclcpp/typesupport_helpers.cpp b/rclcpp/src/rclcpp/typesupport_helpers.cpp new file mode 100644 index 0000000000..04dd8c4c9f --- /dev/null +++ b/rclcpp/src/rclcpp/typesupport_helpers.cpp @@ -0,0 +1,180 @@ +// Copyright 2018, Bosch Software Innovations GmbH. +// Copyright 2021, Apex.AI 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 "rclcpp/typesupport_helpers.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include "ament_index_cpp/get_package_prefix.hpp" +#include "ament_index_cpp/get_resources.hpp" +#include "rcpputils/shared_library.hpp" +#include "rcpputils/find_library.hpp" +#include "rosidl_runtime_cpp/message_type_support_decl.hpp" + +namespace rclcpp +{ + +namespace +{ + +// Look for the library in the ament prefix paths. +std::string get_typesupport_library_path( + const std::string & package_name, const std::string & typesupport_identifier) +{ + const char * dynamic_library_folder; +#ifdef _WIN32 + dynamic_library_folder = "/bin/"; +#elif __APPLE__ + dynamic_library_folder = "/lib/"; +#else + dynamic_library_folder = "/lib/"; +#endif + + std::string package_prefix; + try { + package_prefix = ament_index_cpp::get_package_prefix(package_name); + } catch (ament_index_cpp::PackageNotFoundError & e) { + throw std::runtime_error(e.what()); + } + + const std::string library_path = rcpputils::path_for_library( + package_prefix + dynamic_library_folder, + package_name + "__" + typesupport_identifier); + if (library_path.empty()) { + throw std::runtime_error( + "Typesupport library for " + package_name + " does not exist in '" + package_prefix + + "'."); + } + return library_path; +} + +std::tuple +extract_type_identifier(const std::string & full_type) +{ + char type_separator = '/'; + auto sep_position_back = full_type.find_last_of(type_separator); + auto sep_position_front = full_type.find_first_of(type_separator); + if (sep_position_back == std::string::npos || + sep_position_back == 0 || + sep_position_back == full_type.length() - 1) + { + throw std::runtime_error( + "Message type is not of the form package/type and cannot be processed"); + } + + std::string package_name = full_type.substr(0, sep_position_front); + std::string middle_module = ""; + if (sep_position_back - sep_position_front > 0) { + middle_module = + full_type.substr(sep_position_front + 1, sep_position_back - sep_position_front - 1); + } + std::string type_name = full_type.substr(sep_position_back + 1); + + return std::make_tuple(package_name, middle_module, type_name); +} + +const void * get_typesupport_handle_impl( + const std::string & type, + const std::string & typesupport_identifier, + const std::string & typesupport_name, + const std::string & symbol_part_name, + const std::string & middle_module_additional, + rcpputils::SharedLibrary & library) +{ + std::string package_name; + std::string middle_module; + std::string type_name; + std::tie(package_name, middle_module, type_name) = extract_type_identifier(type); + + if (middle_module.empty()) { + middle_module = middle_module_additional; + } + + auto mk_error = [&package_name, &type_name, &typesupport_name](auto reason) { + std::stringstream rcutils_dynamic_loading_error; + rcutils_dynamic_loading_error << + "Something went wrong loading the typesupport library for " << + typesupport_name << " type " << package_name << + "/" << type_name << ". " << reason; + return rcutils_dynamic_loading_error.str(); + }; + + try { + std::string symbol_name = typesupport_identifier + symbol_part_name + + package_name + "__" + middle_module + "__" + type_name; + const void * (* get_ts)() = nullptr; + // This will throw runtime_error if the symbol was not found. + get_ts = reinterpret_cast(library.get_symbol(symbol_name)); + return get_ts(); + } catch (std::runtime_error &) { + throw std::runtime_error{mk_error("Library could not be found.")}; + } +} + +} // anonymous namespace + +std::shared_ptr +get_typesupport_library(const std::string & type, const std::string & typesupport_identifier) +{ + auto package_name = std::get<0>(extract_type_identifier(type)); + auto library_path = get_typesupport_library_path(package_name, typesupport_identifier); + return std::make_shared(library_path); +} + +const rosidl_message_type_support_t * get_typesupport_handle( + const std::string & type, + const std::string & typesupport_identifier, + rcpputils::SharedLibrary & library) +{ + return get_message_typesupport_handle(type, typesupport_identifier, library); +} + +const rosidl_message_type_support_t * get_message_typesupport_handle( + const std::string & type, + const std::string & typesupport_identifier, + rcpputils::SharedLibrary & library) +{ + static const std::string typesupport_name = "message"; + static const std::string symbol_part_name = "__get_message_type_support_handle__"; + static const std::string middle_module_additional = "msg"; + + return static_cast(get_typesupport_handle_impl( + type, typesupport_identifier, typesupport_name, symbol_part_name, + middle_module_additional, library + )); +} + +const rosidl_service_type_support_t * get_service_typesupport_handle( + const std::string & type, + const std::string & typesupport_identifier, + rcpputils::SharedLibrary & library) +{ + static const std::string typesupport_name = "service"; + static const std::string symbol_part_name = "__get_service_type_support_handle__"; + static const std::string middle_module_additional = "srv"; + + return static_cast(get_typesupport_handle_impl( + type, typesupport_identifier, typesupport_name, symbol_part_name, + middle_module_additional, library + )); +} + +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/utilities.cpp b/rclcpp/src/rclcpp/utilities.cpp index e387f6f92c..f300060010 100644 --- a/rclcpp/src/rclcpp/utilities.cpp +++ b/rclcpp/src/rclcpp/utilities.cpp @@ -14,266 +14,217 @@ #include "rclcpp/utilities.hpp" -#include -#include -#include -#include -#include -#include -#include +#include +#include #include #include +#include "./signal_handler.hpp" +#include "rclcpp/contexts/default_context.hpp" +#include "rclcpp/detail/utilities.hpp" +#include "rclcpp/exceptions.hpp" + #include "rcl/error_handling.h" #include "rcl/rcl.h" -#include "rmw/error_handling.h" -#include "rmw/rmw.h" - -// Determine if sigaction is available -#if __APPLE__ || _POSIX_C_SOURCE >= 1 || _XOPEN_SOURCE || _POSIX_SOURCE -#define HAS_SIGACTION -#endif - -/// Represent the status of the global interrupt signal. -static volatile sig_atomic_t g_signal_status = 0; -/// Guard conditions for interrupting the rmw implementation when the global interrupt signal fired. -static std::map g_sigint_guard_cond_handles; -/// Mutex to protect g_sigint_guard_cond_handles -static std::mutex g_sigint_guard_cond_handles_mutex; -/// Condition variable for timed sleep (see sleep_for). -static std::condition_variable g_interrupt_condition_variable; -static std::atomic g_is_interrupted(false); -/// Mutex for protecting the global condition variable. -static std::mutex g_interrupt_mutex; - -#ifdef HAS_SIGACTION -static struct sigaction old_action; -#else -typedef void (* signal_handler_t)(int); -static signal_handler_t old_signal_handler = 0; -#endif - -#ifdef HAS_SIGACTION -struct sigaction -set_sigaction(int signal_value, const struct sigaction & action) -#else -signal_handler_t -set_signal_handler(int signal_value, signal_handler_t signal_handler) -#endif +namespace rclcpp { -#ifdef HAS_SIGACTION - struct sigaction old_action; - ssize_t ret = sigaction(signal_value, &action, &old_action); - if (ret == -1) -#else - signal_handler_t old_signal_handler = std::signal(signal_value, signal_handler); - // NOLINTNEXTLINE(readability/braces) - if (old_signal_handler == SIG_ERR) -#endif - { - const size_t error_length = 1024; - // NOLINTNEXTLINE(runtime/arrays) - char error_string[error_length]; -#ifndef _WIN32 -#if (defined(_GNU_SOURCE) && !defined(ANDROID)) - char * msg = strerror_r(errno, error_string, error_length); - if (msg != error_string) { - strncpy(error_string, msg, error_length); - msg[error_length - 1] = '\0'; - } -#else - int error_status = strerror_r(errno, error_string, error_length); - if (error_status != 0) { - throw std::runtime_error("Failed to get error string for errno: " + std::to_string(errno)); - } -#endif -#else - strerror_s(error_string, error_length, errno); -#endif - // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here) - throw std::runtime_error( - std::string("Failed to set SIGINT signal handler: (" + std::to_string(errno) + ")") + - error_string); - // *INDENT-ON* - } -#ifdef HAS_SIGACTION - return old_action; -#else - return old_signal_handler; -#endif +void +init( + int argc, + char const * const * argv, + const InitOptions & init_options, + SignalHandlerOptions signal_handler_options) +{ + using rclcpp::contexts::get_global_default_context; + get_global_default_context()->init(argc, argv, init_options); + // Install the signal handlers. + install_signal_handlers(signal_handler_options); } -void -trigger_interrupt_guard_condition(int signal_value) +bool +install_signal_handlers(SignalHandlerOptions signal_handler_options) { - g_signal_status = signal_value; - { - std::lock_guard lock(g_sigint_guard_cond_handles_mutex); - for (auto & kv : g_sigint_guard_cond_handles) { - rcl_ret_t status = rcl_trigger_guard_condition(&(kv.second)); - if (status != RCL_RET_OK) { - fprintf(stderr, - "[rclcpp::error] failed to trigger guard condition: %s\n", rcl_get_error_string_safe()); - } - } - } - g_is_interrupted.store(true); - g_interrupt_condition_variable.notify_all(); + return SignalHandler::get_global_signal_handler().install(signal_handler_options); } -void -#ifdef HAS_SIGACTION -signal_handler(int signal_value, siginfo_t * siginfo, void * context) -#else -signal_handler(int signal_value) -#endif +bool +signal_handlers_installed() { - // TODO(wjwwood): remove? move to console logging at some point? - printf("signal_handler(%d)\n", signal_value); + return SignalHandler::get_global_signal_handler().is_installed(); +} -#ifdef HAS_SIGACTION - if (old_action.sa_flags & SA_SIGINFO) { - if (old_action.sa_sigaction != NULL) { - old_action.sa_sigaction(signal_value, siginfo, context); - } - } else { - if ( - old_action.sa_handler != NULL && // Is set - old_action.sa_handler != SIG_DFL && // Is not default - old_action.sa_handler != SIG_IGN) // Is not ignored - { - old_action.sa_handler(signal_value); - } - } -#else - if (old_signal_handler) { - old_signal_handler(signal_value); - } -#endif +SignalHandlerOptions +get_current_signal_handler_options() +{ + return SignalHandler::get_global_signal_handler().get_current_signal_handler_options(); +} - trigger_interrupt_guard_condition(signal_value); + +bool +uninstall_signal_handlers() +{ + return SignalHandler::get_global_signal_handler().uninstall(); } -void -rclcpp::init(int argc, char * argv[]) +static +std::vector +_remove_ros_arguments( + char const * const * argv, + const rcl_arguments_t * args, + rcl_allocator_t alloc) { - g_is_interrupted.store(false); - if (rcl_init(argc, argv, rcl_get_default_allocator()) != RCL_RET_OK) { - std::string msg = "failed to initialize rmw implementation: "; - msg += rcl_get_error_string_safe(); + rcl_ret_t ret; + int nonros_argc = 0; + const char ** nonros_argv = NULL; + + ret = rcl_remove_ros_arguments( + argv, + args, + alloc, + &nonros_argc, + &nonros_argv); + + if (RCL_RET_OK != ret || nonros_argc < 0) { + // Not using throw_from_rcl_error, because we may need to append deallocation failures. + exceptions::RCLError exc(ret, rcl_get_error_state(), ""); rcl_reset_error(); - throw std::runtime_error(msg); + if (NULL != nonros_argv) { + alloc.deallocate(nonros_argv, alloc.state); + } + throw exc; + } + + std::vector return_arguments(static_cast(nonros_argc)); + + for (size_t ii = 0; ii < static_cast(nonros_argc); ++ii) { + return_arguments[ii] = std::string(nonros_argv[ii]); } -#ifdef HAS_SIGACTION - struct sigaction action; - memset(&action, 0, sizeof(action)); - sigemptyset(&action.sa_mask); - action.sa_sigaction = ::signal_handler; - action.sa_flags = SA_SIGINFO; - ::old_action = set_sigaction(SIGINT, action); - // Register an on_shutdown hook to restore the old action. - rclcpp::on_shutdown( - []() { - set_sigaction(SIGINT, ::old_action); - }); -#else - ::old_signal_handler = set_signal_handler(SIGINT, ::signal_handler); - // Register an on_shutdown hook to restore the old signal handler. - rclcpp::on_shutdown( - []() { - set_signal_handler(SIGINT, ::old_signal_handler); - }); -#endif + + if (NULL != nonros_argv) { + alloc.deallocate(nonros_argv, alloc.state); + } + + return return_arguments; } -bool -rclcpp::ok() +std::vector +init_and_remove_ros_arguments( + int argc, + char const * const * argv, + const InitOptions & init_options) { - return ::g_signal_status == 0; -} + init(argc, argv, init_options); -static std::mutex on_shutdown_mutex_; -static std::vector> on_shutdown_callbacks_; + using rclcpp::contexts::get_global_default_context; + auto rcl_context = get_global_default_context()->get_rcl_context(); + return _remove_ros_arguments(argv, &(rcl_context->global_arguments), rcl_get_default_allocator()); +} -void -rclcpp::shutdown() +std::vector +remove_ros_arguments(int argc, char const * const * argv) { - trigger_interrupt_guard_condition(SIGINT); + rcl_allocator_t alloc = rcl_get_default_allocator(); + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); - { - std::lock_guard lock(on_shutdown_mutex_); - for (auto & on_shutdown_callback : on_shutdown_callbacks_) { - on_shutdown_callback(); + rcl_ret_t ret; + + ret = rcl_parse_arguments(argc, argv, alloc, &parsed_args); + if (RCL_RET_OK != ret) { + exceptions::throw_from_rcl_error(ret, "failed to parse arguments"); + } + + std::vector return_arguments; + try { + return_arguments = _remove_ros_arguments(argv, &parsed_args, alloc); + } catch (exceptions::RCLError & exc) { + if (RCL_RET_OK != rcl_arguments_fini(&parsed_args)) { + exc.formatted_message += std::string( + ", failed also to cleanup parsed arguments, leaking memory: ") + + rcl_get_error_string().str; + rcl_reset_error(); } + throw exc; } + + ret = rcl_arguments_fini(&parsed_args); + if (RCL_RET_OK != ret) { + exceptions::throw_from_rcl_error( + ret, "failed to cleanup parsed arguments, leaking memory"); + } + + return return_arguments; } -void -rclcpp::on_shutdown(std::function callback) +bool +ok(Context::SharedPtr context) { - std::lock_guard lock(on_shutdown_mutex_); - on_shutdown_callbacks_.push_back(callback); + using rclcpp::contexts::get_global_default_context; + if (nullptr == context) { + context = get_global_default_context(); + } + return context->is_valid(); } -rcl_guard_condition_t * -rclcpp::get_sigint_guard_condition(rcl_wait_set_t * wait_set) +bool +shutdown(Context::SharedPtr context, const std::string & reason) { - std::lock_guard lock(g_sigint_guard_cond_handles_mutex); - auto kv = g_sigint_guard_cond_handles.find(wait_set); - if (kv != g_sigint_guard_cond_handles.end()) { - return &kv->second; - } else { - rcl_guard_condition_t handle = - rcl_get_zero_initialized_guard_condition(); - rcl_guard_condition_options_t options = rcl_guard_condition_get_default_options(); - if (rcl_guard_condition_init(&handle, options) != RCL_RET_OK) { - // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here) - throw std::runtime_error(std::string( - "Couldn't initialize guard condition: ") + rcl_get_error_string_safe()); - // *INDENT-ON* - } - g_sigint_guard_cond_handles[wait_set] = handle; - return &g_sigint_guard_cond_handles[wait_set]; + using rclcpp::contexts::get_global_default_context; + auto default_context = get_global_default_context(); + if (nullptr == context) { + context = default_context; } + bool ret = context->shutdown(reason); + if (context == default_context) { + uninstall_signal_handlers(); + } + return ret; } void -rclcpp::release_sigint_guard_condition(rcl_wait_set_t * wait_set) +on_shutdown(std::function callback, Context::SharedPtr context) { - std::lock_guard lock(g_sigint_guard_cond_handles_mutex); - auto kv = g_sigint_guard_cond_handles.find(wait_set); - if (kv != g_sigint_guard_cond_handles.end()) { - if (rcl_guard_condition_fini(&kv->second) != RCL_RET_OK) { - // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here) - throw std::runtime_error(std::string( - "Failed to destroy sigint guard condition: ") + - rcl_get_error_string_safe()); - // *INDENT-ON* - } - g_sigint_guard_cond_handles.erase(kv); - } else { - // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here) - throw std::runtime_error(std::string( - "Tried to release sigint guard condition for nonexistent wait set")); - // *INDENT-ON* + using rclcpp::contexts::get_global_default_context; + if (nullptr == context) { + context = get_global_default_context(); } + context->on_shutdown(callback); } bool -rclcpp::sleep_for(const std::chrono::nanoseconds & nanoseconds) +sleep_for(const std::chrono::nanoseconds & nanoseconds, Context::SharedPtr context) { - std::chrono::nanoseconds time_left = nanoseconds; - { - std::unique_lock lock(::g_interrupt_mutex); - auto start = std::chrono::steady_clock::now(); - ::g_interrupt_condition_variable.wait_for(lock, nanoseconds); - time_left -= std::chrono::steady_clock::now() - start; + using rclcpp::contexts::get_global_default_context; + if (nullptr == context) { + context = get_global_default_context(); } - if (time_left > std::chrono::nanoseconds::zero() && !g_is_interrupted) { - return sleep_for(time_left); + return context->sleep_for(nanoseconds); +} + +const char * +get_c_string(const char * string_in) +{ + return string_in; +} + +const char * +get_c_string(const std::string & string_in) +{ + return string_in.c_str(); +} + +std::vector +get_c_vector_string(const std::vector & strings_in) +{ + std::vector cstrings; + cstrings.reserve(strings_in.size()); + + for (size_t i = 0; i < strings_in.size(); ++i) { + cstrings.push_back(strings_in[i].c_str()); } - // Return true if the timeout elapsed successfully, otherwise false. - return !g_is_interrupted; + + return cstrings; } + +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.cpp b/rclcpp/src/rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.cpp new file mode 100644 index 0000000000..5640ad4d29 --- /dev/null +++ b/rclcpp/src/rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.cpp @@ -0,0 +1,100 @@ +// 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 "rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.hpp" + +namespace rclcpp +{ +namespace wait_set_policies +{ +namespace detail +{ + +WritePreferringReadWriteLock::WritePreferringReadWriteLock( + std::function enter_waiting_function) +: read_mutex_(*this), write_mutex_(*this), enter_waiting_function_(enter_waiting_function) +{} + +WritePreferringReadWriteLock::ReadMutex & +WritePreferringReadWriteLock::get_read_mutex() +{ + return read_mutex_; +} + +WritePreferringReadWriteLock::WriteMutex & +WritePreferringReadWriteLock::get_write_mutex() +{ + return write_mutex_; +} + +WritePreferringReadWriteLock::ReadMutex::ReadMutex(WritePreferringReadWriteLock & parent_lock) +: parent_lock_(parent_lock) +{} + +void +WritePreferringReadWriteLock::ReadMutex::lock() +{ + std::unique_lock lock(parent_lock_.mutex_); + while ( + parent_lock_.number_of_writers_waiting_ > 0 || + parent_lock_.writer_active_ || + parent_lock_.reader_active_) + { + parent_lock_.condition_variable_.wait(lock); + } + parent_lock_.reader_active_ = true; + // implicit unlock of parent_lock_.mutex_ +} + +void +WritePreferringReadWriteLock::ReadMutex::unlock() +{ + std::unique_lock lock(parent_lock_.mutex_); + parent_lock_.reader_active_ = false; + parent_lock_.condition_variable_.notify_all(); + // implicit unlock of parent_lock_.mutex_ +} + +WritePreferringReadWriteLock::WriteMutex::WriteMutex(WritePreferringReadWriteLock & parent_lock) +: parent_lock_(parent_lock) +{} + +void +WritePreferringReadWriteLock::WriteMutex::lock() +{ + std::unique_lock lock(parent_lock_.mutex_); + parent_lock_.number_of_writers_waiting_ += 1; + if (nullptr != parent_lock_.enter_waiting_function_) { + parent_lock_.enter_waiting_function_(); + } + while (parent_lock_.reader_active_ || parent_lock_.writer_active_) { + parent_lock_.condition_variable_.wait(lock); + } + parent_lock_.number_of_writers_waiting_ -= 1; + parent_lock_.writer_active_ = true; + // implicit unlock of parent_lock_.mutex_ +} + +void +WritePreferringReadWriteLock::WriteMutex::unlock() +{ + std::unique_lock lock(parent_lock_.mutex_); + parent_lock_.writer_active_ = false; + parent_lock_.condition_variable_.notify_all(); + // implicit unlock of parent_lock_.mutex_ +} + +} // namespace detail +} // namespace wait_set_policies +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/waitable.cpp b/rclcpp/src/rclcpp/waitable.cpp new file mode 100644 index 0000000000..ef3a50a8d9 --- /dev/null +++ b/rclcpp/src/rclcpp/waitable.cpp @@ -0,0 +1,61 @@ +// 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 "rclcpp/waitable.hpp" + +using rclcpp::Waitable; + +size_t +Waitable::get_number_of_ready_subscriptions() +{ + return 0u; +} + +size_t +Waitable::get_number_of_ready_timers() +{ + return 0u; +} + +size_t +Waitable::get_number_of_ready_clients() +{ + return 0u; +} + +size_t +Waitable::get_number_of_ready_events() +{ + return 0u; +} + +size_t +Waitable::get_number_of_ready_services() +{ + return 0u; +} + +size_t +Waitable::get_number_of_ready_guard_conditions() +{ + return 0u; +} + +bool +Waitable::exchange_in_use_by_wait_set_state(bool in_use_state) +{ + return in_use_by_wait_set_.exchange(in_use_state); +} diff --git a/rclcpp/test/CMakeLists.txt b/rclcpp/test/CMakeLists.txt new file mode 100644 index 0000000000..c5be0d8103 --- /dev/null +++ b/rclcpp/test/CMakeLists.txt @@ -0,0 +1,15 @@ +find_package(rosidl_default_generators REQUIRED) + +find_package(test_msgs REQUIRED) + +include(cmake/rclcpp_add_build_failure_test.cmake) + +set(TEST_RESOURCES_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/resources") + +add_subdirectory(benchmark) +add_subdirectory(rclcpp) + +ament_add_gtest(test_rclcpp_gtest_macros utils/test_rclcpp_gtest_macros.cpp) +if(TARGET test_rclcpp_gtest_macros) + target_link_libraries(test_rclcpp_gtest_macros ${PROJECT_NAME}) +endif() diff --git a/rclcpp/test/benchmark/CMakeLists.txt b/rclcpp/test/benchmark/CMakeLists.txt new file mode 100644 index 0000000000..6b93711df2 --- /dev/null +++ b/rclcpp/test/benchmark/CMakeLists.txt @@ -0,0 +1,41 @@ +find_package(ament_cmake_google_benchmark REQUIRED) +find_package(performance_test_fixture REQUIRED) + +# These benchmarks are only being created and run for the default RMW +# implementation. We are looking to test the performance of the ROS 2 code, not +# the underlying middleware. + +add_performance_test(benchmark_client benchmark_client.cpp) +if(TARGET benchmark_client) + target_link_libraries(benchmark_client ${PROJECT_NAME} ${rcl_interfaces_TARGETS} ${test_msgs_TARGETS}) +endif() + +add_performance_test(benchmark_executor benchmark_executor.cpp) +if(TARGET benchmark_executor) + target_link_libraries(benchmark_executor ${PROJECT_NAME} ${test_msgs_TARGETS}) +endif() + +add_performance_test(benchmark_init_shutdown benchmark_init_shutdown.cpp) +if(TARGET benchmark_init_shutdown) + target_link_libraries(benchmark_init_shutdown ${PROJECT_NAME}) +endif() + +add_performance_test(benchmark_node benchmark_node.cpp) +if(TARGET benchmark_node) + target_link_libraries(benchmark_node ${PROJECT_NAME}) +endif() + +add_performance_test(benchmark_node_parameters_interface benchmark_node_parameters_interface.cpp) +if(TARGET benchmark_node_parameters_interface) + target_link_libraries(benchmark_node_parameters_interface ${PROJECT_NAME}) +endif() + +ament_add_google_benchmark(benchmark_parameter_client benchmark_parameter_client.cpp) +if(TARGET benchmark_parameter_client) + target_link_libraries(benchmark_parameter_client ${PROJECT_NAME}) +endif() + +add_performance_test(benchmark_service benchmark_service.cpp) +if(TARGET benchmark_service) + target_link_libraries(benchmark_service ${PROJECT_NAME} ${rcl_interfaces_TARGETS} ${test_msgs_TARGETS}) +endif() diff --git a/rclcpp/test/benchmark/benchmark_client.cpp b/rclcpp/test/benchmark/benchmark_client.cpp new file mode 100644 index 0000000000..26ee58b633 --- /dev/null +++ b/rclcpp/test/benchmark/benchmark_client.cpp @@ -0,0 +1,159 @@ +// 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 "performance_test_fixture/performance_test_fixture.hpp" +#include "rclcpp/rclcpp.hpp" +#include "test_msgs/srv/empty.hpp" + +using performance_test_fixture::PerformanceTest; + +constexpr char empty_service_name[] = "empty_service"; + +class ClientPerformanceTest : public PerformanceTest +{ +public: + explicit ClientPerformanceTest(rclcpp::NodeOptions = rclcpp::NodeOptions()) {} + void SetUp(benchmark::State & state) + { + rclcpp::init(0, nullptr); + node = std::make_unique("node", "ns"); + + auto empty_service_callback = + []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}; + empty_service = + node->create_service(empty_service_name, empty_service_callback); + + performance_test_fixture::PerformanceTest::SetUp(state); + } + + void TearDown(benchmark::State & state) + { + performance_test_fixture::PerformanceTest::TearDown(state); + empty_service.reset(); + node.reset(); + rclcpp::shutdown(); + } + +protected: + std::unique_ptr node; + std::shared_ptr> empty_service; +}; + +BENCHMARK_F(ClientPerformanceTest, construct_client_no_service)(benchmark::State & state) { + // Prime cache + auto outer_client = node->create_client("not_an_existing_service"); + outer_client.reset(); + + reset_heap_counters(); + for (auto _ : state) { + (void)_; + auto client = node->create_client("not_an_existing_service"); + benchmark::DoNotOptimize(client); + benchmark::ClobberMemory(); + + state.PauseTiming(); + client.reset(); + state.ResumeTiming(); + } +} + +BENCHMARK_F(ClientPerformanceTest, construct_client_empty_srv)(benchmark::State & state) { + // Prime cache + auto outer_client = node->create_client(empty_service_name); + outer_client.reset(); + + reset_heap_counters(); + for (auto _ : state) { + (void)_; + auto client = node->create_client(empty_service_name); + benchmark::DoNotOptimize(client); + benchmark::ClobberMemory(); + + state.PauseTiming(); + client.reset(); + state.ResumeTiming(); + } +} + +BENCHMARK_F(ClientPerformanceTest, destroy_client_empty_srv)(benchmark::State & state) { + // Prime cache + auto outer_client = node->create_client(empty_service_name); + outer_client.reset(); + + reset_heap_counters(); + for (auto _ : state) { + (void)_; + state.PauseTiming(); + auto client = node->create_client(empty_service_name); + state.ResumeTiming(); + benchmark::DoNotOptimize(client); + benchmark::ClobberMemory(); + + client.reset(); + } +} + +BENCHMARK_F(ClientPerformanceTest, wait_for_service)(benchmark::State & state) { + int count = 0; + for (auto _ : state) { + (void)_; + state.PauseTiming(); + const std::string service_name = std::string("service_") + std::to_string(count++); + // Create client before service so it has to 'discover' the service after construction + auto client = node->create_client(service_name); + auto callback = + []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}; + auto service = + node->create_service(service_name, callback); + state.ResumeTiming(); + + client->wait_for_service(std::chrono::seconds(1)); + benchmark::ClobberMemory(); + } +} + +BENCHMARK_F(ClientPerformanceTest, async_send_request_only)(benchmark::State & state) { + auto client = node->create_client(empty_service_name); + auto shared_request = std::make_shared(); + + reset_heap_counters(); + for (auto _ : state) { + (void)_; + auto future = client->async_send_request(shared_request); + benchmark::DoNotOptimize(future); + benchmark::ClobberMemory(); + } +} + +BENCHMARK_F(ClientPerformanceTest, async_send_request_and_response)(benchmark::State & state) { + auto client = node->create_client(empty_service_name); + auto shared_request = std::make_shared(); + + reset_heap_counters(); + for (auto _ : state) { + (void)_; + auto future = client->async_send_request(shared_request); + rclcpp::spin_until_future_complete( + node->get_node_base_interface(), future, std::chrono::seconds(1)); + benchmark::DoNotOptimize(future); + benchmark::ClobberMemory(); + } +} diff --git a/rclcpp/test/benchmark/benchmark_executor.cpp b/rclcpp/test/benchmark/benchmark_executor.cpp new file mode 100644 index 0000000000..ea7afa8696 --- /dev/null +++ b/rclcpp/test/benchmark/benchmark_executor.cpp @@ -0,0 +1,275 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "performance_test_fixture/performance_test_fixture.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rcpputils/scope_exit.hpp" +#include "test_msgs/msg/empty.hpp" + +using namespace std::chrono_literals; +using performance_test_fixture::PerformanceTest; + +constexpr unsigned int kNumberOfNodes = 10; + +class PerformanceTestExecutor : public PerformanceTest +{ +public: + void SetUp(benchmark::State & st) + { + rclcpp::init(0, nullptr); + callback_count = 0; + for (unsigned int i = 0u; i < kNumberOfNodes; i++) { + nodes.push_back(std::make_shared("my_node_" + std::to_string(i))); + + publishers.push_back( + nodes[i]->create_publisher( + "/empty_msgs_" + std::to_string(i), rclcpp::QoS(10))); + + auto callback = [this](test_msgs::msg::Empty::ConstSharedPtr) {this->callback_count++;}; + subscriptions.push_back( + nodes[i]->create_subscription( + "/empty_msgs_" + std::to_string(i), rclcpp::QoS(10), std::move(callback))); + } + PerformanceTest::SetUp(st); + } + void TearDown(benchmark::State & st) + { + PerformanceTest::TearDown(st); + subscriptions.clear(); + publishers.clear(); + nodes.clear(); + rclcpp::shutdown(); + } + + test_msgs::msg::Empty empty_msgs; + std::vector nodes; + std::vector::SharedPtr> publishers; + std::vector::SharedPtr> subscriptions; + int callback_count; +}; + +BENCHMARK_F(PerformanceTestExecutor, single_thread_executor_spin_some)(benchmark::State & st) +{ + rclcpp::executors::SingleThreadedExecutor executor; + for (unsigned int i = 0u; i < kNumberOfNodes; i++) { + executor.add_node(nodes[i]); + publishers[i]->publish(empty_msgs); + executor.spin_some(100ms); + } + + callback_count = 0; + reset_heap_counters(); + + for (auto _ : st) { + (void)_; + st.PauseTiming(); + for (unsigned int i = 0u; i < kNumberOfNodes; i++) { + publishers[i]->publish(empty_msgs); + } + st.ResumeTiming(); + + executor.spin_some(100ms); + } + if (callback_count == 0) { + st.SkipWithError("No message was received"); + } +} + +BENCHMARK_F(PerformanceTestExecutor, multi_thread_executor_spin_some)(benchmark::State & st) +{ + rclcpp::executors::MultiThreadedExecutor executor; + for (unsigned int i = 0u; i < kNumberOfNodes; i++) { + executor.add_node(nodes[i]); + publishers[i]->publish(empty_msgs); + executor.spin_some(100ms); + } + + callback_count = 0; + reset_heap_counters(); + + for (auto _ : st) { + (void)_; + st.PauseTiming(); + for (unsigned int i = 0u; i < kNumberOfNodes; i++) { + publishers[i]->publish(empty_msgs); + } + st.ResumeTiming(); + + executor.spin_some(100ms); + } + if (callback_count == 0) { + st.SkipWithError("No message was received"); + } +} + +class PerformanceTestExecutorSimple : public PerformanceTest +{ +public: + void SetUp(benchmark::State & st) + { + rclcpp::init(0, nullptr); + node = std::make_shared("my_node"); + + PerformanceTest::SetUp(st); + } + void TearDown(benchmark::State & st) + { + PerformanceTest::TearDown(st); + node.reset(); + rclcpp::shutdown(); + } + + rclcpp::Node::SharedPtr node; +}; + + +BENCHMARK_F(PerformanceTestExecutorSimple, single_thread_executor_add_node)(benchmark::State & st) +{ + rclcpp::executors::SingleThreadedExecutor executor; + for (auto _ : st) { + (void)_; + executor.add_node(node); + st.PauseTiming(); + executor.remove_node(node); + st.ResumeTiming(); + } +} + +BENCHMARK_F( + PerformanceTestExecutorSimple, single_thread_executor_remove_node)(benchmark::State & st) +{ + rclcpp::executors::SingleThreadedExecutor executor; + for (auto _ : st) { + (void)_; + st.PauseTiming(); + executor.add_node(node); + st.ResumeTiming(); + executor.remove_node(node); + } +} + +BENCHMARK_F(PerformanceTestExecutorSimple, multi_thread_executor_add_node)(benchmark::State & st) +{ + rclcpp::executors::MultiThreadedExecutor executor; + for (auto _ : st) { + (void)_; + executor.add_node(node); + st.PauseTiming(); + executor.remove_node(node); + st.ResumeTiming(); + } +} + +BENCHMARK_F(PerformanceTestExecutorSimple, multi_thread_executor_remove_node)(benchmark::State & st) +{ + rclcpp::executors::MultiThreadedExecutor executor; + for (auto _ : st) { + (void)_; + st.PauseTiming(); + executor.add_node(node); + st.ResumeTiming(); + executor.remove_node(node); + } +} + +BENCHMARK_F( + PerformanceTestExecutorSimple, + single_thread_executor_spin_node_until_future_complete)(benchmark::State & st) +{ + rclcpp::executors::SingleThreadedExecutor executor; + // test success of an immediately finishing future + std::promise promise; + std::future future = promise.get_future(); + promise.set_value(true); + auto shared_future = future.share(); + + auto ret = rclcpp::executors::spin_node_until_future_complete( + executor, node, shared_future, 1s); + if (ret != rclcpp::FutureReturnCode::SUCCESS) { + st.SkipWithError(rcutils_get_error_string().str); + } + + reset_heap_counters(); + + for (auto _ : st) { + (void)_; + ret = rclcpp::executors::spin_node_until_future_complete( + executor, node, shared_future, 1s); + if (ret != rclcpp::FutureReturnCode::SUCCESS) { + st.SkipWithError(rcutils_get_error_string().str); + break; + } + } +} + +BENCHMARK_F( + PerformanceTestExecutorSimple, + multi_thread_executor_spin_node_until_future_complete)(benchmark::State & st) +{ + rclcpp::executors::MultiThreadedExecutor executor; + // test success of an immediately finishing future + std::promise promise; + std::future future = promise.get_future(); + promise.set_value(true); + auto shared_future = future.share(); + + auto ret = rclcpp::executors::spin_node_until_future_complete( + executor, node, shared_future, 1s); + if (ret != rclcpp::FutureReturnCode::SUCCESS) { + st.SkipWithError(rcutils_get_error_string().str); + } + + reset_heap_counters(); + + for (auto _ : st) { + (void)_; + ret = rclcpp::executors::spin_node_until_future_complete( + executor, node, shared_future, 1s); + if (ret != rclcpp::FutureReturnCode::SUCCESS) { + st.SkipWithError(rcutils_get_error_string().str); + break; + } + } +} + +BENCHMARK_F(PerformanceTestExecutorSimple, spin_until_future_complete)(benchmark::State & st) +{ + // test success of an immediately finishing future + std::promise promise; + std::future future = promise.get_future(); + promise.set_value(true); + auto shared_future = future.share(); + + auto ret = rclcpp::spin_until_future_complete(node, shared_future, 1s); + if (ret != rclcpp::FutureReturnCode::SUCCESS) { + st.SkipWithError(rcutils_get_error_string().str); + } + + reset_heap_counters(); + + for (auto _ : st) { + (void)_; + ret = rclcpp::spin_until_future_complete(node, shared_future, 1s); + if (ret != rclcpp::FutureReturnCode::SUCCESS) { + st.SkipWithError(rcutils_get_error_string().str); + break; + } + } +} diff --git a/rclcpp/test/benchmark/benchmark_init_shutdown.cpp b/rclcpp/test/benchmark/benchmark_init_shutdown.cpp new file mode 100644 index 0000000000..c2a60f693a --- /dev/null +++ b/rclcpp/test/benchmark/benchmark_init_shutdown.cpp @@ -0,0 +1,55 @@ +// 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 "performance_test_fixture/performance_test_fixture.hpp" + +#include "rclcpp/rclcpp.hpp" + +using performance_test_fixture::PerformanceTest; + +BENCHMARK_F(PerformanceTest, rclcpp_init)(benchmark::State & state) +{ + // Warmup and prime caches + rclcpp::init(0, nullptr); + rclcpp::shutdown(); + + reset_heap_counters(); + for (auto _ : state) { + (void)_; + rclcpp::init(0, nullptr); + + state.PauseTiming(); + rclcpp::shutdown(); + state.ResumeTiming(); + benchmark::ClobberMemory(); + } +} + +BENCHMARK_F(PerformanceTest, rclcpp_shutdown)(benchmark::State & state) +{ + // Warmup and prime caches + rclcpp::init(0, nullptr); + rclcpp::shutdown(); + + reset_heap_counters(); + for (auto _ : state) { + (void)_; + state.PauseTiming(); + rclcpp::init(0, nullptr); + state.ResumeTiming(); + + rclcpp::shutdown(); + benchmark::ClobberMemory(); + } +} diff --git a/rclcpp/test/benchmark/benchmark_node.cpp b/rclcpp/test/benchmark/benchmark_node.cpp new file mode 100644 index 0000000000..a5d7f4c80f --- /dev/null +++ b/rclcpp/test/benchmark/benchmark_node.cpp @@ -0,0 +1,83 @@ +// 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 "performance_test_fixture/performance_test_fixture.hpp" +#include "rclcpp/rclcpp.hpp" + +using performance_test_fixture::PerformanceTest; + +class NodePerformanceTest : public PerformanceTest +{ +public: + void SetUp(benchmark::State & state) + { + rclcpp::init(0, nullptr); + performance_test_fixture::PerformanceTest::SetUp(state); + } + + void TearDown(benchmark::State & state) + { + performance_test_fixture::PerformanceTest::TearDown(state); + rclcpp::shutdown(); + } +}; + +BENCHMARK_F(NodePerformanceTest, create_node)(benchmark::State & state) +{ + // Warmup and prime caches + auto outer_node = std::make_shared("node"); + outer_node.reset(); + + reset_heap_counters(); + for (auto _ : state) { + (void)_; + // Using pointer to separate construction and destruction in timing + auto node = std::make_shared("node"); +#ifndef __clang_analyzer__ + benchmark::DoNotOptimize(node); +#endif + benchmark::ClobberMemory(); + + // Ensure destruction of node is not counted toward timing + state.PauseTiming(); + node.reset(); + state.ResumeTiming(); + } +} + +BENCHMARK_F(NodePerformanceTest, destroy_node)(benchmark::State & state) +{ + // Warmup and prime caches + auto outer_node = std::make_shared("node"); + outer_node.reset(); + + reset_heap_counters(); + for (auto _ : state) { + (void)_; + // Using pointer to separate construction and destruction in timing + state.PauseTiming(); + auto node = std::make_shared("node"); + state.ResumeTiming(); + +#ifndef __clang_analyzer__ + benchmark::DoNotOptimize(node); +#endif + benchmark::ClobberMemory(); + + node.reset(); + } +} diff --git a/rclcpp/test/benchmark/benchmark_node_parameters_interface.cpp b/rclcpp/test/benchmark/benchmark_node_parameters_interface.cpp new file mode 100644 index 0000000000..04c7071d64 --- /dev/null +++ b/rclcpp/test/benchmark/benchmark_node_parameters_interface.cpp @@ -0,0 +1,278 @@ +// 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 "performance_test_fixture/performance_test_fixture.hpp" + +#include "rclcpp/rclcpp.hpp" + +class NodeParametersInterfaceTest : public performance_test_fixture::PerformanceTest +{ +public: + NodeParametersInterfaceTest() + : node_name("my_node"), + param_prefix("my_prefix"), + param1_name(param_prefix + ".my_param_1"), + param2_name(param_prefix + ".my_param_2"), + param3_name(param_prefix + ".my_param_3") + { + dynamically_typed_descriptor.dynamic_typing = true; + } + + void SetUp(benchmark::State & state) + { + rclcpp::init(0, nullptr); + node = std::make_shared(node_name); + + node->declare_parameter( + param1_name, rclcpp::ParameterValue{}, dynamically_typed_descriptor); + node->declare_parameter( + param2_name, rclcpp::ParameterValue{}, dynamically_typed_descriptor); + node->declare_parameter( + param3_name, rclcpp::ParameterValue{}, dynamically_typed_descriptor); + node->undeclare_parameter(param3_name); + + performance_test_fixture::PerformanceTest::SetUp(state); + } + + void TearDown(benchmark::State & state) + { + performance_test_fixture::PerformanceTest::TearDown(state); + + node.reset(); + rclcpp::shutdown(); + } + + const std::string node_name; + const std::string param_prefix; + const std::string param1_name; + const std::string param2_name; + const std::string param3_name; + rcl_interfaces::msg::ParameterDescriptor dynamically_typed_descriptor; + +protected: + rclcpp::Node::SharedPtr node; +}; + +BENCHMARK_F(NodeParametersInterfaceTest, declare_undeclare)(benchmark::State & state) +{ + for (auto _ : state) { + (void)_; + node->declare_parameter(param3_name, rclcpp::ParameterValue{}, dynamically_typed_descriptor); + node->undeclare_parameter(param3_name); + } +} + +BENCHMARK_F(NodeParametersInterfaceTest, has_parameter_hit)(benchmark::State & state) +{ + for (auto _ : state) { + (void)_; + if (!node->has_parameter(param1_name)) { + state.SkipWithError("Parameter was expected"); + break; + } + } +} + +BENCHMARK_F(NodeParametersInterfaceTest, has_parameter_miss)(benchmark::State & state) +{ + for (auto _ : state) { + (void)_; + if (node->has_parameter(param3_name)) { + state.SkipWithError("Parameter was not expected"); + break; + } + } +} + +BENCHMARK_F(NodeParametersInterfaceTest, set_parameters_bool)(benchmark::State & state) +{ + const std::vector param_values1 + { + rclcpp::Parameter(param1_name, true), + rclcpp::Parameter(param2_name, false), + }; + const std::vector param_values2 + { + rclcpp::Parameter(param1_name, false), + rclcpp::Parameter(param2_name, true), + }; + + reset_heap_counters(); + + for (auto _ : state) { + (void)_; + node->set_parameters(param_values2); + node->set_parameters(param_values1); + } +} + +BENCHMARK_F(NodeParametersInterfaceTest, set_parameters_atomically_bool)(benchmark::State & state) +{ + const std::vector param_values1 + { + rclcpp::Parameter(param1_name, true), + rclcpp::Parameter(param2_name, false), + }; + const std::vector param_values2 + { + rclcpp::Parameter(param1_name, false), + rclcpp::Parameter(param2_name, true), + }; + + reset_heap_counters(); + + for (auto _ : state) { + (void)_; + node->set_parameters_atomically(param_values2); + node->set_parameters_atomically(param_values1); + } +} + +BENCHMARK_F(NodeParametersInterfaceTest, set_parameters_callback_bool)(benchmark::State & state) +{ + const std::vector param_values1 + { + rclcpp::Parameter(param1_name, true), + rclcpp::Parameter(param2_name, false), + }; + const std::vector param_values2 + { + rclcpp::Parameter(param1_name, false), + rclcpp::Parameter(param2_name, true), + }; + + rcl_interfaces::msg::SetParametersResult callback_result; + bool callback_received = false; + callback_result.successful = true; + auto callback = + [&callback_result, &callback_received](const std::vector &) { + callback_received = true; + return callback_result; + }; + auto handle = node->add_on_set_parameters_callback(callback); + + reset_heap_counters(); + + for (auto _ : state) { + (void)_; + node->set_parameters(param_values2); + node->set_parameters(param_values1); + } + + if (!callback_received) { + state.SkipWithError("Callback is not functioning"); + } + + node->remove_on_set_parameters_callback(handle.get()); +} + +BENCHMARK_F(NodeParametersInterfaceTest, set_parameters_string)(benchmark::State & state) +{ + const std::vector param_values1 + { + rclcpp::Parameter(param1_name, "param 1 value A"), + rclcpp::Parameter(param2_name, "param 2 value B"), + }; + const std::vector param_values2 + { + rclcpp::Parameter(param1_name, "param 1 value B"), + rclcpp::Parameter(param2_name, "param 2 value A"), + }; + + reset_heap_counters(); + + for (auto _ : state) { + (void)_; + node->set_parameters(param_values2); + node->set_parameters(param_values1); + } +} + +BENCHMARK_F(NodeParametersInterfaceTest, set_parameters_array)(benchmark::State & state) +{ + const std::vector param_values1 + { + rclcpp::Parameter(param1_name, std::vector {0, 1, 2}), + rclcpp::Parameter(param2_name, std::vector {3, 4, 5}), + }; + const std::vector param_values2 + { + rclcpp::Parameter(param1_name, std::vector {4, 5, 6}), + rclcpp::Parameter(param2_name, std::vector {7, 8, 9}), + }; + + reset_heap_counters(); + + for (auto _ : state) { + (void)_; + node->set_parameters(param_values2); + node->set_parameters(param_values1); + } +} + +BENCHMARK_F(NodeParametersInterfaceTest, get_parameter)(benchmark::State & state) +{ + rclcpp::Parameter param1_value; + + reset_heap_counters(); + + for (auto _ : state) { + (void)_; + node->get_parameter(param1_name, param1_value); + } +} + +BENCHMARK_F(NodeParametersInterfaceTest, list_parameters_hit)(benchmark::State & state) +{ + rcl_interfaces::msg::ListParametersResult param_list; + const std::vector prefixes + { + param_prefix, + }; + + reset_heap_counters(); + + for (auto _ : state) { + (void)_; + param_list = node->list_parameters(prefixes, 10); + if (param_list.names.size() != 2) { + state.SkipWithError("Expected node names"); + break; + } + } +} + +BENCHMARK_F(NodeParametersInterfaceTest, list_parameters_miss)(benchmark::State & state) +{ + rcl_interfaces::msg::ListParametersResult param_list; + const std::vector prefixes + { + "your_param", + }; + + reset_heap_counters(); + + for (auto _ : state) { + (void)_; + param_list = node->list_parameters(prefixes, 10); + if (param_list.names.size() != 0) { + state.SkipWithError("Expected no node names"); + break; + } + } +} diff --git a/rclcpp/test/benchmark/benchmark_parameter_client.cpp b/rclcpp/test/benchmark/benchmark_parameter_client.cpp new file mode 100644 index 0000000000..0d9a6fd9c9 --- /dev/null +++ b/rclcpp/test/benchmark/benchmark_parameter_client.cpp @@ -0,0 +1,337 @@ +// 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 "benchmark/benchmark.h" + +#include "rclcpp/rclcpp.hpp" + +class RemoteNodeTest : public benchmark::Fixture +{ +public: + RemoteNodeTest() + : remote_node_name("my_remote_node") + { + } + +#ifdef __GNUC__ +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Woverloaded-virtual" +#endif + void SetUp(benchmark::State &) + { + remote_context = std::make_shared(); + remote_context->init(0, nullptr, rclcpp::InitOptions().auto_initialize_logging(false)); + + rclcpp::ExecutorOptions exec_options; + exec_options.context = remote_context; + + remote_executor = std::make_shared(exec_options); + + remote_node = std::make_shared( + remote_node_name, rclcpp::NodeOptions().context(remote_context)); + remote_executor->add_node(remote_node); + + remote_thread = std::thread(&rclcpp::executors::SingleThreadedExecutor::spin, remote_executor); + } + + void TearDown(benchmark::State &) + { + remote_executor->cancel(); + remote_context->shutdown("Test is complete"); + remote_thread.join(); + + remote_node.reset(); + remote_executor.reset(); + remote_context.reset(); + } +#ifdef __GNUC__ +#pragma GCC diagnostic pop +#endif + + const std::string remote_node_name; + +protected: + rclcpp::Context::SharedPtr remote_context; + rclcpp::executors::SingleThreadedExecutor::SharedPtr remote_executor; + rclcpp::Node::SharedPtr remote_node; + std::thread remote_thread; +}; + +class ParameterClientTest : public RemoteNodeTest +{ +public: + ParameterClientTest() + : node_name("my_node"), + param_prefix("my_prefix"), + param1_name(param_prefix + ".my_param_1"), + param2_name(param_prefix + ".my_param_2"), + param3_name(param_prefix + ".my_param_3") + { + } + + void SetUp(benchmark::State & state) + { + RemoteNodeTest::SetUp(state); + + rclcpp::init(0, nullptr); + node = std::make_shared(node_name); + + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.dynamic_typing = true; + + remote_node->declare_parameter( + param1_name, rclcpp::ParameterValue("param1_value"), descriptor); + remote_node->declare_parameter( + param2_name, rclcpp::ParameterValue(std::vector {1, 2, 3}), descriptor); + remote_node->declare_parameter(param3_name, rclcpp::ParameterValue{}, descriptor); + remote_node->undeclare_parameter(param3_name); + + params_client = std::make_shared(node, remote_node_name); + + if (!params_client->wait_for_service()) { + state.SkipWithError("Client failed to become ready"); + } + } + + void TearDown(benchmark::State & state) + { + RemoteNodeTest::TearDown(state); + + rclcpp::shutdown(); + node.reset(); + params_client.reset(); + } + + const std::string node_name; + const std::string param_prefix; + const std::string param1_name; + const std::string param2_name; + const std::string param3_name; + +protected: + rclcpp::Node::SharedPtr node; + rclcpp::SyncParametersClient::SharedPtr params_client; +}; + +static bool result_is_successful(rcl_interfaces::msg::SetParametersResult result) +{ + return result.successful; +} + +BENCHMARK_F(ParameterClientTest, create_destroy_client)(benchmark::State & state) +{ + for (auto _ : state) { + (void)_; + params_client.reset(); + params_client = std::make_shared(node, remote_node_name); + if (!params_client->wait_for_service()) { + state.SkipWithError("Client failed to become ready"); + break; + } + } +} + +BENCHMARK_F(ParameterClientTest, has_parameter_hit)(benchmark::State & state) +{ + for (auto _ : state) { + (void)_; + if (!params_client->has_parameter(param1_name)) { + state.SkipWithError("Parameter was expected"); + break; + } + } +} + +BENCHMARK_F(ParameterClientTest, has_parameter_miss)(benchmark::State & state) +{ + for (auto _ : state) { + (void)_; + if (params_client->has_parameter(param3_name)) { + state.SkipWithError("Parameter was not expected"); + break; + } + } +} + +BENCHMARK_F(ParameterClientTest, set_parameters_bool)(benchmark::State & state) +{ + const std::vector param_values1 + { + rclcpp::Parameter(param1_name, true), + rclcpp::Parameter(param2_name, false), + }; + const std::vector param_values2 + { + rclcpp::Parameter(param1_name, false), + rclcpp::Parameter(param2_name, true), + }; + + for (auto _ : state) { + (void)_; + std::vector results = + params_client->set_parameters(param_values2); + if (!std::all_of(results.begin(), results.end(), result_is_successful)) { + state.SkipWithError("Failed to set one or more parameters"); + break; + } + + results = params_client->set_parameters(param_values1); + if (!std::all_of(results.begin(), results.end(), result_is_successful)) { + state.SkipWithError("Failed to set one or more parameters"); + break; + } + } +} + +BENCHMARK_F(ParameterClientTest, set_parameters_atomically_bool)(benchmark::State & state) +{ + const std::vector param_values1 + { + rclcpp::Parameter(param1_name, true), + rclcpp::Parameter(param2_name, false), + }; + const std::vector param_values2 + { + rclcpp::Parameter(param1_name, false), + rclcpp::Parameter(param2_name, true), + }; + + for (auto _ : state) { + (void)_; + rcl_interfaces::msg::SetParametersResult result = + params_client->set_parameters_atomically(param_values2); + if (!result.successful) { + state.SkipWithError(("Failed to set parameters: " + result.reason).c_str()); + break; + } + + result = params_client->set_parameters_atomically(param_values1); + if (!result.successful) { + state.SkipWithError(("Failed to set parameters: " + result.reason).c_str()); + break; + } + } +} + +BENCHMARK_F(ParameterClientTest, set_parameters_string)(benchmark::State & state) +{ + const std::vector param_values1 + { + rclcpp::Parameter(param1_name, "param 1 value A"), + rclcpp::Parameter(param2_name, "param 2 value B"), + }; + const std::vector param_values2 + { + rclcpp::Parameter(param1_name, "param 1 value B"), + rclcpp::Parameter(param2_name, "param 2 value A"), + }; + + for (auto _ : state) { + (void)_; + std::vector results = + params_client->set_parameters(param_values2); + if (!std::all_of(results.begin(), results.end(), result_is_successful)) { + state.SkipWithError("Failed to set one or more parameters"); + break; + } + + results = params_client->set_parameters(param_values1); + if (!std::all_of(results.begin(), results.end(), result_is_successful)) { + state.SkipWithError("Failed to set one or more parameters"); + break; + } + } +} + +BENCHMARK_F(ParameterClientTest, set_parameters_array)(benchmark::State & state) +{ + const std::vector param_values1 + { + rclcpp::Parameter(param1_name, std::vector {0, 1, 2}), + rclcpp::Parameter(param2_name, std::vector {3, 4, 5}), + }; + const std::vector param_values2 + { + rclcpp::Parameter(param1_name, std::vector {4, 5, 6}), + rclcpp::Parameter(param2_name, std::vector {7, 8, 9}), + }; + + for (auto _ : state) { + (void)_; + std::vector results = + params_client->set_parameters(param_values2); + if (!std::all_of(results.begin(), results.end(), result_is_successful)) { + state.SkipWithError("Failed to set one or more parameters"); + break; + } + + results = params_client->set_parameters(param_values1); + if (!std::all_of(results.begin(), results.end(), result_is_successful)) { + state.SkipWithError("Failed to set one or more parameters"); + break; + } + } +} + +BENCHMARK_F(ParameterClientTest, get_parameters)(benchmark::State & state) +{ + for (auto _ : state) { + (void)_; + std::vector results = params_client->get_parameters({param1_name}); + if (results.size() != 1 || results[0].get_name() != param1_name) { + state.SkipWithError("Got the wrong parameter(s)"); + break; + } + } +} + +BENCHMARK_F(ParameterClientTest, list_parameters_hit)(benchmark::State & state) +{ + const std::vector prefixes + { + param_prefix, + }; + + for (auto _ : state) { + (void)_; + rcl_interfaces::msg::ListParametersResult param_list = + params_client->list_parameters(prefixes, 10); + if (param_list.names.size() != 2) { + state.SkipWithError("Expected parameters"); + break; + } + } +} + +BENCHMARK_F(ParameterClientTest, list_parameters_miss)(benchmark::State & state) +{ + const std::vector prefixes + { + "your_prefix", + }; + + for (auto _ : state) { + (void)_; + rcl_interfaces::msg::ListParametersResult param_list = + params_client->list_parameters(prefixes, 10); + if (param_list.names.size() != 0) { + state.SkipWithError("Expected no parameters"); + break; + } + } +} diff --git a/rclcpp/test/benchmark/benchmark_service.cpp b/rclcpp/test/benchmark/benchmark_service.cpp new file mode 100644 index 0000000000..a42723da90 --- /dev/null +++ b/rclcpp/test/benchmark/benchmark_service.cpp @@ -0,0 +1,145 @@ +// 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 "performance_test_fixture/performance_test_fixture.hpp" +#include "rclcpp/rclcpp.hpp" +#include "test_msgs/srv/empty.hpp" + +using performance_test_fixture::PerformanceTest; + +constexpr char empty_service_name[] = "empty_service"; + +class ServicePerformanceTest : public PerformanceTest +{ +public: + ServicePerformanceTest() + : callback_count(0) {} + + void SetUp(benchmark::State & state) + { + rclcpp::init(0, nullptr); + node = std::make_unique("node", "ns"); + empty_client = node->create_client(empty_service_name); + callback_count = 0; + + performance_test_fixture::PerformanceTest::SetUp(state); + } + + void TearDown(benchmark::State & state) + { + performance_test_fixture::PerformanceTest::TearDown(state); + + empty_client.reset(); + node.reset(); + rclcpp::shutdown(); + } + + void ServiceCallback( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) + { + callback_count++; + } + +protected: + std::unique_ptr node; + std::shared_ptr> empty_client; + int callback_count; +}; + +BENCHMARK_F(ServicePerformanceTest, construct_service_no_client)(benchmark::State & state) { + auto callback = std::bind( + &ServicePerformanceTest::ServiceCallback, + this, std::placeholders::_1, std::placeholders::_2); + + auto outer_service = node->create_service("not_a_service", callback); + + reset_heap_counters(); + for (auto _ : state) { + (void)_; + auto service = node->create_service("not_a_service", callback); + benchmark::DoNotOptimize(service); + benchmark::ClobberMemory(); + + state.PauseTiming(); + service.reset(); + state.ResumeTiming(); + } +} + +BENCHMARK_F(ServicePerformanceTest, construct_service_empty_srv)(benchmark::State & state) { + auto callback = std::bind( + &ServicePerformanceTest::ServiceCallback, + this, std::placeholders::_1, std::placeholders::_2); + auto outer_service = node->create_service(empty_service_name, callback); + + reset_heap_counters(); + for (auto _ : state) { + (void)_; + auto service = node->create_service(empty_service_name, callback); + benchmark::DoNotOptimize(service); + benchmark::ClobberMemory(); + + state.PauseTiming(); + service.reset(); + state.ResumeTiming(); + } +} + +BENCHMARK_F(ServicePerformanceTest, destroy_service_empty_srv)(benchmark::State & state) { + auto callback = std::bind( + &ServicePerformanceTest::ServiceCallback, + this, std::placeholders::_1, std::placeholders::_2); + auto outer_service = node->create_service(empty_service_name, callback); + + reset_heap_counters(); + for (auto _ : state) { + (void)_; + state.PauseTiming(); + auto service = node->create_service(empty_service_name, callback); + state.ResumeTiming(); + benchmark::DoNotOptimize(service); + benchmark::ClobberMemory(); + + service.reset(); + } +} + +BENCHMARK_F(ServicePerformanceTest, async_send_response)(benchmark::State & state) { + auto callback = std::bind( + &ServicePerformanceTest::ServiceCallback, + this, std::placeholders::_1, std::placeholders::_2); + auto service = node->create_service(empty_service_name, callback); + + reset_heap_counters(); + for (auto _ : state) { + (void)_; + state.PauseTiming(); + // Clear executor queue + rclcpp::spin_some(node->get_node_base_interface()); + + auto request = std::make_shared(); + auto future = empty_client->async_send_request(request); + state.ResumeTiming(); + benchmark::DoNotOptimize(service); + benchmark::ClobberMemory(); + + rclcpp::spin_until_future_complete(node->get_node_base_interface(), future); + } + if (callback_count == 0) { + state.SkipWithError("Service callback was not called"); + } +} diff --git a/rclcpp/test/cmake/rclcpp_add_build_failure_test.cmake b/rclcpp/test/cmake/rclcpp_add_build_failure_test.cmake new file mode 100644 index 0000000000..5ccde6eba8 --- /dev/null +++ b/rclcpp/test/cmake/rclcpp_add_build_failure_test.cmake @@ -0,0 +1,56 @@ +# Copyright 2019 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# +# Register a test which tries to compile a file and expects it to fail to build. +# +# This will create two targets, one by the given target name and a test target +# which has the same name prefixed with `test_`. +# For example, if target is `should_not_compile__use_const_argument` then there +# will be an executable target called `should_not_compile__use_const_argument` +# and a test target called `test_should_not_compile__use_const_argument`. +# +# :param target: the name of the target to be created +# :type target: string +# :param ARGN: the list of source files to be used to create the test executable +# :type ARGN: list of strings +# +macro(rclcpp_add_build_failure_test target) + if(${ARGC} EQUAL 0) + message( + FATAL_ERROR + "rclcpp_add_build_failure_test() requires a target name and " + "at least one source file") + endif() + + add_executable(${target} ${ARGN}) + set_target_properties(${target} + PROPERTIES + EXCLUDE_FROM_ALL TRUE + EXCLUDE_FROM_DEFAULT_BUILD TRUE) + + add_test( + NAME test_${target} + COMMAND + ${CMAKE_COMMAND} + --build . + --target ${target} + --config $ + WORKING_DIRECTORY ${CMAKE_BINARY_DIR}) + set_tests_properties(test_${target} + PROPERTIES + WILL_FAIL TRUE + LABELS "build_failure" + ) +endmacro() diff --git a/rclcpp/test/mock_msgs/srv/Mock.srv b/rclcpp/test/mock_msgs/srv/Mock.srv deleted file mode 100644 index 06a8369050..0000000000 --- a/rclcpp/test/mock_msgs/srv/Mock.srv +++ /dev/null @@ -1,3 +0,0 @@ -bool request ---- -bool response diff --git a/rclcpp/test/mocking_utils/patch.hpp b/rclcpp/test/mocking_utils/patch.hpp new file mode 100644 index 0000000000..8f23d543b9 --- /dev/null +++ b/rclcpp/test/mocking_utils/patch.hpp @@ -0,0 +1,578 @@ +// 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); +}; + +/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6) +/// 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, ArgT6); +}; + +/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7) +/// 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, ArgT6, ArgT7); +}; + +/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7, ArgT8) +/// 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, ArgT6, ArgT7, ArgT8); +}; + +/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7, +/// ArgT8, ArgT9) 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, ArgT6, ArgT7, ArgT8, ArgT9); +}; + +/// 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/rclcpp/test/msg/Header.msg b/rclcpp/test/msg/Header.msg new file mode 100644 index 0000000000..bfe5dc5366 --- /dev/null +++ b/rclcpp/test/msg/Header.msg @@ -0,0 +1,3 @@ +# Simple Header message with a timestamp field. + +builtin_interfaces/Time stamp diff --git a/rclcpp/test/msg/LargeMessage.msg b/rclcpp/test/msg/LargeMessage.msg new file mode 100644 index 0000000000..1e383c0bae --- /dev/null +++ b/rclcpp/test/msg/LargeMessage.msg @@ -0,0 +1,3 @@ +# A message with a size larger than the default Linux stack size +uint8[10485760] data +uint64 size diff --git a/rclcpp/test/msg/MessageWithHeader.msg b/rclcpp/test/msg/MessageWithHeader.msg new file mode 100644 index 0000000000..3bd1b45534 --- /dev/null +++ b/rclcpp/test/msg/MessageWithHeader.msg @@ -0,0 +1,3 @@ +# Message containing a simple Header field. + +Header header diff --git a/rclcpp/test/msg/String.msg b/rclcpp/test/msg/String.msg new file mode 100644 index 0000000000..44e5aaf86b --- /dev/null +++ b/rclcpp/test/msg/String.msg @@ -0,0 +1 @@ +string data \ No newline at end of file diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt new file mode 100644 index 0000000000..35fa8fdd32 --- /dev/null +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -0,0 +1,696 @@ +find_package(ament_cmake_gtest REQUIRED) + +find_package(rmw_implementation_cmake REQUIRED) + +add_definitions(-DTEST_RESOURCES_DIRECTORY="${TEST_RESOURCES_DIRECTORY}") + +rosidl_generate_interfaces(${PROJECT_NAME}_test_msgs + ../msg/Header.msg + ../msg/LargeMessage.msg + ../msg/MessageWithHeader.msg + ../msg/String.msg + DEPENDENCIES builtin_interfaces + LIBRARY_NAME ${PROJECT_NAME} + SKIP_INSTALL +) +# Need the target name to depend on generated interface libraries +rosidl_get_typesupport_target(cpp_typesupport_target "${PROJECT_NAME}_test_msgs" "rosidl_typesupport_cpp") + +ament_add_gtest( + test_allocator_common + allocator/test_allocator_common.cpp) +if(TARGET test_allocator_common) + target_link_libraries(test_allocator_common ${PROJECT_NAME}) +endif() +ament_add_gtest( + test_allocator_deleter + allocator/test_allocator_deleter.cpp) +if(TARGET test_allocator_deleter) + target_link_libraries(test_allocator_deleter ${PROJECT_NAME}) +endif() +ament_add_gtest( + test_exceptions + exceptions/test_exceptions.cpp) +ament_add_test_label(test_exceptions mimick) +if(TARGET test_exceptions) + target_link_libraries(test_exceptions ${PROJECT_NAME} mimick) +endif() + +ament_add_gtest(test_allocator_memory_strategy strategies/test_allocator_memory_strategy.cpp) +if(TARGET test_allocator_memory_strategy) + target_link_libraries(test_allocator_memory_strategy ${PROJECT_NAME} rcpputils::rcpputils ${test_msgs_TARGETS}) +endif() +ament_add_gtest(test_message_pool_memory_strategy strategies/test_message_pool_memory_strategy.cpp) +if(TARGET test_message_pool_memory_strategy) + target_link_libraries(test_message_pool_memory_strategy ${PROJECT_NAME} ${test_msgs_TARGETS}) +endif() +ament_add_gtest(test_any_service_callback test_any_service_callback.cpp) +if(TARGET test_any_service_callback) + target_link_libraries(test_any_service_callback ${PROJECT_NAME} ${test_msgs_TARGETS}) +endif() +ament_add_gtest(test_any_subscription_callback test_any_subscription_callback.cpp) +if(TARGET test_any_subscription_callback) + target_link_libraries(test_any_subscription_callback ${PROJECT_NAME} ${test_msgs_TARGETS}) +endif() +ament_add_gtest(test_client test_client.cpp) +ament_add_test_label(test_client mimick) +if(TARGET test_client) + target_link_libraries(test_client ${PROJECT_NAME} mimick ${rcl_interfaces_TARGETS} ${test_msgs_TARGETS}) +endif() +ament_add_gtest(test_clock test_clock.cpp) +ament_add_test_label(test_clock mimick) +if(TARGET test_clock) + target_link_libraries(test_clock ${PROJECT_NAME} mimick ${rcl_interfaces_TARGETS} ${test_msgs_TARGETS}) +endif() +ament_add_gtest(test_copy_all_parameter_values test_copy_all_parameter_values.cpp) +if(TARGET test_copy_all_parameter_values) + target_link_libraries(test_copy_all_parameter_values ${PROJECT_NAME}) +endif() +ament_add_gtest(test_create_timer test_create_timer.cpp) +if(TARGET test_create_timer) + target_link_libraries(test_create_timer ${PROJECT_NAME}) + target_include_directories(test_create_timer PRIVATE ./) +endif() +ament_add_gtest(test_generic_client test_generic_client.cpp) +ament_add_test_label(test_generic_client mimick) +if(TARGET test_generic_client) + target_link_libraries(test_generic_client ${PROJECT_NAME} + mimick + ${rcl_interfaces_TARGETS} + rmw::rmw + rosidl_runtime_cpp::rosidl_runtime_cpp + rosidl_typesupport_cpp::rosidl_typesupport_cpp + ${test_msgs_TARGETS} + ) +endif() +ament_add_gtest(test_generic_service test_generic_service.cpp) +ament_add_test_label(test_generic_service mimick) +if(TARGET test_generic_service) + target_link_libraries(test_generic_service ${PROJECT_NAME} + mimick + ${rcl_interfaces_TARGETS} + rmw::rmw + rosidl_runtime_cpp::rosidl_runtime_cpp + rosidl_typesupport_cpp::rosidl_typesupport_cpp + ${test_msgs_TARGETS} + ) +endif() +ament_add_gtest(test_client_common test_client_common.cpp) +ament_add_test_label(test_client_common mimick) +if(TARGET test_client_common) + target_link_libraries(test_client_common ${PROJECT_NAME} + mimick + ${rcl_interfaces_TARGETS} + rmw::rmw + rosidl_runtime_cpp::rosidl_runtime_cpp + rosidl_typesupport_cpp::rosidl_typesupport_cpp + ${test_msgs_TARGETS} + ) +endif() +ament_add_gtest(test_create_subscription test_create_subscription.cpp) +if(TARGET test_create_subscription) + target_link_libraries(test_create_subscription ${PROJECT_NAME} ${test_msgs_TARGETS}) +endif() +ament_add_gtest(test_expand_topic_or_service_name test_expand_topic_or_service_name.cpp) +ament_add_test_label(test_expand_topic_or_service_name mimick) +if(TARGET test_expand_topic_or_service_name) + target_link_libraries(test_expand_topic_or_service_name ${PROJECT_NAME} mimick rcl::rcl rmw::rmw) +endif() +ament_add_gtest(test_function_traits test_function_traits.cpp) +if(TARGET test_function_traits) + target_link_libraries(test_function_traits ${PROJECT_NAME}) +endif() +ament_add_gtest( + test_future_return_code + test_future_return_code.cpp) +if(TARGET test_future_return_code) + target_link_libraries(test_future_return_code ${PROJECT_NAME}) +endif() +ament_add_gmock(test_intra_process_manager test_intra_process_manager.cpp) +if(TARGET test_intra_process_manager) + target_link_libraries(test_intra_process_manager ${PROJECT_NAME} ${rcl_interfaces_TARGETS} rmw::rmw) +endif() +ament_add_gmock(test_intra_process_manager_with_allocators test_intra_process_manager_with_allocators.cpp) +if(TARGET test_intra_process_manager_with_allocators) + target_link_libraries(test_intra_process_manager_with_allocators ${PROJECT_NAME} ${test_msgs_TARGETS}) +endif() +ament_add_gtest(test_ring_buffer_implementation test_ring_buffer_implementation.cpp) +if(TARGET test_ring_buffer_implementation) + target_link_libraries(test_ring_buffer_implementation ${PROJECT_NAME}) +endif() +ament_add_gtest(test_intra_process_buffer test_intra_process_buffer.cpp) +if(TARGET test_intra_process_buffer) + target_link_libraries(test_intra_process_buffer ${PROJECT_NAME}) +endif() + +ament_add_gtest(test_loaned_message test_loaned_message.cpp) +ament_add_test_label(test_loaned_message mimick) +target_link_libraries(test_loaned_message ${PROJECT_NAME} mimick ${test_msgs_TARGETS}) + +ament_add_gtest(test_memory_strategy test_memory_strategy.cpp) +target_link_libraries(test_memory_strategy ${PROJECT_NAME} ${test_msgs_TARGETS}) + +ament_add_gtest(test_message_memory_strategy test_message_memory_strategy.cpp) +target_link_libraries(test_message_memory_strategy ${PROJECT_NAME} ${test_msgs_TARGETS}) + +ament_add_gtest(test_node test_node.cpp TIMEOUT 240) +ament_add_test_label(test_node mimick) +if(TARGET test_node) + target_link_libraries(test_node ${PROJECT_NAME} mimick rcpputils::rcpputils rmw::rmw ${test_msgs_TARGETS}) +endif() + +ament_add_gtest(test_node_interfaces__get_node_interfaces + node_interfaces/test_get_node_interfaces.cpp) +if(TARGET test_node_interfaces__get_node_interfaces) + target_link_libraries(test_node_interfaces__get_node_interfaces ${PROJECT_NAME}) +endif() +ament_add_gtest(test_node_interfaces__node_base + node_interfaces/test_node_base.cpp) +ament_add_test_label(test_node_interfaces__node_base mimick) +if(TARGET test_node_interfaces__node_base) + target_link_libraries(test_node_interfaces__node_base ${PROJECT_NAME} mimick rcl::rcl rmw::rmw) +endif() +ament_add_gtest(test_node_interfaces__node_clock + node_interfaces/test_node_clock.cpp) +if(TARGET test_node_interfaces__node_clock) + target_link_libraries(test_node_interfaces__node_clock ${PROJECT_NAME}) +endif() +ament_add_gtest(test_node_interfaces__node_graph + node_interfaces/test_node_graph.cpp + TIMEOUT 120) +ament_add_test_label(test_node_interfaces__node_graph mimick) +if(TARGET test_node_interfaces__node_graph) + target_link_libraries(test_node_interfaces__node_graph ${PROJECT_NAME} mimick rcl::rcl ${test_msgs_TARGETS}) +endif() +ament_add_gtest(test_node_interfaces__node_interfaces + node_interfaces/test_node_interfaces.cpp) +if(TARGET test_node_interfaces__node_interfaces) + target_link_libraries(test_node_interfaces__node_interfaces ${PROJECT_NAME}) +endif() +ament_add_gtest(test_node_interfaces__node_parameters + node_interfaces/test_node_parameters.cpp) +ament_add_test_label(test_node_interfaces__node_parameters mimick) +if(TARGET test_node_interfaces__node_parameters) + target_link_libraries(test_node_interfaces__node_parameters ${PROJECT_NAME} mimick rcpputils::rcpputils) +endif() +ament_add_gtest(test_node_interfaces__node_services + node_interfaces/test_node_services.cpp) +ament_add_test_label(test_node_interfaces__node_services mimick) +if(TARGET test_node_interfaces__node_services) + target_link_libraries(test_node_interfaces__node_services ${PROJECT_NAME} mimick rcl::rcl) +endif() +ament_add_gtest(test_node_interfaces__node_timers + node_interfaces/test_node_timers.cpp) +ament_add_test_label(test_node_interfaces__node_timers mimick) +if(TARGET test_node_interfaces__node_timers) + target_link_libraries(test_node_interfaces__node_timers ${PROJECT_NAME} mimick rcl::rcl) +endif() +ament_add_gtest(test_node_interfaces__node_topics + node_interfaces/test_node_topics.cpp) +ament_add_test_label(test_node_interfaces__node_topics mimick) +if(TARGET test_node_interfaces__node_topics) + target_link_libraries(test_node_interfaces__node_topics ${PROJECT_NAME} mimick rcl::rcl ${test_msgs_TARGETS}) +endif() +ament_add_gtest(test_node_interfaces__node_type_descriptions + node_interfaces/test_node_type_descriptions.cpp) +if(TARGET test_node_interfaces__node_type_descriptions) + target_link_libraries(test_node_interfaces__node_type_descriptions ${PROJECT_NAME}) +endif() +ament_add_gtest(test_node_interfaces__node_waitables + node_interfaces/test_node_waitables.cpp) +ament_add_test_label(test_node_interfaces__node_waitables mimick) +if(TARGET test_node_interfaces__node_waitables) + target_link_libraries(test_node_interfaces__node_waitables ${PROJECT_NAME} mimick rcl::rcl) +endif() +ament_add_gtest(test_node_interfaces__test_template_utils # Compile time test + node_interfaces/detail/test_template_utils.cpp) +if(TARGET test_node_interfaces__test_template_utils) + target_link_libraries(test_node_interfaces__test_template_utils ${PROJECT_NAME}) +endif() + +# TODO(wjwwood): reenable these build failure tests when I can get Jenkins to ignore their output +# rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ref_rclcpp_node +# node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_rclcpp_node.cpp) +# target_link_libraries(build_failure__get_node_topics_interface_const_ref_rclcpp_node +# ${PROJECT_NAME}) + +# rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ptr_rclcpp_node +# node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_rclcpp_node.cpp) +# target_link_libraries(build_failure__get_node_topics_interface_const_ptr_rclcpp_node +# ${PROJECT_NAME}) + +# rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ref_wrapped_node +# node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_wrapped_node.cpp) +# target_link_libraries(build_failure__get_node_topics_interface_const_ref_rclcpp_node +# ${PROJECT_NAME}) + +# rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ptr_wrapped_node +# node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_wrapped_node.cpp) +# target_link_libraries(build_failure__get_node_topics_interface_const_ptr_rclcpp_node +# ${PROJECT_NAME}) + +ament_add_gtest(test_node_global_args test_node_global_args.cpp) +if(TARGET test_node_global_args) + target_link_libraries(test_node_global_args ${PROJECT_NAME}) +endif() +ament_add_gtest(test_node_options test_node_options.cpp) +ament_add_test_label(test_node_options mimick) +if(TARGET test_node_options) + target_link_libraries(test_node_options ${PROJECT_NAME} mimick rcl::rcl) +endif() +ament_add_gtest(test_init_options test_init_options.cpp) +ament_add_test_label(test_init_options mimick) +if(TARGET test_init_options) + target_link_libraries(test_init_options ${PROJECT_NAME} mimick rcl::rcl) +endif() +ament_add_gmock(test_parameter_client test_parameter_client.cpp) +if(TARGET test_parameter_client) + target_link_libraries(test_parameter_client ${PROJECT_NAME} ${rcl_interfaces_TARGETS}) +endif() +ament_add_gtest(test_parameter_service test_parameter_service.cpp) +if(TARGET test_parameter_service) + target_link_libraries(test_parameter_service ${PROJECT_NAME}) +endif() +ament_add_gtest(test_parameter_events_filter test_parameter_events_filter.cpp) +if(TARGET test_parameter_events_filter) + target_link_libraries(test_parameter_events_filter ${PROJECT_NAME} ${rcl_interfaces_TARGETS}) +endif() +ament_add_gtest(test_parameter test_parameter.cpp) +if(TARGET test_parameter) + target_link_libraries(test_parameter ${PROJECT_NAME}) +endif() +ament_add_gtest(test_parameter_event_handler test_parameter_event_handler.cpp) +if(TARGET test_parameter_event_handler) + target_link_libraries(test_parameter_event_handler ${PROJECT_NAME}) +endif() +ament_add_gtest(test_parameter_map test_parameter_map.cpp) +if(TARGET test_parameter_map) + target_link_libraries(test_parameter_map ${PROJECT_NAME} rcl::rcl rcl_yaml_param_parser::rcl_yaml_param_parser rcutils::rcutils) +endif() +ament_add_gtest(test_publisher test_publisher.cpp TIMEOUT 120) +ament_add_test_label(test_publisher mimick) +if(TARGET test_publisher) + target_link_libraries(test_publisher ${PROJECT_NAME} mimick rcl::rcl rcutils::rcutils ${test_msgs_TARGETS}) +endif() + +set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR}") +if(WIN32) + set(append_library_dirs "${append_library_dirs}/$") +endif() + +ament_add_gtest(test_publisher_with_type_adapter test_publisher_with_type_adapter.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}" +) +if(TARGET test_publisher_with_type_adapter) + target_link_libraries(test_publisher_with_type_adapter + ${PROJECT_NAME} + ${cpp_typesupport_target}) +endif() + +ament_add_gtest(test_subscription_with_type_adapter test_subscription_with_type_adapter.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}" +) +if(TARGET test_subscription_with_type_adapter) + target_link_libraries(test_subscription_with_type_adapter + ${PROJECT_NAME} + ${cpp_typesupport_target}) +endif() + +ament_add_gtest(test_subscription_publisher_with_same_type_adapter test_subscription_publisher_with_same_type_adapter.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}" +) +if(TARGET test_subscription_publisher_with_same_type_adapter) + target_link_libraries(test_subscription_publisher_with_same_type_adapter + ${PROJECT_NAME} + ${cpp_typesupport_target} + ${statistics_msgs_TARGETS} + ) +endif() + +ament_add_gtest(test_publisher_subscription_count_api test_publisher_subscription_count_api.cpp) +if(TARGET test_publisher_subscription_count_api) + target_link_libraries(test_publisher_subscription_count_api ${PROJECT_NAME} ${test_msgs_TARGETS}) +endif() +ament_add_gtest(test_qos test_qos.cpp) +if(TARGET test_qos) + target_link_libraries(test_qos + ${PROJECT_NAME} + rmw::rmw + ) +endif() + +ament_add_gmock(test_qos_overriding_options test_qos_overriding_options.cpp) +if(TARGET test_qos_overriding_options) + target_link_libraries(test_qos_overriding_options ${PROJECT_NAME}) +endif() +ament_add_gmock(test_qos_parameters test_qos_parameters.cpp) +if(TARGET test_qos_parameters) + target_link_libraries(test_qos_parameters ${PROJECT_NAME}) +endif() +ament_add_gtest(test_rate test_rate.cpp) +if(TARGET test_rate) + target_link_libraries(test_rate ${PROJECT_NAME}) +endif() +ament_add_gtest(test_serialized_message_allocator test_serialized_message_allocator.cpp) +if(TARGET test_serialized_message_allocator) + target_link_libraries(test_serialized_message_allocator ${PROJECT_NAME} rcl::rcl ${test_msgs_TARGETS}) +endif() +ament_add_gtest(test_serialized_message test_serialized_message.cpp) +if(TARGET test_serialized_message) + target_link_libraries(test_serialized_message ${PROJECT_NAME} rcpputils::rcpputils ${test_msgs_TARGETS}) +endif() +ament_add_gtest(test_service test_service.cpp) +ament_add_test_label(test_service mimick) +if(TARGET test_service) + target_link_libraries(test_service ${PROJECT_NAME} mimick ${rcl_interfaces_TARGES} ${test_msgs_TARGETS}) +endif() +ament_add_gmock(test_service_introspection test_service_introspection.cpp) +ament_add_test_label(test_service_introspection mimick) +if(TARGET test_service_introspection) + target_link_libraries(test_service_introspection ${PROJECT_NAME} mimick ${service_msgs_TARGETS} ${test_msgs_TARGETS}) +endif() +# Creating and destroying nodes is slow with Connext, so this needs larger timeout. +ament_add_gtest(test_subscription test_subscription.cpp TIMEOUT 120) +ament_add_test_label(test_subscription mimick) +if(TARGET test_subscription) + target_link_libraries(test_subscription ${PROJECT_NAME} mimick ${test_msgs_TARGETS}) +endif() +ament_add_gtest(test_subscription_publisher_count_api test_subscription_publisher_count_api.cpp) +if(TARGET test_subscription_publisher_count_api) + target_link_libraries(test_subscription_publisher_count_api ${PROJECT_NAME} ${test_msgs_TARGETS}) +endif() +ament_add_gtest(test_subscription_traits test_subscription_traits.cpp) +if(TARGET test_subscription_traits) + target_link_libraries(test_subscription_traits ${PROJECT_NAME} rcl::rcl ${test_msgs_TARGETS}) +endif() +ament_add_gtest(test_type_support test_type_support.cpp) +if(TARGET test_type_support) + target_link_libraries(test_type_support ${PROJECT_NAME} ${test_msgs_TARGETS}) +endif() +ament_add_gmock(test_typesupport_helpers test_typesupport_helpers.cpp) +if(TARGET test_typesupport_helpers) + target_link_libraries(test_typesupport_helpers ${PROJECT_NAME} rcpputils::rcpputils) +endif() +ament_add_gtest(test_find_weak_nodes test_find_weak_nodes.cpp) +if(TARGET test_find_weak_nodes) + target_link_libraries(test_find_weak_nodes ${PROJECT_NAME}) +endif() + +ament_add_gtest(test_externally_defined_services test_externally_defined_services.cpp) +target_link_libraries(test_externally_defined_services ${PROJECT_NAME} rcl::rcl ${test_msgs_TARGETS}) + +ament_add_gtest(test_duration test_duration.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") +if(TARGET test_duration) + target_link_libraries(test_duration ${PROJECT_NAME} rcl::rcl) +endif() + +ament_add_gtest(test_logger test_logger.cpp) +target_link_libraries(test_logger ${PROJECT_NAME} rcutils::rcutils) + +ament_add_gmock(test_logging test_logging.cpp) +target_link_libraries(test_logging ${PROJECT_NAME} rcutils::rcutils) + +ament_add_gmock(test_context test_context.cpp) +target_link_libraries(test_context ${PROJECT_NAME}) + +ament_add_gtest(test_time test_time.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") +if(TARGET test_time) + target_link_libraries(test_time ${PROJECT_NAME} rcl::rcl rcutils::rcutils) +endif() + +ament_add_gtest(test_timer test_timer.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") +ament_add_test_label(test_timer mimick) +if(TARGET test_timer) + target_link_libraries(test_timer ${PROJECT_NAME} mimick rcl::rcl) +endif() + +ament_add_gtest(test_timers_manager test_timers_manager.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") +if(TARGET test_timers_manager) + target_link_libraries(test_timers_manager ${PROJECT_NAME}) +endif() + +ament_add_gtest(test_time_source test_time_source.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") +if(TARGET test_time_source) + target_link_libraries(test_time_source ${PROJECT_NAME} rcl::rcl) +endif() + +ament_add_gtest(test_utilities test_utilities.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") +ament_add_test_label(test_utilities mimick) +if(TARGET test_utilities) + target_link_libraries(test_utilities ${PROJECT_NAME} mimick rcl::rcl) +endif() + +ament_add_gtest(test_wait_for_message test_wait_for_message.cpp) +if(TARGET test_wait_for_message) + target_link_libraries(test_wait_for_message ${PROJECT_NAME} ${test_msgs_TARGETS}) +endif() + +ament_add_gtest(test_logger_service test_logger_service.cpp) +if(TARGET test_logger_service) + target_link_libraries(test_logger_service ${PROJECT_NAME} ${rcl_interfaces_TARGETS}) +endif() + +ament_add_gtest(test_interface_traits test_interface_traits.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") +if(TARGET test_interface_traits) + target_link_libraries(test_interface_traits ${PROJECT_NAME}) +endif() + +ament_add_gtest(test_reinitialized_timers + executors/test_reinitialized_timers.cpp + TIMEOUT 30) +if(TARGET test_reinitialized_timers) + target_link_libraries(test_reinitialized_timers ${PROJECT_NAME}) +endif() + +ament_add_gtest( + test_executors + executors/test_executors.cpp + executors/test_waitable.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}" + TIMEOUT 180) +if(CMAKE_BUILD_TYPE STREQUAL "Debug" AND MSVC) + target_compile_options(test_executors PRIVATE "/bigobj") +endif() +if(TARGET test_executors) + target_link_libraries(test_executors ${PROJECT_NAME} rcl::rcl ${test_msgs_TARGETS}) +endif() + +ament_add_gtest( + test_executors_timer_cancel_behavior + executors/test_executors_timer_cancel_behavior.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}" + TIMEOUT 180) +if(TARGET test_executors_timer_cancel_behavior) + target_link_libraries(test_executors_timer_cancel_behavior ${PROJECT_NAME} ${rosgraph_msgs_TARGETS}) +endif() + +ament_add_gtest( + test_executors_callback_group_behavior + executors/test_executors_callback_group_behavior.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}" + TIMEOUT 180) +if(TARGET test_executors_callback_group_behavior) + target_link_libraries(test_executors_callback_group_behavior ${PROJECT_NAME}) +endif() + +ament_add_gtest( + test_executors_intraprocess + executors/test_executors_intraprocess.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}" + TIMEOUT 180) +if(TARGET test_executors_intraprocess) + target_link_libraries(test_executors_intraprocess ${PROJECT_NAME} ${test_msgs_TARGETS}) +endif() + +ament_add_gtest( + test_executors_busy_waiting + executors/test_executors_busy_waiting.cpp + executors/test_waitable.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}" + TIMEOUT 180) +if(TARGET test_executors_busy_waiting) + target_link_libraries(test_executors_busy_waiting ${PROJECT_NAME}) +endif() + +ament_add_gtest( + test_executors_warmup + executors/test_executors_warmup.cpp + executors/test_waitable.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}" + TIMEOUT 180) +if(TARGET test_executors_warmup) + target_link_libraries(test_executors_warmup ${PROJECT_NAME} ${test_msgs_TARGETS}) +endif() + +ament_add_gtest(test_static_single_threaded_executor executors/test_static_single_threaded_executor.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") +ament_add_test_label(test_static_single_threaded_executor mimick) +if(TARGET test_static_single_threaded_executor) + target_link_libraries(test_static_single_threaded_executor ${PROJECT_NAME} mimick ${test_msgs_TARGETS}) +endif() + +ament_add_gtest(test_multi_threaded_executor executors/test_multi_threaded_executor.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") +if(TARGET test_multi_threaded_executor) + target_link_libraries(test_multi_threaded_executor ${PROJECT_NAME}) +endif() + +ament_add_gtest(test_entities_collector executors/test_entities_collector.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}" TIMEOUT 120) +if(TARGET test_entities_collector) + target_link_libraries(test_entities_collector ${PROJECT_NAME}) +endif() + +ament_add_gtest(test_executor_notify_waitable executors/test_executor_notify_waitable.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}" TIMEOUT 120) +ament_add_test_label(test_executor_notify_waitable mimick) +if(TARGET test_executor_notify_waitable) + target_link_libraries(test_executor_notify_waitable ${PROJECT_NAME} mimick rcpputils::rcpputils) +endif() + +ament_add_gtest(test_events_executor executors/test_events_executor.cpp TIMEOUT 60) +if(TARGET test_events_executor) + target_link_libraries(test_events_executor ${PROJECT_NAME} ${test_msgs_TARGETS}) +endif() + +ament_add_gtest(test_events_queue executors/test_events_queue.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") +if(TARGET test_events_queue) + target_link_libraries(test_events_queue ${PROJECT_NAME}) +endif() + +ament_add_gtest(test_guard_condition test_guard_condition.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") +ament_add_test_label(test_guard_condition mimick) +if(TARGET test_guard_condition) + target_link_libraries(test_guard_condition ${PROJECT_NAME} mimick) +endif() + +ament_add_gtest(test_wait_set test_wait_set.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") +if(TARGET test_wait_set) + target_link_libraries(test_wait_set ${PROJECT_NAME} ${rcl_interfaces_TARGETS} ${test_msgs_TARGETS}) +endif() + +ament_add_gtest(test_subscription_topic_statistics topic_statistics/test_subscription_topic_statistics.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}" +) +if(TARGET test_subscription_topic_statistics) + target_link_libraries(test_subscription_topic_statistics + ${PROJECT_NAME} + libstatistics_collector::libstatistics_collector + ${statistics_msgs_TARGETS} + ${test_msgs_TARGETS} + ) +endif() + +ament_add_gtest(test_subscription_options test_subscription_options.cpp) +if(TARGET test_subscription_options) + target_link_libraries(test_subscription_options ${PROJECT_NAME}) +endif() + +ament_add_gtest(test_dynamic_storage wait_set_policies/test_dynamic_storage.cpp) +if(TARGET test_dynamic_storage) + target_link_libraries(test_dynamic_storage ${PROJECT_NAME} ${test_msgs_TARGETS}) +endif() + +ament_add_gtest(test_storage_policy_common wait_set_policies/test_storage_policy_common.cpp) +ament_add_test_label(test_storage_policy_common mimick) +if(TARGET test_storage_policy_common) + target_link_libraries(test_storage_policy_common ${PROJECT_NAME} mimick ${test_msgs_TARGETS}) +endif() + +ament_add_gtest(test_static_storage wait_set_policies/test_static_storage.cpp) +if(TARGET test_static_storage) + target_link_libraries(test_static_storage ${PROJECT_NAME} ${test_msgs_TARGETS}) +endif() + +ament_add_gtest(test_thread_safe_synchronization wait_set_policies/test_thread_safe_synchronization.cpp) +if(TARGET test_thread_safe_synchronization) + target_link_libraries(test_thread_safe_synchronization ${PROJECT_NAME} ${test_msgs_TARGETS}) +endif() + +ament_add_gtest(test_intra_process_waitable waitables/test_intra_process_waitable.cpp) +if(TARGET test_intra_process_waitable) + target_link_libraries(test_intra_process_waitable ${PROJECT_NAME} ${test_msgs_TARGETS}) +endif() + +ament_add_gtest(test_rosout_qos test_rosout_qos.cpp) +if(TARGET test_rosout_qos) + target_link_libraries(test_rosout_qos ${PROJECT_NAME} rcl::rcl rmw::rmw) +endif() + +ament_add_gtest(test_rosout_subscription test_rosout_subscription.cpp) +if(TARGET test_rosout_subscription) + target_link_libraries(test_rosout_subscription ${PROJECT_NAME} ${rcl_interfaces_TARGETS}) +endif() + +ament_add_gtest(test_executor test_executor.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}" + TIMEOUT 120) +ament_add_test_label(test_executor mimick) +if(TARGET test_executor) + target_link_libraries(test_executor ${PROJECT_NAME} mimick ${test_msgs_TARGETS}) +endif() + +ament_add_gtest(test_graph_listener test_graph_listener.cpp) +ament_add_test_label(test_graph_listener mimick) +if(TARGET test_graph_listener) + target_link_libraries(test_graph_listener ${PROJECT_NAME} mimick) +endif() + +ament_add_gmock_executable(test_qos_event test_qos_event.cpp) +if(TARGET test_qos_event) + target_link_libraries(test_qos_event ${PROJECT_NAME} mimick rcutils::rcutils rmw::rmw ${test_msgs_TARGETS}) +endif() + +ament_add_gmock_executable(test_generic_pubsub test_generic_pubsub.cpp) +if(TARGET test_generic_pubsub) + target_link_libraries(test_generic_pubsub ${PROJECT_NAME} rcl::rcl ${test_msgs_TARGETS}) +endif() + +ament_add_gmock_executable(test_add_callback_groups_to_executor test_add_callback_groups_to_executor.cpp) +if(TARGET test_add_callback_groups_to_executor) + target_link_libraries(test_add_callback_groups_to_executor ${PROJECT_NAME} ${test_msgs_TARGETS}) +endif() + +ament_add_gmock_executable(test_subscription_content_filter test_subscription_content_filter.cpp) +if(TARGET test_subscription_content_filter) + target_link_libraries(test_subscription_content_filter ${PROJECT_NAME} mimick ${test_msgs_TARGETS}) +endif() + +function(test_on_all_rmws) + set(rmw_implementation_env_var RMW_IMPLEMENTATION=${rmw_implementation}) + + ament_add_gmock_test(test_qos_event + TEST_NAME test_qos_event${target_suffix} + ENV ${rmw_implementation_env_var} + ) + ament_add_test_label(test_qos_event${target_suffix} mimick) + + ament_add_gmock_test(test_generic_pubsub + TEST_NAME test_generic_pubsub${target_suffix} + ENV ${rmw_implementation_env_var} + ) + + ament_add_gmock_test(test_add_callback_groups_to_executor + TEST_NAME test_add_callback_groups_to_executor${target_suffix} + ENV ${rmw_implementation_env_var} + TIMEOUT 120 + ) + + ament_add_gmock_test(test_subscription_content_filter + TEST_NAME test_subscription_content_filter${target_suffix} + ENV ${rmw_implementation_env_var} + TIMEOUT 120 + ) + ament_add_test_label(test_subscription_content_filter${target_suffix} mimick) +endfunction() +call_for_each_rmw_implementation(test_on_all_rmws) diff --git a/rclcpp/test/rclcpp/allocator/test_allocator_common.cpp b/rclcpp/test/rclcpp/allocator/test_allocator_common.cpp new file mode 100644 index 0000000000..4619b7665d --- /dev/null +++ b/rclcpp/test/rclcpp/allocator/test_allocator_common.cpp @@ -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. + +#include + +#include + +#include "rclcpp/allocator/allocator_common.hpp" + +TEST(TestAllocatorCommon, retyped_allocate) { + std::allocator allocator; + void * untyped_allocator = &allocator; + void * allocated_mem = + rclcpp::allocator::retyped_allocate>(1u, untyped_allocator); + // The more natural check here is ASSERT_NE(nullptr, ptr), but clang static + // analysis throws a false-positive memory leak warning. Use ASSERT_TRUE instead. + ASSERT_TRUE(nullptr != allocated_mem); + + auto code = [&untyped_allocator, allocated_mem]() { + rclcpp::allocator::retyped_deallocate>( + allocated_mem, untyped_allocator); + }; + EXPECT_NO_THROW(code()); + + allocated_mem = allocator.allocate(1); + // The more natural check here is ASSERT_NE(nullptr, ptr), but clang static + // analysis throws a false-positive memory leak warning. Use ASSERT_TRUE instead. + ASSERT_TRUE(nullptr != allocated_mem); + void * reallocated_mem = + rclcpp::allocator::retyped_reallocate>( + allocated_mem, 2u, untyped_allocator); + // The more natural check here is ASSERT_NE(nullptr, ptr), but clang static + // analysis throws a false-positive memory leak warning. Use ASSERT_TRUE instead. + ASSERT_TRUE(nullptr != reallocated_mem); + + auto code2 = [&untyped_allocator, reallocated_mem]() { + rclcpp::allocator::retyped_deallocate>( + reallocated_mem, untyped_allocator); + }; + EXPECT_NO_THROW(code2()); +} + +TEST(TestAllocatorCommon, retyped_zero_allocate_basic) { + std::allocator allocator; + void * untyped_allocator = &allocator; + void * allocated_mem = + rclcpp::allocator::retyped_zero_allocate>(20u, 1u, untyped_allocator); + ASSERT_TRUE(nullptr != allocated_mem); + + auto code = [&untyped_allocator, allocated_mem]() { + rclcpp::allocator::retyped_deallocate>( + allocated_mem, untyped_allocator); + }; + EXPECT_NO_THROW(code()); +} + +TEST(TestAllocatorCommon, retyped_zero_allocate) { + std::allocator allocator; + void * untyped_allocator = &allocator; + void * allocated_mem = + rclcpp::allocator::retyped_zero_allocate>(20u, 1u, untyped_allocator); + // The more natural check here is ASSERT_NE(nullptr, ptr), but clang static + // analysis throws a false-positive memory leak warning. Use ASSERT_TRUE instead. + ASSERT_TRUE(nullptr != allocated_mem); + + auto code = [&untyped_allocator, allocated_mem]() { + rclcpp::allocator::retyped_deallocate>( + allocated_mem, untyped_allocator); + }; + EXPECT_NO_THROW(code()); + + allocated_mem = allocator.allocate(1); + // The more natural check here is ASSERT_NE(nullptr, ptr), but clang static + // analysis throws a false-positive memory leak warning. Use ASSERT_TRUE instead. + ASSERT_TRUE(nullptr != allocated_mem); + void * reallocated_mem = + rclcpp::allocator::retyped_reallocate>( + allocated_mem, 2u, untyped_allocator); + // The more natural check here is ASSERT_NE(nullptr, ptr), but clang static + // analysis throws a false-positive memory leak warning. Use ASSERT_TRUE instead. + ASSERT_TRUE(nullptr != reallocated_mem); + + auto code2 = [&untyped_allocator, reallocated_mem]() { + rclcpp::allocator::retyped_deallocate>( + reallocated_mem, untyped_allocator); + }; + EXPECT_NO_THROW(code2()); +} + +TEST(TestAllocatorCommon, get_rcl_allocator) { + std::allocator allocator; + auto rcl_allocator = rclcpp::allocator::get_rcl_allocator(allocator); + EXPECT_NE(nullptr, rcl_allocator.allocate); + EXPECT_NE(nullptr, rcl_allocator.deallocate); + EXPECT_NE(nullptr, rcl_allocator.reallocate); + EXPECT_NE(nullptr, rcl_allocator.zero_allocate); + // Not testing state as that may or may not be null depending on platform +} + +TEST(TestAllocatorCommon, get_void_rcl_allocator) { + std::allocator allocator; + auto rcl_allocator = + rclcpp::allocator::get_rcl_allocator>(allocator); + EXPECT_NE(nullptr, rcl_allocator.allocate); + EXPECT_NE(nullptr, rcl_allocator.deallocate); + EXPECT_NE(nullptr, rcl_allocator.reallocate); + EXPECT_NE(nullptr, rcl_allocator.zero_allocate); + // Not testing state as that may or may not be null depending on platform +} diff --git a/rclcpp/test/rclcpp/allocator/test_allocator_deleter.cpp b/rclcpp/test/rclcpp/allocator/test_allocator_deleter.cpp new file mode 100644 index 0000000000..614534acb5 --- /dev/null +++ b/rclcpp/test/rclcpp/allocator/test_allocator_deleter.cpp @@ -0,0 +1,103 @@ +// 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 "rclcpp/allocator/allocator_deleter.hpp" + +#include "../../utils/rclcpp_gtest_macros.hpp" + +TEST(TestAllocatorDeleter, construct_destruct) { + std::allocator allocator; + + rclcpp::allocator::AllocatorDeleter> deleter; + EXPECT_EQ(nullptr, deleter.get_allocator()); + deleter.set_allocator(&allocator); + EXPECT_EQ(&allocator, deleter.get_allocator()); + + rclcpp::allocator::AllocatorDeleter> deleter2(&allocator); + EXPECT_EQ(&allocator, deleter2.get_allocator()); + + rclcpp::allocator::AllocatorDeleter> deleter3(deleter2); + EXPECT_EQ(&allocator, deleter3.get_allocator()); +} + +TEST(TestAllocatorDeleter, delete) { + std::allocator allocator; + int * some_mem = allocator.allocate(1u); + // The more natural check here is ASSERT_NE(nullptr, ptr), but clang static + // analysis throws a false-positive memory leak warning. Use ASSERT_TRUE instead. + ASSERT_TRUE(nullptr != some_mem); + + rclcpp::allocator::AllocatorDeleter> deleter(&allocator); + EXPECT_NO_THROW(deleter(some_mem)); +} + +TEST(TestAllocatorDeleter, set_allocator_for_deleter_AllocatorDeleter) { + using AllocatorT = std::allocator; + using DeleterT = rclcpp::allocator::AllocatorDeleter; + AllocatorT allocator; + DeleterT deleter(&allocator); + + std::allocator allocator2; + rclcpp::allocator::set_allocator_for_deleter(&deleter, &allocator2); + EXPECT_EQ(&allocator2, deleter.get_allocator()); + + auto throwing_statement = [&allocator]() { + DeleterT * null_del_ptr = nullptr; + rclcpp::allocator::set_allocator_for_deleter( + null_del_ptr, &allocator); + }; + RCLCPP_EXPECT_THROW_EQ( + throwing_statement(), + std::invalid_argument("Argument was NULL to set_allocator_for_deleter")); + + auto throwing_statement2 = [&deleter]() { + AllocatorT * null_alloc_ptr = nullptr; + rclcpp::allocator::set_allocator_for_deleter( + &deleter, null_alloc_ptr); + }; + + RCLCPP_EXPECT_THROW_EQ( + throwing_statement2(), + std::invalid_argument("Argument was NULL to set_allocator_for_deleter")); +} + +TEST(TestAllocatorDeleter, set_allocator_for_deleter_std_default_delete) { + using AllocatorT = std::allocator; + using DeleterT = std::default_delete; + auto not_throwing_statement = []() { + AllocatorT allocator; + DeleterT deleter; + rclcpp::allocator::set_allocator_for_deleter(&deleter, &allocator); + }; + EXPECT_NO_THROW(not_throwing_statement()); +} + +TEST(TestAllocatorDeleter, set_allocator_for_deleter_unexpected_template) { + class SomeAllocatorClass {}; + class SomeDeleterClass {}; + using AllocatorT = SomeAllocatorClass; + using DeleterT = SomeDeleterClass; + auto throwing_statement = []() { + DeleterT deleter; + AllocatorT allocator; + rclcpp::allocator::set_allocator_for_deleter(&deleter, &allocator); + }; + RCLCPP_EXPECT_THROW_EQ( + throwing_statement(), + std::runtime_error("Reached unexpected template specialization")); +} diff --git a/rclcpp/test/rclcpp/exceptions/test_exceptions.cpp b/rclcpp/test/rclcpp/exceptions/test_exceptions.cpp new file mode 100644 index 0000000000..184dfc7a06 --- /dev/null +++ b/rclcpp/test/rclcpp/exceptions/test_exceptions.cpp @@ -0,0 +1,58 @@ +// 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 "rclcpp/exceptions/exceptions.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "../../mocking_utils/patch.hpp" +#include "../../utils/rclcpp_gtest_macros.hpp" + +TEST(TestExceptions, throw_from_rcl_error) { + EXPECT_THROW( + rclcpp::exceptions::throw_from_rcl_error(RCL_RET_BAD_ALLOC, ""), + rclcpp::exceptions::RCLBadAlloc); + + EXPECT_THROW( + rclcpp::exceptions::throw_from_rcl_error(RCL_RET_INVALID_ARGUMENT, ""), + rclcpp::exceptions::RCLInvalidArgument); + + EXPECT_THROW( + rclcpp::exceptions::throw_from_rcl_error(RCL_RET_INVALID_ROS_ARGS, ""), + rclcpp::exceptions::RCLInvalidROSArgsError); + + EXPECT_THROW( + rclcpp::exceptions::throw_from_rcl_error(RCL_RET_ERROR, ""), + rclcpp::exceptions::RCLError); + + RCLCPP_EXPECT_THROW_EQ( + rclcpp::exceptions::throw_from_rcl_error(RCL_RET_OK, ""), + std::invalid_argument("ret is RCL_RET_OK")); + + { + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_get_error_state, nullptr); + RCLCPP_EXPECT_THROW_EQ( + rclcpp::exceptions::throw_from_rcl_error(RCL_RET_ERROR, ""), + std::runtime_error("rcl error state is not set")); + } +} + +TEST(TestExceptions, basic_constructors) { + EXPECT_STREQ("node is invalid", rclcpp::exceptions::InvalidNodeError().what()); + rcl_error_state_t error_state{"error", __FILE__, __LINE__}; + EXPECT_STREQ( + "prefix: error not set", + rclcpp::exceptions::RCLInvalidROSArgsError(RCL_RET_ERROR, &error_state, "prefix: ").what()); +} diff --git a/rclcpp/test/rclcpp/executors/executor_types.hpp b/rclcpp/test/rclcpp/executors/executor_types.hpp new file mode 100644 index 0000000000..baa13be9d5 --- /dev/null +++ b/rclcpp/test/rclcpp/executors/executor_types.hpp @@ -0,0 +1,101 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__EXECUTORS__EXECUTOR_TYPES_HPP_ +#define RCLCPP__EXECUTORS__EXECUTOR_TYPES_HPP_ + +#include + +#include +#include + +#include "rclcpp/experimental/executors/events_executor/events_executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/executors/static_single_threaded_executor.hpp" +#include "rclcpp/executors/multi_threaded_executor.hpp" + +// suppress deprecated StaticSingleThreadedExecutor warning +// we define an alias that explicitly indicates that this class is deprecated, while avoiding +// polluting a lot of files the gcc pragmas +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif +using DeprecatedStaticSingleThreadedExecutor = rclcpp::executors::StaticSingleThreadedExecutor; +// remove warning suppression +#if !defined(_WIN32) +# pragma GCC diagnostic pop +#else // !defined(_WIN32) +# pragma warning(pop) +#endif + +#ifdef __clang__ +# pragma clang diagnostic push +# pragma clang diagnostic ignored "-Wdeprecated-declarations" +#endif +using ExecutorTypes = + ::testing::Types< + rclcpp::executors::SingleThreadedExecutor, + rclcpp::executors::MultiThreadedExecutor, + DeprecatedStaticSingleThreadedExecutor, + rclcpp::experimental::executors::EventsExecutor>; +#ifdef __clang__ +# pragma clang diagnostic pop +#endif + +class ExecutorTypeNames +{ +public: + template + static std::string GetName(int idx) + { + (void)idx; + if (std::is_same()) { + return "SingleThreadedExecutor"; + } + + if (std::is_same()) { + return "MultiThreadedExecutor"; + } +#ifdef __clang__ +# pragma clang diagnostic push +# pragma clang diagnostic ignored "-Wdeprecated-declarations" +#endif + if (std::is_same()) { + return "StaticSingleThreadedExecutor"; + } +#ifdef __clang__ +# pragma clang diagnostic pop +#endif + + if (std::is_same()) { + return "EventsExecutor"; + } + + return ""; + } +}; + +// StaticSingleThreadedExecutor is not included in these tests for now, due to: +// https://github.com/ros2/rclcpp/issues/1219 +using StandardExecutors = + ::testing::Types< + rclcpp::executors::SingleThreadedExecutor, + rclcpp::executors::MultiThreadedExecutor, + rclcpp::experimental::executors::EventsExecutor>; + +#endif // RCLCPP__EXECUTORS__EXECUTOR_TYPES_HPP_ diff --git a/rclcpp/test/rclcpp/executors/test_entities_collector.cpp b/rclcpp/test/rclcpp/executors/test_entities_collector.cpp new file mode 100644 index 0000000000..930dc68aff --- /dev/null +++ b/rclcpp/test/rclcpp/executors/test_entities_collector.cpp @@ -0,0 +1,320 @@ +// Copyright 2023 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 "rclcpp/executors/executor_notify_waitable.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/executors/executor_entities_collector.hpp" + +#include "../../utils/rclcpp_gtest_macros.hpp" + +class TestExecutorEntitiesCollector : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + + notify_waitable = std::make_shared(); + entities_collector = std::make_shared( + notify_waitable); + } + + void TearDown() + { + rclcpp::shutdown(); + } + + std::shared_ptr notify_waitable; + std::shared_ptr entities_collector; +}; + +TEST_F(TestExecutorEntitiesCollector, add_remove_node) { + auto node1 = std::make_shared("node1", "ns"); + + // Add a node + EXPECT_NO_THROW(entities_collector->add_node(node1->get_node_base_interface())); + EXPECT_NO_THROW(entities_collector->update_collections()); + + // Remove a node + EXPECT_NO_THROW(entities_collector->remove_node(node1->get_node_base_interface())); + EXPECT_NO_THROW(entities_collector->update_collections()); +} + +TEST_F(TestExecutorEntitiesCollector, add_node_twice) { + auto node1 = std::make_shared("node1", "ns"); + + EXPECT_NO_THROW(entities_collector->add_node(node1->get_node_base_interface())); + + RCLCPP_EXPECT_THROW_EQ( + entities_collector->add_node(node1->get_node_base_interface()), + std::runtime_error("Node '/ns/node1' has already been added to an executor.")); + + EXPECT_NO_THROW(entities_collector->update_collections()); +} + +TEST_F(TestExecutorEntitiesCollector, add_associated_node) { + auto node1 = std::make_shared("node1", "ns"); + + // Simulate node being associated somewhere else + auto & has_executor = node1->get_node_base_interface()->get_associated_with_executor_atomic(); + has_executor.store(true); + + // Add an already-associated node + RCLCPP_EXPECT_THROW_EQ( + entities_collector->remove_node(node1->get_node_base_interface()), + std::runtime_error("Node '/ns/node1' needs to be associated with this executor.")); + + has_executor.store(false); +} + +TEST_F(TestExecutorEntitiesCollector, remove_unassociated_node) { + auto node1 = std::make_shared("node1", "ns"); + + // Add an already-associated node + RCLCPP_EXPECT_THROW_EQ( + entities_collector->remove_node(node1->get_node_base_interface()), + std::runtime_error("Node '/ns/node1' needs to be associated with an executor.")); + + // Simulate node being associated somewhere else + auto & has_executor = node1->get_node_base_interface()->get_associated_with_executor_atomic(); + has_executor.store(true); + + // Add an already-associated node + RCLCPP_EXPECT_THROW_EQ( + entities_collector->remove_node(node1->get_node_base_interface()), + std::runtime_error("Node '/ns/node1' needs to be associated with this executor.")); +} + +TEST_F(TestExecutorEntitiesCollector, add_remove_node_before_update) { + auto notify_waitable = std::make_shared(); + auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(notify_waitable); + + auto node1 = std::make_shared("node1", "ns"); + auto node2 = std::make_shared("node2", "ns"); + + // Add and remove nodes without running updatenode + EXPECT_NO_THROW(entities_collector.add_node(node1->get_node_base_interface())); + EXPECT_NO_THROW(entities_collector.add_node(node2->get_node_base_interface())); + EXPECT_NO_THROW(entities_collector.remove_node(node1->get_node_base_interface())); + EXPECT_NO_THROW(entities_collector.remove_node(node2->get_node_base_interface())); + EXPECT_NO_THROW(entities_collector.update_collections()); +} + +TEST_F(TestExecutorEntitiesCollector, add_callback_group) { + auto notify_waitable = std::make_shared(); + auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(notify_waitable); + + auto node = std::make_shared("node1", "ns"); + rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + // Add a callback group and update + entities_collector.add_callback_group(cb_group); + + ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 0u); + ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 0u); + ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u); + + entities_collector.update_collections(); + + ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 1u); + ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 1u); + ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u); + + // Remove callback group and update + entities_collector.remove_callback_group(cb_group); + + ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 1u); + ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 1u); + ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u); + + entities_collector.update_collections(); + + ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 0u); + ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 0u); + ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u); +} + +TEST_F(TestExecutorEntitiesCollector, add_node_default_callback_group) { + auto notify_waitable = std::make_shared(); + auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(notify_waitable); + + auto node = std::make_shared("node1", "ns"); + entities_collector.add_node(node->get_node_base_interface()); + + ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 0u); + ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 0u); + ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u); + + entities_collector.update_collections(); + + ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 1u); + ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 0u); + ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 1u); +} + +TEST_F(TestExecutorEntitiesCollector, add_callback_group_after_add_node) { + auto notify_waitable = std::make_shared(); + auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(notify_waitable); + + auto node = std::make_shared("node1", "ns"); + rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + entities_collector.add_node(node->get_node_base_interface()); + + ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 0u); + ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 0u); + ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u); + + entities_collector.update_collections(); + + ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 2u); + ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 0u); + ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 2u); + + RCLCPP_EXPECT_THROW_EQ( + entities_collector.add_callback_group(cb_group), + std::runtime_error("Callback group has already been added to an executor.")); +} + +TEST_F(TestExecutorEntitiesCollector, add_callback_group_twice) { + auto notify_waitable = std::make_shared(); + auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(notify_waitable); + + auto node = std::make_shared("node1", "ns"); + rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + entities_collector.add_callback_group(cb_group); + + ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 0u); + ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 0u); + ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u); + + entities_collector.update_collections(); + + ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 1u); + ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 1u); + ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u); + + cb_group->get_associated_with_executor_atomic().exchange(false); + RCLCPP_EXPECT_THROW_EQ( + entities_collector.add_callback_group(cb_group), + std::runtime_error("Callback group has already been added to this executor.")); +} + +TEST_F(TestExecutorEntitiesCollector, remove_callback_group_after_node) { + auto notify_waitable = std::make_shared(); + auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(notify_waitable); + + auto node = std::make_shared("node1", "ns"); + rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + entities_collector.add_callback_group(cb_group); + + ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 0u); + ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 0u); + ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u); + + entities_collector.update_collections(); + + ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 1u); + ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 1u); + ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u); + + node.reset(); + + /** + * TODO(mjcarroll): Assert this when we are enforcing that nodes must be destroyed + * after their created callback groups. + RCLCPP_EXPECT_THROW_EQ( + entities_collector.remove_callback_group(cb_group), + std::runtime_error("Node must not be deleted before its callback group(s).")); + */ + EXPECT_NO_THROW(entities_collector.update_collections()); +} + +TEST_F(TestExecutorEntitiesCollector, remove_callback_group_after_node2) { + auto notify_waitable = std::make_shared(); + auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(notify_waitable); + + auto node = std::make_shared("node1", "ns"); + rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + entities_collector.add_callback_group(cb_group); + + ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 0u); + ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 0u); + ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u); + + entities_collector.update_collections(); + + ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 1u); + ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 1u); + ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u); + + EXPECT_NO_THROW(entities_collector.remove_callback_group(cb_group)); + + node.reset(); + + /** + * TODO(mjcarroll): Assert this when we are enforcing that nodes must be destroyed + * after their created callback groups. + RCLCPP_EXPECT_THROW_EQ( + entities_collector.remove_callback_group(cb_group), + std::runtime_error("Node must not be deleted before its callback group(s).")); + */ + EXPECT_NO_THROW(entities_collector.update_collections()); +} + +TEST_F(TestExecutorEntitiesCollector, remove_callback_group_twice) { + auto notify_waitable = std::make_shared(); + auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(notify_waitable); + + auto node = std::make_shared("node1", "ns"); + rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + entities_collector.add_callback_group(cb_group); + entities_collector.update_collections(); + + ASSERT_EQ(entities_collector.get_all_callback_groups().size(), 1u); + ASSERT_EQ(entities_collector.get_manually_added_callback_groups().size(), 1u); + ASSERT_EQ(entities_collector.get_automatically_added_callback_groups().size(), 0u); + + entities_collector.remove_callback_group(cb_group); + entities_collector.update_collections(); + + RCLCPP_EXPECT_THROW_EQ( + entities_collector.remove_callback_group(cb_group), + std::runtime_error("Callback group needs to be associated with an executor.")); +} + +TEST_F(TestExecutorEntitiesCollector, remove_node_opposite_order) { + auto notify_waitable = std::make_shared(); + auto entities_collector = rclcpp::executors::ExecutorEntitiesCollector(notify_waitable); + + auto node1 = std::make_shared("node1", "ns"); + EXPECT_NO_THROW(entities_collector.add_node(node1->get_node_base_interface())); + + auto node2 = std::make_shared("node2", "ns"); + EXPECT_NO_THROW(entities_collector.add_node(node2->get_node_base_interface())); + + EXPECT_NO_THROW(entities_collector.remove_node(node2->get_node_base_interface())); +} diff --git a/rclcpp/test/rclcpp/executors/test_events_executor.cpp b/rclcpp/test/rclcpp/executors/test_events_executor.cpp new file mode 100644 index 0000000000..2c6504426e --- /dev/null +++ b/rclcpp/test/rclcpp/executors/test_events_executor.cpp @@ -0,0 +1,499 @@ +// Copyright 2023 iRobot Corporation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT 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 "rclcpp/experimental/executors/events_executor/events_executor.hpp" + +#include "test_msgs/srv/empty.hpp" +#include "test_msgs/msg/empty.hpp" + +using namespace std::chrono_literals; + +using rclcpp::experimental::executors::EventsExecutor; + +class TestEventsExecutor : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + } + + void TearDown() + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestEventsExecutor, run_pub_sub) +{ + auto node = std::make_shared("node"); + + bool msg_received = false; + auto subscription = node->create_subscription( + "topic", rclcpp::SensorDataQoS(), + [&msg_received](test_msgs::msg::Empty::ConstSharedPtr msg) + { + (void)msg; + msg_received = true; + }); + + auto publisher = node->create_publisher("topic", rclcpp::SensorDataQoS()); + + EventsExecutor executor; + executor.add_node(node); + + bool spin_exited = false; + std::thread spinner([&spin_exited, &executor]() { + executor.spin(); + spin_exited = true; + }); + + auto msg = std::make_unique(); + publisher->publish(std::move(msg)); + + // Wait some time for the subscription to receive the message + auto start = std::chrono::high_resolution_clock::now(); + while ( + !msg_received && + !spin_exited && + (std::chrono::high_resolution_clock::now() - start < 1s)) + { + std::this_thread::sleep_for(25ms); + } + + executor.cancel(); + spinner.join(); + executor.remove_node(node); + + EXPECT_TRUE(msg_received); + EXPECT_TRUE(spin_exited); +} + +TEST_F(TestEventsExecutor, run_clients_servers) +{ + auto node = std::make_shared("node"); + + bool request_received = false; + bool response_received = false; + auto service = + node->create_service( + "service", + [&request_received]( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) + { + request_received = true; + }); + auto client = node->create_client("service"); + + EventsExecutor executor; + executor.add_node(node); + + bool spin_exited = false; + std::thread spinner([&spin_exited, &executor]() { + executor.spin(); + spin_exited = true; + }); + + auto request = std::make_shared(); + client->async_send_request( + request, + [&response_received](rclcpp::Client::SharedFuture result_future) { + (void)result_future; + response_received = true; + }); + + // Wait some time for the client-server to be invoked + auto start = std::chrono::steady_clock::now(); + while ( + !response_received && + !spin_exited && + (std::chrono::steady_clock::now() - start < 1s)) + { + std::this_thread::sleep_for(5ms); + } + + executor.cancel(); + spinner.join(); + executor.remove_node(node); + + EXPECT_TRUE(request_received); + EXPECT_TRUE(response_received); + EXPECT_TRUE(spin_exited); +} + +TEST_F(TestEventsExecutor, spin_once_max_duration_timeout) +{ + auto node = std::make_shared("node"); + + EventsExecutor executor; + executor.add_node(node); + + // Consume previous events so we have a fresh start + executor.spin_all(1s); + + size_t t_runs = 0; + auto t = node->create_wall_timer( + 10s, + [&]() { + t_runs++; + }); + + // This first spin_once takes care of the waitable event + // generated by the addition of the timer to the node + executor.spin_once(1s); + EXPECT_EQ(0u, t_runs); + + auto start = std::chrono::steady_clock::now(); + + // This second spin_once should take care of the timer, + executor.spin_once(10ms); + + // but doesn't spin the time enough to call the timer callback. + EXPECT_EQ(0u, t_runs); + EXPECT_TRUE(std::chrono::steady_clock::now() - start < 200ms); +} + +TEST_F(TestEventsExecutor, spin_once_max_duration_timer) +{ + auto node = std::make_shared("node"); + + EventsExecutor executor; + executor.add_node(node); + + // Consume previous events so we have a fresh start + executor.spin_all(1s); + + size_t t_runs = 0; + auto t = node->create_wall_timer( + 10ms, + [&]() { + t_runs++; + }); + + // This first spin_once takes care of the waitable event + // generated by the addition of the timer to the node + executor.spin_once(1s); + EXPECT_EQ(0u, t_runs); + + auto start = std::chrono::steady_clock::now(); + + // This second spin_once should take care of the timer + executor.spin_once(11ms); + + EXPECT_EQ(1u, t_runs); + EXPECT_TRUE(std::chrono::steady_clock::now() - start < 200ms); +} + +TEST_F(TestEventsExecutor, spin_some_max_duration) +{ + { + auto node = std::make_shared("node"); + + size_t t_runs = 0; + auto t = node->create_wall_timer( + 10s, + [&]() { + t_runs++; + }); + + EventsExecutor executor; + executor.add_node(node); + + auto start = std::chrono::steady_clock::now(); + executor.spin_some(10ms); + + EXPECT_EQ(0u, t_runs); + EXPECT_TRUE(std::chrono::steady_clock::now() - start < 200ms); + } + + { + auto node = std::make_shared("node"); + + size_t t_runs = 0; + auto t = node->create_wall_timer( + 10ms, + [&]() { + t_runs++; + }); + + // Sleep some time for the timer to be ready when spin + std::this_thread::sleep_for(10ms); + + EventsExecutor executor; + executor.add_node(node); + + auto start = std::chrono::steady_clock::now(); + executor.spin_some(10s); + + EXPECT_EQ(1u, t_runs); + EXPECT_TRUE(std::chrono::steady_clock::now() - start < 200ms); + } +} + +TEST_F(TestEventsExecutor, spin_some_zero_duration) +{ + auto node = std::make_shared("node"); + + size_t t_runs = 0; + auto t = node->create_wall_timer( + 20ms, + [&]() { + t_runs++; + }); + + // Sleep some time for the timer to be ready when spin + std::this_thread::sleep_for(20ms); + + EventsExecutor executor; + executor.add_node(node); + executor.spin_some(0ms); + + EXPECT_EQ(1u, t_runs); +} + +TEST_F(TestEventsExecutor, spin_all_max_duration) +{ + { + auto node = std::make_shared("node"); + + size_t t_runs = 0; + auto t = node->create_wall_timer( + 10s, + [&]() { + t_runs++; + }); + + EventsExecutor executor; + executor.add_node(node); + + auto start = std::chrono::steady_clock::now(); + executor.spin_all(10ms); + + EXPECT_EQ(0u, t_runs); + EXPECT_TRUE(std::chrono::steady_clock::now() - start < 200ms); + } + + { + auto node = std::make_shared("node"); + + size_t t_runs = 0; + auto t = node->create_wall_timer( + 10ms, + [&]() { + t_runs++; + }); + + // Sleep some time for the timer to be ready when spin + std::this_thread::sleep_for(10ms); + + EventsExecutor executor; + executor.add_node(node); + + auto start = std::chrono::steady_clock::now(); + executor.spin_all(10s); + + EXPECT_EQ(1u, t_runs); + EXPECT_TRUE(std::chrono::steady_clock::now() - start < 200ms); + } + + EventsExecutor executor; + EXPECT_THROW(executor.spin_all(0ms), std::invalid_argument); + EXPECT_THROW(executor.spin_all(-5ms), std::invalid_argument); +} + +TEST_F(TestEventsExecutor, cancel_while_timers_running) +{ + auto node = std::make_shared("node"); + + EventsExecutor executor; + executor.add_node(node); + + // Take care of previous events for a fresh start + executor.spin_all(1s); + + size_t t1_runs = 0; + auto t1 = node->create_wall_timer( + 1ms, + [&]() { + t1_runs++; + std::this_thread::sleep_for(50ms); + }); + + size_t t2_runs = 0; + auto t2 = node->create_wall_timer( + 1ms, + [&]() { + t2_runs++; + std::this_thread::sleep_for(50ms); + }); + + + std::thread spinner([&executor]() {executor.spin();}); + + std::this_thread::sleep_for(10ms); + // Call cancel while t1 callback is still being executed + executor.cancel(); + spinner.join(); + + // Depending on the latency on the system, t2 may start to execute before cancel is signaled + EXPECT_GE(1u, t1_runs); + EXPECT_GE(1u, t2_runs); +} + +TEST_F(TestEventsExecutor, cancel_while_timers_waiting) +{ + auto node = std::make_shared("node"); + + size_t t1_runs = 0; + auto t1 = node->create_wall_timer( + 100s, + [&]() { + t1_runs++; + }); + + EventsExecutor executor; + executor.add_node(node); + + auto start = std::chrono::steady_clock::now(); + std::thread spinner([&executor]() {executor.spin();}); + + std::this_thread::sleep_for(10ms); + executor.cancel(); + spinner.join(); + + EXPECT_EQ(0u, t1_runs); + EXPECT_TRUE(std::chrono::steady_clock::now() - start < 1s); +} + +TEST_F(TestEventsExecutor, destroy_entities) +{ + // This test fails on Windows! We skip it for now + GTEST_SKIP(); + + // Create a publisher node and start publishing messages + auto node_pub = std::make_shared("node_pub"); + auto publisher = node_pub->create_publisher("topic", rclcpp::QoS(10)); + auto timer = node_pub->create_wall_timer( + 2ms, [&]() {publisher->publish(std::make_unique());}); + EventsExecutor executor_pub; + executor_pub.add_node(node_pub); + std::thread spinner([&executor_pub]() {executor_pub.spin();}); + + // Create a node with two different subscriptions to the topic + auto node_sub = std::make_shared("node_sub"); + size_t callback_count_1 = 0; + auto subscription_1 = + node_sub->create_subscription( + "topic", rclcpp::QoS(10), [&](test_msgs::msg::Empty::ConstSharedPtr) {callback_count_1++;}); + size_t callback_count_2 = 0; + auto subscription_2 = + node_sub->create_subscription( + "topic", rclcpp::QoS(10), [&](test_msgs::msg::Empty::ConstSharedPtr) {callback_count_2++;}); + EventsExecutor executor_sub; + executor_sub.add_node(node_sub); + + // Wait some time while messages are published + std::this_thread::sleep_for(10ms); + + // Destroy one of the two subscriptions + subscription_1.reset(); + + // Let subscriptions executor spin + executor_sub.spin_some(10ms); + + // The callback count of the destroyed subscription remained at 0 + EXPECT_EQ(0u, callback_count_1); + EXPECT_LT(0u, callback_count_2); + + executor_pub.cancel(); + spinner.join(); +} + +// Testing construction of a subscriptions with QoS event callback functions. +std::string * g_pub_log_msg; +std::string * g_sub_log_msg; +std::promise * g_log_msgs_promise; +TEST_F(TestEventsExecutor, test_default_incompatible_qos_callbacks) +{ + auto node = std::make_shared("node"); + rcutils_logging_output_handler_t original_output_handler = rcutils_logging_get_output_handler(); + + std::string pub_log_msg; + std::string sub_log_msg; + std::promise log_msgs_promise; + g_pub_log_msg = &pub_log_msg; + g_sub_log_msg = &sub_log_msg; + g_log_msgs_promise = &log_msgs_promise; + auto logger_callback = []( + const rcutils_log_location_t * /*location*/, + int /*level*/, const char * /*name*/, rcutils_time_point_value_t /*timestamp*/, + const char * format, va_list * args) -> void { + char buffer[1024]; + vsnprintf(buffer, sizeof(buffer), format, *args); + const std::string msg = buffer; + if (msg.rfind("New subscription discovered on topic '/test_topic'", 0) == 0) { + *g_pub_log_msg = buffer; + } else if (msg.rfind("New publisher discovered on topic '/test_topic'", 0) == 0) { + *g_sub_log_msg = buffer; + } + + if (!g_pub_log_msg->empty() && !g_sub_log_msg->empty()) { + g_log_msgs_promise->set_value(); + } + }; + rcutils_logging_set_output_handler(logger_callback); + + std::shared_future log_msgs_future = log_msgs_promise.get_future(); + + rclcpp::QoS qos_profile_publisher(10); + qos_profile_publisher.durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); + auto publisher = node->create_publisher( + "test_topic", qos_profile_publisher); + + rclcpp::QoS qos_profile_subscription(10); + qos_profile_subscription.durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + auto subscription = node->create_subscription( + "test_topic", qos_profile_subscription, [&](test_msgs::msg::Empty::ConstSharedPtr) {}); + + EventsExecutor ex; + ex.add_node(node->get_node_base_interface()); + + const auto timeout = std::chrono::seconds(10); + ex.spin_until_future_complete(log_msgs_future, timeout); + + rclcpp::QoSCheckCompatibleResult qos_compatible = rclcpp::qos_check_compatible( + publisher->get_actual_qos(), subscription->get_actual_qos()); + if (qos_compatible.compatibility == rclcpp::QoSCompatibility::Error) { + EXPECT_EQ( + "New subscription discovered on topic '/test_topic', requesting incompatible QoS. " + "No messages will be sent to it. Last incompatible policy: DURABILITY_QOS_POLICY", + pub_log_msg); + EXPECT_EQ( + "New publisher discovered on topic '/test_topic', offering incompatible QoS. " + "No messages will be sent to it. Last incompatible policy: DURABILITY_QOS_POLICY", + sub_log_msg); + } else { + EXPECT_EQ("", pub_log_msg); + EXPECT_EQ("", sub_log_msg); + } + + rcutils_logging_set_output_handler(original_output_handler); +} diff --git a/rclcpp/test/rclcpp/executors/test_events_queue.cpp b/rclcpp/test/rclcpp/executors/test_events_queue.cpp new file mode 100644 index 0000000000..741e6ad384 --- /dev/null +++ b/rclcpp/test/rclcpp/executors/test_events_queue.cpp @@ -0,0 +1,83 @@ +// Copyright 2023 iRobot Corporation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT 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 "rclcpp/experimental/executors/events_executor/events_executor_event_types.hpp" +#include "rclcpp/experimental/executors/events_executor/simple_events_queue.hpp" + +using namespace std::chrono_literals; + +TEST(TestEventsQueue, SimpleQueueTest) +{ + // Create a SimpleEventsQueue and a local queue + auto simple_queue = std::make_unique(); + rclcpp::experimental::executors::ExecutorEvent event {}; + bool ret = false; + + // Make sure the queue is empty at startup + EXPECT_TRUE(simple_queue->empty()); + EXPECT_EQ(simple_queue->size(), 0u); + + // Push 11 messages + for (uint32_t i = 1; i < 11; i++) { + rclcpp::experimental::executors::ExecutorEvent stub_event {}; + stub_event.num_events = 1; + simple_queue->enqueue(stub_event); + + EXPECT_FALSE(simple_queue->empty()); + EXPECT_EQ(simple_queue->size(), i); + } + + // Pop one message + ret = simple_queue->dequeue(event); + EXPECT_TRUE(ret); + EXPECT_FALSE(simple_queue->empty()); + EXPECT_EQ(simple_queue->size(), 9u); + + // Pop one message + ret = simple_queue->dequeue(event, std::chrono::nanoseconds(0)); + EXPECT_TRUE(ret); + EXPECT_FALSE(simple_queue->empty()); + EXPECT_EQ(simple_queue->size(), 8u); + + while (!simple_queue->empty()) { + ret = simple_queue->dequeue(event); + EXPECT_TRUE(ret); + } + + EXPECT_TRUE(simple_queue->empty()); + EXPECT_EQ(simple_queue->size(), 0u); + + ret = simple_queue->dequeue(event, std::chrono::nanoseconds(0)); + EXPECT_FALSE(ret); + + // Lets push an event into the queue and get it back + rclcpp::experimental::executors::ExecutorEvent push_event = { + simple_queue.get(), + nullptr, + 99, + rclcpp::experimental::executors::ExecutorEventType::SUBSCRIPTION_EVENT, + 1}; + + simple_queue->enqueue(push_event); + ret = simple_queue->dequeue(event); + EXPECT_TRUE(ret); + EXPECT_EQ(push_event.entity_key, event.entity_key); + EXPECT_EQ(push_event.waitable_data, event.waitable_data); + EXPECT_EQ(push_event.type, event.type); + EXPECT_EQ(push_event.num_events, event.num_events); +} diff --git a/rclcpp/test/rclcpp/executors/test_executor_notify_waitable.cpp b/rclcpp/test/rclcpp/executors/test_executor_notify_waitable.cpp new file mode 100644 index 0000000000..ab7f730a2e --- /dev/null +++ b/rclcpp/test/rclcpp/executors/test_executor_notify_waitable.cpp @@ -0,0 +1,97 @@ +// Copyright 2023 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 "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "rcpputils/scope_exit.hpp" + +#include "rclcpp/executors/executor_notify_waitable.hpp" + +#include "../../utils/rclcpp_gtest_macros.hpp" + + +class TestExecutorNotifyWaitable : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + } + + void TearDown() + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestExecutorNotifyWaitable, construct_destruct) { + { + auto waitable = std::make_shared(); + waitable.reset(); + } + { + auto on_execute_callback = []() {}; + auto waitable = + std::make_shared(on_execute_callback); + waitable.reset(); + } +} + +TEST_F(TestExecutorNotifyWaitable, add_remove_guard_conditions) { + auto on_execute_callback = []() {}; + auto waitable = + std::make_shared(on_execute_callback); + + auto node = std::make_shared("my_node", "/ns"); + auto notify_guard_condition = + + node->get_node_base_interface()->get_shared_notify_guard_condition(); + EXPECT_NO_THROW(waitable->add_guard_condition(notify_guard_condition)); + EXPECT_NO_THROW(waitable->remove_guard_condition(notify_guard_condition)); +} + +TEST_F(TestExecutorNotifyWaitable, wait) { + int on_execute_calls = 0; + auto on_execute_callback = [&on_execute_calls]() {on_execute_calls++;}; + + auto waitable = + std::make_shared(on_execute_callback); + + auto node = std::make_shared("my_node", "/ns"); + auto notify_guard_condition = + node->get_node_base_interface()->get_shared_notify_guard_condition(); + EXPECT_NO_THROW(waitable->add_guard_condition(notify_guard_condition)); + + auto default_cbg = node->get_node_base_interface()->get_default_callback_group(); + ASSERT_NE(nullptr, default_cbg->get_notify_guard_condition()); + + auto waitables = node->get_node_waitables_interface(); + waitables->add_waitable(std::static_pointer_cast(waitable), default_cbg); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + executor.spin_all(std::chrono::seconds(1)); + EXPECT_EQ(1u, on_execute_calls); + + // on_execute_callback doesn't change if the topology doesn't change + executor.spin_all(std::chrono::seconds(1)); + EXPECT_EQ(1u, on_execute_calls); +} diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp new file mode 100644 index 0000000000..cea0900a39 --- /dev/null +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -0,0 +1,845 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/** + * This test checks all implementations of rclcpp::executor to check they pass they basic API + * tests. Anything specific to any executor in particular should go in a separate test file. + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rcl/error_handling.h" +#include "rcl/time.h" +#include "rclcpp/clock.hpp" +#include "rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp" +#include "rclcpp/duration.hpp" +#include "rclcpp/guard_condition.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/time_source.hpp" + +#include "test_msgs/msg/empty.hpp" +#include "test_msgs/srv/empty.hpp" + +#include "./executor_types.hpp" +#include "./test_waitable.hpp" + +using namespace std::chrono_literals; + +template +class TestExecutors : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + + const auto test_info = ::testing::UnitTest::GetInstance()->current_test_info(); + std::stringstream test_name; + test_name << test_info->test_case_name() << "_" << test_info->name(); + node = std::make_shared("node", test_name.str()); + + callback_count = 0; + + const std::string topic_name = std::string("topic_") + test_name.str(); + publisher = node->create_publisher(topic_name, rclcpp::QoS(10)); + auto callback = [this](test_msgs::msg::Empty::ConstSharedPtr) {this->callback_count++;}; + subscription = + node->create_subscription( + topic_name, rclcpp::QoS(10), std::move(callback)); + } + + void TearDown() + { + publisher.reset(); + subscription.reset(); + node.reset(); + + rclcpp::shutdown(); + } + + rclcpp::Node::SharedPtr node; + rclcpp::Publisher::SharedPtr publisher; + rclcpp::Subscription::SharedPtr subscription; + int callback_count; +}; + +template +class TestExecutorsStable : public TestExecutors {}; + +TYPED_TEST_SUITE(TestExecutors, ExecutorTypes, ExecutorTypeNames); + +TYPED_TEST_SUITE(TestExecutorsStable, StandardExecutors, ExecutorTypeNames); + +// Make sure that executors detach from nodes when destructing +TYPED_TEST(TestExecutors, detachOnDestruction) +{ + using ExecutorType = TypeParam; + { + ExecutorType executor; + executor.add_node(this->node); + } + { + ExecutorType executor; + EXPECT_NO_THROW(executor.add_node(this->node)); + } +} + +// Make sure that the executor can automatically remove expired nodes correctly +TYPED_TEST(TestExecutors, addTemporaryNode) { + using ExecutorType = TypeParam; + ExecutorType executor; + + { + // Let node go out of scope before executor.spin() + auto node = std::make_shared("temporary_node"); + executor.add_node(node); + } + + // Sleep for a short time to verify executor.spin() is going, and didn't throw. + std::thread spinner([&]() {EXPECT_NO_THROW(executor.spin());}); + + std::this_thread::sleep_for(50ms); + executor.cancel(); + spinner.join(); +} + +// Make sure that a spinning empty executor can be cancelled +TYPED_TEST(TestExecutors, emptyExecutor) +{ + using ExecutorType = TypeParam; + ExecutorType executor; + std::thread spinner([&]() {EXPECT_NO_THROW(executor.spin());}); + std::this_thread::sleep_for(50ms); + executor.cancel(); + spinner.join(); +} + +// Check executor throws properly if the same node is added a second time +TYPED_TEST(TestExecutors, addNodeTwoExecutors) +{ + using ExecutorType = TypeParam; + ExecutorType executor1; + ExecutorType executor2; + EXPECT_NO_THROW(executor1.add_node(this->node)); + EXPECT_THROW(executor2.add_node(this->node), std::runtime_error); + executor1.remove_node(this->node, true); +} + +// Check simple spin example +TYPED_TEST(TestExecutors, spinWithTimer) +{ + using ExecutorType = TypeParam; + ExecutorType executor; + + bool timer_completed = false; + auto timer = this->node->create_wall_timer(1ms, [&]() {timer_completed = true;}); + executor.add_node(this->node); + + std::thread spinner([&]() {executor.spin();}); + + auto start = std::chrono::steady_clock::now(); + while (!timer_completed && (std::chrono::steady_clock::now() - start) < 10s) { + std::this_thread::sleep_for(1ms); + } + + EXPECT_TRUE(timer_completed); + // Cancel needs to be called before join, so that executor.spin() returns. + executor.cancel(); + spinner.join(); + executor.remove_node(this->node, true); +} + +TYPED_TEST(TestExecutors, spinWhileAlreadySpinning) +{ + using ExecutorType = TypeParam; + ExecutorType executor; + + std::atomic_bool timer_completed = false; + auto timer = this->node->create_wall_timer( + 1ms, [&]() { + timer_completed.store(true); + }); + + executor.add_node(this->node); + std::thread spinner([&]() {executor.spin();}); + + // Sleep for a short time to verify executor.spin() is going, and didn't throw. + auto start = std::chrono::steady_clock::now(); + while (!timer_completed.load() && (std::chrono::steady_clock::now() - start) < 10s) { + std::this_thread::sleep_for(1ms); + } + + EXPECT_TRUE(timer_completed); + EXPECT_THROW(executor.spin(), std::runtime_error); + + // Shutdown needs to be called before join, so that executor.spin() returns. + executor.cancel(); + spinner.join(); + executor.remove_node(this->node, true); +} + +// Check executor exits immediately if future is complete. +TYPED_TEST(TestExecutors, testSpinUntilFutureComplete) +{ + using ExecutorType = TypeParam; + ExecutorType executor; + executor.add_node(this->node); + + // test success of an immediately finishing future + std::promise promise; + std::future future = promise.get_future(); + promise.set_value(true); + + // spin_until_future_complete is expected to exit immediately, but would block up until its + // timeout if the future is not checked before spin_once_impl. + auto start = std::chrono::steady_clock::now(); + auto shared_future = future.share(); + auto ret = executor.spin_until_future_complete(shared_future, 1s); + executor.remove_node(this->node, true); + // Check it didn't reach timeout + EXPECT_GT(500ms, (std::chrono::steady_clock::now() - start)); + EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); +} + +// Same test, but uses a shared future. +TYPED_TEST(TestExecutors, testSpinUntilSharedFutureComplete) +{ + using ExecutorType = TypeParam; + ExecutorType executor; + executor.add_node(this->node); + + // test success of an immediately finishing future + std::promise promise; + std::future future = promise.get_future(); + promise.set_value(true); + + // spin_until_future_complete is expected to exit immediately, but would block up until its + // timeout if the future is not checked before spin_once_impl. + auto shared_future = future.share(); + auto start = std::chrono::steady_clock::now(); + auto ret = executor.spin_until_future_complete(shared_future, 1s); + executor.remove_node(this->node, true); + + // Check it didn't reach timeout + EXPECT_GT(500ms, (std::chrono::steady_clock::now() - start)); + EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); +} + +// For a longer running future that should require several iterations of spin_once +TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteNoTimeout) +{ + using ExecutorType = TypeParam; + ExecutorType executor; + executor.add_node(this->node); + + // This future doesn't immediately terminate, so some work gets performed. + std::future future = std::async( + std::launch::async, + [this]() { + auto start = std::chrono::steady_clock::now(); + while (this->callback_count < 1 && (std::chrono::steady_clock::now() - start) < 1s) { + std::this_thread::sleep_for(1ms); + } + }); + + bool spin_exited = false; + + // Timeout set to negative for no timeout. + std::thread spinner([&]() { + auto ret = executor.spin_until_future_complete(future, -1s); + EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); + executor.remove_node(this->node, true); + executor.cancel(); + spin_exited = true; + }); + + // Do some work for longer than the future needs. + for (int i = 0; i < 100; ++i) { + this->publisher->publish(test_msgs::msg::Empty()); + std::this_thread::sleep_for(1ms); + if (spin_exited) { + break; + } + } + + // Not testing accuracy, just want to make sure that some work occurred. + EXPECT_LT(0, this->callback_count); + + // If this fails, the test will probably crash because spinner goes out of scope while the thread + // is active. However, it beats letting this run until the gtest timeout. + ASSERT_TRUE(spin_exited); + executor.cancel(); + spinner.join(); +} + +// Check spin_until_future_complete timeout works as expected +TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteWithTimeout) +{ + using ExecutorType = TypeParam; + ExecutorType executor; + executor.add_node(this->node); + + bool spin_exited = false; + + // Needs to run longer than spin_until_future_complete's timeout. + std::future future = std::async( + std::launch::async, + [&spin_exited]() { + auto start = std::chrono::steady_clock::now(); + while (!spin_exited && (std::chrono::steady_clock::now() - start) < 1s) { + std::this_thread::sleep_for(1ms); + } + }); + + // Short timeout + std::thread spinner([&]() { + auto ret = executor.spin_until_future_complete(future, 1ms); + EXPECT_EQ(rclcpp::FutureReturnCode::TIMEOUT, ret); + executor.remove_node(this->node, true); + spin_exited = true; + }); + + // Do some work for longer than timeout needs. + for (int i = 0; i < 100; ++i) { + this->publisher->publish(test_msgs::msg::Empty()); + std::this_thread::sleep_for(1ms); + if (spin_exited) { + break; + } + } + + EXPECT_TRUE(spin_exited); + spinner.join(); +} + +TYPED_TEST(TestExecutors, spinAll) +{ + using ExecutorType = TypeParam; + ExecutorType executor; + auto waitable_interfaces = this->node->get_node_waitables_interface(); + auto my_waitable = std::make_shared(); + waitable_interfaces->add_waitable(my_waitable, nullptr); + executor.add_node(this->node); + + // Long timeout, but should not block test if spin_all works as expected as we cancel the + // executor. + bool spin_exited = false; + std::thread spinner([&spin_exited, &executor, this]() { + executor.spin_all(1s); + executor.remove_node(this->node, true); + spin_exited = true; + }); + + // Do some work until sufficient calls to the waitable occur + auto start = std::chrono::steady_clock::now(); + while ( + my_waitable->get_count() <= 1 && + !spin_exited && + (std::chrono::steady_clock::now() - start < 1s)) + { + my_waitable->trigger(); + this->publisher->publish(test_msgs::msg::Empty()); + std::this_thread::sleep_for(1ms); + } + + executor.cancel(); + start = std::chrono::steady_clock::now(); + while (!spin_exited && (std::chrono::steady_clock::now() - start) < 1s) { + std::this_thread::sleep_for(1ms); + } + + EXPECT_LT(1u, my_waitable->get_count()); + waitable_interfaces->remove_waitable(my_waitable, nullptr); + ASSERT_TRUE(spin_exited); + spinner.join(); +} + +// Helper function to convert chrono durations into a scalar that GoogleTest +// can more easily compare and print. +template +auto +to_nanoseconds_helper(DurationT duration) +{ + return std::chrono::duration_cast(duration).count(); +} + +// The purpose of this test is to check that the ExecutorT.spin_some() method: +// - works nominally (it can execute entities) +// - it can execute multiple items at once +// - it does not wait for work to be available before returning +TYPED_TEST(TestExecutors, spin_some) +{ + using ExecutorType = TypeParam; + + // Use an isolated callback group to avoid interference from any housekeeping + // items that may be in the default callback group of the node. + constexpr bool automatically_add_to_executor_with_node = false; + auto isolated_callback_group = this->node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + automatically_add_to_executor_with_node); + + // Check that spin_some() returns quickly when there is no work to be done. + // This can be a false positive if there is somehow some work for the executor + // to do that has not been considered, but the isolated callback group should + // avoid that. + { + ExecutorType executor; + executor.add_callback_group(isolated_callback_group, this->node->get_node_base_interface()); + + auto start = std::chrono::steady_clock::now(); + // spin_some with some non-trival "max_duration" and check that it does not + // take anywhere near that long to execute. + constexpr auto max_duration = 10s; + executor.spin_some(max_duration); + EXPECT_LT( + to_nanoseconds_helper(std::chrono::steady_clock::now() - start), + to_nanoseconds_helper(max_duration / 2)) + << "spin_some() took a long time to execute when it should have done " + << "nothing and should not have blocked either, but this could be a " + << "false negative if the computer is really slow"; + } + + // Check that having one thing ready gets executed by spin_some(). + auto waitable_interfaces = this->node->get_node_waitables_interface(); + auto my_waitable1 = std::make_shared(); + waitable_interfaces->add_waitable(my_waitable1, isolated_callback_group); + { + ExecutorType executor; + executor.add_callback_group(isolated_callback_group, this->node->get_node_base_interface()); + + my_waitable1->trigger(); + + // The long duration should not matter, as executing the waitable is + // non-blocking, and spin_some() should exit after completing the available + // work. + auto start = std::chrono::steady_clock::now(); + constexpr auto max_duration = 10s; + executor.spin_some(max_duration); + EXPECT_LT( + to_nanoseconds_helper(std::chrono::steady_clock::now() - start), + to_nanoseconds_helper(max_duration / 2)) + << "spin_some() took a long time to execute when it should have very " + << "little to do and should not have blocked either, but this could be a " + << "false negative if the computer is really slow"; + + EXPECT_EQ(my_waitable1->get_count(), 1u) + << "spin_some() failed to execute a waitable that was triggered"; + } + + // Check that multiple things being ready are executed by spin_some(). + auto my_waitable2 = std::make_shared(); + waitable_interfaces->add_waitable(my_waitable2, isolated_callback_group); + { + ExecutorType executor; + executor.add_callback_group(isolated_callback_group, this->node->get_node_base_interface()); + + const size_t original_my_waitable1_count = my_waitable1->get_count(); + my_waitable1->trigger(); + my_waitable2->trigger(); + + // The long duration should not matter, as executing the waitable is + // non-blocking, and spin_some() should exit after completing the available + // work. + auto start = std::chrono::steady_clock::now(); + constexpr auto max_duration = 10s; + executor.spin_some(max_duration); + EXPECT_LT( + to_nanoseconds_helper(std::chrono::steady_clock::now() - start), + to_nanoseconds_helper(max_duration / 2)) + << "spin_some() took a long time to execute when it should have very " + << "little to do and should not have blocked either, but this could be a " + << "false negative if the computer is really slow"; + + EXPECT_EQ(my_waitable1->get_count(), original_my_waitable1_count + 1) + << "spin_some() failed to execute a waitable that was triggered"; + EXPECT_EQ(my_waitable2->get_count(), 1u) + << "spin_some() failed to execute a waitable that was triggered"; + } +} + +// The purpose of this test is to check that the ExecutorT.spin_some() method: +// - does not continue executing after max_duration has elapsed +// TODO(wjwwood): The `StaticSingleThreadedExecutor` +// do not properly implement max_duration (it seems), so disable this test +// for them in the meantime. +// see: https://github.com/ros2/rclcpp/issues/2462 +TYPED_TEST(TestExecutorsStable, spin_some_max_duration) +{ + using ExecutorType = TypeParam; + + // Use an isolated callback group to avoid interference from any housekeeping + // items that may be in the default callback group of the node. + constexpr bool automatically_add_to_executor_with_node = false; + auto isolated_callback_group = this->node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + automatically_add_to_executor_with_node); + + // Set up a situation with two waitables that take time to execute, such that + // the time it takes to execute two waitables far exceeds the max_duration + // given to spin_some(), which should result in spin_some() starting to + // execute one of them, have the max duration elapse, finish executing one + // of them, then returning before starting on the second. + constexpr auto max_duration = 100ms; // relatively short because we expect to exceed it + constexpr auto waitable_callback_duration = max_duration * 2; + auto long_running_callback = [&waitable_callback_duration]() { + std::this_thread::sleep_for(waitable_callback_duration); + }; + + auto waitable_interfaces = this->node->get_node_waitables_interface(); + + auto my_waitable1 = std::make_shared(); + my_waitable1->set_on_execute_callback(long_running_callback); + waitable_interfaces->add_waitable(my_waitable1, isolated_callback_group); + + auto my_waitable2 = std::make_shared(); + my_waitable2->set_on_execute_callback(long_running_callback); + waitable_interfaces->add_waitable(my_waitable2, isolated_callback_group); + + my_waitable1->trigger(); + my_waitable2->trigger(); + + ExecutorType executor; + executor.add_callback_group(isolated_callback_group, this->node->get_node_base_interface()); + + auto start = std::chrono::steady_clock::now(); + // spin_some and check that it does not take longer than two of waitable_callback_duration, + // nor significantly less than a single waitable_callback_duration. + executor.spin_some(max_duration); + auto spin_some_run_time = std::chrono::steady_clock::now() - start; + EXPECT_GT( + to_nanoseconds_helper(spin_some_run_time), + to_nanoseconds_helper(waitable_callback_duration / 2)) + << "spin_some() took less than half the expected time to execute a single " + << "waitable, which implies it did not actually execute one when it was " + << "expected to"; + EXPECT_LT( + to_nanoseconds_helper(spin_some_run_time), + to_nanoseconds_helper(waitable_callback_duration * 2)) + << "spin_some() took longer than expected to execute by a significant margin, but " + << "this could be a false positive on a very slow computer"; + + // check that exactly one of the waitables were executed (do not depend on a specific order) + size_t number_of_waitables_executed = my_waitable1->get_count() + my_waitable2->get_count(); + EXPECT_EQ(number_of_waitables_executed, 1u) + << "expected exactly one of the two waitables to be executed, but " + << "my_waitable1->get_count(): " << my_waitable1->get_count() << " and " + << "my_waitable2->get_count(): " << my_waitable2->get_count(); +} + +// Check spin_node_until_future_complete with node base pointer +TYPED_TEST(TestExecutors, testSpinNodeUntilFutureCompleteNodeBasePtr) +{ + using ExecutorType = TypeParam; + ExecutorType executor; + + std::promise promise; + std::future future = promise.get_future(); + promise.set_value(true); + + auto shared_future = future.share(); + auto ret = rclcpp::executors::spin_node_until_future_complete( + executor, this->node->get_node_base_interface(), shared_future, 1s); + EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); +} + +// Check spin_node_until_future_complete with node pointer +TYPED_TEST(TestExecutors, testSpinNodeUntilFutureCompleteNodePtr) +{ + using ExecutorType = TypeParam; + ExecutorType executor; + + std::promise promise; + std::future future = promise.get_future(); + promise.set_value(true); + + auto shared_future = future.share(); + auto ret = rclcpp::executors::spin_node_until_future_complete( + executor, this->node, shared_future, 1s); + EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); +} + +// Check spin_until_future_complete can be properly interrupted. +TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteInterrupted) +{ + using ExecutorType = TypeParam; + ExecutorType executor; + executor.add_node(this->node); + + bool spin_exited = false; + + // This needs to block longer than it takes to get to the shutdown call below and for + // spin_until_future_complete to return + std::future future = std::async( + std::launch::async, + [&spin_exited]() { + auto start = std::chrono::steady_clock::now(); + while (!spin_exited && (std::chrono::steady_clock::now() - start) < 1s) { + std::this_thread::sleep_for(1ms); + } + }); + + // Long timeout + std::thread spinner([&spin_exited, &executor, &future]() { + auto ret = executor.spin_until_future_complete(future, 1s); + EXPECT_EQ(rclcpp::FutureReturnCode::INTERRUPTED, ret); + spin_exited = true; + }); + + // Do some minimal work + this->publisher->publish(test_msgs::msg::Empty()); + std::this_thread::sleep_for(1ms); + + // Force interruption + rclcpp::shutdown(); + + // Give it time to exit + auto start = std::chrono::steady_clock::now(); + while (!spin_exited && (std::chrono::steady_clock::now() - start) < 1s) { + std::this_thread::sleep_for(1ms); + } + + EXPECT_TRUE(spin_exited); + spinner.join(); +} + +// This test verifies that the add_node operation is robust wrt race conditions. +// It's mostly meant to prevent regressions in the events-executor, but the operation should be +// thread-safe in all executor implementations. +// The initial implementation of the events-executor contained a bug where the executor +// would end up in an inconsistent state and stop processing interrupt/shutdown notifications. +// Manually adding a node to the executor results in a) producing a notify waitable event +// and b) refreshing the executor collections. +// The inconsistent state would happen if the event was processed before the collections were +// finished to be refreshed: the executor would pick up the event but be unable to process it. +// This would leave the `entities_need_rebuild_` flag to true, preventing additional +// notify waitable events to be pushed. +// The behavior is observable only under heavy load, so this test spawns several worker +// threads. Due to the nature of the bug, this test may still succeed even if the +// bug is present. However repeated runs will show its flakiness nature and indicate +// an eventual regression. +TYPED_TEST(TestExecutors, testRaceConditionAddNode) +{ + using ExecutorType = TypeParam; + + // Spawn some threads to do some heavy work + std::atomic should_cancel = false; + std::vector stress_threads; + for (size_t i = 0; i < 5 * std::thread::hardware_concurrency(); i++) { + stress_threads.emplace_back( + [&should_cancel, i]() { + // This is just some arbitrary heavy work + volatile size_t total = 0; + for (size_t k = 0; k < 549528914167; k++) { + if (should_cancel) { + break; + } + total += k * (i + 42); + (void)total; + } + }); + } + + // Create an executor + ExecutorType executor; + // Start spinning + auto executor_thread = std::thread( + [&executor]() { + executor.spin(); + }); + // Add a node to the executor + executor.add_node(this->node); + + // Cancel the executor (make sure that it's already spinning first) + while (!executor.is_spinning() && rclcpp::ok()) { + continue; + } + executor.cancel(); + + // Try to join the thread after cancelling the executor + // This is the "test". We want to make sure that we can still cancel the executor + // regardless of the presence of race conditions + executor_thread.join(); + + // The test is now completed: we can join the stress threads + should_cancel = true; + for (auto & t : stress_threads) { + t.join(); + } +} + +// Check that executors are correctly notified while they are spinning +// we notify twice to ensure that the notify waitable is still working +// after the first notification +TYPED_TEST(TestExecutors, notifyTwiceWhileSpinning) +{ + using ExecutorType = TypeParam; + + // Create executor, add the node and start spinning + ExecutorType executor; + executor.add_node(this->node); + std::thread spinner([&]() {executor.spin();}); + + // Wait for executor to be spinning + while (!executor.is_spinning()) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + + // Create the first subscription while the executor is already spinning + std::atomic sub1_msg_count {0}; + auto sub1 = this->node->template create_subscription( + this->publisher->get_topic_name(), + rclcpp::QoS(10), + [&sub1_msg_count](test_msgs::msg::Empty::ConstSharedPtr) { + sub1_msg_count++; + }); + + // Publish a message and verify it's received + this->publisher->publish(test_msgs::msg::Empty()); + auto start = std::chrono::steady_clock::now(); + while (sub1_msg_count == 0 && (std::chrono::steady_clock::now() - start) < 10s) { + std::this_thread::sleep_for(1ms); + } + EXPECT_EQ(sub1_msg_count, 1u); + + // Create a second subscription while the executor is already spinning + std::atomic sub2_msg_count {0}; + auto sub2 = this->node->template create_subscription( + this->publisher->get_topic_name(), + rclcpp::QoS(10), + [&sub2_msg_count](test_msgs::msg::Empty::ConstSharedPtr) { + sub2_msg_count++; + }); + + // Publish a message and verify it's received by both subscriptions + this->publisher->publish(test_msgs::msg::Empty()); + start = std::chrono::steady_clock::now(); + while ( + sub1_msg_count == 1 && + sub2_msg_count == 0 && + (std::chrono::steady_clock::now() - start) < 10s) + { + std::this_thread::sleep_for(1ms); + } + EXPECT_EQ(sub1_msg_count, 2u); + EXPECT_EQ(sub2_msg_count, 1u); + + // Cancel needs to be called before join, so that executor.spin() returns. + executor.cancel(); + spinner.join(); +} + +// Check spin_until_future_complete with node base pointer (instantiates its own executor) +TEST(TestExecutors, testSpinUntilFutureCompleteNodeBasePtr) +{ + rclcpp::init(0, nullptr); + + { + auto node = std::make_shared("node"); + + std::promise promise; + std::future future = promise.get_future(); + promise.set_value(true); + + auto shared_future = future.share(); + auto ret = rclcpp::spin_until_future_complete( + node->get_node_base_interface(), shared_future, 1s); + EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); + } + + rclcpp::shutdown(); +} + +// Check spin_until_future_complete with node pointer (instantiates its own executor) +TEST(TestExecutors, testSpinUntilFutureCompleteNodePtr) +{ + rclcpp::init(0, nullptr); + + { + auto node = std::make_shared("node"); + + std::promise promise; + std::future future = promise.get_future(); + promise.set_value(true); + + auto shared_future = future.share(); + auto ret = rclcpp::spin_until_future_complete(node, shared_future, 1s); + EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); + } + + rclcpp::shutdown(); +} + +// Check spin functions with non default context +TEST(TestExecutors, testSpinWithNonDefaultContext) +{ + auto non_default_context = std::make_shared(); + non_default_context->init(0, nullptr); + + { + auto node = + std::make_unique("node", rclcpp::NodeOptions().context(non_default_context)); + + EXPECT_NO_THROW(rclcpp::spin_some(node->get_node_base_interface())); + + EXPECT_NO_THROW(rclcpp::spin_all(node->get_node_base_interface(), 1s)); + + auto check_spin_until_future_complete = [&]() { + std::promise promise; + std::future future = promise.get_future(); + promise.set_value(true); + + auto shared_future = future.share(); + auto ret = rclcpp::spin_until_future_complete( + node->get_node_base_interface(), shared_future, 1s); + EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); + }; + EXPECT_NO_THROW(check_spin_until_future_complete()); + } + + rclcpp::shutdown(non_default_context); +} + +TYPED_TEST(TestExecutors, release_ownership_entity_after_spinning_cancel) +{ + using ExecutorType = TypeParam; + ExecutorType executor; + + auto future = std::async(std::launch::async, [&executor] {executor.spin();}); + + auto node = std::make_shared("test_node"); + auto callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, test_msgs::srv::Empty::Response::SharedPtr) { + }; + auto server = node->create_service("test_service", callback); + while (!executor.is_spinning()) { + std::this_thread::sleep_for(50ms); + } + executor.add_node(node); + std::this_thread::sleep_for(50ms); + executor.cancel(); + std::future_status future_status = future.wait_for(1s); + EXPECT_EQ(future_status, std::future_status::ready); + + EXPECT_EQ(server.use_count(), 1); +} diff --git a/rclcpp/test/rclcpp/executors/test_executors_busy_waiting.cpp b/rclcpp/test/rclcpp/executors/test_executors_busy_waiting.cpp new file mode 100644 index 0000000000..d481638afb --- /dev/null +++ b/rclcpp/test/rclcpp/executors/test_executors_busy_waiting.cpp @@ -0,0 +1,195 @@ +// Copyright 2024 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 "rclcpp/rclcpp.hpp" + +#include "./executor_types.hpp" +#include "./test_waitable.hpp" + +using namespace std::chrono_literals; + +template +class TestBusyWaiting : public ::testing::Test +{ +public: + void SetUp() override + { + rclcpp::init(0, nullptr); + + const auto test_info = ::testing::UnitTest::GetInstance()->current_test_info(); + std::stringstream test_name; + test_name << test_info->test_case_name() << "_" << test_info->name(); + node = std::make_shared("node", test_name.str()); + callback_group = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + /* automatically_add_to_executor_with_node =*/ false); + + auto waitable_interfaces = node->get_node_waitables_interface(); + waitable = std::make_shared(); + waitable_interfaces->add_waitable(waitable, callback_group); + } + + void TearDown() override + { + rclcpp::shutdown(); + } + + void + set_up_and_trigger_waitable(std::function extra_callback = nullptr) + { + this->has_executed = false; + this->waitable->set_on_execute_callback([this, extra_callback]() { + if (!this->has_executed) { + // trigger once to see if the second trigger is handled or not + // this follow up trigger simulates new entities becoming ready while + // the executor is executing something else, e.g. subscription got data + // or a timer expired, etc. + // spin_some would not handle this second trigger, since it collects + // work only once, whereas spin_all should handle it since it + // collects work multiple times + this->waitable->trigger(); + this->has_executed = true; + } + if (nullptr != extra_callback) { + extra_callback(); + } + }); + this->waitable->trigger(); + } + + void + check_for_busy_waits(std::chrono::steady_clock::time_point start_time) + { + // rough time based check, since the work to be done was very small it + // should be safe to check that we didn't use more than half the + // max duration, which itself is much larger than necessary + // however, it could still produce a false-positive + EXPECT_LT( + std::chrono::steady_clock::now() - start_time, + max_duration / 2) + << "executor took a long time to execute when it should have done " + << "nothing and should not have blocked either, but this could be a " + << "false negative if the computer is really slow"; + + // this check is making some assumptions about the implementation of the + // executors, but it should be safe to say that a busy wait may result in + // hundreds or thousands of calls to is_ready(), but "normal" executor + // behavior should be within an order of magnitude of the number of + // times that the waitable was executed + ASSERT_LT(waitable->get_is_ready_call_count(), 10u * this->waitable->get_count()); + } + + static constexpr auto max_duration = 10s; + + rclcpp::Node::SharedPtr node; + rclcpp::CallbackGroup::SharedPtr callback_group; + std::shared_ptr waitable; + std::chrono::steady_clock::time_point start_time; + bool has_executed; +}; + +TYPED_TEST_SUITE(TestBusyWaiting, ExecutorTypes, ExecutorTypeNames); + +TYPED_TEST(TestBusyWaiting, test_spin_all) +{ + using ExecutorType = TypeParam; + ExecutorType executor; + executor.add_callback_group( + this->callback_group, + this->node->get_node_base_interface()); + + this->set_up_and_trigger_waitable(); + + auto start_time = std::chrono::steady_clock::now(); + executor.spin_all(this->max_duration); + this->check_for_busy_waits(start_time); + // this should get the initial trigger, and the follow up from in the callback + ASSERT_EQ(this->waitable->get_count(), 2u); +} + +TYPED_TEST(TestBusyWaiting, test_spin_some) +{ + using ExecutorType = TypeParam; + ExecutorType executor; + executor.add_callback_group( + this->callback_group, + this->node->get_node_base_interface()); + + this->set_up_and_trigger_waitable(); + + auto start_time = std::chrono::steady_clock::now(); + executor.spin_some(this->max_duration); + this->check_for_busy_waits(start_time); + // this should get the inital trigger, but not the follow up in the callback + ASSERT_EQ(this->waitable->get_count(), 1u); +} + +TYPED_TEST(TestBusyWaiting, test_spin) +{ + using ExecutorType = TypeParam; + ExecutorType executor; + executor.add_callback_group( + this->callback_group, + this->node->get_node_base_interface()); + + std::condition_variable cv; + std::mutex cv_m; + bool first_check_passed = false; + + this->set_up_and_trigger_waitable([&cv, &cv_m, &first_check_passed]() { + cv.notify_one(); + if (!first_check_passed) { + std::unique_lock lk(cv_m); + cv.wait_for(lk, 1s, [&]() {return first_check_passed;}); + } + }); + + auto start_time = std::chrono::steady_clock::now(); + std::thread t([&executor]() { + executor.spin(); + }); + + // wait until thread has started (first execute of waitable) + { + std::unique_lock lk(cv_m); + cv.wait_for(lk, 10s); + } + EXPECT_GT(this->waitable->get_count(), 0u); + + first_check_passed = true; + cv.notify_one(); + + // wait until the executor has finished (second execute of waitable) + { + std::unique_lock lk(cv_m); + cv.wait_for(lk, 10s); + } + EXPECT_EQ(this->waitable->get_count(), 2u); + + executor.cancel(); + t.join(); + + this->check_for_busy_waits(start_time); + // this should get the initial trigger, and the follow up from in the callback + ASSERT_EQ(this->waitable->get_count(), 2u); +} diff --git a/rclcpp/test/rclcpp/executors/test_executors_callback_group_behavior.cpp b/rclcpp/test/rclcpp/executors/test_executors_callback_group_behavior.cpp new file mode 100644 index 0000000000..49391cd838 --- /dev/null +++ b/rclcpp/test/rclcpp/executors/test_executors_callback_group_behavior.cpp @@ -0,0 +1,156 @@ +// Copyright 2024 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/** + * This test checks that when callback groups go out of scope, that their associated executable + * entities should not be returned as valid executables. + * + * The test makes use of a bit of executor internals, but is meant to prevent regressions of behavior. + * Ref: https://github.com/ros2/rclcpp/issues/2474 + */ + +#include + +#include +#include + +#include +#include +#include + +std::chrono::milliseconds g_timer_period {1}; + +class CustomExecutor : public rclcpp::Executor +{ +public: + explicit CustomExecutor(const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions()) + : rclcpp::Executor(options) + {} + + ~CustomExecutor() override = default; + + void spin() override {} + + void collect() + { + this->collect_entities(); + } + + void wait() + { + this->wait_for_work(g_timer_period * 10); + } + + size_t collected_timers() const + { + return this->current_collection_.timers.size(); + } + + rclcpp::AnyExecutable next() + { + rclcpp::AnyExecutable ret; + this->get_next_ready_executable(ret); + return ret; + } +}; + + +TEST(TestCallbackGroup, valid_callback_group) +{ + rclcpp::init(0, nullptr); + + // Create a timer associated with a callback group + auto node = std::make_shared("node"); + + std::promise promise; + std::future future = promise.get_future(); + auto timer_callback = [&promise]() { + promise.set_value(); + }; + + // Add the callback group to the executor + auto executor = CustomExecutor(); + auto cbg = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, true); + auto timer = node->create_wall_timer(g_timer_period, timer_callback, cbg); + executor.add_callback_group(cbg, node->get_node_base_interface()); + + ASSERT_EQ( + rclcpp::FutureReturnCode::SUCCESS, + executor.spin_until_future_complete(future, std::chrono::seconds(10))); + + // Collect the entities + executor.collect(); + EXPECT_EQ(1u, executor.collected_timers()); + + executor.wait(); + auto next_executable = executor.next(); + EXPECT_EQ(timer, next_executable.timer); + EXPECT_EQ(cbg, next_executable.callback_group); + EXPECT_NE(nullptr, next_executable.data); + + EXPECT_EQ(nullptr, next_executable.client); + EXPECT_EQ(nullptr, next_executable.service); + EXPECT_EQ(nullptr, next_executable.subscription); + EXPECT_EQ(nullptr, next_executable.waitable); + + rclcpp::shutdown(); +} + +TEST(TestCallbackGroup, invalid_callback_group) +{ + rclcpp::init(0, nullptr); + + // Create a timer associated with a callback group + auto node = std::make_shared("node"); + + std::promise promise; + std::future future = promise.get_future(); + auto timer_callback = [&promise]() { + promise.set_value(); + }; + + // Add the callback group to the executor + auto executor = CustomExecutor(); + auto cbg = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); + auto timer = node->create_wall_timer(g_timer_period, timer_callback, cbg); + executor.add_callback_group(cbg, node->get_node_base_interface()); + + ASSERT_EQ( + rclcpp::FutureReturnCode::SUCCESS, + executor.spin_until_future_complete(future, std::chrono::seconds(10))); + + // Collect the entities + executor.collect(); + EXPECT_EQ(1u, executor.collected_timers()); + + executor.wait(); + + cbg.reset(); + + // Since the callback group has been reset, this should not be allowed to + // be a valid executable (timer and cbg should both be nullptr). + // In the regression, timer == next_executable.timer whil + // next_executable.callback_group == nullptr, which was incorrect. + auto next_executable = executor.next(); + EXPECT_EQ(nullptr, next_executable.timer); + EXPECT_EQ(nullptr, next_executable.callback_group); + + EXPECT_EQ(nullptr, next_executable.client); + EXPECT_EQ(nullptr, next_executable.service); + EXPECT_EQ(nullptr, next_executable.subscription); + EXPECT_EQ(nullptr, next_executable.waitable); + EXPECT_EQ(nullptr, next_executable.data); + + rclcpp::shutdown(); +} diff --git a/rclcpp/test/rclcpp/executors/test_executors_intraprocess.cpp b/rclcpp/test/rclcpp/executors/test_executors_intraprocess.cpp new file mode 100644 index 0000000000..af5f7e432e --- /dev/null +++ b/rclcpp/test/rclcpp/executors/test_executors_intraprocess.cpp @@ -0,0 +1,125 @@ +// Copyright 2024 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 "rclcpp/node.hpp" +#include "rclcpp/utilities.hpp" + +#include "test_msgs/msg/empty.hpp" + +#include "./executor_types.hpp" + +template +class TestIntraprocessExecutors : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + const auto test_info = ::testing::UnitTest::GetInstance()->current_test_info(); + std::stringstream test_name; + test_name << test_info->test_case_name() << "_" << test_info->name(); + node = std::make_shared("node", test_name.str()); + + callback_count = 0u; + + const std::string topic_name = std::string("topic_") + test_name.str(); + + rclcpp::PublisherOptions po; + po.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; + publisher = node->create_publisher(topic_name, rclcpp::QoS(1), po); + + auto callback = [this](test_msgs::msg::Empty::ConstSharedPtr) { + this->callback_count.fetch_add(1u); + }; + + rclcpp::SubscriptionOptions so; + so.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; + subscription = + node->create_subscription( + topic_name, rclcpp::QoS(kNumMessages), std::move(callback), so); + } + + void TearDown() + { + publisher.reset(); + subscription.reset(); + node.reset(); + } + + const size_t kNumMessages = 100; + + rclcpp::Node::SharedPtr node; + rclcpp::Publisher::SharedPtr publisher; + rclcpp::Subscription::SharedPtr subscription; + std::atomic_size_t callback_count; +}; + +TYPED_TEST_SUITE(TestIntraprocessExecutors, ExecutorTypes, ExecutorTypeNames); + +TYPED_TEST(TestIntraprocessExecutors, testIntraprocessRetrigger) { + // This tests that executors will continue to service intraprocess subscriptions in the case + // that publishers aren't continuing to publish. + // This was previously broken in that intraprocess guard conditions were only triggered on + // publish and the test was added to prevent future regressions. + static constexpr size_t kNumMessages = 100; + + using ExecutorType = TypeParam; + ExecutorType executor; + executor.add_node(this->node); + + EXPECT_EQ(0u, this->callback_count.load()); + this->publisher->publish(test_msgs::msg::Empty()); + + // Wait for up to 5 seconds for the first message to come available. + const std::chrono::milliseconds sleep_per_loop(10); + int loops = 0; + while (1u != this->callback_count.load() && loops < 500) { + rclcpp::sleep_for(sleep_per_loop); + executor.spin_some(); + loops++; + } + EXPECT_EQ(1u, this->callback_count.load()); + + // reset counter + this->callback_count.store(0u); + + for (size_t ii = 0; ii < kNumMessages; ++ii) { + this->publisher->publish(test_msgs::msg::Empty()); + } + + // Fire a timer every 10ms up to 5 seconds waiting for subscriptions to be read. + loops = 0; + auto timer = this->node->create_wall_timer( + std::chrono::milliseconds(10), [this, &executor, &loops]() { + loops++; + if (kNumMessages == this->callback_count.load() || loops == 500) { + executor.cancel(); + } + }); + executor.spin(); + EXPECT_EQ(kNumMessages, this->callback_count.load()); +} diff --git a/rclcpp/test/rclcpp/executors/test_executors_timer_cancel_behavior.cpp b/rclcpp/test/rclcpp/executors/test_executors_timer_cancel_behavior.cpp new file mode 100644 index 0000000000..fbf38146fb --- /dev/null +++ b/rclcpp/test/rclcpp/executors/test_executors_timer_cancel_behavior.cpp @@ -0,0 +1,474 @@ +// Copyright 2024 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 "rclcpp/node.hpp" +#include "rclcpp/parameter_client.hpp" +#include "rclcpp/utilities.hpp" + +#include "rosgraph_msgs/msg/clock.hpp" + +#include "./executor_types.hpp" + +using namespace std::chrono_literals; + +class TimerNode : public rclcpp::Node +{ +public: + explicit TimerNode(std::string subname) + : Node("timer_node", subname) + { + } + + void CreateTimer1() + { + timer1_ = rclcpp::create_timer( + this->get_node_base_interface(), get_node_timers_interface(), + get_clock(), 1ms, + std::bind(&TimerNode::Timer1Callback, this)); + } + + void CreateTimer2() + { + timer2_ = rclcpp::create_timer( + this->get_node_base_interface(), get_node_timers_interface(), + get_clock(), 1ms, + std::bind(&TimerNode::Timer2Callback, this)); + } + + int GetTimer1Cnt() + { + const std::lock_guard lock(mutex_); + return cnt1_; + } + int GetTimer2Cnt() + { + const std::lock_guard lock(mutex_); + return cnt2_; + } + + void ResetTimer1() + { + timer1_->reset(); + } + + void ResetTimer2() + { + timer2_->reset(); + } + + void CancelTimer1() + { + RCLCPP_DEBUG(this->get_logger(), "Timer 1 cancelling!"); + timer1_->cancel(); + } + + void CancelTimer2() + { + RCLCPP_DEBUG(this->get_logger(), "Timer 2 cancelling!"); + timer2_->cancel(); + } + +private: + void Timer1Callback() + { + { + const std::lock_guard lock(mutex_); + cnt1_++; + } + RCLCPP_DEBUG(this->get_logger(), "Timer 1! (%d)", cnt1_); + } + + void Timer2Callback() + { + { + const std::lock_guard lock(mutex_); + cnt2_++; + } + RCLCPP_DEBUG(this->get_logger(), "Timer 2! (%d)", cnt2_); + } + + std::mutex mutex_; + + rclcpp::TimerBase::SharedPtr timer1_; + rclcpp::TimerBase::SharedPtr timer2_; + int cnt1_ = 0; + int cnt2_ = 0; +}; + +// Sets up a separate thread to publish /clock messages. +// Clock rate relative to real clock is controlled by realtime_update_rate. +// This is set conservatively slow to ensure unit tests are reliable on Windows +// environments, where timing performance is subpar. +// +// Use `sleep_for` in tests to advance the clock. Clock should run and be published +// in separate thread continuously to ensure correct behavior in node under test. +class ClockPublisher : public rclcpp::Node +{ +public: + explicit ClockPublisher(float simulated_clock_step = .001f, float realtime_update_rate = 0.25f) + : Node("clock_publisher"), + ros_update_duration_(0, 0), + realtime_clock_step_(0, 0), + rostime_(0, 0) + { + clock_publisher_ = this->create_publisher("clock", 10); + realtime_clock_step_ = + rclcpp::Duration::from_seconds(simulated_clock_step / realtime_update_rate); + ros_update_duration_ = rclcpp::Duration::from_seconds(simulated_clock_step); + + timer_thread_ = std::thread(&ClockPublisher::RunTimer, this); + } + + ~ClockPublisher() + { + running_ = false; + if (timer_thread_.joinable()) { + timer_thread_.join(); + } + } + + bool wait_for_connection(std::chrono::nanoseconds timeout) + { + auto end_time = std::chrono::steady_clock::now() + timeout; + while (clock_publisher_->get_subscription_count() == 0 && + (std::chrono::steady_clock::now() < end_time)) + { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + + return clock_publisher_->get_subscription_count() != 0; + } + + void sleep_for(rclcpp::Duration duration) + { + rclcpp::Time start_time(0, 0, RCL_ROS_TIME); + { + const std::lock_guard lock(mutex_); + start_time = rostime_; + } + rclcpp::Time current_time = start_time; + + while (true) { + { + const std::lock_guard lock(mutex_); + current_time = rostime_; + } + if ((current_time - start_time) >= duration) { + return; + } + std::this_thread::sleep_for(realtime_clock_step_.to_chrono()); + { + const std::lock_guard lock(mutex_); + rostime_ += ros_update_duration_; + } + } + } + +private: + void RunTimer() + { + while (running_) { + PublishClock(); + std::this_thread::sleep_for(realtime_clock_step_.to_chrono()); + } + } + + void PublishClock() + { + auto message = rosgraph_msgs::msg::Clock(); + { + const std::lock_guard lock(mutex_); + message.clock = rostime_; + } + clock_publisher_->publish(message); + } + + rclcpp::Publisher::SharedPtr clock_publisher_; + + rclcpp::Duration ros_update_duration_; + rclcpp::Duration realtime_clock_step_; + // Rostime must be guarded by a mutex, since accessible in running thread + // as well as sleep_for + rclcpp::Time rostime_; + std::mutex mutex_; + std::thread timer_thread_; + std::atomic running_ = true; +}; + + +template +class TestTimerCancelBehavior : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + const auto test_info = ::testing::UnitTest::GetInstance()->current_test_info(); + std::stringstream test_name; + test_name << test_info->test_case_name() << "_" << test_info->name(); + node = std::make_shared(test_name.str()); + param_client = std::make_shared(node); + ASSERT_TRUE(param_client->wait_for_service(5s)); + + auto set_parameters_results = param_client->set_parameters( + {rclcpp::Parameter("use_sim_time", true)}); + for (auto & result : set_parameters_results) { + ASSERT_TRUE(result.successful); + } + + // Check if the clock type is simulation time + EXPECT_EQ(RCL_ROS_TIME, node->get_clock()->get_clock_type()); + + // Create timers + this->node->CreateTimer1(); + this->node->CreateTimer2(); + + // Run standalone thread to publish clock time + sim_clock_node = std::make_shared(); + + // Spin the executor in a standalone thread + executor.add_node(this->node); + standalone_thread = std::thread( + [this]() { + executor.spin(); + }); + + EXPECT_TRUE(this->sim_clock_node->wait_for_connection(50ms)); + EXPECT_EQ(RCL_ROS_TIME, node->get_clock()->ros_time_is_active()); + } + + void TearDown() + { + node.reset(); + + // Clean up thread object + if (standalone_thread.joinable()) { + standalone_thread.join(); + } + } + + std::shared_ptr node; + std::shared_ptr sim_clock_node; + rclcpp::SyncParametersClient::SharedPtr param_client; + std::thread standalone_thread; + T executor; +}; + +#if !defined(_WIN32) +# ifdef __clang__ +# pragma clang diagnostic push +# pragma clang diagnostic ignored "-Wdeprecated-declarations" +# endif +#endif +using MainExecutorTypes = + ::testing::Types< + rclcpp::executors::SingleThreadedExecutor, + rclcpp::executors::MultiThreadedExecutor, + DeprecatedStaticSingleThreadedExecutor>; +#ifdef __clang__ +# pragma clang diagnostic pop +#endif +// TODO(@fujitatomoya): this test excludes EventExecutor because it does not +// support simulation time used for this test to relax the racy condition. +// See more details for https://github.com/ros2/rclcpp/issues/2457. +TYPED_TEST_SUITE(TestTimerCancelBehavior, MainExecutorTypes, ExecutorTypeNames); + +TYPED_TEST(TestTimerCancelBehavior, testTimer1CancelledWithExecutorSpin) { + // Validate that cancelling one timer yields no change in behavior for other + // timers. Specifically, this tests the behavior when using spin() to run the + // executor, which is the most common usecase. + + // Cancel to stop the spin after some time. + this->sim_clock_node->sleep_for(50ms); + this->node->CancelTimer1(); + this->sim_clock_node->sleep_for(150ms); + this->executor.cancel(); + + int t1_runs = this->node->GetTimer1Cnt(); + int t2_runs = this->node->GetTimer2Cnt(); + EXPECT_NE(t1_runs, t2_runs); + // Check that t2 has significantly more calls + EXPECT_LT(t1_runs + 50, t2_runs); +} + +TYPED_TEST(TestTimerCancelBehavior, testTimer2CancelledWithExecutorSpin) { + // Validate that cancelling one timer yields no change in behavior for other + // timers. Specifically, this tests the behavior when using spin() to run the + // executor, which is the most common usecase. + + // Cancel to stop the spin after some time. + this->sim_clock_node->sleep_for(50ms); + this->node->CancelTimer2(); + this->sim_clock_node->sleep_for(150ms); + this->executor.cancel(); + + int t1_runs = this->node->GetTimer1Cnt(); + int t2_runs = this->node->GetTimer2Cnt(); + EXPECT_NE(t1_runs, t2_runs); + // Check that t1 has significantly more calls + EXPECT_LT(t2_runs + 50, t1_runs); +} + +TYPED_TEST(TestTimerCancelBehavior, testHeadTimerCancelThenResetBehavior) { + // Validate that cancelling timer doesn't affect operation of other timers, + // and that the cancelled timer starts executing normally once reset manually. + + // Cancel to stop the spin after some time. + this->sim_clock_node->sleep_for(50ms); + this->node->CancelTimer1(); + this->sim_clock_node->sleep_for(150ms); + int t1_runs_initial = this->node->GetTimer1Cnt(); + int t2_runs_initial = this->node->GetTimer2Cnt(); + + // Manually reset timer 1, then sleep again + // Counts should update. + this->node->ResetTimer1(); + this->sim_clock_node->sleep_for(150ms); + int t1_runs_final = this->node->GetTimer1Cnt(); + int t2_runs_final = this->node->GetTimer2Cnt(); + + this->executor.cancel(); + + // T1 should have been restarted, and execute about 15 additional times. + // Check 10 greater than initial, to account for some timing jitter. + EXPECT_LT(t1_runs_initial + 50, t1_runs_final); + + EXPECT_LT(t1_runs_initial + 50, t2_runs_initial); + // Check that t2 has significantly more calls, and keeps getting called. + EXPECT_LT(t2_runs_initial + 50, t2_runs_final); +} + +TYPED_TEST(TestTimerCancelBehavior, testBackTimerCancelThenResetBehavior) { + // Validate that cancelling timer doesn't affect operation of other timers, + // and that the cancelled timer starts executing normally once reset manually. + + // Cancel to stop the spin after some time. + this->sim_clock_node->sleep_for(50ms); + this->node->CancelTimer2(); + this->sim_clock_node->sleep_for(150ms); + int t1_runs_initial = this->node->GetTimer1Cnt(); + int t2_runs_initial = this->node->GetTimer2Cnt(); + + // Manually reset timer 1, then sleep again + // Counts should update. + this->node->ResetTimer2(); + this->sim_clock_node->sleep_for(150ms); + int t1_runs_final = this->node->GetTimer1Cnt(); + int t2_runs_final = this->node->GetTimer2Cnt(); + + this->executor.cancel(); + + // T2 should have been restarted, and execute about 15 additional times. + // Check 10 greater than initial, to account for some timing jitter. + EXPECT_LT(t2_runs_initial + 50, t2_runs_final); + + EXPECT_LT(t2_runs_initial + 50, t1_runs_initial); + // Check that t1 has significantly more calls, and keeps getting called. + EXPECT_LT(t1_runs_initial + 50, t1_runs_final); +} + +TYPED_TEST(TestTimerCancelBehavior, testBothTimerCancelThenResetT1Behavior) { + // Validate behavior from cancelling 2 timers, then only re-enabling one of them. + // Ensure that only the reset timer is executed. + + // Cancel to stop the spin after some time. + this->sim_clock_node->sleep_for(50ms); + this->node->CancelTimer1(); + this->node->CancelTimer2(); + this->sim_clock_node->sleep_for(150ms); + int t1_runs_initial = this->node->GetTimer1Cnt(); + int t2_runs_initial = this->node->GetTimer2Cnt(); + + // Manually reset timer 1, then sleep again + // Counts should update. + this->node->ResetTimer1(); + this->sim_clock_node->sleep_for(150ms); + int t1_runs_intermediate = this->node->GetTimer1Cnt(); + int t2_runs_intermediate = this->node->GetTimer2Cnt(); + + this->node->ResetTimer2(); + this->sim_clock_node->sleep_for(150ms); + int t1_runs_final = this->node->GetTimer1Cnt(); + int t2_runs_final = this->node->GetTimer2Cnt(); + + this->executor.cancel(); + + // T1 and T2 should have the same initial count. + EXPECT_LE(std::abs(t1_runs_initial - t2_runs_initial), 1); + + // Expect that T1 has up to 15 more calls than t2. Add some buffer + // to account for jitter. + EXPECT_EQ(t2_runs_initial, t2_runs_intermediate); + EXPECT_LT(t1_runs_initial + 50, t1_runs_intermediate); + + // Expect that by end of test, both are running properly again. + EXPECT_LT(t1_runs_intermediate + 50, t1_runs_final); + EXPECT_LT(t2_runs_intermediate + 50, t2_runs_final); +} + +TYPED_TEST(TestTimerCancelBehavior, testBothTimerCancelThenResetT2Behavior) { + // Validate behavior from cancelling 2 timers, then only re-enabling one of them. + // Ensure that only the reset timer is executed. + + // Cancel to stop the spin after some time. + this->sim_clock_node->sleep_for(50ms); + this->node->CancelTimer1(); + this->node->CancelTimer2(); + this->sim_clock_node->sleep_for(150ms); + int t1_runs_initial = this->node->GetTimer1Cnt(); + int t2_runs_initial = this->node->GetTimer2Cnt(); + + // Manually reset timer 1, then sleep again + // Counts should update. + this->node->ResetTimer2(); + this->sim_clock_node->sleep_for(150ms); + int t1_runs_intermediate = this->node->GetTimer1Cnt(); + int t2_runs_intermediate = this->node->GetTimer2Cnt(); + + this->node->ResetTimer1(); + this->sim_clock_node->sleep_for(150ms); + int t1_runs_final = this->node->GetTimer1Cnt(); + int t2_runs_final = this->node->GetTimer2Cnt(); + + this->executor.cancel(); + + // T1 and T2 should have the same initial count. + EXPECT_LE(std::abs(t1_runs_initial - t2_runs_initial), 1); + + // Expect that T1 has up to 15 more calls than t2. Add some buffer + // to account for jitter. + EXPECT_EQ(t1_runs_initial, t1_runs_intermediate); + EXPECT_LT(t2_runs_initial + 50, t2_runs_intermediate); + + // Expect that by end of test, both are running properly again. + EXPECT_LT(t1_runs_intermediate + 50, t1_runs_final); + EXPECT_LT(t2_runs_intermediate + 50, t2_runs_final); +} diff --git a/rclcpp/test/rclcpp/executors/test_executors_warmup.cpp b/rclcpp/test/rclcpp/executors/test_executors_warmup.cpp new file mode 100644 index 0000000000..7ab26a9da9 --- /dev/null +++ b/rclcpp/test/rclcpp/executors/test_executors_warmup.cpp @@ -0,0 +1,244 @@ +// Copyright 2024 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/** + * This test checks all implementations of rclcpp::executor to check they pass they basic API + * tests. Anything specific to any executor in particular should go in a separate test file. + */ + +#include + +#include +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "test_msgs/msg/empty.hpp" + +#include "./executor_types.hpp" + +using namespace std::chrono_literals; + +template +class TestExecutorsWarmup : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + } + + void TearDown() + { + rclcpp::shutdown(); + } +}; + +TYPED_TEST_SUITE(TestExecutorsWarmup, ExecutorTypes, ExecutorTypeNames); + +// This test verifies that spin_all is correctly collecting work multiple times +// even when one of the items of work is a notifier waitable event and thus results in +// rebuilding the entities collection. +// When spin_all goes back to collect more work, it should see the ready items from +// the new added entities +TYPED_TEST(TestExecutorsWarmup, spin_all_doesnt_require_warmup) +{ + using ExecutorType = TypeParam; + ExecutorType executor; + + // Enable intra-process to guarantee deterministic and synchronous delivery of the message / event + auto node_options = rclcpp::NodeOptions().use_intra_process_comms(true); + auto node = std::make_shared("test_node", node_options); + + // Add node to the executor before creating the entities + executor.add_node(node); + + // Create entities, this will produce a notifier waitable event, telling the executor to refresh + // the entities collection + auto publisher = node->create_publisher("test_topic", rclcpp::QoS(10)); + size_t callback_count = 0; + auto callback = [&callback_count](test_msgs::msg::Empty::ConstSharedPtr) {callback_count++;}; + auto subscription = + node->create_subscription( + "test_topic", rclcpp::QoS(10), std::move(callback)); + + ASSERT_EQ(callback_count, 0u); + + // Publish a message so that the new entities (i.e. the subscriber) already have work to do + publisher->publish(test_msgs::msg::Empty()); + + // We need to select a duration that is greater than + // the time taken to refresh the entities collection and rebuild the waitset. + // spin-all is expected to process the notifier waitable event, rebuild the collection, + // and then collect more work, finding the subscription message event. + // This duration has been selected empirically. + executor.spin_all(std::chrono::milliseconds(500)); + + // Verify that the callback is called as part of the spin above + EXPECT_EQ(callback_count, 1u); +} + +// Same test as `spin_all_doesnt_require_warmup`, but uses a callback group +// This test reproduces the bug reported by https://github.com/ros2/rclcpp/issues/2589 +TYPED_TEST(TestExecutorsWarmup, spin_all_doesnt_require_warmup_with_cbgroup) +{ + using ExecutorType = TypeParam; + + // TODO(alsora): Enable when https://github.com/ros2/rclcpp/pull/2595 gets merged + if ( + std::is_same() || + std::is_same()) + { + GTEST_SKIP(); + } + + ExecutorType executor; + + // Enable intra-process to guarantee deterministic and synchronous delivery of the message / event + auto node_options = rclcpp::NodeOptions().use_intra_process_comms(true); + auto node = std::make_shared("test_node", node_options); + + auto callback_group = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + false); + + // Add callback group to the executor before creating the entities + executor.add_callback_group(callback_group, node->get_node_base_interface()); + + // Create entities, this will produce a notifier waitable event, telling the executor to refresh + // the entities collection + auto publisher = node->create_publisher("test_topic", rclcpp::QoS(10)); + size_t callback_count = 0; + auto callback = [&callback_count](test_msgs::msg::Empty::ConstSharedPtr) {callback_count++;}; + rclcpp::SubscriptionOptions sub_options; + sub_options.callback_group = callback_group; + auto subscription = + node->create_subscription( + "test_topic", rclcpp::QoS(10), std::move(callback), sub_options); + + ASSERT_EQ(callback_count, 0u); + + // Publish a message so that the new entities (i.e. the subscriber) already have work to do + publisher->publish(test_msgs::msg::Empty()); + + // We need to select a duration that is greater than + // the time taken to refresh the entities collection and rebuild the waitset. + // spin-all is expected to process the notifier waitable event, rebuild the collection, + // and then collect more work, finding the subscription message event. + // This duration has been selected empirically. + executor.spin_all(std::chrono::milliseconds(500)); + + // Verify that the callback is called as part of the spin above + EXPECT_EQ(callback_count, 1u); +} + +TYPED_TEST(TestExecutorsWarmup, spin_some_doesnt_require_warmup) +{ + using ExecutorType = TypeParam; + + // TODO(alsora): currently only the events-executor passes this test. + // Enable single-threaded and multi-threaded executors + // when https://github.com/ros2/rclcpp/pull/2595 gets merged + if ( + !std::is_same()) + { + GTEST_SKIP(); + } + + ExecutorType executor; + + // Enable intra-process to guarantee deterministic and synchronous delivery of the message / event + auto node_options = rclcpp::NodeOptions().use_intra_process_comms(true); + auto node = std::make_shared("test_node", node_options); + + // Add node to the executor before creating the entities + executor.add_node(node); + + // Create entities, this will produce a notifier waitable event, telling the executor to refresh + // the entities collection + auto publisher = node->create_publisher("test_topic", rclcpp::QoS(10)); + size_t callback_count = 0; + auto callback = [&callback_count](test_msgs::msg::Empty::ConstSharedPtr) {callback_count++;}; + auto subscription = + node->create_subscription( + "test_topic", rclcpp::QoS(10), std::move(callback)); + + ASSERT_EQ(callback_count, 0u); + + // Publish a message so that the new entities (i.e. the subscriber) already have work to do + publisher->publish(test_msgs::msg::Empty()); + + // NOTE: intra-process communication is enabled, so the subscription will immediately see + // the new message, no risk of race conditions where spin_some gets called before the + // message has been delivered. + executor.spin_some(); + + // Verify that the callback is called as part of the spin above + EXPECT_EQ(callback_count, 1u); +} + +TYPED_TEST(TestExecutorsWarmup, spin_some_doesnt_require_warmup_with_cbgroup) +{ + using ExecutorType = TypeParam; + + // TODO(alsora): currently only the events-executor passes this test. + // Enable single-threaded and multi-threaded executors + // when https://github.com/ros2/rclcpp/pull/2595 gets merged + if ( + !std::is_same()) + { + GTEST_SKIP(); + } + + ExecutorType executor; + + // Enable intra-process to guarantee deterministic and synchronous delivery of the message / event + auto node_options = rclcpp::NodeOptions().use_intra_process_comms(true); + auto node = std::make_shared("test_node", node_options); + + auto callback_group = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + false); + + // Add callback group to the executor before creating the entities + executor.add_callback_group(callback_group, node->get_node_base_interface()); + + // Create entities, this will produce a notifier waitable event, telling the executor to refresh + // the entities collection + auto publisher = node->create_publisher("test_topic", rclcpp::QoS(10)); + size_t callback_count = 0; + auto callback = [&callback_count](test_msgs::msg::Empty::ConstSharedPtr) {callback_count++;}; + rclcpp::SubscriptionOptions sub_options; + sub_options.callback_group = callback_group; + auto subscription = + node->create_subscription( + "test_topic", rclcpp::QoS(10), std::move(callback), sub_options); + + ASSERT_EQ(callback_count, 0u); + + // Publish a message so that the new entities (i.e. the subscriber) already have work to do + publisher->publish(test_msgs::msg::Empty()); + + // NOTE: intra-process communication is enabled, so the subscription will immediately see + // the new message, no risk of race conditions where spin_some gets called before the + // message has been delivered. + executor.spin_some(); + + // Verify that the callback is called as part of the spin above + EXPECT_EQ(callback_count, 1u); +} diff --git a/rclcpp/test/rclcpp/executors/test_multi_threaded_executor.cpp b/rclcpp/test/rclcpp/executors/test_multi_threaded_executor.cpp new file mode 100644 index 0000000000..09dfa03f90 --- /dev/null +++ b/rclcpp/test/rclcpp/executors/test_multi_threaded_executor.cpp @@ -0,0 +1,104 @@ +// 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 "rclcpp/exceptions.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/executors.hpp" + +using namespace std::chrono_literals; + +class TestMultiThreadedExecutor : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } +}; + +constexpr std::chrono::milliseconds PERIOD_MS = 1000ms; +constexpr double PERIOD = PERIOD_MS.count() / 1000.0; +constexpr double TOLERANCE = PERIOD / 4.0; + +/* + Test that timers are not taken multiple times when using reentrant callback groups. + */ +TEST_F(TestMultiThreadedExecutor, timer_over_take) { +#ifdef __linux__ + // This seems to be the most effective way to force the bug to happen on Linux. + // This is unnecessary on MacOS, since the default scheduler causes it. + struct sched_param param; + param.sched_priority = 0; + if (sched_setscheduler(0, SCHED_BATCH, ¶m) != 0) { + perror("sched_setscheduler"); + } +#endif + + bool yield_before_execute = true; + + rclcpp::executors::MultiThreadedExecutor executor( + rclcpp::ExecutorOptions(), 2u, yield_before_execute); + + ASSERT_GT(executor.get_number_of_threads(), 1u); + + std::shared_ptr node = + std::make_shared("test_multi_threaded_executor_timer_over_take"); + + auto cbg = node->create_callback_group(rclcpp::CallbackGroupType::Reentrant); + + rclcpp::Clock system_clock(RCL_STEADY_TIME); + std::mutex last_mutex; + auto last = system_clock.now(); + + std::atomic_int timer_count {0}; + + auto timer_callback = [&timer_count, &executor, &system_clock, &last_mutex, &last]() { + // While this tolerance is a little wide, if the bug occurs, the next step will + // happen almost instantly. The purpose of this test is not to measure the jitter + // in timers, just assert that a reasonable amount of time has passed. + rclcpp::Time now = system_clock.now(); + timer_count++; + + if (timer_count > 5) { + executor.cancel(); + } + + { + std::lock_guard lock(last_mutex); + double diff = static_cast(std::abs((now - last).nanoseconds())) / 1.0e9; + last = now; + + if (diff < PERIOD - TOLERANCE) { + executor.cancel(); + ASSERT_GT(diff, PERIOD - TOLERANCE); + } + } + }; + + auto timer = node->create_wall_timer(PERIOD_MS, timer_callback, cbg); + executor.add_node(node); + executor.spin(); +} diff --git a/rclcpp/test/rclcpp/executors/test_reinitialized_timers.cpp b/rclcpp/test/rclcpp/executors/test_reinitialized_timers.cpp new file mode 100644 index 0000000000..99725cb95e --- /dev/null +++ b/rclcpp/test/rclcpp/executors/test_reinitialized_timers.cpp @@ -0,0 +1,88 @@ +// Copyright 2024 iRobot Corporation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT 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 "rclcpp/rclcpp.hpp" + +#include "./executor_types.hpp" + +template +class TestTimersLifecycle : public testing::Test +{ +public: + void SetUp() override {rclcpp::init(0, nullptr);} + + void TearDown() override {rclcpp::shutdown();} +}; + +TYPED_TEST_SUITE(TestTimersLifecycle, ExecutorTypes, ExecutorTypeNames); + +TYPED_TEST(TestTimersLifecycle, timers_lifecycle_reinitialized_object) +{ + using ExecutorType = TypeParam; + ExecutorType executor; + + auto timers_period = std::chrono::milliseconds(50); + auto node = std::make_shared("test_node"); + + executor.add_node(node); + + size_t count_1 = 0; + auto timer_1 = rclcpp::create_timer( + node, node->get_clock(), rclcpp::Duration(timers_period), [&count_1]() {count_1++;}); + + size_t count_2 = 0; + auto timer_2 = rclcpp::create_timer( + node, node->get_clock(), rclcpp::Duration(timers_period), [&count_2]() {count_2++;}); + + { + std::thread executor_thread([&executor]() {executor.spin();}); + + while (count_2 < 10u) { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + executor.cancel(); + executor_thread.join(); + + EXPECT_GE(count_2, 10u); + EXPECT_LE(count_2 - count_1, 1u); + } + + count_1 = 0; + timer_1 = rclcpp::create_timer( + node, node->get_clock(), rclcpp::Duration(timers_period), [&count_1]() {count_1++;}); + + count_2 = 0; + timer_2 = rclcpp::create_timer( + node, node->get_clock(), rclcpp::Duration(timers_period), [&count_2]() {count_2++;}); + + { + std::thread executor_thread([&executor]() {executor.spin();}); + + while (count_2 < 10u) { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + executor.cancel(); + executor_thread.join(); + + EXPECT_GE(count_2, 10u); + EXPECT_LE(count_2 - count_1, 1u); + } +} diff --git a/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp b/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp new file mode 100644 index 0000000000..1c8c5b3abe --- /dev/null +++ b/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp @@ -0,0 +1,617 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "rcpputils/scope_exit.hpp" + +#include "test_msgs/msg/empty.hpp" +#include "test_msgs/srv/empty.hpp" + +#include "../../mocking_utils/patch.hpp" +#include "../../utils/rclcpp_gtest_macros.hpp" + +namespace +{ + +struct NumberOfEntities +{ + size_t subscriptions = 0; + size_t timers = 0; + size_t services = 0; + size_t clients = 0; + size_t waitables = 0; +}; + +std::unique_ptr get_number_of_default_entities(rclcpp::Node::SharedPtr node) +{ + auto number_of_entities = std::make_unique(); + node->for_each_callback_group( + [&number_of_entities](rclcpp::CallbackGroup::SharedPtr group) + { + if (!group->can_be_taken_from().load()) { + return; + } + group->find_subscription_ptrs_if( + [&number_of_entities](rclcpp::SubscriptionBase::SharedPtr &) + { + number_of_entities->subscriptions++; return false; + }); + group->find_timer_ptrs_if( + [&number_of_entities](rclcpp::TimerBase::SharedPtr &) + { + number_of_entities->timers++; return false; + }); + group->find_service_ptrs_if( + [&number_of_entities](rclcpp::ServiceBase::SharedPtr &) + { + number_of_entities->services++; return false; + }); + group->find_client_ptrs_if( + [&number_of_entities](rclcpp::ClientBase::SharedPtr &) + { + number_of_entities->clients++; return false; + }); + group->find_waitable_ptrs_if( + [&number_of_entities](rclcpp::Waitable::SharedPtr &) + { + number_of_entities->waitables++; return false; + }); + }); + + return number_of_entities; +} + +} // namespace + +class TestStaticExecutorEntitiesCollector : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + entities_collector_ = + std::make_shared(); + } + + void TearDown() + { + rclcpp::shutdown(); + } + + rclcpp::executors::StaticExecutorEntitiesCollector::SharedPtr entities_collector_; +}; + +TEST_F(TestStaticExecutorEntitiesCollector, construct_destruct) { + EXPECT_EQ(0u, entities_collector_->get_number_of_subscriptions()); + EXPECT_EQ(0u, entities_collector_->get_number_of_timers()); + EXPECT_EQ(0u, entities_collector_->get_number_of_services()); + EXPECT_EQ(0u, entities_collector_->get_number_of_clients()); + EXPECT_EQ(0u, entities_collector_->get_number_of_waitables()); +} + +TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node) { + auto node1 = std::make_shared("node1", "ns"); + EXPECT_NO_THROW(entities_collector_->add_node(node1->get_node_base_interface())); + + // Check adding second time + RCLCPP_EXPECT_THROW_EQ( + entities_collector_->add_node(node1->get_node_base_interface()), + std::runtime_error("Node has already been added to an executor.")); + + auto node2 = std::make_shared("node2", "ns"); + EXPECT_FALSE(entities_collector_->remove_node(node2->get_node_base_interface())); + EXPECT_NO_THROW(entities_collector_->add_node(node2->get_node_base_interface())); + + EXPECT_TRUE(entities_collector_->remove_node(node1->get_node_base_interface())); + EXPECT_FALSE(entities_collector_->remove_node(node1->get_node_base_interface())); + EXPECT_TRUE(entities_collector_->remove_node(node2->get_node_base_interface())); + + auto node3 = std::make_shared("node3", "ns"); + node3->get_node_base_interface()->get_associated_with_executor_atomic().exchange(true); + EXPECT_FALSE(entities_collector_->remove_node(node3->get_node_base_interface())); +} + +TEST_F(TestStaticExecutorEntitiesCollector, init_bad_arguments) { + auto node = std::make_shared("node", "ns"); + entities_collector_->add_node(node->get_node_base_interface()); + + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_allocator_t allocator = rcl_get_default_allocator(); + auto shared_context = node->get_node_base_interface()->get_context(); + rcl_context_t * context = shared_context->get_rcl_context().get(); + EXPECT_EQ( + RCL_RET_OK, + rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator)); + RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));}); + + rclcpp::GuardCondition guard_condition(shared_context); + + // Check memory strategy is nullptr + rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy = nullptr; + EXPECT_THROW( + entities_collector_->init(&wait_set, memory_strategy), + std::runtime_error); +} + +TEST_F(TestStaticExecutorEntitiesCollector, add_remove_basic_node) { + auto node = std::make_shared("node", "ns"); + const auto expected_number_of_entities = get_number_of_default_entities(node); + EXPECT_NE(nullptr, expected_number_of_entities); + entities_collector_->add_node(node->get_node_base_interface()); + + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_allocator_t allocator = rcl_get_default_allocator(); + auto shared_context = node->get_node_base_interface()->get_context(); + rcl_context_t * context = shared_context->get_rcl_context().get(); + EXPECT_EQ( + RCL_RET_OK, + rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator)); + RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));}); + + auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); + rclcpp::GuardCondition guard_condition(shared_context); + + entities_collector_->init(&wait_set, memory_strategy); + RCPPUTILS_SCOPE_EXIT(entities_collector_->fini()); + EXPECT_EQ( + expected_number_of_entities->subscriptions, + entities_collector_->get_number_of_subscriptions()); + EXPECT_EQ(expected_number_of_entities->timers, entities_collector_->get_number_of_timers()); + EXPECT_EQ(expected_number_of_entities->services, entities_collector_->get_number_of_services()); + EXPECT_EQ(expected_number_of_entities->clients, entities_collector_->get_number_of_clients()); + // One extra for the executor + EXPECT_EQ( + 1u + expected_number_of_entities->waitables, + entities_collector_->get_number_of_waitables()); + + EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface())); + entities_collector_->init(&wait_set, memory_strategy); + EXPECT_EQ(0u, entities_collector_->get_number_of_subscriptions()); + EXPECT_EQ(0u, entities_collector_->get_number_of_timers()); + EXPECT_EQ(0u, entities_collector_->get_number_of_services()); + EXPECT_EQ(0u, entities_collector_->get_number_of_clients()); + + // Still one for the executor + EXPECT_EQ(1u, entities_collector_->get_number_of_waitables()); +} + +TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_out_of_scope) { + rclcpp::Context::SharedPtr shared_context = nullptr; + { + auto node1 = std::make_shared("node1", "ns"); + auto node2 = std::make_shared("node2", "ns"); + auto node3 = std::make_shared("node3", "ns"); + entities_collector_->add_node(node1->get_node_base_interface()); + entities_collector_->add_node(node2->get_node_base_interface()); + entities_collector_->add_node(node3->get_node_base_interface()); + shared_context = node1->get_node_base_interface()->get_context(); + } + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_context_t * context = shared_context->get_rcl_context().get(); + EXPECT_EQ( + RCL_RET_OK, + rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator)); + RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));}); + + auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); + rclcpp::GuardCondition guard_condition(shared_context); + + // Expect weak_node pointers to be cleaned up and used + entities_collector_->init(&wait_set, memory_strategy); + RCPPUTILS_SCOPE_EXIT(entities_collector_->fini()); + EXPECT_EQ(0u, entities_collector_->get_number_of_subscriptions()); + EXPECT_EQ(0u, entities_collector_->get_number_of_timers()); + EXPECT_EQ(0u, entities_collector_->get_number_of_services()); + EXPECT_EQ(0u, entities_collector_->get_number_of_clients()); + + // Still one for the executor + EXPECT_EQ(1u, entities_collector_->get_number_of_waitables()); +} + +class TestWaitable : public rclcpp::Waitable +{ +public: + void add_to_wait_set(rcl_wait_set_t &) override {} + + bool is_ready(const rcl_wait_set_t &) override {return true;} + + std::shared_ptr + take_data() override + { + return nullptr; + } + void + execute(const std::shared_ptr &) override {} +}; + +TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_with_entities) { + auto node = std::make_shared("node", "ns"); + auto expected_number_of_entities = get_number_of_default_entities(node); + EXPECT_NE(nullptr, expected_number_of_entities); + + // Create 1 of each entity type + auto subscription = + node->create_subscription( + "topic", rclcpp::QoS(10), [](test_msgs::msg::Empty::ConstSharedPtr) {}); + auto timer = + node->create_wall_timer(std::chrono::seconds(60), []() {}); + auto service = + node->create_service( + "service", + []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}); + auto client = node->create_client("service"); + auto waitable = std::make_shared(); + + // Adding a subscription could add another waitable, so we need to get the + // current number of waitables just before adding the new waitable. + expected_number_of_entities->waitables = get_number_of_default_entities(node)->waitables; + node->get_node_waitables_interface()->add_waitable(waitable, nullptr); + + entities_collector_->add_node(node->get_node_base_interface()); + + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_allocator_t allocator = rcl_get_default_allocator(); + auto shared_context = node->get_node_base_interface()->get_context(); + rcl_context_t * context = shared_context->get_rcl_context().get(); + EXPECT_EQ( + RCL_RET_OK, + rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator)); + RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));}); + + auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); + + rclcpp::GuardCondition guard_condition(shared_context); + + entities_collector_->init(&wait_set, memory_strategy); + RCPPUTILS_SCOPE_EXIT(entities_collector_->fini()); + + EXPECT_EQ( + 1u + expected_number_of_entities->subscriptions, + entities_collector_->get_number_of_subscriptions()); + EXPECT_EQ(1u + expected_number_of_entities->timers, entities_collector_->get_number_of_timers()); + EXPECT_EQ( + 1u + expected_number_of_entities->services, + entities_collector_->get_number_of_services()); + EXPECT_EQ( + 1u + expected_number_of_entities->clients, + entities_collector_->get_number_of_clients()); + + // One extra for the executor + EXPECT_EQ( + 2u + expected_number_of_entities->waitables, + entities_collector_->get_number_of_waitables()); + + entities_collector_->remove_node(node->get_node_base_interface()); + entities_collector_->init(&wait_set, memory_strategy); + EXPECT_EQ(0u, entities_collector_->get_number_of_subscriptions()); + EXPECT_EQ(0u, entities_collector_->get_number_of_timers()); + EXPECT_EQ(0u, entities_collector_->get_number_of_services()); + EXPECT_EQ(0u, entities_collector_->get_number_of_clients()); + // Still one for the executor + EXPECT_EQ(1u, entities_collector_->get_number_of_waitables()); +} + +TEST_F(TestStaticExecutorEntitiesCollector, add_callback_group) { + auto node = std::make_shared("node1", "ns"); + rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + entities_collector_->add_callback_group(cb_group, node->get_node_base_interface()); + ASSERT_EQ(entities_collector_->get_all_callback_groups().size(), 1u); +} + +TEST_F(TestStaticExecutorEntitiesCollector, add_callback_group_after_add_node) { + auto node = std::make_shared("node1", "ns"); + rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + entities_collector_->add_node(node->get_node_base_interface()); + RCLCPP_EXPECT_THROW_EQ( + entities_collector_->add_callback_group(cb_group, node->get_node_base_interface()), + std::runtime_error("Callback group has already been added to an executor.")); +} + +TEST_F(TestStaticExecutorEntitiesCollector, add_callback_group_twice) { + auto node = std::make_shared("node1", "ns"); + rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + entities_collector_->add_callback_group(cb_group, node->get_node_base_interface()); + ASSERT_EQ(entities_collector_->get_all_callback_groups().size(), 1u); + cb_group->get_associated_with_executor_atomic().exchange(false); + RCLCPP_EXPECT_THROW_EQ( + entities_collector_->add_callback_group(cb_group, node->get_node_base_interface()), + std::runtime_error("Callback group was already added to executor.")); +} + +TEST_F(TestStaticExecutorEntitiesCollector, prepare_wait_set_rcl_wait_set_clear_error) { + auto node = std::make_shared("node", "ns"); + entities_collector_->add_node(node->get_node_base_interface()); + + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_allocator_t allocator = rcl_get_default_allocator(); + auto shared_context = node->get_node_base_interface()->get_context(); + rcl_context_t * context = shared_context->get_rcl_context().get(); + EXPECT_EQ( + RCL_RET_OK, + rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator)); + RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));}); + + auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); + rclcpp::GuardCondition guard_condition(shared_context); + + entities_collector_->init(&wait_set, memory_strategy); + RCPPUTILS_SCOPE_EXIT(entities_collector_->fini()); + + { + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_clear, RCL_RET_ERROR); + std::shared_ptr data = entities_collector_->take_data(); + RCLCPP_EXPECT_THROW_EQ( + entities_collector_->execute(data), + std::runtime_error("Couldn't clear wait set")); + } + + EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface())); +} + +TEST_F(TestStaticExecutorEntitiesCollector, prepare_wait_set_rcl_wait_set_resize_error) { + auto node = std::make_shared("node", "ns"); + entities_collector_->add_node(node->get_node_base_interface()); + + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_allocator_t allocator = rcl_get_default_allocator(); + auto shared_context = node->get_node_base_interface()->get_context(); + rcl_context_t * context = shared_context->get_rcl_context().get(); + EXPECT_EQ( + RCL_RET_OK, + rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator)); + RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));}); + + auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); + rclcpp::GuardCondition guard_condition(shared_context); + + entities_collector_->init(&wait_set, memory_strategy); + RCPPUTILS_SCOPE_EXIT(entities_collector_->fini()); + + { + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_resize, RCL_RET_ERROR); + std::shared_ptr data = entities_collector_->take_data(); + RCLCPP_EXPECT_THROW_EQ( + entities_collector_->execute(data), + std::runtime_error("Couldn't resize the wait set: error not set")); + } + + EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface())); +} + +TEST_F(TestStaticExecutorEntitiesCollector, refresh_wait_set_not_initialized) { + RCLCPP_EXPECT_THROW_EQ( + entities_collector_->refresh_wait_set(std::chrono::nanoseconds(1000)), + std::runtime_error("Couldn't clear wait set")); + rcl_reset_error(); +} + +TEST_F(TestStaticExecutorEntitiesCollector, refresh_wait_set_rcl_wait_failed) { + auto node = std::make_shared("node", "ns"); + entities_collector_->add_node(node->get_node_base_interface()); + + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_allocator_t allocator = rcl_get_default_allocator(); + auto shared_context = node->get_node_base_interface()->get_context(); + rcl_context_t * context = shared_context->get_rcl_context().get(); + EXPECT_EQ( + RCL_RET_OK, + rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator)); + RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));}); + + auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); + rclcpp::GuardCondition guard_condition(shared_context); + + entities_collector_->init(&wait_set, memory_strategy); + RCPPUTILS_SCOPE_EXIT(entities_collector_->fini()); + + { + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + entities_collector_->refresh_wait_set(std::chrono::nanoseconds(1000)), + std::runtime_error("rcl_wait() failed: error not set")); + } + + EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface())); +} + +TEST_F(TestStaticExecutorEntitiesCollector, refresh_wait_set_add_handles_to_wait_set_failed) { + auto node = std::make_shared("node", "ns"); + + // Create 1 of each entity type + auto subscription = + node->create_subscription( + "topic", rclcpp::QoS(10), [](test_msgs::msg::Empty::ConstSharedPtr) {}); + auto timer = + node->create_wall_timer(std::chrono::seconds(60), []() {}); + auto service = + node->create_service( + "service", + []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}); + auto client = node->create_client("service"); + auto waitable = std::make_shared(); + + node->get_node_waitables_interface()->add_waitable(waitable, nullptr); + + entities_collector_->add_node(node->get_node_base_interface()); + + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_allocator_t allocator = rcl_get_default_allocator(); + auto shared_context = node->get_node_base_interface()->get_context(); + rcl_context_t * context = shared_context->get_rcl_context().get(); + EXPECT_EQ( + RCL_RET_OK, + rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator)); + RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));}); + + auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); + + rclcpp::GuardCondition guard_condition(shared_context); + + entities_collector_->init(&wait_set, memory_strategy); + RCPPUTILS_SCOPE_EXIT(entities_collector_->fini()); + + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_wait_set_add_subscription, + RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + entities_collector_->refresh_wait_set(std::chrono::nanoseconds(1000)), + std::runtime_error("Couldn't fill wait set")); + } + + entities_collector_->remove_node(node->get_node_base_interface()); +} + +TEST_F(TestStaticExecutorEntitiesCollector, add_to_wait_set_nullptr) { + auto node = std::make_shared("node", "ns"); + entities_collector_->add_node(node->get_node_base_interface()); + + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_allocator_t allocator = rcl_get_default_allocator(); + auto shared_context = node->get_node_base_interface()->get_context(); + rcl_context_t * context = shared_context->get_rcl_context().get(); + EXPECT_EQ( + RCL_RET_OK, + rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator)); + RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));}); + + auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); + rclcpp::GuardCondition guard_condition(shared_context); + + entities_collector_->init(&wait_set, memory_strategy); + RCPPUTILS_SCOPE_EXIT(entities_collector_->fini()); + + EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface())); +} + +TEST_F(TestStaticExecutorEntitiesCollector, fill_memory_strategy_invalid_group) { + auto node = std::make_shared("node", "ns"); + entities_collector_->add_node(node->get_node_base_interface()); + + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_allocator_t allocator = rcl_get_default_allocator(); + auto shared_context = node->get_node_base_interface()->get_context(); + rcl_context_t * context = shared_context->get_rcl_context().get(); + EXPECT_EQ( + RCL_RET_OK, + rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator)); + RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));}); + + auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); + rclcpp::GuardCondition guard_condition(shared_context); + + rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + entities_collector_->add_callback_group(cb_group, node->get_node_base_interface()); + ASSERT_EQ(entities_collector_->get_all_callback_groups().size(), 2u); + + cb_group.reset(); + + entities_collector_->init(&wait_set, memory_strategy); + RCPPUTILS_SCOPE_EXIT(entities_collector_->fini()); + ASSERT_EQ(entities_collector_->get_all_callback_groups().size(), 1u); + + EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface())); +} + +TEST_F(TestStaticExecutorEntitiesCollector, remove_callback_group_after_node) { + auto node = std::make_shared("node1", "ns"); + rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + entities_collector_->add_callback_group(cb_group, node->get_node_base_interface()); + ASSERT_EQ(entities_collector_->get_all_callback_groups().size(), 1u); + + node.reset(); + + RCLCPP_EXPECT_THROW_EQ( + entities_collector_->remove_callback_group(cb_group), + std::runtime_error("Node must not be deleted before its callback group(s).")); +} + +TEST_F(TestStaticExecutorEntitiesCollector, remove_callback_group_twice) { + auto node = std::make_shared("node1", "ns"); + rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + entities_collector_->add_callback_group(cb_group, node->get_node_base_interface()); + ASSERT_EQ(entities_collector_->get_all_callback_groups().size(), 1u); + + entities_collector_->remove_callback_group(cb_group); + + RCLCPP_EXPECT_THROW_EQ( + entities_collector_->remove_callback_group(cb_group), + std::runtime_error("Callback group needs to be associated with executor.")); +} + +TEST_F(TestStaticExecutorEntitiesCollector, remove_node_opposite_order) { + auto node1 = std::make_shared("node1", "ns"); + EXPECT_NO_THROW(entities_collector_->add_node(node1->get_node_base_interface())); + + auto node2 = std::make_shared("node2", "ns"); + EXPECT_NO_THROW(entities_collector_->add_node(node2->get_node_base_interface())); + + EXPECT_TRUE(entities_collector_->remove_node(node2->get_node_base_interface())); +} + +TEST_F( + TestStaticExecutorEntitiesCollector, + add_callback_groups_from_nodes_associated_to_executor_add) { + auto node = std::make_shared("node", "ns"); + rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + entities_collector_->add_callback_group(cb_group, node->get_node_base_interface()); + entities_collector_->add_node(node->get_node_base_interface()); + + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_allocator_t allocator = rcl_get_default_allocator(); + auto shared_context = node->get_node_base_interface()->get_context(); + rcl_context_t * context = shared_context->get_rcl_context().get(); + EXPECT_EQ( + RCL_RET_OK, + rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator)); + RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));}); + + auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); + rclcpp::GuardCondition guard_condition(shared_context); + + entities_collector_->init(&wait_set, memory_strategy); + RCPPUTILS_SCOPE_EXIT(entities_collector_->fini()); + + cb_group->get_associated_with_executor_atomic().exchange(false); + std::shared_ptr data = entities_collector_->take_data(); + entities_collector_->execute(data); + + EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface())); +} diff --git a/rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp b/rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp new file mode 100644 index 0000000000..8e330cbebc --- /dev/null +++ b/rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp @@ -0,0 +1,192 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/executors.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "test_msgs/srv/empty.hpp" + +#include "./executor_types.hpp" +#include "../../mocking_utils/patch.hpp" +#include "../../utils/rclcpp_gtest_macros.hpp" + +using namespace std::chrono_literals; + +class TestStaticSingleThreadedExecutor : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + } + + void TearDown() + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestStaticSingleThreadedExecutor, add_callback_group_trigger_guard_failed) { +#ifdef __clang__ +# pragma clang diagnostic push +# pragma clang diagnostic ignored "-Wdeprecated-declarations" +#endif + + DeprecatedStaticSingleThreadedExecutor executor; +#ifdef __clang__ +# pragma clang diagnostic pop +#endif + auto node = std::make_shared("node", "ns"); + rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + executor.add_callback_group(cb_group, node->get_node_base_interface(), true), + std::runtime_error("Failed to handle entities update on callback group add: error not set")); + } +} + +TEST_F(TestStaticSingleThreadedExecutor, add_node_trigger_guard_failed) { +#ifdef __clang__ +# pragma clang diagnostic push +# pragma clang diagnostic ignored "-Wdeprecated-declarations" +#endif + + DeprecatedStaticSingleThreadedExecutor executor; +#ifdef __clang__ +# pragma clang diagnostic pop +#endif + auto node = std::make_shared("node", "ns"); + + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + executor.add_node(node), + std::runtime_error("Failed to handle entities update on node add: error not set")); + } +} + +TEST_F(TestStaticSingleThreadedExecutor, remove_callback_group_trigger_guard_failed) { +#ifdef __clang__ +# pragma clang diagnostic push +# pragma clang diagnostic ignored "-Wdeprecated-declarations" +#endif + + DeprecatedStaticSingleThreadedExecutor executor; +#ifdef __clang__ +# pragma clang diagnostic pop +#endif + auto node = std::make_shared("node", "ns"); + rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + executor.add_callback_group(cb_group, node->get_node_base_interface(), true); + + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + executor.remove_callback_group(cb_group, true), + std::runtime_error( + "Failed to handle entities update on callback group remove: error not set")); + } +} + +TEST_F(TestStaticSingleThreadedExecutor, remove_node_failed) { +#ifdef __clang__ +# pragma clang diagnostic push +# pragma clang diagnostic ignored "-Wdeprecated-declarations" +#endif + + DeprecatedStaticSingleThreadedExecutor executor; +#ifdef __clang__ +# pragma clang diagnostic pop +#endif + auto node = std::make_shared("node", "ns"); + + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + executor.remove_node(node, true), + std::runtime_error("Node '/ns/node' needs to be associated with an executor.")); + } +} + +TEST_F(TestStaticSingleThreadedExecutor, remove_node_trigger_guard_failed) { +#ifdef __clang__ +# pragma clang diagnostic push +# pragma clang diagnostic ignored "-Wdeprecated-declarations" +#endif + + DeprecatedStaticSingleThreadedExecutor executor; +#ifdef __clang__ +# pragma clang diagnostic pop +#endif + auto node = std::make_shared("node", "ns"); + + executor.add_node(node); + + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + executor.remove_node(node, true), + std::runtime_error("Failed to handle entities update on node remove: error not set")); + } +} + +TEST_F(TestStaticSingleThreadedExecutor, execute_service) { +#ifdef __clang__ +# pragma clang diagnostic push +# pragma clang diagnostic ignored "-Wdeprecated-declarations" +#endif + + DeprecatedStaticSingleThreadedExecutor executor; +#ifdef __clang__ +# pragma clang diagnostic pop +#endif + auto node = std::make_shared("node", "ns"); + executor.add_node(node); + + auto service = + node->create_service( + "service", + []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}); + auto client = node->create_client("service"); + + std::promise promise; + std::future future = promise.get_future(); + EXPECT_EQ( + rclcpp::FutureReturnCode::TIMEOUT, + executor.spin_until_future_complete(future, std::chrono::milliseconds(1))); + + executor.remove_node(node); + executor.spin_until_future_complete(future, std::chrono::milliseconds(1)); +} diff --git a/rclcpp/test/rclcpp/executors/test_waitable.cpp b/rclcpp/test/rclcpp/executors/test_waitable.cpp new file mode 100644 index 0000000000..dfae485f30 --- /dev/null +++ b/rclcpp/test/rclcpp/executors/test_waitable.cpp @@ -0,0 +1,125 @@ +// Copyright 2024 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 "rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp" +#include "rclcpp/waitable.hpp" + +#include "rcl/wait.h" + +#include "test_waitable.hpp" + +using namespace std::chrono_literals; + +void +TestWaitable::add_to_wait_set(rcl_wait_set_t & wait_set) +{ + if (trigger_count_ > 0) { + // Keep the gc triggered until the trigger count is reduced back to zero. + // This is necessary if trigger() results in the wait set waking, but not + // executing this waitable, in which case it needs to be re-triggered. + gc_.trigger(); + } + rclcpp::detail::add_guard_condition_to_rcl_wait_set(wait_set, gc_); +} + +void TestWaitable::trigger() +{ + trigger_count_++; + gc_.trigger(); +} + +bool +TestWaitable::is_ready(const rcl_wait_set_t & wait_set) +{ + is_ready_count_++; + for (size_t i = 0; i < wait_set.size_of_guard_conditions; ++i) { + auto rcl_guard_condition = wait_set.guard_conditions[i]; + if (&gc_.get_rcl_guard_condition() == rcl_guard_condition) { + return true; + } + } + return false; +} + +std::shared_ptr +TestWaitable::take_data() +{ + return nullptr; +} + +std::shared_ptr +TestWaitable::take_data_by_entity_id(size_t id) +{ + (void) id; + return nullptr; +} + +void +TestWaitable::execute(const std::shared_ptr &) +{ + trigger_count_--; + count_++; + if (nullptr != on_execute_callback_) { + on_execute_callback_(); + } else { + // TODO(wjwwood): I don't know why this was here, but probably it should + // not be there, or test cases where that is important should use the + // on_execute_callback? + std::this_thread::sleep_for(3ms); + } +} + +void +TestWaitable::set_on_execute_callback(std::function on_execute_callback) +{ + on_execute_callback_ = on_execute_callback; +} + +void +TestWaitable::set_on_ready_callback(std::function callback) +{ + auto gc_callback = [callback](size_t count) { + callback(count, 0); + }; + gc_.set_on_trigger_callback(gc_callback); +} + +void +TestWaitable::clear_on_ready_callback() +{ + gc_.set_on_trigger_callback(nullptr); +} + +size_t +TestWaitable::get_number_of_ready_guard_conditions() +{ + return 1; +} + +size_t +TestWaitable::get_count() const +{ + return count_; +} + +size_t +TestWaitable::get_is_ready_call_count() const +{ + return is_ready_count_; +} diff --git a/rclcpp/test/rclcpp/executors/test_waitable.hpp b/rclcpp/test/rclcpp/executors/test_waitable.hpp new file mode 100644 index 0000000000..6c6df18578 --- /dev/null +++ b/rclcpp/test/rclcpp/executors/test_waitable.hpp @@ -0,0 +1,75 @@ +// Copyright 2024 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 RCLCPP__EXECUTORS__TEST_WAITABLE_HPP_ +#define RCLCPP__EXECUTORS__TEST_WAITABLE_HPP_ + +#include +#include +#include + +#include "rclcpp/waitable.hpp" +#include "rclcpp/guard_condition.hpp" + +#include "rcl/wait.h" + +class TestWaitable : public rclcpp::Waitable +{ +public: + TestWaitable() = default; + + void + add_to_wait_set(rcl_wait_set_t & wait_set) override; + + void trigger(); + + bool + is_ready(const rcl_wait_set_t & wait_set) override; + + std::shared_ptr + take_data() override; + + std::shared_ptr + take_data_by_entity_id(size_t id) override; + + void + execute(const std::shared_ptr &) override; + + void + set_on_execute_callback(std::function on_execute_callback); + + void + set_on_ready_callback(std::function callback) override; + + void + clear_on_ready_callback() override; + + size_t + get_number_of_ready_guard_conditions() override; + + size_t + get_count() const; + + size_t + get_is_ready_call_count() const; + +private: + std::atomic trigger_count_ = 0; + std::atomic is_ready_count_ = 0; + std::atomic count_ = 0; + rclcpp::GuardCondition gc_; + std::function on_execute_callback_ = nullptr; +}; + +#endif // RCLCPP__EXECUTORS__TEST_WAITABLE_HPP_ diff --git a/rclcpp/test/rclcpp/node_interfaces/detail/test_template_utils.cpp b/rclcpp/test/rclcpp/node_interfaces/detail/test_template_utils.cpp new file mode 100644 index 0000000000..9ae715ebce --- /dev/null +++ b/rclcpp/test/rclcpp/node_interfaces/detail/test_template_utils.cpp @@ -0,0 +1,56 @@ +// 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 "rclcpp/detail/template_contains.hpp" +#include "rclcpp/detail/template_unique.hpp" + + +TEST(NoOpTests, test_node_interfaces_template_utils) { +} // This is just to let gtest work + +namespace rclcpp +{ +namespace detail +{ + +// This tests template logic at compile time +namespace +{ + +struct test_template_unique +{ + static_assert(template_unique_v, "failed"); + static_assert(template_unique_v, "failed"); + static_assert(!template_unique_v, "failed"); + static_assert(!template_unique_v, "failed"); + static_assert(!template_unique_v, "failed"); +}; + + +struct test_template_contains +{ + static_assert(template_contains_v, "failed"); + static_assert(template_contains_v, "failed"); + static_assert(template_contains_v, "failed"); + static_assert(!template_contains_v, "failed"); + static_assert(!template_contains_v, "failed"); + static_assert(!template_contains_v, "failed"); +}; + +} // namespace + +} // namespace detail +} // namespace rclcpp diff --git a/rclcpp/test/rclcpp/node_interfaces/node_wrapper.hpp b/rclcpp/test/rclcpp/node_interfaces/node_wrapper.hpp new file mode 100644 index 0000000000..b1413e1f05 --- /dev/null +++ b/rclcpp/test/rclcpp/node_interfaces/node_wrapper.hpp @@ -0,0 +1,64 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__NODE_INTERFACES__NODE_WRAPPER_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_WRAPPER_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" + +class NodeWrapper +{ +public: + explicit NodeWrapper(const std::string & name) + : node(std::make_shared(name)) + {} + + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr + get_node_base_interface() {return this->node->get_node_base_interface();} + + rclcpp::node_interfaces::NodeClockInterface::SharedPtr + get_node_clock_interface() {return this->node->get_node_clock_interface();} + + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr + get_node_graph_interface() {return this->node->get_node_graph_interface();} + + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr + get_node_logging_interface() {return this->node->get_node_logging_interface();} + + rclcpp::node_interfaces::NodeTimersInterface::SharedPtr + get_node_timers_interface() {return this->node->get_node_timers_interface();} + + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr + get_node_topics_interface() {return this->node->get_node_topics_interface();} + + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr + get_node_services_interface() {return this->node->get_node_services_interface();} + + rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr + get_node_waitables_interface() {return this->node->get_node_waitables_interface();} + + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr + get_node_parameters_interface() {return this->node->get_node_parameters_interface();} + + rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr + get_node_time_source_interface() {return this->node->get_node_time_source_interface();} + +private: + rclcpp::Node::SharedPtr node; +}; + +#endif // RCLCPP__NODE_INTERFACES__NODE_WRAPPER_HPP_ diff --git a/rclcpp/test/rclcpp/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_rclcpp_node.cpp b/rclcpp/test/rclcpp/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_rclcpp_node.cpp new file mode 100644 index 0000000000..121f64126f --- /dev/null +++ b/rclcpp/test/rclcpp/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_rclcpp_node.cpp @@ -0,0 +1,28 @@ +// 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 "rclcpp/rclcpp.hpp" +#include "rclcpp/node_interfaces/get_node_topics_interface.hpp" + +int main(void) +{ + auto node = std::make_shared("test_node"); + std::shared_ptr const_node_ptr = node; + // Should fail because a const node cannot have a non-const method called on it. + rclcpp::node_interfaces::NodeTopicsInterface * result = + rclcpp::node_interfaces::get_node_topics_interface(const_node_ptr); + (void)result; +} diff --git a/rclcpp/test/rclcpp/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_wrapped_node.cpp b/rclcpp/test/rclcpp/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_wrapped_node.cpp new file mode 100644 index 0000000000..4f5b8311af --- /dev/null +++ b/rclcpp/test/rclcpp/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ptr_wrapped_node.cpp @@ -0,0 +1,30 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/node_interfaces/get_node_topics_interface.hpp" + +#include "../node_wrapper.hpp" + +int main(void) +{ + auto node = std::make_shared("test_wrapped_node"); + std::shared_ptr const_node_ptr = node; + // Should fail because a const node cannot have a non-const method called on it. + rclcpp::node_interfaces::NodeTopicsInterface * result = + rclcpp::node_interfaces::get_node_topics_interface(const_node_ptr); + (void)result; +} diff --git a/rclcpp/test/rclcpp/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_rclcpp_node.cpp b/rclcpp/test/rclcpp/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_rclcpp_node.cpp new file mode 100644 index 0000000000..ed147418f5 --- /dev/null +++ b/rclcpp/test/rclcpp/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_rclcpp_node.cpp @@ -0,0 +1,28 @@ +// 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 "rclcpp/rclcpp.hpp" +#include "rclcpp/node_interfaces/get_node_topics_interface.hpp" + +int main(void) +{ + auto node = std::make_shared("test_node"); + const rclcpp::Node & const_node_reference = *node; + // Should fail because a const node cannot have a non-const method called on it. + rclcpp::node_interfaces::NodeTopicsInterface * result = + rclcpp::node_interfaces::get_node_topics_interface(const_node_reference); + (void)result; +} diff --git a/rclcpp/test/rclcpp/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_wrapped_node.cpp b/rclcpp/test/rclcpp/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_wrapped_node.cpp new file mode 100644 index 0000000000..6d7c19cac7 --- /dev/null +++ b/rclcpp/test/rclcpp/node_interfaces/test_does_not_compile/get_node_topics_interface_const_ref_wrapped_node.cpp @@ -0,0 +1,30 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/node_interfaces/get_node_topics_interface.hpp" + +#include "../node_wrapper.hpp" + +int main(void) +{ + auto node = std::make_shared("test_wrapped_node"); + const NodeWrapper & const_node_reference = *node; + // Should fail because a const node cannot have a non-const method called on it. + rclcpp::node_interfaces::NodeTopicsInterface * result = + rclcpp::node_interfaces::get_node_topics_interface(const_node_reference); + (void)result; +} diff --git a/rclcpp/test/rclcpp/node_interfaces/test_get_node_interfaces.cpp b/rclcpp/test/rclcpp/node_interfaces/test_get_node_interfaces.cpp new file mode 100644 index 0000000000..d890f74d08 --- /dev/null +++ b/rclcpp/test/rclcpp/node_interfaces/test_get_node_interfaces.cpp @@ -0,0 +1,150 @@ +// 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 "gtest/gtest.h" + +#include "rclcpp/node_interfaces/get_node_topics_interface.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "./node_wrapper.hpp" + +static const std::string node_suffix = "test_get_node_interfaces"; // NOLINT + +class TestGetNodeInterfaces : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + node = std::make_shared(node_suffix); + wrapped_node = std::make_shared("wrapped_" + node_suffix); + } + + static void TearDownTestCase() + { + node.reset(); + wrapped_node.reset(); + rclcpp::shutdown(); + } + + static rclcpp::Node::SharedPtr node; + static std::shared_ptr wrapped_node; +}; + +rclcpp::Node::SharedPtr TestGetNodeInterfaces::node = nullptr; +std::shared_ptr TestGetNodeInterfaces::wrapped_node = nullptr; + +TEST_F(TestGetNodeInterfaces, null_rclcpp_node_shared_ptr) { + rclcpp::Node::SharedPtr null_node; + EXPECT_THROW( + { + rclcpp::node_interfaces::get_node_topics_interface(null_node); + }, std::invalid_argument); +} + +TEST_F(TestGetNodeInterfaces, rclcpp_node_shared_ptr) { + auto result = rclcpp::node_interfaces::get_node_topics_interface(this->node); + static_assert( + std::is_same< + std::shared_ptr, + decltype(result) + >::value, "expected std::shared_ptr"); +} + +TEST_F(TestGetNodeInterfaces, null_node_shared_ptr) { + std::shared_ptr null_node; + EXPECT_THROW( + { + rclcpp::node_interfaces::get_node_topics_interface(null_node); + }, std::invalid_argument); +} + +TEST_F(TestGetNodeInterfaces, node_shared_ptr) { + auto result = rclcpp::node_interfaces::get_node_topics_interface(this->wrapped_node); + static_assert( + std::is_same< + std::shared_ptr, + decltype(result) + >::value, "expected std::shared_ptr"); +} + +TEST_F(TestGetNodeInterfaces, rclcpp_node_reference) { + rclcpp::Node & node_reference = *this->node; + auto result = rclcpp::node_interfaces::get_node_topics_interface(node_reference); + static_assert( + std::is_same< + std::shared_ptr, + decltype(result) + >::value, "expected std::shared_ptr"); +} + +TEST_F(TestGetNodeInterfaces, node_reference) { + NodeWrapper & wrapped_node_reference = *this->wrapped_node; + auto result = rclcpp::node_interfaces::get_node_topics_interface(wrapped_node_reference); + static_assert( + std::is_same< + std::shared_ptr, + decltype(result) + >::value, "expected std::shared_ptr"); +} + +TEST_F(TestGetNodeInterfaces, rclcpp_node_pointer) { + rclcpp::Node * node_pointer = this->node.get(); + auto result = rclcpp::node_interfaces::get_node_topics_interface(node_pointer); + static_assert( + std::is_same< + std::shared_ptr, + decltype(result) + >::value, "expected std::shared_ptr"); +} + +TEST_F(TestGetNodeInterfaces, null_rclcpp_node_pointer) { + rclcpp::Node * null_node{nullptr}; + EXPECT_THROW( + { + rclcpp::node_interfaces::get_node_topics_interface(null_node); + }, std::invalid_argument); +} + +TEST_F(TestGetNodeInterfaces, node_pointer) { + NodeWrapper * wrapped_node_pointer = this->wrapped_node.get(); + auto result = rclcpp::node_interfaces::get_node_topics_interface(wrapped_node_pointer); + static_assert( + std::is_same< + std::shared_ptr, + decltype(result) + >::value, "expected std::shared_ptr"); +} + +TEST_F(TestGetNodeInterfaces, null_node_pointer) { + NodeWrapper * null_node{nullptr}; + EXPECT_THROW( + { + rclcpp::node_interfaces::get_node_topics_interface(null_node); + }, std::invalid_argument); +} + +TEST_F(TestGetNodeInterfaces, interface_shared_pointer) { + std::shared_ptr interface_shared_ptr = + this->node->get_node_topics_interface(); + auto result = rclcpp::node_interfaces::get_node_topics_interface(interface_shared_ptr); + static_assert( + std::is_same< + std::shared_ptr, + decltype(result) + >::value, "expected std::shared_ptr"); +} diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_base.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_base.cpp new file mode 100644 index 0000000000..a40bc712bd --- /dev/null +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_base.cpp @@ -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. + +#include + +#include +#include + +#include "rcl/node_options.h" +#include "rclcpp/node.hpp" +#include "rclcpp/node_interfaces/node_base.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rmw/validate_namespace.h" +#include "rmw/validate_node_name.h" + +#include "../../mocking_utils/patch.hpp" +#include "../../utils/rclcpp_gtest_macros.hpp" + +class TestNodeBase : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + } + + void TearDown() + { + rclcpp::shutdown(); + } +}; + +// Required for mocking_utils below +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, ==) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, !=) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, <) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, >) + +TEST_F(TestNodeBase, construct_from_node) +{ + std::shared_ptr node = std::make_shared("node", "ns"); + + // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure + // the proper type is being tested and covered. + auto * node_base = + dynamic_cast(node->get_node_base_interface().get()); + ASSERT_NE(nullptr, node_base); + + EXPECT_STREQ("node", node_base->get_name()); + EXPECT_STREQ("/ns", node_base->get_namespace()); + + EXPECT_STREQ("/ns/node", node_base->get_fully_qualified_name()); + EXPECT_NE(nullptr, node_base->get_context()); + EXPECT_NE(nullptr, node_base->get_rcl_node_handle()); + EXPECT_NE(nullptr, node_base->get_shared_rcl_node_handle()); + + const auto * const_node_base = node_base; + EXPECT_NE(nullptr, const_node_base->get_rcl_node_handle()); + EXPECT_NE(nullptr, const_node_base->get_shared_rcl_node_handle()); +} + +TEST_F(TestNodeBase, construct_destruct_rcl_guard_condition_init_error) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_guard_condition_init, RCL_RET_ERROR); + EXPECT_THROW( + std::make_shared("node", "ns").reset(), + rclcpp::exceptions::RCLError); +} + +TEST_F(TestNodeBase, construct_destruct_rcl_node_init_error) { + auto mock_node_init = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_node_init, RCL_RET_ERROR); + + // This function is called only if rcl_node_init fails, so both mocked functions are required + // This just logs an error, so behavior shouldn't change + auto mock_guard_condition_fini = mocking_utils::inject_on_return( + "lib:rclcpp", rcl_guard_condition_fini, RCL_RET_ERROR); + + EXPECT_THROW( + std::make_shared("node", "ns").reset(), + rclcpp::exceptions::RCLError); +} + +TEST_F(TestNodeBase, construct_destruct_rcl_node_init_rcl_invalid_node_name) { + auto mock_node_init = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_node_init, RCL_RET_NODE_INVALID_NAME); + + // `rmw_validate_node_name` is only called if `rcl_node_init` returns INVALID_NAME + auto mock_validate_node_name = mocking_utils::patch_and_return( + "lib:rclcpp", rmw_validate_node_name, RMW_RET_ERROR); + + EXPECT_THROW( + std::make_shared("node", "ns").reset(), + rclcpp::exceptions::RCLError); +} + +TEST_F(TestNodeBase, construct_destruct_rcl_node_init_rcl_invalid_node_name_invalid_argument) { + auto mock_node_init = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_node_init, RCL_RET_NODE_INVALID_NAME); + + // `rmw_validate_node_name` is only called if `rcl_node_init` returns INVALID_NAME + auto mock_validate_node_name = mocking_utils::patch_and_return( + "lib:rclcpp", rmw_validate_node_name, RMW_RET_INVALID_ARGUMENT); + + EXPECT_THROW( + std::make_shared("node", "ns").reset(), + rclcpp::exceptions::RCLInvalidArgument); +} + +TEST_F(TestNodeBase, construct_destruct_rcl_node_init_rcl_invalid_node_name_valid_rmw_node_name) { + auto mock_node_init = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_node_init, RCL_RET_NODE_INVALID_NAME); + + // `rmw_validate_node_name` is only called if `rcl_node_init` returns INVALID_NAME + auto mock = mocking_utils::patch( + "lib:rclcpp", rmw_validate_node_name, [](const char *, int * validation_result, size_t *) + { + *validation_result = RMW_NODE_NAME_VALID; + return RMW_RET_OK; + }); + + RCLCPP_EXPECT_THROW_EQ( + std::make_shared("node", "ns").reset(), + std::runtime_error("valid rmw node name but invalid rcl node name")); +} + +TEST_F(TestNodeBase, construct_destruct_rcl_node_init_rcl_invalid_namespace) { + auto mock_node_init = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_node_init, RCL_RET_NODE_INVALID_NAMESPACE); + + // `rmw_validate_namespace` is only called if `rcl_node_init` returns INVALID_NAMESPACE + auto mock_validate_namespace = mocking_utils::patch_and_return( + "lib:rclcpp", rmw_validate_namespace, RMW_RET_ERROR); + + EXPECT_THROW( + std::make_shared("node", "ns").reset(), + rclcpp::exceptions::RCLError); +} + +TEST_F(TestNodeBase, construct_destruct_rcl_node_init_rcl_invalid_namespace_rmw_invalid_argument) { + auto mock_node_init = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_node_init, RCL_RET_NODE_INVALID_NAMESPACE); + + // `rmw_validate_namespace` is only called if `rcl_node_init` returns INVALID_NAMESPACE + auto mock_validate_namespace = mocking_utils::patch_and_return( + "lib:rclcpp", rmw_validate_namespace, RMW_RET_INVALID_ARGUMENT); + + EXPECT_THROW( + std::make_shared("node", "ns").reset(), + rclcpp::exceptions::RCLInvalidArgument); +} + +TEST_F(TestNodeBase, construct_destruct_rcl_node_init_rcl_invalid_namespace_valid_rmw_namespace) { + auto mock_node_init = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_node_init, RCL_RET_NODE_INVALID_NAMESPACE); + + // `rmw_validate_namespace` is only called if `rcl_node_init` returns INVALID_NAMESPACE + auto mock = mocking_utils::patch( + "lib:rclcpp", rmw_validate_namespace, [](const char *, int * validation_result, size_t *) + { + *validation_result = RMW_NAMESPACE_VALID; + return RMW_RET_OK; + }); + + RCLCPP_EXPECT_THROW_EQ( + std::make_shared("node", "ns").reset(), + std::runtime_error("valid rmw node namespace but invalid rcl node namespace")); +} + +TEST_F(TestNodeBase, construct_destruct_rcl_node_init_fini_error) { + auto mock_node_fini = mocking_utils::inject_on_return( + "lib:rclcpp", rcl_node_fini, RCL_RET_ERROR); + + EXPECT_NO_THROW(std::make_shared("node", "ns").reset()); +} + +TEST_F(TestNodeBase, construct_destruct_rcl_guard_condition_fini_error) { + auto mock_node_fini = mocking_utils::inject_on_return( + "lib:rclcpp", rcl_guard_condition_fini, RCL_RET_ERROR); + + EXPECT_NO_THROW(std::make_shared("node", "ns").reset()); +} diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_clock.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_clock.cpp new file mode 100644 index 0000000000..dec171b655 --- /dev/null +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_clock.cpp @@ -0,0 +1,49 @@ +// 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 "rclcpp/node_interfaces/node_clock.hpp" +#include "rclcpp/node.hpp" + +class TestNodeClock : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + } + + void TearDown() + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestNodeClock, construct_from_node) +{ + std::shared_ptr node = std::make_shared("node", "ns"); + + // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure + // the proper type is being tested and covered. + auto * node_clock = + dynamic_cast(node->get_node_clock_interface().get()); + ASSERT_NE(nullptr, node_clock); + EXPECT_NE(nullptr, node_clock->get_clock()); + + const auto * const_node_clock = node_clock; + EXPECT_NE(nullptr, const_node_clock->get_clock()); +} diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp new file mode 100644 index 0000000000..a2f19aa82b --- /dev/null +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp @@ -0,0 +1,733 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include + +#include "rcl/graph.h" +#include "rcl/node_options.h" +#include "rcl/remap.h" +#include "rclcpp/node.hpp" +#include "rclcpp/node_interfaces/node_base.hpp" +#include "rclcpp/node_interfaces/node_graph.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rcutils/strdup.h" +#include "test_msgs/msg/empty.h" +#include "test_msgs/msg/empty.hpp" +#include "test_msgs/srv/empty.hpp" + +#include "../../mocking_utils/patch.hpp" +#include "../../utils/rclcpp_gtest_macros.hpp" + +namespace +{ + +constexpr char node_name[] = "node"; +constexpr char node_namespace[] = "ns"; +constexpr char absolute_namespace[] = "/ns"; + +} // namespace + +class TestNodeGraph : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + void SetUp() + { + node_ = std::make_shared(node_name, node_namespace); + + // This dynamic cast is not necessary for the unittests, but instead is used to ensure + // the proper type is being tested and covered. + node_graph_ = + dynamic_cast(node_->get_node_graph_interface().get()); + ASSERT_NE(nullptr, node_graph_); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + +protected: + std::shared_ptr node() {return node_;} + + const rclcpp::node_interfaces::NodeGraph * node_graph() const {return node_graph_;} + + size_t get_num_graph_things(std::function predicate) + { + constexpr std::chrono::milliseconds timeout(100); + + size_t tries = 0; + size_t num_things = 0; + while (tries++ < 5) { + num_things = predicate(); + if (num_things >= 1) { + break; + } + + auto event = node()->get_graph_event(); + EXPECT_NO_THROW(node()->wait_for_graph_change(event, timeout)); + } + + return num_things; + } + + size_t get_num_topics() + { + return get_num_graph_things( + [this]() -> size_t { + auto topic_names_and_types = node_graph()->get_topic_names_and_types(); + return topic_names_and_types.size(); + }); + } + + size_t get_num_services() + { + return get_num_graph_things( + [this]() -> size_t { + auto service_names_and_types = node_graph()->get_service_names_and_types(); + return service_names_and_types.size(); + }); + } + +private: + std::shared_ptr node_; + rclcpp::node_interfaces::NodeGraph * node_graph_; +}; + +TEST_F(TestNodeGraph, construct_from_node) +{ + EXPECT_LT(0u, get_num_topics()); + + EXPECT_LT(0u, get_num_services()); + + auto names = node_graph()->get_node_names(); + EXPECT_EQ(1u, names.size()); + + auto names_and_namespaces = node_graph()->get_node_names_and_namespaces(); + EXPECT_EQ(1u, names_and_namespaces.size()); + + auto names_namespaces_and_enclaves = + node_graph()->get_node_names_with_enclaves(); + EXPECT_EQ(1u, names_namespaces_and_enclaves.size()); + + EXPECT_EQ(0u, node_graph()->count_publishers("not_a_topic")); + EXPECT_EQ(0u, node_graph()->count_subscribers("not_a_topic")); + EXPECT_EQ(0u, node_graph()->count_clients("not_a_service")); + EXPECT_EQ(0u, node_graph()->count_services("not_a_service")); + + EXPECT_NE(nullptr, node_graph()->get_graph_guard_condition()); + + // get_graph_event is non-const + EXPECT_NE(nullptr, node()->get_node_graph_interface()->get_graph_event()); + EXPECT_LE(1u, node_graph()->count_graph_users()); +} + +TEST_F(TestNodeGraph, get_topic_names_and_types) +{ + ASSERT_LT(0u, get_num_topics()); +} + +TEST_F(TestNodeGraph, get_topic_names_and_types_rcl_error) +{ + auto mock_get_topic_names = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_get_topic_names_and_types, RCL_RET_ERROR); + auto mock_names_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_names_and_types_fini, RCL_RET_ERROR); + + RCLCPP_EXPECT_THROW_EQ( + node_graph()->get_topic_names_and_types(), + std::runtime_error( + "failed to get topic names and types: error not set, failed also to cleanup topic names and" + " types, leaking memory: error not set")); +} + +TEST_F(TestNodeGraph, get_topic_names_and_types_rcl_names_and_types_fini_error) +{ + auto mock_names_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_names_and_types_fini, RCL_RET_ERROR); + + RCLCPP_EXPECT_THROW_EQ( + node_graph()->get_topic_names_and_types(), + std::runtime_error("could not destroy topic names and types: error not set")); +} + +TEST_F(TestNodeGraph, get_service_names_and_types) +{ + ASSERT_LT(0u, get_num_services()); +} + +TEST_F(TestNodeGraph, get_service_names_and_types_rcl_error) +{ + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_get_service_names_and_types, RCL_RET_ERROR); + auto mock_names_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_names_and_types_fini, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_graph()->get_service_names_and_types(), + std::runtime_error( + "failed to get service names and types: error not set, failed also to cleanup service names" + " and types, leaking memory: error not set")); +} + +TEST_F(TestNodeGraph, get_service_names_and_types_rcl_names_and_types_fini) +{ + auto mock_names_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_names_and_types_fini, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_graph()->get_service_names_and_types(), + std::runtime_error("could not destroy service names and types: error not set")); +} + +TEST_F(TestNodeGraph, get_service_names_and_types_by_node) +{ + auto callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}; + auto service = + node()->create_service("node1_service", std::move(callback)); + + const std::string node2_name = "node2"; + auto node2 = std::make_shared(node2_name, node_namespace); + + // rcl_get_service_names_and_types_by_node() expects the node to exist, otherwise it fails + EXPECT_THROW( + node_graph()->get_service_names_and_types_by_node("not_a_node", "not_absolute_namespace"), + std::runtime_error); + + // Check that node1_service exists for node1 but not node2. This shouldn't exercise graph + // discovery as node_graph belongs to node1 anyway. This is just to test the API itself. + auto services_of_node1 = + node_graph()->get_service_names_and_types_by_node(node_name, absolute_namespace); + auto services_of_node2 = + node_graph()->get_service_names_and_types_by_node(node2_name, absolute_namespace); + + auto start = std::chrono::steady_clock::now(); + while (std::chrono::steady_clock::now() - start < std::chrono::seconds(3)) { + services_of_node1 = + node_graph()->get_service_names_and_types_by_node(node_name, absolute_namespace); + services_of_node2 = + node_graph()->get_service_names_and_types_by_node(node2_name, absolute_namespace); + if (services_of_node1.find("/ns/node1_service") != services_of_node1.end()) { + break; + } + } + + EXPECT_TRUE(services_of_node1.find("/ns/node1_service") != services_of_node1.end()); + EXPECT_FALSE(services_of_node2.find("/ns/node1_service") != services_of_node2.end()); +} + +TEST_F(TestNodeGraph, get_client_names_and_types_by_node) +{ + auto client = node()->create_client("node1_service"); + + const std::string node2_name = "node2"; + auto node2 = std::make_shared(node2_name, node_namespace); + + // rcl_get_client_names_and_types_by_node() expects the node to exist, otherwise it fails + EXPECT_THROW( + node_graph()->get_client_names_and_types_by_node("not_a_node", "not_absolute_namespace"), + rclcpp::exceptions::RCLError); + + // Check that node1_service exists for node1 but not node2. This shouldn't exercise graph + // discovery as node_graph belongs to node1 anyway. This is just to test the API itself. + auto services_of_node1 = + node_graph()->get_client_names_and_types_by_node(node_name, absolute_namespace); + auto services_of_node2 = + node_graph()->get_client_names_and_types_by_node(node2_name, absolute_namespace); + + auto start = std::chrono::steady_clock::now(); + while (std::chrono::steady_clock::now() - start < std::chrono::seconds(3)) { + services_of_node1 = + node_graph()->get_client_names_and_types_by_node(node_name, absolute_namespace); + services_of_node2 = + node_graph()->get_client_names_and_types_by_node(node2_name, absolute_namespace); + if (services_of_node1.find("/ns/node1_service") != services_of_node1.end()) { + break; + } + } + + EXPECT_TRUE(services_of_node1.find("/ns/node1_service") != services_of_node1.end()); + EXPECT_FALSE(services_of_node2.find("/ns/node1_service") != services_of_node2.end()); +} + +TEST_F(TestNodeGraph, get_service_names_and_types_by_node_rcl_errors) +{ + auto callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}; + auto service = + node()->create_service("node1_service", std::move(callback)); + + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_get_service_names_and_types_by_node, RCL_RET_ERROR); + auto mock_names_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_names_and_types_fini, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_graph()->get_service_names_and_types_by_node(node_name, node_namespace), + std::runtime_error( + "failed to get service names and types by node: error not set, failed also to cleanup" + " service names and types, leaking memory: error not set")); +} + + +TEST_F(TestNodeGraph, get_client_names_and_types_by_node_rcl_errors) +{ + auto client = node()->create_client("node1_service"); + + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_get_client_names_and_types_by_node, RCL_RET_ERROR); + auto mock_names_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_names_and_types_fini, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_graph()->get_client_names_and_types_by_node(node_name, node_namespace), + std::runtime_error( + "failed to get service names and types by node: error not set")); +} + +TEST_F(TestNodeGraph, get_service_names_and_types_by_node_names_and_types_fini_error) +{ + auto callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}; + auto service = + node()->create_service("node1_service", std::move(callback)); + auto mock_names_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_names_and_types_fini, RCL_RET_ERROR); + + EXPECT_THROW( + node_graph()->get_service_names_and_types_by_node(node_name, absolute_namespace), + rclcpp::exceptions::RCLError); +} + +TEST_F(TestNodeGraph, get_client_names_and_types_by_node_names_and_types_fini_error) +{ + auto client = node()->create_client("node1_service"); + auto mock_names_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_names_and_types_fini, RCL_RET_ERROR); + + EXPECT_NO_THROW( + node_graph()->get_client_names_and_types_by_node(node_name, absolute_namespace)); +} + +TEST_F(TestNodeGraph, get_publisher_names_and_types_by_node) +{ + const rclcpp::QoS publisher_qos(1); + auto publisher = node()->create_publisher("node1_topic", publisher_qos); + + const std::string node2_name = "node2"; + auto node2 = std::make_shared(node2_name, node_namespace); + + // rcl_get_publisher_names_and_types_by_node() expects the node to exist, otherwise it fails + EXPECT_THROW( + node_graph()->get_publisher_names_and_types_by_node("not_a_node", "not_absolute_namespace"), + rclcpp::exceptions::RCLError); + + // Check that node1_topic exists for node1 but not node2. This shouldn't exercise graph + // discovery as node_graph belongs to node1 anyway. This is just to test the API itself. + auto topics_of_node1 = + node_graph()->get_publisher_names_and_types_by_node(node_name, absolute_namespace); + auto topics_of_node2 = + node_graph()->get_publisher_names_and_types_by_node(node2_name, absolute_namespace); + + auto start = std::chrono::steady_clock::now(); + while (std::chrono::steady_clock::now() - start < std::chrono::seconds(3)) { + topics_of_node1 = + node_graph()->get_publisher_names_and_types_by_node(node_name, absolute_namespace); + topics_of_node2 = + node_graph()->get_publisher_names_and_types_by_node(node2_name, absolute_namespace); + if (topics_of_node1.find("/ns/node1_topic") != topics_of_node1.end()) { + break; + } + } + + EXPECT_TRUE(topics_of_node1.find("/ns/node1_topic") != topics_of_node1.end()); + EXPECT_FALSE(topics_of_node2.find("/ns/node1_topic") != topics_of_node2.end()); +} + +TEST_F(TestNodeGraph, get_subscriber_names_and_types_by_node) +{ + const rclcpp::QoS subscriber_qos(10); + auto callback = [](test_msgs::msg::Empty::ConstSharedPtr) {}; + auto subscription = + node()->create_subscription( + "node1_topic", subscriber_qos, std::move(callback)); + + const std::string node2_name = "node2"; + auto node2 = std::make_shared(node2_name, node_namespace); + + // rcl_get_subscriber_names_and_types_by_node() expects the node to exist, otherwise it fails + EXPECT_THROW( + node_graph()->get_subscriber_names_and_types_by_node("not_a_node", "not_absolute_namespace"), + rclcpp::exceptions::RCLError); + + // Check that node1_topic exists for node1 but not node2. This shouldn't exercise graph + // discovery as node_graph belongs to node1 anyway. This is just to test the API itself. + auto topics_of_node1 = + node_graph()->get_subscriber_names_and_types_by_node(node_name, absolute_namespace); + auto topics_of_node2 = + node_graph()->get_subscriber_names_and_types_by_node(node2_name, absolute_namespace); + + auto start = std::chrono::steady_clock::now(); + while (std::chrono::steady_clock::now() - start < std::chrono::seconds(3)) { + topics_of_node1 = + node_graph()->get_subscriber_names_and_types_by_node(node_name, absolute_namespace); + topics_of_node2 = + node_graph()->get_subscriber_names_and_types_by_node(node2_name, absolute_namespace); + if (topics_of_node1.find("/ns/node1_topic") != topics_of_node1.end()) { + break; + } + } + + EXPECT_TRUE(topics_of_node1.find("/ns/node1_topic") != topics_of_node1.end()); + EXPECT_FALSE(topics_of_node2.find("/ns/node1_topic") != topics_of_node2.end()); +} + +TEST_F(TestNodeGraph, get_publisher_names_and_types_by_node_rcl_errors) +{ + const rclcpp::QoS publisher_qos(1); + auto publisher = node()->create_publisher("topic", publisher_qos); + + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_get_publisher_names_and_types_by_node, RCL_RET_ERROR); + auto mock_names_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_names_and_types_fini, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_graph()->get_publisher_names_and_types_by_node(node_name, node_namespace), + std::runtime_error( + "failed to get topic names and types by node: error not set")); +} + + +TEST_F(TestNodeGraph, get_subscriber_names_and_types_by_node_rcl_errors) +{ + const rclcpp::QoS subscriber_qos(10); + auto callback = [](test_msgs::msg::Empty::ConstSharedPtr) {}; + auto subscription = + node()->create_subscription( + "topic", subscriber_qos, std::move(callback)); + + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_get_subscriber_names_and_types_by_node, RCL_RET_ERROR); + auto mock_names_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_names_and_types_fini, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_graph()->get_subscriber_names_and_types_by_node(node_name, node_namespace), + std::runtime_error( + "failed to get topic names and types by node: error not set")); +} + +TEST_F(TestNodeGraph, get_publisher_names_and_types_by_node_names_and_types_fini_error) +{ + const rclcpp::QoS publisher_qos(1); + auto publisher = node()->create_publisher("topic", publisher_qos); + + auto mock_names_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_names_and_types_fini, RCL_RET_ERROR); + + EXPECT_NO_THROW( + node_graph()->get_publisher_names_and_types_by_node(node_name, absolute_namespace)); +} + +TEST_F(TestNodeGraph, get_subscriber_names_and_types_by_node_names_and_types_fini_error) +{ + const rclcpp::QoS subscriber_qos(10); + auto callback = [](test_msgs::msg::Empty::ConstSharedPtr) {}; + auto subscription = + node()->create_subscription( + "topic", subscriber_qos, std::move(callback)); + + auto mock_names_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_names_and_types_fini, RCL_RET_ERROR); + + EXPECT_NO_THROW( + node_graph()->get_subscriber_names_and_types_by_node(node_name, absolute_namespace)); +} + +TEST_F(TestNodeGraph, get_node_names_and_namespaces) +{ + auto names_and_namespaces = node_graph()->get_node_names_and_namespaces(); + EXPECT_EQ(1u, names_and_namespaces.size()); +} + +TEST_F(TestNodeGraph, get_node_names_with_enclaves) +{ + auto names_namespaces_and_enclaves = + node_graph()->get_node_names_with_enclaves(); + EXPECT_EQ(1u, names_namespaces_and_enclaves.size()); +} + +TEST_F(TestNodeGraph, get_node_names_and_namespaces_rcl_errors) +{ + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_get_node_names, RCL_RET_ERROR); + auto mock_names_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcutils_string_array_fini, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_graph()->get_node_names_and_namespaces(), + std::runtime_error( + "failed to get node names: error not set, failed also to cleanup node names, leaking memory:" + " error not set, failed also to cleanup node namespaces, leaking memory: error not set")); +} + +TEST_F(TestNodeGraph, get_node_names_with_enclaves_rcl_errors) +{ + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_get_node_names_with_enclaves, RCL_RET_ERROR); + auto mock_names_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcutils_string_array_fini, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_graph()->get_node_names_with_enclaves(), + std::runtime_error( + "failed to get node names with enclaves: error not set, failed also to cleanup node names, " + "leaking memory: error not set, failed also to cleanup node namespaces, leaking memory: " + "error not set, failed also to cleanup node enclaves, leaking memory: error not set")); +} + +TEST_F(TestNodeGraph, get_node_names_and_namespaces_fini_errors) +{ + auto mock_names_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcutils_string_array_fini, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_graph()->get_node_names_and_namespaces(), + std::runtime_error("could not destroy node names, could not destroy node namespaces")); +} + +TEST_F(TestNodeGraph, get_node_names_with_enclaves_fini_errors) +{ + auto mock_names_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcutils_string_array_fini, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_graph()->get_node_names_with_enclaves(), + std::runtime_error( + "failed to finalize array, could not destroy node names, leaking memory: error not set" + ", could not destroy node namespaces, leaking memory: error not set" + ", could not destroy node enclaves, leaking memory: error not set")); +} + +TEST_F(TestNodeGraph, count_publishers_rcl_error) +{ + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_count_publishers, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_graph()->count_publishers("topic"), + std::runtime_error("could not count publishers: error not set")); +} + +TEST_F(TestNodeGraph, count_subscribers_rcl_error) +{ + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_count_subscribers, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_graph()->count_subscribers("topic"), + std::runtime_error("could not count subscribers: error not set")); +} + +TEST_F(TestNodeGraph, count_clients_rcl_error) +{ + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_count_clients, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_graph()->count_clients("service"), + std::runtime_error("could not count clients: error not set")); +} + +TEST_F(TestNodeGraph, count_services_rcl_error) +{ + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_count_services, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_graph()->count_services("service"), + std::runtime_error("could not count services: error not set")); +} + +TEST_F(TestNodeGraph, notify_shutdown) +{ + EXPECT_NO_THROW(node()->get_node_graph_interface()->notify_shutdown()); +} + +TEST_F(TestNodeGraph, wait_for_graph_change) +{ + auto node_graph_interface = node()->get_node_graph_interface(); + EXPECT_NO_THROW(node_graph_interface->notify_graph_change()); + EXPECT_THROW( + node_graph_interface->wait_for_graph_change(nullptr, std::chrono::milliseconds(1)), + rclcpp::exceptions::InvalidEventError); + + auto event = std::make_shared(); + EXPECT_THROW( + node_graph_interface->wait_for_graph_change(event, std::chrono::milliseconds(0)), + rclcpp::exceptions::EventNotRegisteredError); +} + +TEST_F(TestNodeGraph, notify_graph_change_rcl_error) +{ + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node()->get_node_graph_interface()->notify_graph_change(), + std::runtime_error("failed to notify wait set on graph change: error not set")); +} + +TEST_F(TestNodeGraph, get_info_by_topic) +{ + const rclcpp::QoS publisher_qos(1); + auto publisher = node()->create_publisher("topic", publisher_qos); + auto callback = [](test_msgs::msg::Empty::ConstSharedPtr) {}; + + const rclcpp::QoS subscriber_qos(10); + auto subscription = + node()->create_subscription( + "topic", subscriber_qos, std::move(callback)); + + EXPECT_EQ(0u, node_graph()->get_publishers_info_by_topic("topic", true).size()); + + std::vector publishers; + size_t num_publishers = get_num_graph_things( + [this, &publishers]() { + publishers = node_graph()->get_publishers_info_by_topic("topic", false); + return publishers.size(); + }); + ASSERT_EQ(1u, num_publishers); + + auto publisher_endpoint_info = publishers[0]; + const auto const_publisher_endpoint_info = publisher_endpoint_info; + EXPECT_STREQ(node_name, publisher_endpoint_info.node_name().c_str()); + EXPECT_STREQ(node_name, const_publisher_endpoint_info.node_name().c_str()); + EXPECT_STREQ(absolute_namespace, publisher_endpoint_info.node_namespace().c_str()); + EXPECT_STREQ(absolute_namespace, const_publisher_endpoint_info.node_namespace().c_str()); + EXPECT_STREQ("test_msgs/msg/Empty", publisher_endpoint_info.topic_type().c_str()); + EXPECT_STREQ("test_msgs/msg/Empty", const_publisher_endpoint_info.topic_type().c_str()); + EXPECT_EQ(rclcpp::EndpointType::Publisher, publisher_endpoint_info.endpoint_type()); + EXPECT_EQ(rclcpp::EndpointType::Publisher, const_publisher_endpoint_info.endpoint_type()); + + rclcpp::QoS actual_qos = publisher_endpoint_info.qos_profile(); + EXPECT_EQ(actual_qos.reliability(), rclcpp::ReliabilityPolicy::Reliable); + + rclcpp::QoS const_actual_qos = const_publisher_endpoint_info.qos_profile(); + EXPECT_EQ(const_actual_qos.reliability(), rclcpp::ReliabilityPolicy::Reliable); + + const rosidl_type_hash_t expected_type_hash = *test_msgs__msg__Empty__get_type_hash(nullptr); + EXPECT_EQ( + 0, memcmp( + &publisher_endpoint_info.topic_type_hash(), + &expected_type_hash, + sizeof(rosidl_type_hash_t))); + EXPECT_EQ( + 0, memcmp( + &const_publisher_endpoint_info.topic_type_hash(), + &expected_type_hash, + sizeof(rosidl_type_hash_t))); + + auto endpoint_gid = publisher_endpoint_info.endpoint_gid(); + auto const_endpoint_gid = const_publisher_endpoint_info.endpoint_gid(); + bool endpoint_gid_is_all_zeros = true; + for (size_t i = 0; i < RMW_GID_STORAGE_SIZE; ++i) { + endpoint_gid_is_all_zeros &= (endpoint_gid[i] == 0); + EXPECT_EQ(endpoint_gid[i], const_endpoint_gid[i]); + } + EXPECT_FALSE(endpoint_gid_is_all_zeros); +} + +TEST_F(TestNodeGraph, get_info_by_topic_rcl_node_get_options_error) +{ + const rclcpp::QoS publisher_qos(1); + auto publisher = node()->create_publisher("topic", publisher_qos); + + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_node_get_options, nullptr); + RCLCPP_EXPECT_THROW_EQ( + node_graph()->get_publishers_info_by_topic("topic", false), + std::runtime_error("Need valid node options in get_info_by_topic()")); +} + +// Required for mocking_utils below +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(TestNodeGraph, get_info_by_topic_rcl_remap_topic_name_error) +{ + const rclcpp::QoS publisher_qos(1); + auto publisher = node()->create_publisher("topic", publisher_qos); + + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_remap_topic_name, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_graph()->get_publishers_info_by_topic("topic", false), + std::runtime_error("Failed to remap topic name /ns/topic: error not set")); +} + +TEST_F(TestNodeGraph, get_info_by_topic_rcl_remap_topic_name_nullptr) +{ + const rclcpp::QoS publisher_qos(1); + auto publisher = node()->create_publisher("topic", publisher_qos); + + // Should be cleaned up by get_info_by_topic + char * some_string = rcutils_strdup("", rcl_get_default_allocator()); + ASSERT_NE(nullptr, some_string); + auto mock = + mocking_utils::patch( + "lib:rclcpp", rcl_remap_topic_name, [&some_string]( + const rcl_arguments_t *, const rcl_arguments_t *, const char *, const char *, const char *, + rcl_allocator_t, char ** output_name) + { + *output_name = some_string; + return RCL_RET_OK; + }); + EXPECT_NO_THROW(node_graph()->get_publishers_info_by_topic("topic", false)); +} + +TEST_F(TestNodeGraph, get_info_by_topic_rcl_errors) +{ + const rclcpp::QoS publisher_qos(1); + auto publisher = node()->create_publisher("topic", publisher_qos); + + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_get_publishers_info_by_topic, RCL_RET_ERROR); + auto mock_info_array_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_topic_endpoint_info_array_fini, RCL_RET_ERROR); + EXPECT_THROW( + node_graph()->get_publishers_info_by_topic("topic", false), + rclcpp::exceptions::RCLError); +} + +TEST_F(TestNodeGraph, get_info_by_topic_unsupported) +{ + const rclcpp::QoS publisher_qos(1); + auto publisher = node()->create_publisher("topic", publisher_qos); + + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_get_publishers_info_by_topic, RCL_RET_UNSUPPORTED); + EXPECT_THROW( + node_graph()->get_publishers_info_by_topic("topic", false), + rclcpp::exceptions::RCLError); +} + +TEST_F(TestNodeGraph, get_info_by_topic_endpoint_info_array_fini_error) +{ + const rclcpp::QoS publisher_qos(1); + auto publisher = node()->create_publisher("topic", publisher_qos); + + auto mock_info_array_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_topic_endpoint_info_array_fini, RCL_RET_ERROR); + EXPECT_THROW( + node_graph()->get_publishers_info_by_topic("topic", false), + rclcpp::exceptions::RCLError); +} diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_interfaces.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_interfaces.cpp new file mode 100644 index 0000000000..fd6d6173d6 --- /dev/null +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_interfaces.cpp @@ -0,0 +1,254 @@ +// 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 "rclcpp/node.hpp" +#include "rclcpp/node_interfaces/node_interfaces.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_graph_interface.hpp" + +class TestNodeInterfaces : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestNodeInterfaces, default_constructor) { + auto node = std::make_shared("my_node"); + using rclcpp::node_interfaces::NodeInterfaces; + using rclcpp::node_interfaces::NodeBaseInterface; + using rclcpp::node_interfaces::NodeGraphInterface; + NodeInterfaces interfaces; + interfaces = NodeInterfaces(*node); +} + +/* + Testing NodeInterfaces construction from nodes. + */ +TEST_F(TestNodeInterfaces, node_interfaces_nominal) { + auto node = std::make_shared("my_node"); + + // Create a NodeInterfaces for base and graph using a rclcpp::Node. + { + using rclcpp::node_interfaces::NodeInterfaces; + using rclcpp::node_interfaces::NodeBaseInterface; + using rclcpp::node_interfaces::NodeGraphInterface; + auto node_interfaces = NodeInterfaces(*node); + } + + // Implicit conversion of rclcpp::Node into function that uses NodeInterfaces of base. + { + using rclcpp::node_interfaces::NodeInterfaces; + using rclcpp::node_interfaces::NodeBaseInterface; + auto some_func = [](NodeInterfaces ni) { + auto base_interface = ni.get(); + }; + + some_func(*node); + } + + // Implicit narrowing of NodeInterfaces into a new interface NodeInterfaces with fewer interfaces. + { + using rclcpp::node_interfaces::NodeInterfaces; + using rclcpp::node_interfaces::NodeBaseInterface; + using rclcpp::node_interfaces::NodeGraphInterface; + auto some_func = [](NodeInterfaces ni_with_one) { + auto base_interface = ni_with_one.get(); + }; + + NodeInterfaces ni_with_two(*node); + + some_func(ni_with_two); + } + + // Create a NodeInterfaces via aggregation of interfaces in constructor. + { + using rclcpp::node_interfaces::NodeInterfaces; + using rclcpp::node_interfaces::NodeBaseInterface; + using rclcpp::node_interfaces::NodeGraphInterface; + auto loose_node_base = node->get_node_base_interface(); + auto loose_node_graph = node->get_node_graph_interface(); + auto ni = NodeInterfaces( + loose_node_base, + loose_node_graph); + } +} + +/* + Test construction with all standard rclcpp::node_interfaces::Node*Interfaces. + */ +TEST_F(TestNodeInterfaces, node_interfaces_standard_interfaces) { + auto node = std::make_shared("my_node", "/ns"); + + auto ni = rclcpp::node_interfaces::NodeInterfaces< + rclcpp::node_interfaces::NodeBaseInterface, + rclcpp::node_interfaces::NodeClockInterface, + rclcpp::node_interfaces::NodeGraphInterface, + rclcpp::node_interfaces::NodeLoggingInterface, + rclcpp::node_interfaces::NodeTimersInterface, + rclcpp::node_interfaces::NodeTopicsInterface, + rclcpp::node_interfaces::NodeServicesInterface, + rclcpp::node_interfaces::NodeWaitablesInterface, + rclcpp::node_interfaces::NodeParametersInterface, + rclcpp::node_interfaces::NodeTimeSourceInterface + >(*node); +} + +/* + Testing getters. + */ +TEST_F(TestNodeInterfaces, ni_init) { + auto node = std::make_shared("my_node", "/ns"); + + using rclcpp::node_interfaces::NodeInterfaces; + using rclcpp::node_interfaces::NodeBaseInterface; + using rclcpp::node_interfaces::NodeClockInterface; + using rclcpp::node_interfaces::NodeGraphInterface; + using rclcpp::node_interfaces::NodeLoggingInterface; + using rclcpp::node_interfaces::NodeTimersInterface; + using rclcpp::node_interfaces::NodeTopicsInterface; + using rclcpp::node_interfaces::NodeServicesInterface; + using rclcpp::node_interfaces::NodeWaitablesInterface; + using rclcpp::node_interfaces::NodeParametersInterface; + using rclcpp::node_interfaces::NodeTimeSourceInterface; + + auto ni = NodeInterfaces< + NodeBaseInterface, + NodeClockInterface, + NodeGraphInterface, + NodeLoggingInterface, + NodeTimersInterface, + NodeTopicsInterface, + NodeServicesInterface, + NodeWaitablesInterface, + NodeParametersInterface, + NodeTimeSourceInterface + >(*node); + + { + auto base = ni.get(); + base = ni.get_node_base_interface(); + EXPECT_STREQ(base->get_name(), "my_node"); // Test for functionality + } + { + auto clock = ni.get(); + clock = ni.get_node_clock_interface(); + clock->get_clock(); + } + { + auto graph = ni.get(); + graph = ni.get_node_graph_interface(); + } + { + auto logging = ni.get(); + logging = ni.get_node_logging_interface(); + } + { + auto timers = ni.get(); + timers = ni.get_node_timers_interface(); + } + { + auto topics = ni.get(); + topics = ni.get_node_topics_interface(); + } + { + auto services = ni.get(); + services = ni.get_node_services_interface(); + } + { + auto waitables = ni.get(); + waitables = ni.get_node_waitables_interface(); + } + { + auto parameters = ni.get(); + parameters = ni.get_node_parameters_interface(); + } + { + auto time_source = ni.get(); + time_source = ni.get_node_time_source_interface(); + } +} + +/* + Testing macro'ed getters. + */ +TEST_F(TestNodeInterfaces, ni_all_init) { + auto node = std::make_shared("my_node", "/ns"); + + using rclcpp::node_interfaces::NodeInterfaces; + using rclcpp::node_interfaces::NodeBaseInterface; + using rclcpp::node_interfaces::NodeClockInterface; + using rclcpp::node_interfaces::NodeGraphInterface; + using rclcpp::node_interfaces::NodeLoggingInterface; + using rclcpp::node_interfaces::NodeTimersInterface; + using rclcpp::node_interfaces::NodeTopicsInterface; + using rclcpp::node_interfaces::NodeServicesInterface; + using rclcpp::node_interfaces::NodeWaitablesInterface; + using rclcpp::node_interfaces::NodeParametersInterface; + using rclcpp::node_interfaces::NodeTimeSourceInterface; + + auto ni = rclcpp::node_interfaces::NodeInterfaces(*node); + + { + auto base = ni.get(); + base = ni.get_node_base_interface(); + EXPECT_STREQ(base->get_name(), "my_node"); // Test for functionality + } + { + auto clock = ni.get(); + clock = ni.get_node_clock_interface(); + clock->get_clock(); + } + { + auto graph = ni.get(); + graph = ni.get_node_graph_interface(); + } + { + auto logging = ni.get(); + logging = ni.get_node_logging_interface(); + } + { + auto timers = ni.get(); + timers = ni.get_node_timers_interface(); + } + { + auto topics = ni.get(); + topics = ni.get_node_topics_interface(); + } + { + auto services = ni.get(); + services = ni.get_node_services_interface(); + } + { + auto waitables = ni.get(); + waitables = ni.get_node_waitables_interface(); + } + { + auto parameters = ni.get(); + parameters = ni.get_node_parameters_interface(); + } + { + auto time_source = ni.get(); + time_source = ni.get_node_time_source_interface(); + } +} diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp new file mode 100644 index 0000000000..0cc06792a8 --- /dev/null +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp @@ -0,0 +1,457 @@ +// 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. + +/** + * NodeParameters is a complicated interface with lots of code, but it is tested elsewhere + * very thoroughly. This currently just includes unittests for the currently uncovered + * functionality. + */ + +#include + +#include +#include +#include +#include +#include + +#include "rclcpp/node.hpp" +#include "rclcpp/node_interfaces/node_parameters.hpp" + +#include "../../mocking_utils/patch.hpp" +#include "../../utils/rclcpp_gtest_macros.hpp" + +#include "rcpputils/filesystem_helper.hpp" + +class TestNodeParameters : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + rclcpp::NodeOptions options; + options.allow_undeclared_parameters(true); + node = std::make_shared("node", "ns", options); + + // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure + // the proper type is being tested and covered. + node_parameters = + dynamic_cast( + node->get_node_parameters_interface().get()); + ASSERT_NE(nullptr, node_parameters); + test_resources_path /= "test_node_parameters"; + } + + void TearDown() + { + rclcpp::shutdown(); + } + +protected: + std::shared_ptr node; + rclcpp::node_interfaces::NodeParameters * node_parameters; + + std::filesystem::path test_resources_path{TEST_RESOURCES_DIRECTORY}; +}; + +TEST_F(TestNodeParameters, construct_destruct_rcl_errors) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_arguments_get_param_overrides, RCL_RET_ERROR); + EXPECT_THROW( + std::make_shared("node2", "ns").reset(), + rclcpp::exceptions::RCLError); +} + +TEST_F(TestNodeParameters, list_parameters) +{ + std::vector prefixes; + const auto list_result = node_parameters->list_parameters(prefixes, 1u); + + // Currently the default parameters are 'use_sim_time' and 'start_type_description_service' + size_t number_of_parameters = list_result.names.size(); + EXPECT_GE(2u, number_of_parameters); + + const std::string parameter_name = "new_parameter"; + const rclcpp::ParameterValue value(true); + const rcl_interfaces::msg::ParameterDescriptor descriptor; + const auto added_parameter_value = + node_parameters->declare_parameter(parameter_name, value, descriptor, false); + EXPECT_EQ(value.get(), added_parameter_value.get()); + + auto list_result2 = node_parameters->list_parameters(prefixes, 1u); + EXPECT_EQ(number_of_parameters + 1u, list_result2.names.size()); + + EXPECT_NE( + std::find(list_result2.names.begin(), list_result2.names.end(), parameter_name), + list_result2.names.end()); + + // Check prefixes and the depth relative to the given prefixes + const std::string parameter_name2 = "prefix.new_parameter"; + const rclcpp::ParameterValue value2(true); + const rcl_interfaces::msg::ParameterDescriptor descriptor2; + const auto added_parameter_value2 = + node_parameters->declare_parameter(parameter_name2, value2, descriptor2, false); + EXPECT_EQ(value2.get(), added_parameter_value2.get()); + prefixes = {"prefix"}; + auto list_result3 = node_parameters->list_parameters(prefixes, 1u); + EXPECT_EQ(1u, list_result3.names.size()); + EXPECT_NE( + std::find(list_result3.names.begin(), list_result3.names.end(), parameter_name2), + list_result3.names.end()); + + // Check if prefix equals parameter name + prefixes = {"new_parameter"}; + auto list_result4 = node_parameters->list_parameters(prefixes, 2u); + EXPECT_EQ(1u, list_result4.names.size()); + EXPECT_NE( + std::find(list_result4.names.begin(), list_result4.names.end(), parameter_name), + list_result4.names.end()); + + // Return all parameters when the depth = 0 + auto list_result5 = node_parameters->list_parameters(prefixes, 0u); + EXPECT_EQ(1u, list_result5.names.size()); + EXPECT_NE( + std::find(list_result5.names.begin(), list_result5.names.end(), parameter_name), + list_result5.names.end()); +} + +TEST_F(TestNodeParameters, parameter_overrides) +{ + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + node_options.append_parameter_override("param1", true); + node_options.append_parameter_override("param2", 42); + + std::shared_ptr node2 = std::make_shared("node2", "ns", node_options); + + auto * node_parameters_interface = + dynamic_cast( + node2->get_node_parameters_interface().get()); + ASSERT_NE(nullptr, node_parameters_interface); + + const auto & parameter_overrides = node_parameters_interface->get_parameter_overrides(); + EXPECT_EQ(2u, parameter_overrides.size()); +} + +TEST_F(TestNodeParameters, set_parameters) { + rclcpp::NodeOptions node_options; + node_options.allow_undeclared_parameters(true); + + rcl_interfaces::msg::ParameterDescriptor bool_descriptor; + bool_descriptor.name = "bool_parameter"; + bool_descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + bool_descriptor.read_only = false; + node_parameters->declare_parameter( + "bool_parameter", rclcpp::ParameterValue(false), bool_descriptor, false); + + rcl_interfaces::msg::ParameterDescriptor read_only_descriptor; + read_only_descriptor.name = "read_only_parameter"; + read_only_descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + read_only_descriptor.read_only = true; + node_parameters->declare_parameter( + "read_only_parameter", rclcpp::ParameterValue(42), read_only_descriptor, false); + + const std::vector parameters = { + rclcpp::Parameter("bool_parameter", true), + rclcpp::Parameter("read_only_parameter", 42), + }; + auto result = node_parameters->set_parameters(parameters); + ASSERT_EQ(parameters.size(), result.size()); + EXPECT_TRUE(result[0].successful); + EXPECT_FALSE(result[1].successful); + EXPECT_STREQ( + "parameter 'read_only_parameter' cannot be set because it is read-only", + result[1].reason.c_str()); + + RCLCPP_EXPECT_THROW_EQ( + node_parameters->set_parameters({rclcpp::Parameter("", true)}), + rclcpp::exceptions::InvalidParametersException("parameter name must not be empty")); + + result = node_parameters->set_parameters({rclcpp::Parameter("undeclared_parameter", 3.14159)}); + ASSERT_EQ(1u, result.size()); + EXPECT_TRUE(result[0].successful); +} + +TEST_F(TestNodeParameters, add_remove_on_set_parameters_callback) { + rcl_interfaces::msg::ParameterDescriptor bool_descriptor; + bool_descriptor.name = "bool_parameter"; + bool_descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + bool_descriptor.read_only = false; + node_parameters->declare_parameter( + "bool_parameter", rclcpp::ParameterValue(false), bool_descriptor, false); + const std::vector parameters = {rclcpp::Parameter("bool_parameter", true)}; + + const std::string reason = "some totally not made up reason"; + auto callback = [reason](const std::vector &) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = false; + result.reason = reason; + return result; + }; + + auto handle = node_parameters->add_on_set_parameters_callback(callback); + auto result = node_parameters->set_parameters(parameters); + ASSERT_EQ(1u, result.size()); + EXPECT_FALSE(result[0].successful); + EXPECT_EQ(reason, result[0].reason); + + EXPECT_NO_THROW(node_parameters->remove_on_set_parameters_callback(handle.get())); + + RCLCPP_EXPECT_THROW_EQ( + node_parameters->remove_on_set_parameters_callback(handle.get()), + std::runtime_error("On set parameter callback doesn't exist")); +} + +TEST_F(TestNodeParameters, add_remove_pre_set_parameters_callback) { + // `add_pre_set_parameters_callback` used to modify parameters list. + auto modify_parameter_list_callback = [](std::vector & parameters) { + for (const auto & param : parameters) { + if (param.get_name() == "param1") { + parameters.emplace_back("param2", 2.0); + } + } + }; + + // `add_pre_set_parameters_callback` used to make the parameters list empty. + auto empty_parameter_list_callback = [](std::vector & parameters) { + parameters = {}; + }; + + auto handle1 = + node_parameters->add_pre_set_parameters_callback(modify_parameter_list_callback); + + double default_value = 0.0; + node_parameters->declare_parameter( + "param1", rclcpp::ParameterValue(default_value)); + node_parameters->declare_parameter( + "param2", rclcpp::ParameterValue(default_value)); + + // verify that `declare_parameter` does not call any of the callbacks registered with + // `add_pre_set_parameters_callback` + EXPECT_TRUE(node_parameters->has_parameter("param1")); + EXPECT_EQ(node_parameters->get_parameter("param1").get_value(), default_value); + EXPECT_TRUE(node_parameters->has_parameter("param2")); + EXPECT_EQ(node_parameters->get_parameter("param2").get_value(), default_value); + + // verify that the `param2` was set successfully conditioned on setting of + // `param1` + const std::vector parameters_to_be_set = { + rclcpp::Parameter("param1", 1.0)}; + auto result = node_parameters->set_parameters(parameters_to_be_set); + // we expect the result size to be same as the original "parameters_to_be_set" + // since the pre set parameter callback will set the modified param list atomically. + ASSERT_EQ(1u, result.size()); + EXPECT_TRUE(result[0].successful); + EXPECT_TRUE(node_parameters->has_parameter("param1")); + EXPECT_EQ(node_parameters->get_parameter("param1").get_value(), 1.0); + EXPECT_TRUE(node_parameters->has_parameter("param2")); + EXPECT_EQ(node_parameters->get_parameter("param2").get_value(), 2.0); + EXPECT_NO_THROW(node_parameters->remove_pre_set_parameters_callback(handle1.get())); + RCLCPP_EXPECT_THROW_EQ( + node_parameters->remove_pre_set_parameters_callback(handle1.get()), + std::runtime_error("Pre set parameter callback doesn't exist")); + + // verify that the result should be unsuccessful if the pre set callback makes + // parameter list empty + auto handle2 = + node_parameters->add_pre_set_parameters_callback(empty_parameter_list_callback); + auto results = node_parameters->set_parameters(parameters_to_be_set); + + std::string reason = "parameter list cannot be empty, this might be due to " + "pre_set_parameters_callback modifying the original parameters list."; + EXPECT_FALSE(results[0].successful); + EXPECT_EQ(results[0].reason, reason); + EXPECT_NO_THROW(node_parameters->remove_pre_set_parameters_callback(handle2.get())); + RCLCPP_EXPECT_THROW_EQ( + node_parameters->remove_pre_set_parameters_callback(handle2.get()), + std::runtime_error("Pre set parameter callback doesn't exist")); +} + +TEST_F(TestNodeParameters, add_remove_post_set_parameters_callback) { + rcl_interfaces::msg::ParameterDescriptor param1_descriptor; + param1_descriptor.name = "double_parameter1"; + param1_descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + param1_descriptor.read_only = false; + + rcl_interfaces::msg::ParameterDescriptor param2_descriptor; + param2_descriptor.name = "double_parameter2"; + param2_descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + param2_descriptor.read_only = false; + + double variable_tracking_param1_internally = node_parameters->declare_parameter( + "param1", rclcpp::ParameterValue(0.0), param1_descriptor, false).get(); + double variable_tracking_param2_internally = node_parameters->declare_parameter( + "param2", rclcpp::ParameterValue(0.0), param2_descriptor, false).get(); + + EXPECT_EQ(variable_tracking_param1_internally, 0.0); + EXPECT_EQ(variable_tracking_param2_internally, 0.0); + + const std::vector parameters_to_be_set = { + rclcpp::Parameter("param1", 1.0), + rclcpp::Parameter("param2", 2.0)}; + + // register a callback for successful set parameter and change the internally tracked variables. + auto callback = [&](const std::vector & parameters) { + for (const auto & param : parameters) { + if (param.get_name() == "param1") { + variable_tracking_param1_internally = param.get_value(); + } else if (param.get_name() == "param2") { + variable_tracking_param2_internally = param.get_value(); + } + } + }; + + auto handle = node_parameters->add_post_set_parameters_callback(callback); + auto result = node_parameters->set_parameters(parameters_to_be_set); + ASSERT_EQ(2u, result.size()); + EXPECT_TRUE(result[0].successful); + EXPECT_TRUE(result[1].successful); + EXPECT_TRUE(node_parameters->has_parameter("param1")); + EXPECT_TRUE(node_parameters->has_parameter("param2")); + EXPECT_EQ(variable_tracking_param1_internally, 1.0); + EXPECT_EQ(variable_tracking_param2_internally, 2.0); + + EXPECT_NO_THROW(node_parameters->remove_post_set_parameters_callback(handle.get())); + + RCLCPP_EXPECT_THROW_EQ( + node_parameters->remove_post_set_parameters_callback(handle.get()), + std::runtime_error("Post set parameter callback doesn't exist")); +} + +TEST_F(TestNodeParameters, wildcard_with_namespace) +{ + rclcpp::NodeOptions opts; + opts.arguments( + { + "--ros-args", + "--params-file", (test_resources_path / "wildcards.yaml").string() + }); + + std::shared_ptr node = std::make_shared("node2", "ns", opts); + + auto * node_parameters = + dynamic_cast( + node->get_node_parameters_interface().get()); + ASSERT_NE(nullptr, node_parameters); + + const auto & parameter_overrides = node_parameters->get_parameter_overrides(); + EXPECT_EQ(7u, parameter_overrides.size()); + EXPECT_EQ(parameter_overrides.at("full_wild").get(), "full_wild"); + EXPECT_EQ(parameter_overrides.at("namespace_wild").get(), "namespace_wild"); + EXPECT_EQ( + parameter_overrides.at("namespace_wild_another").get(), + "namespace_wild_another"); + EXPECT_EQ( + parameter_overrides.at("namespace_wild_one_star").get(), + "namespace_wild_one_star"); + EXPECT_EQ(parameter_overrides.at("node_wild_in_ns").get(), "node_wild_in_ns"); + EXPECT_EQ( + parameter_overrides.at("node_wild_in_ns_another").get(), + "node_wild_in_ns_another"); + EXPECT_EQ(parameter_overrides.at("explicit_in_ns").get(), "explicit_in_ns"); + EXPECT_EQ(parameter_overrides.count("should_not_appear"), 0u); +} + +TEST_F(TestNodeParameters, wildcard_no_namespace) +{ + rclcpp::NodeOptions opts; + opts.arguments( + { + "--ros-args", + "--params-file", (test_resources_path / "wildcards.yaml").string() + }); + + std::shared_ptr node = std::make_shared("node2", opts); + + auto * node_parameters = + dynamic_cast( + node->get_node_parameters_interface().get()); + ASSERT_NE(nullptr, node_parameters); + + const auto & parameter_overrides = node_parameters->get_parameter_overrides(); + EXPECT_EQ(5u, parameter_overrides.size()); + EXPECT_EQ(parameter_overrides.at("full_wild").get(), "full_wild"); + EXPECT_EQ(parameter_overrides.at("namespace_wild").get(), "namespace_wild"); + EXPECT_EQ( + parameter_overrides.at("namespace_wild_another").get(), + "namespace_wild_another"); + EXPECT_EQ(parameter_overrides.at("node_wild_no_ns").get(), "node_wild_no_ns"); + EXPECT_EQ(parameter_overrides.at("explicit_no_ns").get(), "explicit_no_ns"); + EXPECT_EQ(parameter_overrides.count("should_not_appear"), 0u); + // "/*" match exactly one token, not expect to get `namespace_wild_one_star` + EXPECT_EQ(parameter_overrides.count("namespace_wild_one_star"), 0u); +} + +TEST_F(TestNodeParameters, params_by_order) +{ + rclcpp::NodeOptions opts; + opts.arguments( + { + "--ros-args", + "--params-file", (test_resources_path / "params_by_order.yaml").string() + }); + + std::shared_ptr node = std::make_shared("node2", "ns", opts); + + auto * node_parameters = + dynamic_cast( + node->get_node_parameters_interface().get()); + ASSERT_NE(nullptr, node_parameters); + + const auto & parameter_overrides = node_parameters->get_parameter_overrides(); + EXPECT_EQ(3u, parameter_overrides.size()); + EXPECT_EQ(parameter_overrides.at("a_value").get(), "last_one_win"); + EXPECT_EQ(parameter_overrides.at("foo").get(), "foo"); + EXPECT_EQ(parameter_overrides.at("bar").get(), "bar"); +} + +TEST_F(TestNodeParameters, complicated_wildcards) +{ + rclcpp::NodeOptions opts; + opts.arguments( + { + "--ros-args", + "--params-file", (test_resources_path / "complicated_wildcards.yaml").string() + }); + + { + // regex matched: /**/foo/*/bar + std::shared_ptr node = + std::make_shared("node2", "/a/b/c/foo/d/bar", opts); + + auto * node_parameters = + dynamic_cast( + node->get_node_parameters_interface().get()); + ASSERT_NE(nullptr, node_parameters); + + const auto & parameter_overrides = node_parameters->get_parameter_overrides(); + EXPECT_EQ(2u, parameter_overrides.size()); + EXPECT_EQ(parameter_overrides.at("foo").get(), "foo"); + EXPECT_EQ(parameter_overrides.at("bar").get(), "bar"); + } + + { + // regex not matched: /**/foo/*/bar + std::shared_ptr node = + std::make_shared("node2", "/a/b/c/foo/bar", opts); + + auto * node_parameters = + dynamic_cast( + node->get_node_parameters_interface().get()); + ASSERT_NE(nullptr, node_parameters); + + const auto & parameter_overrides = node_parameters->get_parameter_overrides(); + EXPECT_EQ(0u, parameter_overrides.size()); + } +} diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp new file mode 100644 index 0000000000..7b00fea972 --- /dev/null +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp @@ -0,0 +1,144 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include + +#include "rcl/node_options.h" +#include "rclcpp/node.hpp" +#include "rclcpp/node_interfaces/node_services.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "../../mocking_utils/patch.hpp" +#include "../../utils/rclcpp_gtest_macros.hpp" + +class TestService : public rclcpp::ServiceBase +{ +public: + explicit TestService(rclcpp::Node * node) + : rclcpp::ServiceBase(node->get_node_base_interface()->get_shared_rcl_node_handle()) {} + + std::shared_ptr create_request() override {return nullptr;} + std::shared_ptr create_request_header() override {return nullptr;} + void handle_request(std::shared_ptr, std::shared_ptr) override {} +}; + +class TestClient : public rclcpp::ClientBase +{ +public: + explicit TestClient(rclcpp::Node * node) + : rclcpp::ClientBase(node->get_node_base_interface().get(), node->get_node_graph_interface()) {} + + std::shared_ptr create_response() override {return nullptr;} + std::shared_ptr create_request_header() override {return nullptr;} + void handle_response( + std::shared_ptr, std::shared_ptr) override {} +}; + +class TestNodeService : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + + rclcpp::NodeOptions options{}; + options.arguments(std::vector{"-r", "foo:=bar"}); + node = std::make_shared("node", "ns", options); + + // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure + // the proper type is being tested and covered. + node_services = + dynamic_cast( + node->get_node_services_interface().get()); + ASSERT_NE(nullptr, node_services); + } + + void TearDown() + { + rclcpp::shutdown(); + } + +protected: + std::shared_ptr node; + rclcpp::node_interfaces::NodeServices * node_services; +}; + +TEST_F(TestNodeService, add_service) +{ + auto service = std::make_shared(node.get()); + auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + EXPECT_NO_THROW( + node_services->add_service(service, callback_group)); + + // Check that adding a service from node to a callback group of different_node throws exception. + std::shared_ptr different_node = std::make_shared("node2", "ns"); + + auto callback_group_in_different_node = + different_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + RCLCPP_EXPECT_THROW_EQ( + node_services->add_service(service, callback_group_in_different_node), + rclcpp::exceptions::MissingGroupNodeException("service")); +} + +TEST_F(TestNodeService, add_service_rcl_trigger_guard_condition_error) +{ + auto service = std::make_shared(node.get()); + auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_services->add_service(service, callback_group), + std::runtime_error("failed to notify wait set on service creation: error not set")); +} + +TEST_F(TestNodeService, add_client) +{ + auto client = std::make_shared(node.get()); + auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + EXPECT_NO_THROW(node_services->add_client(client, callback_group)); + + // Check that adding a client from node to a callback group of different_node throws exception. + std::shared_ptr different_node = std::make_shared("node2", "ns"); + + auto callback_group_in_different_node = + different_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + RCLCPP_EXPECT_THROW_EQ( + node_services->add_client(client, callback_group_in_different_node), + rclcpp::exceptions::MissingGroupNodeException("client")); +} + +TEST_F(TestNodeService, add_client_rcl_trigger_guard_condition_error) +{ + auto client = std::make_shared(node.get()); + auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_services->add_client(client, callback_group), + std::runtime_error("failed to notify wait set on client creation: error not set")); +} + +TEST_F(TestNodeService, resolve_service_name) +{ + EXPECT_EQ("/ns/bar", node_services->resolve_service_name("foo", false)); + EXPECT_EQ("/ns/foo", node_services->resolve_service_name("foo", true)); + EXPECT_EQ("/foo", node_services->resolve_service_name("/foo", true)); + EXPECT_THROW( + node_services->resolve_service_name("this is not a valid name!~>", true), + rclcpp::exceptions::RCLError); +} diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp new file mode 100644 index 0000000000..ce6343fb5c --- /dev/null +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp @@ -0,0 +1,91 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +#include "rcl/node_options.h" +#include "rclcpp/node.hpp" +#include "rclcpp/node_interfaces/node_timers.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "../../mocking_utils/patch.hpp" +#include "../../utils/rclcpp_gtest_macros.hpp" + +class TestTimer : public rclcpp::TimerBase +{ +public: + explicit TestTimer(rclcpp::Node * node) + : TimerBase(node->get_clock(), std::chrono::nanoseconds(1), + node->get_node_base_interface()->get_context()) {} + + std::shared_ptr call() override {return nullptr;} + void execute_callback(const std::shared_ptr &) override {} + bool is_steady() override {return false;} +}; + +class TestNodeTimers : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + node = std::make_shared("node", "ns"); + + // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure + // the proper type is being tested and covered. + node_timers = + dynamic_cast(node->get_node_timers_interface().get()); + ASSERT_NE(nullptr, node_timers); + } + + void TearDown() + { + rclcpp::shutdown(); + } + +protected: + std::shared_ptr node; + rclcpp::node_interfaces::NodeTimers * node_timers; +}; + +TEST_F(TestNodeTimers, add_timer) +{ + auto timer = std::make_shared(node.get()); + auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + EXPECT_NO_THROW(node_timers->add_timer(timer, callback_group)); + + // Check that adding timer from node to callback group in different_node throws exception. + std::shared_ptr different_node = std::make_shared("node2", "ns"); + + auto callback_group_in_different_node = + different_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + RCLCPP_EXPECT_THROW_EQ( + node_timers->add_timer(timer, callback_group_in_different_node), + rclcpp::exceptions::MissingGroupNodeException("timer")); +} + +TEST_F(TestNodeTimers, add_timer_rcl_trigger_guard_condition_error) +{ + auto timer = std::make_shared(node.get()); + auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_timers->add_timer(timer, callback_group), + std::runtime_error("failed to notify wait set on timer creation: error not set")); +} diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp new file mode 100644 index 0000000000..e8f873f693 --- /dev/null +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp @@ -0,0 +1,190 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include + +#include "rcl/node_options.h" +#include "rclcpp/node.hpp" +#include "rclcpp/node_interfaces/node_topics.hpp" +#include "rclcpp/rclcpp.hpp" +#include "test_msgs/msg/empty.hpp" + +#include "../../mocking_utils/patch.hpp" +#include "../../utils/rclcpp_gtest_macros.hpp" + +using rclcpp::dynamic_typesupport::DynamicMessage; +using rclcpp::dynamic_typesupport::DynamicMessageType; +using rclcpp::dynamic_typesupport::DynamicSerializationSupport; + + +namespace +{ + +const rosidl_message_type_support_t EmptyTypeSupport() +{ + return *rosidl_typesupport_cpp::get_message_type_support_handle(); +} + +const rclcpp::PublisherOptionsWithAllocator> PublisherOptions() +{ + return rclcpp::PublisherOptionsWithAllocator>(); +} + +const rclcpp::SubscriptionOptionsWithAllocator> SubscriptionOptions() +{ + return rclcpp::SubscriptionOptionsWithAllocator>(); +} + +} // namespace + +class TestPublisher : public rclcpp::PublisherBase +{ +public: + explicit TestPublisher(rclcpp::Node * node) + : rclcpp::PublisherBase( + node->get_node_base_interface().get(), "topic", EmptyTypeSupport(), + PublisherOptions().to_rcl_publisher_options(rclcpp::QoS(10)), + PublisherOptions().event_callbacks, PublisherOptions().use_default_callbacks) {} +}; + +class TestSubscription : public rclcpp::SubscriptionBase +{ +public: + explicit TestSubscription(rclcpp::Node * node) + : rclcpp::SubscriptionBase( + node->get_node_base_interface().get(), EmptyTypeSupport(), "topic", + SubscriptionOptions().to_rcl_subscription_options(rclcpp::QoS(10)), + SubscriptionOptions().event_callbacks, SubscriptionOptions().use_default_callbacks) {} + std::shared_ptr create_message() override {return nullptr;} + + std::shared_ptr + create_serialized_message() override {return nullptr;} + + void handle_message(std::shared_ptr &, const rclcpp::MessageInfo &) override {} + void handle_loaned_message(void *, const rclcpp::MessageInfo &) override {} + void handle_serialized_message( + const std::shared_ptr &, const rclcpp::MessageInfo &) override {} + void return_message(std::shared_ptr &) override {} + void return_serialized_message(std::shared_ptr &) override {} + + DynamicMessageType::SharedPtr get_shared_dynamic_message_type() override {return nullptr;} + DynamicMessage::SharedPtr get_shared_dynamic_message() override {return nullptr;} + DynamicSerializationSupport::SharedPtr get_shared_dynamic_serialization_support() override + { + return nullptr; + } + DynamicMessage::SharedPtr create_dynamic_message() override {return nullptr;} + void return_dynamic_message(DynamicMessage::SharedPtr &) override {} + void handle_dynamic_message( + const DynamicMessage::SharedPtr &, + const rclcpp::MessageInfo &) override {} +}; + +class TestNodeTopics : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + rclcpp::NodeOptions options{}; + options.arguments(std::vector{"-r", "foo:=bar"}); + node = std::make_shared("node", "ns", options); + + // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure + // the proper type is being tested and covered. + node_topics = + dynamic_cast(node->get_node_topics_interface().get()); + ASSERT_NE(nullptr, node_topics); + } + + void TearDown() + { + rclcpp::shutdown(); + } + +protected: + std::shared_ptr node; + rclcpp::node_interfaces::NodeTopics * node_topics; +}; + +TEST_F(TestNodeTopics, add_publisher) +{ + auto publisher = std::make_shared(node.get()); + auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + EXPECT_NO_THROW(node_topics->add_publisher(publisher, callback_group)); + + // Check that adding publisher from node to a callback group in different_node throws exception. + std::shared_ptr different_node = std::make_shared("node2", "ns"); + + auto callback_group_in_different_node = + different_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + EXPECT_THROW( + node_topics->add_publisher(publisher, callback_group_in_different_node), + std::runtime_error); +} + +TEST_F(TestNodeTopics, add_publisher_rcl_trigger_guard_condition_error) +{ + auto publisher = std::make_shared(node.get()); + auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_topics->add_publisher(publisher, callback_group), + std::runtime_error("failed to notify wait set on publisher creation: error not set")); +} + +TEST_F(TestNodeTopics, add_subscription) +{ + auto subscription = std::make_shared(node.get()); + auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + EXPECT_NO_THROW(node_topics->add_subscription(subscription, callback_group)); + + // Check that adding subscription from node to callback group in different_node throws exception. + std::shared_ptr different_node = std::make_shared("node2", "ns"); + + auto callback_group_in_different_node = + different_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + EXPECT_THROW( + node_topics->add_subscription(subscription, callback_group_in_different_node), + std::runtime_error); +} + +TEST_F(TestNodeTopics, add_subscription_rcl_trigger_guard_condition_error) +{ + auto subscription = std::make_shared(node.get()); + auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_topics->add_subscription(subscription, callback_group), + std::runtime_error("failed to notify wait set on subscription creation: error not set")); +} + +TEST_F(TestNodeTopics, resolve_topic_name) +{ + EXPECT_EQ("/ns/bar", node_topics->resolve_topic_name("foo", false)); + EXPECT_EQ("/ns/foo", node_topics->resolve_topic_name("foo", true)); + EXPECT_EQ("/foo", node_topics->resolve_topic_name("/foo", true)); + EXPECT_THROW( + node_topics->resolve_topic_name("this is not a valid name!~>", true), + rclcpp::exceptions::RCLError); +} diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_type_descriptions.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_type_descriptions.cpp new file mode 100644 index 0000000000..1a4603528a --- /dev/null +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_type_descriptions.cpp @@ -0,0 +1,61 @@ +// Copyright 2023 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 "rclcpp/node.hpp" +#include "rclcpp/node_interfaces/node_type_descriptions.hpp" + +class TestNodeTypeDescriptions : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + } + + void TearDown() + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestNodeTypeDescriptions, interface_created) +{ + rclcpp::Node node{"node", "ns"}; + ASSERT_NE(nullptr, node.get_node_type_descriptions_interface()); +} + +TEST_F(TestNodeTypeDescriptions, disabled_no_service) +{ + rclcpp::NodeOptions node_options; + node_options.append_parameter_override("start_type_description_service", false); + rclcpp::Node node{"node", "ns", node_options}; + + auto services = node.get_node_graph_interface()->get_service_names_and_types_by_node( + "node", "/ns"); + EXPECT_TRUE(services.find("/ns/node/get_type_description") == services.end()); +} + +TEST_F(TestNodeTypeDescriptions, enabled_creates_service) +{ + rclcpp::NodeOptions node_options; + node_options.append_parameter_override("start_type_description_service", true); + rclcpp::Node node{"node", "ns", node_options}; + + auto services = node.get_node_graph_interface()->get_service_names_and_types_by_node( + "node", "/ns"); + + EXPECT_TRUE(services.find("/ns/node/get_type_description") != services.end()); +} diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp new file mode 100644 index 0000000000..2b6b1092be --- /dev/null +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp @@ -0,0 +1,97 @@ +// 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 "rcl/node_options.h" +#include "rclcpp/node.hpp" +#include "rclcpp/node_interfaces/node_waitables.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "../../mocking_utils/patch.hpp" +#include "../../utils/rclcpp_gtest_macros.hpp" + +class TestWaitable : public rclcpp::Waitable +{ +public: + void add_to_wait_set(rcl_wait_set_t &) override {} + bool is_ready(const rcl_wait_set_t &) override {return false;} + + std::shared_ptr take_data() override {return nullptr;} + void execute(const std::shared_ptr &) override {} + + void set_on_ready_callback(std::function) override {} + void clear_on_ready_callback() override {} + + std::shared_ptr take_data_by_entity_id(size_t) override {return nullptr;} +}; + +class TestNodeWaitables : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + node = std::make_shared("node", "ns"); + + node_waitables = + dynamic_cast( + node->get_node_waitables_interface().get()); + ASSERT_NE(nullptr, node_waitables); + } + + void TearDown() + { + rclcpp::shutdown(); + } + +protected: + std::shared_ptr node; + rclcpp::node_interfaces::NodeWaitables * node_waitables; +}; + +TEST_F(TestNodeWaitables, add_remove_waitable) +{ + std::shared_ptr node2 = std::make_shared("node2", "ns"); + + auto callback_group1 = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + auto callback_group2 = node2->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + auto waitable = std::make_shared(); + EXPECT_NO_THROW( + node_waitables->add_waitable(waitable, callback_group1)); + RCLCPP_EXPECT_THROW_EQ( + node_waitables->add_waitable(waitable, callback_group2), + rclcpp::exceptions::MissingGroupNodeException("waitable")); + EXPECT_NO_THROW(node_waitables->remove_waitable(waitable, callback_group1)); + EXPECT_NO_THROW(node_waitables->remove_waitable(waitable, callback_group2)); + + auto waitable2 = std::make_shared(); + EXPECT_NO_THROW(node_waitables->add_waitable(waitable2, nullptr)); + EXPECT_NO_THROW(node_waitables->remove_waitable(waitable2, nullptr)); +} + +TEST_F(TestNodeWaitables, add_waitable_rcl_trigger_guard_condition_error) +{ + auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + auto waitable = std::make_shared(); + + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_waitables->add_waitable(waitable, callback_group), + std::runtime_error("failed to notify wait set on waitable creation: error not set")); +} diff --git a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp new file mode 100644 index 0000000000..27c2711916 --- /dev/null +++ b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp @@ -0,0 +1,904 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include + +#include "gtest/gtest.h" + +#include "rclcpp/strategies/allocator_memory_strategy.hpp" +#include "rcpputils/scope_exit.hpp" +#include "test_msgs/msg/empty.hpp" +#include "test_msgs/srv/empty.hpp" + +using rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy; +typedef std::map> WeakCallbackGroupsToNodesMap; + +static bool test_waitable_result = false; + +/** + * Mock Waitable class, with a globally setable boolean result. + */ +class TestWaitable : public rclcpp::Waitable +{ +public: + void add_to_wait_set(rcl_wait_set_t &) override + { + if (!test_waitable_result) { + throw std::runtime_error("TestWaitable add_to_wait_set failed"); + } + } + + bool is_ready(const rcl_wait_set_t &) override + { + return test_waitable_result; + } + + std::shared_ptr take_data() override {return nullptr;} + void execute(const std::shared_ptr &) override {} + + void set_on_ready_callback(std::function) override {} + void clear_on_ready_callback() override {} + + std::shared_ptr take_data_by_entity_id(size_t) override {return nullptr;} +}; + +struct RclWaitSetSizes +{ + size_t size_of_subscriptions = 0; + size_t size_of_guard_conditions = 0; + size_t size_of_timers = 0; + size_t size_of_clients = 0; + size_t size_of_services = 0; + size_t size_of_events = 0; + size_t size_of_waitables = 0; +}; + +// For a standard rclcpp node, this should be more than enough capacity for each type. +RclWaitSetSizes SufficientWaitSetCapacities() +{ + return {100, 100, 100, 100, 100, 100, 100}; +} + +class TestAllocatorMemoryStrategy : public ::testing::Test +{ +public: + TestAllocatorMemoryStrategy() + : allocator_memory_strategy_(nullptr) {} + + void SetUp() override + { + rclcpp::init(0, nullptr); + allocator_ = std::make_shared>(); + // Even though this is just using a basic allocator, the custom allocator constructor was + // not covered in general testing. + allocator_memory_strategy_ = std::make_shared>(allocator_); + } + + void TearDown() override + { + allocator_memory_strategy_.reset(); + rclcpp::shutdown(); + } + +protected: + std::shared_ptr> allocator_memory_strategy() + { + return allocator_memory_strategy_; + } + + // + // Convience methods, but it adds entities to vectors so the weak pointers kept by the node + // interfaces remain alive and valid + // + + std::shared_ptr create_node_with_disabled_callback_groups(const std::string & name) + { + auto node = std::make_shared(name, "ns"); + + node->for_each_callback_group( + [](rclcpp::CallbackGroup::SharedPtr group) + { + group->can_be_taken_from() = false; + }); + return node; + } + + std::shared_ptr create_node_with_subscription(const std::string & name) + { + auto subscription_callback = [](test_msgs::msg::Empty::ConstSharedPtr) {}; + const rclcpp::QoS qos(10); + auto node_with_subscription = create_node_with_disabled_callback_groups(name); + + rclcpp::SubscriptionOptions subscription_options; + + auto callback_group = + node_with_subscription->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + subscription_options.callback_group = callback_group; + callback_groups_.push_back(callback_group); + + auto subscription = node_with_subscription->create_subscription< + test_msgs::msg::Empty, decltype(subscription_callback)>( + "topic", qos, std::move(subscription_callback), subscription_options); + subscriptions_.push_back(subscription); + + return node_with_subscription; + } + + std::shared_ptr create_node_with_service(const std::string & name) + { + auto service_callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}; + auto node_with_service = create_node_with_disabled_callback_groups(name); + + auto callback_group = + node_with_service->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + callback_groups_.push_back(callback_group); + + services_.push_back( + node_with_service->create_service( + "service", std::move(service_callback), rclcpp::ServicesQoS(), callback_group)); + return node_with_service; + } + + std::shared_ptr create_node_with_client(const std::string & name) + { + auto node_with_client = create_node_with_disabled_callback_groups(name); + auto callback_group = + node_with_client->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + callback_groups_.push_back(callback_group); + + clients_.push_back( + node_with_client->create_client( + "service", rclcpp::ServicesQoS(), callback_group)); + return node_with_client; + } + + std::shared_ptr create_node_with_timer(const std::string & name) + { + auto timer_callback = []() {}; + auto node_with_timer = create_node_with_disabled_callback_groups(name); + + auto callback_group = + node_with_timer->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + callback_groups_.push_back(callback_group); + + timers_.push_back( + node_with_timer->create_wall_timer( + std::chrono::milliseconds(1), timer_callback, callback_group)); + return node_with_timer; + } + + ::testing::AssertionResult TestNumberOfEntitiesAfterCollection( + std::shared_ptr node, + const RclWaitSetSizes & expected) + { + WeakCallbackGroupsToNodesMap weak_groups_to_nodes; + node->for_each_callback_group( + [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) + { + weak_groups_to_nodes.insert( + std::pair( + group_ptr, + node->get_node_base_interface())); + }); + allocator_memory_strategy()->collect_entities(weak_groups_to_nodes); + EXPECT_EQ( + expected.size_of_subscriptions, allocator_memory_strategy()->number_of_ready_subscriptions()); + EXPECT_EQ( + expected.size_of_guard_conditions, allocator_memory_strategy()->number_of_guard_conditions()); + EXPECT_EQ(expected.size_of_timers, allocator_memory_strategy()->number_of_ready_timers()); + EXPECT_EQ(expected.size_of_clients, allocator_memory_strategy()->number_of_ready_clients()); + EXPECT_EQ(expected.size_of_services, allocator_memory_strategy()->number_of_ready_services()); + EXPECT_EQ(expected.size_of_events, allocator_memory_strategy()->number_of_ready_events()); + EXPECT_EQ(expected.size_of_waitables, allocator_memory_strategy()->number_of_waitables()); + if (::testing::Test::HasFailure()) { + return ::testing::AssertionFailure() << + "Expected number of entities did not match actual counts"; + } + return ::testing::AssertionSuccess(); + } + + ::testing::AssertionResult TestAddHandlesToWaitSet( + std::shared_ptr node, + const RclWaitSetSizes & insufficient_capacities) + { + WeakCallbackGroupsToNodesMap weak_groups_to_nodes; + node->for_each_callback_group( + [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) + { + weak_groups_to_nodes.insert( + std::pair( + group_ptr, + node->get_node_base_interface())); + }); + allocator_memory_strategy()->collect_entities(weak_groups_to_nodes); + + auto context = node->get_node_base_interface()->get_context(); + rcl_context_t * rcl_context = context->get_rcl_context().get(); + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_allocator_t allocator = rcl_get_default_allocator(); + RclWaitSetSizes sufficient_capacities = SufficientWaitSetCapacities(); + + rcl_ret_t ret = rcl_wait_set_init( + &wait_set, + sufficient_capacities.size_of_subscriptions, + sufficient_capacities.size_of_guard_conditions, + sufficient_capacities.size_of_timers, + sufficient_capacities.size_of_clients, + sufficient_capacities.size_of_services, + sufficient_capacities.size_of_events, + rcl_context, + allocator); + if (ret != RCL_RET_OK) { + return ::testing::AssertionFailure() << + "Calling rcl_wait_set_init() with expected sufficient capacities failed"; + } + + RCPPUTILS_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)); + }); + + if (!allocator_memory_strategy()->add_handles_to_wait_set(&wait_set)) { + return ::testing::AssertionFailure() << + "Calling add_handles_to_wait_set() with a wait_set with expected sufficient capacities" + " failed"; + } + + rcl_wait_set_t wait_set_no_capacity = rcl_get_zero_initialized_wait_set(); + ret = rcl_wait_set_init( + &wait_set_no_capacity, + insufficient_capacities.size_of_subscriptions, + insufficient_capacities.size_of_guard_conditions, + insufficient_capacities.size_of_timers, + insufficient_capacities.size_of_clients, + insufficient_capacities.size_of_services, + insufficient_capacities.size_of_events, + rcl_context, + allocator); + + if (ret != RCL_RET_OK) { + return ::testing::AssertionFailure() << + "Calling rcl_wait_set_init() with expected insufficient capacities failed"; + } + + RCPPUTILS_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set_no_capacity)); + }); + + if (allocator_memory_strategy()->add_handles_to_wait_set(&wait_set_no_capacity)) { + return ::testing::AssertionFailure() << + "Calling add_handles_to_wait_set() with a wait_set with insufficient capacities" + " unexpectedly succeeded"; + } + return ::testing::AssertionSuccess(); + } + + ::testing::AssertionResult TestGetNextEntity( + std::shared_ptr node_with_entity1, + std::shared_ptr node_with_entity2, + std::function get_next_entity_func) + { + auto basic_node = create_node_with_disabled_callback_groups("basic_node"); + WeakCallbackGroupsToNodesMap weak_groups_to_nodes; + basic_node->for_each_callback_group( + [basic_node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) + { + weak_groups_to_nodes.insert( + std::pair( + group_ptr, + basic_node->get_node_base_interface())); + }); + node_with_entity1->for_each_callback_group( + [node_with_entity1, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) + { + weak_groups_to_nodes.insert( + std::pair( + group_ptr, + node_with_entity1->get_node_base_interface())); + }); + allocator_memory_strategy()->collect_entities(weak_groups_to_nodes); + + rclcpp::AnyExecutable result = get_next_entity_func(weak_groups_to_nodes); + if (result.node_base != node_with_entity1->get_node_base_interface()) { + return ::testing::AssertionFailure() << + "Failed to get expected entity with specified get_next_*() function"; + } + + auto basic_node2 = std::make_shared("basic_node2", "ns"); + WeakCallbackGroupsToNodesMap weak_groups_to_uncollected_nodes; + basic_node2->for_each_callback_group( + [basic_node2, &weak_groups_to_uncollected_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) + { + weak_groups_to_uncollected_nodes.insert( + std::pair( + group_ptr, + basic_node2->get_node_base_interface())); + }); + node_with_entity2->for_each_callback_group( + [node_with_entity2, + &weak_groups_to_uncollected_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) + { + weak_groups_to_uncollected_nodes.insert( + std::pair( + group_ptr, + node_with_entity2->get_node_base_interface())); + }); + rclcpp::AnyExecutable failed_result = get_next_entity_func(weak_groups_to_uncollected_nodes); + if (nullptr != failed_result.node_base) { + return ::testing::AssertionFailure() << + "A node was retrieved with the specified get_next_*() function despite" + " none of the nodes that were passed to it were added to the" + " allocator_memory_strategy. Retrieved node: " << failed_result.node_base->get_name(); + } + return ::testing::AssertionSuccess(); + } + + ::testing::AssertionResult TestGetNextEntityMutuallyExclusive( + std::shared_ptr node_with_entity, + std::function get_next_entity_func) + { + auto basic_node = std::make_shared("basic_node", "ns"); + auto basic_node_base = basic_node->get_node_base_interface(); + auto node_base = node_with_entity->get_node_base_interface(); + WeakCallbackGroupsToNodesMap weak_groups_to_nodes; + basic_node_base->for_each_callback_group( + [basic_node_base, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) + { + weak_groups_to_nodes.insert( + std::pair( + group_ptr, + basic_node_base)); + }); + node_base->for_each_callback_group( + [node_base, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) + { + weak_groups_to_nodes.insert( + std::pair( + group_ptr, + node_base)); + }); + allocator_memory_strategy()->collect_entities(weak_groups_to_nodes); + + // It's important that these be set after collect_entities() otherwise collect_entities() will + // not do anything + node_base->get_default_callback_group()->can_be_taken_from() = false; + basic_node_base->get_default_callback_group()->can_be_taken_from() = false; + for (auto & callback_group : callback_groups_) { + callback_group->can_be_taken_from() = false; + } + + rclcpp::AnyExecutable result = get_next_entity_func(weak_groups_to_nodes); + + if (nullptr != result.node_base) { + return ::testing::AssertionFailure() << + "A node was retrieved with the specified get_next_*() function despite" + " setting can_be_taken_from() to false for all nodes and callback_groups"; + } + + return ::testing::AssertionSuccess(); + } + + std::vector> callback_groups_; + +private: + std::shared_ptr> allocator_; + std::shared_ptr> allocator_memory_strategy_; + + // These are generally kept as weak pointers in the rclcpp::Node interfaces, so they need to be + // owned by this class. + std::vector subscriptions_; + std::vector services_; + std::vector clients_; + std::vector timers_; + std::vector waitables_; +}; + +TEST_F(TestAllocatorMemoryStrategy, construct_destruct) { + auto basic_node = create_node_with_disabled_callback_groups("basic_node"); + WeakCallbackGroupsToNodesMap weak_groups_to_nodes; + basic_node->for_each_callback_group( + [basic_node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) + { + weak_groups_to_nodes.insert( + std::pair( + group_ptr, + basic_node->get_node_base_interface())); + }); + allocator_memory_strategy()->collect_entities(weak_groups_to_nodes); + EXPECT_EQ(0u, allocator_memory_strategy()->number_of_ready_subscriptions()); + EXPECT_EQ(0u, allocator_memory_strategy()->number_of_ready_services()); + EXPECT_EQ(0u, allocator_memory_strategy()->number_of_ready_events()); + EXPECT_EQ(0u, allocator_memory_strategy()->number_of_ready_clients()); + EXPECT_EQ(0u, allocator_memory_strategy()->number_of_guard_conditions()); + EXPECT_EQ(0u, allocator_memory_strategy()->number_of_ready_timers()); + EXPECT_EQ(0u, allocator_memory_strategy()->number_of_waitables()); +} + +TEST_F(TestAllocatorMemoryStrategy, add_remove_guard_conditions) { + rclcpp::GuardCondition guard_condition1; + rclcpp::GuardCondition guard_condition2; + rclcpp::GuardCondition guard_condition3; + + EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(guard_condition1)); + EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(guard_condition2)); + EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(guard_condition3)); + EXPECT_EQ(3u, allocator_memory_strategy()->number_of_guard_conditions()); + + // Adding a second time should not add to vector + EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(guard_condition1)); + EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(guard_condition2)); + EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(guard_condition3)); + EXPECT_EQ(3u, allocator_memory_strategy()->number_of_guard_conditions()); + + EXPECT_NO_THROW(allocator_memory_strategy()->remove_guard_condition(&guard_condition1)); + EXPECT_NO_THROW(allocator_memory_strategy()->remove_guard_condition(&guard_condition2)); + EXPECT_NO_THROW(allocator_memory_strategy()->remove_guard_condition(&guard_condition3)); + EXPECT_EQ(0u, allocator_memory_strategy()->number_of_guard_conditions()); + + // Removing second time should have no effect + EXPECT_NO_THROW(allocator_memory_strategy()->remove_guard_condition(&guard_condition1)); + EXPECT_NO_THROW(allocator_memory_strategy()->remove_guard_condition(&guard_condition2)); + EXPECT_NO_THROW(allocator_memory_strategy()->remove_guard_condition(&guard_condition3)); + EXPECT_EQ(0u, allocator_memory_strategy()->number_of_guard_conditions()); +} + +TEST_F(TestAllocatorMemoryStrategy, add_remove_waitables) { + EXPECT_THROW(allocator_memory_strategy()->add_waitable_handle(nullptr), std::runtime_error); + EXPECT_EQ(0u, allocator_memory_strategy()->number_of_waitables()); + + rclcpp::Waitable::SharedPtr waitable = std::make_shared(); + EXPECT_NO_THROW(allocator_memory_strategy()->add_waitable_handle(waitable)); + EXPECT_EQ(1u, allocator_memory_strategy()->number_of_waitables()); + + EXPECT_NO_THROW(allocator_memory_strategy()->clear_handles()); + EXPECT_EQ(0u, allocator_memory_strategy()->number_of_waitables()); +} + +TEST_F(TestAllocatorMemoryStrategy, number_of_entities_with_subscription) { + RclWaitSetSizes expected_sizes = {}; + expected_sizes.size_of_subscriptions = 1; + expected_sizes.size_of_events = 2; + expected_sizes.size_of_waitables = 2; + auto node_with_subscription = create_node_with_subscription("subscription_node"); + EXPECT_TRUE(TestNumberOfEntitiesAfterCollection(node_with_subscription, expected_sizes)); +} + +TEST_F(TestAllocatorMemoryStrategy, number_of_entities_with_service) { + RclWaitSetSizes expected_sizes = {}; + expected_sizes.size_of_services = 1; + auto node_with_service = create_node_with_service("service_node"); + EXPECT_TRUE(TestNumberOfEntitiesAfterCollection(node_with_service, expected_sizes)); +} + +TEST_F(TestAllocatorMemoryStrategy, number_of_entities_with_client) { + RclWaitSetSizes expected_sizes = {}; + expected_sizes.size_of_clients = 1; + auto node_with_client = create_node_with_client("client_node"); + EXPECT_TRUE(TestNumberOfEntitiesAfterCollection(node_with_client, expected_sizes)); +} + +TEST_F(TestAllocatorMemoryStrategy, number_of_entities_with_timer) { + RclWaitSetSizes expected_sizes = {}; + expected_sizes.size_of_timers = 1; + auto node_with_timer = create_node_with_timer("timer_node"); + EXPECT_TRUE(TestNumberOfEntitiesAfterCollection(node_with_timer, expected_sizes)); +} + +TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_bad_arguments) { + auto node = create_node_with_subscription("subscription_node"); + WeakCallbackGroupsToNodesMap weak_groups_to_nodes; + node->for_each_callback_group( + [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) + { + weak_groups_to_nodes.insert( + std::pair( + group_ptr, + node->get_node_base_interface())); + }); + allocator_memory_strategy()->collect_entities(weak_groups_to_nodes); + EXPECT_FALSE(allocator_memory_strategy()->add_handles_to_wait_set(nullptr)); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); +} + +TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_subscription) { + auto node_with_subscription = create_node_with_subscription("subscription_node"); + RclWaitSetSizes insufficient_capacities = SufficientWaitSetCapacities(); + insufficient_capacities.size_of_subscriptions = 0; + EXPECT_TRUE(TestAddHandlesToWaitSet(node_with_subscription, insufficient_capacities)); +} + +TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_service) { + auto node_with_service = create_node_with_service("service_node"); + RclWaitSetSizes insufficient_capacities = SufficientWaitSetCapacities(); + insufficient_capacities.size_of_services = 0; + EXPECT_TRUE(TestAddHandlesToWaitSet(node_with_service, insufficient_capacities)); +} + +TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_client) { + auto node_with_client = create_node_with_client("client_node"); + RclWaitSetSizes insufficient_capacities = SufficientWaitSetCapacities(); + insufficient_capacities.size_of_clients = 0; + EXPECT_TRUE(TestAddHandlesToWaitSet(node_with_client, insufficient_capacities)); +} + +TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_guard_condition) { + auto node = create_node_with_disabled_callback_groups("node"); + auto context = node->get_node_base_interface()->get_context(); + + rclcpp::GuardCondition guard_condition(context); + + EXPECT_NO_THROW(rclcpp::GuardCondition guard_condition(context);); + + allocator_memory_strategy()->add_guard_condition(guard_condition); + + RclWaitSetSizes insufficient_capacities = SufficientWaitSetCapacities(); + insufficient_capacities.size_of_guard_conditions = 0; + EXPECT_THROW(TestAddHandlesToWaitSet(node, insufficient_capacities), std::runtime_error); +} + +TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_timer) { + auto node_with_timer = create_node_with_timer("timer_node"); + RclWaitSetSizes insufficient_capacities = SufficientWaitSetCapacities(); + insufficient_capacities.size_of_timers = 0; + EXPECT_TRUE(TestAddHandlesToWaitSet(node_with_timer, insufficient_capacities)); +} + +TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_waitable) { + rcl_reset_error(); + + rclcpp::Waitable::SharedPtr waitable = std::make_shared(); + EXPECT_NO_THROW(allocator_memory_strategy()->add_waitable_handle(waitable)); + EXPECT_EQ(1u, allocator_memory_strategy()->number_of_waitables()); + + test_waitable_result = true; + EXPECT_TRUE(allocator_memory_strategy()->add_handles_to_wait_set(nullptr)); + + test_waitable_result = false; + EXPECT_THROW( + allocator_memory_strategy()->add_handles_to_wait_set(nullptr), + std::runtime_error); + + // This calls TestWaitable's functions, so rcl errors are not set + EXPECT_FALSE(rcl_error_is_set()); +} + +TEST_F(TestAllocatorMemoryStrategy, get_next_subscription) { + auto node1 = create_node_with_subscription("node1"); + auto node2 = create_node_with_subscription("node2"); + + auto get_next_entity = [this](const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { + rclcpp::AnyExecutable result; + allocator_memory_strategy()->get_next_subscription(result, weak_groups_to_nodes); + return result; + }; + + EXPECT_TRUE(TestGetNextEntity(node1, node2, get_next_entity)); +} + +TEST_F(TestAllocatorMemoryStrategy, get_next_service) { + auto node1 = create_node_with_service("node1"); + auto node2 = create_node_with_service("node2"); + + auto get_next_entity = [this](const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { + rclcpp::AnyExecutable result; + allocator_memory_strategy()->get_next_service(result, weak_groups_to_nodes); + return result; + }; + + EXPECT_TRUE(TestGetNextEntity(node1, node2, get_next_entity)); +} + +TEST_F(TestAllocatorMemoryStrategy, get_next_client) { + auto node1 = create_node_with_client("node1"); + auto node2 = create_node_with_client("node2"); + + auto get_next_entity = [this](const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { + rclcpp::AnyExecutable result; + allocator_memory_strategy()->get_next_client(result, weak_groups_to_nodes); + return result; + }; + + EXPECT_TRUE(TestGetNextEntity(node1, node2, get_next_entity)); +} + +TEST_F(TestAllocatorMemoryStrategy, get_next_timer) { + auto node1 = create_node_with_timer("node1"); + auto node2 = create_node_with_timer("node2"); + + auto get_next_entity = [this](const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { + rclcpp::AnyExecutable result; + allocator_memory_strategy()->get_next_timer(result, weak_groups_to_nodes); + return result; + }; + + EXPECT_TRUE(TestGetNextEntity(node1, node2, get_next_entity)); +} + +TEST_F(TestAllocatorMemoryStrategy, get_next_waitable) { + auto node1 = std::make_shared("waitable_node", "ns"); + auto node2 = std::make_shared("waitable_node2", "ns"); + rclcpp::Waitable::SharedPtr waitable1 = std::make_shared(); + rclcpp::Waitable::SharedPtr waitable2 = std::make_shared(); + node1->get_node_waitables_interface()->add_waitable(waitable1, nullptr); + node2->get_node_waitables_interface()->add_waitable(waitable2, nullptr); + + auto get_next_entity = [this](const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { + rclcpp::AnyExecutable result; + allocator_memory_strategy()->get_next_waitable(result, weak_groups_to_nodes); + return result; + }; + + EXPECT_TRUE(TestGetNextEntity(node1, node2, get_next_entity)); +} + +TEST_F(TestAllocatorMemoryStrategy, get_next_subscription_mutually_exclusive) { + auto node = create_node_with_subscription("node"); + + auto get_next_entity = [this](const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { + rclcpp::AnyExecutable result; + allocator_memory_strategy()->get_next_subscription(result, weak_groups_to_nodes); + return result; + }; + + EXPECT_TRUE(TestGetNextEntityMutuallyExclusive(node, get_next_entity)); +} + +TEST_F(TestAllocatorMemoryStrategy, get_next_service_mutually_exclusive) { + auto node = create_node_with_service("node"); + + auto get_next_entity = [this](const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { + rclcpp::AnyExecutable result; + allocator_memory_strategy()->get_next_service(result, weak_groups_to_nodes); + return result; + }; + + EXPECT_TRUE(TestGetNextEntityMutuallyExclusive(node, get_next_entity)); +} + +TEST_F(TestAllocatorMemoryStrategy, get_next_client_mutually_exclusive) { + auto node = create_node_with_client("node"); + + auto get_next_entity = [this](const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { + rclcpp::AnyExecutable result; + allocator_memory_strategy()->get_next_client(result, weak_groups_to_nodes); + return result; + }; + + EXPECT_TRUE(TestGetNextEntityMutuallyExclusive(node, get_next_entity)); +} + +TEST_F(TestAllocatorMemoryStrategy, get_next_timer_mutually_exclusive) { + auto node = create_node_with_timer("node"); + + auto get_next_entity = [this](const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { + rclcpp::AnyExecutable result; + allocator_memory_strategy()->get_next_timer(result, weak_groups_to_nodes); + return result; + }; + + EXPECT_TRUE(TestGetNextEntityMutuallyExclusive(node, get_next_entity)); +} + +TEST_F(TestAllocatorMemoryStrategy, get_next_waitable_mutually_exclusive) { + auto node = std::make_shared("waitable_node", "ns"); + rclcpp::Waitable::SharedPtr waitable = std::make_shared(); + auto callback_group = + node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + node->get_node_waitables_interface()->add_waitable(waitable, callback_group); + + auto get_next_entity = + [this, callback_group](const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { + // This callback group isn't in the base class' callback_group list, so this needs to be done + // before get_next_waitable() is called. + callback_group->can_be_taken_from() = false; + + rclcpp::AnyExecutable result; + allocator_memory_strategy()->get_next_waitable(result, weak_groups_to_nodes); + return result; + }; + + EXPECT_TRUE(TestGetNextEntityMutuallyExclusive(node, get_next_entity)); +} + +TEST_F(TestAllocatorMemoryStrategy, get_next_subscription_out_of_scope) { + WeakCallbackGroupsToNodesMap weak_groups_to_nodes; + auto node = create_node_with_disabled_callback_groups("node"); + // Force subscription to go out of scope and cleanup after collecting entities. + { + rclcpp::SubscriptionOptions subscription_options; + + auto callback_group = + node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + subscription_options.callback_group = callback_group; + + auto subscription_callback = [](test_msgs::msg::Empty::ConstSharedPtr) {}; + const rclcpp::QoS qos(10); + + auto subscription = node->create_subscription< + test_msgs::msg::Empty, decltype(subscription_callback)>( + "topic", qos, std::move(subscription_callback), subscription_options); + + node->for_each_callback_group( + [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) + { + weak_groups_to_nodes.insert( + std::pair( + group_ptr, + node->get_node_base_interface())); + }); + allocator_memory_strategy()->collect_entities(weak_groups_to_nodes); + } + EXPECT_EQ(1u, allocator_memory_strategy()->number_of_ready_subscriptions()); + + rclcpp::AnyExecutable result; + allocator_memory_strategy()->get_next_subscription(result, weak_groups_to_nodes); + EXPECT_EQ(nullptr, result.node_base); +} + +TEST_F(TestAllocatorMemoryStrategy, get_next_service_out_of_scope) { + WeakCallbackGroupsToNodesMap weak_groups_to_nodes; + auto node = create_node_with_disabled_callback_groups("node"); + // Force service to go out of scope and cleanup after collecting entities. + { + auto callback_group = + node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + auto service_callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}; + auto service = node->create_service( + "service", std::move(service_callback), rclcpp::ServicesQoS(), callback_group); + + node->for_each_callback_group( + [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) + { + weak_groups_to_nodes.insert( + std::pair( + group_ptr, + node->get_node_base_interface())); + }); + allocator_memory_strategy()->collect_entities(weak_groups_to_nodes); + } + EXPECT_EQ(1u, allocator_memory_strategy()->number_of_ready_services()); + + rclcpp::AnyExecutable result; + allocator_memory_strategy()->get_next_service(result, weak_groups_to_nodes); + EXPECT_EQ(nullptr, result.node_base); +} + +TEST_F(TestAllocatorMemoryStrategy, get_next_client_out_of_scope) { + auto node = create_node_with_disabled_callback_groups("node"); + WeakCallbackGroupsToNodesMap weak_groups_to_nodes; + node->for_each_callback_group( + [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) + { + weak_groups_to_nodes.insert( + std::pair( + group_ptr, + node->get_node_base_interface())); + }); + // Force client to go out of scope and cleanup after collecting entities. + { + auto callback_group = + node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + auto client = node->create_client( + "service", rclcpp::ServicesQoS(), callback_group); + + weak_groups_to_nodes.insert( + std::pair( + rclcpp::CallbackGroup::WeakPtr(callback_group), + node->get_node_base_interface())); + + allocator_memory_strategy()->collect_entities(weak_groups_to_nodes); + } + EXPECT_EQ(1u, allocator_memory_strategy()->number_of_ready_clients()); + + rclcpp::AnyExecutable result; + allocator_memory_strategy()->get_next_client(result, weak_groups_to_nodes); + EXPECT_EQ(nullptr, result.node_base); +} + +TEST_F(TestAllocatorMemoryStrategy, get_next_timer_out_of_scope) { + auto node = create_node_with_disabled_callback_groups("node"); + WeakCallbackGroupsToNodesMap weak_groups_to_nodes; + // Force timer to go out of scope and cleanup after collecting entities. + { + auto callback_group = + node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + auto timer = node->create_wall_timer( + std::chrono::seconds(10), []() {}, callback_group); + node->for_each_callback_group( + [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) + { + weak_groups_to_nodes.insert( + std::pair( + group_ptr, + node->get_node_base_interface())); + }); + allocator_memory_strategy()->collect_entities(weak_groups_to_nodes); + } + EXPECT_EQ(1u, allocator_memory_strategy()->number_of_ready_timers()); + + rclcpp::AnyExecutable result; + allocator_memory_strategy()->get_next_timer(result, weak_groups_to_nodes); + EXPECT_EQ(nullptr, result.node_base); +} + +TEST_F(TestAllocatorMemoryStrategy, get_next_waitable_out_of_scope) { + auto node = create_node_with_disabled_callback_groups("node"); + WeakCallbackGroupsToNodesMap weak_groups_to_nodes; + // Force waitable to go out of scope and cleanup after collecting entities. + { + auto callback_group = + node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + node->for_each_callback_group( + [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) + { + weak_groups_to_nodes.insert( + std::pair( + group_ptr, + node->get_node_base_interface())); + }); + allocator_memory_strategy()->collect_entities(weak_groups_to_nodes); + auto waitable = std::make_shared(); + node->get_node_waitables_interface()->add_waitable(waitable, callback_group); + allocator_memory_strategy()->add_waitable_handle(waitable); + } + // Since all callback groups have been locked, except the one we added, this should only be 1 + EXPECT_EQ(1u, allocator_memory_strategy()->number_of_waitables()); + + rclcpp::AnyExecutable result; + allocator_memory_strategy()->get_next_waitable(result, weak_groups_to_nodes); + EXPECT_EQ(nullptr, result.node_base); +} diff --git a/rclcpp/test/rclcpp/strategies/test_message_pool_memory_strategy.cpp b/rclcpp/test/rclcpp/strategies/test_message_pool_memory_strategy.cpp new file mode 100644 index 0000000000..69198adf59 --- /dev/null +++ b/rclcpp/test/rclcpp/strategies/test_message_pool_memory_strategy.cpp @@ -0,0 +1,83 @@ +// 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 "gtest/gtest.h" + +#include "rclcpp/strategies/message_pool_memory_strategy.hpp" +#include "test_msgs/msg/empty.hpp" +#include "../../utils/rclcpp_gtest_macros.hpp" + +using rclcpp::strategies::message_pool_memory_strategy::MessagePoolMemoryStrategy; + +class TestMessagePoolMemoryStrategy : public ::testing::Test +{ +public: + void SetUp() + { + message_memory_strategy_ = + std::make_shared>(); + } + +protected: + std::shared_ptr> message_memory_strategy_; +}; + +TEST_F(TestMessagePoolMemoryStrategy, construct_destruct) { + ASSERT_NE(nullptr, message_memory_strategy_); + EXPECT_NE(nullptr, message_memory_strategy_->message_allocator_); + EXPECT_NE(nullptr, message_memory_strategy_->serialized_message_allocator_); + EXPECT_NE(nullptr, message_memory_strategy_->buffer_allocator_); +} + +TEST_F(TestMessagePoolMemoryStrategy, borrow_return) { + auto message = message_memory_strategy_->borrow_message(); + ASSERT_NE(nullptr, message); + + EXPECT_NO_THROW(message_memory_strategy_->return_message(message)); +} + +TEST_F(TestMessagePoolMemoryStrategy, borrow_too_many) { + auto message = message_memory_strategy_->borrow_message(); + ASSERT_NE(nullptr, message); + + // Size is 1, borrowing second time should fail + RCLCPP_EXPECT_THROW_EQ( + message_memory_strategy_->borrow_message(), + std::runtime_error("No more free slots in the pool")); + EXPECT_NO_THROW(message_memory_strategy_->return_message(message)); +} + +TEST_F(TestMessagePoolMemoryStrategy, borrow_hold_reference) { + { + auto message = message_memory_strategy_->borrow_message(); + ASSERT_NE(nullptr, message); + + // Return it. + EXPECT_NO_THROW(message_memory_strategy_->return_message(message)); + + // But we are still holding the reference, so we expect that there is still no room in the pool. + RCLCPP_EXPECT_THROW_EQ( + message_memory_strategy_->borrow_message(), + std::runtime_error("No more free slots in the pool")); + } + + // Now that we've dropped the reference (left the scope), we expect to be able to borrow again. + + auto message2 = message_memory_strategy_->borrow_message(); + ASSERT_NE(nullptr, message2); + + EXPECT_NO_THROW(message_memory_strategy_->return_message(message2)); +} diff --git a/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp b/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp new file mode 100644 index 0000000000..2c628e5cf8 --- /dev/null +++ b/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp @@ -0,0 +1,412 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include +#include + +#include "rclcpp/node.hpp" +#include "test_msgs/msg/empty.hpp" + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/executors.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "./executors/executor_types.hpp" + +using namespace std::chrono_literals; + +template +class TestAddCallbackGroupsToExecutor : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + static void TearDownTestCase() + { + rclcpp::shutdown(); + } +}; + +template +class TestAddCallbackGroupsToExecutorStable : public TestAddCallbackGroupsToExecutor {}; + +TYPED_TEST_SUITE(TestAddCallbackGroupsToExecutor, ExecutorTypes, ExecutorTypeNames); + +TYPED_TEST_SUITE(TestAddCallbackGroupsToExecutorStable, StandardExecutors, ExecutorTypeNames); + +/* + * Test adding callback groups. + */ +TYPED_TEST(TestAddCallbackGroupsToExecutor, add_callback_groups) +{ + using ExecutorType = TypeParam; + + auto node = std::make_shared("my_node", "/ns"); + auto timer_callback = []() {}; + rclcpp::CallbackGroup::SharedPtr cb_grp = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + rclcpp::TimerBase::SharedPtr timer_ = node->create_wall_timer( + 2s, timer_callback, cb_grp); + ExecutorType executor; + executor.add_callback_group(cb_grp, node->get_node_base_interface()); + ASSERT_EQ(executor.get_all_callback_groups().size(), 1u); + ASSERT_EQ(executor.get_manually_added_callback_groups().size(), 1u); + ASSERT_EQ(executor.get_automatically_added_callback_groups_from_nodes().size(), 0u); + + const rclcpp::QoS qos(10); + auto options = rclcpp::SubscriptionOptions(); + auto callback = [](test_msgs::msg::Empty::ConstSharedPtr) {}; + rclcpp::CallbackGroup::SharedPtr cb_grp2 = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + options.callback_group = cb_grp2; + auto subscription = + node->create_subscription("topic_name", qos, callback, options); + executor.add_callback_group(cb_grp2, node->get_node_base_interface()); + ASSERT_EQ(executor.get_all_callback_groups().size(), 2u); + ASSERT_EQ(executor.get_manually_added_callback_groups().size(), 2u); + ASSERT_EQ(executor.get_automatically_added_callback_groups_from_nodes().size(), 0u); + + executor.add_node(node); + ASSERT_EQ(executor.get_manually_added_callback_groups().size(), 2u); + ASSERT_EQ(executor.get_automatically_added_callback_groups_from_nodes().size(), 1u); + + executor.remove_node(node); + ASSERT_EQ(executor.get_manually_added_callback_groups().size(), 2u); + ASSERT_EQ(executor.get_automatically_added_callback_groups_from_nodes().size(), 0u); + + executor.remove_callback_group(cb_grp); + ASSERT_EQ(executor.get_manually_added_callback_groups().size(), 1u); + ASSERT_EQ(executor.get_automatically_added_callback_groups_from_nodes().size(), 0u); + + executor.remove_callback_group(cb_grp2); + ASSERT_EQ(executor.get_manually_added_callback_groups().size(), 0u); + ASSERT_EQ(executor.get_automatically_added_callback_groups_from_nodes().size(), 0u); +} + +/* + * Test removing callback groups. + */ +TYPED_TEST(TestAddCallbackGroupsToExecutor, remove_callback_groups) +{ + using ExecutorType = TypeParam; + + auto node = std::make_shared("my_node", "/ns"); + auto timer_callback = []() {}; + rclcpp::CallbackGroup::SharedPtr cb_grp = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + rclcpp::TimerBase::SharedPtr timer_ = node->create_wall_timer( + 2s, timer_callback, cb_grp); + ExecutorType executor; + executor.add_callback_group(cb_grp, node->get_node_base_interface()); + const rclcpp::QoS qos(10); + auto options = rclcpp::SubscriptionOptions(); + auto callback = [](test_msgs::msg::Empty::ConstSharedPtr) {}; + rclcpp::CallbackGroup::SharedPtr cb_grp2 = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + options.callback_group = cb_grp2; + auto subscription = + node->create_subscription("topic_name", qos, callback, options); + executor.add_callback_group(cb_grp2, node->get_node_base_interface()); + + executor.remove_callback_group(cb_grp); + ASSERT_EQ(executor.get_all_callback_groups().size(), 1u); + executor.remove_callback_group(cb_grp2); + ASSERT_EQ(executor.get_all_callback_groups().size(), 0u); +} + +/* + * Test adding duplicate callback groups to executor. + */ +TYPED_TEST(TestAddCallbackGroupsToExecutor, add_duplicate_callback_groups) +{ + using ExecutorType = TypeParam; + + ExecutorType executor; + auto node = std::make_shared("my_node", "/ns"); + auto timer_callback = []() {}; + rclcpp::CallbackGroup::SharedPtr cb_grp = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + rclcpp::TimerBase::SharedPtr timer_ = node->create_wall_timer( + 2s, timer_callback, cb_grp); + executor.add_callback_group(cb_grp, node->get_node_base_interface()); + EXPECT_THROW( + executor.add_callback_group(cb_grp, node->get_node_base_interface()), + std::exception); +} + +/* + * Test adding callback group after node is added to executor. + */ +TYPED_TEST(TestAddCallbackGroupsToExecutor, add_callback_groups_after_add_node_to_executor) +{ + using ExecutorType = TypeParam; + + auto count_callback_groups_in_node = [](auto node) { + size_t num = 0; + node->get_node_base_interface()->for_each_callback_group( + [&num](auto) { + num++; + }); + return num; + }; + + ExecutorType executor; + auto node = std::make_shared("my_node", "/ns"); + executor.add_node(node->get_node_base_interface()); + ASSERT_EQ(executor.get_all_callback_groups().size(), count_callback_groups_in_node(node)); + std::atomic_size_t timer_count {0}; + auto timer_callback = [&executor, &timer_count]() { + auto cur_timer_count = timer_count++; + if (cur_timer_count > 0) { + executor.cancel(); + } + }; + rclcpp::CallbackGroup::SharedPtr cb_grp = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + rclcpp::TimerBase::SharedPtr timer_ = node->create_wall_timer( + 1s, timer_callback, cb_grp); + rclcpp::CallbackGroup::SharedPtr cb_grp2 = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, false); + auto timer2_callback = []() {}; + rclcpp::TimerBase::SharedPtr timer2_ = node->create_wall_timer( + 2s, timer2_callback, cb_grp2); + rclcpp::CallbackGroup::SharedPtr cb_grp3 = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, true); + auto timer3_callback = []() {}; + rclcpp::TimerBase::SharedPtr timer3_ = node->create_wall_timer( + 2s, timer3_callback, cb_grp3); + executor.spin(); + ASSERT_GT(timer_count.load(), 0u); +} + +/* + * Test adding unallowable callback group. + */ +TYPED_TEST(TestAddCallbackGroupsToExecutor, add_unallowable_callback_groups) +{ + using ExecutorType = TypeParam; + + ExecutorType executor; + auto node = std::make_shared("my_node", "/ns"); + auto timer_callback = []() {}; + rclcpp::CallbackGroup::SharedPtr cb_grp = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, false); + rclcpp::TimerBase::SharedPtr timer_ = node->create_wall_timer( + 2s, timer_callback, cb_grp); + executor.add_callback_group(cb_grp, node->get_node_base_interface()); + ASSERT_EQ(executor.get_all_callback_groups().size(), 1u); + + const rclcpp::QoS qos(10); + auto options = rclcpp::SubscriptionOptions(); + auto callback = [](test_msgs::msg::Empty::ConstSharedPtr) {}; + rclcpp::CallbackGroup::SharedPtr cb_grp2 = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, false); + options.callback_group = cb_grp2; + auto subscription = + node->create_subscription("topic_name", qos, callback, options); + executor.add_callback_group(cb_grp2, node->get_node_base_interface()); + ASSERT_EQ(executor.get_all_callback_groups().size(), 2u); + + auto timer2_callback = []() {}; + rclcpp::CallbackGroup::SharedPtr cb_grp3 = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, false); + rclcpp::TimerBase::SharedPtr timer2_ = node->create_wall_timer( + 2s, timer2_callback, cb_grp3); + executor.add_node(node->get_node_base_interface()); + ASSERT_EQ(executor.get_all_callback_groups().size(), 3u); +} + +/* + * Test callback groups from one node to many executors. + */ +TYPED_TEST(TestAddCallbackGroupsToExecutor, one_node_many_callback_groups_many_executors) +{ + using ExecutorType = TypeParam; + + ExecutorType timer_executor; + ExecutorType sub_executor; + auto node = std::make_shared("my_node", "/ns"); + auto timer_callback = []() {}; + rclcpp::CallbackGroup::SharedPtr cb_grp = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, false); + rclcpp::TimerBase::SharedPtr timer_ = node->create_wall_timer( + 2s, timer_callback, cb_grp); + timer_executor.add_callback_group(cb_grp, node->get_node_base_interface()); + const rclcpp::QoS qos(10); + auto options = rclcpp::SubscriptionOptions(); + auto callback = [](test_msgs::msg::Empty::ConstSharedPtr) {}; + rclcpp::CallbackGroup::SharedPtr cb_grp2 = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, false); + options.callback_group = cb_grp2; + auto subscription = + node->create_subscription("topic_name", qos, callback, options); + sub_executor.add_callback_group(cb_grp2, node->get_node_base_interface()); + ASSERT_EQ(sub_executor.get_all_callback_groups().size(), 1u); + ASSERT_EQ(timer_executor.get_all_callback_groups().size(), 1u); + auto timer2_callback = []() {}; + rclcpp::CallbackGroup::SharedPtr cb_grp3 = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, false); + rclcpp::TimerBase::SharedPtr timer2 = node->create_wall_timer( + 2s, timer2_callback, cb_grp3); + sub_executor.add_node(node); + ASSERT_EQ(sub_executor.get_all_callback_groups().size(), 2u); + timer_executor.add_callback_group(cb_grp3, node->get_node_base_interface()); + ASSERT_EQ(timer_executor.get_all_callback_groups().size(), 2u); +} + +/* + * Test callback groups from one node to many executors. + * A subscriber on a new executor with a callback group not received a message + * because the executor can't be triggered while a subscriber created, see + * https://github.com/ros2/rclcpp/issues/1611 +*/ +TYPED_TEST(TestAddCallbackGroupsToExecutorStable, subscriber_triggered_to_receive_message) +{ + using ExecutorType = TypeParam; + + auto node = std::make_shared("my_node", "/ns"); + + // create a thread running an executor with a new callback group for a coming subscriber + rclcpp::CallbackGroup::SharedPtr cb_grp = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, false); + ExecutorType cb_grp_executor; + + std::promise received_message_promise; + auto received_message_future = received_message_promise.get_future(); + rclcpp::FutureReturnCode return_code = rclcpp::FutureReturnCode::TIMEOUT; + std::thread cb_grp_thread = std::thread( + [&cb_grp, &node, &cb_grp_executor, &received_message_future, &return_code]() { + cb_grp_executor.add_callback_group(cb_grp, node->get_node_base_interface()); + return_code = cb_grp_executor.spin_until_future_complete(received_message_future, 10s); + }); + + // expect the subscriber to receive a message + auto sub_callback = [&received_message_promise](test_msgs::msg::Empty::ConstSharedPtr) { + received_message_promise.set_value(true); + }; + + std::promise timer_promise; + // create a subscription using the 'cb_grp' callback group + rclcpp::QoS qos = rclcpp::QoS(1).reliable(); + auto options = rclcpp::SubscriptionOptions(); + options.callback_group = cb_grp; + rclcpp::Subscription::SharedPtr subscription = + node->create_subscription("topic_name", qos, sub_callback, options); + // create a publisher to send data + rclcpp::Publisher::SharedPtr publisher = + node->create_publisher("topic_name", qos); + auto timer_callback = + [&publisher, &timer_promise]() { + if (publisher->get_subscription_count() == 0) { + // If discovery hasn't happened yet, get out. + return; + } + publisher->publish(test_msgs::msg::Empty()); + timer_promise.set_value(); + }; + + // Another executor to run the timer with a callback + ExecutorType timer_executor; + + rclcpp::TimerBase::SharedPtr timer = node->create_wall_timer(100ms, timer_callback); + timer_executor.add_node(node); + auto future = timer_promise.get_future(); + timer_executor.spin_until_future_complete(future); + cb_grp_thread.join(); + + ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); + EXPECT_TRUE(received_message_future.get()); +} + +/* + * Test callback group created after spin. + * A subscriber with a new callback group that created after executor spin not received a message + * because the executor can't be triggered while a subscriber created, see + * https://github.com/ros2/rclcpp/issues/2067 +*/ +TYPED_TEST(TestAddCallbackGroupsToExecutorStable, callback_group_create_after_spin) +{ + using ExecutorType = TypeParam; + + auto node = std::make_shared("my_node", "/ns"); + + // create a publisher to send data + rclcpp::QoS qos = rclcpp::QoS(1).reliable().transient_local(); + rclcpp::Publisher::SharedPtr publisher = + node->create_publisher("topic_name", qos); + publisher->publish(test_msgs::msg::Empty()); + + // create a thread running an executor + ExecutorType executor; + executor.add_node(node); + std::promise received_message_promise; + auto received_message_future = received_message_promise.get_future(); + rclcpp::FutureReturnCode return_code = rclcpp::FutureReturnCode::TIMEOUT; + std::thread executor_thread = std::thread( + [&executor, &received_message_future, &return_code]() { + return_code = executor.spin_until_future_complete(received_message_future, 5s); + }); + + // to create a callback group after spin + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + rclcpp::CallbackGroup::SharedPtr cb_grp = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + // expect the subscriber to receive a message + auto sub_callback = [&received_message_promise](test_msgs::msg::Empty::ConstSharedPtr) { + received_message_promise.set_value(true); + }; + // create a subscription using the `cb_grp` callback group + auto options = rclcpp::SubscriptionOptions(); + options.callback_group = cb_grp; + rclcpp::Subscription::SharedPtr subscription = + node->create_subscription("topic_name", qos, sub_callback, options); + + executor_thread.join(); + ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); + EXPECT_TRUE(received_message_future.get()); +} + +/* + * Test removing callback group from executor that its not associated with. + */ +TYPED_TEST(TestAddCallbackGroupsToExecutor, remove_callback_group) +{ + using ExecutorType = TypeParam; + + ExecutorType executor; + auto node = std::make_shared("my_node", "/ns"); + auto timer_callback = []() {}; + rclcpp::CallbackGroup::SharedPtr cb_grp = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + rclcpp::TimerBase::SharedPtr timer_ = node->create_wall_timer( + 2s, timer_callback, cb_grp); + EXPECT_THROW( + executor.remove_callback_group(cb_grp), + std::exception); + executor.add_callback_group(cb_grp, node->get_node_base_interface()); + EXPECT_NO_THROW(executor.remove_callback_group(cb_grp)); + EXPECT_THROW( + executor.remove_callback_group(cb_grp), + std::exception); +} diff --git a/rclcpp/test/rclcpp/test_any_service_callback.cpp b/rclcpp/test/rclcpp/test_any_service_callback.cpp new file mode 100644 index 0000000000..ef46155d99 --- /dev/null +++ b/rclcpp/test/rclcpp/test_any_service_callback.cpp @@ -0,0 +1,111 @@ +// 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. + +// This file includes basic API tests for the AnyServiceCallback class. +// It is also tested in test_externally_defined_services.cpp + +#include + +#include +#include +#include + +#include "rclcpp/any_service_callback.hpp" +#include "rclcpp/service.hpp" +#include "test_msgs/srv/empty.hpp" + +class TestAnyServiceCallback : public ::testing::Test +{ +public: + void SetUp() + { + request_header_ = std::make_shared(); + request_ = std::make_shared(); + response_ = std::make_shared(); + } + +protected: + rclcpp::AnyServiceCallback any_service_callback_; + std::shared_ptr request_header_; + std::shared_ptr request_; + std::shared_ptr response_; +}; + +TEST_F(TestAnyServiceCallback, no_set_and_dispatch_throw) { + EXPECT_THROW( + any_service_callback_.dispatch(nullptr, request_header_, request_), + std::runtime_error); +} + +TEST_F(TestAnyServiceCallback, set_and_dispatch_no_header) { + int callback_calls = 0; + auto callback = [&callback_calls]( + const std::shared_ptr, + std::shared_ptr) + { + callback_calls++; + }; + + any_service_callback_.set(callback); + EXPECT_NO_THROW( + EXPECT_NE(nullptr, any_service_callback_.dispatch(nullptr, request_header_, request_))); + EXPECT_EQ(callback_calls, 1); +} + + +TEST_F(TestAnyServiceCallback, set_and_dispatch_header) { + int callback_with_header_calls = 0; + auto callback_with_header = [&callback_with_header_calls]( + const std::shared_ptr, + const std::shared_ptr, + std::shared_ptr) + { + callback_with_header_calls++; + }; + + any_service_callback_.set(callback_with_header); + EXPECT_NO_THROW( + EXPECT_NE(nullptr, any_service_callback_.dispatch(nullptr, request_header_, request_))); + EXPECT_EQ(callback_with_header_calls, 1); +} + +TEST_F(TestAnyServiceCallback, set_and_dispatch_defered) { + int callback_with_header_calls = 0; + auto callback_with_header = [&callback_with_header_calls]( + const std::shared_ptr, const std::shared_ptr) + { + callback_with_header_calls++; + }; + + any_service_callback_.set(callback_with_header); + EXPECT_NO_THROW( + EXPECT_EQ(nullptr, any_service_callback_.dispatch(nullptr, request_header_, request_))); + EXPECT_EQ(callback_with_header_calls, 1); +} + +TEST_F(TestAnyServiceCallback, set_and_dispatch_defered_with_service_handle) { + int callback_with_header_calls = 0; + auto callback_with_header = [&callback_with_header_calls]( + std::shared_ptr>, + const std::shared_ptr, + const std::shared_ptr) + { + callback_with_header_calls++; + }; + + any_service_callback_.set(callback_with_header); + EXPECT_NO_THROW( + EXPECT_EQ(nullptr, any_service_callback_.dispatch(nullptr, request_header_, request_))); + EXPECT_EQ(callback_with_header_calls, 1); +} diff --git a/rclcpp/test/rclcpp/test_any_subscription_callback.cpp b/rclcpp/test/rclcpp/test_any_subscription_callback.cpp new file mode 100644 index 0000000000..58a9211937 --- /dev/null +++ b/rclcpp/test/rclcpp/test_any_subscription_callback.cpp @@ -0,0 +1,723 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include + +#include "rclcpp/any_subscription_callback.hpp" +#include "test_msgs/msg/empty.hpp" + +// Type adapter to be used in tests. +struct MyEmpty {}; + +template<> +struct rclcpp::TypeAdapter +{ + using is_specialized = std::true_type; + using custom_type = MyEmpty; + using ros_message_type = test_msgs::msg::Empty; + + static + void + convert_to_ros_message(const custom_type &, ros_message_type &) + {} + + static + void + convert_to_custom(const ros_message_type &, custom_type &) + {} +}; + +using MyTA = rclcpp::TypeAdapter; + +class TestAnySubscriptionCallback : public ::testing::Test +{ +public: + TestAnySubscriptionCallback() {} + + static + std::unique_ptr + get_unique_ptr_msg() + { + return std::make_unique(); + } + +protected: + rclcpp::AnySubscriptionCallback any_subscription_callback_; + std::shared_ptr msg_shared_ptr_{std::make_shared()}; + rclcpp::MessageInfo message_info_; +}; + +class TestAnySubscriptionCallbackTA : public ::testing::Test +{ +public: + TestAnySubscriptionCallbackTA() {} + + static + std::unique_ptr + get_unique_ptr_msg() + { + return std::make_unique(); + } + +protected: + rclcpp::AnySubscriptionCallback any_subscription_callback_; + std::shared_ptr msg_shared_ptr_{std::make_shared()}; + rclcpp::MessageInfo message_info_; +}; + +TEST_F(TestAnySubscriptionCallback, construct_destruct) { + // Default constructor. + rclcpp::AnySubscriptionCallback asc1; + + // Constructor with allocator. + std::allocator allocator; + rclcpp::AnySubscriptionCallback asc2(allocator); +} + +TEST_F(TestAnySubscriptionCallback, is_serialized_message_callback) { + { + rclcpp::AnySubscriptionCallback asc; + asc.set([](const rclcpp::SerializedMessage &) {}); + EXPECT_TRUE(asc.is_serialized_message_callback()); + EXPECT_NO_THROW( + asc.dispatch( + std::make_shared(), + rclcpp::MessageInfo{})); + } + { + rclcpp::AnySubscriptionCallback asc; + asc.set([](const rclcpp::SerializedMessage &, const rclcpp::MessageInfo &) {}); + EXPECT_TRUE(asc.is_serialized_message_callback()); + EXPECT_NO_THROW( + asc.dispatch( + std::make_shared(), + rclcpp::MessageInfo{})); + } + { + rclcpp::AnySubscriptionCallback asc; + asc.set([](const rclcpp::SerializedMessage &, const rclcpp::MessageInfo &) {}); + EXPECT_TRUE(asc.is_serialized_message_callback()); + EXPECT_NO_THROW( + asc.dispatch( + std::make_shared(), + rclcpp::MessageInfo{})); + } + { + rclcpp::AnySubscriptionCallback asc; + asc.set([](std::unique_ptr) {}); + EXPECT_TRUE(asc.is_serialized_message_callback()); + EXPECT_NO_THROW( + asc.dispatch( + std::make_shared(), + rclcpp::MessageInfo{})); + } + { + rclcpp::AnySubscriptionCallback asc; + asc.set([](std::unique_ptr, const rclcpp::MessageInfo &) {}); + EXPECT_TRUE(asc.is_serialized_message_callback()); + EXPECT_NO_THROW( + asc.dispatch( + std::make_shared(), + rclcpp::MessageInfo{})); + } + { + rclcpp::AnySubscriptionCallback asc; + asc.set([](std::shared_ptr) {}); + EXPECT_TRUE(asc.is_serialized_message_callback()); + EXPECT_NO_THROW( + asc.dispatch( + std::make_shared(), + rclcpp::MessageInfo{})); + } + { + rclcpp::AnySubscriptionCallback asc; + asc.set([](std::shared_ptr, const rclcpp::MessageInfo &) {}); + EXPECT_TRUE(asc.is_serialized_message_callback()); + EXPECT_NO_THROW( + asc.dispatch( + std::make_shared(), + rclcpp::MessageInfo{})); + } + { + rclcpp::AnySubscriptionCallback asc; + asc.set([](const std::shared_ptr &) {}); + EXPECT_TRUE(asc.is_serialized_message_callback()); + EXPECT_NO_THROW( + asc.dispatch( + std::make_shared(), + rclcpp::MessageInfo{})); + } + { + rclcpp::AnySubscriptionCallback asc; + asc.set( + []( + const std::shared_ptr &, + const rclcpp::MessageInfo &) {}); + EXPECT_TRUE(asc.is_serialized_message_callback()); + EXPECT_NO_THROW( + asc.dispatch( + std::make_shared(), + rclcpp::MessageInfo{})); + } + { + rclcpp::AnySubscriptionCallback asc; + asc.set([](std::shared_ptr) {}); + EXPECT_TRUE(asc.is_serialized_message_callback()); + EXPECT_NO_THROW( + asc.dispatch( + std::make_shared(), + rclcpp::MessageInfo{})); + } + { + rclcpp::AnySubscriptionCallback asc; + asc.set([](std::shared_ptr, const rclcpp::MessageInfo &) {}); + EXPECT_TRUE(asc.is_serialized_message_callback()); + EXPECT_NO_THROW( + asc.dispatch( + std::make_shared(), + rclcpp::MessageInfo{})); + } +} + +TEST_F(TestAnySubscriptionCallback, unset_dispatch_throw) { + EXPECT_THROW( + any_subscription_callback_.dispatch(msg_shared_ptr_, message_info_), + std::runtime_error); + EXPECT_THROW( + any_subscription_callback_.dispatch_intra_process(msg_shared_ptr_, message_info_), + std::runtime_error); + EXPECT_THROW( + any_subscription_callback_.dispatch_intra_process(get_unique_ptr_msg(), message_info_), + std::runtime_error); +} + +// +// Parameterized test to test across all callback types and dispatch types. +// + +template +class InstanceContextImpl +{ +public: + InstanceContextImpl() = default; + virtual ~InstanceContextImpl() = default; + + explicit InstanceContextImpl(rclcpp::AnySubscriptionCallback asc) + : any_subscription_callback_(asc) + {} + + virtual + rclcpp::AnySubscriptionCallback + get_any_subscription_callback_to_test() const + { + return any_subscription_callback_; + } + +protected: + rclcpp::AnySubscriptionCallback any_subscription_callback_; +}; + +template +class InstanceContext +{ +public: + InstanceContext(const std::string & name, std::shared_ptr> impl) + : name(name), impl_(impl) + {} + + InstanceContext( + const std::string & name, + rclcpp::AnySubscriptionCallback asc) + : name(name), impl_(std::make_shared>(asc)) + {} + + InstanceContext(const InstanceContext & other) + : InstanceContext(other.name, other.impl_) {} + + rclcpp::AnySubscriptionCallback + get_any_subscription_callback_to_test() const + { + return impl_->get_any_subscription_callback_to_test(); + } + + std::string name; + +protected: + std::shared_ptr> impl_; +}; + +class DispatchTests + : public TestAnySubscriptionCallback, + public ::testing::WithParamInterface> +{}; + +class DispatchTestsWithTA + : public TestAnySubscriptionCallbackTA, + public ::testing::WithParamInterface> +{}; + +auto +format_parameter(const ::testing::TestParamInfo & info) +{ + return info.param.name; +} + +auto +format_parameter_with_ta(const ::testing::TestParamInfo & info) +{ + return info.param.name; +} + +/* Testing dispatch with shared_ptr as input */ +TEST_P(DispatchTests, test_inter_shared_dispatch) { + auto any_subscription_callback_to_test = GetParam().get_any_subscription_callback_to_test(); + any_subscription_callback_to_test.dispatch(msg_shared_ptr_, message_info_); +} + +/* Testing dispatch with shared_ptr as input */ +TEST_P(DispatchTests, test_intra_shared_dispatch) { + auto any_subscription_callback_to_test = GetParam().get_any_subscription_callback_to_test(); + any_subscription_callback_to_test.dispatch_intra_process(msg_shared_ptr_, message_info_); +} + +/* Testing dispatch with unique_ptr as input */ +TEST_P(DispatchTests, test_intra_unique_dispatch) { + auto any_subscription_callback_to_test = GetParam().get_any_subscription_callback_to_test(); + any_subscription_callback_to_test.dispatch_intra_process(get_unique_ptr_msg(), message_info_); +} + +/* Testing dispatch with shared_ptr as input */ +TEST_P(DispatchTestsWithTA, test_intra_shared_dispatch) { + auto any_subscription_callback_to_test = GetParam().get_any_subscription_callback_to_test(); + any_subscription_callback_to_test.dispatch_intra_process(msg_shared_ptr_, message_info_); +} + +/* Testing dispatch with unique_ptr as input */ +TEST_P(DispatchTestsWithTA, test_intra_unique_dispatch) { + auto any_subscription_callback_to_test = GetParam().get_any_subscription_callback_to_test(); + any_subscription_callback_to_test.dispatch_intra_process(get_unique_ptr_msg(), message_info_); +} + +// Generic classes for testing callbacks using std::bind to class methods. +template +class BindContextImpl : public InstanceContextImpl +{ + static constexpr size_t number_of_callback_args{sizeof...(CallbackArgs)}; + +public: + using InstanceContextImpl::InstanceContextImpl; + virtual ~BindContextImpl() = default; + + void on_message(CallbackArgs ...) const {} + + rclcpp::AnySubscriptionCallback + get_any_subscription_callback_to_test() const override + { + if constexpr (number_of_callback_args == 1) { + return rclcpp::AnySubscriptionCallback().set( + std::bind(&BindContextImpl::on_message, this, std::placeholders::_1) + ); + } else { + return rclcpp::AnySubscriptionCallback().set( + std::bind(&BindContextImpl::on_message, this, std::placeholders::_1, std::placeholders::_2) + ); + } + } +}; + +template +class BindContext : public InstanceContext +{ +public: + explicit BindContext(const std::string & name) + : InstanceContext(name, std::make_shared>()) + {} +}; + +// +// Versions of `const MessageT &` +// +void const_ref_free_func(const test_msgs::msg::Empty &) {} +void const_ref_w_info_free_func(const test_msgs::msg::Empty &, const rclcpp::MessageInfo &) {} + +INSTANTIATE_TEST_SUITE_P( + ConstRefCallbackTests, + DispatchTests, + ::testing::Values( + // lambda + InstanceContext{"lambda", rclcpp::AnySubscriptionCallback().set( + [](const test_msgs::msg::Empty &) {})}, + InstanceContext{"lambda_with_info", + rclcpp::AnySubscriptionCallback().set( + [](const test_msgs::msg::Empty &, const rclcpp::MessageInfo &) {})}, + // free function + InstanceContext{"free_function", rclcpp::AnySubscriptionCallback().set( + const_ref_free_func)}, + InstanceContext{"free_function_with_info", + rclcpp::AnySubscriptionCallback().set( + const_ref_w_info_free_func)}, + // bind function + BindContext("bind_method"), + BindContext( + "bind_method_with_info") + ), + format_parameter +); + +void const_ref_ta_free_func(const MyEmpty &) {} +void const_ref_ta_w_info_free_func(const MyEmpty &, const rclcpp::MessageInfo &) {} + +INSTANTIATE_TEST_SUITE_P( + ConstRefTACallbackTests, + DispatchTestsWithTA, + ::testing::Values( + // lambda + InstanceContext{"lambda_ta", rclcpp::AnySubscriptionCallback().set( + [](const MyEmpty &) {})}, + InstanceContext{"lambda_ta_with_info", + rclcpp::AnySubscriptionCallback().set( + [](const MyEmpty &, const rclcpp::MessageInfo &) {})}, + InstanceContext{"lambda", rclcpp::AnySubscriptionCallback().set( + [](const test_msgs::msg::Empty &) {})}, + InstanceContext{"lambda_with_info", + rclcpp::AnySubscriptionCallback().set( + [](const test_msgs::msg::Empty &, const rclcpp::MessageInfo &) {})}, + // free function + InstanceContext{"free_function_ta", rclcpp::AnySubscriptionCallback().set( + const_ref_ta_free_func)}, + InstanceContext{"free_function_ta_with_info", + rclcpp::AnySubscriptionCallback().set( + const_ref_ta_w_info_free_func)}, + InstanceContext{"free_function", rclcpp::AnySubscriptionCallback().set( + const_ref_free_func)}, + InstanceContext{"free_function_with_info", + rclcpp::AnySubscriptionCallback().set( + const_ref_w_info_free_func)}, + // bind function + BindContext("bind_method_ta"), + BindContext( + "bind_method_ta_with_info"), + BindContext("bind_method"), + BindContext( + "bind_method_with_info") + ), + format_parameter_with_ta +); + +// +// Versions of `std::unique_ptr` +// +void unique_ptr_free_func(std::unique_ptr) {} +void unique_ptr_w_info_free_func( + std::unique_ptr, const rclcpp::MessageInfo &) +{} + +INSTANTIATE_TEST_SUITE_P( + UniquePtrCallbackTests, + DispatchTests, + ::testing::Values( + // lambda + InstanceContext{"lambda", rclcpp::AnySubscriptionCallback().set( + [](std::unique_ptr) {})}, + InstanceContext{"lambda_with_info", + rclcpp::AnySubscriptionCallback().set( + [](std::unique_ptr, const rclcpp::MessageInfo &) {})}, + // free function + InstanceContext{"free_function", rclcpp::AnySubscriptionCallback().set( + unique_ptr_free_func)}, + InstanceContext{"free_function_with_info", + rclcpp::AnySubscriptionCallback().set( + unique_ptr_w_info_free_func)}, + // bind function + BindContext>("bind_method"), + BindContext< + test_msgs::msg::Empty, + std::unique_ptr, + const rclcpp::MessageInfo & + >("bind_method_with_info") + ), + format_parameter +); + +void unique_ptr_ta_free_func(std::unique_ptr) {} +void unique_ptr_ta_w_info_free_func(std::unique_ptr, const rclcpp::MessageInfo &) {} + +INSTANTIATE_TEST_SUITE_P( + UniquePtrCallbackTests, + DispatchTestsWithTA, + ::testing::Values( + // lambda + InstanceContext{"lambda_ta", rclcpp::AnySubscriptionCallback().set( + [](std::unique_ptr) {})}, + InstanceContext{"lambda_ta_with_info", + rclcpp::AnySubscriptionCallback().set( + [](std::unique_ptr, const rclcpp::MessageInfo &) {})}, + InstanceContext{"lambda", rclcpp::AnySubscriptionCallback().set( + [](std::unique_ptr) {})}, + InstanceContext{"lambda_with_info", + rclcpp::AnySubscriptionCallback().set( + [](std::unique_ptr, const rclcpp::MessageInfo &) {})}, + // free function + InstanceContext{"free_function_ta", rclcpp::AnySubscriptionCallback().set( + unique_ptr_ta_free_func)}, + InstanceContext{"free_function_ta_with_info", + rclcpp::AnySubscriptionCallback().set( + unique_ptr_ta_w_info_free_func)}, + InstanceContext{"free_function", rclcpp::AnySubscriptionCallback().set( + unique_ptr_free_func)}, + InstanceContext{"free_function_with_info", + rclcpp::AnySubscriptionCallback().set( + unique_ptr_w_info_free_func)}, + // bind function + BindContext>("bind_method_ta"), + BindContext, const rclcpp::MessageInfo &>( + "bind_method_ta_with_info"), + BindContext>("bind_method"), + BindContext, const rclcpp::MessageInfo &>( + "bind_method_with_info") + ), + format_parameter_with_ta +); + +// +// Versions of `std::shared_ptr` +// +void shared_const_ptr_free_func(std::shared_ptr) {} +void shared_const_ptr_w_info_free_func( + std::shared_ptr, const rclcpp::MessageInfo &) +{} + +INSTANTIATE_TEST_SUITE_P( + SharedConstPtrCallbackTests, + DispatchTests, + ::testing::Values( + // lambda + InstanceContext{"lambda", rclcpp::AnySubscriptionCallback().set( + [](std::shared_ptr) {})}, + InstanceContext{"lambda_with_info", + rclcpp::AnySubscriptionCallback().set( + [](std::shared_ptr, const rclcpp::MessageInfo &) {})}, + // free function + InstanceContext{"free_function", rclcpp::AnySubscriptionCallback().set( + shared_const_ptr_free_func)}, + InstanceContext{"free_function_with_info", + rclcpp::AnySubscriptionCallback().set( + shared_const_ptr_w_info_free_func)}, + // bind function + BindContext>("bind_method"), + BindContext, + const rclcpp::MessageInfo &>( + "bind_method_with_info") + ), + format_parameter +); + +void shared_const_ptr_ta_free_func(std::shared_ptr) {} +void shared_const_ptr_ta_w_info_free_func( + std::shared_ptr, const rclcpp::MessageInfo &) +{} + +INSTANTIATE_TEST_SUITE_P( + SharedConstPtrCallbackTests, + DispatchTestsWithTA, + ::testing::Values( + // lambda + InstanceContext{"lambda_ta", rclcpp::AnySubscriptionCallback().set( + [](std::shared_ptr) {})}, + InstanceContext{"lambda_ta_with_info", + rclcpp::AnySubscriptionCallback().set( + [](std::shared_ptr, const rclcpp::MessageInfo &) {})}, + InstanceContext{"lambda", rclcpp::AnySubscriptionCallback().set( + [](std::shared_ptr) {})}, + InstanceContext{"lambda_with_info", + rclcpp::AnySubscriptionCallback().set( + [](std::shared_ptr, const rclcpp::MessageInfo &) {})}, + // free function + InstanceContext{"free_function_ta", rclcpp::AnySubscriptionCallback().set( + shared_const_ptr_ta_free_func)}, + InstanceContext{"free_function_ta_with_info", + rclcpp::AnySubscriptionCallback().set( + shared_const_ptr_ta_w_info_free_func)}, + InstanceContext{"free_function", rclcpp::AnySubscriptionCallback().set( + shared_const_ptr_free_func)}, + InstanceContext{"free_function_with_info", + rclcpp::AnySubscriptionCallback().set( + shared_const_ptr_w_info_free_func)}, + // bind function + BindContext>("bind_method_ta"), + BindContext, const rclcpp::MessageInfo &>( + "bind_method_ta_with_info"), + BindContext>("bind_method"), + BindContext, const rclcpp::MessageInfo &>( + "bind_method_with_info") + ), + format_parameter_with_ta +); + +// +// Versions of `const std::shared_ptr &` +// +void const_ref_shared_const_ptr_free_func(const std::shared_ptr &) {} +void const_ref_shared_const_ptr_w_info_free_func( + const std::shared_ptr &, const rclcpp::MessageInfo &) +{} + +INSTANTIATE_TEST_SUITE_P( + ConstRefSharedConstPtrCallbackTests, + DispatchTests, + ::testing::Values( + // lambda + InstanceContext{"lambda", rclcpp::AnySubscriptionCallback().set( + [](const std::shared_ptr &) {})}, + InstanceContext{"lambda_with_info", + rclcpp::AnySubscriptionCallback().set( + [](const std::shared_ptr &, const rclcpp::MessageInfo &) {})}, + // free function + InstanceContext{"free_function", rclcpp::AnySubscriptionCallback().set( + const_ref_shared_const_ptr_free_func)}, + InstanceContext{"free_function_with_info", + rclcpp::AnySubscriptionCallback().set( + const_ref_shared_const_ptr_w_info_free_func)}, + // bind function + BindContext &>("bind_method"), + BindContext &, + const rclcpp::MessageInfo &>( + "bind_method_with_info") + ), + format_parameter +); + +void const_ref_shared_const_ptr_ta_free_func(const std::shared_ptr &) {} +void const_ref_shared_const_ptr_ta_w_info_free_func( + const std::shared_ptr &, const rclcpp::MessageInfo &) +{} + +INSTANTIATE_TEST_SUITE_P( + ConstRefSharedConstPtrCallbackTests, + DispatchTestsWithTA, + ::testing::Values( + // lambda + InstanceContext{"lambda_ta", rclcpp::AnySubscriptionCallback().set( + [](const std::shared_ptr &) {})}, + InstanceContext{"lambda_ta_with_info", + rclcpp::AnySubscriptionCallback().set( + [](const std::shared_ptr &, const rclcpp::MessageInfo &) {})}, + InstanceContext{"lambda", rclcpp::AnySubscriptionCallback().set( + [](const std::shared_ptr &) {})}, + InstanceContext{"lambda_with_info", + rclcpp::AnySubscriptionCallback().set( + [](const std::shared_ptr &, const rclcpp::MessageInfo &) {})}, + // free function + InstanceContext{"free_function_ta", rclcpp::AnySubscriptionCallback().set( + const_ref_shared_const_ptr_ta_free_func)}, + InstanceContext{"free_function_ta_with_info", + rclcpp::AnySubscriptionCallback().set( + const_ref_shared_const_ptr_ta_w_info_free_func)}, + InstanceContext{"free_function", rclcpp::AnySubscriptionCallback().set( + const_ref_shared_const_ptr_free_func)}, + InstanceContext{"free_function_with_info", + rclcpp::AnySubscriptionCallback().set( + const_ref_shared_const_ptr_w_info_free_func)}, + // bind function + BindContext &>("bind_method_ta"), + BindContext &, const rclcpp::MessageInfo &>( + "bind_method_ta_with_info"), + BindContext &>("bind_method"), + BindContext &, + const rclcpp::MessageInfo &>( + "bind_method_with_info") + ), + format_parameter_with_ta +); + +// +// Versions of `std::shared_ptr` +// +void shared_ptr_free_func(std::shared_ptr) {} +void shared_ptr_w_info_free_func( + std::shared_ptr, const rclcpp::MessageInfo &) +{} + +INSTANTIATE_TEST_SUITE_P( + SharedPtrCallbackTests, + DispatchTests, + ::testing::Values( + // lambda + InstanceContext{"lambda", rclcpp::AnySubscriptionCallback().set( + [](std::shared_ptr) {})}, + InstanceContext{"lambda_with_info", + rclcpp::AnySubscriptionCallback().set( + [](std::shared_ptr, const rclcpp::MessageInfo &) {})}, + // free function + InstanceContext{"free_function", rclcpp::AnySubscriptionCallback().set( + shared_ptr_free_func)}, + InstanceContext{"free_function_with_info", + rclcpp::AnySubscriptionCallback().set( + shared_ptr_w_info_free_func)}, + // bind function + BindContext>("bind_method"), + BindContext, + const rclcpp::MessageInfo &>( + "bind_method_with_info") + ), + format_parameter +); + +void shared_ptr_ta_free_func(std::shared_ptr) {} +void shared_ptr_ta_w_info_free_func( + std::shared_ptr, const rclcpp::MessageInfo &) +{} + +INSTANTIATE_TEST_SUITE_P( + SharedPtrCallbackTests, + DispatchTestsWithTA, + ::testing::Values( + // lambda + InstanceContext{"lambda_ta", rclcpp::AnySubscriptionCallback().set( + [](std::shared_ptr) {})}, + InstanceContext{"lambda_ta_with_info", + rclcpp::AnySubscriptionCallback().set( + [](std::shared_ptr, const rclcpp::MessageInfo &) {})}, + InstanceContext{"lambda", rclcpp::AnySubscriptionCallback().set( + [](std::shared_ptr) {})}, + InstanceContext{"lambda_with_info", + rclcpp::AnySubscriptionCallback().set( + [](std::shared_ptr, const rclcpp::MessageInfo &) {})}, + // free function + InstanceContext{"free_function_ta", rclcpp::AnySubscriptionCallback().set( + shared_ptr_ta_free_func)}, + InstanceContext{"free_function_ta_with_info", + rclcpp::AnySubscriptionCallback().set( + shared_ptr_ta_w_info_free_func)}, + InstanceContext{"free_function", rclcpp::AnySubscriptionCallback().set( + shared_ptr_free_func)}, + InstanceContext{"free_function_with_info", + rclcpp::AnySubscriptionCallback().set( + shared_ptr_w_info_free_func)}, + // bind function + BindContext>("bind_method_ta"), + BindContext, const rclcpp::MessageInfo &>( + "bind_method_ta_with_info"), + BindContext>("bind_method"), + BindContext, const rclcpp::MessageInfo &>( + "bind_method_with_info") + ), + format_parameter_with_ta +); diff --git a/rclcpp/test/rclcpp/test_client.cpp b/rclcpp/test/rclcpp/test_client.cpp new file mode 100644 index 0000000000..506d981dd6 --- /dev/null +++ b/rclcpp/test/rclcpp/test_client.cpp @@ -0,0 +1,176 @@ +// 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 "rclcpp/exceptions.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "rcl_interfaces/srv/list_parameters.hpp" + +#include "../mocking_utils/patch.hpp" + +#include "test_msgs/srv/empty.hpp" + +class TestClient : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node = std::make_shared("my_node", "/ns"); + } + + void TearDown() + { + node.reset(); + } + + rclcpp::Node::SharedPtr node; +}; + +class TestClientSub : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node = std::make_shared("my_node", "/ns"); + subnode = node->create_sub_node("sub_ns"); + } + + void TearDown() + { + node.reset(); + } + + rclcpp::Node::SharedPtr node; + rclcpp::Node::SharedPtr subnode; +}; + +/* + Testing client construction and destruction. + */ +TEST_F(TestClient, construction_and_destruction) { + using rcl_interfaces::srv::ListParameters; + { + auto client = node->create_client("service"); + } + { + auto client = node->create_client( + "service", rclcpp::ServicesQoS()); + } + { + ASSERT_THROW( + { + auto client = node->create_client("invalid_service?"); + }, rclcpp::exceptions::InvalidServiceNameError); + } +} + +TEST_F(TestClient, construction_with_free_function) { + { + auto client = rclcpp::create_client( + node->get_node_base_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface(), + "service", + rclcpp::ServicesQoS(), + nullptr); + } + { + ASSERT_THROW( + { + auto client = rclcpp::create_client( + node->get_node_base_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface(), + "invalid_?service", + rclcpp::ServicesQoS(), + nullptr); + }, rclcpp::exceptions::InvalidServiceNameError); + } +} + +TEST_F(TestClient, construct_with_rcl_error) { + { + // reset() is not necessary for this exception, but handles unused return value warning + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_client_init, RCL_RET_ERROR); + EXPECT_THROW( + node->create_client("service").reset(), + rclcpp::exceptions::RCLError); + } + { + // reset() is required for this one + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_client_fini, RCL_RET_ERROR); + EXPECT_NO_THROW(node->create_client("service").reset()); + } +} + +TEST_F(TestClient, wait_for_service) { + const std::string service_name = "service"; + auto client = node->create_client(service_name); + EXPECT_FALSE(client->wait_for_service(std::chrono::nanoseconds(0))); + EXPECT_FALSE(client->wait_for_service(std::chrono::milliseconds(10))); + + auto callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}; + + auto service = + node->create_service(service_name, std::move(callback)); + + EXPECT_TRUE(client->wait_for_service(std::chrono::nanoseconds(-1))); + EXPECT_TRUE(client->service_is_ready()); +} + +/* + Testing client construction and destruction for subnodes. + */ +TEST_F(TestClientSub, construction_and_destruction) { + using rcl_interfaces::srv::ListParameters; + { + auto client = subnode->create_client("service"); + EXPECT_STREQ(client->get_service_name(), "/ns/sub_ns/service"); + } + + { + ASSERT_THROW( + { + auto client = node->create_client("invalid_service?"); + }, rclcpp::exceptions::InvalidServiceNameError); + } +} diff --git a/rclcpp/test/rclcpp/test_client_common.cpp b/rclcpp/test/rclcpp/test_client_common.cpp new file mode 100644 index 0000000000..65475bd8fc --- /dev/null +++ b/rclcpp/test/rclcpp/test_client_common.cpp @@ -0,0 +1,591 @@ +// Copyright 2024 Sony Group Corporation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT 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 "../mocking_utils/patch.hpp" +#include "../utils/rclcpp_gtest_macros.hpp" + +#include "rclcpp/create_generic_client.hpp" +#include "rclcpp/exceptions.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "test_msgs/srv/empty.hpp" + +template +class TestAllClientTypesWithServer : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node = std::make_shared("test_node", "ns"); + + auto callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}; + + service = node->create_service(service_name, std::move(callback)); + } + + template + auto SendEmptyRequestAndWait( + std::chrono::milliseconds timeout = std::chrono::milliseconds(1000)) + { + if constexpr (std::is_same_v) { + return GenericClientSendEmptyRequestAndWait(timeout); + } else if constexpr (std::is_same_v>) { + return ClientSendEmptyRequestAndWait(timeout); + } else { + return ::testing::AssertionFailure() << "No test for this client type"; + } + } + + ::testing::AssertionResult GenericClientSendEmptyRequestAndWait( + std::chrono::milliseconds timeout = std::chrono::milliseconds(1000)) + { + auto client = node->create_generic_client(service_name, "test_msgs/srv/Empty"); + if (!client->wait_for_service()) { + return ::testing::AssertionFailure() << "Service is not available yet"; + } + + auto request = std::make_shared(); + + auto future_and_req_id = client->async_send_request(request.get()); + + auto ret = rclcpp::spin_until_future_complete(node, future_and_req_id, timeout); + if (ret != rclcpp::FutureReturnCode::SUCCESS) { + return ::testing::AssertionFailure() << "Waiting for response timed out"; + } + + if (client->remove_pending_request(future_and_req_id.request_id)) { + return ::testing::AssertionFailure() << "Should not be able to remove a finished request"; + } + + return ::testing::AssertionSuccess(); + } + + ::testing::AssertionResult ClientSendEmptyRequestAndWait( + std::chrono::milliseconds timeout = std::chrono::milliseconds(1000)) + { + using SharedFuture = rclcpp::Client::SharedFuture; + + auto client = node->create_client(service_name); + if (!client->wait_for_service()) { + return ::testing::AssertionFailure() << "Waiting for service failed"; + } + + auto request = std::make_shared(); + bool received_response = false; + ::testing::AssertionResult request_result = ::testing::AssertionSuccess(); + auto callback = [&received_response, &request_result](SharedFuture future_response) { + if (nullptr == future_response.get()) { + request_result = ::testing::AssertionFailure() << "Future response was null"; + } + received_response = true; + }; + + auto req_id = client->async_send_request(request, std::move(callback)); + + auto start = std::chrono::steady_clock::now(); + while (!received_response && + (std::chrono::steady_clock::now() - start) < timeout) + { + rclcpp::spin_some(node); + } + + if (!received_response) { + return ::testing::AssertionFailure() << "Waiting for response timed out"; + } + if (client->remove_pending_request(req_id)) { + return ::testing::AssertionFailure() << "Should not be able to remove a finished request"; + } + + return request_result; + } + + template + auto create_client( + rclcpp::Node::SharedPtr node, + const std::string service_name = "empty_service", + const rclcpp::QoS & qos = rclcpp::ServicesQoS()) + { + if constexpr (std::is_same_v) { + return node->create_generic_client(service_name, "test_msgs/srv/Empty", qos); + } else if constexpr (std::is_same_v>) { + return node->template create_client(service_name, qos); + } else { + ASSERT_TRUE(false) << "Not know how to create this kind of client"; + } + } + + template + auto async_send_request(std::shared_ptr client, std::shared_ptr request) + { + if constexpr (std::is_same_v) { + return client->async_send_request(request.get()); + } else if constexpr (std::is_same_v>) { + return client->async_send_request(request); + } else { + ASSERT_TRUE(false) << "Not know how to send request for this kind of client"; + } + } + + template + auto take_response( + std::shared_ptr client, + ResponseType & response, + std::shared_ptr request_header) + { + if constexpr (std::is_same_v) { + return client->take_response(static_cast(&response), *request_header.get()); + } else if constexpr (std::is_same_v>) { + return client->take_response(response, *request_header.get()); + } else { + ASSERT_TRUE(false) << "Not know how to take response for this kind of client"; + } + } + + std::shared_ptr node; + std::shared_ptr> service; + const std::string service_name{"empty_service"}; +}; + +using ClientType = + ::testing::Types< + rclcpp::Client, + rclcpp::GenericClient>; + +class ClientTypeNames +{ +public: + template + static std::string GetName(int idx) + { + (void)idx; + if (std::is_same_v>) { + return "Client"; + } + + if (std::is_same_v) { + return "GenericClient"; + } + + return ""; + } +}; + +TYPED_TEST_SUITE(TestAllClientTypesWithServer, ClientType, ClientTypeNames); + +TYPED_TEST(TestAllClientTypesWithServer, async_send_request) +{ + using ClientType = TypeParam; + EXPECT_TRUE(this->template SendEmptyRequestAndWait()); +} + +TYPED_TEST(TestAllClientTypesWithServer, test_client_remove_pending_request) +{ + using ClientType = TypeParam; + + auto client = this->template create_client(this->node); + + auto request = std::make_shared(); + + auto future_and_req_id = this->template async_send_request< + ClientType, test_msgs::srv::Empty::Request>(client, request); + + EXPECT_TRUE(client->remove_pending_request(future_and_req_id.request_id)); +} + +TYPED_TEST(TestAllClientTypesWithServer, prune_requests_older_than_no_pruned) +{ + using ClientType = TypeParam; + + auto client = this->template create_client(this->node); + + auto request = std::make_shared(); + + auto future = this->template async_send_request< + ClientType, test_msgs::srv::Empty::Request>(client, request); + auto time = std::chrono::system_clock::now() + std::chrono::seconds(1); + + EXPECT_EQ(1u, client->prune_requests_older_than(time)); +} + +TYPED_TEST(TestAllClientTypesWithServer, prune_requests_older_than_with_pruned) +{ + using ClientType = TypeParam; + + auto client = this->template create_client(this->node); + + auto request = std::make_shared(); + + auto future = this->template async_send_request< + ClientType, test_msgs::srv::Empty::Request>(client, request); + auto time = std::chrono::system_clock::now() + std::chrono::seconds(1); + + std::vector pruned_requests; + EXPECT_EQ(1u, client->prune_requests_older_than(time, &pruned_requests)); + ASSERT_EQ(1u, pruned_requests.size()); + EXPECT_EQ(future.request_id, pruned_requests[0]); +} + +TYPED_TEST(TestAllClientTypesWithServer, async_send_request_rcl_send_request_error) +{ + using ClientType = TypeParam; + + // Checking rcl_send_request in rclcpp::Client::async_send_request() or + // rclcpp::GenericClient::async_send_request() + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_send_request, RCL_RET_ERROR); + EXPECT_THROW(this->template SendEmptyRequestAndWait(), rclcpp::exceptions::RCLError); +} + +TYPED_TEST(TestAllClientTypesWithServer, async_send_request_rcl_service_server_is_available_error) +{ + using ClientType = TypeParam; + + { + // Checking rcl_service_server_is_available in rclcpp::ClientBase::service_is_ready + auto client = this->template create_client(this->node); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_service_server_is_available, RCL_RET_NODE_INVALID); + EXPECT_THROW(client->service_is_ready(), rclcpp::exceptions::RCLError); + } + { + // Checking rcl_service_server_is_available exception in rclcpp::ClientBase::service_is_ready + auto client = this->template create_client(this->node); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_service_server_is_available, RCL_RET_ERROR); + EXPECT_THROW(client->service_is_ready(), rclcpp::exceptions::RCLError); + } + { + // Checking rcl_service_server_is_available exception in rclcpp::ClientBase::service_is_ready + auto client = this->template create_client(this->node); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_service_server_is_available, RCL_RET_ERROR); + EXPECT_THROW(client->service_is_ready(), rclcpp::exceptions::RCLError); + } +} + +TYPED_TEST(TestAllClientTypesWithServer, take_response) +{ + using ClientType = TypeParam; + + auto client = this->template create_client(this->node); + ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1))); + auto request = std::make_shared(); + auto request_header = client->create_request_header(); + test_msgs::srv::Empty::Response response; + + this->template async_send_request< + ClientType, test_msgs::srv::Empty::Request>(client, request); + + EXPECT_FALSE(this->take_response(client, response, request_header)); + + { + // Checking rcl_take_response in rclcpp::ClientBase::take_type_erased_response + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_take_response, RCL_RET_OK); + EXPECT_TRUE(this->take_response(client, response, request_header)); + } + { + // Checking rcl_take_response in rclcpp::ClientBase::take_type_erased_response + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_take_response, RCL_RET_CLIENT_TAKE_FAILED); + EXPECT_FALSE(this->take_response(client, response, request_header)); + } + { + // Checking rcl_take_response in rclcpp::ClientBase::take_type_erased_response + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_take_response, RCL_RET_ERROR); + EXPECT_THROW( + this->take_response(client, response, request_header), + rclcpp::exceptions::RCLError); + } +} + +/* + Testing on_new_response callbacks. + */ +TYPED_TEST(TestAllClientTypesWithServer, on_new_response_callback) +{ + using ClientType = TypeParam; + + auto client_node = std::make_shared("test_client_node", "ns"); + auto server_node = std::make_shared("test_server_node", "ns"); + + rclcpp::ServicesQoS client_qos; + client_qos.keep_last(3); + + auto client = this->template create_client(client_node, "test_service", client_qos); + + std::atomic server_requests_count {0}; + auto server_callback = [&server_requests_count]( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {server_requests_count++;}; + auto server = server_node->create_service( + "test_service", server_callback, client_qos); + auto request = std::make_shared(); + + std::atomic c1 {0}; + auto increase_c1_cb = [&c1](size_t count_msgs) {c1 += count_msgs;}; + client->set_on_new_response_callback(increase_c1_cb); + + this->template async_send_request(client, request); + auto start = std::chrono::steady_clock::now(); + while (server_requests_count == 0 && + (std::chrono::steady_clock::now() - start) < std::chrono::seconds(10)) + { + rclcpp::spin_some(server_node); + } + + ASSERT_EQ(server_requests_count, 1u); + + start = std::chrono::steady_clock::now(); + do { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } while (c1 == 0 && std::chrono::steady_clock::now() - start < std::chrono::seconds(10)); + + EXPECT_EQ(c1.load(), 1u); + + std::atomic c2 {0}; + auto increase_c2_cb = [&c2](size_t count_msgs) {c2 += count_msgs;}; + client->set_on_new_response_callback(increase_c2_cb); + + this->template async_send_request(client, request); + start = std::chrono::steady_clock::now(); + while (server_requests_count == 1 && + (std::chrono::steady_clock::now() - start) < std::chrono::seconds(10)) + { + rclcpp::spin_some(server_node); + } + + ASSERT_EQ(server_requests_count, 2u); + + start = std::chrono::steady_clock::now(); + do { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } while (c1 == 0 && std::chrono::steady_clock::now() - start < std::chrono::seconds(10)); + + EXPECT_EQ(c1.load(), 1u); + EXPECT_EQ(c2.load(), 1u); + + client->clear_on_new_response_callback(); + + this->template async_send_request(client, request); + this->template async_send_request(client, request); + this->template async_send_request(client, request); + start = std::chrono::steady_clock::now(); + while (server_requests_count < 5 && + (std::chrono::steady_clock::now() - start) < std::chrono::seconds(10)) + { + rclcpp::spin_some(server_node); + } + + ASSERT_EQ(server_requests_count, 5u); + + std::atomic c3 {0}; + auto increase_c3_cb = [&c3](size_t count_msgs) {c3 += count_msgs;}; + client->set_on_new_response_callback(increase_c3_cb); + + start = std::chrono::steady_clock::now(); + do { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } while (c3 < 3 && std::chrono::steady_clock::now() - start < std::chrono::seconds(10)); + + EXPECT_EQ(c1.load(), 1u); + EXPECT_EQ(c2.load(), 1u); + EXPECT_EQ(c3.load(), 3u); + + std::function invalid_cb = nullptr; + EXPECT_THROW(client->set_on_new_response_callback(invalid_cb), std::invalid_argument); +} + +TYPED_TEST(TestAllClientTypesWithServer, client_qos) +{ + using ClientType = TypeParam; + + rclcpp::ServicesQoS qos_profile; + qos_profile.liveliness(rclcpp::LivelinessPolicy::Automatic); + rclcpp::Duration duration(std::chrono::nanoseconds(1)); + qos_profile.deadline(duration); + qos_profile.lifespan(duration); + qos_profile.liveliness_lease_duration(duration); + + auto client = this->template create_client( + this->node, this->service_name, qos_profile); + + auto rp_qos = client->get_request_publisher_actual_qos(); + auto rs_qos = client->get_response_subscription_actual_qos(); + + EXPECT_EQ(qos_profile, rp_qos); + // Lifespan has no meaning for subscription/readers + rs_qos.lifespan(qos_profile.lifespan()); + EXPECT_EQ(qos_profile, rs_qos); +} + +TYPED_TEST(TestAllClientTypesWithServer, rcl_client_request_publisher_get_actual_qos_error) +{ + using ClientType = TypeParam; + + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_client_request_publisher_get_actual_qos, nullptr); + auto client = this->template create_client(this->node, "service"); + RCLCPP_EXPECT_THROW_EQ( + client->get_request_publisher_actual_qos(), + std::runtime_error("failed to get client's request publisher qos settings: error not set")); +} + +TYPED_TEST(TestAllClientTypesWithServer, rcl_client_response_subscription_get_actual_qos_error) +{ + using ClientType = TypeParam; + + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_client_response_subscription_get_actual_qos, nullptr); + auto client = this->template create_client(this->node, "service"); + RCLCPP_EXPECT_THROW_EQ( + client->get_response_subscription_actual_qos(), + std::runtime_error("failed to get client's response subscription qos settings: error not set")); +} + +// The following tests are only for rclcpp::Client +void client_async_send_request_callback_with_request( + rclcpp::Node::SharedPtr node, const std::string service_name) +{ + using SharedFutureWithRequest = + rclcpp::Client::SharedFutureWithRequest; + + auto client = node->create_client(service_name); + ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1))); + + auto request = std::make_shared(); + bool received_response = false; + auto callback = [&request, &received_response](SharedFutureWithRequest future) { + auto request_response_pair = future.get(); + EXPECT_EQ(request, request_response_pair.first); + EXPECT_NE(nullptr, request_response_pair.second); + received_response = true; + }; + auto req_id = client->async_send_request(request, std::move(callback)); + + auto start = std::chrono::steady_clock::now(); + while (!received_response && + (std::chrono::steady_clock::now() - start) < std::chrono::seconds(1)) + { + rclcpp::spin_some(node); + } + EXPECT_TRUE(received_response); + EXPECT_FALSE(client->remove_pending_request(req_id)); +} +TYPED_TEST(TestAllClientTypesWithServer, async_send_request_callback_with_request) +{ + using ClientType = TypeParam; + + if (std::is_same_v>) { + client_async_send_request_callback_with_request(this->node, this->service_name); + } else if (std::is_same_v) { + GTEST_SKIP() << "Skipping test for GenericClient"; + } else { + GTEST_SKIP() << "Skipping test"; + } +} + +void client_qos_depth(rclcpp::Node::SharedPtr node) +{ + using namespace std::literals::chrono_literals; + + rclcpp::ServicesQoS client_qos_profile; + client_qos_profile.keep_last(2); + + auto client = node->create_client("test_qos_depth", client_qos_profile); + + uint64_t server_cb_count_ = 0; + auto server_callback = [&]( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {server_cb_count_++;}; + + auto server_node = std::make_shared("server_node", "/ns"); + + rclcpp::QoS server_qos(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); + + auto server = server_node->create_service( + "test_qos_depth", std::move(server_callback), server_qos); + + auto request = std::make_shared(); + ::testing::AssertionResult request_result = ::testing::AssertionSuccess(); + + using SharedFuture = rclcpp::Client::SharedFuture; + uint64_t client_cb_count_ = 0; + auto client_callback = [&client_cb_count_, &request_result](SharedFuture future_response) { + if (nullptr == future_response.get()) { + request_result = ::testing::AssertionFailure() << "Future response was null"; + } + client_cb_count_++; + }; + + uint64_t client_requests = 5; + for (uint64_t i = 0; i < client_requests; i++) { + client->async_send_request(request, client_callback); + std::this_thread::sleep_for(10ms); + } + + auto start = std::chrono::steady_clock::now(); + while ((server_cb_count_ < client_requests) && + (std::chrono::steady_clock::now() - start) < 2s) + { + rclcpp::spin_some(server_node); + std::this_thread::sleep_for(2ms); + } + + EXPECT_GT(server_cb_count_, client_qos_profile.depth()); + + start = std::chrono::steady_clock::now(); + while ((client_cb_count_ < client_qos_profile.depth()) && + (std::chrono::steady_clock::now() - start) < 1s) + { + rclcpp::spin_some(node); + } + + // Spin an extra time to check if client QoS depth has been ignored, + // so more client callbacks might be called than expected. + rclcpp::spin_some(node); + + EXPECT_EQ(client_cb_count_, client_qos_profile.depth()); +} + +TYPED_TEST(TestAllClientTypesWithServer, qos_depth) +{ + using ClientType = TypeParam; + + if (std::is_same_v>) { + client_qos_depth(this->node); + } else if (std::is_same_v) { + GTEST_SKIP() << "Skipping test for GenericClient"; + } else { + GTEST_SKIP() << "Skipping test"; + } +} diff --git a/rclcpp/test/rclcpp/test_clock.cpp b/rclcpp/test/rclcpp/test_clock.cpp new file mode 100644 index 0000000000..2ddd775707 --- /dev/null +++ b/rclcpp/test/rclcpp/test_clock.cpp @@ -0,0 +1,229 @@ +// Copyright 2024 Cellumation GmbH +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT 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/error_handling.h" +#include "rcl/time.h" +#include "rclcpp/clock.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/time_source.hpp" + +#include "../utils/rclcpp_gtest_macros.hpp" + +using namespace std::chrono_literals; + +class TestClockWakeup : public ::testing::TestWithParam +{ +public: + void test_wakeup_before_sleep(const rclcpp::Clock::SharedPtr & clock) + { + std::atomic_bool thread_finished = false; + + std::thread wait_thread = std::thread( + [&clock, &thread_finished]() + { + // make sure the thread starts sleeping late + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + clock->sleep_until(clock->now() + std::chrono::seconds(3)); + thread_finished = true; + }); + + // notify the clock, that the sleep shall be interrupted + clock->cancel_sleep_or_wait(); + + auto start_time = std::chrono::steady_clock::now(); + auto cur_time = start_time; + while (!thread_finished && start_time + std::chrono::seconds(1) > cur_time) { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + cur_time = std::chrono::steady_clock::now(); + } + + wait_thread.join(); + + EXPECT_TRUE(thread_finished); + EXPECT_LT(cur_time, start_time + std::chrono::seconds(1)); + } + + void test_wakeup_after_sleep(const rclcpp::Clock::SharedPtr & clock) + { + std::atomic_bool thread_finished = false; + + std::thread wait_thread = std::thread( + [&clock, &thread_finished]() + { + clock->sleep_until(clock->now() + std::chrono::seconds(3)); + thread_finished = true; + }); + + // make sure the thread is already sleeping before we send the cancel + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + // notify the clock, that the sleep shall be interrupted + clock->cancel_sleep_or_wait(); + + auto start_time = std::chrono::steady_clock::now(); + auto cur_time = start_time; + while (!thread_finished && start_time + std::chrono::seconds(1) > cur_time) { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + cur_time = std::chrono::steady_clock::now(); + } + + wait_thread.join(); + + EXPECT_TRUE(thread_finished); + EXPECT_LT(cur_time, start_time + std::chrono::seconds(1)); + } + +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node = std::make_shared("my_node"); + } + + void TearDown() + { + node.reset(); + } + + rclcpp::Node::SharedPtr node; +}; + +INSTANTIATE_TEST_SUITE_P( + Clocks, + TestClockWakeup, + ::testing::Values( + RCL_SYSTEM_TIME, RCL_ROS_TIME, RCL_STEADY_TIME +)); + +TEST_P(TestClockWakeup, wakeup_sleep) { + auto clock = std::make_shared(GetParam()); + test_wakeup_after_sleep(clock); + test_wakeup_before_sleep(clock); +} + +TEST_F(TestClockWakeup, wakeup_sleep_ros_time_active) { + node->set_parameter({"use_sim_time", true}); + auto clock = std::make_shared(RCL_ROS_TIME); + rclcpp::TimeSource time_source(node); + time_source.attachClock(clock); + + EXPECT_TRUE(clock->ros_time_is_active()); + + test_wakeup_after_sleep(clock); + test_wakeup_before_sleep(clock); +} + +TEST_F(TestClockWakeup, no_wakeup_on_sim_time) { + node->set_parameter({"use_sim_time", true}); + auto clock = std::make_shared(RCL_ROS_TIME); + rclcpp::TimeSource time_source(node); + time_source.attachClock(clock); + + EXPECT_TRUE(clock->ros_time_is_active()); + + std::atomic_bool thread_finished = false; + + std::thread wait_thread = std::thread( + [&clock, &thread_finished]() + { + // make sure the thread starts sleeping late + clock->sleep_until(clock->now() + std::chrono::milliseconds(10)); + thread_finished = true; + }); + + // make sure, that the sim time clock does not wakeup, as no clock is provided + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + EXPECT_FALSE(thread_finished); + + // notify the clock, that the sleep shall be interrupted + clock->cancel_sleep_or_wait(); + + auto start_time = std::chrono::steady_clock::now(); + auto cur_time = start_time; + while (!thread_finished && start_time + std::chrono::seconds(1) > cur_time) { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + cur_time = std::chrono::steady_clock::now(); + } + + wait_thread.join(); + + EXPECT_TRUE(thread_finished); + EXPECT_LT(cur_time, start_time + std::chrono::seconds(1)); +} + +TEST_F(TestClockWakeup, multiple_threads_wait_on_one_clock) { + auto clock = std::make_shared(RCL_ROS_TIME); + + std::vector thread_finished(10, false); + + std::vector threads; + + for (size_t nr = 0; nr < thread_finished.size(); nr++) { + threads.push_back( + std::thread( + [&clock, &thread_finished, nr]() + { + // make sure the thread starts sleeping late + clock->sleep_until(clock->now() + std::chrono::seconds(10)); + thread_finished[nr] = true; + })); + } + + // wait a bit so all threads can execute the sleep_until + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + + for (const bool & finished : thread_finished) { + EXPECT_FALSE(finished); + } + + rclcpp::shutdown(); + + auto start_time = std::chrono::steady_clock::now(); + auto cur_time = start_time; + bool threads_finished = false; + while (!threads_finished && start_time + std::chrono::seconds(1) > cur_time) { + threads_finished = true; + for (const bool finished : thread_finished) { + if (!finished) { + threads_finished = false; + } + } + + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + cur_time = std::chrono::steady_clock::now(); + } + + for (const bool finished : thread_finished) { + EXPECT_TRUE(finished); + } + + for (auto & thread : threads) { + thread.join(); + } + + EXPECT_LT(cur_time, start_time + std::chrono::seconds(1)); +} diff --git a/rclcpp/test/rclcpp/test_context.cpp b/rclcpp/test/rclcpp/test_context.cpp new file mode 100644 index 0000000000..c8779371fe --- /dev/null +++ b/rclcpp/test/rclcpp/test_context.cpp @@ -0,0 +1,238 @@ +// Copyright 2023 Sony Group Corporation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT 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 "rclcpp/context.hpp" +#include "rclcpp/rclcpp.hpp" + +TEST(TestContext, check_pre_shutdown_callback_order) { + auto context = std::make_shared(); + context->init(0, nullptr); + + int result[4] = {0, 0, 0, 0}; + int index = 0; + + auto callback1 = [&result, &index]() { + result[index] = 1; + index++; + }; + auto callback2 = [&result, &index]() { + result[index] = 2; + index++; + }; + auto callback3 = [&result, &index]() { + result[index] = 3; + index++; + }; + auto callback4 = [&result, &index]() { + result[index] = 4; + index++; + }; + + context->add_pre_shutdown_callback(callback1); + context->add_pre_shutdown_callback(callback2); + context->add_pre_shutdown_callback(callback3); + context->add_pre_shutdown_callback(callback4); + + context->shutdown("for test"); + + EXPECT_TRUE(result[0] == 1 && result[1] == 2 && result[2] == 3 && result[3] == 4); +} + +TEST(TestContext, check_on_shutdown_callback_order) { + auto context = std::make_shared(); + context->init(0, nullptr); + + int result[4] = {0, 0, 0, 0}; + int index = 0; + + auto callback1 = [&result, &index]() { + result[index] = 1; + index++; + }; + auto callback2 = [&result, &index]() { + result[index] = 2; + index++; + }; + auto callback3 = [&result, &index]() { + result[index] = 3; + index++; + }; + auto callback4 = [&result, &index]() { + result[index] = 4; + index++; + }; + + context->add_on_shutdown_callback(callback1); + context->add_on_shutdown_callback(callback2); + context->add_on_shutdown_callback(callback3); + context->add_on_shutdown_callback(callback4); + + context->shutdown("for test"); + + EXPECT_TRUE(result[0] == 1 && result[1] == 2 && result[2] == 3 && result[3] == 4); +} + +TEST(TestContext, check_mixed_register_shutdown_callback_order) { + auto context = std::make_shared(); + context->init(0, nullptr); + + int result[8] = {0, 0, 0, 0, 0, 0, 0, 0}; + int index = 0; + + auto callback1 = [&result, &index]() { + result[index] = 1; + index++; + }; + auto callback2 = [&result, &index]() { + result[index] = 2; + index++; + }; + auto callback3 = [&result, &index]() { + result[index] = 3; + index++; + }; + auto callback4 = [&result, &index]() { + result[index] = 4; + index++; + }; + auto callback5 = [&result, &index]() { + result[index] = 5; + index++; + }; + auto callback6 = [&result, &index]() { + result[index] = 6; + index++; + }; + auto callback7 = [&result, &index]() { + result[index] = 7; + index++; + }; + auto callback8 = [&result, &index]() { + result[index] = 8; + index++; + }; + + // Mixed register + context->add_pre_shutdown_callback(callback1); + context->add_on_shutdown_callback(callback5); + context->add_pre_shutdown_callback(callback2); + context->add_on_shutdown_callback(callback6); + context->add_pre_shutdown_callback(callback3); + context->add_on_shutdown_callback(callback7); + context->add_pre_shutdown_callback(callback4); + context->add_on_shutdown_callback(callback8); + + context->shutdown("for test"); + + EXPECT_TRUE( + result[0] == 1 && result[1] == 2 && result[2] == 3 && result[3] == 4 && + result[4] == 5 && result[5] == 6 && result[6] == 7 && result[7] == 8); +} + +TEST(TestContext, check_pre_shutdown_callback_order_after_del) { + auto context = std::make_shared(); + context->init(0, nullptr); + + int result[4] = {0, 0, 0, 0}; + int index = 0; + + auto callback1 = [&result, &index]() { + result[index] = 1; + index++; + }; + auto callback2 = [&result, &index]() { + result[index] = 2; + index++; + }; + auto callback3 = [&result, &index]() { + result[index] = 3; + index++; + }; + auto callback4 = [&result, &index]() { + result[index] = 4; + index++; + }; + + context->add_pre_shutdown_callback(callback1); + auto callback_handle = context->add_pre_shutdown_callback(callback2); + context->add_pre_shutdown_callback(callback3); + context->add_pre_shutdown_callback(callback4); + + EXPECT_TRUE(context->remove_pre_shutdown_callback(callback_handle)); + EXPECT_FALSE(context->remove_pre_shutdown_callback(callback_handle)); + + context->shutdown("for test"); + + EXPECT_TRUE(result[0] == 1 && result[1] == 3 && result[2] == 4 && result[3] == 0); +} + +TEST(TestContext, check_on_shutdown_callback_order_after_del) { + auto context = std::make_shared(); + context->init(0, nullptr); + + int result[4] = {0, 0, 0, 0}; + int index = 0; + + auto callback1 = [&result, &index]() { + result[index] = 1; + index++; + }; + auto callback2 = [&result, &index]() { + result[index] = 2; + index++; + }; + auto callback3 = [&result, &index]() { + result[index] = 3; + index++; + }; + auto callback4 = [&result, &index]() { + result[index] = 4; + index++; + }; + + context->add_on_shutdown_callback(callback1); + auto callback_handle = context->add_on_shutdown_callback(callback2); + context->add_on_shutdown_callback(callback3); + context->add_on_shutdown_callback(callback4); + + EXPECT_TRUE(context->remove_on_shutdown_callback(callback_handle)); + EXPECT_FALSE(context->remove_on_shutdown_callback(callback_handle)); + + context->shutdown("for test"); + + EXPECT_TRUE(result[0] == 1 && result[1] == 3 && result[2] == 4 && result[3] == 0); +} + +// This test checks that contexts will be properly destroyed when leaving a scope, after a +// guard condition has been created. +TEST(TestContext, check_context_destroyed) { + rclcpp::Context::SharedPtr ctx; + { + ctx = std::make_shared(); + ctx->init(0, nullptr); + + auto group = std::make_shared( + rclcpp::CallbackGroupType::MutuallyExclusive, + ctx->weak_from_this(), + false); + + rclcpp::GuardCondition::SharedPtr gc = group->get_notify_guard_condition(); + ASSERT_NE(gc, nullptr); + + ASSERT_EQ(ctx.use_count(), 1u); + } + + ASSERT_EQ(ctx.use_count(), 1u); +} diff --git a/rclcpp/test/rclcpp/test_copy_all_parameter_values.cpp b/rclcpp/test/rclcpp/test_copy_all_parameter_values.cpp new file mode 100644 index 0000000000..e3020efd1e --- /dev/null +++ b/rclcpp/test/rclcpp/test_copy_all_parameter_values.cpp @@ -0,0 +1,88 @@ +// Copyright 2023 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT 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 "rclcpp/copy_all_parameter_values.hpp" +#include "rclcpp/rclcpp.hpp" + +class TestNode : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestNode, TestParamCopying) +{ + auto node1 = std::make_shared("test_node1"); + auto node2 = std::make_shared("test_node2"); + + // Tests for (1) multiple types, (2) recursion, (3) overriding values + node1->declare_parameter("Foo1", rclcpp::ParameterValue(std::string(("bar1")))); + node1->declare_parameter("Foo2", rclcpp::ParameterValue(0.123)); + node1->declare_parameter("Foo", rclcpp::ParameterValue(std::string(("bar")))); + node1->declare_parameter("Foo.bar", rclcpp::ParameterValue(std::string(("steve")))); + node2->declare_parameter("Foo", rclcpp::ParameterValue(std::string(("barz2")))); + + // Show Node2 is empty of Node1's parameters, but contains its own + EXPECT_FALSE(node2->has_parameter("Foo1")); + EXPECT_FALSE(node2->has_parameter("Foo2")); + EXPECT_FALSE(node2->has_parameter("Foo.bar")); + EXPECT_TRUE(node2->has_parameter("Foo")); + EXPECT_EQ(node2->get_parameter("Foo").as_string(), std::string("barz2")); + + bool override = false; + rclcpp::copy_all_parameter_values(node1, node2, override); + + // Test new parameters exist, of expected value, and original param is not overridden + EXPECT_TRUE(node2->has_parameter("Foo1")); + EXPECT_EQ(node2->get_parameter("Foo1").as_string(), std::string("bar1")); + EXPECT_TRUE(node2->has_parameter("Foo2")); + EXPECT_EQ(node2->get_parameter("Foo2").as_double(), 0.123); + EXPECT_TRUE(node2->has_parameter("Foo.bar")); + EXPECT_EQ(node2->get_parameter("Foo.bar").as_string(), std::string("steve")); + EXPECT_TRUE(node2->has_parameter("Foo")); + EXPECT_EQ(node2->get_parameter("Foo").as_string(), std::string("barz2")); + + // Test if parameter overrides are permissible that Node2's value is overridden + override = true; + rclcpp::copy_all_parameter_values(node1, node2, override); + EXPECT_EQ(node2->get_parameter("Foo").as_string(), std::string("bar")); +} + +TEST_F(TestNode, TestParamCopyingExceptions) +{ + auto node1 = std::make_shared("test_node1"); + auto node2 = std::make_shared("test_node2"); + + // Tests for Parameter value conflicts handled + node1->declare_parameter("Foo", rclcpp::ParameterValue(std::string(("bar")))); + node2->declare_parameter("Foo", rclcpp::ParameterValue(0.123)); + + bool override = true; + EXPECT_NO_THROW( + rclcpp::copy_all_parameter_values(node1, node2, override)); + + // Tests for Parameter read-only handled + node1->declare_parameter("Foo1", rclcpp::ParameterValue(std::string(("bar")))); + node2->declare_parameter("Foo1", rclcpp::ParameterValue(0.123)); + EXPECT_NO_THROW(rclcpp::copy_all_parameter_values(node1, node2, override)); +} diff --git a/rclcpp/test/rclcpp/test_create_subscription.cpp b/rclcpp/test/rclcpp/test_create_subscription.cpp new file mode 100644 index 0000000000..06536116a1 --- /dev/null +++ b/rclcpp/test/rclcpp/test_create_subscription.cpp @@ -0,0 +1,94 @@ +// 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 "rclcpp/create_subscription.hpp" +#include "rclcpp/node.hpp" +#include "test_msgs/msg/empty.hpp" + +using namespace std::chrono_literals; + +class TestCreateSubscription : public ::testing::Test +{ +public: + void SetUp() override + { + rclcpp::init(0, nullptr); + } + + void TearDown() override + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestCreateSubscription, create) { + auto node = std::make_shared("my_node", "/ns"); + const rclcpp::QoS qos(10); + auto options = rclcpp::SubscriptionOptions(); + auto callback = [](test_msgs::msg::Empty::ConstSharedPtr) {}; + auto subscription = + rclcpp::create_subscription(node, "topic_name", qos, callback, options); + + ASSERT_NE(nullptr, subscription); + EXPECT_STREQ("/ns/topic_name", subscription->get_topic_name()); +} + +TEST_F(TestCreateSubscription, create_with_overriding_options) { + auto node = std::make_shared("my_node", "/ns"); + const rclcpp::QoS qos(10); + auto options = rclcpp::SubscriptionOptions(); + options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); + auto callback = [](test_msgs::msg::Empty::ConstSharedPtr) {}; + auto subscription = + rclcpp::create_subscription(node, "topic_name", qos, callback, options); + + ASSERT_NE(nullptr, subscription); + EXPECT_STREQ("/ns/topic_name", subscription->get_topic_name()); +} + +TEST_F(TestCreateSubscription, create_separated_node_topics_and_parameters) { + auto node = std::make_shared("my_node", "/ns"); + const rclcpp::QoS qos(10); + auto options = rclcpp::SubscriptionOptions(); + auto callback = [](test_msgs::msg::Empty::ConstSharedPtr) {}; + + auto node_parameters = node->get_node_parameters_interface(); + auto node_topics = node->get_node_topics_interface(); + auto subscription = rclcpp::create_subscription( + node_parameters, node_topics, "topic_name", qos, callback, options); + + ASSERT_NE(nullptr, subscription); + EXPECT_STREQ("/ns/topic_name", subscription->get_topic_name()); +} + +TEST_F(TestCreateSubscription, create_with_statistics) { + auto node = std::make_shared("my_node", "/ns"); + const rclcpp::QoS qos(10); + auto options = rclcpp::SubscriptionOptions(); + options.topic_stats_options.state = rclcpp::TopicStatisticsState::Enable; + options.topic_stats_options.publish_topic = "topic_statistics"; + options.topic_stats_options.publish_period = 5min; + + auto callback = [](test_msgs::msg::Empty::ConstSharedPtr) {}; + auto subscription = + rclcpp::create_subscription(node, "topic_name", qos, callback, options); + + ASSERT_NE(nullptr, subscription); + EXPECT_STREQ("/ns/topic_name", subscription->get_topic_name()); +} diff --git a/rclcpp/test/rclcpp/test_create_timer.cpp b/rclcpp/test/rclcpp/test_create_timer.cpp new file mode 100644 index 0000000000..c4dc1f7e61 --- /dev/null +++ b/rclcpp/test/rclcpp/test_create_timer.cpp @@ -0,0 +1,221 @@ +// 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 "node_interfaces/node_wrapper.hpp" +#include "rclcpp/create_timer.hpp" +#include "rclcpp/executors.hpp" +#include "rclcpp/node.hpp" + +using namespace std::chrono_literals; + +TEST(TestCreateTimer, timer_executes) +{ + rclcpp::init(0, nullptr); + auto node = std::make_shared("test_create_timer_node"); + + std::atomic got_callback{false}; + + rclcpp::TimerBase::SharedPtr timer; + timer = rclcpp::create_timer( + node, + node->get_clock(), + rclcpp::Duration(0ms), + [&got_callback, &timer]() { + got_callback = true; + timer->cancel(); + }); + + rclcpp::spin_some(node); + + ASSERT_TRUE(got_callback); + rclcpp::shutdown(); +} + +TEST(TestCreateTimer, call_with_node_wrapper_compiles) +{ + rclcpp::init(0, nullptr); + NodeWrapper node("test_create_timer_call_with_node_wrapper_compiles"); + + rclcpp::TimerBase::SharedPtr timer; + timer = rclcpp::create_timer( + node, + node.get_node_clock_interface()->get_clock(), + rclcpp::Duration(0ms), + []() {}); + rclcpp::shutdown(); +} + +TEST(TestCreateWallTimer, call_wall_timer_with_bad_arguments) +{ + rclcpp::init(0, nullptr); + NodeWrapper node("test_create_wall_timers_with_bad_arguments"); + auto callback = []() {}; + rclcpp::CallbackGroup::SharedPtr group = nullptr; + auto node_interface = + rclcpp::node_interfaces::get_node_base_interface(node).get(); + auto timers_interface = + rclcpp::node_interfaces::get_node_timers_interface(node).get(); + + // Negative period + EXPECT_THROW( + rclcpp::create_wall_timer(-1ms, callback, group, node_interface, timers_interface), + std::invalid_argument); + + // Very negative period + constexpr auto nanoseconds_min = std::chrono::nanoseconds::min(); + EXPECT_THROW( + rclcpp::create_wall_timer( + nanoseconds_min, callback, group, node_interface, timers_interface), + std::invalid_argument); + + // Period must be less than nanoseconds::max() + constexpr auto nanoseconds_max = std::chrono::nanoseconds::min(); + EXPECT_THROW( + rclcpp::create_wall_timer( + nanoseconds_max, callback, group, node_interface, timers_interface), + std::invalid_argument); + + EXPECT_NO_THROW( + rclcpp::create_wall_timer( + nanoseconds_max - 1us, callback, group, node_interface, timers_interface)); + + EXPECT_NO_THROW( + rclcpp::create_wall_timer(0ms, callback, group, node_interface, timers_interface)); + + // Period must be less than nanoseconds::max() + constexpr auto hours_max = std::chrono::hours::max(); + EXPECT_THROW( + rclcpp::create_wall_timer(hours_max, callback, group, node_interface, timers_interface), + std::invalid_argument); + + // node_interface is null + EXPECT_THROW( + rclcpp::create_wall_timer(1ms, callback, group, nullptr, timers_interface), + std::invalid_argument); + + // timers_interface is null + EXPECT_THROW( + rclcpp::create_wall_timer(1ms, callback, group, node_interface, nullptr), + std::invalid_argument); + rclcpp::shutdown(); +} + +TEST(TestCreateTimer, call_timer_with_bad_arguments) +{ + rclcpp::init(0, nullptr); + NodeWrapper node("test_create_timers_with_bad_arguments"); + auto callback = []() {}; + rclcpp::CallbackGroup::SharedPtr group = nullptr; + auto node_interface = + rclcpp::node_interfaces::get_node_base_interface(node).get(); + auto timers_interface = + rclcpp::node_interfaces::get_node_timers_interface(node).get(); + + auto clock = node.get_node_clock_interface()->get_clock(); + + // Negative period + EXPECT_THROW( + rclcpp::create_timer( + clock, -1ms, callback, group, node_interface, timers_interface), + std::invalid_argument); + + // Very negative period + constexpr auto nanoseconds_min = std::chrono::nanoseconds::min(); + EXPECT_THROW( + rclcpp::create_timer( + clock, nanoseconds_min, callback, group, node_interface, timers_interface), + std::invalid_argument); + + // Period must be less than nanoseconds::max() + constexpr auto nanoseconds_max = std::chrono::nanoseconds::min(); + EXPECT_THROW( + rclcpp::create_timer( + clock, nanoseconds_max, callback, group, node_interface, timers_interface), + std::invalid_argument); + + EXPECT_NO_THROW( + rclcpp::create_timer( + clock, nanoseconds_max - 1us, callback, group, node_interface, timers_interface)); + + EXPECT_NO_THROW( + rclcpp::create_timer(clock, 0ms, callback, group, node_interface, timers_interface)); + + // Period must be less than nanoseconds::max() + constexpr auto hours_max = std::chrono::hours::max(); + EXPECT_THROW( + rclcpp::create_timer( + clock, hours_max, callback, group, node_interface, timers_interface), + std::invalid_argument); + + // node_interface is null + EXPECT_THROW( + rclcpp::create_timer(clock, 1ms, callback, group, nullptr, timers_interface), + std::invalid_argument); + + // timers_interface is null + EXPECT_THROW( + rclcpp::create_timer(clock, 1ms, callback, group, node_interface, nullptr), + std::invalid_argument); + + rclcpp::shutdown(); +} + +static void test_timer_callback(void) {} + +TEST(TestCreateTimer, timer_function_pointer) +{ + rclcpp::init(0, nullptr); + auto node = std::make_shared("timer_function_pointer_node"); + + // make sure build succeeds with function pointer instead of lambda + auto some_timer = rclcpp::create_timer( + node, + node->get_clock(), + rclcpp::Duration(0ms), + test_timer_callback); + + rclcpp::shutdown(); +} + +TEST(TestCreateTimer, timer_without_autostart) +{ + rclcpp::init(0, nullptr); + auto node = std::make_shared("test_create_timer_node"); + + rclcpp::TimerBase::SharedPtr timer; + timer = rclcpp::create_timer( + node, + node->get_clock(), + rclcpp::Duration(0ms), + []() {}, + nullptr, + false); + + EXPECT_TRUE(timer->is_canceled()); + EXPECT_EQ(timer->time_until_trigger().count(), std::chrono::nanoseconds::max().count()); + + timer->reset(); + EXPECT_LE(timer->time_until_trigger().count(), std::chrono::nanoseconds::max().count()); + EXPECT_FALSE(timer->is_canceled()); + + timer->cancel(); + + rclcpp::shutdown(); +} diff --git a/rclcpp/test/rclcpp/test_duration.cpp b/rclcpp/test/rclcpp/test_duration.cpp new file mode 100644 index 0000000000..2347514d7a --- /dev/null +++ b/rclcpp/test/rclcpp/test_duration.cpp @@ -0,0 +1,366 @@ +// 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 "rcl/error_handling.h" +#include "rcl/time.h" +#include "rclcpp/clock.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/duration.hpp" + +#include "../utils/rclcpp_gtest_macros.hpp" + +using namespace std::chrono_literals; + +class TestDuration : public ::testing::Test +{ +}; + +TEST_F(TestDuration, operators) { + rclcpp::Duration old(1, 0); + rclcpp::Duration young(2, 0); + + EXPECT_TRUE(old < young); + EXPECT_TRUE(young > old); + EXPECT_TRUE(old <= young); + EXPECT_TRUE(young >= old); + EXPECT_FALSE(young == old); + EXPECT_TRUE(young != old); + + rclcpp::Duration add = old + young; + EXPECT_EQ(add.nanoseconds(), old.nanoseconds() + young.nanoseconds()); + EXPECT_EQ(add, old + young); + + rclcpp::Duration sub = young - old; + EXPECT_EQ(sub.nanoseconds(), young.nanoseconds() - old.nanoseconds()); + EXPECT_EQ(sub, young - old); + + rclcpp::Duration addequal = old; + addequal += young; + EXPECT_EQ(addequal.nanoseconds(), old.nanoseconds() + young.nanoseconds()); + EXPECT_EQ(addequal, old + young); + + rclcpp::Duration subequal = young; + subequal -= old; + EXPECT_EQ(subequal.nanoseconds(), young.nanoseconds() - old.nanoseconds()); + EXPECT_EQ(subequal, young - old); + + rclcpp::Duration scale = old * 3; + EXPECT_EQ(scale.nanoseconds(), old.nanoseconds() * 3); + + rclcpp::Duration scaleequal = old; + scaleequal *= 3; + EXPECT_EQ(scaleequal.nanoseconds(), old.nanoseconds() * 3); + + rclcpp::Duration time = rclcpp::Duration(0, 0); + rclcpp::Duration copy_constructor_duration(time); + rclcpp::Duration assignment_op_duration = rclcpp::Duration(1, 0); + (void)assignment_op_duration; + assignment_op_duration = time; + + EXPECT_TRUE(time == copy_constructor_duration); + EXPECT_TRUE(time == assignment_op_duration); +} + +TEST_F(TestDuration, operators_with_message_stamp) { + builtin_interfaces::msg::Time time_msg = rclcpp::Time(0, 100000000u); // 0.1s + rclcpp::Duration pos_duration(1, 100000000u); // 1.1s + rclcpp::Duration neg_duration(-2, 900000000u); // -1.1s + + builtin_interfaces::msg::Time res_addpos = time_msg + pos_duration; + EXPECT_EQ(res_addpos.sec, 1); + EXPECT_EQ(res_addpos.nanosec, 200000000u); + + builtin_interfaces::msg::Time res_addneg = time_msg + neg_duration; + EXPECT_EQ(res_addneg.sec, -1); + EXPECT_EQ(res_addneg.nanosec, 0); + + builtin_interfaces::msg::Time res_subpos = time_msg - pos_duration; + EXPECT_EQ(res_subpos.sec, -1); + EXPECT_EQ(res_subpos.nanosec, 0); + + builtin_interfaces::msg::Time res_subneg = time_msg - neg_duration; + EXPECT_EQ(res_subneg.sec, 1); + EXPECT_EQ(res_subneg.nanosec, 200000000u); + + builtin_interfaces::msg::Time neg_time_msg; + neg_time_msg.sec = -1; + auto max = rclcpp::Duration::from_nanoseconds(std::numeric_limits::max()); + + EXPECT_THROW(neg_time_msg + max, std::runtime_error); + EXPECT_THROW(time_msg + max, std::overflow_error); +} + +TEST_F(TestDuration, chrono_overloads) { + int64_t ns = 123456789l; + auto chrono_ns = std::chrono::nanoseconds(ns); + auto d1 = rclcpp::Duration::from_nanoseconds(ns); + auto d2 = rclcpp::Duration(chrono_ns); + auto d3 = rclcpp::Duration(123456789ns); + EXPECT_EQ(d1, d2); + EXPECT_EQ(d1, d3); + EXPECT_EQ(d2, d3); + + // check non-nanosecond durations + std::chrono::milliseconds chrono_ms(100); + auto d4 = rclcpp::Duration(chrono_ms); + EXPECT_EQ(chrono_ms, d4.to_chrono()); + std::chrono::duration chrono_float_seconds(3.14); + auto d5 = rclcpp::Duration(chrono_float_seconds); + EXPECT_EQ(chrono_float_seconds, d5.to_chrono()); +} + +TEST_F(TestDuration, overflows) { + auto max = rclcpp::Duration::from_nanoseconds(std::numeric_limits::max()); + auto min = rclcpp::Duration::from_nanoseconds(std::numeric_limits::min()); + + rclcpp::Duration one(1ns); + rclcpp::Duration negative_one(-1ns); + + EXPECT_THROW(max + one, std::overflow_error); + EXPECT_THROW(min - one, std::underflow_error); + EXPECT_THROW(negative_one + min, std::underflow_error); + EXPECT_THROW(negative_one - max, std::underflow_error); + + rclcpp::Duration base_d = max * 0.3; + EXPECT_THROW(base_d * 4, std::overflow_error); + EXPECT_THROW(base_d * (-4), std::underflow_error); + + rclcpp::Duration base_d_neg = max * (-0.3); + EXPECT_THROW(base_d_neg * (-4), std::overflow_error); + EXPECT_THROW(base_d_neg * 4, std::underflow_error); +} + +TEST_F(TestDuration, negative_duration) { + rclcpp::Duration assignable_duration = rclcpp::Duration(0ns) - rclcpp::Duration(5, 0); + + { + // avoid windows converting a literal number less than -INT_MAX to unsigned int C4146 + int64_t expected_value = -5000; + expected_value *= 1000 * 1000; + EXPECT_EQ(expected_value, assignable_duration.nanoseconds()); + } + + { + builtin_interfaces::msg::Duration duration_msg; + duration_msg.sec = -4; + duration_msg.nanosec = 250000000; + + assignable_duration = duration_msg; + // avoid windows converting a literal number less than -INT_MAX to unsigned int C4146 + int64_t expected_value = -3750; + expected_value *= 1000 * 1000; + EXPECT_EQ(expected_value, assignable_duration.nanoseconds()); + } +} + +TEST_F(TestDuration, maximum_duration) { + rclcpp::Duration max_duration = rclcpp::Duration::max(); + rclcpp::Duration max(std::numeric_limits::max(), 999999999); + + EXPECT_EQ(max_duration, max); +} + +static const int64_t HALF_SEC_IN_NS = 500 * 1000 * 1000; +static const int64_t ONE_SEC_IN_NS = 1000 * 1000 * 1000; +static const int64_t ONE_AND_HALF_SEC_IN_NS = 3 * HALF_SEC_IN_NS; +static const int64_t MAX_NANOSECONDS = std::numeric_limits::max(); + +TEST_F(TestDuration, from_seconds) { + EXPECT_EQ(rclcpp::Duration(0ns), rclcpp::Duration::from_seconds(0.0)); + EXPECT_EQ(rclcpp::Duration(0ns), rclcpp::Duration::from_seconds(0)); + EXPECT_EQ(rclcpp::Duration(1, HALF_SEC_IN_NS), rclcpp::Duration::from_seconds(1.5)); + EXPECT_EQ( + rclcpp::Duration::from_nanoseconds(-ONE_AND_HALF_SEC_IN_NS), + rclcpp::Duration::from_seconds(-1.5)); +} + +TEST_F(TestDuration, from_rmw_time) { + constexpr auto max_rcl_duration = std::numeric_limits::max(); + { + rmw_time_t rmw_duration{}; + rmw_duration.sec = RCL_NS_TO_S(max_rcl_duration) + 1uLL; + EXPECT_EQ(rclcpp::Duration::from_rmw_time(rmw_duration).nanoseconds(), max_rcl_duration); + } + { + rmw_time_t rmw_duration{}; + rmw_duration.nsec = max_rcl_duration + 1uLL; + EXPECT_EQ(rclcpp::Duration::from_rmw_time(rmw_duration).nanoseconds(), max_rcl_duration); + } + { + rmw_time_t rmw_duration{}; + rmw_duration.nsec = max_rcl_duration; + rmw_duration.sec = RCL_NS_TO_S(max_rcl_duration); + EXPECT_EQ(rclcpp::Duration::from_rmw_time(rmw_duration).nanoseconds(), max_rcl_duration); + } + { + rmw_time_t rmw_duration{}; + rmw_duration.sec = 1u; + rmw_duration.nsec = 1000u; + EXPECT_EQ( + rclcpp::Duration::from_rmw_time(rmw_duration).nanoseconds(), + static_cast(RCL_S_TO_NS(rmw_duration.sec) + rmw_duration.nsec)); + } +} + +TEST_F(TestDuration, std_chrono_constructors) { + EXPECT_EQ(rclcpp::Duration(0ns), rclcpp::Duration(0.0s)); + EXPECT_EQ(rclcpp::Duration(0ns), rclcpp::Duration(0s)); + EXPECT_EQ(rclcpp::Duration(1, HALF_SEC_IN_NS), rclcpp::Duration(1.5s)); + EXPECT_EQ(rclcpp::Duration(-1, 0), rclcpp::Duration(-1s)); +} + +TEST_F(TestDuration, conversions) { + { + auto duration = rclcpp::Duration::from_nanoseconds(HALF_SEC_IN_NS); + const auto duration_msg = static_cast(duration); + EXPECT_EQ(duration_msg.sec, 0); + EXPECT_EQ(duration_msg.nanosec, HALF_SEC_IN_NS); + EXPECT_EQ(rclcpp::Duration(duration_msg).nanoseconds(), HALF_SEC_IN_NS); + + const auto rmw_time = duration.to_rmw_time(); + EXPECT_EQ(rmw_time.sec, 0u); + EXPECT_EQ(rmw_time.nsec, static_cast(HALF_SEC_IN_NS)); + + const auto chrono_duration = duration.to_chrono(); + EXPECT_EQ(chrono_duration.count(), HALF_SEC_IN_NS); + } + + { + auto duration = rclcpp::Duration::from_nanoseconds(ONE_SEC_IN_NS); + const auto duration_msg = static_cast(duration); + EXPECT_EQ(duration_msg.sec, 1); + EXPECT_EQ(duration_msg.nanosec, 0u); + EXPECT_EQ(rclcpp::Duration(duration_msg).nanoseconds(), ONE_SEC_IN_NS); + + const auto rmw_time = duration.to_rmw_time(); + EXPECT_EQ(rmw_time.sec, 1u); + EXPECT_EQ(rmw_time.nsec, 0u); + + const auto chrono_duration = duration.to_chrono(); + EXPECT_EQ(chrono_duration.count(), ONE_SEC_IN_NS); + } + + { + auto duration = rclcpp::Duration::from_nanoseconds(ONE_AND_HALF_SEC_IN_NS); + auto duration_msg = static_cast(duration); + EXPECT_EQ(duration_msg.sec, 1); + EXPECT_EQ(duration_msg.nanosec, HALF_SEC_IN_NS); + EXPECT_EQ(rclcpp::Duration(duration_msg).nanoseconds(), ONE_AND_HALF_SEC_IN_NS); + + auto rmw_time = duration.to_rmw_time(); + EXPECT_EQ(rmw_time.sec, 1u); + EXPECT_EQ(rmw_time.nsec, static_cast(HALF_SEC_IN_NS)); + + auto chrono_duration = duration.to_chrono(); + EXPECT_EQ(chrono_duration.count(), ONE_AND_HALF_SEC_IN_NS); + } + + { + auto duration = rclcpp::Duration::from_nanoseconds(-HALF_SEC_IN_NS); + auto duration_msg = static_cast(duration); + EXPECT_EQ(duration_msg.sec, -1); + EXPECT_EQ(duration_msg.nanosec, HALF_SEC_IN_NS); + EXPECT_EQ(rclcpp::Duration(duration_msg).nanoseconds(), -HALF_SEC_IN_NS); + + EXPECT_THROW(duration.to_rmw_time(), std::runtime_error); + + auto chrono_duration = duration.to_chrono(); + EXPECT_EQ(chrono_duration.count(), -HALF_SEC_IN_NS); + } + + { + auto duration = rclcpp::Duration::from_nanoseconds(-ONE_SEC_IN_NS); + auto duration_msg = static_cast(duration); + EXPECT_EQ(duration_msg.sec, -1); + EXPECT_EQ(duration_msg.nanosec, 0u); + EXPECT_EQ(rclcpp::Duration(duration_msg).nanoseconds(), -ONE_SEC_IN_NS); + + EXPECT_THROW(duration.to_rmw_time(), std::runtime_error); + + auto chrono_duration = duration.to_chrono(); + EXPECT_EQ(chrono_duration.count(), -ONE_SEC_IN_NS); + } + + { + auto duration = rclcpp::Duration::from_nanoseconds(-ONE_AND_HALF_SEC_IN_NS); + auto duration_msg = static_cast(duration); + EXPECT_EQ(duration_msg.sec, -2); + EXPECT_EQ(duration_msg.nanosec, HALF_SEC_IN_NS); + EXPECT_EQ(rclcpp::Duration(duration_msg).nanoseconds(), -ONE_AND_HALF_SEC_IN_NS); + + EXPECT_THROW(duration.to_rmw_time(), std::runtime_error); + + auto chrono_duration = duration.to_chrono(); + EXPECT_EQ(chrono_duration.count(), -ONE_AND_HALF_SEC_IN_NS); + } + + { + auto duration = rclcpp::Duration::from_nanoseconds(MAX_NANOSECONDS); + + const auto duration_msg = static_cast(duration); + EXPECT_EQ(duration_msg.sec, std::numeric_limits::max()); + EXPECT_EQ(duration_msg.nanosec, std::numeric_limits::max()); + + auto rmw_time = duration.to_rmw_time(); + EXPECT_EQ(rmw_time.sec, 9223372036u); + EXPECT_EQ(rmw_time.nsec, 854775807u); + + auto chrono_duration = duration.to_chrono(); + EXPECT_EQ(chrono_duration.count(), MAX_NANOSECONDS); + } + + { + auto duration = rclcpp::Duration::from_nanoseconds(-MAX_NANOSECONDS); + + const auto duration_msg = static_cast(duration); + EXPECT_EQ(duration_msg.sec, std::numeric_limits::min()); + EXPECT_EQ(duration_msg.nanosec, 0u); + + EXPECT_THROW(duration.to_rmw_time(), std::runtime_error); + + auto chrono_duration = duration.to_chrono(); + EXPECT_EQ(chrono_duration.count(), -MAX_NANOSECONDS); + } +} + +TEST_F(TestDuration, test_some_constructors) { + builtin_interfaces::msg::Duration duration_msg; + duration_msg.sec = 1; + duration_msg.nanosec = 1000; + rclcpp::Duration duration_from_msg(duration_msg); + EXPECT_EQ(RCL_S_TO_NS(1) + 1000, duration_from_msg.nanoseconds()); + + rcl_duration_t duration_struct; + duration_struct.nanoseconds = 4000; + rclcpp::Duration duration_from_struct(duration_struct); + EXPECT_EQ(4000, duration_from_struct.nanoseconds()); +} + +TEST_F(TestDuration, test_some_exceptions) { + rclcpp::Duration test_duration(0ns); + RCLCPP_EXPECT_THROW_EQ( + test_duration = + rclcpp::Duration::from_nanoseconds(INT64_MAX) - rclcpp::Duration(-1ns), + std::overflow_error("duration subtraction leads to int64_t overflow")); + RCLCPP_EXPECT_THROW_EQ( + test_duration = test_duration * (std::numeric_limits::infinity()), + std::runtime_error("abnormal scale in rclcpp::Duration")); +} diff --git a/rclcpp/test/rclcpp/test_executor.cpp b/rclcpp/test/rclcpp/test_executor.cpp new file mode 100644 index 0000000000..21a19cee18 --- /dev/null +++ b/rclcpp/test/rclcpp/test_executor.cpp @@ -0,0 +1,510 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include + +#include "rclcpp/executor.hpp" +#include "rclcpp/memory_strategy.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/strategies/allocator_memory_strategy.hpp" + +#include "../mocking_utils/patch.hpp" +#include "../utils/rclcpp_gtest_macros.hpp" + +// This file tests the abstract rclcpp::Executor class. For tests of the concrete classes +// that implement this class, please see the test/rclcpp/executors subdirectory. + +class DummyExecutor : public rclcpp::Executor +{ +public: + DummyExecutor() + : rclcpp::Executor() + { + } + + void spin() override + { + } + + void spin_nanoseconds(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node) + { + spin_node_once_nanoseconds(node, std::chrono::milliseconds(100)); + } +}; + +class TestExecutor : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + } + + void TearDown() + { + rclcpp::shutdown(); + } +}; + +// Required for mocking_utils below +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, ==) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, !=) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, <) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_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, <) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, >) + +TEST_F(TestExecutor, add_remove_node_thread_safe) { + using namespace std::chrono_literals; + + // Create an Executor + rclcpp::executors::SingleThreadedExecutor executor; + + auto future = std::async(std::launch::async, [&executor] {executor.spin();}); + + // Add and remove nodes repeatedly + // Test that this does not cause a segfault + size_t num_nodes = 100; + for (size_t i = 0; i < num_nodes; ++i) { + std::ostringstream name; + name << "node_" << i; + auto node = std::make_shared(name.str()); + executor.add_node(node); + // Sleeping here helps exaggerate the issue + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + executor.remove_node(node); + } + std::future_status future_status = std::future_status::timeout; + do { + executor.cancel(); + future_status = future.wait_for(1s); + } while (future_status == std::future_status::timeout); + EXPECT_EQ(future_status, std::future_status::ready); + future.get(); +} + +TEST_F(TestExecutor, constructor_bad_guard_condition_init) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_guard_condition_init, RCL_RET_ERROR); + EXPECT_THROW( + static_cast(std::make_unique()), + rclcpp::exceptions::RCLError); +} + +TEST_F(TestExecutor, constructor_bad_wait_set_init) { + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_init, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + static_cast(std::make_unique()), + std::runtime_error("Failed to create wait set: error not set")); +} + +TEST_F(TestExecutor, add_callback_group_twice) { + DummyExecutor dummy; + auto node = std::make_shared("node", "ns"); + rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + dummy.add_callback_group(cb_group, node->get_node_base_interface(), false); + cb_group->get_associated_with_executor_atomic().exchange(false); + RCLCPP_EXPECT_THROW_EQ( + dummy.add_callback_group(cb_group, node->get_node_base_interface(), false), + std::runtime_error("Callback group has already been added to this executor.")); +} + +TEST_F(TestExecutor, add_callback_group_failed_trigger_guard_condition) { + DummyExecutor dummy; + auto node = std::make_shared("node", "ns"); + rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + dummy.add_callback_group(cb_group, node->get_node_base_interface(), true), + std::runtime_error("Failed to handle entities update on callback group add: error not set")); +} + +TEST_F(TestExecutor, remove_callback_group_null_node) { + DummyExecutor dummy; + auto node = std::make_shared("node", "ns"); + rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + dummy.add_callback_group(cb_group, node->get_node_base_interface(), true); + + node.reset(); + + + /** + * TODO(mjcarroll): Assert this when we are enforcing that nodes must be destroyed + * after their created callback groups. + RCLCPP_EXPECT_THROW_EQ( + dummy.remove_callback_group(cb_group, false), + std::runtime_error("Node must not be deleted before its callback group(s).")); + */ + EXPECT_NO_THROW(dummy.remove_callback_group(cb_group, false)); +} + +TEST_F(TestExecutor, remove_callback_group_failed_trigger_guard_condition) { + DummyExecutor dummy; + auto node = std::make_shared("node", "ns"); + rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + dummy.add_callback_group(cb_group, node->get_node_base_interface(), true); + + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + dummy.remove_callback_group(cb_group, true), + std::runtime_error( + "Failed to handle entities update on callback group remove: error not set")); +} + +TEST_F(TestExecutor, remove_node_not_associated) { + DummyExecutor dummy; + auto node = std::make_shared("node", "ns"); + rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + RCLCPP_EXPECT_THROW_EQ( + dummy.remove_node(node->get_node_base_interface(), false), + std::runtime_error("Node '/ns/node' needs to be associated with an executor.")); +} + +TEST_F(TestExecutor, remove_node_associated_with_different_executor) { + DummyExecutor dummy1; + auto node1 = std::make_shared("node1", "ns"); + dummy1.add_node(node1->get_node_base_interface(), false); + + DummyExecutor dummy2; + auto node2 = std::make_shared("node2", "ns"); + dummy2.add_node(node2->get_node_base_interface(), false); + + RCLCPP_EXPECT_THROW_EQ( + dummy2.remove_node(node1->get_node_base_interface(), false), + std::runtime_error("Node '/ns/node1' needs to be associated with this executor.")); +} + +TEST_F(TestExecutor, spin_node_once_nanoseconds) { + DummyExecutor dummy; + auto node = std::make_shared("node", "ns"); + bool timer_fired = false; + auto timer = + node->create_wall_timer(std::chrono::milliseconds(1), [&timer_fired]() {timer_fired = true;}); + + // Wait for the wall timer to have expired. + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + EXPECT_FALSE(timer_fired); + dummy.spin_nanoseconds(node->get_node_base_interface()); + EXPECT_TRUE(timer_fired); +} + +TEST_F(TestExecutor, spin_node_some) { + DummyExecutor dummy; + auto node = std::make_shared("node", "ns"); + bool timer_fired = false; + auto timer = + node->create_wall_timer(std::chrono::milliseconds(1), [&timer_fired]() {timer_fired = true;}); + + // Wait for the wall timer to have expired. + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + EXPECT_FALSE(timer_fired); + dummy.spin_node_some(node); + EXPECT_TRUE(timer_fired); +} + +TEST_F(TestExecutor, spin_all_invalid_duration) { + DummyExecutor dummy; + auto node = std::make_shared("node", "ns"); + + RCLCPP_EXPECT_THROW_EQ( + dummy.spin_all(std::chrono::nanoseconds(-1)), + std::invalid_argument("max_duration must be greater than or equal to 0")); +} + +TEST_F(TestExecutor, spin_some_in_spin_some) { + DummyExecutor dummy; + auto node = std::make_shared("node", "ns"); + bool spin_some_in_spin_some = false; + auto timer = + node->create_wall_timer( + std::chrono::milliseconds(1), [&]() { + try { + dummy.spin_some(std::chrono::milliseconds(1)); + } catch (const std::runtime_error & err) { + if (err.what() == std::string("spin_some() called while already spinning")) { + spin_some_in_spin_some = true; + } + } + }); + + dummy.add_node(node); + // Wait for the wall timer to have expired. + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + EXPECT_FALSE(spin_some_in_spin_some); + dummy.spin_some(std::chrono::milliseconds(1)); + EXPECT_TRUE(spin_some_in_spin_some); +} + +TEST_F(TestExecutor, spin_some_elapsed) { + DummyExecutor dummy; + auto node = std::make_shared("node", "ns"); + bool timer_called = false; + auto timer = + node->create_wall_timer( + std::chrono::milliseconds(1), [&]() { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + timer_called = true; + }); + + dummy.add_node(node); + // Wait for the wall timer to have expired. + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + dummy.spin_some(std::chrono::milliseconds(1)); + + ASSERT_TRUE(timer_called); +} + +TEST_F(TestExecutor, spin_once_in_spin_once) { + DummyExecutor dummy; + auto node = std::make_shared("node", "ns"); + bool spin_once_in_spin_once = false; + auto timer = + node->create_wall_timer( + std::chrono::milliseconds(1), [&]() { + try { + dummy.spin_once(std::chrono::milliseconds(1)); + } catch (const std::runtime_error & err) { + if (err.what() == std::string("spin_once() called while already spinning")) { + spin_once_in_spin_once = true; + } + } + }); + + dummy.add_node(node); + // Wait for the wall timer to have expired. + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + EXPECT_FALSE(spin_once_in_spin_once); + dummy.spin_once(std::chrono::milliseconds(1)); + EXPECT_TRUE(spin_once_in_spin_once); +} + +TEST_F(TestExecutor, cancel_failed_trigger_guard_condition) { + DummyExecutor dummy; + + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + dummy.cancel(), + std::runtime_error("Failed to trigger guard condition in cancel: error not set")); +} + +TEST_F(TestExecutor, create_executor_fail_wait_set_clear) { + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_clear, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + DummyExecutor dummy, + std::runtime_error("Couldn't clear the wait set: error not set")); +} + +TEST_F(TestExecutor, spin_all_fail_wait_set_clear) { + DummyExecutor dummy; + auto node = std::make_shared("node", "ns"); + auto timer = + node->create_wall_timer(std::chrono::milliseconds(1), [&]() {}); + + dummy.add_node(node); + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_clear, RCL_RET_ERROR); + + RCLCPP_EXPECT_THROW_EQ( + dummy.spin_all(std::chrono::milliseconds(1)), + std::runtime_error("Couldn't clear the wait set: error not set")); +} + +TEST_F(TestExecutor, spin_some_fail_wait_set_resize) { + DummyExecutor dummy; + auto node = std::make_shared("node", "ns"); + auto timer = + node->create_wall_timer(std::chrono::milliseconds(1), [&]() {}); + + dummy.add_node(node); + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_resize, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + dummy.spin_some(std::chrono::milliseconds(1)), + std::runtime_error("Couldn't resize the wait set: error not set")); +} + +TEST_F(TestExecutor, spin_some_fail_add_handles_to_wait_set) { + DummyExecutor dummy; + auto node = std::make_shared("node", "ns"); + auto timer = + node->create_wall_timer(std::chrono::milliseconds(1), [&]() {}); + + dummy.add_node(node); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_wait_set_add_subscription, + RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + dummy.spin_some(std::chrono::milliseconds(1)), + std::runtime_error("Couldn't fill wait set: error not set")); +} + +TEST_F(TestExecutor, spin_some_fail_wait) { + DummyExecutor dummy; + auto node = std::make_shared("node", "ns"); + auto timer = + node->create_wall_timer(std::chrono::milliseconds(1), [&]() {}); + + dummy.add_node(node); + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + dummy.spin_some(std::chrono::milliseconds(1)), + std::runtime_error("rcl_wait() failed: error not set")); +} + +TEST_F(TestExecutor, spin_until_future_complete_in_spin_until_future_complete) { + DummyExecutor dummy; + auto node = std::make_shared("node", "ns"); + bool spin_until_future_complete_in_spin_until_future_complete = false; + auto timer = + node->create_wall_timer( + std::chrono::milliseconds(1), [&]() { + try { + std::promise promise; + std::future future = promise.get_future(); + dummy.spin_until_future_complete(future, std::chrono::milliseconds(1)); + } catch (const std::runtime_error & err) { + if (err.what() == std::string( + "spin_until_future_complete() called while already spinning")) + { + spin_until_future_complete_in_spin_until_future_complete = true; + } + } + }); + + dummy.add_node(node); + // Wait for the wall timer to have expired. + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + EXPECT_FALSE(spin_until_future_complete_in_spin_until_future_complete); + std::promise promise; + std::future future = promise.get_future(); + dummy.spin_until_future_complete(future, std::chrono::milliseconds(1)); + EXPECT_TRUE(spin_until_future_complete_in_spin_until_future_complete); +} + +TEST_F(TestExecutor, spin_node_once_base_interface) { + DummyExecutor dummy; + auto node = std::make_shared("node", "ns"); + bool spin_called = false; + auto timer = + node->create_wall_timer( + std::chrono::milliseconds(1), [&]() { + spin_called = true; + }); + + // Wait for the wall timer to have expired. + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + EXPECT_FALSE(spin_called); + dummy.spin_node_once(node->get_node_base_interface()); + EXPECT_TRUE(spin_called); +} + +TEST_F(TestExecutor, spin_node_once_node) { + DummyExecutor dummy; + auto node = std::make_shared("node", "ns"); + bool spin_called = false; + auto timer = + node->create_wall_timer( + std::chrono::milliseconds(1), [&]() { + spin_called = true; + }); + + // Wait for the wall timer to have expired. + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + EXPECT_FALSE(spin_called); + dummy.spin_node_once(node); + EXPECT_TRUE(spin_called); +} + +TEST_F(TestExecutor, spin_node_all_base_interface) { + DummyExecutor dummy; + auto node = std::make_shared("node", "ns"); + bool spin_called = false; + auto timer = + node->create_wall_timer( + std::chrono::milliseconds(1), [&]() { + spin_called = true; + }); + + // Wait for the wall timer to have expired. + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + EXPECT_FALSE(spin_called); + dummy.spin_node_all(node->get_node_base_interface(), std::chrono::milliseconds(50)); + EXPECT_TRUE(spin_called); +} + +TEST_F(TestExecutor, spin_node_all_node) { + DummyExecutor dummy; + auto node = std::make_shared("node", "ns"); + bool spin_called = false; + auto timer = + node->create_wall_timer( + std::chrono::milliseconds(1), [&]() { + spin_called = true; + }); + + // Wait for the wall timer to have expired. + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + EXPECT_FALSE(spin_called); + dummy.spin_node_all(node, std::chrono::milliseconds(50)); + EXPECT_TRUE(spin_called); +} + +TEST_F(TestExecutor, spin_until_future_complete_future_already_complete) { + DummyExecutor dummy; + auto node = std::make_shared("node", "ns"); + std::promise promise; + std::future future = promise.get_future(); + promise.set_value(); + EXPECT_EQ( + rclcpp::FutureReturnCode::SUCCESS, + dummy.spin_until_future_complete(future, std::chrono::milliseconds(1))); +} + +TEST_F(TestExecutor, is_spinning) { + DummyExecutor dummy; + ASSERT_FALSE(dummy.is_spinning()); + + auto node = std::make_shared("node", "ns"); + bool timer_called = false; + auto timer = + node->create_wall_timer( + std::chrono::milliseconds(1), [&]() { + timer_called = true; + EXPECT_TRUE(dummy.is_spinning()); + }); + + dummy.add_node(node); + // Wait for the wall timer to have expired. + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + dummy.spin_some(std::chrono::milliseconds(1)); + + ASSERT_TRUE(timer_called); +} diff --git a/rclcpp/test/rclcpp/test_expand_topic_or_service_name.cpp b/rclcpp/test/rclcpp/test_expand_topic_or_service_name.cpp new file mode 100644 index 0000000000..841aa8fd1c --- /dev/null +++ b/rclcpp/test/rclcpp/test_expand_topic_or_service_name.cpp @@ -0,0 +1,244 @@ +// 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/expand_topic_name.h" +#include "rcl/validate_topic_name.h" +#include "rmw/validate_full_topic_name.h" +#include "rmw/validate_namespace.h" +#include "rmw/validate_node_name.h" + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/expand_topic_or_service_name.hpp" + +#include "../mocking_utils/patch.hpp" +#include "../utils/rclcpp_gtest_macros.hpp" + +/* + Testing expand_topic_or_service_name. + */ +TEST(TestExpandTopicOrServiceName, normal) { + using rclcpp::expand_topic_or_service_name; + { + ASSERT_EQ("/ns/chatter", expand_topic_or_service_name("chatter", "node", "/ns")); + } +} + +/* + Testing exceptions of expand_topic_or_service_name. + */ +TEST(TestExpandTopicOrServiceName, exceptions) { + using rclcpp::expand_topic_or_service_name; + { + ASSERT_THROW( + { + expand_topic_or_service_name("chatter", "invalid_node?", "/ns"); + }, rclcpp::exceptions::InvalidNodeNameError); + } + + { + ASSERT_THROW( + { + expand_topic_or_service_name("chatter", "node", "/invalid_ns?"); + }, rclcpp::exceptions::InvalidNamespaceError); + } + + { + ASSERT_THROW( + { + expand_topic_or_service_name("chatter/42invalid", "node", "/ns"); + }, rclcpp::exceptions::InvalidTopicNameError); + } + + { + ASSERT_THROW( + { + // this one will only fail on the "full" topic name validation check + expand_topic_or_service_name("chatter/{ns}/invalid", "node", "/ns"); + }, rclcpp::exceptions::InvalidTopicNameError); + } + + { + ASSERT_THROW( + { + // is_service = true + expand_topic_or_service_name("chatter/42invalid", "node", "/ns", true); + }, rclcpp::exceptions::InvalidServiceNameError); + } + + { + ASSERT_THROW( + { + // is_service = true + // this one will only fail on the "full" topic name validation check + expand_topic_or_service_name("chatter/{ns}/invalid", "node", "/ns", true); + }, rclcpp::exceptions::InvalidServiceNameError); + } +} + +// Required for mocking_utils below +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(TestExpandTopicOrServiceName, rcutils_string_map_init_fail_bad_alloc) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcutils_string_map_init, RCUTILS_RET_BAD_ALLOC); + RCLCPP_EXPECT_THROW_EQ( + rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"), + std::bad_alloc()); +} + +TEST(TestExpandTopicOrServiceName, rcutils_string_map_init_fail_other) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcutils_string_map_init, RCUTILS_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"), + std::runtime_error("error not set")); +} + +TEST(TestExpandTopicOrServiceName, rcl_get_default_topic_name_substitution_fail) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_get_default_topic_name_substitutions, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"), + std::runtime_error("error not set")); +} + +TEST(TestExpandTopicOrServiceName, rcl_get_default_topic_name_substitution_and_map_fini_fail) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_get_default_topic_name_substitutions, RCL_RET_ERROR); + auto mock2 = mocking_utils::patch_and_return( + "lib:rclcpp", rcutils_string_map_fini, RCUTILS_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"), + std::runtime_error("error not set")); +} + +TEST(TestExpandTopicOrServiceName, rcutils_string_map_fini_fail_bad_alloc) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcutils_string_map_fini, RCUTILS_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"), + std::runtime_error("error not set")); +} + +TEST(TestExpandTopicOrServiceName, rmw_valid_full_topic_name_fail_invalid_argument) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rmw_validate_full_topic_name, RMW_RET_INVALID_ARGUMENT); + RCLCPP_EXPECT_THROW_EQ( + rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"), + rclcpp::exceptions::RCLInvalidArgument( + RCL_RET_INVALID_ARGUMENT, rcl_get_error_state(), "failed to validate full topic name")); +} + +TEST(TestExpandTopicOrServiceName, rcl_expand_topic_name_fail) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_expand_topic_name, RCL_RET_TOPIC_NAME_INVALID); + RCLCPP_EXPECT_THROW_EQ( + rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"), + std::runtime_error("topic name unexpectedly valid")); +} + +TEST(TestExpandTopicOrServiceName, rcl_validate_topic_name_fail) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_expand_topic_name, RCL_RET_TOPIC_NAME_INVALID); + auto mock2 = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_validate_topic_name, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"), + rclcpp::exceptions::RCLError( + RCL_RET_ERROR, rcl_get_error_state(), "failed to validate full topic name")); +} + +TEST(TestExpandTopicOrServiceName, rmw_validate_node_name_fail_invalid_argument) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_expand_topic_name, RCL_RET_NODE_INVALID_NAME); + auto mock2 = mocking_utils::patch_and_return( + "lib:rclcpp", rmw_validate_node_name, RMW_RET_INVALID_ARGUMENT); + RCLCPP_EXPECT_THROW_EQ( + rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"), + rclcpp::exceptions::RCLInvalidArgument( + RCL_RET_INVALID_ARGUMENT, rcl_get_error_state(), "failed to validate node name")); +} + +TEST(TestExpandTopicOrServiceName, rmw_validate_node_name_fail_other) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_expand_topic_name, RCL_RET_NODE_INVALID_NAME); + auto mock2 = mocking_utils::patch_and_return( + "lib:rclcpp", rmw_validate_node_name, RMW_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"), + rclcpp::exceptions::RCLError( + RCL_RET_ERROR, rcl_get_error_state(), "failed to validate node name")); +} + +TEST(TestExpandTopicOrServiceName, rmw_validate_namespace_fail_invalid_argument) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_expand_topic_name, RCL_RET_NODE_INVALID_NAMESPACE); + auto mock2 = mocking_utils::patch_and_return( + "lib:rclcpp", rmw_validate_namespace, RMW_RET_INVALID_ARGUMENT); + RCLCPP_EXPECT_THROW_EQ( + rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"), + rclcpp::exceptions::RCLInvalidArgument( + RCL_RET_INVALID_ARGUMENT, rcl_get_error_state(), "failed to validate namespace")); +} + +TEST(TestExpandTopicOrServiceName, rmw_validate_namespace_fail_other) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_expand_topic_name, RCL_RET_NODE_INVALID_NAMESPACE); + auto mock2 = mocking_utils::patch_and_return( + "lib:rclcpp", rmw_validate_namespace, RMW_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"), + rclcpp::exceptions::RCLError( + RCL_RET_ERROR, rcl_get_error_state(), "failed to validate namespace")); +} + +TEST(TestExpandTopicOrServiceName, rcl_expand_topic_name_fail_other) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_expand_topic_name, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"), + rclcpp::exceptions::RCLError(RCL_RET_ERROR, rcl_get_error_state(), "error not set")); +} + +TEST(TestExpandTopicOrServiceName, rcl_expand_topic_name_fail_invalid_node_name) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_expand_topic_name, RCL_RET_NODE_INVALID_NAME); + RCLCPP_EXPECT_THROW_EQ( + rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"), + std::runtime_error("invalid rcl node name but valid rmw node name")); +} + +TEST(TestExpandTopicOrServiceName, rcl_expand_topic_name_fail_invalid_node_namespace) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_expand_topic_name, RCL_RET_NODE_INVALID_NAMESPACE); + RCLCPP_EXPECT_THROW_EQ( + rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"), + std::runtime_error("invalid rcl namespace but valid rmw namespace")); +} + +TEST(TestExpandTopicOrServiceName, rmw_validate_full_topic_name_fail_other) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rmw_validate_full_topic_name, RMW_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"), + rclcpp::exceptions::RCLError( + RCL_RET_ERROR, rcl_get_error_state(), "failed to validate full topic name")); +} diff --git a/rclcpp/test/test_externally_defined_services.cpp b/rclcpp/test/rclcpp/test_externally_defined_services.cpp similarity index 73% rename from rclcpp/test/test_externally_defined_services.cpp rename to rclcpp/test/rclcpp/test_externally_defined_services.cpp index 1045c8025d..21a7d11142 100644 --- a/rclcpp/test/test_externally_defined_services.cpp +++ b/rclcpp/test/rclcpp/test_externally_defined_services.cpp @@ -24,8 +24,8 @@ #include "rcl/service.h" -#include "rclcpp/srv/mock.hpp" -#include "rclcpp/srv/mock.h" +#include "test_msgs/srv/empty.hpp" +#include "test_msgs/srv/empty.h" class TestExternallyDefinedServices : public ::testing::Test { @@ -34,20 +34,24 @@ class TestExternallyDefinedServices : public ::testing::Test { rclcpp::init(0, nullptr); } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } }; void callback( - const std::shared_ptr/*req*/, - std::shared_ptr/*resp*/) + const std::shared_ptr/*req*/, + std::shared_ptr/*resp*/) {} TEST_F(TestExternallyDefinedServices, default_behavior) { auto node_handle = rclcpp::Node::make_shared("base_node"); try { - auto srv = node_handle->create_service("test", - callback); + auto srv = node_handle->create_service("test", callback); } catch (const std::exception &) { FAIL(); return; @@ -55,19 +59,18 @@ TEST_F(TestExternallyDefinedServices, default_behavior) { SUCCEED(); } - TEST_F(TestExternallyDefinedServices, extern_defined_uninitialized) { auto node_handle = rclcpp::Node::make_shared("base_node"); // mock for externally defined service rcl_service_t service_handle = rcl_get_zero_initialized_service(); - rclcpp::AnyServiceCallback cb; + rclcpp::AnyServiceCallback cb; // don't initialize the service // expect fail try { - rclcpp::Service( + rclcpp::Service( node_handle->get_node_base_interface()->get_shared_rcl_node_handle(), &service_handle, cb); } catch (const std::runtime_error &) { @@ -85,7 +88,7 @@ TEST_F(TestExternallyDefinedServices, extern_defined_initialized) { rcl_service_t service_handle = rcl_get_zero_initialized_service(); rcl_service_options_t service_options = rcl_service_get_default_options(); const rosidl_service_type_support_t * ts = - rosidl_typesupport_cpp::get_service_type_support_handle(); + rosidl_typesupport_cpp::get_service_type_support_handle(); rcl_ret_t ret = rcl_service_init( &service_handle, node_handle->get_node_base_interface()->get_rcl_node_handle(), @@ -95,10 +98,10 @@ TEST_F(TestExternallyDefinedServices, extern_defined_initialized) { return; } - rclcpp::AnyServiceCallback cb; + rclcpp::AnyServiceCallback cb; try { - rclcpp::Service( + rclcpp::Service( node_handle->get_node_base_interface()->get_shared_rcl_node_handle(), &service_handle, cb); } catch (const std::runtime_error &) { @@ -106,6 +109,15 @@ TEST_F(TestExternallyDefinedServices, extern_defined_initialized) { return; } + // Destruct the service + ret = rcl_service_fini( + &service_handle, + node_handle->get_node_base_interface()->get_rcl_node_handle()); + if (ret != RCL_RET_OK) { + FAIL(); + return; + } + SUCCEED(); } @@ -116,7 +128,7 @@ TEST_F(TestExternallyDefinedServices, extern_defined_destructor) { rcl_service_t service_handle = rcl_get_zero_initialized_service(); rcl_service_options_t service_options = rcl_service_get_default_options(); const rosidl_service_type_support_t * ts = - rosidl_typesupport_cpp::get_service_type_support_handle(); + rosidl_typesupport_cpp::get_service_type_support_handle(); rcl_ret_t ret = rcl_service_init( &service_handle, node_handle->get_node_base_interface()->get_rcl_node_handle(), @@ -125,11 +137,11 @@ TEST_F(TestExternallyDefinedServices, extern_defined_destructor) { FAIL(); return; } - rclcpp::AnyServiceCallback cb; + rclcpp::AnyServiceCallback cb; { // Call constructor - rclcpp::Service srv_cpp( + rclcpp::Service srv_cpp( node_handle->get_node_base_interface()->get_shared_rcl_node_handle(), &service_handle, cb); // Call destructor @@ -139,5 +151,15 @@ TEST_F(TestExternallyDefinedServices, extern_defined_destructor) { FAIL(); return; } + + // Destruct the service + ret = rcl_service_fini( + &service_handle, + node_handle->get_node_base_interface()->get_rcl_node_handle()); + if (ret != RCL_RET_OK) { + FAIL(); + return; + } + SUCCEED(); } diff --git a/rclcpp/test/test_find_weak_nodes.cpp b/rclcpp/test/rclcpp/test_find_weak_nodes.cpp similarity index 53% rename from rclcpp/test/test_find_weak_nodes.cpp rename to rclcpp/test/rclcpp/test_find_weak_nodes.cpp index 4d9ede7a30..739a247b39 100644 --- a/rclcpp/test/test_find_weak_nodes.cpp +++ b/rclcpp/test/rclcpp/test_find_weak_nodes.cpp @@ -27,6 +27,11 @@ class TestFindWeakNodes : public ::testing::Test { rclcpp::init(0, nullptr); } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } }; TEST_F(TestFindWeakNodes, allocator_strategy_with_weak_nodes) { @@ -36,22 +41,27 @@ TEST_F(TestFindWeakNodes, allocator_strategy_with_weak_nodes) { rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy<>>(); auto existing_node = rclcpp::Node::make_shared("existing_node"); auto dead_node = rclcpp::Node::make_shared("dead_node"); - rclcpp::memory_strategy::MemoryStrategy::WeakNodeVector weak_nodes; - weak_nodes.push_back(existing_node->get_node_base_interface()); - weak_nodes.push_back(dead_node->get_node_base_interface()); + auto existing_group = existing_node->get_node_base_interface()->get_default_callback_group(); + auto dead_group = dead_node->get_node_base_interface()->get_default_callback_group(); + rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap weak_groups_to_nodes; + weak_groups_to_nodes[existing_group] = existing_node->get_node_base_interface(); + weak_groups_to_nodes[dead_group] = dead_node->get_node_base_interface(); // AND // Delete dead_node, creating a dangling pointer in weak_nodes dead_node.reset(); - ASSERT_FALSE(weak_nodes[0].expired()); - ASSERT_TRUE(weak_nodes[1].expired()); + ASSERT_FALSE(weak_groups_to_nodes[existing_group].expired()); + ASSERT_TRUE(weak_groups_to_nodes[dead_group].expired()); // WHEN - bool has_invalid_weak_nodes = memory_strategy->collect_entities(weak_nodes); + bool has_invalid_weak_groups_or_nodes = memory_strategy->collect_entities(weak_groups_to_nodes); // THEN // The result of finding dangling node pointers should be true - ASSERT_TRUE(has_invalid_weak_nodes); + ASSERT_TRUE(has_invalid_weak_groups_or_nodes); + + // Prevent memory leak due to the order of destruction + memory_strategy->clear_handles(); } TEST_F(TestFindWeakNodes, allocator_strategy_no_weak_nodes) { @@ -61,16 +71,21 @@ TEST_F(TestFindWeakNodes, allocator_strategy_no_weak_nodes) { rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy<>>(); auto existing_node1 = rclcpp::Node::make_shared("existing_node1"); auto existing_node2 = rclcpp::Node::make_shared("existing_node2"); - rclcpp::memory_strategy::MemoryStrategy::WeakNodeVector weak_nodes; - weak_nodes.push_back(existing_node1->get_node_base_interface()); - weak_nodes.push_back(existing_node2->get_node_base_interface()); - ASSERT_FALSE(weak_nodes[0].expired()); - ASSERT_FALSE(weak_nodes[1].expired()); + auto existing_group1 = existing_node1->get_node_base_interface()->get_default_callback_group(); + auto existing_group2 = existing_node2->get_node_base_interface()->get_default_callback_group(); + rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap weak_groups_to_nodes; + weak_groups_to_nodes[existing_group1] = existing_node1->get_node_base_interface(); + weak_groups_to_nodes[existing_group2] = existing_node2->get_node_base_interface(); + ASSERT_FALSE(weak_groups_to_nodes[existing_group1].expired()); + ASSERT_FALSE(weak_groups_to_nodes[existing_group2].expired()); // WHEN - bool has_invalid_weak_nodes = memory_strategy->collect_entities(weak_nodes); + bool has_invalid_weak_groups_or_nodes = memory_strategy->collect_entities(weak_groups_to_nodes); // THEN // The result of finding dangling node pointers should be false - ASSERT_FALSE(has_invalid_weak_nodes); + ASSERT_FALSE(has_invalid_weak_groups_or_nodes); + + // Prevent memory leak due to the order of destruction + memory_strategy->clear_handles(); } diff --git a/rclcpp/test/test_function_traits.cpp b/rclcpp/test/rclcpp/test_function_traits.cpp similarity index 90% rename from rclcpp/test/test_function_traits.cpp rename to rclcpp/test/rclcpp/test_function_traits.cpp index 3121bda587..20c9521c4e 100644 --- a/rclcpp/test/test_function_traits.cpp +++ b/rclcpp/test/rclcpp/test_function_traits.cpp @@ -80,6 +80,12 @@ struct ObjectMember return 7; } + int callback_one_bool_const(bool a) const + { + (void)a; + return 7; + } + int callback_two_bools(bool a, bool b) { (void)a; @@ -387,6 +393,7 @@ TEST(TestFunctionTraits, argument_types) { auto bind_one_bool = std::bind( &ObjectMember::callback_one_bool, &object_member, std::placeholders::_1); + (void)bind_one_bool; // to quiet clang static_assert( std::is_same< @@ -394,9 +401,21 @@ TEST(TestFunctionTraits, argument_types) { rclcpp::function_traits::function_traits::template argument_type<0> >::value, "Functor accepts a bool as first argument"); + auto bind_one_bool_const = std::bind( + &ObjectMember::callback_one_bool_const, &object_member, std::placeholders::_1); + (void)bind_one_bool_const; // to quiet clang + + static_assert( + std::is_same< + bool, + rclcpp::function_traits::function_traits::template + argument_type<0> + >::value, "Functor accepts a bool as first argument"); + auto bind_two_bools = std::bind( &ObjectMember::callback_two_bools, &object_member, std::placeholders::_1, std::placeholders::_2); + (void)bind_two_bools; // to quiet clang static_assert( std::is_same< @@ -413,6 +432,7 @@ TEST(TestFunctionTraits, argument_types) { auto bind_one_bool_one_float = std::bind( &ObjectMember::callback_one_bool_one_float, &object_member, std::placeholders::_1, std::placeholders::_2); + (void)bind_one_bool_one_float; // to quiet clang static_assert( std::is_same< @@ -431,6 +451,7 @@ TEST(TestFunctionTraits, argument_types) { >::value, "Functor accepts a float as second argument"); auto bind_one_int = std::bind(func_one_int, std::placeholders::_1); + (void)bind_one_int; // to quiet clang static_assert( std::is_same< @@ -439,6 +460,7 @@ TEST(TestFunctionTraits, argument_types) { >::value, "Functor accepts an int as first argument"); auto bind_two_ints = std::bind(func_two_ints, std::placeholders::_1, std::placeholders::_2); + (void)bind_two_ints; // to quiet clang static_assert( std::is_same< @@ -454,6 +476,7 @@ TEST(TestFunctionTraits, argument_types) { auto bind_one_int_one_char = std::bind( func_one_int_one_char, std::placeholders::_1, std::placeholders::_2); + (void)bind_one_int_one_char; // to quiet clang static_assert( std::is_same< @@ -514,18 +537,21 @@ TEST(TestFunctionTraits, check_arguments) { (void)one; return 1; }; + (void)lambda_one_int; // to quiet clang auto lambda_two_ints = [](int one, int two) { (void)one; (void)two; return 2; }; + (void)lambda_two_ints; // to quiet clang auto lambda_one_int_one_char = [](int one, char two) { (void)one; (void)two; return 3; }; + (void)lambda_one_int_one_char; // to quiet clang static_assert( rclcpp::function_traits::check_arguments::value, @@ -556,11 +582,21 @@ TEST(TestFunctionTraits, check_arguments) { auto bind_one_bool = std::bind( &ObjectMember::callback_one_bool, &object_member, std::placeholders::_1); + (void)bind_one_bool; // to quiet clang // Test std::bind functions static_assert( rclcpp::function_traits::check_arguments::value, "Functor accepts a single bool as arguments"); + + auto bind_one_bool_const = std::bind( + &ObjectMember::callback_one_bool_const, &object_member, std::placeholders::_1); + (void)bind_one_bool_const; // to quiet clang + + // Test std::bind functions + static_assert( + rclcpp::function_traits::check_arguments::value, + "Functor accepts a single bool as arguments"); } /* @@ -706,3 +742,25 @@ TEST(TestFunctionTraits, sfinae_match) { EXPECT_EQ("foo", func_accept_callback_return_type(lambda_no_args_string)); } + +class TestMember : public ::testing::Test +{ +public: + void MemberFunctor(int, float, std::string) {} +}; + +/* + Regression test for https://github.com/ros2/rclcpp/issues/479, specific to classes using the + TEST_F GTest macro. +*/ +TEST_F(TestMember, bind_member_functor) { + auto bind_member_functor = std::bind( + &TestMember::MemberFunctor, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3); + (void)bind_member_functor; // to quiet clang + + static_assert( + rclcpp::function_traits::check_arguments::value, + "Functor accepts an int, a float and a string as arguments"); +} diff --git a/rclcpp/test/rclcpp/test_future_return_code.cpp b/rclcpp/test/rclcpp/test_future_return_code.cpp new file mode 100644 index 0000000000..0ad2dcd44b --- /dev/null +++ b/rclcpp/test/rclcpp/test_future_return_code.cpp @@ -0,0 +1,42 @@ +// 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 "rclcpp/future_return_code.hpp" + +TEST(TestFutureReturnCode, to_string) { + EXPECT_EQ( + "Unknown enum value (-1)", rclcpp::to_string(rclcpp::FutureReturnCode(-1))); + EXPECT_EQ( + "SUCCESS (0)", rclcpp::to_string(rclcpp::FutureReturnCode::SUCCESS)); + EXPECT_EQ( + "INTERRUPTED (1)", rclcpp::to_string(rclcpp::FutureReturnCode::INTERRUPTED)); + EXPECT_EQ( + "TIMEOUT (2)", rclcpp::to_string(rclcpp::FutureReturnCode::TIMEOUT)); + EXPECT_EQ( + "Unknown enum value (3)", rclcpp::to_string(rclcpp::FutureReturnCode(3))); + EXPECT_EQ( + "Unknown enum value (100)", rclcpp::to_string(rclcpp::FutureReturnCode(100))); +} + +TEST(FutureReturnCode, ostream) { + std::ostringstream ostream; + + ostream << rclcpp::FutureReturnCode::SUCCESS; + ASSERT_EQ("SUCCESS (0)", ostream.str()); +} diff --git a/rclcpp/test/rclcpp/test_generic_client.cpp b/rclcpp/test/rclcpp/test_generic_client.cpp new file mode 100644 index 0000000000..433348220b --- /dev/null +++ b/rclcpp/test/rclcpp/test_generic_client.cpp @@ -0,0 +1,299 @@ +// Copyright 2023 Sony Group Corporation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT 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 "rclcpp/create_generic_client.hpp" +#include "rclcpp/exceptions.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/serialization.hpp" + +#include "rcl_interfaces/srv/list_parameters.hpp" + +#include "../mocking_utils/patch.hpp" + +#include "test_msgs/srv/empty.hpp" +#include "test_msgs/srv/basic_types.hpp" + +using namespace std::chrono_literals; + +// All tests are from test_client + +class TestGenericClient : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node = std::make_shared("test_node", "/ns"); + } + + void TearDown() + { + node.reset(); + } + + rclcpp::Node::SharedPtr node; +}; + +class TestGenericClientSub : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node = std::make_shared("test_node", "/ns"); + subnode = node->create_sub_node("sub_ns"); + } + void TearDown() + { + node.reset(); + } + + rclcpp::Node::SharedPtr node; + rclcpp::Node::SharedPtr subnode; +}; + +/* + Testing client construction and destruction. + */ +TEST_F(TestGenericClient, construction_and_destruction) { + { + auto client = node->create_generic_client("test_service", "test_msgs/srv/Empty"); + } + + { + ASSERT_THROW( + { + auto client = node->create_generic_client("invalid_test_service?", "test_msgs/srv/Empty"); + }, rclcpp::exceptions::InvalidServiceNameError); + } + + { + ASSERT_THROW( + { + auto client = node->create_generic_client("test_service", "test_msgs/srv/InvalidType"); + }, rclcpp::exceptions::InvalidServiceTypeError); + } +} + +TEST_F(TestGenericClient, construction_with_free_function) { + { + auto client = rclcpp::create_generic_client( + node->get_node_base_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface(), + "test_service", + "test_msgs/srv/Empty", + rclcpp::ServicesQoS(), + nullptr); + } + { + ASSERT_THROW( + { + auto client = rclcpp::create_generic_client( + node->get_node_base_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface(), + "invalid_?test_service", + "test_msgs/srv/Empty", + rclcpp::ServicesQoS(), + nullptr); + }, rclcpp::exceptions::InvalidServiceNameError); + } + { + ASSERT_THROW( + { + auto client = rclcpp::create_generic_client( + node->get_node_base_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface(), + "test_service", + "test_msgs/srv/InvalidType", + rclcpp::ServicesQoS(), + nullptr); + }, std::runtime_error); + } + { + auto client = rclcpp::create_generic_client( + node, + "test_service", + "test_msgs/srv/Empty", + rclcpp::ServicesQoS(), + nullptr); + } + { + ASSERT_THROW( + { + auto client = rclcpp::create_generic_client( + node, + "invalid_?test_service", + "test_msgs/srv/Empty", + rclcpp::ServicesQoS(), + nullptr); + }, rclcpp::exceptions::InvalidServiceNameError); + } + { + ASSERT_THROW( + { + auto client = rclcpp::create_generic_client( + node, + "invalid_?test_service", + "test_msgs/srv/InvalidType", + rclcpp::ServicesQoS(), + nullptr); + }, std::runtime_error); + } +} + +TEST_F(TestGenericClient, construct_with_rcl_error) { + { + // reset() is not necessary for this exception, but handles unused return value warning + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_client_init, RCL_RET_ERROR); + EXPECT_THROW( + node->create_generic_client("test_service", "test_msgs/srv/Empty").reset(), + rclcpp::exceptions::RCLError); + } + { + // reset() is required for this one + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_client_fini, RCL_RET_ERROR); + EXPECT_NO_THROW( + node->create_generic_client("test_service", "test_msgs/srv/Empty").reset()); + } +} + +TEST_F(TestGenericClient, wait_for_service) { + const std::string service_name = "test_service"; + + auto client = node->create_generic_client(service_name, "test_msgs/srv/Empty"); + EXPECT_FALSE(client->wait_for_service(std::chrono::nanoseconds(0))); + EXPECT_FALSE(client->wait_for_service(std::chrono::milliseconds(10))); + + auto callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}; + + auto service = + node->create_service(service_name, std::move(callback)); + + EXPECT_TRUE(client->wait_for_service(std::chrono::nanoseconds(-1))); + EXPECT_TRUE(client->service_is_ready()); +} + +/* + Testing generic client construction and destruction for subnodes. + */ +TEST_F(TestGenericClientSub, construction_and_destruction) { + { + auto client = subnode->create_generic_client("test_service", "test_msgs/srv/Empty"); + EXPECT_STREQ(client->get_service_name(), "/ns/test_service"); + } + + { + ASSERT_THROW( + { + auto client = node->create_generic_client("invalid_service?", "test_msgs/srv/Empty"); + }, rclcpp::exceptions::InvalidServiceNameError); + } +} + +TEST_F(TestGenericClientSub, async_send_request_with_request) { + const std::string service_name = "test_service"; + int64_t expected_change = 1111; + + auto client = node->create_generic_client(service_name, "test_msgs/srv/BasicTypes"); + + auto callback = [&expected_change]( + const test_msgs::srv::BasicTypes::Request::SharedPtr request, + test_msgs::srv::BasicTypes::Response::SharedPtr response) { + response->int64_value = request->int64_value + expected_change; + }; + + auto service = + node->create_service(service_name, std::move(callback)); + + ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(5))); + ASSERT_TRUE(client->service_is_ready()); + + test_msgs::srv::BasicTypes::Request request; + request.int64_value = 12345678; + + auto future = client->async_send_request(static_cast(&request)); + rclcpp::spin_until_future_complete( + node->get_node_base_interface(), future, std::chrono::seconds(5)); + ASSERT_TRUE(future.valid()); + auto get_untyped_response = future.get(); + auto typed_response = + static_cast(get_untyped_response.get()); + EXPECT_EQ(typed_response->int64_value, (request.int64_value + expected_change)); +} + +TEST_F(TestGenericClientSub, async_send_request_with_request_and_callback) { + const std::string service_name = "test_service"; + int64_t expected_change = 2222; + + auto client = node->create_generic_client(service_name, "test_msgs/srv/BasicTypes"); + + auto server_callback = [&expected_change]( + const test_msgs::srv::BasicTypes::Request::SharedPtr request, + test_msgs::srv::BasicTypes::Response::SharedPtr response) { + response->int64_value = request->int64_value + expected_change; + }; + + auto service = + node->create_service(service_name, std::move(server_callback)); + + ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(5))); + ASSERT_TRUE(client->service_is_ready()); + + test_msgs::srv::BasicTypes::Request request; + request.int64_value = 12345678; + + auto client_callback = [&request, &expected_change]( + rclcpp::GenericClient::SharedFuture future) { + auto untyped_response = future.get(); + auto typed_response = + static_cast(untyped_response.get()); + EXPECT_EQ(typed_response->int64_value, (request.int64_value + expected_change)); + }; + + auto future = + client->async_send_request(static_cast(&request), client_callback); + rclcpp::spin_until_future_complete( + node->get_node_base_interface(), future, std::chrono::seconds(5)); +} diff --git a/rclcpp/test/rclcpp/test_generic_pubsub.cpp b/rclcpp/test/rclcpp/test_generic_pubsub.cpp new file mode 100644 index 0000000000..79fbfcc33a --- /dev/null +++ b/rclcpp/test/rclcpp/test_generic_pubsub.cpp @@ -0,0 +1,311 @@ +// Copyright 2018, Bosch Software Innovations GmbH. +// Copyright 2021, Apex.AI 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 "test_msgs/message_fixtures.hpp" +#include "test_msgs/msg/basic_types.hpp" + +#include "rcl/graph.h" + +#include "rclcpp/generic_publisher.hpp" +#include "rclcpp/generic_subscription.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/serialization.hpp" +#include "rclcpp/serialized_message.hpp" + +using namespace ::testing; // NOLINT +using namespace rclcpp; // NOLINT + +class RclcppGenericNodeFixture : public Test +{ +public: + RclcppGenericNodeFixture() + { + node_ = std::make_shared("pubsub"); + publisher_node_ = std::make_shared( + "publisher_node", + rclcpp::NodeOptions().start_parameter_event_publisher(false)); + } + + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void create_publisher(const std::string & topic) + { + auto publisher = publisher_node_->create_publisher(topic, 10); + publishers_.push_back(publisher); + } + + template + std::vector subscribe_raw_messages( + size_t expected_recv_msg_count, const std::string & topic_name, const std::string & type) + { + std::vector messages; + size_t counter = 0; + auto subscription = node_->create_generic_subscription( + topic_name, type, rclcpp::QoS(1), + [&counter, &messages, this](const std::shared_ptr message) { + T2 deserialized_message; + rclcpp::Serialization serializer; + serializer.deserialize_message(message.get(), &deserialized_message); + messages.push_back(this->get_data_from_msg(deserialized_message)); + counter++; + }); + + while (counter < expected_recv_msg_count) { + rclcpp::spin_some(node_); + } + return messages; + } + + template + rclcpp::SerializedMessage serialize_message(const T1 & data) + { + T2 message; + write_message(data, message); + + rclcpp::Serialization ser; + SerializedMessage result; + ser.serialize_message(&message, &result); + return result; + } + + void sleep_to_allow_topics_discovery() + { + // This is a short sleep to allow the node some time to discover the topic + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + } + + template + bool wait_for(const Condition & condition, const Duration & timeout) + { + using clock = std::chrono::system_clock; + auto start = clock::now(); + while (!condition()) { + if ((clock::now() - start) > timeout) { + return false; + } + rclcpp::spin_some(node_); + } + return true; + } + + std::shared_ptr node_; + rclcpp::Node::SharedPtr publisher_node_; + std::vector> publishers_; + +private: + void write_message(const std::string & data, test_msgs::msg::Strings & message) + { + message.string_value = data; + } + + void write_message(const int64_t & data, test_msgs::msg::BasicTypes & message) + { + message.int64_value = data; + } + + std::string get_data_from_msg(const test_msgs::msg::Strings & message) + { + return message.string_value; + } + + int64_t get_data_from_msg(const test_msgs::msg::BasicTypes & message) + { + return message.int64_value; + } +}; + + +TEST_F(RclcppGenericNodeFixture, publisher_and_subscriber_work) +{ + // We currently publish more messages because they can get lost + std::vector test_messages = {"Hello World", "Hello World"}; + std::string topic_name = "/string_topic"; + std::string type = "test_msgs/msg/Strings"; + + auto publisher = node_->create_generic_publisher( + topic_name, type, rclcpp::QoS(1)); + + auto subscriber_future_ = std::async( + std::launch::async, [this, topic_name, type] { + return subscribe_raw_messages(1, topic_name, type); + }); + + // TODO(karsten1987): Port 'wait_for_sub' to rclcpp + auto allocator = node_->get_node_options().allocator(); + auto success = false; + auto ret = rcl_wait_for_subscribers( + node_->get_node_base_interface()->get_rcl_node_handle(), + &allocator, + topic_name.c_str(), + 1u, + static_cast(1e9), + &success); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_TRUE(success); + + for (const auto & message : test_messages) { + publisher->publish(serialize_message(message)); + } + + auto subscribed_messages = subscriber_future_.get(); + EXPECT_THAT(subscribed_messages, SizeIs(Not(0))); + EXPECT_THAT(subscribed_messages[0], StrEq("Hello World")); +} + +TEST_F(RclcppGenericNodeFixture, publish_loaned_msg_work) +{ + // We currently publish more messages because they can get lost + std::vector test_messages = {100, 100}; + std::string topic_name = "/int64_topic"; + std::string type = "test_msgs/msg/BasicTypes"; + + auto publisher = node_->create_generic_publisher(topic_name, type, rclcpp::QoS(1)); + + if (publisher->can_loan_messages()) { + auto subscriber_future_ = std::async( + std::launch::async, [this, topic_name, type] { + return subscribe_raw_messages( + 1, topic_name, type); + }); + + auto allocator = node_->get_node_options().allocator(); + auto success = false; + auto ret = rcl_wait_for_subscribers( + node_->get_node_base_interface()->get_rcl_node_handle(), + &allocator, + topic_name.c_str(), + 1u, + static_cast(2e9), + &success); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_TRUE(success); + + for (const auto & message : test_messages) { + publisher->publish_as_loaned_msg( + serialize_message(message)); + } + + auto subscribed_messages = subscriber_future_.get(); + EXPECT_THAT(subscribed_messages, SizeIs(Not(0))); + EXPECT_EQ(subscribed_messages[0], test_messages[0]); + } else { + ASSERT_THROW( + { + publisher->publish_as_loaned_msg( + serialize_message(test_messages[0])); + }, rclcpp::exceptions::RCLError); + } +} + +TEST_F(RclcppGenericNodeFixture, generic_subscription_uses_qos) +{ + // If the GenericSubscription does not use the provided QoS profile, + // its request will be incompatible with the Publisher's offer and no messages will be passed. + using namespace std::chrono_literals; + std::string topic_name = "string_topic"; + std::string topic_type = "test_msgs/msg/Strings"; + rclcpp::QoS qos = rclcpp::SensorDataQoS(); + + auto publisher = node_->create_publisher(topic_name, qos); + auto subscription = node_->create_generic_subscription( + topic_name, topic_type, qos, + [](std::shared_ptr/* message */) {}); + auto connected = [publisher, subscription]() -> bool { + return publisher->get_subscription_count() && subscription->get_publisher_count(); + }; + // It normally takes < 20ms, 5s chosen as "a very long time" + ASSERT_TRUE(wait_for(connected, 5s)); +} + +TEST_F(RclcppGenericNodeFixture, generic_publisher_uses_qos) +{ + // If the GenericPublisher does not use the provided QoS profile, + // its offer will be incompatible with the Subscription's request and no messages will be passed. + using namespace std::chrono_literals; + std::string topic_name = "string_topic"; + std::string topic_type = "test_msgs/msg/Strings"; + rclcpp::QoS qos = rclcpp::QoS(1).transient_local(); + + auto publisher = node_->create_generic_publisher(topic_name, topic_type, qos); + auto subscription = node_->create_subscription( + topic_name, qos, + [](std::shared_ptr/* message */) {}); + auto connected = [publisher, subscription]() -> bool { + return publisher->get_subscription_count() && subscription->get_publisher_count(); + }; + // It normally takes < 20ms, 5s chosen as "a very long time" + ASSERT_TRUE(wait_for(connected, 5s)); +} + +TEST_F(RclcppGenericNodeFixture, generic_subscription_different_callbacks) +{ + using namespace std::chrono_literals; + std::string topic_name = "string_topic"; + std::string topic_type = "test_msgs/msg/Strings"; + rclcpp::QoS qos = rclcpp::QoS(1); + + auto publisher = node_->create_publisher(topic_name, qos); + + // Test shared_ptr for const messages + { + auto subscription = node_->create_generic_subscription( + topic_name, topic_type, qos, + [](const std::shared_ptr/* message */) {}); + auto connected = [publisher, subscription]() -> bool { + return publisher->get_subscription_count() && subscription->get_publisher_count(); + }; + // It normally takes < 20ms, 5s chosen as "a very long time" + ASSERT_TRUE(wait_for(connected, 5s)); + } + + // Test unique_ptr + { + auto subscription = node_->create_generic_subscription( + topic_name, topic_type, qos, + [](std::unique_ptr/* message */) {}); + auto connected = [publisher, subscription]() -> bool { + return publisher->get_subscription_count() && subscription->get_publisher_count(); + }; + // It normally takes < 20ms, 5s chosen as "a very long time" + ASSERT_TRUE(wait_for(connected, 5s)); + } + + // Test message callback + { + auto subscription = node_->create_generic_subscription( + topic_name, topic_type, qos, + [](rclcpp::SerializedMessage /* message */) {}); + auto connected = [publisher, subscription]() -> bool { + return publisher->get_subscription_count() && subscription->get_publisher_count(); + }; + // It normally takes < 20ms, 5s chosen as "a very long time" + ASSERT_TRUE(wait_for(connected, 5s)); + } +} diff --git a/rclcpp/test/rclcpp/test_generic_service.cpp b/rclcpp/test/rclcpp/test_generic_service.cpp new file mode 100644 index 0000000000..554fdf0c0f --- /dev/null +++ b/rclcpp/test/rclcpp/test_generic_service.cpp @@ -0,0 +1,422 @@ +// Copyright 2024 Sony Group Corporation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT 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 "rclcpp/exceptions.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "../mocking_utils/patch.hpp" +#include "../utils/rclcpp_gtest_macros.hpp" + +#include "rcl_interfaces/srv/list_parameters.hpp" +#include "test_msgs/srv/empty.hpp" +#include "test_msgs/srv/basic_types.hpp" + +using namespace std::chrono_literals; + +class TestGenericService : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node = std::make_shared("test_node", "/ns"); + } + + void TearDown() + { + node.reset(); + } + + rclcpp::Node::SharedPtr node; +}; + +class TestGenericServiceSub : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node = std::make_shared("test_node", "/ns"); + subnode = node->create_sub_node("sub_ns"); + } + + void TearDown() + { + node.reset(); + } + + rclcpp::Node::SharedPtr node; + rclcpp::Node::SharedPtr subnode; +}; + +/* + Testing service construction and destruction. + */ +TEST_F(TestGenericService, construction_and_destruction) { + auto callback = []( + rclcpp::GenericService::SharedRequest, + rclcpp::GenericService::SharedResponse) {}; + { + auto generic_service = node->create_generic_service( + "test_generic_service", "rcl_interfaces/srv/ListParameters", callback); + EXPECT_NE(nullptr, generic_service->get_service_handle()); + const rclcpp::ServiceBase * const_service_base = generic_service.get(); + EXPECT_NE(nullptr, const_service_base->get_service_handle()); + } + + { + ASSERT_THROW( + { + auto generic_service = node->create_generic_service( + "invalid_service?", "test_msgs/srv/Empty", callback); + }, rclcpp::exceptions::InvalidServiceNameError); + } + + { + ASSERT_THROW( + { + auto generic_service = node->create_generic_service( + "test_generic_service", "test_msgs/srv/NotExist", callback); + }, rclcpp::exceptions::InvalidServiceTypeError); + } +} + +/* + Testing service construction and destruction for subnodes. + */ +TEST_F(TestGenericServiceSub, construction_and_destruction) { + auto callback = []( + rclcpp::GenericService::SharedRequest, + rclcpp::GenericService::SharedResponse) {}; + { + auto generic_service = subnode->create_generic_service( + "test_generic_service", "rcl_interfaces/srv/ListParameters", callback); + EXPECT_STREQ(generic_service->get_service_name(), "/ns/sub_ns/test_generic_service"); + } + + { + ASSERT_THROW( + { + auto generic_service = subnode->create_generic_service( + "invalid_service?", "test_msgs/srv/Empty", callback); + }, rclcpp::exceptions::InvalidServiceNameError); + } + + { + ASSERT_THROW( + { + auto generic_service = subnode->create_generic_service( + "test_generic_service", "test_msgs/srv/NotExist", callback); + }, rclcpp::exceptions::InvalidServiceTypeError); + } +} + +TEST_F(TestGenericService, construction_and_destruction_rcl_errors) { + auto callback = []( + rclcpp::GenericService::SharedRequest, rclcpp::GenericService::SharedResponse) {}; + + { + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_service_init, RCL_RET_ERROR); + // reset() isn't necessary for this exception, it just avoids unused return value warning + EXPECT_THROW( + node->create_generic_service("service", "test_msgs/srv/Empty", callback).reset(), + rclcpp::exceptions::RCLError); + } + { + // reset() is required for this one + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_service_fini, RCL_RET_ERROR); + EXPECT_NO_THROW( + node->create_generic_service("service", "test_msgs/srv/Empty", callback).reset()); + } +} + +TEST_F(TestGenericService, generic_service_take_request) { + auto callback = []( + rclcpp::GenericService::SharedRequest, rclcpp::GenericService::SharedResponse) {}; + auto generic_service = + node->create_generic_service("test_service", "test_msgs/srv/Empty", callback); + { + auto request_id = generic_service->create_request_header(); + auto request = generic_service->create_request(); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_take_request, RCL_RET_OK); + EXPECT_TRUE(generic_service->take_request(request, *request_id.get())); + } + { + auto request_id = generic_service->create_request_header(); + auto request = generic_service->create_request(); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_take_request, RCL_RET_SERVICE_TAKE_FAILED); + EXPECT_FALSE(generic_service->take_request(request, *request_id.get())); + } + { + auto request_id = generic_service->create_request_header(); + auto request = generic_service->create_request(); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_take_request, RCL_RET_ERROR); + EXPECT_THROW( + generic_service->take_request(request, *request_id.get()), rclcpp::exceptions::RCLError); + } +} + +TEST_F(TestGenericService, generic_service_send_response) { + auto callback = []( + const rclcpp::GenericService::SharedRequest, rclcpp::GenericService::SharedResponse) {}; + auto generic_service = + node->create_generic_service("test_service", "test_msgs/srv/Empty", callback); + + { + auto request_id = generic_service->create_request_header(); + auto response = generic_service->create_response(); + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_send_response, RCL_RET_OK); + EXPECT_NO_THROW(generic_service->send_response(*request_id.get(), response)); + } + + { + auto request_id = generic_service->create_request_header(); + auto response = generic_service->create_response(); + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_send_response, RCL_RET_ERROR); + EXPECT_THROW( + generic_service->send_response(*request_id.get(), response), + rclcpp::exceptions::RCLError); + } +} + +/* + Testing on_new_request callbacks. + */ +TEST_F(TestGenericService, generic_service_on_new_request_callback) { + auto server_callback = []( + const rclcpp::GenericService::SharedRequest, rclcpp::GenericService::SharedResponse) {FAIL();}; + rclcpp::ServicesQoS service_qos; + service_qos.keep_last(3); + auto generic_service = node->create_generic_service( + "~/test_service", "test_msgs/srv/Empty", server_callback, service_qos); + + std::atomic c1 {0}; + auto increase_c1_cb = [&c1](size_t count_msgs) {c1 += count_msgs;}; + generic_service->set_on_new_request_callback(increase_c1_cb); + + auto client = node->create_client( + "~/test_service", service_qos); + { + auto request = std::make_shared(); + client->async_send_request(request); + } + + auto start = std::chrono::steady_clock::now(); + do { + std::this_thread::sleep_for(100ms); + } while (c1 == 0 && std::chrono::steady_clock::now() - start < 10s); + + EXPECT_EQ(c1.load(), 1u); + + std::atomic c2 {0}; + auto increase_c2_cb = [&c2](size_t count_msgs) {c2 += count_msgs;}; + generic_service->set_on_new_request_callback(increase_c2_cb); + + { + auto request = std::make_shared(); + client->async_send_request(request); + } + + start = std::chrono::steady_clock::now(); + do { + std::this_thread::sleep_for(100ms); + } while (c2 == 0 && std::chrono::steady_clock::now() - start < 10s); + + EXPECT_EQ(c1.load(), 1u); + EXPECT_EQ(c2.load(), 1u); + + generic_service->clear_on_new_request_callback(); + + { + auto request = std::make_shared(); + client->async_send_request(request); + client->async_send_request(request); + client->async_send_request(request); + } + + std::atomic c3 {0}; + auto increase_c3_cb = [&c3](size_t count_msgs) {c3 += count_msgs;}; + generic_service->set_on_new_request_callback(increase_c3_cb); + + start = std::chrono::steady_clock::now(); + do { + std::this_thread::sleep_for(100ms); + } while (c3 < 3 && std::chrono::steady_clock::now() - start < 10s); + + EXPECT_EQ(c1.load(), 1u); + EXPECT_EQ(c2.load(), 1u); + EXPECT_EQ(c3.load(), 3u); + + std::function invalid_cb = nullptr; + EXPECT_THROW(generic_service->set_on_new_request_callback(invalid_cb), std::invalid_argument); +} + +TEST_F(TestGenericService, rcl_service_response_publisher_get_actual_qos_error) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_service_response_publisher_get_actual_qos, nullptr); + auto callback = []( + const rclcpp::GenericService::SharedRequest, rclcpp::GenericService::SharedResponse) {}; + auto generic_service = + node->create_generic_service("test_service", "test_msgs/srv/Empty", callback); + RCLCPP_EXPECT_THROW_EQ( + generic_service->get_response_publisher_actual_qos(), + std::runtime_error("failed to get service's response publisher qos settings: error not set")); +} + +TEST_F(TestGenericService, rcl_service_request_subscription_get_actual_qos_error) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_service_request_subscription_get_actual_qos, nullptr); + auto callback = []( + const rclcpp::GenericService::SharedRequest, rclcpp::GenericService::SharedResponse) {}; + auto generic_service = + node->create_generic_service("test_service", "test_msgs/srv/Empty", callback); + RCLCPP_EXPECT_THROW_EQ( + generic_service->get_request_subscription_actual_qos(), + std::runtime_error("failed to get service's request subscription qos settings: error not set")); +} + +TEST_F(TestGenericService, generic_service_qos) { + rclcpp::ServicesQoS qos_profile; + qos_profile.liveliness(rclcpp::LivelinessPolicy::Automatic); + rclcpp::Duration duration(std::chrono::nanoseconds(1)); + qos_profile.deadline(duration); + qos_profile.lifespan(duration); + qos_profile.liveliness_lease_duration(duration); + + auto callback = []( + const rclcpp::GenericService::SharedRequest, rclcpp::GenericService::SharedResponse) {}; + auto generic_service = + node->create_generic_service("test_service", "test_msgs/srv/Empty", callback, qos_profile); + + auto rs_qos = generic_service->get_request_subscription_actual_qos(); + auto rp_qos = generic_service->get_response_publisher_actual_qos(); + + EXPECT_EQ(qos_profile, rp_qos); + // Lifespan has no meaning for subscription/readers + rs_qos.lifespan(qos_profile.lifespan()); + EXPECT_EQ(qos_profile, rs_qos); +} + +TEST_F(TestGenericService, generic_service_qos_depth) { + uint64_t server_cb_count_ = 0; + auto server_callback = [&]( + const rclcpp::GenericService::SharedRequest, + rclcpp::GenericService::SharedResponse) {server_cb_count_++;}; + + auto server_node = std::make_shared("server_node", "/ns"); + + rclcpp::QoS server_qos_profile(2); + + auto generic_service = server_node->create_generic_service( + "test_qos_depth", "test_msgs/srv/Empty", std::move(server_callback), server_qos_profile); + + rclcpp::QoS client_qos_profile(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); + auto client = node->create_client("test_qos_depth", client_qos_profile); + + ::testing::AssertionResult request_result = ::testing::AssertionSuccess(); + auto request = std::make_shared(); + + auto client_callback = [&request_result]( + rclcpp::Client::SharedFuture future_response) { + if (nullptr == future_response.get()) { + request_result = ::testing::AssertionFailure() << "Future response was null"; + } + }; + + uint64_t client_requests = 5; + for (uint64_t i = 0; i < client_requests; i++) { + client->async_send_request(request, client_callback); + std::this_thread::sleep_for(10ms); + } + + auto start = std::chrono::steady_clock::now(); + while ((server_cb_count_ < server_qos_profile.depth()) && + (std::chrono::steady_clock::now() - start) < 1s) + { + rclcpp::spin_some(server_node); + std::this_thread::sleep_for(1ms); + } + + // Spin an extra time to check if server QoS depth has been ignored, + // so more server responses might be processed than expected. + rclcpp::spin_some(server_node); + + EXPECT_EQ(server_cb_count_, server_qos_profile.depth()); +} + +TEST_F(TestGenericService, generic_service_and_client) { + const std::string service_name = "test_service"; + const std::string service_type = "test_msgs/srv/BasicTypes"; + int64_t expected_change = 87654321; + + auto callback = [&expected_change]( + const rclcpp::GenericService::SharedRequest request, + rclcpp::GenericService::SharedResponse response) { + auto typed_request = static_cast(request.get()); + auto typed_response = static_cast(response.get()); + + typed_response->int64_value = typed_request->int64_value + expected_change; + }; + auto generic_service = node->create_generic_service(service_name, service_type, callback); + + auto client = node->create_client(service_name); + + ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(5))); + ASSERT_TRUE(client->service_is_ready()); + + auto request = std::make_shared(); + request->int64_value = 12345678; + + auto generic_client_callback = [&request, &expected_change]( + std::shared_future future) { + auto response = future.get(); + EXPECT_EQ(response->int64_value, (request->int64_value + expected_change)); + }; + + auto future = + client->async_send_request(request, generic_client_callback); + rclcpp::spin_until_future_complete( + node->get_node_base_interface(), future, std::chrono::seconds(5)); +} diff --git a/rclcpp/test/rclcpp/test_graph_listener.cpp b/rclcpp/test/rclcpp/test_graph_listener.cpp new file mode 100644 index 0000000000..24c370e65d --- /dev/null +++ b/rclcpp/test/rclcpp/test_graph_listener.cpp @@ -0,0 +1,298 @@ +// 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 "rclcpp/contexts/default_context.hpp" +#include "rclcpp/graph_listener.hpp" +#include "rclcpp/node_interfaces/node_graph.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "../mocking_utils/patch.hpp" +#include "../utils/rclcpp_gtest_macros.hpp" + +namespace +{ + +constexpr char node_name[] = "node"; +constexpr char node_namespace[] = "ns"; +constexpr char shutdown_error_str[] = "GraphListener already shutdown"; + +} // namespace + +class TestGraphListener : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + node_ = std::make_shared(node_name, node_namespace); + + node_graph_ = node_->get_node_graph_interface(); + ASSERT_NE(nullptr, node_graph_); + + graph_listener_ = + std::make_shared( + rclcpp::contexts::get_global_default_context()); + } + + void TearDown() + { + rclcpp::shutdown(); + } + +protected: + std::shared_ptr node() {return node_;} + rclcpp::node_interfaces::NodeGraphInterface * node_graph() {return node_graph_.get();} + std::shared_ptr graph_listener() {return graph_listener_;} + +private: + std::shared_ptr node_; + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_; + std::shared_ptr graph_listener_; +}; + +/* Run base class init/shutdown */ +TEST_F(TestGraphListener, construction_and_destruction) { + EXPECT_FALSE(graph_listener()->has_node(node_graph())); + EXPECT_FALSE(graph_listener()->is_shutdown()); +} + +// Required for mocking_utils below +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, ==) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, !=) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, <) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, >) + +/* Error creating a new graph listener */ +TEST_F(TestGraphListener, error_construct_graph_listener) { + using rclcpp::contexts::get_global_default_context; + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_guard_condition_init, RCL_RET_ERROR); + + RCLCPP_EXPECT_THROW_EQ( + { + auto graph_listener_error = + std::make_shared(get_global_default_context()); + graph_listener_error.reset(); + }, std::runtime_error("failed to create guard condition: error not set")); +} + +// Required for mocking_utils below +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, >) + +/* Errors that occur when initializing the graph_listener */ +TEST_F(TestGraphListener, error_start_graph_listener) { + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_wait_set_init, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + graph_listener()->start_if_not_started(), + std::runtime_error("failed to initialize wait set: error not set")); + } + { + EXPECT_NO_THROW(graph_listener()->shutdown()); + RCLCPP_EXPECT_THROW_EQ( + graph_listener()->start_if_not_started(), + std::runtime_error(shutdown_error_str)); + } +} + +class TestGraphListenerProtectedMethods : public rclcpp::graph_listener::GraphListener +{ +public: + explicit TestGraphListenerProtectedMethods(std::shared_ptr parent_context) + : rclcpp::graph_listener::GraphListener{parent_context} + {} + + void run_protected() + { + this->run(); + } + + void mock_init_wait_set() + { + this->init_wait_set(); + } + + void mock_cleanup_wait_set() + { + this->cleanup_wait_set(); + } +}; + +/* Errors running graph protected methods */ +TEST_F(TestGraphListener, error_run_graph_listener_destroy_context) { + auto context_to_destroy = std::make_shared(); + context_to_destroy->init(0, nullptr); + auto graph_listener_error = + std::make_shared(context_to_destroy); + context_to_destroy.reset(); + EXPECT_THROW( + graph_listener_error->run_protected(), + rclcpp::exceptions::RCLError); +} + +TEST_F(TestGraphListener, error_run_graph_listener_mock_wait_set_clear) { + auto global_context = rclcpp::contexts::get_global_default_context(); + auto graph_listener_test = + std::make_shared(global_context); + graph_listener_test->mock_init_wait_set(); + auto mock_wait_set_clear = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_wait_set_clear, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + graph_listener_test->run_protected(), + std::runtime_error("failed to clear wait set: error not set")); +} + +TEST_F(TestGraphListener, error_run_graph_listener_mock_wait_set_add_guard_condition) { + auto global_context = rclcpp::contexts::get_global_default_context(); + auto graph_listener_test = + std::make_shared(global_context); + graph_listener_test->mock_init_wait_set(); + auto mock_wait_set_clear = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_wait_set_add_guard_condition, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + graph_listener_test->run_protected(), + std::runtime_error("failed to add guard condition to wait set: error not set")); +} + +TEST_F(TestGraphListener, error_run_graph_listener_mock_wait_error) { + auto global_context = rclcpp::contexts::get_global_default_context(); + auto graph_listener_test = + std::make_shared(global_context); + graph_listener_test->mock_init_wait_set(); + auto mock_wait_set_clear = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_wait, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + graph_listener_test->run_protected(), + std::runtime_error("failed to wait on wait set: error not set")); +} + +TEST_F(TestGraphListener, error_run_graph_listener_mock_wait_timeout) { + auto global_context = rclcpp::contexts::get_global_default_context(); + auto graph_listener_test = + std::make_shared(global_context); + graph_listener_test->mock_init_wait_set(); + auto mock_wait_set_clear = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_wait, RCL_RET_TIMEOUT); + RCLCPP_EXPECT_THROW_EQ( + graph_listener_test->run_protected(), + std::runtime_error("rcl_wait unexpectedly timed out")); +} + +/* Add/Remove node usage */ +TEST_F(TestGraphListener, test_graph_listener_add_remove_node) { + EXPECT_FALSE(graph_listener()->has_node(node_graph())); + + graph_listener()->add_node(node_graph()); + EXPECT_TRUE(graph_listener()->has_node(node_graph())); + + graph_listener()->remove_node(node_graph()); + EXPECT_FALSE(graph_listener()->has_node(node_graph())); +} + +/* Add/Remove node error usage */ +TEST_F(TestGraphListener, test_errors_graph_listener_add_remove_node) { + // nullptrs tests + EXPECT_FALSE(graph_listener()->has_node(nullptr)); + + RCLCPP_EXPECT_THROW_EQ( + graph_listener()->add_node(nullptr), + std::invalid_argument("node is nullptr")); + + RCLCPP_EXPECT_THROW_EQ( + graph_listener()->remove_node(nullptr), + std::invalid_argument("node is nullptr")); + + // Already added + graph_listener()->add_node(node_graph()); + EXPECT_TRUE(graph_listener()->has_node(node_graph())); + RCLCPP_EXPECT_THROW_EQ( + graph_listener()->add_node(node_graph()), + std::runtime_error("node already added")); + + // Remove node not found + graph_listener()->remove_node(node_graph()); + EXPECT_FALSE(graph_listener()->has_node(node_graph())); + RCLCPP_EXPECT_THROW_EQ( + graph_listener()->remove_node(node_graph()), + std::runtime_error("node not found")); + + // Add and remove after shutdown + EXPECT_NO_THROW(graph_listener()->shutdown()); + RCLCPP_EXPECT_THROW_EQ( + graph_listener()->add_node(node_graph()), + std::runtime_error(shutdown_error_str)); + // Remove works the same + RCLCPP_EXPECT_THROW_EQ( + graph_listener()->remove_node(node_graph()), + std::runtime_error("node not found")); +} + +/* Shutdown errors */ +TEST_F(TestGraphListener, test_graph_listener_shutdown_wait_fini_error_nothrow) { + auto global_context = rclcpp::contexts::get_global_default_context(); + auto graph_listener_test = + std::make_shared(global_context); + + graph_listener_test->start_if_not_started(); + + { + auto mock_wait_set_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_wait_set_fini, RCL_RET_ERROR); + // Exception is logged when using nothrow_t + EXPECT_NO_THROW(graph_listener_test->shutdown(std::nothrow_t())); + } + + graph_listener_test->mock_cleanup_wait_set(); +} + +TEST_F(TestGraphListener, test_graph_listener_shutdown_wait_fini_error_throw) { + auto global_context = rclcpp::contexts::get_global_default_context(); + auto graph_listener_test = + std::make_shared(global_context); + + graph_listener()->start_if_not_started(); + + { + auto mock_wait_set_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_wait_set_fini, RCL_RET_ERROR); + + RCLCPP_EXPECT_THROW_EQ( + graph_listener()->shutdown(), + std::runtime_error("failed to finalize wait set: error not set")); + } + + graph_listener_test->mock_cleanup_wait_set(); +} + +TEST_F(TestGraphListener, test_graph_listener_shutdown_guard_fini_error_throw) { + auto global_context = rclcpp::contexts::get_global_default_context(); + auto graph_listener_test = + std::make_shared(global_context); + + graph_listener_test->start_if_not_started(); + + auto mock_wait_set_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_guard_condition_fini, RCL_RET_ERROR); + + EXPECT_NO_THROW(graph_listener_test->shutdown()); + + graph_listener_test->mock_cleanup_wait_set(); +} diff --git a/rclcpp/test/rclcpp/test_guard_condition.cpp b/rclcpp/test/rclcpp/test_guard_condition.cpp new file mode 100644 index 0000000000..c9fc138ccc --- /dev/null +++ b/rclcpp/test/rclcpp/test_guard_condition.cpp @@ -0,0 +1,171 @@ +// 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 "rclcpp/rclcpp.hpp" + +#include "../mocking_utils/patch.hpp" + +class TestGuardCondition : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } +}; + +/* + * Testing normal construction and destruction. + */ +TEST_F(TestGuardCondition, construction_and_destruction) { + { + auto gc = std::make_shared(); + (void)gc; + } + + { + // invalid context (nullptr) + ASSERT_THROW( + { + auto gc = std::make_shared(nullptr); + (void)gc; + }, std::invalid_argument); + } + + { + // invalid context (uninitialized) + auto context = std::make_shared(); + ASSERT_THROW( + { + auto gc = std::make_shared(context); + (void)gc; + }, rclcpp::exceptions::RCLInvalidArgument); + } + + { + auto mock = mocking_utils::inject_on_return( + "lib:rclcpp", rcl_guard_condition_fini, RCL_RET_ERROR); + auto gc = std::make_shared(); + // This just logs an error on destruction + EXPECT_NO_THROW(gc.reset()); + } +} + +/* + * Testing rcl guard condition accessor. + */ +TEST_F(TestGuardCondition, get_rcl_guard_condition) { + { + auto gc = std::make_shared(); + gc->get_rcl_guard_condition(); + } +} + +/* + * Testing tigger method. + */ +TEST_F(TestGuardCondition, trigger) { + { + auto gc = std::make_shared(); + EXPECT_NO_THROW(gc->trigger()); + + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); + auto gc = std::make_shared(); + EXPECT_THROW(gc->trigger(), rclcpp::exceptions::RCLError); + } + } +} + +/* + * Testing addition to a wait set + */ +TEST_F(TestGuardCondition, add_to_wait_set) { + { + { + auto gc = std::make_shared(); + + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_wait_set_add_guard_condition, RCL_RET_OK); + + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + EXPECT_NO_THROW(gc->add_to_wait_set(wait_set)); + EXPECT_NO_THROW(gc->add_to_wait_set(wait_set)); + + rcl_wait_set_t wait_set_2 = rcl_get_zero_initialized_wait_set(); + EXPECT_THROW(gc->add_to_wait_set(wait_set_2), std::runtime_error); + } + } +} + +/* + * Testing set on trigger callback + */ +TEST_F(TestGuardCondition, set_on_trigger_callback) { + { + auto gc = std::make_shared(); + + std::atomic c1 {0}; + auto increase_c1_cb = [&c1](size_t count_msgs) {c1 += count_msgs;}; + gc->set_on_trigger_callback(increase_c1_cb); + + EXPECT_EQ(c1.load(), 0u); + EXPECT_NO_THROW(gc->trigger()); + EXPECT_EQ(c1.load(), 1u); + + std::atomic c2 {0}; + auto increase_c2_cb = [&c2](size_t count_msgs) {c2 += count_msgs;}; + gc->set_on_trigger_callback(increase_c2_cb); + + EXPECT_NO_THROW(gc->trigger()); + EXPECT_EQ(c1.load(), 1u); + EXPECT_EQ(c2.load(), 1u); + + gc->set_on_trigger_callback(nullptr); + EXPECT_NO_THROW(gc->trigger()); + EXPECT_EQ(c1.load(), 1u); + EXPECT_EQ(c2.load(), 1u); + + gc->set_on_trigger_callback(increase_c1_cb); + EXPECT_EQ(c1.load(), 2u); + } +} + +/* + * Testing that callback and waitset are both notified by triggering gc + */ +TEST_F(TestGuardCondition, callback_and_waitset) { + auto gc = std::make_shared(); + std::atomic c1 {0}; + auto increase_c1_cb = [&c1](size_t count_msgs) {c1 += count_msgs;}; + gc->set_on_trigger_callback(increase_c1_cb); + + rclcpp::WaitSet wait_set; + wait_set.add_guard_condition(gc); + + gc->trigger(); + + EXPECT_EQ(rclcpp::WaitResultKind::Ready, wait_set.wait(std::chrono::seconds(1)).kind()); + EXPECT_EQ(c1.load(), 1u); +} diff --git a/rclcpp/test/rclcpp/test_init_options.cpp b/rclcpp/test/rclcpp/test_init_options.cpp new file mode 100644 index 0000000000..25c23e1ddd --- /dev/null +++ b/rclcpp/test/rclcpp/test_init_options.cpp @@ -0,0 +1,140 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include + +#include "rcl/allocator.h" +#include "rcl/domain_id.h" + +#include "rclcpp/init_options.hpp" + +#include "../mocking_utils/patch.hpp" +#include "../utils/rclcpp_gtest_macros.hpp" + +TEST(TestInitOptions, test_construction) { + rcl_allocator_t allocator = rcl_get_default_allocator(); + auto options = rclcpp::InitOptions(allocator); + const rcl_init_options_t * rcl_options = options.get_rcl_init_options(); + ASSERT_TRUE(rcl_options != nullptr); + ASSERT_TRUE(rcl_options->impl != nullptr); + + { + auto options_copy = rclcpp::InitOptions(options); + const rcl_init_options_t * rcl_options_copy = options_copy.get_rcl_init_options(); + ASSERT_TRUE(rcl_options_copy != nullptr); + ASSERT_TRUE(rcl_options_copy->impl != nullptr); + } + + { + auto options_copy = options; + const rcl_init_options_t * rcl_options_copy = options_copy.get_rcl_init_options(); + ASSERT_TRUE(rcl_options_copy != nullptr); + ASSERT_TRUE(rcl_options_copy->impl != nullptr); + } +} + +TEST(TestInitOptions, test_initialize_logging) { + { + auto options = rclcpp::InitOptions(); + EXPECT_TRUE(options.auto_initialize_logging()); + } + + { + auto options = rclcpp::InitOptions().auto_initialize_logging(true); + EXPECT_TRUE(options.auto_initialize_logging()); + } + + { + auto options = rclcpp::InitOptions().auto_initialize_logging(false); + EXPECT_FALSE(options.auto_initialize_logging()); + } +} + +TEST(TestInitOptions, test_domain_id) { + rcl_allocator_t allocator = rcl_get_default_allocator(); + auto options = rclcpp::InitOptions(allocator); + size_t domain_id = RCL_DEFAULT_DOMAIN_ID; + EXPECT_EQ(RCL_RET_OK, rcl_get_default_domain_id(&domain_id)); + + options.use_default_domain_id(); + EXPECT_EQ(domain_id, options.get_domain_id()); + options.set_domain_id(42); + EXPECT_EQ((size_t)42, options.get_domain_id()); + options.use_default_domain_id(); + EXPECT_EQ(domain_id, options.get_domain_id()); +} + +// Required for mocking_utils below +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(TestInitOptions, constructor_rcl_init_options_init_failed) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_init_options_init, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + rclcpp::InitOptions(), + std::runtime_error("failed to initialize rcl init options: error not set")); +} + +TEST(TestInitOptions, constructor_rcl_init_options_copy_failed) { + rcl_init_options_t rcl_opts; + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_init_options_copy, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + new rclcpp::InitOptions(rcl_opts), + std::runtime_error("failed to copy rcl init options: error not set")); +} + +TEST(TestInitOptions, copy_constructor_rcl_init_options_copy_failed) { + rclcpp::InitOptions options; + rclcpp::InitOptions options2; + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_init_options_copy, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + options2.operator=(options), + std::runtime_error("failed to copy rcl init options: error not set")); +} + +TEST(TestInitOptions, use_default_domain_id_rcl_get_default_domain_id_failed) { + rclcpp::InitOptions options; + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_get_default_domain_id, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + options.use_default_domain_id(), + std::runtime_error("failed to get default domain id: error not set")); +} + +TEST(TestInitOptions, set_domain_id_rcl_init_options_set_domain_id_failed) { + rclcpp::InitOptions options; + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_init_options_set_domain_id, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + options.set_domain_id(0), + std::runtime_error("failed to set domain id to rcl init options: error not set")); +} + +TEST(TestInitOptions, get_domain_id_rcl_init_options_get_domain_id_failed) { + rclcpp::InitOptions options; + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_init_options_get_domain_id, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + options.get_domain_id(), + std::runtime_error("failed to get domain id from rcl init options: error not set")); +} diff --git a/rclcpp/test/rclcpp/test_interface_traits.cpp b/rclcpp/test/rclcpp/test_interface_traits.cpp new file mode 100644 index 0000000000..071ac22ce7 --- /dev/null +++ b/rclcpp/test/rclcpp/test_interface_traits.cpp @@ -0,0 +1,78 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +#include "rclcpp/contexts/default_context.hpp" +#include "rclcpp/node_interfaces/node_base_interface_traits.hpp" +#include "rclcpp/node_interfaces/node_base.hpp" +#include "rclcpp/node.hpp" + +class MyNode +{ +public: + std::shared_ptr get_node_base_interface() const + { + rclcpp::NodeOptions options; + return std::make_shared( + "my_node_name", + "my_node_namespace", + rclcpp::contexts::get_global_default_context(), + *options.get_rcl_node_options(), + options.use_intra_process_comms(), + options.enable_topic_statistics()); + } +}; + +class WrongNode +{ +public: + std::shared_ptr not_get_node_base_interface() + { + return nullptr; + } +}; + +template::value + >::type * = nullptr> +void get_node_name(const T & nodelike) +{ + ASSERT_STREQ("my_node_name", nodelike.get_node_base_interface()->get_name()); +} + +class TestInterfaceTraits : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestInterfaceTraits, has_node_base_interface) { + ASSERT_TRUE(rclcpp::node_interfaces::has_node_base_interface::value); + ASSERT_FALSE(rclcpp::node_interfaces::has_node_base_interface::value); + ASSERT_TRUE(rclcpp::node_interfaces::has_node_base_interface::value); + + get_node_name(MyNode()); +} diff --git a/rclcpp/test/rclcpp/test_intra_process_buffer.cpp b/rclcpp/test/rclcpp/test_intra_process_buffer.cpp new file mode 100644 index 0000000000..fa427de74b --- /dev/null +++ b/rclcpp/test/rclcpp/test_intra_process_buffer.cpp @@ -0,0 +1,366 @@ +// 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 "gtest/gtest.h" + +#include "rclcpp/rclcpp.hpp" + +/* + Construtctor + */ +TEST(TestIntraProcessBuffer, constructor) { + using MessageT = char; + using Alloc = std::allocator; + using Deleter = std::default_delete; + using SharedMessageT = std::shared_ptr; + using UniqueMessageT = std::unique_ptr; + using SharedIntraProcessBufferT = rclcpp::experimental::buffers::TypedIntraProcessBuffer< + MessageT, Alloc, Deleter, SharedMessageT>; + using UniqueIntraProcessBufferT = rclcpp::experimental::buffers::TypedIntraProcessBuffer< + MessageT, Alloc, Deleter, UniqueMessageT>; + + auto shared_buffer_impl = + std::make_unique>(2); + + SharedIntraProcessBufferT shared_intra_process_buffer(std::move(shared_buffer_impl)); + + EXPECT_EQ(true, shared_intra_process_buffer.use_take_shared_method()); + + auto unique_buffer_impl = + std::make_unique>(2); + + UniqueIntraProcessBufferT unique_intra_process_buffer(std::move(unique_buffer_impl)); + + EXPECT_EQ(false, unique_intra_process_buffer.use_take_shared_method()); +} + +/* + Add data to an intra-process buffer with an implementations that stores shared_ptr + Messages are extracted using the same data as the implementation, i.e. shared_ptr + - Add shared_ptr no copies are expected + - Add unique_ptr no copies are expected + */ +TEST(TestIntraProcessBuffer, shared_buffer_add) { + using MessageT = char; + using Alloc = std::allocator; + using Deleter = std::default_delete; + using SharedMessageT = std::shared_ptr; + using SharedIntraProcessBufferT = rclcpp::experimental::buffers::TypedIntraProcessBuffer< + MessageT, Alloc, Deleter, SharedMessageT>; + + auto buffer_impl = + std::make_unique>(2); + + SharedIntraProcessBufferT intra_process_buffer(std::move(buffer_impl)); + + auto original_shared_msg = std::make_shared('a'); + auto original_message_pointer = reinterpret_cast(original_shared_msg.get()); + + intra_process_buffer.add_shared(original_shared_msg); + + EXPECT_EQ(2L, original_shared_msg.use_count()); + + SharedMessageT popped_shared_msg; + popped_shared_msg = intra_process_buffer.consume_shared(); + auto popped_message_pointer = reinterpret_cast(popped_shared_msg.get()); + + EXPECT_EQ(original_shared_msg.use_count(), popped_shared_msg.use_count()); + EXPECT_EQ(*original_shared_msg, *popped_shared_msg); + EXPECT_EQ(original_message_pointer, popped_message_pointer); + + auto original_unique_msg = std::make_unique('b'); + original_message_pointer = reinterpret_cast(original_unique_msg.get()); + auto original_value = *original_unique_msg; + + intra_process_buffer.add_unique(std::move(original_unique_msg)); + + popped_shared_msg = intra_process_buffer.consume_shared(); + popped_message_pointer = reinterpret_cast(popped_shared_msg.get()); + + EXPECT_EQ(1L, popped_shared_msg.use_count()); + EXPECT_EQ(original_value, *popped_shared_msg); + EXPECT_EQ(original_message_pointer, popped_message_pointer); +} + +/* + Add data to an intra-process buffer with an implementations that stores unique_ptr + Messages are extracted using the same data as the implementation, i.e. unique_ptr + - Add shared_ptr a copy is expected + - Add unique_ptr no copies are expected + */ +TEST(TestIntraProcessBuffer, unique_buffer_add) { + using MessageT = char; + using Alloc = std::allocator; + using Deleter = std::default_delete; + using UniqueMessageT = std::unique_ptr; + using UniqueIntraProcessBufferT = rclcpp::experimental::buffers::TypedIntraProcessBuffer< + MessageT, Alloc, Deleter, UniqueMessageT>; + + auto buffer_impl = + std::make_unique>(2); + + UniqueIntraProcessBufferT intra_process_buffer(std::move(buffer_impl)); + + auto original_shared_msg = std::make_shared('a'); + auto original_message_pointer = reinterpret_cast(original_shared_msg.get()); + + intra_process_buffer.add_shared(original_shared_msg); + + EXPECT_EQ(1L, original_shared_msg.use_count()); + + UniqueMessageT popped_unique_msg; + popped_unique_msg = intra_process_buffer.consume_unique(); + auto popped_message_pointer = reinterpret_cast(popped_unique_msg.get()); + + EXPECT_EQ(*original_shared_msg, *popped_unique_msg); + EXPECT_NE(original_message_pointer, popped_message_pointer); + + auto original_unique_msg = std::make_unique('b'); + original_message_pointer = reinterpret_cast(original_unique_msg.get()); + auto original_value = *original_unique_msg; + + intra_process_buffer.add_unique(std::move(original_unique_msg)); + + popped_unique_msg = intra_process_buffer.consume_unique(); + popped_message_pointer = reinterpret_cast(popped_unique_msg.get()); + + EXPECT_EQ(original_value, *popped_unique_msg); + EXPECT_EQ(original_message_pointer, popped_message_pointer); +} + +/* + Consume data from an intra-process buffer with an implementations that stores shared_ptr + Messages are inserted using the same data as the implementation, i.e. shared_ptr + - Request shared_ptr no copies are expected + - Request unique_ptr a copy is expected + */ +TEST(TestIntraProcessBuffer, shared_buffer_consume) { + using MessageT = char; + using Alloc = std::allocator; + using Deleter = std::default_delete; + using SharedMessageT = std::shared_ptr; + using UniqueMessageT = std::unique_ptr; + using SharedIntraProcessBufferT = rclcpp::experimental::buffers::TypedIntraProcessBuffer< + MessageT, Alloc, Deleter, SharedMessageT>; + + auto buffer_impl = + std::make_unique>(2); + + SharedIntraProcessBufferT intra_process_buffer(std::move(buffer_impl)); + + auto original_shared_msg = std::make_shared('a'); + auto original_message_pointer = reinterpret_cast(original_shared_msg.get()); + + intra_process_buffer.add_shared(original_shared_msg); + + EXPECT_EQ(2L, original_shared_msg.use_count()); + + SharedMessageT popped_shared_msg; + popped_shared_msg = intra_process_buffer.consume_shared(); + auto popped_message_pointer = reinterpret_cast(popped_shared_msg.get()); + + EXPECT_EQ(original_shared_msg.use_count(), popped_shared_msg.use_count()); + EXPECT_EQ(*original_shared_msg, *popped_shared_msg); + EXPECT_EQ(original_message_pointer, popped_message_pointer); + + original_shared_msg = std::make_shared('b'); + original_message_pointer = reinterpret_cast(original_shared_msg.get()); + + intra_process_buffer.add_shared(original_shared_msg); + + UniqueMessageT popped_unique_msg; + popped_unique_msg = intra_process_buffer.consume_unique(); + popped_message_pointer = reinterpret_cast(popped_unique_msg.get()); + + EXPECT_EQ(1L, original_shared_msg.use_count()); + EXPECT_EQ(*original_shared_msg, *popped_unique_msg); + EXPECT_NE(original_message_pointer, popped_message_pointer); + + original_shared_msg = std::make_shared('c'); + original_message_pointer = reinterpret_cast(original_shared_msg.get()); + auto original_shared_msg_2 = std::make_shared('d'); + auto original_message_pointer_2 = reinterpret_cast(original_shared_msg_2.get()); + intra_process_buffer.add_shared(original_shared_msg); + intra_process_buffer.add_shared(original_shared_msg_2); + + auto shared_data_vec = intra_process_buffer.get_all_data_shared(); + EXPECT_EQ(2L, shared_data_vec.size()); + EXPECT_EQ(3L, original_shared_msg.use_count()); + EXPECT_EQ(original_shared_msg.use_count(), shared_data_vec[0].use_count()); + EXPECT_EQ(*original_shared_msg, *shared_data_vec[0]); + EXPECT_EQ(original_message_pointer, reinterpret_cast(shared_data_vec[0].get())); + EXPECT_EQ(3L, original_shared_msg_2.use_count()); + EXPECT_EQ(original_shared_msg_2.use_count(), shared_data_vec[1].use_count()); + EXPECT_EQ(*original_shared_msg_2, *shared_data_vec[1]); + EXPECT_EQ(original_message_pointer_2, reinterpret_cast(shared_data_vec[1].get())); + + auto unique_data_vec = intra_process_buffer.get_all_data_unique(); + EXPECT_EQ(2L, unique_data_vec.size()); + EXPECT_EQ(3L, original_shared_msg.use_count()); + EXPECT_EQ(*original_shared_msg, *unique_data_vec[0]); + EXPECT_NE(original_message_pointer, reinterpret_cast(unique_data_vec[0].get())); + EXPECT_EQ(3L, original_shared_msg_2.use_count()); + EXPECT_EQ(*original_shared_msg_2, *unique_data_vec[1]); + EXPECT_NE(original_message_pointer_2, reinterpret_cast(unique_data_vec[1].get())); +} + +/* + Consume data from an intra-process buffer with an implementations that stores unique_ptr + Messages are inserted using the same data as the implementation, i.e. unique_ptr + - Request shared_ptr no copies are expected + - Request unique_ptr no copies are expected + */ +TEST(TestIntraProcessBuffer, unique_buffer_consume) { + using MessageT = char; + using Alloc = std::allocator; + using Deleter = std::default_delete; + using SharedMessageT = std::shared_ptr; + using UniqueMessageT = std::unique_ptr; + using UniqueIntraProcessBufferT = rclcpp::experimental::buffers::TypedIntraProcessBuffer< + MessageT, Alloc, Deleter, UniqueMessageT>; + + auto buffer_impl = + std::make_unique>(2); + + UniqueIntraProcessBufferT intra_process_buffer(std::move(buffer_impl)); + + auto original_unique_msg = std::make_unique('a'); + auto original_message_pointer = reinterpret_cast(original_unique_msg.get()); + auto original_value = *original_unique_msg; + + intra_process_buffer.add_unique(std::move(original_unique_msg)); + + SharedMessageT popped_shared_msg; + popped_shared_msg = intra_process_buffer.consume_shared(); + auto popped_message_pointer = reinterpret_cast(popped_shared_msg.get()); + + EXPECT_EQ(original_value, *popped_shared_msg); + EXPECT_EQ(original_message_pointer, popped_message_pointer); + + original_unique_msg = std::make_unique('b'); + original_message_pointer = reinterpret_cast(original_unique_msg.get()); + original_value = *original_unique_msg; + + intra_process_buffer.add_unique(std::move(original_unique_msg)); + + UniqueMessageT popped_unique_msg; + popped_unique_msg = intra_process_buffer.consume_unique(); + popped_message_pointer = reinterpret_cast(popped_unique_msg.get()); + + EXPECT_EQ(original_value, *popped_unique_msg); + EXPECT_EQ(original_message_pointer, popped_message_pointer); + + original_unique_msg = std::make_unique('c'); + original_message_pointer = reinterpret_cast(original_unique_msg.get()); + original_value = *original_unique_msg; + auto original_unique_msg_2 = std::make_unique('d'); + auto original_message_pointer_2 = reinterpret_cast(original_unique_msg.get()); + auto original_value_2 = *original_unique_msg_2; + intra_process_buffer.add_unique(std::move(original_unique_msg)); + intra_process_buffer.add_unique(std::move(original_unique_msg_2)); + + auto shared_data_vec = intra_process_buffer.get_all_data_shared(); + EXPECT_EQ(2L, shared_data_vec.size()); + EXPECT_EQ(1L, shared_data_vec[0].use_count()); + EXPECT_EQ(original_value, *shared_data_vec[0]); + EXPECT_NE(original_message_pointer, reinterpret_cast(shared_data_vec[0].get())); + EXPECT_EQ(1L, shared_data_vec[1].use_count()); + EXPECT_EQ(original_value_2, *shared_data_vec[1]); + EXPECT_NE(original_message_pointer_2, reinterpret_cast(shared_data_vec[1].get())); + + auto unique_data_vec = intra_process_buffer.get_all_data_unique(); + EXPECT_EQ(2L, unique_data_vec.size()); + EXPECT_EQ(1L, shared_data_vec[0].use_count()); + EXPECT_EQ(original_value, *unique_data_vec[0]); + EXPECT_NE(original_message_pointer, reinterpret_cast(unique_data_vec[0].get())); + EXPECT_EQ(1L, shared_data_vec[1].use_count()); + EXPECT_EQ(original_value_2, *unique_data_vec[1]); + EXPECT_NE(original_message_pointer_2, reinterpret_cast(unique_data_vec[1].get())); +} + +/* + Check the available buffer capacity while storing and consuming data from an intra-process + buffer. + The initial available buffer capacity should equal the buffer size. + Inserting a message should decrease the available buffer capacity by 1. + Consuming a message should increase the available buffer capacity by 1. + */ +TEST(TestIntraProcessBuffer, available_capacity) { + using MessageT = char; + using Alloc = std::allocator; + using Deleter = std::default_delete; + using SharedMessageT = std::shared_ptr; + using UniqueMessageT = std::unique_ptr; + using UniqueIntraProcessBufferT = rclcpp::experimental::buffers::TypedIntraProcessBuffer< + MessageT, Alloc, Deleter, UniqueMessageT>; + + constexpr auto history_depth = 5u; + + auto buffer_impl = + std::make_unique>( + history_depth); + + UniqueIntraProcessBufferT intra_process_buffer(std::move(buffer_impl)); + + EXPECT_EQ(history_depth, intra_process_buffer.available_capacity()); + + auto original_unique_msg = std::make_unique('a'); + auto original_message_pointer = reinterpret_cast(original_unique_msg.get()); + auto original_value = *original_unique_msg; + + intra_process_buffer.add_unique(std::move(original_unique_msg)); + + EXPECT_EQ(history_depth - 1u, intra_process_buffer.available_capacity()); + + SharedMessageT popped_shared_msg; + popped_shared_msg = intra_process_buffer.consume_shared(); + auto popped_message_pointer = reinterpret_cast(popped_shared_msg.get()); + + EXPECT_EQ(history_depth, intra_process_buffer.available_capacity()); + EXPECT_EQ(original_value, *popped_shared_msg); + EXPECT_EQ(original_message_pointer, popped_message_pointer); + + original_unique_msg = std::make_unique('b'); + original_message_pointer = reinterpret_cast(original_unique_msg.get()); + original_value = *original_unique_msg; + + intra_process_buffer.add_unique(std::move(original_unique_msg)); + + auto second_unique_msg = std::make_unique('c'); + auto second_message_pointer = reinterpret_cast(second_unique_msg.get()); + auto second_value = *second_unique_msg; + + intra_process_buffer.add_unique(std::move(second_unique_msg)); + + EXPECT_EQ(history_depth - 2u, intra_process_buffer.available_capacity()); + + UniqueMessageT popped_unique_msg; + popped_unique_msg = intra_process_buffer.consume_unique(); + popped_message_pointer = reinterpret_cast(popped_unique_msg.get()); + + EXPECT_EQ(history_depth - 1u, intra_process_buffer.available_capacity()); + EXPECT_EQ(original_value, *popped_unique_msg); + EXPECT_EQ(original_message_pointer, popped_message_pointer); + + popped_unique_msg = intra_process_buffer.consume_unique(); + popped_message_pointer = reinterpret_cast(popped_unique_msg.get()); + + EXPECT_EQ(history_depth, intra_process_buffer.available_capacity()); + EXPECT_EQ(second_value, *popped_unique_msg); + EXPECT_EQ(second_message_pointer, popped_message_pointer); +} diff --git a/rclcpp/test/rclcpp/test_intra_process_manager.cpp b/rclcpp/test/rclcpp/test_intra_process_manager.cpp new file mode 100644 index 0000000000..6a4bfe56d8 --- /dev/null +++ b/rclcpp/test/rclcpp/test_intra_process_manager.cpp @@ -0,0 +1,990 @@ +// 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 +#include + +#define RCLCPP_BUILDING_LIBRARY 1 +#include "rclcpp/allocator/allocator_common.hpp" +#include "rclcpp/context.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/qos.hpp" +#include "rmw/types.h" +#include "rmw/qos_profiles.h" + +// NOLINTNEXTLINE(build/include_order) +#include + +namespace rclcpp +{ +namespace experimental +{ +namespace buffers +{ +namespace mock +{ + +class IntraProcessBufferBase +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(IntraProcessBufferBase) + + virtual ~IntraProcessBufferBase() {} +}; + +} // namespace mock +} // namespace buffers +} // namespace experimental +} // namespace rclcpp + +namespace rclcpp +{ +namespace experimental +{ +namespace buffers +{ +namespace mock +{ +template< + typename MessageT, + typename Alloc = std::allocator, + typename MessageDeleter = std::default_delete> +class IntraProcessBuffer : public IntraProcessBufferBase +{ +public: + using ConstMessageSharedPtr = std::shared_ptr; + using MessageUniquePtr = std::unique_ptr; + + RCLCPP_SMART_PTR_DEFINITIONS(IntraProcessBuffer) + + IntraProcessBuffer() + {} + + void add(ConstMessageSharedPtr msg) + { + message_ptr = reinterpret_cast(msg.get()); + shared_msg = msg; + ++num_msgs; + } + + void add(MessageUniquePtr msg) + { + message_ptr = reinterpret_cast(msg.get()); + unique_msg = std::move(msg); + ++num_msgs; + } + + void pop(std::uintptr_t & msg_ptr) + { + msg_ptr = message_ptr; + message_ptr = 0; + --num_msgs; + } + + size_t size() const + { + return num_msgs; + } + + std::vector get_all_data_shared() + { + if (shared_msg) { + return {shared_msg}; + } else if (unique_msg) { + return {std::make_shared(*unique_msg)}; + } + return {}; + } + + std::vector get_all_data_unique() + { + std::vector result; + if (shared_msg) { + result.push_back(std::make_unique(*shared_msg)); + } else if (unique_msg) { + result.push_back(std::make_unique(*unique_msg)); + } + return result; + } + +private: + // need to store the messages somewhere otherwise the memory address will be reused + ConstMessageSharedPtr shared_msg; + MessageUniquePtr unique_msg; + + std::uintptr_t message_ptr; + // count add and pop + size_t num_msgs = 0u; +}; + +} // namespace mock +} // namespace buffers +} // namespace experimental +} // namespace rclcpp +namespace rclcpp +{ +// forward declaration +namespace experimental +{ +class IntraProcessManager; +} // namespace experimental + +namespace mock +{ + +using IntraProcessManagerSharedPtr = + std::shared_ptr; + +using IntraProcessManagerWeakPtr = + std::weak_ptr; + +class PublisherBase +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(PublisherBase) + + explicit PublisherBase(const std::string & topic, const rclcpp::QoS & qos) + : topic_name(topic), + qos_profile(qos) + {} + + virtual ~PublisherBase() + {} + + const char * get_topic_name() const + { + return topic_name.c_str(); + } + + void set_intra_process_manager( + uint64_t intra_process_publisher_id, + IntraProcessManagerSharedPtr ipm) + { + intra_process_publisher_id_ = intra_process_publisher_id; + weak_ipm_ = ipm; + } + + rclcpp::QoS + get_actual_qos() const + { + return qos_profile; + } + + bool + is_durability_transient_local() const + { + return qos_profile.durability() == rclcpp::DurabilityPolicy::TransientLocal; + } + + bool + operator==(const rmw_gid_t & gid) const + { + (void)gid; + return false; + } + + bool + operator==(const rmw_gid_t * gid) const + { + (void)gid; + return false; + } + + uint64_t intra_process_publisher_id_; + IntraProcessManagerWeakPtr weak_ipm_; + +private: + std::string topic_name; + rclcpp::QoS qos_profile; +}; + +template> +class Publisher : public PublisherBase +{ +public: + using MessageAllocTraits = allocator::AllocRebind; + using MessageAlloc = typename MessageAllocTraits::allocator_type; + using MessageDeleter = allocator::Deleter; + using MessageUniquePtr = std::unique_ptr; + using MessageSharedPtr = std::shared_ptr; + + RCLCPP_SMART_PTR_DEFINITIONS(Publisher) + + explicit Publisher(const std::string & topic, const rclcpp::QoS & qos) + : PublisherBase(topic, qos) + { + auto allocator = std::make_shared(); + message_allocator_ = std::make_shared(*allocator.get()); + if (qos.durability() == rclcpp::DurabilityPolicy::TransientLocal) { + buffer = std::make_shared>(); + } + } + + // The following functions use the IntraProcessManager + // so they are declared after including it to avoid "invalid use of incomplete type" + void publish(MessageUniquePtr msg); + + std::shared_ptr message_allocator_; + typename rclcpp::experimental::buffers::mock::IntraProcessBuffer::SharedPtr buffer{nullptr}; +}; + +} // namespace mock +} // namespace rclcpp + +namespace rclcpp +{ +namespace experimental +{ +namespace mock +{ + +class SubscriptionIntraProcessBase +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(SubscriptionIntraProcessBase) + + explicit SubscriptionIntraProcessBase( + rclcpp::Context::SharedPtr context, + const std::string & topic, + const rclcpp::QoS & qos) + : topic_name(topic), qos_profile(qos) + { + (void)context; + } + + virtual ~SubscriptionIntraProcessBase() {} + + virtual bool + use_take_shared_method() const = 0; + + QoS + get_actual_qos() + { + return qos_profile; + } + + const char * + get_topic_name() + { + return topic_name.c_str(); + } + + bool + is_durability_transient_local() const + { + return qos_profile.durability() == rclcpp::DurabilityPolicy::TransientLocal; + } + + virtual + size_t + available_capacity() const = 0; + + std::string topic_name; + rclcpp::QoS qos_profile; +}; + +template< + typename MessageT, + typename Alloc = std::allocator, + typename Deleter = std::default_delete, + typename ROSMessageType = MessageT +> +class SubscriptionIntraProcessBuffer : public SubscriptionIntraProcessBase +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(SubscriptionIntraProcessBuffer) + + explicit SubscriptionIntraProcessBuffer(const std::string & topic, const rclcpp::QoS & qos) + : SubscriptionIntraProcessBase(nullptr, topic, qos), take_shared_method(false) + { + buffer = std::make_unique>(); + } + + void + provide_intra_process_message(std::shared_ptr msg) + { + buffer->add(msg); + } + + void + provide_intra_process_message(std::unique_ptr msg) + { + buffer->add(std::move(msg)); + } + + void + provide_intra_process_data(std::shared_ptr msg) + { + buffer->add(msg); + } + + void + provide_intra_process_data(std::unique_ptr msg) + { + buffer->add(std::move(msg)); + } + + std::uintptr_t + pop() + { + std::uintptr_t ptr; + buffer->pop(ptr); + return ptr; + } + + bool + use_take_shared_method() const override + { + return take_shared_method; + } + + size_t + available_capacity() const override + { + return qos_profile.depth() - buffer->size(); + } + + bool take_shared_method; + + typename rclcpp::experimental::buffers::mock::IntraProcessBuffer::UniquePtr buffer; +}; + +template< + typename MessageT, + typename Alloc = std::allocator, + typename Deleter = std::default_delete> +class SubscriptionIntraProcess : public SubscriptionIntraProcessBuffer< + MessageT, + Alloc, + Deleter +> +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(SubscriptionIntraProcess) + + explicit SubscriptionIntraProcess(const std::string & topic, const rclcpp::QoS & qos) + : SubscriptionIntraProcessBuffer(topic, qos) + { + } +}; + +} // namespace mock +} // namespace experimental +} // namespace rclcpp + +// Prevent the header files of the mocked classes to be included +#define RCLCPP__PUBLISHER_HPP_ +#define RCLCPP__PUBLISHER_BASE_HPP_ +#define RCLCPP__EXPERIMENTAL__BUFFERS__INTRA_PROCESS_BUFFER_HPP_ +#define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_HPP_ +#define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BUFFER_HPP_ +#define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_ +// Force ipm to use our mock publisher class. +#define Publisher mock::Publisher +#define PublisherBase mock::PublisherBase +#define IntraProcessBufferBase mock::IntraProcessBufferBase +#define IntraProcessBuffer mock::IntraProcessBuffer +#define SubscriptionIntraProcessBase mock::SubscriptionIntraProcessBase +#define SubscriptionIntraProcessBuffer mock::SubscriptionIntraProcessBuffer +#define SubscriptionIntraProcess mock::SubscriptionIntraProcess +#include "../src/rclcpp/intra_process_manager.cpp" // NOLINT +#undef Publisher +#undef PublisherBase +#undef IntraProcessBuffer +#undef SubscriptionIntraProcessBase +#undef SubscriptionIntraProcess + +using ::testing::_; +using ::testing::UnorderedElementsAre; + +namespace rclcpp +{ +namespace mock +{ + +template +void Publisher::publish(MessageUniquePtr msg) +{ + auto ipm = weak_ipm_.lock(); + if (!ipm) { + throw std::runtime_error( + "intra process publish called after destruction of intra process manager"); + } + if (!msg) { + throw std::runtime_error("cannot publish msg which is a null pointer"); + } + + if (buffer) { + auto shared_msg = ipm->template do_intra_process_publish_and_return_shared( + intra_process_publisher_id_, + std::move(msg), + *message_allocator_); + buffer->add(shared_msg); + } else { + ipm->template do_intra_process_publish( + intra_process_publisher_id_, + std::move(msg), + *message_allocator_); + } +} + +} // namespace mock +} // namespace rclcpp + +/* + * This tests how the class connects and disconnects publishers and subscriptions: + * - Creates 2 publishers on different topics and a subscription to one of them. + * Add everything to the intra-process manager. + * - All the entities are expected to have different ids. + * - Check the subscriptions count for each publisher. + * - One of the publishers is expected to have 1 subscription, while the other 0. + * - Check the subscription count for a non existing publisher id, should return 0. + * - Add a new publisher and a new subscription both with reliable QoS. + * - The subscriptions count of the previous publisher is expected to remain unchanged, + * while the new publisher is expected to have 2 subscriptions (it's compatible with both QoS). + * - Remove the just added subscriptions. + * - The count for the last publisher is expected to decrease to 1. + */ +TEST(TestIntraProcessManager, add_pub_sub) { + using IntraProcessManagerT = rclcpp::experimental::IntraProcessManager; + using MessageT = rcl_interfaces::msg::Log; + using PublisherT = rclcpp::mock::Publisher; + using SubscriptionIntraProcessT = rclcpp::experimental::mock::SubscriptionIntraProcess; + + auto ipm = std::make_shared(); + + auto p1 = std::make_shared("topic", rclcpp::QoS(10).best_effort()); + + auto p2 = std::make_shared("different_topic_name", rclcpp::QoS(10).best_effort()); + + auto s1 = std::make_shared("topic", rclcpp::QoS(10).best_effort()); + + auto p1_id = ipm->add_publisher(p1); + auto p2_id = ipm->add_publisher(p2); + auto s1_id = ipm->template add_subscription(s1); + + bool unique_ids = p1_id != p2_id && p2_id != s1_id; + ASSERT_TRUE(unique_ids); + + // p1 has 1 subcription, s1 + size_t p1_subs = ipm->get_subscription_count(p1_id); + // p2 has 0 subscriptions + size_t p2_subs = ipm->get_subscription_count(p2_id); + // Non-existent publisher_id has 0 subscriptions + size_t non_existing_pub_subs = ipm->get_subscription_count(42); + ASSERT_EQ(1u, p1_subs); + ASSERT_EQ(0u, p2_subs); + ASSERT_EQ(0u, non_existing_pub_subs); + + auto p3 = std::make_shared("topic", rclcpp::QoS(10).reliable()); + + auto s2 = std::make_shared("topic", rclcpp::QoS(10).reliable()); + + // s2 may be able to communicate with p1 depending on the RMW + auto s2_id = ipm->template add_subscription(s2); + // p3 can definitely communicate with s2, may be able to communicate with s1 depending on the RMW + auto p3_id = ipm->add_publisher(p3); + + // p1 definitely matches subscription s1, since the topic name and QoS match exactly. + // If the RMW can match best-effort publishers to reliable subscriptions (like Zenoh can), + // then p1 will also match s2. + p1_subs = ipm->get_subscription_count(p1_id); + // No subscriptions with a topic name of "different_topic_name" were added. + p2_subs = ipm->get_subscription_count(p2_id); + // On all current RMWs (DDS and Zenoh), a reliable publisher like p3 can communicate with both + // reliable and best-effort subscriptions (s1 and s2). + size_t p3_subs = ipm->get_subscription_count(p3_id); + + rclcpp::QoSCheckCompatibleResult qos_compatible = + rclcpp::qos_check_compatible(p1->get_actual_qos(), s2->get_actual_qos()); + if (qos_compatible.compatibility == rclcpp::QoSCompatibility::Error) { + ASSERT_EQ(1u, p1_subs); + } else { + ASSERT_EQ(2u, p1_subs); + } + ASSERT_EQ(0u, p2_subs); + ASSERT_EQ(2u, p3_subs); + + ipm->remove_subscription(s2_id); + p1_subs = ipm->get_subscription_count(p1_id); + p2_subs = ipm->get_subscription_count(p2_id); + p3_subs = ipm->get_subscription_count(p3_id); + ASSERT_EQ(1u, p1_subs); + ASSERT_EQ(0u, p2_subs); + ASSERT_EQ(1u, p3_subs); +} + +/* + * This tests the minimal usage of the class where there is a single subscription per publisher: + * - Publishes a unique_ptr message with a subscription requesting ownership. + * - The received message is expected to be the same. + * - Remove the first subscription from ipm and add a new one. + * - Publishes a unique_ptr message with a subscription not requesting ownership. + * - The received message is expected to be the same, the first subscription do not receive it. + * - Publishes a shared_ptr message with a subscription not requesting ownership. + * - The received message is expected to be the same. + */ +TEST(TestIntraProcessManager, single_subscription) { + using IntraProcessManagerT = rclcpp::experimental::IntraProcessManager; + using MessageT = rcl_interfaces::msg::Log; + using PublisherT = rclcpp::mock::Publisher; + using SubscriptionIntraProcessT = rclcpp::experimental::mock::SubscriptionIntraProcess; + + auto ipm = std::make_shared(); + + auto p1 = std::make_shared("topic", rclcpp::QoS(10)); + auto p1_id = ipm->add_publisher(p1); + p1->set_intra_process_manager(p1_id, ipm); + + auto s1 = std::make_shared("topic", rclcpp::QoS(10)); + s1->take_shared_method = false; + auto s1_id = ipm->template add_subscription(s1); + + auto unique_msg = std::make_unique(); + auto original_message_pointer = reinterpret_cast(unique_msg.get()); + p1->publish(std::move(unique_msg)); + auto received_message_pointer_1 = s1->pop(); + ASSERT_EQ(original_message_pointer, received_message_pointer_1); + + ipm->remove_subscription(s1_id); + auto s2 = std::make_shared("topic", rclcpp::QoS(10)); + s2->take_shared_method = true; + auto s2_id = ipm->template add_subscription(s2); + (void)s2_id; + + unique_msg = std::make_unique(); + original_message_pointer = reinterpret_cast(unique_msg.get()); + p1->publish(std::move(unique_msg)); + received_message_pointer_1 = s1->pop(); + auto received_message_pointer_2 = s2->pop(); + ASSERT_EQ(original_message_pointer, received_message_pointer_2); + ASSERT_EQ(0u, received_message_pointer_1); + + unique_msg = std::make_unique(); + original_message_pointer = reinterpret_cast(unique_msg.get()); + p1->publish(std::move(unique_msg)); + received_message_pointer_2 = s2->pop(); + ASSERT_EQ(original_message_pointer, received_message_pointer_2); +} + +/* + * This tests the usage of the class where there are multiple subscriptions of the same type: + * - Publishes a unique_ptr message with 2 subscriptions requesting ownership. + * - One is expected to receive the published message, while the other will receive a copy. + * - Publishes a unique_ptr message with 2 subscriptions not requesting ownership. + * - Both received messages are expected to be the same as the published one. + * - Publishes a shared_ptr message with 2 subscriptions requesting ownership. + * - Both received messages are expected to be a copy of the published one. + * - Publishes a shared_ptr message with 2 subscriptions not requesting ownership. + * - Both received messages are expected to be the same as the published one. + */ +TEST(TestIntraProcessManager, multiple_subscriptions_same_type) { + using IntraProcessManagerT = rclcpp::experimental::IntraProcessManager; + using MessageT = rcl_interfaces::msg::Log; + using PublisherT = rclcpp::mock::Publisher; + using SubscriptionIntraProcessT = rclcpp::experimental::mock::SubscriptionIntraProcess; + + auto ipm = std::make_shared(); + + auto p1 = std::make_shared("topic", rclcpp::QoS(10)); + auto p1_id = ipm->add_publisher(p1); + p1->set_intra_process_manager(p1_id, ipm); + + auto s1 = std::make_shared("topic", rclcpp::QoS(10)); + s1->take_shared_method = false; + auto s1_id = ipm->template add_subscription(s1); + + auto s2 = std::make_shared("topic", rclcpp::QoS(10)); + s2->take_shared_method = false; + auto s2_id = ipm->template add_subscription(s2); + + auto unique_msg = std::make_unique(); + auto original_message_pointer = reinterpret_cast(unique_msg.get()); + p1->publish(std::move(unique_msg)); + bool received_original_1 = s1->pop() == original_message_pointer; + bool received_original_2 = s2->pop() == original_message_pointer; + std::vector received_original_vec = + {received_original_1, received_original_2}; + ASSERT_THAT(received_original_vec, UnorderedElementsAre(true, false)); + + ipm->remove_subscription(s1_id); + ipm->remove_subscription(s2_id); + + auto s3 = std::make_shared("topic", rclcpp::QoS(10)); + s3->take_shared_method = true; + auto s3_id = ipm->template add_subscription(s3); + + auto s4 = std::make_shared("topic", rclcpp::QoS(10)); + s4->take_shared_method = true; + auto s4_id = ipm->template add_subscription(s4); + + unique_msg = std::make_unique(); + original_message_pointer = reinterpret_cast(unique_msg.get()); + p1->publish(std::move(unique_msg)); + auto received_message_pointer_3 = s3->pop(); + auto received_message_pointer_4 = s4->pop(); + ASSERT_EQ(original_message_pointer, received_message_pointer_3); + ASSERT_EQ(original_message_pointer, received_message_pointer_4); + + ipm->remove_subscription(s3_id); + ipm->remove_subscription(s4_id); + + auto s5 = std::make_shared("topic", rclcpp::QoS(10)); + s5->take_shared_method = false; + auto s5_id = ipm->template add_subscription(s5); + + auto s6 = std::make_shared("topic", rclcpp::QoS(10)); + s6->take_shared_method = false; + auto s6_id = ipm->template add_subscription(s6); + + unique_msg = std::make_unique(); + original_message_pointer = reinterpret_cast(unique_msg.get()); + p1->publish(std::move(unique_msg)); + auto received_message_pointer_5 = s5->pop(); + auto received_message_pointer_6 = s6->pop(); + ASSERT_NE(original_message_pointer, received_message_pointer_5); + // Someone gets the original unique_ptr, the last one to take. + ASSERT_EQ(original_message_pointer, received_message_pointer_6); + + ipm->remove_subscription(s5_id); + ipm->remove_subscription(s6_id); + + auto s7 = std::make_shared("topic", rclcpp::QoS(10)); + s7->take_shared_method = true; + auto s7_id = ipm->template add_subscription(s7); + (void)s7_id; + + auto s8 = std::make_shared("topic", rclcpp::QoS(10)); + s8->take_shared_method = true; + auto s8_id = ipm->template add_subscription(s8); + (void)s8_id; + + unique_msg = std::make_unique(); + original_message_pointer = reinterpret_cast(unique_msg.get()); + p1->publish(std::move(unique_msg)); + auto received_message_pointer_7 = s7->pop(); + auto received_message_pointer_8 = s8->pop(); + ASSERT_EQ(original_message_pointer, received_message_pointer_7); + ASSERT_EQ(original_message_pointer, received_message_pointer_8); +} + +/* + * This tests the usage of the class where there are multiple subscriptions of different types: + * - Publishes a unique_ptr message with 1 subscription requesting ownership and 1 not. + * - The one requesting ownership is expected to receive the published message, + * while the other is expected to receive a copy. + * - Publishes a unique_ptr message with 2 subscriptions requesting ownership and 1 not. + * - One of the subscriptions requesting ownership is expected to receive the published message, + * while both other subscriptions are expected to receive different copies. + * - Publishes a unique_ptr message with 2 subscriptions requesting ownership and 2 not. + * - The 2 subscriptions not requesting ownership are expected to both receive the same copy + * of the message, one of the subscription requesting ownership is expected to receive a + * different copy, while the last is expected to receive the published message. + * - Publishes a shared_ptr message with 1 subscription requesting ownership and 1 not. + * - The subscription requesting ownership is expected to receive a copy of the message, while + * the other is expected to receive the published message + */ +TEST(TestIntraProcessManager, multiple_subscriptions_different_type) { + using IntraProcessManagerT = rclcpp::experimental::IntraProcessManager; + using MessageT = rcl_interfaces::msg::Log; + using PublisherT = rclcpp::mock::Publisher; + using SubscriptionIntraProcessT = rclcpp::experimental::mock::SubscriptionIntraProcess; + + auto ipm = std::make_shared(); + + auto p1 = std::make_shared("topic", rclcpp::QoS(10)); + auto p1_id = ipm->add_publisher(p1); + p1->set_intra_process_manager(p1_id, ipm); + + auto s1 = std::make_shared("topic", rclcpp::QoS(10)); + s1->take_shared_method = true; + auto s1_id = ipm->template add_subscription(s1); + + auto s2 = std::make_shared("topic", rclcpp::QoS(10)); + s2->take_shared_method = false; + auto s2_id = ipm->template add_subscription(s2); + + auto unique_msg = std::make_unique(); + auto original_message_pointer = reinterpret_cast(unique_msg.get()); + p1->publish(std::move(unique_msg)); + auto received_message_pointer_1 = s1->pop(); + auto received_message_pointer_2 = s2->pop(); + ASSERT_NE(original_message_pointer, received_message_pointer_1); + ASSERT_EQ(original_message_pointer, received_message_pointer_2); + + ipm->remove_subscription(s1_id); + ipm->remove_subscription(s2_id); + + auto s3 = std::make_shared("topic", rclcpp::QoS(10)); + s3->take_shared_method = false; + auto s3_id = ipm->template add_subscription(s3); + + auto s4 = std::make_shared("topic", rclcpp::QoS(10)); + s4->take_shared_method = false; + auto s4_id = ipm->template add_subscription(s4); + + auto s5 = std::make_shared("topic", rclcpp::QoS(10)); + s5->take_shared_method = true; + auto s5_id = ipm->template add_subscription(s5); + + unique_msg = std::make_unique(); + original_message_pointer = reinterpret_cast(unique_msg.get()); + p1->publish(std::move(unique_msg)); + auto received_message_pointer_3 = s3->pop(); + auto received_message_pointer_4 = s4->pop(); + auto received_message_pointer_5 = s5->pop(); + bool received_original_3 = received_message_pointer_3 == original_message_pointer; + bool received_original_4 = received_message_pointer_4 == original_message_pointer; + bool received_original_5 = received_message_pointer_5 == original_message_pointer; + std::vector received_original_vec = + {received_original_3, received_original_4, received_original_5}; + ASSERT_THAT(received_original_vec, UnorderedElementsAre(true, false, false)); + ASSERT_NE(received_message_pointer_3, received_message_pointer_4); + ASSERT_NE(received_message_pointer_5, received_message_pointer_3); + ASSERT_NE(received_message_pointer_5, received_message_pointer_4); + + ipm->remove_subscription(s3_id); + ipm->remove_subscription(s4_id); + ipm->remove_subscription(s5_id); + + auto s6 = std::make_shared("topic", rclcpp::QoS(10)); + s6->take_shared_method = true; + auto s6_id = ipm->template add_subscription(s6); + + auto s7 = std::make_shared("topic", rclcpp::QoS(10)); + s7->take_shared_method = true; + auto s7_id = ipm->template add_subscription(s7); + + auto s8 = std::make_shared("topic", rclcpp::QoS(10)); + s8->take_shared_method = false; + auto s8_id = ipm->template add_subscription(s8); + + auto s9 = std::make_shared("topic", rclcpp::QoS(10)); + s9->take_shared_method = false; + auto s9_id = ipm->template add_subscription(s9); + + unique_msg = std::make_unique(); + original_message_pointer = reinterpret_cast(unique_msg.get()); + p1->publish(std::move(unique_msg)); + auto received_message_pointer_6 = s6->pop(); + auto received_message_pointer_7 = s7->pop(); + auto received_message_pointer_8 = s8->pop(); + auto received_message_pointer_9 = s9->pop(); + bool received_original_8 = received_message_pointer_8 == original_message_pointer; + bool received_original_9 = received_message_pointer_9 == original_message_pointer; + received_original_vec = {received_original_8, received_original_9}; + ASSERT_EQ(received_message_pointer_6, received_message_pointer_7); + ASSERT_NE(original_message_pointer, received_message_pointer_6); + ASSERT_NE(original_message_pointer, received_message_pointer_7); + ASSERT_THAT(received_original_vec, UnorderedElementsAre(true, false)); + ASSERT_NE(received_message_pointer_8, received_message_pointer_6); + ASSERT_NE(received_message_pointer_9, received_message_pointer_6); + + ipm->remove_subscription(s6_id); + ipm->remove_subscription(s7_id); + ipm->remove_subscription(s8_id); + ipm->remove_subscription(s9_id); + + auto s10 = std::make_shared("topic", rclcpp::QoS(10)); + s10->take_shared_method = false; + auto s10_id = ipm->template add_subscription(s10); + (void)s10_id; + + auto s11 = std::make_shared("topic", rclcpp::QoS(10)); + s11->take_shared_method = true; + auto s11_id = ipm->template add_subscription(s11); + (void)s11_id; + + unique_msg = std::make_unique(); + original_message_pointer = reinterpret_cast(unique_msg.get()); + p1->publish(std::move(unique_msg)); + auto received_message_pointer_10 = s10->pop(); + auto received_message_pointer_11 = s11->pop(); + EXPECT_EQ(original_message_pointer, received_message_pointer_10); + EXPECT_NE(original_message_pointer, received_message_pointer_11); +} + +/* + * This tests the method "lowest_available_capacity": + * - Creates 1 publisher. + * - The available buffer capacity should be at least history size. + * - Add 2 subscribers. + * - Add everything to the intra-process manager. + * - All the entities are expected to have different ids. + * - Check the subscriptions count for the publisher. + * - The available buffer capacity should be the history size. + * - Publish one message (without receiving it). + * - The available buffer capacity should decrease by 1. + * - Publish another message (without receiving it). + * - The available buffer capacity should decrease by 1. + * - One subscriber receives one message. + * - The available buffer capacity should stay the same, + * as the other subscriber still has not freed its buffer. + * - The other subscriber receives one message. + * - The available buffer capacity should increase by 1. + * - One subscription goes out of scope. + * - The available buffer capacity should not change. + */ +TEST(TestIntraProcessManager, lowest_available_capacity) { + using IntraProcessManagerT = rclcpp::experimental::IntraProcessManager; + using MessageT = rcl_interfaces::msg::Log; + using PublisherT = rclcpp::mock::Publisher; + using SubscriptionIntraProcessT = rclcpp::experimental::mock::SubscriptionIntraProcess; + + constexpr auto history_depth = 10u; + + auto ipm = std::make_shared(); + + auto p1 = std::make_shared("topic", rclcpp::QoS(history_depth).best_effort()); + + auto s1 = + std::make_shared("topic", rclcpp::QoS(history_depth).best_effort()); + auto s2 = + std::make_shared("topic", rclcpp::QoS(history_depth).best_effort()); + + auto p1_id = ipm->add_publisher(p1); + p1->set_intra_process_manager(p1_id, ipm); + + auto c1 = ipm->lowest_available_capacity(p1_id); + + ASSERT_LE(0u, c1); + + auto s1_id = ipm->template add_subscription(s1); + auto s2_id = ipm->template add_subscription(s2); + + bool unique_ids = s1_id != s2_id && p1_id != s1_id; + ASSERT_TRUE(unique_ids); + + size_t p1_subs = ipm->get_subscription_count(p1_id); + size_t non_existing_pub_subs = ipm->get_subscription_count(42); + ASSERT_EQ(2u, p1_subs); + ASSERT_EQ(0u, non_existing_pub_subs); + + c1 = ipm->lowest_available_capacity(p1_id); + auto non_existing_pub_c = ipm->lowest_available_capacity(42); + + ASSERT_EQ(history_depth, c1); + ASSERT_EQ(0u, non_existing_pub_c); + + auto unique_msg = std::make_unique(); + p1->publish(std::move(unique_msg)); + + c1 = ipm->lowest_available_capacity(p1_id); + ASSERT_EQ(history_depth - 1u, c1); + + unique_msg = std::make_unique(); + p1->publish(std::move(unique_msg)); + + c1 = ipm->lowest_available_capacity(p1_id); + ASSERT_EQ(history_depth - 2u, c1); + + s1->pop(); + + c1 = ipm->lowest_available_capacity(p1_id); + ASSERT_EQ(history_depth - 2u, c1); + + s2->pop(); + + c1 = ipm->lowest_available_capacity(p1_id); + ASSERT_EQ(history_depth - 1u, c1); + + ipm->get_subscription_intra_process(s1_id).reset(); + + c1 = ipm->lowest_available_capacity(p1_id); + ASSERT_EQ(history_depth - 1u, c1); +} + +/* + * This tests the check inside add_publisher for transient_local + * durability publishers + * - add_publisher should throw runtime_error when no valid buffer ptr + * is passed with a transient_local publisher + */ +TEST(TestIntraProcessManager, transient_local_invalid_buffer) { + using IntraProcessManagerT = rclcpp::experimental::IntraProcessManager; + using MessageT = rcl_interfaces::msg::Log; + using PublisherT = rclcpp::mock::Publisher; + constexpr auto history_depth = 10u; + + auto ipm = std::make_shared(); + + auto p1 = std::make_shared("topic", rclcpp::QoS(history_depth).transient_local()); + + ASSERT_THROW( + { + ipm->add_publisher(p1, nullptr); + }, std::runtime_error); +} + +/* + * This tests publishing function for transient_local durability publihers + * - A message is published before three transient_local subscriptions are added to + * ipm. Two of the subscriptions use take_shared method. Delivery of the message is verified + * along with the contents and pointer addresses from the subscriptions. + */ +TEST(TestIntraProcessManager, transient_local) { + using IntraProcessManagerT = rclcpp::experimental::IntraProcessManager; + using MessageT = rcl_interfaces::msg::Log; + using PublisherT = rclcpp::mock::Publisher; + using SubscriptionIntraProcessT = rclcpp::experimental::mock::SubscriptionIntraProcess; + + constexpr auto history_depth = 10u; + + auto ipm = std::make_shared(); + + auto p1 = std::make_shared("topic", rclcpp::QoS(history_depth).transient_local()); + + auto s1 = std::make_shared( + "topic", rclcpp::QoS(history_depth).transient_local()); + auto s2 = std::make_shared( + "topic", rclcpp::QoS(history_depth).transient_local()); + auto s3 = std::make_shared( + "topic", rclcpp::QoS(history_depth).transient_local()); + + s1->take_shared_method = false; + s2->take_shared_method = true; + s3->take_shared_method = true; + + auto p1_id = ipm->add_publisher(p1, p1->buffer); + + p1->set_intra_process_manager(p1_id, ipm); + + auto unique_msg = std::make_unique(); + unique_msg->msg = "Test"; + p1->publish(std::move(unique_msg)); + + ipm->template add_subscription(s1); + ipm->template add_subscription(s2); + ipm->template add_subscription(s3); + + auto received_message_pointer_1 = s1->pop(); + auto received_message_pointer_2 = s2->pop(); + auto received_message_pointer_3 = s3->pop(); + ASSERT_NE(0u, received_message_pointer_1); + ASSERT_NE(0u, received_message_pointer_2); + ASSERT_NE(0u, received_message_pointer_3); + ASSERT_EQ(received_message_pointer_3, received_message_pointer_2); + ASSERT_EQ( + reinterpret_cast(received_message_pointer_1)->msg, + reinterpret_cast(received_message_pointer_2)->msg); + ASSERT_EQ( + reinterpret_cast(received_message_pointer_1)->msg, + reinterpret_cast(received_message_pointer_3)->msg); + ASSERT_EQ("Test", reinterpret_cast(received_message_pointer_1)->msg); +} diff --git a/rclcpp/test/rclcpp/test_intra_process_manager_with_allocators.cpp b/rclcpp/test/rclcpp/test_intra_process_manager_with_allocators.cpp new file mode 100644 index 0000000000..6d192ca86b --- /dev/null +++ b/rclcpp/test/rclcpp/test_intra_process_manager_with_allocators.cpp @@ -0,0 +1,303 @@ +// 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 +#include +#include + +#include "test_msgs/msg/empty.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/allocator/allocator_common.hpp" +#include "rclcpp/strategies/allocator_memory_strategy.hpp" + +// For demonstration purposes only, not necessary for allocator_traits +static uint32_t num_allocs = 0; +static uint32_t num_deallocs = 0; +// A very simple custom allocator. Counts calls to allocate and deallocate. +template +struct MyAllocator +{ +public: + using value_type = T; + using size_type = std::size_t; + using pointer = T *; + using const_pointer = const T *; + using difference_type = typename std::pointer_traits::difference_type; + + MyAllocator() noexcept + { + } + + ~MyAllocator() noexcept {} + + template + MyAllocator(const MyAllocator &) noexcept + { + } + + T * allocate(size_t size, const void * = 0) + { + if (size == 0) { + return nullptr; + } + num_allocs++; + return static_cast(std::malloc(size * sizeof(T))); + } + + void deallocate(T * ptr, size_t size) + { + (void)size; + if (!ptr) { + return; + } + num_deallocs++; + std::free(ptr); + } + + template + struct rebind + { + typedef MyAllocator other; + }; +}; + +// Explicit specialization for void +template<> +struct MyAllocator +{ +public: + using value_type = void; + using pointer = void *; + using const_pointer = const void *; + + MyAllocator() noexcept + { + } + + ~MyAllocator() noexcept {} + + template + MyAllocator(const MyAllocator &) noexcept + { + } + + template + struct rebind + { + typedef MyAllocator other; + }; +}; + +template +constexpr bool operator==( + const MyAllocator &, + const MyAllocator &) noexcept +{ + return true; +} + +template +constexpr bool operator!=( + const MyAllocator &, + const MyAllocator &) noexcept +{ + return false; +} + +template< + typename PublishedMessageAllocatorT, + typename PublisherAllocatorT, + typename SubscribedMessageAllocatorT, + typename SubscriptionAllocatorT, + typename MessageMemoryStrategyAllocatorT, + typename MemoryStrategyAllocatorT, + typename ExpectedExceptionT +> +void +do_custom_allocator_test( + PublishedMessageAllocatorT published_message_allocator, + PublisherAllocatorT publisher_allocator, + SubscribedMessageAllocatorT /* subscribed_message_allocator */, // intentionally unused + SubscriptionAllocatorT subscription_allocator, + MessageMemoryStrategyAllocatorT message_memory_strategy, + MemoryStrategyAllocatorT memory_strategy_allocator) +{ + using PublishedMessageAllocTraits = + rclcpp::allocator::AllocRebind; + using PublishedMessageAlloc = typename PublishedMessageAllocTraits::allocator_type; + using PublishedMessageDeleter = + rclcpp::allocator::Deleter; + + using SubscribedMessageAllocTraits = + rclcpp::allocator::AllocRebind; + using SubscribedMessageAlloc = typename SubscribedMessageAllocTraits::allocator_type; + using SubscribedMessageDeleter = + rclcpp::allocator::Deleter; + + // init and node + auto context = std::make_shared(); + context->init(0, nullptr); + auto node = std::make_shared( + "custom_allocator_test", + rclcpp::NodeOptions().context(context).use_intra_process_comms(true)); + + // publisher + auto shared_publisher_allocator = std::make_shared(publisher_allocator); + rclcpp::PublisherOptionsWithAllocator publisher_options; + publisher_options.allocator = shared_publisher_allocator; + auto publisher = + node->create_publisher("custom_allocator_test", 10, publisher_options); + + // callback for subscription + uint32_t counter = 0; + std::promise> received_message; + auto received_message_future = received_message.get_future(); + auto callback = + [&counter, &received_message]( + std::unique_ptr msg) + { + ++counter; + received_message.set_value(std::move(msg)); + }; + + // subscription + auto shared_subscription_allocator = + std::make_shared(subscription_allocator); + rclcpp::SubscriptionOptionsWithAllocator subscription_options; + subscription_options.allocator = shared_subscription_allocator; + auto shared_message_strategy_allocator = + std::make_shared(message_memory_strategy); + auto msg_mem_strat = std::make_shared< + rclcpp::message_memory_strategy::MessageMemoryStrategy< + test_msgs::msg::Empty, + MessageMemoryStrategyAllocatorT + > + >(shared_message_strategy_allocator); + using CallbackMessageT = + typename rclcpp::subscription_traits::has_message_type::type; + auto subscriber = node->create_subscription< + test_msgs::msg::Empty, + decltype(callback), + SubscriptionAllocatorT, + rclcpp::Subscription, + rclcpp::message_memory_strategy::MessageMemoryStrategy< + CallbackMessageT, + MessageMemoryStrategyAllocatorT + > + >( + "custom_allocator_test", + 10, + std::forward(callback), + subscription_options, + msg_mem_strat); + + // executor memory strategy + using rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy; + auto shared_memory_strategy_allocator = std::make_shared( + memory_strategy_allocator); + std::shared_ptr memory_strategy = + std::make_shared>( + shared_memory_strategy_allocator); + + // executor + rclcpp::ExecutorOptions options; + options.memory_strategy = memory_strategy; + options.context = context; + rclcpp::executors::SingleThreadedExecutor executor(options); + + executor.add_node(node); + + // rebind message_allocator to ensure correct type + PublishedMessageDeleter message_deleter; + PublishedMessageAlloc rebound_message_allocator = published_message_allocator; + rclcpp::allocator::set_allocator_for_deleter(&message_deleter, &rebound_message_allocator); + + // allocate a message + auto ptr = PublishedMessageAllocTraits::allocate(rebound_message_allocator, 1); + PublishedMessageAllocTraits::construct(rebound_message_allocator, ptr); + std::unique_ptr msg(ptr, message_deleter); + + // publisher and receive + if constexpr (std::is_same_v) { + // no exception expected + EXPECT_NO_THROW( + { + publisher->publish(std::move(msg)); + executor.spin_until_future_complete(received_message_future, std::chrono::seconds(10)); + }); + EXPECT_EQ(ptr, received_message_future.get().get()); + EXPECT_EQ(1u, counter); + } else { + // exception expected + EXPECT_THROW( + { + publisher->publish(std::move(msg)); + executor.spin_until_future_complete(received_message_future, std::chrono::seconds(10)); + }, ExpectedExceptionT); + } +} + +/* + This tests the case where a custom allocator is used correctly, i.e. the same + custom allocator on both sides. + */ +TEST(TestIntraProcessManagerWithAllocators, custom_allocator) { + using PublishedMessageAllocatorT = std::allocator; + using PublisherAllocatorT = std::allocator; + using SubscribedMessageAllocatorT = std::allocator; + using SubscriptionAllocatorT = std::allocator; + using MessageMemoryStrategyAllocatorT = std::allocator; + using MemoryStrategyAllocatorT = std::allocator; + auto allocator = std::allocator(); + do_custom_allocator_test< + PublishedMessageAllocatorT, + PublisherAllocatorT, + SubscribedMessageAllocatorT, + SubscriptionAllocatorT, + MessageMemoryStrategyAllocatorT, + MemoryStrategyAllocatorT, + void // no exception expected + >(allocator, allocator, allocator, allocator, allocator, allocator); +} + +/* + This tests the case where a custom allocator is used incorrectly, i.e. different + custom allocators on both sides. + */ +TEST(TestIntraProcessManagerWithAllocators, custom_allocator_wrong) { + // explicitly use a different allocator here to provoke a failure + using PublishedMessageAllocatorT = std::allocator; + using PublisherAllocatorT = std::allocator; + using SubscribedMessageAllocatorT = MyAllocator; + using SubscriptionAllocatorT = MyAllocator; + using MessageMemoryStrategyAllocatorT = MyAllocator; + using MemoryStrategyAllocatorT = std::allocator; + auto allocator = std::allocator(); + auto my_allocator = MyAllocator(); + do_custom_allocator_test< + PublishedMessageAllocatorT, + PublisherAllocatorT, + SubscribedMessageAllocatorT, + SubscriptionAllocatorT, + MessageMemoryStrategyAllocatorT, + MemoryStrategyAllocatorT, + std::runtime_error // expected exception + >(allocator, allocator, my_allocator, my_allocator, my_allocator, allocator); +} diff --git a/rclcpp/test/rclcpp/test_loaned_message.cpp b/rclcpp/test/rclcpp/test_loaned_message.cpp new file mode 100644 index 0000000000..0623210079 --- /dev/null +++ b/rclcpp/test/rclcpp/test_loaned_message.cpp @@ -0,0 +1,146 @@ +// 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 "rclcpp/loaned_message.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "test_msgs/msg/basic_types.hpp" + +#include "../mocking_utils/patch.hpp" + +using MessageT = test_msgs::msg::BasicTypes; +using LoanedMessageT = rclcpp::LoanedMessage; + +class TestLoanedMessage : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestLoanedMessage, loan_from_pub) { + auto node = std::make_shared("loaned_message_test_node"); + auto pub = node->create_publisher("loaned_message_test_topic", 1); + + auto loaned_msg = pub->borrow_loaned_message(); + ASSERT_TRUE(loaned_msg.is_valid()); + loaned_msg.get().float64_value = 42.0f; + ASSERT_EQ(42.0f, loaned_msg.get().float64_value); + + SUCCEED(); +} + +TEST_F(TestLoanedMessage, release) { + auto node = std::make_shared("loaned_message_test_node"); + auto pub = node->create_publisher("loaned_message_test_topic", 1); + + std::unique_ptr> msg; + { + auto loaned_msg = pub->borrow_loaned_message(); + ASSERT_TRUE(loaned_msg.is_valid()); + loaned_msg.get().float64_value = 42.0f; + ASSERT_EQ(42.0f, loaned_msg.get().float64_value); + msg = loaned_msg.release(); + // call destructor implicitly. + // destructor not allowed to free memory because of not having ownership + // of the data after a call to release. + } + + ASSERT_EQ(42.0f, msg->float64_value); + + // Generally, the memory released from `LoanedMessage::release()` will be freed + // in deleter of unique_ptr or is managed in the middleware after calling + // `Publisher::do_loaned_message_publish` inside Publisher::publish(). + if (pub->can_loan_messages()) { + ASSERT_EQ( + RCL_RET_OK, + rcl_return_loaned_message_from_publisher(pub->get_publisher_handle().get(), msg.get())); + } + + SUCCEED(); +} + +TEST_F(TestLoanedMessage, construct_with_loaned_message_publisher) { + auto node = std::make_shared("loaned_message_test_node"); + auto publisher = node->create_publisher("topic", 10); + std::allocator allocator; + + auto mock_can_loan = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_publisher_can_loan_messages, true); + + { + auto mock_borrow_loaned = mocking_utils::patch_and_return( + "self", rcl_borrow_loaned_message, RCL_RET_ERROR); + + EXPECT_THROW( + std::make_shared(*publisher.get(), allocator).reset(), + rclcpp::exceptions::RCLError); + } + + MessageT message; + auto borrow_loaned_message_callback = + [&message]( + const rcl_publisher_t *, const rosidl_message_type_support_t *, void ** ros_message) { + *ros_message = &message; + return RCL_RET_OK; + }; + auto mock_borrow_loaned = mocking_utils::patch( + "self", rcl_borrow_loaned_message, borrow_loaned_message_callback); + + { + auto mock_return_loaned = mocking_utils::patch_and_return( + "self", rcl_return_loaned_message_from_publisher, RCL_RET_OK); + + auto loaned_message = std::make_shared(*publisher.get(), allocator); + EXPECT_TRUE(loaned_message->is_valid()); + EXPECT_NO_THROW(loaned_message.reset()); + } + + { + auto loaned_message = std::make_shared(*publisher.get(), allocator); + EXPECT_TRUE(loaned_message->is_valid()); + + auto mock_return_loaned = mocking_utils::patch_and_return( + "self", rcl_return_loaned_message_from_publisher, RCL_RET_ERROR); + + // No exception, it just logs an error + EXPECT_NO_THROW(loaned_message.reset()); + } +} + +TEST_F(TestLoanedMessage, move_loaned_message) { + auto node = std::make_shared("loaned_message_test_node"); + auto pub = node->create_publisher("loaned_message_test_topic", 1); + + auto loaned_msg_to_move = pub->borrow_loaned_message(); + // Force the move constructor to invoke + auto loaned_msg_moved_to = LoanedMessageT(std::move(loaned_msg_to_move)); + + ASSERT_TRUE(loaned_msg_moved_to.is_valid()); + + loaned_msg_moved_to.get().float32_value = 42.0f; + ASSERT_EQ(42.0f, loaned_msg_moved_to.get().float32_value); + SUCCEED(); +} diff --git a/rclcpp/test/rclcpp/test_logger.cpp b/rclcpp/test/rclcpp/test_logger.cpp new file mode 100644 index 0000000000..35118cf2df --- /dev/null +++ b/rclcpp/test/rclcpp/test_logger.cpp @@ -0,0 +1,217 @@ +// 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 "rcutils/env.h" + +#include "rclcpp/logger.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp/node.hpp" + +TEST(TestLogger, factory_functions) { + rclcpp::Logger logger = rclcpp::get_logger("test_logger"); + EXPECT_STREQ("test_logger", logger.get_name()); + rclcpp::Logger logger_copy = rclcpp::Logger(logger); + EXPECT_STREQ("test_logger", logger_copy.get_name()); +} + +TEST(TestLogger, hierarchy) { + rclcpp::Logger logger = rclcpp::get_logger("test_logger"); + rclcpp::Logger sublogger = logger.get_child("child"); + EXPECT_STREQ("test_logger.child", sublogger.get_name()); + rclcpp::Logger subsublogger = sublogger.get_child("grandchild"); + EXPECT_STREQ("test_logger.child.grandchild", subsublogger.get_name()); +} + +TEST(TestLogger, get_node_logger) { + rclcpp::init(0, nullptr); + auto node = std::make_shared("my_node", "/ns"); + auto node_base = rclcpp::node_interfaces::get_node_base_interface(node); + auto logger = rclcpp::get_node_logger(node_base->get_rcl_node_handle()); + EXPECT_STREQ(logger.get_name(), "ns.my_node"); + + logger = rclcpp::get_node_logger(nullptr); + rcl_reset_error(); + EXPECT_STREQ(logger.get_name(), "rclcpp"); + rclcpp::shutdown(); +} + +struct LogEvent +{ + bool console_output_handler_called; + std::string message; +}; +LogEvent g_last_log_event; + +TEST(TestLogger, set_level) { + ASSERT_EQ(RCUTILS_RET_OK, rcutils_logging_initialize()); + + rclcpp::Logger logger = rclcpp::get_logger("test_logger"); + EXPECT_THROW( + { + logger.set_level(static_cast(99999)); + }, rclcpp::exceptions::RCLInvalidArgument); + + auto rcutils_logging_console_output_handler = []( + const rcutils_log_location_t *, + int, const char *, rcutils_time_point_value_t, + const char * format, va_list * args) -> void + { + g_last_log_event.console_output_handler_called = true; + char buffer[1024]; + vsnprintf(buffer, sizeof(buffer), format, *args); + g_last_log_event.message = buffer; + }; + + rcutils_logging_output_handler_t previous_output_handler = rcutils_logging_get_output_handler(); + rcutils_logging_set_output_handler(rcutils_logging_console_output_handler); + + // default + RCLCPP_DEBUG(logger, "message %s", "debug"); + EXPECT_FALSE(g_last_log_event.console_output_handler_called); + RCLCPP_INFO(logger, "message %s", "info"); + EXPECT_TRUE(g_last_log_event.console_output_handler_called); + EXPECT_EQ("message info", g_last_log_event.message); + + // unset + g_last_log_event.console_output_handler_called = false; + logger.set_level(rclcpp::Logger::Level::Unset); + RCLCPP_DEBUG(logger, "message"); + RCLCPP_DEBUG(logger, "message %s", "debug"); + EXPECT_FALSE(g_last_log_event.console_output_handler_called); + RCLCPP_INFO(logger, "message"); + RCLCPP_INFO(logger, "message %s", "info"); + EXPECT_TRUE(g_last_log_event.console_output_handler_called); + EXPECT_EQ("message info", g_last_log_event.message); + + // debug + g_last_log_event.console_output_handler_called = false; + logger.set_level(rclcpp::Logger::Level::Debug); + RCLCPP_DEBUG(logger, "message %s", "debug"); + EXPECT_TRUE(g_last_log_event.console_output_handler_called); + EXPECT_EQ("message debug", g_last_log_event.message); + RCLCPP_INFO(logger, "message %s", "info"); + EXPECT_EQ("message info", g_last_log_event.message); + + // info + g_last_log_event.console_output_handler_called = false; + logger.set_level(rclcpp::Logger::Level::Info); + RCLCPP_DEBUG(logger, "message %s", "debug"); + EXPECT_FALSE(g_last_log_event.console_output_handler_called); + RCLCPP_INFO(logger, "message %s", "info"); + EXPECT_TRUE(g_last_log_event.console_output_handler_called); + EXPECT_EQ("message info", g_last_log_event.message); + + // warn + g_last_log_event.console_output_handler_called = false; + logger.set_level(rclcpp::Logger::Level::Warn); + RCLCPP_DEBUG(logger, "message %s", "debug"); + EXPECT_FALSE(g_last_log_event.console_output_handler_called); + RCLCPP_INFO(logger, "message %s", "info"); + EXPECT_FALSE(g_last_log_event.console_output_handler_called); + RCLCPP_WARN(logger, "message %s", "warn"); + EXPECT_TRUE(g_last_log_event.console_output_handler_called); + EXPECT_EQ("message warn", g_last_log_event.message); + + // error + g_last_log_event.console_output_handler_called = false; + logger.set_level(rclcpp::Logger::Level::Error); + RCLCPP_DEBUG(logger, "message %s", "debug"); + EXPECT_FALSE(g_last_log_event.console_output_handler_called); + RCLCPP_INFO(logger, "message %s", "info"); + EXPECT_FALSE(g_last_log_event.console_output_handler_called); + RCLCPP_WARN(logger, "message %s", "warn"); + EXPECT_FALSE(g_last_log_event.console_output_handler_called); + RCLCPP_ERROR(logger, "message %s", "error"); + EXPECT_TRUE(g_last_log_event.console_output_handler_called); + EXPECT_EQ("message error", g_last_log_event.message); + + // fatal + g_last_log_event.console_output_handler_called = false; + logger.set_level(rclcpp::Logger::Level::Fatal); + RCLCPP_DEBUG(logger, "message %s", "debug"); + EXPECT_FALSE(g_last_log_event.console_output_handler_called); + RCLCPP_INFO(logger, "message %s", "info"); + EXPECT_FALSE(g_last_log_event.console_output_handler_called); + RCLCPP_WARN(logger, "message %s", "warn"); + EXPECT_FALSE(g_last_log_event.console_output_handler_called); + RCLCPP_ERROR(logger, "message %s", "error"); + EXPECT_FALSE(g_last_log_event.console_output_handler_called); + RCLCPP_FATAL(logger, "message %s", "fatal"); + EXPECT_TRUE(g_last_log_event.console_output_handler_called); + EXPECT_EQ("message fatal", g_last_log_event.message); + + rcutils_logging_set_output_handler(previous_output_handler); + EXPECT_EQ(RCUTILS_RET_OK, rcutils_logging_shutdown()); +} + +TEST(TestLogger, get_effective_level) { + ASSERT_EQ(RCUTILS_RET_OK, rcutils_logging_initialize()); + + rclcpp::Logger logger = rclcpp::get_logger("test_logger"); + rclcpp::Logger child_logger = rclcpp::get_logger("test_logger.child"); + + // set child logger level unset to test effective level + child_logger.set_level(rclcpp::Logger::Level::Unset); + + // default + EXPECT_EQ(rclcpp::Logger::Level::Info, logger.get_effective_level()); + EXPECT_EQ(rclcpp::Logger::Level::Info, child_logger.get_effective_level()); + + // unset + logger.set_level(rclcpp::Logger::Level::Unset); + EXPECT_EQ(rclcpp::Logger::Level::Info, logger.get_effective_level()); + EXPECT_EQ(rclcpp::Logger::Level::Info, child_logger.get_effective_level()); + + // debug + logger.set_level(rclcpp::Logger::Level::Debug); + EXPECT_EQ(rclcpp::Logger::Level::Debug, logger.get_effective_level()); + EXPECT_EQ(rclcpp::Logger::Level::Debug, child_logger.get_effective_level()); + + // info + logger.set_level(rclcpp::Logger::Level::Info); + EXPECT_EQ(rclcpp::Logger::Level::Info, logger.get_effective_level()); + EXPECT_EQ(rclcpp::Logger::Level::Info, child_logger.get_effective_level()); + + // warn + logger.set_level(rclcpp::Logger::Level::Warn); + EXPECT_EQ(rclcpp::Logger::Level::Warn, logger.get_effective_level()); + EXPECT_EQ(rclcpp::Logger::Level::Warn, child_logger.get_effective_level()); + + // error + logger.set_level(rclcpp::Logger::Level::Error); + EXPECT_EQ(rclcpp::Logger::Level::Error, logger.get_effective_level()); + EXPECT_EQ(rclcpp::Logger::Level::Error, child_logger.get_effective_level()); + + // fatal + logger.set_level(rclcpp::Logger::Level::Fatal); + EXPECT_EQ(rclcpp::Logger::Level::Fatal, logger.get_effective_level()); + EXPECT_EQ(rclcpp::Logger::Level::Fatal, child_logger.get_effective_level()); +} + +TEST(TestLogger, get_logging_directory) { + ASSERT_EQ(true, rcutils_set_env("HOME", "/fake_home_dir")); + ASSERT_EQ(true, rcutils_set_env("USERPROFILE", nullptr)); + ASSERT_EQ(true, rcutils_set_env("ROS_LOG_DIR", nullptr)); + ASSERT_EQ(true, rcutils_set_env("ROS_HOME", nullptr)); + + auto path = rclcpp::get_log_directory(); + auto expected_path = std::filesystem::path{"/fake_home_dir"} / ".ros" / "log"; + EXPECT_EQ(path, expected_path); +} diff --git a/rclcpp/test/rclcpp/test_logger_service.cpp b/rclcpp/test/rclcpp/test_logger_service.cpp new file mode 100644 index 0000000000..92392f82aa --- /dev/null +++ b/rclcpp/test/rclcpp/test_logger_service.cpp @@ -0,0 +1,214 @@ +// Copyright 2023 Sony Group Corporation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT 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 "rclcpp/rclcpp.hpp" +#include "rclcpp/node.hpp" +#include "rcl_interfaces/srv/get_logger_levels.hpp" +#include "rcl_interfaces/srv/set_logger_levels.hpp" + +using namespace std::chrono_literals; + +class TestLoggerService : public ::testing::Test +{ +protected: + void SetUp() + { + rclcpp::init(0, nullptr); + rclcpp::NodeOptions options = rclcpp::NodeOptions(); + options.enable_logger_service(true); + node_ = std::make_shared("test_logger_service", "/test", options); + } + + void TearDown() + { + rclcpp::shutdown(); + } + + rclcpp::Node::SharedPtr node_; + std::thread thread_; +}; + +TEST_F(TestLoggerService, check_connect_get_logger_service) { + auto client = node_->create_client( + "/test/test_logger_service/get_logger_levels"); + ASSERT_TRUE(client->wait_for_service(2s)); +} + +TEST_F(TestLoggerService, check_connect_set_logger_service) { + auto client = node_->create_client( + "/test/test_logger_service/set_logger_levels"); + ASSERT_TRUE(client->wait_for_service(2s)); +} + +TEST_F(TestLoggerService, test_set_and_get_one_logging_level) { + std::string test_logger_name = "rcl"; + uint8_t test_logger_level = 20; + { + auto client = node_->create_client( + "/test/test_logger_service/set_logger_levels"); + ASSERT_TRUE(client->wait_for_service(1s)); + auto request = std::make_shared(); + auto level = rcl_interfaces::msg::LoggerLevel(); + level.name = test_logger_name; + level.level = test_logger_level; + request->levels.push_back(level); + auto result = client->async_send_request(request); + ASSERT_EQ( + rclcpp::spin_until_future_complete(node_, result), + rclcpp::FutureReturnCode::SUCCESS); + auto result_get = result.get(); + ASSERT_EQ(result_get->results.size(), 1u); + ASSERT_TRUE(result_get->results[0].successful); + ASSERT_STREQ(result_get->results[0].reason.c_str(), ""); + } + + { + auto client = node_->create_client( + "/test/test_logger_service/get_logger_levels"); + ASSERT_TRUE(client->wait_for_service(1s)); + auto request = std::make_shared(); + request->names.emplace_back(test_logger_name); + auto result = client->async_send_request(request); + ASSERT_EQ( + rclcpp::spin_until_future_complete(node_, result), + rclcpp::FutureReturnCode::SUCCESS); + auto result_get = result.get(); + ASSERT_EQ(result_get->levels.size(), 1u); + ASSERT_STREQ(result_get->levels[0].name.c_str(), test_logger_name.c_str()); + ASSERT_EQ(result_get->levels[0].level, test_logger_level); + } +} + +TEST_F(TestLoggerService, test_set_and_get_multi_logging_level) { + std::vector> test_data { + {"rcl", 30}, + {"rclcpp", 40}, + {"/test/test_logger_service", 50} + }; + + // Set multi log levels + { + auto client = node_->create_client( + "/test/test_logger_service/set_logger_levels"); + ASSERT_TRUE(client->wait_for_service(1s)); + auto request = std::make_shared(); + for (auto & set_level : test_data) { + auto level = rcl_interfaces::msg::LoggerLevel(); + level.name = std::get<0>(set_level); + level.level = std::get<1>(set_level); + request->levels.push_back(level); + } + auto result = client->async_send_request(request); + ASSERT_EQ( + rclcpp::spin_until_future_complete(node_, result), + rclcpp::FutureReturnCode::SUCCESS); + auto result_get = result.get(); + ASSERT_EQ(result_get->results.size(), test_data.size()); + for (uint32_t i = 0; i < test_data.size(); i++) { + ASSERT_TRUE(result_get->results[0].successful); + } + } + + // Get multi log levels + { + auto client = node_->create_client( + "/test/test_logger_service/get_logger_levels"); + ASSERT_TRUE(client->wait_for_service(1s)); + auto request = std::make_shared(); + for (auto & set_level : test_data) { + request->names.emplace_back(std::get<0>(set_level)); + } + auto result = client->async_send_request(request); + ASSERT_EQ( + rclcpp::spin_until_future_complete(node_, result), + rclcpp::FutureReturnCode::SUCCESS); + auto result_get = result.get(); + ASSERT_EQ(result_get->levels.size(), test_data.size()); + for (uint32_t i = 0; i < test_data.size(); i++) { + ASSERT_STREQ(result_get->levels[i].name.c_str(), std::get<0>(test_data[i]).c_str()); + ASSERT_EQ(result_get->levels[i].level, std::get<1>(test_data[i])); + } + } +} + +TEST_F(TestLoggerService, test_set_logging_level_with_invalid_param) { + std::vector> test_data { + {"rcl", 12}, + {"/test/test_logger_service", 22} + }; + + // Set multi log levels + { + auto client = node_->create_client( + "/test/test_logger_service/set_logger_levels"); + ASSERT_TRUE(client->wait_for_service(1s)); + auto request = std::make_shared(); + for (auto & set_level : test_data) { + auto level = rcl_interfaces::msg::LoggerLevel(); + level.name = std::get<0>(set_level); + level.level = std::get<1>(set_level); + request->levels.push_back(level); + } + auto result = client->async_send_request(request); + ASSERT_EQ( + rclcpp::spin_until_future_complete(node_, result), + rclcpp::FutureReturnCode::SUCCESS); + auto result_get = result.get(); + ASSERT_EQ(result_get->results.size(), test_data.size()); + for (uint32_t i = 0; i < test_data.size(); i++) { + ASSERT_FALSE(result_get->results[i].successful); + // Check string starts with prefix + ASSERT_EQ( + result_get->results[i].reason.rfind("Unable to determine severity_string for severity", 0), + 0); + } + } +} + +TEST_F(TestLoggerService, test_set_logging_level_with_partial_invalid_param) { + std::vector> test_data { + {"rcl", 20}, + {"rclcpp", 22}, + {"/test/test_logger_service", 30} + }; + + // Set multi log levels + { + auto client = node_->create_client( + "/test/test_logger_service/set_logger_levels"); + ASSERT_TRUE(client->wait_for_service(1s)); + auto request = std::make_shared(); + for (auto & set_level : test_data) { + auto level = rcl_interfaces::msg::LoggerLevel(); + level.name = std::get<0>(set_level); + level.level = std::get<1>(set_level); + request->levels.push_back(level); + } + auto result = client->async_send_request(request); + ASSERT_EQ( + rclcpp::spin_until_future_complete(node_, result), + rclcpp::FutureReturnCode::SUCCESS); + auto result_get = result.get(); + ASSERT_EQ(result_get->results.size(), test_data.size()); + ASSERT_TRUE(result_get->results[0].successful); + ASSERT_FALSE(result_get->results[1].successful); + ASSERT_TRUE(result_get->results[2].successful); + } +} diff --git a/rclcpp/test/rclcpp/test_logging.cpp b/rclcpp/test/rclcpp/test_logging.cpp new file mode 100644 index 0000000000..8e2214f8e6 --- /dev/null +++ b/rclcpp/test/rclcpp/test_logging.cpp @@ -0,0 +1,261 @@ +// 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 "rclcpp/clock.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/logging.hpp" +#include "rcutils/logging.h" +#include "rcutils/time.h" + +using ::testing::EndsWith; + +size_t g_log_calls = 0; +rclcpp::Logger g_logger = rclcpp::get_logger("name"); + +struct LogEvent +{ + const rcutils_log_location_t * location; + int level; + std::string name; + rcutils_time_point_value_t timestamp; + std::string message; +}; +LogEvent g_last_log_event; + +class TestLoggingMacros : public ::testing::Test +{ +public: + rcutils_logging_output_handler_t previous_output_handler; + void SetUp() + { + g_log_calls = 0; + ASSERT_EQ(RCUTILS_RET_OK, rcutils_logging_initialize()); + rcutils_logging_set_default_logger_level(RCUTILS_LOG_SEVERITY_DEBUG); + + auto rcutils_logging_console_output_handler = []( + const rcutils_log_location_t * location, + int level, const char * name, rcutils_time_point_value_t timestamp, + const char * format, va_list * args) -> void + { + g_log_calls += 1; + g_last_log_event.location = location; + g_last_log_event.level = level; + g_last_log_event.name = name ? name : ""; + g_last_log_event.timestamp = timestamp; + char buffer[1024]; + vsnprintf(buffer, sizeof(buffer), format, *args); + g_last_log_event.message = buffer; + }; + + this->previous_output_handler = rcutils_logging_get_output_handler(); + rcutils_logging_set_output_handler(rcutils_logging_console_output_handler); + } + + void TearDown() + { + rcutils_logging_set_output_handler(this->previous_output_handler); + ASSERT_EQ(RCUTILS_RET_OK, rcutils_logging_shutdown()); + EXPECT_FALSE(g_rcutils_logging_initialized); + } +}; + +class DummyNode +{ +public: + DummyNode() + { + clock_ = rclcpp::Clock::make_shared(RCL_ROS_TIME); + } + rclcpp::Clock::SharedPtr get_clock() + { + return clock_; + } + +private: + rclcpp::Clock::SharedPtr clock_; +}; + +TEST_F(TestLoggingMacros, test_logging_named) { + for (int i : {1, 2, 3}) { + RCLCPP_DEBUG(g_logger, "message %d", i); + } + size_t expected_location = __LINE__ - 2u; + EXPECT_EQ(3u, g_log_calls); + EXPECT_TRUE(g_last_log_event.location != NULL); + if (g_last_log_event.location) { + EXPECT_STREQ("TestBody", g_last_log_event.location->function_name); + EXPECT_THAT(g_last_log_event.location->file_name, EndsWith("test_logging.cpp")); + EXPECT_EQ(expected_location, g_last_log_event.location->line_number); + } + EXPECT_EQ(RCUTILS_LOG_SEVERITY_DEBUG, g_last_log_event.level); + EXPECT_EQ("name", g_last_log_event.name); + EXPECT_EQ("message 3", g_last_log_event.message); +} + +TEST_F(TestLoggingMacros, test_logging_stream) { + for (std::string i : {"one", "two", "three"}) { + RCLCPP_DEBUG_STREAM(g_logger, "message " << i); + } + EXPECT_EQ(3u, g_log_calls); + EXPECT_EQ("message three", g_last_log_event.message); + + RCLCPP_DEBUG_STREAM(g_logger, 4 << "th message"); + EXPECT_EQ("4th message", g_last_log_event.message); + + RCLCPP_DEBUG_STREAM(g_logger, "message " << 5); + EXPECT_EQ("message 5", g_last_log_event.message); +} + +TEST_F(TestLoggingMacros, test_logging_once) { + for (int i : {1, 2, 3}) { + RCLCPP_INFO_ONCE(g_logger, "message %d", i); + } + EXPECT_EQ(1u, g_log_calls); + EXPECT_EQ(RCUTILS_LOG_SEVERITY_INFO, g_last_log_event.level); + EXPECT_EQ("name", g_last_log_event.name); + EXPECT_EQ("message 1", g_last_log_event.message); + + // Check that another instance has a context that's independent to the call above's + g_log_calls = 0; + for (int i : {1, 2, 3}) { + RCLCPP_INFO_ONCE(g_logger, "second message %d", i); + } + EXPECT_EQ(1u, g_log_calls); + EXPECT_EQ(RCUTILS_LOG_SEVERITY_INFO, g_last_log_event.level); + EXPECT_EQ("name", g_last_log_event.name); + EXPECT_EQ("second message 1", g_last_log_event.message); +} + +TEST_F(TestLoggingMacros, test_logging_expression) { + for (int i : {1, 2, 3, 4, 5, 6}) { + RCLCPP_INFO_EXPRESSION(g_logger, i % 3, "message %d", i); + } + EXPECT_EQ(4u, g_log_calls); + EXPECT_EQ("message 5", g_last_log_event.message); +} + +int g_counter = 0; + +bool mod3() +{ + return (g_counter % 3) != 0; +} + +TEST_F(TestLoggingMacros, test_logging_function) { + for (int i : {1, 2, 3, 4, 5, 6}) { + g_counter = i; + RCLCPP_INFO_FUNCTION(g_logger, &mod3, "message %d", i); + } + EXPECT_EQ(4u, g_log_calls); + EXPECT_EQ("message 5", g_last_log_event.message); +} + +TEST_F(TestLoggingMacros, test_logging_skipfirst) { + for (uint32_t i : {1, 2, 3, 4, 5}) { + RCLCPP_WARN_SKIPFIRST(g_logger, "message %u", i); + EXPECT_EQ(i - 1, g_log_calls); + } +} + +TEST_F(TestLoggingMacros, test_throttle) { + using namespace std::chrono_literals; + rclcpp::Clock steady_clock(RCL_STEADY_TIME); + for (uint64_t i = 0; i < 3; ++i) { + RCLCPP_DEBUG_THROTTLE(g_logger, steady_clock, 10000, "Throttling"); + } + EXPECT_EQ(1u, g_log_calls); + RCLCPP_DEBUG_SKIPFIRST_THROTTLE(g_logger, steady_clock, 1, "Skip first throttling"); + EXPECT_EQ(1u, g_log_calls); + for (uint64_t i = 0; i < 6; ++i) { + RCLCPP_DEBUG_THROTTLE(g_logger, steady_clock, 100, "Throttling"); + RCLCPP_DEBUG_SKIPFIRST_THROTTLE(g_logger, steady_clock, 400, "Throttling"); + std::this_thread::sleep_for(50ms); + } + EXPECT_EQ(4u, g_log_calls); + rclcpp::Clock ros_clock(RCL_ROS_TIME); + ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(ros_clock.get_clock_handle())); + RCLCPP_DEBUG_THROTTLE(g_logger, ros_clock, 10000, "Throttling"); + rcl_clock_t * clock = ros_clock.get_clock_handle(); + ASSERT_TRUE(clock); + EXPECT_EQ(4u, g_log_calls); + EXPECT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock, RCUTILS_MS_TO_NS(10))); + for (uint64_t i = 0; i < 2; ++i) { + RCLCPP_DEBUG_THROTTLE(g_logger, ros_clock, 10, "Throttling"); + if (i == 0) { + EXPECT_EQ(5u, g_log_calls); + rcl_time_point_value_t clock_ns = ros_clock.now().nanoseconds() + RCUTILS_MS_TO_NS(10); + EXPECT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock, clock_ns)); + } else { + EXPECT_EQ(6u, g_log_calls); + } + } + DummyNode node; + rcl_clock_t * node_clock = node.get_clock()->get_clock_handle(); + ASSERT_TRUE(node_clock); + ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(node_clock)); + EXPECT_EQ(6u, g_log_calls); + EXPECT_EQ(RCL_RET_OK, rcl_set_ros_time_override(node_clock, RCUTILS_MS_TO_NS(10))); + for (uint64_t i = 0; i < 3; ++i) { + RCLCPP_DEBUG_THROTTLE(g_logger, *node.get_clock(), 10, "Throttling"); + if (i == 0) { + EXPECT_EQ(7u, g_log_calls); + rcl_time_point_value_t clock_ns = node.get_clock()->now().nanoseconds() + RCUTILS_MS_TO_NS(5); + EXPECT_EQ(RCL_RET_OK, rcl_set_ros_time_override(node_clock, clock_ns)); + } else if (i == 1) { + EXPECT_EQ(7u, g_log_calls); + rcl_time_point_value_t clock_ns = node.get_clock()->now().nanoseconds() + RCUTILS_MS_TO_NS(5); + EXPECT_EQ(RCL_RET_OK, rcl_set_ros_time_override(node_clock, clock_ns)); + } else { + EXPECT_EQ(8u, g_log_calls); + } + } +} + +TEST_F(TestLoggingMacros, test_parameter_expression) { + RCLCPP_DEBUG_STREAM(*&g_logger, "message"); + EXPECT_EQ(1u, g_log_calls); + EXPECT_EQ("message", g_last_log_event.message); +} + +bool log_function(rclcpp::Logger logger) +{ + RCLCPP_INFO(logger, "successful log"); + return true; +} + +bool log_function_const(const rclcpp::Logger logger) +{ + RCLCPP_INFO(logger, "successful log"); + return true; +} + +bool log_function_const_ref(const rclcpp::Logger & logger) +{ + RCLCPP_INFO(logger, "successful log"); + return true; +} + +TEST_F(TestLoggingMacros, test_log_from_node) { + auto logger = rclcpp::get_logger("test_logging_logger"); + EXPECT_TRUE(log_function(logger)); + EXPECT_TRUE(log_function_const(logger)); + EXPECT_TRUE(log_function_const_ref(logger)); +} diff --git a/rclcpp/test/rclcpp/test_memory_strategy.cpp b/rclcpp/test/rclcpp/test_memory_strategy.cpp new file mode 100644 index 0000000000..7ceb4e7be4 --- /dev/null +++ b/rclcpp/test/rclcpp/test_memory_strategy.cpp @@ -0,0 +1,538 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include + +#include "rclcpp/strategies/allocator_memory_strategy.hpp" +#include "rclcpp/memory_strategy.hpp" +#include "test_msgs/msg/empty.hpp" +#include "test_msgs/srv/empty.hpp" + +using rclcpp::memory_strategy::MemoryStrategy; +typedef std::map> WeakCallbackGroupsToNodesMap; + +/** + * Mock Waitable class + */ +class TestWaitable : public rclcpp::Waitable +{ +public: + void add_to_wait_set(rcl_wait_set_t &) override {} + bool is_ready(const rcl_wait_set_t &) override {return true;} + + std::shared_ptr take_data() override {return nullptr;} + void execute(const std::shared_ptr &) override {} + + void set_on_ready_callback(std::function) override {} + void clear_on_ready_callback() override {} + + std::shared_ptr take_data_by_entity_id(size_t) override {return nullptr;} +}; + +class TestMemoryStrategy : public ::testing::Test +{ +public: + TestMemoryStrategy() + : memory_strategy_(nullptr) {} + + void SetUp() override + { + rclcpp::init(0, nullptr); + + // This doesn't test AllocatorMemoryStrategy directly, so we cast to the base class. + // AllocatorMemoryStrategy is more commonly used than MessagePoolMemoryStrategy + // so we use this derived class for these tests. + memory_strategy_ = + std::make_shared< + rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy<>>(); + } + + void TearDown() override + { + rclcpp::shutdown(); + } + +protected: + std::shared_ptr memory_strategy() + { + return memory_strategy_; + } + +private: + std::shared_ptr memory_strategy_; +}; + +TEST_F(TestMemoryStrategy, construct_destruct) { + EXPECT_NE(nullptr, memory_strategy()); +} + +TEST_F(TestMemoryStrategy, get_subscription_by_handle) { + WeakCallbackGroupsToNodesMap weak_groups_to_nodes; + std::shared_ptr subscription_handle; + rclcpp::SubscriptionBase::SharedPtr found_subscription = nullptr; + { + auto node = std::make_shared("node", "ns"); + node->for_each_callback_group( + [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) + { + weak_groups_to_nodes.insert( + std::pair( + group_ptr, + node->get_node_base_interface())); + }); + memory_strategy()->collect_entities(weak_groups_to_nodes); + { + auto callback_group = + node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + auto subscription_callback = [](test_msgs::msg::Empty::ConstSharedPtr) {}; + const rclcpp::QoS qos(10); + + { + auto subscription = node->create_subscription< + test_msgs::msg::Empty, decltype(subscription_callback)>( + "topic", qos, std::move(subscription_callback)); + + subscription_handle = subscription->get_subscription_handle(); + + EXPECT_EQ( + subscription, + memory_strategy()->get_subscription_by_handle(subscription_handle, weak_groups_to_nodes)); + } // subscription goes out of scope + EXPECT_EQ( + nullptr, + memory_strategy()->get_subscription_by_handle(subscription_handle, weak_groups_to_nodes)); + } // callback_group goes out of scope + EXPECT_EQ( + nullptr, + memory_strategy()->get_subscription_by_handle(subscription_handle, weak_groups_to_nodes)); + } // Node goes out of scope + EXPECT_EQ( + nullptr, + memory_strategy()->get_subscription_by_handle(subscription_handle, weak_groups_to_nodes)); +} + +TEST_F(TestMemoryStrategy, get_service_by_handle) { + WeakCallbackGroupsToNodesMap weak_groups_to_nodes; + std::shared_ptr service_handle; + rclcpp::ServiceBase::SharedPtr found_service = nullptr; + { + auto node = std::make_shared("node", "ns"); + node->for_each_callback_group( + [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) + { + weak_groups_to_nodes.insert( + std::pair( + group_ptr, + node->get_node_base_interface())); + }); + memory_strategy()->collect_entities(weak_groups_to_nodes); + { + auto callback_group = + node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + auto service_callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}; + const rclcpp::QoS qos(10); + weak_groups_to_nodes.insert( + std::pair( + rclcpp::CallbackGroup::WeakPtr(callback_group), + node->get_node_base_interface())); + { + auto service = node->create_service( + "service", std::move(service_callback), + rclcpp::ServicesQoS(), callback_group); + + service_handle = service->get_service_handle(); + + EXPECT_EQ( + service, + memory_strategy()->get_service_by_handle(service_handle, weak_groups_to_nodes)); + } // service goes out of scope + EXPECT_EQ( + nullptr, + memory_strategy()->get_service_by_handle(service_handle, weak_groups_to_nodes)); + } // callback_group goes out of scope + EXPECT_EQ( + nullptr, + memory_strategy()->get_service_by_handle(service_handle, weak_groups_to_nodes)); + } // Node goes out of scope + EXPECT_EQ( + nullptr, + memory_strategy()->get_service_by_handle(service_handle, weak_groups_to_nodes)); +} + +TEST_F(TestMemoryStrategy, get_client_by_handle) { + WeakCallbackGroupsToNodesMap weak_groups_to_nodes; + std::shared_ptr client_handle; + rclcpp::ClientBase::SharedPtr found_client = nullptr; + { + auto node = std::make_shared("node", "ns"); + node->for_each_callback_group( + [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) + { + weak_groups_to_nodes.insert( + std::pair( + group_ptr, + node->get_node_base_interface())); + }); + memory_strategy()->collect_entities(weak_groups_to_nodes); + { + auto callback_group = + node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + { + auto client = node->create_client( + "service", rclcpp::ServicesQoS(), callback_group); + + client_handle = client->get_client_handle(); + weak_groups_to_nodes.insert( + std::pair( + rclcpp::CallbackGroup::WeakPtr(callback_group), + node->get_node_base_interface())); + + EXPECT_EQ( + client, + memory_strategy()->get_client_by_handle(client_handle, weak_groups_to_nodes)); + } // client goes out of scope + EXPECT_EQ( + nullptr, + memory_strategy()->get_client_by_handle(client_handle, weak_groups_to_nodes)); + } // callback_group goes out of scope + EXPECT_EQ( + nullptr, + memory_strategy()->get_client_by_handle(client_handle, weak_groups_to_nodes)); + } // Node goes out of scope + EXPECT_EQ( + nullptr, + memory_strategy()->get_client_by_handle(client_handle, weak_groups_to_nodes)); +} + +TEST_F(TestMemoryStrategy, get_timer_by_handle) { + WeakCallbackGroupsToNodesMap weak_groups_to_nodes; + std::shared_ptr timer_handle; + rclcpp::TimerBase::SharedPtr found_timer = nullptr; + { + auto node = std::make_shared("node", "ns"); + node->for_each_callback_group( + [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) + { + weak_groups_to_nodes.insert( + std::pair( + group_ptr, + node->get_node_base_interface())); + }); + memory_strategy()->collect_entities(weak_groups_to_nodes); + { + auto callback_group = + node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + { + auto timer_callback = []() {}; + auto timer = node->create_wall_timer( + std::chrono::milliseconds(1), timer_callback, callback_group); + weak_groups_to_nodes.insert( + std::pair( + rclcpp::CallbackGroup::WeakPtr(callback_group), + node->get_node_base_interface())); + + timer_handle = timer->get_timer_handle(); + + EXPECT_EQ( + timer, + memory_strategy()->get_timer_by_handle(timer_handle, weak_groups_to_nodes)); + } // timer goes out of scope + EXPECT_EQ( + nullptr, + memory_strategy()->get_timer_by_handle(timer_handle, weak_groups_to_nodes)); + } // callback_group goes out of scope + EXPECT_EQ( + nullptr, + memory_strategy()->get_timer_by_handle(timer_handle, weak_groups_to_nodes)); + } // Node goes out of scope + EXPECT_EQ( + nullptr, + memory_strategy()->get_timer_by_handle(timer_handle, weak_groups_to_nodes)); +} + +TEST_F(TestMemoryStrategy, get_node_by_group) { + WeakCallbackGroupsToNodesMap weak_groups_to_nodes; + rclcpp::CallbackGroup::SharedPtr callback_group = nullptr; + { + auto node = std::make_shared("node", "ns"); + auto node_handle = node->get_node_base_interface(); + node_handle->for_each_callback_group( + [node_handle, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) + { + weak_groups_to_nodes.insert( + std::pair( + group_ptr, + node_handle)); + }); + memory_strategy()->collect_entities(weak_groups_to_nodes); + EXPECT_EQ( + nullptr, + memory_strategy()->get_node_by_group(nullptr, weak_groups_to_nodes)); + + callback_group = + node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + // Nothing in the weak_groups_to_nodes, so find fails. + EXPECT_EQ( + nullptr, + memory_strategy()->get_node_by_group(callback_group, weak_groups_to_nodes)); + + weak_groups_to_nodes.insert( + std::pair( + rclcpp::CallbackGroup::WeakPtr(callback_group), + node->get_node_base_interface())); + EXPECT_EQ( + node_handle, + memory_strategy()->get_node_by_group(callback_group, weak_groups_to_nodes)); + // Clear the handles to not hold NodeBase. + memory_strategy()->clear_handles(); + } // Node goes out of scope + // Callback group still exists, so lookup returns nullptr because node is destroyed. + EXPECT_EQ( + nullptr, + memory_strategy()->get_node_by_group(callback_group, weak_groups_to_nodes)); +} + +TEST_F(TestMemoryStrategy, get_group_by_subscription) { + WeakCallbackGroupsToNodesMap weak_groups_to_nodes; + rclcpp::SubscriptionBase::SharedPtr subscription = nullptr; + rclcpp::CallbackGroup::SharedPtr callback_group = nullptr; + { + auto node = std::make_shared("node", "ns"); + node->for_each_callback_group( + [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) + { + weak_groups_to_nodes.insert( + std::pair( + group_ptr, + node->get_node_base_interface())); + }); + memory_strategy()->collect_entities(weak_groups_to_nodes); + { + // This group is just used to test that a callback group that is held as a weak pointer + // by node, doesn't confuse get_group_by_subscription() when it goes out of scope + auto non_persistant_group = + node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + callback_group = + node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + auto subscription_callback = [](test_msgs::msg::Empty::ConstSharedPtr) {}; + const rclcpp::QoS qos(10); + + rclcpp::SubscriptionOptions subscription_options; + + // This callback group is held as a shared_ptr in subscription_options, which means it + // stays alive as long as subscription does. + subscription_options.callback_group = callback_group; + + subscription = node->create_subscription< + test_msgs::msg::Empty, decltype(subscription_callback)>( + "topic", qos, std::move(subscription_callback), subscription_options); + weak_groups_to_nodes.insert( + std::pair( + rclcpp::CallbackGroup::WeakPtr(callback_group), + rclcpp::node_interfaces::NodeBaseInterface::WeakPtr(node->get_node_base_interface()))); + EXPECT_EQ( + callback_group, + memory_strategy()->get_group_by_subscription(subscription, weak_groups_to_nodes)); + } // callback_group goes out of scope + EXPECT_EQ( + callback_group, + memory_strategy()->get_group_by_subscription(subscription, weak_groups_to_nodes)); + } // Node goes out of scope + // NodeBase(SubscriptionBase->rcl_node_t->NodeBase) is still alive. + EXPECT_EQ( + callback_group, + memory_strategy()->get_group_by_subscription(subscription, weak_groups_to_nodes)); +} + +TEST_F(TestMemoryStrategy, get_group_by_service) { + WeakCallbackGroupsToNodesMap weak_groups_to_nodes; + rclcpp::ServiceBase::SharedPtr service = nullptr; + { + auto node = std::make_shared("node", "ns"); + node->for_each_callback_group( + [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) + { + weak_groups_to_nodes.insert( + std::pair( + group_ptr, + node->get_node_base_interface())); + }); + memory_strategy()->collect_entities(weak_groups_to_nodes); + { + auto callback_group = + node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + auto service_callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}; + const rclcpp::QoS qos(10); + + service = node->create_service( + "service", std::move(service_callback), + rclcpp::ServicesQoS(), callback_group); + weak_groups_to_nodes.insert( + std::pair( + rclcpp::CallbackGroup::WeakPtr(callback_group), + rclcpp::node_interfaces::NodeBaseInterface::WeakPtr(node->get_node_base_interface()))); + EXPECT_EQ( + callback_group, + memory_strategy()->get_group_by_service(service, weak_groups_to_nodes)); + } // callback_group goes out of scope + EXPECT_EQ( + nullptr, + memory_strategy()->get_group_by_service(service, weak_groups_to_nodes)); + } // Node goes out of scope + EXPECT_EQ( + nullptr, + memory_strategy()->get_group_by_service(service, weak_groups_to_nodes)); +} + +TEST_F(TestMemoryStrategy, get_group_by_client) { + WeakCallbackGroupsToNodesMap weak_groups_to_nodes; + rclcpp::ClientBase::SharedPtr client = nullptr; + { + auto node = std::make_shared("node", "ns"); + node->for_each_callback_group( + [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) + { + weak_groups_to_nodes.insert( + std::pair( + group_ptr, + node->get_node_base_interface())); + }); + memory_strategy()->collect_entities(weak_groups_to_nodes); + { + auto callback_group = + node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + client = node->create_client( + "service", rclcpp::ServicesQoS(), callback_group); + weak_groups_to_nodes.insert( + std::pair( + rclcpp::CallbackGroup::WeakPtr(callback_group), + rclcpp::node_interfaces::NodeBaseInterface::WeakPtr(node->get_node_base_interface()))); + EXPECT_EQ( + callback_group, + memory_strategy()->get_group_by_client(client, weak_groups_to_nodes)); + } // callback_group goes out of scope + EXPECT_EQ( + nullptr, + memory_strategy()->get_group_by_client(client, weak_groups_to_nodes)); + } // Node goes out of scope + EXPECT_EQ( + nullptr, + memory_strategy()->get_group_by_client(client, weak_groups_to_nodes)); +} + +TEST_F(TestMemoryStrategy, get_group_by_timer) { + WeakCallbackGroupsToNodesMap weak_groups_to_nodes; + rclcpp::TimerBase::SharedPtr timer = nullptr; + { + auto node = std::make_shared("node", "ns"); + node->for_each_callback_group( + [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) + { + weak_groups_to_nodes.insert( + std::pair( + group_ptr, + node->get_node_base_interface())); + }); + memory_strategy()->collect_entities(weak_groups_to_nodes); + { + auto callback_group = + node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + auto timer_callback = []() {}; + timer = node->create_wall_timer( + std::chrono::milliseconds(1), timer_callback, callback_group); + weak_groups_to_nodes.insert( + std::pair( + rclcpp::CallbackGroup::WeakPtr(callback_group), + rclcpp::node_interfaces::NodeBaseInterface::WeakPtr(node->get_node_base_interface()))); + EXPECT_EQ( + callback_group, + memory_strategy()->get_group_by_timer(timer, weak_groups_to_nodes)); + } // callback_group goes out of scope + EXPECT_EQ( + nullptr, + memory_strategy()->get_group_by_timer(timer, weak_groups_to_nodes)); + } // Node goes out of scope + EXPECT_EQ( + nullptr, + memory_strategy()->get_group_by_timer(timer, weak_groups_to_nodes)); +} + +TEST_F(TestMemoryStrategy, get_group_by_waitable) { + WeakCallbackGroupsToNodesMap weak_groups_to_nodes; + rclcpp::Waitable::SharedPtr waitable = nullptr; + { + auto node = std::make_shared("node", "ns"); + node->for_each_callback_group( + [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) + { + weak_groups_to_nodes.insert( + std::pair( + group_ptr, + node->get_node_base_interface())); + }); + memory_strategy()->collect_entities(weak_groups_to_nodes); + { + waitable = std::make_shared(); + auto callback_group = + node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + node->get_node_waitables_interface()->add_waitable(waitable, callback_group); + weak_groups_to_nodes.insert( + std::pair( + rclcpp::CallbackGroup::WeakPtr(callback_group), + rclcpp::node_interfaces::NodeBaseInterface::WeakPtr(node->get_node_base_interface()))); + EXPECT_EQ( + callback_group, + memory_strategy()->get_group_by_waitable(waitable, weak_groups_to_nodes)); + } // callback_group goes out of scope + EXPECT_EQ( + nullptr, + memory_strategy()->get_group_by_waitable(waitable, weak_groups_to_nodes)); + } // Node goes out of scope + EXPECT_EQ( + nullptr, + memory_strategy()->get_group_by_waitable(waitable, weak_groups_to_nodes)); +} diff --git a/rclcpp/test/rclcpp/test_message_memory_strategy.cpp b/rclcpp/test/rclcpp/test_message_memory_strategy.cpp new file mode 100644 index 0000000000..26ebd5ec37 --- /dev/null +++ b/rclcpp/test/rclcpp/test_message_memory_strategy.cpp @@ -0,0 +1,58 @@ +// 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 "rclcpp/message_memory_strategy.hpp" +#include "test_msgs/msg/empty.hpp" + +TEST(TestMemoryStrategies, construct_destruct) { + rclcpp::message_memory_strategy::MessageMemoryStrategy memory_strategy1; + + EXPECT_NE(nullptr, memory_strategy1.message_allocator_); + EXPECT_NE(nullptr, memory_strategy1.serialized_message_allocator_); + EXPECT_NE(nullptr, memory_strategy1.buffer_allocator_); + + auto allocator = std::make_shared>(); + rclcpp::message_memory_strategy::MessageMemoryStrategy memory_strategy2( + allocator); + + EXPECT_NE(nullptr, memory_strategy2.message_allocator_); + EXPECT_NE(nullptr, memory_strategy2.serialized_message_allocator_); + EXPECT_NE(nullptr, memory_strategy2.buffer_allocator_); +} + +TEST(TestMemoryStrategies, standard_allocation) { + auto memory_strategy = + rclcpp::message_memory_strategy::MessageMemoryStrategy< + test_msgs::msg::Empty>::create_default(); + ASSERT_NE(nullptr, memory_strategy); + + auto borrowed_message = memory_strategy->borrow_message(); + ASSERT_NE(nullptr, borrowed_message); + EXPECT_NO_THROW(memory_strategy->return_message(borrowed_message)); + + auto serialized_message = memory_strategy->borrow_serialized_message(); + ASSERT_NE(nullptr, serialized_message); + EXPECT_EQ(0u, serialized_message->capacity()); + EXPECT_NO_THROW(memory_strategy->return_serialized_message(serialized_message)); + + memory_strategy->set_default_buffer_capacity(42); + serialized_message = memory_strategy->borrow_serialized_message(); + ASSERT_NE(nullptr, serialized_message); + EXPECT_EQ(42u, serialized_message->capacity()); + EXPECT_NO_THROW(memory_strategy->return_serialized_message(serialized_message)); +} diff --git a/rclcpp/test/rclcpp/test_node.cpp b/rclcpp/test/rclcpp/test_node.cpp new file mode 100644 index 0000000000..c956b273be --- /dev/null +++ b/rclcpp/test/rclcpp/test_node.cpp @@ -0,0 +1,3476 @@ +// 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 + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "rcpputils/filesystem_helper.hpp" +#include "rcpputils/scope_exit.hpp" + +#include "rmw/validate_namespace.h" + +#include "test_msgs/msg/basic_types.hpp" +#include "test_msgs/msg/empty.hpp" +#include "test_msgs/srv/empty.hpp" + +#include "../mocking_utils/patch.hpp" + +class TestNode : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() override + { + test_resources_path /= "test_node"; + } + + std::filesystem::path test_resources_path{TEST_RESOURCES_DIRECTORY}; +}; + +/* + Testing node construction and destruction. + */ +TEST_F(TestNode, construction_and_destruction) { + { + auto node = std::make_shared("my_node", "/ns"); + EXPECT_NE(nullptr, node->get_node_base_interface()); + EXPECT_NE(nullptr, node->get_node_clock_interface()); + EXPECT_NE(nullptr, node->get_node_graph_interface()); + EXPECT_NE(nullptr, node->get_node_logging_interface()); + EXPECT_NE(nullptr, node->get_node_time_source_interface()); + EXPECT_NE(nullptr, node->get_node_timers_interface()); + EXPECT_NE(nullptr, node->get_node_topics_interface()); + EXPECT_NE(nullptr, node->get_node_services_interface()); + EXPECT_NE(nullptr, node->get_node_parameters_interface()); + EXPECT_NE(nullptr, node->get_node_waitables_interface()); + EXPECT_NE(nullptr, node->get_node_options().get_rcl_node_options()); + EXPECT_NE(nullptr, node->get_graph_event()); + EXPECT_NE(nullptr, node->get_clock()); + EXPECT_NE(nullptr, node->get_node_type_descriptions_interface()); + } + + { + ASSERT_THROW( + { + auto node = std::make_shared("invalid_node?", "/ns"); + (void)node; + }, rclcpp::exceptions::InvalidNodeNameError); + } + + { + ASSERT_THROW( + { + auto node = std::make_shared("my_node", "/invalid_ns?"); + (void)node; + }, rclcpp::exceptions::InvalidNamespaceError); + } + + { + rclcpp::NodeOptions options; + ASSERT_NO_THROW( + { + const auto node = std::make_shared("my_node", "/ns", options); + EXPECT_EQ(RCL_ROS_TIME, node->get_clock()->get_clock_type()); + }); + } + + { + rclcpp::NodeOptions options; + options.parameter_overrides( + { + {"use_sim_time", true}, + }); + ASSERT_NO_THROW( + { + const auto node = std::make_shared("my_node", "/ns", options); + EXPECT_EQ(RCL_ROS_TIME, node->get_clock()->get_clock_type()); + }); + } + + { + rclcpp::NodeOptions options; + options.clock_type(RCL_SYSTEM_TIME); + ASSERT_NO_THROW( + { + const auto node = std::make_shared("my_node", "/ns", options); + EXPECT_EQ(RCL_SYSTEM_TIME, node->get_clock()->get_clock_type()); + }); + } + + { + rclcpp::NodeOptions options; + options.parameter_overrides( + { + {"use_sim_time", true}, + }); + options.clock_type(RCL_SYSTEM_TIME); + ASSERT_THROW( + const auto node = std::make_shared( + "my_node", "/ns", + options), std::invalid_argument); + } + + { + rclcpp::NodeOptions options; + options.clock_type(RCL_STEADY_TIME); + ASSERT_NO_THROW( + { + const auto node = std::make_shared("my_node", "/ns", options); + EXPECT_EQ(RCL_STEADY_TIME, node->get_clock()->get_clock_type()); + }); + } + + { + rclcpp::NodeOptions options; + options.parameter_overrides( + { + {"use_sim_time", true}, + }); + options.clock_type(RCL_STEADY_TIME); + ASSERT_THROW( + const auto node = std::make_shared( + "my_node", "/ns", + options), std::invalid_argument); + } +} + +/* + Testing lifecycles of subscriptions and publishers after node dies + */ +TEST_F(TestNode, pub_and_sub_lifecycles) { + using test_msgs::msg::Empty; + rclcpp::Publisher::SharedPtr pub; + rclcpp::Subscription::SharedPtr sub; + const auto callback = [](Empty::ConstSharedPtr) {}; + + { + // Create the node and context in a nested scope so that their + // std::shared_ptrs expire before we use pub and sub + auto context = std::make_shared(); + context->init(0, nullptr); + rclcpp::NodeOptions options; + options.context(context); + + const auto node = std::make_shared("my_node", "/ns", options); + pub = node->create_publisher("topic", 10); + sub = node->create_subscription("topic", 10, callback); + } + + pub->publish(Empty()); +} + +TEST_F(TestNode, get_name_and_namespace) { + { + auto node = std::make_shared("my_node", "/ns"); + EXPECT_STREQ("my_node", node->get_name()); + EXPECT_STREQ("/ns", node->get_namespace()); + EXPECT_STREQ("/ns", node->get_effective_namespace().c_str()); + EXPECT_STREQ("/ns/my_node", node->get_fully_qualified_name()); + } + { + auto options = rclcpp::NodeOptions() + .arguments({"--ros-args", "-r", "__ns:=/another_ns"}); + auto node = std::make_shared("my_node", "/ns", options); + EXPECT_STREQ("my_node", node->get_name()); + EXPECT_STREQ("/another_ns", node->get_namespace()); + EXPECT_STREQ("/another_ns/my_node", node->get_fully_qualified_name()); + } + { + auto node = std::make_shared("my_node", "ns"); + EXPECT_STREQ("my_node", node->get_name()); + EXPECT_STREQ("/ns", node->get_namespace()); + EXPECT_STREQ("/ns", node->get_effective_namespace().c_str()); + EXPECT_STREQ("/ns/my_node", node->get_fully_qualified_name()); + } + { + auto node = std::make_shared("my_node"); + EXPECT_STREQ("my_node", node->get_name()); + EXPECT_STREQ("/", node->get_namespace()); + EXPECT_STREQ("/", node->get_effective_namespace().c_str()); + EXPECT_STREQ("/my_node", node->get_fully_qualified_name()); + } + { + auto node = std::make_shared("my_node", ""); + EXPECT_STREQ("my_node", node->get_name()); + EXPECT_STREQ("/", node->get_namespace()); + EXPECT_STREQ("/", node->get_effective_namespace().c_str()); + EXPECT_STREQ("/my_node", node->get_fully_qualified_name()); + } + { + auto node = std::make_shared("my_node", "/my/ns"); + EXPECT_STREQ("my_node", node->get_name()); + EXPECT_STREQ("/my/ns", node->get_namespace()); + EXPECT_STREQ("/my/ns", node->get_effective_namespace().c_str()); + EXPECT_STREQ("/my/ns/my_node", node->get_fully_qualified_name()); + } + { + auto node = std::make_shared("my_node", "my/ns"); + EXPECT_STREQ("my_node", node->get_name()); + EXPECT_STREQ("/my/ns", node->get_namespace()); + EXPECT_STREQ("/my/ns", node->get_effective_namespace().c_str()); + EXPECT_STREQ("/my/ns/my_node", node->get_fully_qualified_name()); + } + { + auto node1 = std::make_shared("my_node1", "my/ns"); + auto node2 = std::make_shared("my_node2", "my/ns"); + auto node3 = std::make_shared("my_node3", "/ns2"); + auto node4 = std::make_shared("my_node4", "my/ns3"); + auto names_and_namespaces = node1->get_node_names(); + auto name_namespace_set = std::unordered_set( + names_and_namespaces.begin(), + names_and_namespaces.end()); + std::function Set_Contains = [&](std::string string_key) + { + return name_namespace_set.find(string_key) != name_namespace_set.end(); + }; + EXPECT_TRUE(Set_Contains("/my/ns/my_node1")); + EXPECT_TRUE(Set_Contains("/my/ns/my_node2")); + EXPECT_TRUE(Set_Contains("/ns2/my_node3")); + EXPECT_TRUE(Set_Contains("/my/ns3/my_node4")); + } +} + +TEST_F(TestNode, subnode_get_name_and_namespace) { + { + auto node = std::make_shared("my_node", "ns"); + auto subnode = node->create_sub_node("sub_ns"); + EXPECT_STREQ("my_node", subnode->get_name()); + EXPECT_STREQ("/ns", subnode->get_namespace()); + EXPECT_STREQ("sub_ns", subnode->get_sub_namespace().c_str()); + EXPECT_STREQ("/ns/sub_ns", subnode->get_effective_namespace().c_str()); + } + { + auto node = std::make_shared("my_node", "/ns"); + auto subnode = node->create_sub_node("sub_ns"); + EXPECT_STREQ("my_node", subnode->get_name()); + EXPECT_STREQ("/ns", subnode->get_namespace()); + EXPECT_STREQ("sub_ns", subnode->get_sub_namespace().c_str()); + EXPECT_STREQ("/ns/sub_ns", subnode->get_effective_namespace().c_str()); + auto subnode2 = subnode->create_sub_node("sub_ns2"); + EXPECT_STREQ("my_node", subnode2->get_name()); + EXPECT_STREQ("/ns", subnode2->get_namespace()); + EXPECT_STREQ("sub_ns/sub_ns2", subnode2->get_sub_namespace().c_str()); + EXPECT_STREQ("/ns/sub_ns/sub_ns2", subnode2->get_effective_namespace().c_str()); + } + { + auto node = std::make_shared("my_node"); + auto subnode = node->create_sub_node("sub_ns"); + EXPECT_STREQ("my_node", subnode->get_name()); + EXPECT_STREQ("/", subnode->get_namespace()); + EXPECT_STREQ("sub_ns", subnode->get_sub_namespace().c_str()); + EXPECT_STREQ("/sub_ns", subnode->get_effective_namespace().c_str()); + auto subnode2 = subnode->create_sub_node("sub_ns2"); + EXPECT_STREQ("my_node", subnode2->get_name()); + EXPECT_STREQ("/", subnode2->get_namespace()); + EXPECT_STREQ("sub_ns/sub_ns2", subnode2->get_sub_namespace().c_str()); + EXPECT_STREQ("/sub_ns/sub_ns2", subnode2->get_effective_namespace().c_str()); + } + { + auto node = std::make_shared("my_node"); + ASSERT_THROW( + { + auto subnode = node->create_sub_node("/sub_ns"); + }, rclcpp::exceptions::NameValidationError); + } +} +/* + Testing node construction and destruction. + */ +TEST_F(TestNode, subnode_construction_and_destruction) { + { + ASSERT_NO_THROW( + { + auto node = std::make_shared("my_node", "ns"); + auto subnode = node->create_sub_node("sub_ns"); + }); + } + { + ASSERT_THROW( + { + auto node = std::make_shared("my_node", "ns"); + auto subnode = node->create_sub_node("invalid_ns?"); + }, rclcpp::exceptions::InvalidNamespaceError); + } + { + ASSERT_THROW( + { + auto node = std::make_shared("my_node", "ns/"); + }, rclcpp::exceptions::InvalidNamespaceError); + } + { + ASSERT_THROW( + { + auto node = std::make_shared("my_node", "ns/"); + auto subnode = node->create_sub_node("/sub_ns"); + }, rclcpp::exceptions::InvalidNamespaceError); + } + { + ASSERT_THROW( + { + auto node = std::make_shared("my_node", "ns"); + auto subnode = node->create_sub_node("/sub_ns"); + }, rclcpp::exceptions::NameValidationError); + } + { + ASSERT_THROW( + { + auto node = std::make_shared("my_node", "ns"); + auto subnode = node->create_sub_node("~sub_ns"); + }, rclcpp::exceptions::InvalidNamespaceError); + } + { + ASSERT_THROW( + { + auto node = std::make_shared("my_node", "/ns"); + auto subnode = node->create_sub_node("invalid_ns?"); + }, rclcpp::exceptions::InvalidNamespaceError); + } + { + ASSERT_NO_THROW( + { + auto node = std::make_shared("my_node", "/ns"); + auto subnode = node->create_sub_node("sub_ns"); + }); + } + { + ASSERT_THROW( + { + auto node = std::make_shared("my_node", "/ns"); + auto subnode = node->create_sub_node("/sub_ns"); + }, rclcpp::exceptions::NameValidationError); + } + { + ASSERT_THROW( + { + auto node = std::make_shared("my_node", "/ns"); + auto subnode = node->create_sub_node("~sub_ns"); + }, rclcpp::exceptions::InvalidNamespaceError); + } + { + ASSERT_THROW( + { + auto node = std::make_shared("my_node", "/ns"); + auto subnode = node->create_sub_node(""); + }, rclcpp::exceptions::NameValidationError); + } +} + +TEST_F(TestNode, get_logger) { + { + auto node = std::make_shared("my_node"); + EXPECT_STREQ("my_node", node->get_logger().get_name()); + } + { + auto node = std::make_shared("my_node", "/ns"); + EXPECT_STREQ("ns.my_node", node->get_logger().get_name()); + } + { + auto node = std::make_shared("my_node", "ns"); + EXPECT_STREQ("ns.my_node", node->get_logger().get_name()); + } + { + auto node = std::make_shared("my_node", "/my/ns"); + EXPECT_STREQ("my.ns.my_node", node->get_logger().get_name()); + } + { + auto node = std::make_shared("my_node", "my/ns"); + EXPECT_STREQ("my.ns.my_node", node->get_logger().get_name()); + } +} + +TEST_F(TestNode, get_clock) { + auto node = std::make_shared("my_node", "/ns"); + auto ros_clock = node->get_clock(); + EXPECT_NE(nullptr, ros_clock); + EXPECT_EQ(ros_clock->get_clock_type(), RCL_ROS_TIME); + + const rclcpp::Node & const_node = *node.get(); + EXPECT_NE(nullptr, const_node.get_clock()); +} + +TEST_F(TestNode, now) { + auto node = std::make_shared("my_node", "/ns"); + auto clock = node->get_clock(); + auto now_builtin = node->now().nanoseconds(); + auto now_external = clock->now().nanoseconds(); + EXPECT_GE(now_external, now_builtin); + EXPECT_LT(now_external - now_builtin, 5000000L); +} + +std::string +operator"" _unq(const char * prefix, size_t prefix_length) +{ + static uint64_t count = 0; + return std::string(prefix, prefix_length) + "_" + std::to_string(++count); +} + +TEST_F(TestNode, declare_parameter_with_no_initial_values) { + // test cases without initial values + auto node = std::make_shared("test_declare_parameter_node"_unq); + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.dynamic_typing = true; + // no default, no initial + const std::string parameter_name = "parameter"_unq; + rclcpp::ParameterValue value = node->declare_parameter( + parameter_name, rclcpp::ParameterValue{}, descriptor); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_NOT_SET); + // Does not throw if unset before access + EXPECT_EQ( + rclcpp::PARAMETER_NOT_SET, + node->get_parameter(parameter_name).get_parameter_value().get_type()); + } + { + // int default, no initial + rclcpp::ParameterValue default_value(42); + rclcpp::ParameterValue value = node->declare_parameter("parameter"_unq, default_value); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER); + EXPECT_EQ(value.get(), default_value.get()); + } + { + // int default, no initial, custom parameter descriptor + auto name = "parameter"_unq; + rclcpp::ParameterValue default_value(42); + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.read_only = true; + rclcpp::ParameterValue value = + node->declare_parameter(name, default_value, descriptor); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER); + EXPECT_EQ(value.get(), default_value.get()); + rcl_interfaces::msg::ParameterDescriptor actual_descriptor = + node->describe_parameter(name); + EXPECT_EQ(actual_descriptor.read_only, descriptor.read_only); + } + { + // int default, no initial, implicit template specialization + int default_value = 42; + EXPECT_EQ(node->declare_parameter("parameter"_unq, default_value), default_value); + } + { + // parameter already declared throws + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.dynamic_typing = true; + node->declare_parameter(name, rclcpp::ParameterValue{}, descriptor); + EXPECT_THROW( + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.dynamic_typing = true; + node->declare_parameter(name, rclcpp::ParameterValue{}, descriptor); + }, + rclcpp::exceptions::ParameterAlreadyDeclaredException); + } + { + // parameter name invalid throws + EXPECT_THROW( + {node->declare_parameter("", 5);}, + rclcpp::exceptions::InvalidParametersException); + } + { + // parameter rejected throws + auto name = "parameter"_unq; + auto on_set_parameters = + [&name](const std::vector & parameters) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + for (const auto & parameter : parameters) { + if ( + parameter.get_name() == name && + parameter.get_type() != rclcpp::PARAMETER_INTEGER) + { + result.successful = false; + result.reason = "'" + name + "' must be an integer"; + } + } + return result; + }; + auto handler = node->add_on_set_parameters_callback(on_set_parameters); + RCPPUTILS_SCOPE_EXIT( + {node->remove_on_set_parameters_callback(handler.get());}); // always reset + EXPECT_THROW( + {node->declare_parameter(name, "not an int");}, + rclcpp::exceptions::InvalidParameterValueException); + } +} + +TEST_F(TestNode, declare_parameter_with_allow_undeclared_parameters) { + // test cases without initial values + auto node = std::make_shared( + "test_declare_parameter_node"_unq, "/", + rclcpp::NodeOptions{}.allow_undeclared_parameters(true)); + { + // declared parameters static typing is still enforced + auto param_name = "parameter"_unq; + auto value = node->declare_parameter(param_name, 5); + EXPECT_EQ(value, 5); + EXPECT_FALSE(node->set_parameter({param_name, "asd"}).successful); + auto param = node->get_parameter(param_name); + EXPECT_EQ(param.get_type(), rclcpp::PARAMETER_INTEGER); + EXPECT_EQ(param.get_value(), 5); + } + { + // not for automatically declared parameters + auto param_name = "parameter"_unq; + EXPECT_TRUE(node->set_parameter({param_name, 5}).successful); + auto param = node->get_parameter(param_name); + EXPECT_EQ(param.get_type(), rclcpp::PARAMETER_INTEGER); + EXPECT_EQ(param.get_value(), 5); + EXPECT_TRUE(node->set_parameter({param_name, "asd"}).successful); + param = node->get_parameter(param_name); + EXPECT_EQ(param.get_type(), rclcpp::PARAMETER_STRING); + EXPECT_EQ(param.get_value(), "asd"); + } + { + // declare after set is invalid + auto param_name = "parameter"_unq; + EXPECT_TRUE(node->set_parameter({param_name, 5}).successful); + auto param = node->get_parameter(param_name); + EXPECT_EQ(param.get_type(), rclcpp::PARAMETER_INTEGER); + EXPECT_EQ(param.get_value(), 5); + EXPECT_THROW( + node->declare_parameter(param_name, 5), + rclcpp::exceptions::ParameterAlreadyDeclaredException); + } +} + +auto get_fixed_on_parameter_set_callback(const std::string & name, bool successful) +{ + return + [name, successful](const std::vector & parameters) { + (void)parameters; + rcl_interfaces::msg::SetParametersResult result; + result.successful = successful; + return result; + }; +} + +TEST_F(TestNode, test_registering_multiple_callbacks_api) { + auto node = std::make_shared("test_declare_parameter_node"_unq); + { + int64_t default_value{42}; + auto name1 = "parameter"_unq; + auto scoped_callback1 = node->add_on_set_parameters_callback( + get_fixed_on_parameter_set_callback(name1, true)); + EXPECT_NE(scoped_callback1, nullptr); + int64_t value{node->declare_parameter(name1, default_value)}; + EXPECT_EQ(value, default_value); + + auto name2 = "parameter"_unq; + auto scoped_callback2 = node->add_on_set_parameters_callback( + get_fixed_on_parameter_set_callback(name2, false)); + EXPECT_NE(scoped_callback2, nullptr); + EXPECT_THROW( + {node->declare_parameter(name2, default_value);}, + rclcpp::exceptions::InvalidParameterValueException); + + auto name3 = "parameter"_unq; + scoped_callback2.reset(); + value = node->declare_parameter(name3, default_value); + EXPECT_EQ(value, default_value); + } + { + int64_t default_value{42}; + auto name1 = "parameter"_unq; + auto scoped_callback1 = node->add_on_set_parameters_callback( + get_fixed_on_parameter_set_callback(name1, true)); + EXPECT_NE(scoped_callback1, nullptr); + int64_t value{node->declare_parameter(name1, default_value)}; + EXPECT_EQ(value, default_value); + + auto name2 = "parameter"_unq; + auto scoped_callback2 = node->add_on_set_parameters_callback( + get_fixed_on_parameter_set_callback(name2, false)); + EXPECT_NE(scoped_callback2, nullptr); + EXPECT_THROW( + {node->declare_parameter(name2, default_value);}, + rclcpp::exceptions::InvalidParameterValueException); + + auto name3 = "parameter"_unq; + node->remove_on_set_parameters_callback(scoped_callback2.get()); + value = node->declare_parameter(name3, default_value); + EXPECT_EQ(value, default_value); + } + { + int64_t default_value{42}; + auto name1 = "parameter"_unq; + rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr scoped_callback( + node->add_on_set_parameters_callback( + get_fixed_on_parameter_set_callback(name1, false))); + rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr scoped_callback_copy(scoped_callback); + scoped_callback.reset(); + + EXPECT_THROW( + {node->declare_parameter("parameter"_unq, default_value);}, + rclcpp::exceptions::InvalidParameterValueException); + + scoped_callback_copy.reset(); + // All the shared_ptr has been reset + int64_t value = node->declare_parameter("parameter"_unq, default_value); + EXPECT_EQ(value, default_value); + } +} + +TEST_F(TestNode, declare_parameter_with_overrides) { + // test cases with overrides + rclcpp::NodeOptions no; + no.parameter_overrides( + { + {"parameter_no_default", 42}, + {"parameter_no_default_set", 42}, + {"parameter_no_default_set_cvref", 42}, + {"parameter_and_default", 42}, + {"parameter_and_default_ignore_override", 42}, + {"parameter_custom", 42}, + {"parameter_template", 42}, + {"parameter_already_declared", 42}, + {"parameter_rejected", 42}, + {"parameter_type_mismatch", "not an int"}, + }); + + auto node = std::make_shared("test_declare_parameter_node"_unq, no); + { + // no default, with override + rclcpp::ParameterValue value = node->declare_parameter( + "parameter_no_default", rclcpp::ParameterType::PARAMETER_INTEGER); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER); + EXPECT_EQ(value.get(), 42); + } + { + // no default, with override, and set after + rclcpp::ParameterValue value = node->declare_parameter( + "parameter_no_default_set", rclcpp::ParameterType::PARAMETER_INTEGER); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER); + EXPECT_EQ(value.get(), 42); + // check that the value is changed after a set + node->set_parameter({"parameter_no_default_set", 44}); + EXPECT_EQ(node->get_parameter("parameter_no_default_set").get_value(), 44); + } + { + // no default, with override + const rclcpp::ParameterValue & value = node->declare_parameter( + "parameter_no_default_set_cvref", rclcpp::ParameterType::PARAMETER_INTEGER); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER); + EXPECT_EQ(value.get(), 42); + // check that the value is changed after a set + node->set_parameter({"parameter_no_default_set_cvref", 44}); + EXPECT_EQ(value.get(), 44); + } + { + // int default, with override + rclcpp::ParameterValue default_value(43); + rclcpp::ParameterValue value = node->declare_parameter("parameter_and_default", default_value); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER); + EXPECT_EQ(value.get(), 42); // and not 43 which is the default value + } + { + // int default, with override and ignoring it + rclcpp::ParameterValue default_value(43); + rclcpp::ParameterValue value = node->declare_parameter( + "parameter_and_default_ignore_override", + default_value, + rcl_interfaces::msg::ParameterDescriptor(), + true); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER); + EXPECT_EQ(value.get(), 43); // and not 42, the parameter override is ignored. + } + { + // int default, with initial, custom parameter descriptor + rclcpp::ParameterValue default_value(43); + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.read_only = true; + rclcpp::ParameterValue value = + node->declare_parameter("parameter_custom", default_value, descriptor); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER); + EXPECT_EQ(value.get(), 42); // and not 43 which is the default value + rcl_interfaces::msg::ParameterDescriptor actual_descriptor = + node->describe_parameter("parameter_custom"); + EXPECT_EQ(actual_descriptor.read_only, descriptor.read_only); + } + { + // int default, with initial, implicit template specialization + int default_value = 43; + // is equal to 42, not 43 which is the default value + EXPECT_EQ(node->declare_parameter("parameter_template", default_value), 42); + } + { + // parameter already declared throws + auto name = "parameter_already_declared"; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.dynamic_typing = true; + node->declare_parameter(name, rclcpp::ParameterValue{}, descriptor); + EXPECT_THROW( + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.dynamic_typing = true; + node->declare_parameter(name, rclcpp::ParameterValue{}, descriptor); + }, + rclcpp::exceptions::ParameterAlreadyDeclaredException); + } + { + // parameter name invalid throws + EXPECT_THROW( + {node->declare_parameter("", 5);}, + rclcpp::exceptions::InvalidParametersException); + } + { + // parameter rejected throws, with initial value + auto name = std::string("parameter_rejected"); + auto on_set_parameters = + [&name](const std::vector & parameters) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + for (const auto & parameter : parameters) { + if ( + parameter.get_name() == name && + parameter.get_type() == rclcpp::PARAMETER_INTEGER) + { + if (parameter.get_value() < 43) { + result.successful = false; + result.reason = "'" + name + "' must be an integer and less than 43"; + } + } + } + return result; + }; + auto handler = node->add_on_set_parameters_callback(on_set_parameters); + RCPPUTILS_SCOPE_EXIT( + {node->remove_on_set_parameters_callback(handler.get());}); // always reset + EXPECT_THROW( + {node->declare_parameter(name, 43);}, + rclcpp::exceptions::InvalidParameterValueException); + } + { + // default type and initial value type do not match + EXPECT_THROW( + {node->declare_parameter("parameter_type_mismatch", 42);}, + rclcpp::exceptions::InvalidParameterTypeException); + } + { + // default type and expected type do not match + EXPECT_THROW( + {node->declare_parameter( + "parameter_type_mismatch", rclcpp::ParameterType::PARAMETER_INTEGER);}, + rclcpp::exceptions::InvalidParameterTypeException); + } + { + // statically typed parameter must be initialized + EXPECT_THROW( + {node->declare_parameter("static_and_uninitialized");}, + rclcpp::exceptions::UninitializedStaticallyTypedParameterException); + } + { + // cannot pass an expected type and a descriptor with dynamic_typing=True + rcl_interfaces::msg::ParameterDescriptor descriptor{}; + descriptor.dynamic_typing = true; + EXPECT_THROW( + {node->declare_parameter( + "invalid_argument", rclcpp::ParameterType::PARAMETER_INTEGER, descriptor);}, + std::invalid_argument); + } +} + +TEST_F(TestNode, declare_parameters_with_no_initial_values) { + // test cases without initial values + auto node = std::make_shared("test_declare_parameters_node"_unq); + { + // with namespace, defaults, no custom descriptors, no initial + int64_t bigger_than_int = INT64_MAX - 42; + auto values = node->declare_parameters( + "namespace1", { + {"parameter_a", 42}, + {"parameter_b", 256}, + {"parameter_c", bigger_than_int}, + }); + std::vector expected = {42, 256, bigger_than_int}; + EXPECT_EQ(values, expected); + EXPECT_TRUE(node->has_parameter("namespace1.parameter_a")); + EXPECT_TRUE(node->has_parameter("namespace1.parameter_b")); + EXPECT_FALSE(node->has_parameter("namespace1")); + } + { + // without namespace, defaults, no custom descriptors, no initial + auto values = node->declare_parameters( + "", { + {"parameter_without_ns_a", 42}, + {"parameter_without_ns_b", 256}, + }); + std::vector expected = {42, 256}; + EXPECT_EQ(values, expected); + EXPECT_TRUE(node->has_parameter("parameter_without_ns_a")); + EXPECT_TRUE(node->has_parameter("parameter_without_ns_b")); + } + { + // with namespace, defaults, custom descriptors, no initial + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.read_only = true; + auto values = node->declare_parameters( + "namespace2", { + {"parameter_a", {42, descriptor}}, + {"parameter_b", {256, descriptor}}, + }); + std::vector expected = {42, 256}; + EXPECT_EQ(values, expected); + EXPECT_TRUE(node->has_parameter("namespace2.parameter_a")); + EXPECT_TRUE(node->has_parameter("namespace2.parameter_b")); + EXPECT_FALSE(node->has_parameter("namespace2")); + } + { + // without namespace, defaults, custom descriptors, no initial + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.read_only = true; + auto values = node->declare_parameters( + "", { + {"parameter_without_ns_c", {42, descriptor}}, + {"parameter_without_ns_d", {256, descriptor}}, + }); + std::vector expected = {42, 256}; + EXPECT_EQ(values, expected); + EXPECT_TRUE(node->has_parameter("parameter_without_ns_c")); + EXPECT_TRUE(node->has_parameter("parameter_without_ns_d")); + } + { + // empty parameters + auto values = node->declare_parameters("", {}); + std::vector expected {}; + EXPECT_EQ(values, expected); + } + { + // parameter already declared throws, even with not_set type + auto name = "parameter"_unq; + node->declare_parameter(name, 42); + EXPECT_THROW( + {node->declare_parameters("", {{name, 42}});}, + rclcpp::exceptions::ParameterAlreadyDeclaredException); + } + { + // parameter name invalid throws + EXPECT_THROW( + {node->declare_parameters("", {{"", 42}});}, + rclcpp::exceptions::InvalidParametersException); + } + { + // parameter rejected throws + auto name = "parameter"_unq; + auto on_set_parameters = + [&name](const std::vector & parameters) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + for (const auto & parameter : parameters) { + if ( + parameter.get_name() == name && + parameter.get_type() != rclcpp::PARAMETER_INTEGER) + { + result.successful = false; + result.reason = "'" + name + "' must be an integer"; + } + } + return result; + }; + auto handler = node->add_on_set_parameters_callback(on_set_parameters); + RCPPUTILS_SCOPE_EXIT( + {node->remove_on_set_parameters_callback(handler.get());}); // always reset + EXPECT_THROW( + {node->declare_parameters("", {{name, "not an int"}});}, + rclcpp::exceptions::InvalidParameterValueException); + } +} + +TEST_F(TestNode, declare_parameter_with_cli_overrides) { + const std::string parameters_filepath = ( + test_resources_path / "test_parameters.yaml").string(); + // test cases with overrides + rclcpp::NodeOptions no; + no.arguments( + { + "--ros-args", + "-p", "parameter_bool:=false", + "-p", "parameter_int:=42", + "-p", "parameter_double:=0.42", + "-p", "parameter_string:=foo", + "--params-file", parameters_filepath.c_str(), + "-p", "parameter_bool_array:=[false, true]", + "-p", "parameter_int_array:=[-21, 42]", + "-p", "parameter_double_array:=[-1.0, .42]", + "-p", "parameter_string_array:=[foo, bar]" + }); + + // To match parameters YAML file content, use a well-known node name for this test only. + auto node = std::make_shared("test_declare_parameter_node", no); + { + rclcpp::ParameterValue value = node->declare_parameter( + "parameter_bool", rclcpp::ParameterType::PARAMETER_BOOL); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_BOOL); + EXPECT_EQ(value.get(), true); + } + { + rclcpp::ParameterValue value = node->declare_parameter( + "parameter_int", rclcpp::ParameterType::PARAMETER_INTEGER); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER); + EXPECT_EQ(value.get(), 21); // set to 42 in CLI, overriden by file + } + { + rclcpp::ParameterValue value = node->declare_parameter( + "parameter_double", rclcpp::ParameterType::PARAMETER_DOUBLE); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_DOUBLE); + EXPECT_EQ(value.get(), 0.42); + } + { + rclcpp::ParameterValue value = node->declare_parameter( + "parameter_string", rclcpp::ParameterType::PARAMETER_STRING); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_STRING); + EXPECT_EQ(value.get(), "foo"); + } + { + rclcpp::ParameterValue value = node->declare_parameter( + "parameter_bool_array", rclcpp::ParameterType::PARAMETER_BOOL_ARRAY); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_BOOL_ARRAY); + std::vector expected_value{false, true}; + EXPECT_EQ(value.get>(), expected_value); + } + { + rclcpp::ParameterValue value = node->declare_parameter( + "parameter_int_array", rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER_ARRAY); + std::vector expected_value{-21, 42}; + EXPECT_EQ(value.get>(), expected_value); + } + { + rclcpp::ParameterValue value = node->declare_parameter( + "parameter_double_array", rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_DOUBLE_ARRAY); + std::vector expected_value{-1.0, 0.42}; + EXPECT_EQ(value.get>(), expected_value); + } + { + rclcpp::ParameterValue value = node->declare_parameter( + "parameter_string_array", rclcpp::ParameterType::PARAMETER_STRING_ARRAY); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_STRING_ARRAY); + std::vector expected_value{"foo", "bar"}; + // set to [baz, baz, baz] in file, overriden by CLI + EXPECT_EQ(value.get>(), expected_value); + } +} + +TEST_F(TestNode, undeclare_parameter) { + auto node = std::make_shared("test_undeclare_parameter_node"_unq); + { + // normal use + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.dynamic_typing = true; + node->declare_parameter(name, rclcpp::ParameterValue{}, descriptor); + EXPECT_TRUE(node->has_parameter(name)); + node->undeclare_parameter(name); + EXPECT_FALSE(node->has_parameter(name)); + } + { + // not declared throws + auto name = "parameter"_unq; + EXPECT_FALSE(node->has_parameter(name)); + EXPECT_THROW( + {node->undeclare_parameter(name);}, + rclcpp::exceptions::ParameterNotDeclaredException); + } + { + // statically typed parameter throws + auto name = "parameter"_unq; + node->declare_parameter(name, 42); + EXPECT_TRUE(node->has_parameter(name)); + EXPECT_THROW( + {node->undeclare_parameter(name);}, + rclcpp::exceptions::InvalidParameterTypeException); + EXPECT_TRUE(node->has_parameter(name)); + } + { + // read only parameter throws + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.read_only = true; + node->declare_parameter(name, 42, descriptor); + EXPECT_TRUE(node->has_parameter(name)); + EXPECT_THROW( + {node->undeclare_parameter(name);}, + rclcpp::exceptions::ParameterImmutableException); + EXPECT_TRUE(node->has_parameter(name)); + } +} + +TEST_F(TestNode, has_parameter) { + auto node = std::make_shared("test_has_parameter_node"_unq); + // normal use + auto name = "parameter"_unq; + EXPECT_FALSE(node->has_parameter(name)); + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.dynamic_typing = true; + node->declare_parameter(name, rclcpp::ParameterValue{}, descriptor); + EXPECT_TRUE(node->has_parameter(name)); + node->undeclare_parameter(name); + EXPECT_FALSE(node->has_parameter(name)); +} + +TEST_F(TestNode, list_parameters) { + auto node = std::make_shared("test_list_parameter_node"_unq); + // normal use + auto name = "parameter"_unq; + const size_t before_size = node->list_parameters({}, 1u).names.size(); + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.dynamic_typing = true; + node->declare_parameter(name, rclcpp::ParameterValue{}, descriptor); + EXPECT_EQ(1u + before_size, node->list_parameters({}, 1u).names.size()); + node->undeclare_parameter(name); + EXPECT_EQ(before_size, node->list_parameters({}, 1u).names.size()); +} + +TEST_F(TestNode, set_parameter_undeclared_parameters_not_allowed) { + auto node = std::make_shared( + "test_set_parameter_node"_unq, + rclcpp::NodeOptions().allow_undeclared_parameters(false)); + { + // normal use + auto name = "parameter"_unq; + EXPECT_FALSE(node->has_parameter(name)); + node->declare_parameter(name, 42); + EXPECT_TRUE(node->has_parameter(name)); + EXPECT_EQ(node->get_parameter(name).get_value(), 42); + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 43)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 43); + } + { + // normal use, change type not allowed + auto name = "parameter"_unq; + EXPECT_FALSE(node->has_parameter(name)); + node->declare_parameter(name, 42); + EXPECT_TRUE(node->has_parameter(name)); + EXPECT_EQ(node->get_parameter(name).get_value(), 42); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, "not an integer")).successful); + } + { + // normal use, change type + auto name = "parameter"_unq; + EXPECT_FALSE(node->has_parameter(name)); + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.dynamic_typing = true; + node->declare_parameter(name, 42, descriptor); + EXPECT_TRUE(node->has_parameter(name)); + EXPECT_EQ(node->get_parameter(name).get_value(), 42); + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, "not an integer")).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), std::string("not an integer")); + } + { + // normal use, multiple parameters + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + auto name3 = "parameter"_unq; + node->declare_parameter(name1, 42); + EXPECT_TRUE(node->has_parameter(name1)); + node->declare_parameter(name2, true); + EXPECT_TRUE(node->has_parameter(name2)); + node->declare_parameter(name3, "something"); + EXPECT_TRUE(node->has_parameter(name3)); + + EXPECT_EQ(node->get_parameter(name1).get_value(), 42); + + EXPECT_EQ(node->get_parameter(name2).get_value(), true); + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name2, false)).successful); + EXPECT_EQ(node->get_parameter(name2).get_value(), false); + } + { + // setting an undeclared parameter throws + auto name = "parameter"_unq; + EXPECT_FALSE(node->has_parameter(name)); + EXPECT_THROW( + {node->set_parameter(rclcpp::Parameter(name, 42));}, + rclcpp::exceptions::ParameterNotDeclaredException); + EXPECT_FALSE(node->has_parameter(name)); + } + { + // rejecting parameter does not throw, but fails + auto name = "parameter"_unq; + node->declare_parameter(name, 42); + + auto on_set_parameters = + [](const std::vector &) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = false; + result.reason = "no parameter may not be set right now"; + return result; + }; + auto handler = node->add_on_set_parameters_callback(on_set_parameters); + RCPPUTILS_SCOPE_EXIT( + {node->remove_on_set_parameters_callback(handler.get());}); // always reset + + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 43)).successful); + } + { + // setting type of rclcpp::PARAMETER_NOT_SET, when already not set, does not undeclare + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.dynamic_typing = true; + auto value = node->declare_parameter(name, rclcpp::ParameterValue{}, descriptor); + EXPECT_TRUE(node->has_parameter(name)); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_NOT_SET); + + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name)).successful); + + EXPECT_TRUE(node->has_parameter(name)); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_NOT_SET); + } + { + // setting type of rclcpp::PARAMETER_NOT_SET, when already to another type, will fail + auto name = "parameter"_unq; + node->declare_parameter(name, 42); + EXPECT_TRUE(node->has_parameter(name)); + auto value = node->get_parameter(name); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER); + + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name)).successful); + } + { + // setting type of rclcpp::PARAMETER_NOT_SET, + // when dynamic typing is allowing and already set to another type, will undeclare + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.dynamic_typing = true; + node->declare_parameter(name, 42, descriptor); + EXPECT_TRUE(node->has_parameter(name)); + auto value = node->get_parameter(name); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER); + + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name)).successful); + + EXPECT_FALSE(node->has_parameter(name)); + } + { + // setting a parameter with integer range descriptor + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.integer_range.resize(1); + auto & integer_range = descriptor.integer_range.at(0); + integer_range.from_value = 10; + integer_range.to_value = 18; + integer_range.step = 2; + node->declare_parameter(name, 10, descriptor); + EXPECT_TRUE(node->has_parameter(name)); + auto value = node->get_parameter(name); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER); + EXPECT_EQ(value.get_value(), 10); + + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 14)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 14); + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 18)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 18); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 15)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 18); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 20)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 18); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 8)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 18); + } + { + // setting a parameter with integer range descriptor, from_value > to_value + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.integer_range.resize(1); + auto & integer_range = descriptor.integer_range.at(0); + integer_range.from_value = 20; + integer_range.to_value = 18; + integer_range.step = 1; + node->declare_parameter(name, 20, descriptor); + EXPECT_TRUE(node->has_parameter(name)); + auto value = node->get_parameter(name); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER); + EXPECT_EQ(value.get_value(), 20); + + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 18)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 18); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 19)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 18); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 10)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 18); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 25)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 18); + } + { + // setting a parameter with integer range descriptor, from_value = to_value + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.integer_range.resize(1); + auto & integer_range = descriptor.integer_range.at(0); + integer_range.from_value = 18; + integer_range.to_value = 18; + integer_range.step = 1; + node->declare_parameter(name, 18, descriptor); + EXPECT_TRUE(node->has_parameter(name)); + auto value = node->get_parameter(name); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER); + EXPECT_EQ(value.get_value(), 18); + + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 17)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 18); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 19)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 18); + } + { + // setting a parameter with integer range descriptor, step > distance(from_value, to_value) + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.integer_range.resize(1); + auto & integer_range = descriptor.integer_range.at(0); + integer_range.from_value = 18; + integer_range.to_value = 25; + integer_range.step = 10; + node->declare_parameter(name, 18, descriptor); + EXPECT_TRUE(node->has_parameter(name)); + auto value = node->get_parameter(name); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER); + EXPECT_EQ(value.get_value(), 18); + + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 25)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 25); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 17)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 25); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 19)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 25); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 26)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 25); + } + { + // setting a parameter with integer range descriptor, distance not multiple of the step. + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.integer_range.resize(1); + auto & integer_range = descriptor.integer_range.at(0); + integer_range.from_value = 18; + integer_range.to_value = 28; + integer_range.step = 7; + node->declare_parameter(name, 18, descriptor); + EXPECT_TRUE(node->has_parameter(name)); + auto value = node->get_parameter(name); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER); + EXPECT_EQ(value.get_value(), 18); + + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 28)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 28); + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 25)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 25); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 17)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 25); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 19)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 25); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 32)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 25); + } + { + // setting a parameter with integer range descriptor, step=0 + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.integer_range.resize(1); + auto & integer_range = descriptor.integer_range.at(0); + integer_range.from_value = 10; + integer_range.to_value = 18; + integer_range.step = 0; + node->declare_parameter(name, 10, descriptor); + EXPECT_TRUE(node->has_parameter(name)); + auto value = node->get_parameter(name); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER); + EXPECT_EQ(value.get_value(), 10); + + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 11)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11); + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 15)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 15); + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 18)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 18); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 9)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 18); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 19)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 18); + } + { + // setting a parameter with integer range descriptor and wrong default value will throw + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.integer_range.resize(1); + auto & integer_range = descriptor.integer_range.at(0); + integer_range.from_value = 10; + integer_range.to_value = 18; + integer_range.step = 2; + ASSERT_THROW( + node->declare_parameter(name, 42, descriptor), + rclcpp::exceptions::InvalidParameterValueException); + } + { + // setting a parameter with floating point range descriptor + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.floating_point_range.resize(1); + auto & floating_point_range = descriptor.floating_point_range.at(0); + floating_point_range.from_value = 10.0; + floating_point_range.to_value = 11.0; + floating_point_range.step = 0.2; + node->declare_parameter(name, 10.0, descriptor); + EXPECT_TRUE(node->has_parameter(name)); + auto value = node->get_parameter(name); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_DOUBLE); + EXPECT_EQ(value.get_value(), 10.0); + + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 10.2)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 10.2); + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 11.0)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 11.3)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 12.0)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 9.4)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + } + { + // setting a parameter with floating point range descriptor, negative step + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.floating_point_range.resize(1); + auto & floating_point_range = descriptor.floating_point_range.at(0); + floating_point_range.from_value = 10.0; + floating_point_range.to_value = 11.0; + floating_point_range.step = -0.2; + node->declare_parameter(name, 10.0, descriptor); + EXPECT_TRUE(node->has_parameter(name)); + auto value = node->get_parameter(name); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_DOUBLE); + EXPECT_EQ(value.get_value(), 10.0); + + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 10.2)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 10.2); + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 11.0)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 11.3)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 12.0)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 9.4)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + } + { + // setting a parameter with floating point range descriptor, from_value > to_value + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.floating_point_range.resize(1); + auto & floating_point_range = descriptor.floating_point_range.at(0); + floating_point_range.from_value = 11.0; + floating_point_range.to_value = 10.0; + floating_point_range.step = 0.2; + node->declare_parameter(name, 10.0, descriptor); + EXPECT_TRUE(node->has_parameter(name)); + auto value = node->get_parameter(name); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_DOUBLE); + EXPECT_EQ(value.get_value(), 10.0); + + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 11.0)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 11.2)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 12.0)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 9.4)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + } + { + // setting a parameter with floating point range descriptor, from_value = to_value + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.floating_point_range.resize(1); + auto & floating_point_range = descriptor.floating_point_range.at(0); + floating_point_range.from_value = 10.0; + floating_point_range.to_value = 10.0; + floating_point_range.step = 0.2; + node->declare_parameter(name, 10.0, descriptor); + EXPECT_TRUE(node->has_parameter(name)); + auto value = node->get_parameter(name); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_DOUBLE); + EXPECT_EQ(value.get_value(), 10.0); + + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 11.2)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 10.0); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 12.0)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 10.0); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 9.4)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 10.0); + } + { + // setting a parameter with floating point range descriptor, step > distance + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.floating_point_range.resize(1); + auto & floating_point_range = descriptor.floating_point_range.at(0); + floating_point_range.from_value = 10.0; + floating_point_range.to_value = 11.0; + floating_point_range.step = 2.2; + node->declare_parameter(name, 10.0, descriptor); + EXPECT_TRUE(node->has_parameter(name)); + auto value = node->get_parameter(name); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_DOUBLE); + EXPECT_EQ(value.get_value(), 10.0); + + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 11.0)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 12.2)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 7.8)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + } + { + // setting a parameter with floating point range descriptor, distance not multiple of the step. + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.floating_point_range.resize(1); + auto & floating_point_range = descriptor.floating_point_range.at(0); + floating_point_range.from_value = 10.0; + floating_point_range.to_value = 11.0; + floating_point_range.step = 0.7; + node->declare_parameter(name, 10.0, descriptor); + EXPECT_TRUE(node->has_parameter(name)); + auto value = node->get_parameter(name); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_DOUBLE); + EXPECT_EQ(value.get_value(), 10.0); + + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 11.0)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 10.7)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 10.7); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 11.4)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 10.7); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 9.3)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 10.7); + } + { + // setting a parameter with floating point range descriptor, step=0 + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.floating_point_range.resize(1); + auto & floating_point_range = descriptor.floating_point_range.at(0); + floating_point_range.from_value = 10.0; + floating_point_range.to_value = 11.0; + floating_point_range.step = 0.0; + node->declare_parameter(name, 10.0, descriptor); + EXPECT_TRUE(node->has_parameter(name)); + auto value = node->get_parameter(name); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_DOUBLE); + EXPECT_EQ(value.get_value(), 10.0); + + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 10.0001)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 10.0001); + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 10.5479051)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 10.5479051); + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 11.0)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 11.001)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 9.999)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 11.0); + } + { + // setting a parameter with a different type is still possible + // when having a descriptor specifying a type (type is a status, not a constraint). + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rclcpp::PARAMETER_INTEGER; + node->declare_parameter(name, "asd", descriptor); + EXPECT_TRUE(node->has_parameter(name)); + auto value = node->get_parameter(name); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_STRING); + EXPECT_EQ(value.get_value(), "asd"); + } + { + // adding a parameter in "pre set parameter" callback, when that + // parameter has not been declared before will throw + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + auto default_value = 0; // default value of name1 param + + // declare name1 parameter only + node->declare_parameter(name1, default_value); + + // add undeclared parameter with name2 to modified list of parameters + auto pre_set_parameters = + [&](std::vector & parameters) { + for (const auto & param : parameters) { + if (param.get_name() == name1) { + parameters.emplace_back(rclcpp::Parameter(name2, 2)); + } + } + }; + + auto handler = node->add_pre_set_parameters_callback(pre_set_parameters); + EXPECT_THROW( + node->set_parameter(rclcpp::Parameter(name1, 4)), + rclcpp::exceptions::ParameterNotDeclaredException); + EXPECT_TRUE(node->has_parameter(name1)); + EXPECT_EQ(node->get_parameter(name1).get_value(), default_value); + EXPECT_FALSE(node->has_parameter(name2)); + RCPPUTILS_SCOPE_EXIT( + {node->remove_pre_set_parameters_callback(handler.get());}); // always reset + } +} + +TEST_F(TestNode, set_parameter_undeclared_parameters_allowed) { + rclcpp::NodeOptions no; + no.parameter_overrides( + { + {"parameter_with_override", 30}, + }); + no.allow_undeclared_parameters(true); + auto node = std::make_shared("test_set_parameter_node"_unq, no); + { + // overrides are ignored when not declaring a parameter + auto name = "parameter_with_override"; + EXPECT_FALSE(node->has_parameter(name)); + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 43)).successful); + EXPECT_TRUE(node->has_parameter(name)); + EXPECT_EQ(node->get_parameter(name).get_value(), 43); + } + { + // normal use (declare first) still works with this true + auto name = "parameter"_unq; + EXPECT_FALSE(node->has_parameter(name)); + node->declare_parameter(name, 42); + EXPECT_TRUE(node->has_parameter(name)); + EXPECT_EQ(node->get_parameter(name).get_value(), 42); + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 43)).successful); + EXPECT_EQ(node->get_parameter(name).get_value(), 43); + } + { + // setting a parameter that is not declared implicitly declares it + auto name = "parameter"_unq; + EXPECT_FALSE(node->has_parameter(name)); + + EXPECT_TRUE(node->set_parameter(rclcpp::Parameter(name, 43)).successful); + EXPECT_TRUE(node->has_parameter(name)); + EXPECT_EQ(node->get_parameter(name).get_value(), 43); + } + { + // adding a parameter in "pre set parameter" callback, when that + // parameter has not been declared will not throw if undeclared + // parameters are allowed + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + + // declare name1 parameter only + node->declare_parameter(name1, 0); + + // add undeclared parameter with name2 to modified list of parameters + auto pre_set_parameters = + [&](std::vector & parameters) { + for (const auto & param : parameters) { + if (param.get_name() == name1) { + parameters.emplace_back(rclcpp::Parameter(name2, 2)); + } + } + }; + + auto handler = node->add_pre_set_parameters_callback(pre_set_parameters); + auto result = node->set_parameter(rclcpp::Parameter(name1, 1)); + EXPECT_TRUE(result.successful); + EXPECT_TRUE(node->has_parameter(name1)); + EXPECT_TRUE(node->has_parameter(name2)); + EXPECT_EQ(node->get_parameter(name1).get_value(), 1); + EXPECT_EQ(node->get_parameter(name2).get_value(), 2); + RCPPUTILS_SCOPE_EXIT( + {node->remove_pre_set_parameters_callback(handler.get());}); // always reset + } +} + +TEST_F(TestNode, set_parameters_undeclared_parameters_not_allowed) { + auto node = std::make_shared( + "test_set_parameters_node"_unq, + rclcpp::NodeOptions().allow_undeclared_parameters(false)); + { + // normal use + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + auto name3 = "parameter"_unq; + node->declare_parameter(name1, 1); + node->declare_parameter(name2, true); + node->declare_parameter(name3, "blue"); + + auto rets = node->set_parameters( + { + {name1, 2}, + {name2, false}, + {name3, "red"}, + }); + EXPECT_TRUE(std::all_of(rets.begin(), rets.end(), [](auto & r) {return r.successful;})); + EXPECT_TRUE(node->has_parameter(name1)); + EXPECT_TRUE(node->has_parameter(name2)); + EXPECT_TRUE(node->has_parameter(name3)); + } + { + // overwrite and order of setting + auto name = "parameter"_unq; + node->declare_parameter(name, 1); + + auto rets = node->set_parameters( + { + {name, 42}, + {name, 2}, + }); + EXPECT_TRUE(std::all_of(rets.begin(), rets.end(), [](auto & r) {return r.successful;})); + EXPECT_TRUE(node->has_parameter(name)); + EXPECT_EQ(node->get_parameter(name).get_value(), 2); + } + { + // undeclared parameter throws, + // and preceding parameters are still set, but proceeding values are not + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + auto name3 = "parameter"_unq; + node->declare_parameter(name1, 1); + node->declare_parameter(name3, 100); + + EXPECT_THROW( + { + node->set_parameters( + { + {name1, 2}, + {name2, "not declared :("}, + {name3, 101}, + }); + }, + rclcpp::exceptions::ParameterNotDeclaredException); + + EXPECT_TRUE(node->has_parameter(name1)); + EXPECT_FALSE(node->has_parameter(name2)); + EXPECT_TRUE(node->has_parameter(name3)); + EXPECT_EQ(node->get_parameter(name1).get_value(), 2); + EXPECT_EQ(node->get_parameter(name3).get_value(), 100); + } + { + // rejecting parameter does not throw, but fails + // all parameters to be set are attempted to be set + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + auto name3 = "parameter"_unq; + node->declare_parameter(name1, 1); + node->declare_parameter(name2, true); + node->declare_parameter(name3, "blue"); + + auto on_set_parameters = + [&name2](const std::vector & ps) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + if (std::any_of(ps.begin(), ps.end(), [&](auto & p) {return p.get_name() == name2;})) { + result.successful = false; + result.reason = "parameter '" + name2 + "' may not be set right now"; + } + return result; + }; + auto handler = node->add_on_set_parameters_callback(on_set_parameters); + RCPPUTILS_SCOPE_EXIT( + {node->remove_on_set_parameters_callback(handler.get());}); // always reset + + auto rets = node->set_parameters( + { + {name1, 2}, + {name2, false}, + {name3, "red"}, + }); + EXPECT_EQ(rets.size(), 3U); + EXPECT_TRUE(rets[0].successful); + EXPECT_FALSE(rets[1].successful); + EXPECT_NE(rets[1].reason.find("may not be set right now"), std::string::npos); + EXPECT_TRUE(rets[2].successful); + EXPECT_EQ(node->get_parameter(name1).get_value(), 2); + EXPECT_EQ(node->get_parameter(name2).get_value(), true); // old value + EXPECT_EQ(node->get_parameter(name3).get_value(), "red"); + } + { + // setting type of rclcpp::PARAMETER_NOT_SET, when already not set, does not undeclare + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.dynamic_typing = true; + auto value = node->declare_parameter(name, rclcpp::ParameterValue{}, descriptor); + EXPECT_TRUE(node->has_parameter(name)); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_NOT_SET); + + EXPECT_TRUE(node->set_parameters({rclcpp::Parameter(name)})[0].successful); + + EXPECT_TRUE(node->has_parameter(name)); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_NOT_SET); + } + { + // setting type of rclcpp::PARAMETER_NOT_SET, when already to another type, will fail + auto name = "parameter"_unq; + node->declare_parameter(name, 42); + EXPECT_TRUE(node->has_parameter(name)); + auto value = node->get_parameter(name); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER); + + EXPECT_FALSE(node->set_parameters({rclcpp::Parameter(name)})[0].successful); + } + { + // setting type of rclcpp::PARAMETER_NOT_SET, + // when already to another type and dynamic typic allowed, will undeclare + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.dynamic_typing = true; + node->declare_parameter(name, 42, descriptor); + EXPECT_TRUE(node->has_parameter(name)); + auto value = node->get_parameter(name); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER); + + EXPECT_TRUE(node->set_parameters({rclcpp::Parameter(name)})[0].successful); + + EXPECT_FALSE(node->has_parameter(name)); + } + { + // adding a parameter in "pre set parameter" callback when that + // parameter has not been declared before will throw. However, when + // multiple params are being set using "set_parameters", the params + // which are not conditioned on each other in "pre set callback" will + // still be set successfully. This is the desired behaviour since + // "set_parameters" sets params non atomically. + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + auto name3 = "parameter"_unq; + auto default_value = 0; + + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rclcpp::PARAMETER_INTEGER; + + // declare name1 and name2 parameter only + node->declare_parameter(name1, default_value, descriptor); + node->declare_parameter(name2, default_value, descriptor); + + // add undeclared parameter with name3 to modified list of parameters + // conditioned of name2 param + auto pre_set_parameters = + [&](std::vector & parameters) { + for (const auto & param : parameters) { + if (param.get_name() == name2) { + parameters.emplace_back(rclcpp::Parameter(name3, 3)); + } + } + }; + + auto handler = node->add_pre_set_parameters_callback(pre_set_parameters); + EXPECT_THROW( + node->set_parameters({rclcpp::Parameter(name1, 1), rclcpp::Parameter(name2, 2)}), + rclcpp::exceptions::ParameterNotDeclaredException); + EXPECT_TRUE(node->has_parameter(name1)); + EXPECT_TRUE(node->has_parameter(name2)); + EXPECT_FALSE(node->has_parameter(name3)); + + // we still expect the value of name1 param to be set successfully, since + // the setting of name2 param is only conditioned on setting of name3 param + EXPECT_EQ(node->get_parameter(name1).get_value(), 1); + EXPECT_EQ(node->get_parameter(name2).get_value(), default_value); + RCPPUTILS_SCOPE_EXIT( + {node->remove_pre_set_parameters_callback(handler.get());}); // always reset + } +} + +// test set_parameters with undeclared allowed +TEST_F(TestNode, set_parameters_undeclared_parameters_allowed) { + auto node = std::make_shared( + "test_set_parameters_node"_unq, + rclcpp::NodeOptions().allow_undeclared_parameters(true)); + { + // normal use (declare first) still works with this true + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + + EXPECT_FALSE(node->has_parameter(name1)); + EXPECT_FALSE(node->has_parameter(name2)); + + node->declare_parameter(name1, 42); + EXPECT_TRUE(node->has_parameter(name1)); + EXPECT_EQ(node->get_parameter(name1).get_value(), 42); + node->declare_parameter(name2, "test"); + EXPECT_TRUE(node->has_parameter(name2)); + EXPECT_EQ(node->get_parameter(name2).get_value(), "test"); + + auto rets = node->set_parameters( + { + rclcpp::Parameter(name1, 43), + rclcpp::Parameter(name2, "other"), + }); + EXPECT_TRUE(std::all_of(rets.begin(), rets.end(), [](auto & r) {return r.successful;})); + EXPECT_EQ(node->get_parameter(name1).get_value(), 43); + EXPECT_EQ(node->get_parameter(name2).get_value(), "other"); + } + { + // setting a parameter that is not declared implicitly declares it + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + + EXPECT_FALSE(node->has_parameter(name1)); + EXPECT_FALSE(node->has_parameter(name2)); + + auto rets = node->set_parameters( + { + rclcpp::Parameter(name1, 42), + rclcpp::Parameter(name2, "test"), + }); + EXPECT_TRUE(std::all_of(rets.begin(), rets.end(), [](auto & r) {return r.successful;})); + EXPECT_EQ(node->get_parameter(name1).get_value(), 42); + EXPECT_EQ(node->get_parameter(name2).get_value(), "test"); + } + { + // adding a parameter in "pre set parameter" callback when that + // parameter has not been declared before will not throw when + // undeclared parameters are allowed. + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + auto name3 = "parameter"_unq; + auto default_value = 0; + + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rclcpp::PARAMETER_INTEGER; + + // declare name1 and name2 parameter only + node->declare_parameter(name1, default_value, descriptor); + node->declare_parameter(name2, default_value, descriptor); + + // add undeclared parameter with name3 to modified list of parameters + // conditioned of name2 param + auto pre_set_parameters = + [&](std::vector & parameters) { + for (const auto & param : parameters) { + if (param.get_name() == name2) { + parameters.emplace_back(rclcpp::Parameter(name3, 3)); + } + } + }; + + auto handler = node->add_pre_set_parameters_callback(pre_set_parameters); + auto results = node->set_parameters({rclcpp::Parameter(name1, 1), rclcpp::Parameter(name2, 2)}); + EXPECT_EQ(2u, results.size()); + EXPECT_TRUE(results[0].successful); + EXPECT_TRUE(results[1].successful); + EXPECT_TRUE(node->has_parameter(name1)); + EXPECT_TRUE(node->has_parameter(name2)); + EXPECT_TRUE(node->has_parameter(name3)); + EXPECT_EQ(node->get_parameter(name1).get_value(), 1); + EXPECT_EQ(node->get_parameter(name2).get_value(), 2); + EXPECT_EQ(node->get_parameter(name3).get_value(), 3); + + RCPPUTILS_SCOPE_EXIT( + {node->remove_pre_set_parameters_callback(handler.get());}); // always reset + } +} + +TEST_F(TestNode, set_parameters_atomically_undeclared_parameters_not_allowed) { + auto node = std::make_shared( + "test_set_parameters_atomically_node"_unq, + rclcpp::NodeOptions().allow_undeclared_parameters(false)); + { + // normal use + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + auto name3 = "parameter"_unq; + node->declare_parameter(name1, 1); + node->declare_parameter(name2, true); + node->declare_parameter(name3, "blue"); + + auto ret = node->set_parameters_atomically( + { + {name1, 2}, + {name2, false}, + {name3, "red"}, + }); + EXPECT_TRUE(ret.successful); + EXPECT_TRUE(node->has_parameter(name1)); + EXPECT_TRUE(node->has_parameter(name2)); + EXPECT_TRUE(node->has_parameter(name3)); + } + { + // overwrite and order of setting + auto name = "parameter"_unq; + node->declare_parameter(name, 1); + + auto ret = node->set_parameters_atomically( + { + {name, 42}, + {name, 2}, + }); + EXPECT_TRUE(ret.successful); + EXPECT_TRUE(node->has_parameter(name)); + EXPECT_EQ(node->get_parameter(name).get_value(), 2); + } + { + // undeclared parameter throws, + // and no parameters were changed + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + auto name3 = "parameter"_unq; + node->declare_parameter(name1, 1); + node->declare_parameter(name3, 100); + + EXPECT_THROW( + { + node->set_parameters_atomically( + { + {name1, 2}, + {name2, "not declared :("}, + {name3, 101}, + }); + }, + rclcpp::exceptions::ParameterNotDeclaredException); + + EXPECT_TRUE(node->has_parameter(name1)); + EXPECT_FALSE(node->has_parameter(name2)); + EXPECT_TRUE(node->has_parameter(name3)); + // both have old values + EXPECT_EQ(node->get_parameter(name1).get_value(), 1); + EXPECT_EQ(node->get_parameter(name3).get_value(), 100); + } + { + // rejecting parameter does not throw, but fails + // and no parameters are changed + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + auto name3 = "parameter"_unq; + node->declare_parameter(name1, 1); + node->declare_parameter(name2, true); + node->declare_parameter(name3, "blue"); + + auto on_set_parameters = + [&name2](const std::vector & ps) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + if (std::any_of(ps.begin(), ps.end(), [&](auto & p) {return p.get_name() == name2;})) { + result.successful = false; + result.reason = "parameter '" + name2 + "' may not be set right now"; + } + return result; + }; + auto handler = node->add_on_set_parameters_callback(on_set_parameters); + RCPPUTILS_SCOPE_EXIT( + {node->remove_on_set_parameters_callback(handler.get());}); // always reset + + auto ret = node->set_parameters_atomically( + { + {name1, 2}, + {name2, false}, // should fail to be set, failing the whole operation + {name3, "red"}, + }); + EXPECT_FALSE(ret.successful); + // all have old values + EXPECT_EQ(node->get_parameter(name1).get_value(), 1); + EXPECT_EQ(node->get_parameter(name2).get_value(), true); + EXPECT_EQ(node->get_parameter(name3).get_value(), "blue"); + } + { + // setting type of rclcpp::PARAMETER_NOT_SET, when already not set, does not undeclare + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.dynamic_typing = true; + auto value = node->declare_parameter(name, rclcpp::ParameterValue{}, descriptor); + EXPECT_TRUE(node->has_parameter(name)); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_NOT_SET); + + EXPECT_TRUE(node->set_parameters_atomically({rclcpp::Parameter(name)}).successful); + + EXPECT_TRUE(node->has_parameter(name)); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_NOT_SET); + } + { + // setting type of rclcpp::PARAMETER_NOT_SET, when already to another type, will fail + auto name = "parameter"_unq; + node->declare_parameter(name, 42); + EXPECT_TRUE(node->has_parameter(name)); + auto value = node->get_parameter(name); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER); + + EXPECT_FALSE(node->set_parameters_atomically({rclcpp::Parameter(name)}).successful); + } + { + // setting type of rclcpp::PARAMETER_NOT_SET, + // when dynamic typing is allowed and already declared to another type, will undeclare + auto name = "parameter"_unq; + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.dynamic_typing = true; + node->declare_parameter(name, 42, descriptor); + EXPECT_TRUE(node->has_parameter(name)); + auto value = node->get_parameter(name); + EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_INTEGER); + + EXPECT_TRUE(node->set_parameters_atomically({rclcpp::Parameter(name)}).successful); + + EXPECT_FALSE(node->has_parameter(name)); + } + { + // adding a parameter in "pre set parameter" callback when that + // parameter has not been declared before will throw and since + // multiple params are being set using "set_parameters_atomically", + // a failure in set of one param will result in all params being + // set unsuccessfully. + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + auto name3 = "parameter"_unq; + auto default_value = 0; + + // declare name1 and name2 parameter only + node->declare_parameter(name1, default_value); + node->declare_parameter(name2, default_value); + + // add undeclared parameter with name3 to modified list of parameters + // conditioned of name2 param + auto pre_set_parameters = + [&](std::vector & parameters) { + for (const auto & param : parameters) { + if (param.get_name() == name2) { + parameters.emplace_back(rclcpp::Parameter(name3, 3)); + } + } + }; + + auto handler = node->add_pre_set_parameters_callback(pre_set_parameters); + EXPECT_THROW( + node->set_parameters_atomically( + {rclcpp::Parameter(name1, 1), + rclcpp::Parameter(name2, 2)}), + rclcpp::exceptions::ParameterNotDeclaredException); + EXPECT_TRUE(node->has_parameter(name1)); + EXPECT_TRUE(node->has_parameter(name2)); + EXPECT_FALSE(node->has_parameter(name3)); + + // the values of all the params is still default. + EXPECT_EQ(node->get_parameter(name1).get_value(), default_value); + EXPECT_EQ(node->get_parameter(name2).get_value(), default_value); + RCPPUTILS_SCOPE_EXIT( + {node->remove_pre_set_parameters_callback(handler.get());}); // always reset + } +} + +// test set_parameters with undeclared allowed +TEST_F(TestNode, set_parameters_atomically_undeclared_parameters_allowed) { + auto node = std::make_shared( + "test_set_parameters_atomically_node"_unq, + rclcpp::NodeOptions().allow_undeclared_parameters(true)); + { + // normal use (declare first) still works with this true + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + + EXPECT_FALSE(node->has_parameter(name1)); + EXPECT_FALSE(node->has_parameter(name2)); + + node->declare_parameter(name1, 42); + EXPECT_TRUE(node->has_parameter(name1)); + EXPECT_EQ(node->get_parameter(name1).get_value(), 42); + node->declare_parameter(name2, "test"); + EXPECT_TRUE(node->has_parameter(name2)); + EXPECT_EQ(node->get_parameter(name2).get_value(), "test"); + + auto ret = node->set_parameters_atomically( + { + rclcpp::Parameter(name1, 43), + rclcpp::Parameter(name2, "other"), + }); + EXPECT_TRUE(ret.successful); + EXPECT_EQ(node->get_parameter(name1).get_value(), 43); + EXPECT_EQ(node->get_parameter(name2).get_value(), "other"); + } + { + // setting a parameter that is not declared implicitly declares it + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + + EXPECT_FALSE(node->has_parameter(name1)); + EXPECT_FALSE(node->has_parameter(name2)); + + auto ret = node->set_parameters_atomically( + { + rclcpp::Parameter(name1, 42), + rclcpp::Parameter(name2, "test"), + }); + EXPECT_TRUE(ret.successful); + EXPECT_EQ(node->get_parameter(name1).get_value(), 42); + EXPECT_EQ(node->get_parameter(name2).get_value(), "test"); + } + { + // if an undeclared parameter is implicitly declared, but a later parameter set fails, + // then the implicitly "to be" declared parameter remains undeclared + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + auto name3 = "parameter"_unq; + + node->declare_parameter(name1, 42); + EXPECT_TRUE(node->has_parameter(name1)); + EXPECT_EQ(node->get_parameter(name1).get_value(), 42); + node->declare_parameter(name3, "test"); + EXPECT_TRUE(node->has_parameter(name3)); + EXPECT_EQ(node->get_parameter(name3).get_value(), "test"); + + auto on_set_parameters = + [&name3](const std::vector & ps) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + if (std::any_of(ps.begin(), ps.end(), [&](auto & p) {return p.get_name() == name3;})) { + result.successful = false; + result.reason = "parameter '" + name3 + "' may not be set right now"; + } + return result; + }; + auto handler = node->add_on_set_parameters_callback(on_set_parameters); + RCPPUTILS_SCOPE_EXIT( + {node->remove_on_set_parameters_callback(handler.get());}); // always reset + + auto ret = node->set_parameters_atomically( + { + rclcpp::Parameter(name1, 43), + rclcpp::Parameter(name2, true), // this would cause implicit declaration + rclcpp::Parameter(name3, "other"), // this set should fail, and fail the whole operation + }); + EXPECT_FALSE(ret.successful); + // name1 and name2 remain with the old values + EXPECT_EQ(node->get_parameter(name1).get_value(), 42); + EXPECT_FALSE(node->has_parameter(name2)); // important! name2 remains undeclared + EXPECT_EQ(node->get_parameter(name3).get_value(), "test"); + } + { + // adding a parameter in "pre set parameter" callback when that + // parameter has not been declared before will not throw when + // undeclared parameters are allowed. + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + auto name3 = "parameter"_unq; + auto default_value = 0; + + // declare name1 and name2 parameter only + node->declare_parameter(name1, default_value); + node->declare_parameter(name2, default_value); + + // add undeclared parameter with name3 to modified list of parameters + // conditioned of name2 param + auto pre_set_parameters = + [&](std::vector & parameters) { + for (const auto & param : parameters) { + if (param.get_name() == name2) { + parameters.emplace_back(rclcpp::Parameter(name3, 3)); + } + } + }; + + auto handler = node->add_pre_set_parameters_callback(pre_set_parameters); + auto result = node->set_parameters_atomically( + {rclcpp::Parameter(name1, 1), + rclcpp::Parameter(name2, 2)}); + EXPECT_TRUE(result.successful); + EXPECT_TRUE(node->has_parameter(name1)); + EXPECT_TRUE(node->has_parameter(name2)); + EXPECT_TRUE(node->has_parameter(name3)); + EXPECT_EQ(node->get_parameter(name1).get_value(), 1); + EXPECT_EQ(node->get_parameter(name2).get_value(), 2); + EXPECT_EQ(node->get_parameter(name3).get_value(), 3); + + RCPPUTILS_SCOPE_EXIT( + {node->remove_pre_set_parameters_callback(handler.get());}); // always reset + } +} + +// test get_parameter with undeclared not allowed +TEST_F(TestNode, get_parameter_undeclared_parameters_not_allowed) { + auto node = std::make_shared( + "test_get_parameter_node"_unq, + rclcpp::NodeOptions().allow_undeclared_parameters(false)); + { + // normal use + auto name = "parameter"_unq; + + node->declare_parameter(name, 42); + EXPECT_TRUE(node->has_parameter(name)); + + // version that throws on undeclared + EXPECT_EQ(node->get_parameter(name).get_value(), 42); + // version that returns bool and never throws, and stores in rclcpp::Parameter + { + rclcpp::Parameter parameter; + EXPECT_TRUE(node->get_parameter(name, parameter)); + EXPECT_EQ(parameter.get_value(), 42); + } + // version that returns bool and never throws, but is templated to store in a primitive type + { + int value; + EXPECT_TRUE(node->get_parameter(name, value)); + EXPECT_EQ(value, 42); + } + } + { + // getting an undeclared parameter throws + auto name = "parameter"_unq; + EXPECT_FALSE(node->has_parameter(name)); + + EXPECT_THROW({node->get_parameter(name);}, rclcpp::exceptions::ParameterNotDeclaredException); + { + rclcpp::Parameter parameter; + EXPECT_FALSE(node->get_parameter(name, parameter)); + } + { + int value; + EXPECT_FALSE(node->get_parameter(name, value)); + } + } + { + // for templated version, throws if the parameter type doesn't match the requested type + auto name = "parameter"_unq; + + node->declare_parameter(name, "not an int"); + + EXPECT_THROW( + { + int value; + node->get_parameter(name, value); + }, + rclcpp::exceptions::InvalidParameterTypeException); + } +} + +// test get_parameter with undeclared allowed +TEST_F(TestNode, get_parameter_undeclared_parameters_allowed) { + auto node = std::make_shared( + "test_get_parameter_node"_unq, + rclcpp::NodeOptions().allow_undeclared_parameters(true)); + { + // normal use (declare first) still works + auto name = "parameter"_unq; + + node->declare_parameter(name, 42); + EXPECT_TRUE(node->has_parameter(name)); + + // version that throws on undeclared + EXPECT_EQ(node->get_parameter(name).get_value(), 42); + // version that returns bool and never throws, and stores in rclcpp::Parameter + { + rclcpp::Parameter parameter; + EXPECT_TRUE(node->get_parameter(name, parameter)); + EXPECT_EQ(parameter.get_value(), 42); + } + // version that returns bool and never throws, but is templated to store in a primitive type + { + int value; + EXPECT_TRUE(node->get_parameter(name, value)); + EXPECT_EQ(value, 42); + } + } + { + // getting an undeclared parameter returns default constructed rclcpp::Parameter or false + auto name = "parameter"_unq; + EXPECT_FALSE(node->has_parameter(name)); + + EXPECT_EQ(node->get_parameter(name).get_type(), rclcpp::PARAMETER_NOT_SET); + { + rclcpp::Parameter parameter; + EXPECT_FALSE(node->get_parameter(name, parameter)); + } + { + int value; + EXPECT_FALSE(node->get_parameter(name, value)); + } + } + { + // for templated version, return false if the parameter not declared + auto name = "parameter"_unq; + + EXPECT_EQ(node->get_parameter(name).get_type(), rclcpp::PARAMETER_NOT_SET); + int value = 42; + EXPECT_FALSE(node->get_parameter(name, value)); + EXPECT_EQ(value, 42); + } +} + +// test get_parameter_or with undeclared not allowed +TEST_F(TestNode, get_parameter_or_undeclared_parameters_not_allowed) { + auto node = std::make_shared( + "test_get_parameter_or_node"_unq, + rclcpp::NodeOptions().allow_undeclared_parameters(false)); + { + // normal use (declare first) still works + auto name = "parameter"_unq; + + node->declare_parameter(name, 42); + EXPECT_TRUE(node->has_parameter(name)); + + { + int value; + EXPECT_TRUE(node->get_parameter_or(name, value, 43)); + EXPECT_EQ(value, 42); + } + } + { + // normal use, no declare first + auto name = "parameter"_unq; + + { + int value; + EXPECT_FALSE(node->get_parameter_or(name, value, 43)); + EXPECT_EQ(value, 43); + } + } +} + +// test get_parameter_or with undeclared allowed +TEST_F(TestNode, get_parameter_or_undeclared_parameters_allowed) { + auto node = std::make_shared( + "test_get_parameter_node"_unq, + rclcpp::NodeOptions().allow_undeclared_parameters(true)); + { + // normal use (declare first) still works + auto name = "parameter"_unq; + + node->declare_parameter(name, 42); + EXPECT_TRUE(node->has_parameter(name)); + + { + int value; + EXPECT_TRUE(node->get_parameter_or(name, value, 43)); + EXPECT_EQ(value, 42); + } + } + { + // normal use, no declare first + auto name = "parameter"_unq; + + { + int value; + EXPECT_FALSE(node->get_parameter_or(name, value, 43)); + EXPECT_EQ(value, 43); + } + } +} + +// test get_parameter_or with return value +TEST_F(TestNode, get_parameter_or_with_return_value) { + auto node = std::make_shared( + "test_get_parameter_or_node"_unq); + { + // normal use (declare first) still works + auto name = "parameter"_unq; + + node->declare_parameter(name, 42); + EXPECT_TRUE(node->has_parameter(name)); + + { + const int value = node->get_parameter_or(name, 43); + EXPECT_EQ(value, 42); + } + } + { + // normal use, no declare first + auto name = "parameter"_unq; + + { + const int value = node->get_parameter_or(name, 43); + EXPECT_EQ(value, 43); + } + } +} + +// test get_parameters with undeclared not allowed +TEST_F(TestNode, get_parameters_undeclared_parameters_not_allowed) { + auto node = std::make_shared( + "test_get_parameters_node"_unq, + rclcpp::NodeOptions().allow_undeclared_parameters(false)); + { + // normal use + auto base_name1 = "parameter"_unq; + auto name1 = "ints." + base_name1; + auto base_name2 = "parameter"_unq; + auto name2 = "strings." + base_name2; + auto base_name3 = "parameter"_unq; + auto name3 = "ints." + base_name3; + + node->declare_parameter(name1, 42); + node->declare_parameter(name2, "test"); + node->declare_parameter(name3, 100); + EXPECT_TRUE(node->has_parameter(name1)); + EXPECT_TRUE(node->has_parameter(name2)); + EXPECT_TRUE(node->has_parameter(name3)); + + // non-templated version, get all + { + std::vector expected = { + {name1, 42}, + {name2, "test"}, + {name3, 100}, + }; + EXPECT_EQ(node->get_parameters({name1, name2, name3}), expected); + } + // non-templated version, get some + { + std::vector expected = { + {name1, 42}, + {name3, 100}, + }; + EXPECT_EQ(node->get_parameters({name1, name3}), expected); + } + // non-templated version, get some, different types + { + std::vector expected = { + {name1, 42}, + {name2, "test"}, + }; + EXPECT_EQ(node->get_parameters({name1, name2}), expected); + } + // non-templated version, get some, wrong order (request order preserved) + { + std::vector expected = { + {name3, 100}, + {name1, 42}, + }; + EXPECT_EQ(node->get_parameters({name3, name1}), expected); + } + // templated version, get all int's + { + std::map expected = { + {base_name1, 42}, + {base_name3, 100}, + }; + std::map actual; + EXPECT_TRUE(node->get_parameters("ints", actual)); + EXPECT_EQ(actual, expected); + } + // templated version, get the one string + { + std::map expected = { + {base_name2, "test"}, + }; + std::map actual; + EXPECT_TRUE(node->get_parameters("strings", actual)); + EXPECT_EQ(actual, expected); + } + } + { + // getting an undeclared parameter throws, or in the alternative signature returns false + auto name = "prefix.parameter"_unq; + EXPECT_FALSE(node->has_parameter(name)); + + EXPECT_THROW( + {node->get_parameters({name});}, + rclcpp::exceptions::ParameterNotDeclaredException); + { + std::map values; + EXPECT_TRUE(values.empty()); + EXPECT_FALSE(node->get_parameters("prefix", values)); + EXPECT_TRUE(values.empty()); + } + } + { + // templated version with empty prefix will get all parameters + auto node_local = std::make_shared("test_get_parameters_node"_unq); + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + + node_local->declare_parameter("prefix." + name1, 42); + node_local->declare_parameter("prefix." + name2, 100); + + { + std::map actual; + EXPECT_TRUE(node_local->get_parameters("prefix", actual)); + EXPECT_NE(actual.find(name1), actual.end()); + EXPECT_NE(actual.find(name2), actual.end()); + } + + // will throw if set of parameters is non-homogeneous + auto name3 = "prefix1.parameter"_unq; + node_local->declare_parameter("prefix." + name3, "not an int"); + + { + std::map actual; + EXPECT_THROW( + { + node_local->get_parameters("prefix", actual); + }, + rclcpp::exceptions::InvalidParameterTypeException); + } + } +} + +// test get_parameters with undeclared allowed +TEST_F(TestNode, get_parameters_undeclared_parameters_allowed) { + auto node = std::make_shared( + "test_get_parameters_node"_unq, + rclcpp::NodeOptions().allow_undeclared_parameters(true)); + { + // normal use + auto base_name1 = "parameter"_unq; + auto name1 = "ints." + base_name1; + auto base_name2 = "parameter"_unq; + auto name2 = "strings." + base_name2; + auto base_name3 = "parameter"_unq; + auto name3 = "ints." + base_name3; + + EXPECT_FALSE(node->has_parameter(name1)); + EXPECT_FALSE(node->has_parameter(name2)); + EXPECT_FALSE(node->has_parameter(name3)); + + { + // non-templated version, get all, none set, no throw + std::vector expected = { + {name1, {}}, + {name2, {}}, + {name3, {}}, + }; + EXPECT_EQ(node->get_parameters({name1, name2, name3}), expected); + } + { + // templated version, get all int's, none set, no throw + std::map actual; + EXPECT_FALSE(node->get_parameters("ints", actual)); + EXPECT_TRUE(actual.empty()); + } + { + // templated version, get the one string, none set, no throw + std::map actual; + EXPECT_FALSE(node->get_parameters("strings", actual)); + EXPECT_TRUE(actual.empty()); + } + } +} + +// test describe parameter with undeclared not allowed +TEST_F(TestNode, describe_parameter_undeclared_parameters_not_allowed) { + auto node = std::make_shared( + "test_get_parameter_node"_unq, + rclcpp::NodeOptions().allow_undeclared_parameters(false)); + { + // normal use + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + + node->declare_parameter(name1, 42); + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.read_only = true; + node->declare_parameter(name2, "test", descriptor); + + { + auto result = node->describe_parameter(name1); + EXPECT_EQ(result.name, name1); + EXPECT_EQ(result.type, rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER); + EXPECT_FALSE(result.read_only); + } + { + auto result = node->describe_parameter(name2); + EXPECT_EQ(result.name, name2); + EXPECT_EQ(result.type, rcl_interfaces::msg::ParameterType::PARAMETER_STRING); + EXPECT_TRUE(result.read_only); + } + } + { + // non-existent parameter throws + auto name = "parameter"_unq; + + { + EXPECT_THROW( + { + node->describe_parameter(name); + }, rclcpp::exceptions::ParameterNotDeclaredException); + } + } +} + +// test describe parameter with undeclared allowed +TEST_F(TestNode, describe_parameter_undeclared_parameters_allowed) { + auto node = std::make_shared( + "test_get_parameter_node"_unq, + rclcpp::NodeOptions().allow_undeclared_parameters(true)); + { + // normal use still works + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + + node->declare_parameter(name1, 42); + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.read_only = true; + node->declare_parameter(name2, "test", descriptor); + + { + auto result = node->describe_parameter(name1); + EXPECT_EQ(result.name, name1); + EXPECT_EQ(result.type, rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER); + EXPECT_FALSE(result.read_only); + } + { + auto result = node->describe_parameter(name2); + EXPECT_EQ(result.name, name2); + EXPECT_EQ(result.type, rcl_interfaces::msg::ParameterType::PARAMETER_STRING); + EXPECT_TRUE(result.read_only); + } + } + { + // non-existent parameter does not throw, but returns default constructed one + auto name = "parameter"_unq; + + { + auto result = node->describe_parameter(name); + rcl_interfaces::msg::ParameterDescriptor expected; + expected.name = name; + EXPECT_EQ(result, expected); + } + } +} + +// test describe parameters with undeclared not allowed +TEST_F(TestNode, describe_parameters_undeclared_parameters_not_allowed) { + auto node = std::make_shared( + "test_get_parameters_node"_unq, + rclcpp::NodeOptions().allow_undeclared_parameters(false)); + { + // normal use + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + + node->declare_parameter(name1, 42); + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.read_only = true; + node->declare_parameter(name2, "test", descriptor); + + auto results = node->describe_parameters({name1, name2}); + + EXPECT_EQ(results.size(), 2u); + + EXPECT_EQ(results[0].name, name1); + EXPECT_EQ(results[0].type, rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER); + EXPECT_FALSE(results[0].read_only); + + EXPECT_EQ(results[1].name, name2); + EXPECT_EQ(results[1].type, rcl_interfaces::msg::ParameterType::PARAMETER_STRING); + EXPECT_TRUE(results[1].read_only); + } + { + // non-existent parameter throws + auto name = "parameter"_unq; + + { + EXPECT_THROW( + { + node->describe_parameters({name}); + }, rclcpp::exceptions::ParameterNotDeclaredException); + } + } + { + // non-existent parameter throws, even with existing parameters in the list requested + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + + node->declare_parameter(name1, 42); + + { + EXPECT_THROW( + { + node->describe_parameters({name1, name2}); + }, rclcpp::exceptions::ParameterNotDeclaredException); + } + } + { + // check that repeated names in input work, and that output is stable (same order as input) + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + + node->declare_parameter(name1, 42); + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.read_only = true; + node->declare_parameter(name2, "test", descriptor); + + auto results = node->describe_parameters({name2, name1, name2}); + + EXPECT_EQ(results.size(), 3u); + + EXPECT_EQ(results[0].name, name2); + EXPECT_EQ(results[0].type, rcl_interfaces::msg::ParameterType::PARAMETER_STRING); + EXPECT_TRUE(results[0].read_only); + + EXPECT_EQ(results[1].name, name1); + EXPECT_EQ(results[1].type, rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER); + EXPECT_FALSE(results[1].read_only); + + EXPECT_EQ(results[2].name, name2); + EXPECT_EQ(results[2].type, rcl_interfaces::msg::ParameterType::PARAMETER_STRING); + EXPECT_TRUE(results[2].read_only); + } +} + +// test describe parameters with undeclared allowed +TEST_F(TestNode, describe_parameters_undeclared_parameters_allowed) { + auto node = std::make_shared( + "test_get_parameters_node"_unq, + rclcpp::NodeOptions().allow_undeclared_parameters(true)); + { + // normal use still works (declare first) + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + + node->declare_parameter(name1, 42); + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.read_only = true; + node->declare_parameter(name2, "test", descriptor); + + auto results = node->describe_parameters({name1, name2}); + + EXPECT_EQ(results.size(), 2u); + + EXPECT_EQ(results[0].name, name1); + EXPECT_EQ(results[0].type, rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER); + EXPECT_FALSE(results[0].read_only); + + EXPECT_EQ(results[1].name, name2); + EXPECT_EQ(results[1].type, rcl_interfaces::msg::ParameterType::PARAMETER_STRING); + EXPECT_TRUE(results[1].read_only); + } + { + // non-existent parameter does not throw + auto name = "parameter"_unq; + + auto results = node->describe_parameters({name}); + + EXPECT_EQ(results.size(), 1u); + + EXPECT_EQ(results[0].name, name); + EXPECT_EQ(results[0].type, rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET); + EXPECT_FALSE(results[0].read_only); + } + { + // check that repeated names in input work, and that output is stable (same order as input) + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + + auto results = node->describe_parameters({name2, name1, name2}); + + EXPECT_EQ(results.size(), 3u); + + EXPECT_EQ(results[0].name, name2); + EXPECT_EQ(results[0].type, rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET); + EXPECT_FALSE(results[0].read_only); + + EXPECT_EQ(results[1].name, name1); + EXPECT_EQ(results[1].type, rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET); + EXPECT_FALSE(results[1].read_only); + + EXPECT_EQ(results[2].name, name2); + EXPECT_EQ(results[2].type, rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET); + EXPECT_FALSE(results[2].read_only); + } +} + +// test get parameter types with undeclared not allowed +TEST_F(TestNode, get_parameter_types_undeclared_parameters_not_allowed) { + auto node = std::make_shared( + "test_get_parameter_types_node"_unq, + rclcpp::NodeOptions().allow_undeclared_parameters(false)); + { + // normal use + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + + node->declare_parameter(name1, 42); + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.read_only = true; + node->declare_parameter(name2, "test", descriptor); + + auto results = node->get_parameter_types({name1, name2}); + + EXPECT_EQ(results.size(), 2u); + + EXPECT_EQ(results[0], rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER); + + EXPECT_EQ(results[1], rcl_interfaces::msg::ParameterType::PARAMETER_STRING); + } + { + // non-existent parameter throws + auto name = "parameter"_unq; + + { + EXPECT_THROW( + { + node->get_parameter_types({name}); + }, rclcpp::exceptions::ParameterNotDeclaredException); + } + } + { + // check that repeated names in input work, and that output is stable (same order as input) + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + + node->declare_parameter(name1, 42); + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.read_only = true; + node->declare_parameter(name2, "test", descriptor); + + auto results = node->get_parameter_types({name2, name1, name2}); + + EXPECT_EQ(results.size(), 3u); + + EXPECT_EQ(results[0], rcl_interfaces::msg::ParameterType::PARAMETER_STRING); + + EXPECT_EQ(results[1], rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER); + + EXPECT_EQ(results[2], rcl_interfaces::msg::ParameterType::PARAMETER_STRING); + } +} + +// test get parameter types with undeclared allowed +TEST_F(TestNode, get_parameter_types_undeclared_parameters_allowed) { + auto node = std::make_shared( + "test_get_parameter_types_node"_unq, + rclcpp::NodeOptions().allow_undeclared_parameters(true)); + { + // normal use still works (declare first) + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + + node->declare_parameter(name1, 42); + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.read_only = true; + node->declare_parameter(name2, "test", descriptor); + + auto results = node->get_parameter_types({name1, name2}); + + EXPECT_EQ(results.size(), 2u); + + EXPECT_EQ(results[0], rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER); + + EXPECT_EQ(results[1], rcl_interfaces::msg::ParameterType::PARAMETER_STRING); + } + { + // non-existent parameter does not throw + auto name = "parameter"_unq; + + auto results = node->get_parameter_types({name}); + + EXPECT_EQ(results.size(), 1u); + + EXPECT_EQ(results[0], rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET); + } + { + // check that repeated names in input work, and that output is stable (same order as input) + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + + auto results = node->get_parameter_types({name2, name1, name2}); + + EXPECT_EQ(results.size(), 3u); + + EXPECT_EQ(results[0], rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET); + + EXPECT_EQ(results[1], rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET); + + EXPECT_EQ(results[2], rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET); + } +} + +// test declare parameter with int, int64_t, float and double vector +TEST_F(TestNode, declare_parameter_with_vector) { + auto node = std::make_shared( + "test_declare_parameter_with_vector"_unq, + rclcpp::NodeOptions().allow_undeclared_parameters(true)); + { + // declare parameter and then get types to check + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + auto name3 = "parameter"_unq; + auto name4 = "parameter"_unq; + + node->declare_parameter(name1, std::vector{}); + node->declare_parameter(name2, std::vector{}); + node->declare_parameter(name3, std::vector{}); + node->declare_parameter(name4, std::vector{}); + + EXPECT_TRUE(node->has_parameter(name1)); + EXPECT_TRUE(node->has_parameter(name2)); + EXPECT_TRUE(node->has_parameter(name3)); + EXPECT_TRUE(node->has_parameter(name4)); + + auto results = node->get_parameter_types({name1, name2, name3, name4}); + EXPECT_EQ(results.size(), 4u); + EXPECT_EQ(results[0], rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY); + EXPECT_EQ(results[1], rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY); + EXPECT_EQ(results[2], rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY); + EXPECT_EQ(results[3], rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY); + } + { + // declare parameter and then get values to check + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + auto name3 = "parameter"_unq; + auto name4 = "parameter"_unq; + + int64_t bigger_than_int = INT64_MAX - 42; + double bigger_than_float = std::numeric_limits::max() - 42; + node->declare_parameter(name1, std::vector{1, 2}); + node->declare_parameter(name2, std::vector{3, bigger_than_int}); + node->declare_parameter(name3, std::vector{1.5f, 2.8f}); + node->declare_parameter(name4, std::vector{3.0, bigger_than_float}); + + std::vector expected = { + {name1, std::vector{1, 2}}, + {name2, std::vector{3, bigger_than_int}}, + {name3, std::vector{1.5f, 2.8f}}, + {name4, std::vector{3.0, bigger_than_float}}, + }; + EXPECT_EQ(node->get_parameters({name1, name2, name3, name4}), expected); + } +} + +// test non-array data types for declare parameter function templates that are explicitly defined +TEST_F(TestNode, declare_parameter_allowed_simple_types_function_templates) { + auto node = std::make_shared( + "test_declare_parameter_allowed_simple_types_function_templates"_unq); + { + // declare parameter and then get types to check + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + auto name3 = "parameter"_unq; + auto name4 = "parameter"_unq; + auto name5 = "parameter"_unq; + auto name6 = "parameter"_unq; + auto name7 = "parameter"_unq; + + node->declare_parameter(name1, false); + node->declare_parameter(name2, 1234); + node->declare_parameter(name3, 12340); + node->declare_parameter(name4, static_cast(12.34)); + node->declare_parameter(name5, 12.34); // called float64 in ros2 design parameters page + node->declare_parameter(name6, "test string"); + auto str = "test param"; + node->declare_parameter(name7, str); + + EXPECT_TRUE(node->has_parameter(name1)); + EXPECT_TRUE(node->has_parameter(name2)); + EXPECT_TRUE(node->has_parameter(name3)); + EXPECT_TRUE(node->has_parameter(name4)); + EXPECT_TRUE(node->has_parameter(name5)); + EXPECT_TRUE(node->has_parameter(name6)); + EXPECT_TRUE(node->has_parameter(name7)); + + auto results = node->get_parameter_types({name1, name2, name3, name4, name5, name6, name7}); + EXPECT_EQ(results.size(), 7u); + EXPECT_EQ(results[0], rcl_interfaces::msg::ParameterType::PARAMETER_BOOL); + EXPECT_EQ(results[1], rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER); + EXPECT_EQ(results[2], rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER); + EXPECT_EQ(results[3], rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE); + EXPECT_EQ(results[4], rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE); + EXPECT_EQ(results[5], rcl_interfaces::msg::ParameterType::PARAMETER_STRING); + EXPECT_EQ(results[6], rcl_interfaces::msg::ParameterType::PARAMETER_STRING); + } + { + // declare parameter and then get values to check + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + auto name3 = "parameter"_unq; + auto name4 = "parameter"_unq; + auto name5 = "parameter"_unq; + auto name6 = "parameter"_unq; + auto name7 = "parameter"_unq; + + node->declare_parameter(name1, false); + node->declare_parameter(name2, 4321); + node->declare_parameter(name3, 43210); + node->declare_parameter(name4, static_cast(43.21)); + node->declare_parameter(name5, 12.34); // called float64 in ros2 design parameters page + node->declare_parameter(name6, "test string"); + auto str = "test param"; + node->declare_parameter(name7, str); + + std::vector expected = { + {name1, false}, + {name2, 4321}, + {name3, 43210}, + {name4, static_cast(43.21)}, + {name5, 12.34}, + {name6, "test string"}, + {name7, str} + }; + EXPECT_EQ(node->get_parameters({name1, name2, name3, name4, name5, name6, name7}), expected); + } +} + +// test array data types for declare parameter function templates that are explicitly defined +TEST_F(TestNode, declare_parameter_allowed_array_types_function_templates) { + auto node = std::make_shared( + "test_declare_parameter_allowed_array_types_function_templates"_unq); + { + // declare parameter and then get types to check + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + auto name3 = "parameter"_unq; + auto name4 = "parameter"_unq; + auto name5 = "parameter"_unq; + auto name6 = "parameter"_unq; + auto name7 = "parameter"_unq; + + node->declare_parameter>(name1, std::vector{3, 4, 5, 7, 9}); + node->declare_parameter>(name2, std::vector{false, true}); + node->declare_parameter>(name3, std::vector{1234, 2345}); + node->declare_parameter>(name4, std::vector{12340, 9876}); + node->declare_parameter>( + name5, std::vector{static_cast(12.34), + static_cast(98.78)}); + node->declare_parameter>( + name6, + std::vector{12.34, 55.66}); // called float64 in ros2 design parameters page + node->declare_parameter>( + name7, std::vector{"test string", + "another test str"}); + + EXPECT_TRUE(node->has_parameter(name1)); + EXPECT_TRUE(node->has_parameter(name2)); + EXPECT_TRUE(node->has_parameter(name3)); + EXPECT_TRUE(node->has_parameter(name4)); + EXPECT_TRUE(node->has_parameter(name5)); + EXPECT_TRUE(node->has_parameter(name6)); + EXPECT_TRUE(node->has_parameter(name7)); + + auto results = node->get_parameter_types({name1, name2, name3, name4, name5, name6, name7}); + EXPECT_EQ(results.size(), 7u); + EXPECT_EQ(results[0], rcl_interfaces::msg::ParameterType::PARAMETER_BYTE_ARRAY); + EXPECT_EQ(results[1], rcl_interfaces::msg::ParameterType::PARAMETER_BOOL_ARRAY); + EXPECT_EQ(results[2], rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY); + EXPECT_EQ(results[3], rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY); + EXPECT_EQ(results[4], rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY); + EXPECT_EQ(results[5], rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY); + EXPECT_EQ(results[6], rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY); + } + { + // declare parameter and then get values to check + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + auto name3 = "parameter"_unq; + auto name4 = "parameter"_unq; + auto name5 = "parameter"_unq; + auto name6 = "parameter"_unq; + auto name7 = "parameter"_unq; + + std::vector byte_arr = {0xD, 0xE, 0xA, 0xD}; + node->declare_parameter>(name1, byte_arr); + node->declare_parameter>(name2, std::vector{true, false, true}); + node->declare_parameter>(name3, std::vector{22, 33, 55, 77}); + node->declare_parameter>(name4, std::vector{456, 765}); + node->declare_parameter>( + name5, std::vector{static_cast(99.11), + static_cast(11.99)}); + node->declare_parameter>( + name6, + std::vector{12.21, 55.55, 98.89}); // called float64 in ros2 design parameters page + node->declare_parameter>( + name7, std::vector{"ros2", + "colcon", "ignition"}); + + std::vector expected = { + {name1, std::vector{0xD, 0xE, 0xA, 0xD}}, + {name2, std::vector{true, false, true}}, + {name3, std::vector{22, 33, 55, 77}}, + {name4, std::vector{456, 765}}, + {name5, std::vector{static_cast(99.11), static_cast(11.99)}}, + {name6, std::vector{12.21, 55.55, 98.89}}, + {name7, std::vector{"ros2", "colcon", "ignition"}} + }; + EXPECT_EQ(node->get_parameters({name1, name2, name3, name4, name5, name6, name7}), expected); + } +} + +void expect_qos_profile_eq( + const rmw_qos_profile_t & qos1, const rmw_qos_profile_t & qos2, bool is_publisher) +{ + // Depth and history are skipped because they are not retrieved. + EXPECT_EQ(qos1.reliability, qos2.reliability); + EXPECT_EQ(qos1.durability, qos2.durability); + EXPECT_EQ(qos1.deadline.sec, qos2.deadline.sec); + EXPECT_EQ(qos1.deadline.nsec, qos2.deadline.nsec); + if (is_publisher) { + EXPECT_EQ(qos1.lifespan.sec, qos2.lifespan.sec); + EXPECT_EQ(qos1.lifespan.nsec, qos2.lifespan.nsec); + } + EXPECT_EQ(qos1.liveliness, qos2.liveliness); + EXPECT_EQ(qos1.liveliness_lease_duration.sec, qos2.liveliness_lease_duration.sec); + EXPECT_EQ(qos1.liveliness_lease_duration.nsec, qos2.liveliness_lease_duration.nsec); +} + +namespace +{ + +constexpr std::chrono::nanoseconds DEFAULT_EVENT_TIMEOUT = std::chrono::seconds(3); + +constexpr std::chrono::nanoseconds DEFAULT_EVENT_SLEEP_PERIOD = std::chrono::milliseconds(100); + +bool wait_for_event( + std::shared_ptr node, + std::function predicate, + std::chrono::nanoseconds timeout = DEFAULT_EVENT_TIMEOUT, + std::chrono::nanoseconds sleep_period = DEFAULT_EVENT_SLEEP_PERIOD) +{ + auto start = std::chrono::steady_clock::now(); + std::chrono::nanoseconds time_slept(0); + + bool predicate_result; + while (!(predicate_result = predicate()) && time_slept < timeout) { + rclcpp::Event::SharedPtr graph_event = node->get_graph_event(); + node->wait_for_graph_change(graph_event, sleep_period); + time_slept = std::chrono::duration_cast( + std::chrono::steady_clock::now() - start); + } + return predicate_result; +} + +} // namespace + +// test that calling get_publishers_info_by_topic and get_subscriptions_info_by_topic +TEST_F(TestNode, get_publishers_subscriptions_info_by_topic) { + auto node = std::make_shared("my_node", "/ns"); + std::string topic_name = "test_topic_info"; + std::string fq_topic_name = rclcpp::expand_topic_or_service_name( + topic_name, node->get_name(), node->get_namespace()); + + // Lists should be empty + EXPECT_TRUE(node->get_publishers_info_by_topic(fq_topic_name).empty()); + EXPECT_TRUE(node->get_subscriptions_info_by_topic(fq_topic_name).empty()); + + // Add a publisher + rclcpp::QoSInitialization qos_initialization = + { + RMW_QOS_POLICY_HISTORY_KEEP_ALL, + 10 + }; + rmw_qos_profile_t rmw_qos_profile_default = + { + RMW_QOS_POLICY_HISTORY_KEEP_ALL, + 10, + RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, + RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, + {1, 12345}, + {20, 9887665}, + RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC, + {5, 23456}, + false + }; + rclcpp::QoS qos = rclcpp::QoS(qos_initialization, rmw_qos_profile_default); + auto publisher = node->create_publisher(topic_name, qos); + // Wait for the underlying RMW implementation to catch up with graph changes + auto topic_is_published = + [&]() {return node->get_publishers_info_by_topic(fq_topic_name).size() > 0u;}; + ASSERT_TRUE(wait_for_event(node, topic_is_published)); + // List should have one item + auto publisher_list = node->get_publishers_info_by_topic(fq_topic_name); + ASSERT_EQ(publisher_list.size(), (size_t)1); + // Subscription list should be empty + EXPECT_TRUE(node->get_subscriptions_info_by_topic(fq_topic_name).empty()); + // Verify publisher list has the right data. + EXPECT_EQ(node->get_name(), publisher_list[0].node_name()); + EXPECT_EQ(node->get_namespace(), publisher_list[0].node_namespace()); + EXPECT_EQ("test_msgs/msg/BasicTypes", publisher_list[0].topic_type()); + EXPECT_EQ(rclcpp::EndpointType::Publisher, publisher_list[0].endpoint_type()); + auto actual_qos_profile = publisher_list[0].qos_profile().get_rmw_qos_profile(); + { + SCOPED_TRACE("Publisher QOS 1"); + expect_qos_profile_eq(qos.get_rmw_qos_profile(), actual_qos_profile, true); + } + + // Add a subscription + rclcpp::QoSInitialization qos_initialization2 = + { + RMW_QOS_POLICY_HISTORY_KEEP_LAST, + 0 + }; + rmw_qos_profile_t rmw_qos_profile_default2 = + { + RMW_QOS_POLICY_HISTORY_KEEP_LAST, + 0, + RMW_QOS_POLICY_RELIABILITY_RELIABLE, + RMW_QOS_POLICY_DURABILITY_VOLATILE, + {15, 1678}, + {29, 2345}, + RMW_QOS_POLICY_LIVELINESS_AUTOMATIC, + {5, 23456}, + false + }; + rclcpp::QoS qos2 = rclcpp::QoS(qos_initialization2, rmw_qos_profile_default2); + auto callback = [](test_msgs::msg::BasicTypes::ConstSharedPtr msg) { + (void)msg; + }; + auto subscriber = + node->create_subscription(topic_name, qos2, callback); + // Wait for the underlying RMW implementation to catch up with graph changes + auto topic_is_subscribed = + [&]() {return node->get_subscriptions_info_by_topic(fq_topic_name).size() > 0u;}; + ASSERT_TRUE(wait_for_event(node, topic_is_subscribed)); + // Both lists should have one item + publisher_list = node->get_publishers_info_by_topic(fq_topic_name); + auto subscription_list = node->get_subscriptions_info_by_topic(fq_topic_name); + EXPECT_EQ(publisher_list.size(), (size_t)1); + EXPECT_EQ(subscription_list.size(), (size_t)1); + + // Verify publisher and subscription list has the right data. + EXPECT_EQ(node->get_name(), publisher_list[0].node_name()); + EXPECT_EQ(node->get_namespace(), publisher_list[0].node_namespace()); + EXPECT_EQ("test_msgs/msg/BasicTypes", publisher_list[0].topic_type()); + EXPECT_EQ(rclcpp::EndpointType::Publisher, publisher_list[0].endpoint_type()); + auto publisher_qos_profile = publisher_list[0].qos_profile().get_rmw_qos_profile(); + { + SCOPED_TRACE("Publisher QOS 2"); + expect_qos_profile_eq(qos.get_rmw_qos_profile(), publisher_qos_profile, true); + } + + EXPECT_EQ(node->get_name(), subscription_list[0].node_name()); + EXPECT_EQ(node->get_namespace(), subscription_list[0].node_namespace()); + EXPECT_EQ("test_msgs/msg/BasicTypes", subscription_list[0].topic_type()); + EXPECT_EQ(rclcpp::EndpointType::Subscription, subscription_list[0].endpoint_type()); + auto subscription_qos_profile = subscription_list[0].qos_profile().get_rmw_qos_profile(); + { + SCOPED_TRACE("Subscription QOS"); + expect_qos_profile_eq(qos2.get_rmw_qos_profile(), subscription_qos_profile, false); + } + + // Error cases + EXPECT_THROW( + { + publisher_list = node->get_publishers_info_by_topic("13"); + }, rclcpp::exceptions::InvalidTopicNameError); + EXPECT_THROW( + { + subscription_list = node->get_subscriptions_info_by_topic("13"); + }, rclcpp::exceptions::InvalidTopicNameError); +} + +TEST_F(TestNode, callback_groups) { + auto node = std::make_shared("node", "ns"); + size_t num_callback_groups_in_basic_node = 0; + node->for_each_callback_group( + [&num_callback_groups_in_basic_node](rclcpp::CallbackGroup::SharedPtr group) + { + (void)group; + num_callback_groups_in_basic_node++; + }); + + auto group1 = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + size_t num_callback_groups = 0; + node->for_each_callback_group( + [&num_callback_groups](rclcpp::CallbackGroup::SharedPtr group) + { + (void)group; + num_callback_groups++; + }); + EXPECT_EQ(1u + num_callback_groups_in_basic_node, num_callback_groups); + + auto group2 = node->create_callback_group(rclcpp::CallbackGroupType::Reentrant); + size_t num_callback_groups2 = 0; + node->for_each_callback_group( + [&num_callback_groups2](rclcpp::CallbackGroup::SharedPtr group) + { + (void)group; + num_callback_groups2++; + }); + EXPECT_EQ(2u + num_callback_groups_in_basic_node, num_callback_groups2); +} + +// This is tested more thoroughly in node_interfaces/test_node_graph +TEST_F(TestNode, get_entity_names) { + auto node = std::make_shared("node", "ns"); + const auto node_names = node->get_node_names(); + EXPECT_NE( + node_names.end(), + std::find(node_names.begin(), node_names.end(), node->get_fully_qualified_name())); + + const auto topic_names_and_types = node->get_topic_names_and_types(); + EXPECT_EQ(topic_names_and_types.end(), topic_names_and_types.find("topic")); + + EXPECT_EQ(0u, node->count_publishers("topic")); + EXPECT_EQ(0u, node->count_subscribers("topic")); + + const auto service_names_and_types = node->get_service_names_and_types(); + EXPECT_EQ(service_names_and_types.end(), service_names_and_types.find("service")); + + EXPECT_EQ(0u, node->count_clients("service")); + EXPECT_EQ(0u, node->count_services("service")); + + const auto service_names_and_types_by_node = + node->get_service_names_and_types_by_node("node", "/ns"); + EXPECT_EQ( + service_names_and_types_by_node.end(), + service_names_and_types_by_node.find("service")); +} + +TEST_F(TestNode, wait_for_graph_event) { + // Even though this node is only used in the std::thread below, it's here to ensure there is no + // race condition in its destruction and modification of the node_graph + auto node = std::make_shared("node", "ns"); + + constexpr std::chrono::seconds timeout(10); + auto thread_start = std::chrono::steady_clock::now(); + auto thread_completion = thread_start; + + // This runs until the graph is updated + std::thread graph_event_wait_thread([&thread_completion, node, timeout]() { + auto event = node->get_graph_event(); + EXPECT_NO_THROW(node->wait_for_graph_change(event, timeout)); + thread_completion = std::chrono::steady_clock::now(); + }); + + // Start creating nodes until at least one event triggers in graph_event_wait_thread or until 100 + // nodes have been created (at which point this is a failure) + std::vector> nodes; + while (thread_completion == thread_start && nodes.size() < 100) { + nodes.emplace_back(std::make_shared("node"_unq, "ns")); + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + graph_event_wait_thread.join(); + // Nodes will probably only be of size 1 + EXPECT_LT(0u, nodes.size()); + EXPECT_GT(100u, nodes.size()); + EXPECT_NE(thread_start, thread_completion); + EXPECT_GT(timeout, thread_completion - thread_start); +} + +TEST_F(TestNode, create_sub_node_rmw_validate_namespace_error) { + auto node = std::make_shared("node", "ns"); + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rmw_validate_namespace, RMW_RET_INVALID_ARGUMENT); + + // reset() is not necessary for this exception, but it handles unused return value warning + EXPECT_THROW( + node->create_sub_node("ns").reset(), + rclcpp::exceptions::RCLInvalidArgument); + } + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rmw_validate_namespace, RMW_RET_ERROR); + EXPECT_THROW( + node->create_sub_node("ns").reset(), + rclcpp::exceptions::RCLError); + } +} + +TEST_F(TestNode, static_and_dynamic_typing) { + rclcpp::NodeOptions no; + no.parameter_overrides( + { + {"integer_parameter_override_ok", 43}, + {"string_parameter_override_should_throw", 43}, + {"integer_must_provide_override", 43}, + {"cool_way_of_declaring_a_string_without_a_default", "hello!"} + }); + auto node = std::make_shared("node", "ns", no); + { + auto param = node->declare_parameter("an_int", 42); + EXPECT_EQ(42, param); + auto result = node->set_parameter({"an_int", "string value"}); + EXPECT_FALSE(result.successful); + result = node->set_parameter({"an_int", 43}); + EXPECT_TRUE(result.successful); + EXPECT_TRUE(node->get_parameter("an_int", param)); + EXPECT_EQ(43, param); + } + { + auto param = node->declare_parameter("integer_parameter_override_ok", 42); + EXPECT_EQ(43, param); + } + { + EXPECT_THROW( + node->declare_parameter("string_parameter_override_should_throw", "a string"), + rclcpp::exceptions::InvalidParameterTypeException); + } + { + auto param = node->declare_parameter( + "integer_must_provide_override", rclcpp::PARAMETER_INTEGER); + EXPECT_EQ(43, param.get()); + } + { + auto param = node->declare_parameter( + "cool_way_of_declaring_a_string_without_a_default"); + EXPECT_EQ("hello!", param); + } + { + auto param = node->declare_parameter("integer_override_not_given", rclcpp::PARAMETER_INTEGER); + EXPECT_EQ(rclcpp::PARAMETER_NOT_SET, param.get_type()); + // Throws if not set before access + EXPECT_THROW( + node->get_parameter("integer_override_not_given"), + rclcpp::exceptions::ParameterUninitializedException); + EXPECT_THROW( + node->get_parameters({"integer_override_not_given"}), + rclcpp::exceptions::ParameterUninitializedException); + } + { + auto param = node->declare_parameter("integer_set_after_declare", rclcpp::PARAMETER_INTEGER); + EXPECT_EQ(rclcpp::PARAMETER_NOT_SET, param.get_type()); + auto result = node->set_parameter(rclcpp::Parameter{"integer_set_after_declare", 44}); + ASSERT_TRUE(result.successful) << result.reason; + auto get_param = node->get_parameter("integer_set_after_declare"); + EXPECT_EQ(44, get_param.as_int()); + } + { + EXPECT_THROW( + node->declare_parameter("parameter_not_set_is_not_a_valid_type", rclcpp::PARAMETER_NOT_SET), + std::invalid_argument); + } + { + EXPECT_THROW( + node->declare_parameter( + "uninitialized_not_valid_except_dynamic_typing", rclcpp::ParameterValue{}), + rclcpp::exceptions::InvalidParameterTypeException); + } +} + +TEST_F(TestNode, parameter_uninitialized_exception_even_if_allow_undeclared) { + rclcpp::NodeOptions no; + no.allow_undeclared_parameters(true); + auto node = std::make_shared("node", "ns", no); + { + const std::string param_name = "integer_override_not_given"; + auto param_value = node->declare_parameter(param_name, rclcpp::PARAMETER_INTEGER); + EXPECT_EQ(rclcpp::PARAMETER_NOT_SET, param_value.get_type()); + // Throws if not set before access + EXPECT_THROW( + node->get_parameter(param_name), + rclcpp::exceptions::ParameterUninitializedException); + EXPECT_THROW( + node->get_parameters({param_name}), + rclcpp::exceptions::ParameterUninitializedException); + } +} + +TEST_F(TestNode, get_parameter_with_node_allow_undeclared) { + rclcpp::NodeOptions no; + no.allow_undeclared_parameters(true); + auto node = std::make_shared("node", "ns", no); + { + const std::string param_name = "allow_undeclared_param"; + auto param = node->get_parameter(param_name); + EXPECT_EQ(param_name, param.get_name()); + EXPECT_EQ(rclcpp::PARAMETER_NOT_SET, param.get_type()); + EXPECT_EQ(rclcpp::ParameterValue{}, param.get_parameter_value()); + } +} diff --git a/rclcpp/test/rclcpp/test_node_global_args.cpp b/rclcpp/test/rclcpp/test_node_global_args.cpp new file mode 100644 index 0000000000..c3b0726d66 --- /dev/null +++ b/rclcpp/test/rclcpp/test_node_global_args.cpp @@ -0,0 +1,65 @@ +// 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 "rclcpp/exceptions.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/node_options.hpp" +#include "rclcpp/rclcpp.hpp" + +class TestNodeWithGlobalArgs : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + const char * const args[] = {"proc", "--ros-args", "-r", "__node:=global_node_name"}; + const int argc = sizeof(args) / sizeof(const char *); + rclcpp::init(argc, args); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestNodeWithGlobalArgs, local_arguments_before_global) { + auto options = rclcpp::NodeOptions() + .arguments({"--ros-args", "-r", "__node:=local_arguments_test"}); + + auto node = rclcpp::Node::make_shared("orig_name", options); + EXPECT_STREQ("local_arguments_test", node->get_name()); +} + +TEST_F(TestNodeWithGlobalArgs, use_or_ignore_global_arguments) { + { // Don't use global args + auto options = rclcpp::NodeOptions() + .use_global_arguments(false); + + auto node = rclcpp::Node::make_shared("orig_name", options); + EXPECT_STREQ("orig_name", node->get_name()); + } + { // Do use global args + auto options = rclcpp::NodeOptions() + .use_global_arguments(true); + + auto node = rclcpp::Node::make_shared("orig_name", options); + EXPECT_STREQ("global_node_name", node->get_name()); + } +} diff --git a/rclcpp/test/rclcpp/test_node_options.cpp b/rclcpp/test/rclcpp/test_node_options.cpp new file mode 100644 index 0000000000..7abc36f38e --- /dev/null +++ b/rclcpp/test/rclcpp/test_node_options.cpp @@ -0,0 +1,383 @@ +// 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 "rcl/allocator.h" +#include "rcl/arguments.h" +#include "rcl/remap.h" + +#include "rclcpp/node_options.hpp" + +#include "../mocking_utils/patch.hpp" + + +TEST(TestNodeOptions, ros_args_only) { + rcl_allocator_t allocator = rcl_get_default_allocator(); + auto options = rclcpp::NodeOptions(allocator) + .arguments({"--ros-args", "-r", "__node:=some_node", "-r", "__ns:=/some_ns"}); + + const rcl_node_options_t * rcl_options = options.get_rcl_node_options(); + ASSERT_TRUE(rcl_options != nullptr); + ASSERT_EQ(0, rcl_arguments_get_count_unparsed(&rcl_options->arguments)); + ASSERT_EQ(0, rcl_arguments_get_count_unparsed_ros(&rcl_options->arguments)); + + char * node_name = nullptr; + EXPECT_EQ( + RCL_RET_OK, rcl_remap_node_name( + &rcl_options->arguments, nullptr, "my_node", allocator, &node_name)); + ASSERT_TRUE(node_name != nullptr); + EXPECT_STREQ("some_node", node_name); + allocator.deallocate(node_name, allocator.state); + + char * namespace_name = nullptr; + EXPECT_EQ( + RCL_RET_OK, rcl_remap_node_namespace( + &rcl_options->arguments, nullptr, "my_ns", allocator, &namespace_name)); + ASSERT_TRUE(namespace_name != nullptr); + EXPECT_STREQ("/some_ns", namespace_name); + allocator.deallocate(namespace_name, allocator.state); +} + +TEST(TestNodeOptions, ros_args_and_non_ros_args) { + rcl_allocator_t allocator = rcl_get_default_allocator(); + auto options = rclcpp::NodeOptions(allocator).arguments( + { + "--non-ros-flag", "--ros-args", "-r", "__node:=some_node", + "-r", "__ns:=/some_ns", "--", "non-ros-arg"}); + + const rcl_node_options_t * rcl_options = options.get_rcl_node_options(); + ASSERT_TRUE(rcl_options != nullptr); + ASSERT_EQ(0, rcl_arguments_get_count_unparsed_ros(&rcl_options->arguments)); + ASSERT_EQ(2, rcl_arguments_get_count_unparsed(&rcl_options->arguments)); + + char * node_name = nullptr; + EXPECT_EQ( + RCL_RET_OK, rcl_remap_node_name( + &rcl_options->arguments, nullptr, "my_node", allocator, &node_name)); + ASSERT_TRUE(node_name != nullptr); + EXPECT_STREQ("some_node", node_name); + allocator.deallocate(node_name, allocator.state); + + char * namespace_name = nullptr; + EXPECT_EQ( + RCL_RET_OK, rcl_remap_node_namespace( + &rcl_options->arguments, nullptr, "my_ns", allocator, &namespace_name)); + ASSERT_TRUE(namespace_name != nullptr); + EXPECT_STREQ("/some_ns", namespace_name); + allocator.deallocate(namespace_name, allocator.state); + + int * output_indices = nullptr; + EXPECT_EQ( + RCL_RET_OK, rcl_arguments_get_unparsed( + &rcl_options->arguments, allocator, &output_indices)); + ASSERT_TRUE(output_indices != nullptr); + const std::vector & args = options.arguments(); + EXPECT_EQ("--non-ros-flag", args[output_indices[0]]); + EXPECT_EQ("non-ros-arg", args[output_indices[1]]); + allocator.deallocate(output_indices, allocator.state); +} + +TEST(TestNodeOptions, bad_ros_args) { + rcl_allocator_t allocator = rcl_get_default_allocator(); + auto options = rclcpp::NodeOptions(allocator) + .arguments({"--ros-args", "-r", "foo:="}); + + EXPECT_THROW( + options.get_rcl_node_options(), + rclcpp::exceptions::RCLInvalidROSArgsError); + + options.arguments({"--ros-args", "-r", "foo:=bar", "not-a-ros-arg"}); + EXPECT_THROW( + options.get_rcl_node_options(), + rclcpp::exceptions::UnknownROSArgsError); +} + +TEST(TestNodeOptions, use_global_arguments) { + { + auto options = rclcpp::NodeOptions(); + EXPECT_TRUE(options.use_global_arguments()); + EXPECT_TRUE(options.get_rcl_node_options()->use_global_arguments); + } + + { + auto options = rclcpp::NodeOptions().use_global_arguments(false); + EXPECT_FALSE(options.use_global_arguments()); + EXPECT_FALSE(options.get_rcl_node_options()->use_global_arguments); + } + + { + auto options = rclcpp::NodeOptions().use_global_arguments(true); + EXPECT_TRUE(options.use_global_arguments()); + EXPECT_TRUE(options.get_rcl_node_options()->use_global_arguments); + } + + { + auto options = rclcpp::NodeOptions(); + EXPECT_TRUE(options.use_global_arguments()); + EXPECT_TRUE(options.get_rcl_node_options()->use_global_arguments); + options.use_global_arguments(false); + EXPECT_FALSE(options.use_global_arguments()); + EXPECT_FALSE(options.get_rcl_node_options()->use_global_arguments); + options.use_global_arguments(true); + EXPECT_TRUE(options.use_global_arguments()); + EXPECT_TRUE(options.get_rcl_node_options()->use_global_arguments); + } +} + +TEST(TestNodeOptions, enable_rosout) { + { + auto options = rclcpp::NodeOptions(); + EXPECT_TRUE(options.enable_rosout()); + EXPECT_TRUE(options.get_rcl_node_options()->enable_rosout); + } + + { + auto options = rclcpp::NodeOptions().enable_rosout(false); + EXPECT_FALSE(options.enable_rosout()); + EXPECT_FALSE(options.get_rcl_node_options()->enable_rosout); + } + + { + auto options = rclcpp::NodeOptions().enable_rosout(true); + EXPECT_TRUE(options.enable_rosout()); + EXPECT_TRUE(options.get_rcl_node_options()->enable_rosout); + } + + { + auto options = rclcpp::NodeOptions(); + EXPECT_TRUE(options.enable_rosout()); + EXPECT_TRUE(options.get_rcl_node_options()->enable_rosout); + options.enable_rosout(false); + EXPECT_FALSE(options.enable_rosout()); + EXPECT_FALSE(options.get_rcl_node_options()->enable_rosout); + options.enable_rosout(true); + EXPECT_TRUE(options.enable_rosout()); + EXPECT_TRUE(options.get_rcl_node_options()->enable_rosout); + } +} + +TEST(TestNodeOptions, copy) { + std::vector expected_args{"--unknown-flag", "arg"}; + auto options = rclcpp::NodeOptions().arguments(expected_args).use_global_arguments(false); + const rcl_node_options_t * rcl_options = options.get_rcl_node_options(); + + { + rclcpp::NodeOptions copied_options = options; + EXPECT_FALSE(copied_options.use_global_arguments()); + EXPECT_EQ(expected_args, copied_options.arguments()); + const rcl_node_options_t * copied_rcl_options = copied_options.get_rcl_node_options(); + EXPECT_EQ(copied_rcl_options->use_global_arguments, rcl_options->use_global_arguments); + EXPECT_EQ( + rcl_arguments_get_count_unparsed(&copied_rcl_options->arguments), + rcl_arguments_get_count_unparsed(&rcl_options->arguments)); + } + + { + auto other_options = rclcpp::NodeOptions().use_global_arguments(true); + (void)other_options.get_rcl_node_options(); // force C structure initialization + other_options = options; + EXPECT_FALSE(other_options.use_global_arguments()); + EXPECT_EQ(expected_args, other_options.arguments()); + const rcl_node_options_t * other_rcl_options = other_options.get_rcl_node_options(); + EXPECT_EQ(other_rcl_options->use_global_arguments, rcl_options->use_global_arguments); + EXPECT_EQ( + rcl_arguments_get_count_unparsed(&other_rcl_options->arguments), + rcl_arguments_get_count_unparsed(&rcl_options->arguments)); + } + + { + // The following scope test is missing: + // "arguments" because it is already tested in the above scopes + // "parameter_event_publisher_options" because it can not be directly compared with EXPECT_EQ + // "allocator" because it can not be directly compared with EXPECT_EQ + + // We separate attribute modification from variable initialisation (copy assignment operator) + // to be sure the "non_default_options"'s properties are correctly set before testing the + // assignment operator. + auto non_default_options = rclcpp::NodeOptions(); + non_default_options + .parameter_overrides({rclcpp::Parameter("foo", 0), rclcpp::Parameter("bar", "1")}) + .use_global_arguments(false) + .enable_rosout(false) + .use_intra_process_comms(true) + .enable_topic_statistics(true) + .start_parameter_services(false) + .enable_logger_service(true) + .start_parameter_event_publisher(false) + .clock_type(RCL_SYSTEM_TIME) + .clock_qos(rclcpp::SensorDataQoS()) + .use_clock_thread(false) + .parameter_event_qos(rclcpp::ClockQoS()) + .rosout_qos(rclcpp::ParameterEventsQoS()) + .allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true); + + auto copied_options = non_default_options; + EXPECT_EQ(non_default_options.parameter_overrides(), copied_options.parameter_overrides()); + EXPECT_EQ(non_default_options.use_global_arguments(), copied_options.use_global_arguments()); + EXPECT_EQ(non_default_options.enable_rosout(), copied_options.enable_rosout()); + EXPECT_EQ(non_default_options.use_intra_process_comms(), + copied_options.use_intra_process_comms()); + EXPECT_EQ(non_default_options.enable_topic_statistics(), + copied_options.enable_topic_statistics()); + EXPECT_EQ(non_default_options.start_parameter_services(), + copied_options.start_parameter_services()); + EXPECT_EQ(non_default_options.enable_logger_service(), copied_options.enable_logger_service()); + EXPECT_EQ(non_default_options.start_parameter_event_publisher(), + copied_options.start_parameter_event_publisher()); + EXPECT_EQ(non_default_options.clock_type(), copied_options.clock_type()); + EXPECT_EQ(non_default_options.clock_qos(), copied_options.clock_qos()); + EXPECT_EQ(non_default_options.use_clock_thread(), copied_options.use_clock_thread()); + EXPECT_EQ(non_default_options.parameter_event_qos(), copied_options.parameter_event_qos()); + EXPECT_EQ(non_default_options.rosout_qos(), copied_options.rosout_qos()); + EXPECT_EQ(non_default_options.allow_undeclared_parameters(), + copied_options.allow_undeclared_parameters()); + EXPECT_EQ(non_default_options.automatically_declare_parameters_from_overrides(), + copied_options.automatically_declare_parameters_from_overrides()); + } +} + +TEST(TestNodeOptions, append_parameter_override) { + std::vector expected_args{"--unknown-flag", "arg"}; + auto options = rclcpp::NodeOptions().arguments(expected_args).use_global_arguments(false); + rclcpp::Parameter parameter("some_parameter", 10); + options.append_parameter_override("some_parameter", 10); + EXPECT_EQ(1u, options.parameter_overrides().size()); + EXPECT_EQ(std::string("some_parameter"), options.parameter_overrides()[0].get_name()); +} + +TEST(TestNodeOptions, rcl_node_options_fini_error) { + auto mock = mocking_utils::inject_on_return("lib:rclcpp", rcl_node_options_fini, RCL_RET_ERROR); + auto options = std::make_shared(); + // Necessary to setup internal pointer + ASSERT_NE(nullptr, options->get_rcl_node_options()); + // If fini fails, this should just log an error and continue + EXPECT_NO_THROW(options.reset()); +} + +TEST(TestNodeOptions, bool_setters_and_getters) { + rclcpp::NodeOptions options; + + options.use_global_arguments(false); + EXPECT_FALSE(options.use_global_arguments()); + EXPECT_FALSE(options.get_rcl_node_options()->use_global_arguments); + options.use_global_arguments(true); + EXPECT_TRUE(options.use_global_arguments()); + EXPECT_TRUE(options.get_rcl_node_options()->use_global_arguments); + + options.enable_rosout(false); + EXPECT_FALSE(options.enable_rosout()); + EXPECT_FALSE(options.get_rcl_node_options()->enable_rosout); + options.enable_rosout(true); + EXPECT_TRUE(options.enable_rosout()); + EXPECT_TRUE(options.get_rcl_node_options()->enable_rosout); + + options.use_intra_process_comms(false); + EXPECT_FALSE(options.use_intra_process_comms()); + options.use_intra_process_comms(true); + EXPECT_TRUE(options.use_intra_process_comms()); + + options.enable_topic_statistics(false); + EXPECT_FALSE(options.enable_topic_statistics()); + options.enable_topic_statistics(true); + EXPECT_TRUE(options.enable_topic_statistics()); + + options.start_parameter_services(false); + EXPECT_FALSE(options.start_parameter_services()); + options.start_parameter_services(true); + EXPECT_TRUE(options.start_parameter_services()); + + options.allow_undeclared_parameters(false); + EXPECT_FALSE(options.allow_undeclared_parameters()); + options.allow_undeclared_parameters(true); + EXPECT_TRUE(options.allow_undeclared_parameters()); + + options.start_parameter_event_publisher(false); + EXPECT_FALSE(options.start_parameter_event_publisher()); + options.start_parameter_event_publisher(true); + EXPECT_TRUE(options.start_parameter_event_publisher()); + + options.automatically_declare_parameters_from_overrides(false); + EXPECT_FALSE(options.automatically_declare_parameters_from_overrides()); + options.automatically_declare_parameters_from_overrides(true); + EXPECT_TRUE(options.automatically_declare_parameters_from_overrides()); + + options.enable_logger_service(false); + EXPECT_FALSE(options.enable_logger_service()); + options.enable_logger_service(true); + EXPECT_TRUE(options.enable_logger_service()); +} + +TEST(TestNodeOptions, parameter_event_qos) { + rclcpp::NodeOptions options; + rclcpp::QoS qos1(1); + rclcpp::QoS qos2(2); + EXPECT_NE(qos1, options.parameter_event_qos()); + EXPECT_NE(qos2, options.parameter_event_qos()); + options.parameter_event_qos(qos1); + EXPECT_EQ(qos1, options.parameter_event_qos()); + options.parameter_event_qos(qos2); + EXPECT_EQ(qos2, options.parameter_event_qos()); +} + +TEST(TestNodeOptions, parameter_event_publisher_options) { + rclcpp::NodeOptions options; + rclcpp::PublisherOptionsBase publisher_options; + publisher_options.use_default_callbacks = true; + options.parameter_event_publisher_options(publisher_options); + EXPECT_TRUE(options.parameter_event_publisher_options().use_default_callbacks); + + publisher_options.use_default_callbacks = false; + options.parameter_event_publisher_options(publisher_options); + EXPECT_FALSE(options.parameter_event_publisher_options().use_default_callbacks); +} + +TEST(TestNodeOptions, set_get_allocator) { + rclcpp::NodeOptions options; + EXPECT_NE(nullptr, options.allocator().allocate); + EXPECT_NE(nullptr, options.allocator().deallocate); + EXPECT_NE(nullptr, options.allocator().reallocate); + EXPECT_NE(nullptr, options.allocator().zero_allocate); + + rcl_allocator_t fake_allocator; + fake_allocator.allocate = [](size_t, void *) -> void * {return nullptr;}; + fake_allocator.deallocate = [](void *, void *) {}; + fake_allocator.reallocate = [](void *, size_t, void *) -> void * {return nullptr;}; + fake_allocator.zero_allocate = [](size_t, size_t, void *) -> void * {return nullptr;}; + fake_allocator.state = rcl_get_default_allocator().state; + + options.allocator(fake_allocator); + EXPECT_EQ(fake_allocator.allocate, options.allocator().allocate); + EXPECT_EQ(fake_allocator.deallocate, options.allocator().deallocate); + EXPECT_EQ(fake_allocator.reallocate, options.allocator().reallocate); + EXPECT_EQ(fake_allocator.zero_allocate, options.allocator().zero_allocate); + EXPECT_EQ(fake_allocator.state, options.allocator().state); + + // Check invalid allocator + EXPECT_THROW(options.get_rcl_node_options(), std::bad_alloc); +} + +TEST(TestNodeOptions, clock_type) { + rclcpp::NodeOptions options; + EXPECT_EQ(RCL_ROS_TIME, options.clock_type()); + options.clock_type(RCL_SYSTEM_TIME); + EXPECT_EQ(RCL_SYSTEM_TIME, options.clock_type()); + options.clock_type(RCL_STEADY_TIME); + EXPECT_EQ(RCL_STEADY_TIME, options.clock_type()); +} diff --git a/rclcpp/test/rclcpp/test_parameter.cpp b/rclcpp/test/rclcpp/test_parameter.cpp new file mode 100644 index 0000000000..a2e0548089 --- /dev/null +++ b/rclcpp/test/rclcpp/test_parameter.cpp @@ -0,0 +1,954 @@ +// 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 +#include +#include + +#include "rclcpp/parameter.hpp" +#include "rclcpp/rclcpp.hpp" + +class TestParameter : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestParameter, construct_destruct) { + EXPECT_NO_THROW(std::make_shared().reset()); + EXPECT_NO_THROW(std::make_shared("some_parameter").reset()); + EXPECT_NO_THROW(std::make_shared("some_parameter", 10).reset()); +} + +TEST_F(TestParameter, not_set_variant) { + // Direct instantiation + rclcpp::Parameter not_set_variant; + EXPECT_EQ(rclcpp::PARAMETER_NOT_SET, not_set_variant.get_type()); + EXPECT_EQ("not set", not_set_variant.get_type_name()); + std::stringstream ss; + ss << not_set_variant; + EXPECT_EQ("{\"name\": \"\", \"type\": \"not set\", \"value\": \"not set\"}", ss.str()); + + EXPECT_THROW(not_set_variant.as_bool(), rclcpp::ParameterTypeException); + EXPECT_THROW(not_set_variant.as_int(), rclcpp::ParameterTypeException); + EXPECT_THROW(not_set_variant.as_double(), rclcpp::ParameterTypeException); + EXPECT_THROW(not_set_variant.as_string(), rclcpp::ParameterTypeException); + EXPECT_THROW(not_set_variant.as_byte_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(not_set_variant.as_bool_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(not_set_variant.as_integer_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(not_set_variant.as_double_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(not_set_variant.as_string_array(), rclcpp::ParameterTypeException); + + rcl_interfaces::msg::Parameter not_set_param = not_set_variant.to_parameter_msg(); + EXPECT_EQ("", not_set_param.name); + EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET, not_set_param.value.type); + + // From parameter message + EXPECT_EQ( + rclcpp::ParameterType::PARAMETER_NOT_SET, + rclcpp::Parameter::from_parameter_msg(not_set_param).get_type()); + + EXPECT_THROW( + not_set_variant.get_value(), + rclcpp::exceptions::InvalidParameterTypeException); + + // Check == and != operators work as expected + EXPECT_EQ(not_set_variant, not_set_variant); + rclcpp::Parameter not_set_variant2; + EXPECT_EQ(not_set_variant, not_set_variant2); + rclcpp::Parameter bool_variant("bool_param", true); + EXPECT_NE(not_set_variant, bool_variant); +} + +TEST_F(TestParameter, bool_variant) { + // Direct instantiation + rclcpp::Parameter bool_variant_true("bool_param", true); + EXPECT_EQ("bool_param", bool_variant_true.get_name()); + EXPECT_EQ(rclcpp::ParameterType::PARAMETER_BOOL, bool_variant_true.get_type()); + EXPECT_EQ("bool", bool_variant_true.get_type_name()); + EXPECT_TRUE(bool_variant_true.get_value()); + EXPECT_TRUE(bool_variant_true.get_value_message().bool_value); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_BOOL, + bool_variant_true.get_value_message().type); + EXPECT_TRUE(bool_variant_true.as_bool()); + + EXPECT_THROW(bool_variant_true.as_int(), rclcpp::ParameterTypeException); + EXPECT_THROW(bool_variant_true.as_double(), rclcpp::ParameterTypeException); + EXPECT_THROW(bool_variant_true.as_string(), rclcpp::ParameterTypeException); + EXPECT_THROW(bool_variant_true.as_byte_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(bool_variant_true.as_bool_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(bool_variant_true.as_integer_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(bool_variant_true.as_double_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(bool_variant_true.as_string_array(), rclcpp::ParameterTypeException); + + EXPECT_EQ("true", bool_variant_true.value_to_string()); + std::stringstream ss; + ss << bool_variant_true; + EXPECT_EQ("{\"name\": \"bool_param\", \"type\": \"bool\", \"value\": \"true\"}", ss.str()); + + + rclcpp::Parameter bool_variant_false("bool_param", false); + EXPECT_FALSE(bool_variant_false.get_value()); + EXPECT_FALSE(bool_variant_false.get_value_message().bool_value); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_BOOL, + bool_variant_false.get_value_message().type); + + rcl_interfaces::msg::Parameter bool_param = bool_variant_true.to_parameter_msg(); + EXPECT_EQ("bool_param", bool_param.name); + EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_BOOL, bool_param.value.type); + EXPECT_TRUE(bool_param.value.bool_value); + + // From parameter message + rclcpp::Parameter from_msg_true = + rclcpp::Parameter::from_parameter_msg(bool_param); + EXPECT_EQ("bool_param", from_msg_true.get_name()); + EXPECT_EQ(rclcpp::ParameterType::PARAMETER_BOOL, from_msg_true.get_type()); + EXPECT_EQ("bool", from_msg_true.get_type_name()); + EXPECT_TRUE(from_msg_true.get_value()); + EXPECT_TRUE(from_msg_true.get_value_message().bool_value); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_BOOL, + bool_variant_false.get_value_message().type); + + bool_param.value.bool_value = false; + rclcpp::Parameter from_msg_false = + rclcpp::Parameter::from_parameter_msg(bool_param); + EXPECT_FALSE(from_msg_false.get_value()); + EXPECT_FALSE(from_msg_false.get_value_message().bool_value); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_BOOL, + bool_variant_false.get_value_message().type); + + // Check == and != operators work as expected + EXPECT_EQ(bool_variant_true, bool_variant_true); + EXPECT_NE(bool_variant_false, bool_variant_true); + EXPECT_EQ(bool_variant_true, from_msg_true); +} + +TEST_F(TestParameter, integer_variant) { + const int TEST_VALUE {42}; + + // Direct instantiation + rclcpp::Parameter integer_variant("integer_param", TEST_VALUE); + EXPECT_EQ("integer_param", integer_variant.get_name()); + EXPECT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, integer_variant.get_type()); + EXPECT_EQ("integer", integer_variant.get_type_name()); + EXPECT_EQ( + TEST_VALUE, + integer_variant.get_value()); + EXPECT_EQ(TEST_VALUE, integer_variant.get_value_message().integer_value); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER, + integer_variant.get_value_message().type); + EXPECT_EQ(TEST_VALUE, integer_variant.as_int()); + + EXPECT_THROW(integer_variant.as_bool(), rclcpp::ParameterTypeException); + EXPECT_THROW(integer_variant.as_double(), rclcpp::ParameterTypeException); + EXPECT_THROW(integer_variant.as_string(), rclcpp::ParameterTypeException); + EXPECT_THROW(integer_variant.as_byte_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(integer_variant.as_bool_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(integer_variant.as_integer_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(integer_variant.as_double_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(integer_variant.as_string_array(), rclcpp::ParameterTypeException); + + EXPECT_EQ("42", integer_variant.value_to_string()); + std::stringstream ss; + ss << integer_variant; + EXPECT_EQ("{\"name\": \"integer_param\", \"type\": \"integer\", \"value\": \"42\"}", ss.str()); + + rcl_interfaces::msg::Parameter integer_param = integer_variant.to_parameter_msg(); + EXPECT_EQ("integer_param", integer_param.name); + EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER, integer_param.value.type); + EXPECT_EQ(TEST_VALUE, integer_param.value.integer_value); + + // From parameter message + rclcpp::Parameter from_msg = + rclcpp::Parameter::from_parameter_msg(integer_param); + EXPECT_EQ("integer_param", from_msg.get_name()); + EXPECT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, from_msg.get_type()); + EXPECT_EQ("integer", from_msg.get_type_name()); + EXPECT_EQ( + TEST_VALUE, + from_msg.get_value()); + EXPECT_EQ(TEST_VALUE, from_msg.get_value_message().integer_value); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER, + from_msg.get_value_message().type); + + // Check == and != operators work as expected + EXPECT_EQ(integer_variant, integer_variant); + EXPECT_NE(integer_variant, rclcpp::Parameter("integer_param", 41)); + EXPECT_EQ(integer_variant, from_msg); +} + +TEST_F(TestParameter, long_integer_variant) { + const int64_t TEST_VALUE {std::numeric_limits::max()}; + + // Direct instantiation + rclcpp::Parameter long_variant("long_integer_param", TEST_VALUE); + EXPECT_EQ("long_integer_param", long_variant.get_name()); + EXPECT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, long_variant.get_type()); + EXPECT_EQ("integer", long_variant.get_type_name()); + EXPECT_EQ( + TEST_VALUE, + long_variant.get_value()); + EXPECT_EQ(TEST_VALUE, long_variant.get_value_message().integer_value); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER, + long_variant.get_value_message().type); + EXPECT_EQ(TEST_VALUE, long_variant.as_int()); + + EXPECT_THROW(long_variant.as_bool(), rclcpp::ParameterTypeException); + EXPECT_THROW(long_variant.as_double(), rclcpp::ParameterTypeException); + EXPECT_THROW(long_variant.as_string(), rclcpp::ParameterTypeException); + EXPECT_THROW(long_variant.as_byte_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(long_variant.as_bool_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(long_variant.as_integer_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(long_variant.as_double_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(long_variant.as_string_array(), rclcpp::ParameterTypeException); + + EXPECT_EQ("9223372036854775807", long_variant.value_to_string()); + std::stringstream ss; + ss << long_variant; + EXPECT_EQ( + "{\"name\": \"long_integer_param\", \"type\": \"integer\", \"value\": " + "\"9223372036854775807\"}", + ss.str()); + + rcl_interfaces::msg::Parameter integer_param = long_variant.to_parameter_msg(); + EXPECT_EQ("long_integer_param", integer_param.name); + EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER, integer_param.value.type); + EXPECT_EQ(TEST_VALUE, integer_param.value.integer_value); + + // From parameter message + rclcpp::Parameter from_msg = + rclcpp::Parameter::from_parameter_msg(integer_param); + EXPECT_EQ("long_integer_param", from_msg.get_name()); + EXPECT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, from_msg.get_type()); + EXPECT_EQ("integer", from_msg.get_type_name()); + EXPECT_EQ( + TEST_VALUE, + from_msg.get_value()); + EXPECT_EQ(TEST_VALUE, from_msg.get_value_message().integer_value); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER, + from_msg.get_value_message().type); + + + // Check == and != operators work as expected + EXPECT_EQ(long_variant, long_variant); + EXPECT_NE(long_variant, rclcpp::Parameter("long_integer_param", 41)); + EXPECT_EQ(long_variant, from_msg); +} + +TEST_F(TestParameter, float_variant) { + const float TEST_VALUE {42.0f}; + + // Direct instantiation + rclcpp::Parameter float_variant("float_param", TEST_VALUE); + EXPECT_EQ("float_param", float_variant.get_name()); + EXPECT_EQ(rclcpp::ParameterType::PARAMETER_DOUBLE, float_variant.get_type()); + EXPECT_EQ("double", float_variant.get_type_name()); + EXPECT_EQ( + TEST_VALUE, + float_variant.get_value()); + EXPECT_EQ(TEST_VALUE, float_variant.get_value_message().double_value); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE, + float_variant.get_value_message().type); + EXPECT_EQ(TEST_VALUE, float_variant.as_double()); + + EXPECT_THROW(float_variant.as_bool(), rclcpp::ParameterTypeException); + EXPECT_THROW(float_variant.as_int(), rclcpp::ParameterTypeException); + EXPECT_THROW(float_variant.as_string(), rclcpp::ParameterTypeException); + EXPECT_THROW(float_variant.as_byte_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(float_variant.as_bool_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(float_variant.as_integer_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(float_variant.as_double_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(float_variant.as_string_array(), rclcpp::ParameterTypeException); + + EXPECT_EQ("42.000000", float_variant.value_to_string()); + std::stringstream ss; + ss << float_variant; + EXPECT_EQ( + "{\"name\": \"float_param\", \"type\": \"double\", \"value\": \"42.000000\"}", ss.str()); + + rcl_interfaces::msg::Parameter float_param = float_variant.to_parameter_msg(); + EXPECT_EQ("float_param", float_param.name); + EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE, float_param.value.type); + EXPECT_EQ(TEST_VALUE, float_param.value.double_value); + + // From parameter message + rclcpp::Parameter from_msg = + rclcpp::Parameter::from_parameter_msg(float_param); + EXPECT_EQ("float_param", from_msg.get_name()); + EXPECT_EQ(rclcpp::ParameterType::PARAMETER_DOUBLE, from_msg.get_type()); + EXPECT_EQ("double", from_msg.get_type_name()); + EXPECT_EQ( + TEST_VALUE, + from_msg.get_value()); + EXPECT_EQ(TEST_VALUE, from_msg.get_value_message().double_value); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE, + from_msg.get_value_message().type); + + // Check == and != operators work as expected + EXPECT_EQ(float_variant, float_variant); + EXPECT_NE(float_variant, rclcpp::Parameter("float_param", 41.0)); + EXPECT_EQ(float_variant, from_msg); +} + +TEST_F(TestParameter, double_variant) { + const double TEST_VALUE {-42.1}; + + // Direct instantiation + rclcpp::Parameter double_variant("double_param", TEST_VALUE); + EXPECT_EQ("double_param", double_variant.get_name()); + EXPECT_EQ(rclcpp::ParameterType::PARAMETER_DOUBLE, double_variant.get_type()); + EXPECT_EQ("double", double_variant.get_type_name()); + EXPECT_EQ( + TEST_VALUE, + double_variant.get_value()); + EXPECT_EQ(TEST_VALUE, double_variant.get_value_message().double_value); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE, + double_variant.get_value_message().type); + EXPECT_EQ(TEST_VALUE, double_variant.as_double()); + + EXPECT_THROW(double_variant.as_bool(), rclcpp::ParameterTypeException); + EXPECT_THROW(double_variant.as_int(), rclcpp::ParameterTypeException); + EXPECT_THROW(double_variant.as_string(), rclcpp::ParameterTypeException); + EXPECT_THROW(double_variant.as_byte_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(double_variant.as_bool_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(double_variant.as_integer_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(double_variant.as_double_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(double_variant.as_string_array(), rclcpp::ParameterTypeException); + + EXPECT_EQ("-42.100000", double_variant.value_to_string()); + std::stringstream ss; + ss << double_variant; + EXPECT_EQ( + "{\"name\": \"double_param\", \"type\": \"double\", \"value\": \"-42.100000\"}", ss.str()); + + rcl_interfaces::msg::Parameter double_param = double_variant.to_parameter_msg(); + EXPECT_EQ("double_param", double_param.name); + EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE, double_param.value.type); + EXPECT_EQ(TEST_VALUE, double_param.value.double_value); + + // From parameter message + rclcpp::Parameter from_msg = + rclcpp::Parameter::from_parameter_msg(double_param); + EXPECT_EQ("double_param", from_msg.get_name()); + EXPECT_EQ(rclcpp::ParameterType::PARAMETER_DOUBLE, from_msg.get_type()); + EXPECT_EQ("double", from_msg.get_type_name()); + EXPECT_EQ( + TEST_VALUE, + from_msg.get_value()); + EXPECT_EQ(TEST_VALUE, from_msg.get_value_message().double_value); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE, + from_msg.get_value_message().type); + + // Check == and != operators work as expected + EXPECT_EQ(double_variant, double_variant); + EXPECT_NE(double_variant, rclcpp::Parameter("double_param", -41.2)); + EXPECT_EQ(double_variant, from_msg); +} + +TEST_F(TestParameter, string_variant) { + const std::string TEST_VALUE {"ROS2"}; + + // Direct instantiation + rclcpp::Parameter string_variant("string_param", TEST_VALUE); + EXPECT_EQ("string_param", string_variant.get_name()); + EXPECT_EQ(rclcpp::ParameterType::PARAMETER_STRING, string_variant.get_type()); + EXPECT_EQ("string", string_variant.get_type_name()); + EXPECT_EQ( + TEST_VALUE, + string_variant.get_value()); + EXPECT_EQ(TEST_VALUE, string_variant.get_value_message().string_value); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_STRING, + string_variant.get_value_message().type); + EXPECT_EQ(TEST_VALUE, string_variant.as_string()); + + EXPECT_THROW(string_variant.as_bool(), rclcpp::ParameterTypeException); + EXPECT_THROW(string_variant.as_int(), rclcpp::ParameterTypeException); + EXPECT_THROW(string_variant.as_double(), rclcpp::ParameterTypeException); + EXPECT_THROW(string_variant.as_byte_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(string_variant.as_bool_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(string_variant.as_integer_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(string_variant.as_double_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(string_variant.as_string_array(), rclcpp::ParameterTypeException); + + EXPECT_EQ(TEST_VALUE, string_variant.value_to_string()); + std::stringstream ss; + ss << string_variant; + EXPECT_EQ("{\"name\": \"string_param\", \"type\": \"string\", \"value\": \"ROS2\"}", ss.str()); + + rcl_interfaces::msg::Parameter string_param = string_variant.to_parameter_msg(); + EXPECT_EQ("string_param", string_param.name); + EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_STRING, string_param.value.type); + EXPECT_EQ(TEST_VALUE, string_param.value.string_value); + + // From parameter message + rclcpp::Parameter from_msg = + rclcpp::Parameter::from_parameter_msg(string_param); + EXPECT_EQ("string_param", from_msg.get_name()); + EXPECT_EQ(rclcpp::ParameterType::PARAMETER_STRING, from_msg.get_type()); + EXPECT_EQ("string", from_msg.get_type_name()); + EXPECT_EQ(TEST_VALUE, from_msg.get_value()); + EXPECT_EQ(TEST_VALUE, from_msg.get_value_message().string_value); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_STRING, + from_msg.get_value_message().type); + + // Check == and != operators work as expected + EXPECT_EQ(string_variant, string_variant); + EXPECT_NE(string_variant, rclcpp::Parameter("string_param", "ROS1")); + EXPECT_EQ(string_variant, from_msg); +} + +TEST_F(TestParameter, byte_array_variant) { + const std::vector TEST_VALUE {0x52, 0x4f, 0x53, 0x32}; + + // Direct instantiation + rclcpp::Parameter byte_array_variant("byte_array_param", TEST_VALUE); + EXPECT_EQ("byte_array_param", byte_array_variant.get_name()); + EXPECT_EQ(rclcpp::ParameterType::PARAMETER_BYTE_ARRAY, byte_array_variant.get_type()); + EXPECT_EQ("byte_array", byte_array_variant.get_type_name()); + EXPECT_EQ( + TEST_VALUE, + byte_array_variant.get_value()); + EXPECT_EQ(TEST_VALUE, byte_array_variant.get_value_message().byte_array_value); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_BYTE_ARRAY, + byte_array_variant.get_value_message().type); + EXPECT_EQ(TEST_VALUE, byte_array_variant.as_byte_array()); + + EXPECT_THROW(byte_array_variant.as_bool(), rclcpp::ParameterTypeException); + EXPECT_THROW(byte_array_variant.as_int(), rclcpp::ParameterTypeException); + EXPECT_THROW(byte_array_variant.as_double(), rclcpp::ParameterTypeException); + EXPECT_THROW(byte_array_variant.as_string(), rclcpp::ParameterTypeException); + EXPECT_THROW(byte_array_variant.as_bool_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(byte_array_variant.as_integer_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(byte_array_variant.as_double_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(byte_array_variant.as_string_array(), rclcpp::ParameterTypeException); + + EXPECT_EQ("[0x52, 0x4f, 0x53, 0x32]", byte_array_variant.value_to_string()); + std::stringstream ss; + ss << byte_array_variant; + EXPECT_EQ( + "{\"name\": \"byte_array_param\", \"type\": \"byte_array\", \"value\": " + "\"[0x52, 0x4f, 0x53, 0x32]\"}", + ss.str()); + + rcl_interfaces::msg::Parameter byte_array_param = byte_array_variant.to_parameter_msg(); + EXPECT_EQ("byte_array_param", byte_array_param.name); + EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_BYTE_ARRAY, byte_array_param.value.type); + EXPECT_EQ(TEST_VALUE, byte_array_param.value.byte_array_value); + + // From parameter message + rclcpp::Parameter from_msg = + rclcpp::Parameter::from_parameter_msg(byte_array_param); + EXPECT_EQ("byte_array_param", from_msg.get_name()); + EXPECT_EQ(rclcpp::ParameterType::PARAMETER_BYTE_ARRAY, from_msg.get_type()); + EXPECT_EQ("byte_array", from_msg.get_type_name()); + EXPECT_EQ( + TEST_VALUE, + from_msg.get_value()); + EXPECT_EQ(TEST_VALUE, from_msg.get_value_message().byte_array_value); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_BYTE_ARRAY, + from_msg.get_value_message().type); + + // Check == and != operators work as expected + const std::vector TEST_VALUE2 {0x1, 0x2, 0x3, 0x4}; + EXPECT_EQ(byte_array_variant, byte_array_variant); + EXPECT_NE(byte_array_variant, rclcpp::Parameter("byte_array_param", TEST_VALUE2)); + EXPECT_EQ(byte_array_variant, from_msg); +} + +TEST_F(TestParameter, bool_array_variant) { + const std::vector TEST_VALUE {false, true, true, false, false, true}; + + // Direct instantiation + rclcpp::Parameter bool_array_variant("bool_array_param", TEST_VALUE); + EXPECT_EQ("bool_array_param", bool_array_variant.get_name()); + EXPECT_EQ(rclcpp::ParameterType::PARAMETER_BOOL_ARRAY, bool_array_variant.get_type()); + EXPECT_EQ("bool_array", bool_array_variant.get_type_name()); + EXPECT_EQ( + TEST_VALUE, + bool_array_variant.get_value()); + EXPECT_EQ(TEST_VALUE, bool_array_variant.get_value_message().bool_array_value); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_BOOL_ARRAY, + bool_array_variant.get_value_message().type); + EXPECT_EQ(TEST_VALUE, bool_array_variant.as_bool_array()); + + EXPECT_THROW(bool_array_variant.as_bool(), rclcpp::ParameterTypeException); + EXPECT_THROW(bool_array_variant.as_int(), rclcpp::ParameterTypeException); + EXPECT_THROW(bool_array_variant.as_double(), rclcpp::ParameterTypeException); + EXPECT_THROW(bool_array_variant.as_string(), rclcpp::ParameterTypeException); + EXPECT_THROW(bool_array_variant.as_byte_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(bool_array_variant.as_integer_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(bool_array_variant.as_double_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(bool_array_variant.as_string_array(), rclcpp::ParameterTypeException); + + EXPECT_EQ("[false, true, true, false, false, true]", bool_array_variant.value_to_string()); + std::stringstream ss; + ss << bool_array_variant; + EXPECT_EQ( + "{\"name\": \"bool_array_param\", \"type\": \"bool_array\", \"value\": " + "\"[false, true, true, false, false, true]\"}", + ss.str()); + + rcl_interfaces::msg::Parameter bool_array_param = bool_array_variant.to_parameter_msg(); + EXPECT_EQ("bool_array_param", bool_array_param.name); + EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_BOOL_ARRAY, bool_array_param.value.type); + EXPECT_EQ(TEST_VALUE, bool_array_param.value.bool_array_value); + + // From parameter message + rclcpp::Parameter from_msg = + rclcpp::Parameter::from_parameter_msg(bool_array_param); + EXPECT_EQ("bool_array_param", from_msg.get_name()); + EXPECT_EQ(rclcpp::ParameterType::PARAMETER_BOOL_ARRAY, from_msg.get_type()); + EXPECT_EQ("bool_array", from_msg.get_type_name()); + EXPECT_EQ( + TEST_VALUE, + from_msg.get_value()); + EXPECT_EQ(TEST_VALUE, from_msg.get_value_message().bool_array_value); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_BOOL_ARRAY, + from_msg.get_value_message().type); + + // Check == and != operators work as expected + const std::vector TEST_VALUE2 {true, true, true}; + EXPECT_EQ(bool_array_variant, bool_array_variant); + EXPECT_NE(bool_array_variant, rclcpp::Parameter("bool_array_param", TEST_VALUE2)); + EXPECT_EQ(bool_array_variant, from_msg); +} + +TEST_F(TestParameter, integer_array_variant) { + const std::vector TEST_VALUE + {42, -99, std::numeric_limits::max(), std::numeric_limits::lowest(), 0}; + + // Direct instantiation + rclcpp::Parameter integer_array_variant("integer_array_param", TEST_VALUE); + + EXPECT_EQ("integer_array_param", integer_array_variant.get_name()); + EXPECT_EQ( + rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY, + integer_array_variant.get_type()); + EXPECT_EQ("integer_array", integer_array_variant.get_type_name()); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY, + integer_array_variant.get_value_message().type); + + // No direct comparison of vectors of ints and long ints + const auto & param_value_ref = + integer_array_variant.get_value(); + auto mismatches = std::mismatch(TEST_VALUE.begin(), TEST_VALUE.end(), param_value_ref.begin()); + EXPECT_EQ(TEST_VALUE.end(), mismatches.first); + EXPECT_EQ(param_value_ref.end(), mismatches.second); + + auto param_value = integer_array_variant.get_value_message().integer_array_value; + mismatches = std::mismatch(TEST_VALUE.begin(), TEST_VALUE.end(), param_value.begin()); + EXPECT_EQ(TEST_VALUE.end(), mismatches.first); + EXPECT_EQ(param_value.end(), mismatches.second); + + param_value = integer_array_variant.as_integer_array(); + mismatches = std::mismatch(TEST_VALUE.begin(), TEST_VALUE.end(), param_value.begin()); + EXPECT_EQ(TEST_VALUE.end(), mismatches.first); + EXPECT_EQ(param_value.end(), mismatches.second); + + EXPECT_THROW(integer_array_variant.as_bool(), rclcpp::ParameterTypeException); + EXPECT_THROW(integer_array_variant.as_int(), rclcpp::ParameterTypeException); + EXPECT_THROW(integer_array_variant.as_double(), rclcpp::ParameterTypeException); + EXPECT_THROW(integer_array_variant.as_string(), rclcpp::ParameterTypeException); + EXPECT_THROW(integer_array_variant.as_byte_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(integer_array_variant.as_bool_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(integer_array_variant.as_double_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(integer_array_variant.as_string_array(), rclcpp::ParameterTypeException); + + EXPECT_EQ( + "[42, -99, 2147483647, -2147483648, 0]", + integer_array_variant.value_to_string()); + std::stringstream ss; + ss << integer_array_variant; + EXPECT_EQ( + "{\"name\": \"integer_array_param\", \"type\": \"integer_array\", \"value\": " + "\"[42, -99, 2147483647, -2147483648, 0]\"}", + ss.str()); + + rcl_interfaces::msg::Parameter integer_array_param = integer_array_variant.to_parameter_msg(); + EXPECT_EQ("integer_array_param", integer_array_param.name); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY, + integer_array_param.value.type); + + param_value = integer_array_param.value.integer_array_value; + mismatches = std::mismatch(TEST_VALUE.begin(), TEST_VALUE.end(), param_value.begin()); + EXPECT_EQ(TEST_VALUE.end(), mismatches.first); + EXPECT_EQ(param_value.end(), mismatches.second); + + // From parameter message + rclcpp::Parameter from_msg = + rclcpp::Parameter::from_parameter_msg(integer_array_param); + EXPECT_EQ("integer_array_param", from_msg.get_name()); + EXPECT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY, from_msg.get_type()); + EXPECT_EQ("integer_array", from_msg.get_type_name()); + + param_value = from_msg.get_value(); + mismatches = std::mismatch(TEST_VALUE.begin(), TEST_VALUE.end(), param_value.begin()); + EXPECT_EQ(TEST_VALUE.end(), mismatches.first); + EXPECT_EQ(param_value.end(), mismatches.second); + + param_value = from_msg.get_value_message().integer_array_value; + mismatches = std::mismatch(TEST_VALUE.begin(), TEST_VALUE.end(), param_value.begin()); + EXPECT_EQ(TEST_VALUE.end(), mismatches.first); + EXPECT_EQ(param_value.end(), mismatches.second); + + EXPECT_EQ( + from_msg.get_value_message().type, + rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY); + + // Check == and != operators work as expected + const std::vector TEST_VALUE2{1, 2, 3, 4, 5}; + EXPECT_EQ(integer_array_variant, integer_array_variant); + EXPECT_NE(integer_array_variant, rclcpp::Parameter("integer_array_param", TEST_VALUE2)); + EXPECT_EQ(integer_array_variant, from_msg); +} + +TEST_F(TestParameter, long_integer_array_variant) { + const std::vector TEST_VALUE + {42, -99, std::numeric_limits::max(), std::numeric_limits::lowest(), 0}; + + rclcpp::Parameter long_array_variant("long_integer_array_param", TEST_VALUE); + EXPECT_EQ("long_integer_array_param", long_array_variant.get_name()); + EXPECT_EQ( + rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY, + long_array_variant.get_type()); + EXPECT_EQ("integer_array", long_array_variant.get_type_name()); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY, + long_array_variant.get_value_message().type); + EXPECT_EQ( + TEST_VALUE, + long_array_variant.get_value()); + EXPECT_EQ(TEST_VALUE, long_array_variant.get_value_message().integer_array_value); + EXPECT_EQ(TEST_VALUE, long_array_variant.as_integer_array()); + + EXPECT_THROW(long_array_variant.as_bool(), rclcpp::ParameterTypeException); + EXPECT_THROW(long_array_variant.as_int(), rclcpp::ParameterTypeException); + EXPECT_THROW(long_array_variant.as_double(), rclcpp::ParameterTypeException); + EXPECT_THROW(long_array_variant.as_string(), rclcpp::ParameterTypeException); + EXPECT_THROW(long_array_variant.as_byte_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(long_array_variant.as_bool_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(long_array_variant.as_double_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(long_array_variant.as_string_array(), rclcpp::ParameterTypeException); + + EXPECT_EQ( + "[42, -99, 9223372036854775807, -9223372036854775808, 0]", + long_array_variant.value_to_string()); + std::stringstream ss; + ss << long_array_variant; + EXPECT_EQ( + "{\"name\": \"long_integer_array_param\", \"type\": \"integer_array\", \"value\": " + "\"[42, -99, 9223372036854775807, -9223372036854775808, 0]\"}", + ss.str()); + + rcl_interfaces::msg::Parameter integer_array_param = long_array_variant.to_parameter_msg(); + EXPECT_EQ("long_integer_array_param", integer_array_param.name); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY, + integer_array_param.value.type); + EXPECT_EQ(TEST_VALUE, integer_array_param.value.integer_array_value); + + // From parameter message + rclcpp::Parameter from_msg = + rclcpp::Parameter::from_parameter_msg(integer_array_param); + EXPECT_EQ("long_integer_array_param", from_msg.get_name()); + EXPECT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY, from_msg.get_type()); + EXPECT_EQ("integer_array", from_msg.get_type_name()); + EXPECT_EQ( + TEST_VALUE, + from_msg.get_value()); + EXPECT_EQ(TEST_VALUE, from_msg.get_value_message().integer_array_value); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY, + from_msg.get_value_message().type); + + // Check == and != operators work as expected + const std::vector TEST_VALUE2{1, 2, 3, 4, 5}; + EXPECT_EQ(long_array_variant, long_array_variant); + EXPECT_NE(long_array_variant, rclcpp::Parameter("long_integer_array_param", TEST_VALUE2)); + EXPECT_EQ(long_array_variant, from_msg); +} + +TEST_F(TestParameter, float_array_variant) { + const std::vector TEST_VALUE + {42.1f, -99.1f, std::numeric_limits::max(), std::numeric_limits::lowest(), 0.1f}; + + // Direct instantiation + rclcpp::Parameter float_array_variant("float_array_param", TEST_VALUE); + + EXPECT_EQ("float_array_param", float_array_variant.get_name()); + EXPECT_EQ( + rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY, + float_array_variant.get_type()); + EXPECT_EQ("double_array", float_array_variant.get_type_name()); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY, + float_array_variant.get_value_message().type); + + // No direct comparison of vectors of floats and doubles + const auto & param_value_ref = + float_array_variant.get_value(); + auto mismatches = std::mismatch(TEST_VALUE.begin(), TEST_VALUE.end(), param_value_ref.begin()); + EXPECT_EQ(TEST_VALUE.end(), mismatches.first); + EXPECT_EQ(param_value_ref.end(), mismatches.second); + + auto param_value = float_array_variant.get_value_message().double_array_value; + mismatches = std::mismatch(TEST_VALUE.begin(), TEST_VALUE.end(), param_value.begin()); + EXPECT_EQ(TEST_VALUE.end(), mismatches.first); + EXPECT_EQ(param_value.end(), mismatches.second); + + param_value = float_array_variant.as_double_array(); + mismatches = std::mismatch(TEST_VALUE.begin(), TEST_VALUE.end(), param_value.begin()); + EXPECT_EQ(TEST_VALUE.end(), mismatches.first); + EXPECT_EQ(param_value.end(), mismatches.second); + + EXPECT_THROW(float_array_variant.as_bool(), rclcpp::ParameterTypeException); + EXPECT_THROW(float_array_variant.as_int(), rclcpp::ParameterTypeException); + EXPECT_THROW(float_array_variant.as_double(), rclcpp::ParameterTypeException); + EXPECT_THROW(float_array_variant.as_string(), rclcpp::ParameterTypeException); + EXPECT_THROW(float_array_variant.as_byte_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(float_array_variant.as_bool_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(float_array_variant.as_integer_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(float_array_variant.as_string_array(), rclcpp::ParameterTypeException); + + EXPECT_EQ( + "[42.1, -99.1, 3.40282e+38, -3.40282e+38, 0.1]", + float_array_variant.value_to_string()); + std::stringstream ss; + ss << float_array_variant; + EXPECT_EQ( + "{\"name\": \"float_array_param\", \"type\": \"double_array\", \"value\": " + "\"[42.1, -99.1, 3.40282e+38, -3.40282e+38, 0.1]\"}", + ss.str()); + + rcl_interfaces::msg::Parameter float_array_param = float_array_variant.to_parameter_msg(); + EXPECT_EQ("float_array_param", float_array_param.name); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY, + float_array_param.value.type); + + param_value = float_array_param.value.double_array_value; + mismatches = std::mismatch(TEST_VALUE.begin(), TEST_VALUE.end(), param_value.begin()); + EXPECT_EQ(TEST_VALUE.end(), mismatches.first); + EXPECT_EQ(param_value.end(), mismatches.second); + + // From parameter message + rclcpp::Parameter from_msg = + rclcpp::Parameter::from_parameter_msg(float_array_param); + EXPECT_EQ("float_array_param", from_msg.get_name()); + EXPECT_EQ(rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY, from_msg.get_type()); + EXPECT_EQ("double_array", from_msg.get_type_name()); + + param_value = from_msg.get_value(); + mismatches = std::mismatch(TEST_VALUE.begin(), TEST_VALUE.end(), param_value.begin()); + EXPECT_EQ(TEST_VALUE.end(), mismatches.first); + EXPECT_EQ(param_value.end(), mismatches.second); + + param_value = from_msg.get_value_message().double_array_value; + mismatches = std::mismatch(TEST_VALUE.begin(), TEST_VALUE.end(), param_value.begin()); + EXPECT_EQ(TEST_VALUE.end(), mismatches.first); + EXPECT_EQ(param_value.end(), mismatches.second); + + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY, + from_msg.get_value_message().type); + + // Check == and != operators work as expected + const std::vector TEST_VALUE2{1.0, 2.0, 3.0, 4.0}; + EXPECT_EQ(float_array_variant, float_array_variant); + EXPECT_NE(float_array_variant, rclcpp::Parameter("float_array_param", TEST_VALUE2)); + EXPECT_EQ(float_array_variant, from_msg); +} + +TEST_F(TestParameter, double_array_variant) { + const std::vector TEST_VALUE + {42.1, -99.1, std::numeric_limits::max(), std::numeric_limits::lowest(), 0.1}; + + rclcpp::Parameter double_array_variant("double_array_param", TEST_VALUE); + EXPECT_EQ("double_array_param", double_array_variant.get_name()); + EXPECT_EQ( + rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY, + double_array_variant.get_type()); + EXPECT_EQ("double_array", double_array_variant.get_type_name()); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY, + double_array_variant.get_value_message().type); + EXPECT_EQ( + TEST_VALUE, + double_array_variant.get_value()); + EXPECT_EQ(TEST_VALUE, double_array_variant.get_value_message().double_array_value); + EXPECT_EQ(TEST_VALUE, double_array_variant.as_double_array()); + + EXPECT_THROW(double_array_variant.as_bool(), rclcpp::ParameterTypeException); + EXPECT_THROW(double_array_variant.as_int(), rclcpp::ParameterTypeException); + EXPECT_THROW(double_array_variant.as_double(), rclcpp::ParameterTypeException); + EXPECT_THROW(double_array_variant.as_string(), rclcpp::ParameterTypeException); + EXPECT_THROW(double_array_variant.as_byte_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(double_array_variant.as_bool_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(double_array_variant.as_integer_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(double_array_variant.as_string_array(), rclcpp::ParameterTypeException); + + EXPECT_EQ( + "[42.1, -99.1, 1.79769e+308, -1.79769e+308, 0.1]", + double_array_variant.value_to_string()); + std::stringstream ss; + ss << double_array_variant; + EXPECT_EQ( + "{\"name\": \"double_array_param\", \"type\": \"double_array\", \"value\": " + "\"[42.1, -99.1, 1.79769e+308, -1.79769e+308, 0.1]\"}", + ss.str()); + + rcl_interfaces::msg::Parameter double_array_param = double_array_variant.to_parameter_msg(); + EXPECT_EQ("double_array_param", double_array_param.name); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY, + double_array_param.value.type); + EXPECT_EQ(TEST_VALUE, double_array_param.value.double_array_value); + + // From parameter message + rclcpp::Parameter from_msg = + rclcpp::Parameter::from_parameter_msg(double_array_param); + EXPECT_EQ("double_array_param", from_msg.get_name()); + EXPECT_EQ(rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY, from_msg.get_type()); + EXPECT_EQ("double_array", from_msg.get_type_name()); + EXPECT_EQ( + TEST_VALUE, + from_msg.get_value()); + EXPECT_EQ(TEST_VALUE, from_msg.get_value_message().double_array_value); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY, + from_msg.get_value_message().type); + + // Check == and != operators work as expected + const std::vector TEST_VALUE2{1.0, 2.0, 3.0, 4.0}; + EXPECT_EQ(double_array_variant, double_array_variant); + EXPECT_NE(double_array_variant, rclcpp::Parameter("double_array_param", TEST_VALUE2)); + EXPECT_EQ(double_array_variant, from_msg); +} + +TEST_F(TestParameter, string_array_variant) { + const std::vector TEST_VALUE {"R", "O", "S2"}; + + // Direct instantiation + rclcpp::Parameter string_array_variant("string_array_param", TEST_VALUE); + EXPECT_EQ("string_array_param", string_array_variant.get_name()); + EXPECT_EQ( + rclcpp::ParameterType::PARAMETER_STRING_ARRAY, + string_array_variant.get_type()); + EXPECT_EQ("string_array", string_array_variant.get_type_name()); + EXPECT_EQ( + TEST_VALUE, + string_array_variant.get_value()); + EXPECT_EQ(TEST_VALUE, string_array_variant.get_value_message().string_array_value); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY, + string_array_variant.get_value_message().type); + EXPECT_EQ(TEST_VALUE, string_array_variant.as_string_array()); + + EXPECT_THROW(string_array_variant.as_bool(), rclcpp::ParameterTypeException); + EXPECT_THROW(string_array_variant.as_int(), rclcpp::ParameterTypeException); + EXPECT_THROW(string_array_variant.as_double(), rclcpp::ParameterTypeException); + EXPECT_THROW(string_array_variant.as_string(), rclcpp::ParameterTypeException); + EXPECT_THROW(string_array_variant.as_byte_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(string_array_variant.as_bool_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(string_array_variant.as_integer_array(), rclcpp::ParameterTypeException); + EXPECT_THROW(string_array_variant.as_double_array(), rclcpp::ParameterTypeException); + + EXPECT_EQ("[R, O, S2]", string_array_variant.value_to_string()); + std::stringstream ss; + ss << string_array_variant; + EXPECT_EQ( + "{\"name\": \"string_array_param\", \"type\": \"string_array\", \"value\": \"[R, O, S2]\"}", + ss.str()); + + rcl_interfaces::msg::Parameter string_array_param = string_array_variant.to_parameter_msg(); + EXPECT_EQ("string_array_param", string_array_param.name); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY, + string_array_param.value.type); + EXPECT_EQ(TEST_VALUE, string_array_param.value.string_array_value); + + // From parameter message + rclcpp::Parameter from_msg = + rclcpp::Parameter::from_parameter_msg(string_array_param); + EXPECT_EQ("string_array_param", from_msg.get_name()); + EXPECT_EQ(rclcpp::ParameterType::PARAMETER_STRING_ARRAY, from_msg.get_type()); + EXPECT_EQ("string_array", from_msg.get_type_name()); + EXPECT_EQ( + TEST_VALUE, + from_msg.get_value()); + EXPECT_EQ(TEST_VALUE, from_msg.get_value_message().string_array_value); + EXPECT_EQ( + rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY, + from_msg.get_value_message().type); + + // Check == and != operators work as expected + const std::vector TEST_VALUE2{"R", "O", "S1"}; + EXPECT_EQ(string_array_variant, string_array_variant); + EXPECT_NE(string_array_variant, rclcpp::Parameter("string_array_param", TEST_VALUE2)); + EXPECT_EQ(string_array_variant, from_msg); +} + +TEST_F(TestParameter, parameter_vector_stringification) { + const std::vector parameters = { + rclcpp::Parameter(), + rclcpp::Parameter("bool_param", true), + rclcpp::Parameter("integer_param", 42), + rclcpp::Parameter("double_param", 3.14159), + rclcpp::Parameter("string_param", "I'm a string"), + }; + + std::stringstream ss; + ss << parameters; + EXPECT_EQ( + "{" + "\"\": {\"type\": \"not set\", \"value\": \"not set\"}, " + "\"bool_param\": {\"type\": \"bool\", \"value\": \"true\"}, " + "\"integer_param\": {\"type\": \"integer\", \"value\": \"42\"}, " + "\"double_param\": {\"type\": \"double\", \"value\": \"3.141590\"}, " + "\"string_param\": {\"type\": \"string\", \"value\": \"I'm a string\"}}", + ss.str()); +} diff --git a/rclcpp/test/rclcpp/test_parameter_client.cpp b/rclcpp/test/rclcpp/test_parameter_client.cpp new file mode 100644 index 0000000000..a709d8cc68 --- /dev/null +++ b/rclcpp/test/rclcpp/test_parameter_client.cpp @@ -0,0 +1,1115 @@ +// 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 +#include + +#include "rclcpp/rclcpp.hpp" + +#include "../utils/rclcpp_gtest_macros.hpp" + +#include "rcl_interfaces/msg/parameter_event.hpp" + +using namespace std::chrono_literals; + +class TestParameterClient : public ::testing::Test +{ +public: + void OnMessage(rcl_interfaces::msg::ParameterEvent::ConstSharedPtr event) + { + (void)event; + } + +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + void SetUp() + { + node = std::make_shared("test_parameter_client", "/ns"); + node_with_option = + std::make_shared( + "test_parameter_client_allow_undeclare", "/ns", + rclcpp::NodeOptions().allow_undeclared_parameters(true)); + } + + void TearDown() + { + node.reset(); + node_with_option.reset(); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + // "start_type_description_service" and "use_sim_time" + const uint64_t builtin_param_count = 2; + rclcpp::Node::SharedPtr node; + rclcpp::Node::SharedPtr node_with_option; +}; + +/* + Testing async parameter client construction and destruction. + */ +TEST_F(TestParameterClient, async_construction_and_destruction) { + { + auto asynchronous_client = std::make_shared(node); + (void)asynchronous_client; + } + + { + auto asynchronous_client = std::make_shared( + node->get_node_base_interface(), + node->get_node_topics_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface()); + (void)asynchronous_client; + } + + { + ASSERT_THROW( + { + auto asynchronous_client = std::make_shared( + node, "invalid_remote_node?"); + (void)asynchronous_client; + }, rclcpp::exceptions::InvalidServiceNameError); + } +} + +/* + Testing sync parameter client construction and destruction. + */ +TEST_F(TestParameterClient, sync_construction_and_destruction) { + { + auto synchronous_client = std::make_shared(node); + (void)synchronous_client; + } + + { + auto synchronous_client = std::make_shared( + std::make_shared(), + node); + (void)synchronous_client; + } + + { + auto synchronous_client = std::make_shared( + std::make_shared(), + node->get_node_base_interface(), + node->get_node_topics_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface()); + (void)synchronous_client; + } + + { + ASSERT_THROW( + { + auto synchronous_client = std::make_shared( + node, "invalid_remote_node?"); + (void)synchronous_client; + }, rclcpp::exceptions::InvalidServiceNameError); + } +} + +/* + Testing different methods for parameter event subscription from asynchronous clients. + */ +TEST_F(TestParameterClient, async_parameter_event_subscription) { + auto callback = std::bind(&TestParameterClient::OnMessage, this, std::placeholders::_1); + { + auto asynchronous_client = std::make_shared(node); + auto event_sub = asynchronous_client->on_parameter_event(callback); + (void)event_sub; + } + + { + auto event_sub = rclcpp::AsyncParametersClient::on_parameter_event(node, callback); + (void)event_sub; + } + + { + auto event_sub = rclcpp::AsyncParametersClient::on_parameter_event( + node->get_node_topics_interface(), + callback); + (void)event_sub; + } +} + +/* + Testing different methods for parameter event subscription from synchronous clients. + */ +TEST_F(TestParameterClient, sync_parameter_event_subscription) { + auto callback = std::bind(&TestParameterClient::OnMessage, this, std::placeholders::_1); + { + auto synchronous_client = std::make_shared(node); + auto event_sub = synchronous_client->on_parameter_event(callback); + (void)event_sub; + } + + { + auto event_sub = rclcpp::SyncParametersClient::on_parameter_event(node, callback); + (void)event_sub; + } + + { + auto event_sub = rclcpp::SyncParametersClient::on_parameter_event( + node->get_node_topics_interface(), + callback); + (void)event_sub; + } +} + +/* + Coverage for simple get_parameter methods + */ +TEST_F(TestParameterClient, sync_parameter_get_parameter) { + rclcpp::SyncParametersClient client(node); + EXPECT_EQ(10, client.get_parameter("not_a_parameter", 10)); + + RCLCPP_EXPECT_THROW_EQ( + client.get_parameter("not_a_parameter"), + std::runtime_error("Parameter 'not_a_parameter' is not set")); +} + +/* + Coverage for async waiting/is_ready + */ +TEST_F(TestParameterClient, sync_parameter_is_ready) { + rclcpp::SyncParametersClient client(node); + EXPECT_TRUE(client.wait_for_service()); + EXPECT_TRUE(client.service_is_ready()); +} + +/* + Coverage for async get_parameter_types + */ +TEST_F(TestParameterClient, async_parameter_get_parameter_types) { + auto asynchronous_client = std::make_shared(node); + bool callback_called = false; + auto callback = [&callback_called](std::shared_future> result) + { + // We expect the result to be empty since we tried to get a parameter that didn't exist. + if (result.valid() && result.get().size() == 0) { + callback_called = true; + } + }; + std::vector names{"foo"}; + std::shared_future> future = + asynchronous_client->get_parameter_types(names, callback); + auto return_code = rclcpp::spin_until_future_complete( + node, future, std::chrono::milliseconds(100)); + ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); + ASSERT_TRUE(callback_called); +} + +/* + Coverage for async get_parameter_types with allow_undeclared_ enabled + */ +TEST_F(TestParameterClient, async_parameter_get_parameter_types_allow_undeclared) { + auto asynchronous_client = + std::make_shared(node_with_option); + bool callback_called = false; + auto callback = [&callback_called](std::shared_future> result) + { + if (result.valid() && result.get().size() == 1 && + result.get()[0] == rclcpp::PARAMETER_NOT_SET) + { + callback_called = true; + } + }; + std::vector names{"foo"}; + std::shared_future> future = + asynchronous_client->get_parameter_types(names, callback); + auto return_code = rclcpp::spin_until_future_complete( + node_with_option, future, std::chrono::milliseconds(100)); + ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); + ASSERT_TRUE(callback_called); +} + +/* + Coverage for async get_parameters + */ +TEST_F(TestParameterClient, async_parameter_get_parameters) { + auto asynchronous_client = std::make_shared(node); + bool callback_called = false; + auto callback = [&callback_called](std::shared_future> result) + { + // We expect the result to be empty since we tried to get a parameter that didn't exist. + if (result.valid() && result.get().size() == 0) { + callback_called = true; + } + }; + std::vector names{"foo"}; + std::shared_future> future = asynchronous_client->get_parameters( + names, callback); + auto return_code = rclcpp::spin_until_future_complete( + node, future, std::chrono::milliseconds(100)); + ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); + ASSERT_TRUE(callback_called); +} + +/* + Coverage for async get_parameters with allow_undeclared_ enabled + */ +TEST_F(TestParameterClient, async_parameter_get_parameters_allow_undeclared) { + auto asynchronous_client = + std::make_shared(node_with_option); + bool callback_called = false; + auto callback = [&callback_called](std::shared_future> result) + { + if (result.valid() && result.get().size() == 1 && result.get()[0].get_name() == "foo") { + callback_called = true; + } + }; + std::vector names{"foo"}; + std::shared_future> future = asynchronous_client->get_parameters( + names, callback); + auto return_code = rclcpp::spin_until_future_complete( + node_with_option, future, std::chrono::milliseconds(100)); + ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); + ASSERT_TRUE(callback_called); +} + +/* + Coverage for async set_parameters_atomically + */ +TEST_F(TestParameterClient, async_parameter_set_parameters_atomically) { + auto asynchronous_client = std::make_shared(node); + bool callback_called = false; + auto callback = + [&callback_called](std::shared_future result) + { + // We expect this to fail since we didn't declare the parameter first. + if (result.valid() && !result.get().successful && + result.get().reason == "One or more parameters were not declared before setting") + { + callback_called = true; + } + }; + std::vector parameters; + parameters.emplace_back(rclcpp::Parameter("foo")); + std::shared_future future = + asynchronous_client->set_parameters_atomically(parameters, callback); + auto return_code = rclcpp::spin_until_future_complete( + node, future, std::chrono::milliseconds(100)); + ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); + ASSERT_TRUE(callback_called); +} + +/* + Coverage for async list_parameters + */ +TEST_F(TestParameterClient, async_parameter_list_parameters) { + auto asynchronous_client = std::make_shared(node); + bool callback_called = false; + auto callback = + [&callback_called](std::shared_future result) + { + if (result.valid() && result.get().names.size() == 0 && result.get().prefixes.size() == 0) { + callback_called = true; + } + }; + std::vector prefixes{"foo"}; + std::shared_future future = + asynchronous_client->list_parameters(prefixes, 0, callback); + auto return_code = rclcpp::spin_until_future_complete( + node, future, std::chrono::milliseconds(100)); + ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); + ASSERT_TRUE(callback_called); +} + +/* + Coverage for sync get_parameter_types + */ +TEST_F(TestParameterClient, sync_parameter_get_parameter_types) { + node->declare_parameter("foo", 4); + node->declare_parameter("bar", "this is bar"); + auto synchronous_client = std::make_shared(node); + + { + std::vector names{"none"}; + std::vector parameter_types = + synchronous_client->get_parameter_types(names, 10s); + ASSERT_EQ(0u, parameter_types.size()); + } + + { + std::vector names{"none", "foo", "bar"}; + std::vector parameter_types = + synchronous_client->get_parameter_types(names, 10s); + ASSERT_EQ(0u, parameter_types.size()); + } + + { + std::vector names{"foo"}; + std::vector parameter_types = + synchronous_client->get_parameter_types(names, 10s); + ASSERT_EQ(1u, parameter_types.size()); + ASSERT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, parameter_types[0]); + } + + { + std::vector names{"bar"}; + std::vector parameter_types = + synchronous_client->get_parameter_types(names, 10s); + ASSERT_EQ(1u, parameter_types.size()); + ASSERT_EQ(rclcpp::ParameterType::PARAMETER_STRING, parameter_types[0]); + } + + { + std::vector names{"foo", "bar"}; + std::vector parameter_types = + synchronous_client->get_parameter_types(names, 10s); + ASSERT_EQ(2u, parameter_types.size()); + ASSERT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, parameter_types[0]); + ASSERT_EQ(rclcpp::ParameterType::PARAMETER_STRING, parameter_types[1]); + } +} + +/* + Coverage for sync get_parameter_types with allow_undeclared_ enabled + */ +TEST_F(TestParameterClient, sync_parameter_get_parameter_types_allow_undeclared) { + node_with_option->declare_parameter("foo", 4); + node_with_option->declare_parameter("bar", "this is bar"); + auto synchronous_client = std::make_shared(node_with_option); + + { + std::vector names{"none"}; + std::vector parameter_types = + synchronous_client->get_parameter_types(names, 10s); + ASSERT_EQ(1u, parameter_types.size()); + ASSERT_EQ(rclcpp::ParameterType::PARAMETER_NOT_SET, parameter_types[0]); + } + + { + std::vector names{"none", "foo", "bar"}; + std::vector parameter_types = + synchronous_client->get_parameter_types(names, 10s); + ASSERT_EQ(3u, parameter_types.size()); + ASSERT_EQ(rclcpp::ParameterType::PARAMETER_NOT_SET, parameter_types[0]); + ASSERT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, parameter_types[1]); + ASSERT_EQ(rclcpp::ParameterType::PARAMETER_STRING, parameter_types[2]); + } + + { + std::vector names{"foo"}; + std::vector parameter_types = + synchronous_client->get_parameter_types(names, 10s); + ASSERT_EQ(1u, parameter_types.size()); + ASSERT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, parameter_types[0]); + } + + { + std::vector names{"bar"}; + std::vector parameter_types = + synchronous_client->get_parameter_types(names, 10s); + ASSERT_EQ(1u, parameter_types.size()); + ASSERT_EQ(rclcpp::ParameterType::PARAMETER_STRING, parameter_types[0]); + } + + { + std::vector names{"foo", "bar"}; + std::vector parameter_types = + synchronous_client->get_parameter_types(names, 10s); + ASSERT_EQ(2u, parameter_types.size()); + ASSERT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, parameter_types[0]); + ASSERT_EQ(rclcpp::ParameterType::PARAMETER_STRING, parameter_types[1]); + } +} + +/* + Coverage for sync get_parameters + */ +TEST_F(TestParameterClient, sync_parameter_get_parameters) { + node->declare_parameter("foo", 4); + node->declare_parameter("bar", "this is bar"); + node->declare_parameter("baz", rclcpp::PARAMETER_INTEGER); + auto synchronous_client = std::make_shared(node); + + { + std::vector names{"none"}; + std::vector parameters = synchronous_client->get_parameters(names, 10s); + ASSERT_EQ(0u, parameters.size()); + } + + { + // not throw ParameterUninitializedException while getting parameter from service + // even if the parameter is not initialized in the node + std::vector names{"baz"}; + std::vector parameters = synchronous_client->get_parameters(names, 10s); + ASSERT_EQ(0u, parameters.size()); + } + + { + std::vector names{"none", "foo", "bar"}; + std::vector parameters = synchronous_client->get_parameters(names, 10s); + ASSERT_EQ(0u, parameters.size()); + } + + { + std::vector names{"foo"}; + std::vector parameters = synchronous_client->get_parameters(names, 10s); + ASSERT_EQ(1u, parameters.size()); + ASSERT_EQ("foo", parameters[0].get_name()); + ASSERT_EQ(4u, parameters[0].as_int()); + } + + { + std::vector names{"bar"}; + std::vector parameters = synchronous_client->get_parameters(names, 10s); + ASSERT_EQ(1u, parameters.size()); + ASSERT_EQ("bar", parameters[0].get_name()); + ASSERT_EQ("this is bar", parameters[0].as_string()); + } + + { + std::vector names{"foo", "bar"}; + std::vector parameters = synchronous_client->get_parameters(names, 10s); + ASSERT_EQ(2u, parameters.size()); + ASSERT_EQ("foo", parameters[0].get_name()); + ASSERT_EQ(4u, parameters[0].as_int()); + ASSERT_EQ("bar", parameters[1].get_name()); + ASSERT_EQ("this is bar", parameters[1].as_string()); + } +} + +/* + Coverage for sync get_parameters with allow_undeclared_ enabled + */ +TEST_F(TestParameterClient, sync_parameter_get_parameters_allow_undeclared) { + node_with_option->declare_parameter("foo", 4); + node_with_option->declare_parameter("bar", "this is bar"); + node_with_option->declare_parameter("baz", rclcpp::PARAMETER_INTEGER); + auto synchronous_client = std::make_shared(node_with_option); + + { + std::vector names{"none"}; + std::vector parameters = synchronous_client->get_parameters(names, 10s); + ASSERT_EQ(1u, parameters.size()); + } + + { + // not throw ParameterUninitializedException while getting parameter from service + // even if the parameter is not initialized in the node + std::vector names{"baz"}; + std::vector parameters = synchronous_client->get_parameters(names, 10s); + ASSERT_EQ(0u, parameters.size()); + } + + { + std::vector names{"none", "foo", "bar"}; + std::vector parameters = synchronous_client->get_parameters(names, 10s); + ASSERT_EQ(3u, parameters.size()); + ASSERT_EQ("foo", parameters[1].get_name()); + ASSERT_EQ(4u, parameters[1].as_int()); + ASSERT_EQ("bar", parameters[2].get_name()); + ASSERT_EQ("this is bar", parameters[2].as_string()); + } + + { + std::vector names{"foo"}; + std::vector parameters = synchronous_client->get_parameters(names, 10s); + ASSERT_EQ(1u, parameters.size()); + ASSERT_EQ("foo", parameters[0].get_name()); + ASSERT_EQ(4u, parameters[0].as_int()); + } + + { + std::vector names{"bar"}; + std::vector parameters = synchronous_client->get_parameters(names, 10s); + ASSERT_EQ(1u, parameters.size()); + ASSERT_EQ("bar", parameters[0].get_name()); + ASSERT_EQ("this is bar", parameters[0].as_string()); + } + + { + std::vector names{"foo", "bar"}; + std::vector parameters = synchronous_client->get_parameters(names, 10s); + ASSERT_EQ(2u, parameters.size()); + ASSERT_EQ("foo", parameters[0].get_name()); + ASSERT_EQ(4u, parameters[0].as_int()); + ASSERT_EQ("bar", parameters[1].get_name()); + ASSERT_EQ("this is bar", parameters[1].as_string()); + } +} + +/* + Coverage for async describe_parameters + */ +TEST_F(TestParameterClient, async_parameter_describe_parameters) { + node->declare_parameter("foo", 4); + node->declare_parameter("bar", "this is bar"); + auto asynchronous_client = std::make_shared(node); + + { + bool callback_called = false; + auto callback = [&callback_called]( + std::shared_future> result) + { + // We expect the result to be empty since we tried to get a parameter that didn't exist. + if (result.valid() && result.get().size() == 0) { + callback_called = true; + } + }; + std::vector names{"none"}; + std::shared_future> future = + asynchronous_client->describe_parameters(names, callback); + auto return_code = rclcpp::spin_until_future_complete( + node, future, std::chrono::milliseconds(100)); + ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); + ASSERT_TRUE(callback_called); + } + + { + bool callback_called = false; + auto callback = [&callback_called]( + std::shared_future> result) + { + if (result.valid() && result.get().size() == 1) { + callback_called = true; + } + }; + std::vector names{"foo"}; + std::shared_future> future = + asynchronous_client->describe_parameters(names, callback); + auto return_code = rclcpp::spin_until_future_complete( + node, future, std::chrono::milliseconds(100)); + ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); + std::vector parameter_descs = future.get(); + ASSERT_EQ(1u, parameter_descs.size()); + ASSERT_EQ("foo", parameter_descs[0].name); + ASSERT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, parameter_descs[0].type); + ASSERT_EQ("", parameter_descs[0].description); + ASSERT_EQ("", parameter_descs[0].additional_constraints); + ASSERT_FALSE(parameter_descs[0].read_only); + ASSERT_TRUE(callback_called); + } + + { + bool callback_called = false; + auto callback = [&callback_called]( + std::shared_future> result) + { + // We expect the result to be empty since we tried to get a parameter that didn't exist. + if (result.valid() && result.get().size() == 0) { + callback_called = true; + } + }; + std::vector names{"foo", "baz"}; + std::shared_future> future = + asynchronous_client->describe_parameters(names, callback); + auto return_code = rclcpp::spin_until_future_complete( + node, future, std::chrono::milliseconds(100)); + ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); + ASSERT_TRUE(callback_called); + } + + { + bool callback_called = false; + auto callback = [&callback_called]( + std::shared_future> result) + { + // We expect the result to be empty since we tried to get a parameter that didn't exist. + if (result.valid() && result.get().size() == 0) { + callback_called = true; + } + }; + std::vector names{"baz", "foo"}; + std::shared_future> future = + asynchronous_client->describe_parameters(names, callback); + auto return_code = rclcpp::spin_until_future_complete( + node, future, std::chrono::milliseconds(100)); + ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); + ASSERT_TRUE(callback_called); + } + + { + bool callback_called = false; + auto callback = [&callback_called]( + std::shared_future> result) + { + if (result.valid() && result.get().size() == 2) { + callback_called = true; + } + }; + std::vector names{"foo", "bar"}; + std::shared_future> future = + asynchronous_client->describe_parameters(names, callback); + auto return_code = rclcpp::spin_until_future_complete( + node, future, std::chrono::milliseconds(100)); + ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); + std::vector parameter_descs = future.get(); + ASSERT_EQ(2u, parameter_descs.size()); + ASSERT_EQ("foo", parameter_descs[0].name); + ASSERT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, parameter_descs[0].type); + ASSERT_EQ("", parameter_descs[0].description); + ASSERT_EQ("", parameter_descs[0].additional_constraints); + ASSERT_FALSE(parameter_descs[0].read_only); + ASSERT_EQ("bar", parameter_descs[1].name); + ASSERT_EQ(rclcpp::ParameterType::PARAMETER_STRING, parameter_descs[1].type); + ASSERT_EQ("", parameter_descs[1].description); + ASSERT_EQ("", parameter_descs[1].additional_constraints); + ASSERT_FALSE(parameter_descs[1].read_only); + ASSERT_TRUE(callback_called); + } +} +/* + Coverage for sync describe_parameters + */ +TEST_F(TestParameterClient, sync_parameter_describe_parameters) { + node->declare_parameter("foo", 4); + node->declare_parameter("bar", "this is bar"); + auto synchronous_client = std::make_shared(node); + + { + std::vector names{"none"}; + std::vector parameter_descs = + synchronous_client->describe_parameters(names, 10s); + ASSERT_EQ(0u, parameter_descs.size()); + } + + { + std::vector names{"foo"}; + std::vector parameter_descs = + synchronous_client->describe_parameters(names, 10s); + ASSERT_EQ(1u, parameter_descs.size()); + ASSERT_EQ("foo", parameter_descs[0].name); + ASSERT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, parameter_descs[0].type); + ASSERT_EQ("", parameter_descs[0].description); + ASSERT_EQ("", parameter_descs[0].additional_constraints); + ASSERT_FALSE(parameter_descs[0].read_only); + } + + { + std::vector names{"foo", "baz"}; + std::vector parameter_descs = + synchronous_client->describe_parameters(names, 10s); + ASSERT_EQ(0u, parameter_descs.size()); + } + + { + std::vector names{"baz", "foo"}; + std::vector parameter_descs = + synchronous_client->describe_parameters(names, 10s); + ASSERT_EQ(0u, parameter_descs.size()); + } + + { + std::vector names{"foo", "bar"}; + std::vector parameter_descs = + synchronous_client->describe_parameters(names, 10s); + ASSERT_EQ(2u, parameter_descs.size()); + ASSERT_EQ("foo", parameter_descs[0].name); + ASSERT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, parameter_descs[0].type); + ASSERT_EQ("", parameter_descs[0].description); + ASSERT_EQ("", parameter_descs[0].additional_constraints); + ASSERT_FALSE(parameter_descs[0].read_only); + ASSERT_EQ("bar", parameter_descs[1].name); + ASSERT_EQ(rclcpp::ParameterType::PARAMETER_STRING, parameter_descs[1].type); + ASSERT_EQ("", parameter_descs[1].description); + ASSERT_EQ("", parameter_descs[1].additional_constraints); + ASSERT_FALSE(parameter_descs[1].read_only); + } +} + +/* + Coverage for async describe_parameters with allow_undeclared_ enabled + */ +TEST_F(TestParameterClient, async_parameter_describe_parameters_allow_undeclared) { + node_with_option->declare_parameter("foo", 4); + node_with_option->declare_parameter("bar", "this is bar"); + auto asynchronous_client = + std::make_shared(node_with_option); + + { + bool callback_called = false; + auto callback = [&callback_called]( + std::shared_future> result) + { + // We expect the result to be defaut since we tried to get a parameter that didn't exist. + if (result.valid() && result.get().size() == 1) { + callback_called = true; + } + }; + std::vector names{"none"}; + std::shared_future> future = + asynchronous_client->describe_parameters(names, callback); + auto return_code = rclcpp::spin_until_future_complete( + node_with_option, future, std::chrono::milliseconds(100)); + ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); + std::vector parameter_descs = future.get(); + ASSERT_EQ(1u, parameter_descs.size()); + ASSERT_EQ("none", parameter_descs[0].name); + ASSERT_EQ(rclcpp::ParameterType::PARAMETER_NOT_SET, parameter_descs[0].type); + ASSERT_EQ("", parameter_descs[0].description); + ASSERT_EQ("", parameter_descs[0].additional_constraints); + ASSERT_FALSE(parameter_descs[0].read_only); + ASSERT_TRUE(callback_called); + } + + { + bool callback_called = false; + auto callback = [&callback_called]( + std::shared_future> result) + { + if (result.valid() && result.get().size() == 2) { + callback_called = true; + } + }; + std::vector names{"foo", "baz"}; + std::shared_future> future = + asynchronous_client->describe_parameters(names, callback); + auto return_code = rclcpp::spin_until_future_complete( + node_with_option, future, std::chrono::milliseconds(100)); + ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); + std::vector parameter_descs = future.get(); + ASSERT_EQ(2u, parameter_descs.size()); + ASSERT_EQ("foo", parameter_descs[0].name); + ASSERT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, parameter_descs[0].type); + ASSERT_EQ("", parameter_descs[0].description); + ASSERT_EQ("", parameter_descs[0].additional_constraints); + ASSERT_FALSE(parameter_descs[0].read_only); + ASSERT_EQ("baz", parameter_descs[1].name); + ASSERT_EQ(rclcpp::ParameterType::PARAMETER_NOT_SET, parameter_descs[1].type); + ASSERT_EQ("", parameter_descs[1].description); + ASSERT_EQ("", parameter_descs[1].additional_constraints); + ASSERT_FALSE(parameter_descs[1].read_only); + ASSERT_TRUE(callback_called); + } + + { + bool callback_called = false; + auto callback = [&callback_called]( + std::shared_future> result) + { + if (result.valid() && result.get().size() == 2) { + callback_called = true; + } + }; + std::vector names{"baz", "foo"}; + std::shared_future> future = + asynchronous_client->describe_parameters(names, callback); + auto return_code = rclcpp::spin_until_future_complete( + node_with_option, future, std::chrono::milliseconds(100)); + ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); + std::vector parameter_descs = future.get(); + ASSERT_EQ(2u, parameter_descs.size()); + ASSERT_EQ("baz", parameter_descs[0].name); + ASSERT_EQ(rclcpp::ParameterType::PARAMETER_NOT_SET, parameter_descs[0].type); + ASSERT_EQ("", parameter_descs[0].description); + ASSERT_EQ("", parameter_descs[0].additional_constraints); + ASSERT_FALSE(parameter_descs[0].read_only); + ASSERT_EQ("foo", parameter_descs[1].name); + ASSERT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, parameter_descs[1].type); + ASSERT_EQ("", parameter_descs[1].description); + ASSERT_EQ("", parameter_descs[1].additional_constraints); + ASSERT_FALSE(parameter_descs[1].read_only); + ASSERT_TRUE(callback_called); + } +} +/* + Coverage for sync describe_parameters with allow_undeclared_ enabled + */ +TEST_F(TestParameterClient, sync_parameter_describe_parameters_allow_undeclared) { + node_with_option->declare_parameter("foo", 4); + node_with_option->declare_parameter("bar", "this is bar"); + auto synchronous_client = + std::make_shared(node_with_option); + + { + std::vector names{"none"}; + std::vector parameter_descs = + synchronous_client->describe_parameters(names, 10s); + ASSERT_EQ(1u, parameter_descs.size()); + ASSERT_EQ("none", parameter_descs[0].name); + ASSERT_EQ(rclcpp::ParameterType::PARAMETER_NOT_SET, parameter_descs[0].type); + ASSERT_EQ("", parameter_descs[0].description); + ASSERT_EQ("", parameter_descs[0].additional_constraints); + ASSERT_FALSE(parameter_descs[0].read_only); + } + + { + std::vector names{"foo", "baz"}; + std::vector parameter_descs = + synchronous_client->describe_parameters(names, 10s); + ASSERT_EQ(2u, parameter_descs.size()); + ASSERT_EQ("foo", parameter_descs[0].name); + ASSERT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, parameter_descs[0].type); + ASSERT_EQ("", parameter_descs[0].description); + ASSERT_EQ("", parameter_descs[0].additional_constraints); + ASSERT_FALSE(parameter_descs[0].read_only); + ASSERT_EQ("baz", parameter_descs[1].name); + ASSERT_EQ(rclcpp::ParameterType::PARAMETER_NOT_SET, parameter_descs[1].type); + ASSERT_EQ("", parameter_descs[1].description); + ASSERT_EQ("", parameter_descs[1].additional_constraints); + ASSERT_FALSE(parameter_descs[1].read_only); + } + + { + std::vector names{"baz", "foo"}; + std::vector parameter_descs = + synchronous_client->describe_parameters(names, 10s); + ASSERT_EQ(2u, parameter_descs.size()); + ASSERT_EQ("baz", parameter_descs[0].name); + ASSERT_EQ(rclcpp::ParameterType::PARAMETER_NOT_SET, parameter_descs[0].type); + ASSERT_EQ("", parameter_descs[0].description); + ASSERT_EQ("", parameter_descs[0].additional_constraints); + ASSERT_FALSE(parameter_descs[0].read_only); + ASSERT_EQ("foo", parameter_descs[1].name); + ASSERT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, parameter_descs[1].type); + ASSERT_EQ("", parameter_descs[1].description); + ASSERT_EQ("", parameter_descs[1].additional_constraints); + ASSERT_FALSE(parameter_descs[1].read_only); + } +} + +/* + Coverage for async delete_parameters + */ +TEST_F(TestParameterClient, async_parameter_delete_parameters) { + auto asynchronous_client = + std::make_shared(node_with_option); + // set parameter + auto set_future = asynchronous_client->set_parameters({rclcpp::Parameter("foo", 4)}); + rclcpp::spin_until_future_complete( + node_with_option, set_future, std::chrono::milliseconds(100)); + ASSERT_EQ(set_future.get()[0].successful, true); + // delete one parameter + auto delete_future = asynchronous_client->delete_parameters({"foo"}); + rclcpp::spin_until_future_complete( + node_with_option, delete_future, std::chrono::milliseconds(100)); + ASSERT_EQ(delete_future.get()[0].successful, true); + // check that deleted parameter isn't set + auto get_future2 = asynchronous_client->get_parameters({"foo"}); + rclcpp::spin_until_future_complete( + node_with_option, get_future2, std::chrono::milliseconds(100)); + ASSERT_EQ( + get_future2.get()[0].get_type(), + rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET); +} +/* + Coverage for sync delete_parameters + */ +TEST_F(TestParameterClient, sync_parameter_delete_parameters) { + auto synchronous_client = + std::make_shared(node_with_option); + // set parameter + auto set_result = synchronous_client->set_parameters({rclcpp::Parameter("foo", 4)}); + // delete one parameter + auto delete_result = synchronous_client->delete_parameters({"foo"}); + // check that deleted parameter isn't set + auto get_result = synchronous_client->get_parameters({"foo"}); + ASSERT_EQ( + get_result[0].get_type(), + rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET); +} + +/* + Coverage for async load_parameters + */ +TEST_F(TestParameterClient, async_parameter_load_parameters) { + const uint64_t expected_param_count = 4 + builtin_param_count; + auto load_node = std::make_shared( + "load_node", + "namespace", + rclcpp::NodeOptions().allow_undeclared_parameters(true)); + auto asynchronous_client = + std::make_shared(load_node, "/namespace/load_node"); + // load parameters + std::filesystem::path test_resources_path{TEST_RESOURCES_DIRECTORY}; + const std::string parameters_filepath = ( + test_resources_path / "test_node" / "load_parameters.yaml").string(); + auto load_future = asynchronous_client->load_parameters(parameters_filepath); + auto result_code = rclcpp::spin_until_future_complete( + load_node, load_future, std::chrono::milliseconds(100)); + ASSERT_EQ(result_code, rclcpp::FutureReturnCode::SUCCESS); + ASSERT_EQ(load_future.get()[0].successful, true); + // list parameters + auto list_parameters = asynchronous_client->list_parameters({}, 3); + rclcpp::spin_until_future_complete( + load_node, list_parameters, std::chrono::milliseconds(100)); + ASSERT_EQ(list_parameters.get().names.size(), expected_param_count); +} +/* + Coverage for sync load_parameters + */ +TEST_F(TestParameterClient, sync_parameter_load_parameters) { + const uint64_t expected_param_count = 4 + builtin_param_count; + auto load_node = std::make_shared( + "load_node", + "namespace", + rclcpp::NodeOptions().allow_undeclared_parameters(true)); + auto synchronous_client = + std::make_shared(load_node); + // load parameters + std::filesystem::path test_resources_path{TEST_RESOURCES_DIRECTORY}; + const std::string parameters_filepath = ( + test_resources_path / "test_node" / "load_parameters.yaml").string(); + auto load_future = synchronous_client->load_parameters(parameters_filepath); + ASSERT_EQ(load_future[0].successful, true); + // list parameters + auto list_parameters = synchronous_client->list_parameters({}, 3); + ASSERT_EQ(list_parameters.names.size(), static_cast(expected_param_count)); +} + +/* + Coverage for async load_parameters with complicated regex expression + */ +TEST_F(TestParameterClient, async_parameter_load_parameters_complicated_regex) { + const uint64_t expected_param_count = 5 + builtin_param_count; + auto load_node = std::make_shared( + "load_node", + "namespace", + rclcpp::NodeOptions().allow_undeclared_parameters(true)); + auto asynchronous_client = + std::make_shared(load_node, "/namespace/load_node"); + // load parameters + std::filesystem::path test_resources_path{TEST_RESOURCES_DIRECTORY}; + const std::string parameters_filepath = ( + test_resources_path / "test_node" / "load_complicated_parameters.yaml").string(); + auto load_future = asynchronous_client->load_parameters(parameters_filepath); + auto result_code = rclcpp::spin_until_future_complete( + load_node, load_future, std::chrono::milliseconds(100)); + ASSERT_EQ(result_code, rclcpp::FutureReturnCode::SUCCESS); + ASSERT_EQ(load_future.get()[0].successful, true); + // list parameters + auto list_parameters = asynchronous_client->list_parameters({}, 3); + rclcpp::spin_until_future_complete( + load_node, list_parameters, std::chrono::milliseconds(100)); + ASSERT_EQ(list_parameters.get().names.size(), expected_param_count); + // to check the parameter "a_value" + std::string param_name = "a_value"; + auto param = load_node->get_parameter(param_name); + ASSERT_EQ(param.get_value(), "last_one_win"); +} + +/* + Coverage for async load_parameters to load file without valid parameters + */ +TEST_F(TestParameterClient, async_parameter_load_no_valid_parameter) { + auto load_node = std::make_shared( + "load_node", + "namespace", + rclcpp::NodeOptions().allow_undeclared_parameters(true)); + auto asynchronous_client = + std::make_shared(load_node, "/namespace/load_node"); + // load parameters + std::filesystem::path test_resources_path{TEST_RESOURCES_DIRECTORY}; + const std::string parameters_filepath = ( + test_resources_path / "test_node" / "no_valid_parameters.yaml").string(); + EXPECT_THROW( + asynchronous_client->load_parameters(parameters_filepath), + rclcpp::exceptions::InvalidParametersException); +} + +/* + Coverage for async load_parameters from maps with complicated regex expression + */ +TEST_F(TestParameterClient, async_parameter_load_parameters_from_map) { + const uint64_t expected_param_count = 5 + builtin_param_count; + auto load_node = std::make_shared( + "load_node", + "namespace", + rclcpp::NodeOptions().allow_undeclared_parameters(true)); + auto asynchronous_client = + std::make_shared(load_node, "/namespace/load_node"); + // load parameters + rclcpp::ParameterMap parameter_map = { + {"/**", + { + {"bar", 5}, + {"foo", 3.5}, + {"a_value", "first"} + } + }, + {"/*/load_node", + { + {"bar_foo", "ok"}, + {"a_value", "second"} + } + }, + {"/namespace/load_node", + { + {"foo_bar", true}, + {"a_value", "third"} + } + }, + {"/bar", + { + {"fatal", 10} + } + }, + {"/**/namespace/*", + { + {"a_value", "not_win"} + } + } + }; + + auto load_future = asynchronous_client->load_parameters(parameter_map); + auto result_code = rclcpp::spin_until_future_complete( + load_node, load_future, std::chrono::milliseconds(100)); + ASSERT_EQ(result_code, rclcpp::FutureReturnCode::SUCCESS); + ASSERT_EQ(load_future.get()[0].successful, true); + // list parameters + auto list_parameters = asynchronous_client->list_parameters({}, 3); + rclcpp::spin_until_future_complete( + load_node, list_parameters, std::chrono::milliseconds(100)); + ASSERT_EQ(list_parameters.get().names.size(), expected_param_count); + // to check the parameter "a_value" + std::string param_name = "a_value"; + auto param = load_node->get_parameter(param_name); + // rclcpp::ParameterMap is an unordered map, no guarantee which value will be set for `a_value`. + EXPECT_THAT( + (std::array{"first", "second", "third", "not_win"}), + testing::Contains(param.get_value())); +} + +/* + Coverage for async load_parameters from maps without valid parameters + */ +TEST_F(TestParameterClient, async_parameter_load_from_map_no_valid_parameter) { + auto load_node = std::make_shared( + "load_node", + "namespace", + rclcpp::NodeOptions().allow_undeclared_parameters(true)); + auto asynchronous_client = + std::make_shared(load_node, "/namespace/load_node"); + // load parameters + rclcpp::ParameterMap parameter_map = { + {"/no/valid/parameters/node", + { + {"bar", 5}, + {"bar", 3.5} + } + } + }; + EXPECT_THROW( + asynchronous_client->load_parameters(parameter_map), + rclcpp::exceptions::InvalidParametersException); +} diff --git a/rclcpp/test/rclcpp/test_parameter_event_handler.cpp b/rclcpp/test/rclcpp/test_parameter_event_handler.cpp new file mode 100644 index 0000000000..0cf4a7a445 --- /dev/null +++ b/rclcpp/test/rclcpp/test_parameter_event_handler.cpp @@ -0,0 +1,445 @@ +// Copyright 2019 Intel Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" + +class TestParameterEventHandler : public rclcpp::ParameterEventHandler +{ +public: + explicit TestParameterEventHandler(rclcpp::Node::SharedPtr node) + : ParameterEventHandler(node) + {} + + void test_event(rcl_interfaces::msg::ParameterEvent::ConstSharedPtr event) + { + callbacks_->event_callback(*event); + } + + size_t num_event_callbacks() + { + return callbacks_->event_callbacks_.size(); + } + + size_t num_parameter_callbacks() + { + return callbacks_->parameter_callbacks_.size(); + } +}; + +class TestNode : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + void SetUp() + { + rclcpp::NodeOptions options; + node = std::make_shared( + "test_parameter_events_subscriber", options); + + remote_node_name = "/remote_node"; + diff_ns_name = "/ns/remote_node"; + + param_handler = std::make_shared(node); + + same_node_int = std::make_shared(); + same_node_double = std::make_shared(); + multiple = std::make_shared(); + remote_node_string = std::make_shared(); + diff_ns_bool = std::make_shared(); + diff_node_int = std::make_shared(); + + same_node_int->node = node->get_fully_qualified_name(); + same_node_double->node = node->get_fully_qualified_name(); + multiple->node = node->get_fully_qualified_name(); + remote_node_string->node = remote_node_name; + diff_ns_bool->node = diff_ns_name; + diff_node_int->node = remote_node_name; + + rcl_interfaces::msg::Parameter p; + p.name = "my_int"; + p.value.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + p.value.integer_value = 1; + same_node_int->changed_parameters.push_back(p); + diff_node_int->changed_parameters.push_back(p); + multiple->changed_parameters.push_back(p); + + p.name = "my_double"; + p.value.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + p.value.double_value = 1.0; + same_node_double->changed_parameters.push_back(p); + multiple->changed_parameters.push_back(p); + + p.name = "my_string"; + p.value.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + p.value.string_value = "test"; + remote_node_string->changed_parameters.push_back(p); + + p.name = "my_bool"; + p.value.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + p.value.bool_value = true; + diff_ns_bool->changed_parameters.push_back(p); + } + + void TearDown() + { + node.reset(); + param_handler.reset(); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + rcl_interfaces::msg::ParameterEvent::SharedPtr same_node_int; + rcl_interfaces::msg::ParameterEvent::SharedPtr same_node_double; + rcl_interfaces::msg::ParameterEvent::SharedPtr diff_node_int; + rcl_interfaces::msg::ParameterEvent::SharedPtr remote_node_string; + rcl_interfaces::msg::ParameterEvent::SharedPtr multiple; + rcl_interfaces::msg::ParameterEvent::SharedPtr diff_ns_bool; + + rclcpp::Node::SharedPtr node; + std::string remote_node_name; + std::string diff_ns_name; + std::shared_ptr param_handler; +}; + +TEST_F(TestNode, RegisterParameterCallback) +{ + bool received; + auto cb = [&received](const rclcpp::Parameter &) {received = true;}; + + auto h1 = param_handler->add_parameter_callback("my_double", cb); + auto h2 = param_handler->add_parameter_callback("my_int", cb); + auto h3 = param_handler->add_parameter_callback("my_string", cb, remote_node_name); + auto h4 = param_handler->add_parameter_callback("my_bool", cb, diff_ns_name); + + received = false; + param_handler->test_event(same_node_double); + EXPECT_EQ(received, true); + + received = false; + param_handler->test_event(same_node_int); + EXPECT_EQ(received, true); + + received = false; + param_handler->test_event(remote_node_string); + EXPECT_EQ(received, true); + + received = false; + param_handler->test_event(diff_ns_bool); + EXPECT_EQ(received, true); +} + +TEST_F(TestNode, SameParameterDifferentNode) +{ + int64_t int_param_node1{0}; + int64_t int_param_node2{0}; + + auto cb1 = [&int_param_node1](const rclcpp::Parameter & p) { + int_param_node1 = p.get_value(); + }; + auto cb2 = [&int_param_node2](const rclcpp::Parameter & p) { + int_param_node2 = p.get_value(); + }; + + // Set individual parameters + auto h1 = param_handler->add_parameter_callback("my_int", cb1); + auto h2 = param_handler->add_parameter_callback("my_int", cb2, remote_node_name); + + param_handler->test_event(same_node_int); + EXPECT_EQ(int_param_node1, 1); + EXPECT_NE(int_param_node2, 1); + + int_param_node1 = 0; + int_param_node2 = 0; + + param_handler->test_event(diff_node_int); + EXPECT_NE(int_param_node1, 1); + EXPECT_EQ(int_param_node2, 1); + + param_handler->remove_parameter_callback(h1); + param_handler->remove_parameter_callback(h2); + EXPECT_EQ(param_handler->num_parameter_callbacks(), 0UL); +} + +TEST_F(TestNode, GetParameterFromEvent) +{ + using rclcpp::ParameterEventHandler; + std::string node_name = node->get_fully_qualified_name(); + std::string wrong_name = "/wrong_node_name"; + + rclcpp::Parameter p; + EXPECT_TRUE( + ParameterEventHandler::get_parameter_from_event(*multiple, p, "my_int", node_name)); + EXPECT_EQ(p.get_value(), 1); + // False if parameter not with correct node name + EXPECT_FALSE( + ParameterEventHandler::get_parameter_from_event(*multiple, p, "my_int", wrong_name)); + // False if parameter not part of event + EXPECT_FALSE( + ParameterEventHandler::get_parameter_from_event(*diff_ns_bool, p, "my_int", node_name)); + + EXPECT_NO_THROW( + ParameterEventHandler::get_parameter_from_event(*multiple, "my_int", node_name)); + // Throws if parameter not with correct node name + EXPECT_THROW( + ParameterEventHandler::get_parameter_from_event(*multiple, "my_int", wrong_name), + std::runtime_error); + + // Parameter not part of event + // with correct node + rclcpp::Parameter expect_notset_ret("my_notset", rclcpp::PARAMETER_NOT_SET); + rclcpp::Parameter ret; + EXPECT_NO_THROW( + ret = ParameterEventHandler::get_parameter_from_event(*multiple, "my_notset", node_name);); + EXPECT_EQ(ret, expect_notset_ret); + // with incorrect node + EXPECT_THROW( + ParameterEventHandler::get_parameter_from_event(*multiple, "my_notset", wrong_name), + std::runtime_error); +} + +TEST_F(TestNode, GetParametersFromEvent) +{ + using rclcpp::ParameterEventHandler; + std::string node_name = node->get_fully_qualified_name(); + + auto params = ParameterEventHandler::get_parameters_from_event(*multiple); + EXPECT_EQ(params.size(), 2u); + bool found_int = false; + bool found_double = false; + for (auto & p : params) { + if (p.get_name() == std::string("my_int")) { + found_int = true; + EXPECT_EQ(p.get_value(), 1); + } else if (p.get_name() == std::string("my_double")) { + found_double = true; + EXPECT_EQ(p.get_value(), 1.0); + } + } + EXPECT_EQ(found_int, true); + EXPECT_EQ(found_double, true); + + params = ParameterEventHandler::get_parameters_from_event(*remote_node_string); + EXPECT_EQ(params.size(), 1u); + bool found_string = false; + for (auto & p : params) { + if (p.get_name() == std::string("my_string")) { + found_string = true; + EXPECT_EQ(p.get_value(), std::string("test")); + } + } + EXPECT_EQ(found_string, true); + + params = ParameterEventHandler::get_parameters_from_event(*diff_ns_bool); + EXPECT_EQ(params.size(), 1u); + bool found_bool = false; + for (auto & p : params) { + if (p.get_name() == std::string("my_bool")) { + found_bool = true; + EXPECT_EQ(p.get_value(), true); + } + } + EXPECT_EQ(found_bool, true); +} + +TEST_F(TestNode, EventCallback) +{ + using rclcpp::ParameterEventHandler; + + double double_param = 0.0; + int64_t int_param = 0; + bool bool_param{false}; + bool received{false}; + + double product; + auto cb = + [&int_param, &double_param, &product, &received, + this](const rcl_interfaces::msg::ParameterEvent & event) + { + auto node_name = node->get_fully_qualified_name(); + + if (event.node == node_name) { + received = true; + } + + rclcpp::Parameter p; + if (ParameterEventHandler::get_parameter_from_event(event, p, "my_int", node_name)) { + int_param = p.get_value(); + } + try { + p = ParameterEventHandler::get_parameter_from_event(event, "my_double", node_name); + double_param = p.get_value(); + } catch (...) { + } + + product = static_cast(int_param) * double_param; + }; + + auto cb2 = + [&bool_param, this](const rcl_interfaces::msg::ParameterEvent & event) + { + rclcpp::Parameter p; + if (event.node == diff_ns_name) { + if (ParameterEventHandler::get_parameter_from_event( + event, p, "my_bool", diff_ns_name)) + { + bool_param = p.get_value(); + } + } + }; + + auto event_handle1 = param_handler->add_parameter_event_callback(cb); + auto event_handle2 = param_handler->add_parameter_event_callback(cb2); + + bool_param = false; + param_handler->test_event(multiple); + EXPECT_EQ(received, true); + EXPECT_EQ(product, 1.0); + EXPECT_EQ(bool_param, false); + + param_handler->test_event(diff_ns_bool); + EXPECT_EQ(bool_param, true); + + // Test removal of event callback + received = false; + bool_param = false; + param_handler->remove_parameter_event_callback(event_handle1); + param_handler->test_event(multiple); + param_handler->test_event(diff_ns_bool); + EXPECT_EQ(received, false); + EXPECT_EQ(bool_param, true); + + // Should throw if callback handle no longer exists or already removed + EXPECT_THROW( + param_handler->remove_parameter_event_callback(event_handle1), std::runtime_error); +} + +TEST_F(TestNode, MultipleParameterCallbacks) +{ + bool received_1{false}; + bool received_2{false}; + + auto cb1 = [&received_1](const rclcpp::Parameter &) {received_1 = true;}; + auto cb2 = [&received_2](const rclcpp::Parameter &) {received_2 = true;}; + auto cb3 = [](const rclcpp::Parameter &) { /*do nothing*/}; + + auto h1 = param_handler->add_parameter_callback("my_int", cb1); + auto h2 = param_handler->add_parameter_callback("my_int", cb2); + auto h3 = param_handler->add_parameter_callback("my_double", cb3); + + // Test multiple callbacks per parameter + param_handler->test_event(same_node_int); + EXPECT_EQ(received_1, true); + EXPECT_EQ(received_2, true); + + // Test removal of parameter callback by callback handle + received_1 = false; + received_2 = false; + param_handler->remove_parameter_callback(h1); + param_handler->test_event(same_node_int); + EXPECT_EQ(received_1, false); + EXPECT_EQ(received_2, true); + + // Test removal of parameter callback by name + received_2 = false; + param_handler->remove_parameter_callback(h2); + param_handler->test_event(same_node_int); + EXPECT_EQ(received_2, false); + + // Should throw if callback handle no longer exists or already removed + EXPECT_THROW(param_handler->remove_parameter_callback(h1), std::runtime_error); + EXPECT_THROW(param_handler->remove_parameter_callback(h2), std::runtime_error); + + param_handler->remove_parameter_callback(h3); + + // All callbacks should have been removed + EXPECT_EQ(received_2, 0); + EXPECT_EQ(param_handler->num_event_callbacks(), 0UL); +} + +TEST_F(TestNode, LastInFirstCallForParameterCallbacks) +{ + rclcpp::Time time_1; + rclcpp::Time time_2; + + // The callbacks will log the current time for comparison purposes. Add a bit of a stall + // to ensure that the time noted in the back-to-back calls isn't the same + auto cb1 = [this, &time_1](const rclcpp::Parameter &) { + time_1 = node->now(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + }; + auto cb2 = [this, &time_2](const rclcpp::Parameter &) { + time_2 = node->now(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + }; + + auto h1 = param_handler->add_parameter_callback("my_int", cb1); + auto h2 = param_handler->add_parameter_callback("my_int", cb2); + + // Test multiple callbacks per parameter + param_handler->test_event(same_node_int); + + // The most-recently install handler should be called first + EXPECT_EQ(time_2 < time_1, true); + + param_handler->remove_parameter_callback(h1); + param_handler->remove_parameter_callback(h2); + EXPECT_EQ(param_handler->num_parameter_callbacks(), 0UL); +} + +TEST_F(TestNode, LastInFirstCallForParameterEventCallbacks) +{ + rclcpp::Time time_1; + rclcpp::Time time_2; + + // The callbacks will log the current time for comparison purposes. Add a bit of a stall + // to ensure that the time noted in the back-to-back calls isn't the same + auto cb1 = + [this, &time_1](const rcl_interfaces::msg::ParameterEvent &) + { + time_1 = node->now(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + }; + auto cb2 = + [this, &time_2](const rcl_interfaces::msg::ParameterEvent &) + { + time_2 = node->now(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + }; + + auto h1 = param_handler->add_parameter_event_callback(cb1); + auto h2 = param_handler->add_parameter_event_callback(cb2); + + // Test multiple callbacks per parameter + param_handler->test_event(same_node_int); + + // The most-recently install handler should be called first + EXPECT_EQ(time_2 < time_1, true); + + param_handler->remove_parameter_event_callback(h1); + param_handler->remove_parameter_event_callback(h2); + EXPECT_EQ(param_handler->num_event_callbacks(), 0UL); +} diff --git a/rclcpp/test/test_parameter_events_filter.cpp b/rclcpp/test/rclcpp/test_parameter_events_filter.cpp similarity index 100% rename from rclcpp/test/test_parameter_events_filter.cpp rename to rclcpp/test/rclcpp/test_parameter_events_filter.cpp diff --git a/rclcpp/test/rclcpp/test_parameter_map.cpp b/rclcpp/test/rclcpp/test_parameter_map.cpp new file mode 100644 index 0000000000..0158b7c7e7 --- /dev/null +++ b/rclcpp/test/rclcpp/test_parameter_map.cpp @@ -0,0 +1,485 @@ +// 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 +#include +#include +#include + +#include "rclcpp/parameter_map.hpp" + +rcl_params_t * +make_params(std::vector node_names) +{ + rcl_allocator_t alloc = rcl_get_default_allocator(); + rcl_params_t * c_params = rcl_yaml_node_struct_init(alloc); + c_params->num_nodes = node_names.size(); + c_params->allocator = alloc; + if (c_params->num_nodes) { + // Copy node names + for (size_t n = 0; n < node_names.size(); ++n) { + c_params->node_names[n] = static_cast(alloc.allocate( + sizeof(char) * (node_names[n].size() + 1), alloc.state)); + std::snprintf(c_params->node_names[n], node_names[n].size() + 1, "%s", node_names[n].c_str()); + } + // zero init node params + for (size_t n = 0; n < node_names.size(); ++n) { + c_params->params[n].parameter_names = NULL; + c_params->params[n].parameter_values = NULL; + c_params->params[n].num_params = 0; + } + } + return c_params; +} + +void +make_node_params(rcl_params_t * c_params, size_t node_idx, std::vector param_names) +{ + rcl_allocator_t alloc = c_params->allocator; + ASSERT_LT(node_idx, c_params->num_nodes); + ASSERT_GT(param_names.size(), 0u); + + rcl_node_params_s * c_node_params = &(c_params->params[node_idx]); + c_node_params->num_params = param_names.size(); + + // Copy parameter names + c_node_params->parameter_names = static_cast( + alloc.allocate(sizeof(char *) * param_names.size(), alloc.state)); + for (size_t p = 0; p < param_names.size(); ++p) { + const std::string & param_name = param_names[p]; + c_node_params->parameter_names[p] = static_cast(alloc.allocate( + sizeof(char) * (param_name.size() + 1), alloc.state)); + std::snprintf( + c_node_params->parameter_names[p], param_name.size() + 1, "%s", param_name.c_str()); + } + // zero init parameter value + c_node_params->parameter_values = static_cast(alloc.allocate( + sizeof(rcl_variant_t) * param_names.size(), alloc.state)); + for (size_t p = 0; p < param_names.size(); ++p) { + c_node_params->parameter_values[p].bool_value = NULL; + c_node_params->parameter_values[p].integer_value = NULL; + c_node_params->parameter_values[p].double_value = NULL; + c_node_params->parameter_values[p].string_value = NULL; + c_node_params->parameter_values[p].byte_array_value = NULL; + c_node_params->parameter_values[p].bool_array_value = NULL; + c_node_params->parameter_values[p].integer_array_value = NULL; + c_node_params->parameter_values[p].double_array_value = NULL; + c_node_params->parameter_values[p].string_array_value = NULL; + } +} + +TEST(Test_parameter_map_from, null_c_parameter) +{ + EXPECT_THROW(rclcpp::parameter_map_from(NULL), rclcpp::exceptions::InvalidParametersException); +} + +TEST(Test_parameter_map_from, null_node_names) +{ + rcl_params_t * c_params = make_params({}); + c_params->num_nodes = 1; + + EXPECT_THROW( + rclcpp::parameter_map_from(c_params), rclcpp::exceptions::InvalidParametersException); + + c_params->num_nodes = 0; + rcl_yaml_node_struct_fini(c_params); +} + +TEST(Test_parameter_map_from, null_node_params) +{ + rcl_params_t * c_params = make_params({"foo"}); + std::snprintf(c_params->node_names[0], 3 + 1, "foo"); + auto allocated_params = c_params->params; + c_params->params = NULL; + + EXPECT_THROW( + rclcpp::parameter_map_from(c_params), rclcpp::exceptions::InvalidParametersException); + + c_params->params = allocated_params; + rcl_yaml_node_struct_fini(c_params); +} + +TEST(Test_parameter_map_from, null_node_name_in_node_names) +{ + rcl_params_t * c_params = make_params({"foo"}); + auto allocated_name = c_params->node_names[0]; + c_params->node_names[0] = NULL; + + EXPECT_THROW( + rclcpp::parameter_map_from(c_params), rclcpp::exceptions::InvalidParametersException); + + c_params->node_names[0] = allocated_name; + rcl_yaml_node_struct_fini(c_params); +} + +TEST(Test_parameter_map_from, null_node_param_value) +{ + rcl_params_t * c_params = make_params({"foo"}); + make_node_params(c_params, 0, {"bar"}); + + EXPECT_THROW( + rclcpp::parameter_map_from(c_params), rclcpp::exceptions::InvalidParameterValueException); + + rcl_yaml_node_struct_fini(c_params); +} + +TEST(Test_parameter_map_from, null_node_param_name) +{ + rcl_params_t * c_params = make_params({"foo"}); + make_node_params(c_params, 0, {"bar"}); + auto allocated_name = c_params->params[0].parameter_names[0]; + c_params->params[0].parameter_names[0] = NULL; + + EXPECT_THROW( + rclcpp::parameter_map_from(c_params), rclcpp::exceptions::InvalidParametersException); + + c_params->params[0].parameter_names[0] = allocated_name; + rcl_yaml_node_struct_fini(c_params); +} + +TEST(Test_parameter_map_from, bool_param_value) +{ + rcl_params_t * c_params = make_params({"foo"}); + make_node_params(c_params, 0, {"true_bool", "false_bool"}); + bool true_bool = true; + bool false_bool = false; + c_params->params[0].parameter_values[0].bool_value = &true_bool; + c_params->params[0].parameter_values[1].bool_value = &false_bool; + + rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params); + const std::vector & params = map.at("/foo"); + EXPECT_STREQ("true_bool", params.at(0).get_name().c_str()); + EXPECT_TRUE(params.at(0).get_value()); + EXPECT_STREQ("false_bool", params.at(1).get_name().c_str()); + EXPECT_FALSE(params.at(1).get_value()); + + c_params->params[0].parameter_values[0].bool_value = NULL; + c_params->params[0].parameter_values[1].bool_value = NULL; + rcl_yaml_node_struct_fini(c_params); +} + +TEST(Test_parameter_map_from, integer_param_value) +{ + rcl_params_t * c_params = make_params({"bar"}); + make_node_params(c_params, 0, {"positive.int", "negative.int"}); + int64_t positive_int = 42; + int64_t negative_int = -12345; + c_params->params[0].parameter_values[0].integer_value = &positive_int; + c_params->params[0].parameter_values[1].integer_value = &negative_int; + + rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params); + const std::vector & params = map.at("/bar"); + EXPECT_STREQ("positive.int", params.at(0).get_name().c_str()); + EXPECT_EQ(42, params.at(0).get_value()); + EXPECT_STREQ("negative.int", params.at(1).get_name().c_str()); + EXPECT_EQ(-12345, params.at(1).get_value()); + + c_params->params[0].parameter_values[0].integer_value = NULL; + c_params->params[0].parameter_values[1].integer_value = NULL; + rcl_yaml_node_struct_fini(c_params); +} + +TEST(Test_parameter_map_from, double_param_value) +{ + rcl_params_t * c_params = make_params({"foo/bar"}); + make_node_params(c_params, 0, {"positive.double", "negative.double"}); + double positive_double = 3.14; + double negative_double = -2.718; + c_params->params[0].parameter_values[0].double_value = &positive_double; + c_params->params[0].parameter_values[1].double_value = &negative_double; + + rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params); + const std::vector & params = map.at("/foo/bar"); + EXPECT_STREQ("positive.double", params.at(0).get_name().c_str()); + EXPECT_DOUBLE_EQ(3.14, params.at(0).get_value()); + EXPECT_STREQ("negative.double", params.at(1).get_name().c_str()); + EXPECT_DOUBLE_EQ(-2.718, params.at(1).get_value()); + + c_params->params[0].parameter_values[0].double_value = NULL; + c_params->params[0].parameter_values[1].double_value = NULL; + rcl_yaml_node_struct_fini(c_params); +} + +TEST(Test_parameter_map_from, string_param_value) +{ + rcl_params_t * c_params = make_params({"/foo/bar"}); + make_node_params(c_params, 0, {"string_param"}); + std::string hello_world = "hello world"; + char * c_hello_world = new char[hello_world.length() + 1]; + std::snprintf(c_hello_world, hello_world.size() + 1, "%s", hello_world.c_str()); + c_params->params[0].parameter_values[0].string_value = c_hello_world; + + rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params); + const std::vector & params = map.at("/foo/bar"); + EXPECT_STREQ("string_param", params.at(0).get_name().c_str()); + EXPECT_STREQ(hello_world.c_str(), params.at(0).get_value().c_str()); + + c_params->params[0].parameter_values[0].string_value = NULL; + delete[] c_hello_world; + rcl_yaml_node_struct_fini(c_params); +} + +#define MAKE_ARRAY_VALUE(VAR, TYPE, V1, V2) \ + do { \ + VAR.values = new TYPE[2]; \ + VAR.size = 2; \ + VAR.values[0] = V1; \ + VAR.values[1] = V2; \ + } while (false) + +#define FREE_ARRAY_VALUE(VAR) \ + do { \ + delete[] VAR.values; \ + } while (false) + +TEST(Test_parameter_map_from, byte_array_param_value) +{ + rcl_params_t * c_params = make_params({"/foobar"}); + make_node_params(c_params, 0, {"byte_array_param"}); + rcl_byte_array_t c_byte_array; + MAKE_ARRAY_VALUE(c_byte_array, uint8_t, 0xf0, 0xaa); + c_params->params[0].parameter_values[0].byte_array_value = &c_byte_array; + + rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params); + const std::vector & params = map.at("/foobar"); + EXPECT_STREQ("byte_array_param", params.at(0).get_name().c_str()); + std::vector byte_array = params.at(0).get_value>(); + ASSERT_EQ(2u, byte_array.size()); + EXPECT_EQ(0xf0, byte_array.at(0)); + EXPECT_EQ(0xaa, byte_array.at(1)); + + c_params->params[0].parameter_values[0].byte_array_value = NULL; + FREE_ARRAY_VALUE(c_byte_array); + rcl_yaml_node_struct_fini(c_params); +} + +TEST(Test_parameter_map_from, bool_array_param_value) +{ + rcl_params_t * c_params = make_params({"foo/bar/baz"}); + make_node_params(c_params, 0, {"bool_array_param"}); + rcl_bool_array_t c_bool_array; + MAKE_ARRAY_VALUE(c_bool_array, bool, true, false); + c_params->params[0].parameter_values[0].bool_array_value = &c_bool_array; + + rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params); + const std::vector & params = map.at("/foo/bar/baz"); + EXPECT_STREQ("bool_array_param", params.at(0).get_name().c_str()); + std::vector bool_array = params.at(0).get_value>(); + ASSERT_EQ(2u, bool_array.size()); + EXPECT_TRUE(bool_array.at(0)); + EXPECT_FALSE(bool_array.at(1)); + + c_params->params[0].parameter_values[0].bool_array_value = NULL; + FREE_ARRAY_VALUE(c_bool_array); + rcl_yaml_node_struct_fini(c_params); +} + +TEST(Test_parameter_map_from, integer_array_param_value) +{ + rcl_params_t * c_params = make_params({"foo"}); + make_node_params(c_params, 0, {"integer_array_param"}); + rcl_int64_array_t c_integer_array; + MAKE_ARRAY_VALUE(c_integer_array, int64_t, 42, -12345); + c_params->params[0].parameter_values[0].integer_array_value = &c_integer_array; + + rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params); + const std::vector & params = map.at("/foo"); + EXPECT_STREQ("integer_array_param", params.at(0).get_name().c_str()); + std::vector integer_array = params.at(0).get_value>(); + ASSERT_EQ(2u, integer_array.size()); + EXPECT_EQ(42, integer_array.at(0)); + EXPECT_EQ(-12345, integer_array.at(1)); + + c_params->params[0].parameter_values[0].integer_array_value = NULL; + FREE_ARRAY_VALUE(c_integer_array); + rcl_yaml_node_struct_fini(c_params); +} + +TEST(Test_parameter_map_from, double_array_param_value) +{ + rcl_params_t * c_params = make_params({"foo"}); + make_node_params(c_params, 0, {"double_array_param"}); + rcl_double_array_t c_double_array; + MAKE_ARRAY_VALUE(c_double_array, double, 3.14, -2.718); + c_params->params[0].parameter_values[0].double_array_value = &c_double_array; + + rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params); + const std::vector & params = map.at("/foo"); + EXPECT_STREQ("double_array_param", params.at(0).get_name().c_str()); + std::vector double_array = params.at(0).get_value>(); + ASSERT_EQ(2u, double_array.size()); + EXPECT_DOUBLE_EQ(3.14, double_array.at(0)); + EXPECT_DOUBLE_EQ(-2.718, double_array.at(1)); + + c_params->params[0].parameter_values[0].double_array_value = NULL; + FREE_ARRAY_VALUE(c_double_array); + rcl_yaml_node_struct_fini(c_params); +} + +TEST(Test_parameter_map_from, string_array_param_value) +{ + rcl_params_t * c_params = make_params({"foo"}); + make_node_params(c_params, 0, {"string_array_param"}); + rcutils_string_array_t c_string_array = rcutils_get_zero_initialized_string_array(); + ASSERT_EQ(RCUTILS_RET_OK, rcutils_string_array_init(&c_string_array, 2, &(c_params->allocator))); + c_string_array.data[0] = rcutils_strdup("Hello", c_params->allocator); + c_string_array.data[1] = rcutils_strdup("World", c_params->allocator); + c_params->params[0].parameter_values[0].string_array_value = &c_string_array; + + rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params); + const std::vector & params = map.at("/foo"); + EXPECT_STREQ("string_array_param", params.at(0).get_name().c_str()); + std::vector string_array = params.at(0).get_value>(); + ASSERT_EQ(2u, string_array.size()); + EXPECT_STREQ("Hello", string_array.at(0).c_str()); + EXPECT_STREQ("World", string_array.at(1).c_str()); + + EXPECT_EQ(RCUTILS_RET_OK, rcutils_string_array_fini(&c_string_array)); + c_params->params[0].parameter_values[0].string_array_value = NULL; + rcl_yaml_node_struct_fini(c_params); +} + +TEST(Test_parameter_map_from, one_node_one_param_by_node_fqn) +{ + rcl_params_t * c_params = make_params({"foo"}); + make_node_params(c_params, 0, {"string_param"}); + + std::string hello_world = "hello world"; + char * c_hello_world = new char[hello_world.length() + 1]; + std::snprintf(c_hello_world, hello_world.size() + 1, "%s", hello_world.c_str()); + c_params->params[0].parameter_values[0].string_value = c_hello_world; + + rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params, "/foo"); + const std::vector & params = map.at("/foo"); + EXPECT_STREQ("string_param", params.at(0).get_name().c_str()); + EXPECT_STREQ(hello_world.c_str(), params.at(0).get_value().c_str()); + + c_params->params[0].parameter_values[0].string_value = NULL; + delete[] c_hello_world; + rcl_yaml_node_struct_fini(c_params); +} + +TEST(Test_parameter_map_from, multi_nodes_same_param_name_by_node_fqn) +{ + std::vector node_names_keys = { + "/**", // index: 0 + "/*", // index: 1 + "/**/node", // index: 2 + "/*/node", // index: 3 + "/ns/node" // index: 4 + }; + + rcl_params_t * c_params = make_params(node_names_keys); + + std::vector param_values; + for (size_t i = 0; i < node_names_keys.size(); ++i) { + make_node_params(c_params, i, {"string_param"}); + std::string hello_world = "hello world" + std::to_string(i); + char * c_hello_world = new char[hello_world.length() + 1]; + std::snprintf(c_hello_world, hello_world.size() + 1, "%s", hello_world.c_str()); + c_params->params[i].parameter_values[0].string_value = c_hello_world; + param_values.push_back(c_hello_world); + } + + std::unordered_map> node_fqn_expected = { + {"/ns/foo/another_node", {0}}, + {"/another", {0, 1}}, + {"/node", {0, 1, 2}}, + {"/another_ns/node", {0, 2, 3}}, + {"/ns/node", {0, 2, 3, 4}}, + }; + + for (auto & kv : node_fqn_expected) { + rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params, kv.first.c_str()); + const std::vector & params = map.at(kv.first); + + EXPECT_EQ(kv.second.size(), params.size()); + for (size_t i = 0; i < params.size(); ++i) { + std::string param_value = "hello world" + std::to_string(kv.second[i]); + EXPECT_STREQ("string_param", params.at(i).get_name().c_str()); + EXPECT_STREQ(param_value.c_str(), params.at(i).get_value().c_str()); + } + } + + for (size_t i = 0; i < node_names_keys.size(); ++i) { + c_params->params[i].parameter_values[0].string_value = NULL; + } + for (auto c_hello_world : param_values) { + delete[] c_hello_world; + } + rcl_yaml_node_struct_fini(c_params); +} + +TEST(Test_parameter_map_from, multi_nodes_diff_param_name_by_node_fqn) +{ + std::vector node_names_keys = { + "/**", // index: 0 + "/*", // index: 1 + "/**/node", // index: 2 + "/*/node", // index: 3 + "/ns/**", // index: 4 + "/ns/*", // index: 5 + "/ns/**/node", // index: 6 + "/ns/*/node", // index: 7 + "/ns/**/a/*/node", // index: 8 + "/ns/node" // index: 9 + }; + + rcl_params_t * c_params = make_params(node_names_keys); + + for (size_t i = 0; i < node_names_keys.size(); ++i) { + std::string param_name = "string_param" + std::to_string(i); + make_node_params(c_params, i, {param_name}); + } + + std::string hello_world = "hello world"; + char * c_hello_world = new char[hello_world.length() + 1]; + std::snprintf(c_hello_world, hello_world.size() + 1, "%s", hello_world.c_str()); + + for (size_t i = 0; i < node_names_keys.size(); ++i) { + c_params->params[i].parameter_values[0].string_value = c_hello_world; + } + + std::unordered_map> node_fqn_expected = { + {"/ns/node", {0, 2, 3, 4, 5, 6, 9}}, + {"/node", {0, 1, 2}}, + {"/ns/foo/node", {0, 2, 4, 6, 7}}, + {"/ns/foo/a/node", {0, 2, 4, 6}}, + {"/ns/foo/a/bar/node", {0, 2, 4, 6, 8}}, + {"/ns/a/bar/node", {0, 2, 4, 6, 8}}, + {"/ns/foo/zoo/a/bar/node", {0, 2, 4, 6, 8}}, + }; + + for (auto & kv : node_fqn_expected) { + rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params, kv.first.c_str()); + const std::vector & params = map.at(kv.first); + EXPECT_EQ(kv.second.size(), params.size()); + for (size_t i = 0; i < params.size(); ++i) { + std::string param_name = "string_param" + std::to_string(kv.second[i]); + EXPECT_STREQ(param_name.c_str(), params.at(i).get_name().c_str()); + EXPECT_STREQ(hello_world.c_str(), params.at(i).get_value().c_str()); + } + } + + for (size_t i = 0; i < node_names_keys.size(); ++i) { + c_params->params[i].parameter_values[0].string_value = NULL; + } + delete[] c_hello_world; + rcl_yaml_node_struct_fini(c_params); +} diff --git a/rclcpp/test/rclcpp/test_parameter_service.cpp b/rclcpp/test/rclcpp/test_parameter_service.cpp new file mode 100644 index 0000000000..7d63d0866d --- /dev/null +++ b/rclcpp/test/rclcpp/test_parameter_service.cpp @@ -0,0 +1,121 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "../../src/rclcpp/parameter_service_names.hpp" + +using namespace std::chrono_literals; + +// This tests the ParameterService as it is included in an rclcpp::Node. Creating a separate +// ParameterService would interfere with the built-in one +class TestParameterService : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node = std::make_shared("test_parameter_service", "/ns"); + client = std::make_shared(node); + ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1))); + } + + rclcpp::Node::SharedPtr node; + rclcpp::SyncParametersClient::SharedPtr client; +}; + +TEST_F(TestParameterService, get_parameters) { + node->declare_parameter("parameter1", rclcpp::ParameterValue(42)); + EXPECT_EQ(42, client->get_parameter("parameter1", 0)); + + EXPECT_EQ(-42, client->get_parameter("undeclared_parameter", -42)); +} + +TEST_F(TestParameterService, get_parameter_types) { + node->declare_parameter("parameter1", rclcpp::ParameterValue(42)); + + const std::vector declared_parameters = {"parameter1"}; + const auto parameter_types = client->get_parameter_types(declared_parameters, 10s); + ASSERT_EQ(1u, parameter_types.size()); + EXPECT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, parameter_types[0]); + + const std::vector undeclared_parameters = {"parameter2"}; + const auto undeclared_parameter_types = client->get_parameter_types(undeclared_parameters, 10s); + EXPECT_EQ(0u, undeclared_parameter_types.size()); +} + +TEST_F(TestParameterService, set_parameters) { + node->declare_parameter("parameter1", rclcpp::ParameterValue(42)); + ASSERT_EQ(42, client->get_parameter("parameter1", 0)); + + const std::vector parameters = { + rclcpp::Parameter("parameter1", 0), + }; + client->set_parameters(parameters, 10s); + EXPECT_EQ(0, client->get_parameter("parameter1", 100)); +} + +TEST_F(TestParameterService, set_parameters_atomically) { + node->declare_parameter("parameter1", rclcpp::ParameterValue(42)); + ASSERT_EQ(42, client->get_parameter("parameter1", 0)); + + const std::vector parameters = { + rclcpp::Parameter("parameter1", 0), + }; + client->set_parameters_atomically(parameters, 10s); + EXPECT_EQ(0, client->get_parameter("parameter1", 100)); +} + +TEST_F(TestParameterService, list_parameters) { + const size_t number_parameters_in_basic_node = client->list_parameters({}, 1, 10s).names.size(); + node->declare_parameter("parameter1", rclcpp::ParameterValue(42)); + + const auto list_result = client->list_parameters({}, 1, 10s); + EXPECT_EQ(1u + number_parameters_in_basic_node, list_result.names.size()); +} + +TEST_F(TestParameterService, describe_parameters) { + node->declare_parameter("parameter1", rclcpp::ParameterValue(42)); + + { + const std::vector names{"parameter1"}; + std::vector parameter_descs = + client->describe_parameters(names, 10s); + ASSERT_EQ(1u, parameter_descs.size()); + EXPECT_EQ("parameter1", parameter_descs[0].name); + EXPECT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, parameter_descs[0].type); + } + + { + const std::vector names{"undeclared_parameter"}; + std::vector parameter_descs = + client->describe_parameters(names, 10s); + EXPECT_EQ(0u, parameter_descs.size()); + } +} diff --git a/rclcpp/test/rclcpp/test_publisher.cpp b/rclcpp/test/rclcpp/test_publisher.cpp new file mode 100644 index 0000000000..e6e09bcb7b --- /dev/null +++ b/rclcpp/test/rclcpp/test_publisher.cpp @@ -0,0 +1,774 @@ +// 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 "rcl/publisher.h" + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "rcutils/env.h" + +#include "../mocking_utils/patch.hpp" +#include "../utils/rclcpp_gtest_macros.hpp" + +#include "test_msgs/msg/empty.hpp" +#include "test_msgs/msg/strings.hpp" + +class TestPublisher : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + +protected: + void initialize(const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions()) + { + node = std::make_shared("my_node", "/ns", node_options); + } + + void TearDown() + { + node.reset(); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + rclcpp::Node::SharedPtr node; +}; + +struct TestParameters +{ + TestParameters(rclcpp::QoS qos, const std::string & description) + : qos(qos), description(description) {} + rclcpp::QoS qos; + std::string description; +}; + +std::ostream & operator<<(std::ostream & out, const TestParameters & params) +{ + out << params.description; + return out; +} + +class TestPublisherInvalidIntraprocessQos + : public TestPublisher, + public ::testing::WithParamInterface +{}; + +class TestPublisherSub : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + void SetUp() + { + node = std::make_shared("my_node", "/ns"); + subnode = node->create_sub_node("sub_ns"); + } + + void TearDown() + { + node.reset(); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + rclcpp::Node::SharedPtr node; + rclcpp::Node::SharedPtr subnode; +}; + +/* + Testing publisher construction and destruction. + */ +TEST_F(TestPublisher, construction_and_destruction) { + initialize(); + using test_msgs::msg::Empty; + { + auto publisher = node->create_publisher("topic", 42); + (void)publisher; + } + + { + ASSERT_THROW( + { + auto publisher = node->create_publisher("invalid_topic?", 42); + }, rclcpp::exceptions::InvalidTopicNameError); + } +} + +/* + Testing publisher creation signatures. + */ +TEST_F(TestPublisher, various_creation_signatures) { + initialize(); + using test_msgs::msg::Empty; + { + auto publisher = node->create_publisher("topic", 42); + (void)publisher; + } + { + auto publisher = node->create_publisher("topic", rclcpp::QoS(42)); + (void)publisher; + } + { + auto publisher = + node->create_publisher("topic", rclcpp::QoS(rclcpp::KeepLast(42))); + (void)publisher; + } + { + auto publisher = + node->create_publisher("topic", rclcpp::QoS(rclcpp::KeepAll())); + (void)publisher; + } + { + auto publisher = + node->create_publisher("topic", 42, rclcpp::PublisherOptions()); + (void)publisher; + } + { + auto publisher = + rclcpp::create_publisher(node, "topic", 42, rclcpp::PublisherOptions()); + (void)publisher; + } + { + rclcpp::PublisherOptions options; + options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); + auto publisher = + rclcpp::create_publisher(node, "topic", 42, options); + (void)publisher; + } + { + auto publisher = rclcpp::create_publisher( + node->get_node_topics_interface(), "topic", 42, rclcpp::PublisherOptions()); + (void)publisher; + } + { + auto node_topics_interface = node->get_node_topics_interface(); + auto publisher = rclcpp::create_publisher( + node_topics_interface, "topic", 42, rclcpp::PublisherOptions()); + (void)publisher; + } + { + auto node_parameters = node->get_node_parameters_interface(); + auto node_topics = node->get_node_topics_interface(); + auto publisher = rclcpp::create_publisher( + node_parameters, node_topics, "topic", 42, rclcpp::PublisherOptions()); + (void)publisher; + } +} + +/* + Testing publisher with intraprocess enabled and invalid QoS + */ +TEST_P(TestPublisherInvalidIntraprocessQos, test_publisher_throws) { + initialize(rclcpp::NodeOptions().use_intra_process_comms(true)); + rclcpp::QoS qos = GetParam().qos; + using test_msgs::msg::Empty; + { + ASSERT_THROW( + {auto publisher = node->create_publisher("topic", qos);}, + std::invalid_argument); + } +} + +static std::vector invalid_qos_profiles() +{ + std::vector parameters; + + parameters.reserve(1); + parameters.push_back( + TestParameters( + rclcpp::QoS(rclcpp::KeepAll()), + "keep_all_qos")); + + return parameters; +} + +INSTANTIATE_TEST_SUITE_P( + TestPublisherThrows, TestPublisherInvalidIntraprocessQos, + ::testing::ValuesIn(invalid_qos_profiles()), + ::testing::PrintToStringParamName()); + +/* + Testing publisher construction and destruction for subnodes. + */ +TEST_F(TestPublisherSub, construction_and_destruction) { + using test_msgs::msg::Empty; + { + auto publisher = subnode->create_publisher("topic", 42); + + EXPECT_STREQ(publisher->get_topic_name(), "/ns/sub_ns/topic"); + } + + { + auto publisher = subnode->create_publisher("/topic", 42); + + EXPECT_STREQ(publisher->get_topic_name(), "/topic"); + } + + { + ASSERT_THROW( + { + auto publisher = subnode->create_publisher("invalid_topic?", 42); + }, rclcpp::exceptions::InvalidTopicNameError); + } +} + +// Auxiliary class used to test getter for const PublisherBase +const rosidl_message_type_support_t EmptyTypeSupport() +{ + return *rosidl_typesupport_cpp::get_message_type_support_handle(); +} + +const rclcpp::PublisherOptionsWithAllocator> PublisherOptions() +{ + return rclcpp::PublisherOptionsWithAllocator>(); +} + +class TestPublisherBase : public rclcpp::PublisherBase +{ +public: + explicit TestPublisherBase(rclcpp::Node * node) + : rclcpp::PublisherBase( + node->get_node_base_interface().get(), "topic", EmptyTypeSupport(), + PublisherOptions().to_rcl_publisher_options(rclcpp::QoS(10)), + PublisherOptions().event_callbacks, PublisherOptions().use_default_callbacks) {} +}; + +/* + Testing some publisher getters + */ +TEST_F(TestPublisher, basic_getters) { + initialize(); + using test_msgs::msg::Empty; + { + using rclcpp::QoS; + using rclcpp::KeepLast; + const size_t qos_depth_size = 10u; + auto publisher = node->create_publisher("topic", QoS(KeepLast(qos_depth_size))); + + size_t publisher_queue_size = publisher->get_queue_size(); + EXPECT_EQ(qos_depth_size, publisher_queue_size); + + const rmw_gid_t & publisher_rmw_gid = publisher->get_gid(); + EXPECT_NE(nullptr, publisher_rmw_gid.implementation_identifier); + + std::shared_ptr publisher_handle = publisher->get_publisher_handle(); + EXPECT_NE(nullptr, publisher_handle); + + EXPECT_TRUE(publisher->assert_liveliness()); + } + { + const TestPublisherBase publisher = TestPublisherBase(node.get()); + std::shared_ptr publisher_handle = publisher.get_publisher_handle(); + EXPECT_NE(nullptr, publisher_handle); + + const rmw_gid_t & publisher_rmw_gid = publisher.get_gid(); + EXPECT_NE(nullptr, publisher_rmw_gid.implementation_identifier); + + // Test == operator of publisher with rmw_gid_t + EXPECT_EQ(publisher, publisher_rmw_gid); + } +} + +TEST_F(TestPublisher, serialized_message_publish) { + initialize(); + rclcpp::PublisherOptionsWithAllocator> options; + // This is the default, but it's also important for this test to succeed. + options.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; + auto publisher = node->create_publisher("topic", 10, options); + + rclcpp::SerializedMessage serialized_msg; + // Mock successful rcl publish because the serialized_msg above is poorly formed + auto mock = mocking_utils::patch_and_return( + "self", rcl_publish_serialized_message, RCL_RET_OK); + EXPECT_NO_THROW(publisher->publish(serialized_msg)); + + EXPECT_NO_THROW(publisher->publish(serialized_msg.get_rcl_serialized_message())); +} + +TEST_F(TestPublisher, rcl_publisher_init_error) { + initialize(); + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_publisher_init, RCL_RET_ERROR); + EXPECT_THROW( + node->create_publisher("topic", 10).reset(), + rclcpp::exceptions::RCLError); +} + +TEST_F(TestPublisher, rcl_publisher_get_rmw_handle_error) { + initialize(); + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_publisher_get_rmw_handle, nullptr); + RCLCPP_EXPECT_THROW_EQ( + node->create_publisher("topic", 10), + std::runtime_error("failed to get rmw handle: error not set")); +} + +TEST_F(TestPublisher, rcl_publisher_get_gid_for_publisher_error) { + initialize(); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rmw_get_gid_for_publisher, RMW_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node->create_publisher("topic", 10), + std::runtime_error("failed to get publisher gid: error not set")); +} + +TEST_F(TestPublisher, rcl_publisher_fini_error) { + initialize(); + auto mock = mocking_utils::inject_on_return("lib:rclcpp", rcl_publisher_fini, RCL_RET_ERROR); + auto publisher = node->create_publisher("topic", 10); + ASSERT_EQ(1, publisher.use_count()); + // Failure in rcl_publisher_fini should just log error + EXPECT_NO_THROW(publisher.reset()); +} + +TEST_F(TestPublisher, rcl_publisher_get_options_error) { + initialize(); + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_publisher_get_options, nullptr); + auto publisher = node->create_publisher("topic", 10); + RCLCPP_EXPECT_THROW_EQ( + publisher->get_queue_size(), + std::runtime_error("failed to get publisher options: error not set")); +} + +TEST_F(TestPublisher, rcl_publisher_get_subscription_count_publisher_invalid) { + initialize(); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_publisher_get_subscription_count, RCL_RET_PUBLISHER_INVALID); + auto publisher = node->create_publisher("topic", 10); + EXPECT_THROW( + publisher->get_subscription_count(), + rclcpp::exceptions::RCLError); +} + +TEST_F(TestPublisher, rcl_publisher_get_actual_qos_error) { + initialize(); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_publisher_get_actual_qos, nullptr); + auto publisher = node->create_publisher("topic", 10); + RCLCPP_EXPECT_THROW_EQ( + publisher->get_actual_qos(), + std::runtime_error("failed to get qos settings: error not set")); +} + +TEST_F(TestPublisher, publishers_equal_rmw_compare_gids_error) { + initialize(); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rmw_compare_gids_equal, RMW_RET_ERROR); + const auto publisher = node->create_publisher("topic", 10); + const rmw_gid_t * gid = nullptr; + auto throwing_fn = [publisher, gid]() + { + // The == operator is expected to throw here, but this lambda avoids unused result warning + return (*publisher.get() == gid) ? true : false; + }; + + RCLCPP_EXPECT_THROW_EQ( + throwing_fn(), + std::runtime_error("failed to compare gids: error not set")); +} + +TEST_F(TestPublisher, intra_process_publish_failures) { + initialize(); + rclcpp::PublisherOptionsWithAllocator> options; + options.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; + auto publisher = node->create_publisher("topic", 10, options); + + auto msg_unique = std::make_unique(); + EXPECT_NO_THROW(publisher->publish(std::move(msg_unique))); + + rclcpp::SerializedMessage serialized_msg; + RCLCPP_EXPECT_THROW_EQ( + publisher->publish(serialized_msg), + std::runtime_error("storing serialized messages in intra process is not supported yet")); + + std::allocator allocator; + { + rclcpp::LoanedMessage loaned_msg(*publisher, allocator); + EXPECT_NO_THROW(publisher->publish(std::move(loaned_msg))); + } + + { + rclcpp::LoanedMessage loaned_msg(*publisher, allocator); + auto msg = loaned_msg.release(); // this will unmanage the ownership of the message + RCLCPP_EXPECT_THROW_EQ( + publisher->publish(std::move(loaned_msg)), + std::runtime_error("loaned message is not valid")); + // if the message is actually loaned from the middleware but not be published, + // it is user responsibility to return the message to the middleware manually + if (publisher->can_loan_messages()) { + ASSERT_EQ( + RCL_RET_OK, + rcl_return_loaned_message_from_publisher( + publisher->get_publisher_handle().get(), msg.get())); + } + } + RCLCPP_EXPECT_THROW_EQ( + node->create_publisher( + "topic", rclcpp::QoS(0), options), + std::invalid_argument( + "intraprocess communication is not allowed with a zero qos history depth value")); +} + +TEST_F(TestPublisher, inter_process_publish_failures) { + initialize(); + rclcpp::PublisherOptionsWithAllocator> options; + options.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; + auto publisher = node->create_publisher("topic", 10, options); + + auto msg_unique = std::make_unique(); + EXPECT_NO_THROW(publisher->publish(std::move(msg_unique))); + + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_publish, RCL_RET_PUBLISHER_INVALID); + test_msgs::msg::Empty msg; + EXPECT_THROW(publisher->publish(msg), rclcpp::exceptions::RCLError); + } + + { + // Using 'self' instead of 'lib:rclcpp' because `rcl_publish_serialized_message` is entirely + // defined in a header. Also, this one requires mocking because the serialized_msg is poorly + // formed and this just tests rclcpp functionality. + auto mock = mocking_utils::patch_and_return( + "self", rcl_publish_serialized_message, RCL_RET_OK); + rclcpp::SerializedMessage serialized_msg; + EXPECT_NO_THROW(publisher->publish(serialized_msg)); + } + + { + // Using 'self' instead of 'lib:rclcpp' because `rcl_publish_serialized_message` is entirely + // defined in a header + auto mock = mocking_utils::patch_and_return( + "self", rcl_publish_serialized_message, RCL_RET_ERROR); + rclcpp::SerializedMessage serialized_msg; + EXPECT_THROW(publisher->publish(serialized_msg), rclcpp::exceptions::RCLError); + } + + std::allocator allocator; + rclcpp::LoanedMessage loaned_msg(*publisher, allocator); + EXPECT_NO_THROW(publisher->publish(std::move(loaned_msg))); +} + +template> +class TestPublisherProtectedMethods : public rclcpp::Publisher +{ +public: + using rclcpp::Publisher::Publisher; + + void publish_loaned_message(rclcpp::LoanedMessage && loaned_msg) + { + this->do_loaned_message_publish(loaned_msg.release()); + } + + void call_default_incompatible_qos_callback(rclcpp::QOSOfferedIncompatibleQoSInfo & event) const + { + this->default_incompatible_qos_callback(event); + } +}; + +TEST_F(TestPublisher, do_loaned_message_publish_error) { + initialize(); + using PublisherT = TestPublisherProtectedMethods>; + // This test only passes when message is allocated on heap, not middleware. + // Since `do_loaned_message_publish()` will fail, there is no way to return the message + // to the middleware. + // This eventually fails to destroy publisher handle in the implementation. + ASSERT_TRUE(rcutils_set_env("ROS_DISABLE_LOANED_MESSAGES", "1")); + auto publisher = + node->create_publisher, PublisherT>("topic", 10); + + auto msg = publisher->borrow_loaned_message(); + + { + // Using 'self' instead of 'lib:rclcpp' because `rcl_publish_loaned_message` is entirely + // defined in a header + auto mock = mocking_utils::patch_and_return( + "self", rcl_publish_loaned_message, RCL_RET_PUBLISHER_INVALID); + EXPECT_THROW(publisher->publish_loaned_message(std::move(msg)), rclcpp::exceptions::RCLError); + } +} + +TEST_F(TestPublisher, default_incompatible_qos_callback) { + initialize(); + using PublisherT = TestPublisherProtectedMethods>; + auto publisher = + node->create_publisher, PublisherT>("topic", 10); + rclcpp::QOSOfferedIncompatibleQoSInfo event; + event.last_policy_kind = RMW_QOS_POLICY_INVALID; + // This message just logs an error message + EXPECT_NO_THROW(publisher->call_default_incompatible_qos_callback(event)); +} + +TEST_F(TestPublisher, run_event_handlers) { + initialize(); + auto publisher = node->create_publisher("topic", 10); + + for (const auto & key_event_pair : publisher->get_event_handlers()) { + auto handler = key_event_pair.second; + const std::shared_ptr data = handler->take_data(); + handler->execute(data); + } +} + +TEST_F(TestPublisher, get_network_flow_endpoints_errors) { + initialize(); + const rclcpp::QoS publisher_qos(1); + auto publisher = node->create_publisher("topic", publisher_qos); + + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_publisher_get_network_flow_endpoints, RCL_RET_ERROR); + auto mock_network_flow_endpoint_array_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_network_flow_endpoint_array_fini, RCL_RET_ERROR); + EXPECT_THROW( + publisher->get_network_flow_endpoints(), + rclcpp::exceptions::RCLError); + } + { + auto mock_network_flow_endpoint_array_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_network_flow_endpoint_array_fini, RCL_RET_ERROR); + EXPECT_THROW( + publisher->get_network_flow_endpoints(), + rclcpp::exceptions::RCLError); + } + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_publisher_get_network_flow_endpoints, RCL_RET_OK); + auto mock_network_flow_endpoint_array_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_network_flow_endpoint_array_fini, RCL_RET_OK); + EXPECT_NO_THROW(publisher->get_network_flow_endpoints()); + } +} + +TEST_F(TestPublisher, check_wait_for_all_acked_return) { + initialize(); + const rclcpp::QoS publisher_qos(1); + auto publisher = node->create_publisher("topic", publisher_qos); + + { + // Using 'self' instead of 'lib:rclcpp' because `rcl_publisher_wait_for_all_acked` is entirely + // defined in a header + auto mock = mocking_utils::patch_and_return( + "self", rcl_publisher_wait_for_all_acked, RCL_RET_OK); + EXPECT_TRUE(publisher->wait_for_all_acked(std::chrono::milliseconds(-1))); + } + + { + // Using 'self' instead of 'lib:rclcpp' because `rcl_publisher_wait_for_all_acked` is entirely + // defined in a header + auto mock = mocking_utils::patch_and_return( + "self", rcl_publisher_wait_for_all_acked, RCL_RET_TIMEOUT); + EXPECT_FALSE(publisher->wait_for_all_acked(std::chrono::milliseconds(-1))); + } + + { + // Using 'self' instead of 'lib:rclcpp' because `rcl_publisher_wait_for_all_acked` is entirely + // defined in a header + auto mock = mocking_utils::patch_and_return( + "self", rcl_publisher_wait_for_all_acked, RCL_RET_UNSUPPORTED); + EXPECT_THROW( + publisher->wait_for_all_acked(std::chrono::milliseconds(-1)), + rclcpp::exceptions::RCLError); + } + + { + // Using 'self' instead of 'lib:rclcpp' because `rcl_publisher_wait_for_all_acked` is entirely + // defined in a header + auto mock = mocking_utils::patch_and_return( + "self", rcl_publisher_wait_for_all_acked, RCL_RET_ERROR); + EXPECT_THROW( + publisher->wait_for_all_acked(std::chrono::milliseconds(-1)), + rclcpp::exceptions::RCLError); + } +} + +class TestPublisherWaitForAllAcked + : public TestPublisher, public ::testing::WithParamInterface> +{ +}; + +TEST_P(TestPublisherWaitForAllAcked, check_wait_for_all_acked_with_QosPolicy) { + initialize(); + + auto do_nothing = [](std::shared_ptr) {}; + auto pub = node->create_publisher("topic", std::get<0>(GetParam())); + auto sub = node->create_subscription( + "topic", + std::get<1>(GetParam()), + do_nothing); + + auto msg = std::make_shared(); + for (int i = 0; i < 20; i++) { + ASSERT_NO_THROW(pub->publish(*msg)); + } + EXPECT_TRUE(pub->wait_for_all_acked(std::chrono::milliseconds(6000))); +} + +TEST_F(TestPublisher, lowest_available_ipm_capacity) { + constexpr auto history_depth = 10u; + + initialize(rclcpp::NodeOptions().use_intra_process_comms(true)); + + rclcpp::PublisherOptionsWithAllocator> options_ipm_disabled; + options_ipm_disabled.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; + + rclcpp::PublisherOptionsWithAllocator> options_ipm_enabled; + options_ipm_enabled.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; + + auto do_nothing = [](std::shared_ptr) {}; + auto pub_ipm_disabled = node->create_publisher( + "topic", history_depth, + options_ipm_disabled); + auto pub_ipm_enabled = node->create_publisher( + "topic", history_depth, + options_ipm_enabled); + auto sub = node->create_subscription( + "topic", + history_depth, + do_nothing); + + ASSERT_EQ(1, pub_ipm_enabled->get_intra_process_subscription_count()); + ASSERT_EQ(0, pub_ipm_disabled->lowest_available_ipm_capacity()); + ASSERT_EQ(history_depth, pub_ipm_enabled->lowest_available_ipm_capacity()); + + auto msg = std::make_shared(); + ASSERT_NO_THROW(pub_ipm_disabled->publish(*msg)); + ASSERT_NO_THROW(pub_ipm_enabled->publish(*msg)); + + ASSERT_EQ(0, pub_ipm_disabled->lowest_available_ipm_capacity()); + ASSERT_EQ(history_depth - 1u, pub_ipm_enabled->lowest_available_ipm_capacity()); +} + +INSTANTIATE_TEST_SUITE_P( + TestWaitForAllAckedWithParm, + TestPublisherWaitForAllAcked, + ::testing::Values( + std::pair( + rclcpp::QoS(1).reliable(), rclcpp::QoS(1).reliable()), + std::pair( + rclcpp::QoS(1).best_effort(), rclcpp::QoS(1).best_effort()), + std::pair( + rclcpp::QoS(1).reliable(), rclcpp::QoS(1).best_effort()))); + +TEST_F(TestPublisher, intra_process_transient_local) { + constexpr auto history_depth = 10u; + initialize(rclcpp::NodeOptions().use_intra_process_comms(true)); + rclcpp::PublisherOptionsWithAllocator> pub_options_ipm_disabled; + pub_options_ipm_disabled.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; + + rclcpp::PublisherOptionsWithAllocator> pub_options_ipm_enabled; + pub_options_ipm_enabled.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; + + auto pub_ipm_enabled_transient_local_enabled = node->create_publisher( + "topic1", + rclcpp::QoS(rclcpp::KeepLast(history_depth)).transient_local(), pub_options_ipm_enabled); + auto pub_ipm_disabled_transient_local_enabled = node->create_publisher( + "topic2", + rclcpp::QoS(rclcpp::KeepLast(history_depth)).transient_local(), pub_options_ipm_disabled); + auto pub_ipm_enabled_transient_local_disabled = node->create_publisher( + "topic3", + rclcpp::QoS(rclcpp::KeepLast(history_depth)), pub_options_ipm_enabled); + auto pub_ipm_disabled_transient_local_disabled = node->create_publisher( + "topic4", + rclcpp::QoS(rclcpp::KeepLast(history_depth)), pub_options_ipm_disabled); + + test_msgs::msg::Empty msg; + pub_ipm_enabled_transient_local_enabled->publish(msg); + pub_ipm_disabled_transient_local_enabled->publish(msg); + pub_ipm_enabled_transient_local_disabled->publish(msg); + pub_ipm_disabled_transient_local_disabled->publish(msg); + + auto do_nothing = [](std::shared_ptr) {}; + struct IntraProcessCallback + { + void callback_fun(size_t s) + { + (void) s; + called = true; + } + bool called = false; + }; + rclcpp::SubscriptionOptions sub_options_ipm_disabled; + sub_options_ipm_disabled.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; + rclcpp::SubscriptionOptions sub_options_ipm_enabled; + sub_options_ipm_enabled.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; + IntraProcessCallback callback1, callback2, callback3, callback4; + auto sub_ipm_enabled_transient_local_enabled = node->create_subscription( + "topic1", + rclcpp::QoS(rclcpp::KeepLast(history_depth)).transient_local(), + do_nothing, sub_options_ipm_enabled); + sub_ipm_enabled_transient_local_enabled->set_on_new_intra_process_message_callback( + std::bind(&IntraProcessCallback::callback_fun, &callback1, std::placeholders::_1)); + auto sub_ipm_disabled_transient_local_enabled = node->create_subscription( + "topic2", + rclcpp::QoS(rclcpp::KeepLast(history_depth)).transient_local(), + do_nothing, sub_options_ipm_disabled); + sub_ipm_disabled_transient_local_enabled->set_on_new_intra_process_message_callback( + std::bind(&IntraProcessCallback::callback_fun, &callback2, std::placeholders::_1)); + auto sub_ipm_enabled_transient_local_disabled = node->create_subscription( + "topic3", + rclcpp::QoS(rclcpp::KeepLast(history_depth)), + do_nothing, sub_options_ipm_enabled); + sub_ipm_enabled_transient_local_disabled->set_on_new_intra_process_message_callback( + std::bind(&IntraProcessCallback::callback_fun, &callback3, std::placeholders::_1)); + auto sub_ipm_disabled_transient_local_disabled = node->create_subscription( + "topic4", + rclcpp::QoS(rclcpp::KeepLast(history_depth)), + do_nothing, sub_options_ipm_disabled); + sub_ipm_disabled_transient_local_disabled->set_on_new_intra_process_message_callback( + std::bind(&IntraProcessCallback::callback_fun, &callback4, std::placeholders::_1)); + + EXPECT_TRUE(pub_ipm_enabled_transient_local_enabled->is_durability_transient_local()); + EXPECT_TRUE(pub_ipm_disabled_transient_local_enabled->is_durability_transient_local()); + EXPECT_FALSE(pub_ipm_enabled_transient_local_disabled->is_durability_transient_local()); + EXPECT_FALSE(pub_ipm_disabled_transient_local_disabled->is_durability_transient_local()); + + EXPECT_EQ(1, pub_ipm_enabled_transient_local_enabled->get_intra_process_subscription_count()); + EXPECT_EQ(0, pub_ipm_disabled_transient_local_enabled->get_intra_process_subscription_count()); + EXPECT_EQ(1, pub_ipm_enabled_transient_local_disabled->get_intra_process_subscription_count()); + EXPECT_EQ(0, pub_ipm_disabled_transient_local_disabled->get_intra_process_subscription_count()); + + EXPECT_EQ( + history_depth - 1u, + pub_ipm_enabled_transient_local_enabled->lowest_available_ipm_capacity()); + EXPECT_EQ(0, pub_ipm_disabled_transient_local_enabled->lowest_available_ipm_capacity()); + EXPECT_EQ( + history_depth, + pub_ipm_enabled_transient_local_disabled->lowest_available_ipm_capacity()); + EXPECT_EQ(0, pub_ipm_disabled_transient_local_disabled->lowest_available_ipm_capacity()); + + EXPECT_TRUE(callback1.called); + EXPECT_FALSE(callback2.called); + EXPECT_FALSE(callback3.called); + EXPECT_FALSE(callback4.called); +} diff --git a/rclcpp/test/rclcpp/test_publisher_subscription_count_api.cpp b/rclcpp/test/rclcpp/test_publisher_subscription_count_api.cpp new file mode 100644 index 0000000000..eac46c1d8d --- /dev/null +++ b/rclcpp/test/rclcpp/test_publisher_subscription_count_api.cpp @@ -0,0 +1,249 @@ +// 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 "rclcpp/exceptions.hpp" +#include "rclcpp/publisher.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "test_msgs/msg/empty.hpp" + +namespace +{ + + +template +class NodeCreationPolicy +{ +public: + rclcpp::NodeOptions & node_options() + { + return options_; + } + +private: + rclcpp::NodeOptions options_; +}; + +template +class NodeCreationPolicy +{ +public: + NodeCreationPolicy() + { + gather(options_); + } + + rclcpp::NodeOptions & node_options() + { + return options_; + } + +private: + template + static rclcpp::NodeOptions & + gather(rclcpp::NodeOptions & options) + { + return U::gather(options); + } + + template + static rclcpp::NodeOptions & + gather(rclcpp::NodeOptions & options) + { + return gather(U::gather(options)); + } + + rclcpp::NodeOptions options_; +}; + +template +struct ShouldUseIntraprocess +{ + static rclcpp::NodeOptions & gather(rclcpp::NodeOptions & options) + { + return options.use_intra_process_comms(value); + } +}; + +using UseIntraprocess = ShouldUseIntraprocess; +using DoNotUseIntraprocess = ShouldUseIntraprocess; + +struct UseCustomContext +{ + static rclcpp::NodeOptions & gather(rclcpp::NodeOptions & options) + { + auto context = rclcpp::Context::make_shared(); + context->init(0, nullptr); + return options.context(context); + } +}; + +struct PrintTestDescription +{ + template + static std::string GetName(int i) + { + static_cast(i); + return T::description; + } +}; + +} // namespace + + +template +class TestPublisherSubscriptionCount : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + +protected: + static void OnMessage(test_msgs::msg::Empty::ConstSharedPtr msg) + { + (void)msg; + } + + std::chrono::milliseconds offset{2000}; +}; + +/* Testing publisher subscription count api and internal process subscription count. + * Two subscriptions in the same topic, both using intraprocess comm. + */ +struct TwoSubscriptionsIntraprocessComm +{ + static constexpr const char * description = + "two_subscriptions_intraprocess_comm"; + using FirstNodeCreationPolicy = NodeCreationPolicy; + using SecondNodeCreationPolicy = NodeCreationPolicy; + + static constexpr bool first_node_talks_intraprocess{true}; + static constexpr bool both_nodes_talk_intraprocess{true}; +}; + +/* Testing publisher subscription count api and internal process subscription count. + * Two subscriptions, one using intra-process comm and the other not using it. + */ +struct TwoSubscriptionsOneIntraprocessOneNot +{ + static constexpr const char * description = + "two_subscriptions_one_intraprocess_one_not"; + using FirstNodeCreationPolicy = NodeCreationPolicy; + using SecondNodeCreationPolicy = NodeCreationPolicy<>; + + static constexpr bool first_node_talks_intraprocess{true}; + static constexpr bool both_nodes_talk_intraprocess{false}; +}; + +/* Testing publisher subscription count api and internal process subscription count. + * Two contexts, both using intra-process. + */ +struct TwoSubscriptionsInTwoContextsWithIntraprocessComm +{ + static constexpr const char * description = + "two_subscriptions_in_two_contexts_with_intraprocess_comm"; + using FirstNodeCreationPolicy = NodeCreationPolicy; + using SecondNodeCreationPolicy = NodeCreationPolicy; + + static constexpr bool first_node_talks_intraprocess{true}; + static constexpr bool both_nodes_talk_intraprocess{false}; +}; + +/* Testing publisher subscription count api and internal process subscription count. + * Two contexts, both of them not using intra-process comm. + */ +struct TwoSubscriptionsInTwoContextsWithoutIntraprocessComm +{ + static constexpr const char * description = + "two_subscriptions_in_two_contexts_without_intraprocess_comm"; + using FirstNodeCreationPolicy = NodeCreationPolicy<>; + using SecondNodeCreationPolicy = NodeCreationPolicy; + + static constexpr bool first_node_talks_intraprocess{false}; + static constexpr bool both_nodes_talk_intraprocess{false}; +}; + +using AllTestDescriptions = ::testing::Types< + TwoSubscriptionsIntraprocessComm, + TwoSubscriptionsOneIntraprocessOneNot, + TwoSubscriptionsInTwoContextsWithIntraprocessComm, + TwoSubscriptionsInTwoContextsWithoutIntraprocessComm +>; +TYPED_TEST_SUITE(TestPublisherSubscriptionCount, AllTestDescriptions, PrintTestDescription); + + +using test_msgs::msg::Empty; + +TYPED_TEST(TestPublisherSubscriptionCount, increasing_and_decreasing_counts) +{ + using TestDescription = TypeParam; + typename TestDescription::FirstNodeCreationPolicy my_node_creation_policy; + rclcpp::Node::SharedPtr node = std::make_shared( + "my_node", + "/ns", + my_node_creation_policy.node_options()); + auto publisher = node->create_publisher("/topic", 10); + + EXPECT_EQ(publisher->get_subscription_count(), 0u); + EXPECT_EQ(publisher->get_intra_process_subscription_count(), 0u); + { + auto sub = node->create_subscription( + "/topic", 10, &TestPublisherSubscriptionCount::OnMessage); + rclcpp::sleep_for(this->offset); + EXPECT_EQ(publisher->get_subscription_count(), 1u); + EXPECT_EQ( + publisher->get_intra_process_subscription_count(), + (TestDescription::first_node_talks_intraprocess ? 1u : 0u)); + { + typename TestDescription::SecondNodeCreationPolicy another_node_creation_policy; + rclcpp::Node::SharedPtr another_node = std::make_shared( + "another_node", + "/ns", + another_node_creation_policy.node_options()); + auto another_sub = another_node->create_subscription( + "/topic", 10, &TestPublisherSubscriptionCount::OnMessage); + + rclcpp::sleep_for(this->offset); + EXPECT_EQ(publisher->get_subscription_count(), 2u); + EXPECT_EQ( + publisher->get_intra_process_subscription_count(), + (TestDescription::first_node_talks_intraprocess ? 1u : 0u) + + (TestDescription::both_nodes_talk_intraprocess ? 1u : 0u)); + } + rclcpp::sleep_for(this->offset); + EXPECT_EQ(publisher->get_subscription_count(), 1u); + EXPECT_EQ( + publisher->get_intra_process_subscription_count(), + (TestDescription::first_node_talks_intraprocess ? 1u : 0u)); + } + /** + * Counts should be zero here, as all are subscriptions are out of scope. + * Subscriptions count checking is always preceeded with an sleep, as random failures had been + * detected without it. */ + rclcpp::sleep_for(this->offset); + EXPECT_EQ(publisher->get_subscription_count(), 0u); + EXPECT_EQ(publisher->get_intra_process_subscription_count(), 0u); +} diff --git a/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp b/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp new file mode 100644 index 0000000000..d70b0e382d --- /dev/null +++ b/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp @@ -0,0 +1,403 @@ +// 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 +#include +#include + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/loaned_message.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "rclcpp/msg/large_message.hpp" +#include "rclcpp/msg/string.hpp" + + +using namespace std::chrono_literals; + +static const int g_max_loops = 200; +static const std::chrono::milliseconds g_sleep_per_loop(10); + + +class TestPublisher : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } +}; + +namespace rclcpp +{ + +template<> +struct TypeAdapter +{ + using is_specialized = std::true_type; + using custom_type = std::string; + using ros_message_type = rclcpp::msg::String; + + static void + convert_to_ros_message( + const custom_type & source, + ros_message_type & destination) + { + destination.data = source; + } + + static void + convert_to_custom( + const ros_message_type & source, + custom_type & destination) + { + destination = source.data; + } +}; + +// Throws in conversion +template<> +struct TypeAdapter +{ + using is_specialized = std::true_type; + using custom_type = int; + using ros_message_type = rclcpp::msg::String; + + static void + convert_to_ros_message( + const custom_type & source, + ros_message_type & destination) + { + (void) source; + (void) destination; + throw std::runtime_error("This should not happen"); + } + + static void + convert_to_custom( + const ros_message_type & source, + custom_type & destination) + { + (void) source; + (void) destination; + } +}; + +template<> +struct TypeAdapter +{ + using is_specialized = std::true_type; + using custom_type = std::string; + using ros_message_type = rclcpp::msg::LargeMessage; + + static void + convert_to_ros_message( + const custom_type & source, + ros_message_type & destination) + { + destination.size = source.size(); + std::memcpy(destination.data.data(), source.data(), source.size()); + } + + static void + convert_to_custom( + const ros_message_type & source, + custom_type & destination) + { + destination.resize(source.size); + std::memcpy(destination.data(), source.data.data(), source.size); + } +}; + +} // namespace rclcpp + +/* + * Testing publisher creation signatures with a type adapter. + */ +TEST_F(TestPublisher, various_creation_signatures) { + for (auto is_intra_process : {true, false}) { + rclcpp::NodeOptions options; + options.use_intra_process_comms(is_intra_process); + auto node = std::make_shared("my_node", "/ns", options); + { + using StringTypeAdapter = rclcpp::TypeAdapter; + auto publisher = node->create_publisher("topic", 42); + (void)publisher; + } + { + using StringTypeAdapter = rclcpp::adapt_type::as; + auto publisher = node->create_publisher("topic", 42); + (void)publisher; + } + } +} + +/* + * Testing that conversion errors are passed up. + */ +TEST_F(TestPublisher, conversion_exception_is_passed_up) { + using BadStringTypeAdapter = rclcpp::TypeAdapter; + for (auto is_intra_process : {true, false}) { + rclcpp::NodeOptions options; + options.use_intra_process_comms(is_intra_process); + + auto callback = + [](const rclcpp::msg::String::ConstSharedPtr msg) -> void + { + (void)msg; + }; + + auto node = std::make_shared("my_node", "/ns", options); + auto pub = node->create_publisher("topic_name", 1); + // A subscription is created to ensure the existence of a buffer in the intra proccess + // manager which will trigger the faulty conversion. + auto sub = node->create_subscription("topic_name", 1, callback); + EXPECT_THROW(pub->publish(1), std::runtime_error); + } +} + +using UseTakeSharedMethod = bool; +class TestPublisherFixture + : public TestPublisher, + public ::testing::WithParamInterface +{ +}; + +/* + * Testing that publisher sends type adapted types and ROS message types with intra proccess communications. + */ +TEST_P( + TestPublisherFixture, + check_type_adapted_message_is_sent_and_received_intra_process) { + using StringTypeAdapter = rclcpp::TypeAdapter; + const std::string message_data = "Message Data"; + const std::string topic_name = "topic_name"; + bool is_received; + + auto node = rclcpp::Node::make_shared( + "test_intra_process", + rclcpp::NodeOptions().use_intra_process_comms(true)); + auto pub = node->create_publisher(topic_name, 10); + rclcpp::Subscription::SharedPtr sub; + if (GetParam()) { + auto callback = + [message_data, &is_received]( + const rclcpp::msg::String::ConstSharedPtr msg, + const rclcpp::MessageInfo & message_info + ) -> void + { + is_received = true; + ASSERT_STREQ(message_data.c_str(), msg->data.c_str()); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + sub = node->create_subscription(topic_name, 1, callback); + } else { + auto callback_unique = + [message_data, &is_received]( + rclcpp::msg::String::UniquePtr msg, + const rclcpp::MessageInfo & message_info + ) -> void + { + is_received = true; + ASSERT_STREQ(message_data.c_str(), msg->data.c_str()); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + sub = node->create_subscription(topic_name, 1, callback_unique); + } + + auto wait_for_message_to_be_received = [&is_received, &node]() { + rclcpp::executors::SingleThreadedExecutor executor; + int i = 0; + executor.add_node(node); + executor.spin_once(std::chrono::milliseconds(0)); + while (!is_received && i < g_max_loops) { + printf("spin_node_once() - callback (1) expected - try %d/%d\n", ++i, g_max_loops); + executor.spin_once(g_sleep_per_loop); + } + }; + { + { // std::string passed by reference + is_received = false; + pub->publish(message_data); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(); + ASSERT_TRUE(is_received); + } + { // unique pointer to std::string + is_received = false; + auto pu_message = std::make_unique(message_data); + pub->publish(std::move(pu_message)); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(); + ASSERT_TRUE(is_received); + } + { // ROS message passed by reference + is_received = false; + rclcpp::msg::String msg; + msg.data = message_data; + pub->publish(msg); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(); + ASSERT_TRUE(is_received); + } + { // unique ptr to ROS message + is_received = false; + auto pu_msg = std::make_unique(); + pu_msg->data = message_data; + pub->publish(std::move(pu_msg)); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(); + ASSERT_TRUE(is_received); + } + /* TODO(audrow) Enable once loaned messages are supported for intra process communication + { // loaned ROS message + // is_received = false; + // std::allocator allocator; + // rclcpp::LoanedMessage loaned_msg(*pub, allocator); + // loaned_msg.get().data = message_data; + // pub->publish(std::move(loaned_msg)); + // ASSERT_FALSE(is_received); + // wait_for_message_to_be_received(); + // ASSERT_TRUE(is_received); + } + */ + } +} + +INSTANTIATE_TEST_SUITE_P( + TestPublisherFixtureWithParam, + TestPublisherFixture, + ::testing::Values( + true, // use take shared method + false // not use take shared method +)); + +/* + * Testing that publisher sends type adapted types and ROS message types with inter proccess communications. + */ +TEST_F(TestPublisher, check_type_adapted_message_is_sent_and_received) { + using StringTypeAdapter = rclcpp::TypeAdapter; + + auto node = std::make_shared("my_node", "/ns", rclcpp::NodeOptions()); + + const std::string message_data = "Message Data"; + const std::string topic_name = "topic_name"; + + auto do_nothing = [](std::shared_ptr) {FAIL();}; + auto pub = node->create_publisher(topic_name, 1); + auto sub = node->create_subscription(topic_name, 1, do_nothing); + + auto assert_no_message_was_received_yet = [sub]() { + rclcpp::msg::String msg; + rclcpp::MessageInfo msg_info; + EXPECT_FALSE(sub->take(msg, msg_info)); + }; + auto assert_message_was_received = [sub, message_data]() { + rclcpp::msg::String msg; + rclcpp::MessageInfo msg_info; + bool message_received = false; + auto start = std::chrono::steady_clock::now(); + do { + message_received = sub->take(msg, msg_info); + std::this_thread::sleep_for(100ms); + } while (!message_received && std::chrono::steady_clock::now() - start < 10s); + EXPECT_TRUE(message_received); + ASSERT_STREQ(message_data.c_str(), msg.data.c_str()); + }; + + { // std::string passed by reference + assert_no_message_was_received_yet(); + pub->publish(message_data); + assert_message_was_received(); + } + { // unique pointer to std::string + assert_no_message_was_received_yet(); + auto pu_message = std::make_unique(message_data); + pub->publish(std::move(pu_message)); + assert_message_was_received(); + } + { // ROS message passed by reference + assert_no_message_was_received_yet(); + rclcpp::msg::String msg; + msg.data = message_data; + pub->publish(msg); + assert_message_was_received(); + } + { // unique ptr to ROS message + assert_no_message_was_received_yet(); + auto pu_msg = std::make_unique(); + pu_msg->data = message_data; + pub->publish(std::move(pu_msg)); + assert_message_was_received(); + } + { // loaned ROS message + assert_no_message_was_received_yet(); + std::allocator allocator; + rclcpp::LoanedMessage loaned_msg(*pub, allocator); + loaned_msg.get().data = message_data; + pub->publish(std::move(loaned_msg)); + rclcpp::PublisherOptionsWithAllocator> options; + assert_message_was_received(); + } +} + +TEST_F(TestPublisher, test_large_message_unique) +{ + // There have been some bugs in the past when trying to type-adapt large messages + // (larger than the stack size). Here we just make sure that a 10MB message works, + // which is larger than the default stack size on Linux. + + using StringTypeAdapter = rclcpp::TypeAdapter; + + auto node = std::make_shared("my_node", "/ns", rclcpp::NodeOptions()); + + const std::string topic_name = "topic_name"; + + auto pub = node->create_publisher(topic_name, 1); + + static constexpr size_t length = 10 * 1024 * 1024; + auto message_data = std::make_unique(length, '#'); + pub->publish(std::move(message_data)); +} + +TEST_F(TestPublisher, test_large_message_constref) +{ + // There have been some bugs in the past when trying to type-adapt large messages + // (larger than the stack size). Here we just make sure that a 10MB message works, + // which is larger than the default stack size on Linux. + + using StringTypeAdapter = rclcpp::TypeAdapter; + + auto node = std::make_shared("my_node", "/ns", rclcpp::NodeOptions()); + + const std::string topic_name = "topic_name"; + + auto pub = node->create_publisher(topic_name, 1); + + static constexpr size_t length = 10 * 1024 * 1024; + std::string message_data(length, '#'); + pub->publish(message_data); +} diff --git a/rclcpp/test/rclcpp/test_qos.cpp b/rclcpp/test/rclcpp/test_qos.cpp new file mode 100644 index 0000000000..eecfaf97a7 --- /dev/null +++ b/rclcpp/test/rclcpp/test_qos.cpp @@ -0,0 +1,274 @@ +// Copyright 2020 Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include "rclcpp/qos.hpp" +#include "rmw/rmw.h" + +#include "rmw/types.h" + +TEST(TestQoS, equality_history) { + rclcpp::QoS a(10); + rclcpp::QoS b(10); + EXPECT_EQ(a, b); + a.keep_last(5); + EXPECT_NE(a, b); + a.keep_all(); + b.keep_all(); + EXPECT_EQ(a, b); +} + +TEST(TestQoS, equality_reliability) { + rclcpp::QoS a(10); + rclcpp::QoS b(10); + b.best_effort(); + EXPECT_NE(a, b); +} + +TEST(TestQoS, equality_durability) { + rclcpp::QoS a(10); + rclcpp::QoS b(10); + a.transient_local(); + EXPECT_NE(a, b); +} + +TEST(TestQoS, equality_deadline) { + rclcpp::QoS a(10); + rclcpp::QoS b(10); + rmw_time_t deadline{0, 1000}; + a.deadline(deadline); + EXPECT_NE(a, b); +} + +TEST(TestQoS, equality_lifespan) { + rclcpp::QoS a(10); + rclcpp::QoS b(10); + rmw_time_t lifespan{3, 0}; + a.lifespan(lifespan); + EXPECT_NE(a, b); +} + +TEST(TestQoS, equality_liveliness) { + rclcpp::QoS a(10); + rclcpp::QoS b(10); + rmw_time_t duration{0, 1000000}; + a.liveliness_lease_duration(duration); + EXPECT_NE(a, b); + b.liveliness_lease_duration(duration); + EXPECT_EQ(a, b); + a.liveliness(RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC); + EXPECT_NE(a, b); +} + +TEST(TestQoS, equality_namespace) { + rclcpp::QoS a(10); + rclcpp::QoS b(10); + a.avoid_ros_namespace_conventions(true); + EXPECT_NE(a, b); +} + +TEST(TestQoS, setters_and_getters) { + rclcpp::QoS qos(10); + + qos.keep_all(); + EXPECT_EQ(rclcpp::HistoryPolicy::KeepAll, qos.history()); + + qos.keep_last(20); + EXPECT_EQ(rclcpp::HistoryPolicy::KeepLast, qos.history()); + EXPECT_EQ(20u, qos.depth()); + + qos.reliable(); + EXPECT_EQ(rclcpp::ReliabilityPolicy::Reliable, qos.reliability()); + + qos.reliability_best_available(); + EXPECT_EQ(rclcpp::ReliabilityPolicy::BestAvailable, qos.reliability()); + + qos.reliability(rclcpp::ReliabilityPolicy::BestEffort); + EXPECT_EQ(rclcpp::ReliabilityPolicy::BestEffort, qos.reliability()); + + qos.durability_volatile(); + EXPECT_EQ(rclcpp::DurabilityPolicy::Volatile, qos.durability()); + + qos.transient_local(); + EXPECT_EQ(rclcpp::DurabilityPolicy::TransientLocal, qos.durability()); + + qos.durability_best_available(); + EXPECT_EQ(rclcpp::DurabilityPolicy::BestAvailable, qos.durability()); + + qos.durability(rclcpp::DurabilityPolicy::Volatile); + EXPECT_EQ(rclcpp::DurabilityPolicy::Volatile, qos.durability()); + + qos.history(RMW_QOS_POLICY_HISTORY_KEEP_ALL); + EXPECT_EQ(rclcpp::HistoryPolicy::KeepAll, qos.history()); + + qos.history(rclcpp::HistoryPolicy::KeepLast); + EXPECT_EQ(rclcpp::HistoryPolicy::KeepLast, qos.history()); + + constexpr rcl_duration_value_t duration_ns = 12345; + constexpr std::chrono::nanoseconds duration(duration_ns); + qos.deadline(duration); + EXPECT_EQ(duration_ns, qos.deadline().nanoseconds()); + + const rmw_time_t rmw_time {0, 54321}; + qos.deadline(rmw_time); + EXPECT_EQ(rmw_time.sec, qos.get_rmw_qos_profile().deadline.sec); + EXPECT_EQ(rmw_time.nsec, qos.get_rmw_qos_profile().deadline.nsec); + + qos.lifespan(duration); + EXPECT_EQ(duration_ns, qos.lifespan().nanoseconds()); + + qos.lifespan(rmw_time); + EXPECT_EQ(rmw_time.sec, qos.get_rmw_qos_profile().lifespan.sec); + EXPECT_EQ(rmw_time.nsec, qos.get_rmw_qos_profile().lifespan.nsec); + + qos.liveliness(RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC); + EXPECT_EQ(rclcpp::LivelinessPolicy::ManualByTopic, qos.liveliness()); + + qos.liveliness(rclcpp::LivelinessPolicy::Automatic); + EXPECT_EQ(rclcpp::LivelinessPolicy::Automatic, qos.liveliness()); + + qos.liveliness_lease_duration(duration); + EXPECT_EQ(duration_ns, qos.liveliness_lease_duration().nanoseconds()); + + qos.liveliness_lease_duration(rmw_time); + EXPECT_EQ(rmw_time.sec, qos.get_rmw_qos_profile().liveliness_lease_duration.sec); + EXPECT_EQ(rmw_time.nsec, qos.get_rmw_qos_profile().liveliness_lease_duration.nsec); + + qos.avoid_ros_namespace_conventions(true); + EXPECT_TRUE(qos.avoid_ros_namespace_conventions()); + qos.avoid_ros_namespace_conventions(false); + EXPECT_FALSE(qos.avoid_ros_namespace_conventions()); +} + +bool operator==(const rmw_qos_profile_t & lhs, const rmw_qos_profile_t & rhs) +{ + if (lhs.history != rhs.history) { + return false; + } + switch (lhs.history) { + case RMW_QOS_POLICY_HISTORY_KEEP_ALL: + return true; + case RMW_QOS_POLICY_HISTORY_KEEP_LAST: + case RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT: + case RMW_QOS_POLICY_HISTORY_UNKNOWN: + return lhs.depth == rhs.depth; + } + throw std::runtime_error("This line shouldn't be reached"); +} + +TEST(TestQoS, DerivedTypes) { + rclcpp::SensorDataQoS sensor_data_qos; + EXPECT_EQ(rmw_qos_profile_sensor_data, sensor_data_qos.get_rmw_qos_profile()); + + rclcpp::ParametersQoS parameter_qos; + EXPECT_EQ(rmw_qos_profile_parameters, parameter_qos.get_rmw_qos_profile()); + + rclcpp::ServicesQoS services_qos; + EXPECT_EQ(rmw_qos_profile_services_default, services_qos.get_rmw_qos_profile()); + + rclcpp::ParameterEventsQoS parameter_events_qos; + EXPECT_EQ(rmw_qos_profile_parameter_events, parameter_events_qos.get_rmw_qos_profile()); + + rclcpp::RosoutQoS rosout_qos; + EXPECT_EQ(rcl_qos_profile_rosout_default, rosout_qos.get_rmw_qos_profile()); + + rclcpp::SystemDefaultsQoS system_default_qos; + const rclcpp::KeepLast expected_initialization(RMW_QOS_POLICY_DEPTH_SYSTEM_DEFAULT); + const rclcpp::QoS expected_default(expected_initialization); + EXPECT_EQ(expected_default.get_rmw_qos_profile(), system_default_qos.get_rmw_qos_profile()); + + rclcpp::BestAvailableQoS best_available_qos; + EXPECT_EQ(rmw_qos_profile_best_available, best_available_qos.get_rmw_qos_profile()); +} + +TEST(TestQoS, policy_name_from_kind) { + EXPECT_EQ( + "DURABILITY_QOS_POLICY", + rclcpp::qos_policy_name_from_kind(RMW_QOS_POLICY_DURABILITY)); + + EXPECT_EQ( + "DEADLINE_QOS_POLICY", + rclcpp::qos_policy_name_from_kind(RMW_QOS_POLICY_DEADLINE)); + + EXPECT_EQ( + "LIVELINESS_QOS_POLICY", + rclcpp::qos_policy_name_from_kind(RMW_QOS_POLICY_LIVELINESS)); + + EXPECT_EQ( + "RELIABILITY_QOS_POLICY", + rclcpp::qos_policy_name_from_kind(RMW_QOS_POLICY_RELIABILITY)); + + EXPECT_EQ( + "HISTORY_QOS_POLICY", + rclcpp::qos_policy_name_from_kind(RMW_QOS_POLICY_HISTORY)); + + EXPECT_EQ( + "LIFESPAN_QOS_POLICY", + rclcpp::qos_policy_name_from_kind(RMW_QOS_POLICY_LIFESPAN)); +} + +TEST(TestQoS, qos_check_compatible) +{ + // Compatible + { + rclcpp::QoS qos = rclcpp::QoS(1) + .reliable() + .durability_volatile() + .deadline(rclcpp::Duration(1, 0u)) + .lifespan(rclcpp::Duration(1, 0u)) + .liveliness(rclcpp::LivelinessPolicy::Automatic) + .liveliness_lease_duration(rclcpp::Duration(1, 0u)); + rclcpp::QoSCheckCompatibleResult ret = rclcpp::qos_check_compatible(qos, qos); + EXPECT_EQ(ret.compatibility, rclcpp::QoSCompatibility::Ok); + EXPECT_EQ(ret.reason, std::string("")); + } + + // Note, the following incompatible tests assume we are using a DDS middleware, + // and may not be valid for other RMWs. + // TODO(jacobperron): programmatically check if current RMW is one of the officially + // supported DDS middlewares before running the following tests + + // If the RMW implementation is rmw_zenoh_cpp, we do not expect any QoS incompatibilities. + std::string rmw_implementation_str = std::string(rmw_get_implementation_identifier()); + // Incompatible + { + rclcpp::QoS pub_qos = rclcpp::QoS(1).best_effort(); + rclcpp::QoS sub_qos = rclcpp::QoS(1).reliable(); + rclcpp::QoSCheckCompatibleResult ret = rclcpp::qos_check_compatible(pub_qos, sub_qos); + if (rmw_implementation_str == "rmw_zenoh_cpp") { + EXPECT_EQ(ret.compatibility, rclcpp::QoSCompatibility::Ok); + EXPECT_TRUE(ret.reason.empty()); + } else { + EXPECT_EQ(ret.compatibility, rclcpp::QoSCompatibility::Error); + EXPECT_FALSE(ret.reason.empty()); + } + } + + // Warn of possible incompatibility + { + rclcpp::SystemDefaultsQoS pub_qos; + rclcpp::QoS sub_qos = rclcpp::QoS(1).reliable(); + rclcpp::QoSCheckCompatibleResult ret = rclcpp::qos_check_compatible(pub_qos, sub_qos); + if (rmw_implementation_str == "rmw_zenoh_cpp") { + EXPECT_EQ(ret.compatibility, rclcpp::QoSCompatibility::Ok); + EXPECT_TRUE(ret.reason.empty()); + } else { + EXPECT_EQ(ret.compatibility, rclcpp::QoSCompatibility::Warning); + EXPECT_FALSE(ret.reason.empty()); + } + } +} diff --git a/rclcpp/test/rclcpp/test_qos_event.cpp b/rclcpp/test/rclcpp/test_qos_event.cpp new file mode 100644 index 0000000000..634d4837a5 --- /dev/null +++ b/rclcpp/test/rclcpp/test_qos_event.cpp @@ -0,0 +1,626 @@ +// Copyright 2020 Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rcutils/logging.h" +#include "rmw/rmw.h" +#include "test_msgs/msg/empty.hpp" + +#include "../mocking_utils/patch.hpp" + +using namespace std::chrono_literals; + +class TestQosEvent : public ::testing::Test +{ +protected: + void SetUp() + { + // We initialize and shutdown the context (and hence also the rmw_context), + // for each test case to reset the ROS graph for each test case. + rclcpp::init(0, nullptr); + + rmw_implementation_str = std::string(rmw_get_implementation_identifier()); + + node = std::make_shared("test_qos_event", "/ns"); + + message_callback = [node = node.get()](test_msgs::msg::Empty::ConstSharedPtr /*msg*/) { + RCLCPP_INFO(node->get_logger(), "Message received"); + }; + } + + void TearDown() + { + node.reset(); + rclcpp::shutdown(); + } + + std::string rmw_implementation_str; + static constexpr char topic_name[] = "test_topic"; + rclcpp::Node::SharedPtr node; + std::function message_callback; +}; + +constexpr char TestQosEvent::topic_name[]; + +/* + Testing construction of a publishers with QoS event callback functions. + */ +TEST_F(TestQosEvent, test_publisher_constructor) +{ + rclcpp::PublisherOptions options; + + // options arg with no callbacks + auto publisher = node->create_publisher( + topic_name, 10, options); + + if (rmw_implementation_str != "rmw_zenoh_cpp") { + // options arg with one of the callbacks + options.event_callbacks.deadline_callback = + [node = node.get()](rclcpp::QOSDeadlineOfferedInfo & event) { + RCLCPP_INFO( + node->get_logger(), + "Offered deadline missed - total %d (delta %d)", + event.total_count, event.total_count_change); + }; + publisher = node->create_publisher( + topic_name, 10, options); + + // options arg with two of the callbacks + options.event_callbacks.liveliness_callback = + [node = node.get()](rclcpp::QOSLivelinessLostInfo & event) { + RCLCPP_INFO( + node->get_logger(), + "Liveliness lost - total %d (delta %d)", + event.total_count, event.total_count_change); + }; + publisher = node->create_publisher( + topic_name, 10, options); + } + // options arg with three of the callbacks + options.event_callbacks.incompatible_qos_callback = + [node = node.get()](rclcpp::QOSOfferedIncompatibleQoSInfo & event) { + RCLCPP_INFO( + node->get_logger(), + "Offered incompatible qos - total %d (delta %d), last_policy_kind: %d", + event.total_count, event.total_count_change, event.last_policy_kind); + }; + publisher = node->create_publisher( + topic_name, 10, options); +} + +/* + Testing construction of a subscriptions with QoS event callback functions. + */ +TEST_F(TestQosEvent, test_subscription_constructor) +{ + // While rmw_zenoh does not support Deadline/LivelinessChanged events, + // it does support IncompatibleQoS + rclcpp::SubscriptionOptions options; + + // options arg with no callbacks + auto subscription = node->create_subscription( + topic_name, 10, message_callback, options); + + if (rmw_implementation_str != "rmw_zenoh_cpp") { + // options arg with one of the callbacks + options.event_callbacks.deadline_callback = + [node = node.get()](rclcpp::QOSDeadlineRequestedInfo & event) { + RCLCPP_INFO( + node->get_logger(), + "Requested deadline missed - total %d (delta %d)", + event.total_count, event.total_count_change); + }; + subscription = node->create_subscription( + topic_name, 10, message_callback, options); + + // options arg with two of the callbacks + options.event_callbacks.liveliness_callback = + [node = node.get()](rclcpp::QOSLivelinessChangedInfo & event) { + RCLCPP_INFO( + node->get_logger(), + "Liveliness changed - alive %d (delta %d), not alive %d (delta %d)", + event.alive_count, event.alive_count_change, + event.not_alive_count, event.not_alive_count_change); + }; + subscription = node->create_subscription( + topic_name, 10, message_callback, options); + } + // options arg with three of the callbacks + options.event_callbacks.incompatible_qos_callback = + [node = node.get()](rclcpp::QOSRequestedIncompatibleQoSInfo & event) { + RCLCPP_INFO( + node->get_logger(), + "Requested incompatible qos - total %d (delta %d), last_policy_kind: %d", + event.total_count, event.total_count_change, event.last_policy_kind); + }; + subscription = node->create_subscription( + topic_name, 10, message_callback, options); +} + +/* + Testing construction of a subscriptions with QoS event callback functions. + */ +std::string * g_pub_log_msg; +std::string * g_sub_log_msg; +std::promise * g_log_msgs_promise; +TEST_F(TestQosEvent, test_default_incompatible_qos_callbacks) +{ + rcutils_logging_output_handler_t original_output_handler = rcutils_logging_get_output_handler(); + + std::string pub_log_msg; + std::string sub_log_msg; + std::promise log_msgs_promise; + g_pub_log_msg = &pub_log_msg; + g_sub_log_msg = &sub_log_msg; + g_log_msgs_promise = &log_msgs_promise; + auto logger_callback = []( + const rcutils_log_location_t * /*location*/, + int /*level*/, const char * /*name*/, rcutils_time_point_value_t /*timestamp*/, + const char * format, va_list * args) -> void { + char buffer[1024]; + vsnprintf(buffer, sizeof(buffer), format, *args); + const std::string msg = buffer; + if (msg.rfind("New subscription discovered", 0) == 0) { + *g_pub_log_msg = buffer; + } else if (msg.rfind("New publisher discovered", 0) == 0) { + *g_sub_log_msg = buffer; + } + + if (!g_pub_log_msg->empty() && !g_sub_log_msg->empty()) { + g_log_msgs_promise->set_value(); + } + }; + rcutils_logging_set_output_handler(logger_callback); + + std::shared_future log_msgs_future = log_msgs_promise.get_future(); + + rclcpp::QoS qos_profile_publisher(10); + qos_profile_publisher.durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); + auto publisher = node->create_publisher( + topic_name, qos_profile_publisher); + + rclcpp::QoS qos_profile_subscription(10); + qos_profile_subscription.durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + auto subscription = node->create_subscription( + topic_name, qos_profile_subscription, message_callback); + + rclcpp::executors::SingleThreadedExecutor ex; + ex.add_node(node->get_node_base_interface()); + + // This future won't complete on fastrtps, so just timeout immediately + const auto timeout = std::chrono::seconds(10); + ex.spin_until_future_complete(log_msgs_future, timeout); + + if (rmw_implementation_str == "rmw_zenoh_cpp") { + EXPECT_EQ(rclcpp::QoSCompatibility::Ok, + qos_check_compatible(qos_profile_publisher, qos_profile_subscription).compatibility); + } else { + EXPECT_EQ( + "New subscription discovered on topic '/ns/test_topic', requesting incompatible QoS. " + "No messages will be sent to it. Last incompatible policy: DURABILITY_QOS_POLICY", + pub_log_msg); + EXPECT_EQ( + "New publisher discovered on topic '/ns/test_topic', offering incompatible QoS. " + "No messages will be sent to it. Last incompatible policy: DURABILITY_QOS_POLICY", + sub_log_msg); + } + + rcutils_logging_set_output_handler(original_output_handler); +} + +TEST_F(TestQosEvent, construct_destruct_rcl_error) { + auto publisher = node->create_publisher(topic_name, 10); + auto rcl_handle = publisher->get_publisher_handle(); + ASSERT_NE(nullptr, rcl_handle); + + // This callback requires some type of parameter, but it could be anything + auto callback = [](int) {}; + const rcl_publisher_event_type_t event_type = rmw_implementation_str == "rmw_zenoh_cpp" ? + RCL_PUBLISHER_MATCHED : RCL_PUBLISHER_OFFERED_DEADLINE_MISSED; + + { + // Logs error and returns + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_publisher_event_init, RCL_RET_ERROR); + + auto throwing_statement = [callback, rcl_handle, event_type]() { + // reset() is not needed for the exception, but it handles unused return value warning + std::make_shared< + rclcpp::EventHandler>>( + callback, rcl_publisher_event_init, rcl_handle, event_type).reset(); + }; + // This is done through a lambda because the compiler is having trouble parsing the templated + // function inside a macro. + EXPECT_THROW(throwing_statement(), rclcpp::exceptions::RCLError); + } + + { + // Logs error and returns + auto mock = mocking_utils::inject_on_return( + "lib:rclcpp", rcl_event_fini, RCL_RET_ERROR); + + auto throwing_statement = [callback, rcl_handle, event_type]() { + // reset() is needed for this exception + std::make_shared< + rclcpp::EventHandler>>( + callback, rcl_publisher_event_init, rcl_handle, event_type).reset(); + }; + + // This is done through a lambda because the compiler is having trouble parsing the templated + // function inside a macro. + EXPECT_NO_THROW(throwing_statement()); + } +} + +TEST_F(TestQosEvent, execute) { + if (rmw_implementation_str == "rmw_zenoh_cpp") { + GTEST_SKIP(); + } + auto publisher = node->create_publisher(topic_name, 10); + auto rcl_handle = publisher->get_publisher_handle(); + + bool handler_callback_executed = false; + // This callback requires some type of parameter, but it could be anything + auto callback = [&handler_callback_executed](int) {handler_callback_executed = true;}; + const rcl_publisher_event_type_t event_type = RCL_PUBLISHER_OFFERED_DEADLINE_MISSED; + + rclcpp::EventHandler handler( + callback, rcl_publisher_event_init, rcl_handle, event_type); + + std::shared_ptr data = handler.take_data(); + EXPECT_NO_THROW(handler.execute(data)); + EXPECT_TRUE(handler_callback_executed); + + { + handler_callback_executed = false; + // Logs error and returns early + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_take_event, RCL_RET_ERROR); + std::shared_ptr data = handler.take_data(); + EXPECT_THROW(handler.execute(data), std::runtime_error); + EXPECT_FALSE(handler_callback_executed); + } +} + +TEST_F(TestQosEvent, add_to_wait_set) { + auto publisher = node->create_publisher(topic_name, 10); + auto rcl_handle = publisher->get_publisher_handle(); + + // This callback requires some type of parameter, but it could be anything + auto callback = [](int) {}; + + const rcl_publisher_event_type_t event_type = rmw_implementation_str == "rmw_zenoh_cpp" ? + RCL_PUBLISHER_MATCHED : RCL_PUBLISHER_OFFERED_DEADLINE_MISSED; + rclcpp::EventHandler handler( + callback, rcl_publisher_event_init, rcl_handle, event_type); + + EXPECT_EQ(1u, handler.get_number_of_ready_events()); + + { + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_add_event, RCL_RET_OK); + EXPECT_NO_THROW(handler.add_to_wait_set(wait_set)); + } + + { + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_wait_set_add_event, RCL_RET_ERROR); + EXPECT_THROW(handler.add_to_wait_set(wait_set), rclcpp::exceptions::RCLError); + } +} + +TEST_F(TestQosEvent, test_on_new_event_callback) +{ + if (rmw_implementation_str == "rmw_zenoh_cpp") { + GTEST_SKIP(); + } + + auto offered_deadline = rclcpp::Duration(std::chrono::milliseconds(1)); + auto requested_deadline = rclcpp::Duration(std::chrono::milliseconds(2)); + + rclcpp::QoS qos_profile_publisher(10); + qos_profile_publisher.deadline(offered_deadline); + rclcpp::PublisherOptions pub_options; + pub_options.event_callbacks.deadline_callback = [](auto) {FAIL();}; + auto publisher = node->create_publisher( + topic_name, qos_profile_publisher, pub_options); + + rclcpp::QoS qos_profile_subscription(10); + qos_profile_subscription.deadline(requested_deadline); + rclcpp::SubscriptionOptions sub_options; + sub_options.event_callbacks.deadline_callback = [](auto) {FAIL();}; + auto subscription = node->create_subscription( + topic_name, qos_profile_subscription, message_callback, sub_options); + + std::atomic c1 {0}; + auto increase_c1_cb = [&c1](size_t count_events) {c1 += count_events;}; + publisher->set_on_new_qos_event_callback(increase_c1_cb, RCL_PUBLISHER_OFFERED_DEADLINE_MISSED); + + { + test_msgs::msg::Empty msg; + publisher->publish(msg); + } + + std::this_thread::sleep_for(std::chrono::seconds(1)); + + EXPECT_GT(c1, 1u); + + std::atomic c2 {0}; + auto increase_c2_cb = [&c2](size_t count_events) {c2 += count_events;}; + subscription->set_on_new_qos_event_callback( + increase_c2_cb, + RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED); + + EXPECT_GT(c2, 1u); +} + +TEST_F(TestQosEvent, test_invalid_on_new_event_callback) +{ + auto pub = node->create_publisher(topic_name, 10); + auto sub = node->create_subscription(topic_name, 10, message_callback); + auto dummy_cb = [](size_t count_events) {(void)count_events;}; + + if (rmw_implementation_str != "rmw_zenoh_cpp") { + EXPECT_NO_THROW( + pub->set_on_new_qos_event_callback(dummy_cb, RCL_PUBLISHER_OFFERED_DEADLINE_MISSED)); + + EXPECT_NO_THROW( + pub->clear_on_new_qos_event_callback(RCL_PUBLISHER_OFFERED_DEADLINE_MISSED)); + + EXPECT_NO_THROW( + pub->set_on_new_qos_event_callback(dummy_cb, RCL_PUBLISHER_LIVELINESS_LOST)); + + EXPECT_NO_THROW( + pub->clear_on_new_qos_event_callback(RCL_PUBLISHER_LIVELINESS_LOST)); + } + EXPECT_NO_THROW( + pub->set_on_new_qos_event_callback(dummy_cb, RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS)); + + EXPECT_NO_THROW( + pub->clear_on_new_qos_event_callback(RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS)); + + EXPECT_NO_THROW( + pub->set_on_new_qos_event_callback(dummy_cb, RCL_PUBLISHER_MATCHED)); + + EXPECT_NO_THROW( + pub->clear_on_new_qos_event_callback(RCL_PUBLISHER_MATCHED)); + + if (rmw_implementation_str == "rmw_zenoh_cpp") { + EXPECT_NO_THROW( + sub->set_on_new_qos_event_callback(dummy_cb, RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED)); + + EXPECT_NO_THROW( + sub->clear_on_new_qos_event_callback(RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED)); + + EXPECT_NO_THROW( + sub->set_on_new_qos_event_callback(dummy_cb, RCL_SUBSCRIPTION_LIVELINESS_CHANGED)); + + EXPECT_NO_THROW( + sub->clear_on_new_qos_event_callback(RCL_SUBSCRIPTION_LIVELINESS_CHANGED)); + } + EXPECT_NO_THROW( + sub->set_on_new_qos_event_callback(dummy_cb, RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS)); + + EXPECT_NO_THROW( + sub->clear_on_new_qos_event_callback(RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS)); + + EXPECT_NO_THROW( + sub->set_on_new_qos_event_callback(dummy_cb, RCL_SUBSCRIPTION_MATCHED)); + + EXPECT_NO_THROW( + sub->clear_on_new_qos_event_callback(RCL_SUBSCRIPTION_MATCHED)); + + if (rmw_implementation_str != "rmw_zenoh_cpp") { + std::function invalid_cb; + + rclcpp::SubscriptionOptions sub_options; + sub_options.event_callbacks.deadline_callback = [](auto) {}; + sub = node->create_subscription( + topic_name, 10, message_callback, sub_options); + + EXPECT_THROW( + sub->set_on_new_qos_event_callback(invalid_cb, RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED), + std::invalid_argument); + + rclcpp::PublisherOptions pub_options; + pub_options.event_callbacks.deadline_callback = [](auto) {}; + pub = node->create_publisher(topic_name, 10, pub_options); + + EXPECT_THROW( + pub->set_on_new_qos_event_callback(invalid_cb, RCL_PUBLISHER_OFFERED_DEADLINE_MISSED), + std::invalid_argument); + } +} + +TEST_F(TestQosEvent, test_pub_matched_event_by_set_event_callback) +{ + std::atomic_size_t matched_count = 0; + + rclcpp::PublisherOptions pub_options; + pub_options.event_callbacks.matched_callback = [](auto) {}; + auto pub = node->create_publisher( + topic_name, 10, pub_options); + + std::promise prom; + auto matched_event_callback = [&matched_count, &prom](size_t count) { + matched_count += count; + prom.set_value(); + }; + + pub->set_on_new_qos_event_callback(matched_event_callback, RCL_PUBLISHER_MATCHED); + + rclcpp::executors::SingleThreadedExecutor ex; + ex.add_node(node->get_node_base_interface()); + + const auto timeout = std::chrono::seconds(10); + + { + auto sub1 = node->create_subscription(topic_name, 10, message_callback); + ex.spin_until_future_complete(prom.get_future(), timeout); + prom = {}; + EXPECT_EQ(matched_count, static_cast(1)); + + { + auto sub2 = node->create_subscription( + topic_name, 10, message_callback); + ex.spin_until_future_complete(prom.get_future(), timeout); + prom = {}; + EXPECT_EQ(matched_count, static_cast(2)); + } + ex.spin_until_future_complete(prom.get_future(), timeout); + prom = {}; + EXPECT_EQ(matched_count, static_cast(3)); + } + + ex.spin_until_future_complete(prom.get_future(), timeout); + EXPECT_EQ(matched_count, static_cast(4)); +} + +TEST_F(TestQosEvent, test_sub_matched_event_by_set_event_callback) +{ + std::atomic_size_t matched_count = 0; + + rclcpp::SubscriptionOptions sub_options; + sub_options.event_callbacks.matched_callback = [](auto) {}; + auto sub = node->create_subscription( + topic_name, 10, message_callback, sub_options); + + std::promise prom; + auto matched_event_callback = [&matched_count, &prom](size_t count) { + matched_count += count; + prom.set_value(); + }; + + sub->set_on_new_qos_event_callback(matched_event_callback, RCL_SUBSCRIPTION_MATCHED); + + rclcpp::executors::SingleThreadedExecutor ex; + ex.add_node(node->get_node_base_interface()); + + const auto timeout = std::chrono::seconds(10000); + + { + auto pub1 = node->create_publisher(topic_name, 10); + + ex.spin_until_future_complete(prom.get_future(), timeout); + prom = {}; + EXPECT_EQ(matched_count, static_cast(1)); + + { + auto pub2 = node->create_publisher(topic_name, 10); + ex.spin_until_future_complete(prom.get_future(), timeout); + prom = {}; + EXPECT_EQ(matched_count, static_cast(2)); + } + + ex.spin_until_future_complete(prom.get_future(), timeout); + prom = {}; + EXPECT_EQ(matched_count, static_cast(3)); + } + + ex.spin_until_future_complete(prom.get_future(), timeout); + EXPECT_EQ(matched_count, static_cast(4)); +} + +TEST_F(TestQosEvent, test_pub_matched_event_by_option_event_callback) +{ + rmw_matched_status_t matched_expected_result; + std::promise prom; + + rclcpp::PublisherOptions pub_options; + pub_options.event_callbacks.matched_callback = + [&matched_expected_result, &prom](rmw_matched_status_t & s) { + EXPECT_EQ(s.total_count, matched_expected_result.total_count); + EXPECT_EQ(s.total_count_change, matched_expected_result.total_count_change); + EXPECT_EQ(s.current_count, matched_expected_result.current_count); + EXPECT_EQ(s.current_count_change, matched_expected_result.current_count_change); + prom.set_value(); + }; + + auto pub = node->create_publisher( + topic_name, 10, pub_options); + + rclcpp::executors::SingleThreadedExecutor ex; + ex.add_node(node->get_node_base_interface()); + + // Create a connected subscription + matched_expected_result.total_count = 1; + matched_expected_result.total_count_change = 1; + matched_expected_result.current_count = 1; + matched_expected_result.current_count_change = 1; + + const auto timeout = std::chrono::seconds(10); + + { + auto sub = node->create_subscription(topic_name, 10, message_callback); + ex.spin_until_future_complete(prom.get_future(), timeout); + prom = {}; + + // destroy a connected subscription + matched_expected_result.total_count = 1; + matched_expected_result.total_count_change = 0; + matched_expected_result.current_count = 0; + matched_expected_result.current_count_change = -1; + } + ex.spin_until_future_complete(prom.get_future(), timeout); +} + +TEST_F(TestQosEvent, test_sub_matched_event_by_option_event_callback) +{ + rmw_matched_status_t matched_expected_result; + + std::promise prom; + rclcpp::SubscriptionOptions sub_options; + sub_options.event_callbacks.matched_callback = + [&matched_expected_result, &prom](rmw_matched_status_t & s) { + EXPECT_EQ(s.total_count, matched_expected_result.total_count); + EXPECT_EQ(s.total_count_change, matched_expected_result.total_count_change); + EXPECT_EQ(s.current_count, matched_expected_result.current_count); + EXPECT_EQ(s.current_count_change, matched_expected_result.current_count_change); + prom.set_value(); + }; + auto sub = node->create_subscription( + topic_name, 10, message_callback, sub_options); + + rclcpp::executors::SingleThreadedExecutor ex; + ex.add_node(node->get_node_base_interface()); + + // Create a connected publisher + matched_expected_result.total_count = 1; + matched_expected_result.total_count_change = 1; + matched_expected_result.current_count = 1; + matched_expected_result.current_count_change = 1; + + const auto timeout = std::chrono::seconds(10); + { + auto pub1 = node->create_publisher(topic_name, 10); + ex.spin_until_future_complete(prom.get_future(), timeout); + prom = {}; + + // destroy a connected publisher + matched_expected_result.total_count = 1; + matched_expected_result.total_count_change = 0; + matched_expected_result.current_count = 0; + matched_expected_result.current_count_change = -1; + } + ex.spin_until_future_complete(prom.get_future(), timeout); +} diff --git a/rclcpp/test/rclcpp/test_qos_overriding_options.cpp b/rclcpp/test/rclcpp/test_qos_overriding_options.cpp new file mode 100644 index 0000000000..f15fbe43ff --- /dev/null +++ b/rclcpp/test/rclcpp/test_qos_overriding_options.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 "gmock/gmock.h" + +#include "rclcpp/qos_overriding_options.hpp" + +TEST(TestQosOverridingOptions, test_overriding_options) { + auto options = rclcpp::QosOverridingOptions::with_default_policies(); + EXPECT_EQ(options.get_id(), ""); + EXPECT_EQ(options.get_validation_callback(), nullptr); + EXPECT_THAT( + options.get_policy_kinds(), testing::ElementsAre( + rclcpp::QosPolicyKind::History, + rclcpp::QosPolicyKind::Depth, + rclcpp::QosPolicyKind::Reliability)); +} + +TEST(TestQosOverridingOptions, test_qos_policy_kind_to_cstr) { + EXPECT_THROW( + rclcpp::qos_policy_kind_to_cstr(rclcpp::QosPolicyKind::Invalid), + std::invalid_argument); +} diff --git a/rclcpp/test/rclcpp/test_qos_parameters.cpp b/rclcpp/test/rclcpp/test_qos_parameters.cpp new file mode 100644 index 0000000000..be9a12cb5e --- /dev/null +++ b/rclcpp/test/rclcpp/test_qos_parameters.cpp @@ -0,0 +1,263 @@ +// 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 "gmock/gmock.h" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/qos_overriding_options.hpp" +#include "rclcpp/detail/qos_parameters.hpp" + +TEST(TestQosParameters, declare) { + rclcpp::init(0, nullptr); + auto node = std::make_shared( + "my_node", "/ns", rclcpp::NodeOptions().parameter_overrides( + { + rclcpp::Parameter( + "qos_overrides./my/fully/qualified/topic_name.publisher.reliability", "best_effort"), + })); + + for (size_t i = 0; i < 2; ++i) { + // The first iteration will declare parameters, the second will get the previosuly declared + // ones, check both have the same result. + rclcpp::QoS qos{rclcpp::KeepLast(10)}; + qos = rclcpp::detail::declare_qos_parameters( + rclcpp::QosOverridingOptions::with_default_policies(), + node, + "/my/fully/qualified/topic_name", + qos, + rclcpp::detail::PublisherQosParametersTraits{}); + + EXPECT_EQ( + node->get_parameter( + "qos_overrides./my/fully/qualified/topic_name.publisher.history").get_value(), + "keep_last"); + EXPECT_EQ( + node->get_parameter( + "qos_overrides./my/fully/qualified/topic_name.publisher.depth").get_value(), + 10); + EXPECT_EQ( + node->get_parameter( + "qos_overrides./my/fully/qualified/topic_name.publisher.reliability" + ).get_value(), + "best_effort"); + EXPECT_EQ(RMW_QOS_POLICY_HISTORY_KEEP_LAST, qos.get_rmw_qos_profile().history); + EXPECT_EQ(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, qos.get_rmw_qos_profile().reliability); + EXPECT_EQ(10u, qos.get_rmw_qos_profile().depth); + + std::map qos_params; + EXPECT_TRUE( + node->get_node_parameters_interface()->get_parameters_by_prefix( + "qos_overrides./my/fully/qualified/topic_name.publisher", qos_params)); + EXPECT_EQ(3u, qos_params.size()); + } + + rclcpp::shutdown(); +} + +TEST(TestQosParameters, declare_with_callback) { + rclcpp::init(0, nullptr); + auto node = std::make_shared( + "my_node", "/ns", rclcpp::NodeOptions().parameter_overrides( + { + rclcpp::Parameter( + "qos_overrides./my/fully/qualified/topic_name.publisher.reliability", "best_effort"), + })); + + rclcpp::QoS qos{rclcpp::KeepLast(10)}; + // *INDENT-OFF*, uncrustify suggestion makes the code unreadable + EXPECT_THROW( + rclcpp::detail::declare_qos_parameters( + { + {rclcpp::QosPolicyKind::Lifespan}, + [](const rclcpp::QoS &) { + return rclcpp::QosCallbackResult{}; + } + }, + node, + "/my/fully/qualified/topic_name/fails_validation", + qos, + rclcpp::detail::PublisherQosParametersTraits{}), + rclcpp::exceptions::InvalidQosOverridesException); + + rclcpp::detail::declare_qos_parameters( + rclcpp::QosOverridingOptions::with_default_policies([](const rclcpp::QoS &) { + rclcpp::QosCallbackResult result; + result.successful = true; + return result; + }), + node, + "/my/fully/qualified/topic_name", + qos, + rclcpp::detail::PublisherQosParametersTraits{}); + // *INDENT-ON* + + rclcpp::shutdown(); +} + +constexpr int64_t kDuration{1000000}; + +TEST(TestQosParameters, declare_qos_subscription_parameters) { + rclcpp::init(0, nullptr); + auto node = std::make_shared( + "my_node", "/ns", rclcpp::NodeOptions().parameter_overrides( + { + rclcpp::Parameter( + "qos_overrides./my/fully/qualified/topic_name.subscription.reliability", "best_effort"), + rclcpp::Parameter( + "qos_overrides./my/fully/qualified/topic_name.subscription.deadline", kDuration), + rclcpp::Parameter( + "qos_overrides./my/fully/qualified/topic_name.subscription.liveliness_lease_duration", + kDuration), + })); + + for (size_t i = 0; i < 2; ++i) { + // The first iteration will declare parameters, the second will get the previosuly declared + // ones, check both have the same result. + rclcpp::QoS qos{rclcpp::KeepLast(10)}; + qos = rclcpp::detail::declare_qos_parameters( + { + rclcpp::QosPolicyKind::AvoidRosNamespaceConventions, rclcpp::QosPolicyKind::Deadline, + rclcpp::QosPolicyKind::Depth, rclcpp::QosPolicyKind::Durability, + // lifespan will be ignored + rclcpp::QosPolicyKind::History, rclcpp::QosPolicyKind::Lifespan, + rclcpp::QosPolicyKind::Liveliness, rclcpp::QosPolicyKind::LivelinessLeaseDuration, + rclcpp::QosPolicyKind::Reliability + }, + node, + "/my/fully/qualified/topic_name", + qos, + rclcpp::detail::SubscriptionQosParametersTraits{}); + + EXPECT_EQ( + node->get_parameter( + "qos_overrides./my/fully/qualified/topic_name.subscription.avoid_ros_namespace_conventions" + ).get_value(), false); + EXPECT_EQ( + node->get_parameter( + "qos_overrides./my/fully/qualified/topic_name.subscription.deadline" + ).get_value(), kDuration); + EXPECT_EQ( + node->get_parameter( + "qos_overrides./my/fully/qualified/topic_name.subscription.depth" + ).get_value(), 10); + EXPECT_EQ( + node->get_parameter( + "qos_overrides./my/fully/qualified/topic_name.subscription.durability" + ).get_value(), "volatile"); + EXPECT_EQ( + node->get_parameter( + "qos_overrides./my/fully/qualified/topic_name.subscription.history" + ).get_value(), "keep_last"); + EXPECT_FALSE( + node->has_parameter( + "qos_overrides./my/fully/qualified/topic_name.subscription.lifespan")); + EXPECT_EQ( + node->get_parameter( + "qos_overrides./my/fully/qualified/topic_name.subscription.liveliness" + ).get_value(), "system_default"); + EXPECT_EQ( + node->get_parameter( + "qos_overrides./my/fully/qualified/topic_name.subscription.liveliness_lease_duration" + ).get_value(), kDuration); + EXPECT_EQ( + node->get_parameter( + "qos_overrides./my/fully/qualified/topic_name.subscription.reliability" + ).get_value(), + "best_effort"); + + std::map qos_params; + EXPECT_TRUE( + node->get_node_parameters_interface()->get_parameters_by_prefix( + "qos_overrides./my/fully/qualified/topic_name.subscription", qos_params)); + EXPECT_EQ(8u, qos_params.size()); + } + rclcpp::shutdown(); +} + +TEST(TestQosParameters, declare_with_id) { + rclcpp::init(0, nullptr); + auto node = std::make_shared("my_node", "/ns"); + + rclcpp::QoS qos{rclcpp::KeepLast{10}}; + qos = rclcpp::detail::declare_qos_parameters( + rclcpp::QosOverridingOptions::with_default_policies(nullptr, "my_id"), + node, + "/my/fully/qualified/topic_name", + qos, + rclcpp::detail::PublisherQosParametersTraits{}); + + std::map qos_params; + EXPECT_TRUE( + node->get_node_parameters_interface()->get_parameters_by_prefix( + "qos_overrides./my/fully/qualified/topic_name.publisher_my_id", qos_params)); + EXPECT_EQ(3u, qos_params.size()); + + rclcpp::shutdown(); +} + +TEST(TestQosParameters, declare_no_parameters_interface) { + rclcpp::init(0, nullptr); + auto node = std::make_shared("my_node", "/ns"); + + rclcpp::QoS qos{rclcpp::KeepLast{10}}; + auto node_base_interface = node->get_node_base_interface(); + EXPECT_THROW( + rclcpp::detail::declare_qos_parameters( + rclcpp::QosOverridingOptions::with_default_policies(), + node_base_interface, + "/my/fully/qualified/topic_name", + qos, + rclcpp::detail::PublisherQosParametersTraits{}), + std::runtime_error); + + qos = rclcpp::detail::declare_qos_parameters( + rclcpp::QosOverridingOptions{}, + node_base_interface, + "/my/fully/qualified/topic_name", + qos, + rclcpp::detail::PublisherQosParametersTraits{}); + + std::map qos_params; + EXPECT_FALSE( + node->get_node_parameters_interface()->get_parameters_by_prefix( + "qos_overrides./my/fully/qualified/topic_name", qos_params)); + + rclcpp::shutdown(); +} + +TEST(TestQosParameters, internal_functions_failure_modes) { + rclcpp::QoS qos{rclcpp::KeepLast{10}}; + EXPECT_THROW( + rclcpp::detail::apply_qos_override( + rclcpp::QosPolicyKind::Invalid, rclcpp::ParameterValue{}, qos), + std::invalid_argument); + EXPECT_THROW( + rclcpp::detail::get_default_qos_param_value( + rclcpp::QosPolicyKind::Invalid, qos), + std::invalid_argument); + EXPECT_THROW( + rclcpp::detail::check_if_stringified_policy_is_null( + nullptr, rclcpp::QosPolicyKind::Reliability), + std::invalid_argument); +} + +TEST(TestQosParameters, keep_last_zero) { + rclcpp::KeepLast keep_last(0); + + EXPECT_EQ(keep_last.depth, RMW_QOS_POLICY_DEPTH_SYSTEM_DEFAULT); +} diff --git a/rclcpp/test/rclcpp/test_rate.cpp b/rclcpp/test/rclcpp/test_rate.cpp new file mode 100644 index 0000000000..86d22cbd94 --- /dev/null +++ b/rclcpp/test/rclcpp/test_rate.cpp @@ -0,0 +1,175 @@ +// 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 "rclcpp/rate.hpp" + +#include "../utils/rclcpp_gtest_macros.hpp" + +class TestRate : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + } + + void TearDown() + { + rclcpp::shutdown(); + } +}; + +/* + Basic tests for the Rate and WallRate classes. + */ +TEST_F(TestRate, rate_basics) { + auto period = std::chrono::milliseconds(1000); + auto offset = std::chrono::milliseconds(500); + auto epsilon = std::chrono::milliseconds(100); + double overrun_ratio = 1.5; + + auto start = std::chrono::system_clock::now(); + rclcpp::Rate r(period); + EXPECT_EQ(rclcpp::Duration(period), r.period()); + ASSERT_EQ(RCL_SYSTEM_TIME, r.get_type()); + ASSERT_TRUE(r.sleep()); + auto one = std::chrono::system_clock::now(); + auto delta = one - start; + EXPECT_LT(period, delta + epsilon); + EXPECT_GT(period * overrun_ratio, delta); + + rclcpp::sleep_for(offset); + ASSERT_TRUE(r.sleep()); + auto two = std::chrono::system_clock::now(); + delta = two - start; + EXPECT_LT(2 * period, delta); + EXPECT_GT(2 * period * overrun_ratio, delta); + + rclcpp::sleep_for(offset); + auto two_offset = std::chrono::system_clock::now(); + r.reset(); + ASSERT_TRUE(r.sleep()); + auto three = std::chrono::system_clock::now(); + delta = three - two_offset; + EXPECT_LT(period, delta + epsilon); + EXPECT_GT(period * overrun_ratio, delta); + + rclcpp::sleep_for(offset + period); + auto four = std::chrono::system_clock::now(); + ASSERT_FALSE(r.sleep()); + auto five = std::chrono::system_clock::now(); + delta = five - four; + ASSERT_TRUE(epsilon > delta); +} + +TEST_F(TestRate, wall_rate_basics) { + auto period = std::chrono::milliseconds(100); + auto offset = std::chrono::milliseconds(50); + auto epsilon = std::chrono::milliseconds(1); + double overrun_ratio = 1.5; + + auto start = std::chrono::steady_clock::now(); + rclcpp::WallRate r(period); + EXPECT_EQ(rclcpp::Duration(period), r.period()); + ASSERT_EQ(RCL_STEADY_TIME, r.get_type()); + ASSERT_TRUE(r.sleep()); + auto one = std::chrono::steady_clock::now(); + auto delta = one - start; + EXPECT_LT(period, delta); + EXPECT_GT(period * overrun_ratio, delta); + + rclcpp::sleep_for(offset); + ASSERT_TRUE(r.sleep()); + auto two = std::chrono::steady_clock::now(); + delta = two - start; + EXPECT_LT(2 * period, delta + epsilon); + EXPECT_GT(2 * period * overrun_ratio, delta); + + rclcpp::sleep_for(offset); + auto two_offset = std::chrono::steady_clock::now(); + r.reset(); + ASSERT_TRUE(r.sleep()); + auto three = std::chrono::steady_clock::now(); + delta = three - two_offset; + EXPECT_LT(period, delta); + EXPECT_GT(period * overrun_ratio, delta); + + rclcpp::sleep_for(offset + period); + auto four = std::chrono::steady_clock::now(); + ASSERT_FALSE(r.sleep()); + auto five = std::chrono::steady_clock::now(); + delta = five - four; + EXPECT_GT(epsilon, delta); +} + +TEST_F(TestRate, from_double) { + { + rclcpp::Rate rate(1.0); + EXPECT_EQ(rclcpp::Duration(std::chrono::seconds(1)), rate.period()); + } + { + rclcpp::Rate rate(2.0); + EXPECT_EQ(rclcpp::Duration(std::chrono::milliseconds(500)), rate.period()); + } + { + rclcpp::WallRate rate(0.5); + EXPECT_EQ(rclcpp::Duration(std::chrono::seconds(2)), rate.period()); + } + { + rclcpp::WallRate rate(4.0); + EXPECT_EQ(rclcpp::Duration(std::chrono::milliseconds(250)), rate.period()); + } +} + +TEST_F(TestRate, clock_types) { + { + rclcpp::Rate rate(1.0, std::make_shared(RCL_SYSTEM_TIME)); + EXPECT_EQ(RCL_SYSTEM_TIME, rate.get_type()); + } + { + rclcpp::Rate rate(1.0, std::make_shared(RCL_STEADY_TIME)); + EXPECT_EQ(RCL_STEADY_TIME, rate.get_type()); + } + { + rclcpp::Rate rate(1.0, std::make_shared(RCL_ROS_TIME)); + EXPECT_EQ(RCL_ROS_TIME, rate.get_type()); + } +} + +TEST_F(TestRate, incorrect_constuctor) { + // Constructor with 0-frequency + RCLCPP_EXPECT_THROW_EQ( + rclcpp::Rate rate(0.0), + std::invalid_argument("rate must be greater than 0")); + + // Constructor with negative frequency + RCLCPP_EXPECT_THROW_EQ( + rclcpp::Rate rate(-1.0), + std::invalid_argument("rate must be greater than 0")); + + // Constructor with 0-duration + RCLCPP_EXPECT_THROW_EQ( + rclcpp::Rate rate(rclcpp::Duration(0, 0)), + std::invalid_argument("period must be greater than 0")); + + // Constructor with negative duration + RCLCPP_EXPECT_THROW_EQ( + rclcpp::Rate rate(rclcpp::Duration(-1, 0)), + std::invalid_argument("period must be greater than 0")); +} diff --git a/rclcpp/test/rclcpp/test_ring_buffer_implementation.cpp b/rclcpp/test/rclcpp/test_ring_buffer_implementation.cpp new file mode 100644 index 0000000000..0abd9b1a89 --- /dev/null +++ b/rclcpp/test/rclcpp/test_ring_buffer_implementation.cpp @@ -0,0 +1,141 @@ +// 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 "gtest/gtest.h" + +#include "rclcpp/experimental/buffers/buffer_implementation_base.hpp" +#include "rclcpp/experimental/buffers/ring_buffer_implementation.hpp" + +/* + * Construtctor + */ +TEST(TestRingBufferImplementation, constructor) { + // Cannot create a buffer of size zero. + EXPECT_THROW( + rclcpp::experimental::buffers::RingBufferImplementation rb(0), + std::invalid_argument); + + rclcpp::experimental::buffers::RingBufferImplementation rb(1); + + EXPECT_EQ(false, rb.has_data()); + EXPECT_EQ(false, rb.is_full()); +} + +/* + * Basic usage + * - insert data and check that it has data + * - get all data + * - extract data + * - overwrite old data writing over the buffer capacity + */ +TEST(TestRingBufferImplementation, basic_usage) { + rclcpp::experimental::buffers::RingBufferImplementation rb(2); + + rb.enqueue('a'); + + EXPECT_EQ(true, rb.has_data()); + EXPECT_EQ(false, rb.is_full()); + + char v = rb.dequeue(); + + EXPECT_EQ('a', v); + EXPECT_EQ(false, rb.has_data()); + EXPECT_EQ(false, rb.is_full()); + + rb.enqueue('b'); + rb.enqueue('c'); + + EXPECT_EQ(true, rb.has_data()); + EXPECT_EQ(true, rb.is_full()); + + rb.enqueue('d'); + + const auto all_data_vec = rb.get_all_data(); + + EXPECT_EQ(2u, all_data_vec.size()); + EXPECT_EQ('c', all_data_vec[0]); + EXPECT_EQ('d', all_data_vec[1]); + + EXPECT_EQ(true, rb.has_data()); + EXPECT_EQ(true, rb.is_full()); + + v = rb.dequeue(); + + EXPECT_EQ('c', v); + EXPECT_EQ(true, rb.has_data()); + EXPECT_EQ(false, rb.is_full()); + + v = rb.dequeue(); + + EXPECT_EQ('d', v); + EXPECT_EQ(false, rb.has_data()); + EXPECT_EQ(false, rb.is_full()); +} + +/* + * Basic usage with unique_ptr + * - insert unique_ptr data and check that it has data + * - get all data + * - extract data + * - overwrite old data writing over the buffer capacity + */ +TEST(TestRingBufferImplementation, basic_usage_unique_ptr) { + rclcpp::experimental::buffers::RingBufferImplementation> rb(2); + + auto a = std::make_unique('a'); + auto b = std::make_unique('b'); + auto original_b_pointer = reinterpret_cast(b.get()); + auto c = std::make_unique('c'); + auto original_c_pointer = reinterpret_cast(c.get()); + + rb.enqueue(std::move(a)); + + EXPECT_EQ(true, rb.has_data()); + EXPECT_EQ(false, rb.is_full()); + + rb.enqueue(std::move(b)); + rb.enqueue(std::move(c)); + + EXPECT_EQ(true, rb.has_data()); + EXPECT_EQ(true, rb.is_full()); + + const auto all_data_vec = rb.get_all_data(); + + EXPECT_EQ(2u, all_data_vec.size()); + EXPECT_EQ('b', *all_data_vec[0]); + EXPECT_EQ('c', *all_data_vec[1]); + EXPECT_NE(original_b_pointer, reinterpret_cast(all_data_vec[0].get())); + EXPECT_NE(original_c_pointer, reinterpret_cast(all_data_vec[1].get())); + + EXPECT_EQ(true, rb.has_data()); + EXPECT_EQ(true, rb.is_full()); + + auto uni_ptr = rb.dequeue(); + + EXPECT_EQ('b', *uni_ptr); + EXPECT_EQ(original_b_pointer, reinterpret_cast(uni_ptr.get())); + EXPECT_EQ(true, rb.has_data()); + EXPECT_EQ(false, rb.is_full()); + + uni_ptr = rb.dequeue(); + + EXPECT_EQ('c', *uni_ptr); + EXPECT_EQ(original_c_pointer, reinterpret_cast(uni_ptr.get())); + EXPECT_EQ(false, rb.has_data()); + EXPECT_EQ(false, rb.is_full()); +} diff --git a/rclcpp/test/rclcpp/test_rosout_qos.cpp b/rclcpp/test/rclcpp/test_rosout_qos.cpp new file mode 100644 index 0000000000..aacaf84189 --- /dev/null +++ b/rclcpp/test/rclcpp/test_rosout_qos.cpp @@ -0,0 +1,79 @@ +// 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 "rclcpp/node_options.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/duration.hpp" +#include "rcl/logging_rosout.h" +#include "rcl/init.h" +#include "rmw/types.h" + +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; +} + +bool operator!=( + const rmw_qos_profile_t & lhs, + const rmw_qos_profile_t & rhs) +{ + return !(lhs == rhs); +} + +/* + Test rosout_qos function with default value. + */ +TEST(TestRosoutQoS, test_rosout_qos_with_default_value) { + rclcpp::NodeOptions node_options; + rclcpp::QoS rosout_qos_profile = node_options.rosout_qos(); + rmw_qos_profile_t rmw_qos_profile = rosout_qos_profile.get_rmw_qos_profile(); + EXPECT_EQ(rcl_qos_profile_rosout_default, rmw_qos_profile); + EXPECT_EQ(rcl_qos_profile_rosout_default, node_options.get_rcl_node_options()->rosout_qos); +} + +/* + Test `rosout_qos` function with custom value. + */ +TEST(TestRosoutQoS, test_rosout_qos_with_custom_value) { + rmw_time_t life_span; + life_span.sec = 10; + life_span.nsec = 0; + auto qos_profile = rclcpp::QoS(rclcpp::KeepLast(1000)).transient_local().lifespan(life_span); + auto options = rclcpp::NodeOptions().rosout_qos(qos_profile); + rclcpp::QoS rosout_qos = options.rosout_qos(); + rmw_qos_profile_t rmw_qos_profile = rosout_qos.get_rmw_qos_profile(); + + EXPECT_EQ(rmw_qos_profile, qos_profile.get_rmw_qos_profile()); + EXPECT_EQ(rmw_qos_profile, options.get_rcl_node_options()->rosout_qos); +} diff --git a/rclcpp/test/rclcpp/test_rosout_subscription.cpp b/rclcpp/test/rclcpp/test_rosout_subscription.cpp new file mode 100644 index 0000000000..08786a3fcb --- /dev/null +++ b/rclcpp/test/rclcpp/test_rosout_subscription.cpp @@ -0,0 +1,190 @@ +// 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 "rclcpp/rclcpp.hpp" + +#include "rcl_interfaces/msg/log.hpp" + +using namespace std::chrono_literals; + +class TestRosoutSubscription : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node = std::make_shared("test_rosout_subscription", "/ns"); + sub = node->create_subscription( + "/rosout", 10, [this](rcl_interfaces::msg::Log::ConstSharedPtr msg) { + if (msg->msg == this->rosout_msg_data && + msg->name == this->rosout_msg_name) + { + received_msg_promise.set_value(true); + } + }); + } + + void TearDown() + { + node.reset(); + } + + rclcpp::Node::SharedPtr node; + rclcpp::Subscription::SharedPtr sub; + std::promise received_msg_promise; + std::string rosout_msg_data; + std::string rosout_msg_name; +}; + +TEST_F(TestRosoutSubscription, test_rosoutsubscription_getchild) { + std::string logger_name = "ns.test_rosout_subscription.child"; + this->rosout_msg_data = "SOMETHING"; + this->rosout_msg_name = logger_name; + { + // before calling get_child of Logger + { + RCLCPP_INFO( + rclcpp::get_logger(logger_name), "%s", this->rosout_msg_data.c_str()); + auto future = received_msg_promise.get_future(); + auto return_code = rclcpp::spin_until_future_complete(this->node, future, 3s); + ASSERT_EQ(rclcpp::FutureReturnCode::TIMEOUT, return_code); + received_msg_promise = {}; + } + + rclcpp::Logger child_logger = this->node->get_logger().get_child("child"); + ASSERT_EQ(child_logger.get_name(), logger_name); + + // after calling get_child of Logger + // 1. use child_logger directly + { + RCLCPP_INFO(child_logger, "%s", this->rosout_msg_data.c_str()); + auto future = received_msg_promise.get_future(); + auto return_code = rclcpp::spin_until_future_complete(this->node, future, 3s); + ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); + EXPECT_TRUE(future.get()); + received_msg_promise = {}; + } + + // 2. use rclcpp::get_logger + { + RCLCPP_INFO(rclcpp::get_logger(logger_name), "%s", this->rosout_msg_data.c_str()); + auto future = received_msg_promise.get_future(); + auto return_code = rclcpp::spin_until_future_complete(this->node, future, 3s); + ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); + EXPECT_TRUE(future.get()); + received_msg_promise = {}; + } + } + + // `child_logger` is end of life, there is no sublogger + { + RCLCPP_INFO(rclcpp::get_logger(logger_name), "%s", this->rosout_msg_data.c_str()); + auto future = received_msg_promise.get_future(); + auto return_code = rclcpp::spin_until_future_complete(this->node, future, 3s); + ASSERT_EQ(rclcpp::FutureReturnCode::TIMEOUT, return_code); + received_msg_promise = {}; + } +} + +TEST_F(TestRosoutSubscription, test_rosoutsubscription_parent_log) { + std::string logger_name = "ns.test_rosout_subscription"; + this->rosout_msg_data = "SOMETHING"; + this->rosout_msg_name = logger_name; + + rclcpp::Logger logger = this->node->get_logger(); + ASSERT_EQ(logger.get_name(), logger_name); + RCLCPP_INFO(logger, "%s", this->rosout_msg_data.c_str()); + auto future = received_msg_promise.get_future(); + auto return_code = rclcpp::spin_until_future_complete(this->node, future, 3s); + ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); + EXPECT_TRUE(future.get()); + received_msg_promise = {}; +} + +TEST_F(TestRosoutSubscription, test_rosoutsubscription_child_log) { + std::string logger_name = "ns.test_rosout_subscription.child1"; + this->rosout_msg_data = "SOMETHING"; + this->rosout_msg_name = logger_name; + + rclcpp::Logger logger = this->node->get_logger(); + RCLCPP_INFO(logger, "%s", this->rosout_msg_data.c_str()); + auto future = received_msg_promise.get_future(); + auto return_code = rclcpp::spin_until_future_complete(this->node, future, 3s); + ASSERT_EQ(rclcpp::FutureReturnCode::TIMEOUT, return_code); + received_msg_promise = {}; + + logger = this->node->get_logger().get_child("child1"); + RCLCPP_INFO(logger, "%s", this->rosout_msg_data.c_str()); + future = received_msg_promise.get_future(); + return_code = rclcpp::spin_until_future_complete(this->node, future, 3s); + ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); + EXPECT_TRUE(future.get()); + received_msg_promise = {}; + + logger = this->node->get_logger().get_child("child2"); + RCLCPP_INFO(logger, "%s", this->rosout_msg_data.c_str()); + future = received_msg_promise.get_future(); + return_code = rclcpp::spin_until_future_complete(this->node, future, 3s); + ASSERT_EQ(rclcpp::FutureReturnCode::TIMEOUT, return_code); + received_msg_promise = {}; + + this->rosout_msg_name = "ns.test_rosout_subscription.child2"; + RCLCPP_INFO(logger, "%s", this->rosout_msg_data.c_str()); + future = received_msg_promise.get_future(); + return_code = rclcpp::spin_until_future_complete(this->node, future, 3s); + ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); + EXPECT_TRUE(future.get()); + received_msg_promise = {}; +} + +TEST_F(TestRosoutSubscription, test_rosoutsubscription_getchild_hierarchy) { + std::string logger_name = "ns.test_rosout_subscription.child.grandchild"; + this->rosout_msg_data = "SOMETHING"; + this->rosout_msg_name = logger_name; + + rclcpp::Logger grandchild_logger = + this->node->get_logger().get_child("child").get_child("grandchild"); + ASSERT_EQ(grandchild_logger.get_name(), logger_name); + RCLCPP_INFO(grandchild_logger, "%s", this->rosout_msg_data.c_str()); + auto future = received_msg_promise.get_future(); + auto return_code = rclcpp::spin_until_future_complete(this->node, future, 3s); + ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); + EXPECT_TRUE(future.get()); + received_msg_promise = {}; +} + +TEST_F(TestRosoutSubscription, test_rosoutsubscription_node_rosout_disabled) { + rclcpp::NodeOptions options = rclcpp::NodeOptions().enable_rosout(false); + auto node = std::make_shared("my_node", options); + auto log_func = [&node] { + auto logger = node->get_logger().get_child("child"); + RCLCPP_INFO(logger, "test"); + }; + ASSERT_NO_THROW(log_func()); +} diff --git a/rclcpp/test/rclcpp/test_serialized_message.cpp b/rclcpp/test/rclcpp/test_serialized_message.cpp new file mode 100644 index 0000000000..67a2d9b2be --- /dev/null +++ b/rclcpp/test/rclcpp/test_serialized_message.cpp @@ -0,0 +1,291 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include + +#include "rclcpp/serialization.hpp" +#include "rclcpp/serialized_message.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "rcpputils/asserts.hpp" + +#include "test_msgs/message_fixtures.hpp" +#include "test_msgs/msg/basic_types.hpp" + +TEST(TestSerializedMessage, empty_initialize) { + rclcpp::SerializedMessage serialized_message; + EXPECT_EQ(0u, serialized_message.size()); + EXPECT_EQ(0u, serialized_message.capacity()); +} + +TEST(TestSerializedMessage, initialize_with_capacity) { + rclcpp::SerializedMessage serialized_message(13); + EXPECT_EQ(0u, serialized_message.size()); + EXPECT_EQ(13u, serialized_message.capacity()); +} + +TEST(TestSerializedMessage, various_constructors) { + const std::string content = "Hello World"; + const auto content_size = content.size() + 1; // accounting for null terminator + + rclcpp::SerializedMessage serialized_message(content_size); + // manually copy some content + auto & rcl_handle = serialized_message.get_rcl_serialized_message(); + std::memcpy(rcl_handle.buffer, content.c_str(), content.size()); + rcl_handle.buffer[content.size()] = '\0'; + rcl_handle.buffer_length = content_size; + EXPECT_STREQ(content.c_str(), reinterpret_cast(rcl_handle.buffer)); + EXPECT_EQ(content_size, serialized_message.capacity()); + EXPECT_EQ(content_size, serialized_message.size()); + + // Copy Constructor + rclcpp::SerializedMessage other_serialized_message(serialized_message); + EXPECT_EQ(content_size, other_serialized_message.capacity()); + EXPECT_EQ(content_size, other_serialized_message.size()); + auto & other_rcl_handle = other_serialized_message.get_rcl_serialized_message(); + EXPECT_STREQ( + reinterpret_cast(rcl_handle.buffer), + reinterpret_cast(other_rcl_handle.buffer)); + + // Move Constructor + rclcpp::SerializedMessage yet_another_serialized_message(std::move(other_serialized_message)); + auto & yet_another_rcl_handle = yet_another_serialized_message.get_rcl_serialized_message(); + EXPECT_TRUE(nullptr == other_rcl_handle.buffer); + EXPECT_TRUE(nullptr != yet_another_rcl_handle.buffer); + EXPECT_EQ(content_size, yet_another_serialized_message.size()); + EXPECT_EQ(content_size, yet_another_serialized_message.capacity()); +} + +TEST(TestSerializedMessage, various_constructors_from_rcl) { + const std::string content = "Hello World"; + const auto content_size = content.size() + 1; // accounting for null terminator + + auto default_allocator = rcl_get_default_allocator(); + + auto rcl_serialized_msg = rmw_get_zero_initialized_serialized_message(); + auto ret = rmw_serialized_message_init(&rcl_serialized_msg, 13, &default_allocator); + ASSERT_EQ(RCL_RET_OK, ret); + // manually copy some content + std::memcpy(rcl_serialized_msg.buffer, content.c_str(), content.size()); + rcl_serialized_msg.buffer[content.size()] = '\0'; + rcl_serialized_msg.buffer_length = content_size; + EXPECT_EQ(13u, rcl_serialized_msg.buffer_capacity); + + // Copy Constructor from rcl_serialized_message_t + rclcpp::SerializedMessage serialized_message(rcl_serialized_msg); + EXPECT_EQ(13u, serialized_message.capacity()); + EXPECT_EQ(content_size, serialized_message.size()); + + // Move Constructor from rcl_serialized_message_t + rclcpp::SerializedMessage another_serialized_message(std::move(rcl_serialized_msg)); + EXPECT_TRUE(nullptr == rcl_serialized_msg.buffer); + EXPECT_EQ(0u, rcl_serialized_msg.buffer_capacity); + EXPECT_EQ(0u, rcl_serialized_msg.buffer_length); + EXPECT_EQ(13u, another_serialized_message.capacity()); + EXPECT_EQ(content_size, another_serialized_message.size()); + + // Verify that despite being fini'd, the copy is real + ret = rmw_serialized_message_fini(&rcl_serialized_msg); + ASSERT_EQ(RCUTILS_RET_INVALID_ARGUMENT, ret); // Buffer is null, because it was moved + EXPECT_EQ(nullptr, rcl_serialized_msg.buffer); + EXPECT_EQ(0u, rcl_serialized_msg.buffer_capacity); + EXPECT_EQ(0u, rcl_serialized_msg.buffer_length); + EXPECT_EQ(13u, serialized_message.capacity()); + EXPECT_EQ(content_size, serialized_message.size()); + + auto rcl_handle = serialized_message.get_rcl_serialized_message(); + EXPECT_TRUE(nullptr != rcl_handle.buffer); +} + +TEST(TestSerializedMessage, release) { + const std::string content = "Hello World"; + const auto content_size = content.size() + 1; // accounting for null terminator + + rcl_serialized_message_t released_handle = rmw_get_zero_initialized_serialized_message(); + { + rclcpp::SerializedMessage serialized_msg(13); + // manually copy some content + auto & rcl_serialized_msg = serialized_msg.get_rcl_serialized_message(); + std::memcpy(rcl_serialized_msg.buffer, content.c_str(), content.size()); + rcl_serialized_msg.buffer[content.size()] = '\0'; + rcl_serialized_msg.buffer_length = content_size; + EXPECT_EQ(13u, serialized_msg.capacity()); + + released_handle = serialized_msg.release_rcl_serialized_message(); + // scope exit of serialized_msg + } + + EXPECT_TRUE(nullptr != released_handle.buffer); + EXPECT_EQ(13u, released_handle.buffer_capacity); + EXPECT_EQ(content_size, released_handle.buffer_length); + // cleanup memory manually + EXPECT_EQ(RCL_RET_OK, rmw_serialized_message_fini(&released_handle)); +} + +TEST(TestSerializedMessage, reserve) { + rclcpp::SerializedMessage serialized_msg(13); + EXPECT_EQ(13u, serialized_msg.capacity()); + + // Resize using reserve method + serialized_msg.reserve(15); + EXPECT_EQ(15u, serialized_msg.capacity()); +} + +TEST(TestSerializedMessage, serialization) { + using MessageT = test_msgs::msg::BasicTypes; + + rclcpp::Serialization serializer; + + auto basic_type_ros_msgs = get_messages_basic_types(); + for (const auto & ros_msg : basic_type_ros_msgs) { + // convert ros msg to serialized msg + rclcpp::SerializedMessage serialized_msg; + serializer.serialize_message(ros_msg.get(), &serialized_msg); + + // convert serialized msg back to ros msg + MessageT deserialized_ros_msg; + serializer.deserialize_message(&serialized_msg, &deserialized_ros_msg); + + EXPECT_EQ(*ros_msg, deserialized_ros_msg); + } +} + +TEST(TestSerializedMessage, assignment_operators) { + const std::string content = "Hello World"; + const auto content_size = content.size() + 1; // accounting for null terminator + auto default_allocator = rcl_get_default_allocator(); + auto rcl_serialized_msg = rmw_get_zero_initialized_serialized_message(); + auto ret = rmw_serialized_message_init(&rcl_serialized_msg, 13, &default_allocator); + ASSERT_EQ(RCL_RET_OK, ret); + + // manually copy some content + std::memcpy(rcl_serialized_msg.buffer, content.c_str(), content_size); + rcl_serialized_msg.buffer_length = content_size; + EXPECT_EQ(13u, rcl_serialized_msg.buffer_capacity); + rclcpp::SerializedMessage serialized_message_to_assign(rcl_serialized_msg); + EXPECT_EQ(13u, serialized_message_to_assign.capacity()); + EXPECT_EQ(content_size, serialized_message_to_assign.size()); + + // Test copy assignment with = operator, on another rclcpp::SerializedMessage + rclcpp::SerializedMessage serialized_msg_copy(2); + EXPECT_EQ(2u, serialized_msg_copy.capacity()); + EXPECT_EQ(0u, serialized_msg_copy.size()); + serialized_msg_copy = serialized_message_to_assign; + EXPECT_EQ(13u, serialized_msg_copy.capacity()); + EXPECT_EQ(content_size, serialized_msg_copy.size()); + + // Test copy assignment with = operator, with a rcl_serialized_message_t + rclcpp::SerializedMessage serialized_msg_copy_rcl(2); + EXPECT_EQ(2u, serialized_msg_copy_rcl.capacity()); + EXPECT_EQ(0u, serialized_msg_copy_rcl.size()); + serialized_msg_copy_rcl = rcl_serialized_msg; + EXPECT_EQ(13u, serialized_msg_copy_rcl.capacity()); + EXPECT_EQ(content_size, serialized_msg_copy_rcl.size()); + + // Test move assignment with = operator + rclcpp::SerializedMessage serialized_msg_move(2); + EXPECT_EQ(2u, serialized_msg_move.capacity()); + EXPECT_EQ(0u, serialized_msg_move.size()); + serialized_msg_move = std::move(serialized_message_to_assign); + EXPECT_EQ(13u, serialized_msg_move.capacity()); + EXPECT_EQ(content_size, serialized_msg_move.size()); + + // Test move assignment with = operator, with a rcl_serialized_message_t + rclcpp::SerializedMessage serialized_msg_move_rcl(2); + EXPECT_EQ(2u, serialized_msg_move_rcl.capacity()); + EXPECT_EQ(0u, serialized_msg_move_rcl.size()); + serialized_msg_move_rcl = std::move(rcl_serialized_msg); + EXPECT_EQ(13u, serialized_msg_move_rcl.capacity()); + EXPECT_EQ(content_size, serialized_msg_move_rcl.size()); + + // Error because it was moved + EXPECT_EQ(RCUTILS_RET_INVALID_ARGUMENT, rmw_serialized_message_fini(&rcl_serialized_msg)); +} + +TEST(TestSerializedMessage, failed_init_throws) { + rclcpp::SerializedMessage serialized_msg(13); + EXPECT_EQ(13u, serialized_msg.capacity()); + + // Constructor with invalid size throws exception + EXPECT_THROW( + {rclcpp::SerializedMessage serialized_msg_fail(-1);}, + rclcpp::exceptions::RCLBadAlloc); + + // Constructor copy with rmw_serialized bad msg throws + auto default_allocator = rcl_get_default_allocator(); + auto rcl_serialized_msg = rmw_get_zero_initialized_serialized_message(); + auto ret = rmw_serialized_message_init(&rcl_serialized_msg, 13, &default_allocator); + ASSERT_EQ(RCL_RET_OK, ret); + EXPECT_EQ(13u, rcl_serialized_msg.buffer_capacity); + rcl_serialized_msg.buffer_capacity = -1; + EXPECT_THROW( + {rclcpp::SerializedMessage serialized_msg_fail_2(rcl_serialized_msg);}, + rclcpp::exceptions::RCLBadAlloc); + + rcl_serialized_msg.buffer_capacity = 13; + EXPECT_EQ(RCL_RET_OK, rmw_serialized_message_fini(&rcl_serialized_msg)); +} + +void serialize_default_ros_msg() +{ + using MessageT = test_msgs::msg::BasicTypes; + rclcpp::Serialization serializer; + MessageT ros_msg; + rclcpp::SerializedMessage serialized_msg; + + serializer.serialize_message(&ros_msg, &serialized_msg); +} + +void serialize_default_ros_msg_into_nullptr() +{ + using MessageT = test_msgs::msg::BasicTypes; + rclcpp::Serialization serializer; + MessageT ros_msg; + + serializer.serialize_message(&ros_msg, nullptr); +} + +void deserialize_default_serialized_message() +{ + using MessageT = test_msgs::msg::BasicTypes; + rclcpp::Serialization serializer; + MessageT ros_msg; + rclcpp::SerializedMessage serialized_msg; + + serializer.deserialize_message(&serialized_msg, &ros_msg); +} + +void deserialize_nullptr() +{ + using MessageT = test_msgs::msg::BasicTypes; + rclcpp::Serialization serializer; + MessageT ros_msg; + rclcpp::SerializedMessage serialized_msg; + + serializer.deserialize_message(&serialized_msg, &ros_msg); +} + +TEST(TestSerializedMessage, serialization_empty_messages) +{ + EXPECT_NO_THROW(serialize_default_ros_msg()); + EXPECT_THROW(serialize_default_ros_msg_into_nullptr(), rcpputils::IllegalStateException); + EXPECT_THROW(serialize_default_ros_msg_into_nullptr(), rcpputils::IllegalStateException); + EXPECT_THROW(deserialize_default_serialized_message(), rcpputils::IllegalStateException); + EXPECT_THROW(deserialize_nullptr(), rcpputils::IllegalStateException); +} diff --git a/rclcpp/test/rclcpp/test_serialized_message_allocator.cpp b/rclcpp/test/rclcpp/test_serialized_message_allocator.cpp new file mode 100644 index 0000000000..78ab77a28f --- /dev/null +++ b/rclcpp/test/rclcpp/test_serialized_message_allocator.cpp @@ -0,0 +1,68 @@ +// 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 "rclcpp/rclcpp.hpp" + +#include "rcl/types.h" + +#include "test_msgs/msg/empty.hpp" + +TEST(TestSerializedMessageAllocator, default_allocator) { + using DummyMessageT = float; + auto mem_strategy = + rclcpp::message_memory_strategy::MessageMemoryStrategy::create_default(); + + auto msg0 = mem_strategy->borrow_serialized_message(); + ASSERT_EQ(msg0->capacity(), 0u); + mem_strategy->return_serialized_message(msg0); + + auto msg100 = mem_strategy->borrow_serialized_message(100); + ASSERT_EQ(msg100->capacity(), 100u); + mem_strategy->return_serialized_message(msg100); + + auto msg200 = mem_strategy->borrow_serialized_message(); + auto ret = rmw_serialized_message_resize(&msg200->get_rcl_serialized_message(), 200); + ASSERT_EQ(RCL_RET_OK, ret); + EXPECT_EQ(0u, msg200->size()); + EXPECT_EQ(200u, msg200->capacity()); + mem_strategy->return_serialized_message(msg200); + + auto msg1000 = mem_strategy->borrow_serialized_message(1000); + ASSERT_EQ(msg1000->capacity(), 1000u); + ret = rmw_serialized_message_resize(&msg1000->get_rcl_serialized_message(), 2000); + ASSERT_EQ(RCL_RET_OK, ret); + EXPECT_EQ(2000u, msg1000->capacity()); + mem_strategy->return_serialized_message(msg1000); +} + +TEST(TestSerializedMessageAllocator, borrow_from_subscription) { + rclcpp::init(0, NULL); + + auto node = std::make_shared("test_serialized_message_allocator_node"); + std::shared_ptr sub = + node->create_subscription( + "~/dummy_topic", 10, + [](std::shared_ptr test_msg) {(void) test_msg;}); + + auto msg0 = sub->create_serialized_message(); + EXPECT_EQ(0u, msg0->capacity()); + sub->return_serialized_message(msg0); + + rclcpp::shutdown(); +} diff --git a/rclcpp/test/rclcpp/test_service.cpp b/rclcpp/test/rclcpp/test_service.cpp new file mode 100644 index 0000000000..c6b3d3ace1 --- /dev/null +++ b/rclcpp/test/rclcpp/test_service.cpp @@ -0,0 +1,401 @@ +// 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 "rclcpp/exceptions.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "../mocking_utils/patch.hpp" +#include "../utils/rclcpp_gtest_macros.hpp" + +#include "rcl_interfaces/srv/list_parameters.hpp" +#include "test_msgs/srv/empty.hpp" +#include "test_msgs/srv/empty.h" + +using namespace std::chrono_literals; + +class TestService : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node = std::make_shared("my_node", "/ns"); + } + + void TearDown() + { + node.reset(); + } + + rclcpp::Node::SharedPtr node; +}; + +class TestServiceSub : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node = std::make_shared("my_node", "/ns"); + subnode = node->create_sub_node("sub_ns"); + } + + void TearDown() + { + node.reset(); + } + + rclcpp::Node::SharedPtr node; + rclcpp::Node::SharedPtr subnode; +}; + +/* + Testing service construction and destruction. + */ +TEST_F(TestService, construction_and_destruction) { + auto callback = []( + const rcl_interfaces::srv::ListParameters::Request::SharedPtr, + rcl_interfaces::srv::ListParameters::Response::SharedPtr) {}; + { + auto service = node->create_service("service", callback); + EXPECT_NE(nullptr, service->get_service_handle()); + const rclcpp::ServiceBase * const_service_base = service.get(); + EXPECT_NE(nullptr, const_service_base->get_service_handle()); + } + + { + ASSERT_THROW( + { + auto service = node->create_service( + "invalid_service?", callback); + }, rclcpp::exceptions::InvalidServiceNameError); + } +} + +/* + Testing service construction and destruction for subnodes. + */ +TEST_F(TestServiceSub, construction_and_destruction) { + auto callback = []( + const rcl_interfaces::srv::ListParameters::Request::SharedPtr, + rcl_interfaces::srv::ListParameters::Response::SharedPtr) {}; + { + auto service = subnode->create_service( + "service", callback); + EXPECT_STREQ(service->get_service_name(), "/ns/sub_ns/service"); + } + + { + ASSERT_THROW( + { + auto service = subnode->create_service( + "invalid_service?", callback); + }, rclcpp::exceptions::InvalidServiceNameError); + } +} + +TEST_F(TestService, construction_and_destruction_rcl_errors) { + auto callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, test_msgs::srv::Empty::Response::SharedPtr) {}; + + { + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_service_init, RCL_RET_ERROR); + // reset() isn't necessary for this exception, it just avoids unused return value warning + EXPECT_THROW( + node->create_service("service", callback).reset(), + rclcpp::exceptions::RCLError); + } + { + // reset() is required for this one + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_service_fini, RCL_RET_ERROR); + EXPECT_NO_THROW(node->create_service("service", callback).reset()); + } +} + +/* Testing basic getters */ +TEST_F(TestService, basic_public_getters) { + auto callback = []( + const rcl_interfaces::srv::ListParameters::Request::SharedPtr, + rcl_interfaces::srv::ListParameters::Response::SharedPtr) {}; + auto service = node->create_service("service", callback); + EXPECT_STREQ(service->get_service_name(), "/ns/service"); + std::shared_ptr service_handle = service->get_service_handle(); + EXPECT_NE(nullptr, service_handle); + + { + // Create a extern defined const service + auto node_handle_int = rclcpp::Node::make_shared("base_node"); + rcl_service_t service_handle = rcl_get_zero_initialized_service(); + rcl_service_options_t service_options = rcl_service_get_default_options(); + const rosidl_service_type_support_t * ts = + rosidl_typesupport_cpp::get_service_type_support_handle(); + rcl_ret_t ret = rcl_service_init( + &service_handle, + node_handle_int->get_node_base_interface()->get_rcl_node_handle(), + ts, "base_node_service", &service_options); + if (ret != RCL_RET_OK) { + FAIL(); + return; + } + rclcpp::AnyServiceCallback cb; + const rclcpp::Service base( + node_handle_int->get_node_base_interface()->get_shared_rcl_node_handle(), + &service_handle, cb); + // Use get_service_handle specific to const service + std::shared_ptr const_service_handle = base.get_service_handle(); + EXPECT_NE(nullptr, const_service_handle); + + EXPECT_EQ( + RCL_RET_OK, rcl_service_fini( + &service_handle, + node_handle_int->get_node_base_interface()->get_rcl_node_handle())); + } +} + +TEST_F(TestService, take_request) { + auto callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, test_msgs::srv::Empty::Response::SharedPtr) {}; + auto server = node->create_service("service", callback); + { + auto request_id = server->create_request_header(); + test_msgs::srv::Empty::Request request; + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_take_request, RCL_RET_OK); + EXPECT_TRUE(server->take_request(request, *request_id.get())); + } + { + auto request_id = server->create_request_header(); + test_msgs::srv::Empty::Request request; + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_take_request, RCL_RET_SERVICE_TAKE_FAILED); + EXPECT_FALSE(server->take_request(request, *request_id.get())); + } + { + auto request_id = server->create_request_header(); + test_msgs::srv::Empty::Request request; + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_take_request, RCL_RET_ERROR); + EXPECT_THROW(server->take_request(request, *request_id.get()), rclcpp::exceptions::RCLError); + } +} + +TEST_F(TestService, send_response) { + auto callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, test_msgs::srv::Empty::Response::SharedPtr) {}; + auto server = node->create_service("service", callback); + + { + auto request_id = server->create_request_header(); + test_msgs::srv::Empty::Response response; + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_send_response, RCL_RET_OK); + EXPECT_NO_THROW(server->send_response(*request_id.get(), response)); + } + + { + auto request_id = server->create_request_header(); + test_msgs::srv::Empty::Response response; + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_send_response, RCL_RET_ERROR); + EXPECT_THROW( + server->send_response(*request_id.get(), response), + rclcpp::exceptions::RCLError); + } +} + +/* + Testing on_new_request callbacks. + */ +TEST_F(TestService, on_new_request_callback) { + auto server_callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {FAIL();}; + rclcpp::ServicesQoS service_qos; + service_qos.keep_last(3); + auto server = node->create_service( + "~/test_service", server_callback, service_qos); + + std::atomic c1 {0}; + auto increase_c1_cb = [&c1](size_t count_msgs) {c1 += count_msgs;}; + server->set_on_new_request_callback(increase_c1_cb); + + auto client = node->create_client( + "~/test_service", service_qos); + { + auto request = std::make_shared(); + client->async_send_request(request); + } + + auto start = std::chrono::steady_clock::now(); + do { + std::this_thread::sleep_for(100ms); + } while (c1 == 0 && std::chrono::steady_clock::now() - start < 10s); + + EXPECT_EQ(c1.load(), 1u); + + std::atomic c2 {0}; + auto increase_c2_cb = [&c2](size_t count_msgs) {c2 += count_msgs;}; + server->set_on_new_request_callback(increase_c2_cb); + + { + auto request = std::make_shared(); + client->async_send_request(request); + } + + start = std::chrono::steady_clock::now(); + do { + std::this_thread::sleep_for(100ms); + } while (c2 == 0 && std::chrono::steady_clock::now() - start < 10s); + + EXPECT_EQ(c1.load(), 1u); + EXPECT_EQ(c2.load(), 1u); + + server->clear_on_new_request_callback(); + + { + auto request = std::make_shared(); + client->async_send_request(request); + client->async_send_request(request); + client->async_send_request(request); + } + + std::atomic c3 {0}; + auto increase_c3_cb = [&c3](size_t count_msgs) {c3 += count_msgs;}; + server->set_on_new_request_callback(increase_c3_cb); + + start = std::chrono::steady_clock::now(); + do { + std::this_thread::sleep_for(100ms); + } while (c3 < 3 && std::chrono::steady_clock::now() - start < 10s); + + EXPECT_EQ(c1.load(), 1u); + EXPECT_EQ(c2.load(), 1u); + EXPECT_EQ(c3.load(), 3u); + + std::function invalid_cb = nullptr; + EXPECT_THROW(server->set_on_new_request_callback(invalid_cb), std::invalid_argument); +} + +TEST_F(TestService, rcl_service_response_publisher_get_actual_qos_error) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_service_response_publisher_get_actual_qos, nullptr); + auto callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, test_msgs::srv::Empty::Response::SharedPtr) {}; + auto server = node->create_service("service", callback); + RCLCPP_EXPECT_THROW_EQ( + server->get_response_publisher_actual_qos(), + std::runtime_error("failed to get service's response publisher qos settings: error not set")); +} + +TEST_F(TestService, rcl_service_request_subscription_get_actual_qos_error) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_service_request_subscription_get_actual_qos, nullptr); + auto callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, test_msgs::srv::Empty::Response::SharedPtr) {}; + auto server = node->create_service("service", callback); + RCLCPP_EXPECT_THROW_EQ( + server->get_request_subscription_actual_qos(), + std::runtime_error("failed to get service's request subscription qos settings: error not set")); +} + + +TEST_F(TestService, server_qos) { + rclcpp::ServicesQoS qos_profile; + qos_profile.liveliness(rclcpp::LivelinessPolicy::Automatic); + rclcpp::Duration duration(std::chrono::nanoseconds(1)); + qos_profile.deadline(duration); + qos_profile.lifespan(duration); + qos_profile.liveliness_lease_duration(duration); + + auto callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, test_msgs::srv::Empty::Response::SharedPtr) {}; + + auto server = node->create_service("service", callback, qos_profile); + auto rs_qos = server->get_request_subscription_actual_qos(); + auto rp_qos = server->get_response_publisher_actual_qos(); + + EXPECT_EQ(qos_profile, rp_qos); + // Lifespan has no meaning for subscription/readers + rs_qos.lifespan(qos_profile.lifespan()); + EXPECT_EQ(qos_profile, rs_qos); +} + +TEST_F(TestService, server_qos_depth) { + uint64_t server_cb_count_ = 0; + auto server_callback = [&]( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {server_cb_count_++;}; + + auto server_node = std::make_shared("server_node", "/ns"); + + rclcpp::QoS server_qos_profile(2); + + auto server = server_node->create_service( + "test_qos_depth", std::move(server_callback), server_qos_profile); + + rclcpp::QoS client_qos_profile(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); + auto client = node->create_client("test_qos_depth", client_qos_profile); + + ::testing::AssertionResult request_result = ::testing::AssertionSuccess(); + auto request = std::make_shared(); + + auto client_callback = [&request_result]( + rclcpp::Client::SharedFuture future_response) { + if (nullptr == future_response.get()) { + request_result = ::testing::AssertionFailure() << "Future response was null"; + } + }; + + uint64_t client_requests = 5; + for (uint64_t i = 0; i < client_requests; i++) { + client->async_send_request(request, client_callback); + std::this_thread::sleep_for(10ms); + } + + auto start = std::chrono::steady_clock::now(); + while ((server_cb_count_ < server_qos_profile.depth()) && + (std::chrono::steady_clock::now() - start) < 1s) + { + rclcpp::spin_some(server_node); + std::this_thread::sleep_for(1ms); + } + + // Spin an extra time to check if server QoS depth has been ignored, + // so more server responses might be processed than expected. + rclcpp::spin_some(server_node); + + EXPECT_EQ(server_cb_count_, server_qos_profile.depth()); +} diff --git a/rclcpp/test/rclcpp/test_service_introspection.cpp b/rclcpp/test/rclcpp/test_service_introspection.cpp new file mode 100644 index 0000000000..af60eec281 --- /dev/null +++ b/rclcpp/test/rclcpp/test_service_introspection.cpp @@ -0,0 +1,353 @@ +// 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 + +#include +#include + +#include "gmock/gmock.h" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/exceptions.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp/node_options.hpp" +#include "rclcpp/parameter.hpp" + +#include "../mocking_utils/patch.hpp" +#include "../utils/rclcpp_gtest_macros.hpp" + +#include "test_msgs/srv/basic_types.hpp" +#include "service_msgs/msg/service_event_info.hpp" + +using namespace std::chrono_literals; +using test_msgs::srv::BasicTypes; +using service_msgs::msg::ServiceEventInfo; + + +class TestServiceIntrospection : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node = std::make_shared( + "my_node", "/ns"); + + auto srv_callback = + [](const BasicTypes::Request::SharedPtr & req, const BasicTypes::Response::SharedPtr & resp) { + resp->set__bool_value(!req->bool_value); + resp->set__int64_value(req->int64_value); + return resp; + }; + + auto callback = [this](const std::shared_ptr & msg) { + events.push_back(msg); + (void)msg; + }; + + client = node->create_client("service"); + service = node->create_service("service", srv_callback); + sub = node->create_subscription("service/_service_event", 10, callback); + events.clear(); + } + + void TearDown() + { + node.reset(); + } + + rclcpp::Node::SharedPtr node; + rclcpp::Client::SharedPtr client; + rclcpp::Service::SharedPtr service; + rclcpp::Subscription::SharedPtr sub; + std::vector> events; + std::chrono::milliseconds timeout = std::chrono::milliseconds(1000); +}; + +TEST_F(TestServiceIntrospection, service_introspection_nominal) +{ + auto request = std::make_shared(); + request->set__bool_value(true); + request->set__int64_value(42); + + client->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_CONTENTS); + service->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_CONTENTS); + + auto future = client->async_send_request(request); + ASSERT_EQ( + rclcpp::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future, timeout)); + + BasicTypes::Response::SharedPtr response = future.get(); + ASSERT_EQ(response->bool_value, false); + ASSERT_EQ(response->int64_value, 42); + + // wrap up work to get all the service_event messages + auto start = std::chrono::steady_clock::now(); + while (events.size() < 4 && (std::chrono::steady_clock::now() - start) < timeout) { + rclcpp::spin_some(node); + } + + std::map> event_map; + for (auto & event : events) { + event_map[event->info.event_type] = event; + } + ASSERT_EQ(event_map.size(), 4U); + + rmw_gid_t client_gid; + rmw_get_gid_for_client(rcl_client_get_rmw_handle(client->get_client_handle().get()), &client_gid); + std::array client_gid_arr; + std::move(std::begin(client_gid.data), std::end(client_gid.data), client_gid_arr.begin()); + ASSERT_THAT( + client_gid_arr, + testing::Eq(event_map[ServiceEventInfo::REQUEST_SENT]->info.client_gid)); + // TODO(@fujitatomoya): Remove this if statement once rmw implementations support test. + // rmw_cyclonedds_cpp does not pass this test requirement for now. + // See more details for https://github.com/ros2/rmw/issues/357 + if (std::string(rmw_get_implementation_identifier()).find("rmw_cyclonedds_cpp") != 0) { + ASSERT_THAT( + client_gid_arr, + testing::Eq(event_map[ServiceEventInfo::REQUEST_RECEIVED]->info.client_gid)); + ASSERT_THAT( + client_gid_arr, + testing::Eq(event_map[ServiceEventInfo::RESPONSE_SENT]->info.client_gid)); + } + ASSERT_THAT( + client_gid_arr, + testing::Eq(event_map[ServiceEventInfo::RESPONSE_RECEIVED]->info.client_gid)); + + ASSERT_EQ( + event_map[ServiceEventInfo::REQUEST_SENT]->info.sequence_number, + event_map[ServiceEventInfo::REQUEST_RECEIVED]->info.sequence_number); + ASSERT_EQ( + event_map[ServiceEventInfo::RESPONSE_SENT]->info.sequence_number, + event_map[ServiceEventInfo::RESPONSE_RECEIVED]->info.sequence_number); + ASSERT_EQ( + event_map[ServiceEventInfo::REQUEST_SENT]->info.sequence_number, + event_map[ServiceEventInfo::RESPONSE_SENT]->info.sequence_number); + ASSERT_EQ( + event_map[ServiceEventInfo::REQUEST_RECEIVED]->info.sequence_number, + event_map[ServiceEventInfo::RESPONSE_RECEIVED]->info.sequence_number); + + ASSERT_EQ(event_map[ServiceEventInfo::REQUEST_SENT]->request[0].int64_value, 42); + ASSERT_EQ(event_map[ServiceEventInfo::REQUEST_SENT]->request[0].bool_value, true); + ASSERT_EQ(event_map[ServiceEventInfo::RESPONSE_SENT]->response[0].int64_value, 42); + ASSERT_EQ(event_map[ServiceEventInfo::RESPONSE_SENT]->response[0].bool_value, false); + ASSERT_EQ(event_map[ServiceEventInfo::RESPONSE_SENT]->request.size(), 0U); + ASSERT_EQ(event_map[ServiceEventInfo::REQUEST_RECEIVED]->response.size(), 0U); +} + +TEST_F(TestServiceIntrospection, service_introspection_enable_disable_events) +{ + client->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_OFF); + service->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_OFF); + + auto request = std::make_shared(); + request->set__bool_value(true); + request->set__int64_value(42); + auto future = client->async_send_request(request); + ASSERT_EQ( + rclcpp::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future, timeout)); + auto start = std::chrono::steady_clock::now(); + while ((std::chrono::steady_clock::now() - start) < timeout) { + rclcpp::spin_some(node); + } + EXPECT_EQ(events.size(), 0U); + + events.clear(); + + client->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_METADATA); + service->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_OFF); + + future = client->async_send_request(request); + ASSERT_EQ( + rclcpp::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future, timeout)); + start = std::chrono::steady_clock::now(); + while (events.size() < 2 && (std::chrono::steady_clock::now() - start) < timeout) { + rclcpp::spin_some(node); + } + EXPECT_EQ(events.size(), 2U); + + events.clear(); + + client->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_OFF); + service->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_METADATA); + + future = client->async_send_request(request); + ASSERT_EQ( + rclcpp::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future, timeout)); + start = std::chrono::steady_clock::now(); + while (events.size() < 2 && (std::chrono::steady_clock::now() - start) < timeout) { + rclcpp::spin_some(node); + } + EXPECT_EQ(events.size(), 2U); + + events.clear(); + + client->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_METADATA); + service->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_METADATA); + + future = client->async_send_request(request); + ASSERT_EQ( + rclcpp::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future, timeout)); + start = std::chrono::steady_clock::now(); + while (events.size() < 4 && (std::chrono::steady_clock::now() - start) < timeout) { + rclcpp::spin_some(node); + } + EXPECT_EQ(events.size(), 4U); +} + +TEST_F(TestServiceIntrospection, service_introspection_enable_disable_event_content) +{ + client->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_METADATA); + service->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_METADATA); + + auto request = std::make_shared(); + request->set__bool_value(true); + request->set__int64_value(42); + auto future = client->async_send_request(request); + ASSERT_EQ( + rclcpp::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future, timeout)); + auto start = std::chrono::steady_clock::now(); + while (events.size() < 4 && (std::chrono::steady_clock::now() - start) < timeout) { + rclcpp::spin_some(node); + } + EXPECT_EQ(events.size(), 4U); + for (const auto & event : events) { + EXPECT_EQ(event->request.size(), 0U); + EXPECT_EQ(event->response.size(), 0U); + } + + events.clear(); + + client->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_CONTENTS); + service->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_METADATA); + + future = client->async_send_request(request); + ASSERT_EQ( + rclcpp::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future, timeout)); + start = std::chrono::steady_clock::now(); + while (events.size() < 4 && (std::chrono::steady_clock::now() - start) < timeout) { + rclcpp::spin_some(node); + } + EXPECT_EQ(events.size(), 4U); + for (const auto & event : events) { + switch (event->info.event_type) { + case ServiceEventInfo::REQUEST_SENT: + EXPECT_EQ(event->request.size(), 1U); + break; + case ServiceEventInfo::REQUEST_RECEIVED: + EXPECT_EQ(event->request.size(), 0U); + break; + case ServiceEventInfo::RESPONSE_SENT: + EXPECT_EQ(event->response.size(), 0U); + break; + case ServiceEventInfo::RESPONSE_RECEIVED: + EXPECT_EQ(event->response.size(), 1U); + break; + } + } + + events.clear(); + + client->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_METADATA); + service->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_CONTENTS); + + future = client->async_send_request(request); + ASSERT_EQ( + rclcpp::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future, timeout)); + start = std::chrono::steady_clock::now(); + while (events.size() < 4 && (std::chrono::steady_clock::now() - start) < timeout) { + rclcpp::spin_some(node); + } + EXPECT_EQ(events.size(), 4U); + for (const auto & event : events) { + switch (event->info.event_type) { + case ServiceEventInfo::REQUEST_SENT: + EXPECT_EQ(event->request.size(), 0U); + break; + case ServiceEventInfo::REQUEST_RECEIVED: + EXPECT_EQ(event->request.size(), 1U); + break; + case ServiceEventInfo::RESPONSE_SENT: + EXPECT_EQ(event->response.size(), 1U); + break; + case ServiceEventInfo::RESPONSE_RECEIVED: + EXPECT_EQ(event->response.size(), 0U); + break; + } + } + + events.clear(); + + client->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_CONTENTS); + service->configure_introspection( + node->get_clock(), rclcpp::SystemDefaultsQoS(), RCL_SERVICE_INTROSPECTION_CONTENTS); + + future = client->async_send_request(request); + ASSERT_EQ( + rclcpp::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future, timeout)); + start = std::chrono::steady_clock::now(); + while (events.size() < 4 && (std::chrono::steady_clock::now() - start) < timeout) { + rclcpp::spin_some(node); + } + EXPECT_EQ(events.size(), 4U); + for (const auto & event : events) { + switch (event->info.event_type) { + case ServiceEventInfo::REQUEST_SENT: + case ServiceEventInfo::REQUEST_RECEIVED: + EXPECT_EQ(event->request.size(), 1U); + break; + case ServiceEventInfo::RESPONSE_SENT: + case ServiceEventInfo::RESPONSE_RECEIVED: + EXPECT_EQ(event->response.size(), 1U); + break; + } + } +} diff --git a/rclcpp/test/rclcpp/test_subscription.cpp b/rclcpp/test/rclcpp/test_subscription.cpp new file mode 100644 index 0000000000..06f6f9785b --- /dev/null +++ b/rclcpp/test/rclcpp/test_subscription.cpp @@ -0,0 +1,670 @@ +// 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 "rclcpp/exceptions.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "../mocking_utils/patch.hpp" +#include "../utils/rclcpp_gtest_macros.hpp" + +#include "test_msgs/msg/empty.hpp" + +using namespace std::chrono_literals; + +class TestSubscription : public ::testing::Test +{ +public: + void on_message(test_msgs::msg::Empty::ConstSharedPtr msg) + { + (void)msg; + } + +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void initialize(const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions()) + { + node_ = std::make_shared("test_subscription", "/ns", node_options); + } + + rclcpp::Node::SharedPtr node_; +}; + +/* + Testing subscription construction and destruction. + */ +TEST_F(TestSubscription, construction_and_destruction) { + initialize(); + using test_msgs::msg::Empty; + auto callback = [](Empty::ConstSharedPtr msg) { + (void)msg; + }; + { + constexpr size_t depth = 10u; + auto sub = node_->create_subscription("topic", depth, callback); + + EXPECT_NE(nullptr, sub->get_subscription_handle()); + // Converting to base class was necessary for the compiler to choose the const version of + // get_subscription_handle() + const rclcpp::SubscriptionBase * const_sub = sub.get(); + EXPECT_NE(nullptr, const_sub->get_subscription_handle()); + EXPECT_TRUE(sub->use_take_shared_method()); + + EXPECT_NE(nullptr, sub->get_message_type_support_handle().typesupport_identifier); + EXPECT_NE(nullptr, sub->get_message_type_support_handle().data); + EXPECT_EQ(depth, sub->get_actual_qos().get_rmw_qos_profile().depth); + } + + { + ASSERT_THROW( + { + auto sub = node_->create_subscription("invalid_topic?", 10, callback); + }, rclcpp::exceptions::InvalidTopicNameError); + } +} + +/* + Testing subscription creation signatures. + */ +TEST_F(TestSubscription, various_creation_signatures) { + initialize(); + using test_msgs::msg::Empty; + auto cb = [](test_msgs::msg::Empty::ConstSharedPtr) {}; + { + auto sub = node_->create_subscription("topic", 1, cb); + (void)sub; + } + { + auto sub = node_->create_subscription("topic", rclcpp::QoS(1), cb); + (void)sub; + } + { + auto sub = + node_->create_subscription("topic", rclcpp::QoS(rclcpp::KeepLast(1)), cb); + (void)sub; + } + { + auto sub = + node_->create_subscription("topic", rclcpp::QoS(rclcpp::KeepAll()), cb); + (void)sub; + } + { + auto sub = node_->create_subscription( + "topic", 42, cb, rclcpp::SubscriptionOptions()); + (void)sub; + } + { + auto sub = rclcpp::create_subscription( + node_, "topic", 42, cb, rclcpp::SubscriptionOptions()); + (void)sub; + } + { + rclcpp::SubscriptionOptionsWithAllocator> options; + options.allocator = std::make_shared>(); + EXPECT_NE(nullptr, options.get_allocator()); + auto sub = rclcpp::create_subscription( + node_, "topic", 42, cb, options); + (void)sub; + } + { + rclcpp::SubscriptionOptionsBase options_base; + rclcpp::SubscriptionOptionsWithAllocator> options(options_base); + auto sub = rclcpp::create_subscription( + node_, "topic", 42, cb, options); + (void)sub; + } +} + +class SubscriptionClass final +{ +public: + void custom_create_subscription() + { + auto node = std::make_shared("test_subscription_member_callback", "/ns"); + auto callback = std::bind(&SubscriptionClass::on_message, this, std::placeholders::_1); + auto sub = node->create_subscription("topic", 10, callback); + } + +private: + void on_message(test_msgs::msg::Empty::ConstSharedPtr msg) + { + (void)msg; + } +}; + +class SubscriptionClassNodeInheritance final : public rclcpp::Node +{ +public: + SubscriptionClassNodeInheritance() + : Node("subscription_class_node_inheritance") + { + } + + void custom_create_subscription() + { + auto callback = std::bind( + &SubscriptionClassNodeInheritance::on_message, this, std::placeholders::_1); + auto sub = this->create_subscription("topic", 10, callback); + } + +private: + void on_message(test_msgs::msg::Empty::ConstSharedPtr msg) + { + (void)msg; + } +}; + +/* + Testing subscriptions using std::bind. + */ +TEST_F(TestSubscription, callback_bind) { + initialize(); + { + // Member callback for plain class + SubscriptionClass subscription_object; + subscription_object.custom_create_subscription(); + } + { + // Member callback for class inheriting from rclcpp::Node + SubscriptionClassNodeInheritance subscription_object; + subscription_object.custom_create_subscription(); + } + { + // Member callback for class inheriting from testing::Test + // Regression test for https://github.com/ros2/rclcpp/issues/479 where the TEST_F GTest macro + // was interfering with rclcpp's `function_traits`. + auto callback = std::bind(&TestSubscription::on_message, this, std::placeholders::_1); + auto sub = node_->create_subscription("topic", 1, callback); + } +} + +/* + Testing take. + */ +TEST_F(TestSubscription, take) { + initialize(); + auto do_nothing = [](std::shared_ptr) {FAIL();}; + { + auto sub = node_->create_subscription("~/test_take", 1, do_nothing); + test_msgs::msg::Empty msg; + rclcpp::MessageInfo msg_info; + EXPECT_FALSE(sub->take(msg, msg_info)); + } + { + rclcpp::SubscriptionOptions so; + so.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; + auto sub = node_->create_subscription("~/test_take", 1, do_nothing, so); + rclcpp::PublisherOptions po; + po.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; + auto pub = node_->create_publisher("~/test_take", 1, po); + { + test_msgs::msg::Empty msg; + pub->publish(msg); + } + test_msgs::msg::Empty msg; + rclcpp::MessageInfo msg_info; + bool message_received = false; + auto start = std::chrono::steady_clock::now(); + do { + message_received = sub->take(msg, msg_info); + std::this_thread::sleep_for(100ms); + } while (!message_received && std::chrono::steady_clock::now() - start < 10s); + EXPECT_TRUE(message_received); + } + // TODO(wjwwood): figure out a good way to test the intra-process exclusion behavior. +} + +/* + Testing take_serialized. + */ +TEST_F(TestSubscription, take_serialized) { + initialize(); + auto do_nothing = [](std::shared_ptr) {FAIL();}; + { + auto sub = node_->create_subscription("~/test_take", 1, do_nothing); + std::shared_ptr msg = sub->create_serialized_message(); + rclcpp::MessageInfo msg_info; + EXPECT_FALSE(sub->take_serialized(*msg, msg_info)); + } + { + rclcpp::SubscriptionOptions so; + so.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; + auto sub = node_->create_subscription("~/test_take", 1, do_nothing, so); + rclcpp::PublisherOptions po; + po.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; + auto pub = node_->create_publisher("~/test_take", 1, po); + { + test_msgs::msg::Empty msg; + pub->publish(msg); + } + std::shared_ptr msg = sub->create_serialized_message(); + rclcpp::MessageInfo msg_info; + bool message_received = false; + auto start = std::chrono::steady_clock::now(); + do { + message_received = sub->take_serialized(*msg, msg_info); + std::this_thread::sleep_for(100ms); + } while (!message_received && std::chrono::steady_clock::now() - start < 10s); + EXPECT_TRUE(message_received); + } +} + +TEST_F(TestSubscription, rcl_subscription_init_error) { + initialize(); + auto callback = [](std::shared_ptr) {}; + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_subscription_init, RCL_RET_TOPIC_NAME_INVALID); + + // reset() is not needed for triggering exception, just to avoid an unused return value warning + EXPECT_THROW( + node_->create_subscription("topic", 10, callback).reset(), + rclcpp::exceptions::RCLError); +} + +TEST_F(TestSubscription, rcl_subscription_fini_error) { + initialize(); + auto callback = [](std::shared_ptr) {}; + auto mock = mocking_utils::inject_on_return( + "lib:rclcpp", rcl_subscription_fini, RCL_RET_ERROR); + + // Cleanup just fails, no exception expected + EXPECT_NO_THROW( + node_->create_subscription("topic", 10, callback).reset()); +} + +TEST_F(TestSubscription, rcl_subscription_get_actual_qos_error) { + initialize(); + auto callback = [](std::shared_ptr) {}; + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_subscription_get_actual_qos, nullptr); + + auto sub = node_->create_subscription("topic", 10, callback); + RCLCPP_EXPECT_THROW_EQ( + sub->get_actual_qos(), std::runtime_error("failed to get qos settings: error not set")); +} + +TEST_F(TestSubscription, rcl_take_type_erased_error) { + initialize(); + auto callback = [](std::shared_ptr) {}; + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_take, RCL_RET_ERROR); + + auto sub = node_->create_subscription("topic", 10, callback); + test_msgs::msg::Empty msg; + rclcpp::MessageInfo message_info; + + EXPECT_THROW(sub->take_type_erased(&msg, message_info), rclcpp::exceptions::RCLError); +} + +TEST_F(TestSubscription, rcl_take_serialized_message_error) { + initialize(); + auto callback = [](std::shared_ptr) {}; + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_take_serialized_message, RCL_RET_ERROR); + + auto sub = node_->create_subscription("topic", 10, callback); + rclcpp::SerializedMessage msg; + rclcpp::MessageInfo message_info; + + EXPECT_THROW(sub->take_serialized(msg, message_info), rclcpp::exceptions::RCLError); +} + +TEST_F(TestSubscription, rcl_subscription_get_publisher_count_error) { + initialize(); + auto callback = [](std::shared_ptr) {}; + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_subscription_get_publisher_count, RCL_RET_ERROR); + + auto sub = node_->create_subscription("topic", 10, callback); + EXPECT_THROW(sub->get_publisher_count(), rclcpp::exceptions::RCLError); +} + +TEST_F(TestSubscription, handle_loaned_message) { + initialize(); + auto callback = [](std::shared_ptr) {}; + auto sub = node_->create_subscription("topic", 10, callback); + + test_msgs::msg::Empty msg; + rclcpp::MessageInfo message_info; + EXPECT_NO_THROW(sub->handle_loaned_message(&msg, message_info)); +} + +/* + Testing on_new_message callbacks. + */ +TEST_F(TestSubscription, on_new_message_callback) { + initialize(rclcpp::NodeOptions().use_intra_process_comms(false)); + using test_msgs::msg::Empty; + + auto do_nothing = [](std::shared_ptr) {FAIL();}; + auto sub = node_->create_subscription("~/test_take", 10, do_nothing); + + std::atomic c1 {0}; + auto increase_c1_cb = [&c1](size_t count_msgs) {c1 += count_msgs;}; + sub->set_on_new_message_callback(increase_c1_cb); + + auto pub = node_->create_publisher("~/test_take", 3); + { + test_msgs::msg::Empty msg; + pub->publish(msg); + } + + auto start = std::chrono::steady_clock::now(); + do { + std::this_thread::sleep_for(100ms); + } while (c1 == 0 && std::chrono::steady_clock::now() - start < 10s); + + EXPECT_EQ(c1.load(), 1u); + + std::atomic c2 {0}; + auto increase_c2_cb = [&c2](size_t count_msgs) {c2 += count_msgs;}; + sub->set_on_new_message_callback(increase_c2_cb); + + { + test_msgs::msg::Empty msg; + pub->publish(msg); + } + + start = std::chrono::steady_clock::now(); + do { + std::this_thread::sleep_for(100ms); + } while (c2 == 0 && std::chrono::steady_clock::now() - start < 10s); + + EXPECT_EQ(c1.load(), 1u); + EXPECT_EQ(c2.load(), 1u); + + sub->clear_on_new_message_callback(); + + { + test_msgs::msg::Empty msg; + pub->publish(msg); + pub->publish(msg); + pub->publish(msg); + } + + std::atomic c3 {0}; + auto increase_c3_cb = [&c3](size_t count_msgs) {c3 += count_msgs;}; + sub->set_on_new_message_callback(increase_c3_cb); + + start = std::chrono::steady_clock::now(); + do { + std::this_thread::sleep_for(100ms); + } while (c3 < 3 && std::chrono::steady_clock::now() - start < 10s); + + EXPECT_EQ(c1.load(), 1u); + EXPECT_EQ(c2.load(), 1u); + EXPECT_EQ(c3.load(), 3u); + + std::function invalid_cb = nullptr; + EXPECT_THROW(sub->set_on_new_message_callback(invalid_cb), std::invalid_argument); +} + +/* + Testing on_new_intra_process_message callbacks. + */ +TEST_F(TestSubscription, on_new_intra_process_message_callback) { + initialize(rclcpp::NodeOptions().use_intra_process_comms(true)); + using test_msgs::msg::Empty; + + auto do_nothing = [](std::shared_ptr) {FAIL();}; + auto sub = node_->create_subscription("~/test_take", 10, do_nothing); + + std::atomic c1 {0}; + auto increase_c1_cb = [&c1](size_t count_msgs) {c1 += count_msgs;}; + sub->set_on_new_intra_process_message_callback(increase_c1_cb); + + auto pub = node_->create_publisher("~/test_take", 1); + { + test_msgs::msg::Empty msg; + pub->publish(msg); + } + + auto start = std::chrono::steady_clock::now(); + do { + std::this_thread::sleep_for(100ms); + } while (c1 == 0 && std::chrono::steady_clock::now() - start < 10s); + + EXPECT_EQ(c1.load(), 1u); + + std::atomic c2 {0}; + auto increase_c2_cb = [&c2](size_t count_msgs) {c2 += count_msgs;}; + sub->set_on_new_intra_process_message_callback(increase_c2_cb); + + { + test_msgs::msg::Empty msg; + pub->publish(msg); + } + + start = std::chrono::steady_clock::now(); + do { + std::this_thread::sleep_for(100ms); + } while (c2 == 0 && std::chrono::steady_clock::now() - start < 10s); + + EXPECT_EQ(c1.load(), 1u); + EXPECT_EQ(c2.load(), 1u); + + sub->clear_on_new_intra_process_message_callback(); + + { + test_msgs::msg::Empty msg; + pub->publish(msg); + pub->publish(msg); + pub->publish(msg); + } + + std::atomic c3 {0}; + auto increase_c3_cb = [&c3](size_t count_msgs) {c3 += count_msgs;}; + sub->set_on_new_intra_process_message_callback(increase_c3_cb); + + start = std::chrono::steady_clock::now(); + do { + std::this_thread::sleep_for(100ms); + } while (c3 == 0 && std::chrono::steady_clock::now() - start < 10s); + + EXPECT_EQ(c1.load(), 1u); + EXPECT_EQ(c2.load(), 1u); + EXPECT_EQ(c3.load(), 3u); + + std::function invalid_cb = nullptr; + EXPECT_THROW(sub->set_on_new_intra_process_message_callback(invalid_cb), std::invalid_argument); +} + +TEST_F(TestSubscription, get_network_flow_endpoints_errors) { + initialize(); + const rclcpp::QoS subscription_qos(1); + auto subscription_callback = [](test_msgs::msg::Empty::ConstSharedPtr msg) { + (void)msg; + }; + auto subscription = node_->create_subscription( + "topic", subscription_qos, subscription_callback); + + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_subscription_get_network_flow_endpoints, RCL_RET_ERROR); + auto mock_network_flow_endpoint_array_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_network_flow_endpoint_array_fini, RCL_RET_ERROR); + EXPECT_THROW( + subscription->get_network_flow_endpoints(), + rclcpp::exceptions::RCLError); + } + { + auto mock_network_flow_endpoint_array_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_network_flow_endpoint_array_fini, RCL_RET_ERROR); + EXPECT_THROW( + subscription->get_network_flow_endpoints(), + rclcpp::exceptions::RCLError); + } + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_subscription_get_network_flow_endpoints, RCL_RET_OK); + auto mock_network_flow_endpoint_array_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_network_flow_endpoint_array_fini, RCL_RET_OK); + EXPECT_NO_THROW(subscription->get_network_flow_endpoints()); + } +} + +class TestSubscriptionSub : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node_ = std::make_shared("test_subscription", "/ns"); + subnode_ = node_->create_sub_node("sub_ns"); + } + + rclcpp::Node::SharedPtr node_; + rclcpp::Node::SharedPtr subnode_; +}; + +/* + Testing subscription construction and destruction for subnodes. + */ +TEST_F(TestSubscriptionSub, construction_and_destruction) { + auto callback = [](test_msgs::msg::Empty::ConstSharedPtr msg) { + (void)msg; + }; + { + auto sub = subnode_->create_subscription("topic", 1, callback); + EXPECT_STREQ(sub->get_topic_name(), "/ns/sub_ns/topic"); + } + + { + auto sub = subnode_->create_subscription("/topic", 1, callback); + EXPECT_STREQ(sub->get_topic_name(), "/topic"); + } + + { + auto sub = subnode_->create_subscription("~/topic", 1, callback); + std::string expected_topic_name = + std::string(node_->get_namespace()) + "/" + node_->get_name() + "/topic"; + EXPECT_STREQ(sub->get_topic_name(), expected_topic_name.c_str()); + } + + { + ASSERT_THROW( + { + auto sub = node_->create_subscription("invalid_topic?", 1, callback); + }, rclcpp::exceptions::InvalidTopicNameError); + } +} + +struct TestParameters final +{ + TestParameters(rclcpp::QoS qos, std::string description) + : qos(qos), description(description) {} + rclcpp::QoS qos; + std::string description; +}; + +std::ostream & operator<<(std::ostream & out, const TestParameters & params) +{ + out << params.description; + return out; +} + +class TestSubscriptionInvalidIntraprocessQos + : public TestSubscription, + public ::testing::WithParamInterface +{}; + +static std::vector invalid_qos_profiles() +{ + std::vector parameters; + + parameters.reserve(1); + parameters.push_back( + TestParameters( + rclcpp::QoS(rclcpp::KeepAll()), + "keep_all_qos")); + + return parameters; +} + +INSTANTIATE_TEST_SUITE_P( + TestSubscriptionThrows, TestSubscriptionInvalidIntraprocessQos, + ::testing::ValuesIn(invalid_qos_profiles()), + ::testing::PrintToStringParamName()); + +/* + Testing subscription with intraprocess enabled and invalid QoS + */ +TEST_P(TestSubscriptionInvalidIntraprocessQos, test_subscription_throws) { + initialize(rclcpp::NodeOptions().use_intra_process_comms(true)); + rclcpp::QoS qos = GetParam().qos; + { + auto callback = std::bind( + &TestSubscriptionInvalidIntraprocessQos::on_message, + this, + std::placeholders::_1); + + ASSERT_THROW( + {auto subscription = node_->create_subscription( + "topic", + qos, + callback);}, + std::invalid_argument); + } +} + +/* + Testing subscription with invalid use_intra_process_comm + */ +TEST_P(TestSubscriptionInvalidIntraprocessQos, test_subscription_throws_intraprocess) { + rclcpp::SubscriptionOptionsWithAllocator> options; + options.use_intra_process_comm = static_cast(5); + + initialize(); + rclcpp::QoS qos = GetParam().qos; + auto callback = std::bind( + &TestSubscriptionInvalidIntraprocessQos::on_message, + this, + std::placeholders::_1); + + RCLCPP_EXPECT_THROW_EQ( + {auto subscription = node_->create_subscription( + "topic", + qos, + callback, + options);}, + std::runtime_error("Unrecognized IntraProcessSetting value")); +} diff --git a/rclcpp/test/rclcpp/test_subscription_content_filter.cpp b/rclcpp/test/rclcpp/test_subscription_content_filter.cpp new file mode 100644 index 0000000000..95a1874d4e --- /dev/null +++ b/rclcpp/test/rclcpp/test_subscription_content_filter.cpp @@ -0,0 +1,347 @@ +// 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 +#include +#include +#include +#include +#include + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/wait_for_message.hpp" + +#include "../mocking_utils/patch.hpp" +#include "../utils/rclcpp_gtest_macros.hpp" + +#include "test_msgs/msg/basic_types.hpp" + +class TestContentFilterSubscription : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node = std::make_shared("test_content_filter_node", "/ns"); + context = node->get_node_options().context(); + qos.reliable().transient_local(); + + auto options = rclcpp::SubscriptionOptions(); + options.content_filter_options.filter_expression = filter_expression_init; + options.content_filter_options.expression_parameters = expression_parameters_1; + + auto callback = [](std::shared_ptr) {}; + sub = node->create_subscription( + "content_filter_topic", qos, callback, options); + } + + void TearDown() + { + sub.reset(); + node.reset(); + } + + template + bool wait_for(const Condition & condition, const Duration & timeout) + { + using clock = std::chrono::system_clock; + auto start = clock::now(); + while (!condition()) { + if ((clock::now() - start) > timeout) { + return false; + } + rclcpp::spin_some(node); + } + return true; + } + +protected: + rclcpp::Node::SharedPtr node; + rclcpp::Context::SharedPtr context; + rclcpp::QoS qos{rclcpp::KeepLast{10}}; + rclcpp::Subscription::SharedPtr sub; + + std::string filter_expression_init = "int32_value = %0"; + std::vector expression_parameters_1 = {"3"}; + std::vector expression_parameters_2 = {"4"}; +}; + +bool operator==(const test_msgs::msg::BasicTypes & m1, const test_msgs::msg::BasicTypes & m2) +{ + return m1.bool_value == m2.bool_value && + m1.byte_value == m2.byte_value && + m1.char_value == m2.char_value && + m1.float32_value == m2.float32_value && + m1.float64_value == m2.float64_value && + m1.int8_value == m2.int8_value && + m1.uint8_value == m2.uint8_value && + m1.int16_value == m2.int16_value && + m1.uint16_value == m2.uint16_value && + m1.int32_value == m2.int32_value && + m1.uint32_value == m2.uint32_value && + m1.int64_value == m2.int64_value && + m1.uint64_value == m2.uint64_value; +} + +TEST_F(TestContentFilterSubscription, is_cft_enabled) +{ + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_subscription_is_cft_enabled, false); + EXPECT_FALSE(sub->is_cft_enabled()); + } + + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_subscription_is_cft_enabled, true); + EXPECT_TRUE(sub->is_cft_enabled()); + } +} + +TEST_F(TestContentFilterSubscription, get_content_filter_error) +{ + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_subscription_get_content_filter, RCL_RET_ERROR); + + rclcpp::ContentFilterOptions options; + EXPECT_THROW( + options = sub->get_content_filter(), + rclcpp::exceptions::RCLError); +} + +TEST_F(TestContentFilterSubscription, set_content_filter_error) +{ + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_subscription_set_content_filter, RCL_RET_ERROR); + + std::string filter_expression = "int32_value = %0"; + std::string expression_parameter = "100"; + EXPECT_THROW( + sub->set_content_filter(filter_expression, {expression_parameter}), + rclcpp::exceptions::RCLError); +} + +TEST_F(TestContentFilterSubscription, get_content_filter) +{ + rclcpp::ContentFilterOptions options; + + if (sub->is_cft_enabled()) { + EXPECT_NO_THROW( + options = sub->get_content_filter()); + + EXPECT_EQ(options.filter_expression, filter_expression_init); + EXPECT_EQ(options.expression_parameters, expression_parameters_1); + } else { + EXPECT_THROW( + options = sub->get_content_filter(), + rclcpp::exceptions::RCLError); + } +} + +TEST_F(TestContentFilterSubscription, set_content_filter) +{ + if (sub->is_cft_enabled()) { + EXPECT_NO_THROW( + sub->set_content_filter(filter_expression_init, expression_parameters_2)); + } else { + EXPECT_THROW( + sub->set_content_filter(filter_expression_init, expression_parameters_2), + rclcpp::exceptions::RCLError); + } +} + +TEST_F(TestContentFilterSubscription, content_filter_get_begin) +{ + std::string rmw_implementation_str = std::string(rmw_get_implementation_identifier()); + if (rmw_implementation_str == "rmw_zenoh_cpp") { + GTEST_SKIP(); + } + + using namespace std::chrono_literals; + { + test_msgs::msg::BasicTypes msg; + rclcpp::MessageInfo msg_info; + EXPECT_FALSE(sub->take(msg, msg_info)); + } + { + rclcpp::PublisherOptions po; + auto pub = node->create_publisher("content_filter_topic", qos, po); + + auto connected = [pub, sub = this->sub]() -> bool { + return pub->get_subscription_count() && sub->get_publisher_count(); + }; + ASSERT_TRUE(wait_for(connected, 10s)); + + test_msgs::msg::BasicTypes original_message; + original_message.int32_value = 3; + pub->publish(original_message); + + test_msgs::msg::BasicTypes output_message; + bool receive = wait_for_message(output_message, sub, context, 10s); + EXPECT_TRUE(receive); + EXPECT_EQ(original_message, output_message); + + if (sub->is_cft_enabled()) { + EXPECT_NO_THROW( + sub->set_content_filter(filter_expression_init, expression_parameters_2)); + // waiting to allow for filter propagation + std::this_thread::sleep_for(std::chrono::seconds(10)); + + test_msgs::msg::BasicTypes original_message; + original_message.int32_value = 3; + pub->publish(original_message); + + test_msgs::msg::BasicTypes output_message; + bool receive = wait_for_message(output_message, sub, context, 10s); + EXPECT_FALSE(receive); + } + } +} + +TEST_F(TestContentFilterSubscription, content_filter_get_later) +{ + std::string rmw_implementation_str = std::string(rmw_get_implementation_identifier()); + if (rmw_implementation_str == "rmw_zenoh_cpp") { + GTEST_SKIP(); + } + + using namespace std::chrono_literals; + { + test_msgs::msg::BasicTypes msg; + rclcpp::MessageInfo msg_info; + EXPECT_FALSE(sub->take(msg, msg_info)); + } + { + rclcpp::PublisherOptions po; + auto pub = node->create_publisher("content_filter_topic", qos, po); + + auto connected = [pub, sub = this->sub]() -> bool { + return pub->get_subscription_count() && sub->get_publisher_count(); + }; + ASSERT_TRUE(wait_for(connected, 10s)); + + test_msgs::msg::BasicTypes original_message; + original_message.int32_value = 4; + pub->publish(original_message); + + test_msgs::msg::BasicTypes output_message; + bool receive = wait_for_message(output_message, sub, context, 10s); + if (sub->is_cft_enabled()) { + EXPECT_FALSE(receive); + } else { + EXPECT_TRUE(receive); + EXPECT_EQ(original_message, output_message); + } + + if (sub->is_cft_enabled()) { + EXPECT_NO_THROW( + sub->set_content_filter(filter_expression_init, expression_parameters_2)); + // waiting to allow for filter propagation + std::this_thread::sleep_for(std::chrono::seconds(10)); + + test_msgs::msg::BasicTypes original_message; + original_message.int32_value = 4; + pub->publish(original_message); + + test_msgs::msg::BasicTypes output_message; + bool receive = wait_for_message(output_message, sub, context, 10s); + EXPECT_TRUE(receive); + EXPECT_EQ(original_message, output_message); + } + } +} + +TEST_F(TestContentFilterSubscription, content_filter_reset) +{ + std::string rmw_implementation_str = std::string(rmw_get_implementation_identifier()); + if (rmw_implementation_str == "rmw_zenoh_cpp") { + GTEST_SKIP(); + } + + using namespace std::chrono_literals; + { + test_msgs::msg::BasicTypes msg; + rclcpp::MessageInfo msg_info; + EXPECT_FALSE(sub->take(msg, msg_info)); + } + { + rclcpp::PublisherOptions po; + auto pub = node->create_publisher("content_filter_topic", qos, po); + + auto connected = [pub, sub = this->sub]() -> bool { + return pub->get_subscription_count() && sub->get_publisher_count(); + }; + ASSERT_TRUE(wait_for(connected, 10s)); + + test_msgs::msg::BasicTypes original_message; + original_message.int32_value = 4; + pub->publish(original_message); + + test_msgs::msg::BasicTypes output_message; + bool receive = wait_for_message(output_message, sub, context, 10s); + if (sub->is_cft_enabled()) { + EXPECT_FALSE(receive); + } else { + EXPECT_TRUE(receive); + EXPECT_EQ(original_message, output_message); + } + + if (sub->is_cft_enabled()) { + EXPECT_NO_THROW( + sub->set_content_filter("")); + // waiting to allow for filter propagation + std::this_thread::sleep_for(std::chrono::seconds(10)); + + test_msgs::msg::BasicTypes original_message; + original_message.int32_value = 4; + pub->publish(original_message); + + test_msgs::msg::BasicTypes output_message; + bool receive = wait_for_message(output_message, sub, context, 10s); + EXPECT_TRUE(receive); + EXPECT_EQ(original_message, output_message); + } + } +} + +TEST_F(TestContentFilterSubscription, create_two_content_filters_with_same_topic_name_and_destroy) +{ + // Create another content filter + auto options = rclcpp::SubscriptionOptions(); + + std::string filter_expression = "int32_value > %0"; + std::vector expression_parameters = {"4"}; + + options.content_filter_options.filter_expression = filter_expression; + options.content_filter_options.expression_parameters = expression_parameters; + + auto callback = [](std::shared_ptr) {}; + auto sub_2 = node->create_subscription( + "content_filter_topic", qos, callback, options); + + EXPECT_NE(nullptr, sub_2); + sub_2.reset(); +} diff --git a/rclcpp/test/rclcpp/test_subscription_options.cpp b/rclcpp/test/rclcpp/test_subscription_options.cpp new file mode 100644 index 0000000000..1bcde41130 --- /dev/null +++ b/rclcpp/test/rclcpp/test_subscription_options.cpp @@ -0,0 +1,111 @@ +// Copyright 2020 Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include + +#include "rclcpp/node.hpp" +#include "rclcpp/node_options.hpp" +#include "rclcpp/subscription_options.hpp" + +#include "../utils/rclcpp_gtest_macros.hpp" + +using namespace std::chrono_literals; + +namespace +{ +constexpr const char defaultPublishTopic[] = "/statistics"; +} + +class TestSubscriptionOptions : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + +protected: + void initialize(const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions()) + { + node = std::make_shared("test_subscription_options", node_options); + } + + void TearDown() + { + node.reset(); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + rclcpp::Node::SharedPtr node; +}; + +TEST_F(TestSubscriptionOptions, topic_statistics_options_default_and_set) { + auto options = rclcpp::SubscriptionOptions(); + + EXPECT_EQ(options.topic_stats_options.state, rclcpp::TopicStatisticsState::NodeDefault); + EXPECT_EQ(options.topic_stats_options.publish_topic, defaultPublishTopic); + EXPECT_EQ(options.topic_stats_options.publish_period, 1s); + EXPECT_EQ(options.topic_stats_options.qos, rclcpp::SystemDefaultsQoS().keep_last(10)); + + options.topic_stats_options.state = rclcpp::TopicStatisticsState::Enable; + options.topic_stats_options.publish_topic = "topic_statistics"; + options.topic_stats_options.publish_period = 5min; + options.topic_stats_options.qos = rclcpp::BestAvailableQoS(); + + EXPECT_EQ(options.topic_stats_options.state, rclcpp::TopicStatisticsState::Enable); + EXPECT_EQ(options.topic_stats_options.publish_topic, "topic_statistics"); + EXPECT_EQ(options.topic_stats_options.publish_period, 5min); + EXPECT_EQ(options.topic_stats_options.qos, rclcpp::BestAvailableQoS()); +} + +TEST_F(TestSubscriptionOptions, topic_statistics_options_node_default_mode) { + initialize(); + auto subscription_options = rclcpp::SubscriptionOptions(); + + EXPECT_EQ( + subscription_options.topic_stats_options.state, + rclcpp::TopicStatisticsState::NodeDefault); + EXPECT_FALSE( + rclcpp::detail::resolve_enable_topic_statistics( + subscription_options, + *(node->get_node_base_interface()))); + + initialize(rclcpp::NodeOptions().enable_topic_statistics(true)); + EXPECT_TRUE( + rclcpp::detail::resolve_enable_topic_statistics( + subscription_options, + *(node->get_node_base_interface()))); + + subscription_options.topic_stats_options.state = rclcpp::TopicStatisticsState::Disable; + EXPECT_FALSE( + rclcpp::detail::resolve_enable_topic_statistics( + subscription_options, + *(node->get_node_base_interface()))); + + subscription_options.topic_stats_options.state = static_cast(5); + RCLCPP_EXPECT_THROW_EQ( + rclcpp::detail::resolve_enable_topic_statistics( + subscription_options, + *(node->get_node_base_interface())), + std::runtime_error("Unrecognized EnableTopicStatistics value")); +} diff --git a/rclcpp/test/rclcpp/test_subscription_publisher_count_api.cpp b/rclcpp/test/rclcpp/test_subscription_publisher_count_api.cpp new file mode 100644 index 0000000000..92297e0e04 --- /dev/null +++ b/rclcpp/test/rclcpp/test_subscription_publisher_count_api.cpp @@ -0,0 +1,166 @@ +// Copyright 2019-2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "test_msgs/msg/empty.hpp" + +namespace +{ + +template +class NodeCreationPolicy +{ +public: + rclcpp::NodeOptions & node_options() + { + return options_; + } + +private: + rclcpp::NodeOptions options_; +}; + +template +class NodeCreationPolicy +{ +public: + NodeCreationPolicy() + { + gather(options_); + } + + rclcpp::NodeOptions & node_options() + { + return options_; + } + +private: + template + static rclcpp::NodeOptions & + gather(rclcpp::NodeOptions & options) + { + return U::gather(options); + } + + template + static rclcpp::NodeOptions & + gather(rclcpp::NodeOptions & options) + { + return gather(U::gather(options)); + } + + rclcpp::NodeOptions options_; +}; + +struct UseCustomContext +{ + static rclcpp::NodeOptions & gather(rclcpp::NodeOptions & options) + { + auto context = rclcpp::Context::make_shared(); + context->init(0, nullptr); + return options.context(context); + } +}; + +struct PrintTestDescription +{ + template + static std::string GetName(int i) + { + static_cast(i); + return T::description; + } +}; + +} // namespace + + +template +class TestSubscriptionPublisherCount : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + +protected: + static void OnMessage(test_msgs::msg::Empty::ConstSharedPtr msg) + { + (void)msg; + } + + std::chrono::milliseconds offset{2000}; +}; + +struct OneContextPerTest +{ + static constexpr const char * description = "one_context_test"; + using NodeCreationPolicy = ::NodeCreationPolicy<>; +}; + +struct TwoContextsPerTest +{ + static constexpr const char * description = "two_contexts_test"; + using NodeCreationPolicy = ::NodeCreationPolicy; +}; + +using AllTestDescriptions = ::testing::Types; +TYPED_TEST_SUITE(TestSubscriptionPublisherCount, AllTestDescriptions, PrintTestDescription); + + +using test_msgs::msg::Empty; + +TYPED_TEST(TestSubscriptionPublisherCount, increasing_and_decreasing_counts) +{ + using TestDescription = TypeParam; + rclcpp::Node::SharedPtr node = std::make_shared("my_node", "/ns"); + auto subscription = node->create_subscription( + "/topic", 10, &TestSubscriptionPublisherCount::OnMessage); + + EXPECT_EQ(subscription->get_publisher_count(), 0u); + { + auto pub = node->create_publisher("/topic", 10); + rclcpp::sleep_for(this->offset); + EXPECT_EQ(subscription->get_publisher_count(), 1u); + { + typename TestDescription::NodeCreationPolicy node_creation_policy; + rclcpp::Node::SharedPtr another_node = std::make_shared( + "another_node", + "/ns", + node_creation_policy.node_options()); + auto another_pub = + another_node->create_publisher("/topic", 10); + + rclcpp::sleep_for(this->offset); + EXPECT_EQ(subscription->get_publisher_count(), 2u); + } + rclcpp::sleep_for(this->offset); + EXPECT_EQ(subscription->get_publisher_count(), 1u); + } + rclcpp::sleep_for(this->offset); + EXPECT_EQ(subscription->get_publisher_count(), 0u); +} diff --git a/rclcpp/test/rclcpp/test_subscription_publisher_with_same_type_adapter.cpp b/rclcpp/test/rclcpp/test_subscription_publisher_with_same_type_adapter.cpp new file mode 100644 index 0000000000..5cee82c3ef --- /dev/null +++ b/rclcpp/test/rclcpp/test_subscription_publisher_with_same_type_adapter.cpp @@ -0,0 +1,991 @@ +// 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 +#include +#include + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "rclcpp/msg/string.hpp" +#include "statistics_msgs/msg/statistic_data_point.hpp" + +static const int g_max_loops = 200; +static const std::chrono::milliseconds g_sleep_per_loop(10); + +class test_intra_process_within_one_node : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } +}; + +namespace rclcpp +{ + +template<> +struct TypeAdapter +{ + using is_specialized = std::true_type; + using custom_type = std::string; + using ros_message_type = rclcpp::msg::String; + + static void + convert_to_ros_message( + const custom_type & source, + ros_message_type & destination) + { + (void) source; + (void) destination; + throw std::runtime_error("This should not happen"); + } + + static void + convert_to_custom( + const ros_message_type & source, + custom_type & destination) + { + (void) source; + (void) destination; + throw std::runtime_error("This should not happen"); + } +}; + +template<> +struct TypeAdapter +{ + using is_specialized = std::true_type; + using custom_type = double; + using ros_message_type = statistics_msgs::msg::StatisticDataPoint; + + static void + convert_to_ros_message( + const custom_type & source, + ros_message_type & destination) + { + destination.data_type = 0; + destination.data = source; + } + + static void + convert_to_custom( + const ros_message_type & source, + custom_type & destination) + { + destination = source.data; + } +}; + +} // namespace rclcpp + +void wait_for_message_to_be_received( + bool & is_received, + const std::shared_ptr & node) +{ + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + executor.spin_once(std::chrono::milliseconds(0)); + int i = 0; + while (!is_received && i < g_max_loops) { + printf("spin_node_once() - callback (1) expected - try %d/%d\n", ++i, g_max_loops); + executor.spin_once(g_sleep_per_loop); + } +} + +/* + * Testing that subscriber receives type adapted types and ROS message types with intra proccess communications. + */ +TEST_F( + test_intra_process_within_one_node, + type_adapted_messages_unique_pub_received_by_intra_process_subscription) +{ + using StringTypeAdapter = rclcpp::TypeAdapter; + const std::string message_data = "Message Data"; + const std::string topic_name = "topic_name"; + + auto node = rclcpp::Node::make_shared( + "test_intra_process", + rclcpp::NodeOptions().use_intra_process_comms(true)); + + auto pub = node->create_publisher(topic_name, 1); + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::string &, publish with unique_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + const std::string & msg) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), msg.c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_message = std::make_unique(message_data); + pub->publish(std::move(pu_message)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::string & with message info, publish with unique_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + const std::string & msg, const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), msg.c_str()); + EXPECT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_message = std::make_unique(message_data); + pub->publish(std::move(pu_message)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::unique_ptr, publish with unique_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::unique_ptr msg) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_message = std::make_unique(message_data); + pub->publish(std::move(pu_message)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::unique_ptr with message info, publish with unique_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::unique_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + EXPECT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_message = std::make_unique(message_data); + pub->publish(std::move(pu_message)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr, publish with unique_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_message = std::make_unique(message_data); + pub->publish(std::move(pu_message)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr with message info, publish with unique_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + EXPECT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_message = std::make_unique(message_data); + pub->publish(std::move(pu_message)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr, publish with unique_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_message = std::make_unique(message_data); + pub->publish(std::move(pu_message)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr with message info, publish with unique_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + EXPECT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_message = std::make_unique(message_data); + pub->publish(std::move(pu_message)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::shared_ptr &, publish with unique_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + const std::shared_ptr & msg) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_message = std::make_unique(message_data); + pub->publish(std::move(pu_message)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::shared_ptr & with message info, + // publish with unique_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + const std::shared_ptr & msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + EXPECT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_message = std::make_unique(message_data); + pub->publish(std::move(pu_message)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } +} + +TEST_F( + test_intra_process_within_one_node, + type_adapted_messages_const_ref_pub_received_by_intra_process_subscription) +{ + using StringTypeAdapter = rclcpp::TypeAdapter; + const std::string message_data = "Message Data"; + const std::string topic_name = "topic_name"; + + auto node = rclcpp::Node::make_shared( + "test_intra_process", + rclcpp::NodeOptions().use_intra_process_comms(true)); + + auto pub = node->create_publisher(topic_name, 1); + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::string &, publish with const std::string & + bool is_received = false; + auto callback = + [message_data, &is_received]( + const std::string & msg) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), msg.c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + std::string pu_message(message_data); + pub->publish(pu_message); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::string & with message info, publish with const std::string & + bool is_received = false; + auto callback = + [message_data, &is_received]( + const std::string & msg, const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), msg.c_str()); + EXPECT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + std::string pu_message(message_data); + pub->publish(pu_message); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::unique_ptr, publish with const std::string & + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::unique_ptr msg) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + std::string pu_message(message_data); + pub->publish(pu_message); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::unique_ptr with message info, publish with const std::string & + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::unique_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + EXPECT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + std::string pu_message(message_data); + pub->publish(pu_message); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr, publish with const std::string & + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + std::string pu_message(message_data); + pub->publish(pu_message); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr with message info, publish with const std::string & + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + EXPECT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + std::string pu_message(message_data); + pub->publish(pu_message); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr, publish with const std::string & + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + std::string pu_message(message_data); + pub->publish(pu_message); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr with message info, + // publish with const std::string & + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + EXPECT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + std::string pu_message(message_data); + pub->publish(pu_message); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::shared_ptr &, publish with const std::string & + bool is_received = false; + auto callback = + [message_data, &is_received]( + const std::shared_ptr & msg) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + std::string pu_message(message_data); + pub->publish(pu_message); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::shared_ptr & with message info, + // publish with const std::string & + bool is_received = false; + auto callback = + [message_data, &is_received]( + const std::shared_ptr & msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + EXPECT_STREQ(message_data.c_str(), (*msg).c_str()); + EXPECT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + std::string pu_message(message_data); + pub->publish(pu_message); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } +} + +TEST_F( + test_intra_process_within_one_node, + type_adapted_messages_ros_message_ref_pub_received_by_intra_process_subscription) +{ + using DoubleTypeAdapter = rclcpp::TypeAdapter; + const double message_data = 0.894; + const std::string topic_name = "topic_name"; + + auto node = rclcpp::Node::make_shared( + "test_intra_process", + rclcpp::NodeOptions().use_intra_process_comms(true)); + + auto pub = node->create_publisher(topic_name, 1); + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::string &, publish with rclcpp::msg::String & + bool is_received = false; + auto callback = + [message_data, &is_received]( + const double & msg) -> void { + is_received = true; + ASSERT_EQ(message_data, msg); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + statistics_msgs::msg::StatisticDataPoint msg; + msg.data = message_data; + pub->publish(msg); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::string & with message info, publish with rclcpp::msg::String & + bool is_received = false; + auto callback = + [message_data, &is_received]( + const double & msg, const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_EQ(message_data, msg); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + statistics_msgs::msg::StatisticDataPoint msg; + msg.data = message_data; + pub->publish(msg); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::unique_ptr, publish with rclcpp::msg::String & + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::unique_ptr msg) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + statistics_msgs::msg::StatisticDataPoint msg; + msg.data = message_data; + pub->publish(msg); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::unique_ptr with message info, publish with rclcpp::msg::String & + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::unique_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + statistics_msgs::msg::StatisticDataPoint msg; + msg.data = message_data; + pub->publish(msg); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr, publish with rclcpp::msg::String & + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + statistics_msgs::msg::StatisticDataPoint msg; + msg.data = message_data; + pub->publish(msg); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr with message info, publish with rclcpp::msg::String & + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + statistics_msgs::msg::StatisticDataPoint msg; + msg.data = message_data; + pub->publish(msg); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr, publish with rclcpp::msg::String & + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + statistics_msgs::msg::StatisticDataPoint msg; + msg.data = message_data; + pub->publish(msg); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr with message info, + // publish with rclcpp::msg::String & + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + statistics_msgs::msg::StatisticDataPoint msg; + msg.data = message_data; + pub->publish(msg); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::shared_ptr &, publish with rclcpp::msg::String & + bool is_received = false; + auto callback = + [message_data, &is_received]( + const std::shared_ptr & msg) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + statistics_msgs::msg::StatisticDataPoint msg; + msg.data = message_data; + pub->publish(msg); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::shared_ptr & with message info, + // publish with rclcpp::msg::String & + bool is_received = false; + auto callback = + [message_data, &is_received]( + const std::shared_ptr & msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + statistics_msgs::msg::StatisticDataPoint msg; + msg.data = message_data; + pub->publish(msg); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } +} + +TEST_F( + test_intra_process_within_one_node, + type_adapted_messages_unique_ptr_ros_message_pub_received_by_intra_process_subscription) +{ + using DoubleTypeAdapter = rclcpp::TypeAdapter; + const double message_data = 0.7508; + const std::string topic_name = "topic_name"; + + auto node = rclcpp::Node::make_shared( + "test_intra_process", + rclcpp::NodeOptions().use_intra_process_comms(true)); + + auto pub = node->create_publisher(topic_name, 1); + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::string &, publish with unique statistics_msgs::msg::StatisticDataPoint + bool is_received = false; + auto callback = + [message_data, &is_received]( + const double & msg) -> void { + is_received = true; + ASSERT_EQ(message_data, msg); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_msg = std::make_unique(); + pu_msg->data = message_data; + pub->publish(std::move(pu_msg)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::string & with message info, publish with unique + // statistics_msgs::msg::StatisticDataPoint + bool is_received = false; + auto callback = + [message_data, &is_received]( + const double & msg, const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_EQ(message_data, msg); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_msg = std::make_unique(); + pu_msg->data = message_data; + pub->publish(std::move(pu_msg)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::unique_ptr, publish with unique + // statistics_msgs::msg::StatisticDataPoint + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::unique_ptr msg) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_msg = std::make_unique(); + pu_msg->data = message_data; + pub->publish(std::move(pu_msg)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::unique_ptr with message info, publish with unique + // statistics_msgs::msg::StatisticDataPoint + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::unique_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_msg = std::make_unique(); + pu_msg->data = message_data; + pub->publish(std::move(pu_msg)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr, publish with unique + // statistics_msgs::msg::StatisticDataPoint + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_msg = std::make_unique(); + pu_msg->data = message_data; + pub->publish(std::move(pu_msg)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr with message info, publish with unique + // statistics_msgs::msg::StatisticDataPoint + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_msg = std::make_unique(); + pu_msg->data = message_data; + pub->publish(std::move(pu_msg)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr, publish with unique + // statistics_msgs::msg::StatisticDataPoint + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_msg = std::make_unique(); + pu_msg->data = message_data; + pub->publish(std::move(pu_msg)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback std::shared_ptr with message info, publish with unique + // statistics_msgs::msg::StatisticDataPoint + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_msg = std::make_unique(); + pu_msg->data = message_data; + pub->publish(std::move(pu_msg)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::shared_ptr &, publish with unique + // statistics_msgs::msg::StatisticDataPoint + bool is_received = false; + auto callback = + [message_data, &is_received]( + const std::shared_ptr & msg) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_msg = std::make_unique(); + pu_msg->data = message_data; + pub->publish(std::move(pu_msg)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // create_publisher with TypeAdapt struct, create_subscription with TypeAdapt struct, + // callback const std::shared_ptr & with message info, publish with unique + // statistics_msgs::msg::StatisticDataPoint + bool is_received = false; + auto callback = + [message_data, &is_received]( + const std::shared_ptr & msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_EQ(message_data, *msg); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + + auto pu_msg = std::make_unique(); + pu_msg->data = message_data; + pub->publish(std::move(pu_msg)); + + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } +} diff --git a/rclcpp/test/rclcpp/test_subscription_traits.cpp b/rclcpp/test/rclcpp/test_subscription_traits.cpp new file mode 100644 index 0000000000..c7b062aefc --- /dev/null +++ b/rclcpp/test/rclcpp/test_subscription_traits.cpp @@ -0,0 +1,199 @@ +// 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/types.h" + +#include "rclcpp/allocator/allocator_common.hpp" +#include "rclcpp/serialized_message.hpp" +#include "rclcpp/subscription_traits.hpp" + +#include "test_msgs/msg/empty.hpp" + +void serialized_callback_copy(rcl_serialized_message_t unused) +{ + (void) unused; +} + +void serialized_callback_shared_ptr(std::shared_ptr unused) +{ + (void) unused; +} + +void not_serialized_callback(char * unused) +{ + (void) unused; +} + +void not_serialized_shared_ptr_callback(std::shared_ptr unused) +{ + (void) unused; +} + +void not_serialized_unique_ptr_callback( + test_msgs::msg::Empty::UniquePtrWithDeleter, + test_msgs::msg::Empty>> unused) +{ + (void) unused; +} + +void rclcpp_serialized_callback_copy(rclcpp::SerializedMessage unused) +{ + (void) unused; +} + +void rclcpp_serialized_callback_shared_ptr(std::shared_ptr unused) +{ + (void) unused; +} + +TEST(TestSubscriptionTraits, is_serialized_callback) { + // Test regular functions + auto cb1 = &serialized_callback_copy; + static_assert( + rclcpp::subscription_traits::is_serialized_callback::value == false, + "passing a rcl_serialized_message_t * is not a serialized callback"); + + auto cb2 = &serialized_callback_shared_ptr; + static_assert( + rclcpp::subscription_traits::is_serialized_callback::value == false, + "passing a std::shared_ptr is not a serialized callback"); + + auto cb3 = ¬_serialized_callback; + static_assert( + rclcpp::subscription_traits::is_serialized_callback::value == false, + "passing a char * is not a serialized callback"); + + auto cb4 = ¬_serialized_shared_ptr_callback; + static_assert( + rclcpp::subscription_traits::is_serialized_callback::value == false, + "passing a std::shared_tr is not a serialized callback"); + + auto cb5 = [](rcl_serialized_message_t unused) -> void + { + (void) unused; + }; + static_assert( + rclcpp::subscription_traits::is_serialized_callback::value == false, + "passing rcl_serialized_message_t is not a serialized callback"); + + using MessageT = test_msgs::msg::Empty; + using MessageTAllocator = std::allocator; + using MessageTDeallocator = rclcpp::allocator::Deleter; + auto cb6 = [](MessageT::UniquePtrWithDeleter unique_msg_ptr) -> void + { + (void) unique_msg_ptr; + }; + static_assert( + rclcpp::subscription_traits::is_serialized_callback::value == false, + "passing a std::unique_ptr of test_msgs::msg::Empty is not a serialized callback"); + + auto cb7 = ¬_serialized_unique_ptr_callback; + static_assert( + rclcpp::subscription_traits::is_serialized_callback::value == false, + "passing a fancy unique_ptr of test_msgs::msg::Empty is not a serialized callback"); + + auto cb8 = &rclcpp_serialized_callback_copy; + static_assert( + rclcpp::subscription_traits::is_serialized_callback::value == true, + "rclcpp::SerializedMessage in a first argument callback makes it a serialized callback"); + + auto cb9 = &rclcpp_serialized_callback_shared_ptr; + static_assert( + rclcpp::subscription_traits::is_serialized_callback::value == true, + "std::shared_ptr in a callback makes it a serialized callback"); +} + +TEST(TestSubscriptionTraits, callback_messages) { + static_assert( + std::is_same< + std::shared_ptr, + rclcpp::function_traits::function_traits< + decltype(not_serialized_shared_ptr_callback) + >::template argument_type<0> + >::value, "wrong!"); + + static_assert( + std::is_same< + char, + rclcpp::subscription_traits::extract_message_type< + rclcpp::function_traits::function_traits< + decltype(not_serialized_shared_ptr_callback) + >::template argument_type<0> + >::type + >::value, "wrong!"); + + auto cb1 = &serialized_callback_copy; + static_assert( + std::is_same< + rcl_serialized_message_t, + rclcpp::subscription_traits::has_message_type::type>::value, + "serialized callback message type is rcl_serialized_message_t"); + + auto cb2 = &serialized_callback_shared_ptr; + static_assert( + std::is_same< + rcl_serialized_message_t, + rclcpp::subscription_traits::has_message_type::type>::value, + "serialized callback message type is rcl_serialized_message_t"); + + auto cb3 = ¬_serialized_callback; + static_assert( + std::is_same< + char *, + rclcpp::subscription_traits::has_message_type::type>::value, + "not serialized callback message type is char"); + + auto cb4 = ¬_serialized_shared_ptr_callback; + static_assert( + std::is_same< + char, + rclcpp::subscription_traits::has_message_type::type>::value, + "not serialized shared_ptr callback message type is std::shared_ptr"); + + auto cb5 = [](rcl_serialized_message_t unused) -> void + { + (void) unused; + }; + static_assert( + std::is_same< + rcl_serialized_message_t, + rclcpp::subscription_traits::has_message_type::type>::value, + "serialized callback message type is rcl_serialized_message_t"); + + using MessageT = test_msgs::msg::Empty; + using MessageTAllocator = std::allocator; + using MessageTDeallocator = rclcpp::allocator::Deleter; + auto cb6 = [](std::unique_ptr unique_msg_ptr) -> void + { + (void) unique_msg_ptr; + }; + static_assert( + std::is_same< + test_msgs::msg::Empty, + rclcpp::subscription_traits::has_message_type::type>::value, + "passing a std::unique_ptr of test_msgs::msg::Empty has message type Empty"); + + auto cb7 = ¬_serialized_unique_ptr_callback; + static_assert( + std::is_same< + test_msgs::msg::Empty, + rclcpp::subscription_traits::has_message_type::type>::value, + "passing a fancy std::unique_ptr of test_msgs::msg::Empty has message type Empty"); +} diff --git a/rclcpp/test/rclcpp/test_subscription_with_type_adapter.cpp b/rclcpp/test/rclcpp/test_subscription_with_type_adapter.cpp new file mode 100644 index 0000000000..8e74592f9e --- /dev/null +++ b/rclcpp/test/rclcpp/test_subscription_with_type_adapter.cpp @@ -0,0 +1,563 @@ +// 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 + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "rclcpp/msg/string.hpp" + + +using namespace std::chrono_literals; + +static const int g_max_loops = 200; +static const std::chrono::milliseconds g_sleep_per_loop(10); + + +class TestSubscription : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } +}; + +namespace rclcpp +{ + +template<> +struct TypeAdapter +{ + using is_specialized = std::true_type; + using custom_type = std::string; + using ros_message_type = rclcpp::msg::String; + + static void + convert_to_ros_message( + const custom_type & source, + ros_message_type & destination) + { + destination.data = source; + } + + static void + convert_to_custom( + const ros_message_type & source, + custom_type & destination) + { + destination = source.data; + } +}; + +} // namespace rclcpp + +void wait_for_message_to_be_received( + bool & is_received, + const std::shared_ptr & node) +{ + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + executor.spin_once(std::chrono::milliseconds(0)); + int i = 0; + while (!is_received && i < g_max_loops) { + printf("spin_node_once() - callback (1) expected - try %d/%d\n", ++i, g_max_loops); + executor.spin_once(g_sleep_per_loop); + } +} + +bool wait_for_match( + const std::shared_ptr sub, + const std::shared_ptr pub) +{ + int i = 0; + bool matched = false; + while (!matched && i < g_max_loops) { + matched = sub->get_publisher_count() > 0 && pub->get_subscription_count() > 0; + std::this_thread::sleep_for(g_sleep_per_loop); + } + return matched; +} + +/* + * Testing publisher creation signatures with a type adapter. + */ +TEST_F(TestSubscription, various_creation_signatures) { + auto node = std::make_shared("my_node", "/ns", rclcpp::NodeOptions()); + { + using StringTypeAdapter = rclcpp::TypeAdapter; + auto sub = + node->create_subscription("topic", 42, [](const std::string &) {}); + (void)sub; + } + { + using StringTypeAdapter = rclcpp::adapt_type::as; + auto sub = + node->create_subscription("topic", 42, [](const std::string &) {}); + (void)sub; + } +} + +/* + * Testing that subscriber receives type adapted types and ROS message types with intra proccess communications. + */ +TEST_F( + TestSubscription, + check_type_adapted_messages_are_received_by_intra_process_subscription) { + using StringTypeAdapter = rclcpp::TypeAdapter; + const std::string message_data = "Message Data"; + const std::string topic_name = "topic_name"; + + auto node = rclcpp::Node::make_shared( + "test_intra_process", + rclcpp::NodeOptions().use_intra_process_comms(true)); + auto pub = node->create_publisher(topic_name, 1); + + rclcpp::msg::String msg; + msg.data = message_data; + + { + { // const std::string & + bool is_received = false; + auto callback = + [message_data, &is_received]( + const std::string & msg) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), msg.c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + ASSERT_TRUE(wait_for_match(sub, pub)); + pub->publish(msg); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // const std::string & with message info + bool is_received = false; + auto callback = + [message_data, &is_received]( + const std::string & msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), msg.c_str()); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + ASSERT_TRUE(wait_for_match(sub, pub)); + pub->publish(msg); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // std::shared_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + ASSERT_TRUE(wait_for_match(sub, pub)); + pub->publish(msg); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // std::shared_ptr with message info + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + ASSERT_TRUE(wait_for_match(sub, pub)); + pub->publish(msg); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // std::unique_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::unique_ptr msg) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + ASSERT_TRUE(wait_for_match(sub, pub)); + pub->publish(msg); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // std::unique_ptr with message info + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::unique_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + ASSERT_TRUE(wait_for_match(sub, pub)); + pub->publish(msg); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // const rclcpp::msg::String & + bool is_received = false; + auto callback = + [message_data, &is_received]( + const rclcpp::msg::String & msg) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), msg.data.c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + ASSERT_TRUE(wait_for_match(sub, pub)); + pub->publish(msg); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // const rclcpp::msg::String & with message info + bool is_received = false; + auto callback = + [message_data, &is_received]( + const rclcpp::msg::String & msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), msg.data.c_str()); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + ASSERT_TRUE(wait_for_match(sub, pub)); + pub->publish(msg); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // std::shared_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), msg->data.c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + ASSERT_TRUE(wait_for_match(sub, pub)); + pub->publish(msg); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // std::shared_ptr with message info + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), msg->data.c_str()); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + ASSERT_TRUE(wait_for_match(sub, pub)); + pub->publish(msg); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // std::unique_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::unique_ptr msg) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), msg->data.c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + ASSERT_TRUE(wait_for_match(sub, pub)); + pub->publish(msg); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // std::unique_ptr with message info + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::unique_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), msg->data.c_str()); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + ASSERT_TRUE(wait_for_match(sub, pub)); + pub->publish(msg); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + } +} + +/* + * Testing that subscriber receives type adapted types and ROS message types with inter proccess communications. + */ +TEST_F( + TestSubscription, + check_type_adapted_messages_are_received_by_inter_process_subscription) { + using StringTypeAdapter = rclcpp::TypeAdapter; + const std::string message_data = "Message Data"; + const std::string topic_name = "topic_name"; + + auto node = rclcpp::Node::make_shared( + "test_intra_process", + rclcpp::NodeOptions().use_intra_process_comms(false)); + auto pub = node->create_publisher(topic_name, 1); + + rclcpp::msg::String msg; + msg.data = message_data; + + { + { // const std::string & + bool is_received = false; + auto callback = + [message_data, &is_received](const std::string & msg) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), msg.c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + ASSERT_TRUE(wait_for_match(sub, pub)); + pub->publish(msg); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // const std::string & with message info + bool is_received = false; + auto callback = + [message_data, &is_received]( + const std::string & msg, const rclcpp::MessageInfo & message_info) -> void + { + is_received = true; + ASSERT_STREQ(message_data.c_str(), msg.c_str()); + ASSERT_FALSE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + ASSERT_TRUE(wait_for_match(sub, pub)); + pub->publish(msg); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // std::shared_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + ASSERT_TRUE(wait_for_match(sub, pub)); + pub->publish(msg); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // std::shared_ptr with message info + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); + ASSERT_FALSE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + ASSERT_TRUE(wait_for_match(sub, pub)); + pub->publish(msg); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // std::unique_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::unique_ptr msg) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + ASSERT_TRUE(wait_for_match(sub, pub)); + pub->publish(msg); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // std::unique_ptr with message info + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::unique_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); + ASSERT_FALSE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + ASSERT_TRUE(wait_for_match(sub, pub)); + pub->publish(msg); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // const rclcpp::msg::String & + bool is_received = false; + auto callback = + [message_data, &is_received]( + const rclcpp::msg::String & msg) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), msg.data.c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + ASSERT_TRUE(wait_for_match(sub, pub)); + pub->publish(msg); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // const rclcpp::msg::String & with message info + bool is_received = false; + auto callback = + [message_data, &is_received]( + const rclcpp::msg::String & msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), msg.data.c_str()); + ASSERT_FALSE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + ASSERT_TRUE(wait_for_match(sub, pub)); + pub->publish(msg); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // std::shared_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), msg->data.c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + ASSERT_TRUE(wait_for_match(sub, pub)); + pub->publish(msg); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // std::shared_ptr with message info + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), msg->data.c_str()); + ASSERT_FALSE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + ASSERT_TRUE(wait_for_match(sub, pub)); + pub->publish(msg); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // std::unique_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::unique_ptr msg) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), msg->data.c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + ASSERT_TRUE(wait_for_match(sub, pub)); + pub->publish(msg); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // std::unique_ptr with message info + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::unique_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), msg->data.c_str()); + ASSERT_FALSE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + ASSERT_TRUE(wait_for_match(sub, pub)); + pub->publish(msg); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + } +} diff --git a/rclcpp/test/rclcpp/test_time.cpp b/rclcpp/test/rclcpp/test_time.cpp new file mode 100644 index 0000000000..8406de4efb --- /dev/null +++ b/rclcpp/test/rclcpp/test_time.cpp @@ -0,0 +1,867 @@ +// 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 "rcl/error_handling.h" +#include "rcl/time.h" +#include "rclcpp/clock.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/time_source.hpp" +#include "rclcpp/utilities.hpp" +#include "rcutils/time.h" + +#include "../utils/rclcpp_gtest_macros.hpp" + +namespace +{ + +using namespace std::chrono_literals; + +bool logical_eq(const bool a, const bool b) +{ + return (a && b) || ((!a) && !(b)); +} + +} // namespace + + +class TestTime : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } +}; + +TEST_F(TestTime, clock_type_access) { + rclcpp::Clock ros_clock(RCL_ROS_TIME); + EXPECT_EQ(RCL_ROS_TIME, ros_clock.get_clock_type()); + + rclcpp::Clock system_clock(RCL_SYSTEM_TIME); + EXPECT_EQ(RCL_SYSTEM_TIME, system_clock.get_clock_type()); + + rclcpp::Clock steady_clock(RCL_STEADY_TIME); + EXPECT_EQ(RCL_STEADY_TIME, steady_clock.get_clock_type()); +} + +// Check that the clock may go out of the scope before the jump callback without leading in UB. +TEST_F(TestTime, clock_jump_callback_destruction_order) { + rclcpp::JumpHandler::SharedPtr handler; + { + rclcpp::Clock ros_clock(RCL_ROS_TIME); + rcl_jump_threshold_t threshold; + threshold.min_backward.nanoseconds = -1; + threshold.min_forward.nanoseconds = 1; + handler = ros_clock.create_jump_callback([]() {}, [](const rcl_time_jump_t &) {}, threshold); + } +} + +TEST_F(TestTime, time_sources) { + using builtin_interfaces::msg::Time; + rclcpp::Clock ros_clock(RCL_ROS_TIME); + Time ros_now = ros_clock.now(); + EXPECT_NE(0, ros_now.sec); + EXPECT_NE(0u, ros_now.nanosec); + + rclcpp::Clock system_clock(RCL_SYSTEM_TIME); + Time system_now = system_clock.now(); + EXPECT_NE(0, system_now.sec); + EXPECT_NE(0u, system_now.nanosec); + + rclcpp::Clock steady_clock(RCL_STEADY_TIME); + Time steady_now = steady_clock.now(); + EXPECT_NE(0, steady_now.sec); + EXPECT_NE(0u, steady_now.nanosec); +} + +static const int64_t HALF_SEC_IN_NS = RCUTILS_MS_TO_NS(500); +static const int64_t ONE_SEC_IN_NS = RCUTILS_MS_TO_NS(1000); +static const int64_t ONE_AND_HALF_SEC_IN_NS = 3 * HALF_SEC_IN_NS; + +TEST_F(TestTime, conversions) { + rclcpp::Clock system_clock(RCL_SYSTEM_TIME); + + { + rclcpp::Time now = system_clock.now(); + builtin_interfaces::msg::Time now_msg = now; + + rclcpp::Time now_again = now_msg; + EXPECT_EQ(now.nanoseconds(), now_again.nanoseconds()); + } + + { + rclcpp::Time positive_time = rclcpp::Time(12345, 67890u); + + builtin_interfaces::msg::Time msg = positive_time; + EXPECT_EQ(msg.sec, 12345); + EXPECT_EQ(msg.nanosec, 67890u); + + rclcpp::Time time = msg; + EXPECT_EQ(time.nanoseconds(), positive_time.nanoseconds()); + EXPECT_EQ( + RCL_S_TO_NS(static_cast(msg.sec)) + static_cast(msg.nanosec), + time.nanoseconds()); + EXPECT_EQ(static_cast(msg.sec), RCL_NS_TO_S(time.nanoseconds())); + } + + // throw on construction/assignment of negative times + { + builtin_interfaces::msg::Time negative_time_msg; + negative_time_msg.sec = -1; + negative_time_msg.nanosec = 1; + + EXPECT_ANY_THROW( + { + rclcpp::Time negative_time = negative_time_msg; + }); + + EXPECT_ANY_THROW(rclcpp::Time(-1, 1)); + + EXPECT_ANY_THROW(rclcpp::Time(-1)); + + EXPECT_ANY_THROW( + { + rclcpp::Time assignment(1, 2); + assignment = negative_time_msg; + }); + } + + { + const rclcpp::Time time(HALF_SEC_IN_NS); + const auto time_msg = static_cast(time); + EXPECT_EQ(time_msg.sec, 0); + EXPECT_EQ(time_msg.nanosec, HALF_SEC_IN_NS); + EXPECT_EQ(rclcpp::Time(time_msg).nanoseconds(), HALF_SEC_IN_NS); + } + + { + const rclcpp::Time time(ONE_SEC_IN_NS); + const auto time_msg = static_cast(time); + EXPECT_EQ(time_msg.sec, 1); + EXPECT_EQ(time_msg.nanosec, 0u); + EXPECT_EQ(rclcpp::Time(time_msg).nanoseconds(), ONE_SEC_IN_NS); + } + + { + const rclcpp::Time time(ONE_AND_HALF_SEC_IN_NS); + auto time_msg = static_cast(time); + EXPECT_EQ(time_msg.sec, 1); + EXPECT_EQ(time_msg.nanosec, HALF_SEC_IN_NS); + EXPECT_EQ(rclcpp::Time(time_msg).nanoseconds(), ONE_AND_HALF_SEC_IN_NS); + } +} + +TEST_F(TestTime, operators) { + rclcpp::Time old(1, 0); + rclcpp::Time young(2, 0); + + EXPECT_TRUE(old < young); + EXPECT_TRUE(young > old); + EXPECT_TRUE(old <= young); + EXPECT_TRUE(young >= old); + EXPECT_FALSE(young == old); + EXPECT_TRUE(young != old); + + rclcpp::Duration sub = young - old; + EXPECT_EQ(sub.nanoseconds(), (young.nanoseconds() - old.nanoseconds())); + EXPECT_EQ(sub, young - old); + + rclcpp::Time young_changed(young); + young_changed -= rclcpp::Duration::from_nanoseconds(old.nanoseconds()); + EXPECT_EQ(sub.nanoseconds(), young_changed.nanoseconds()); + + rclcpp::Time system_time(0, 0, RCL_SYSTEM_TIME); + rclcpp::Time steady_time(0, 0, RCL_STEADY_TIME); + + EXPECT_ANY_THROW((void)(system_time == steady_time)); + EXPECT_ANY_THROW((void)(system_time != steady_time)); + EXPECT_ANY_THROW((void)(system_time <= steady_time)); + EXPECT_ANY_THROW((void)(system_time >= steady_time)); + EXPECT_ANY_THROW((void)(system_time < steady_time)); + EXPECT_ANY_THROW((void)(system_time > steady_time)); + EXPECT_ANY_THROW((void)(system_time - steady_time)); + + rclcpp::Clock system_clock(RCL_SYSTEM_TIME); + rclcpp::Clock steady_clock(RCL_STEADY_TIME); + + rclcpp::Time now = system_clock.now(); + rclcpp::Time later = steady_clock.now(); + + EXPECT_ANY_THROW((void)(now == later)); + EXPECT_ANY_THROW((void)(now != later)); + EXPECT_ANY_THROW((void)(now <= later)); + EXPECT_ANY_THROW((void)(now >= later)); + EXPECT_ANY_THROW((void)(now < later)); + EXPECT_ANY_THROW((void)(now > later)); + EXPECT_ANY_THROW((void)(now - later)); + + for (auto time_source : {RCL_ROS_TIME, RCL_SYSTEM_TIME, RCL_STEADY_TIME}) { + rclcpp::Time time = rclcpp::Time(0, 0, time_source); + rclcpp::Time copy_constructor_time(time); + rclcpp::Time assignment_op_time = rclcpp::Time(1, 0, time_source); + assignment_op_time = time; + + EXPECT_TRUE(time == copy_constructor_time); + EXPECT_TRUE(time == assignment_op_time); + } +} + +TEST_F(TestTime, overflow_detectors) { + ///////////////////////////////////////////////////////////////////////////// + // Test logical_eq call first: + EXPECT_TRUE(logical_eq(false, false)); + EXPECT_FALSE(logical_eq(false, true)); + EXPECT_FALSE(logical_eq(true, false)); + EXPECT_TRUE(logical_eq(true, true)); + + ///////////////////////////////////////////////////////////////////////////// + // Exhaustive test of all int8_t values + using test_type_t = int8_t; + // big_type_t encompasses test_type_t: + // big_type_t::min < test_type_t::min + // big_type_t::max > test_type_t::max + using big_type_t = int16_t; + const big_type_t min_val = std::numeric_limits::min(); + const big_type_t max_val = std::numeric_limits::max(); + // 256 * 256 = 64K total loops, should be pretty fast on everything + for (big_type_t y = min_val; y <= max_val; ++y) { + for (big_type_t x = min_val; x <= max_val; ++x) { + const big_type_t sum = static_cast(x + y); + const big_type_t diff = static_cast(x - y); + + const bool add_will_overflow = + rclcpp::add_will_overflow(test_type_t(x), test_type_t(y)); + const bool add_did_overflow = sum > max_val; + EXPECT_TRUE(logical_eq(add_will_overflow, add_did_overflow)); + + const bool add_will_underflow = + rclcpp::add_will_underflow(test_type_t(x), test_type_t(y)); + const bool add_did_underflow = sum < min_val; + EXPECT_TRUE(logical_eq(add_will_underflow, add_did_underflow)); + + const bool sub_will_overflow = + rclcpp::sub_will_overflow(test_type_t(x), test_type_t(y)); + const bool sub_did_overflow = diff > max_val; + EXPECT_TRUE(logical_eq(sub_will_overflow, sub_did_overflow)); + + const bool sub_will_underflow = + rclcpp::sub_will_underflow(test_type_t(x), test_type_t(y)); + const bool sub_did_underflow = diff < min_val; + EXPECT_TRUE(logical_eq(sub_will_underflow, sub_did_underflow)); + } + } + + // Few selected tests for int64_t + EXPECT_TRUE(rclcpp::add_will_overflow(INT64_MAX, 1)); + EXPECT_FALSE(rclcpp::add_will_overflow(INT64_MAX, -1)); + EXPECT_TRUE(rclcpp::add_will_underflow(INT64_MIN, -1)); + EXPECT_FALSE(rclcpp::add_will_underflow(INT64_MIN, 1)); + + EXPECT_FALSE(rclcpp::sub_will_overflow(INT64_MAX, 1)); + EXPECT_TRUE(rclcpp::sub_will_overflow(INT64_MAX, -1)); + EXPECT_FALSE(rclcpp::sub_will_underflow(INT64_MIN, -1)); + EXPECT_TRUE(rclcpp::sub_will_underflow(INT64_MIN, 1)); +} + +TEST_F(TestTime, overflows) { + rclcpp::Time max_time(std::numeric_limits::max()); + rclcpp::Duration one(1ns); + rclcpp::Duration two(2ns); + + // Cross max + EXPECT_THROW(max_time + one, std::overflow_error); + EXPECT_THROW(rclcpp::Time(max_time) += one, std::overflow_error); + EXPECT_NO_THROW(max_time - max_time); + + // Cross zero + rclcpp::Time one_time(1); + EXPECT_THROW(one_time - two, std::runtime_error); + EXPECT_THROW(rclcpp::Time(one_time) -= two, std::runtime_error); + + rclcpp::Time two_time(2); + EXPECT_NO_THROW(one_time - two_time); +} + +TEST_F(TestTime, seconds) { + EXPECT_DOUBLE_EQ(0.0, rclcpp::Time(0, 0).seconds()); + EXPECT_DOUBLE_EQ(4.5, rclcpp::Time(4, 500000000).seconds()); + EXPECT_DOUBLE_EQ(2.5, rclcpp::Time(0, 2500000000).seconds()); +} + +TEST_F(TestTime, test_max) { + // Same clock types + for (rcl_clock_type_t type = RCL_ROS_TIME; + type != RCL_STEADY_TIME; type = static_cast(type + 1)) + { + const rclcpp::Time time_max = rclcpp::Time::max(type); + const rclcpp::Time max_time(std::numeric_limits::max(), 999999999, type); + EXPECT_DOUBLE_EQ(max_time.seconds(), time_max.seconds()); + EXPECT_EQ(max_time.nanoseconds(), time_max.nanoseconds()); + } + // Different clock types + { + const rclcpp::Time time_max = rclcpp::Time::max(RCL_ROS_TIME); + const rclcpp::Time max_time(std::numeric_limits::max(), 999999999, RCL_STEADY_TIME); + EXPECT_ANY_THROW((void)(time_max == max_time)); + EXPECT_ANY_THROW((void)(time_max != max_time)); + EXPECT_ANY_THROW((void)(time_max <= max_time)); + EXPECT_ANY_THROW((void)(time_max >= max_time)); + EXPECT_ANY_THROW((void)(time_max < max_time)); + EXPECT_ANY_THROW((void)(time_max > max_time)); + EXPECT_ANY_THROW((void)(time_max - max_time)); + } +} + +TEST_F(TestTime, test_constructor_from_rcl_time_point) { + const rcl_time_point_value_t test_nano_seconds = 555; + const rcl_clock_type_t test_clock_type = RCL_ROS_TIME; + rcl_time_point_t test_time_point; + test_time_point.nanoseconds = test_nano_seconds; + test_time_point.clock_type = test_clock_type; + + const rclcpp::Time time_max = rclcpp::Time(test_time_point); + + EXPECT_EQ(test_nano_seconds, time_max.nanoseconds()); + EXPECT_EQ(test_nano_seconds, test_time_point.nanoseconds); + EXPECT_EQ(test_clock_type, time_max.get_clock_type()); + EXPECT_EQ(test_clock_type, test_time_point.clock_type); +} + +TEST_F(TestTime, test_assignment_operator_from_builtin_msg_time) { + rclcpp::Clock ros_clock(RCL_ROS_TIME); + const builtin_interfaces::msg::Time ros_now = ros_clock.now(); + EXPECT_NE(0, ros_now.sec); + EXPECT_NE(0u, ros_now.nanosec); + + rclcpp::Time test_time(0u, RCL_CLOCK_UNINITIALIZED); + EXPECT_EQ(0u, test_time.nanoseconds()); + EXPECT_EQ(RCL_CLOCK_UNINITIALIZED, test_time.get_clock_type()); + + test_time = ros_now; + EXPECT_NE(0, test_time.nanoseconds()); + // The clock type is hardcoded internally + EXPECT_EQ(RCL_ROS_TIME, test_time.get_clock_type()); +} + +TEST_F(TestTime, test_sum_operator) { + const rclcpp::Duration one(1ns); + const rclcpp::Time test_time(0u); + EXPECT_EQ(0u, test_time.nanoseconds()); + + const rclcpp::Time new_time = one + test_time; + EXPECT_EQ(1, new_time.nanoseconds()); +} + +TEST_F(TestTime, test_overflow_underflow_throws) { + rclcpp::Time test_time(0u); + + RCLCPP_EXPECT_THROW_EQ( + test_time = rclcpp::Time(INT64_MAX) + rclcpp::Duration(1ns), + std::overflow_error("addition leads to int64_t overflow")); + + RCLCPP_EXPECT_THROW_EQ( + test_time = rclcpp::Time(INT64_MAX) - rclcpp::Duration(-1ns), + std::overflow_error("time subtraction leads to int64_t overflow")); + + test_time = rclcpp::Time(INT64_MAX); + RCLCPP_EXPECT_THROW_EQ( + test_time += rclcpp::Duration(1ns), + std::overflow_error("addition leads to int64_t overflow")); + + test_time = rclcpp::Time(INT64_MAX); + RCLCPP_EXPECT_THROW_EQ( + test_time -= rclcpp::Duration(-1ns), + std::overflow_error("time subtraction leads to int64_t overflow")); + + RCLCPP_EXPECT_THROW_EQ( + test_time = rclcpp::Duration::from_nanoseconds(INT64_MAX) + rclcpp::Time(1), + std::overflow_error("addition leads to int64_t overflow")); +} + +class TestClockSleep : public ::testing::Test +{ +protected: + void SetUp() + { + // Shutdown in case there was a dangling global context from other test fixtures + rclcpp::shutdown(); + rclcpp::init(0, nullptr); + node = std::make_shared("clock_sleep_node"); + param_client = std::make_shared(node); + ASSERT_TRUE(param_client->wait_for_service(5s)); + } + + void TearDown() + { + node.reset(); + rclcpp::shutdown(); + } + + rclcpp::Node::SharedPtr node; + rclcpp::SyncParametersClient::SharedPtr param_client; +}; + +TEST_F(TestClockSleep, bad_clock_type) { + rclcpp::Clock clock(RCL_SYSTEM_TIME); + rclcpp::Time steady_until(12345, 0, RCL_STEADY_TIME); + RCLCPP_EXPECT_THROW_EQ( + clock.sleep_until(steady_until), + std::runtime_error("until's clock type does not match this clock's type")); + + rclcpp::Time ros_until(54321, 0, RCL_ROS_TIME); + RCLCPP_EXPECT_THROW_EQ( + clock.sleep_until(ros_until), + std::runtime_error("until's clock type does not match this clock's type")); +} + +TEST_F(TestClockSleep, sleep_until_invalid_context) { + rclcpp::Clock clock(RCL_SYSTEM_TIME); + auto until = clock.now(); + + RCLCPP_EXPECT_THROW_EQ( + clock.sleep_until(until, nullptr), + std::runtime_error("context cannot be slept with because it's invalid")); + + auto uninitialized_context = std::make_shared(); + RCLCPP_EXPECT_THROW_EQ( + clock.sleep_until(until, uninitialized_context), + std::runtime_error("context cannot be slept with because it's invalid")); + + auto shutdown_context = std::make_shared(); + shutdown_context->init(0, nullptr); + shutdown_context->shutdown("i am a teapot"); + RCLCPP_EXPECT_THROW_EQ( + clock.sleep_until(until, shutdown_context), + std::runtime_error("context cannot be slept with because it's invalid")); +} + +TEST_F(TestClockSleep, sleep_until_non_global_context) { + rclcpp::Clock clock(RCL_SYSTEM_TIME); + auto until = clock.now() + rclcpp::Duration(0, 1); + + auto non_global_context = std::make_shared(); + non_global_context->init(0, nullptr); + ASSERT_TRUE(clock.sleep_until(until, non_global_context)); +} + +TEST_F(TestClockSleep, sleep_until_basic_system) { + const auto milliseconds = 300; + rclcpp::Clock clock(RCL_SYSTEM_TIME); + auto delay = rclcpp::Duration(0, RCUTILS_MS_TO_NS(milliseconds)); + auto sleep_until = clock.now() + delay; + + auto start = std::chrono::system_clock::now(); + ASSERT_TRUE(clock.sleep_until(sleep_until)); + auto end = std::chrono::system_clock::now(); + + EXPECT_GE(clock.now(), sleep_until); + EXPECT_GE(end - start, std::chrono::milliseconds(milliseconds)); +} + +TEST_F(TestClockSleep, sleep_until_basic_steady) { + const auto milliseconds = 300; + rclcpp::Clock clock(RCL_STEADY_TIME); + auto delay = rclcpp::Duration(0, RCUTILS_MS_TO_NS(milliseconds)); + auto sleep_until = clock.now() + delay; + + auto steady_start = std::chrono::steady_clock::now(); + ASSERT_TRUE(clock.sleep_until(sleep_until)); + auto steady_end = std::chrono::steady_clock::now(); + + EXPECT_GE(clock.now(), sleep_until); + EXPECT_GE(steady_end - steady_start, std::chrono::milliseconds(milliseconds)); +} + +TEST_F(TestClockSleep, sleep_until_steady_past_returns_immediately) { + rclcpp::Clock clock(RCL_STEADY_TIME); + auto until = clock.now() - rclcpp::Duration(1000, 0); + // This should return immediately, other possible behavior might be sleep forever and timeout + ASSERT_TRUE(clock.sleep_until(until)); +} + +TEST_F(TestClockSleep, sleep_until_system_past_returns_immediately) { + rclcpp::Clock clock(RCL_SYSTEM_TIME); + auto until = clock.now() - rclcpp::Duration(1000, 0); + // This should return immediately, other possible behavior might be sleep forever and timeout + ASSERT_TRUE(clock.sleep_until(until)); +} + +TEST_F(TestClockSleep, sleep_until_ros_time_enable_interrupt) { + auto clock = std::make_shared(RCL_ROS_TIME); + rclcpp::TimeSource time_source; + time_source.attachNode(node); + time_source.attachClock(clock); + + // 5 second timeout, but it should be interrupted right away + const auto until = clock->now() + rclcpp::Duration(5, 0); + + // Try sleeping with ROS time off, then turn it on to interrupt + bool sleep_succeeded = true; + auto sleep_thread = std::thread( + [clock, until, &sleep_succeeded]() { + sleep_succeeded = clock->sleep_until(until); + }); + // yield execution long enough to let the sleep thread get to waiting on the condition variable + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + auto set_parameters_results = param_client->set_parameters( + {rclcpp::Parameter("use_sim_time", true)}); + for (auto & result : set_parameters_results) { + ASSERT_TRUE(result.successful); + } + sleep_thread.join(); + EXPECT_FALSE(sleep_succeeded); +} + +TEST_F(TestClockSleep, sleep_until_ros_time_disable_interrupt) { + param_client->set_parameters({rclcpp::Parameter("use_sim_time", true)}); + auto clock = std::make_shared(RCL_ROS_TIME); + rclcpp::TimeSource time_source; + time_source.attachNode(node); + time_source.attachClock(clock); + + // /clock shouldn't be publishing, shouldn't be possible to reach timeout + const auto until = clock->now() + rclcpp::Duration(600, 0); + + // Try sleeping with ROS time off, then turn it on to interrupt + bool sleep_succeeded = true; + auto sleep_thread = std::thread( + [clock, until, &sleep_succeeded]() { + sleep_succeeded = clock->sleep_until(until); + }); + // yield execution long enough to let the sleep thread get to waiting on the condition variable + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + auto set_parameters_results = param_client->set_parameters( + {rclcpp::Parameter("use_sim_time", false)}); + for (auto & result : set_parameters_results) { + ASSERT_TRUE(result.successful); + } + sleep_thread.join(); + EXPECT_FALSE(sleep_succeeded); +} + +TEST_F(TestClockSleep, sleep_until_shutdown_interrupt) { + param_client->set_parameters({rclcpp::Parameter("use_sim_time", true)}); + auto clock = std::make_shared(RCL_ROS_TIME); + rclcpp::TimeSource time_source; + time_source.attachNode(node); + time_source.attachClock(clock); + + // the timeout doesn't matter here - no /clock is being published, so it should never wake + const auto until = clock->now() + rclcpp::Duration(600, 0); + + bool sleep_succeeded = true; + auto sleep_thread = std::thread( + [clock, until, &sleep_succeeded]() { + sleep_succeeded = clock->sleep_until(until); + }); + // yield execution long enough to let the sleep thread get to waiting on the condition variable + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + rclcpp::shutdown(); + sleep_thread.join(); + EXPECT_FALSE(sleep_succeeded); +} + +TEST_F(TestClockSleep, sleep_until_basic_ros) { + rclcpp::Clock clock(RCL_ROS_TIME); + rcl_clock_t * rcl_clock = clock.get_clock_handle(); + + ASSERT_EQ(RCL_ROS_TIME, clock.get_clock_type()); + + // Not zero, because 0 means time not initialized + const rcl_time_point_value_t start_time = 1337; + const rcl_time_point_value_t end_time = start_time + 1; + + // Initialize time + ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(rcl_clock)); + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(rcl_clock, start_time)); + + const auto until = rclcpp::Time(end_time, RCL_ROS_TIME); + + bool sleep_succeeded = false; + auto sleep_thread = std::thread( + [&clock, until, &sleep_succeeded]() { + sleep_succeeded = clock.sleep_until(until); + }); + + // yield execution long enough to let the sleep thread get to waiting on the condition variable + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + + // False because still sleeping + EXPECT_FALSE(sleep_succeeded); + + // Jump time to the end + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(rcl_clock, end_time)); + ASSERT_EQ(until, clock.now()); + + sleep_thread.join(); + EXPECT_TRUE(sleep_succeeded); +} + +TEST_F(TestClockSleep, sleep_for_invalid_context) { + rclcpp::Clock clock(RCL_SYSTEM_TIME); + auto rel_time = rclcpp::Duration(1, 0u); + + RCLCPP_EXPECT_THROW_EQ( + clock.sleep_for(rel_time, nullptr), + std::runtime_error("context cannot be slept with because it's invalid")); + + auto uninitialized_context = std::make_shared(); + RCLCPP_EXPECT_THROW_EQ( + clock.sleep_for(rel_time, uninitialized_context), + std::runtime_error("context cannot be slept with because it's invalid")); + + auto shutdown_context = std::make_shared(); + shutdown_context->init(0, nullptr); + shutdown_context->shutdown("i am a teapot"); + RCLCPP_EXPECT_THROW_EQ( + clock.sleep_for(rel_time, shutdown_context), + std::runtime_error("context cannot be slept with because it's invalid")); +} + +TEST_F(TestClockSleep, sleep_for_non_global_context) { + rclcpp::Clock clock(RCL_SYSTEM_TIME); + auto rel_time = rclcpp::Duration(0, 1); + + auto non_global_context = std::make_shared(); + non_global_context->init(0, nullptr); + ASSERT_TRUE(clock.sleep_for(rel_time, non_global_context)); +} + +TEST_F(TestClockSleep, sleep_for_basic_system) { + const auto milliseconds = 300; + rclcpp::Clock clock(RCL_SYSTEM_TIME); + auto rel_time = rclcpp::Duration(0, RCUTILS_MS_TO_NS(milliseconds)); + + auto start = std::chrono::system_clock::now(); + ASSERT_TRUE(clock.sleep_for(rel_time)); + auto end = std::chrono::system_clock::now(); + + EXPECT_GE(end - start, std::chrono::milliseconds(milliseconds)); +} + +TEST_F(TestClockSleep, sleep_for_basic_steady) { + const auto milliseconds = 300; + rclcpp::Clock clock(RCL_STEADY_TIME); + auto rel_time = rclcpp::Duration(0, RCUTILS_MS_TO_NS(milliseconds)); + + auto steady_start = std::chrono::steady_clock::now(); + ASSERT_TRUE(clock.sleep_for(rel_time)); + auto steady_end = std::chrono::steady_clock::now(); + + EXPECT_GE(steady_end - steady_start, std::chrono::milliseconds(milliseconds)); +} + +TEST_F(TestClockSleep, sleep_for_steady_past_returns_immediately) { + rclcpp::Clock clock(RCL_STEADY_TIME); + auto rel_time = rclcpp::Duration(-1000, 0); + // This should return immediately + ASSERT_TRUE(clock.sleep_for(rel_time)); +} + +TEST_F(TestClockSleep, sleep_for_system_past_returns_immediately) { + rclcpp::Clock clock(RCL_SYSTEM_TIME); + auto rel_time = rclcpp::Duration(-1000, 0); + // This should return immediately + ASSERT_TRUE(clock.sleep_for(rel_time)); +} + +TEST_F(TestClockSleep, sleep_for_ros_time_enable_interrupt) { + auto clock = std::make_shared(RCL_ROS_TIME); + rclcpp::TimeSource time_source; + time_source.attachNode(node); + time_source.attachClock(clock); + + // 5 second timeout, but it should be interrupted right away + const auto rel_time = rclcpp::Duration(5, 0); + + // Try sleeping with ROS time off, then turn it on to interrupt + bool sleep_succeeded = true; + auto sleep_thread = std::thread( + [clock, rel_time, &sleep_succeeded]() { + sleep_succeeded = clock->sleep_for(rel_time); + }); + // yield execution long enough to let the sleep thread get to waiting on the condition variable + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + auto set_parameters_results = param_client->set_parameters( + {rclcpp::Parameter("use_sim_time", true)}); + for (auto & result : set_parameters_results) { + ASSERT_TRUE(result.successful); + } + sleep_thread.join(); + EXPECT_FALSE(sleep_succeeded); +} + +TEST_F(TestClockSleep, sleep_for_ros_time_disable_interrupt) { + param_client->set_parameters({rclcpp::Parameter("use_sim_time", true)}); + auto clock = std::make_shared(RCL_ROS_TIME); + rclcpp::TimeSource time_source; + time_source.attachNode(node); + time_source.attachClock(clock); + + // /clock shouldn't be publishing, shouldn't be possible to reach timeout + const auto rel_time = rclcpp::Duration(600, 0); + + // Try sleeping with ROS time off, then turn it on to interrupt + bool sleep_succeeded = true; + auto sleep_thread = std::thread( + [clock, rel_time, &sleep_succeeded]() { + sleep_succeeded = clock->sleep_for(rel_time); + }); + // yield execution long enough to let the sleep thread get to waiting on the condition variable + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + auto set_parameters_results = param_client->set_parameters( + {rclcpp::Parameter("use_sim_time", false)}); + for (auto & result : set_parameters_results) { + ASSERT_TRUE(result.successful); + } + sleep_thread.join(); + EXPECT_FALSE(sleep_succeeded); +} + +TEST_F(TestClockSleep, sleep_for_shutdown_interrupt) { + param_client->set_parameters({rclcpp::Parameter("use_sim_time", true)}); + auto clock = std::make_shared(RCL_ROS_TIME); + rclcpp::TimeSource time_source; + time_source.attachNode(node); + time_source.attachClock(clock); + + // the timeout doesn't matter here - no /clock is being published, so it should never wake + const auto rel_time = rclcpp::Duration(600, 0); + + bool sleep_succeeded = true; + auto sleep_thread = std::thread( + [clock, rel_time, &sleep_succeeded]() { + sleep_succeeded = clock->sleep_for(rel_time); + }); + // yield execution long enough to let the sleep thread get to waiting on the condition variable + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + rclcpp::shutdown(); + sleep_thread.join(); + EXPECT_FALSE(sleep_succeeded); +} + +TEST_F(TestClockSleep, sleep_for_basic_ros) { + rclcpp::Clock clock(RCL_ROS_TIME); + rcl_clock_t * rcl_clock = clock.get_clock_handle(); + + ASSERT_EQ(RCL_ROS_TIME, clock.get_clock_type()); + + // Not zero, because 0 means time not initialized + const rcl_time_point_value_t start_time = 1337; + const rcl_time_point_value_t end_time = start_time + 1; + + // Initialize time + ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(rcl_clock)); + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(rcl_clock, start_time)); + + const auto rel_time = rclcpp::Duration(0, 1u); + + bool sleep_succeeded = false; + auto sleep_thread = std::thread( + [&clock, rel_time, &sleep_succeeded]() { + sleep_succeeded = clock.sleep_for(rel_time); + }); + + // yield execution long enough to let the sleep thread get to waiting on the condition variable + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + + // False because still sleeping + EXPECT_FALSE(sleep_succeeded); + + // Jump time to the end + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(rcl_clock, end_time)); + ASSERT_EQ(end_time, clock.now().nanoseconds()); + + sleep_thread.join(); + EXPECT_TRUE(sleep_succeeded); +} + +class TestClockStarted : public ::testing::Test +{ +protected: + void SetUp() + { + rclcpp::init(0, nullptr); + } + + void TearDown() + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestClockStarted, started) { + // rclcpp::Clock ros_clock(RCL_ROS_TIME); + // auto ros_clock_handle = ros_clock.get_clock_handle(); + // + // // At this point, the ROS clock is reading system time since the ROS time override isn't on + // // So we expect it to be started (it's extremely unlikely that system time is at epoch start) + // EXPECT_TRUE(ros_clock.started()); + // EXPECT_TRUE(ros_clock.wait_until_started()); + // EXPECT_TRUE(ros_clock.wait_until_started(rclcpp::Duration(0, static_cast(1e7)))); + // EXPECT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(ros_clock_handle)); + // EXPECT_TRUE(ros_clock.ros_time_is_active()); + // EXPECT_FALSE(ros_clock.started()); + // EXPECT_EQ(RCL_RET_OK, rcl_set_ros_time_override(ros_clock_handle, 1)); + // EXPECT_TRUE(ros_clock.started()); + // + // rclcpp::Clock system_clock(RCL_SYSTEM_TIME); + // EXPECT_TRUE(system_clock.started()); + // EXPECT_TRUE(system_clock.wait_until_started()); + // EXPECT_TRUE(system_clock.wait_until_started(rclcpp::Duration(0, static_cast(1e7)))); + // + // rclcpp::Clock steady_clock(RCL_STEADY_TIME); + // EXPECT_TRUE(steady_clock.started()); + // EXPECT_TRUE(steady_clock.wait_until_started()); + // EXPECT_TRUE(steady_clock.wait_until_started(rclcpp::Duration(0, static_cast(1e7)))); + // + // rclcpp::Clock uninit_clock(RCL_CLOCK_UNINITIALIZED); + // RCLCPP_EXPECT_THROW_EQ( + // uninit_clock.started(), std::runtime_error("clock is not rcl_clock_valid")); + // RCLCPP_EXPECT_THROW_EQ( + // uninit_clock.wait_until_started(rclcpp::Duration(0, static_cast(1e7))), + // std::runtime_error("clock cannot be waited on as it is not rcl_clock_valid")); +} + +TEST_F(TestClockStarted, started_timeout) { + rclcpp::Clock ros_clock(RCL_ROS_TIME); + auto ros_clock_handle = ros_clock.get_clock_handle(); + + EXPECT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(ros_clock_handle)); + EXPECT_TRUE(ros_clock.ros_time_is_active()); + + EXPECT_EQ(RCL_RET_OK, rcl_set_ros_time_override(ros_clock_handle, 0)); + + EXPECT_FALSE(ros_clock.started()); + EXPECT_FALSE(ros_clock.wait_until_started(rclcpp::Duration(0, static_cast(1e7)))); + + std::thread t([]() { + std::this_thread::sleep_for(std::chrono::seconds(1)); + rclcpp::shutdown(); + }); + + // Test rclcpp shutdown escape hatch (otherwise this waits indefinitely) + EXPECT_FALSE(ros_clock.wait_until_started()); + t.join(); +} diff --git a/rclcpp/test/rclcpp/test_time_source.cpp b/rclcpp/test/rclcpp/test_time_source.cpp new file mode 100644 index 0000000000..1437e47126 --- /dev/null +++ b/rclcpp/test/rclcpp/test_time_source.cpp @@ -0,0 +1,824 @@ +// 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 "rcl/error_handling.h" +#include "rcl/time.h" +#include "rclcpp/clock.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/time_source.hpp" + +#include "../utils/rclcpp_gtest_macros.hpp" + +using namespace std::chrono_literals; + +class TestTimeSource : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node = std::make_shared("my_node"); + } + + void TearDown() + { + node.reset(); + } + + rclcpp::Node::SharedPtr node; +}; + +void spin_until_time( + rclcpp::Clock::SharedPtr clock, + rclcpp::Node::SharedPtr node, + std::chrono::nanoseconds end_time, + bool expect_time_update) +{ + // Call spin_once on the node until either: + // 1) We see the ros_clock's simulated time change to the expected end_time + // -or- + // 2) 1 second has elapsed in the real world + // If 'expect_time_update' is True, and we timed out waiting for simulated time to + // update, we'll have the test fail + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + + auto start = std::chrono::system_clock::now(); + while (std::chrono::system_clock::now() < (start + 1s)) { + if (!rclcpp::ok()) { + break; // Break for ctrl-c + } + + executor.spin_once(10ms); + + if (clock->now().nanoseconds() == end_time.count()) { + return; + } + } + + if (expect_time_update) { + // If we were expecting ROS clock->now to be updated and we didn't take the early return from + // the loop up above, that's a failure + ASSERT_TRUE(false) << "Timed out waiting for ROS time to update"; + } +} + +void spin_until_ros_time_updated( + rclcpp::Clock::SharedPtr clock, + rclcpp::Node::SharedPtr node, + rclcpp::ParameterValue value) +{ + // Similar to above: Call spin_once until we see the clock's ros_time_is_active method + // match the ParameterValue + // Unlike spin_until_time, there aren't any test cases where we don't expect the value to + // update. In the event that the ParameterValue is not set, we'll pump messages for a full second + // but we don't cause the test to fail + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + + auto start = std::chrono::system_clock::now(); + while (std::chrono::system_clock::now() < (start + 2s)) { + if (!rclcpp::ok()) { + break; // Break for ctrl-c + } + + executor.spin_once(10ms); + + // In the case where we didn't intend to change the parameter, we'll still pump + if (value.get_type() == rclcpp::ParameterType::PARAMETER_NOT_SET) { + continue; + } + + if (clock->ros_time_is_active() == value.get()) { + return; + } + } +} + +void trigger_clock_changes( + rclcpp::Node::SharedPtr node, + std::shared_ptr clock, + bool expect_time_update = true) +{ + auto clock_pub = node->create_publisher("clock", 10); + + for (int i = 0; i < 5; ++i) { + if (!rclcpp::ok()) { + break; // Break for ctrl-c + } + rosgraph_msgs::msg::Clock msg; + msg.clock.sec = i; + msg.clock.nanosec = 1000; + clock_pub->publish(msg); + + // workaround. Long-term, there can be a more elegant fix where we hook a future up + // to a clock change callback and spin until future complete, but that's an upstream + // change + spin_until_time( + clock, + node, + std::chrono::seconds(i) + std::chrono::nanoseconds(1000), + expect_time_update + ); + } +} + +void set_use_sim_time_parameter( + rclcpp::Node::SharedPtr node, + rclcpp::ParameterValue value, + rclcpp::Clock::SharedPtr clock) +{ + auto parameters_client = std::make_shared(node); + + using namespace std::chrono_literals; + EXPECT_TRUE(parameters_client->wait_for_service(2s)); + auto set_parameters_results = parameters_client->set_parameters( + { + rclcpp::Parameter("use_sim_time", value) + }); + for (auto & result : set_parameters_results) { + EXPECT_TRUE(result.successful); + } + + // Same as above - workaround for a little bit of asynchronus behavior. The sim_time paramater + // is set synchronously, but the way the ros clock gets notified involves a pub/sub that happens + // AFTER the synchronous notification that the parameter was set. This may also get fixed + // upstream + spin_until_ros_time_updated(clock, node, value); +} + +TEST_F(TestTimeSource, detachUnattached) { + rclcpp::TimeSource ts; + + ASSERT_NO_THROW(ts.detachNode()); + + // Try multiple detach to see if error + ASSERT_NO_THROW(ts.detachNode()); +} + +TEST_F(TestTimeSource, reattach) { + rclcpp::TimeSource ts; + // Try reattach + ASSERT_NO_THROW(ts.attachNode(node)); + ASSERT_NO_THROW(ts.attachNode(node)); +} + +TEST_F(TestTimeSource, ROS_time_valid_attach_detach) { + rclcpp::TimeSource ts; + auto ros_clock = std::make_shared(RCL_ROS_TIME); + + EXPECT_FALSE(ros_clock->ros_time_is_active()); + ts.attachClock(ros_clock); + auto now = ros_clock->now(); + EXPECT_FALSE(ros_clock->ros_time_is_active()); + + ts.attachNode(node); + EXPECT_FALSE(ros_clock->ros_time_is_active()); + + ts.detachNode(); + EXPECT_FALSE(ros_clock->ros_time_is_active()); + + ts.attachNode(node); + EXPECT_FALSE(ros_clock->ros_time_is_active()); + + ts.detachClock(ros_clock); + EXPECT_FALSE(ros_clock->ros_time_is_active()); +} + +TEST_F(TestTimeSource, ROS_time_valid_wall_time) { + rclcpp::TimeSource ts; + auto ros_clock = std::make_shared(RCL_ROS_TIME); + auto ros_clock2 = std::make_shared(RCL_ROS_TIME); + + ts.attachClock(ros_clock); + EXPECT_FALSE(ros_clock->ros_time_is_active()); + + ts.attachNode(node); + EXPECT_FALSE(ros_clock->ros_time_is_active()); + + ts.attachClock(ros_clock2); + EXPECT_FALSE(ros_clock2->ros_time_is_active()); +} + +TEST_F(TestTimeSource, ROS_time_valid_sim_time) { + rclcpp::TimeSource ts; + auto ros_clock = std::make_shared(RCL_ROS_TIME); + auto ros_clock2 = std::make_shared(RCL_ROS_TIME); + + ts.attachClock(ros_clock); + EXPECT_FALSE(ros_clock->ros_time_is_active()); + + set_use_sim_time_parameter(node, rclcpp::ParameterValue(true), ros_clock); + ts.attachNode(node); + EXPECT_TRUE(ros_clock->ros_time_is_active()); + + ts.attachClock(ros_clock2); + EXPECT_TRUE(ros_clock2->ros_time_is_active()); +} + +TEST_F(TestTimeSource, ROS_invalid_sim_time) { + rclcpp::TimeSource ts(node); + EXPECT_FALSE(node->set_parameter(rclcpp::Parameter("use_sim_time", "not boolean")).successful); +} + +TEST(TimeSource, invalid_sim_time_parameter_override) +{ + rclcpp::init(0, nullptr); + + rclcpp::NodeOptions options; + options.automatically_declare_parameters_from_overrides(true); + options.append_parameter_override("use_sim_time", "not boolean"); + + RCLCPP_EXPECT_THROW_EQ( + rclcpp::Node("my_node", options), + std::invalid_argument("Invalid type for parameter 'use_sim_time', should be 'bool'")); + + rclcpp::shutdown(); +} + +TEST(TimeSource, valid_clock_type_for_sim_time) +{ + rclcpp::init(0, nullptr); + + rclcpp::NodeOptions options; + auto node = std::make_shared("my_node", options); + EXPECT_TRUE( + node->set_parameter( + rclcpp::Parameter( + "use_sim_time", rclcpp::ParameterValue( + true))).successful); + rclcpp::shutdown(); +} + +TEST(TimeSource, invalid_clock_type_for_sim_time) +{ + rclcpp::init(0, nullptr); + + rclcpp::NodeOptions options; + options.clock_type(RCL_STEADY_TIME); + auto node = std::make_shared("my_node", options); + EXPECT_FALSE( + node->set_parameter( + rclcpp::Parameter( + "use_sim_time", rclcpp::ParameterValue( + true))).successful); + rclcpp::shutdown(); +} + +TEST_F(TestTimeSource, clock) { + rclcpp::TimeSource ts(node); + auto ros_clock = std::make_shared(RCL_ROS_TIME); + EXPECT_FALSE(ros_clock->ros_time_is_active()); + ts.attachClock(ros_clock); + EXPECT_FALSE(ros_clock->ros_time_is_active()); + + trigger_clock_changes(node, ros_clock, false); + + // Even now that we've received a message, ROS time should still not be active since the + // parameter has not been explicitly set. + EXPECT_FALSE(ros_clock->ros_time_is_active()); + + // Activate ROS time. + set_use_sim_time_parameter(node, rclcpp::ParameterValue(true), ros_clock); + EXPECT_TRUE(ros_clock->ros_time_is_active()); + + trigger_clock_changes(node, ros_clock); + + auto t_out = ros_clock->now(); + + // Time from clock should now reflect what was published on the /clock topic. + auto t_low = rclcpp::Time(1, 0, RCL_ROS_TIME); + auto t_high = rclcpp::Time(10, 100000, RCL_ROS_TIME); + EXPECT_NE(0L, t_out.nanoseconds()); + EXPECT_LT(t_low.nanoseconds(), t_out.nanoseconds()); + EXPECT_GT(t_high.nanoseconds(), t_out.nanoseconds()); +} + +class CallbackObject +{ +public: + int pre_callback_calls_ = 0; + int last_precallback_id_ = 0; + void pre_callback(int id) + { + last_precallback_id_ = id; + ++pre_callback_calls_; + } + + int post_callback_calls_ = 0; + int last_postcallback_id_ = 0; + rcl_time_jump_t last_timejump_; + void post_callback(const rcl_time_jump_t & jump, int id) + { + last_postcallback_id_ = id; last_timejump_ = jump; + ++post_callback_calls_; + } +}; + +TEST_F(TestTimeSource, callbacks) { + CallbackObject cbo; + rcl_jump_threshold_t jump_threshold; + jump_threshold.min_forward.nanoseconds = 1; + jump_threshold.min_backward.nanoseconds = -1; + jump_threshold.on_clock_change = true; + + rclcpp::TimeSource ts(node); + auto ros_clock = std::make_shared(RCL_ROS_TIME); + + // Register a callback for time jumps + rclcpp::JumpHandler::SharedPtr callback_handler = ros_clock->create_jump_callback( + std::bind(&CallbackObject::pre_callback, &cbo, 1), + std::bind(&CallbackObject::post_callback, &cbo, std::placeholders::_1, 1), + jump_threshold); + + EXPECT_EQ(0, cbo.last_precallback_id_); + EXPECT_EQ(0, cbo.last_postcallback_id_); + + EXPECT_FALSE(ros_clock->ros_time_is_active()); + + ts.attachClock(ros_clock); + EXPECT_FALSE(ros_clock->ros_time_is_active()); + + // Last arg below is 'expect_time_update' Since ros_time is not active yet, we don't expect + // the simulated time to be updated by trigger_clock_changes. The method will pump messages + // anyway, but won't fail the test when the simulated time doesn't update + trigger_clock_changes(node, ros_clock, false); + auto t_low = rclcpp::Time(1, 0, RCL_ROS_TIME); + auto t_high = rclcpp::Time(10, 100000, RCL_ROS_TIME); + + // Callbacks will not be triggered since ROS time is not active. + EXPECT_EQ(0, cbo.last_precallback_id_); + EXPECT_EQ(0, cbo.last_postcallback_id_); + + // Activate ROS time. + set_use_sim_time_parameter(node, rclcpp::ParameterValue(true), ros_clock); + EXPECT_TRUE(ros_clock->ros_time_is_active()); + + trigger_clock_changes(node, ros_clock); + + auto t_out = ros_clock->now(); + + EXPECT_NE(0L, t_out.nanoseconds()); + EXPECT_LT(t_low.nanoseconds(), t_out.nanoseconds()); + EXPECT_GT(t_high.nanoseconds(), t_out.nanoseconds()); + + // Callbacks will now have been triggered since ROS time is active. + EXPECT_EQ(1, cbo.last_precallback_id_); + EXPECT_EQ(1, cbo.last_postcallback_id_); + + // Change callbacks + rclcpp::JumpHandler::SharedPtr callback_handler2 = ros_clock->create_jump_callback( + std::bind(&CallbackObject::pre_callback, &cbo, 2), + std::bind(&CallbackObject::post_callback, &cbo, std::placeholders::_1, 2), + jump_threshold); + + trigger_clock_changes(node, ros_clock); + + EXPECT_EQ(2, cbo.last_precallback_id_); + EXPECT_EQ(2, cbo.last_postcallback_id_); + + EXPECT_TRUE(ros_clock->ros_time_is_active()); + + t_out = ros_clock->now(); + + EXPECT_NE(0L, t_out.nanoseconds()); + EXPECT_LT(t_low.nanoseconds(), t_out.nanoseconds()); + EXPECT_GT(t_high.nanoseconds(), t_out.nanoseconds()); + + // Register a callback handler with only pre_callback + rclcpp::JumpHandler::SharedPtr callback_handler3 = ros_clock->create_jump_callback( + std::bind(&CallbackObject::pre_callback, &cbo, 3), + std::function(), + jump_threshold); + + trigger_clock_changes(node, ros_clock); + EXPECT_EQ(3, cbo.last_precallback_id_); + EXPECT_EQ(2, cbo.last_postcallback_id_); + + // Register a callback handler with only post_callback + rclcpp::JumpHandler::SharedPtr callback_handler4 = ros_clock->create_jump_callback( + std::function(), + std::bind(&CallbackObject::post_callback, &cbo, std::placeholders::_1, 4), + jump_threshold); + + trigger_clock_changes(node, ros_clock); + EXPECT_EQ(3, cbo.last_precallback_id_); + EXPECT_EQ(4, cbo.last_postcallback_id_); +} + + +TEST_F(TestTimeSource, callback_handler_erasure) { + CallbackObject cbo; + rcl_jump_threshold_t jump_threshold; + jump_threshold.min_forward.nanoseconds = 1; + jump_threshold.min_backward.nanoseconds = -1; + jump_threshold.on_clock_change = true; + + rclcpp::TimeSource ts(node); + auto ros_clock = std::make_shared(RCL_ROS_TIME); + ts.attachClock(ros_clock); + EXPECT_FALSE(ros_clock->ros_time_is_active()); + + // Register a callback for time jumps + rclcpp::JumpHandler::SharedPtr callback_handler = ros_clock->create_jump_callback( + std::bind(&CallbackObject::pre_callback, &cbo, 1), + std::bind(&CallbackObject::post_callback, &cbo, std::placeholders::_1, 1), + jump_threshold); + + // Second callback handler + rclcpp::JumpHandler::SharedPtr callback_handler2 = ros_clock->create_jump_callback( + std::bind(&CallbackObject::pre_callback, &cbo, 1), + std::bind(&CallbackObject::post_callback, &cbo, std::placeholders::_1, 1), + jump_threshold); + + // Callbacks will not be triggered since ROS time is not active. + EXPECT_EQ(0, cbo.last_precallback_id_); + EXPECT_EQ(0, cbo.last_postcallback_id_); + + // Activate ROS time. + set_use_sim_time_parameter(node, rclcpp::ParameterValue(true), ros_clock); + EXPECT_TRUE(ros_clock->ros_time_is_active()); + + trigger_clock_changes(node, ros_clock); + + auto t_low = rclcpp::Time(1, 0, RCL_ROS_TIME); + auto t_high = rclcpp::Time(10, 100000, RCL_ROS_TIME); + + // Callbacks will now have been triggered since ROS time is active. + EXPECT_EQ(1, cbo.last_precallback_id_); + EXPECT_EQ(1, cbo.last_postcallback_id_); + + auto t_out = ros_clock->now(); + + EXPECT_NE(0L, t_out.nanoseconds()); + EXPECT_LT(t_low.nanoseconds(), t_out.nanoseconds()); + EXPECT_GT(t_high.nanoseconds(), t_out.nanoseconds()); + + // Requeue a pointer in a new position + callback_handler = ros_clock->create_jump_callback( + std::bind(&CallbackObject::pre_callback, &cbo, 2), + std::bind(&CallbackObject::post_callback, &cbo, std::placeholders::_1, 2), + jump_threshold); + + // Remove the last callback in the vector + callback_handler2.reset(); + + trigger_clock_changes(node, ros_clock); + + EXPECT_EQ(2, cbo.last_precallback_id_); + EXPECT_EQ(2, cbo.last_postcallback_id_); + + EXPECT_TRUE(ros_clock->ros_time_is_active()); + + t_out = ros_clock->now(); + + EXPECT_NE(0L, t_out.nanoseconds()); + EXPECT_LT(t_low.nanoseconds(), t_out.nanoseconds()); + EXPECT_GT(t_high.nanoseconds(), t_out.nanoseconds()); +} + +TEST_F(TestTimeSource, parameter_activation) { + rclcpp::TimeSource ts(node); + auto ros_clock = std::make_shared(RCL_ROS_TIME); + EXPECT_FALSE(ros_clock->ros_time_is_active()); + + ts.attachClock(ros_clock); + EXPECT_FALSE(ros_clock->ros_time_is_active()); + + set_use_sim_time_parameter(node, rclcpp::ParameterValue(true), ros_clock); + EXPECT_TRUE(ros_clock->ros_time_is_active()); + + set_use_sim_time_parameter(node, rclcpp::ParameterValue(false), ros_clock); + EXPECT_FALSE(ros_clock->ros_time_is_active()); + + // If the use_sim_time parameter is not explicitly set to True, this clock's use of sim time + // should not be affected by the presence of a clock publisher. + trigger_clock_changes(node, ros_clock, false); + EXPECT_FALSE(ros_clock->ros_time_is_active()); + set_use_sim_time_parameter(node, rclcpp::ParameterValue(false), ros_clock); + EXPECT_FALSE(ros_clock->ros_time_is_active()); + set_use_sim_time_parameter(node, rclcpp::ParameterValue(true), ros_clock); + EXPECT_TRUE(ros_clock->ros_time_is_active()); +} + +TEST_F(TestTimeSource, no_pre_jump_callback) { + CallbackObject cbo; + rcl_jump_threshold_t jump_threshold; + jump_threshold.min_forward.nanoseconds = 0; + jump_threshold.min_backward.nanoseconds = 0; + jump_threshold.on_clock_change = true; + + rclcpp::TimeSource ts(node); + auto ros_clock = std::make_shared(RCL_ROS_TIME); + + // Register a callback for time jumps + rclcpp::JumpHandler::SharedPtr callback_handler = ros_clock->create_jump_callback( + nullptr, + std::bind(&CallbackObject::post_callback, &cbo, std::placeholders::_1, 1), + jump_threshold); + + ASSERT_EQ(0, cbo.last_precallback_id_); + ASSERT_EQ(0, cbo.last_postcallback_id_); + ts.attachClock(ros_clock); + + // Activate ROS time + set_use_sim_time_parameter(node, rclcpp::ParameterValue(true), ros_clock); + ASSERT_TRUE(ros_clock->ros_time_is_active()); + + EXPECT_EQ(0, cbo.last_precallback_id_); + EXPECT_EQ(0, cbo.pre_callback_calls_); + EXPECT_EQ(1, cbo.last_postcallback_id_); + EXPECT_EQ(1, cbo.post_callback_calls_); +} + +// A TimeSource-inheriting class +// that allows access to TimeSource protected attributes +// use_clock_thread_ and clock_executor_thread_ +class ClockThreadTestingTimeSource : public rclcpp::TimeSource +{ +public: + ClockThreadTestingTimeSource() + : rclcpp::TimeSource() + { + } + + bool GetUseClockThreadOption() + { + return this->get_use_clock_thread(); + } + + bool IsClockThreadJoinable() + { + return this->clock_thread_is_joinable(); + } +}; + +TEST_F(TestTimeSource, check_use_clock_thread_value) { + // Create three nodes, with use_clock_thread option + // respectively set to default, true, and false + + auto default_node_ = std::make_shared( + "default_option_node"); + + auto clock_thread_node_ = std::make_shared( + "clock_thread_node", + rclcpp::NodeOptions().use_clock_thread(true)); + + auto no_clock_thread_node_ = std::make_shared( + "no_clock_thread_node", + rclcpp::NodeOptions().use_clock_thread(false)); + + // Test value of use_clock_thread_ TimeSource attribute + // when the different nodes are attached + + ClockThreadTestingTimeSource ts; + + ts.attachNode(default_node_); + ASSERT_TRUE(ts.GetUseClockThreadOption()); + ts.detachNode(); + + ts.attachNode(clock_thread_node_); + ASSERT_TRUE(ts.GetUseClockThreadOption()); + ts.detachNode(); + + ts.attachNode(no_clock_thread_node_); + ASSERT_FALSE(ts.GetUseClockThreadOption()); + ts.detachNode(); +} + +TEST_F(TestTimeSource, check_clock_thread_status) { + // Test if TimeSource clock-dedicated thread is running + // according to the use_sim_time parameter + // and to the options of the attached node + ClockThreadTestingTimeSource ts; + + // Tests for default options node + auto default_node_ = std::make_shared( + "default_option_node"); + + default_node_->set_parameter(rclcpp::Parameter("use_sim_time", true)); + ts.attachNode(default_node_); + ASSERT_TRUE(ts.IsClockThreadJoinable()); + ts.detachNode(); + + default_node_->set_parameter(rclcpp::Parameter("use_sim_time", false)); + ts.attachNode(default_node_); + ASSERT_FALSE(ts.IsClockThreadJoinable()); + ts.detachNode(); + + // Tests for node with use_clock_thread option forced to false + auto no_clock_thread_node_ = std::make_shared( + "no_clock_thread_node", + rclcpp::NodeOptions().use_clock_thread(false)); + + no_clock_thread_node_->set_parameter(rclcpp::Parameter("use_sim_time", true)); + ts.attachNode(no_clock_thread_node_); + ASSERT_FALSE(ts.IsClockThreadJoinable()); + ts.detachNode(); + + no_clock_thread_node_->set_parameter(rclcpp::Parameter("use_sim_time", false)); + ts.attachNode(no_clock_thread_node_); + ASSERT_FALSE(ts.IsClockThreadJoinable()); + ts.detachNode(); +} + +// A Node-inheriting class +// that regularly publishes a incremented Clock msg on topic `/clock' +class SimClockPublisherNode : public rclcpp::Node +{ +public: + SimClockPublisherNode() + : rclcpp::Node("sim_clock_publisher_node"), + pub_time_(0, 0) + { + // Create a clock publisher + clock_pub_ = this->create_publisher( + "/clock", + rclcpp::QoS(1) + ); + + // Create a 1ms timer + pub_timer_ = this->create_wall_timer( + std::chrono::milliseconds(1), + std::bind( + &SimClockPublisherNode::timer_callback, + this) + ); + } + + ~SimClockPublisherNode() + { + // Cleanly stop executor and thread + node_executor.cancel(); + node_thread_.join(); + } + + void SpinNode() + { + // Spin node in its own dedicated thread + node_thread_ = std::thread( + [this]() { + node_executor.add_node(this->get_node_base_interface()); + node_executor.spin(); + }); + } + +private: + void timer_callback() + { + // Increment the time, update the clock msg and publish it + pub_time_ += rclcpp::Duration(0, 1000000); + clock_msg_.clock = pub_time_; + clock_pub_->publish(clock_msg_); + } + + rclcpp::Publisher::SharedPtr clock_pub_; + rclcpp::TimerBase::SharedPtr pub_timer_; + rclcpp::Time pub_time_; + rosgraph_msgs::msg::Clock clock_msg_; + std::thread node_thread_; + rclcpp::executors::SingleThreadedExecutor node_executor; +}; + +// A Node-inheriting class +// that check its clock time within a timer callback +class ClockThreadTestingNode : public rclcpp::Node +{ +public: + ClockThreadTestingNode() + : rclcpp::Node("clock_thread_testing_node") + { + // Set use_sim_time parameter to true to subscribe to `/clock` topic + this->set_parameter(rclcpp::Parameter("use_sim_time", true)); + + // Create a 100ms timer + timer_ = this->create_timer( + std::chrono::milliseconds(100), + std::bind( + &ClockThreadTestingNode::timer_callback, + this) + ); + } + + bool GetIsCallbackFrozen() + { + return is_callback_frozen_; + } + +private: + void timer_callback() + { + rclcpp::Time start_time = this->now(); + bool is_time_out = false; + + // While loop condition tests + // if the node's clock time is incremented + while (rclcpp::ok() && + !is_time_out) + { + rclcpp::sleep_for(std::chrono::milliseconds(100)); + rclcpp::Time time_now = this->now(); + rclcpp::Duration time_spent = time_now - start_time; + is_time_out = time_spent.seconds() > 1.0; + } + + // If out of while loop, set variable to false + // and cancel timer to avoid to enter the callback again + is_callback_frozen_ = false; + timer_->cancel(); + } + + rclcpp::TimerBase::SharedPtr timer_; + bool is_callback_frozen_ = true; +}; + +// TODO(ivanpauno): This test was using a wall timer, when it was supposed to use sim time. +// It was also using `use_clock_tread = false`, when it was supposed to be `true`. +// Fixing the test to work as originally intended makes it super flaky. +// Disabling it until the test is fixed. +// TEST_F(TestTimeSource, check_sim_time_updated_in_callback_if_use_clock_thread) { +// // Test if clock time of a node with +// // parameter use_sim_time = true and option use_clock_thread = true +// // is updated while node is not spinning +// // (in a timer callback) + +// // Create a "sim time" publisher and spin it +// SimClockPublisherNode pub_node; +// pub_node.SpinNode(); + +// // Spin node for 2 seconds +// ClockThreadTestingNode clock_thread_testing_node; +// auto steady_clock = rclcpp::Clock(RCL_STEADY_TIME); +// auto start_time = steady_clock.now(); +// while (rclcpp::ok() && +// (steady_clock.now() - start_time).seconds() < 2.0) +// { +// rclcpp::spin_some(clock_thread_testing_node.get_node_base_interface()); +// } + +// // Node should have get out of timer callback +// ASSERT_FALSE(clock_thread_testing_node.GetIsCallbackFrozen()); +// } + +TEST_F(TestTimeSource, clock_sleep_until_with_ros_time_basic) { + SimClockPublisherNode pub_node; + pub_node.SpinNode(); + + node->set_parameter({"use_sim_time", true}); + auto clock = std::make_shared(RCL_ROS_TIME); + rclcpp::TimeSource time_source(node); + time_source.attachClock(clock); + + // Wait until time source has definitely received a first ROS time from the pub node + { + rcl_jump_threshold_t threshold; + threshold.on_clock_change = false; + threshold.min_backward.nanoseconds = -1; + threshold.min_forward.nanoseconds = 1; + + std::condition_variable cv; + std::mutex mutex; + auto handler = clock->create_jump_callback( + nullptr, + [&cv](const rcl_time_jump_t &) {cv.notify_all();}, + threshold); + std::unique_lock lock(mutex); + cv.wait(lock); + } + + auto now = clock->now(); + // Any amount of time will do, just need to make sure that we awake and return true + auto until = now + rclcpp::Duration(0, 500); + EXPECT_TRUE(clock->sleep_until(until)); +} diff --git a/rclcpp/test/rclcpp/test_timer.cpp b/rclcpp/test/rclcpp/test_timer.cpp new file mode 100644 index 0000000000..55c0722ae1 --- /dev/null +++ b/rclcpp/test/rclcpp/test_timer.cpp @@ -0,0 +1,382 @@ +// 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 "rcl/timer.h" + +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "../mocking_utils/patch.hpp" +#include "../utils/rclcpp_gtest_macros.hpp" + +using namespace std::chrono_literals; + +/// We want to test everything for both the wall and generic timer. +enum class TimerType +{ + WALL_TIMER, + GENERIC_TIMER, +}; + +/// Timer testing bring up and teardown +class TestTimer : public ::testing::TestWithParam +{ +protected: + void SetUp() override + { + rclcpp::init(0, nullptr); + executor = std::make_shared(); + + has_timer_run.store(false); + cancel_timer.store(false); + + test_node = std::make_shared("test_timer_node"); + + auto timer_callback = [this]() -> void { + this->has_timer_run.store(true); + + if (this->cancel_timer.load()) { + this->timer->cancel(); + } + // prevent any tests running timer from blocking + this->executor->cancel(); + }; + + // Store the timer type for use in TEST_P declarations. + timer_type = GetParam(); + switch (timer_type) { + case TimerType::WALL_TIMER: + timer = test_node->create_wall_timer(100ms, timer_callback); + EXPECT_TRUE(timer->is_steady()); + break; + case TimerType::GENERIC_TIMER: + timer = test_node->create_timer(100ms, timer_callback); + EXPECT_FALSE(timer->is_steady()); + break; + } + timer_without_autostart = test_node->create_wall_timer( + 100ms, + [this]() -> void + { + this->has_timer_run.store(true); + + if (this->cancel_timer.load()) { + this->timer->cancel(); + } + // prevent any tests running timer from blocking + this->executor->cancel(); + }, nullptr, false); + EXPECT_TRUE(timer_without_autostart->is_steady()); + + executor->add_node(test_node); + // don't start spinning, let the test dictate when + } + + void TearDown() override + { + timer.reset(); + test_node.reset(); + executor.reset(); + rclcpp::shutdown(); + } + + // set to true if the timer callback executed, false otherwise + TimerType timer_type; + std::atomic has_timer_run; + // flag used to cancel the timer in the timer callback. If true cancel the timer, otherwise + // cancel the executor (preventing any tests from blocking) + std::atomic cancel_timer; + rclcpp::Node::SharedPtr test_node; + std::shared_ptr timer; + std::shared_ptr timer_without_autostart; + std::shared_ptr executor; +}; + +/// check if initial states are set as expected +void test_initial_conditions( + std::shared_ptr & timer, + std::atomic & has_timer_run) +{ + ASSERT_FALSE(timer->is_canceled()); + ASSERT_FALSE(has_timer_run.load()); +} + +/// Simple test +TEST_P(TestTimer, test_simple_cancel) +{ + // expect clean state, don't run otherwise + test_initial_conditions(timer, has_timer_run); + + // cancel + timer->cancel(); + EXPECT_TRUE(timer->is_canceled()); + + EXPECT_FALSE(has_timer_run.load()); +} + +/// Test state when using reset +TEST_P(TestTimer, test_is_canceled_reset) +{ + // expect clean state, don't run otherwise + test_initial_conditions(timer, has_timer_run); + + // reset shouldn't affect state (not canceled yet) + timer->reset(); + EXPECT_LE(timer->time_until_trigger().count(), std::chrono::nanoseconds::max().count()); + EXPECT_FALSE(timer->is_canceled()); + + // cancel after reset + timer->cancel(); + EXPECT_TRUE(timer->is_canceled()); + EXPECT_EQ(timer->time_until_trigger().count(), std::chrono::nanoseconds::max().count()); + + // reset and cancel + timer->reset(); + EXPECT_FALSE(timer->is_canceled()); + timer->cancel(); + EXPECT_TRUE(timer->is_canceled()); + + EXPECT_FALSE(has_timer_run.load()); +} + +/// Run and check state, cancel the executor +TEST_P(TestTimer, test_run_cancel_executor) +{ + // expect clean state, don't run otherwise + test_initial_conditions(timer, has_timer_run); + + // run the timer (once, this forces an executor cancel so spin won't block) + // but the timer was not explicitly cancelled + executor->spin(); + EXPECT_TRUE(has_timer_run.load()); + + // force a timer cancel + EXPECT_FALSE(timer->is_canceled()); + timer->cancel(); + EXPECT_TRUE(timer->is_canceled()); +} + +/// Run and check state, cancel the timer +TEST_P(TestTimer, test_run_cancel_timer) +{ + // expect clean state, don't run otherwise + test_initial_conditions(timer, has_timer_run); + + // force a timer cancellation + cancel_timer.store(true); + // run the timer (once, this forces an executor cancel so spin won't block) + executor->spin(); + EXPECT_TRUE(has_timer_run.load()); + EXPECT_TRUE(timer->is_canceled()); +} + +TEST_P(TestTimer, test_bad_arguments) { + auto node_base = rclcpp::node_interfaces::get_node_base_interface(test_node); + auto context = node_base->get_context(); + + auto steady_clock = std::make_shared(RCL_STEADY_TIME); + + // Negative period + EXPECT_THROW( + rclcpp::GenericTimer(steady_clock, -1ms, []() {}, context), + rclcpp::exceptions::RCLInvalidArgument); + + // Very negative period + constexpr auto nanoseconds_min = std::chrono::nanoseconds::min(); + EXPECT_THROW( + rclcpp::GenericTimer( + steady_clock, nanoseconds_min, []() {}, context), + rclcpp::exceptions::RCLInvalidArgument); + + // nanoseconds max, should be ok + constexpr auto nanoseconds_max = std::chrono::nanoseconds::max(); + EXPECT_NO_THROW( + rclcpp::GenericTimer( + steady_clock, nanoseconds_max, []() {}, context)); + + // 0 duration period, should be ok + EXPECT_NO_THROW( + rclcpp::GenericTimer(steady_clock, 0ms, []() {}, context)); + + // context is null, which resorts to default + EXPECT_NO_THROW( + rclcpp::GenericTimer(steady_clock, 1ms, []() {}, nullptr)); + + // Clock is unitialized + auto unitialized_clock = std::make_shared(RCL_CLOCK_UNINITIALIZED); + EXPECT_THROW( + rclcpp::GenericTimer(unitialized_clock, 1us, []() {}, context), + rclcpp::exceptions::RCLError); +} + +TEST_P(TestTimer, callback_with_timer) { + rclcpp::TimerBase * timer_ptr = nullptr; + auto timer_callback = [&timer_ptr](rclcpp::TimerBase & timer) { + timer_ptr = &timer; + }; + switch (timer_type) { + case TimerType::WALL_TIMER: + timer = test_node->create_wall_timer(1ms, timer_callback); + break; + case TimerType::GENERIC_TIMER: + timer = test_node->create_timer(1ms, timer_callback); + break; + } + auto start = std::chrono::steady_clock::now(); + while (nullptr == timer_ptr && + (std::chrono::steady_clock::now() - start) < std::chrono::milliseconds(100)) + { + executor->spin_once(std::chrono::milliseconds(10)); + } + EXPECT_EQ(timer.get(), timer_ptr); + EXPECT_LE(std::chrono::nanoseconds(0).count(), timer_ptr->time_until_trigger().count()); + EXPECT_FALSE(timer_ptr->is_ready()); +} + +TEST_P(TestTimer, callback_with_timer_info) { + rclcpp::TimerInfo info; + auto timer_callback = [&info](const rclcpp::TimerInfo & timer_info) { + info = timer_info; + }; + switch (timer_type) { + case TimerType::WALL_TIMER: + timer = test_node->create_wall_timer(1ms, timer_callback); + break; + case TimerType::GENERIC_TIMER: + timer = test_node->create_timer(1ms, timer_callback); + break; + } + auto start = std::chrono::steady_clock::now(); + while (info.actual_call_time.nanoseconds() == 0 && + (std::chrono::steady_clock::now() - start) < std::chrono::milliseconds(100)) + { + executor->spin_once(std::chrono::milliseconds(10)); + } + EXPECT_GE(info.actual_call_time, info.expected_call_time); +} + +TEST_P(TestTimer, callback_with_period_zero) { + rclcpp::TimerBase * timer_ptr = nullptr; + auto timer_callback = [&timer_ptr](rclcpp::TimerBase & timer) { + timer_ptr = &timer; + }; + switch (timer_type) { + case TimerType::WALL_TIMER: + timer = test_node->create_wall_timer(0ms, timer_callback); + break; + case TimerType::GENERIC_TIMER: + timer = test_node->create_timer(0ms, timer_callback); + break; + } + auto start = std::chrono::steady_clock::now(); + while (nullptr == timer_ptr && + (std::chrono::steady_clock::now() - start) < std::chrono::milliseconds(100)) + { + executor->spin_once(std::chrono::milliseconds(10)); + } + ASSERT_EQ(timer.get(), timer_ptr); + EXPECT_GE(std::chrono::nanoseconds(0).count(), timer_ptr->time_until_trigger().count()); + EXPECT_TRUE(timer_ptr->is_ready()); +} + +/// Test internal failures using mocks +TEST_P(TestTimer, test_failures_with_exceptions) +{ + // expect clean state, don't run otherwise + test_initial_conditions(timer, has_timer_run); + { + std::shared_ptr timer_to_test_destructor; + // Test destructor failure, just logs a msg + auto mock = mocking_utils::inject_on_return("lib:rclcpp", rcl_timer_fini, RCL_RET_ERROR); + if (timer_type == TimerType::WALL_TIMER) { + timer_to_test_destructor = + test_node->create_wall_timer(std::chrono::milliseconds(0), [](void) {}); + } else { + timer_to_test_destructor = + test_node->create_timer(std::chrono::milliseconds(0), [](void) {}); + } + timer_to_test_destructor.reset(); + } + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_timer_cancel, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + timer->cancel(), std::runtime_error("Couldn't cancel timer: error not set")); + } + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_timer_is_canceled, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + timer->is_canceled(), + std::runtime_error("Couldn't get timer cancelled state: error not set")); + } + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_timer_reset, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + timer->reset(), std::runtime_error("Couldn't reset timer: error not set")); + } + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_timer_is_ready, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + timer->is_ready(), std::runtime_error("Failed to check timer: error not set")); + } + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_timer_get_time_until_next_call, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + timer->time_until_trigger(), + std::runtime_error("Timer could not get time until next call: error not set")); + } +} + +INSTANTIATE_TEST_SUITE_P( + PerTimerType, TestTimer, + ::testing::Values(TimerType::WALL_TIMER, TimerType::GENERIC_TIMER), + [](const ::testing::TestParamInfo & info) -> std::string { + switch (info.param) { + case TimerType::WALL_TIMER: + return std::string("wall_timer"); + case TimerType::GENERIC_TIMER: + return std::string("generic_timer"); + default: + break; + } + return std::string("unknown"); + } +); + +/// Simple test of a timer without autostart +TEST_P(TestTimer, test_timer_without_autostart) +{ + EXPECT_TRUE(timer_without_autostart->is_canceled()); + EXPECT_EQ( + timer_without_autostart->time_until_trigger().count(), + std::chrono::nanoseconds::max().count()); + // Reset to change start timer + timer_without_autostart->reset(); + EXPECT_LE( + timer_without_autostart->time_until_trigger().count(), + std::chrono::nanoseconds::max().count()); + EXPECT_FALSE(timer_without_autostart->is_canceled()); +} diff --git a/rclcpp/test/rclcpp/test_timers_manager.cpp b/rclcpp/test/rclcpp/test_timers_manager.cpp new file mode 100644 index 0000000000..ef7605aadc --- /dev/null +++ b/rclcpp/test/rclcpp/test_timers_manager.cpp @@ -0,0 +1,439 @@ +// Copyright 2023 iRobot Corporation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT 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 "rclcpp/contexts/default_context.hpp" +#include "rclcpp/experimental/timers_manager.hpp" + +using namespace std::chrono_literals; + +using rclcpp::experimental::TimersManager; + +using CallbackT = std::function; +using TimerT = rclcpp::WallTimer; + +class TestTimersManager : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + } + + void TearDown() + { + rclcpp::shutdown(); + } +}; + +static void execute_all_ready_timers(std::shared_ptr timers_manager) +{ + bool head_was_ready = false; + do { + head_was_ready = timers_manager->execute_head_timer(); + } while (head_was_ready); +} + +TEST_F(TestTimersManager, empty_manager) +{ + auto timers_manager = std::make_shared( + rclcpp::contexts::get_global_default_context()); + + EXPECT_EQ(std::chrono::nanoseconds::max(), timers_manager->get_head_timeout()); + EXPECT_FALSE(timers_manager->execute_head_timer()); + EXPECT_NO_THROW(timers_manager->clear()); + EXPECT_NO_THROW(timers_manager->start()); + EXPECT_NO_THROW(timers_manager->stop()); +} + +TEST_F(TestTimersManager, add_run_remove_timer) +{ + size_t t_runs = 0; + std::chrono::milliseconds timer_period(10); + + auto t = TimerT::make_shared( + timer_period, + [&t_runs]() { + t_runs++; + }, + rclcpp::contexts::get_global_default_context()); + std::weak_ptr t_weak = t; + + // Add the timer to the timers manager + auto timers_manager = std::make_shared( + rclcpp::contexts::get_global_default_context()); + timers_manager->add_timer(t); + + // Sleep for more 3 times the timer period + std::this_thread::sleep_for(3 * timer_period); + + // The timer is executed only once, even if we slept 3 times the period + execute_all_ready_timers(timers_manager); + EXPECT_EQ(1u, t_runs); + + // Remove the timer from the manager + timers_manager->remove_timer(t); + + t.reset(); + // The timer is now not valid anymore + EXPECT_FALSE(t_weak.lock() != nullptr); +} + +TEST_F(TestTimersManager, clear) +{ + auto timers_manager = std::make_shared( + rclcpp::contexts::get_global_default_context()); + + auto t1 = TimerT::make_shared(1ms, CallbackT(), rclcpp::contexts::get_global_default_context()); + std::weak_ptr t1_weak = t1; + auto t2 = TimerT::make_shared(1ms, CallbackT(), rclcpp::contexts::get_global_default_context()); + std::weak_ptr t2_weak = t2; + + timers_manager->add_timer(t1); + timers_manager->add_timer(t2); + + EXPECT_TRUE(t1_weak.lock() != nullptr); + EXPECT_TRUE(t2_weak.lock() != nullptr); + + timers_manager->clear(); + + t1.reset(); + t2.reset(); + + EXPECT_FALSE(t1_weak.lock() != nullptr); + EXPECT_FALSE(t2_weak.lock() != nullptr); +} + +TEST_F(TestTimersManager, remove_not_existing_timer) +{ + auto timers_manager = std::make_shared( + rclcpp::contexts::get_global_default_context()); + + // Try to remove a nullptr timer + EXPECT_NO_THROW(timers_manager->remove_timer(nullptr)); + + auto t = TimerT::make_shared(1ms, CallbackT(), rclcpp::contexts::get_global_default_context()); + timers_manager->add_timer(t); + + // Remove twice the same timer + timers_manager->remove_timer(t); + EXPECT_NO_THROW(timers_manager->remove_timer(t)); +} + +TEST_F(TestTimersManager, timers_thread_exclusive_usage) +{ + auto timers_manager = std::make_shared( + rclcpp::contexts::get_global_default_context()); + + timers_manager->start(); + + EXPECT_THROW(timers_manager->start(), std::exception); + EXPECT_THROW(timers_manager->get_head_timeout(), std::exception); + EXPECT_THROW(timers_manager->execute_head_timer(), std::exception); + + timers_manager->stop(); + + EXPECT_NO_THROW(timers_manager->get_head_timeout()); + EXPECT_NO_THROW(timers_manager->execute_head_timer()); +} + +TEST_F(TestTimersManager, add_timer_twice) +{ + auto timers_manager = std::make_shared( + rclcpp::contexts::get_global_default_context()); + + auto t = TimerT::make_shared(1ms, CallbackT(), rclcpp::contexts::get_global_default_context()); + + timers_manager->add_timer(t); + EXPECT_NO_THROW(timers_manager->add_timer(t)); +} + +TEST_F(TestTimersManager, add_nullptr) +{ + auto timers_manager = std::make_shared( + rclcpp::contexts::get_global_default_context()); + + EXPECT_THROW(timers_manager->add_timer(nullptr), std::exception); +} + +TEST_F(TestTimersManager, head_not_ready) +{ + auto timers_manager = std::make_shared( + rclcpp::contexts::get_global_default_context()); + + size_t t_runs = 0; + auto t = TimerT::make_shared( + 10s, + [&t_runs]() { + t_runs++; + }, + rclcpp::contexts::get_global_default_context()); + + timers_manager->add_timer(t); + + // Timer will take 10s to get ready, so nothing to execute here + bool ret = timers_manager->execute_head_timer(); + EXPECT_FALSE(ret); + EXPECT_EQ(0u, t_runs); +} + +TEST_F(TestTimersManager, start_stop_timers_thread) +{ + auto timers_manager = std::make_shared( + rclcpp::contexts::get_global_default_context()); + + auto t = TimerT::make_shared(1ms, []() {}, rclcpp::contexts::get_global_default_context()); + timers_manager->add_timer(t); + + // Calling start multiple times will throw an error + EXPECT_NO_THROW(timers_manager->start()); + EXPECT_THROW(timers_manager->start(), std::exception); + + // Calling stop multiple times does not throw an error + EXPECT_NO_THROW(timers_manager->stop()); + EXPECT_NO_THROW(timers_manager->stop()); +} + +TEST_F(TestTimersManager, timers_thread) +{ + auto timers_manager = std::make_shared( + rclcpp::contexts::get_global_default_context()); + + int t1_runs = 0; + auto t1 = TimerT::make_shared( + 1ms, + [&t1_runs]() { + t1_runs++; + }, + rclcpp::contexts::get_global_default_context()); + + int t2_runs = 0; + auto t2 = TimerT::make_shared( + 1ms, + [&t2_runs]() { + t2_runs++; + }, + rclcpp::contexts::get_global_default_context()); + + // Add timers + timers_manager->add_timer(t1); + timers_manager->add_timer(t2); + + // Run timers thread for a while + timers_manager->start(); + std::this_thread::sleep_for(50ms); + timers_manager->stop(); + + EXPECT_LT(1u, t1_runs); + EXPECT_LT(1u, t2_runs); + EXPECT_LE(std::abs(t1_runs - t2_runs), 1); +} + +TEST_F(TestTimersManager, destructor) +{ + size_t t_runs = 0; + auto t = TimerT::make_shared( + 1ms, + [&t_runs]() { + t_runs++; + }, + rclcpp::contexts::get_global_default_context()); + std::weak_ptr t_weak = t; + + // When the timers manager is destroyed, it will stop the thread + // and clear the timers + { + auto timers_manager = std::make_shared( + rclcpp::contexts::get_global_default_context()); + + timers_manager->add_timer(t); + + timers_manager->start(); + std::this_thread::sleep_for(100ms); + + EXPECT_LT(1u, t_runs); + } + + // The thread is not running anymore, so this value does not increase + size_t runs = t_runs; + std::this_thread::sleep_for(100ms); + EXPECT_EQ(runs, t_runs); + t.reset(); + EXPECT_FALSE(t_weak.lock() != nullptr); +} + +TEST_F(TestTimersManager, add_remove_while_thread_running) +{ + auto timers_manager = std::make_shared( + rclcpp::contexts::get_global_default_context()); + + size_t t1_runs = 0; + auto t1 = TimerT::make_shared( + 1ms, + [&t1_runs]() { + t1_runs++; + }, + rclcpp::contexts::get_global_default_context()); + + size_t t2_runs = 0; + auto t2 = TimerT::make_shared( + 1ms, + [&t2_runs]() { + t2_runs++; + }, + rclcpp::contexts::get_global_default_context()); + + // Add timers + timers_manager->add_timer(t1); + + // Start timers thread + timers_manager->start(); + + // After a while remove t1 and add t2 + std::this_thread::sleep_for(50ms); + timers_manager->remove_timer(t1); + size_t tmp_t1 = t1_runs; + timers_manager->add_timer(t2); + + // Wait some more time and then stop + std::this_thread::sleep_for(50ms); + timers_manager->stop(); + + // t1 has stopped running + EXPECT_EQ(tmp_t1, t1_runs); + // t2 is correctly running + EXPECT_LT(1u, t2_runs); +} + +TEST_F(TestTimersManager, infinite_loop) +{ + // This test makes sure that even if timers have a period shorter than the duration + // of their callback the functions never block indefinitely. + + auto timers_manager = std::make_shared( + rclcpp::contexts::get_global_default_context()); + + size_t t1_runs = 0; + auto t1 = TimerT::make_shared( + 1ms, + [&t1_runs]() { + t1_runs++; + std::this_thread::sleep_for(5ms); + }, + rclcpp::contexts::get_global_default_context()); + + size_t t2_runs = 0; + auto t2 = TimerT::make_shared( + 1ms, + [&t2_runs]() { + t2_runs++; + std::this_thread::sleep_for(5ms); + }, + rclcpp::contexts::get_global_default_context()); + + timers_manager->add_timer(t1); + timers_manager->add_timer(t2); + + // Start a timers thread and make sure that we can stop it later + timers_manager->start(); + std::this_thread::sleep_for(50ms); + timers_manager->stop(); + + EXPECT_LT(0u, t1_runs); + EXPECT_LT(0u, t2_runs); +} + +// Validate that cancelling one timer yields no change in behavior for other +// timers. +TEST_F(TestTimersManager, check_one_timer_cancel_doesnt_affect_other_timers) +{ + auto timers_manager = std::make_shared( + rclcpp::contexts::get_global_default_context()); + + std::atomic t1_runs = 0; + const size_t cancel_iter = 5; + std::shared_ptr t1; + // After a while cancel t1. Don't remove it though. + // Simulates typical usage in a Node where a timer is cancelled but not removed, + // since typical users aren't going to mess around with the timer manager. + t1 = TimerT::make_shared( + 1ms, + [ =, &t1_runs, &t1]() { + t1_runs++; + if (t1_runs == cancel_iter) { + t1->cancel(); + } + }, + rclcpp::contexts::get_global_default_context()); + + std::atomic t2_runs = 0; + auto t2 = TimerT::make_shared( + 1ms, + [&t2_runs]() { + t2_runs++; + }, + rclcpp::contexts::get_global_default_context()); + + // Add timers + timers_manager->add_timer(t1); + timers_manager->add_timer(t2); + + // Start timers thread + timers_manager->start(); + + // Wait for t1 to be canceled + auto loop_start_time = std::chrono::high_resolution_clock::now(); + while (!t1->is_canceled()) { + auto now = std::chrono::high_resolution_clock::now(); + if (now - loop_start_time >= std::chrono::seconds(30)) { + FAIL() << "timeout waiting for t1 to be canceled"; + break; + } + std::this_thread::sleep_for(3ms); + } + + EXPECT_TRUE(t1->is_canceled()); + EXPECT_FALSE(t2->is_canceled()); + EXPECT_EQ(t1_runs, cancel_iter); + + // Verify that t2 is still being invoked + const size_t start_t2_runs = t2_runs; + const size_t num_t2_extra_runs = 6; + loop_start_time = std::chrono::high_resolution_clock::now(); + while (t2_runs < start_t2_runs + num_t2_extra_runs) { + auto now = std::chrono::high_resolution_clock::now(); + if (now - loop_start_time >= std::chrono::seconds(30)) { + FAIL() << "timeout waiting for t2 to do some runs"; + break; + } + std::this_thread::sleep_for(3ms); + } + + EXPECT_TRUE(t1->is_canceled()); + EXPECT_FALSE(t2->is_canceled()); + // t1 hasn't run since before + EXPECT_EQ(t1_runs, cancel_iter); + // t2 has run the expected additional number of times + EXPECT_GE(t2_runs, start_t2_runs + num_t2_extra_runs); + // the t2 runs are strictly more than the t1 runs + EXPECT_GT(t2_runs, t1_runs); + + timers_manager->stop(); +} diff --git a/rclcpp/test/rclcpp/test_type_support.cpp b/rclcpp/test/rclcpp/test_type_support.cpp new file mode 100644 index 0000000000..682907e2a9 --- /dev/null +++ b/rclcpp/test/rclcpp/test_type_support.cpp @@ -0,0 +1,202 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/type_support_decl.hpp" +#include "test_msgs/msg/empty.hpp" + +class TestTypeSupport : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + ::testing::AssertionResult test_type_support_init_fini(const rosidl_service_type_support_t * ts) + { + rcl_service_t service_handle = rcl_get_zero_initialized_service(); + rcl_service_options_t service_options = rcl_service_get_default_options(); + auto node_handle = node->get_node_base_interface()->get_rcl_node_handle(); + rcl_ret_t ret = rcl_service_init( + &service_handle, node_handle, ts, "base_node_service", &service_options); + if (ret != RCL_RET_OK) { + return ::testing::AssertionFailure() << + "Failed rcl_service_init with error string: " << rcl_get_error_string().str; + } + ret = rcl_service_fini(&service_handle, node_handle); + if (ret != RCL_RET_OK) { + return ::testing::AssertionFailure() << + "Failed rcl_service_fini with error string: " << rcl_get_error_string().str; + } + return ::testing::AssertionSuccess(); + } + +protected: + rclcpp::Node::SharedPtr node = std::make_shared("my_node", "/ns"); +}; + +const rclcpp::PublisherOptionsWithAllocator> PublisherOptions() +{ + return rclcpp::PublisherOptionsWithAllocator>(); +} + +// Auxiliary classes used to test rosidl_message_type_support_t getters +// defined in type_support.hpp +const rosidl_message_type_support_t * ts_parameter_event = + rclcpp::type_support::get_parameter_event_msg_type_support(); + +class TestTSParameterEvent : public rclcpp::PublisherBase +{ +public: + explicit TestTSParameterEvent(rclcpp::Node * node) + : rclcpp::PublisherBase( + node->get_node_base_interface().get(), + "topicTSParameterEvent", + *ts_parameter_event, + PublisherOptions().to_rcl_publisher_options(rclcpp::QoS(10)), + PublisherOptions().event_callbacks, PublisherOptions().use_default_callbacks) {} +}; + +const rosidl_message_type_support_t * ts_set_parameter_result = + rclcpp::type_support::get_set_parameters_result_msg_type_support(); + +class TestTSSetParameterResult : public rclcpp::PublisherBase +{ +public: + explicit TestTSSetParameterResult(rclcpp::Node * node) + : rclcpp::PublisherBase( + node->get_node_base_interface().get(), + "topicTSSetParameterResult", + *ts_set_parameter_result, + PublisherOptions().to_rcl_publisher_options(rclcpp::QoS(10)), + PublisherOptions().event_callbacks, PublisherOptions().use_default_callbacks) {} +}; + +const rosidl_message_type_support_t * ts_parameter_descriptor = + rclcpp::type_support::get_parameter_descriptor_msg_type_support(); + +class TestTSParameterDescriptor : public rclcpp::PublisherBase +{ +public: + explicit TestTSParameterDescriptor(rclcpp::Node * node) + : rclcpp::PublisherBase( + node->get_node_base_interface().get(), + "topicTSParameterDescriptor", + *ts_parameter_descriptor, + PublisherOptions().to_rcl_publisher_options(rclcpp::QoS(10)), + PublisherOptions().event_callbacks, PublisherOptions().use_default_callbacks) {} +}; + +const rosidl_message_type_support_t * ts_list_parameter_result = + rclcpp::type_support::get_list_parameters_result_msg_type_support(); + +class TestTSListParametersResult : public rclcpp::PublisherBase +{ +public: + explicit TestTSListParametersResult(rclcpp::Node * node) + : rclcpp::PublisherBase( + node->get_node_base_interface().get(), + "topicTSListParametersResult", + *ts_list_parameter_result, + PublisherOptions().to_rcl_publisher_options(rclcpp::QoS(10)), + PublisherOptions().event_callbacks, PublisherOptions().use_default_callbacks) {} +}; + +/* + Test that the publisher is created properly for different msg typesupport + */ +TEST_F(TestTypeSupport, basic_getters) { + { + auto publisher = TestTSParameterEvent(node.get()); + std::shared_ptr publisher_handle = publisher.get_publisher_handle(); + EXPECT_NE(nullptr, publisher_handle); + } + { + auto publisher = TestTSSetParameterResult(node.get()); + std::shared_ptr publisher_handle = publisher.get_publisher_handle(); + EXPECT_NE(nullptr, publisher_handle); + } + { + auto publisher = TestTSParameterDescriptor(node.get()); + std::shared_ptr publisher_handle = publisher.get_publisher_handle(); + EXPECT_NE(nullptr, publisher_handle); + } + { + auto publisher = TestTSListParametersResult(node.get()); + std::shared_ptr publisher_handle = publisher.get_publisher_handle(); + EXPECT_NE(nullptr, publisher_handle); + } +} + +/* Testing type support getters */ +TEST_F(TestTypeSupport, test_service_ts_get_params_srv) { + const rosidl_service_type_support_t * ts = + rclcpp::type_support::get_get_parameters_srv_type_support(); + ASSERT_NE(nullptr, ts); + EXPECT_TRUE(test_type_support_init_fini(ts)); +} + +TEST_F(TestTypeSupport, test_service_ts_get_params_srv_type) { + const rosidl_service_type_support_t * ts = + rclcpp::type_support::get_get_parameters_srv_type_support(); + ASSERT_NE(nullptr, ts); + EXPECT_TRUE(test_type_support_init_fini(ts)); +} + +TEST_F(TestTypeSupport, test_service_ts_get_parameters_types_srv) { + const rosidl_service_type_support_t * ts = + rclcpp::type_support::get_get_parameter_types_srv_type_support(); + ASSERT_NE(nullptr, ts); + EXPECT_TRUE(test_type_support_init_fini(ts)); +} + +TEST_F(TestTypeSupport, test_service_ts_set_params_srv) { + const rosidl_service_type_support_t * ts = + rclcpp::type_support::get_set_parameters_srv_type_support(); + ASSERT_NE(nullptr, ts); + EXPECT_TRUE(test_type_support_init_fini(ts)); +} + +TEST_F(TestTypeSupport, test_service_ts_list_params_srv) { + const rosidl_service_type_support_t * ts = + rclcpp::type_support::get_list_parameters_srv_type_support(); + ASSERT_NE(nullptr, ts); + EXPECT_TRUE(test_type_support_init_fini(ts)); +} + +TEST_F(TestTypeSupport, test_service_ts_describe_params_srv) { + const rosidl_service_type_support_t * ts = + rclcpp::type_support::get_describe_parameters_srv_type_support(); + ASSERT_NE(nullptr, ts); + EXPECT_TRUE(test_type_support_init_fini(ts)); +} + +TEST_F(TestTypeSupport, test_service_ts_set_params_atomically_srv) { + const rosidl_service_type_support_t * ts = + rclcpp::type_support::get_set_parameters_atomically_srv_type_support(); + ASSERT_NE(nullptr, ts); + EXPECT_TRUE(test_type_support_init_fini(ts)); +} diff --git a/rclcpp/test/rclcpp/test_typesupport_helpers.cpp b/rclcpp/test/rclcpp/test_typesupport_helpers.cpp new file mode 100644 index 0000000000..2117b89455 --- /dev/null +++ b/rclcpp/test/rclcpp/test_typesupport_helpers.cpp @@ -0,0 +1,126 @@ +// Copyright 2018, Bosch Software Innovations GmbH. +// Copyright 2021, Apex.AI 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 "rcpputils/shared_library.hpp" + +#include "rclcpp/typesupport_helpers.hpp" + +using namespace ::testing; // NOLINT + +TEST(TypesupportHelpersTest, throws_exception_if_filetype_has_no_type) { + EXPECT_ANY_THROW( + rclcpp::get_typesupport_library("just_a_package_name", "rosidl_typesupport_cpp")); +} + +TEST(TypesupportHelpersTest, throws_exception_if_filetype_has_slash_at_the_start_only) { + EXPECT_ANY_THROW( + rclcpp::get_typesupport_library("/name_with_slash_at_start", "rosidl_typesupport_cpp")); +} + +TEST(TypesupportHelpersTest, throws_exception_if_filetype_has_slash_at_the_end_only) { + EXPECT_ANY_THROW( + rclcpp::get_typesupport_library("name_with_slash_at_end/", "rosidl_typesupport_cpp")); +} + +TEST(TypesupportHelpersTest, throws_exception_if_library_cannot_be_found) { + EXPECT_THROW( + rclcpp::get_typesupport_library("invalid/message", "rosidl_typesupport_cpp"), + std::runtime_error); +} + +TEST(TypesupportHelpersTest, returns_c_type_info_for_valid_legacy_library) { + try { + auto library = rclcpp::get_typesupport_library( + "test_msgs/BasicTypes", "rosidl_typesupport_cpp"); + auto string_typesupport = rclcpp::get_message_typesupport_handle( + "test_msgs/BasicTypes", "rosidl_typesupport_cpp", *library); + + EXPECT_THAT( + std::string(string_typesupport->typesupport_identifier), + ContainsRegex("rosidl_typesupport")); + } catch (const std::exception & e) { + FAIL() << e.what(); + } +} + +TEST(TypesupportHelpersTest, returns_c_type_info_for_valid_library) { + try { + auto library = rclcpp::get_typesupport_library( + "test_msgs/msg/BasicTypes", "rosidl_typesupport_cpp"); + auto string_typesupport = rclcpp::get_message_typesupport_handle( + "test_msgs/msg/BasicTypes", "rosidl_typesupport_cpp", *library); + + EXPECT_THAT( + std::string(string_typesupport->typesupport_identifier), + ContainsRegex("rosidl_typesupport")); + } catch (const std::runtime_error & e) { + FAIL() << e.what(); + } +} + +TEST(TypesupportHelpersTest, returns_service_type_info_for_valid_legacy_library) { + try { + auto library = rclcpp::get_typesupport_library( + "test_msgs/Empty", "rosidl_typesupport_cpp"); + auto empty_typesupport = rclcpp::get_service_typesupport_handle( + "test_msgs/Empty", "rosidl_typesupport_cpp", *library); + + EXPECT_THAT( + std::string(empty_typesupport->typesupport_identifier), + ContainsRegex("rosidl_typesupport")); + } catch (const std::runtime_error & e) { + FAIL() << e.what(); + } +} + +TEST(TypesupportHelpersTest, returns_service_type_info_for_valid_library) { + try { + auto library = rclcpp::get_typesupport_library( + "test_msgs/srv/Empty", "rosidl_typesupport_cpp"); + auto empty_typesupport = rclcpp::get_service_typesupport_handle( + "test_msgs/srv/Empty", "rosidl_typesupport_cpp", *library); + + EXPECT_THAT( + std::string(empty_typesupport->typesupport_identifier), + ContainsRegex("rosidl_typesupport")); + } catch (const std::runtime_error & e) { + FAIL() << e.what(); + } +} + +TEST(TypesupportHelpersTest, test_throw_exception_with_invalid_type) { + // message + std::string invalid_type = "test_msgs/msg/InvalidType"; + auto library = rclcpp::get_typesupport_library(invalid_type, "rosidl_typesupport_cpp"); + EXPECT_THROW( + rclcpp::get_message_typesupport_handle(invalid_type, "rosidl_typesupport_cpp", *library), + std::runtime_error); + EXPECT_THROW( + rclcpp::get_service_typesupport_handle(invalid_type, "rosidl_typesupport_cpp", *library), + std::runtime_error); + + // service + invalid_type = "test_msgs/srv/InvalidType"; + library = rclcpp::get_typesupport_library(invalid_type, "rosidl_typesupport_cpp"); + EXPECT_THROW( + rclcpp::get_service_typesupport_handle(invalid_type, "rosidl_typesupport_cpp", *library), + std::runtime_error); +} diff --git a/rclcpp/test/rclcpp/test_utilities.cpp b/rclcpp/test/rclcpp/test_utilities.cpp new file mode 100644 index 0000000000..b64b0a31c6 --- /dev/null +++ b/rclcpp/test/rclcpp/test_utilities.cpp @@ -0,0 +1,310 @@ +// 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/init.h" +#include "rcl/logging.h" + +#include "rclcpp/contexts/default_context.hpp" +#include "rclcpp/exceptions.hpp" +#include "rclcpp/utilities.hpp" + +#ifdef __clang__ +#pragma clang diagnostic push +#pragma clang diagnostic ignored "-Wordered-compare-function-pointers" +#endif +// TODO(ahcorde): the function mocking_utils::patch_and_return called with +// rcl_logging_configure_with_output_handler is returning: "Comparison between pointer and integer" +// Disabling this warning is fine for now. +// Related issue https://github.com/ros2/rclcpp/issues/2488 +#include "../mocking_utils/patch.hpp" + +#ifdef __clang__ +#pragma clang diagnostic pop +#endif + +#include "../utils/rclcpp_gtest_macros.hpp" + +TEST(TestUtilities, remove_ros_arguments) { + const char * const argv[] = { + "process_name", + "-d", "--ros-args", + "-r", "__ns:=/foo/bar", + "-r", "__ns:=/fiz/buz", + "--", "--foo=bar", "--baz" + }; + int argc = sizeof(argv) / sizeof(const char *); + auto args = rclcpp::remove_ros_arguments(argc, argv); + + ASSERT_EQ(4u, args.size()); + ASSERT_EQ(std::string{"process_name"}, args[0]); + ASSERT_EQ(std::string{"-d"}, args[1]); + ASSERT_EQ(std::string{"--foo=bar"}, args[2]); + ASSERT_EQ(std::string{"--baz"}, args[3]); +} + +TEST(TestUtilities, init_with_args) { + EXPECT_FALSE(rclcpp::signal_handlers_installed()); + const char * const argv[] = {"process_name"}; + int argc = sizeof(argv) / sizeof(const char *); + auto other_args = rclcpp::init_and_remove_ros_arguments(argc, argv); + + ASSERT_EQ(1u, other_args.size()); + ASSERT_EQ(std::string{"process_name"}, other_args[0]); + EXPECT_TRUE(rclcpp::signal_handlers_installed()); + + EXPECT_TRUE(rclcpp::ok()); + rclcpp::shutdown(); +} + +TEST(TestUtilities, init_with_args_contains_ros) { + EXPECT_FALSE(rclcpp::signal_handlers_installed()); + const char * const argv[] = { + "process_name", + "-d", "--ros-args", + "-r", "__ns:=/foo/bar", + "-r", "__ns:=/fiz/buz", + "--", "--foo=bar", "--baz" + }; + int argc = sizeof(argv) / sizeof(const char *); + auto args = rclcpp::init_and_remove_ros_arguments(argc, argv); + + ASSERT_EQ(4u, args.size()); + ASSERT_EQ(std::string{"process_name"}, args[0]); + ASSERT_EQ(std::string{"-d"}, args[1]); + ASSERT_EQ(std::string{"--foo=bar"}, args[2]); + ASSERT_EQ(std::string{"--baz"}, args[3]); + EXPECT_TRUE(rclcpp::signal_handlers_installed()); + + EXPECT_TRUE(rclcpp::ok()); + rclcpp::shutdown(); +} + +TEST(TestUtilities, multi_init) { + auto context1 = std::make_shared(); + auto context2 = std::make_shared(); + + EXPECT_FALSE(rclcpp::ok(context1)); + EXPECT_FALSE(rclcpp::ok(context2)); + + context1->init(0, nullptr); + + EXPECT_TRUE(rclcpp::ok(context1)); + EXPECT_FALSE(rclcpp::ok(context2)); + + context2->init(0, nullptr); + + EXPECT_TRUE(rclcpp::ok(context1)); + EXPECT_TRUE(rclcpp::ok(context2)); + + rclcpp::shutdown(context1); + + EXPECT_FALSE(rclcpp::ok(context1)); + EXPECT_TRUE(rclcpp::ok(context2)); + + rclcpp::shutdown(context2); + + EXPECT_FALSE(rclcpp::ok(context1)); + EXPECT_FALSE(rclcpp::ok(context2)); +} + +TEST(TestUtilities, test_pre_shutdown_callback_add_remove) { + auto context1 = std::make_shared(); + context1->init(0, nullptr); + + bool is_called1 = false; + bool is_called2 = false; + auto callback1 = [&is_called1]() {is_called1 = true;}; + auto callback2 = [&is_called2]() {is_called2 = true;}; + + EXPECT_EQ(0u, context1->get_pre_shutdown_callbacks().size()); + + rclcpp::PreShutdownCallbackHandle callback_handle1 = + context1->add_pre_shutdown_callback(callback1); + EXPECT_EQ(1u, context1->get_pre_shutdown_callbacks().size()); + + rclcpp::PreShutdownCallbackHandle callback_handle2 = + context1->add_pre_shutdown_callback(callback2); + EXPECT_EQ(2u, context1->get_pre_shutdown_callbacks().size()); + + rclcpp::PreShutdownCallbackHandle wrong_callback_handle; + EXPECT_FALSE(context1->remove_pre_shutdown_callback(wrong_callback_handle)); + + EXPECT_TRUE(context1->remove_pre_shutdown_callback(callback_handle1)); + EXPECT_EQ(1u, context1->get_pre_shutdown_callbacks().size()); + + rclcpp::shutdown(context1); + + EXPECT_FALSE(is_called1); + EXPECT_TRUE(is_called2); +} + +TEST(TestUtilities, test_context_basic_access) { + auto context1 = std::make_shared(); + EXPECT_NE(nullptr, context1->get_init_options().get_rcl_init_options()); + EXPECT_EQ(0u, context1->get_on_shutdown_callbacks().size()); + EXPECT_EQ(0u, context1->get_pre_shutdown_callbacks().size()); + EXPECT_EQ(std::string{""}, context1->shutdown_reason()); +} + +TEST(TestUtilities, test_context_basic_access_const_methods) { + auto context1 = std::make_shared(); + + EXPECT_NE(nullptr, context1->get_init_options().get_rcl_init_options()); + EXPECT_EQ(0u, context1->get_on_shutdown_callbacks().size()); + EXPECT_EQ(0u, context1->get_pre_shutdown_callbacks().size()); +} + +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, ==) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, !=) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, >) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, <) + +TEST(TestUtilities, test_context_init_shutdown_fails) { + { + auto context_fail_init = std::make_shared(); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_init, RCL_RET_ERROR); + EXPECT_THROW(context_fail_init->init(0, nullptr), rclcpp::exceptions::RCLError); + EXPECT_FALSE(context_fail_init->is_valid()); + } + + { + auto context_fail_init = std::make_shared(); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_logging_configure_with_output_handler, RCL_RET_ERROR); + EXPECT_THROW(context_fail_init->init(0, nullptr), rclcpp::exceptions::RCLError); + EXPECT_FALSE(context_fail_init->is_valid()); + } + + { + auto context = std::make_shared(); + context->init(0, nullptr); + auto mock = mocking_utils::inject_on_return( + "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); + // This will log a message, no throw expected + EXPECT_NO_THROW(context->shutdown("")); + } + + { + auto context = std::make_shared(); + context->init(0, nullptr); + auto mock = mocking_utils::inject_on_return( + "lib:rclcpp", rcl_shutdown, RCL_RET_ERROR); + EXPECT_THROW(context->shutdown(""), rclcpp::exceptions::RCLError); + } + + { + auto context = std::make_shared(); + context->init(0, nullptr); + auto mock = mocking_utils::inject_on_return( + "lib:rclcpp", rcl_logging_fini, RCL_RET_ERROR); + // This will log a message, no throw expected + EXPECT_NO_THROW(context->shutdown("")); + } + + { + auto context_to_destroy = std::make_shared(); + auto mock = mocking_utils::inject_on_return( + "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); + // This will log a message, no throw expected + EXPECT_NO_THROW({context_to_destroy.reset();}); + } +} + +// Required for mocking_utils below +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(TestUtilities, remove_ros_arguments_rcl_parse_arguments_failed) { + const char * const argv[] = { + "process_name", + "-d", "--ros-args", + "-r", "__ns:=/foo/bar", + "-r", "__ns:=/fiz/buz", + "--", "--foo=bar", "--baz" + }; + int argc = sizeof(argv) / sizeof(const char *); + + auto mock = mocking_utils::inject_on_return( + "lib:rclcpp", rcl_parse_arguments, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + rclcpp::remove_ros_arguments(argc, argv), + rclcpp::exceptions::RCLError( + RCL_RET_ERROR, rcl_get_error_state(), "failed to parse arguments")); +} + +TEST(TestUtilities, remove_ros_arguments_rcl_remove_ros_arguments_failed) { + const char * const argv[] = { + "process_name", + "-d", "--ros-args", + "-r", "__ns:=/foo/bar", + "-r", "__ns:=/fiz/buz", + "--", "--foo=bar", "--baz" + }; + int argc = sizeof(argv) / sizeof(const char *); + + auto mock = mocking_utils::inject_on_return( + "lib:rclcpp", rcl_remove_ros_arguments, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + rclcpp::remove_ros_arguments(argc, argv), + rclcpp::exceptions::RCLError( + RCL_RET_ERROR, rcl_get_error_state(), "")); +} + +TEST(TestUtilities, remove_ros_arguments_rcl_remove_ros_arguments_failed_and_fini) { + const char * const argv[] = { + "process_name", + "-d", "--ros-args", + "-r", "__ns:=/foo/bar", + "-r", "__ns:=/fiz/buz", + "--", "--foo=bar", "--baz" + }; + int argc = sizeof(argv) / sizeof(const char *); + + auto mock = mocking_utils::inject_on_return( + "lib:rclcpp", rcl_remove_ros_arguments, RCL_RET_ERROR); + auto mock2 = mocking_utils::inject_on_return( + "lib:rclcpp", rcl_arguments_fini, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + rclcpp::remove_ros_arguments(argc, argv), + rclcpp::exceptions::RCLError( + RCL_RET_ERROR, rcl_get_error_state(), + ", failed also to cleanup parsed arguments, leaking memory: ")); +} + +TEST(TestUtilities, remove_ros_arguments_rcl_arguments_fini_failed) { + const char * const argv[] = { + "process_name", + "-d", "--ros-args", + "-r", "__ns:=/foo/bar", + "-r", "__ns:=/fiz/buz", + "--", "--foo=bar", "--baz" + }; + int argc = sizeof(argv) / sizeof(const char *); + + auto mock = mocking_utils::inject_on_return( + "lib:rclcpp", rcl_arguments_fini, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + rclcpp::remove_ros_arguments(argc, argv), + rclcpp::exceptions::RCLError( + RCL_RET_ERROR, rcl_get_error_state(), "failed to cleanup parsed arguments, leaking memory")); +} diff --git a/rclcpp/test/rclcpp/test_wait_for_message.cpp b/rclcpp/test/rclcpp/test_wait_for_message.cpp new file mode 100644 index 0000000000..dadc06f2da --- /dev/null +++ b/rclcpp/test/rclcpp/test_wait_for_message.cpp @@ -0,0 +1,109 @@ +// 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 "rclcpp/node.hpp" +#include "rclcpp/wait_for_message.hpp" + +#include "test_msgs/msg/strings.hpp" +#include "test_msgs/message_fixtures.hpp" + +using namespace std::chrono_literals; + +TEST(TestUtilities, wait_for_message) { + rclcpp::init(0, nullptr); + + auto node = std::make_shared("wait_for_message_node"); + + using MsgT = test_msgs::msg::Strings; + auto pub = node->create_publisher("wait_for_message_topic", 10); + + MsgT out; + auto received = false; + auto wait = std::async( + [&]() { + auto ret = rclcpp::wait_for_message(out, node, "wait_for_message_topic", 5s); + EXPECT_TRUE(ret); + received = true; + }); + + for (auto i = 0u; i < 10 && received == false; ++i) { + pub->publish(*get_messages_strings()[0]); + std::this_thread::sleep_for(1s); + } + ASSERT_TRUE(received); + EXPECT_EQ(out, *get_messages_strings()[0]); + + rclcpp::shutdown(); +} + +TEST(TestUtilities, wait_for_message_indefinitely) { + rclcpp::init(0, nullptr); + + auto node = std::make_shared("wait_for_message_node2"); + + using MsgT = test_msgs::msg::Strings; + MsgT out; + auto received = false; + auto wait = std::async( + [&]() { + auto ret = rclcpp::wait_for_message(out, node, "wait_for_message_topic" /*, -1 */); + EXPECT_TRUE(ret); + received = true; + }); + + rclcpp::shutdown(); + + ASSERT_FALSE(received); +} + +TEST(TestUtilities, wait_for_message_twice_one_sub) { + rclcpp::init(0, nullptr); + + auto node = std::make_shared("wait_for_message_node3"); + + using MsgT = test_msgs::msg::Strings; + auto pub = node->create_publisher("wait_for_message_topic", 10); + auto sub = node->create_subscription( + "wait_for_message_topic", 1, [](const std::shared_ptr) {}); + + MsgT out1; + MsgT out2; + auto received = false; + auto wait = std::async( + [&]() { + auto ret = rclcpp::wait_for_message(out1, sub, node->get_node_options().context(), 5s); + EXPECT_TRUE(ret); + ret = rclcpp::wait_for_message(out2, sub, node->get_node_options().context(), 5s); + EXPECT_TRUE(ret); + received = true; + }); + + for (auto i = 0u; i < 10 && received == false; ++i) { + pub->publish(*get_messages_strings()[0]); + std::this_thread::sleep_for(1s); + } + + ASSERT_NO_THROW(wait.get()); + ASSERT_TRUE(received); + EXPECT_EQ(out1, *get_messages_strings()[0]); + EXPECT_EQ(out2, *get_messages_strings()[0]); + + rclcpp::shutdown(); +} diff --git a/rclcpp/test/rclcpp/test_wait_set.cpp b/rclcpp/test/rclcpp/test_wait_set.cpp new file mode 100644 index 0000000000..78e102f390 --- /dev/null +++ b/rclcpp/test/rclcpp/test_wait_set.cpp @@ -0,0 +1,366 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include + +#include "rcl_interfaces/srv/list_parameters.hpp" +#include "rclcpp/rclcpp.hpp" +#include "test_msgs/msg/basic_types.hpp" + +#include "../utils/rclcpp_gtest_macros.hpp" + +class TestWaitSet : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } +}; + +/* + * Testing normal construction and destruction. + */ +TEST_F(TestWaitSet, construction_and_destruction) { + { + rclcpp::WaitSet wait_set; + (void)wait_set; + } + + { + rclcpp::WaitSet wait_set( + std::vector{}, + std::vector{}, + std::vector{}, + std::vector{}, + std::vector{}, + std::vector{}); + (void)wait_set; + } + + { + auto gc = std::make_shared(); + rclcpp::WaitSet wait_set({}, {gc}); + (void)wait_set; + } + + { + auto context = std::make_shared(); + context->init(0, nullptr); + auto gc = std::make_shared(context); + rclcpp::WaitSet wait_set({}, {gc}, {}, {}, {}, {}, context); + (void)wait_set; + } + + { + // invalid context (nullptr) + ASSERT_THROW( + { + rclcpp::WaitSet wait_set( + std::vector{}, + std::vector{}, + std::vector{}, + std::vector{}, + std::vector{}, + std::vector{}, + nullptr); + (void)wait_set; + }, std::invalid_argument); + } + + { + // invalid context (uninitialized) + auto context = std::make_shared(); + ASSERT_THROW( + { + rclcpp::WaitSet wait_set( + std::vector{}, + std::vector{}, + std::vector{}, + std::vector{}, + std::vector{}, + std::vector{}, + context); + (void)wait_set; + }, rclcpp::exceptions::RCLInvalidArgument); + } +} + +/* + * Testing rcl wait set accessor. + */ +TEST_F(TestWaitSet, get_rcl_wait_set) { + { + rclcpp::WaitSet wait_set; + wait_set.get_rcl_wait_set(); + } +} + +/* + * Testing add/remove for guard condition methods. + */ +TEST_F(TestWaitSet, add_remove_guard_condition) { + // normal, mixed initialization + { + auto gc = std::make_shared(); + auto gc2 = std::make_shared(); + rclcpp::WaitSet wait_set({}, {gc}); + wait_set.add_guard_condition(gc2); + wait_set.remove_guard_condition(gc2); + wait_set.remove_guard_condition(gc); + } + + // out of order removal + { + auto gc = std::make_shared(); + auto gc2 = std::make_shared(); + rclcpp::WaitSet wait_set({}, {gc}); + wait_set.add_guard_condition(gc2); + wait_set.remove_guard_condition(gc); + wait_set.remove_guard_condition(gc2); + } + + // start empty, normal + { + auto gc = std::make_shared(); + rclcpp::WaitSet wait_set; + wait_set.add_guard_condition(gc); + wait_set.remove_guard_condition(gc); + } + + // add invalid (nullptr) + { + rclcpp::WaitSet wait_set; + ASSERT_THROW( + { + wait_set.add_guard_condition(nullptr); + }, std::invalid_argument); + } + + // double add + { + auto gc = std::make_shared(); + rclcpp::WaitSet wait_set; + wait_set.add_guard_condition(gc); + ASSERT_THROW( + { + wait_set.add_guard_condition(gc); + }, std::runtime_error); + } + + // remove invalid (nullptr) + { + rclcpp::WaitSet wait_set; + ASSERT_THROW( + { + wait_set.remove_guard_condition(nullptr); + }, std::invalid_argument); + } + + // remove unrelated + { + auto gc = std::make_shared(); + auto gc2 = std::make_shared(); + rclcpp::WaitSet wait_set({}, {gc}); + ASSERT_THROW( + { + wait_set.remove_guard_condition(gc2); + }, std::runtime_error); + } + + // double remove + { + auto gc = std::make_shared(); + rclcpp::WaitSet wait_set({}, {gc}); + wait_set.remove_guard_condition(gc); + ASSERT_THROW( + { + wait_set.remove_guard_condition(gc); + }, std::runtime_error); + } + + // remove from empty + { + auto gc = std::make_shared(); + rclcpp::WaitSet wait_set; + ASSERT_THROW( + { + wait_set.remove_guard_condition(gc); + }, std::runtime_error); + } +} + +/* + * Testing adding each entity to two separate wait sets. + */ +TEST_F(TestWaitSet, add_guard_condition_to_two_different_wait_set) { + { + rclcpp::WaitSet wait_set1; + rclcpp::WaitSet wait_set2; + auto node = std::make_shared("add_guard_condition_to_two_different_wait_set"); + + auto guard_condition = std::make_shared(); + wait_set1.add_guard_condition(guard_condition); + ASSERT_THROW( + { + wait_set2.add_guard_condition(guard_condition); + }, std::runtime_error); + + auto do_nothing = [](std::shared_ptr) {}; + auto sub = node->create_subscription("~/test", 1, do_nothing); + wait_set1.add_subscription(sub); + ASSERT_THROW( + { + wait_set2.add_subscription(sub); + }, std::runtime_error); + + auto timer = node->create_wall_timer(std::chrono::seconds(1), []() {}); + wait_set1.add_timer(timer); + ASSERT_THROW( + { + wait_set2.add_timer(timer); + }, std::runtime_error); + + auto client = node->create_client("~/test"); + wait_set1.add_client(client); + ASSERT_THROW( + { + wait_set2.add_client(client); + }, std::runtime_error); + + auto srv_do_nothing = []( + const std::shared_ptr, + std::shared_ptr) {}; + auto service = + node->create_service("~/test", srv_do_nothing); + wait_set1.add_service(service); + ASSERT_THROW( + { + wait_set2.add_service(service); + }, std::runtime_error); + + rclcpp::PublisherOptions po; + po.event_callbacks.deadline_callback = [](rclcpp::QOSDeadlineOfferedInfo &) {}; + auto pub = node->create_publisher("~/test", 1, po); + auto qos_event = pub->get_event_handlers().begin()->second; + wait_set1.add_waitable(qos_event, pub); + ASSERT_THROW( + { + wait_set2.add_waitable(qos_event, pub); + }, std::runtime_error); + } +} + + +/* + * Testing adding each entity and waiting, and removing each entity and waiting + */ +TEST_F(TestWaitSet, add_remove_wait) { + rclcpp::WaitSet wait_set; + auto node = std::make_shared("add_remove_wait"); + + auto guard_condition = std::make_shared(); + guard_condition->trigger(); + + // For coverage reasons, this subscription should have event handlers + rclcpp::SubscriptionOptions subscription_options; + subscription_options.event_callbacks.deadline_callback = [](auto) {}; + subscription_options.event_callbacks.liveliness_callback = [](auto) {}; + auto do_nothing = [](std::shared_ptr) {}; + auto sub = + node->create_subscription( + "~/test", 1, do_nothing, subscription_options); + + auto timer = node->create_wall_timer(std::chrono::milliseconds(1), []() {}); + + auto client = node->create_client("~/test"); + + auto srv_do_nothing = []( + const std::shared_ptr, + std::shared_ptr) {}; + auto service = + node->create_service("~/test", srv_do_nothing); + + rclcpp::PublisherOptions publisher_options; + publisher_options.event_callbacks.deadline_callback = + [](rclcpp::QOSDeadlineOfferedInfo &) {}; + auto pub = node->create_publisher( + "~/test", 1, publisher_options); + auto qos_event = pub->get_event_handlers().begin()->second; + + // Subscription mask is required here for coverage. + wait_set.add_subscription(sub, {true, true, true}); + wait_set.add_guard_condition(guard_condition); + wait_set.add_timer(timer); + wait_set.add_client(client); + wait_set.add_service(service); + wait_set.add_waitable(qos_event, pub); + + // At least timer or guard_condition should trigger + EXPECT_EQ(rclcpp::WaitResultKind::Ready, wait_set.wait(std::chrono::seconds(1)).kind()); + + wait_set.remove_subscription(sub, {true, true, true}); + wait_set.remove_guard_condition(guard_condition); + wait_set.remove_timer(timer); + wait_set.remove_client(client); + wait_set.remove_service(service); + wait_set.remove_waitable(qos_event); + + EXPECT_EQ(rclcpp::WaitResultKind::Empty, wait_set.wait(std::chrono::seconds(1)).kind()); +} + +/* + * Get wait_set from result. + */ +TEST_F(TestWaitSet, get_result_from_wait_result) { + rclcpp::WaitSet wait_set; + auto guard_condition = std::make_shared(); + wait_set.add_guard_condition(guard_condition); + guard_condition->trigger(); + + rclcpp::WaitResult result = wait_set.wait(); + ASSERT_EQ(rclcpp::WaitResultKind::Ready, result.kind()); + EXPECT_EQ(&wait_set, &result.get_wait_set()); + + const rclcpp::WaitResult const_result(std::move(result)); + ASSERT_EQ(rclcpp::WaitResultKind::Ready, const_result.kind()); + EXPECT_EQ(&wait_set, &const_result.get_wait_set()); +} + +TEST_F(TestWaitSet, get_result_from_wait_result_not_ready_error) { + rclcpp::WaitSet wait_set; + auto guard_condition = std::make_shared(); + wait_set.add_guard_condition(guard_condition); + + rclcpp::WaitResult result = wait_set.wait(std::chrono::milliseconds(10)); + ASSERT_EQ(rclcpp::WaitResultKind::Timeout, result.kind()); + RCLCPP_EXPECT_THROW_EQ( + result.get_wait_set(), + std::runtime_error("cannot access wait set when the result was not ready")); + + const rclcpp::WaitResult const_result(std::move(result)); + ASSERT_EQ(rclcpp::WaitResultKind::Timeout, const_result.kind()); + RCLCPP_EXPECT_THROW_EQ( + const_result.get_wait_set(), + std::runtime_error("cannot access wait set when the result was not ready")); +} diff --git a/rclcpp/test/rclcpp/topic_statistics/test_subscription_topic_statistics.cpp b/rclcpp/test/rclcpp/topic_statistics/test_subscription_topic_statistics.cpp new file mode 100644 index 0000000000..a6fbbcbc74 --- /dev/null +++ b/rclcpp/test/rclcpp/topic_statistics/test_subscription_topic_statistics.cpp @@ -0,0 +1,423 @@ +// Copyright 2020 Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "libstatistics_collector/moving_average_statistics/types.hpp" + +#include "rclcpp/create_publisher.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/subscription_options.hpp" + +#include "rclcpp/topic_statistics/subscription_topic_statistics.hpp" + +#include "statistics_msgs/msg/metrics_message.hpp" +#include "statistics_msgs/msg/statistic_data_type.hpp" + +#include "test_msgs/msg/empty.hpp" +#include "test_msgs/msg/strings.hpp" + +#include "test_topic_stats_utils.hpp" + +namespace +{ +constexpr const std::chrono::seconds defaultStatisticsPublishPeriod{1}; +constexpr const char kTestPubNodeName[]{"test_pub_stats_node"}; +constexpr const char kTestSubNodeName[]{"test_sub_stats_node"}; +constexpr const char kTestSubStatsTopic[]{"/test_sub_stats_topic"}; +constexpr const char kTestSubStatsEmptyTopic[]{"/test_sub_stats_empty_topic"}; +constexpr const char kTestTopicStatisticsTopic[]{"/test_topic_statistics_topic"}; +constexpr const char kMessageAgeSourceLabel[]{"message_age"}; +constexpr const char kMessagePeriodSourceLabel[]{"message_period"}; +constexpr const uint64_t kNoSamples{0}; +constexpr const std::chrono::seconds kTestTimeout{10}; +constexpr const uint64_t kNumExpectedWindows{4}; +constexpr const uint64_t kNumExpectedMessages{kNumExpectedWindows * 2}; +constexpr const uint64_t kNumExpectedMessageAgeMessages{kNumExpectedWindows}; +constexpr const uint64_t kNumExpectedMessagePeriodMessages{kNumExpectedWindows}; +constexpr const std::chrono::seconds kUnstableMessageAgeWindowDuration{ + defaultStatisticsPublishPeriod * (kNumExpectedWindows / 2)}; +// kUnstableMessageAgeWindowDuration can take following value. +// Min: defaultStatisticsPublishPeriod * 2 +// Max: defaultStatisticsPublishPeriod * (kNumExpectedWindows - 2) +constexpr const std::chrono::seconds kUnstableMessageAgeOffset{std::chrono::seconds{1}}; +} // namespace + +using test_msgs::msg::Empty; +using rclcpp::topic_statistics::SubscriptionTopicStatistics; +using statistics_msgs::msg::MetricsMessage; +using statistics_msgs::msg::StatisticDataPoint; +using statistics_msgs::msg::StatisticDataType; +using libstatistics_collector::moving_average_statistics::StatisticData; + +/** + * Wrapper class to test and expose parts of the SubscriptionTopicStatistics class. + */ +class TestSubscriptionTopicStatistics : public SubscriptionTopicStatistics +{ +public: + TestSubscriptionTopicStatistics( + const std::string & node_name, + rclcpp::Publisher::SharedPtr publisher) + : SubscriptionTopicStatistics(node_name, std::move(publisher)) + { + } + + ~TestSubscriptionTopicStatistics() override = default; + + /// Exposed for testing + using SubscriptionTopicStatistics::get_current_collector_data; +}; + +/** + * PublisherNode wrapper: used to create publisher node + */ +template +class PublisherNode : public rclcpp::Node +{ +public: + PublisherNode( + const std::string & name, const std::string & topic, + const std::chrono::milliseconds & publish_period = std::chrono::milliseconds{100}) + : Node(name) + { + publisher_ = create_publisher(topic, 10); + publish_timer_ = this->create_wall_timer( + publish_period, [this]() { + this->publish_message(); + }); + } + + ~PublisherNode() override = default; + +private: + void publish_message() + { + auto msg = MessageT{}; + publisher_->publish(msg); + } + + typename rclcpp::Publisher::SharedPtr publisher_; + rclcpp::TimerBase::SharedPtr publish_timer_; +}; + +/** + * TransitionMessageStamp publisher emulator node : used to emulate publishing messages by + * directly calling rclcpp::Subscription::handle_message(msg_shared_ptr, message_info). + * The message age results change during the test. + */ +template +class TransitionMessageStampPublisherEmulator : public rclcpp::Node +{ +public: + TransitionMessageStampPublisherEmulator( + const std::string & name, + const std::chrono::seconds transition_duration, const std::chrono::seconds message_age_offset, + typename rclcpp::Subscription::SharedPtr subscription, + const std::chrono::milliseconds & publish_period = std::chrono::milliseconds{100}) + : Node(name), transition_duration_(transition_duration), message_age_offset_(message_age_offset), + subscription_(std::move(subscription)) + { + publish_timer_ = this->create_wall_timer(publish_period, [this]() {this->publish_message();}); + start_time_ = this->now(); + } + +private: + void publish_message() + { + std::shared_ptr msg_shared_ptr = std::make_shared(); + rmw_message_info_t rmw_message_info = rmw_get_zero_initialized_message_info(); + + auto now = this->now(); + auto elapsed_time = now - start_time_; + if (elapsed_time < transition_duration_) { + // Apply only to the topic statistics in the first half + // Subtract offset so message_age is always >= offset. + rmw_message_info.source_timestamp = (now - message_age_offset_).nanoseconds(); + } else { + rmw_message_info.source_timestamp = now.nanoseconds(); + } + rclcpp::MessageInfo message_info{rmw_message_info}; + subscription_->handle_message(msg_shared_ptr, message_info); + } + + std::chrono::seconds transition_duration_; + std::chrono::seconds message_age_offset_; + typename rclcpp::Subscription::SharedPtr subscription_; + rclcpp::Time start_time_; + rclcpp::TimerBase::SharedPtr publish_timer_; +}; + +/** + * Message subscriber node: used to create subscriber with enabled topic statistics collectors + * + */ +template +class SubscriberWithTopicStatistics : public rclcpp::Node +{ +public: + SubscriberWithTopicStatistics( + const std::string & name, const std::string & topic, + std::chrono::milliseconds publish_period = defaultStatisticsPublishPeriod) + : Node(name) + { + // Manually enable topic statistics via options + auto options = rclcpp::SubscriptionOptions(); + options.topic_stats_options.state = rclcpp::TopicStatisticsState::Enable; + options.topic_stats_options.publish_period = publish_period; + + auto callback = [](typename MessageT::UniquePtr msg) { + (void) msg; + }; + subscription_ = create_subscription>( + topic, + rclcpp::QoS(rclcpp::KeepAll()), + callback, + options); + } + ~SubscriberWithTopicStatistics() override = default; + + typename rclcpp::Subscription::SharedPtr get_subscription() + { + return subscription_; + } + +private: + typename rclcpp::Subscription::SharedPtr subscription_; +}; + +/** + * Test fixture to bring up and teardown rclcpp + */ +class TestSubscriptionTopicStatisticsFixture : public ::testing::Test +{ +protected: + void SetUp() override + { + rclcpp::init(0 /* argc */, nullptr /* argv */); + } + + void TearDown() override + { + rclcpp::shutdown(); + } +}; + +/** + * Check if a received statistics message observed data and contains some calculation + * \param message_to_check + */ +void check_if_statistic_message_is_populated(const MetricsMessage & message_to_check) +{ + for (const auto & stats_point : message_to_check.statistics) { + const auto type = stats_point.data_type; + switch (type) { + case StatisticDataType::STATISTICS_DATA_TYPE_SAMPLE_COUNT: + EXPECT_LT(0, stats_point.data) << "unexpected sample count " << stats_point.data; + break; + case StatisticDataType::STATISTICS_DATA_TYPE_AVERAGE: + EXPECT_LT(0, stats_point.data) << "unexpected avg " << stats_point.data; + break; + case StatisticDataType::STATISTICS_DATA_TYPE_MINIMUM: + EXPECT_LT(0, stats_point.data) << "unexpected min " << stats_point.data; + break; + case StatisticDataType::STATISTICS_DATA_TYPE_MAXIMUM: + EXPECT_LT(0, stats_point.data) << "unexpected max " << stats_point.data; + break; + case StatisticDataType::STATISTICS_DATA_TYPE_STDDEV: + EXPECT_LT(0, stats_point.data) << "unexpected stddev " << stats_point.data; + break; + default: + FAIL() << "received unknown statistics type: " << std::dec << + static_cast(type); + } + } +} + +/** + * Test an invalid argument is thrown for a bad input publish period. + */ +TEST_F(TestSubscriptionTopicStatisticsFixture, test_invalid_publish_period) +{ + ASSERT_THROW( + SubscriberWithTopicStatistics( + "test_period_node", "should_throw_invalid_arg", std::chrono::milliseconds(0) + ), + std::invalid_argument); +} + +/** + * Test that we can manually construct the subscription topic statistics utility class + * without any errors and defaults to empty measurements. + */ +TEST_F(TestSubscriptionTopicStatisticsFixture, test_manual_construction) +{ + auto empty_subscriber = std::make_shared>( + kTestSubNodeName, + kTestSubStatsEmptyTopic); + + // Manually create publisher tied to the node + auto topic_stats_publisher = + empty_subscriber->create_publisher( + kTestTopicStatisticsTopic, + 10); + + // Construct a separate instance + auto sub_topic_stats = std::make_unique( + empty_subscriber->get_name(), + topic_stats_publisher); + + // Expect no data has been collected / no samples received + for (const auto & data : sub_topic_stats->get_current_collector_data()) { + EXPECT_TRUE(std::isnan(data.average)); + EXPECT_TRUE(std::isnan(data.min)); + EXPECT_TRUE(std::isnan(data.max)); + EXPECT_TRUE(std::isnan(data.standard_deviation)); + EXPECT_EQ(kNoSamples, data.sample_count); + } +} + +/** + * Publish messages that do not have a header timestamp, test that all statistics messages + * were received, and verify the statistics message contents. + */ +TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_no_header) +{ + // Create an empty publisher + auto empty_publisher = std::make_shared>( + kTestPubNodeName, + kTestSubStatsEmptyTopic); + // empty_subscriber has a topic statistics instance as part of its subscription + // this will listen to and generate statistics for the empty message + + // Create a listener for topic statistics messages + auto statistics_listener = std::make_shared( + "test_receive_single_empty_stats_message_listener", + "/statistics", + kNumExpectedMessages); + + auto empty_subscriber = std::make_shared>( + kTestSubNodeName, + kTestSubStatsEmptyTopic); + + rclcpp::executors::SingleThreadedExecutor ex; + ex.add_node(empty_publisher); + ex.add_node(statistics_listener); + ex.add_node(empty_subscriber); + + // Spin and get future + ex.spin_until_future_complete(statistics_listener->GetFuture(), kTestTimeout); + + // Compare message counts, sample count should be the same as published and received count + EXPECT_EQ(kNumExpectedMessages, statistics_listener->GetNumberOfMessagesReceived()); + + // Check the received message total count + const auto received_messages = statistics_listener->GetReceivedMessages(); + EXPECT_EQ(kNumExpectedMessages, received_messages.size()); + + // check the type of statistics that were received and their counts + uint64_t message_age_count{0}; + uint64_t message_period_count{0}; + + for (const auto & msg : received_messages) { + if (msg.metrics_source == kMessageAgeSourceLabel) { + message_age_count++; + } + if (msg.metrics_source == kMessagePeriodSourceLabel) { + message_period_count++; + } + } + EXPECT_EQ(kNumExpectedMessageAgeMessages, message_age_count); + EXPECT_EQ(kNumExpectedMessagePeriodMessages, message_period_count); + + // Check the collected statistics for message period. + for (const auto & msg : received_messages) { + check_if_statistic_message_is_populated(msg); + } +} + +TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_include_window_reset) +{ + // msg_subscriber_with_topic_statistics has a topic statistics instance as part of its + // subscription this will listen to and generate statistics + auto msg_subscriber_with_topic_statistics = + std::make_shared>( + kTestSubNodeName, + kTestSubStatsTopic); + + // Create a message publisher + auto msg_publisher = + std::make_shared>( + kTestPubNodeName, kUnstableMessageAgeWindowDuration, + kUnstableMessageAgeOffset, msg_subscriber_with_topic_statistics->get_subscription()); + + // Create a listener for topic statistics messages + auto statistics_listener = std::make_shared( + "test_receive_stats_include_window_reset", "/statistics", kNumExpectedMessages); + + rclcpp::executors::SingleThreadedExecutor ex; + ex.add_node(msg_publisher); + ex.add_node(statistics_listener); + ex.add_node(msg_subscriber_with_topic_statistics); + + // Spin and get future + ex.spin_until_future_complete(statistics_listener->GetFuture(), kTestTimeout); + + const auto received_messages = statistics_listener->GetReceivedMessages(); + EXPECT_EQ(kNumExpectedMessages, received_messages.size()); + + auto message_age_offset = + std::chrono::duration(kUnstableMessageAgeOffset).count(); + + // Check that the first statistic contains the offset inside of its window + auto head_message = received_messages[0]; + for (const auto & stats_point : head_message.statistics) { + const auto type = stats_point.data_type; + switch (type) { + case StatisticDataType::STATISTICS_DATA_TYPE_MINIMUM: + case StatisticDataType::STATISTICS_DATA_TYPE_MAXIMUM: + EXPECT_GE(stats_point.data, message_age_offset); + break; + default: + break; + } + } + + // Check that the last statistic does not contain the offset outside of its window + auto tail_message = received_messages[received_messages.size() - 1]; + for (const auto & stats_point : tail_message.statistics) { + const auto type = stats_point.data_type; + switch (type) { + case StatisticDataType::STATISTICS_DATA_TYPE_MINIMUM: + case StatisticDataType::STATISTICS_DATA_TYPE_MAXIMUM: + EXPECT_LT(stats_point.data, message_age_offset); + break; + default: + break; + } + } +} diff --git a/rclcpp/test/rclcpp/topic_statistics/test_topic_stats_utils.hpp b/rclcpp/test/rclcpp/topic_statistics/test_topic_stats_utils.hpp new file mode 100644 index 0000000000..12c8514586 --- /dev/null +++ b/rclcpp/test/rclcpp/topic_statistics/test_topic_stats_utils.hpp @@ -0,0 +1,152 @@ +// Copyright 2020 Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include +#include + +#include "statistics_msgs/msg/metrics_message.hpp" + +#ifndef RCLCPP__TOPIC_STATISTICS__TEST_TOPIC_STATS_UTILS_HPP_ +#define RCLCPP__TOPIC_STATISTICS__TEST_TOPIC_STATS_UTILS_HPP_ + +namespace rclcpp +{ +namespace topic_statistics +{ + +using statistics_msgs::msg::MetricsMessage; + +/** +* Provide an interface to wait for a promise to be satisfied via its future. +*/ +class PromiseSetter +{ +public: + /** + * Reassign the promise member and return it's future. Acquires a mutex in order + * to mutate member variables. + * + * \return the promise member's future, called upon PeriodicMeasurement + */ + std::shared_future GetFuture() + { + std::unique_lock ulock{mutex_}; + use_future_ = true; + promise_ = std::promise(); + return promise_.get_future(); + } + +protected: + /** + * Set the promise to true, which signals the corresponding future. Acquires a mutex and sets + * the promise to true iff GetFuture was invoked before this. + */ + void SetPromise() + { + std::unique_lock ulock{mutex_}; + if (use_future_) { + // only set if GetFuture was called + promise_.set_value(true); + use_future_ = false; // the promise needs to be reassigned to set again + } + } + +private: + mutable std::mutex mutex_; + std::promise promise_; + bool use_future_{false}; +}; + +/** + * Node which listens for published MetricsMessages. This uses the PromiseSetter API + * in order to signal, via a future, that rclcpp should stop spinning upon + * message handling. + */ +class MetricsMessageSubscriber : public rclcpp::Node, public PromiseSetter +{ +public: + /** + * Constructs a MetricsMessageSubscriber. + * \param name the node name + * \param name the topic name + * \param number of messages to receive to trigger the PromiseSetter future + */ + MetricsMessageSubscriber( + const std::string & name, + const std::string & topic_name, + const uint64_t number_of_messages_to_receive = 2) + : rclcpp::Node(name), + number_of_messages_to_receive_(number_of_messages_to_receive) + { + auto callback = [this](MetricsMessage::UniquePtr msg) { + this->MetricsMessageCallback(*msg); + }; + subscription_ = create_subscription>( + topic_name, + 10 /*history_depth*/, + callback); + } + + /** + * Acquires a mutex in order to get the last message received member. + * \return the last message received + */ + std::vector GetReceivedMessages() const + { + std::unique_lock ulock{mutex_}; + return received_messages_; + } + + /** + * Return the number of messages received by this subscriber. + * \return the number of messages received by the subscriber callback + */ + uint64_t GetNumberOfMessagesReceived() const + { + return num_messages_received_; + } + +private: + /** + * Subscriber callback. Acquires a mutex to set the last message received and + * sets the promise to true. + * \param msg + */ + void MetricsMessageCallback(const MetricsMessage & msg) + { + std::unique_lock ulock{mutex_}; + ++num_messages_received_; + received_messages_.push_back(msg); + if (num_messages_received_ >= number_of_messages_to_receive_) { + PromiseSetter::SetPromise(); + } + } + + std::vector received_messages_; + rclcpp::Subscription::SharedPtr subscription_; + mutable std::mutex mutex_; + std::atomic num_messages_received_{0}; + const uint64_t number_of_messages_to_receive_; +}; + +} // namespace topic_statistics +} // namespace rclcpp + +#endif // RCLCPP__TOPIC_STATISTICS__TEST_TOPIC_STATS_UTILS_HPP_ diff --git a/rclcpp/test/rclcpp/wait_set_policies/test_dynamic_storage.cpp b/rclcpp/test/rclcpp/wait_set_policies/test_dynamic_storage.cpp new file mode 100644 index 0000000000..e69d3480d9 --- /dev/null +++ b/rclcpp/test/rclcpp/wait_set_policies/test_dynamic_storage.cpp @@ -0,0 +1,304 @@ +// 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 "rclcpp/rclcpp.hpp" +#include "rclcpp/wait_set.hpp" +#include "../../utils/rclcpp_gtest_macros.hpp" + +#include "test_msgs/msg/empty.hpp" +#include "test_msgs/srv/empty.hpp" + +class TestDynamicStorage : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node = std::make_shared("node", "ns"); + } + + std::shared_ptr node; +}; + +class TestWaitable : public rclcpp::Waitable +{ +public: + TestWaitable() + : is_ready_(false) {} + + void add_to_wait_set(rcl_wait_set_t &) override {} + bool is_ready(const rcl_wait_set_t &) override {return is_ready_;} + + std::shared_ptr take_data() override {return nullptr;} + void execute(const std::shared_ptr &) override {} + + void set_is_ready(bool value) {is_ready_ = value;} + + void set_on_ready_callback(std::function) override {} + void clear_on_ready_callback() override {} + + std::shared_ptr take_data_by_entity_id(size_t) override {return nullptr;} + +private: + bool is_ready_; +}; + +TEST_F(TestDynamicStorage, default_construct_destruct) { + rclcpp::WaitSet wait_set; + EXPECT_TRUE(rcl_wait_set_is_valid(&wait_set.get_rcl_wait_set())); + EXPECT_EQ(rclcpp::WaitResultKind::Empty, wait_set.wait().kind()); +} + +TEST_F(TestDynamicStorage, iterables_construct_destruct) { + auto subscription = node->create_subscription( + "topic", 10, [](test_msgs::msg::Empty::ConstSharedPtr) {}); + auto timer = node->create_wall_timer(std::chrono::seconds(100), []() {}); + auto guard_condition = std::make_shared(); + auto service = + node->create_service( + "service", + []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}); + auto client = node->create_client("service"); + auto waitable = std::make_shared(); + auto subscriptions = + std::vector{{subscription}}; + auto guard_conditions = + std::vector{guard_condition}; + auto timers = + std::vector{timer}; + auto clients = + std::vector{client}; + auto services = + std::vector{service}; + auto waitables = + std::vector{{waitable}}; + rclcpp::WaitSet wait_set(subscriptions, guard_conditions, timers, clients, services, waitables); + + EXPECT_TRUE(rcl_wait_set_is_valid(&wait_set.get_rcl_wait_set())); +} + +TEST_F(TestDynamicStorage, add_remove_dynamically) { + rclcpp::WaitSet wait_set; + + // Adds more coverage + rclcpp::SubscriptionOptionsWithAllocator> options; + options.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; + + auto subscription = node->create_subscription( + "topic", 10, [](test_msgs::msg::Empty::ConstSharedPtr) {}, options); + + rclcpp::SubscriptionWaitSetMask mask{true, true, true}; + wait_set.add_subscription(subscription, mask); + RCLCPP_EXPECT_THROW_EQ( + wait_set.add_subscription(subscription, mask), + std::runtime_error("subscription already associated with a wait set")); + wait_set.remove_subscription(subscription, mask); + + // This is long, so it can stick around and be removed + auto timer = node->create_wall_timer(std::chrono::seconds(100), []() {}); + wait_set.add_timer(timer); + RCLCPP_EXPECT_THROW_EQ( + wait_set.add_timer(timer), + std::runtime_error("timer already in use by another wait set")); + wait_set.remove_timer(timer); + + auto guard_condition = std::make_shared(); + wait_set.add_guard_condition(guard_condition); + RCLCPP_EXPECT_THROW_EQ( + wait_set.add_guard_condition(guard_condition), + std::runtime_error("guard condition already in use by another wait set")); + wait_set.remove_guard_condition(guard_condition); + + auto service = + node->create_service( + "service", + []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}); + wait_set.add_service(service); + RCLCPP_EXPECT_THROW_EQ( + wait_set.add_service(service), + std::runtime_error("service already in use by another wait set")); + wait_set.remove_service(service); + + auto client = node->create_client("service"); + wait_set.add_client(client); + RCLCPP_EXPECT_THROW_EQ( + wait_set.add_client(client), + std::runtime_error("client already in use by another wait set")); + wait_set.remove_client(client); + + auto waitable = std::make_shared(); + wait_set.add_waitable(waitable); + RCLCPP_EXPECT_THROW_EQ( + wait_set.add_waitable(waitable), + std::runtime_error("waitable already in use by another wait set")); + + wait_set.remove_waitable(waitable); + wait_set.prune_deleted_entities(); + EXPECT_EQ(rclcpp::WaitResultKind::Empty, wait_set.wait().kind()); +} + +TEST_F(TestDynamicStorage, add_remove_nullptr) { + rclcpp::WaitSet wait_set; + + RCLCPP_EXPECT_THROW_EQ( + wait_set.add_subscription(nullptr), std::invalid_argument("subscription is nullptr")); + RCLCPP_EXPECT_THROW_EQ( + wait_set.remove_subscription(nullptr), std::invalid_argument("subscription is nullptr")); + + RCLCPP_EXPECT_THROW_EQ( + wait_set.add_guard_condition(nullptr), std::invalid_argument("guard_condition is nullptr")); + RCLCPP_EXPECT_THROW_EQ( + wait_set.remove_guard_condition(nullptr), std::invalid_argument("guard_condition is nullptr")); + + RCLCPP_EXPECT_THROW_EQ( + wait_set.add_timer(nullptr), std::invalid_argument("timer is nullptr")); + RCLCPP_EXPECT_THROW_EQ( + wait_set.remove_timer(nullptr), std::invalid_argument("timer is nullptr")); + + RCLCPP_EXPECT_THROW_EQ( + wait_set.add_client(nullptr), std::invalid_argument("client is nullptr")); + RCLCPP_EXPECT_THROW_EQ( + wait_set.remove_client(nullptr), std::invalid_argument("client is nullptr")); + + RCLCPP_EXPECT_THROW_EQ( + wait_set.add_service(nullptr), std::invalid_argument("service is nullptr")); + RCLCPP_EXPECT_THROW_EQ( + wait_set.remove_service(nullptr), std::invalid_argument("service is nullptr")); + + RCLCPP_EXPECT_THROW_EQ( + wait_set.add_waitable(nullptr), std::invalid_argument("waitable is nullptr")); + RCLCPP_EXPECT_THROW_EQ( + wait_set.remove_waitable(nullptr), std::invalid_argument("waitable is nullptr")); +} + +TEST_F(TestDynamicStorage, add_remove_out_of_scope) { + rclcpp::WaitSet wait_set; + + { + auto subscription = node->create_subscription( + "topic", 10, [](test_msgs::msg::Empty::ConstSharedPtr) {}); + wait_set.add_subscription(subscription); + + // This is short, so if it's not cleaned up, it will trigger wait and it won't timeout + auto timer = node->create_wall_timer(std::chrono::milliseconds(1), []() {}); + wait_set.add_timer(timer); + + auto guard_condition = std::make_shared(); + wait_set.add_guard_condition(guard_condition); + + auto service = + node->create_service( + "service", + []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}); + wait_set.add_service(service); + + auto client = node->create_client("service"); + wait_set.add_client(client); + + auto waitable = std::make_shared(); + wait_set.add_waitable(waitable); + } + + EXPECT_EQ(rclcpp::WaitResultKind::Timeout, wait_set.wait(std::chrono::milliseconds(10)).kind()); +} + +TEST_F(TestDynamicStorage, wait_subscription) { + rclcpp::WaitSet wait_set; + + // Not added to wait_set, just used for publishing to the topic + auto publisher = node->create_publisher("topic", 10); + + auto subscription = node->create_subscription( + "topic", 10, [](test_msgs::msg::Empty::ConstSharedPtr) {}); + wait_set.add_subscription(subscription); + + { + auto wait_result = wait_set.wait(std::chrono::milliseconds(10)); + EXPECT_EQ(rclcpp::WaitResultKind::Timeout, wait_result.kind()); + } + + publisher->publish(test_msgs::msg::Empty()); + { + auto wait_result = wait_set.wait(std::chrono::seconds(-1)); + EXPECT_EQ(rclcpp::WaitResultKind::Ready, wait_result.kind()); + } +} + +TEST_F(TestDynamicStorage, wait_timer) { + rclcpp::WaitSet wait_set; + + auto timer = node->create_wall_timer(std::chrono::milliseconds(1), []() {}); + wait_set.add_timer(timer); + { + auto wait_result = wait_set.wait(std::chrono::seconds(-1)); + EXPECT_EQ(rclcpp::WaitResultKind::Ready, wait_result.kind()); + } +} + +TEST_F(TestDynamicStorage, wait_client_service) { + rclcpp::WaitSet wait_set; + auto guard_condition = std::make_shared(); + wait_set.add_guard_condition(guard_condition); + + auto service = + node->create_service( + "service", + []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}); + wait_set.add_service(service); + + auto client = node->create_client("service"); + wait_set.add_client(client); + { + auto wait_result = wait_set.wait(std::chrono::milliseconds(10)); + EXPECT_EQ(rclcpp::WaitResultKind::Timeout, wait_result.kind()); + } + client->async_send_request(std::make_shared()); + { + auto wait_result = wait_set.wait(std::chrono::seconds(-1)); + EXPECT_EQ(rclcpp::WaitResultKind::Ready, wait_result.kind()); + } +} + +TEST_F(TestDynamicStorage, wait_waitable) { + rclcpp::WaitSet wait_set; + auto waitable = std::make_shared(); + wait_set.add_waitable(waitable); + { + // This waitable doesn't add itself to the rcl_wait_set_t, so Empty is to be expected + auto wait_result = wait_set.wait(std::chrono::seconds(-1)); + EXPECT_EQ(rclcpp::WaitResultKind::Empty, wait_result.kind()); + } +} diff --git a/rclcpp/test/rclcpp/wait_set_policies/test_static_storage.cpp b/rclcpp/test/rclcpp/wait_set_policies/test_static_storage.cpp new file mode 100644 index 0000000000..ed3336e123 --- /dev/null +++ b/rclcpp/test/rclcpp/wait_set_policies/test_static_storage.cpp @@ -0,0 +1,208 @@ +// 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 "rclcpp/rclcpp.hpp" +#include "rclcpp/wait_set.hpp" +#include "rclcpp/wait_set_policies/static_storage.hpp" +#include "../../utils/rclcpp_gtest_macros.hpp" + +#include "test_msgs/msg/empty.hpp" +#include "test_msgs/srv/empty.hpp" + +class TestStaticStorage : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node = std::make_shared("node", "ns"); + } + + std::shared_ptr node; +}; + +class TestWaitable : public rclcpp::Waitable +{ +public: + TestWaitable() + : is_ready_(false) {} + + void add_to_wait_set(rcl_wait_set_t &) override {} + bool is_ready(const rcl_wait_set_t &) override {return is_ready_;} + + std::shared_ptr take_data() override {return nullptr;} + void execute(const std::shared_ptr &) override {} + + void set_is_ready(bool value) {is_ready_ = value;} + + void set_on_ready_callback(std::function) override {} + void clear_on_ready_callback() override {} + + std::shared_ptr take_data_by_entity_id(size_t) override {return nullptr;} + +private: + bool is_ready_; +}; + +TEST_F(TestStaticStorage, iterables_construct_destruct) { + auto subscription = node->create_subscription( + "topic", 10, [](test_msgs::msg::Empty::ConstSharedPtr) {}); + // This is long, so it can stick around and be removed + auto timer = node->create_wall_timer(std::chrono::seconds(100), []() {}); + auto guard_condition = std::make_shared(); + auto service = + node->create_service( + "service", + []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}); + auto client = node->create_client("service"); + auto waitable = std::make_shared(); + rclcpp::StaticWaitSet<1, 1, 1, 1, 1, 1> wait_set( + {{{subscription}}}, {guard_condition}, {timer}, {client}, {service}, {{{waitable}}}); + + EXPECT_TRUE(rcl_wait_set_is_valid(&wait_set.get_rcl_wait_set())); +} + +// Because these StaticWaitSet's have templated sizes larger than the input arguments passed +// to the constructor, their shared-pointer contents will be default constructed to null. This +// test just checks the appropriate exception is thrown. +// std::shared_ptr::reset() is not required for these exceptions, it just +// disables the unused return value warning of std::make_shared +TEST_F(TestStaticStorage, fixed_storage_needs_pruning) { + { + using StaticWaitSet = rclcpp::StaticWaitSet<1, 0, 0, 0, 0, 0>; + RCLCPP_EXPECT_THROW_EQ( + std::make_shared().reset(), + std::runtime_error("unexpected condition, fixed storage policy needs pruning")); + } + { + using StaticWaitSet = rclcpp::StaticWaitSet<0, 1, 0, 0, 0, 0>; + RCLCPP_EXPECT_THROW_EQ( + std::make_shared().reset(), + std::runtime_error("unexpected condition, fixed storage policy needs pruning")); + } + { + using StaticWaitSet = rclcpp::StaticWaitSet<0, 0, 1, 0, 0, 0>; + RCLCPP_EXPECT_THROW_EQ( + std::make_shared().reset(), + std::runtime_error("unexpected condition, fixed storage policy needs pruning")); + } + { + using StaticWaitSet = rclcpp::StaticWaitSet<0, 0, 0, 1, 0, 0>; + RCLCPP_EXPECT_THROW_EQ( + std::make_shared().reset(), + std::runtime_error("unexpected condition, fixed storage policy needs pruning")); + } + { + using StaticWaitSet = rclcpp::StaticWaitSet<0, 0, 0, 0, 1, 0>; + RCLCPP_EXPECT_THROW_EQ( + std::make_shared().reset(), + std::runtime_error("unexpected condition, fixed storage policy needs pruning")); + } + { + using StaticWaitSet = rclcpp::StaticWaitSet<0, 0, 0, 0, 0, 1>; + RCLCPP_EXPECT_THROW_EQ( + std::make_shared().reset(), + std::runtime_error("unexpected condition, fixed storage policy needs pruning")); + } +} + +TEST_F(TestStaticStorage, wait_subscription) { + auto publisher = node->create_publisher("topic", 10); + auto subscription = node->create_subscription( + "topic", 10, [](test_msgs::msg::Empty::ConstSharedPtr) {}); + rclcpp::SubscriptionWaitSetMask mask{true, true, true}; + rclcpp::StaticWaitSet<1, 0, 0, 0, 0, 0> wait_set({{{subscription, mask}}}); + + { + auto wait_result = wait_set.wait(std::chrono::milliseconds(10)); + EXPECT_EQ(rclcpp::WaitResultKind::Timeout, wait_result.kind()); + } + + publisher->publish(test_msgs::msg::Empty()); + { + auto wait_result = wait_set.wait(std::chrono::seconds(-1)); + EXPECT_EQ(rclcpp::WaitResultKind::Ready, wait_result.kind()); + } +} + +TEST_F(TestStaticStorage, wait_timer) { + auto timer = node->create_wall_timer(std::chrono::milliseconds(1), []() {}); + rclcpp::StaticWaitSet<0, 0, 1, 0, 0, 0> wait_set({}, {}, {timer}); + { + auto wait_result = wait_set.wait(std::chrono::seconds(-1)); + EXPECT_EQ(rclcpp::WaitResultKind::Ready, wait_result.kind()); + } +} + +TEST_F(TestStaticStorage, wait_guard_condition) { + auto guard_condition = std::make_shared(); + rclcpp::StaticWaitSet<0, 1, 0, 0, 0, 0> wait_set({}, {guard_condition}); + + { + auto wait_result = wait_set.wait(std::chrono::milliseconds(10)); + EXPECT_EQ(rclcpp::WaitResultKind::Timeout, wait_result.kind()); + } + + guard_condition->trigger(); + { + auto wait_result = wait_set.wait(std::chrono::seconds(-1)); + EXPECT_EQ(rclcpp::WaitResultKind::Ready, wait_result.kind()); + } +} + +TEST_F(TestStaticStorage, wait_client_service) { + auto service = + node->create_service( + "service", + []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}); + + auto client = node->create_client("service"); + rclcpp::StaticWaitSet<0, 0, 0, 1, 1, 0> wait_set({}, {}, {}, {client}, {service}); + { + auto wait_result = wait_set.wait(std::chrono::milliseconds(10)); + EXPECT_EQ(rclcpp::WaitResultKind::Timeout, wait_result.kind()); + } + client->async_send_request(std::make_shared()); + { + auto wait_result = wait_set.wait(std::chrono::seconds(-1)); + EXPECT_EQ(rclcpp::WaitResultKind::Ready, wait_result.kind()); + } +} + +TEST_F(TestStaticStorage, wait_waitable) { + auto waitable = std::make_shared(); + rclcpp::StaticWaitSet<0, 0, 0, 0, 0, 1> wait_set({}, {}, {}, {}, {}, {{{waitable}}}); + { + // This waitable doesn't add itself to the rcl_wait_set_t, so Empty is to be expected + auto wait_result = wait_set.wait(std::chrono::seconds(-1)); + EXPECT_EQ(rclcpp::WaitResultKind::Empty, wait_result.kind()); + } +} diff --git a/rclcpp/test/rclcpp/wait_set_policies/test_storage_policy_common.cpp b/rclcpp/test/rclcpp/wait_set_policies/test_storage_policy_common.cpp new file mode 100644 index 0000000000..f7ae12c2f3 --- /dev/null +++ b/rclcpp/test/rclcpp/wait_set_policies/test_storage_policy_common.cpp @@ -0,0 +1,182 @@ +// 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 "rclcpp/rclcpp.hpp" +#include "rclcpp/wait_set.hpp" +#include "../../mocking_utils/patch.hpp" +#include "../../utils/rclcpp_gtest_macros.hpp" + +#include "test_msgs/msg/empty.hpp" +#include "test_msgs/srv/empty.hpp" + +class TestStoragePolicyCommon : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node = std::make_shared("node", "ns"); + } + + std::shared_ptr node; +}; + +class TestWaitable : public rclcpp::Waitable +{ +public: + TestWaitable() + : is_ready_(false), add_to_wait_set_(false) {} + + void add_to_wait_set(rcl_wait_set_t &) override + { + if (!add_to_wait_set_) { + throw std::runtime_error("waitable unexpectedly failed to be added to wait set"); + } + } + + bool is_ready(const rcl_wait_set_t &) override {return is_ready_;} + + std::shared_ptr take_data() override {return nullptr;} + void execute(const std::shared_ptr &) override {} + + void set_is_ready(bool value) {is_ready_ = value;} + + void set_add_to_wait_set(bool value) {add_to_wait_set_ = value;} + + void set_on_ready_callback(std::function) override {} + void clear_on_ready_callback() override {} + + std::shared_ptr take_data_by_entity_id(size_t) override {return nullptr;} + +private: + bool is_ready_; + bool add_to_wait_set_; +}; + +TEST_F(TestStoragePolicyCommon, rcl_wait_set_fini_error) { + auto wait_set = std::make_shared(); + auto mock = mocking_utils::inject_on_return( + "lib:rclcpp", rcl_wait_set_fini, RCL_RET_ERROR); + EXPECT_NO_THROW(wait_set.reset()); +} + +TEST_F(TestStoragePolicyCommon, rcl_wait_set_resize_error) { + rclcpp::WaitSet wait_set; + + auto subscription = node->create_subscription( + "topic", 10, [](test_msgs::msg::Empty::ConstSharedPtr) {}); + rclcpp::SubscriptionWaitSetMask mask{true, true, true}; + + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_wait_set_resize, RCL_RET_ERROR); + wait_set.add_subscription(subscription, mask); + EXPECT_THROW( + wait_set.wait(), + rclcpp::exceptions::RCLError); +} + +TEST_F(TestStoragePolicyCommon, rcl_wait_set_clear_error) { + rclcpp::WaitSet wait_set; + + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_wait_set_clear, RCL_RET_ERROR); + EXPECT_THROW( + wait_set.wait(), + rclcpp::exceptions::RCLError); +} + +TEST_F(TestStoragePolicyCommon, rcl_wait_set_add_subscription_error) { + rclcpp::WaitSet wait_set; + auto subscription = node->create_subscription( + "topic", 10, [](test_msgs::msg::Empty::ConstSharedPtr) {}); + rclcpp::SubscriptionWaitSetMask mask{true, true, true}; + + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_wait_set_add_subscription, RCL_RET_ERROR); + wait_set.add_subscription(subscription, mask); + EXPECT_THROW( + wait_set.wait(), + rclcpp::exceptions::RCLError); +} +TEST_F(TestStoragePolicyCommon, rcl_wait_set_add_guard_condition_error) { + rclcpp::WaitSet wait_set; + auto guard_condition = std::make_shared(); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_wait_set_add_guard_condition, RCL_RET_ERROR); + wait_set.add_guard_condition(guard_condition); + EXPECT_THROW( + wait_set.wait(), + rclcpp::exceptions::RCLError); +} + +TEST_F(TestStoragePolicyCommon, rcl_wait_set_add_timer_error) { + rclcpp::WaitSet wait_set; + auto timer = node->create_wall_timer(std::chrono::seconds(100), []() {}); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_wait_set_add_timer, RCL_RET_ERROR); + wait_set.add_timer(timer); + EXPECT_THROW( + wait_set.wait(), + rclcpp::exceptions::RCLError); +} + +TEST_F(TestStoragePolicyCommon, rcl_wait_set_add_service_error) { + rclcpp::WaitSet wait_set; + auto service = + node->create_service( + "service", + []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_wait_set_add_service, RCL_RET_ERROR); + wait_set.add_service(service); + EXPECT_THROW( + wait_set.wait(), + rclcpp::exceptions::RCLError); +} + +TEST_F(TestStoragePolicyCommon, rcl_wait_set_add_client_error) { + rclcpp::WaitSet wait_set; + auto client = node->create_client("service"); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_wait_set_add_client, RCL_RET_ERROR); + wait_set.add_client(client); + EXPECT_THROW( + wait_set.wait(), + rclcpp::exceptions::RCLError); +} + +TEST_F(TestStoragePolicyCommon, add_waitable_error) { + rclcpp::WaitSet wait_set; + auto waitable = std::make_shared(); + waitable->set_add_to_wait_set(false); + wait_set.add_waitable(waitable); + RCLCPP_EXPECT_THROW_EQ( + wait_set.wait(), + std::runtime_error("waitable unexpectedly failed to be added to wait set")); +} diff --git a/rclcpp/test/rclcpp/wait_set_policies/test_thread_safe_synchronization.cpp b/rclcpp/test/rclcpp/wait_set_policies/test_thread_safe_synchronization.cpp new file mode 100644 index 0000000000..12c8838188 --- /dev/null +++ b/rclcpp/test/rclcpp/wait_set_policies/test_thread_safe_synchronization.cpp @@ -0,0 +1,309 @@ +// 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 "rclcpp/rclcpp.hpp" +#include "rclcpp/wait_set.hpp" +#include "../../utils/rclcpp_gtest_macros.hpp" + +#include "test_msgs/msg/empty.hpp" +#include "test_msgs/srv/empty.hpp" + +class TestThreadSafeStorage : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node = std::make_shared("node", "ns"); + } + + std::shared_ptr node; +}; + +class TestWaitable : public rclcpp::Waitable +{ +public: + TestWaitable() + : is_ready_(false) {} + + void add_to_wait_set(rcl_wait_set_t &) override {} + bool is_ready(const rcl_wait_set_t &) override {return is_ready_;} + + std::shared_ptr take_data() override {return nullptr;} + void execute(const std::shared_ptr &) override {} + + void set_is_ready(bool value) {is_ready_ = value;} + + void set_on_ready_callback(std::function) override {} + void clear_on_ready_callback() override {} + + std::shared_ptr take_data_by_entity_id(size_t) override {return nullptr;} + +private: + bool is_ready_; +}; + +TEST_F(TestThreadSafeStorage, default_construct_destruct) { + rclcpp::ThreadSafeWaitSet wait_set; + EXPECT_TRUE(rcl_wait_set_is_valid(&wait_set.get_rcl_wait_set())); + + // Expected behavior of thread-safe is to timeout here + EXPECT_EQ(rclcpp::WaitResultKind::Timeout, wait_set.wait(std::chrono::milliseconds(10)).kind()); +} + +TEST_F(TestThreadSafeStorage, iterables_construct_destruct) { + auto subscription = node->create_subscription( + "topic", 10, [](test_msgs::msg::Empty::ConstSharedPtr) {}); + // This is long, so it can stick around + auto timer = node->create_wall_timer(std::chrono::seconds(100), []() {}); + auto guard_condition = std::make_shared(); + auto service = + node->create_service( + "service", + []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}); + auto client = node->create_client("service"); + auto waitable = std::make_shared(); + auto subscriptions = + std::vector{{subscription}}; + auto guard_conditions = + std::vector{guard_condition}; + auto timers = + std::vector{timer}; + auto clients = + std::vector{client}; + auto services = + std::vector{service}; + auto waitables = + std::vector{{waitable}}; + rclcpp::ThreadSafeWaitSet wait_set( + subscriptions, guard_conditions, timers, clients, services, waitables); + + EXPECT_TRUE(rcl_wait_set_is_valid(&wait_set.get_rcl_wait_set())); +} + +TEST_F(TestThreadSafeStorage, add_remove_dynamically) { + rclcpp::ThreadSafeWaitSet wait_set; + + // Adds more coverage + rclcpp::SubscriptionOptionsWithAllocator> options; + options.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; + + auto subscription = node->create_subscription( + "topic", 10, [](test_msgs::msg::Empty::ConstSharedPtr) {}, options); + + rclcpp::SubscriptionWaitSetMask mask{true, true, true}; + wait_set.add_subscription(subscription, mask); + RCLCPP_EXPECT_THROW_EQ( + wait_set.add_subscription(subscription, mask), + std::runtime_error("subscription already associated with a wait set")); + wait_set.remove_subscription(subscription, mask); + + // This is long, so it can stick around and be removed + auto timer = node->create_wall_timer(std::chrono::seconds(100), []() {}); + wait_set.add_timer(timer); + RCLCPP_EXPECT_THROW_EQ( + wait_set.add_timer(timer), + std::runtime_error("timer already in use by another wait set")); + wait_set.remove_timer(timer); + + auto guard_condition = std::make_shared(); + wait_set.add_guard_condition(guard_condition); + RCLCPP_EXPECT_THROW_EQ( + wait_set.add_guard_condition(guard_condition), + std::runtime_error("guard condition already in use by another wait set")); + wait_set.remove_guard_condition(guard_condition); + + auto service = + node->create_service( + "service", + []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}); + wait_set.add_service(service); + RCLCPP_EXPECT_THROW_EQ( + wait_set.add_service(service), + std::runtime_error("service already in use by another wait set")); + wait_set.remove_service(service); + + auto client = node->create_client("service"); + wait_set.add_client(client); + RCLCPP_EXPECT_THROW_EQ( + wait_set.add_client(client), + std::runtime_error("client already in use by another wait set")); + wait_set.remove_client(client); + + auto waitable = std::make_shared(); + wait_set.add_waitable(waitable); + RCLCPP_EXPECT_THROW_EQ( + wait_set.add_waitable(waitable), + std::runtime_error("waitable already in use by another wait set")); + wait_set.remove_waitable(waitable); + wait_set.prune_deleted_entities(); + + // Expected behavior of thread-safe is to timeout here + EXPECT_EQ(rclcpp::WaitResultKind::Timeout, wait_set.wait(std::chrono::milliseconds(10)).kind()); +} + +TEST_F(TestThreadSafeStorage, add_remove_nullptr) { + rclcpp::ThreadSafeWaitSet wait_set; + + RCLCPP_EXPECT_THROW_EQ( + wait_set.add_subscription(nullptr), std::invalid_argument("subscription is nullptr")); + RCLCPP_EXPECT_THROW_EQ( + wait_set.remove_subscription(nullptr), std::invalid_argument("subscription is nullptr")); + + RCLCPP_EXPECT_THROW_EQ( + wait_set.add_guard_condition(nullptr), std::invalid_argument("guard_condition is nullptr")); + RCLCPP_EXPECT_THROW_EQ( + wait_set.remove_guard_condition(nullptr), std::invalid_argument("guard_condition is nullptr")); + + RCLCPP_EXPECT_THROW_EQ( + wait_set.add_timer(nullptr), std::invalid_argument("timer is nullptr")); + RCLCPP_EXPECT_THROW_EQ( + wait_set.remove_timer(nullptr), std::invalid_argument("timer is nullptr")); + + RCLCPP_EXPECT_THROW_EQ( + wait_set.add_client(nullptr), std::invalid_argument("client is nullptr")); + RCLCPP_EXPECT_THROW_EQ( + wait_set.remove_client(nullptr), std::invalid_argument("client is nullptr")); + + RCLCPP_EXPECT_THROW_EQ( + wait_set.add_service(nullptr), std::invalid_argument("service is nullptr")); + RCLCPP_EXPECT_THROW_EQ( + wait_set.remove_service(nullptr), std::invalid_argument("service is nullptr")); + + RCLCPP_EXPECT_THROW_EQ( + wait_set.add_waitable(nullptr), std::invalid_argument("waitable is nullptr")); + RCLCPP_EXPECT_THROW_EQ( + wait_set.remove_waitable(nullptr), std::invalid_argument("waitable is nullptr")); +} + +TEST_F(TestThreadSafeStorage, add_remove_out_of_scope) { + rclcpp::ThreadSafeWaitSet wait_set; + + { + auto subscription = node->create_subscription( + "topic", 10, [](test_msgs::msg::Empty::ConstSharedPtr) {}); + wait_set.add_subscription(subscription); + + // This is short, so if it's not cleaned up, it will trigger wait + auto timer = node->create_wall_timer(std::chrono::milliseconds(1), []() {}); + wait_set.add_timer(timer); + + auto guard_condition = std::make_shared(); + wait_set.add_guard_condition(guard_condition); + + auto service = + node->create_service( + "service", + []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}); + wait_set.add_service(service); + + auto client = node->create_client("service"); + wait_set.add_client(client); + + auto waitable = std::make_shared(); + wait_set.add_waitable(waitable); + } + + EXPECT_EQ(rclcpp::WaitResultKind::Timeout, wait_set.wait(std::chrono::milliseconds(10)).kind()); +} + +TEST_F(TestThreadSafeStorage, wait_subscription) { + rclcpp::ThreadSafeWaitSet wait_set; + + // Not added to wait_set, just used for publishing to the topic + auto publisher = node->create_publisher("topic", 10); + + auto subscription = node->create_subscription( + "topic", 10, [](test_msgs::msg::Empty::ConstSharedPtr) {}); + wait_set.add_subscription(subscription); + + { + auto wait_result = wait_set.wait(std::chrono::milliseconds(10)); + EXPECT_EQ(rclcpp::WaitResultKind::Timeout, wait_result.kind()); + } + + publisher->publish(test_msgs::msg::Empty()); + { + auto wait_result = wait_set.wait(std::chrono::seconds(1)); + EXPECT_EQ(rclcpp::WaitResultKind::Ready, wait_result.kind()); + } +} + +TEST_F(TestThreadSafeStorage, wait_timer) { + rclcpp::ThreadSafeWaitSet wait_set; + + auto timer = node->create_wall_timer(std::chrono::milliseconds(1), []() {}); + wait_set.add_timer(timer); + { + auto wait_result = wait_set.wait(std::chrono::seconds(1)); + EXPECT_EQ(rclcpp::WaitResultKind::Ready, wait_result.kind()); + } +} + +TEST_F(TestThreadSafeStorage, wait_client_service) { + rclcpp::ThreadSafeWaitSet wait_set; + auto guard_condition = std::make_shared(); + wait_set.add_guard_condition(guard_condition); + + auto service = + node->create_service( + "service", + []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}); + wait_set.add_service(service); + + auto client = node->create_client("service"); + wait_set.add_client(client); + { + auto wait_result = wait_set.wait(std::chrono::milliseconds(10)); + EXPECT_EQ(rclcpp::WaitResultKind::Timeout, wait_result.kind()); + } + client->async_send_request(std::make_shared()); + { + auto wait_result = wait_set.wait(std::chrono::seconds(1)); + EXPECT_EQ(rclcpp::WaitResultKind::Ready, wait_result.kind()); + } +} + +TEST_F(TestThreadSafeStorage, wait_waitable) { + rclcpp::ThreadSafeWaitSet wait_set; + auto waitable = std::make_shared(); + wait_set.add_waitable(waitable); + { + // This waitable doesn't add itself to the rcl_wait_set_t, so Timeout is to be expected + auto wait_result = wait_set.wait(std::chrono::milliseconds(10)); + EXPECT_EQ(rclcpp::WaitResultKind::Timeout, wait_result.kind()); + } +} diff --git a/rclcpp/test/rclcpp/waitables/test_intra_process_waitable.cpp b/rclcpp/test/rclcpp/waitables/test_intra_process_waitable.cpp new file mode 100644 index 0000000000..723e80513b --- /dev/null +++ b/rclcpp/test/rclcpp/waitables/test_intra_process_waitable.cpp @@ -0,0 +1,46 @@ +// Copyright 2024 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 "rclcpp/rclcpp.hpp" +#include "test_msgs/msg/empty.hpp" + +#include "./waitable_test_helpers.hpp" + +class TestIntraProcessWaitable : public ::testing::Test +{ +protected: + static void SetUpTestCase() {rclcpp::init(0, nullptr);} + static void TearDownTestCase() {rclcpp::shutdown();} +}; + +TEST_F(TestIntraProcessWaitable, test_that_waitable_stays_ready_after_second_wait) { + auto node = std::make_shared( + "test_node", + rclcpp::NodeOptions().use_intra_process_comms(true)); + + using test_msgs::msg::Empty; + auto sub = node->create_subscription("test_topic", 10, [](const Empty &) {}); + auto pub = node->create_publisher("test_topic", 10); + + auto make_sub_intra_process_waitable_ready = [pub]() { + pub->publish(Empty()); + }; + + rclcpp::test::waitables::do_test_that_waitable_stays_ready_after_second_wait( + sub->get_intra_process_waitable(), + make_sub_intra_process_waitable_ready, + true /* expected_to_stay_ready */); +} diff --git a/rclcpp/test/rclcpp/waitables/waitable_test_helpers.hpp b/rclcpp/test/rclcpp/waitables/waitable_test_helpers.hpp new file mode 100644 index 0000000000..49e074d5b0 --- /dev/null +++ b/rclcpp/test/rclcpp/waitables/waitable_test_helpers.hpp @@ -0,0 +1,117 @@ +// Copyright 2024 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 RCLCPP__WAITABLES__WAITABLE_TEST_HELPERS_HPP_ +#define RCLCPP__WAITABLES__WAITABLE_TEST_HELPERS_HPP_ + +#include + +#include +#include +#include + +#include + +namespace rclcpp +{ +namespace test +{ +namespace waitables +{ + +/// Test that a given waitable is ready after a second wait. +/** + * The purpose of this test is to check that a waitable will remain ready + * on subsequent wait calls, if that is the expected behavior. + * Not all waitables should remain ready after a wait call, which can be + * expressed in the expected_to_stay_ready argument which defaults to true. + * If set to false, it will check that it is not ready after a second wait, as + * well as some other parts of the test. + * + * The given waitable should: + * + * - not be ready initially + * - not be ready after being waited on (and timing out) + * - should become ready after the make_waitable_ready method is called + * - may or may not be ready at this point + * - should be ready after waiting on it, within the wait_timeout + * - should be ready still after a second wait (unless expected_to_stay_ready = false) + * - if expected_to_stay_ready, should become not ready after a take_data/execute + */ +template +void +do_test_that_waitable_stays_ready_after_second_wait( + const std::shared_ptr & waitable, + std::function make_waitable_ready, + bool expected_to_stay_ready = true, + std::chrono::nanoseconds wait_timeout = std::chrono::seconds(5)) +{ + rclcpp::WaitSet wait_set; + wait_set.add_waitable(waitable); + + // not ready initially + EXPECT_FALSE(waitable->is_ready(wait_set.get_rcl_wait_set())) + << "waitable is unexpectedly ready before waiting"; + + // not ready after a wait that timesout + { + auto wait_result = wait_set.wait(std::chrono::seconds(0)); + EXPECT_EQ(wait_result.kind(), rclcpp::WaitResultKind::Timeout) + << "wait set did not timeout as expected"; + EXPECT_FALSE(waitable->is_ready(wait_set.get_rcl_wait_set())) + << "waitable is unexpectedly ready after waiting, but before making ready"; + } + + // make it ready and wait on it + make_waitable_ready(); + { + auto wait_result = wait_set.wait(wait_timeout); + EXPECT_EQ(wait_result.kind(), rclcpp::WaitResultKind::Ready) + << "wait set was not ready after the waitable should have been made ready"; + EXPECT_TRUE(waitable->is_ready(wait_set.get_rcl_wait_set())) + << "waitable is unexpectedly not ready after making it ready and waiting"; + } + + // wait again, and see that it is ready as expected or not expected + { + auto wait_result = wait_set.wait(std::chrono::seconds(0)); + if (expected_to_stay_ready) { + EXPECT_EQ(wait_result.kind(), rclcpp::WaitResultKind::Ready) + << "wait set was not ready on a second wait on the waitable"; + EXPECT_TRUE(waitable->is_ready(wait_set.get_rcl_wait_set())) + << "waitable unexpectedly not ready after second wait"; + } else { + EXPECT_EQ(wait_result.kind(), rclcpp::WaitResultKind::Timeout) + << "wait set did not time out after the waitable should have no longer been ready"; + EXPECT_FALSE(waitable->is_ready(wait_set.get_rcl_wait_set())) + << "waitable was ready after waiting a second time, which was not expected"; + } + } + + // if expected_to_stay_ready, check that take_data/execute makes it not ready + if (expected_to_stay_ready) { + waitable->execute(waitable->take_data()); + auto wait_result = wait_set.wait(std::chrono::seconds(0)); + EXPECT_EQ(wait_result.kind(), rclcpp::WaitResultKind::Timeout) + << "wait set did not time out after the waitable should have no longer been ready"; + EXPECT_FALSE(waitable->is_ready(wait_set.get_rcl_wait_set())) + << "waitable was unexpectedly ready after a take_data and execute"; + } +} + +} // namespace waitables +} // namespace test +} // namespace rclcpp + +#endif // RCLCPP__WAITABLES__WAITABLE_TEST_HELPERS_HPP_ diff --git a/rclcpp/test/resources/test_node/load_complicated_parameters.yaml b/rclcpp/test/resources/test_node/load_complicated_parameters.yaml new file mode 100644 index 0000000000..7722f636c2 --- /dev/null +++ b/rclcpp/test/resources/test_node/load_complicated_parameters.yaml @@ -0,0 +1,25 @@ +/**: + ros__parameters: + bar: 5 + foo: 3.5 + a_value: "first" + +/*: + load_node: + ros__parameters: + bar_foo: "ok" + a_value: "second" + +namespace: + load_node: + ros__parameters: + foo_bar: true + a_value: "third" + +bar: + ros__parameters: + fatal: 10 + +/**/namespace/*: + ros__parameters: + a_value: "last_one_win" diff --git a/rclcpp/test/resources/test_node/load_parameters.yaml b/rclcpp/test/resources/test_node/load_parameters.yaml new file mode 100644 index 0000000000..0171032221 --- /dev/null +++ b/rclcpp/test/resources/test_node/load_parameters.yaml @@ -0,0 +1,18 @@ +/**: + ros__parameters: + bar: 5 + foo: 3.5 + +/*: + load_node: + ros__parameters: + bar_foo: "ok" + +namespace: + load_node: + ros__parameters: + foo_bar: true + +bar: + ros__parameters: + fatal: 10 diff --git a/rclcpp/test/resources/test_node/no_valid_parameters.yaml b/rclcpp/test/resources/test_node/no_valid_parameters.yaml new file mode 100644 index 0000000000..a75356cd77 --- /dev/null +++ b/rclcpp/test/resources/test_node/no_valid_parameters.yaml @@ -0,0 +1,4 @@ +/no/valid/parameters/node: + ros__parameters: + bar: 5 + foo: 3.5 diff --git a/rclcpp/test/resources/test_node/test_parameters.yaml b/rclcpp/test/resources/test_node/test_parameters.yaml new file mode 100644 index 0000000000..449caa36bc --- /dev/null +++ b/rclcpp/test/resources/test_node/test_parameters.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + parameter_bool: true + parameter_int: 42 + parameter_string_array: [baz, baz, baz] +test_declare_parameter_node: + ros__parameters: + parameter_int: 21 diff --git a/rclcpp/test/resources/test_node_parameters/complicated_wildcards.yaml b/rclcpp/test/resources/test_node_parameters/complicated_wildcards.yaml new file mode 100644 index 0000000000..53da409135 --- /dev/null +++ b/rclcpp/test/resources/test_node_parameters/complicated_wildcards.yaml @@ -0,0 +1,5 @@ +/**/foo/*/bar: + node2: + ros__parameters: + foo: "foo" + bar: "bar" diff --git a/rclcpp/test/resources/test_node_parameters/params_by_order.yaml b/rclcpp/test/resources/test_node_parameters/params_by_order.yaml new file mode 100644 index 0000000000..680d96beaf --- /dev/null +++ b/rclcpp/test/resources/test_node_parameters/params_by_order.yaml @@ -0,0 +1,16 @@ +/**: + node2: + ros__parameters: + a_value: "first" + foo: "foo" + +/ns: + node2: + ros__parameters: + a_value: "second" + bar: "bar" + +/*: + node2: + ros__parameters: + a_value: "last_one_win" diff --git a/rclcpp/test/resources/test_node_parameters/wildcards.yaml b/rclcpp/test/resources/test_node_parameters/wildcards.yaml new file mode 100644 index 0000000000..b89b0d8cd0 --- /dev/null +++ b/rclcpp/test/resources/test_node_parameters/wildcards.yaml @@ -0,0 +1,57 @@ +/**: + ros__parameters: + full_wild: "full_wild" + +/**: + node2: + ros__parameters: + namespace_wild: "namespace_wild" + +/**/node2: + ros__parameters: + namespace_wild_another: "namespace_wild_another" + +/*: + node2: + ros__parameters: + namespace_wild_one_star: "namespace_wild_one_star" + +ns: + "*": + ros__parameters: + node_wild_in_ns: "node_wild_in_ns" + +/ns/*: + ros__parameters: + node_wild_in_ns_another: "node_wild_in_ns_another" + +ns: + node2: + ros__parameters: + explicit_in_ns: "explicit_in_ns" + +"*": + ros__parameters: + node_wild_no_ns: "node_wild_no_ns" + +node2: + ros__parameters: + explicit_no_ns: "explicit_no_ns" + +ns: + nodeX: + ros__parameters: + should_not_appear: "incorrect_node_name" + +/**/nodeX: + ros__parameters: + should_not_appear: "incorrect_node_name" + +nsX: + node2: + ros__parameters: + should_not_appear: "incorrect_namespace" + +/nsX/*: + ros__parameters: + should_not_appear: "incorrect_namespace" diff --git a/rclcpp/test/test_client.cpp b/rclcpp/test/test_client.cpp deleted file mode 100644 index 96c86f174a..0000000000 --- a/rclcpp/test/test_client.cpp +++ /dev/null @@ -1,60 +0,0 @@ -// 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 "rclcpp/exceptions.hpp" -#include "rclcpp/rclcpp.hpp" - -#include "rcl_interfaces/srv/list_parameters.hpp" - -class TestClient : public ::testing::Test -{ -protected: - static void SetUpTestCase() - { - rclcpp::init(0, nullptr); - } - - void SetUp() - { - node = std::make_shared("my_node", "/ns"); - } - - void TearDown() - { - node.reset(); - } - - rclcpp::Node::SharedPtr node; -}; - -/* - Testing client construction and destruction. - */ -TEST_F(TestClient, construction_and_destruction) { - using rcl_interfaces::srv::ListParameters; - { - auto client = node->create_client("service"); - } - - { - ASSERT_THROW({ - auto client = node->create_client("invalid_service?"); - }, rclcpp::exceptions::InvalidServiceNameError); - } -} diff --git a/rclcpp/test/test_duration.cpp b/rclcpp/test/test_duration.cpp deleted file mode 100644 index 1c072dab3f..0000000000 --- a/rclcpp/test/test_duration.cpp +++ /dev/null @@ -1,88 +0,0 @@ -// 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 "rcl/error_handling.h" -#include "rcl/time.h" -#include "rclcpp/clock.hpp" -#include "rclcpp/rclcpp.hpp" -#include "rclcpp/duration.hpp" - - -using namespace std::chrono_literals; - -class TestDuration : public ::testing::Test -{ -}; - -// TEST(TestDuration, conversions) { -// TODO(tfoote) Implement conversion methods -// } - -TEST(TestDuration, operators) { - rclcpp::Duration old(1, 0); - rclcpp::Duration young(2, 0); - - EXPECT_TRUE(old < young); - EXPECT_TRUE(young > old); - EXPECT_TRUE(old <= young); - EXPECT_TRUE(young >= old); - EXPECT_FALSE(young == old); - - rclcpp::Duration add = old + young; - EXPECT_EQ(add.nanoseconds(), (rcl_duration_value_t)(old.nanoseconds() + young.nanoseconds())); - EXPECT_EQ(add, old + young); - - rclcpp::Duration sub = young - old; - EXPECT_EQ(sub.nanoseconds(), (rcl_duration_value_t)(young.nanoseconds() - old.nanoseconds())); - EXPECT_EQ(sub, young - old); - - rclcpp::Duration time = rclcpp::Duration(0, 0); - rclcpp::Duration copy_constructor_duration(time); - rclcpp::Duration assignment_op_duration = rclcpp::Duration(1, 0); - assignment_op_duration = time; - - EXPECT_TRUE(time == copy_constructor_duration); - EXPECT_TRUE(time == assignment_op_duration); -} - -TEST(TestDuration, chrono_overloads) { - int64_t ns = 123456789l; - auto chrono_ns = std::chrono::nanoseconds(ns); - auto d1 = rclcpp::Duration(ns); - auto d2 = rclcpp::Duration(chrono_ns); - auto d3 = rclcpp::Duration(123456789ns); - EXPECT_EQ(d1, d2); - EXPECT_EQ(d1, d3); - EXPECT_EQ(d2, d3); -} - -TEST(TestDuration, overflows) { - rclcpp::Duration max(std::numeric_limits::max()); - rclcpp::Duration min(std::numeric_limits::min()); - - rclcpp::Duration one(1); - rclcpp::Duration negative_one(-1); - - EXPECT_THROW(max + one, std::overflow_error); - EXPECT_THROW(min - one, std::underflow_error); - EXPECT_THROW(negative_one + min, std::underflow_error); - EXPECT_THROW(negative_one - max, std::underflow_error); -} diff --git a/rclcpp/test/test_executor.cpp b/rclcpp/test/test_executor.cpp deleted file mode 100644 index 4b51cf19b3..0000000000 --- a/rclcpp/test/test_executor.cpp +++ /dev/null @@ -1,62 +0,0 @@ -// 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 "rcl/error_handling.h" -#include "rcl/time.h" -#include "rclcpp/clock.hpp" -#include "rclcpp/duration.hpp" -#include "rclcpp/rclcpp.hpp" - -using namespace std::chrono_literals; - -class TestExcutors : public ::testing::Test -{ -protected: - static void SetUpTestCase() - { - rclcpp::init(0, nullptr); - } - - void SetUp() - { - node = std::make_shared("my_node"); - } - - void TearDown() - { - node.reset(); - } - - rclcpp::Node::SharedPtr node; -}; - -// Make sure that executors detach from nodes when destructing -TEST_F(TestExcutors, detachOnDestruction) { - { - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node); - } - { - rclcpp::executors::SingleThreadedExecutor executor; - EXPECT_NO_THROW(executor.add_node(node)); - } -} diff --git a/rclcpp/test/test_expand_topic_or_service_name.cpp b/rclcpp/test/test_expand_topic_or_service_name.cpp deleted file mode 100644 index 4553b19c43..0000000000 --- a/rclcpp/test/test_expand_topic_or_service_name.cpp +++ /dev/null @@ -1,74 +0,0 @@ -// 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 "rclcpp/exceptions.hpp" -#include "rclcpp/expand_topic_or_service_name.hpp" - -/* - Testing expand_topic_or_service_name. - */ -TEST(TestExpandTopicOrServiceName, normal) { - using rclcpp::expand_topic_or_service_name; - { - ASSERT_EQ("/ns/chatter", expand_topic_or_service_name("chatter", "node", "/ns")); - } -} - -/* - Testing exceptions of expand_topic_or_service_name. - */ -TEST(TestExpandTopicOrServiceName, exceptions) { - using rclcpp::expand_topic_or_service_name; - { - ASSERT_THROW({ - expand_topic_or_service_name("chatter", "invalid_node?", "/ns"); - }, rclcpp::exceptions::InvalidNodeNameError); - } - - { - ASSERT_THROW({ - expand_topic_or_service_name("chatter", "node", "/invalid_ns?"); - }, rclcpp::exceptions::InvalidNamespaceError); - } - - { - ASSERT_THROW({ - expand_topic_or_service_name("chatter/42invalid", "node", "/ns"); - }, rclcpp::exceptions::InvalidTopicNameError); - } - - { - ASSERT_THROW({ - // this one will only fail on the "full" topic name validation check - expand_topic_or_service_name("chatter/{ns}/invalid", "node", "/ns"); - }, rclcpp::exceptions::InvalidTopicNameError); - } - - { - ASSERT_THROW({ - // is_service = true - expand_topic_or_service_name("chatter/42invalid", "node", "/ns", true); - }, rclcpp::exceptions::InvalidServiceNameError); - } - - { - ASSERT_THROW({ - // is_service = true - // this one will only fail on the "full" topic name validation check - expand_topic_or_service_name("chatter/{ns}/invalid", "node", "/ns", true); - }, rclcpp::exceptions::InvalidServiceNameError); - } -} diff --git a/rclcpp/test/test_intra_process_manager.cpp b/rclcpp/test/test_intra_process_manager.cpp deleted file mode 100644 index 5128ece832..0000000000 --- a/rclcpp/test/test_intra_process_manager.cpp +++ /dev/null @@ -1,862 +0,0 @@ -// 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 "gtest/gtest.h" -#include "rclcpp/allocator/allocator_common.hpp" -#include "rclcpp/macros.hpp" -#include "rmw/types.h" - -// Mock up publisher and subscription base to avoid needing an rmw impl. -namespace rclcpp -{ -namespace mock -{ - -class PublisherBase -{ -public: - RCLCPP_SMART_PTR_DEFINITIONS(PublisherBase) - - PublisherBase() - : mock_topic_name(""), mock_queue_size(0) {} - - const char * get_topic_name() const - { - return mock_topic_name.c_str(); - } - size_t get_queue_size() const - { - return mock_queue_size; - } - - bool - operator==(const rmw_gid_t * gid) const - { - (void)gid; - return false; - } - - std::string mock_topic_name; - size_t mock_queue_size; -}; - -template> -class Publisher : public PublisherBase -{ -public: - using MessageAllocTraits = allocator::AllocRebind; - using MessageAlloc = typename MessageAllocTraits::allocator_type; - using MessageDeleter = allocator::Deleter; - using MessageUniquePtr = std::unique_ptr; - std::shared_ptr allocator_; - - RCLCPP_SMART_PTR_DEFINITIONS(Publisher) - - Publisher() - { - allocator_ = std::make_shared(); - } - - std::shared_ptr get_allocator() - { - return allocator_; - } -}; - -} // namespace mock -} // namespace rclcpp - -namespace rclcpp -{ -namespace mock -{ - -class SubscriptionBase -{ -public: - RCLCPP_SMART_PTR_DEFINITIONS(SubscriptionBase) - - SubscriptionBase() - : mock_topic_name(""), mock_queue_size(0) {} - - const char * get_topic_name() const - { - return mock_topic_name.c_str(); - } - size_t get_queue_size() const - { - return mock_queue_size; - } - - std::string mock_topic_name; - size_t mock_queue_size; -}; - -} // namespace mock -} // namespace rclcpp - -// Prevent rclcpp/publisher.hpp and rclcpp/subscription.hpp from being imported. -#define RCLCPP__PUBLISHER_HPP_ -#define RCLCPP__SUBSCRIPTION_HPP_ -#define RCLCPP_BUILDING_LIBRARY 1 -// Force ipm to use our mock publisher class. -#define Publisher mock::Publisher -#define PublisherBase mock::PublisherBase -#define SubscriptionBase mock::SubscriptionBase -#include "../src/rclcpp/intra_process_manager.cpp" -#include "../src/rclcpp/intra_process_manager_impl.cpp" -#undef SubscriptionBase -#undef Publisher -#undef PublisherBase - -// NOLINTNEXTLINE(build/include_order) -#include - -/* - This tests the "normal" usage of the class: - - Creates two publishers on different topics. - - Creates a subscription which matches one of them. - - Publishes on each publisher with different message content. - - Try's to take the message from the non-matching publish, should fail. - - Try's to take the message from the matching publish, should work. - - Asserts the message it got back was the one that went in (since there's only one subscription). - - Try's to take the message again, should fail. - */ -TEST(TestIntraProcessManager, nominal) { - rclcpp::intra_process_manager::IntraProcessManager ipm; - - auto p1 = std::make_shared< - rclcpp::mock::Publisher - >(); - p1->mock_topic_name = "nominal1"; - p1->mock_queue_size = 2; - - auto p2 = std::make_shared< - rclcpp::mock::Publisher - >(); - p2->mock_topic_name = "nominal2"; - p2->mock_queue_size = 10; - - auto s1 = std::make_shared(); - s1->mock_topic_name = "nominal1"; - s1->mock_queue_size = 10; - - auto p1_id = - ipm.add_publisher>(p1); - auto p2_id = - ipm.add_publisher>(p2); - auto s1_id = ipm.add_subscription(s1); - - auto ipm_msg = std::make_shared(); - ipm_msg->message_sequence = 42; - ipm_msg->publisher_id = 42; - rcl_interfaces::msg::IntraProcessMessage::UniquePtr unique_msg( - new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg) - ); - - auto p1_m1_original_address = unique_msg.get(); - auto p1_m1_id = ipm.store_intra_process_message(p1_id, unique_msg); - ASSERT_EQ(nullptr, unique_msg); - - ipm_msg->message_sequence = 43; - ipm_msg->publisher_id = 43; - unique_msg.reset(new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg)); - - auto p2_m1_id = ipm.store_intra_process_message(p2_id, unique_msg); - ASSERT_EQ(nullptr, unique_msg); - - ipm.take_intra_process_message(p2_id, p2_m1_id, s1_id, unique_msg); - EXPECT_EQ(nullptr, unique_msg); // Should fail since p2 and s1 don't have the same topic. - unique_msg.reset(); - - ipm.take_intra_process_message(p1_id, p1_m1_id, s1_id, unique_msg); - EXPECT_NE(nullptr, unique_msg); - if (unique_msg) { - EXPECT_EQ(42ul, unique_msg->message_sequence); - EXPECT_EQ(42ul, unique_msg->publisher_id); - EXPECT_EQ(p1_m1_original_address, unique_msg.get()); - } - - ipm.take_intra_process_message(p1_id, p1_m1_id, s1_id, unique_msg); - EXPECT_EQ(nullptr, unique_msg); // Should fail, since the message was already taken. - - ipm_msg->message_sequence = 44; - ipm_msg->publisher_id = 44; - unique_msg.reset(new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg)); - - ipm.store_intra_process_message(p1_id, unique_msg); - ASSERT_EQ(nullptr, unique_msg); - - ipm_msg->message_sequence = 45; - ipm_msg->publisher_id = 45; - unique_msg.reset(new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg)); - - ipm.store_intra_process_message(p1_id, unique_msg); - ASSERT_EQ(nullptr, unique_msg); - - ipm_msg->message_sequence = 46; - ipm_msg->publisher_id = 46; - unique_msg.reset(new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg)); - - ipm.store_intra_process_message(p1_id, unique_msg); - ASSERT_NE(nullptr, unique_msg); - if (unique_msg) { - EXPECT_EQ(44ul, unique_msg->message_sequence); - EXPECT_EQ(44ul, unique_msg->publisher_id); - } -} - -/* - Simulates the case where a publisher is removed between publishing and the matching take. - - Creates a publisher and subscription on the same topic. - - Publishes a message. - - Remove the publisher. - - Try's to take the message, should fail since the publisher (and its storage) is gone. - */ -TEST(TestIntraProcessManager, remove_publisher_before_trying_to_take) { - rclcpp::intra_process_manager::IntraProcessManager ipm; - - auto p1 = std::make_shared< - rclcpp::mock::Publisher - >(); - p1->mock_topic_name = "nominal1"; - p1->mock_queue_size = 10; - - auto s1 = std::make_shared(); - s1->mock_topic_name = "nominal1"; - s1->mock_queue_size = 10; - - auto p1_id = - ipm.add_publisher>(p1); - auto s1_id = ipm.add_subscription(s1); - - auto ipm_msg = std::make_shared(); - ipm_msg->message_sequence = 42; - ipm_msg->publisher_id = 42; - rcl_interfaces::msg::IntraProcessMessage::UniquePtr unique_msg( - new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg) - ); - - auto p1_m1_id = ipm.store_intra_process_message(p1_id, unique_msg); - ASSERT_EQ(nullptr, unique_msg); - - ipm.remove_publisher(p1_id); - - ipm.take_intra_process_message(p1_id, p1_m1_id, s1_id, unique_msg); - EXPECT_EQ(nullptr, unique_msg); // Should fail, since the publisher is gone. -} - -/* - Tests whether or not removed subscriptions affect take behavior. - - Creates a publisher and three subscriptions on the same topic. - - Publish a message, keep the original point for later comparison. - - Take with one subscription, should work. - - Remove a different subscription. - - Take with the final subscription, should work. - - Assert the previous take returned ownership of the original object published. - */ -TEST(TestIntraProcessManager, removed_subscription_affects_take) { - rclcpp::intra_process_manager::IntraProcessManager ipm; - - auto p1 = std::make_shared< - rclcpp::mock::Publisher - >(); - p1->mock_topic_name = "nominal1"; - p1->mock_queue_size = 10; - - auto s1 = std::make_shared(); - s1->mock_topic_name = "nominal1"; - s1->mock_queue_size = 10; - - auto s2 = std::make_shared(); - s2->mock_topic_name = "nominal1"; - s2->mock_queue_size = 10; - - auto s3 = std::make_shared(); - s3->mock_topic_name = "nominal1"; - s3->mock_queue_size = 10; - - auto p1_id = - ipm.add_publisher>(p1); - auto s1_id = ipm.add_subscription(s1); - auto s2_id = ipm.add_subscription(s2); - auto s3_id = ipm.add_subscription(s3); - - auto ipm_msg = std::make_shared(); - ipm_msg->message_sequence = 42; - ipm_msg->publisher_id = 42; - rcl_interfaces::msg::IntraProcessMessage::UniquePtr unique_msg( - new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg) - ); - - auto original_message_pointer = unique_msg.get(); - auto p1_m1_id = ipm.store_intra_process_message(p1_id, unique_msg); - ASSERT_EQ(nullptr, unique_msg); - - ipm.take_intra_process_message(p1_id, p1_m1_id, s1_id, unique_msg); - EXPECT_NE(nullptr, unique_msg); - if (unique_msg) { - EXPECT_EQ(42ul, unique_msg->message_sequence); - EXPECT_EQ(42ul, unique_msg->publisher_id); - EXPECT_NE(original_message_pointer, unique_msg.get()); - } - unique_msg.reset(); - - ipm.remove_subscription(s2_id); - - // Take using s3, the remaining subscription. - ipm.take_intra_process_message(p1_id, p1_m1_id, s3_id, unique_msg); - EXPECT_NE(nullptr, unique_msg); - if (unique_msg) { - EXPECT_EQ(42ul, unique_msg->message_sequence); - EXPECT_EQ(42ul, unique_msg->publisher_id); - // Should match the original pointer since s2 was removed first. - EXPECT_EQ(original_message_pointer, unique_msg.get()); - } - - // Take using s2, should fail since s2 was removed. - unique_msg.reset(); - ipm.take_intra_process_message(p1_id, p1_m1_id, s2_id, unique_msg); - EXPECT_EQ(nullptr, unique_msg); -} - -/* - This tests normal operation with multiple subscriptions and one publisher. - - Creates a publisher and three subscriptions on the same topic. - - Publish a message. - - Take with each subscription, checking that the last takes the original back. - */ -TEST(TestIntraProcessManager, multiple_subscriptions_one_publisher) { - rclcpp::intra_process_manager::IntraProcessManager ipm; - - auto p1 = std::make_shared< - rclcpp::mock::Publisher - >(); - p1->mock_topic_name = "nominal1"; - p1->mock_queue_size = 10; - - auto s1 = std::make_shared(); - s1->mock_topic_name = "nominal1"; - s1->mock_queue_size = 10; - - auto s2 = std::make_shared(); - s2->mock_topic_name = "nominal1"; - s2->mock_queue_size = 10; - - auto s3 = std::make_shared(); - s3->mock_topic_name = "nominal1"; - s3->mock_queue_size = 10; - - auto p1_id = - ipm.add_publisher>(p1); - auto s1_id = ipm.add_subscription(s1); - auto s2_id = ipm.add_subscription(s2); - auto s3_id = ipm.add_subscription(s3); - - auto ipm_msg = std::make_shared(); - ipm_msg->message_sequence = 42; - ipm_msg->publisher_id = 42; - rcl_interfaces::msg::IntraProcessMessage::UniquePtr unique_msg( - new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg) - ); - - auto original_message_pointer = unique_msg.get(); - auto p1_m1_id = ipm.store_intra_process_message(p1_id, unique_msg); - ASSERT_EQ(nullptr, unique_msg); - - ipm.take_intra_process_message(p1_id, p1_m1_id, s1_id, unique_msg); - EXPECT_NE(nullptr, unique_msg); - if (unique_msg) { - EXPECT_EQ(42ul, unique_msg->message_sequence); - EXPECT_EQ(42ul, unique_msg->publisher_id); - EXPECT_NE(original_message_pointer, unique_msg.get()); - } - unique_msg.reset(); - - ipm.take_intra_process_message(p1_id, p1_m1_id, s2_id, unique_msg); - EXPECT_NE(nullptr, unique_msg); - if (unique_msg) { - EXPECT_EQ(42ul, unique_msg->message_sequence); - EXPECT_EQ(42ul, unique_msg->publisher_id); - EXPECT_NE(original_message_pointer, unique_msg.get()); - } - unique_msg.reset(); - - ipm.take_intra_process_message(p1_id, p1_m1_id, s3_id, unique_msg); - EXPECT_NE(nullptr, unique_msg); - if (unique_msg) { - EXPECT_EQ(42ul, unique_msg->message_sequence); - EXPECT_EQ(42ul, unique_msg->publisher_id); - // Should match the original pointer. - EXPECT_EQ(original_message_pointer, unique_msg.get()); - } -} - -/* - This tests normal operation with multiple publishers and one subscription. - - Creates a publisher and three subscriptions on the same topic. - - Publish a message. - - Take with each subscription, checking that the last takes the original back. - */ -TEST(TestIntraProcessManager, multiple_publishers_one_subscription) { - rclcpp::intra_process_manager::IntraProcessManager ipm; - - auto p1 = std::make_shared< - rclcpp::mock::Publisher - >(); - p1->mock_topic_name = "nominal1"; - p1->mock_queue_size = 10; - - auto p2 = std::make_shared< - rclcpp::mock::Publisher - >(); - p2->mock_topic_name = "nominal1"; - p2->mock_queue_size = 10; - - auto p3 = std::make_shared< - rclcpp::mock::Publisher - >(); - p3->mock_topic_name = "nominal1"; - p3->mock_queue_size = 10; - - auto s1 = std::make_shared(); - s1->mock_topic_name = "nominal1"; - s1->mock_queue_size = 10; - - auto p1_id = - ipm.add_publisher>(p1); - auto p2_id = - ipm.add_publisher>(p2); - auto p3_id = - ipm.add_publisher>(p3); - auto s1_id = ipm.add_subscription(s1); - - auto ipm_msg = std::make_shared(); - // First publish - ipm_msg->message_sequence = 42; - ipm_msg->publisher_id = 42; - rcl_interfaces::msg::IntraProcessMessage::UniquePtr unique_msg( - new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg) - ); - - auto original_message_pointer1 = unique_msg.get(); - auto p1_m1_id = ipm.store_intra_process_message(p1_id, unique_msg); - ASSERT_EQ(nullptr, unique_msg); - - // Second publish - ipm_msg->message_sequence = 43; - ipm_msg->publisher_id = 43; - unique_msg.reset(new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg)); - - auto original_message_pointer2 = unique_msg.get(); - auto p2_m1_id = ipm.store_intra_process_message(p2_id, unique_msg); - ASSERT_EQ(nullptr, unique_msg); - - // Third publish - ipm_msg->message_sequence = 44; - ipm_msg->publisher_id = 44; - unique_msg.reset(new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg)); - - auto original_message_pointer3 = unique_msg.get(); - auto p3_m1_id = ipm.store_intra_process_message(p3_id, unique_msg); - ASSERT_EQ(nullptr, unique_msg); - - // First take - ipm.take_intra_process_message(p1_id, p1_m1_id, s1_id, unique_msg); - EXPECT_NE(nullptr, unique_msg); - if (unique_msg) { - EXPECT_EQ(42ul, unique_msg->message_sequence); - EXPECT_EQ(42ul, unique_msg->publisher_id); - EXPECT_EQ(original_message_pointer1, unique_msg.get()); - } - unique_msg.reset(); - - // Second take - ipm.take_intra_process_message(p2_id, p2_m1_id, s1_id, unique_msg); - EXPECT_NE(nullptr, unique_msg); - if (unique_msg) { - EXPECT_EQ(43ul, unique_msg->message_sequence); - EXPECT_EQ(43ul, unique_msg->publisher_id); - EXPECT_EQ(original_message_pointer2, unique_msg.get()); - } - unique_msg.reset(); - - // Third take - ipm.take_intra_process_message(p3_id, p3_m1_id, s1_id, unique_msg); - EXPECT_NE(nullptr, unique_msg); - if (unique_msg) { - EXPECT_EQ(44ul, unique_msg->message_sequence); - EXPECT_EQ(44ul, unique_msg->publisher_id); - EXPECT_EQ(original_message_pointer3, unique_msg.get()); - } - unique_msg.reset(); -} - -/* - This tests normal operation with multiple publishers and subscriptions. - - Creates three publishers and three subscriptions on the same topic. - - Publish a message on each publisher. - - Take from each publisher with each subscription, checking the pointer. - */ -TEST(TestIntraProcessManager, multiple_publishers_multiple_subscription) { - rclcpp::intra_process_manager::IntraProcessManager ipm; - - auto p1 = std::make_shared< - rclcpp::mock::Publisher - >(); - p1->mock_topic_name = "nominal1"; - p1->mock_queue_size = 10; - - auto p2 = std::make_shared< - rclcpp::mock::Publisher - >(); - p2->mock_topic_name = "nominal1"; - p2->mock_queue_size = 10; - - auto p3 = std::make_shared< - rclcpp::mock::Publisher - >(); - p3->mock_topic_name = "nominal1"; - p3->mock_queue_size = 10; - - auto s1 = std::make_shared(); - s1->mock_topic_name = "nominal1"; - s1->mock_queue_size = 10; - - auto s2 = std::make_shared(); - s2->mock_topic_name = "nominal1"; - s2->mock_queue_size = 10; - - auto s3 = std::make_shared(); - s3->mock_topic_name = "nominal1"; - s3->mock_queue_size = 10; - - auto p1_id = - ipm.add_publisher>(p1); - auto p2_id = - ipm.add_publisher>(p2); - auto p3_id = - ipm.add_publisher>(p3); - auto s1_id = ipm.add_subscription(s1); - auto s2_id = ipm.add_subscription(s2); - auto s3_id = ipm.add_subscription(s3); - - auto ipm_msg = std::make_shared(); - // First publish - ipm_msg->message_sequence = 42; - ipm_msg->publisher_id = 42; - rcl_interfaces::msg::IntraProcessMessage::UniquePtr unique_msg( - new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg) - ); - - auto original_message_pointer1 = unique_msg.get(); - auto p1_m1_id = ipm.store_intra_process_message(p1_id, unique_msg); - ASSERT_EQ(nullptr, unique_msg); - - // Second publish - ipm_msg->message_sequence = 43; - ipm_msg->publisher_id = 43; - unique_msg.reset(new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg)); - - auto original_message_pointer2 = unique_msg.get(); - auto p2_m1_id = ipm.store_intra_process_message(p2_id, unique_msg); - ASSERT_EQ(nullptr, unique_msg); - - // Third publish - ipm_msg->message_sequence = 44; - ipm_msg->publisher_id = 44; - unique_msg.reset(new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg)); - - auto original_message_pointer3 = unique_msg.get(); - auto p3_m1_id = ipm.store_intra_process_message(p3_id, unique_msg); - ASSERT_EQ(nullptr, unique_msg); - - // First take - ipm.take_intra_process_message(p1_id, p1_m1_id, s1_id, unique_msg); - EXPECT_NE(nullptr, unique_msg); - if (unique_msg) { - EXPECT_EQ(42ul, unique_msg->message_sequence); - EXPECT_EQ(42ul, unique_msg->publisher_id); - EXPECT_NE(original_message_pointer1, unique_msg.get()); - } - unique_msg.reset(); - - ipm.take_intra_process_message(p1_id, p1_m1_id, s2_id, unique_msg); - EXPECT_NE(nullptr, unique_msg); - if (unique_msg) { - EXPECT_EQ(42ul, unique_msg->message_sequence); - EXPECT_EQ(42ul, unique_msg->publisher_id); - EXPECT_NE(original_message_pointer1, unique_msg.get()); - } - unique_msg.reset(); - - ipm.take_intra_process_message(p1_id, p1_m1_id, s3_id, unique_msg); - EXPECT_NE(nullptr, unique_msg); - if (unique_msg) { - EXPECT_EQ(42ul, unique_msg->message_sequence); - EXPECT_EQ(42ul, unique_msg->publisher_id); - EXPECT_EQ(original_message_pointer1, unique_msg.get()); // Final take. - } - unique_msg.reset(); - - // Second take - ipm.take_intra_process_message(p2_id, p2_m1_id, s1_id, unique_msg); - EXPECT_NE(nullptr, unique_msg); - if (unique_msg) { - EXPECT_EQ(43ul, unique_msg->message_sequence); - EXPECT_EQ(43ul, unique_msg->publisher_id); - EXPECT_NE(original_message_pointer2, unique_msg.get()); - } - unique_msg.reset(); - - ipm.take_intra_process_message(p2_id, p2_m1_id, s2_id, unique_msg); - EXPECT_NE(nullptr, unique_msg); - if (unique_msg) { - EXPECT_EQ(43ul, unique_msg->message_sequence); - EXPECT_EQ(43ul, unique_msg->publisher_id); - EXPECT_NE(original_message_pointer2, unique_msg.get()); - } - unique_msg.reset(); - - ipm.take_intra_process_message(p2_id, p2_m1_id, s3_id, unique_msg); - EXPECT_NE(nullptr, unique_msg); - if (unique_msg) { - EXPECT_EQ(43ul, unique_msg->message_sequence); - EXPECT_EQ(43ul, unique_msg->publisher_id); - EXPECT_EQ(original_message_pointer2, unique_msg.get()); - } - unique_msg.reset(); - - // Third take - ipm.take_intra_process_message(p3_id, p3_m1_id, s1_id, unique_msg); - EXPECT_NE(nullptr, unique_msg); - if (unique_msg) { - EXPECT_EQ(44ul, unique_msg->message_sequence); - EXPECT_EQ(44ul, unique_msg->publisher_id); - EXPECT_NE(original_message_pointer3, unique_msg.get()); - } - unique_msg.reset(); - - ipm.take_intra_process_message(p3_id, p3_m1_id, s2_id, unique_msg); - EXPECT_NE(nullptr, unique_msg); - if (unique_msg) { - EXPECT_EQ(44ul, unique_msg->message_sequence); - EXPECT_EQ(44ul, unique_msg->publisher_id); - EXPECT_NE(original_message_pointer3, unique_msg.get()); - } - unique_msg.reset(); - - ipm.take_intra_process_message(p3_id, p3_m1_id, s3_id, unique_msg); - EXPECT_NE(nullptr, unique_msg); - if (unique_msg) { - EXPECT_EQ(44ul, unique_msg->message_sequence); - EXPECT_EQ(44ul, unique_msg->publisher_id); - EXPECT_EQ(original_message_pointer3, unique_msg.get()); - } - unique_msg.reset(); -} - -/* - Tests displacing a message from the ring buffer before take is called. - - Creates a publisher (buffer_size = 2) and a subscription on the same topic. - - Publish a message on the publisher. - - Publish another message. - - Take the second message. - - Publish a message. - - Try to take the first message, should fail. - */ -TEST(TestIntraProcessManager, ring_buffer_displacement) { - rclcpp::intra_process_manager::IntraProcessManager ipm; - - auto p1 = std::make_shared< - rclcpp::mock::Publisher - >(); - p1->mock_topic_name = "nominal1"; - p1->mock_queue_size = 2; - - auto s1 = std::make_shared(); - s1->mock_topic_name = "nominal1"; - s1->mock_queue_size = 10; - - auto p1_id = - ipm.add_publisher>(p1); - auto s1_id = ipm.add_subscription(s1); - - auto ipm_msg = std::make_shared(); - ipm_msg->message_sequence = 42; - ipm_msg->publisher_id = 42; - rcl_interfaces::msg::IntraProcessMessage::UniquePtr unique_msg( - new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg) - ); - - auto original_message_pointer1 = unique_msg.get(); - auto p1_m1_id = ipm.store_intra_process_message(p1_id, unique_msg); - ASSERT_EQ(nullptr, unique_msg); - - ipm_msg->message_sequence = 43; - ipm_msg->publisher_id = 43; - unique_msg.reset(new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg)); - - auto original_message_pointer2 = unique_msg.get(); - auto p1_m2_id = ipm.store_intra_process_message(p1_id, unique_msg); - ASSERT_EQ(nullptr, unique_msg); - - ipm.take_intra_process_message(p1_id, p1_m2_id, s1_id, unique_msg); - EXPECT_NE(nullptr, unique_msg); - if (unique_msg) { - EXPECT_EQ(43ul, unique_msg->message_sequence); - EXPECT_EQ(43ul, unique_msg->publisher_id); - EXPECT_EQ(original_message_pointer2, unique_msg.get()); - } - unique_msg.reset(); - - ipm_msg->message_sequence = 44; - ipm_msg->publisher_id = 44; - unique_msg.reset(new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg)); - - ipm.store_intra_process_message(p1_id, unique_msg); - EXPECT_NE(nullptr, unique_msg); // Should return the thing in the ring buffer it displaced. - if (unique_msg) { - // This should have been the first published message. - EXPECT_EQ(42ul, unique_msg->message_sequence); - EXPECT_EQ(42ul, unique_msg->publisher_id); - EXPECT_EQ(original_message_pointer1, unique_msg.get()); - } - unique_msg.reset(); - - // Since it just got displaced it should no longer be there to take. - ipm.take_intra_process_message(p1_id, p1_m1_id, s1_id, unique_msg); - EXPECT_EQ(nullptr, unique_msg); -} - -/* - Simulates race condition where a subscription is created after publish. - - Creates a publisher. - - Publish a message on the publisher. - - Create a subscription on the same topic. - - Try to take the message with the newly created subscription, should fail. - */ -TEST(TestIntraProcessManager, subscription_creation_race_condition) { - rclcpp::intra_process_manager::IntraProcessManager ipm; - - auto p1 = std::make_shared< - rclcpp::mock::Publisher - >(); - p1->mock_topic_name = "nominal1"; - p1->mock_queue_size = 2; - - auto p1_id = - ipm.add_publisher>(p1); - - auto ipm_msg = std::make_shared(); - ipm_msg->message_sequence = 42; - ipm_msg->publisher_id = 42; - rcl_interfaces::msg::IntraProcessMessage::UniquePtr unique_msg( - new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg) - ); - - auto p1_m1_id = ipm.store_intra_process_message(p1_id, unique_msg); - ASSERT_EQ(nullptr, unique_msg); - - auto s1 = std::make_shared(); - s1->mock_topic_name = "nominal1"; - s1->mock_queue_size = 10; - - auto s1_id = ipm.add_subscription(s1); - - ipm.take_intra_process_message(p1_id, p1_m1_id, s1_id, unique_msg); - EXPECT_EQ(nullptr, unique_msg); -} - -/* - Simulates race condition where a publisher goes out of scope before take. - - Create a subscription. - - Creates a publisher on the same topic in a scope. - - Publish a message on the publisher in a scope. - - Let the scope expire. - - Try to take the message with the subscription, should fail. - */ -TEST(TestIntraProcessManager, publisher_out_of_scope_take) { - rclcpp::intra_process_manager::IntraProcessManager ipm; - - auto s1 = std::make_shared(); - s1->mock_topic_name = "nominal1"; - s1->mock_queue_size = 10; - - auto s1_id = ipm.add_subscription(s1); - - uint64_t p1_id; - uint64_t p1_m1_id; - { - auto p1 = std::make_shared< - rclcpp::mock::Publisher - >(); - p1->mock_topic_name = "nominal1"; - p1->mock_queue_size = 2; - - p1_id = ipm.add_publisher>(p1); - - auto ipm_msg = std::make_shared(); - ipm_msg->message_sequence = 42; - ipm_msg->publisher_id = 42; - rcl_interfaces::msg::IntraProcessMessage::UniquePtr unique_msg( - new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg) - ); - - p1_m1_id = ipm.store_intra_process_message(p1_id, unique_msg); - ASSERT_EQ(nullptr, unique_msg); - - // Explicitly remove publisher from ipm (emulate's publisher's destructor). - ipm.remove_publisher(p1_id); - } - - rcl_interfaces::msg::IntraProcessMessage::UniquePtr unique_msg(nullptr); - // Should fail because the publisher is out of scope. - ipm.take_intra_process_message(p1_id, p1_m1_id, s1_id, unique_msg); - EXPECT_EQ(nullptr, unique_msg); -} - -/* - Simulates race condition where a publisher goes out of scope before store. - - Creates a publisher in a scope. - - Let the scope expire. - - Publish a message on the publisher in a scope, should throw. - */ -TEST(TestIntraProcessManager, publisher_out_of_scope_store) { - rclcpp::intra_process_manager::IntraProcessManager ipm; - - uint64_t p1_id; - { - auto p1 = std::make_shared< - rclcpp::mock::Publisher - >(); - p1->mock_topic_name = "nominal1"; - p1->mock_queue_size = 2; - - p1_id = ipm.add_publisher>(p1); - } - - auto ipm_msg = std::make_shared(); - ipm_msg->message_sequence = 42; - ipm_msg->publisher_id = 42; - rcl_interfaces::msg::IntraProcessMessage::UniquePtr unique_msg( - new rcl_interfaces::msg::IntraProcessMessage(*ipm_msg) - ); - - EXPECT_THROW(ipm.store_intra_process_message(p1_id, unique_msg), std::runtime_error); - ASSERT_EQ(nullptr, unique_msg); -} diff --git a/rclcpp/test/test_logger.cpp b/rclcpp/test/test_logger.cpp deleted file mode 100644 index 258cdd0959..0000000000 --- a/rclcpp/test/test_logger.cpp +++ /dev/null @@ -1,35 +0,0 @@ -// 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 "rclcpp/logger.hpp" -#include "rclcpp/logging.hpp" - -TEST(TestLogger, factory_functions) { - rclcpp::Logger logger = rclcpp::get_logger("test_logger"); - EXPECT_STREQ("test_logger", logger.get_name()); - rclcpp::Logger logger_copy = rclcpp::Logger(logger); - EXPECT_STREQ("test_logger", logger_copy.get_name()); -} - -TEST(TestLogger, hierarchy) { - rclcpp::Logger logger = rclcpp::get_logger("test_logger"); - rclcpp::Logger sublogger = logger.get_child("child"); - EXPECT_STREQ("test_logger.child", sublogger.get_name()); - rclcpp::Logger subsublogger = sublogger.get_child("grandchild"); - EXPECT_STREQ("test_logger.child.grandchild", subsublogger.get_name()); -} diff --git a/rclcpp/test/test_logging.cpp b/rclcpp/test/test_logging.cpp deleted file mode 100644 index 323a3abdb0..0000000000 --- a/rclcpp/test/test_logging.cpp +++ /dev/null @@ -1,141 +0,0 @@ -// 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 "rclcpp/logger.hpp" -#include "rclcpp/logging.hpp" -#include "rcutils/logging.h" -#include "rcutils/time.h" - -using ::testing::EndsWith; - -size_t g_log_calls = 0; -rclcpp::Logger g_logger = rclcpp::get_logger("name"); - -struct LogEvent -{ - rcutils_log_location_t * location; - int level; - std::string name; - std::string message; -}; -LogEvent g_last_log_event; - -class TestLoggingMacros : public ::testing::Test -{ -public: - rcutils_logging_output_handler_t previous_output_handler; - void SetUp() - { - g_log_calls = 0; - ASSERT_EQ(RCUTILS_RET_OK, rcutils_logging_initialize()); - rcutils_logging_set_default_logger_level(RCUTILS_LOG_SEVERITY_DEBUG); - - auto rcutils_logging_console_output_handler = []( - rcutils_log_location_t * location, - int level, const char * name, const char * format, va_list * args) -> void - { - g_log_calls += 1; - g_last_log_event.location = location; - g_last_log_event.level = level; - g_last_log_event.name = name ? name : ""; - char buffer[1024]; - vsnprintf(buffer, sizeof(buffer), format, *args); - g_last_log_event.message = buffer; - }; - - this->previous_output_handler = rcutils_logging_get_output_handler(); - rcutils_logging_set_output_handler(rcutils_logging_console_output_handler); - } - - void TearDown() - { - rcutils_logging_set_output_handler(this->previous_output_handler); - g_rcutils_logging_initialized = false; - EXPECT_FALSE(g_rcutils_logging_initialized); - } -}; - -TEST_F(TestLoggingMacros, test_logging_named) { - for (int i : {1, 2, 3}) { - RCLCPP_DEBUG(g_logger, "message %d", i); - } - EXPECT_EQ(3u, g_log_calls); - EXPECT_TRUE(g_last_log_event.location != NULL); - if (g_last_log_event.location) { - EXPECT_STREQ("TestBody", g_last_log_event.location->function_name); - EXPECT_THAT(g_last_log_event.location->file_name, EndsWith("test_logging.cpp")); - EXPECT_EQ(78u, g_last_log_event.location->line_number); - } - EXPECT_EQ(RCUTILS_LOG_SEVERITY_DEBUG, g_last_log_event.level); - EXPECT_EQ("name", g_last_log_event.name); - EXPECT_EQ("message 3", g_last_log_event.message); -} - -TEST_F(TestLoggingMacros, test_logging_once) { - for (int i : {1, 2, 3}) { - RCLCPP_INFO_ONCE(g_logger, "message %d", i); - } - EXPECT_EQ(1u, g_log_calls); - EXPECT_EQ(RCUTILS_LOG_SEVERITY_INFO, g_last_log_event.level); - EXPECT_EQ("name", g_last_log_event.name); - EXPECT_EQ("message 1", g_last_log_event.message); - - // Check that another instance has a context that's independent to the call above's - g_log_calls = 0; - for (int i : {1, 2, 3}) { - RCLCPP_INFO_ONCE(g_logger, "second message %d", i); - } - EXPECT_EQ(1u, g_log_calls); - EXPECT_EQ(RCUTILS_LOG_SEVERITY_INFO, g_last_log_event.level); - EXPECT_EQ("name", g_last_log_event.name); - EXPECT_EQ("second message 1", g_last_log_event.message); -} - -TEST_F(TestLoggingMacros, test_logging_expression) { - for (int i : {1, 2, 3, 4, 5, 6}) { - RCLCPP_INFO_EXPRESSION(g_logger, i % 3, "message %d", i); - } - EXPECT_EQ(4u, g_log_calls); - EXPECT_EQ("message 5", g_last_log_event.message); -} - -int g_counter = 0; - -bool mod3() -{ - return (g_counter % 3) != 0; -} - -TEST_F(TestLoggingMacros, test_logging_function) { - for (int i : {1, 2, 3, 4, 5, 6}) { - g_counter = i; - RCLCPP_INFO_FUNCTION(g_logger, &mod3, "message %d", i); - } - EXPECT_EQ(4u, g_log_calls); - EXPECT_EQ("message 5", g_last_log_event.message); -} - -TEST_F(TestLoggingMacros, test_logging_skipfirst) { - for (uint32_t i : {1, 2, 3, 4, 5}) { - RCLCPP_WARN_SKIPFIRST(g_logger, "message %u", i); - EXPECT_EQ(i - 1, g_log_calls); - } -} diff --git a/rclcpp/test/test_mapped_ring_buffer.cpp b/rclcpp/test/test_mapped_ring_buffer.cpp deleted file mode 100644 index 528c312778..0000000000 --- a/rclcpp/test/test_mapped_ring_buffer.cpp +++ /dev/null @@ -1,175 +0,0 @@ -// 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 - -#define RCLCPP_BUILDING_LIBRARY 1 // Prevent including unavailable symbols -#include - -#include - -/* - Tests get_copy and pop on an empty mrb. - */ -TEST(TestMappedRingBuffer, empty) { - // Cannot create a buffer of size zero. - EXPECT_THROW(rclcpp::mapped_ring_buffer::MappedRingBuffer mrb(0), std::invalid_argument); - // Getting or popping an empty buffer should result in a nullptr. - rclcpp::mapped_ring_buffer::MappedRingBuffer mrb(1); - - std::unique_ptr actual; - mrb.get_copy_at_key(1, actual); - EXPECT_EQ(nullptr, actual); - - mrb.pop_at_key(1, actual); - EXPECT_EQ(nullptr, actual); -} - -/* - Tests push_and_replace with a temporary object. - */ -TEST(TestMappedRingBuffer, temporary_l_value) { - rclcpp::mapped_ring_buffer::MappedRingBuffer mrb(2); - // Pass in value with temporary object - mrb.push_and_replace(1, std::unique_ptr(new char('a'))); - - std::unique_ptr actual; - mrb.get_copy_at_key(1, actual); - EXPECT_EQ('a', *actual); - - mrb.pop_at_key(1, actual); - EXPECT_EQ('a', *actual); - - mrb.get_copy_at_key(1, actual); - EXPECT_EQ(nullptr, actual); -} - -/* - Tests normal usage of the mrb. - */ -TEST(TestMappedRingBuffer, nominal) { - rclcpp::mapped_ring_buffer::MappedRingBuffer mrb(2); - std::unique_ptr expected(new char('a')); - // Store expected value's address for later comparison. - char * expected_orig = expected.get(); - - EXPECT_FALSE(mrb.push_and_replace(1, expected)); - - std::unique_ptr actual; - mrb.get_copy_at_key(1, actual); - EXPECT_NE(nullptr, actual); - if (actual) { - EXPECT_EQ('a', *actual); - } - EXPECT_NE(expected_orig, actual.get()); - - mrb.pop_at_key(1, actual); - EXPECT_NE(nullptr, actual); - if (actual) { - EXPECT_EQ('a', *actual); - } - EXPECT_EQ(expected_orig, actual.get()); - - mrb.get_copy_at_key(1, actual); - EXPECT_EQ(nullptr, actual); - - expected.reset(new char('a')); - EXPECT_FALSE(mrb.push_and_replace(1, expected)); - - expected.reset(new char('b')); - EXPECT_FALSE(mrb.push_and_replace(2, expected)); - - expected.reset(new char('c')); - EXPECT_TRUE(mrb.push_and_replace(3, expected)); - - mrb.get_copy_at_key(1, actual); - EXPECT_EQ(nullptr, actual); - - mrb.get_copy_at_key(2, actual); - EXPECT_NE(nullptr, actual); - if (actual) { - EXPECT_EQ('b', *actual); - } - - mrb.get_copy_at_key(3, actual); - EXPECT_NE(nullptr, actual); - if (actual) { - EXPECT_EQ('c', *actual); - } -} - -/* - Tests get_ownership on a normal mrb. - */ -TEST(TestMappedRingBuffer, get_ownership) { - rclcpp::mapped_ring_buffer::MappedRingBuffer mrb(2); - std::unique_ptr expected(new char('a')); - // Store expected value's address for later comparison. - char * expected_orig = expected.get(); - - EXPECT_FALSE(mrb.push_and_replace(1, expected)); - - std::unique_ptr actual; - mrb.get_copy_at_key(1, actual); - EXPECT_NE(nullptr, actual); - if (actual) { - EXPECT_EQ('a', *actual); - } - EXPECT_NE(expected_orig, actual.get()); - - mrb.get_ownership_at_key(1, actual); - EXPECT_NE(nullptr, actual); - if (actual) { - EXPECT_EQ('a', *actual); - } - EXPECT_EQ(expected_orig, actual.get()); - - mrb.pop_at_key(1, actual); - EXPECT_NE(nullptr, actual); - if (actual) { - EXPECT_EQ('a', *actual); // The value should be the same. - } - EXPECT_NE(expected_orig, actual.get()); // Even though we pop'ed, we didn't get the original. - - mrb.get_copy_at_key(1, actual); - EXPECT_EQ(nullptr, actual); -} - -/* - Tests the affect of reusing keys (non-unique keys) in a mrb. - */ -TEST(TestMappedRingBuffer, non_unique_keys) { - rclcpp::mapped_ring_buffer::MappedRingBuffer mrb(2); - - std::unique_ptr input(new char('a')); - mrb.push_and_replace(1, input); - input.reset(new char('b')); - - // Different value, same key. - mrb.push_and_replace(1, input); - - std::unique_ptr actual; - mrb.pop_at_key(1, actual); - EXPECT_NE(nullptr, actual); - if (actual) { - EXPECT_EQ('a', *actual); - } - - actual = nullptr; - mrb.pop_at_key(1, actual); - EXPECT_NE(nullptr, actual); - if (actual) { - EXPECT_EQ('b', *actual); - } -} diff --git a/rclcpp/test/test_node.cpp b/rclcpp/test/test_node.cpp deleted file mode 100644 index 2346250538..0000000000 --- a/rclcpp/test/test_node.cpp +++ /dev/null @@ -1,103 +0,0 @@ -// 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 "rclcpp/exceptions.hpp" -#include "rclcpp/node.hpp" -#include "rclcpp/rclcpp.hpp" - -class TestNode : public ::testing::Test -{ -protected: - static void SetUpTestCase() - { - rclcpp::init(0, nullptr); - } -}; - -/* - Testing node construction and destruction. - */ -TEST_F(TestNode, construction_and_destruction) { - { - auto node = std::make_shared("my_node", "/ns"); - } - - { - ASSERT_THROW({ - auto node = std::make_shared("invalid_node?", "/ns"); - }, rclcpp::exceptions::InvalidNodeNameError); - } - - { - ASSERT_THROW({ - auto node = std::make_shared("my_node", "/invalid_ns?"); - }, rclcpp::exceptions::InvalidNamespaceError); - } -} - -TEST_F(TestNode, get_name_and_namespace) { - { - auto node = std::make_shared("my_node", "/ns"); - EXPECT_STREQ("my_node", node->get_name()); - EXPECT_STREQ("/ns", node->get_namespace()); - } - { - auto node = std::make_shared("my_node", "ns"); - EXPECT_STREQ("my_node", node->get_name()); - EXPECT_STREQ("/ns", node->get_namespace()); - } - { - auto node = std::make_shared("my_node", "/my/ns"); - EXPECT_STREQ("my_node", node->get_name()); - EXPECT_STREQ("/my/ns", node->get_namespace()); - } - { - auto node = std::make_shared("my_node", "my/ns"); - EXPECT_STREQ("my_node", node->get_name()); - EXPECT_STREQ("/my/ns", node->get_namespace()); - } -} - -TEST_F(TestNode, get_logger) { - // Currently the namespace is not taken into account with the node logger name - { - auto node = std::make_shared("my_node"); - EXPECT_STREQ("my_node", node->get_logger().get_name()); - } - { - auto node = std::make_shared("my_node", "/ns"); - EXPECT_STREQ("my_node", node->get_logger().get_name()); - } -} - -TEST_F(TestNode, get_clock) { - auto node = std::make_shared("my_node", "/ns"); - auto ros_clock = node->get_clock(); - EXPECT_TRUE(ros_clock != nullptr); - EXPECT_EQ(ros_clock->get_clock_type(), RCL_ROS_TIME); -} - -TEST_F(TestNode, now) { - auto node = std::make_shared("my_node", "/ns"); - auto clock = node->get_clock(); - auto now_builtin = node->now().nanoseconds(); - auto now_external = clock->now().nanoseconds(); - EXPECT_GE(now_external, now_builtin); - EXPECT_LT(now_external - now_builtin, 50000L); -} diff --git a/rclcpp/test/test_publisher.cpp b/rclcpp/test/test_publisher.cpp deleted file mode 100644 index d60be7b062..0000000000 --- a/rclcpp/test/test_publisher.cpp +++ /dev/null @@ -1,60 +0,0 @@ -// 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 "rclcpp/exceptions.hpp" -#include "rclcpp/rclcpp.hpp" - -#include "rcl_interfaces/msg/intra_process_message.hpp" - -class TestPublisher : public ::testing::Test -{ -protected: - static void SetUpTestCase() - { - rclcpp::init(0, nullptr); - } - - void SetUp() - { - node = std::make_shared("my_node", "/ns"); - } - - void TearDown() - { - node.reset(); - } - - rclcpp::Node::SharedPtr node; -}; - -/* - Testing publisher construction and destruction. - */ -TEST_F(TestPublisher, construction_and_destruction) { - using rcl_interfaces::msg::IntraProcessMessage; - { - auto publisher = node->create_publisher("topic"); - } - - { - ASSERT_THROW({ - auto publisher = node->create_publisher("invalid_topic?"); - }, rclcpp::exceptions::InvalidTopicNameError); - } -} diff --git a/rclcpp/test/test_rate.cpp b/rclcpp/test/test_rate.cpp deleted file mode 100644 index a4c1c4386a..0000000000 --- a/rclcpp/test/test_rate.cpp +++ /dev/null @@ -1,100 +0,0 @@ -// 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 "rclcpp/rate.hpp" - -/* - Basic tests for the Rate and WallRate classes. - */ -TEST(TestRate, rate_basics) { - auto period = std::chrono::milliseconds(100); - auto offset = std::chrono::milliseconds(50); - auto epsilon = std::chrono::milliseconds(1); - double overrun_ratio = 1.5; - - auto start = std::chrono::system_clock::now(); - rclcpp::Rate r(period); - ASSERT_FALSE(r.is_steady()); - ASSERT_TRUE(r.sleep()); - auto one = std::chrono::system_clock::now(); - auto delta = one - start; - ASSERT_TRUE(period < delta); - ASSERT_TRUE(period * overrun_ratio > delta); - - rclcpp::sleep_for(offset); - ASSERT_TRUE(r.sleep()); - auto two = std::chrono::system_clock::now(); - delta = two - start; - ASSERT_TRUE(2 * period < delta); - ASSERT_TRUE(2 * period * overrun_ratio > delta); - - rclcpp::sleep_for(offset); - auto two_offset = std::chrono::system_clock::now(); - r.reset(); - ASSERT_TRUE(r.sleep()); - auto three = std::chrono::system_clock::now(); - delta = three - two_offset; - ASSERT_TRUE(period < delta); - ASSERT_TRUE(period * overrun_ratio > delta); - - rclcpp::sleep_for(offset + period); - auto four = std::chrono::system_clock::now(); - ASSERT_FALSE(r.sleep()); - auto five = std::chrono::system_clock::now(); - delta = five - four; - ASSERT_TRUE(epsilon > delta); -} - -TEST(TestRate, wall_rate_basics) { - auto period = std::chrono::milliseconds(100); - auto offset = std::chrono::milliseconds(50); - auto epsilon = std::chrono::milliseconds(1); - double overrun_ratio = 1.5; - - auto start = std::chrono::steady_clock::now(); - rclcpp::WallRate r(period); - ASSERT_TRUE(r.is_steady()); - ASSERT_TRUE(r.sleep()); - auto one = std::chrono::steady_clock::now(); - auto delta = one - start; - ASSERT_TRUE(period < delta); - ASSERT_TRUE(period * overrun_ratio > delta); - - rclcpp::sleep_for(offset); - ASSERT_TRUE(r.sleep()); - auto two = std::chrono::steady_clock::now(); - delta = two - start; - ASSERT_TRUE(2 * period < delta + epsilon); - ASSERT_TRUE(2 * period * overrun_ratio > delta); - - rclcpp::sleep_for(offset); - auto two_offset = std::chrono::steady_clock::now(); - r.reset(); - ASSERT_TRUE(r.sleep()); - auto three = std::chrono::steady_clock::now(); - delta = three - two_offset; - ASSERT_TRUE(period < delta); - ASSERT_TRUE(period * overrun_ratio > delta); - - rclcpp::sleep_for(offset + period); - auto four = std::chrono::steady_clock::now(); - ASSERT_FALSE(r.sleep()); - auto five = std::chrono::steady_clock::now(); - delta = five - four; - ASSERT_TRUE(epsilon > delta); -} diff --git a/rclcpp/test/test_subscription.cpp b/rclcpp/test/test_subscription.cpp deleted file mode 100644 index e3be4c70ac..0000000000 --- a/rclcpp/test/test_subscription.cpp +++ /dev/null @@ -1,63 +0,0 @@ -// 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 "rclcpp/exceptions.hpp" -#include "rclcpp/rclcpp.hpp" - -#include "rcl_interfaces/msg/intra_process_message.hpp" - -class TestSubscription : public ::testing::Test -{ -protected: - static void SetUpTestCase() - { - rclcpp::init(0, nullptr); - } - - void SetUp() - { - node = std::make_shared("my_node", "/ns"); - } - - void TearDown() - { - node.reset(); - } - - rclcpp::Node::SharedPtr node; -}; - -/* - Testing subscription construction and destruction. - */ -TEST_F(TestSubscription, construction_and_destruction) { - using rcl_interfaces::msg::IntraProcessMessage; - auto callback = [](const IntraProcessMessage::SharedPtr msg) { - (void)msg; - }; - { - auto sub = node->create_subscription("topic", callback); - } - - { - ASSERT_THROW({ - auto sub = node->create_subscription("invalid_topic?", callback); - }, rclcpp::exceptions::InvalidTopicNameError); - } -} diff --git a/rclcpp/test/test_time.cpp b/rclcpp/test/test_time.cpp deleted file mode 100644 index 69e37789b6..0000000000 --- a/rclcpp/test/test_time.cpp +++ /dev/null @@ -1,246 +0,0 @@ -// 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/error_handling.h" -#include "rcl/time.h" -#include "rclcpp/clock.hpp" -#include "rclcpp/rclcpp.hpp" -#include "rclcpp/time.hpp" -#include "rclcpp/utilities.hpp" - -namespace -{ - -bool logical_eq(const bool a, const bool b) -{ - return (a && b) || ((!a) && !(b)); -} - -} // namespace - - -class TestTime : public ::testing::Test -{ -protected: - static void SetUpTestCase() - { - rclcpp::init(0, nullptr); - } -}; - -TEST(TestTime, clock_type_access) { - rclcpp::Clock ros_clock(RCL_ROS_TIME); - EXPECT_EQ(RCL_ROS_TIME, ros_clock.get_clock_type()); - - rclcpp::Clock system_clock(RCL_SYSTEM_TIME); - EXPECT_EQ(RCL_SYSTEM_TIME, system_clock.get_clock_type()); - - rclcpp::Clock steady_clock(RCL_STEADY_TIME); - EXPECT_EQ(RCL_STEADY_TIME, steady_clock.get_clock_type()); -} - -TEST(TestTime, time_sources) { - using builtin_interfaces::msg::Time; - rclcpp::Clock ros_clock(RCL_ROS_TIME); - Time ros_now = ros_clock.now(); - EXPECT_NE(0, ros_now.sec); - EXPECT_NE(0u, ros_now.nanosec); - - rclcpp::Clock system_clock(RCL_ROS_TIME); - Time system_now = system_clock.now(); - EXPECT_NE(0, system_now.sec); - EXPECT_NE(0u, system_now.nanosec); - - rclcpp::Clock steady_clock(RCL_STEADY_TIME); - Time steady_now = steady_clock.now(); - EXPECT_NE(0, steady_now.sec); - EXPECT_NE(0u, steady_now.nanosec); -} - -TEST(TestTime, conversions) { - rclcpp::Clock system_clock(RCL_ROS_TIME); - - rclcpp::Time now = system_clock.now(); - builtin_interfaces::msg::Time now_msg = now; - - rclcpp::Time now_again = now_msg; - EXPECT_EQ(now.nanoseconds(), now_again.nanoseconds()); - - builtin_interfaces::msg::Time msg; - msg.sec = 12345; - msg.nanosec = 67890; - - rclcpp::Time time = msg; - EXPECT_EQ( - RCL_S_TO_NS(static_cast(msg.sec)) + static_cast(msg.nanosec), - time.nanoseconds()); - EXPECT_EQ(static_cast(msg.sec), RCL_NS_TO_S(time.nanoseconds())); - - builtin_interfaces::msg::Time negative_time_msg; - negative_time_msg.sec = -1; - negative_time_msg.nanosec = 1; - - EXPECT_ANY_THROW({ - rclcpp::Time negative_time = negative_time_msg; - }); - - EXPECT_ANY_THROW(rclcpp::Time(-1, 1)); - - EXPECT_ANY_THROW({ - rclcpp::Time assignment(1, 2); - assignment = negative_time_msg; - }); -} - -TEST(TestTime, operators) { - rclcpp::Time old(1, 0); - rclcpp::Time young(2, 0); - - EXPECT_TRUE(old < young); - EXPECT_TRUE(young > old); - EXPECT_TRUE(old <= young); - EXPECT_TRUE(young >= old); - EXPECT_FALSE(young == old); - EXPECT_TRUE(young != old); - - rclcpp::Duration sub = young - old; - EXPECT_EQ(sub.nanoseconds(), (rcl_duration_value_t)(young.nanoseconds() - old.nanoseconds())); - EXPECT_EQ(sub, young - old); - - rclcpp::Time system_time(0, 0, RCL_SYSTEM_TIME); - rclcpp::Time steady_time(0, 0, RCL_STEADY_TIME); - - EXPECT_ANY_THROW((void)(system_time == steady_time)); - EXPECT_ANY_THROW((void)(system_time != steady_time)); - EXPECT_ANY_THROW((void)(system_time <= steady_time)); - EXPECT_ANY_THROW((void)(system_time >= steady_time)); - EXPECT_ANY_THROW((void)(system_time < steady_time)); - EXPECT_ANY_THROW((void)(system_time > steady_time)); - EXPECT_ANY_THROW((void)(system_time - steady_time)); - - rclcpp::Clock system_clock(RCL_ROS_TIME); - rclcpp::Clock steady_clock(RCL_STEADY_TIME); - - rclcpp::Time now = system_clock.now(); - rclcpp::Time later = steady_clock.now(); - - EXPECT_ANY_THROW((void)(now == later)); - EXPECT_ANY_THROW((void)(now != later)); - EXPECT_ANY_THROW((void)(now <= later)); - EXPECT_ANY_THROW((void)(now >= later)); - EXPECT_ANY_THROW((void)(now < later)); - EXPECT_ANY_THROW((void)(now > later)); - EXPECT_ANY_THROW((void)(now - later)); - - for (auto time_source : {RCL_ROS_TIME, RCL_SYSTEM_TIME, RCL_STEADY_TIME}) { - rclcpp::Time time = rclcpp::Time(0, 0, time_source); - rclcpp::Time copy_constructor_time(time); - rclcpp::Time assignment_op_time = rclcpp::Time(1, 0, time_source); - assignment_op_time = time; - - EXPECT_TRUE(time == copy_constructor_time); - EXPECT_TRUE(time == assignment_op_time); - } -} - -TEST(TestTime, overflow_detectors) { - ///////////////////////////////////////////////////////////////////////////// - // Test logical_eq call first: - EXPECT_TRUE(logical_eq(false, false)); - EXPECT_FALSE(logical_eq(false, true)); - EXPECT_FALSE(logical_eq(true, false)); - EXPECT_TRUE(logical_eq(true, true)); - - ///////////////////////////////////////////////////////////////////////////// - // Exhaustive test of all int8_t values - using test_type_t = int8_t; - // big_type_t encompasses test_type_t: - // big_type_t::min < test_type_t::min - // big_type_t::max > test_type_t::max - using big_type_t = int16_t; - const big_type_t min_val = std::numeric_limits::min(); - const big_type_t max_val = std::numeric_limits::max(); - // 256 * 256 = 64K total loops, should be pretty fast on everything - for (big_type_t y = min_val; y <= max_val; ++y) { - for (big_type_t x = min_val; x <= max_val; ++x) { - const big_type_t sum = x + y; - const big_type_t diff = x - y; - - const bool add_will_overflow = - rclcpp::add_will_overflow(test_type_t(x), test_type_t(y)); - const bool add_did_overflow = sum > max_val; - EXPECT_TRUE(logical_eq(add_will_overflow, add_did_overflow)); - - const bool add_will_underflow = - rclcpp::add_will_underflow(test_type_t(x), test_type_t(y)); - const bool add_did_underflow = sum < min_val; - EXPECT_TRUE(logical_eq(add_will_underflow, add_did_underflow)); - - const bool sub_will_overflow = - rclcpp::sub_will_overflow(test_type_t(x), test_type_t(y)); - const bool sub_did_overflow = diff > max_val; - EXPECT_TRUE(logical_eq(sub_will_overflow, sub_did_overflow)); - - const bool sub_will_underflow = - rclcpp::sub_will_underflow(test_type_t(x), test_type_t(y)); - const bool sub_did_underflow = diff < min_val; - EXPECT_TRUE(logical_eq(sub_will_underflow, sub_did_underflow)); - } - } - - // Few selected tests for int64_t - EXPECT_TRUE(rclcpp::add_will_overflow(INT64_MAX, 1)); - EXPECT_FALSE(rclcpp::add_will_overflow(INT64_MAX, -1)); - EXPECT_TRUE(rclcpp::add_will_underflow(INT64_MIN, -1)); - EXPECT_FALSE(rclcpp::add_will_underflow(INT64_MIN, 1)); - - EXPECT_FALSE(rclcpp::sub_will_overflow(INT64_MAX, 1)); - EXPECT_TRUE(rclcpp::sub_will_overflow(INT64_MAX, -1)); - EXPECT_FALSE(rclcpp::sub_will_underflow(INT64_MIN, -1)); - EXPECT_TRUE(rclcpp::sub_will_underflow(INT64_MIN, 1)); -} - -TEST(TestTime, overflows) { - rclcpp::Time max_time(std::numeric_limits::max()); - rclcpp::Time min_time(std::numeric_limits::min()); - rclcpp::Duration one(1); - rclcpp::Duration two(2); - - // Cross min/max - EXPECT_THROW(max_time + one, std::overflow_error); - EXPECT_THROW(min_time - one, std::underflow_error); - EXPECT_THROW(max_time - min_time, std::overflow_error); - EXPECT_THROW(min_time - max_time, std::underflow_error); - EXPECT_NO_THROW(max_time - max_time); - EXPECT_NO_THROW(min_time - min_time); - - // Cross zero in both directions - rclcpp::Time one_time(1); - EXPECT_NO_THROW(one_time - two); - - rclcpp::Time minus_one_time(-1); - EXPECT_NO_THROW(minus_one_time + two); - - EXPECT_NO_THROW(one_time - minus_one_time); - EXPECT_NO_THROW(minus_one_time - one_time); - - rclcpp::Time two_time(2); - EXPECT_NO_THROW(one_time - two_time); -} diff --git a/rclcpp/test/test_time_source.cpp b/rclcpp/test/test_time_source.cpp deleted file mode 100644 index a6f620ab7b..0000000000 --- a/rclcpp/test/test_time_source.cpp +++ /dev/null @@ -1,375 +0,0 @@ -// 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 "rcl/error_handling.h" -#include "rcl/time.h" -#include "rclcpp/clock.hpp" -#include "rclcpp/rclcpp.hpp" -#include "rclcpp/time.hpp" -#include "rclcpp/time_source.hpp" - -using namespace std::chrono_literals; - -class TestTimeSource : public ::testing::Test -{ -protected: - static void SetUpTestCase() - { - rclcpp::init(0, nullptr); - } - - void SetUp() - { - node = std::make_shared("my_node"); - } - - void TearDown() - { - node.reset(); - } - - rclcpp::Node::SharedPtr node; -}; - - -TEST_F(TestTimeSource, detachUnattached) { - rclcpp::TimeSource ts; - - ASSERT_NO_THROW(ts.detachNode()); - - // Try multiple detach to see if error - ASSERT_NO_THROW(ts.detachNode()); -} - -TEST_F(TestTimeSource, reattach) { - rclcpp::TimeSource ts; - // Try reattach - ASSERT_NO_THROW(ts.attachNode(node)); - ASSERT_NO_THROW(ts.attachNode(node)); -} - -TEST_F(TestTimeSource, ROS_time_valid) { - rclcpp::TimeSource ts; - auto ros_clock = std::make_shared(RCL_ROS_TIME); - - EXPECT_FALSE(ros_clock->ros_time_is_active()); - ts.attachClock(ros_clock); - auto now = ros_clock->now(); - EXPECT_FALSE(ros_clock->ros_time_is_active()); - - ts.attachNode(node); - EXPECT_FALSE(ros_clock->ros_time_is_active()); - - ts.detachNode(); - EXPECT_FALSE(ros_clock->ros_time_is_active()); - - ts.attachNode(node); - EXPECT_FALSE(ros_clock->ros_time_is_active()); -} - -TEST_F(TestTimeSource, clock) { - rclcpp::TimeSource ts(node); - auto ros_clock = std::make_shared(RCL_ROS_TIME); - EXPECT_FALSE(ros_clock->ros_time_is_active()); - ts.attachClock(ros_clock); - EXPECT_FALSE(ros_clock->ros_time_is_active()); - - auto clock_pub = node->create_publisher("clock", - rmw_qos_profile_default); - rclcpp::WallRate loop_rate(50); - for (int i = 0; i < 5; ++i) { - if (!rclcpp::ok()) { - break; // Break for ctrl-c - } - auto msg = std::make_shared(); - msg->sec = i; - msg->nanosec = 1000; - clock_pub->publish(msg); - // std::cout << "Publishing: '" << msg->sec << ".000000" << msg->nanosec << "'" << std::endl; - rclcpp::spin_some(node); - loop_rate.sleep(); - } - auto t_low = rclcpp::Time(1, 0, RCL_ROS_TIME); - auto t_high = rclcpp::Time(10, 100000, RCL_ROS_TIME); - - // Now that we've recieved a message it should be active with parameter unset - EXPECT_TRUE(ros_clock->ros_time_is_active()); - - auto t_out = ros_clock->now(); - - EXPECT_NE(0L, t_out.nanoseconds()); - EXPECT_LT(t_low.nanoseconds(), t_out.nanoseconds()); - EXPECT_GT(t_high.nanoseconds(), t_out.nanoseconds()); -} - -class CallbackObject -{ -public: - CallbackObject() - : last_precallback_id_(0), - last_postcallback_id_(0) - {} - int last_precallback_id_; - void pre_callback(int id) {last_precallback_id_ = id;} - - int last_postcallback_id_; - rclcpp::TimeJump last_timejump_; - void post_callback(const rclcpp::TimeJump & jump, int id) - { - last_postcallback_id_ = id; last_timejump_ = jump; - } -}; - -TEST_F(TestTimeSource, callbacks) { - CallbackObject cbo; - rclcpp::JumpThreshold jump_threshold; - jump_threshold.min_forward_ = 0; - jump_threshold.min_backward_ = 0; - jump_threshold.on_clock_change_ = true; - - rclcpp::TimeSource ts(node); - auto ros_clock = std::make_shared(RCL_ROS_TIME); - - // Register a callback for time jumps - rclcpp::JumpHandler::SharedPtr callback_handler = ros_clock->create_jump_callback( - std::bind(&CallbackObject::pre_callback, &cbo, 1), - std::bind(&CallbackObject::post_callback, &cbo, std::placeholders::_1, 1), - jump_threshold); - - EXPECT_EQ(0, cbo.last_precallback_id_); - EXPECT_EQ(0, cbo.last_postcallback_id_); - - EXPECT_FALSE(ros_clock->ros_time_is_active()); - - ts.attachClock(ros_clock); - EXPECT_FALSE(ros_clock->ros_time_is_active()); - - auto clock_pub = node->create_publisher("clock", - rmw_qos_profile_default); - - rclcpp::WallRate loop_rate(50); - for (int i = 0; i < 5; ++i) { - if (!rclcpp::ok()) { - break; // Break for ctrl-c - } - auto msg = std::make_shared(); - msg->sec = i; - msg->nanosec = 1000; - clock_pub->publish(msg); - // std::cout << "Publishing: '" << msg->sec << ".000000" << msg->nanosec << "'" << std::endl; - rclcpp::spin_some(node); - loop_rate.sleep(); - } - auto t_low = rclcpp::Time(1, 0, RCL_ROS_TIME); - auto t_high = rclcpp::Time(10, 100000, RCL_ROS_TIME); - - EXPECT_EQ(1, cbo.last_precallback_id_); - EXPECT_EQ(1, cbo.last_postcallback_id_); - - // Now that we've recieved a message it should be active with parameter unset - EXPECT_TRUE(ros_clock->ros_time_is_active()); - - auto t_out = ros_clock->now(); - - EXPECT_NE(0L, t_out.nanoseconds()); - EXPECT_LT(t_low.nanoseconds(), t_out.nanoseconds()); - EXPECT_GT(t_high.nanoseconds(), t_out.nanoseconds()); - - - // Change callbacks - rclcpp::JumpHandler::SharedPtr callback_handler2 = ros_clock->create_jump_callback( - std::bind(&CallbackObject::pre_callback, &cbo, 2), - std::bind(&CallbackObject::post_callback, &cbo, std::placeholders::_1, 2), - jump_threshold); - - for (int i = 0; i < 5; ++i) { - if (!rclcpp::ok()) { - break; // Break for ctrl-c - } - auto msg = std::make_shared(); - msg->sec = i; - msg->nanosec = 2000; - clock_pub->publish(msg); - // std::cout << "Publishing: '" << msg->sec << ".000000" << msg->nanosec << "'" << std::endl; - rclcpp::spin_some(node); - loop_rate.sleep(); - } - - EXPECT_EQ(2, cbo.last_precallback_id_); - EXPECT_EQ(2, cbo.last_postcallback_id_); - - // Now that we've recieved a message it should be active with parameter unset - EXPECT_TRUE(ros_clock->ros_time_is_active()); - - t_out = ros_clock->now(); - - EXPECT_NE(0L, t_out.nanoseconds()); - EXPECT_LT(t_low.nanoseconds(), t_out.nanoseconds()); - EXPECT_GT(t_high.nanoseconds(), t_out.nanoseconds()); -} - -void trigger_clock_changes( - rclcpp::Node::SharedPtr node) -{ - auto clock_pub = node->create_publisher("clock", - rmw_qos_profile_default); - - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node); - - rclcpp::WallRate loop_rate(50); - for (int i = 0; i < 5; ++i) { - if (!rclcpp::ok()) { - break; // Break for ctrl-c - } - auto msg = std::make_shared(); - msg->sec = i; - msg->nanosec = 1000; - clock_pub->publish(msg); - // std::cout << "Publishing: '" << msg->sec << ".000000" << msg->nanosec << "'" << std::endl; - executor.spin_once(1000000ns); - loop_rate.sleep(); - } -} - - -TEST_F(TestTimeSource, callback_handler_erasure) { - CallbackObject cbo; - rclcpp::JumpThreshold jump_threshold; - jump_threshold.min_forward_ = 0; - jump_threshold.min_backward_ = 0; - jump_threshold.on_clock_change_ = true; - - rclcpp::TimeSource ts(node); - auto ros_clock = std::make_shared(RCL_ROS_TIME); - ts.attachClock(ros_clock); - EXPECT_FALSE(ros_clock->ros_time_is_active()); - - // Register a callback for time jumps - rclcpp::JumpHandler::SharedPtr callback_handler = ros_clock->create_jump_callback( - std::bind(&CallbackObject::pre_callback, &cbo, 1), - std::bind(&CallbackObject::post_callback, &cbo, std::placeholders::_1, 1), - jump_threshold); - - // Second callback handler - rclcpp::JumpHandler::SharedPtr callback_handler2 = ros_clock->create_jump_callback( - std::bind(&CallbackObject::pre_callback, &cbo, 1), - std::bind(&CallbackObject::post_callback, &cbo, std::placeholders::_1, 1), - jump_threshold); - - - EXPECT_EQ(0, cbo.last_precallback_id_); - EXPECT_EQ(0, cbo.last_postcallback_id_); - - - EXPECT_FALSE(ros_clock->ros_time_is_active()); - - trigger_clock_changes(node); - - auto t_low = rclcpp::Time(1, 0, RCL_ROS_TIME); - auto t_high = rclcpp::Time(10, 100000, RCL_ROS_TIME); - - EXPECT_EQ(1, cbo.last_precallback_id_); - EXPECT_EQ(1, cbo.last_postcallback_id_); - - // Now that we've recieved a message it should be active with parameter unset - EXPECT_TRUE(ros_clock->ros_time_is_active()); - - auto t_out = ros_clock->now(); - - EXPECT_NE(0L, t_out.nanoseconds()); - EXPECT_LT(t_low.nanoseconds(), t_out.nanoseconds()); - EXPECT_GT(t_high.nanoseconds(), t_out.nanoseconds()); - - // Requeue a pointer in a new position - callback_handler = ros_clock->create_jump_callback( - std::bind(&CallbackObject::pre_callback, &cbo, 2), - std::bind(&CallbackObject::post_callback, &cbo, std::placeholders::_1, 2), - jump_threshold); - - // Remove the last callback in the vector - callback_handler2.reset(); - - trigger_clock_changes(node); - - - EXPECT_EQ(2, cbo.last_precallback_id_); - EXPECT_EQ(2, cbo.last_postcallback_id_); - - // Now that we've recieved a message it should be active with parameter unset - EXPECT_TRUE(ros_clock->ros_time_is_active()); - - t_out = ros_clock->now(); - - EXPECT_NE(0L, t_out.nanoseconds()); - EXPECT_LT(t_low.nanoseconds(), t_out.nanoseconds()); - EXPECT_GT(t_high.nanoseconds(), t_out.nanoseconds()); -} - - -TEST_F(TestTimeSource, parameter_activation) { - rclcpp::TimeSource ts(node); - auto ros_clock = std::make_shared(RCL_ROS_TIME); - EXPECT_FALSE(ros_clock->ros_time_is_active()); - - ts.attachClock(ros_clock); - EXPECT_FALSE(ros_clock->ros_time_is_active()); - - auto parameter_service = std::make_shared(node); - auto parameters_client = std::make_shared(node); - - using namespace std::chrono_literals; - EXPECT_TRUE(parameters_client->wait_for_service(2s)); - auto set_parameters_results = parameters_client->set_parameters({ - rclcpp::parameter::ParameterVariant("use_sim_time", true) - }); - for (auto & result : set_parameters_results) { - EXPECT_TRUE(result.successful); - } - rclcpp::spin_some(node); - EXPECT_TRUE(ros_clock->ros_time_is_active()); - - - set_parameters_results = parameters_client->set_parameters({ - rclcpp::parameter::ParameterVariant("use_sim_time", rclcpp::parameter::PARAMETER_NOT_SET) - }); - for (auto & result : set_parameters_results) { - EXPECT_TRUE(result.successful); - } - EXPECT_TRUE(ros_clock->ros_time_is_active()); - - set_parameters_results = parameters_client->set_parameters({ - rclcpp::parameter::ParameterVariant("use_sim_time", false) - }); - for (auto & result : set_parameters_results) { - EXPECT_TRUE(result.successful); - } - EXPECT_FALSE(ros_clock->ros_time_is_active()); - - set_parameters_results = parameters_client->set_parameters({ - rclcpp::parameter::ParameterVariant("use_sim_time", rclcpp::parameter::PARAMETER_NOT_SET) - }); - for (auto & result : set_parameters_results) { - EXPECT_TRUE(result.successful); - } - EXPECT_FALSE(ros_clock->ros_time_is_active()); -} diff --git a/rclcpp/test/utils/rclcpp_gtest_macros.hpp b/rclcpp/test/utils/rclcpp_gtest_macros.hpp new file mode 100644 index 0000000000..7dd708f974 --- /dev/null +++ b/rclcpp/test/utils/rclcpp_gtest_macros.hpp @@ -0,0 +1,195 @@ +// 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 UTILS__RCLCPP_GTEST_MACROS_HPP_ +#define UTILS__RCLCPP_GTEST_MACROS_HPP_ + +#include + +#include +#include + +#include "rclcpp/exceptions/exceptions.hpp" + +namespace rclcpp +{ +namespace testing +{ +namespace details +{ + +/** + * \brief Check if two thrown objects are equals. + * + * For generic thrown objects, probably is unlikely to be used. This type must + * overload the == and << operators. + */ +template::value>> +::testing::AssertionResult AreThrowableContentsEqual( + const T & expected, const T & actual, const char * expected_exception_expression, + const char * throwing_expression) +{ + if (expected == actual) { + return ::testing::AssertionSuccess() << + "'\nThe value of the non-standard throwable thrown by the expression\n'" << + throwing_expression << "'\n\nmatches the value of the expected thrown object\n'" << + expected_exception_expression << "'\n\t(" << expected << " == " << actual << ")\n"; + } + + return ::testing::AssertionFailure() << + "\nThe value of the non-standard throwable thrown by the expression\n'" << + throwing_expression << "'\n\ndoes not match the value of the expected thrown object\n'" << + expected_exception_expression << "'\n\t(" << expected << " != " << actual << ")\n"; +} + +/** + * \brief Check if two std::exceptions are equal according to their message. + * + * If the exception type also derives from rclcpp::Exception, then the next overload is called + * instead + */ +template::value>> +::testing::AssertionResult AreThrowableContentsEqual( + const std::exception & expected, const std::exception & actual, + const char * expected_exception_expression, + const char * throwing_expression) +{ + if (std::strcmp(expected.what(), actual.what()) == 0) { + return ::testing::AssertionSuccess() << + "'\nThe contents of the std::exception thrown by the expression\n'" << + throwing_expression << "':\n\te.what(): '" << actual.what() << + "'\n\nmatch the contents of the expected std::exception\n'" << + expected_exception_expression << "'\n\te.what(): '" << expected.what() << "'\n"; + } + + return ::testing::AssertionFailure() << + "\nThe contents of the std::exception thrown by the expression\n'" << + throwing_expression << "':\n\te.what(): '" << actual.what() << + "'\n\ndo not match the contents of the expected std::exception\n'" << + expected_exception_expression << "'\n\te.what(): '" << expected.what() << "'\n"; +} + +/** + * \brief Check if two exceptions that derive from rclcpp::RCLErrorBase are equal. + * + * This checks equality based on their return and message. It does not check the formatted + * message, which is what is reported by std::exception::what() for RCLErrors. + */ +template::value>> +::testing::AssertionResult AreThrowableContentsEqual( + const rclcpp::exceptions::RCLErrorBase & expected, + const rclcpp::exceptions::RCLErrorBase & actual, + const char * expected_exception_expression, + const char * throwing_expression) +{ + if ((expected.ret == actual.ret) && (expected.message == actual.message)) { + return ::testing::AssertionSuccess() << + "'\nThe contents of the RCLError thrown by the expression\n'" << throwing_expression << + "':\n\trcl_ret_t: " << actual.ret << "\n\tmessage: '" << actual.message << + "'\n\nmatch the contents of the expected RCLError\n'" << + expected_exception_expression << "'\n\trcl_ret_t: " << expected.ret << + "\n\tmessage: '" << expected.message << "'\n"; + } + + return ::testing::AssertionFailure() << + "'\nThe contents of the RCLError thrown by the expression\n'" << throwing_expression << + "':\n\trcl_ret_t: " << actual.ret << "\n\tmessage: '" << actual.message << + "'\n\ndo not match the contents of the expected RCLError\n'" << + expected_exception_expression << "'\n\trcl_ret_t: " << expected.ret << "\n\tmessage: '" << + expected.message << "'\n"; +} + +} // namespace details +} // namespace testing +} // namespace rclcpp + +/** + * \def CHECK_THROW_EQ_IMPL + * \brief Implemented check if statement throws expected exception. don't use directly, use + * RCLCPP_EXPECT_THROW_EQ or RCLCPP_ASSERT_THROW_EQ instead. + */ +#define CHECK_THROW_EQ_IMPL(throwing_statement, expected_exception, assertion_result) \ + do { \ + using ExceptionT = decltype(expected_exception); \ + try { \ + throwing_statement; \ + assertion_result = ::testing::AssertionFailure() << \ + "\nExpected the expression:\n\t'" #throwing_statement "'\nto throw: \n\t'" << \ + #expected_exception "'\nbut it did not throw.\n"; \ + } catch (const ExceptionT & e) { \ + assertion_result = \ + rclcpp::testing::details::AreThrowableContentsEqual( \ + expected_exception, e, #expected_exception, #throwing_statement); \ + } catch (const std::exception & e) { \ + assertion_result = ::testing::AssertionFailure() << \ + "\nExpected the expression:\n\t'" #throwing_statement "'\nto throw: \n\t'" << \ + #expected_exception "'\nbut it threw:\n\tType: " << typeid(e).name() << \ + "\n\te.what(): '" << e.what() << "'\n"; \ + } catch (...) { \ + assertion_result = ::testing::AssertionFailure() << \ + "\nExpected the expression:\n\t'" #throwing_statement "'\nto throw: \n\t'" << \ + #expected_exception "'\nbut it threw an unrecognized throwable type.\n"; \ + } \ + } while (0) + +/** + * \def RCLCPP_EXPECT_THROW_EQ + * \brief Check if a statement throws the expected exception type and that the exceptions matches + * the expected exception. + * + * Like other gtest EXPECT_ macros, this doesn't halt a test and return on failure. Instead it + * just adds a failure to the current test. + * + * See test_gtest_macros.cpp for examples + */ +#define RCLCPP_EXPECT_THROW_EQ(throwing_statement, expected_exception) \ + do { \ + ::testing::AssertionResult \ + is_the_result_of_the_throwing_expression_equal_to_the_expected_throwable = \ + ::testing::AssertionSuccess(); \ + CHECK_THROW_EQ_IMPL( \ + throwing_statement, \ + expected_exception, \ + is_the_result_of_the_throwing_expression_equal_to_the_expected_throwable); \ + EXPECT_TRUE(is_the_result_of_the_throwing_expression_equal_to_the_expected_throwable); \ + } while (0) + +/** + * \def RCLCPP_ASSERT_THROW_EQ + * \brief Assert that a statement throws the expected exception type and that the exceptions + * matches the expected exception. + * + * See test_gtest_macros.cpp for examples + * + * Like other gtest ASSERT_ macros, this will halt the test on failure and return. + */ +#define RCLCPP_ASSERT_THROW_EQ(throwing_statement, expected_exception) \ + do { \ + ::testing::AssertionResult \ + is_the_result_of_the_throwing_expression_equal_to_the_expected_throwable = \ + ::testing::AssertionSuccess(); \ + CHECK_THROW_EQ_IMPL( \ + throwing_statement, \ + expected_exception, \ + is_the_result_of_the_throwing_expression_equal_to_the_expected_throwable); \ + ASSERT_TRUE(is_the_result_of_the_throwing_expression_equal_to_the_expected_throwable); \ + } while (0) + +#endif // UTILS__RCLCPP_GTEST_MACROS_HPP_ diff --git a/rclcpp/test/utils/test_rclcpp_gtest_macros.cpp b/rclcpp/test/utils/test_rclcpp_gtest_macros.cpp new file mode 100644 index 0000000000..2c3b3ff20b --- /dev/null +++ b/rclcpp/test/utils/test_rclcpp_gtest_macros.cpp @@ -0,0 +1,204 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +#include + +#include "./rclcpp_gtest_macros.hpp" + +#include "rcl/rcl.h" +#include "rclcpp/rclcpp.hpp" + +struct NonStandardThrowable +{ + bool operator==(const NonStandardThrowable &) const + { + return true; + } +}; + +std::ostream & operator<<(std::ostream & os, const NonStandardThrowable &) +{ + os << "NonStandardThrowable"; + return os; +} + +TEST(TestGtestMacros, standard_exceptions) { + RCLCPP_EXPECT_THROW_EQ( + throw std::runtime_error("some runtime error"), + std::runtime_error("some runtime error")); + + RCLCPP_EXPECT_THROW_EQ( + throw std::invalid_argument("some invalid argument error"), + std::invalid_argument("some invalid argument error")); + + RCLCPP_ASSERT_THROW_EQ( + throw std::runtime_error("some runtime error"), + std::runtime_error("some runtime error")); + + RCLCPP_ASSERT_THROW_EQ( + throw std::invalid_argument("some invalid argument error"), + std::invalid_argument("some invalid argument error")); +} + +TEST(TestGtestMacros, standard_exceptions_not_equals) { + ::testing::AssertionResult result = ::testing::AssertionSuccess(); + CHECK_THROW_EQ_IMPL( + throw std::runtime_error("some runtime error"), + std::range_error("some runtime error"), + result); + EXPECT_FALSE(result); + + CHECK_THROW_EQ_IMPL( + throw std::invalid_argument("some invalid argument error"), + std::invalid_argument("some different invalid argument error"), + result); + EXPECT_FALSE(result); +} + +TEST(TestGTestMacros, non_standard_types) { + RCLCPP_EXPECT_THROW_EQ(throw 0, 0); + + RCLCPP_EXPECT_THROW_EQ(throw 42, 42); + + RCLCPP_EXPECT_THROW_EQ(throw std::string("some string"), std::string("some string")); + + RCLCPP_EXPECT_THROW_EQ(throw NonStandardThrowable(), NonStandardThrowable()); + + RCLCPP_ASSERT_THROW_EQ(throw 0, 0); + + RCLCPP_ASSERT_THROW_EQ(throw 42, 42); + + RCLCPP_ASSERT_THROW_EQ(throw std::string("some string"), std::string("some string")); + + RCLCPP_ASSERT_THROW_EQ(throw NonStandardThrowable(), NonStandardThrowable()); +} + +TEST(TestGTestMacros, non_standard_types_not_equals) { + ::testing::AssertionResult result = ::testing::AssertionSuccess(); + + CHECK_THROW_EQ_IMPL(throw 0, 1, result); + EXPECT_FALSE(result); + result = ::testing::AssertionSuccess(); + + CHECK_THROW_EQ_IMPL(throw -42, 42, result); + EXPECT_FALSE(result); + result = ::testing::AssertionSuccess(); + + CHECK_THROW_EQ_IMPL(throw std::string("some string"), std::string("some other string"), result); + EXPECT_FALSE(result); +} + +TEST(TestGTestMacros, rclcpp_exceptions) { + rcutils_error_state_t rcl_error_state = {"this is some error message", __FILE__, __LINE__}; + { + auto expected = + rclcpp::exceptions::RCLError(RCL_RET_ERROR, &rcl_error_state, "exception_prefix"); + auto actual = + rclcpp::exceptions::RCLError(RCL_RET_ERROR, &rcl_error_state, "exception_prefix"); + RCLCPP_EXPECT_THROW_EQ(throw actual, expected); + RCLCPP_ASSERT_THROW_EQ(throw actual, expected); + } + { + auto expected = + rclcpp::exceptions::RCLBadAlloc(RCL_RET_BAD_ALLOC, &rcl_error_state); + auto actual = + rclcpp::exceptions::RCLBadAlloc(RCL_RET_BAD_ALLOC, &rcl_error_state); + RCLCPP_EXPECT_THROW_EQ(throw actual, expected); + } + { + // Prefixes are not checked + auto expected = + rclcpp::exceptions::RCLError( + RCL_RET_ERROR, &rcl_error_state, "exception_prefix"); + auto actual = + rclcpp::exceptions::RCLError( + RCL_RET_ERROR, &rcl_error_state, "different_prefix"); + RCLCPP_EXPECT_THROW_EQ(throw actual, expected); + RCLCPP_ASSERT_THROW_EQ(throw actual, expected); + } + { + // File names are not checked + rcutils_error_state_t different_error_state = rcl_error_state; + std::snprintf( + different_error_state.file, RCUTILS_ERROR_STATE_FILE_MAX_LENGTH, "different_file.cpp"); + auto expected = + rclcpp::exceptions::RCLError( + RCL_RET_ERROR, &rcl_error_state, "exception_prefix"); + auto actual = + rclcpp::exceptions::RCLError( + RCL_RET_ERROR, &different_error_state, "exception_prefix"); + RCLCPP_EXPECT_THROW_EQ(throw actual, expected); + RCLCPP_ASSERT_THROW_EQ(throw actual, expected); + } + { + // Line numbers are not checked + rcutils_error_state_t different_error_state = rcl_error_state; + different_error_state.line_number += 42; + auto expected = + rclcpp::exceptions::RCLError( + RCL_RET_ERROR, &rcl_error_state, "exception_prefix"); + auto actual = + rclcpp::exceptions::RCLError( + RCL_RET_ERROR, &different_error_state, "exception_prefix"); + RCLCPP_EXPECT_THROW_EQ(throw actual, expected); + RCLCPP_ASSERT_THROW_EQ(throw actual, expected); + } +} + +TEST(TestGTestMacros, rclcpp_exceptions_not_equal) { + rcutils_error_state_t rcl_error_state = {"this is some error message", __FILE__, __LINE__}; + { + // Check different return errors + ::testing::AssertionResult result = ::testing::AssertionSuccess(); + auto expected = + rclcpp::exceptions::RCLError(RCL_RET_ERROR, &rcl_error_state, "exception_prefix"); + + auto actual = + rclcpp::exceptions::RCLError(RCL_RET_BAD_ALLOC, &rcl_error_state, "exception_prefix"); + CHECK_THROW_EQ_IMPL(throw actual, expected, result); + EXPECT_FALSE(result); + } + { + // Check different error messages + rcutils_error_state_t different_error_state = rcl_error_state; + std::snprintf( + different_error_state.message, + RCUTILS_ERROR_STATE_MESSAGE_MAX_LENGTH, + "this is a different error message"); + ::testing::AssertionResult result = ::testing::AssertionSuccess(); + auto expected = + rclcpp::exceptions::RCLError( + RCL_RET_ERROR, &rcl_error_state, "exception_prefix"); + auto actual = + rclcpp::exceptions::RCLError( + RCL_RET_ERROR, &different_error_state, "exception_prefix"); + CHECK_THROW_EQ_IMPL(throw actual, expected, result); + EXPECT_FALSE(result); + } + { + // Check different exception types + ::testing::AssertionResult result = ::testing::AssertionSuccess(); + auto expected = + rclcpp::exceptions::RCLError(RCL_RET_ERROR, &rcl_error_state, "exception_prefix"); + auto actual = + rclcpp::exceptions::RCLInvalidArgument(RCL_RET_ERROR, &rcl_error_state, "exception_prefix"); + CHECK_THROW_EQ_IMPL(throw actual, expected, result); + EXPECT_FALSE(result); + } +} diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst new file mode 100644 index 0000000000..a8a7e3899d --- /dev/null +++ b/rclcpp_action/CHANGELOG.rst @@ -0,0 +1,446 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package rclcpp_action +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + + +29.2.0 (2024-11-25) +------------------- + +29.1.0 (2024-11-20) +------------------- +* Fix documentation typo in server_goal_handle.hpp (`#2669 `_) +* Contributors: YR + +29.0.0 (2024-10-03) +------------------- +* Increase the timeout for the cppcheck on rclcpp_action. (`#2640 `_) +* add smart pointer macros definitions to action server and client base classes (`#2631 `_) +* Contributors: Alberto Soragna, Chris Lalancette + +28.3.3 (2024-07-29) +------------------- + +28.3.2 (2024-07-24) +------------------- + +28.3.1 (2024-06-25) +------------------- +* Fix typo in function doc (`#2563 `_) +* Contributors: Christophe Bedard + +28.3.0 (2024-06-17) +------------------- +* Add 'mimick' label to tests which use Mimick (`#2516 `_) +* Contributors: Scott K Logan + +28.2.0 (2024-04-26) +------------------- + +28.1.0 (2024-04-16) +------------------- +* Remove references to index.ros.org. (`#2504 `_) +* Contributors: Chris Lalancette + +28.0.1 (2024-04-16) +------------------- +* Callback after cancel (`#2281 `_) + * feat(Client): Added function to stop callbacks of a goal handle + This function allows us to drop the handle in a locked context. + If we do not do this within a lock, there will be a race condition between + the deletion of the shared_ptr of the handle and the result / feedback + callbacks. + * fix: make Client goal handle recursive + This fixes deadlocks due to release of goal handles in callbacks etc. + * fix(ActionGoalClient): Fixed memory leak for nominal case + This fixes a memory leak due to a self reference in the ClientGoalHandle. + Note, this fix will only work, if the ClientGoalHandle ever receives + a result callback. + * doc: Updated documentation of rclcpp_action::Client::async_send_goal + * docs: Made the async_send_goal documentation more explicit + Co-authored-by: Janosch Machowinski +* Remake of "fix: Fixed race condition in action server between is_ready and take" (`#2495 `_) + Some background information: is_ready, take_data and execute data + may be called from different threads in any order. The code in the old + state expected them to be called in series, without interruption. + This lead to multiple race conditions, as the state of the pimpl objects + was altered by the three functions in a non thread safe way. + Co-authored-by: Janosch Machowinski +* update rclcpp::Waitable API to use references and const (`#2467 `_) +* Contributors: William Woodall, jmachowinski + +28.0.0 (2024-03-28) +------------------- +* Do not generate the exception when action service response timeout. (`#2464 `_) + * Do not generate the exception when action service response timeout. + * address review comment. + --------- +* Modify rclcpp_action::GoalUUID hashing algorithm (`#2441 `_) + * Add unit tests for hashing rclcpp_action::GoalUUID's + * Use the FNV-1a hash algorithm for Goal UUID +* Various cleanups to deal with uncrustify 0.78. (`#2439 `_) + These should also work with uncrustify 0.72. +* Update quality declaration documents (`#2427 `_) +* Contributors: Chris Lalancette, Christophe Bedard, Tomoya Fujita, mauropasse + +27.0.0 (2024-02-07) +------------------- + +26.0.0 (2024-01-24) +------------------- + +25.0.0 (2023-12-26) +------------------- +* Switch to target_link_libraries. (`#2374 `_) +* Contributors: Chris Lalancette + +24.0.0 (2023-11-06) +------------------- + +23.2.0 (2023-10-09) +------------------- + +23.1.0 (2023-10-04) +------------------- + +23.0.0 (2023-09-08) +------------------- +* Update API docs links in package READMEs (`#2302 `_) +* fix(ClientGoalHandle): Made mutex recursive to prevent deadlocks (`#2267 `_) +* Contributors: Christophe Bedard, jmachowinski + +22.2.0 (2023-09-07) +------------------- +* Correct the position of a comment. (`#2290 `_) +* Fix a typo in a comment. (`#2283 `_) +* doc fix: call `canceled` only after goal state is in canceling. (`#2266 `_) +* Contributors: Chris Lalancette, Jiaqi Li, Tomoya Fujita + +22.1.0 (2023-08-21) +------------------- + +22.0.0 (2023-07-11) +------------------- + +21.3.0 (2023-06-12) +------------------- + +21.2.0 (2023-06-07) +------------------- + +21.1.1 (2023-05-11) +------------------- + +21.1.0 (2023-04-27) +------------------- + +21.0.0 (2023-04-18) +------------------- + +20.0.0 (2023-04-13) +------------------- +* extract the result response before the callback is issued. (`#2132 `_) +* Update all rclcpp packages to C++17. (`#2121 `_) +* Fix the GoalUUID to_string representation (`#1999 `_) +* Contributors: Chris Lalancette, Nathan Wiebe Neufeldt, Tomoya Fujita + +19.3.0 (2023-03-01) +------------------- + +19.2.0 (2023-02-24) +------------------- + +19.1.0 (2023-02-14) +------------------- + +19.0.0 (2023-01-30) +------------------- + +18.0.0 (2022-12-29) +------------------- +* Explicitly set callback type (`#2059 `_) +* Update maintainers (`#2043 `_) +* Contributors: Audrow Nash, mauropasse + +17.1.0 (2022-11-02) +------------------- +* Do not clear entities callbacks on destruction (`#2002 `_) +* Contributors: mauropasse + +17.0.0 (2022-09-13) +------------------- +* Revert "Introduce executors new spin_for method, replace spin_until_future_complete with spin_until_complete. (`#1821 `_) (`#1874 `_)" (`#1956 `_) +* Introduce executors new spin_for method, replace spin_until_future_complete with spin_until_complete. (`#1821 `_) (`#1874 `_) +* Contributors: Hubert Liberacki, William Woodall + +16.2.0 (2022-05-03) +------------------- + +16.1.0 (2022-04-29) +------------------- + +16.0.1 (2022-04-13) +------------------- + +16.0.0 (2022-04-08) +------------------- +* remove things that were deprecated during galactic (`#1913 `_) +* Contributors: William Woodall + +15.4.0 (2022-04-05) +------------------- +* add take_data_by_entity_id API to waitable (`#1892 `_) +* Contributors: Alberto Soragna + +15.3.0 (2022-03-30) +------------------- + +15.2.0 (2022-03-24) +------------------- +* Fix rosdoc2 issues (`#1897 `_) +* Contributors: Chris Lalancette + +15.1.0 (2022-03-01) +------------------- +* Add RMW listener APIs (`#1579 `_) +* Install headers to include/${PROJECT_NAME} (`#1888 `_) +* Contributors: Shane Loretz, iRobot ROS + +15.0.0 (2022-01-14) +------------------- +* Fix include order and relative paths for cpplint (`#1859 `_) +* Contributors: Jacob Perron + +14.1.0 (2022-01-05) +------------------- + +14.0.0 (2021-12-17) +------------------- +* Fixes for uncrustify 0.72 (`#1844 `_) +* Use rclcpp::guard_condition (`#1612 `_) +* Remove author by request (`#1818 `_) +* Update maintainers (`#1817 `_) +* Suppress clang dead-store warnings in the benchmarks. (`#1802 `_) +* Contributors: Chris Lalancette, Jacob Perron, mauropasse + +13.1.0 (2021-10-18) +------------------- +* Deprecate the `void shared_ptr` subscription callback signatures (`#1713 `_) +* Contributors: Abrar Rahman Protyasha, Tomoya Fujita + +13.0.0 (2021-08-23) +------------------- +* Use rcpputils/scope_exit.hpp and remove rclcpp/scope_exit.hpp. (`#1727 `_) +* Contributors: Christophe Bedard + +12.0.0 (2021-07-26) +------------------- + +11.2.0 (2021-07-21) +------------------- + +11.1.0 (2021-07-13) +------------------- +* Fixed occasionally missing goal result caused by race condition (`#1677 `_) +* Contributors: Kaven Yau + +11.0.0 (2021-05-18) +------------------- +* Bump the benchmark timeout for benchmark_action_client (`#1671 `_) +* Contributors: Scott K Logan + +10.0.0 (2021-05-11) +------------------- +* Returns CancelResponse::REJECT while goal handle failed to transit to CANCELING state (`#1641 `_) +* Fix action server deadlock issue that caused by other mutexes locked in CancelCallback (`#1635 `_) +* Contributors: Kaven Yau + +9.0.2 (2021-04-14) +------------------ + +9.0.1 (2021-04-12) +------------------ + +9.0.0 (2021-04-06) +------------------ +* updating quality declaration links (re: `ros2/docs.ros2.org#52 `_) (`#1615 `_) +* Contributors: shonigmann + +8.2.0 (2021-03-31) +------------------ + +8.1.0 (2021-03-25) +------------------ + +8.0.0 (2021-03-23) +------------------ + +7.0.1 (2021-03-22) +------------------ + +7.0.0 (2021-03-18) +------------------ +* Add support for rmw_connextdds (`#1574 `_) +* node_handle must be destroyed after client_handle to prevent memory leak (`#1562 `_) +* Contributors: Andrea Sorbini, Tomoya Fujita + +6.3.1 (2021-02-08) +------------------ +* Finalize rcl_handle to prevent leak (`#1528 `_) (`#1529 `_) +* Fix `#1526 `_. (`#1527 `_) +* Contributors: y-okumura-isp + +6.3.0 (2021-01-25) +------------------ +* Fix action server deadlock (`#1285 `_) (`#1313 `_) +* Contributors: Daisuke Sato + +6.2.0 (2021-01-08) +------------------ +* Goal response callback compatibility shim with deprecation of old signature (`#1495 `_) +* [rclcpp_action] Add warnings (`#1405 `_) +* Contributors: Audrow Nash, Ivan Santiago Paunovic + +6.1.0 (2020-12-10) +------------------ +* Update QDs to QL 1 (`#1477 `_) +* Contributors: Stephen Brawner + +6.0.0 (2020-11-18) +------------------ +* Add `take_data` to `Waitable` and `data` to `AnyExecutable` (`#1241 `_) +* Fix test crashes on CentOS 7 (`#1449 `_) +* Bump rclcpp packages to Quality Level 2 (`#1445 `_) +* Add rclcpp_action action_server benchmarks (`#1433 `_) +* Contributors: Audrow Nash, Chris Lalancette, Louise Poubel, brawner + +5.1.0 (2020-11-02) +------------------ +* Benchmark rclcpp_action action_client (`#1429 `_) +* Add missing locking to the rclcpp_action::ServerBase. (`#1421 `_) +* Increase test timeouts of slow running tests with rmw_connext_cpp (`#1400 `_) +* Update maintainers (`#1384 `_) +* Increase coverage rclcpp_action to 95% (`#1290 `_) +* Contributors: Chris Lalancette, Ivan Santiago Paunovic, brawner + +5.0.0 (2020-09-18) +------------------ +* Pass goal handle to goal response callback instead of a future (`#1311 `_) +* Remove deprecated client goal handle method for getting result (`#1309 `_) +* Increase test timeout necessary for Connext (`#1256 `_) +* Contributors: Dirk Thomas, Jacob Perron + +4.0.0 (2020-07-09) +------------------ +* Bump to QD to level 3 and fixed links (`#1158 `_) +* Contributors: Alejandro Hernández Cordero + +3.0.0 (2020-06-18) +------------------ +* Add rcl_action_client_options when creating action client. (`#1133 `_) +* Fix doxygen warnings (`#1163 `_) +* Increase rclcpp_action test coverage (`#1153 `_) +* Contributors: Alejandro Hernández Cordero, Michel Hidalgo, tomoya + +2.0.0 (2020-06-01) +------------------ +* Added missing virtual destructors. (`#1149 `_) +* Add Security Vulnerability Policy pointing to REP-2006. (`#1130 `_) +* Contributors: Chris Lalancette, Ivan Santiago Paunovic + +1.1.0 (2020-05-26) +------------------ +* Action client holds weak pointers to goal handles (`#1122 `_) +* Deprecate ClientGoalHandle::async_result() (`#1120 `_) +* Improve documentation (`#1106 `_) +* Fixed rep links and added more details to dependencies in quality declaration (`#1116 `_) +* Update quality declaration to reflect version 1.0 (`#1115 `_) +* Contributors: Alejandro Hernández Cordero, Jacob Perron, Stephen Brawner + +1.0.0 (2020-05-12) +------------------ + +0.9.1 (2020-05-08) +------------------ +* Added Quality declaration: rclcpp, rclpp_action, rclcpp_components andrclcpp_lifecycle (`#1100 `_) +* Contributors: Alejandro Hernández Cordero + +0.9.0 (2020-04-29) +------------------ +* Increasing test coverage of rclcpp_action (`#1043 `_) +* Export targets in addition to include directories / libraries (`#1096 `_) +* Deprecate redundant namespaces (`#1083 `_) +* Rename rosidl_generator_c namespace to rosidl_runtime_c (`#1062 `_) +* Changed rosidl_generator_c/cpp to rosidl_runtime_c/cpp (`#1014 `_) +* Fix unknown macro errors reported by cppcheck 1.90 (`#1000 `_) +* Removed rosidl_generator_c dependency (`#992 `_) +* Fix typo in action client logger name (`#937 `_) +* Contributors: Alejandro Hernández Cordero, Dirk Thomas, Jacob Perron, Stephen Brawner, William Woodall + +0.8.3 (2019-11-19) +------------------ +* issue-919 Fixed "memory leak" in action clients (`#920 `_) +* Contributors: astere-cpr + +0.8.2 (2019-11-18) +------------------ +* Increased a timeout for the ``test_client`` tests. (`#917 `_) +* Contributors: Michel Hidalgo + +0.8.1 (2019-10-23) +------------------ +* Template node type for rclcpp action server and clients (`#892 `_) +* Trait tests for generated actions (`#853 `_) +* Do not throw exception in action client if take fails (`#888 `_) +* Contributors: Jacob Perron, Michael Carroll, Steven Macenski + +0.8.0 (2019-09-26) +------------------ +* Fix UnknownGoalHandle error string. (`#856 `_) +* Guard against making multiple result requests for a goal handle (`#808 `_) +* Add line break after first open paren in multiline function call (`#785 `_) +* Fix typo in test fixture tear down method name (`#787 `_) +* Contributors: Chris Lalancette, Dan Rose, Jacob Perron + +0.7.5 (2019-05-30) +------------------ + +0.7.4 (2019-05-29) +------------------ +* Guard against calling null goal response callback (`#738 `_) +* Contributors: Jacob Perron + +0.7.3 (2019-05-20) +------------------ + +0.7.2 (2019-05-08) +------------------ +* Added return code to CancelGoal service response. (`#710 `_) +* Contributors: Jacob Perron, William Woodall + +0.7.1 (2019-04-26) +------------------ +* Added optional callbacks to action client for goal, response, and result. (`#701 `_) +* Added overload for node interfaces. (`#700 `_) +* Renamed action state transitions. (`#677 `_) +* Contributors: Jacob Perron, Karsten Knese + +0.7.0 (2019-04-14) +------------------ +* Fixed hard-coded duration type representation so int64_t isn't assumed. (`#648 `_) +* Added documentation to rclcpp_action. (`#650 `_) +* Updated to use separated action types. (`#601 `_) +* Updated to wait for action server before sending goal. (`#637 `_) +* Refactored server goal handle's try_canceling() function. (`#603 `_) +* Contributors: Emerson Knapp, Jacob Perron, Michel Hidalgo, Shane Loretz + +0.6.2 (2018-12-13) +------------------ + +0.6.1 (2018-12-07) +------------------ +* Added wait_for_action_server() for action clients (`#598 `_) +* Updated to adapt to action implicit changes (`#602 `_) +* Added rclcpp_action Server implementation (`#593 `_) +* Added action client implementation (`#594 `_) +* Added skeleton for Action Server and Client (`#579 `_) +* Contributors: Michel Hidalgo, Shane Loretz, William Woodall diff --git a/rclcpp_action/CMakeLists.txt b/rclcpp_action/CMakeLists.txt new file mode 100644 index 0000000000..0fc1065d55 --- /dev/null +++ b/rclcpp_action/CMakeLists.txt @@ -0,0 +1,142 @@ +cmake_minimum_required(VERSION 3.5) + +project(rclcpp_action) + +find_package(ament_cmake_ros REQUIRED) +find_package(action_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rcl REQUIRED) +find_package(rcl_action REQUIRED) +find_package(rcpputils REQUIRED) +find_package(rosidl_runtime_c REQUIRED) + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options( + -Wall -Wextra -Wpedantic -Wnon-virtual-dtor -Woverloaded-virtual + -Wformat=2 -Wconversion -Wshadow -Wsign-conversion -Wcast-qual + ) +endif() + +add_library(${PROJECT_NAME} + src/client.cpp + src/qos.cpp + src/server.cpp + src/server_goal_handle.cpp + src/types.cpp +) +target_include_directories(${PROJECT_NAME} + PUBLIC + "$" + "$") +target_link_libraries(${PROJECT_NAME} PUBLIC + ${action_msgs_TARGETS} + rcl::rcl + rcl_action::rcl_action + rclcpp::rclcpp + rosidl_runtime_c::rosidl_runtime_c +) +target_link_libraries(${PROJECT_NAME} PRIVATE + rcpputils::rcpputils +) + +# 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 "RCLCPP_ACTION_BUILDING_LIBRARY") + +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME}) + +install( + TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +# 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}) + +ament_export_dependencies(action_msgs rcl_action rclcpp rosidl_runtime_c) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_lint_auto REQUIRED) + # Give cppcheck hints about macro definitions coming from outside this package + set(ament_cmake_cppcheck_ADDITIONAL_INCLUDE_DIRS ${rclcpp_INCLUDE_DIRS}) + ament_lint_auto_find_test_dependencies() + + add_subdirectory(test/benchmark) + + ament_add_gtest(test_client test/test_client.cpp TIMEOUT 180) + ament_add_test_label(test_client mimick) + if(TARGET test_client) + target_link_libraries(test_client + ${PROJECT_NAME} + mimick + rcl::rcl + rcl_action::rcl_action + rclcpp::rclcpp + rcutils::rcutils + ${test_msgs_TARGETS} + ) + endif() + + ament_add_gtest(test_server test/test_server.cpp TIMEOUT 180) + ament_add_test_label(test_server mimick) + if(TARGET test_server) + target_link_libraries(test_server + ${PROJECT_NAME} + mimick + rcl_action::rcl_action + rclcpp::rclcpp + ${test_msgs_TARGETS} + ) + endif() + + ament_add_gtest(test_server_goal_handle test/test_server_goal_handle.cpp) + ament_add_test_label(test_server_goal_handle mimick) + if(TARGET test_server_goal_handle) + target_link_libraries(test_server_goal_handle + ${PROJECT_NAME} + ${action_msgs_TARGETS} + mimick + rclcpp::rclcpp + ${test_msgs_TARGETS} + ) + endif() + + ament_add_gtest(test_traits test/test_traits.cpp) + if(TARGET test_traits) + target_link_libraries(test_traits + ${PROJECT_NAME} + ${test_msgs_TARGETS} + ) + endif() + + ament_add_gtest(test_types test/test_types.cpp) + if(TARGET test_types) + target_link_libraries(test_types + ${PROJECT_NAME} + ${test_msgs_TARGETS} + ) + endif() +endif() + +ament_package() + +if(TEST cppcheck) + # must set the property after ament_package() + set_tests_properties(cppcheck PROPERTIES TIMEOUT 600) +endif() diff --git a/rclcpp_action/Doxyfile b/rclcpp_action/Doxyfile new file mode 100644 index 0000000000..e7c9fedb59 --- /dev/null +++ b/rclcpp_action/Doxyfile @@ -0,0 +1,35 @@ +# All settings not listed here will use the Doxygen default values. + +PROJECT_NAME = "rclcpp_action" +PROJECT_NUMBER = master +PROJECT_BRIEF = "C++ ROS Action Client Library" + +# Use these lines to include the generated logging.hpp (update install path if needed) +#INPUT = ../../../../install_isolated/rclcpp/include +#STRIP_FROM_PATH = /Users/william/ros2_ws/install_isolated/rclcpp/include +# Otherwise just generate for the local (non-generated header files) +INPUT = ./include + +RECURSIVE = YES +OUTPUT_DIRECTORY = doc_output + +EXTRACT_ALL = YES +SORT_MEMBER_DOCS = NO + +GENERATE_LATEX = NO + +ENABLE_PREPROCESSING = YES +MACRO_EXPANSION = YES +EXPAND_ONLY_PREDEF = YES +PREDEFINED = RCLCPP_ACTION_PUBLIC= + +# Tag files that do not exist will produce a warning and cross-project linking will not work. +#TAGFILES += "../../../../doxygen_tag_files/cppreference-doxygen-web.tag.xml=http://en.cppreference.com/w/" +# Consider changing "latest" to the version you want to reference (e.g. beta1 or 1.0.0) +#TAGFILES += "../../../../doxygen_tag_files/rclcpp.tag=http://docs.ros2.org/latest/api/rclcpp/" +#TAGFILES += "../../../../doxygen_tag_files/rcl_action.tag=http://docs.ros2.org/latest/api/rcl_action/" +#TAGFILES += "../../../../doxygen_tag_files/rcl.tag=http://docs.ros2.org/latest/api/rcl/" +#TAGFILES += "../../../../doxygen_tag_files/rmw.tag=http://docs.ros2.org/latest/api/rmw/" +#TAGFILES += "../../../../doxygen_tag_files/rcutils.tag=http://docs.ros2.org/latest/api/rcutils/" +# Uncomment to generate tag files for cross-project linking. +# GENERATE_TAGFILE = "../../../../doxygen_tag_files/rclcpp_action.tag" diff --git a/rclcpp_action/QUALITY_DECLARATION.md b/rclcpp_action/QUALITY_DECLARATION.md new file mode 100644 index 0000000000..5812429610 --- /dev/null +++ b/rclcpp_action/QUALITY_DECLARATION.md @@ -0,0 +1,194 @@ +This document is a declaration of software quality for the `rclcpp_action` package, based on the guidelines in [REP-2004](https://www.ros.org/reps/rep-2004.html). + +# rclcpp_action Quality Declaration + +The package `rclcpp_action` 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 Requirements for Quality Level 1 in REP-2004](https://www.ros.org/reps/rep-2004.html). + +## Version Policy [1] + +### Version Scheme [1.i] + +`rclcpp_action` 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] + +`rclcpp_action` is at a stable version, i.e. `>= 1.0.0`. +The current version can be found in its [package.xml](package.xml), and its change history can be found in its [CHANGELOG](CHANGELOG.rst). + +### Public API Declaration [1.iii] + +All symbols in the installed headers are considered part of the public API. + +All installed headers are in the `include` directory of the package, headers in any other folders are not installed and considered private. + +### API Stability Policy [1.iv] + +`rclcpp_action` 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] + +`rclcpp_action` 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] + +`rclcpp_action` 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] + +`rclcpp_action` follows the recommended guidelines for ROS Core packages in the [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#change-control-process). + +### Change Requests [2.i] + +All changes will occur through a pull request, check [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/rclcpp_action/) +* [linux_release](https://ci.ros2.org/view/nightly/job/nightly_linux_release/lastBuild/testReport/rclcpp_action/) +* [mac_osx_release](https://ci.ros2.org/view/nightly/job/nightly_osx_release/lastBuild/testReport/rclcpp_action/) +* [windows_release](https://ci.ros2.org/view/nightly/job/nightly_win_rel/lastBuild/testReport/rclcpp_action/) + +### Documentation Policy [2.v] + +All pull requests must resolve related documentation changes before merging. + +## Documentation [3] + +### Feature Documentation [3.i] + +`rclcpp_action` has a [feature list](http://docs.ros2.org/latest/api/rclcpp_action/) and each item in the list links to the corresponding feature documentation. There is documentation for all of the features, and new features require documentation before being added. + +### Public API Documentation [3.ii] + +The API is publicly available in its [ROS 2 API documentation](http://docs.ros2.org/latest/api/rclcpp_action/). + +### License [3.iii] + +The license for `rclcpp_action` 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](http://build.ros2.org/view/Rpr/job/Rpr__rclcpp__ubuntu_focal_amd64/lastCompletedBuild/testReport/rclcpp_action/) 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 `rclcpp_action`. + +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](http://build.ros2.org/view/Rpr/job/Rpr__rclcpp__ubuntu_focal_amd64/lastCompletedBuild/testReport/rclcpp_action/copyright/). + +## Testing [4] + +### Feature Testing [4.i] + +Each feature in `rclcpp_action` has corresponding tests which simulate typical usage, and they are located in the [`test`](https://github.com/ros2/rclcpp_action/tree/rolling/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/rclcpp_action/) +* [linux_release](https://ci.ros2.org/view/nightly/job/nightly_linux_release/lastBuild/testReport/rclcpp_action/) +* [mac_osx_release](https://ci.ros2.org/view/nightly/job/nightly_osx_release/lastBuild/testReport/rclcpp_action/) +* [windows_release](https://ci.ros2.org/view/nightly/job/nightly_win_rel/lastBuild/testReport/rclcpp_action/) + +### 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. + +### Coverage [4.iii] + +`rclcpp_action` 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 +- achieving and maintaining a reasonable branch line coverage (90-100%) +- no lines are manually skipped in coverage calculations + +Changes are required to make a best effort to keep or increase coverage before being accepted, but decreases are allowed if properly justified and accepted by reviewers. + +Current coverage statistics can be viewed [here](https://ci.ros2.org/job/nightly_linux_coverage/lastCompletedBuild/cobertura/src_ros2_rclcpp_rclcpp_action_src/). 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] + +`rclcpp_action` follows the recommendations for performance testing of C/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. + +The performance tests of `rclcpp_action` are located in the [test/benchmark directory](https://github.com/ros2/rclcpp/tree/rolling/rclcpp_action/test/benchmark). + +System level performance benchmarks that cover features of `rclcpp_action` 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] + +`rclcpp_action` uses and passes all the ROS 2 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/rclcpp_action/) +* [linux_release](https://ci.ros2.org/view/nightly/job/nightly_linux_release/lastBuild/testReport/rclcpp_action/) +* [mac_osx_release](https://ci.ros2.org/view/nightly/job/nightly_osx_release/lastBuild/testReport/rclcpp_action/) +* [windows_release](https://ci.ros2.org/view/nightly/job/nightly_win_rel/lastBuild/testReport/rclcpp_action/) + +## Dependencies [5] + +Below are evaluations of each of `rclcpp_action`'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 and Optional Runtime ROS Dependencies [5.i]/[5.ii] + +`rclcpp_action` has the following runtime ROS dependencies: + +#### `action_msgs` + +`action_msgs` provides messages and services for ROS 2 actions. + +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcl_interfaces/blob/rolling/action_msgs/QUALITY_DECLARATION.md). + +#### `rclcpp` + +The `rclcpp` package provides the ROS client library in C++. + +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rclcpp/blob/rolling/rclcpp/QUALITY_DECLARATION.md). + +#### `rcl_action` + +The `rcl_action` package provides C-based ROS action implementation. + +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcl/blob/rolling/rcl_action/QUALITY_DECLARATION.md). + +### Direct Runtime non-ROS Dependency [5.iii] + +`rclcpp_action` has no run-time or build-time dependencies that need to be considered for this declaration. + +## Platform Support [6] + +`rclcpp_action` supports all of the tier 1 platforms as described in [REP-2000](https://www.ros.org/reps/rep-2000.html#support-tiers), and tests each change against all of them. + +Currently nightly build status can be seen here: +* [linux-aarch64_release](https://ci.ros2.org/view/nightly/job/nightly_linux-aarch64_release/lastBuild/rclcpp_action/) +* [linux_release](https://ci.ros2.org/view/nightly/job/nightly_linux_release/lastBuild/rclcpp_action/) +* [mac_osx_release](https://ci.ros2.org/view/nightly/job/nightly_osx_release/lastBuild/rclcpp_action/) +* [windows_release](https://ci.ros2.org/view/nightly/job/nightly_win_rel/lastBuild/rclcpp_action/) + +## Security + +### Vulnerability Disclosure Policy [7.i] + +This package conforms to the Vulnerability Disclosure Policy in [REP-2006](https://www.ros.org/reps/rep-2006.html). diff --git a/rclcpp_action/README.md b/rclcpp_action/README.md new file mode 100644 index 0000000000..bc3b85e6bd --- /dev/null +++ b/rclcpp_action/README.md @@ -0,0 +1,10 @@ +# `rclcpp_action` + +Adds action APIs for C++. + +The link to the latest rclcpp_action API documentation, which includes a complete list of its main components and features, can be found on the [rclcpp_action package info page](https://docs.ros.org/en/rolling/p/rclcpp_action). +For more information about Actions in ROS 2, see the [design document](http://design.ros2.org/articles/actions.html). + +## Quality Declaration + +This package claims to be in the **Quality Level 1** category, see the [Quality Declaration](QUALITY_DECLARATION.md) for more details. diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp new file mode 100644 index 0000000000..40a326702a --- /dev/null +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -0,0 +1,831 @@ +// 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 RCLCPP_ACTION__CLIENT_HPP_ +#define RCLCPP_ACTION__CLIENT_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rcl/event_callback.h" + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_logging_interface.hpp" +#include "rclcpp/node_interfaces/node_graph_interface.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/waitable.hpp" + +#include "rosidl_runtime_c/action_type_support_struct.h" +#include "rosidl_typesupport_cpp/action_type_support.hpp" + +#include "rclcpp_action/client_goal_handle.hpp" +#include "rclcpp_action/exceptions.hpp" +#include "rclcpp_action/types.hpp" +#include "rclcpp_action/visibility_control.hpp" + + +namespace rclcpp_action +{ +// Forward declaration +class ClientBaseImpl; + +/// Base Action Client implementation +/// \internal +/** + * This class should not be used directly by users wanting to create an aciton client. + * Instead users should use `rclcpp_action::Client<>`. + * + * Internally, this class is responsible for interfacing with the `rcl_action` API. + */ +class ClientBase : public rclcpp::Waitable +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ClientBase) + + RCLCPP_ACTION_PUBLIC + virtual ~ClientBase(); + + /// Enum to identify entities belonging to the action client + enum class EntityType : std::size_t + { + GoalClient, + ResultClient, + CancelClient, + FeedbackSubscription, + StatusSubscription, + }; + + /// Return true if there is an action server that is ready to take goal requests. + RCLCPP_ACTION_PUBLIC + bool + action_server_is_ready() const; + + /// Wait for action_server_is_ready() to become true, or until the given timeout is reached. + template + bool + wait_for_action_server( + std::chrono::duration timeout = std::chrono::duration(-1)) + { + return wait_for_action_server_nanoseconds( + std::chrono::duration_cast(timeout) + ); + } + + // ------------- + // Waitables API + + /// \internal + RCLCPP_ACTION_PUBLIC + size_t + get_number_of_ready_subscriptions() override; + + /// \internal + RCLCPP_ACTION_PUBLIC + size_t + get_number_of_ready_timers() override; + + /// \internal + RCLCPP_ACTION_PUBLIC + size_t + get_number_of_ready_clients() override; + + /// \internal + RCLCPP_ACTION_PUBLIC + size_t + get_number_of_ready_services() override; + + /// \internal + RCLCPP_ACTION_PUBLIC + size_t + get_number_of_ready_guard_conditions() override; + + /// \internal + RCLCPP_ACTION_PUBLIC + void + add_to_wait_set(rcl_wait_set_t & wait_set) override; + + /// \internal + RCLCPP_ACTION_PUBLIC + bool + is_ready(const rcl_wait_set_t & wait_set) override; + + /// \internal + RCLCPP_ACTION_PUBLIC + std::shared_ptr + take_data() override; + + /// \internal + RCLCPP_ACTION_PUBLIC + std::shared_ptr + take_data_by_entity_id(size_t id) override; + + /// \internal + RCLCPP_ACTION_PUBLIC + void + execute(const std::shared_ptr & data) override; + + /// \internal + /// Set a callback to be called when action client entities have an event + /** + * The callback receives a size_t which is the number of messages received + * since the last time this callback was called. + * Normally this is 1, but can be > 1 if messages were received before any + * callback was set. + * + * The callback also receives an int identifier argument, which identifies + * the action client entity which is ready. + * This implies that the provided callback can use the identifier to behave + * differently depending on which entity triggered the waitable to become ready. + * + * Calling it again will clear any previously set callback. + * + * An exception will be thrown if the callback is not callable. + * + * This function is thread-safe. + * + * If you want more information available in the callback, like the subscription + * or other information, you may use a lambda with captures or std::bind. + * + * \param[in] callback functor to be called when a new message is received. + */ + RCLCPP_ACTION_PUBLIC + void + set_on_ready_callback(std::function callback) override; + + /// Unset the callback registered for new events, if any. + RCLCPP_ACTION_PUBLIC + void + clear_on_ready_callback() override; + + // End Waitables API + // ----------------- + +protected: + RCLCPP_ACTION_PUBLIC + ClientBase( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + const std::string & action_name, + const rosidl_action_type_support_t * type_support, + const rcl_action_client_options_t & options); + + /// Wait for action_server_is_ready() to become true, or until the given timeout is reached. + RCLCPP_ACTION_PUBLIC + bool + wait_for_action_server_nanoseconds(std::chrono::nanoseconds timeout); + + // ----------------------------------------------------- + // API for communication between ClientBase and Client<> + using ResponseCallback = std::function response)>; + + /// \internal + RCLCPP_ACTION_PUBLIC + rclcpp::Logger get_logger(); + + /// \internal + RCLCPP_ACTION_PUBLIC + virtual + GoalUUID + generate_goal_id(); + + /// \internal + RCLCPP_ACTION_PUBLIC + virtual + void + send_goal_request( + std::shared_ptr request, + ResponseCallback callback); + + /// \internal + RCLCPP_ACTION_PUBLIC + virtual + void + send_result_request( + std::shared_ptr request, + ResponseCallback callback); + + /// \internal + RCLCPP_ACTION_PUBLIC + virtual + void + send_cancel_request( + std::shared_ptr request, + ResponseCallback callback); + + /// \internal + virtual + std::shared_ptr + create_goal_response() const = 0; + + /// \internal + RCLCPP_ACTION_PUBLIC + virtual + void + handle_goal_response( + const rmw_request_id_t & response_header, + std::shared_ptr goal_response); + + /// \internal + virtual + std::shared_ptr + create_result_response() const = 0; + + /// \internal + RCLCPP_ACTION_PUBLIC + virtual + void + handle_result_response( + const rmw_request_id_t & response_header, + std::shared_ptr result_response); + + /// \internal + virtual + std::shared_ptr + create_cancel_response() const = 0; + + /// \internal + RCLCPP_ACTION_PUBLIC + virtual + void + handle_cancel_response( + const rmw_request_id_t & response_header, + std::shared_ptr cancel_response); + + /// \internal + virtual + std::shared_ptr + create_feedback_message() const = 0; + + /// \internal + virtual + void + handle_feedback_message(std::shared_ptr message) = 0; + + /// \internal + virtual + std::shared_ptr + create_status_message() const = 0; + + /// \internal + virtual + void + handle_status_message(std::shared_ptr message) = 0; + + // End API for communication between ClientBase and Client<> + // --------------------------------------------------------- + + /// \internal + /// Set a callback to be called when the specified entity is ready + RCLCPP_ACTION_PUBLIC + void + set_on_ready_callback( + EntityType entity_type, + rcl_event_callback_t callback, + const void * user_data); + + // Mutex to protect the callbacks storage. + std::recursive_mutex listener_mutex_; + // Storage for std::function callbacks to keep them in scope + std::unordered_map> entity_type_to_on_ready_callback_; + +private: + std::unique_ptr pimpl_; + + /// Set a std::function callback to be called when the specified entity is ready + RCLCPP_ACTION_PUBLIC + void + set_callback_to_entity( + EntityType entity_type, + std::function callback); + + bool on_ready_callback_set_{false}; +}; + +/// Action Client +/** + * This class creates an action client. + * + * To create an instance of an action client use `rclcpp_action::create_client()`. + * + * Internally, this class is responsible for: + * - coverting between the C++ action type and generic types for `rclcpp_action::ClientBase`, and + * - calling user callbacks. + */ +template +class Client : public ClientBase +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Client) + + using Goal = typename ActionT::Goal; + using Feedback = typename ActionT::Feedback; + using GoalHandle = ClientGoalHandle; + using WrappedResult = typename GoalHandle::WrappedResult; + using GoalResponseCallback = std::function; + using FeedbackCallback = typename GoalHandle::FeedbackCallback; + using ResultCallback = typename GoalHandle::ResultCallback; + using CancelRequest = typename ActionT::Impl::CancelGoalService::Request; + using CancelResponse = typename ActionT::Impl::CancelGoalService::Response; + using CancelCallback = std::function; + + /// Options for sending a goal. + /** + * This struct is used to pass parameters to the function `async_send_goal`. + */ + struct SendGoalOptions + { + SendGoalOptions() + : goal_response_callback(nullptr), + feedback_callback(nullptr), + result_callback(nullptr) + { + } + + /// Function called when the goal is accepted or rejected. + /** + * Takes a single argument that is a goal handle shared pointer. + * If the goal is accepted, then the pointer points to a valid goal handle. + * If the goal is rejected, then pointer has the value `nullptr`. + */ + GoalResponseCallback goal_response_callback; + + /// Function called whenever feedback is received for the goal. + FeedbackCallback feedback_callback; + + /// Function called when the result for the goal is received. + ResultCallback result_callback; + }; + + /// Construct an action client. + /** + * This constructs an action client, but it will not work until it has been added to a node. + * Use `rclcpp_action::create_client()` to both construct and add to a node. + * + * \param[in] node_base A pointer to the base interface of a node. + * \param[in] node_graph A pointer to an interface that allows getting graph information about + * a node. + * \param[in] node_logging A pointer to an interface that allows getting a node's logger. + * \param[in] action_name The action name. + * \param[in] client_options Options to pass to the underlying `rcl_action::rcl_action_client_t`. + */ + Client( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + const std::string & action_name, + const rcl_action_client_options_t & client_options = rcl_action_client_get_default_options() + ) + : ClientBase( + node_base, node_graph, node_logging, action_name, + rosidl_typesupport_cpp::get_action_type_support_handle(), + client_options) + { + } + + /// Send an action goal and asynchronously get the result. + /** + * If the goal is accepted by an action server, the returned future is set to a `GoalHandle::SharedPtr`. + * If the goal is rejected by an action server, then the future is set to a `nullptr`. + * + * The goal handle in the future is used to monitor the status of the goal and get the final result. + * + * If callbacks were set in @param options, you will receive callbacks, as long as you hold a reference + * to the shared pointer contained in the returned future, or rclcpp_action::Client is destroyed. Dropping + * the shared pointer to the goal handle will not cancel the goal. In order to cancel it, you must explicitly + * call async_cancel_goal. + * + * WARNING this method has inconsistent behaviour and a memory leak bug. + * If you set the result callback in @param options, the handle will be self referencing, and you will receive + * callbacks even though you do not hold a reference to the shared pointer. In this case, the self reference will + * be deleted if the result callback was received. If there is no result callback, there will be a memory leak. + * + * To prevent the memory leak, you may call stop_callbacks() explicit. This will delete the self reference. + * + * \param[in] goal The goal request. + * \param[in] options Options for sending the goal request. Contains references to callbacks for + * the goal response (accepted/rejected), feedback, and the final result. + * \return A future that completes when the goal has been accepted or rejected. + * If the goal is rejected, then the result will be a `nullptr`. + */ + std::shared_future + async_send_goal(const Goal & goal, const SendGoalOptions & options = SendGoalOptions()) + { + // Put promise in the heap to move it around. + auto promise = std::make_shared>(); + std::shared_future future(promise->get_future()); + using GoalRequest = typename ActionT::Impl::SendGoalService::Request; + auto goal_request = std::make_shared(); + goal_request->goal_id.uuid = this->generate_goal_id(); + goal_request->goal = goal; + this->send_goal_request( + std::static_pointer_cast(goal_request), + [this, goal_request, options, promise](std::shared_ptr response) mutable + { + using GoalResponse = typename ActionT::Impl::SendGoalService::Response; + auto goal_response = std::static_pointer_cast(response); + if (!goal_response->accepted) { + promise->set_value(nullptr); + if (options.goal_response_callback) { + options.goal_response_callback(nullptr); + } + return; + } + GoalInfo goal_info; + goal_info.goal_id.uuid = goal_request->goal_id.uuid; + goal_info.stamp = goal_response->stamp; + // Do not use std::make_shared as friendship cannot be forwarded. + std::shared_ptr goal_handle( + new GoalHandle(goal_info, options.feedback_callback, options.result_callback)); + { + std::lock_guard guard(goal_handles_mutex_); + goal_handles_[goal_handle->get_goal_id()] = goal_handle; + } + promise->set_value(goal_handle); + if (options.goal_response_callback) { + options.goal_response_callback(goal_handle); + } + + if (options.result_callback) { + this->make_result_aware(goal_handle); + } + }); + + // TODO(jacobperron): Encapsulate into it's own function and + // consider exposing an option to disable this cleanup + // To prevent the list from growing out of control, forget about any goals + // with no more user references + { + std::lock_guard guard(goal_handles_mutex_); + auto goal_handle_it = goal_handles_.begin(); + while (goal_handle_it != goal_handles_.end()) { + if (!goal_handle_it->second.lock()) { + RCLCPP_DEBUG( + this->get_logger(), + "Dropping weak reference to goal handle during send_goal()"); + goal_handle_it = goal_handles_.erase(goal_handle_it); + } else { + ++goal_handle_it; + } + } + } + + return future; + } + + /// Asynchronously get the result for an active goal. + /** + * \throws exceptions::UnknownGoalHandleError If the goal unknown or already reached a terminal + * state, or if there was an error requesting the result. + * \param[in] goal_handle The goal handle for which to get the result. + * \param[in] result_callback Optional callback that is called when the result is received. + * \return A future that is set to the goal result when the goal is finished. + */ + std::shared_future + async_get_result( + typename GoalHandle::SharedPtr goal_handle, + ResultCallback result_callback = nullptr) + { + std::lock_guard lock(goal_handles_mutex_); + if (goal_handles_.count(goal_handle->get_goal_id()) == 0) { + throw exceptions::UnknownGoalHandleError(); + } + if (goal_handle->is_invalidated()) { + // This case can happen if there was a failure to send the result request + // during the goal response callback + throw goal_handle->invalidate_exception_; + } + if (result_callback) { + // This will override any previously registered callback + goal_handle->set_result_callback(result_callback); + } + this->make_result_aware(goal_handle); + return goal_handle->async_get_result(); + } + + /// Asynchronously request a goal be canceled. + /** + * \throws exceptions::UnknownGoalHandleError If the goal is unknown or already reached a + * terminal state. + * \param[in] goal_handle The goal handle requesting to be canceled. + * \param[in] cancel_callback Optional callback that is called when the response is received. + * The callback takes one parameter: a shared pointer to the CancelResponse message. + * \return A future to a CancelResponse message that is set when the request has been + * acknowledged by an action server. + * See + * + * action_msgs/CancelGoal.srv. + */ + std::shared_future + async_cancel_goal( + typename GoalHandle::SharedPtr goal_handle, + CancelCallback cancel_callback = nullptr) + { + std::lock_guard lock(goal_handles_mutex_); + if (goal_handles_.count(goal_handle->get_goal_id()) == 0) { + throw exceptions::UnknownGoalHandleError(); + } + auto cancel_request = std::make_shared(); + // cancel_request->goal_info.goal_id = goal_handle->get_goal_id(); + cancel_request->goal_info.goal_id.uuid = goal_handle->get_goal_id(); + return async_cancel(cancel_request, cancel_callback); + } + + /// Asynchronously request for all goals to be canceled. + /** + * \param[in] cancel_callback Optional callback that is called when the response is received. + * The callback takes one parameter: a shared pointer to the CancelResponse message. + * \return A future to a CancelResponse message that is set when the request has been + * acknowledged by an action server. + * See + * + * action_msgs/CancelGoal.srv. + */ + std::shared_future + async_cancel_all_goals(CancelCallback cancel_callback = nullptr) + { + auto cancel_request = std::make_shared(); + // std::fill(cancel_request->goal_info.goal_id.uuid, 0u); + std::fill( + cancel_request->goal_info.goal_id.uuid.begin(), + cancel_request->goal_info.goal_id.uuid.end(), 0u); + return async_cancel(cancel_request, cancel_callback); + } + + /// Stops the callbacks for the goal in a thread safe way + /** + * This will NOT cancel the goal, it will only stop the callbacks. + * + * After the call to this function, it is guaranteed that there + * will be no more callbacks from the goal. This is not guaranteed + * if multiple threads are involved, and the goal_handle is just + * dropped. + * + * \param[in] goal_handle The goal were the callbacks shall be stopped + */ + void stop_callbacks(typename GoalHandle::SharedPtr goal_handle) + { + goal_handle->set_feedback_callback(typename GoalHandle::FeedbackCallback()); + goal_handle->set_result_callback(typename GoalHandle::ResultCallback()); + + std::lock_guard guard(goal_handles_mutex_); + const GoalUUID & goal_id = goal_handle->get_goal_id(); + auto it = goal_handles_.find(goal_id); + if (goal_handles_.end() == it) { + // someone else already deleted the entry + // e.g. the result callback + RCLCPP_DEBUG( + this->get_logger(), + "Given goal is unknown. Ignoring..."); + return; + } + goal_handles_.erase(it); + } + + /// Stops the callbacks for the goal in a thread safe way + /** + * For futher information see stop_callbacks(typename GoalHandle::SharedPtr goal_handle) + */ + void stop_callbacks(const GoalUUID & goal_id) + { + typename GoalHandle::SharedPtr goal_handle; + { + std::lock_guard guard(goal_handles_mutex_); + auto it = goal_handles_.find(goal_id); + if (goal_handles_.end() == it) { + // someone else already deleted the entry + // e.g. the result callback + RCLCPP_DEBUG( + this->get_logger(), + "Given goal is unknown. Ignoring..."); + return; + } + + goal_handle = it->lock(); + } + + if (goal_handle) { + stop_callbacks(goal_handle); + } + } + + /// Asynchronously request all goals at or before a specified time be canceled. + /** + * \param[in] stamp The timestamp for the cancel goal request. + * \param[in] cancel_callback Optional callback that is called when the response is received. + * The callback takes one parameter: a shared pointer to the CancelResponse message. + * \return A future to a CancelResponse message that is set when the request has been + * acknowledged by an action server. + * See + * + * action_msgs/CancelGoal.srv. + */ + std::shared_future + async_cancel_goals_before( + const rclcpp::Time & stamp, + CancelCallback cancel_callback = nullptr) + { + auto cancel_request = std::make_shared(); + // std::fill(cancel_request->goal_info.goal_id.uuid, 0u); + std::fill( + cancel_request->goal_info.goal_id.uuid.begin(), + cancel_request->goal_info.goal_id.uuid.end(), 0u); + cancel_request->goal_info.stamp = stamp; + return async_cancel(cancel_request, cancel_callback); + } + + virtual + ~Client() + { + std::lock_guard guard(goal_handles_mutex_); + auto it = goal_handles_.begin(); + while (it != goal_handles_.end()) { + typename GoalHandle::SharedPtr goal_handle = it->second.lock(); + if (goal_handle) { + goal_handle->invalidate(exceptions::UnawareGoalHandleError()); + } + it = goal_handles_.erase(it); + } + } + +private: + /// \internal + std::shared_ptr + create_goal_response() const override + { + using GoalResponse = typename ActionT::Impl::SendGoalService::Response; + return std::shared_ptr(new GoalResponse()); + } + + /// \internal + std::shared_ptr + create_result_response() const override + { + using GoalResultResponse = typename ActionT::Impl::GetResultService::Response; + return std::shared_ptr(new GoalResultResponse()); + } + + /// \internal + std::shared_ptr + create_cancel_response() const override + { + return std::shared_ptr(new CancelResponse()); + } + + /// \internal + std::shared_ptr + create_feedback_message() const override + { + using FeedbackMessage = typename ActionT::Impl::FeedbackMessage; + return std::shared_ptr(new FeedbackMessage()); + } + + /// \internal + void + handle_feedback_message(std::shared_ptr message) override + { + std::lock_guard guard(goal_handles_mutex_); + using FeedbackMessage = typename ActionT::Impl::FeedbackMessage; + typename FeedbackMessage::SharedPtr feedback_message = + std::static_pointer_cast(message); + const GoalUUID & goal_id = feedback_message->goal_id.uuid; + if (goal_handles_.count(goal_id) == 0) { + RCLCPP_DEBUG( + this->get_logger(), + "Received feedback for unknown goal. Ignoring..."); + return; + } + typename GoalHandle::SharedPtr goal_handle = goal_handles_[goal_id].lock(); + // Forget about the goal if there are no more user references + if (!goal_handle) { + RCLCPP_DEBUG( + this->get_logger(), + "Dropping weak reference to goal handle during feedback callback"); + goal_handles_.erase(goal_id); + return; + } + auto feedback = std::make_shared(); + *feedback = feedback_message->feedback; + goal_handle->call_feedback_callback(goal_handle, feedback); + } + + /// \internal + std::shared_ptr + create_status_message() const override + { + using GoalStatusMessage = typename ActionT::Impl::GoalStatusMessage; + return std::shared_ptr(new GoalStatusMessage()); + } + + /// \internal + void + handle_status_message(std::shared_ptr message) override + { + std::lock_guard guard(goal_handles_mutex_); + using GoalStatusMessage = typename ActionT::Impl::GoalStatusMessage; + auto status_message = std::static_pointer_cast(message); + for (const GoalStatus & status : status_message->status_list) { + const GoalUUID & goal_id = status.goal_info.goal_id.uuid; + if (goal_handles_.count(goal_id) == 0) { + RCLCPP_DEBUG( + this->get_logger(), + "Received status for unknown goal. Ignoring..."); + continue; + } + typename GoalHandle::SharedPtr goal_handle = goal_handles_[goal_id].lock(); + // Forget about the goal if there are no more user references + if (!goal_handle) { + RCLCPP_DEBUG( + this->get_logger(), + "Dropping weak reference to goal handle during status callback"); + goal_handles_.erase(goal_id); + continue; + } + goal_handle->set_status(status.status); + } + } + + /// \internal + void + make_result_aware(typename GoalHandle::SharedPtr goal_handle) + { + // Avoid making more than one request + if (goal_handle->set_result_awareness(true)) { + return; + } + using GoalResultRequest = typename ActionT::Impl::GetResultService::Request; + auto goal_result_request = std::make_shared(); + goal_result_request->goal_id.uuid = goal_handle->get_goal_id(); + try { + this->send_result_request( + std::static_pointer_cast(goal_result_request), + [goal_handle, this](std::shared_ptr response) mutable + { + // Wrap the response in a struct with the fields a user cares about + WrappedResult wrapped_result; + using GoalResultResponse = typename ActionT::Impl::GetResultService::Response; + auto result_response = std::static_pointer_cast(response); + wrapped_result.result = std::make_shared(); + *wrapped_result.result = result_response->result; + wrapped_result.goal_id = goal_handle->get_goal_id(); + wrapped_result.code = static_cast(result_response->status); + goal_handle->set_result(wrapped_result); + std::lock_guard lock(goal_handles_mutex_); + goal_handles_.erase(goal_handle->get_goal_id()); + }); + } catch (rclcpp::exceptions::RCLError & ex) { + // This will cause an exception when the user tries to access the result + goal_handle->invalidate(exceptions::UnawareGoalHandleError(ex.message)); + } + } + + /// \internal + std::shared_future + async_cancel( + typename CancelRequest::SharedPtr cancel_request, + CancelCallback cancel_callback = nullptr) + { + // Put promise in the heap to move it around. + auto promise = std::make_shared>(); + std::shared_future future(promise->get_future()); + this->send_cancel_request( + std::static_pointer_cast(cancel_request), + [cancel_callback, promise](std::shared_ptr response) mutable + { + auto cancel_response = std::static_pointer_cast(response); + promise->set_value(cancel_response); + if (cancel_callback) { + cancel_callback(cancel_response); + } + }); + return future; + } + + std::map goal_handles_; + std::recursive_mutex goal_handles_mutex_; +}; +} // namespace rclcpp_action + +#endif // RCLCPP_ACTION__CLIENT_HPP_ diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp new file mode 100644 index 0000000000..73987ec887 --- /dev/null +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp @@ -0,0 +1,171 @@ +// 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 RCLCPP_ACTION__CLIENT_GOAL_HANDLE_HPP_ +#define RCLCPP_ACTION__CLIENT_GOAL_HANDLE_HPP_ + +#include +#include +#include +#include + +#include "rcl_action/action_client.h" + +#include "action_msgs/msg/goal_status.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/time.hpp" + +#include "rclcpp_action/exceptions.hpp" +#include "rclcpp_action/types.hpp" +#include "rclcpp_action/visibility_control.hpp" + +namespace rclcpp_action +{ +/// The possible statuses that an action goal can finish with. +enum class ResultCode : int8_t +{ + UNKNOWN = action_msgs::msg::GoalStatus::STATUS_UNKNOWN, + SUCCEEDED = action_msgs::msg::GoalStatus::STATUS_SUCCEEDED, + CANCELED = action_msgs::msg::GoalStatus::STATUS_CANCELED, + ABORTED = action_msgs::msg::GoalStatus::STATUS_ABORTED +}; + + +// Forward declarations +template +class Client; + +/// Class for interacting with goals sent from action clients. +/** + * Use this class to check the status of a goal as well as get the result. + * + * This class is not meant to be created by a user, instead it is created when a goal has been + * accepted. + * A `Client` will create an instance and return it to the user (via a future) after calling + * `Client::async_send_goal`. + */ +template +class ClientGoalHandle +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ClientGoalHandle) + + // A wrapper that defines the result of an action + struct WrappedResult + { + /// The unique identifier of the goal + GoalUUID goal_id; + /// A status to indicate if the goal was canceled, aborted, or suceeded + ResultCode code; + /// User defined fields sent back with an action + typename ActionT::Result::SharedPtr result; + }; + + using Feedback = typename ActionT::Feedback; + using Result = typename ActionT::Result; + using FeedbackCallback = + std::function::SharedPtr, + const std::shared_ptr)>; + using ResultCallback = std::function; + + virtual ~ClientGoalHandle(); + + /// Get the unique ID for the goal. + const GoalUUID & + get_goal_id() const; + + /// Get the time when the goal was accepted. + rclcpp::Time + get_goal_stamp() const; + + /// Get the goal status code. + int8_t + get_status(); + + /// Check if an action client has subscribed to feedback for the goal. + bool + is_feedback_aware(); + + /// Check if an action client has requested the result for the goal. + bool + is_result_aware(); + +private: + // The templated Client creates goal handles + friend class Client; + + ClientGoalHandle( + const GoalInfo & info, + FeedbackCallback feedback_callback, + ResultCallback result_callback); + + void + set_feedback_callback(FeedbackCallback callback); + + void + set_result_callback(ResultCallback callback); + + void + call_feedback_callback( + typename ClientGoalHandle::SharedPtr shared_this, + typename std::shared_ptr feedback_message); + + /// Get a future to the goal result. + /** + * This method should not be called if the `ignore_result` flag was set when + * sending the original goal request (see Client::async_send_goal). + * + * `is_result_aware()` can be used to check if it is safe to call this method. + * + * \throws exceptions::UnawareGoalHandleError If the the goal handle is unaware of the result. + * \return A future to the result. + */ + std::shared_future + async_get_result(); + + /// Returns the previous value of awareness + bool + set_result_awareness(bool awareness); + + void + set_status(int8_t status); + + void + set_result(const WrappedResult & wrapped_result); + + void + invalidate(const exceptions::UnawareGoalHandleError & ex); + + bool + is_invalidated() const; + + GoalInfo info_; + + std::exception_ptr invalidate_exception_{nullptr}; + + bool is_result_aware_{false}; + std::promise result_promise_; + std::shared_future result_future_; + + FeedbackCallback feedback_callback_{nullptr}; + ResultCallback result_callback_{nullptr}; + int8_t status_{GoalStatus::STATUS_ACCEPTED}; + + std::recursive_mutex handle_mutex_; +}; +} // namespace rclcpp_action + +#include // NOLINT(build/include_order) +#endif // RCLCPP_ACTION__CLIENT_GOAL_HANDLE_HPP_ diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp new file mode 100644 index 0000000000..58b1d7f248 --- /dev/null +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp @@ -0,0 +1,183 @@ +// 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 RCLCPP_ACTION__CLIENT_GOAL_HANDLE_IMPL_HPP_ +#define RCLCPP_ACTION__CLIENT_GOAL_HANDLE_IMPL_HPP_ + +#include + +#include + +#include "rclcpp/logging.hpp" +#include "rclcpp_action/client_goal_handle.hpp" +#include "rclcpp_action/exceptions.hpp" + +namespace rclcpp_action +{ + +template +ClientGoalHandle::ClientGoalHandle( + const GoalInfo & info, FeedbackCallback feedback_callback, ResultCallback result_callback) +: info_(info), + result_future_(result_promise_.get_future()), + feedback_callback_(feedback_callback), + result_callback_(result_callback) +{ +} + +template +ClientGoalHandle::~ClientGoalHandle() +{ +} + +template +const GoalUUID & +ClientGoalHandle::get_goal_id() const +{ + return info_.goal_id.uuid; +} + +template +rclcpp::Time +ClientGoalHandle::get_goal_stamp() const +{ + return info_.stamp; +} + +template +std::shared_future::WrappedResult> +ClientGoalHandle::async_get_result() +{ + std::lock_guard guard(handle_mutex_); + if (!is_result_aware_) { + throw exceptions::UnawareGoalHandleError(); + } + return result_future_; +} + +template +void +ClientGoalHandle::set_result(const WrappedResult & wrapped_result) +{ + std::lock_guard guard(handle_mutex_); + status_ = static_cast(wrapped_result.code); + result_promise_.set_value(wrapped_result); + if (result_callback_) { + result_callback_(wrapped_result); + result_callback_ = ResultCallback(); + } +} + +template +void +ClientGoalHandle::set_feedback_callback(FeedbackCallback callback) +{ + std::lock_guard guard(handle_mutex_); + feedback_callback_ = callback; +} + +template +void +ClientGoalHandle::set_result_callback(ResultCallback callback) +{ + std::lock_guard guard(handle_mutex_); + result_callback_ = callback; +} + +template +int8_t +ClientGoalHandle::get_status() +{ + std::lock_guard guard(handle_mutex_); + return status_; +} + +template +void +ClientGoalHandle::set_status(int8_t status) +{ + std::lock_guard guard(handle_mutex_); + status_ = status; +} + +template +bool +ClientGoalHandle::is_feedback_aware() +{ + std::lock_guard guard(handle_mutex_); + return feedback_callback_ != nullptr; +} + +template +bool +ClientGoalHandle::is_result_aware() +{ + std::lock_guard guard(handle_mutex_); + return is_result_aware_; +} + +template +bool +ClientGoalHandle::set_result_awareness(bool awareness) +{ + std::lock_guard guard(handle_mutex_); + bool previous = is_result_aware_; + is_result_aware_ = awareness; + return previous; +} + +template +void +ClientGoalHandle::invalidate(const exceptions::UnawareGoalHandleError & ex) +{ + std::lock_guard guard(handle_mutex_); + // Guard against multiple calls + if (is_invalidated()) { + return; + } + is_result_aware_ = false; + invalidate_exception_ = std::make_exception_ptr(ex); + status_ = GoalStatus::STATUS_UNKNOWN; + result_promise_.set_exception(invalidate_exception_); +} + +template +bool +ClientGoalHandle::is_invalidated() const +{ + return invalidate_exception_ != nullptr; +} + +template +void +ClientGoalHandle::call_feedback_callback( + typename ClientGoalHandle::SharedPtr shared_this, + typename std::shared_ptr feedback_message) +{ + if (shared_this.get() != this) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp_action"), "Sent feedback to wrong goal handle."); + return; + } + std::lock_guard guard(handle_mutex_); + if (nullptr == feedback_callback_) { + // Normal, some feedback messages may arrive after the goal result. + RCLCPP_DEBUG(rclcpp::get_logger("rclcpp_action"), "Received feedback but goal ignores it."); + return; + } + feedback_callback_(shared_this, feedback_message); +} + +} // namespace rclcpp_action + +#endif // RCLCPP_ACTION__CLIENT_GOAL_HANDLE_IMPL_HPP_ diff --git a/rclcpp_action/include/rclcpp_action/create_client.hpp b/rclcpp_action/include/rclcpp_action/create_client.hpp new file mode 100644 index 0000000000..f594bca78d --- /dev/null +++ b/rclcpp_action/include/rclcpp_action/create_client.hpp @@ -0,0 +1,122 @@ +// 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 RCLCPP_ACTION__CREATE_CLIENT_HPP_ +#define RCLCPP_ACTION__CREATE_CLIENT_HPP_ + +#include +#include + +#include "rclcpp/node.hpp" + +#include "rclcpp_action/client.hpp" +#include "rclcpp_action/visibility_control.hpp" + +namespace rclcpp_action +{ +/// Create an action client. +/** + * This function is equivalent to \sa create_client()` however is using the individual + * node interfaces to create the client. + * + * \param[in] node_base_interface The node base interface of the corresponding node. + * \param[in] node_graph_interface The node graph interface of the corresponding node. + * \param[in] node_logging_interface The node logging interface of the corresponding node. + * \param[in] node_waitables_interface The node waitables interface of the corresponding node. + * \param[in] name The action name. + * \param[in] group The action client will be added to this callback group. + * If `nullptr`, then the action client is added to the default callback group. + * \param[in] options Options to pass to the underlying `rcl_action_client_t`. + */ +template +typename Client::SharedPtr +create_client( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface, + rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_interface, + const std::string & name, + rclcpp::CallbackGroup::SharedPtr group = nullptr, + const rcl_action_client_options_t & options = rcl_action_client_get_default_options()) +{ + std::weak_ptr weak_node = + node_waitables_interface; + std::weak_ptr weak_group = group; + bool group_is_null = (nullptr == group.get()); + + auto deleter = [weak_node, weak_group, group_is_null](Client * ptr) + { + if (nullptr == ptr) { + return; + } + auto shared_node = weak_node.lock(); + if (shared_node) { + // API expects a shared pointer, give it one with a deleter that does nothing. + std::shared_ptr> fake_shared_ptr(ptr, [](Client *) {}); + + if (group_is_null) { + // Was added to default group + shared_node->remove_waitable(fake_shared_ptr, nullptr); + } else { + // Was added to a specific group + auto shared_group = weak_group.lock(); + if (shared_group) { + shared_node->remove_waitable(fake_shared_ptr, shared_group); + } + } + } + delete ptr; + }; + + std::shared_ptr> action_client( + new Client( + node_base_interface, + node_graph_interface, + node_logging_interface, + name, + options), + deleter); + + node_waitables_interface->add_waitable(action_client, group); + return action_client; +} + +/// Create an action client. +/** + * \param[in] node The action client will be added to this node. + * \param[in] name The action name. + * \param[in] group The action client will be added to this callback group. + * If `nullptr`, then the action client is added to the default callback group. + * \param[in] options Options to pass to the underlying `rcl_action_client_t`. + */ +template +typename Client::SharedPtr +create_client( + NodeT node, + const std::string & name, + rclcpp::CallbackGroup::SharedPtr group = nullptr, + const rcl_action_client_options_t & options = rcl_action_client_get_default_options()) +{ + return rclcpp_action::create_client( + node->get_node_base_interface(), + node->get_node_graph_interface(), + node->get_node_logging_interface(), + node->get_node_waitables_interface(), + name, + group, + options); +} +} // namespace rclcpp_action + +#endif // RCLCPP_ACTION__CREATE_CLIENT_HPP_ diff --git a/rclcpp_action/include/rclcpp_action/create_server.hpp b/rclcpp_action/include/rclcpp_action/create_server.hpp new file mode 100644 index 0000000000..c333a60581 --- /dev/null +++ b/rclcpp_action/include/rclcpp_action/create_server.hpp @@ -0,0 +1,154 @@ +// 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 RCLCPP_ACTION__CREATE_SERVER_HPP_ +#define RCLCPP_ACTION__CREATE_SERVER_HPP_ + +#include +#include + +#include "rcl_action/action_server.h" + +#include "rclcpp/node.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_clock_interface.hpp" +#include "rclcpp/node_interfaces/node_logging_interface.hpp" +#include "rclcpp/node_interfaces/node_waitables_interface.hpp" + +#include "rclcpp_action/server.hpp" +#include "rclcpp_action/visibility_control.hpp" + +namespace rclcpp_action +{ +/// Create an action server. +/** + * All provided callback functions must be non-blocking. + * This function is equivalent to \sa create_server()` however is using the individual + * node interfaces to create the server. + * + * \sa Server::Server() for more information. + * + * \param[in] node_base_interface The node base interface of the corresponding node. + * \param[in] node_clock_interface The node clock interface of the corresponding node. + * \param[in] node_logging_interface The node logging interface of the corresponding node. + * \param[in] node_waitables_interface The node waitables interface of the corresponding node. + * \param[in] name The action name. + * \param[in] handle_goal A callback that decides if a goal should be accepted or rejected. + * \param[in] handle_cancel A callback that decides if a goal should be attempted to be canceled. + * The return from this callback only indicates if the server will try to cancel a goal. + * It does not indicate if the goal was actually canceled. + * \param[in] handle_accepted A callback that is called to give the user a handle to the goal. + * \param[in] options Options to pass to the underlying `rcl_action_server_t`. + * \param[in] group The action server will be added to this callback group. + * If `nullptr`, then the action server is added to the default callback group. + */ +template +typename Server::SharedPtr +create_server( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_interface, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface, + rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_interface, + const std::string & name, + typename Server::GoalCallback handle_goal, + typename Server::CancelCallback handle_cancel, + typename Server::AcceptedCallback handle_accepted, + const rcl_action_server_options_t & options = rcl_action_server_get_default_options(), + rclcpp::CallbackGroup::SharedPtr group = nullptr) +{ + std::weak_ptr weak_node = + node_waitables_interface; + std::weak_ptr weak_group = group; + bool group_is_null = (nullptr == group.get()); + + auto deleter = [weak_node, weak_group, group_is_null](Server * ptr) + { + if (nullptr == ptr) { + return; + } + auto shared_node = weak_node.lock(); + if (shared_node) { + // API expects a shared pointer, give it one with a deleter that does nothing. + std::shared_ptr> fake_shared_ptr(ptr, [](Server *) {}); + + if (group_is_null) { + // Was added to default group + shared_node->remove_waitable(fake_shared_ptr, nullptr); + } else { + // Was added to a specific group + auto shared_group = weak_group.lock(); + if (shared_group) { + shared_node->remove_waitable(fake_shared_ptr, shared_group); + } + } + } + delete ptr; + }; + + std::shared_ptr> action_server(new Server( + node_base_interface, + node_clock_interface, + node_logging_interface, + name, + options, + handle_goal, + handle_cancel, + handle_accepted), deleter); + + node_waitables_interface->add_waitable(action_server, group); + return action_server; +} + +/// Create an action server. +/** + * All provided callback functions must be non-blocking. + * + * \sa Server::Server() for more information. + * + * \param[in] node The action server will be added to this node. + * \param[in] name The action name. + * \param[in] handle_goal A callback that decides if a goal should be accepted or rejected. + * \param[in] handle_cancel A callback that decides if a goal should be attempted to be canceled. + * The return from this callback only indicates if the server will try to cancel a goal. + * It does not indicate if the goal was actually canceled. + * \param[in] handle_accepted A callback that is called to give the user a handle to the goal. + * \param[in] options Options to pass to the underlying `rcl_action_server_t`. + * \param[in] group The action server will be added to this callback group. + * If `nullptr`, then the action server is added to the default callback group. + */ +template +typename Server::SharedPtr +create_server( + NodeT node, + const std::string & name, + typename Server::GoalCallback handle_goal, + typename Server::CancelCallback handle_cancel, + typename Server::AcceptedCallback handle_accepted, + const rcl_action_server_options_t & options = rcl_action_server_get_default_options(), + rclcpp::CallbackGroup::SharedPtr group = nullptr) +{ + return create_server( + node->get_node_base_interface(), + node->get_node_clock_interface(), + node->get_node_logging_interface(), + node->get_node_waitables_interface(), + name, + handle_goal, + handle_cancel, + handle_accepted, + options, + group); +} +} // namespace rclcpp_action +#endif // RCLCPP_ACTION__CREATE_SERVER_HPP_ diff --git a/rclcpp_action/include/rclcpp_action/exceptions.hpp b/rclcpp_action/include/rclcpp_action/exceptions.hpp new file mode 100644 index 0000000000..a1fcf50bff --- /dev/null +++ b/rclcpp_action/include/rclcpp_action/exceptions.hpp @@ -0,0 +1,48 @@ +// 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 RCLCPP_ACTION__EXCEPTIONS_HPP_ +#define RCLCPP_ACTION__EXCEPTIONS_HPP_ + +#include +#include + +namespace rclcpp_action +{ +namespace exceptions +{ +class UnknownGoalHandleError : public std::invalid_argument +{ +public: + UnknownGoalHandleError() + : std::invalid_argument("Goal handle is not known to this client.") + { + } +}; + +class UnawareGoalHandleError : public std::runtime_error +{ +public: + UnawareGoalHandleError( + const std::string & message = "Goal handle is not tracking the goal result.") + : std::runtime_error(message) + { + } +}; + +} // namespace exceptions + +} // namespace rclcpp_action + +#endif // RCLCPP_ACTION__EXCEPTIONS_HPP_ diff --git a/rclcpp_action/include/rclcpp_action/qos.hpp b/rclcpp_action/include/rclcpp_action/qos.hpp new file mode 100644 index 0000000000..2acfddbc10 --- /dev/null +++ b/rclcpp_action/include/rclcpp_action/qos.hpp @@ -0,0 +1,34 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP_ACTION__QOS_HPP_ +#define RCLCPP_ACTION__QOS_HPP_ + +#include + +#include "rclcpp_action/visibility_control.hpp" + +namespace rclcpp_action +{ + +class DefaultActionStatusQoS : public rclcpp::QoS +{ +public: + RCLCPP_ACTION_PUBLIC + DefaultActionStatusQoS(); +}; + +} // namespace rclcpp_action + +#endif // RCLCPP_ACTION__QOS_HPP_ diff --git a/rclcpp_action/include/rclcpp_action/rclcpp_action.hpp b/rclcpp_action/include/rclcpp_action/rclcpp_action.hpp new file mode 100644 index 0000000000..61122e9ff5 --- /dev/null +++ b/rclcpp_action/include/rclcpp_action/rclcpp_action.hpp @@ -0,0 +1,44 @@ +// 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. + +/** \mainpage rclcpp_action: ROS Action Client Library for C++ + * + * `rclcpp_action` provides the canonical C++ API for interacting with ROS Actions. + * It consists of these main components: + * + * - Action Client + * - rclcpp_action/client.hpp + * - rclcpp_action/create_client.hpp + * - rclcpp_action/client_goal_handle.hpp + * - Action Server + * - rclcpp_action/server.hpp + * - rclcpp_action/create_server.hpp + * - rclcpp_action/server_goal_handle.hpp + */ + +#ifndef RCLCPP_ACTION__RCLCPP_ACTION_HPP_ +#define RCLCPP_ACTION__RCLCPP_ACTION_HPP_ + +#include +#include + +#include "rclcpp_action/client.hpp" +#include "rclcpp_action/client_goal_handle.hpp" +#include "rclcpp_action/create_client.hpp" +#include "rclcpp_action/create_server.hpp" +#include "rclcpp_action/server.hpp" +#include "rclcpp_action/server_goal_handle.hpp" +#include "rclcpp_action/visibility_control.hpp" + +#endif // RCLCPP_ACTION__RCLCPP_ACTION_HPP_ diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp new file mode 100644 index 0000000000..a885383614 --- /dev/null +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -0,0 +1,586 @@ +// 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 RCLCPP_ACTION__SERVER_HPP_ +#define RCLCPP_ACTION__SERVER_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "action_msgs/srv/cancel_goal.hpp" +#include "rcl/event_callback.h" +#include "rcl_action/action_server.h" +#include "rosidl_runtime_c/action_type_support_struct.h" +#include "rosidl_typesupport_cpp/action_type_support.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_clock_interface.hpp" +#include "rclcpp/node_interfaces/node_logging_interface.hpp" +#include "rclcpp/waitable.hpp" + +#include "rclcpp_action/visibility_control.hpp" +#include "rclcpp_action/server_goal_handle.hpp" +#include "rclcpp_action/types.hpp" + +namespace rclcpp_action +{ +// Forward declaration +class ServerBaseImpl; + +/// A response returned by an action server callback when a goal is requested. +enum class GoalResponse : int8_t +{ + /// The goal is rejected and will not be executed. + REJECT = 1, + /// The server accepts the goal, and is going to begin execution immediately. + ACCEPT_AND_EXECUTE = 2, + /// The server accepts the goal, and is going to execute it later. + ACCEPT_AND_DEFER = 3, +}; + +/// A response returned by an action server callback when a goal has been asked to be canceled. +enum class CancelResponse : int8_t +{ + /// The server will not try to cancel the goal. + REJECT = 1, + /// The server has agreed to try to cancel the goal. + ACCEPT = 2, +}; + +/// Base Action Server implementation +/// \internal +/** + * This class should not be used directly by users writing an action server. + * Instead users should use `rclcpp_action::Server`. + * + * Internally, this class is responsible for interfacing with the `rcl_action` API. + */ +class ServerBase : public rclcpp::Waitable +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ServerBase) + + /// Enum to identify entities belonging to the action server + enum class EntityType : std::size_t + { + GoalService, + ResultService, + CancelService, + Expired, + }; + + RCLCPP_ACTION_PUBLIC + virtual ~ServerBase(); + + // ------------- + // Waitables API + + /// Return the number of subscriptions used to implement an action server + /// \internal + RCLCPP_ACTION_PUBLIC + size_t + get_number_of_ready_subscriptions() override; + + /// Return the number of timers used to implement an action server + /// \internal + RCLCPP_ACTION_PUBLIC + size_t + get_number_of_ready_timers() override; + + /// Return the number of service clients used to implement an action server + /// \internal + RCLCPP_ACTION_PUBLIC + size_t + get_number_of_ready_clients() override; + + /// Return the number of service servers used to implement an action server + /// \internal + RCLCPP_ACTION_PUBLIC + size_t + get_number_of_ready_services() override; + + /// Return the number of guard conditions used to implement an action server + /// \internal + RCLCPP_ACTION_PUBLIC + size_t + get_number_of_ready_guard_conditions() override; + + /// Add all entities to a wait set. + /// \internal + RCLCPP_ACTION_PUBLIC + void + add_to_wait_set(rcl_wait_set_t & wait_set) override; + + /// Return true if any entity belonging to the action server is ready to be executed. + /// \internal + RCLCPP_ACTION_PUBLIC + bool + is_ready(const rcl_wait_set_t & wait_set) override; + + RCLCPP_ACTION_PUBLIC + std::shared_ptr + take_data() override; + + RCLCPP_ACTION_PUBLIC + std::shared_ptr + take_data_by_entity_id(size_t id) override; + + /// Act on entities in the wait set which are ready to be acted upon. + /// \internal + RCLCPP_ACTION_PUBLIC + void + execute(const std::shared_ptr & data) override; + + /// \internal + /// Set a callback to be called when action server entities have an event + /** + * The callback receives a size_t which is the number of messages received + * since the last time this callback was called. + * Normally this is 1, but can be > 1 if messages were received before any + * callback was set. + * + * The callback also receives an int identifier argument, which identifies + * the action server entity which is ready. + * This implies that the provided callback can use the identifier to behave + * differently depending on which entity triggered the waitable to become ready. + * + * Calling it again will clear any previously set callback. + * + * An exception will be thrown if the callback is not callable. + * + * This function is thread-safe. + * + * If you want more information available in the callback, like the subscription + * or other information, you may use a lambda with captures or std::bind. + * + * \param[in] callback functor to be called when a new message is received. + */ + RCLCPP_ACTION_PUBLIC + void + set_on_ready_callback(std::function callback) override; + + /// Unset the callback to be called whenever the waitable becomes ready. + RCLCPP_ACTION_PUBLIC + void + clear_on_ready_callback() override; + + // End Waitables API + // ----------------- + +protected: + RCLCPP_ACTION_PUBLIC + ServerBase( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + const std::string & name, + const rosidl_action_type_support_t * type_support, + const rcl_action_server_options_t & options); + + // ----------------------------------------------------- + // API for communication between ServerBase and Server<> + + // ServerBase will call this function when a goal request is received. + // The subclass should convert to the real type and call a user's callback. + /// \internal + RCLCPP_ACTION_PUBLIC + virtual + std::pair> + call_handle_goal_callback(GoalUUID &, std::shared_ptr request) = 0; + + // ServerBase will determine which goal ids are being cancelled, and then call this function for + // each goal id. + // The subclass should look up a goal handle and call the user's callback. + /// \internal + RCLCPP_ACTION_PUBLIC + virtual + CancelResponse + call_handle_cancel_callback(const GoalUUID & uuid) = 0; + + /// Given a goal request message, return the UUID contained within. + /// \internal + RCLCPP_ACTION_PUBLIC + virtual + GoalUUID + get_goal_id_from_goal_request(void * message) = 0; + + /// Create an empty goal request message so it can be taken from a lower layer. + /// \internal + RCLCPP_ACTION_PUBLIC + virtual + std::shared_ptr + create_goal_request() = 0; + + /// Call user callback to inform them a goal has been accepted. + /// \internal + RCLCPP_ACTION_PUBLIC + virtual + void + call_goal_accepted_callback( + std::shared_ptr rcl_goal_handle, + GoalUUID uuid, std::shared_ptr goal_request_message) = 0; + + /// Given a result request message, return the UUID contained within. + /// \internal + RCLCPP_ACTION_PUBLIC + virtual + GoalUUID + get_goal_id_from_result_request(void * message) = 0; + + /// Create an empty goal request message so it can be taken from a lower layer. + /// \internal + RCLCPP_ACTION_PUBLIC + virtual + std::shared_ptr + create_result_request() = 0; + + /// Create an empty goal result message so it can be sent as a reply in a lower layer + /// \internal + RCLCPP_ACTION_PUBLIC + virtual + std::shared_ptr + create_result_response(decltype(action_msgs::msg::GoalStatus::status) status) = 0; + + /// \internal + RCLCPP_ACTION_PUBLIC + void + publish_status(); + + /// \internal + RCLCPP_ACTION_PUBLIC + void + notify_goal_terminal_state(); + + /// \internal + RCLCPP_ACTION_PUBLIC + void + publish_result(const GoalUUID & uuid, std::shared_ptr result_msg); + + /// \internal + RCLCPP_ACTION_PUBLIC + void + publish_feedback(std::shared_ptr feedback_msg); + + // End API for communication between ServerBase and Server<> + // --------------------------------------------------------- + +private: + /// Handle a request to add a new goal to the server + /// \internal + RCLCPP_ACTION_PUBLIC + void + execute_goal_request_received( + rcl_ret_t ret, + rcl_action_goal_info_t goal_info, + rmw_request_id_t request_header, + std::shared_ptr message); + + /// Handle a request to cancel goals on the server + /// \internal + RCLCPP_ACTION_PUBLIC + void + execute_cancel_request_received( + rcl_ret_t ret, + std::shared_ptr request, + rmw_request_id_t request_header); + + /// Handle a request to get the result of an action + /// \internal + RCLCPP_ACTION_PUBLIC + void + execute_result_request_received( + rcl_ret_t ret, + std::shared_ptr result_request, + rmw_request_id_t request_header); + + /// Handle a timeout indicating a completed goal should be forgotten by the server + /// \internal + RCLCPP_ACTION_PUBLIC + void + execute_check_expired_goals(); + + /// Private implementation + /// \internal + std::unique_ptr pimpl_; + + /// Set a std::function callback to be called when the specified entity is ready + RCLCPP_ACTION_PUBLIC + void + set_callback_to_entity( + EntityType entity_type, + std::function callback); + +protected: + // Mutex to protect the callbacks storage. + std::recursive_mutex listener_mutex_; + // Storage for std::function callbacks to keep them in scope + std::unordered_map> entity_type_to_on_ready_callback_; + + /// Set a callback to be called when the specified entity is ready + RCLCPP_ACTION_PUBLIC + void + set_on_ready_callback( + EntityType entity_type, + rcl_event_callback_t callback, + const void * user_data); + + bool on_ready_callback_set_{false}; +}; + +/// Action Server +/** + * This class creates an action server. + * + * Create an instance of this server using `rclcpp_action::create_server()`. + * + * Internally, this class is responsible for: + * - coverting between the C++ action type and generic types for `rclcpp_action::ServerBase`, and + * - calling user callbacks. + */ +template +class Server : public ServerBase, public std::enable_shared_from_this> +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Server) + + /// Signature of a callback that accepts or rejects goal requests. + using GoalCallback = std::function)>; + /// Signature of a callback that accepts or rejects requests to cancel a goal. + using CancelCallback = std::function>)>; + /// Signature of a callback that is used to notify when the goal has been accepted. + using AcceptedCallback = std::function>)>; + + /// Construct an action server. + /** + * This constructs an action server, but it will not work until it has been added to a node. + * Use `rclcpp_action::create_server()` to both construct and add to a node. + * + * Three callbacks must be provided: + * - one to accept or reject goals sent to the server, + * - one to accept or reject requests to cancel a goal, + * - one to receive a goal handle after a goal has been accepted. + * All callbacks must be non-blocking. + * The result of a goal should be set using methods on `rclcpp_action::ServerGoalHandle`. + * + * \param[in] node_base a pointer to the base interface of a node. + * \param[in] node_clock a pointer to an interface that allows getting a node's clock. + * \param[in] node_logging a pointer to an interface that allows getting a node's logger. + * \param[in] name the name of an action. + * The same name and type must be used by both the action client and action server to + * communicate. + * \param[in] options Options to pass to the underlying `rcl_action_server_t`. + * \param[in] handle_goal a callback that decides if a goal should be accepted or rejected. + * \param[in] handle_cancel a callback that decides if a goal should be attemted to be canceled. + * The return from this callback only indicates if the server will try to cancel a goal. + * It does not indicate if the goal was actually canceled. + * \param[in] handle_accepted a callback that is called to give the user a handle to the goal. + * execution. + */ + Server( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + const std::string & name, + const rcl_action_server_options_t & options, + GoalCallback handle_goal, + CancelCallback handle_cancel, + AcceptedCallback handle_accepted + ) + : ServerBase( + node_base, + node_clock, + node_logging, + name, + rosidl_typesupport_cpp::get_action_type_support_handle(), + options), + handle_goal_(handle_goal), + handle_cancel_(handle_cancel), + handle_accepted_(handle_accepted) + { + } + + virtual ~Server() = default; + +protected: + // ----------------------------------------------------- + // API for communication between ServerBase and Server<> + + /// \internal + std::pair> + call_handle_goal_callback(GoalUUID & uuid, std::shared_ptr message) override + { + auto request = std::static_pointer_cast< + typename ActionT::Impl::SendGoalService::Request>(message); + auto goal = std::shared_ptr(request, &request->goal); + GoalResponse user_response = handle_goal_(uuid, goal); + + auto ros_response = std::make_shared(); + ros_response->accepted = GoalResponse::ACCEPT_AND_EXECUTE == user_response || + GoalResponse::ACCEPT_AND_DEFER == user_response; + return std::make_pair(user_response, ros_response); + } + + /// \internal + CancelResponse + call_handle_cancel_callback(const GoalUUID & uuid) override + { + std::shared_ptr> goal_handle; + { + std::lock_guard lock(goal_handles_mutex_); + auto element = goal_handles_.find(uuid); + if (element != goal_handles_.end()) { + goal_handle = element->second.lock(); + } + } + + CancelResponse resp = CancelResponse::REJECT; + if (goal_handle) { + resp = handle_cancel_(goal_handle); + if (CancelResponse::ACCEPT == resp) { + try { + goal_handle->_cancel_goal(); + } catch (const rclcpp::exceptions::RCLError & ex) { + RCLCPP_DEBUG( + rclcpp::get_logger("rclcpp_action"), + "Failed to cancel goal in call_handle_cancel_callback: %s", ex.what()); + return CancelResponse::REJECT; + } + } + } + return resp; + } + + /// \internal + void + call_goal_accepted_callback( + std::shared_ptr rcl_goal_handle, + GoalUUID uuid, + std::shared_ptr goal_request_message) override + { + std::shared_ptr> goal_handle; + std::weak_ptr> weak_this = this->shared_from_this(); + + std::function)> on_terminal_state = + [weak_this](const GoalUUID & goal_uuid, std::shared_ptr result_message) + { + std::shared_ptr> shared_this = weak_this.lock(); + if (!shared_this) { + return; + } + // Send result message to anyone that asked + shared_this->publish_result(goal_uuid, result_message); + // Publish a status message any time a goal handle changes state + shared_this->publish_status(); + // notify base so it can recalculate the expired goal timer + shared_this->notify_goal_terminal_state(); + // Delete data now (ServerBase and rcl_action_server_t keep data until goal handle expires) + std::lock_guard lock(shared_this->goal_handles_mutex_); + shared_this->goal_handles_.erase(goal_uuid); + }; + + std::function on_executing = + [weak_this](const GoalUUID & goal_uuid) + { + std::shared_ptr> shared_this = weak_this.lock(); + if (!shared_this) { + return; + } + (void)goal_uuid; + // Publish a status message any time a goal handle changes state + shared_this->publish_status(); + }; + + std::function)> publish_feedback = + [weak_this](std::shared_ptr feedback_msg) + { + std::shared_ptr> shared_this = weak_this.lock(); + if (!shared_this) { + return; + } + shared_this->publish_feedback(std::static_pointer_cast(feedback_msg)); + }; + + auto request = std::static_pointer_cast< + const typename ActionT::Impl::SendGoalService::Request>(goal_request_message); + auto goal = std::shared_ptr(request, &request->goal); + goal_handle.reset( + new ServerGoalHandle( + rcl_goal_handle, uuid, goal, on_terminal_state, on_executing, publish_feedback)); + { + std::lock_guard lock(goal_handles_mutex_); + goal_handles_[uuid] = goal_handle; + } + handle_accepted_(goal_handle); + } + + /// \internal + GoalUUID + get_goal_id_from_goal_request(void * message) override + { + return + static_cast(message)->goal_id.uuid; + } + + /// \internal + std::shared_ptr + create_goal_request() override + { + return std::shared_ptr(new typename ActionT::Impl::SendGoalService::Request()); + } + + /// \internal + GoalUUID + get_goal_id_from_result_request(void * message) override + { + return + static_cast(message)->goal_id.uuid; + } + + /// \internal + std::shared_ptr + create_result_request() override + { + return std::shared_ptr(new typename ActionT::Impl::GetResultService::Request()); + } + + /// \internal + std::shared_ptr + create_result_response(decltype(action_msgs::msg::GoalStatus::status) status) override + { + auto result = std::make_shared(); + result->status = status; + return std::static_pointer_cast(result); + } + + // End API for communication between ServerBase and Server<> + // --------------------------------------------------------- + +private: + GoalCallback handle_goal_; + CancelCallback handle_cancel_; + AcceptedCallback handle_accepted_; + + using GoalHandleWeakPtr = std::weak_ptr>; + /// A map of goal id to goal handle weak pointers. + /// This is used to provide a goal handle to handle_cancel. + std::unordered_map goal_handles_; + std::mutex goal_handles_mutex_; +}; +} // namespace rclcpp_action +#endif // RCLCPP_ACTION__SERVER_HPP_ diff --git a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp new file mode 100644 index 0000000000..07873f4c71 --- /dev/null +++ b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp @@ -0,0 +1,284 @@ +// 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 RCLCPP_ACTION__SERVER_GOAL_HANDLE_HPP_ +#define RCLCPP_ACTION__SERVER_GOAL_HANDLE_HPP_ + +#include +#include +#include + +#include "rcl_action/types.h" +#include "rcl_action/goal_handle.h" + +#include "action_msgs/msg/goal_status.hpp" + +#include "rclcpp_action/visibility_control.hpp" +#include "rclcpp_action/types.hpp" + +namespace rclcpp_action +{ + +/// Base class to interact with goals on a server. +/// \internal +/** + * + * This class in not be used directly by users writing an action server. + * Instead users will be given an instance of `rclcpp_action::ServerGoalHandle<>`. + * + * Internally, this class is responsible for interfacing with the `rcl_action` API. + */ +class ServerGoalHandleBase +{ +public: + /// Indicate if client has requested this goal be cancelled. + /// \return true if a cancellation request has been accepted for this goal. + RCLCPP_ACTION_PUBLIC + bool + is_canceling() const; + + /// Indicate if goal is pending or executing. + /// \return false if goal has reached a terminal state. + RCLCPP_ACTION_PUBLIC + bool + is_active() const; + + /// Indicate if goal is executing. + /// \return true only if the goal is in an executing state. + RCLCPP_ACTION_PUBLIC + bool + is_executing() const; + + RCLCPP_ACTION_PUBLIC + virtual + ~ServerGoalHandleBase(); + +protected: + // ------------------------------------------------------------------------- + // API for communication between ServerGoalHandleBase and ServerGoalHandle<> + + /// \internal + RCLCPP_ACTION_PUBLIC + ServerGoalHandleBase( + std::shared_ptr rcl_handle + ) + : rcl_handle_(rcl_handle) + { + } + + /// \internal + RCLCPP_ACTION_PUBLIC + void + _abort(); + + /// \internal + RCLCPP_ACTION_PUBLIC + void + _succeed(); + + /// \internal + RCLCPP_ACTION_PUBLIC + void + _cancel_goal(); + + /// \internal + RCLCPP_ACTION_PUBLIC + void + _canceled(); + + /// \internal + RCLCPP_ACTION_PUBLIC + void + _execute(); + + /// Transition the goal to canceled state if it never reached a terminal state. + /// \internal + RCLCPP_ACTION_PUBLIC + bool + try_canceling() noexcept; + + // End API for communication between ServerGoalHandleBase and ServerGoalHandle<> + // ----------------------------------------------------------------------------- + +private: + std::shared_ptr rcl_handle_; + mutable std::mutex rcl_handle_mutex_; +}; + +// Forward declare server +template +class Server; + +/// Class to interact with goals on a server. +/** + * Use this class to check the status of a goal as well as set the result. + * + * This class is not meant to be created by a user, instead it is created when a goal has been + * accepted. + * A `Server` will create an instance and give it to the user in their `handle_accepted` callback. + * + * Internally, this class is responsible for converting between the C++ action type and generic + * types for `rclcpp_action::ServerGoalHandleBase`. + */ +template +class ServerGoalHandle : public ServerGoalHandleBase +{ +public: + /// Send an update about the progress of a goal. + /** + * This must only be called when the goal is executing. + * If execution of a goal is deferred then `ServerGoalHandle::set_executing()` must be called + * first. + * + * \throws std::runtime_error If the goal is in any state besides executing. + * + * \param[in] feedback_msg the message to publish to clients. + */ + void + publish_feedback(std::shared_ptr feedback_msg) + { + auto feedback_message = std::make_shared(); + feedback_message->goal_id.uuid = uuid_; + feedback_message->feedback = *feedback_msg; + publish_feedback_(feedback_message); + } + + /// Indicate that a goal could not be reached and has been aborted. + /** + * Only call this if the goal was executing but cannot be completed. + * This is a terminal state, no more methods should be called on a goal handle after this is + * called. + * + * \throws rclcpp::exceptions::RCLError If the goal is in any state besides executing. + * + * \param[in] result_msg the final result to send to clients. + */ + void + abort(typename ActionT::Result::SharedPtr result_msg) + { + _abort(); + auto response = std::make_shared(); + response->status = action_msgs::msg::GoalStatus::STATUS_ABORTED; + response->result = *result_msg; + on_terminal_state_(uuid_, response); + } + + /// Indicate that a goal has succeeded. + /** + * Only call this if the goal is executing and has reached the desired final state. + * This is a terminal state, no more methods should be called on a goal handle after this is + * called. + * + * \throws rclcpp::exceptions::RCLError If the goal is in any state besides executing. + * + * \param[in] result_msg the final result to send to clients. + */ + void + succeed(typename ActionT::Result::SharedPtr result_msg) + { + _succeed(); + auto response = std::make_shared(); + response->status = action_msgs::msg::GoalStatus::STATUS_SUCCEEDED; + response->result = *result_msg; + on_terminal_state_(uuid_, response); + } + + /// Indicate that a goal has been canceled. + /** + * Only call this if the goal is canceling. + * This is a terminal state, no more methods should be called on a goal handle after this is + * called. + * + * \throws rclcpp::exceptions::RCLError If the goal is in any state besides executing. + * + * \param[in] result_msg the final result to send to clients. + */ + void + canceled(typename ActionT::Result::SharedPtr result_msg) + { + _canceled(); + auto response = std::make_shared(); + response->status = action_msgs::msg::GoalStatus::STATUS_CANCELED; + response->result = *result_msg; + on_terminal_state_(uuid_, response); + } + + /// Indicate that the server is starting to execute a goal. + /** + * Only call this if the goal is pending. + * + * \throws rclcpp::exceptions::RCLError If the goal is in any state besides executing. + */ + void + execute() + { + _execute(); + on_executing_(uuid_); + } + + /// Get the user provided message describing the goal. + const std::shared_ptr + get_goal() const + { + return goal_; + } + + /// Get the unique identifier of the goal + const GoalUUID & + get_goal_id() const + { + return uuid_; + } + + virtual ~ServerGoalHandle() + { + // Cancel goal if handle was allowed to destruct without reaching a terminal state + if (try_canceling()) { + auto null_result = std::make_shared(); + null_result->status = action_msgs::msg::GoalStatus::STATUS_CANCELED; + on_terminal_state_(uuid_, null_result); + } + } + +protected: + /// \internal + ServerGoalHandle( + std::shared_ptr rcl_handle, + GoalUUID uuid, + std::shared_ptr goal, + std::function)> on_terminal_state, + std::function on_executing, + std::function)> publish_feedback + ) + : ServerGoalHandleBase(rcl_handle), goal_(goal), uuid_(uuid), + on_terminal_state_(on_terminal_state), on_executing_(on_executing), + publish_feedback_(publish_feedback) + { + } + + /// The user provided message describing the goal. + const std::shared_ptr goal_; + + /// A unique id for the goal request. + const GoalUUID uuid_; + + friend class Server; + + std::function)> on_terminal_state_; + std::function on_executing_; + std::function)> publish_feedback_; +}; +} // namespace rclcpp_action + +#endif // RCLCPP_ACTION__SERVER_GOAL_HANDLE_HPP_ diff --git a/rclcpp_action/include/rclcpp_action/types.hpp b/rclcpp_action/include/rclcpp_action/types.hpp new file mode 100644 index 0000000000..cf359dfaa9 --- /dev/null +++ b/rclcpp_action/include/rclcpp_action/types.hpp @@ -0,0 +1,84 @@ +// 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 RCLCPP_ACTION__TYPES_HPP_ +#define RCLCPP_ACTION__TYPES_HPP_ + +#include +#include +#include +#include + +#include "rcl_action/types.h" + +#include "action_msgs/msg/goal_status.hpp" +#include "action_msgs/msg/goal_info.hpp" + +#include "rclcpp_action/visibility_control.hpp" + +namespace rclcpp_action +{ + +using GoalUUID = std::array; +using GoalStatus = action_msgs::msg::GoalStatus; +using GoalInfo = action_msgs::msg::GoalInfo; + +/// Convert a goal id to a human readable RFC-4122 compliant string. +RCLCPP_ACTION_PUBLIC +std::string +to_string(const GoalUUID & goal_id); + +// Convert C++ GoalID to rcl_action_goal_info_t +RCLCPP_ACTION_PUBLIC +void +convert(const GoalUUID & goal_id, rcl_action_goal_info_t * info); + +// Convert rcl_action_goal_info_t to C++ GoalID +RCLCPP_ACTION_PUBLIC +void +convert(const rcl_action_goal_info_t & info, GoalUUID * goal_id); +} // namespace rclcpp_action + +namespace std +{ +template<> +struct less +{ + bool operator()( + const rclcpp_action::GoalUUID & lhs, + const rclcpp_action::GoalUUID & rhs) const + { + return lhs < rhs; + } +}; + +/// Hash a goal id so it can be used as a key in std::unordered_map +template<> +struct hash +{ + size_t operator()(const rclcpp_action::GoalUUID & uuid) const noexcept + { + // Using the FNV-1a hash algorithm + constexpr size_t FNV_prime = 1099511628211u; + size_t result = 14695981039346656037u; + + for (const auto & byte : uuid) { + result ^= byte; + result *= FNV_prime; + } + return result; + } +}; +} // namespace std +#endif // RCLCPP_ACTION__TYPES_HPP_ diff --git a/rclcpp_action/include/rclcpp_action/visibility_control.hpp b/rclcpp_action/include/rclcpp_action/visibility_control.hpp new file mode 100644 index 0000000000..32b03dd8f5 --- /dev/null +++ b/rclcpp_action/include/rclcpp_action/visibility_control.hpp @@ -0,0 +1,56 @@ +// 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. + +/* This header must be included by all rclcpp headers which declare symbols + * which are defined in the rclcpp library. When not building the rclcpp + * library, i.e. when using the headers in other package's code, the contents + * of this header change the visibility of certain symbols which the rclcpp + * library cannot have, but the consuming code must have inorder to link. + */ + +#ifndef RCLCPP_ACTION__VISIBILITY_CONTROL_HPP_ +#define RCLCPP_ACTION__VISIBILITY_CONTROL_HPP_ + +// 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 RCLCPP_ACTION_EXPORT __attribute__ ((dllexport)) + #define RCLCPP_ACTION_IMPORT __attribute__ ((dllimport)) + #else + #define RCLCPP_ACTION_EXPORT __declspec(dllexport) + #define RCLCPP_ACTION_IMPORT __declspec(dllimport) + #endif + #ifdef RCLCPP_ACTION_BUILDING_LIBRARY + #define RCLCPP_ACTION_PUBLIC RCLCPP_ACTION_EXPORT + #else + #define RCLCPP_ACTION_PUBLIC RCLCPP_ACTION_IMPORT + #endif + #define RCLCPP_ACTION_PUBLIC_TYPE RCLCPP_ACTION_PUBLIC + #define RCLCPP_ACTION_LOCAL +#else + #define RCLCPP_ACTION_EXPORT __attribute__ ((visibility("default"))) + #define RCLCPP_ACTION_IMPORT + #if __GNUC__ >= 4 + #define RCLCPP_ACTION_PUBLIC __attribute__ ((visibility("default"))) + #define RCLCPP_ACTION_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define RCLCPP_ACTION_PUBLIC + #define RCLCPP_ACTION_LOCAL + #endif + #define RCLCPP_ACTION_PUBLIC_TYPE +#endif + +#endif // RCLCPP_ACTION__VISIBILITY_CONTROL_HPP_ diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml new file mode 100644 index 0000000000..6fb522f1fe --- /dev/null +++ b/rclcpp_action/package.xml @@ -0,0 +1,41 @@ + + + + rclcpp_action + 29.2.0 + Adds action APIs for C++. + + Ivan Paunovic + Michel Hidalgo + William Woodall + + Apache License 2.0 + + Dirk Thomas + Jacob Perron + + ament_cmake_ros + + rosidl_runtime_c + + rosidl_runtime_c + + action_msgs + rclcpp + rcl_action + rcl + rcpputils + + ament_cmake + + ament_cmake_gtest + ament_lint_auto + ament_lint_common + mimick_vendor + performance_test_fixture + test_msgs + + + ament_cmake + + diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp new file mode 100644 index 0000000000..3ea9b1fb1a --- /dev/null +++ b/rclcpp_action/src/client.cpp @@ -0,0 +1,800 @@ +// 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 +#include +#include +#include + +#include "rcl_action/action_client.h" +#include "rcl_action/wait.h" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_logging_interface.hpp" + +#include "rclcpp_action/client.hpp" +#include "rclcpp_action/exceptions.hpp" + +namespace rclcpp_action +{ + +struct ClientBaseData +{ + struct FeedbackReadyData + { + FeedbackReadyData(rcl_ret_t retIn, std::shared_ptr msg) + : ret(retIn), feedback_message(msg) {} + rcl_ret_t ret; + std::shared_ptr feedback_message; + }; + struct StatusReadyData + { + StatusReadyData(rcl_ret_t retIn, std::shared_ptr msg) + : ret(retIn), status_message(msg) {} + rcl_ret_t ret; + std::shared_ptr status_message; + }; + struct GoalResponseData + { + GoalResponseData(rcl_ret_t retIn, rmw_request_id_t header, std::shared_ptr response) + : ret(retIn), response_header(header), goal_response(response) {} + rcl_ret_t ret; + rmw_request_id_t response_header; + std::shared_ptr goal_response; + }; + struct CancelResponseData + { + CancelResponseData(rcl_ret_t retIn, rmw_request_id_t header, std::shared_ptr response) + : ret(retIn), response_header(header), cancel_response(response) {} + rcl_ret_t ret; + rmw_request_id_t response_header; + std::shared_ptr cancel_response; + }; + struct ResultResponseData + { + ResultResponseData(rcl_ret_t retIn, rmw_request_id_t header, std::shared_ptr response) + : ret(retIn), response_header(header), result_response(response) {} + rcl_ret_t ret; + rmw_request_id_t response_header; + std::shared_ptr result_response; + }; + + std::variant< + FeedbackReadyData, + StatusReadyData, + GoalResponseData, + CancelResponseData, + ResultResponseData + > data; + + explicit ClientBaseData(FeedbackReadyData && data_in) + : data(std::move(data_in)) {} + explicit ClientBaseData(StatusReadyData && data_in) + : data(std::move(data_in)) {} + explicit ClientBaseData(GoalResponseData && data_in) + : data(std::move(data_in)) {} + explicit ClientBaseData(CancelResponseData && data_in) + : data(std::move(data_in)) {} + explicit ClientBaseData(ResultResponseData && data_in) + : data(std::move(data_in)) {} +}; + +class ClientBaseImpl +{ +public: + ClientBaseImpl( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + const std::string & action_name, + const rosidl_action_type_support_t * type_support, + const rcl_action_client_options_t & client_options) + : node_graph_(node_graph), + node_handle(node_base->get_shared_rcl_node_handle()), + logger(node_logging->get_logger().get_child("rclcpp_action")), + random_bytes_generator(std::random_device{}()) + { + std::weak_ptr weak_node_handle(node_handle); + client_handle = std::shared_ptr( + new rcl_action_client_t, [weak_node_handle](rcl_action_client_t * client) + { + auto handle = weak_node_handle.lock(); + if (handle) { + if (RCL_RET_OK != rcl_action_client_fini(client, handle.get())) { + RCLCPP_ERROR( + rclcpp::get_logger(rcl_node_get_logger_name(handle.get())).get_child("rclcpp_action"), + "Error in destruction of rcl action client handle: %s", rcl_get_error_string().str); + rcl_reset_error(); + } + } else { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp_action"), + "Error in destruction of rcl action client handle: " + "the Node Handle was destructed too early. You will leak memory"); + } + delete client; + }); + *client_handle = rcl_action_get_zero_initialized_client(); + rcl_ret_t ret = rcl_action_client_init( + client_handle.get(), node_handle.get(), type_support, + action_name.c_str(), &client_options); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "could not initialize rcl action client"); + } + + ret = rcl_action_client_wait_set_get_num_entities( + client_handle.get(), + &num_subscriptions, + &num_guard_conditions, + &num_timers, + &num_clients, + &num_services); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "could not retrieve rcl action client details"); + } + } + + size_t num_subscriptions{0u}; + size_t num_guard_conditions{0u}; + size_t num_timers{0u}; + size_t num_clients{0u}; + size_t num_services{0u}; + + // Lock for action_client_ + std::recursive_mutex action_client_mutex_; + + // next ready event for taking, will be set by is_ready and will be processed by take_data + std::atomic next_ready_event; + + rclcpp::Context::SharedPtr context_; + rclcpp::node_interfaces::NodeGraphInterface::WeakPtr node_graph_; + // node_handle must be destroyed after client_handle to prevent memory leak + std::shared_ptr node_handle{nullptr}; + std::shared_ptr client_handle{nullptr}; + rclcpp::Logger logger; + + using ResponseCallback = std::function response)>; + + std::map pending_goal_responses; + std::mutex goal_requests_mutex; + + std::map pending_result_responses; + std::mutex result_requests_mutex; + + std::map pending_cancel_responses; + std::mutex cancel_requests_mutex; + + std::independent_bits_engine< + std::default_random_engine, 8, unsigned int> random_bytes_generator; +}; + +ClientBase::ClientBase( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + const std::string & action_name, + const rosidl_action_type_support_t * type_support, + const rcl_action_client_options_t & client_options) +: pimpl_(new ClientBaseImpl( + node_base, node_graph, node_logging, action_name, type_support, client_options)) +{ +} + +ClientBase::~ClientBase() +{ +} + +bool +ClientBase::action_server_is_ready() const +{ + bool is_ready; + std::lock_guard lock(pimpl_->action_client_mutex_); + rcl_ret_t ret = rcl_action_server_is_available( + this->pimpl_->node_handle.get(), + this->pimpl_->client_handle.get(), + &is_ready); + if (RCL_RET_NODE_INVALID == ret) { + const rcl_node_t * node_handle = this->pimpl_->node_handle.get(); + if (node_handle && !rcl_context_is_valid(node_handle->context)) { + // context is shutdown, do a soft failure + return false; + } + } + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "rcl_action_server_is_available failed"); + } + return is_ready; +} + +bool +ClientBase::wait_for_action_server_nanoseconds(std::chrono::nanoseconds timeout) +{ + auto start = std::chrono::steady_clock::now(); + auto node_ptr = pimpl_->node_graph_.lock(); + if (!node_ptr) { + throw rclcpp::exceptions::InvalidNodeError(); + } + // check to see if the server is ready immediately + if (this->action_server_is_ready()) { + return true; + } + // make an event to reuse, rather than create a new one each time + auto event = node_ptr->get_graph_event(); + if (timeout == std::chrono::nanoseconds(0)) { + // check was non-blocking, return immediately + return false; + } + // update the time even on the first loop to account for time spent in the first call + // to this->server_is_ready() + std::chrono::nanoseconds time_to_wait = + timeout > std::chrono::nanoseconds(0) ? + timeout - (std::chrono::steady_clock::now() - start) : + std::chrono::nanoseconds::max(); + if (time_to_wait < std::chrono::nanoseconds(0)) { + // Do not allow the time_to_wait to become negative when timeout was originally positive. + // Setting time_to_wait to 0 will allow one non-blocking wait because of the do-while. + time_to_wait = std::chrono::nanoseconds(0); + } + do { + if (!rclcpp::ok(this->pimpl_->context_)) { + return false; + } + // Limit each wait to 100ms to workaround an issue specific to the Connext RMW implementation. + // A race condition means that graph changes for services becoming available may trigger the + // wait set to wake up, but then not be reported as ready immediately after the wake up + // (see https://github.com/ros2/rmw_connext/issues/201) + // If no other graph events occur, the wait set will not be triggered again until the timeout + // has been reached, despite the service being available, so we artificially limit the wait + // time to limit the delay. + node_ptr->wait_for_graph_change( + event, std::min(time_to_wait, std::chrono::nanoseconds(RCL_MS_TO_NS(100)))); + // Because of the aforementioned race condition, we check if the service is ready even if the + // graph event wasn't triggered. + event->check_and_clear(); + if (this->action_server_is_ready()) { + return true; + } + // server is not ready, loop if there is time left + if (timeout > std::chrono::nanoseconds(0)) { + time_to_wait = timeout - (std::chrono::steady_clock::now() - start); + } + // if timeout is negative, time_to_wait will never reach zero + } while (time_to_wait > std::chrono::nanoseconds(0)); + return false; // timeout exceeded while waiting for the server to be ready +} + +rclcpp::Logger +ClientBase::get_logger() +{ + return pimpl_->logger; +} + +size_t +ClientBase::get_number_of_ready_subscriptions() +{ + return pimpl_->num_subscriptions; +} + +size_t +ClientBase::get_number_of_ready_guard_conditions() +{ + return pimpl_->num_guard_conditions; +} + +size_t +ClientBase::get_number_of_ready_timers() +{ + return pimpl_->num_timers; +} + +size_t +ClientBase::get_number_of_ready_clients() +{ + return pimpl_->num_clients; +} + +size_t +ClientBase::get_number_of_ready_services() +{ + return pimpl_->num_services; +} + +void +ClientBase::add_to_wait_set(rcl_wait_set_t & wait_set) +{ + std::lock_guard lock(pimpl_->action_client_mutex_); + rcl_ret_t ret = rcl_action_wait_set_add_action_client( + &wait_set, pimpl_->client_handle.get(), nullptr, nullptr); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "ClientBase::add_to_wait_set() failed"); + } +} + +bool +ClientBase::is_ready(const rcl_wait_set_t & wait_set) +{ + bool is_feedback_ready{false}; + bool is_status_ready{false}; + bool is_goal_response_ready{false}; + bool is_cancel_response_ready{false}; + bool is_result_response_ready{false}; + + rcl_ret_t ret; + { + std::lock_guard lock(pimpl_->action_client_mutex_); + ret = rcl_action_client_wait_set_get_entities_ready( + &wait_set, pimpl_->client_handle.get(), + &is_feedback_ready, + &is_status_ready, + &is_goal_response_ready, + &is_cancel_response_ready, + &is_result_response_ready); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "failed to check for any ready entities"); + } + } + + pimpl_->next_ready_event = std::numeric_limits::max(); + + if (is_feedback_ready) { + pimpl_->next_ready_event = static_cast(EntityType::FeedbackSubscription); + return true; + } + + if (is_status_ready) { + pimpl_->next_ready_event = static_cast(EntityType::StatusSubscription); + return true; + } + + if (is_goal_response_ready) { + pimpl_->next_ready_event = static_cast(EntityType::GoalClient); + return true; + } + + if (is_result_response_ready) { + pimpl_->next_ready_event = static_cast(EntityType::ResultClient); + return true; + } + + if (is_cancel_response_ready) { + pimpl_->next_ready_event = static_cast(EntityType::CancelClient); + return true; + } + + return false; +} + +void +ClientBase::handle_goal_response( + const rmw_request_id_t & response_header, + std::shared_ptr response) +{ + std::lock_guard guard(pimpl_->goal_requests_mutex); + const int64_t & sequence_number = response_header.sequence_number; + if (pimpl_->pending_goal_responses.count(sequence_number) == 0) { + RCLCPP_ERROR(pimpl_->logger, "unknown goal response, ignoring..."); + return; + } + pimpl_->pending_goal_responses[sequence_number](response); + pimpl_->pending_goal_responses.erase(sequence_number); +} + +void +ClientBase::send_goal_request(std::shared_ptr request, ResponseCallback callback) +{ + std::unique_lock guard(pimpl_->goal_requests_mutex); + int64_t sequence_number; + rcl_ret_t ret = rcl_action_send_goal_request( + pimpl_->client_handle.get(), request.get(), &sequence_number); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send goal request"); + } + assert(pimpl_->pending_goal_responses.count(sequence_number) == 0); + pimpl_->pending_goal_responses[sequence_number] = callback; +} + +void +ClientBase::handle_result_response( + const rmw_request_id_t & response_header, + std::shared_ptr response) +{ + std::map::node_type pending_result_response; + { + std::lock_guard guard(pimpl_->result_requests_mutex); + const int64_t & sequence_number = response_header.sequence_number; + if (pimpl_->pending_result_responses.count(sequence_number) == 0) { + RCLCPP_ERROR(pimpl_->logger, "unknown result response, ignoring..."); + return; + } + pending_result_response = + pimpl_->pending_result_responses.extract(sequence_number); + } + auto & response_callback = pending_result_response.mapped(); + response_callback(response); +} + +void +ClientBase::send_result_request(std::shared_ptr request, ResponseCallback callback) +{ + std::lock_guard guard(pimpl_->result_requests_mutex); + int64_t sequence_number; + rcl_ret_t ret = rcl_action_send_result_request( + pimpl_->client_handle.get(), request.get(), &sequence_number); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send result request"); + } + assert(pimpl_->pending_result_responses.count(sequence_number) == 0); + pimpl_->pending_result_responses[sequence_number] = callback; +} + +void +ClientBase::handle_cancel_response( + const rmw_request_id_t & response_header, + std::shared_ptr response) +{ + std::lock_guard guard(pimpl_->cancel_requests_mutex); + const int64_t & sequence_number = response_header.sequence_number; + if (pimpl_->pending_cancel_responses.count(sequence_number) == 0) { + RCLCPP_ERROR(pimpl_->logger, "unknown cancel response, ignoring..."); + return; + } + pimpl_->pending_cancel_responses[sequence_number](response); + pimpl_->pending_cancel_responses.erase(sequence_number); +} + +void +ClientBase::send_cancel_request(std::shared_ptr request, ResponseCallback callback) +{ + std::lock_guard guard(pimpl_->cancel_requests_mutex); + int64_t sequence_number; + rcl_ret_t ret = rcl_action_send_cancel_request( + pimpl_->client_handle.get(), request.get(), &sequence_number); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send cancel request"); + } + assert(pimpl_->pending_cancel_responses.count(sequence_number) == 0); + pimpl_->pending_cancel_responses[sequence_number] = callback; +} + +GoalUUID +ClientBase::generate_goal_id() +{ + GoalUUID goal_id; + // TODO(hidmic): Do something better than this for UUID generation. + // std::generate( + // goal_id.uuid.begin(), goal_id.uuid.end(), + // std::ref(pimpl_->random_bytes_generator)); + std::generate( + goal_id.begin(), goal_id.end(), + std::ref(pimpl_->random_bytes_generator)); + return goal_id; +} + +void +ClientBase::set_on_ready_callback(std::function callback) +{ + if (!callback) { + throw std::invalid_argument( + "The callback passed to set_on_ready_callback " + "is not callable."); + } + + set_callback_to_entity(EntityType::GoalClient, callback); + set_callback_to_entity(EntityType::ResultClient, callback); + set_callback_to_entity(EntityType::CancelClient, callback); + set_callback_to_entity(EntityType::FeedbackSubscription, callback); + set_callback_to_entity(EntityType::StatusSubscription, callback); +} + +void +ClientBase::set_callback_to_entity( + EntityType entity_type, + std::function callback) +{ + // Note: we bind the int identifier argument to this waitable's entity types + auto new_callback = + [callback, entity_type, this](size_t number_of_events) { + try { + callback(number_of_events, static_cast(entity_type)); + } catch (const std::exception & exception) { + RCLCPP_ERROR_STREAM( + pimpl_->logger, + "rclcpp_action::ClientBase@" << this << + " caught " << rmw::impl::cpp::demangle(exception) << + " exception in user-provided callback for the 'on ready' callback: " << + exception.what()); + } catch (...) { + RCLCPP_ERROR_STREAM( + pimpl_->logger, + "rclcpp_action::ClientBase@" << this << + " caught unhandled exception in user-provided callback " << + "for the 'on ready' callback"); + } + }; + + // Set it temporarily to the new callback, while we replace the old one. + // This two-step setting, prevents a gap where the old std::function has + // been replaced but the middleware hasn't been told about the new one yet. + set_on_ready_callback( + entity_type, + rclcpp::detail::cpp_callback_trampoline, + static_cast(&new_callback)); + + std::lock_guard lock(listener_mutex_); + // Store the std::function to keep it in scope, also overwrites the existing one. + auto it = entity_type_to_on_ready_callback_.find(entity_type); + + if (it != entity_type_to_on_ready_callback_.end()) { + it->second = new_callback; + } else { + entity_type_to_on_ready_callback_.emplace(entity_type, new_callback); + } + + // Set it again, now using the permanent storage. + it = entity_type_to_on_ready_callback_.find(entity_type); + + if (it != entity_type_to_on_ready_callback_.end()) { + auto & cb = it->second; + set_on_ready_callback( + entity_type, + rclcpp::detail::cpp_callback_trampolinesecond), const void *, size_t>, + static_cast(&cb)); + } + + on_ready_callback_set_ = true; +} + +void +ClientBase::set_on_ready_callback( + EntityType entity_type, + rcl_event_callback_t callback, + const void * user_data) +{ + rcl_ret_t ret = RCL_RET_ERROR; + + switch (entity_type) { + case EntityType::GoalClient: + { + ret = rcl_action_client_set_goal_client_callback( + pimpl_->client_handle.get(), + callback, + user_data); + break; + } + + case EntityType::ResultClient: + { + ret = rcl_action_client_set_result_client_callback( + pimpl_->client_handle.get(), + callback, + user_data); + break; + } + + case EntityType::CancelClient: + { + ret = rcl_action_client_set_cancel_client_callback( + pimpl_->client_handle.get(), + callback, + user_data); + break; + } + + case EntityType::FeedbackSubscription: + { + ret = rcl_action_client_set_feedback_subscription_callback( + pimpl_->client_handle.get(), + callback, + user_data); + break; + } + + case EntityType::StatusSubscription: + { + ret = rcl_action_client_set_status_subscription_callback( + pimpl_->client_handle.get(), + callback, + user_data); + break; + } + + default: + throw std::runtime_error("ClientBase::set_on_ready_callback: Unknown entity type."); + break; + } + + if (RCL_RET_OK != ret) { + using rclcpp::exceptions::throw_from_rcl_error; + throw_from_rcl_error(ret, "failed to set the on ready callback for action client"); + } +} + +void +ClientBase::clear_on_ready_callback() +{ + std::lock_guard lock(listener_mutex_); + + if (on_ready_callback_set_) { + set_on_ready_callback(EntityType::GoalClient, nullptr, nullptr); + set_on_ready_callback(EntityType::ResultClient, nullptr, nullptr); + set_on_ready_callback(EntityType::CancelClient, nullptr, nullptr); + set_on_ready_callback(EntityType::FeedbackSubscription, nullptr, nullptr); + set_on_ready_callback(EntityType::StatusSubscription, nullptr, nullptr); + on_ready_callback_set_ = false; + } + + entity_type_to_on_ready_callback_.clear(); +} + +std::shared_ptr +ClientBase::take_data() +{ + // next_ready_event is an atomic, caching localy + size_t next_ready_event = pimpl_->next_ready_event.exchange(std::numeric_limits::max()); + + if (next_ready_event == std::numeric_limits::max()) { + throw std::runtime_error("Taking data from action client but nothing is ready"); + } + + return take_data_by_entity_id(next_ready_event); +} + +std::shared_ptr +ClientBase::take_data_by_entity_id(size_t id) +{ + std::shared_ptr data_ptr; + rcl_ret_t ret; + + // Mark as ready the entity from which we want to take data + switch (static_cast(id)) { + case EntityType::GoalClient: + { + rmw_request_id_t response_header; + std::shared_ptr goal_response; + { + std::lock_guard lock(pimpl_->action_client_mutex_); + + goal_response = this->create_goal_response(); + ret = rcl_action_take_goal_response( + pimpl_->client_handle.get(), &response_header, goal_response.get()); + } + data_ptr = std::make_shared( + ClientBaseData::GoalResponseData( + ret, response_header, goal_response)); + } + break; + case EntityType::ResultClient: + { + rmw_request_id_t response_header; + std::shared_ptr result_response; + { + std::lock_guard lock(pimpl_->action_client_mutex_); + result_response = this->create_result_response(); + ret = rcl_action_take_result_response( + pimpl_->client_handle.get(), &response_header, result_response.get()); + } + data_ptr = + std::make_shared( + ClientBaseData::ResultResponseData( + ret, response_header, result_response)); + } + break; + case EntityType::CancelClient: + { + rmw_request_id_t response_header; + std::shared_ptr cancel_response; + { + std::lock_guard lock(pimpl_->action_client_mutex_); + cancel_response = this->create_cancel_response(); + ret = rcl_action_take_cancel_response( + pimpl_->client_handle.get(), &response_header, cancel_response.get()); + } + data_ptr = + std::make_shared( + ClientBaseData::CancelResponseData( + ret, response_header, cancel_response)); + } + break; + case EntityType::FeedbackSubscription: + { + std::shared_ptr feedback_message; + { + std::lock_guard lock(pimpl_->action_client_mutex_); + feedback_message = this->create_feedback_message(); + ret = rcl_action_take_feedback( + pimpl_->client_handle.get(), feedback_message.get()); + } + data_ptr = + std::make_shared( + ClientBaseData::FeedbackReadyData( + ret, feedback_message)); + } + break; + case EntityType::StatusSubscription: + { + std::shared_ptr status_message; + { + std::lock_guard lock(pimpl_->action_client_mutex_); + status_message = this->create_status_message(); + ret = rcl_action_take_status( + pimpl_->client_handle.get(), status_message.get()); + } + data_ptr = + std::make_shared( + ClientBaseData::StatusReadyData( + ret, status_message)); + } + break; + } + + return std::static_pointer_cast(data_ptr); +} + +void +ClientBase::execute(const std::shared_ptr & data_in) +{ + if (!data_in) { + throw std::invalid_argument("'data_in' is unexpectedly empty"); + } + + std::shared_ptr data_ptr = std::static_pointer_cast(data_in); + + std::visit( + [&](auto && data) -> void { + using T = std::decay_t; + if constexpr (std::is_same_v) { + if (RCL_RET_OK == data.ret) { + this->handle_feedback_message(data.feedback_message); + } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != data.ret) { + rclcpp::exceptions::throw_from_rcl_error(data.ret, "error taking feedback"); + } + } + if constexpr (std::is_same_v) { + if (RCL_RET_OK == data.ret) { + this->handle_status_message(data.status_message); + } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != data.ret) { + rclcpp::exceptions::throw_from_rcl_error(data.ret, "error taking status"); + } + } + if constexpr (std::is_same_v) { + if (RCL_RET_OK == data.ret) { + this->handle_goal_response(data.response_header, data.goal_response); + } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != data.ret) { + rclcpp::exceptions::throw_from_rcl_error(data.ret, "error taking goal response"); + } + } + if constexpr (std::is_same_v) { + if (RCL_RET_OK == data.ret) { + this->handle_result_response(data.response_header, data.result_response); + } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != data.ret) { + rclcpp::exceptions::throw_from_rcl_error(data.ret, "error taking result response"); + } + } + if constexpr (std::is_same_v) { + if (RCL_RET_OK == data.ret) { + this->handle_cancel_response(data.response_header, data.cancel_response); + } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != data.ret) { + rclcpp::exceptions::throw_from_rcl_error(data.ret, "error taking cancel response"); + } + } + }, data_ptr->data); +} + +} // namespace rclcpp_action diff --git a/rclcpp_action/src/qos.cpp b/rclcpp_action/src/qos.cpp new file mode 100644 index 0000000000..615b825185 --- /dev/null +++ b/rclcpp_action/src/qos.cpp @@ -0,0 +1,28 @@ +// 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 + +namespace rclcpp_action +{ + +DefaultActionStatusQoS::DefaultActionStatusQoS() +: rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rcl_action_qos_profile_status_default)) +{ + this->get_rmw_qos_profile() = rcl_action_qos_profile_status_default; +} + +} // namespace rclcpp_action diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp new file mode 100644 index 0000000000..fefc02d6ad --- /dev/null +++ b/rclcpp_action/src/server.cpp @@ -0,0 +1,912 @@ +// 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 +#include +#include +#include +#include + +#include "rcl_action/action_server.h" +#include "rcl_action/wait.h" + +#include "rcpputils/scope_exit.hpp" + +#include "action_msgs/msg/goal_status_array.hpp" +#include "rclcpp/exceptions.hpp" +#include "rclcpp_action/server.hpp" + +using rclcpp_action::ServerBase; +using rclcpp_action::GoalUUID; + +struct ServerBaseData; + +namespace rclcpp_action +{ + +struct ServerBaseData +{ + using GoalRequestData = std::tuple< + rcl_ret_t, + const rcl_action_goal_info_t, + rmw_request_id_t, + std::shared_ptr + >; + + using CancelRequestData = std::tuple< + rcl_ret_t, + std::shared_ptr, + rmw_request_id_t + >; + + using ResultRequestData = std::tuple, rmw_request_id_t>; + + using GoalExpiredData = struct Empty {}; + + std::variant data; + + explicit ServerBaseData(GoalRequestData && data_in) + : data(std::move(data_in)) {} + explicit ServerBaseData(CancelRequestData && data_in) + : data(std::move(data_in)) {} + explicit ServerBaseData(ResultRequestData && data_in) + : data(std::move(data_in)) {} + explicit ServerBaseData(GoalExpiredData && data_in) + : data(std::move(data_in)) {} +}; + +class ServerBaseImpl +{ +public: + ServerBaseImpl( + rclcpp::Clock::SharedPtr clock, + rclcpp::Logger logger + ) + : clock_(clock), logger_(logger) + { + } + + // Lock for action_server_ + std::recursive_mutex action_server_reentrant_mutex_; + + rclcpp::Clock::SharedPtr clock_; + + // Do not declare this before clock_ as this depends on clock_(see #1526) + std::shared_ptr action_server_; + + size_t num_subscriptions_ = 0; + size_t num_timers_ = 0; + size_t num_clients_ = 0; + size_t num_services_ = 0; + size_t num_guard_conditions_ = 0; + + // Lock for unordered_maps + std::recursive_mutex unordered_map_mutex_; + + // Results to be kept until the goal expires after reaching a terminal state + std::unordered_map> goal_results_; + // Requests for results are kept until a result becomes available + std::unordered_map> result_requests_; + // rcl goal handles are kept so api to send result doesn't try to access freed memory + std::unordered_map> goal_handles_; + + // next ready event for taking, will be set by is_ready and will be processed by take_data + std::atomic next_ready_event; + + rclcpp::Logger logger_; +}; + +} // namespace rclcpp_action + +ServerBase::ServerBase( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + const std::string & name, + const rosidl_action_type_support_t * type_support, + const rcl_action_server_options_t & options +) +: pimpl_(new ServerBaseImpl( + node_clock->get_clock(), node_logging->get_logger().get_child("rclcpp_action"))) +{ + auto deleter = [node_base](rcl_action_server_t * ptr) + { + if (nullptr != ptr) { + rcl_node_t * rcl_node = node_base->get_rcl_node_handle(); + rcl_ret_t ret = rcl_action_server_fini(ptr, rcl_node); + if (RCL_RET_OK != ret) { + RCLCPP_DEBUG( + rclcpp::get_logger("rclcpp_action"), + "failed to fini rcl_action_server_t in deleter"); + } + delete ptr; + } + }; + + pimpl_->action_server_.reset(new rcl_action_server_t, deleter); + *(pimpl_->action_server_) = rcl_action_get_zero_initialized_server(); + + rcl_node_t * rcl_node = node_base->get_rcl_node_handle(); + rcl_clock_t * rcl_clock = pimpl_->clock_->get_clock_handle(); + + rcl_ret_t ret = rcl_action_server_init( + pimpl_->action_server_.get(), rcl_node, rcl_clock, type_support, name.c_str(), &options); + + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + + ret = rcl_action_server_wait_set_get_num_entities( + pimpl_->action_server_.get(), + &pimpl_->num_subscriptions_, + &pimpl_->num_guard_conditions_, + &pimpl_->num_timers_, + &pimpl_->num_clients_, + &pimpl_->num_services_); + + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } +} + +ServerBase::~ServerBase() +{ +} + +size_t +ServerBase::get_number_of_ready_subscriptions() +{ + return pimpl_->num_subscriptions_; +} + +size_t +ServerBase::get_number_of_ready_timers() +{ + return pimpl_->num_timers_; +} + +size_t +ServerBase::get_number_of_ready_clients() +{ + return pimpl_->num_clients_; +} + +size_t +ServerBase::get_number_of_ready_services() +{ + return pimpl_->num_services_; +} + +size_t +ServerBase::get_number_of_ready_guard_conditions() +{ + return pimpl_->num_guard_conditions_; +} + +void +ServerBase::add_to_wait_set(rcl_wait_set_t & wait_set) +{ + std::lock_guard lock(pimpl_->action_server_reentrant_mutex_); + rcl_ret_t ret = rcl_action_wait_set_add_action_server( + &wait_set, pimpl_->action_server_.get(), NULL); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "ServerBase::add_to_wait_set() failed"); + } +} + +bool +ServerBase::is_ready(const rcl_wait_set_t & wait_set) +{ + bool goal_request_ready; + bool cancel_request_ready; + bool result_request_ready; + bool goal_expired; + rcl_ret_t ret; + { + std::lock_guard lock(pimpl_->action_server_reentrant_mutex_); + ret = rcl_action_server_wait_set_get_entities_ready( + &wait_set, + pimpl_->action_server_.get(), + &goal_request_ready, + &cancel_request_ready, + &result_request_ready, + &goal_expired); + } + + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + + pimpl_->next_ready_event = std::numeric_limits::max(); + + if (goal_request_ready) { + pimpl_->next_ready_event = static_cast(EntityType::GoalService); + return true; + } + + if (cancel_request_ready) { + pimpl_->next_ready_event = static_cast(EntityType::CancelService); + return true; + } + + if (result_request_ready) { + pimpl_->next_ready_event = static_cast(EntityType::ResultService); + return true; + } + + if (goal_expired) { + pimpl_->next_ready_event = static_cast(EntityType::Expired); + return true; + } + + return false; +} + +std::shared_ptr +ServerBase::take_data() +{ + size_t next_ready_event = pimpl_->next_ready_event.exchange(std::numeric_limits::max()); + + if (next_ready_event == std::numeric_limits::max()) { + throw std::runtime_error("ServerBase::take_data() called but no data is ready"); + } + + return take_data_by_entity_id(next_ready_event); +} + +std::shared_ptr +ServerBase::take_data_by_entity_id(size_t id) +{ + std::shared_ptr data_ptr; + // Mark as ready the entity from which we want to take data + switch (static_cast(id)) { + case EntityType::GoalService: + { + rcl_ret_t ret; + rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info(); + rmw_request_id_t request_header; + + std::lock_guard lock(pimpl_->action_server_reentrant_mutex_); + + std::shared_ptr message = create_goal_request(); + ret = rcl_action_take_goal_request( + pimpl_->action_server_.get(), + &request_header, + message.get()); + + data_ptr = std::make_shared( + ServerBaseData::GoalRequestData(ret, goal_info, request_header, message)); + } + break; + case EntityType::ResultService: + { + rcl_ret_t ret; + // Get the result request message + rmw_request_id_t request_header; + std::shared_ptr result_request = create_result_request(); + std::lock_guard lock(pimpl_->action_server_reentrant_mutex_); + ret = rcl_action_take_result_request( + pimpl_->action_server_.get(), &request_header, result_request.get()); + + data_ptr = + std::make_shared( + ServerBaseData::ResultRequestData(ret, result_request, request_header)); + } + break; + case EntityType::CancelService: + { + rcl_ret_t ret; + rmw_request_id_t request_header; + + // Initialize cancel request + auto request = std::make_shared(); + + std::lock_guard lock(pimpl_->action_server_reentrant_mutex_); + ret = rcl_action_take_cancel_request( + pimpl_->action_server_.get(), + &request_header, + request.get()); + + data_ptr = + std::make_shared( + ServerBaseData::CancelRequestData(ret, request, request_header)); + } + break; + case EntityType::Expired: + { + data_ptr = + std::make_shared(ServerBaseData::GoalExpiredData()); + } + break; + } + + return std::static_pointer_cast(data_ptr); +} + +void +ServerBase::execute(const std::shared_ptr & data_in) +{ + if (!data_in) { + throw std::runtime_error("ServerBase::execute: give data pointer was null"); + } + + std::shared_ptr data_ptr = std::static_pointer_cast(data_in); + + std::visit( + [&](auto && data) -> void { + using T = std::decay_t; + if constexpr (std::is_same_v) { + execute_goal_request_received( + std::get<0>(data), std::get<1>(data), std::get<2>(data), + std::get<3>(data)); + } + if constexpr (std::is_same_v) { + execute_cancel_request_received(std::get<0>(data), std::get<1>(data), std::get<2>(data)); + } + if constexpr (std::is_same_v) { + execute_result_request_received(std::get<0>(data), std::get<1>(data), std::get<2>(data)); + } + if constexpr (std::is_same_v) { + execute_check_expired_goals(); + } + }, + data_ptr->data); +} + +void +ServerBase::execute_goal_request_received( + rcl_ret_t ret, + rcl_action_goal_info_t goal_info, + rmw_request_id_t request_header, + const std::shared_ptr message) +{ + if (RCL_RET_ACTION_SERVER_TAKE_FAILED == ret) { + // Ignore take failure because connext fails if it receives a sample without valid data. + // This happens when a client shuts down and connext receives a sample saying the client is + // no longer alive. + return; + } else if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + + GoalUUID uuid = get_goal_id_from_goal_request(message.get()); + convert(uuid, &goal_info); + + // Call user's callback, getting the user's response and a ros message to send back + auto response_pair = call_handle_goal_callback(uuid, message); + + { + std::lock_guard lock(pimpl_->action_server_reentrant_mutex_); + ret = rcl_action_send_goal_response( + pimpl_->action_server_.get(), + &request_header, + response_pair.second.get()); + } + + if (RCL_RET_OK != ret) { + if (ret == RCL_RET_TIMEOUT) { + RCLCPP_WARN( + pimpl_->logger_, + "Failed to send goal response %s (timeout): %s", + to_string(uuid).c_str(), rcl_get_error_string().str); + rcl_reset_error(); + return; + } else { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + } + + const auto status = response_pair.first; + + // if goal is accepted, create a goal handle, and store it + if (GoalResponse::ACCEPT_AND_EXECUTE == status || GoalResponse::ACCEPT_AND_DEFER == status) { + RCLCPP_DEBUG(pimpl_->logger_, "Accepted goal %s", to_string(uuid).c_str()); + // rcl_action will set time stamp + auto deleter = [](rcl_action_goal_handle_t * ptr) + { + if (nullptr != ptr) { + rcl_ret_t fail_ret = rcl_action_goal_handle_fini(ptr); + if (RCL_RET_OK != fail_ret) { + RCLCPP_DEBUG( + rclcpp::get_logger("rclcpp_action"), + "failed to fini rcl_action_goal_handle_t in deleter"); + } + delete ptr; + } + }; + rcl_action_goal_handle_t * rcl_handle; + { + std::lock_guard lock(pimpl_->action_server_reentrant_mutex_); + rcl_handle = rcl_action_accept_new_goal(pimpl_->action_server_.get(), &goal_info); + } + if (!rcl_handle) { + throw std::runtime_error("Failed to accept new goal\n"); + } + + std::shared_ptr handle(new rcl_action_goal_handle_t, deleter); + // Copy out goal handle since action server storage disappears when it is fini'd + *handle = *rcl_handle; + + { + std::lock_guard lock(pimpl_->unordered_map_mutex_); + pimpl_->goal_handles_[uuid] = handle; + } + + if (GoalResponse::ACCEPT_AND_EXECUTE == status) { + // Change status to executing + ret = rcl_action_update_goal_state(handle.get(), GOAL_EVENT_EXECUTE); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + } + // publish status since a goal's state has changed (was accepted or has begun execution) + publish_status(); + + // Tell user to start executing action + call_goal_accepted_callback(handle, uuid, message); + } +} + +void +ServerBase::execute_cancel_request_received( + rcl_ret_t ret, + std::shared_ptr request, + rmw_request_id_t request_header) +{ + if (RCL_RET_ACTION_SERVER_TAKE_FAILED == ret) { + // Ignore take failure because connext fails if it receives a sample without valid data. + // This happens when a client shuts down and connext receives a sample saying the client is + // no longer alive. + return; + } else if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + + // Convert c++ message to C message + rcl_action_cancel_request_t cancel_request = rcl_action_get_zero_initialized_cancel_request(); + convert(request->goal_info.goal_id.uuid, &cancel_request.goal_info); + cancel_request.goal_info.stamp.sec = request->goal_info.stamp.sec; + cancel_request.goal_info.stamp.nanosec = request->goal_info.stamp.nanosec; + + // Get a list of goal info that should be attempted to be cancelled + rcl_action_cancel_response_t cancel_response = rcl_action_get_zero_initialized_cancel_response(); + + { + std::lock_guard lock(pimpl_->action_server_reentrant_mutex_); + ret = rcl_action_process_cancel_request( + pimpl_->action_server_.get(), + &cancel_request, + &cancel_response); + } + + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + + RCPPUTILS_SCOPE_EXIT( + { + ret = rcl_action_cancel_response_fini(&cancel_response); + if (RCL_RET_OK != ret) { + RCLCPP_ERROR(pimpl_->logger_, "Failed to fini cancel response"); + } + }); + + auto response = std::make_shared(); + + response->return_code = cancel_response.msg.return_code; + auto & goals = cancel_response.msg.goals_canceling; + // For each canceled goal, call cancel callback + for (size_t i = 0; i < goals.size; ++i) { + const rcl_action_goal_info_t & goal_info = goals.data[i]; + GoalUUID uuid; + convert(goal_info, &uuid); + auto response_code = call_handle_cancel_callback(uuid); + if (CancelResponse::ACCEPT == response_code) { + action_msgs::msg::GoalInfo cpp_info; + cpp_info.goal_id.uuid = uuid; + cpp_info.stamp.sec = goal_info.stamp.sec; + cpp_info.stamp.nanosec = goal_info.stamp.nanosec; + response->goals_canceling.push_back(cpp_info); + } + } + + // If the user rejects all individual requests to cancel goals, + // then we consider the top-level cancel request as rejected. + if (goals.size >= 1u && 0u == response->goals_canceling.size()) { + response->return_code = action_msgs::srv::CancelGoal::Response::ERROR_REJECTED; + } + + if (!response->goals_canceling.empty()) { + // at least one goal state changed, publish a new status message + publish_status(); + } + + { + std::lock_guard lock(pimpl_->action_server_reentrant_mutex_); + ret = rcl_action_send_cancel_response( + pimpl_->action_server_.get(), &request_header, response.get()); + } + + if (ret == RCL_RET_TIMEOUT) { + GoalUUID uuid = request->goal_info.goal_id.uuid; + RCLCPP_WARN( + pimpl_->logger_, + "Failed to send cancel response %s (timeout): %s", + to_string(uuid).c_str(), rcl_get_error_string().str); + rcl_reset_error(); + return; + } + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } +} + +void +ServerBase::execute_result_request_received( + rcl_ret_t ret, + std::shared_ptr result_request, + rmw_request_id_t request_header) +{ + if (RCL_RET_ACTION_SERVER_TAKE_FAILED == ret) { + // Ignore take failure because connext fails if it receives a sample without valid data. + // This happens when a client shuts down and connext receives a sample saying the client is + // no longer alive. + return; + } else if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + + std::shared_ptr result_response; + + // check if the goal exists + GoalUUID uuid = get_goal_id_from_result_request(result_request.get()); + rcl_action_goal_info_t goal_info; + convert(uuid, &goal_info); + bool goal_exists; + { + std::lock_guard lock(pimpl_->action_server_reentrant_mutex_); + goal_exists = rcl_action_server_goal_exists(pimpl_->action_server_.get(), &goal_info); + } + if (!goal_exists) { + // Goal does not exists + result_response = create_result_response(action_msgs::msg::GoalStatus::STATUS_UNKNOWN); + } else { + // Goal exists, check if a result is already available + std::lock_guard lock(pimpl_->unordered_map_mutex_); + auto iter = pimpl_->goal_results_.find(uuid); + if (iter != pimpl_->goal_results_.end()) { + result_response = iter->second; + } else { + // Store the request so it can be responded to later + pimpl_->result_requests_[uuid].push_back(request_header); + } + } + + if (result_response) { + // Send the result now + std::lock_guard lock(pimpl_->action_server_reentrant_mutex_); + rcl_ret_t rcl_ret = rcl_action_send_result_response( + pimpl_->action_server_.get(), &request_header, result_response.get()); + if (rcl_ret == RCL_RET_TIMEOUT) { + RCLCPP_WARN( + pimpl_->logger_, + "Failed to send result response %s (timeout): %s", + to_string(uuid).c_str(), rcl_get_error_string().str); + rcl_reset_error(); + return; + } + if (RCL_RET_OK != rcl_ret) { + rclcpp::exceptions::throw_from_rcl_error(rcl_ret); + } + } +} + +void +ServerBase::execute_check_expired_goals() +{ + // Allocate expecting only one goal to expire at a time + rcl_action_goal_info_t expired_goals[1]; + size_t num_expired = 1; + + // Loop in case more than 1 goal expired + while (num_expired > 0u) { + rcl_ret_t ret; + { + std::lock_guard lock(pimpl_->action_server_reentrant_mutex_); + ret = rcl_action_expire_goals(pimpl_->action_server_.get(), expired_goals, 1, &num_expired); + } + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } else if (num_expired) { + // A goal expired! + GoalUUID uuid; + convert(expired_goals[0], &uuid); + RCLCPP_DEBUG(pimpl_->logger_, "Expired goal %s", to_string(uuid).c_str()); + std::lock_guard lock(pimpl_->unordered_map_mutex_); + pimpl_->goal_results_.erase(uuid); + pimpl_->result_requests_.erase(uuid); + pimpl_->goal_handles_.erase(uuid); + } + } +} + +void +ServerBase::publish_status() +{ + rcl_ret_t ret; + + // We need to hold the lock across this entire method because + // rcl_action_server_get_goal_handles() returns an internal pointer to the + // goal data. + std::lock_guard lock(pimpl_->action_server_reentrant_mutex_); + + // Get all goal handles known to C action server + rcl_action_goal_handle_t ** goal_handles = NULL; + size_t num_goals = 0; + ret = rcl_action_server_get_goal_handles( + pimpl_->action_server_.get(), &goal_handles, &num_goals); + + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + + auto status_msg = std::make_shared(); + status_msg->status_list.reserve(num_goals); + // Populate a c++ status message with the goals and their statuses + rcl_action_goal_status_array_t c_status_array = + rcl_action_get_zero_initialized_goal_status_array(); + ret = rcl_action_get_goal_status_array(pimpl_->action_server_.get(), &c_status_array); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + + RCPPUTILS_SCOPE_EXIT( + { + ret = rcl_action_goal_status_array_fini(&c_status_array); + if (RCL_RET_OK != ret) { + RCLCPP_ERROR(pimpl_->logger_, "Failed to fini status array message"); + } + }); + + for (size_t i = 0; i < c_status_array.msg.status_list.size; ++i) { + auto & c_status_msg = c_status_array.msg.status_list.data[i]; + + action_msgs::msg::GoalStatus msg; + msg.status = c_status_msg.status; + // Convert C goal info to C++ goal info + convert(c_status_msg.goal_info, &msg.goal_info.goal_id.uuid); + msg.goal_info.stamp.sec = c_status_msg.goal_info.stamp.sec; + msg.goal_info.stamp.nanosec = c_status_msg.goal_info.stamp.nanosec; + + status_msg->status_list.push_back(msg); + } + + // Publish the message through the status publisher + ret = rcl_action_publish_status(pimpl_->action_server_.get(), status_msg.get()); + + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } +} + +void +ServerBase::publish_result(const GoalUUID & uuid, std::shared_ptr result_msg) +{ + // Check that the goal exists + rcl_action_goal_info_t goal_info; + convert(uuid, &goal_info); + bool goal_exists; + { + std::lock_guard lock(pimpl_->action_server_reentrant_mutex_); + goal_exists = rcl_action_server_goal_exists(pimpl_->action_server_.get(), &goal_info); + } + + if (!goal_exists) { + throw std::runtime_error("Asked to publish result for goal that does not exist"); + } + + { + /** + * NOTE: There is a potential deadlock issue if both unordered_map_mutex_ and + * action_server_reentrant_mutex_ locked in other block scopes. Unless using + * std::scoped_lock, locking order must be consistent with the current. + * + * Current locking order: + * + * 1. unordered_map_mutex_ + * 2. action_server_reentrant_mutex_ + * + */ + std::lock_guard unordered_map_lock(pimpl_->unordered_map_mutex_); + pimpl_->goal_results_[uuid] = result_msg; + + // if there are clients who already asked for the result, send it to them + auto iter = pimpl_->result_requests_.find(uuid); + if (iter != pimpl_->result_requests_.end()) { + std::lock_guard lock(pimpl_->action_server_reentrant_mutex_); + for (auto & request_header : iter->second) { + rcl_ret_t ret = rcl_action_send_result_response( + pimpl_->action_server_.get(), &request_header, result_msg.get()); + if (ret == RCL_RET_TIMEOUT) { + RCLCPP_WARN( + pimpl_->logger_, + "Failed to send result response %s (timeout): %s", + to_string(uuid).c_str(), rcl_get_error_string().str); + rcl_reset_error(); + } else if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + } + } + } +} + +void +ServerBase::notify_goal_terminal_state() +{ + std::lock_guard lock(pimpl_->action_server_reentrant_mutex_); + rcl_ret_t ret = rcl_action_notify_goal_done(pimpl_->action_server_.get()); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } +} + +void +ServerBase::publish_feedback(std::shared_ptr feedback_msg) +{ + std::lock_guard lock(pimpl_->action_server_reentrant_mutex_); + rcl_ret_t ret = rcl_action_publish_feedback(pimpl_->action_server_.get(), feedback_msg.get()); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to publish feedback"); + } +} + +void +ServerBase::set_on_ready_callback(std::function callback) +{ + if (!callback) { + throw std::invalid_argument( + "The callback passed to set_on_ready_callback " + "is not callable."); + } + + set_callback_to_entity(EntityType::GoalService, callback); + set_callback_to_entity(EntityType::ResultService, callback); + set_callback_to_entity(EntityType::CancelService, callback); +} + +void +ServerBase::set_callback_to_entity( + EntityType entity_type, + std::function callback) +{ + // Note: we bind the int identifier argument to this waitable's entity types + auto new_callback = + [callback, entity_type, this](size_t number_of_events) { + try { + callback(number_of_events, static_cast(entity_type)); + } catch (const std::exception & exception) { + RCLCPP_ERROR_STREAM( + pimpl_->logger_, + "rclcpp_action::ServerBase@" << this << + " caught " << rmw::impl::cpp::demangle(exception) << + " exception in user-provided callback for the 'on ready' callback: " << + exception.what()); + } catch (...) { + RCLCPP_ERROR_STREAM( + pimpl_->logger_, + "rclcpp_action::ServerBase@" << this << + " caught unhandled exception in user-provided callback " << + "for the 'on ready' callback"); + } + }; + + + // Set it temporarily to the new callback, while we replace the old one. + // This two-step setting, prevents a gap where the old std::function has + // been replaced but the middleware hasn't been told about the new one yet. + set_on_ready_callback( + entity_type, + rclcpp::detail::cpp_callback_trampoline, + static_cast(&new_callback)); + + std::lock_guard lock(listener_mutex_); + // Store the std::function to keep it in scope, also overwrites the existing one. + auto it = entity_type_to_on_ready_callback_.find(entity_type); + + if (it != entity_type_to_on_ready_callback_.end()) { + it->second = new_callback; + } else { + entity_type_to_on_ready_callback_.emplace(entity_type, new_callback); + } + + // Set it again, now using the permanent storage. + it = entity_type_to_on_ready_callback_.find(entity_type); + + if (it != entity_type_to_on_ready_callback_.end()) { + auto & cb = it->second; + set_on_ready_callback( + entity_type, + rclcpp::detail::cpp_callback_trampolinesecond), const void *, size_t>, + static_cast(&cb)); + } + + on_ready_callback_set_ = true; +} + +void +ServerBase::set_on_ready_callback( + EntityType entity_type, + rcl_event_callback_t callback, + const void * user_data) +{ + rcl_ret_t ret = RCL_RET_ERROR; + + switch (entity_type) { + case EntityType::GoalService: + { + ret = rcl_action_server_set_goal_service_callback( + pimpl_->action_server_.get(), + callback, + user_data); + break; + } + + case EntityType::ResultService: + { + ret = rcl_action_server_set_result_service_callback( + pimpl_->action_server_.get(), + callback, + user_data); + break; + } + + case EntityType::CancelService: + { + ret = rcl_action_server_set_cancel_service_callback( + pimpl_->action_server_.get(), + callback, + user_data); + break; + } + + default: + throw std::runtime_error("ServerBase::set_on_ready_callback: Unknown entity type."); + break; + } + + if (RCL_RET_OK != ret) { + using rclcpp::exceptions::throw_from_rcl_error; + throw_from_rcl_error(ret, "failed to set the on ready callback for action client"); + } +} + +void +ServerBase::clear_on_ready_callback() +{ + std::lock_guard lock(listener_mutex_); + + if (on_ready_callback_set_) { + set_on_ready_callback(EntityType::GoalService, nullptr, nullptr); + set_on_ready_callback(EntityType::ResultService, nullptr, nullptr); + set_on_ready_callback(EntityType::CancelService, nullptr, nullptr); + on_ready_callback_set_ = false; + } + + entity_type_to_on_ready_callback_.clear(); +} diff --git a/rclcpp_action/src/server_goal_handle.cpp b/rclcpp_action/src/server_goal_handle.cpp new file mode 100644 index 0000000000..7df0ebf0e1 --- /dev/null +++ b/rclcpp_action/src/server_goal_handle.cpp @@ -0,0 +1,140 @@ +// 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_action/action_server.h" +#include "rcl_action/goal_handle.h" + +#include "rclcpp_action/server_goal_handle.hpp" +#include "rclcpp/exceptions.hpp" + +namespace rclcpp_action +{ +ServerGoalHandleBase::~ServerGoalHandleBase() +{ +} + +bool +ServerGoalHandleBase::is_canceling() const +{ + std::lock_guard lock(rcl_handle_mutex_); + rcl_action_goal_state_t state = GOAL_STATE_UNKNOWN; + rcl_ret_t ret = rcl_action_goal_handle_get_status(rcl_handle_.get(), &state); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to get goal handle state"); + } + return GOAL_STATE_CANCELING == state; +} + +bool +ServerGoalHandleBase::is_active() const +{ + std::lock_guard lock(rcl_handle_mutex_); + return rcl_action_goal_handle_is_active(rcl_handle_.get()); +} + +bool +ServerGoalHandleBase::is_executing() const +{ + std::lock_guard lock(rcl_handle_mutex_); + rcl_action_goal_state_t state = GOAL_STATE_UNKNOWN; + rcl_ret_t ret = rcl_action_goal_handle_get_status(rcl_handle_.get(), &state); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to get goal handle state"); + } + return GOAL_STATE_EXECUTING == state; +} + +void +ServerGoalHandleBase::_abort() +{ + std::lock_guard lock(rcl_handle_mutex_); + rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_ABORT); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } +} + +void +ServerGoalHandleBase::_succeed() +{ + std::lock_guard lock(rcl_handle_mutex_); + rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_SUCCEED); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } +} + +void +ServerGoalHandleBase::_cancel_goal() +{ + std::lock_guard lock(rcl_handle_mutex_); + rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_CANCEL_GOAL); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } +} + +void +ServerGoalHandleBase::_canceled() +{ + std::lock_guard lock(rcl_handle_mutex_); + rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_CANCELED); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } +} + +void +ServerGoalHandleBase::_execute() +{ + std::lock_guard lock(rcl_handle_mutex_); + rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_EXECUTE); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } +} + +bool +ServerGoalHandleBase::try_canceling() noexcept +{ + std::lock_guard lock(rcl_handle_mutex_); + rcl_ret_t ret; + // Check if the goal is cancelable + const bool is_cancelable = rcl_action_goal_handle_is_cancelable(rcl_handle_.get()); + if (is_cancelable) { + // Transition to CANCELING + ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_CANCEL_GOAL); + if (RCL_RET_OK != ret) { + return false; + } + } + + rcl_action_goal_state_t state = GOAL_STATE_UNKNOWN; + // Get the current state + ret = rcl_action_goal_handle_get_status(rcl_handle_.get(), &state); + if (RCL_RET_OK != ret) { + return false; + } + + // If it's canceling, cancel it + if (GOAL_STATE_CANCELING == state) { + ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_CANCELED); + return RCL_RET_OK == ret; + } + + return false; +} +} // namespace rclcpp_action diff --git a/rclcpp_action/src/types.cpp b/rclcpp_action/src/types.cpp new file mode 100644 index 0000000000..7f2a71d688 --- /dev/null +++ b/rclcpp_action/src/types.cpp @@ -0,0 +1,56 @@ +// 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 "rclcpp_action/types.hpp" + +#include + +namespace rclcpp_action +{ +std::string +to_string(const GoalUUID & goal_id) +{ + constexpr char HEX[] = "0123456789abcdef"; + std::string result; + result.resize(36); + size_t i = 0; + for (uint8_t byte : goal_id) { + result[i++] = HEX[byte >> 4]; + result[i++] = HEX[byte & 0x0f]; + // A RFC-4122 compliant UUID looks like: + // 00000000-0000-0000-0000-000000000000 + // That means that there is a '-' at offset 8, 13, 18, and 23 + if (i == 8 || i == 13 || i == 18 || i == 23) { + result[i++] = '-'; + } + } + return result; +} + +void +convert(const GoalUUID & goal_id, rcl_action_goal_info_t * info) +{ + for (size_t i = 0; i < UUID_SIZE; ++i) { + info->goal_id.uuid[i] = goal_id[i]; + } +} + +void +convert(const rcl_action_goal_info_t & info, GoalUUID * goal_id) +{ + for (size_t i = 0; i < UUID_SIZE; ++i) { + (*goal_id)[i] = info.goal_id.uuid[i]; + } +} +} // namespace rclcpp_action diff --git a/rclcpp_action/test/benchmark/CMakeLists.txt b/rclcpp_action/test/benchmark/CMakeLists.txt new file mode 100644 index 0000000000..0ce6c6245c --- /dev/null +++ b/rclcpp_action/test/benchmark/CMakeLists.txt @@ -0,0 +1,21 @@ +find_package(performance_test_fixture REQUIRED) + +# These benchmarks are only being created and run for the default RMW +# implementation. We are looking to test the performance of the ROS 2 code, not +# the underlying middleware. + +add_performance_test( + benchmark_action_client + benchmark_action_client.cpp + TIMEOUT 240) +if(TARGET benchmark_action_client) + target_link_libraries(benchmark_action_client ${PROJECT_NAME} rclcpp::rclcpp ${test_msgs_TARGETS}) +endif() + +add_performance_test( + benchmark_action_server + benchmark_action_server.cpp + TIMEOUT 120) +if(TARGET benchmark_action_server) + target_link_libraries(benchmark_action_server ${PROJECT_NAME} rclcpp::rclcpp ${test_msgs_TARGETS}) +endif() diff --git a/rclcpp_action/test/benchmark/benchmark_action_client.cpp b/rclcpp_action/test/benchmark/benchmark_action_client.cpp new file mode 100644 index 0000000000..8b11935117 --- /dev/null +++ b/rclcpp_action/test/benchmark/benchmark_action_client.cpp @@ -0,0 +1,363 @@ +// 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 "performance_test_fixture/performance_test_fixture.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "rclcpp/rclcpp.hpp" +#include "test_msgs/action/fibonacci.hpp" + +using performance_test_fixture::PerformanceTest; + +using Fibonacci = test_msgs::action::Fibonacci; +using GoalHandle = rclcpp_action::ServerGoalHandle; +using CancelResponse = typename Fibonacci::Impl::CancelGoalService::Response; +using GoalUUID = rclcpp_action::GoalUUID; + +constexpr char fibonacci_action_name[] = "fibonacci"; + +namespace +{ + +test_msgs::action::Fibonacci::Goal GetGoalOfOrder(int order) +{ + test_msgs::action::Fibonacci::Goal goal; + goal.order = order; + return goal; +} + +} // namespace + +class ActionClientPerformanceTest : public PerformanceTest +{ +public: + void SetUp(benchmark::State & state) + { + rclcpp::init(0, nullptr); + // Use same node for server and client to avoid interprocess communication + node = std::make_shared("node", "ns"); + performance_test_fixture::PerformanceTest::SetUp(state); + } + + void SetUpServer(const std::string & action_name) + { + // This action server accepts and defers so that execution can be timed separately from + // accepting the goal + action_server = rclcpp_action::create_server( + node, action_name, + [](const GoalUUID &, std::shared_ptr goal) { + if (goal->order <= 0) { + return rclcpp_action::GoalResponse::REJECT; + } + return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER; + }, + [](std::shared_ptr) { + return rclcpp_action::CancelResponse::ACCEPT; + }, + [this](std::shared_ptr goal_handle) { + current_goal_handle = goal_handle; + }); + } + + void ComputeFibonacciAndSetSuccess() + { + // This method is suprisingly slow, primarily due to the goal_handle->execute/succeed calls. + current_goal_handle->execute(); + const auto goal = current_goal_handle->get_goal(); + auto result = std::make_shared(); + + // Should be checked by the server above + assert(goal->order > 0); + result->sequence.resize(static_cast(goal->order)); + result->sequence[0] = 0; + if (goal->order == 1) { + current_goal_handle->succeed(result); + return; + } + result->sequence[1] = 1; + if (goal->order == 2) { + current_goal_handle->succeed(result); + return; + } + for (size_t i = 2; i < static_cast(goal->order); ++i) { + result->sequence[i] = + result->sequence[i - 1] + result->sequence[i - 2]; + } + current_goal_handle->succeed(result); + } + + void TearDown(benchmark::State & state) + { + performance_test_fixture::PerformanceTest::TearDown(state); + // Ensure proper sequencing of destruction + current_goal_handle.reset(); + action_server.reset(); + node.reset(); + rclcpp::shutdown(); + } + +protected: + std::shared_ptr node; + std::shared_ptr> action_server; + + // Goal handle needs to be kept alive by the server in order for client request specific to the + // goal to succeed. + std::shared_ptr current_goal_handle; +}; + +BENCHMARK_F(ActionClientPerformanceTest, construct_client_without_server)(benchmark::State & state) +{ + constexpr char action_name[] = "no_corresponding_server"; + for (auto _ : state) { + (void)_; + auto client = rclcpp_action::create_client(node, action_name); + + // Only timing construction, so destruction needs to happen explicitly while timing is paused + state.PauseTiming(); + client.reset(); + state.ResumeTiming(); + } +} + +BENCHMARK_F(ActionClientPerformanceTest, construct_client_with_server)(benchmark::State & state) +{ + SetUpServer(fibonacci_action_name); + reset_heap_counters(); + for (auto _ : state) { + (void)_; + auto client = rclcpp_action::create_client(node, fibonacci_action_name); + + // Only timing construction, so destruction needs to happen explicitly while timing is paused + state.PauseTiming(); + client.reset(); + state.ResumeTiming(); + } +} + +BENCHMARK_F(ActionClientPerformanceTest, destroy_client)(benchmark::State & state) +{ + for (auto _ : state) { + (void)_; + // This client does not have a corresponding server + state.PauseTiming(); + auto client = rclcpp_action::create_client(node, fibonacci_action_name); + state.ResumeTiming(); + + client.reset(); + } +} + +BENCHMARK_F(ActionClientPerformanceTest, async_send_goal_only)(benchmark::State & state) +{ + auto client = rclcpp_action::create_client(node, fibonacci_action_name); + SetUpServer(fibonacci_action_name); + if (!client->wait_for_action_server(std::chrono::seconds(1))) { + state.SkipWithError("Waiting for server timed out"); + return; + } + + const auto goal = GetGoalOfOrder(5); + + reset_heap_counters(); + for (auto _ : state) { + (void)_; + auto future_goal_handle = client->async_send_goal(goal); + } +} + +BENCHMARK_F(ActionClientPerformanceTest, async_send_goal_rejected)(benchmark::State & state) +{ + auto client = rclcpp_action::create_client(node, fibonacci_action_name); + SetUpServer(fibonacci_action_name); + if (!client->wait_for_action_server(std::chrono::seconds(1))) { + state.SkipWithError("Waiting for server timed out"); + return; + } + + // Order of 0 is invalid + const auto goal = GetGoalOfOrder(0); + + reset_heap_counters(); + for (auto _ : state) { + (void)_; + auto future_goal_handle = client->async_send_goal(goal); + rclcpp::spin_until_future_complete(node, future_goal_handle, std::chrono::seconds(1)); + if (!future_goal_handle.valid()) { + state.SkipWithError("Shared future was invalid"); + return; + } + if (nullptr != future_goal_handle.get()) { + state.SkipWithError("Invalid goal was not rejected"); + return; + } + } +} + +BENCHMARK_F(ActionClientPerformanceTest, async_send_goal_get_accepted_response)( + benchmark::State & state) +{ + auto client = rclcpp_action::create_client(node, fibonacci_action_name); + SetUpServer(fibonacci_action_name); + if (!client->wait_for_action_server(std::chrono::seconds(1))) { + state.SkipWithError("Waiting for server timed out"); + return; + } + + const auto goal = GetGoalOfOrder(10); + + reset_heap_counters(); + for (auto _ : state) { + (void)_; + // This server's execution is deferred + auto future_goal_handle = client->async_send_goal(goal); + rclcpp::spin_until_future_complete(node, future_goal_handle, std::chrono::seconds(1)); + + if (!future_goal_handle.valid()) { + state.SkipWithError("Shared future was invalid"); + return; + } + + auto goal_handle = future_goal_handle.get(); + if (rclcpp_action::GoalStatus::STATUS_ACCEPTED != goal_handle->get_status()) { + state.SkipWithError("Valid goal was not accepted"); + return; + } + } +} + +BENCHMARK_F(ActionClientPerformanceTest, async_get_result)(benchmark::State & state) +{ + auto client = rclcpp_action::create_client(node, fibonacci_action_name); + SetUpServer(fibonacci_action_name); + if (!client->wait_for_action_server(std::chrono::seconds(1))) { + state.SkipWithError("Waiting for server timed out"); + return; + } + + constexpr int expected_order = 5; + const auto goal = GetGoalOfOrder(expected_order); + + reset_heap_counters(); + for (auto _ : state) { + (void)_; + // Send goal, accept and execute while timing is paused + state.PauseTiming(); + auto future_goal_handle = client->async_send_goal(goal); + + // Action server accepts and defers, so this spin doesn't include result + rclcpp::spin_until_future_complete(node, future_goal_handle, std::chrono::seconds(1)); + + if (!future_goal_handle.valid()) { + state.SkipWithError("Shared future was invalid"); + return; + } + auto goal_handle = future_goal_handle.get(); + if (nullptr == goal_handle) { + state.SkipWithError("Goal handle was a nullptr"); + break; + } + + // Perform actual execution and set success + ComputeFibonacciAndSetSuccess(); + state.ResumeTiming(); + + // Measure how long it takes client to receive the succeeded result + auto future_result = client->async_get_result(goal_handle); + rclcpp::spin_until_future_complete(node, future_result, std::chrono::seconds(1)); + const auto & wrapped_result = future_result.get(); + if (rclcpp_action::ResultCode::SUCCEEDED != wrapped_result.code) { + state.SkipWithError("Fibonacci action did not succeed"); + break; + } + + const auto & sequence = wrapped_result.result->sequence; + if (sequence.size() != expected_order || sequence.back() != 3) { + state.SkipWithError("Fibonacci result was not correct"); + break; + } + } +} + +BENCHMARK_F(ActionClientPerformanceTest, async_cancel_goal)(benchmark::State & state) +{ + auto client = rclcpp_action::create_client(node, fibonacci_action_name); + SetUpServer(fibonacci_action_name); + if (!client->wait_for_action_server(std::chrono::seconds(1))) { + state.SkipWithError("Waiting for server timed out"); + return; + } + + constexpr int expected_order = 5; + const auto goal = GetGoalOfOrder(expected_order); + + reset_heap_counters(); + for (auto _ : state) { + (void)_; + state.PauseTiming(); + auto future_goal_handle = client->async_send_goal(goal); + + // Action server accepts and defers, so action can be canceled + rclcpp::spin_until_future_complete(node, future_goal_handle, std::chrono::seconds(1)); + auto goal_handle = future_goal_handle.get(); + state.ResumeTiming(); + + auto future_cancel = client->async_cancel_goal(goal_handle); + rclcpp::spin_until_future_complete(node, future_cancel, std::chrono::seconds(1)); + auto cancel_response = future_cancel.get(); + + using CancelActionResponse = test_msgs::action::Fibonacci::Impl::CancelGoalService::Response; + if (CancelActionResponse::ERROR_NONE != cancel_response->return_code) { + state.SkipWithError("Cancel request did not succeed"); + break; + } + } +} + +BENCHMARK_F(ActionClientPerformanceTest, async_cancel_all_goals)(benchmark::State & state) +{ + auto client = rclcpp_action::create_client(node, fibonacci_action_name); + SetUpServer(fibonacci_action_name); + if (!client->wait_for_action_server(std::chrono::seconds(1))) { + state.SkipWithError("Waiting for server timed out"); + return; + } + + constexpr int expected_order = 5; + const auto goal = GetGoalOfOrder(expected_order); + constexpr int num_concurrently_inflight_goals = 10u; + + reset_heap_counters(); + for (auto _ : state) { + (void)_; + state.PauseTiming(); + for (int i = 0; i < num_concurrently_inflight_goals; ++i) { + auto future_goal_handle = client->async_send_goal(goal); + rclcpp::spin_until_future_complete(node, future_goal_handle, std::chrono::seconds(1)); + } + // Action server accepts and defers, so action can be canceled + state.ResumeTiming(); + + auto future_cancel_all = client->async_cancel_all_goals(); + rclcpp::spin_until_future_complete(node, future_cancel_all, std::chrono::seconds(1)); + auto cancel_response = future_cancel_all.get(); + + using CancelActionResponse = test_msgs::action::Fibonacci::Impl::CancelGoalService::Response; + if (CancelActionResponse::ERROR_NONE != cancel_response->return_code) { + state.SkipWithError("Cancel request did not succeed"); + break; + } + } +} diff --git a/rclcpp_action/test/benchmark/benchmark_action_server.cpp b/rclcpp_action/test/benchmark/benchmark_action_server.cpp new file mode 100644 index 0000000000..6817f86b14 --- /dev/null +++ b/rclcpp_action/test/benchmark/benchmark_action_server.cpp @@ -0,0 +1,326 @@ +// 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 "performance_test_fixture/performance_test_fixture.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "rclcpp/rclcpp.hpp" +#include "test_msgs/action/fibonacci.hpp" + +using performance_test_fixture::PerformanceTest; + +using Fibonacci = test_msgs::action::Fibonacci; +using GoalHandle = rclcpp_action::ServerGoalHandle; +using CancelResponse = typename Fibonacci::Impl::CancelGoalService::Response; +using GoalUUID = rclcpp_action::GoalUUID; + +constexpr char fibonacci_action_name[] = "fibonacci"; + +class ActionServerPerformanceTest : public PerformanceTest +{ +public: + void SetUp(benchmark::State & state) + { + rclcpp::init(0, nullptr); + node = std::make_shared("node", "ns"); + action_client = + rclcpp_action::create_client(node, fibonacci_action_name); + performance_test_fixture::PerformanceTest::SetUp(state); + } + + void TearDown(benchmark::State & state) + { + performance_test_fixture::PerformanceTest::TearDown(state); + + action_client.reset(); + node.reset(); + rclcpp::shutdown(); + } + + auto AsyncSendGoalOfOrder(const int order) + { + test_msgs::action::Fibonacci::Goal goal; + goal.order = order; + + return action_client->async_send_goal(goal); + } + +protected: + std::shared_ptr node; + std::shared_ptr> action_client; +}; + +BENCHMARK_F(ActionServerPerformanceTest, construct_server_without_client)(benchmark::State & state) +{ + constexpr char action_name[] = "no_corresponding_client"; + for (auto _ : state) { + (void)_; + auto action_server = rclcpp_action::create_server( + node, action_name, + [](const GoalUUID &, std::shared_ptr) { + return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER; + }, + [](std::shared_ptr) { + return rclcpp_action::CancelResponse::ACCEPT; + }, + [](std::shared_ptr) {}); + benchmark::DoNotOptimize(action_server); + benchmark::ClobberMemory(); + + state.PauseTiming(); + action_server.reset(); + state.ResumeTiming(); + } +} + +BENCHMARK_F(ActionServerPerformanceTest, construct_server_with_client)(benchmark::State & state) +{ + for (auto _ : state) { + (void)_; + auto action_server = rclcpp_action::create_server( + node, fibonacci_action_name, + [](const GoalUUID &, std::shared_ptr) { + return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER; + }, + [](std::shared_ptr) { + return rclcpp_action::CancelResponse::ACCEPT; + }, + [](std::shared_ptr) {}); + benchmark::DoNotOptimize(action_server); + benchmark::ClobberMemory(); + + state.PauseTiming(); + action_server.reset(); + state.ResumeTiming(); + } +} + +BENCHMARK_F(ActionServerPerformanceTest, destroy_server)(benchmark::State & state) +{ + for (auto _ : state) { + (void)_; + state.PauseTiming(); + auto action_server = rclcpp_action::create_server( + node, fibonacci_action_name, + [](const GoalUUID &, std::shared_ptr) { + return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER; + }, + [](std::shared_ptr) { + return rclcpp_action::CancelResponse::ACCEPT; + }, + [](std::shared_ptr) {}); + state.ResumeTiming(); + benchmark::DoNotOptimize(action_server); + benchmark::ClobberMemory(); + + action_server.reset(); + } +} + +BENCHMARK_F(ActionServerPerformanceTest, action_server_accept_goal)(benchmark::State & state) +{ + std::shared_ptr current_goal_handle = nullptr; + auto action_server = rclcpp_action::create_server( + node, fibonacci_action_name, + [](const GoalUUID &, std::shared_ptr) { + return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER; + }, + [](std::shared_ptr) { + return rclcpp_action::CancelResponse::ACCEPT; + }, + [¤t_goal_handle](std::shared_ptr goal_handle) { + current_goal_handle = goal_handle; + }); + + reset_heap_counters(); + for (auto _ : state) { + (void)_; + state.PauseTiming(); + auto client_goal_handle_future = AsyncSendGoalOfOrder(1); + state.ResumeTiming(); + + rclcpp::spin_until_future_complete(node, client_goal_handle_future); + auto goal_handle = client_goal_handle_future.get(); + if (rclcpp_action::GoalStatus::STATUS_ACCEPTED != goal_handle->get_status()) { + state.SkipWithError("Valid goal was not accepted"); + return; + } + } +} + +BENCHMARK_F(ActionServerPerformanceTest, action_server_cancel_goal)(benchmark::State & state) +{ + // The goal handle needs to be assigned to a variable for the lifetime of the goal so that it is + // not cleaned up before the cancel request is received and processed. + std::shared_ptr server_goal_handle = nullptr; + auto action_server = rclcpp_action::create_server( + node, fibonacci_action_name, + [](const GoalUUID &, std::shared_ptr) { + return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER; + }, + [](std::shared_ptr) { + return rclcpp_action::CancelResponse::ACCEPT; + }, + [&server_goal_handle](std::shared_ptr goal_handle) { + server_goal_handle = goal_handle; + }); + + reset_heap_counters(); + for (auto _ : state) { + (void)_; + state.PauseTiming(); + auto client_goal_handle_future = AsyncSendGoalOfOrder(1); + // This spin completes when the goal has been accepted, but not executed because server + // responds with ACCEPT_AND_DEFER + rclcpp::spin_until_future_complete(node, client_goal_handle_future, std::chrono::seconds(1)); + auto client_goal_handle = client_goal_handle_future.get(); + auto future_cancel = action_client->async_cancel_goal(client_goal_handle); + state.ResumeTiming(); + + rclcpp::spin_until_future_complete(node, future_cancel, std::chrono::seconds(1)); + auto cancel_response = future_cancel.get(); + using CancelActionResponse = test_msgs::action::Fibonacci::Impl::CancelGoalService::Response; + if (CancelActionResponse::ERROR_NONE != cancel_response->return_code) { + state.SkipWithError("Cancel request did not succeed"); + break; + } + } +} + +BENCHMARK_F(ActionServerPerformanceTest, action_server_execute_goal)(benchmark::State & state) +{ + std::shared_ptr server_goal_handle = nullptr; + auto action_server = rclcpp_action::create_server( + node, fibonacci_action_name, + [](const GoalUUID &, std::shared_ptr) { + return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER; + }, + [](std::shared_ptr) { + return rclcpp_action::CancelResponse::ACCEPT; + }, + [&server_goal_handle](std::shared_ptr goal_handle) { + server_goal_handle = goal_handle; + }); + + reset_heap_counters(); + for (auto _ : state) { + (void)_; + state.PauseTiming(); + auto client_goal_handle_future = AsyncSendGoalOfOrder(1); + + rclcpp::spin_until_future_complete(node, client_goal_handle_future); + auto goal_handle = client_goal_handle_future.get(); + if (rclcpp_action::GoalStatus::STATUS_ACCEPTED != goal_handle->get_status()) { + state.SkipWithError("Valid goal was not accepted"); + return; + } + state.ResumeTiming(); + + server_goal_handle->execute(); + } +} + +BENCHMARK_F(ActionServerPerformanceTest, action_server_set_success)(benchmark::State & state) +{ + constexpr int goal_order = 1; + std::shared_ptr server_goal_handle = nullptr; + auto action_server = rclcpp_action::create_server( + node, fibonacci_action_name, + [](const GoalUUID &, std::shared_ptr) { + return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER; + }, + [](std::shared_ptr) { + return rclcpp_action::CancelResponse::ACCEPT; + }, + [&server_goal_handle](std::shared_ptr goal_handle) { + server_goal_handle = goal_handle; + }); + + // MSVC and Clang disagree how goal_order should be captured here. Though this capture is a bit + // too wide, they at least could agree it was fine. In my testing MSVC errored if goal_order was + // not captured, but clang would warn if it was explicitly captured. + const auto result = [&]() { + auto action_result = std::make_shared(); + for (int i = 0; i < goal_order; ++i) { + // Not the fibonacci sequence, but that's not important to this benchmark + action_result->sequence.push_back(i); + } + return action_result; + } (); + + reset_heap_counters(); + for (auto _ : state) { + (void)_; + state.PauseTiming(); + auto client_goal_handle_future = AsyncSendGoalOfOrder(goal_order); + + rclcpp::spin_until_future_complete(node, client_goal_handle_future); + auto goal_handle = client_goal_handle_future.get(); + if (rclcpp_action::GoalStatus::STATUS_ACCEPTED != goal_handle->get_status()) { + state.SkipWithError("Valid goal was not accepted"); + return; + } + server_goal_handle->execute(); + state.ResumeTiming(); + + server_goal_handle->succeed(result); + } +} + +BENCHMARK_F(ActionServerPerformanceTest, action_server_abort)(benchmark::State & state) +{ + constexpr int goal_order = 1; + std::shared_ptr server_goal_handle = nullptr; + auto action_server = rclcpp_action::create_server( + node, fibonacci_action_name, + [](const GoalUUID &, std::shared_ptr) { + return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER; + }, + [](std::shared_ptr) { + return rclcpp_action::CancelResponse::ACCEPT; + }, + [&server_goal_handle](std::shared_ptr goal_handle) { + server_goal_handle = goal_handle; + }); + + // Capturing with & because MSVC and Clang disagree about how to capture goal_order + const auto result = [&]() { + auto action_result = std::make_shared(); + for (int i = 0; i < goal_order; ++i) { + // Not the fibonacci sequence, but that's not important to this benchmark + action_result->sequence.push_back(i); + } + return action_result; + } (); + + reset_heap_counters(); + for (auto _ : state) { + (void)_; + state.PauseTiming(); + auto client_goal_handle_future = AsyncSendGoalOfOrder(goal_order); + + rclcpp::spin_until_future_complete(node, client_goal_handle_future); + auto goal_handle = client_goal_handle_future.get(); + if (rclcpp_action::GoalStatus::STATUS_ACCEPTED != goal_handle->get_status()) { + state.SkipWithError("Valid goal was not accepted"); + return; + } + server_goal_handle->execute(); + state.ResumeTiming(); + + server_goal_handle->abort(result); + } +} diff --git a/rclcpp_action/test/mocking_utils/patch.hpp b/rclcpp_action/test/mocking_utils/patch.hpp new file mode 100644 index 0000000000..4c6630c426 --- /dev/null +++ b/rclcpp_action/test/mocking_utils/patch.hpp @@ -0,0 +1,393 @@ +// 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/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 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 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 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 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 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 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 ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6) +/// 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, ArgT6); +}; + +/// 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 execute normally but always yield a given `return_code` +/// in a given `scope`. +#define inject_on_return(scope, function, return_code) \ + patch( \ + scope, function, ([&, base = function](auto && ... __args) { \ + static_cast(base(std::forward(__args)...)); \ + 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/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp new file mode 100644 index 0000000000..08093cb873 --- /dev/null +++ b/rclcpp_action/test/test_client.cpp @@ -0,0 +1,1039 @@ +// 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 +#include +#include + +#include "gtest/gtest.h" + +#include "rcl/allocator.h" +#include "rcl/time.h" +#include "rcl/types.h" + +#include "rcl_action/names.h" +#include "rcl_action/default_qos.h" +#include "rcl_action/wait.h" + +#include "rclcpp/clock.hpp" +#include "rclcpp/exceptions.hpp" +#include "rclcpp/executors.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/publisher.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/service.hpp" +#include "rclcpp/time.hpp" + +#include "test_msgs/action/fibonacci.hpp" + +#include "rclcpp_action/exceptions.hpp" +#include "rclcpp_action/create_client.hpp" +#include "rclcpp_action/client.hpp" +#include "rclcpp_action/qos.hpp" +#include "rclcpp_action/types.hpp" + +#include "mocking_utils/patch.hpp" + +using namespace std::chrono_literals; + +const auto WAIT_FOR_SERVER_TIMEOUT = 10s; + +class TestClient : public ::testing::Test +{ +protected: + using ActionType = test_msgs::action::Fibonacci; + using ActionGoal = ActionType::Goal; + using ActionGoalHandle = rclcpp_action::ClientGoalHandle; + using ActionGoalRequestService = ActionType::Impl::SendGoalService; + using ActionGoalRequest = ActionGoalRequestService::Request; + using ActionGoalResponse = ActionGoalRequestService::Response; + using ActionGoalResultService = ActionType::Impl::GetResultService; + using ActionGoalResultRequest = ActionGoalResultService::Request; + using ActionGoalResultResponse = ActionGoalResultService::Response; + using ActionCancelGoalService = ActionType::Impl::CancelGoalService; + using ActionCancelGoalRequest = ActionType::Impl::CancelGoalService::Request; + using ActionCancelGoalResponse = ActionType::Impl::CancelGoalService::Response; + using ActionStatusMessage = ActionType::Impl::GoalStatusMessage; + using ActionFeedbackMessage = ActionType::Impl::FeedbackMessage; + using ActionFeedback = ActionType::Feedback; + + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + void SetUpServer() + { + rcl_allocator_t allocator = rcl_get_default_allocator(); + + server_node = std::make_shared(server_node_name, namespace_name); + + char * goal_service_name = nullptr; + rcl_ret_t ret = rcl_action_get_goal_service_name( + action_name, allocator, &goal_service_name); + ASSERT_EQ(RCL_RET_OK, ret); + goal_service = server_node->create_service( + goal_service_name, + [this]( + const ActionGoalRequest::SharedPtr request, + ActionGoalResponse::SharedPtr response) + { + response->stamp = clock.now(); + response->accepted = (request->goal.order >= 0); + if (response->accepted) { + goals[request->goal_id.uuid] = {request, response}; + } + }); + ASSERT_TRUE(goal_service != nullptr); + allocator.deallocate(goal_service_name, allocator.state); + + char * result_service_name = nullptr; + ret = rcl_action_get_result_service_name( + action_name, allocator, &result_service_name); + ASSERT_EQ(RCL_RET_OK, ret); + result_service = server_node->create_service( + result_service_name, + [this]( + const ActionGoalResultRequest::SharedPtr request, + ActionGoalResultResponse::SharedPtr response) + { + if (goals.count(request->goal_id.uuid) == 1) { + auto goal_request = goals[request->goal_id.uuid].first; + auto goal_response = goals[request->goal_id.uuid].second; + ActionStatusMessage status_message; + rclcpp_action::GoalStatus goal_status; + goal_status.goal_info.goal_id.uuid = goal_request->goal_id.uuid; + goal_status.goal_info.stamp = goal_response->stamp; + goal_status.status = rclcpp_action::GoalStatus::STATUS_EXECUTING; + status_message.status_list.push_back(goal_status); + status_publisher->publish(status_message); + client_executor.spin_once(); + ActionFeedbackMessage feedback_message; + feedback_message.goal_id.uuid = goal_request->goal_id.uuid; + feedback_message.feedback.sequence.push_back(0); + feedback_publisher->publish(feedback_message); + client_executor.spin_once(); + if (goal_request->goal.order > 0) { + feedback_message.feedback.sequence.push_back(1); + feedback_publisher->publish(feedback_message); + client_executor.spin_once(); + for (size_t i = 1; i < static_cast(goal_request->goal.order); ++i) { + feedback_message.feedback.sequence.push_back( + feedback_message.feedback.sequence[i] + + feedback_message.feedback.sequence[i - 1]); + feedback_publisher->publish(feedback_message); + client_executor.spin_once(); + } + } + goal_status.status = rclcpp_action::GoalStatus::STATUS_SUCCEEDED; + status_message.status_list[0] = goal_status; + status_publisher->publish(status_message); + client_executor.spin_once(); + response->result.sequence = feedback_message.feedback.sequence; + response->status = rclcpp_action::GoalStatus::STATUS_SUCCEEDED; + goals.erase(request->goal_id.uuid); + } else { + response->status = rclcpp_action::GoalStatus::STATUS_UNKNOWN; + } + }); + ASSERT_TRUE(result_service != nullptr); + allocator.deallocate(result_service_name, allocator.state); + + char * cancel_service_name = nullptr; + ret = rcl_action_get_cancel_service_name( + action_name, allocator, &cancel_service_name); + ASSERT_EQ(RCL_RET_OK, ret); + cancel_service = server_node->create_service( + cancel_service_name, + [this]( + const ActionCancelGoalRequest::SharedPtr request, + ActionCancelGoalResponse::SharedPtr response) + { + rclcpp_action::GoalUUID zero_uuid; + std::fill(zero_uuid.begin(), zero_uuid.end(), 0u); + const rclcpp::Time cancel_stamp = request->goal_info.stamp; + bool cancel_all = ( + request->goal_info.goal_id.uuid == zero_uuid && + cancel_stamp == zero_stamp); + ActionStatusMessage status_message; + auto it = goals.begin(); + while (it != goals.end()) { + auto goal_request = it->second.first; + auto goal_response = it->second.second; + const rclcpp::Time goal_stamp = goal_response->stamp; + bool cancel_this = ( + request->goal_info.goal_id.uuid == goal_request->goal_id.uuid || + cancel_stamp > goal_stamp); + if (cancel_all || cancel_this) { + rclcpp_action::GoalStatus goal_status; + goal_status.goal_info.goal_id.uuid = goal_request->goal_id.uuid; + goal_status.goal_info.stamp = goal_response->stamp; + goal_status.status = rclcpp_action::GoalStatus::STATUS_CANCELED; + status_message.status_list.push_back(goal_status); + response->goals_canceling.push_back(goal_status.goal_info); + it = goals.erase(it); + } else { + ++it; + } + } + status_publisher->publish(status_message); + client_executor.spin_once(); + }); + ASSERT_TRUE(cancel_service != nullptr); + allocator.deallocate(cancel_service_name, allocator.state); + + char * feedback_topic_name = nullptr; + ret = rcl_action_get_feedback_topic_name( + action_name, allocator, &feedback_topic_name); + ASSERT_EQ(RCL_RET_OK, ret); + feedback_publisher = + server_node->create_publisher(feedback_topic_name, 10); + ASSERT_TRUE(feedback_publisher != nullptr); + allocator.deallocate(feedback_topic_name, allocator.state); + + char * status_topic_name = nullptr; + ret = rcl_action_get_status_topic_name( + action_name, allocator, &status_topic_name); + ASSERT_EQ(RCL_RET_OK, ret); + status_publisher = server_node->create_publisher( + status_topic_name, rclcpp_action::DefaultActionStatusQoS()); + ASSERT_TRUE(status_publisher != nullptr); + allocator.deallocate(status_topic_name, allocator.state); + server_executor.add_node(server_node); + } + + void SetUp() override + { + client_node = std::make_shared(client_node_name, namespace_name); + client_executor.add_node(client_node); + + ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(clock.get_clock_handle())); + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(1))); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void TearDownServer() + { + status_publisher.reset(); + feedback_publisher.reset(); + cancel_service.reset(); + result_service.reset(); + goal_service.reset(); + server_node.reset(); + } + + void TearDown() override + { + client_node.reset(); + } + + template + void dual_spin_until_future_complete(std::shared_future & future) + { + std::future_status status; + do { + server_executor.spin_some(); + client_executor.spin_some(); + status = future.wait_for(std::chrono::seconds(0)); + } while (std::future_status::ready != status); + } + + rclcpp::Clock clock{RCL_ROS_TIME}; + const rclcpp::Time zero_stamp{0, 0, RCL_ROS_TIME}; + + rclcpp::Node::SharedPtr server_node; + rclcpp::executors::SingleThreadedExecutor server_executor; + rclcpp::Node::SharedPtr client_node; + rclcpp::executors::SingleThreadedExecutor client_executor; + const char * const server_node_name{"fibonacci_action_test_server"}; + const char * const client_node_name{"fibonacci_action_test_client"}; + const char * const namespace_name{"/rclcpp_action/test/client"}; + const char * const action_name{"fibonacci_test"}; + + std::map< + rclcpp_action::GoalUUID, + std::pair< + typename ActionGoalRequest::SharedPtr, + typename ActionGoalResponse::SharedPtr>> goals; + typename rclcpp::Service::SharedPtr goal_service; + typename rclcpp::Service::SharedPtr result_service; + typename rclcpp::Service::SharedPtr cancel_service; + typename rclcpp::Publisher::SharedPtr feedback_publisher; + typename rclcpp::Publisher::SharedPtr status_publisher; +}; + +class TestClientAgainstServer : public TestClient +{ +protected: + void SetUp() override + { + SetUpServer(); + TestClient::SetUp(); + } + + void TearDown() override + { + TestClient::TearDown(); + TearDownServer(); + } +}; + + +TEST_F(TestClient, construction_and_destruction) +{ + ASSERT_NO_THROW(rclcpp_action::create_client(client_node, action_name).reset()); +} + +TEST_F(TestClient, construction_and_destruction_after_node) +{ + ASSERT_NO_THROW( + { + auto action_client = rclcpp_action::create_client(client_node, action_name); + client_node.reset(); + }); +} + +TEST_F(TestClient, construction_and_destruction_callback_group) +{ + auto group = client_node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + const rcl_action_client_options_t & options = rcl_action_client_get_default_options(); + ASSERT_NO_THROW( + rclcpp_action::create_client( + client_node->get_node_base_interface(), + client_node->get_node_graph_interface(), + client_node->get_node_logging_interface(), + client_node->get_node_waitables_interface(), + action_name, + group, + options + ).reset()); +} + +TEST_F(TestClient, construction_and_destruction_rcl_errors) +{ + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", rcl_action_client_fini, RCL_RET_ERROR); + // It just logs an error message and continues + EXPECT_NO_THROW( + rclcpp_action::create_client(client_node, action_name).reset()); + } + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", rcl_action_client_init, RCL_RET_ERROR); + EXPECT_THROW( + rclcpp_action::create_client(client_node, action_name).reset(), + rclcpp::exceptions::RCLError); + } + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", rcl_action_client_wait_set_get_num_entities, RCL_RET_ERROR); + EXPECT_THROW( + rclcpp_action::create_client(client_node, action_name), + rclcpp::exceptions::RCLError); + } +} + +TEST_F(TestClient, wait_for_action_server) +{ + auto action_client = rclcpp_action::create_client(client_node, action_name); + EXPECT_FALSE(action_client->wait_for_action_server(0ms)); + EXPECT_FALSE(action_client->wait_for_action_server(10ms)); + SetUpServer(); + ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + TearDownServer(); + + client_node.reset(); // Drop node before action client + EXPECT_THROW(action_client->wait_for_action_server(0ms), rclcpp::exceptions::InvalidNodeError); +} + +TEST_F(TestClient, wait_for_action_server_rcl_errors) +{ + auto action_client = rclcpp_action::create_client(client_node, action_name); + SetUpServer(); + ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", rcl_action_server_is_available, RCL_RET_NODE_INVALID); + EXPECT_THROW(action_client->action_server_is_ready(), rclcpp::exceptions::RCLError); + + auto mock_context_is_valid = mocking_utils::patch_and_return( + "lib:rclcpp_action", rcl_context_is_valid, false); + EXPECT_FALSE(action_client->action_server_is_ready()); + } + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", rcl_action_server_is_available, RCL_RET_ERROR); + EXPECT_THROW(action_client->action_server_is_ready(), rclcpp::exceptions::RCLError); + } + TearDownServer(); +} + +TEST_F(TestClient, is_ready) { + auto action_client = rclcpp_action::create_client(client_node, action_name); + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + auto rcl_context = client_node->get_node_base_interface()->get_context()->get_rcl_context().get(); + ASSERT_EQ( + RCL_RET_OK, + rcl_wait_set_init(&wait_set, 10, 10, 10, 10, 10, 10, rcl_context, allocator)); + ASSERT_NO_THROW(action_client->add_to_wait_set(wait_set)); + EXPECT_TRUE(action_client->is_ready(wait_set)); + + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", rcl_action_client_wait_set_get_entities_ready, RCL_RET_ERROR); + EXPECT_THROW(action_client->is_ready(wait_set), rclcpp::exceptions::RCLError); + } + client_node.reset(); // Drop node before action client +} + +TEST_F(TestClientAgainstServer, async_send_goal_no_callbacks) +{ + auto action_client = rclcpp_action::create_client(client_node, action_name); + ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + ActionGoal bad_goal; + bad_goal.order = -5; + auto future_goal_handle = action_client->async_send_goal(bad_goal); + dual_spin_until_future_complete(future_goal_handle); + EXPECT_EQ(nullptr, future_goal_handle.get().get()); + + ActionGoal good_goal; + good_goal.order = 5; + future_goal_handle = action_client->async_send_goal(good_goal); + dual_spin_until_future_complete(future_goal_handle); + auto goal_handle = future_goal_handle.get(); + EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status()); + EXPECT_FALSE(goal_handle->is_feedback_aware()); + EXPECT_FALSE(goal_handle->is_result_aware()); +} + +TEST_F(TestClientAgainstServer, bad_goal_handles) +{ + auto action_client0 = rclcpp_action::create_client(client_node, action_name); + ASSERT_TRUE(action_client0->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + ActionGoal goal; + goal.order = 0; + auto future_goal_handle = action_client0->async_send_goal(goal); + dual_spin_until_future_complete(future_goal_handle); + auto goal_handle = future_goal_handle.get(); + + auto action_client1 = rclcpp_action::create_client(client_node, action_name); + using rclcpp_action::exceptions::UnknownGoalHandleError; + EXPECT_THROW(action_client1->async_get_result(goal_handle), UnknownGoalHandleError); + EXPECT_THROW(action_client1->async_cancel_goal(goal_handle), UnknownGoalHandleError); +} + +TEST_F(TestClientAgainstServer, async_send_goal_no_callbacks_wait_for_result) +{ + auto action_client = rclcpp_action::create_client(client_node, action_name); + ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + ActionGoal goal; + goal.order = 5; + auto future_goal_handle = action_client->async_send_goal(goal); + dual_spin_until_future_complete(future_goal_handle); + auto goal_handle = future_goal_handle.get(); + EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status()); + EXPECT_FALSE(goal_handle->is_feedback_aware()); + EXPECT_FALSE(goal_handle->is_result_aware()); + auto future_result = action_client->async_get_result(goal_handle); + EXPECT_TRUE(goal_handle->is_result_aware()); + dual_spin_until_future_complete(future_result); + auto wrapped_result = future_result.get(); + ASSERT_EQ(6ul, wrapped_result.result->sequence.size()); + EXPECT_EQ(0, wrapped_result.result->sequence[0]); + EXPECT_EQ(1, wrapped_result.result->sequence[1]); + EXPECT_EQ(5, wrapped_result.result->sequence[5]); +} + +TEST_F(TestClientAgainstServer, async_send_goal_no_callbacks_then_invalidate) +{ + auto action_client = rclcpp_action::create_client(client_node, action_name); + ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + ActionGoal goal; + goal.order = 5; + auto future_goal_handle = action_client->async_send_goal(goal); + dual_spin_until_future_complete(future_goal_handle); + auto goal_handle = future_goal_handle.get(); + ASSERT_NE(nullptr, goal_handle); + EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status()); + auto future_result = action_client->async_get_result(goal_handle); + EXPECT_TRUE(goal_handle->is_result_aware()); + + action_client.reset(); // Ensure goal handle is invalidated once client goes out of scope + + EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_UNKNOWN, goal_handle->get_status()); + using rclcpp_action::exceptions::UnawareGoalHandleError; + EXPECT_THROW(future_result.get(), UnawareGoalHandleError); +} + +TEST_F(TestClientAgainstServer, async_send_goal_with_goal_response_callback_wait_for_result) +{ + auto action_client = rclcpp_action::create_client(client_node, action_name); + ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + bool goal_response_received = false; + auto send_goal_ops = rclcpp_action::Client::SendGoalOptions(); + send_goal_ops.goal_response_callback = + [&goal_response_received](typename ActionGoalHandle::SharedPtr goal_handle) + { + if (goal_handle) { + goal_response_received = true; + } + }; + + { + ActionGoal bad_goal; + bad_goal.order = -1; + auto future_goal_handle = action_client->async_send_goal(bad_goal, send_goal_ops); + dual_spin_until_future_complete(future_goal_handle); + auto goal_handle = future_goal_handle.get(); + EXPECT_FALSE(goal_response_received); + EXPECT_EQ(nullptr, goal_handle); + } + + { + ActionGoal goal; + goal.order = 4; + auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops); + dual_spin_until_future_complete(future_goal_handle); + auto goal_handle = future_goal_handle.get(); + EXPECT_TRUE(goal_response_received); + EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status()); + EXPECT_FALSE(goal_handle->is_feedback_aware()); + EXPECT_FALSE(goal_handle->is_result_aware()); + auto future_result = action_client->async_get_result(goal_handle); + EXPECT_TRUE(goal_handle->is_result_aware()); + dual_spin_until_future_complete(future_result); + auto wrapped_result = future_result.get(); + ASSERT_EQ(5u, wrapped_result.result->sequence.size()); + EXPECT_EQ(3, wrapped_result.result->sequence.back()); + } +} + +TEST_F(TestClientAgainstServer, async_send_goal_with_feedback_callback_wait_for_result) +{ + auto action_client = rclcpp_action::create_client(client_node, action_name); + ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + ActionGoal goal; + goal.order = 4; + int feedback_count = 0; + auto send_goal_ops = rclcpp_action::Client::SendGoalOptions(); + send_goal_ops.feedback_callback = [&feedback_count]( + typename ActionGoalHandle::SharedPtr goal_handle, + const std::shared_ptr feedback) + { + (void)goal_handle; + (void)feedback; + feedback_count++; + }; + auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops); + dual_spin_until_future_complete(future_goal_handle); + auto goal_handle = future_goal_handle.get(); + EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status()); + EXPECT_TRUE(goal_handle->is_feedback_aware()); + EXPECT_FALSE(goal_handle->is_result_aware()); + auto future_result = action_client->async_get_result(goal_handle); + EXPECT_TRUE(goal_handle->is_result_aware()); + dual_spin_until_future_complete(future_result); + auto wrapped_result = future_result.get(); + + ASSERT_EQ(5u, wrapped_result.result->sequence.size()); + EXPECT_EQ(3, wrapped_result.result->sequence.back()); + EXPECT_EQ(5, feedback_count); +} + +TEST_F(TestClientAgainstServer, async_send_goal_with_result_callback_wait_for_result) +{ + auto action_client = rclcpp_action::create_client(client_node, action_name); + ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + ActionGoal goal; + goal.order = 4; + bool result_callback_received = false; + auto send_goal_ops = rclcpp_action::Client::SendGoalOptions(); + send_goal_ops.result_callback = + [&result_callback_received]( + const typename ActionGoalHandle::WrappedResult & result) + { + if ( + rclcpp_action::ResultCode::SUCCEEDED == result.code && + result.result->sequence.size() == 5u) + { + result_callback_received = true; + } + }; + auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops); + dual_spin_until_future_complete(future_goal_handle); + auto goal_handle = future_goal_handle.get(); + EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status()); + EXPECT_FALSE(goal_handle->is_feedback_aware()); + EXPECT_TRUE(goal_handle->is_result_aware()); + auto future_result = action_client->async_get_result(goal_handle); + dual_spin_until_future_complete(future_result); + auto wrapped_result = future_result.get(); + + EXPECT_TRUE(result_callback_received); + ASSERT_EQ(5u, wrapped_result.result->sequence.size()); + EXPECT_EQ(3, wrapped_result.result->sequence.back()); +} + +TEST_F(TestClientAgainstServer, async_get_result_with_callback) +{ + auto action_client = rclcpp_action::create_client(client_node, action_name); + ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + ActionGoal goal; + goal.order = 4; + auto future_goal_handle = action_client->async_send_goal(goal); + dual_spin_until_future_complete(future_goal_handle); + auto goal_handle = future_goal_handle.get(); + EXPECT_NE(goal_handle, nullptr); + EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status()); + EXPECT_FALSE(goal_handle->is_feedback_aware()); + EXPECT_FALSE(goal_handle->is_result_aware()); + bool result_callback_received = false; + auto future_result = action_client->async_get_result( + goal_handle, + [&result_callback_received]( + const typename ActionGoalHandle::WrappedResult & result) + { + if ( + rclcpp_action::ResultCode::SUCCEEDED == result.code && + result.result->sequence.size() == 5u) + { + result_callback_received = true; + } + }); + dual_spin_until_future_complete(future_result); + auto wrapped_result = future_result.get(); + + EXPECT_TRUE(result_callback_received); + ASSERT_EQ(5u, wrapped_result.result->sequence.size()); + EXPECT_EQ(3, wrapped_result.result->sequence.back()); +} + +TEST_F(TestClientAgainstServer, async_cancel_one_goal) +{ + auto action_client = rclcpp_action::create_client(client_node, action_name); + ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + ActionGoal goal; + goal.order = 5; + auto future_goal_handle = action_client->async_send_goal(goal); + dual_spin_until_future_complete(future_goal_handle); + auto goal_handle = future_goal_handle.get(); + EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status()); + + auto future_cancel = action_client->async_cancel_goal(goal_handle); + dual_spin_until_future_complete(future_cancel); + ActionCancelGoalResponse::SharedPtr cancel_response = future_cancel.get(); + EXPECT_EQ(ActionCancelGoalResponse::ERROR_NONE, cancel_response->return_code); +} + +TEST_F(TestClientAgainstServer, async_cancel_one_goal_with_callback) +{ + auto action_client = rclcpp_action::create_client(client_node, action_name); + ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + ActionGoal goal; + goal.order = 5; + auto future_goal_handle = action_client->async_send_goal(goal); + dual_spin_until_future_complete(future_goal_handle); + auto goal_handle = future_goal_handle.get(); + EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status()); + + bool cancel_response_received = false; + auto future_cancel = action_client->async_cancel_goal( + goal_handle, + [&cancel_response_received, goal_handle]( + ActionCancelGoalResponse::SharedPtr response) + { + if ( + ActionCancelGoalResponse::ERROR_NONE == response->return_code && + 1ul == response->goals_canceling.size() && + goal_handle->get_goal_id() == response->goals_canceling[0].goal_id.uuid) + { + cancel_response_received = true; + } + }); + dual_spin_until_future_complete(future_cancel); + auto cancel_response = future_cancel.get(); + EXPECT_EQ(ActionCancelGoalResponse::ERROR_NONE, cancel_response->return_code); + ASSERT_EQ(1ul, cancel_response->goals_canceling.size()); + EXPECT_EQ(goal_handle->get_goal_id(), cancel_response->goals_canceling[0].goal_id.uuid); + EXPECT_TRUE(cancel_response_received); +} + +TEST_F(TestClientAgainstServer, async_cancel_all_goals) +{ + auto action_client = rclcpp_action::create_client(client_node, action_name); + ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + ActionGoal goal; + goal.order = 6; + auto future_goal_handle0 = action_client->async_send_goal(goal); + dual_spin_until_future_complete(future_goal_handle0); + auto goal_handle0 = future_goal_handle0.get(); + + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(2))); + + goal.order = 8; + auto future_goal_handle1 = action_client->async_send_goal(goal); + dual_spin_until_future_complete(future_goal_handle1); + auto goal_handle1 = future_goal_handle1.get(); + + if (goal_handle1->get_goal_id() < goal_handle0->get_goal_id()) { + goal_handle0.swap(goal_handle1); + } + + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(3))); + + auto future_cancel_all = action_client->async_cancel_all_goals(); + dual_spin_until_future_complete(future_cancel_all); + auto cancel_response = future_cancel_all.get(); + + EXPECT_EQ(ActionCancelGoalResponse::ERROR_NONE, cancel_response->return_code); + ASSERT_EQ(2ul, cancel_response->goals_canceling.size()); + EXPECT_EQ(goal_handle0->get_goal_id(), cancel_response->goals_canceling[0].goal_id.uuid); + EXPECT_EQ(goal_handle1->get_goal_id(), cancel_response->goals_canceling[1].goal_id.uuid); + EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle0->get_status()); + EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle1->get_status()); +} + +TEST_F(TestClientAgainstServer, async_cancel_all_goals_with_callback) +{ + auto action_client = rclcpp_action::create_client(client_node, action_name); + ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + ActionGoal goal; + goal.order = 6; + auto future_goal_handle0 = action_client->async_send_goal(goal); + dual_spin_until_future_complete(future_goal_handle0); + auto goal_handle0 = future_goal_handle0.get(); + + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(2))); + + goal.order = 8; + auto future_goal_handle1 = action_client->async_send_goal(goal); + dual_spin_until_future_complete(future_goal_handle1); + auto goal_handle1 = future_goal_handle1.get(); + + if (goal_handle1->get_goal_id() < goal_handle0->get_goal_id()) { + goal_handle0.swap(goal_handle1); + } + + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(3))); + + bool cancel_callback_received = false; + auto future_cancel_all = action_client->async_cancel_all_goals( + [&cancel_callback_received, goal_handle0, goal_handle1]( + ActionCancelGoalResponse::SharedPtr response) + { + if ( + response && + 2ul == response->goals_canceling.size() && + goal_handle0->get_goal_id() == response->goals_canceling[0].goal_id.uuid && + goal_handle1->get_goal_id() == response->goals_canceling[1].goal_id.uuid) + { + cancel_callback_received = true; + } + }); + dual_spin_until_future_complete(future_cancel_all); + auto cancel_response = future_cancel_all.get(); + + EXPECT_EQ(ActionCancelGoalResponse::ERROR_NONE, cancel_response->return_code); + EXPECT_TRUE(cancel_callback_received); + ASSERT_EQ(2ul, cancel_response->goals_canceling.size()); + EXPECT_EQ(goal_handle0->get_goal_id(), cancel_response->goals_canceling[0].goal_id.uuid); + EXPECT_EQ(goal_handle1->get_goal_id(), cancel_response->goals_canceling[1].goal_id.uuid); + EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle0->get_status()); + EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle1->get_status()); +} + +TEST_F(TestClientAgainstServer, async_cancel_some_goals) +{ + auto action_client = rclcpp_action::create_client(client_node, action_name); + ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + ActionGoal goal; + goal.order = 6; + auto future_goal_handle0 = action_client->async_send_goal(goal); + dual_spin_until_future_complete(future_goal_handle0); + auto goal_handle0 = future_goal_handle0.get(); + + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(2))); + + goal.order = 8; + auto future_goal_handle1 = action_client->async_send_goal(goal); + dual_spin_until_future_complete(future_goal_handle1); + auto goal_handle1 = future_goal_handle1.get(); + + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(3))); + + auto future_cancel_some = + action_client->async_cancel_goals_before(goal_handle1->get_goal_stamp()); + dual_spin_until_future_complete(future_cancel_some); + auto cancel_response = future_cancel_some.get(); + + EXPECT_EQ(ActionCancelGoalResponse::ERROR_NONE, cancel_response->return_code); + ASSERT_EQ(1ul, cancel_response->goals_canceling.size()); + EXPECT_EQ(goal_handle0->get_goal_id(), cancel_response->goals_canceling[0].goal_id.uuid); + EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle0->get_status()); +} + +TEST_F(TestClientAgainstServer, async_cancel_some_goals_with_callback) +{ + auto action_client = rclcpp_action::create_client(client_node, action_name); + ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + ActionGoal goal; + goal.order = 6; + auto future_goal_handle0 = action_client->async_send_goal(goal); + dual_spin_until_future_complete(future_goal_handle0); + auto goal_handle0 = future_goal_handle0.get(); + + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(2))); + + goal.order = 8; + auto future_goal_handle1 = action_client->async_send_goal(goal); + dual_spin_until_future_complete(future_goal_handle1); + auto goal_handle1 = future_goal_handle1.get(); + + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(3))); + + bool cancel_callback_received = false; + auto future_cancel_some = action_client->async_cancel_goals_before( + goal_handle1->get_goal_stamp(), + [&cancel_callback_received, goal_handle0](ActionCancelGoalResponse::SharedPtr response) + { + if ( + response && + 1ul == response->goals_canceling.size() && + goal_handle0->get_goal_id() == response->goals_canceling[0].goal_id.uuid) + { + cancel_callback_received = true; + } + }); + dual_spin_until_future_complete(future_cancel_some); + auto cancel_response = future_cancel_some.get(); + + EXPECT_EQ(ActionCancelGoalResponse::ERROR_NONE, cancel_response->return_code); + EXPECT_TRUE(cancel_callback_received); + ASSERT_EQ(1ul, cancel_response->goals_canceling.size()); + EXPECT_EQ(goal_handle0->get_goal_id(), cancel_response->goals_canceling[0].goal_id.uuid); + EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle0->get_status()); +} + +TEST_F(TestClientAgainstServer, deadlock_in_callbacks) +{ + std::atomic feedback_callback_called = false; + std::atomic response_callback_called = false; + std::atomic result_callback_called = false; + std::atomic no_deadlock = false; + + std::thread tr = std::thread( + [&]() { + auto action_client = rclcpp_action::create_client(client_node, action_name); + ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + ActionGoal goal; + + using GoalHandle = rclcpp_action::ClientGoalHandle; + rclcpp_action::Client::SendGoalOptions ops; + ops.feedback_callback = [&feedback_callback_called]( + const GoalHandle::SharedPtr handle, ActionType::Feedback::ConstSharedPtr) + { + // call functions on the handle that acquire the lock + handle->get_status(); + handle->is_feedback_aware(); + handle->is_result_aware(); + + feedback_callback_called = true; + }; + ops.goal_response_callback = [&response_callback_called]( + const GoalHandle::SharedPtr & handle) { + // call functions on the handle that acquire the lock + handle->get_status(); + handle->is_feedback_aware(); + handle->is_result_aware(); + + response_callback_called = true; + }; + ops.result_callback = [&result_callback_called]( + const GoalHandle::WrappedResult &) { + result_callback_called = true; + }; + + goal.order = 6; + auto future_goal_handle = action_client->async_send_goal(goal, ops); + dual_spin_until_future_complete(future_goal_handle); + auto goal_handle = future_goal_handle.get(); + + ASSERT_TRUE(goal_handle); + + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(2))); + + auto result_future = action_client->async_get_result(goal_handle); + dual_spin_until_future_complete(result_future); + + EXPECT_TRUE(result_future.valid()); + auto result = result_future.get(); + + no_deadlock = true; + }); + + auto start_time = std::chrono::system_clock::now(); + + while (std::chrono::system_clock::now() - start_time < std::chrono::milliseconds(2000) && + !no_deadlock) + { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + if (no_deadlock) { + tr.join(); + } else { + // In case of a failure, the thread is assumed to be in a deadlock. + // We detach the thread so we don't block further tests. + tr.detach(); + } + + EXPECT_TRUE(no_deadlock); + EXPECT_TRUE(response_callback_called); + EXPECT_TRUE(result_callback_called); + EXPECT_TRUE(feedback_callback_called); +} + +TEST_F(TestClientAgainstServer, send_rcl_errors) +{ + auto action_client = rclcpp_action::create_client(client_node, action_name); + ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + auto send_goal_ops = rclcpp_action::Client::SendGoalOptions(); + send_goal_ops.result_callback = + [](const typename ActionGoalHandle::WrappedResult &) {}; + send_goal_ops.feedback_callback = []( + typename ActionGoalHandle::SharedPtr, const std::shared_ptr) {}; + + { + ActionGoal goal; + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", rcl_action_send_goal_request, RCL_RET_ERROR); + EXPECT_THROW( + action_client->async_send_goal(goal, send_goal_ops), + rclcpp::exceptions::RCLError); + } + { + ActionGoal goal; + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", rcl_action_send_result_request, RCL_RET_ERROR); + auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops); + dual_spin_until_future_complete(future_goal_handle); + auto goal_handle = future_goal_handle.get(); + EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_UNKNOWN, goal_handle->get_status()); + } + { + ActionGoal goal; + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", rcl_action_send_cancel_request, RCL_RET_ERROR); + auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops); + dual_spin_until_future_complete(future_goal_handle); + auto goal_handle = future_goal_handle.get(); + EXPECT_THROW( + action_client->async_cancel_goals_before(goal_handle->get_goal_stamp()), + rclcpp::exceptions::RCLError); + } +} + +TEST_F(TestClientAgainstServer, execute_rcl_errors) +{ + auto action_client = rclcpp_action::create_client(client_node, action_name); + ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT)); + + auto send_goal_ops = rclcpp_action::Client::SendGoalOptions(); + send_goal_ops.result_callback = + [](const typename ActionGoalHandle::WrappedResult &) {}; + send_goal_ops.feedback_callback = []( + typename ActionGoalHandle::SharedPtr, const std::shared_ptr) {}; + + { + ActionGoal goal; + goal.order = 5; + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", rcl_action_take_feedback, RCL_RET_ERROR); + + auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops); + dual_spin_until_future_complete(future_goal_handle); + auto goal_handle = future_goal_handle.get(); + auto future_result = action_client->async_get_result(goal_handle); + EXPECT_THROW( + dual_spin_until_future_complete(future_result), + rclcpp::exceptions::RCLError); + } + { + ActionGoal goal; + goal.order = 5; + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", rcl_action_take_goal_response, RCL_RET_ERROR); + + auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops); + EXPECT_THROW( + dual_spin_until_future_complete(future_goal_handle), + rclcpp::exceptions::RCLError); + } + { + ActionGoal goal; + goal.order = 5; + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", rcl_action_take_result_response, RCL_RET_ERROR); + + auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops); + dual_spin_until_future_complete(future_goal_handle); + auto goal_handle = future_goal_handle.get(); + auto future_result = action_client->async_get_result(goal_handle); + EXPECT_THROW( + dual_spin_until_future_complete(future_result), + rclcpp::exceptions::RCLError); + } + { + ActionGoal goal; + goal.order = 5; + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", rcl_action_take_cancel_response, RCL_RET_ERROR); + + auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops); + dual_spin_until_future_complete(future_goal_handle); + auto goal_handle = future_goal_handle.get(); + auto future_cancel_some = + action_client->async_cancel_goals_before(goal_handle->get_goal_stamp()); + EXPECT_THROW( + dual_spin_until_future_complete(future_cancel_some), + rclcpp::exceptions::RCLError); + } +} diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp new file mode 100644 index 0000000000..c63d1dfdfc --- /dev/null +++ b/rclcpp_action/test/test_server.cpp @@ -0,0 +1,1344 @@ +// 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 "gtest/gtest.h" + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rcpputils/scope_exit.hpp" +#include "test_msgs/action/fibonacci.hpp" + +#include "rcl_action/action_server.h" +#include "rcl_action/wait.h" +#include "rclcpp_action/create_server.hpp" +#include "rclcpp_action/server.hpp" +#include "mocking_utils/patch.hpp" + +using Fibonacci = test_msgs::action::Fibonacci; +using CancelResponse = typename Fibonacci::Impl::CancelGoalService::Response; +using GoalUUID = rclcpp_action::GoalUUID; + +class TestServer : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + std::shared_ptr + send_goal_request( + rclcpp::Node::SharedPtr node, GoalUUID uuid, + std::chrono::milliseconds timeout = std::chrono::milliseconds(-1)) + { + auto client = node->create_client( + "fibonacci/_action/send_goal"); + if (!client->wait_for_service(std::chrono::seconds(20))) { + throw std::runtime_error("send goal service didn't become available"); + } + auto request = std::make_shared(); + request->goal_id.uuid = uuid; + auto future = client->async_send_request(request); + auto return_code = rclcpp::spin_until_future_complete(node, future, timeout); + if (rclcpp::FutureReturnCode::SUCCESS == return_code) { + return request; + } else if (rclcpp::FutureReturnCode::TIMEOUT == return_code) { + throw std::runtime_error("send goal future timed out"); + } else { + throw std::runtime_error("send goal future didn't complete succesfully"); + } + } + + CancelResponse::SharedPtr + send_cancel_request( + rclcpp::Node::SharedPtr node, GoalUUID uuid, + std::chrono::milliseconds timeout = std::chrono::milliseconds(-1)) + { + auto cancel_client = node->create_client( + "fibonacci/_action/cancel_goal"); + if (!cancel_client->wait_for_service(std::chrono::seconds(20))) { + throw std::runtime_error("cancel goal service didn't become available"); + } + auto request = std::make_shared(); + request->goal_info.goal_id.uuid = uuid; + auto future = cancel_client->async_send_request(request); + auto return_code = rclcpp::spin_until_future_complete(node, future, timeout); + if (rclcpp::FutureReturnCode::SUCCESS == return_code) { + return future.get(); + } else if (rclcpp::FutureReturnCode::TIMEOUT == return_code) { + throw std::runtime_error("cancel request future timed out"); + } else { + throw std::runtime_error("cancel request future didn't complete succesfully"); + } + } +}; + +TEST_F(TestServer, construction_and_destruction) +{ + auto node = std::make_shared("construct_node", "/rclcpp_action/construct"); + + ASSERT_NO_THROW( + { + using GoalHandle = rclcpp_action::ServerGoalHandle; + auto as = rclcpp_action::create_server( + node, "fibonacci", + [](const GoalUUID &, std::shared_ptr) { + return rclcpp_action::GoalResponse::REJECT; + }, + [](std::shared_ptr) { + return rclcpp_action::CancelResponse::REJECT; + }, + [](std::shared_ptr) {}); + (void)as; + }); +} + +TEST_F(TestServer, construction_and_destruction_after_node) +{ + auto node = std::make_shared("construct_node", "/rclcpp_action/construct"); + + ASSERT_NO_THROW( + { + using GoalHandle = rclcpp_action::ServerGoalHandle; + auto as = rclcpp_action::create_server( + node, "fibonacci", + [](const GoalUUID &, std::shared_ptr) { + return rclcpp_action::GoalResponse::REJECT; + }, + [](std::shared_ptr) { + return rclcpp_action::CancelResponse::REJECT; + }, + [](std::shared_ptr) {}); + (void)as; + + node.reset(); + }); +} + +TEST_F(TestServer, construction_and_destruction_callback_group) +{ + auto node = std::make_shared("construct_node", "/rclcpp_action/construct"); + auto group = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + const rcl_action_server_options_t & options = rcl_action_server_get_default_options(); + + using GoalHandle = rclcpp_action::ServerGoalHandle; + ASSERT_NO_THROW( + rclcpp_action::create_server( + node->get_node_base_interface(), + node->get_node_clock_interface(), + node->get_node_logging_interface(), + node->get_node_waitables_interface(), + "fibonacci", + [](const GoalUUID &, std::shared_ptr) { + return rclcpp_action::GoalResponse::REJECT; + }, + [](std::shared_ptr) { + return rclcpp_action::CancelResponse::REJECT; + }, + [](std::shared_ptr) {}, + options, + group)); +} + +TEST_F(TestServer, construction_and_destruction_server_init_error) +{ + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", rcl_action_server_init, RCL_RET_ERROR); + auto node = std::make_shared("construct_node", "/rclcpp_action/construct"); + + EXPECT_THROW( + { + using GoalHandle = rclcpp_action::ServerGoalHandle; + auto as = rclcpp_action::create_server( + node, "fibonacci", + [](const GoalUUID &, std::shared_ptr) { + return rclcpp_action::GoalResponse::REJECT; + }, + [](std::shared_ptr) { + return rclcpp_action::CancelResponse::REJECT; + }, + [](std::shared_ptr) {}); + (void)as; + }, rclcpp::exceptions::RCLError); +} + +TEST_F(TestServer, construction_and_destruction_wait_set_error) +{ + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", rcl_action_server_wait_set_get_num_entities, RCL_RET_ERROR); + auto node = std::make_shared("construct_node", "/rclcpp_action/construct"); + + EXPECT_THROW( + { + using GoalHandle = rclcpp_action::ServerGoalHandle; + auto as = rclcpp_action::create_server( + node, "fibonacci", + [](const GoalUUID &, std::shared_ptr) { + return rclcpp_action::GoalResponse::REJECT; + }, + [](std::shared_ptr) { + return rclcpp_action::CancelResponse::REJECT; + }, + [](std::shared_ptr) {}); + (void)as; + }, rclcpp::exceptions::RCLError); +} + +TEST_F(TestServer, construction_and_destruction_sub_node) +{ + auto parent_node = std::make_shared("construct_node", "/rclcpp_action/construct"); + auto sub_node = parent_node->create_sub_node("construct_sub_node"); + + ASSERT_NO_THROW( + { + using GoalHandle = rclcpp_action::ServerGoalHandle; + auto as = rclcpp_action::create_server( + sub_node, "fibonacci", + [](const GoalUUID &, std::shared_ptr) { + return rclcpp_action::GoalResponse::REJECT; + }, + [](std::shared_ptr) { + return rclcpp_action::CancelResponse::REJECT; + }, + [](std::shared_ptr) {}); + (void)as; + }); +} + +TEST_F(TestServer, handle_goal_called) +{ + auto node = std::make_shared("handle_goal_node", "/rclcpp_action/handle_goal"); + GoalUUID received_uuid; + + auto handle_goal = [&received_uuid]( + const GoalUUID & uuid, std::shared_ptr) + { + received_uuid = uuid; + return rclcpp_action::GoalResponse::REJECT; + }; + + using GoalHandle = rclcpp_action::ServerGoalHandle; + auto as = rclcpp_action::create_server( + node, "fibonacci", + handle_goal, + [](std::shared_ptr) { + return rclcpp_action::CancelResponse::REJECT; + }, + [](std::shared_ptr) {}); + (void)as; + + // Create a client that calls the goal request service + // Make sure the UUID received is the same as the one sent + + auto client = node->create_client( + "fibonacci/_action/send_goal"); + + ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(20))); + + auto request = std::make_shared(); + + const GoalUUID uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}}; + request->goal_id.uuid = uuid; + + auto future = client->async_send_request(request); + ASSERT_EQ( + rclcpp::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future)); + + ASSERT_EQ(uuid, received_uuid); +} + +TEST_F(TestServer, handle_accepted_called) +{ + auto node = std::make_shared("handle_exec_node", "/rclcpp_action/handle_accepted"); + const GoalUUID uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}}; + + auto handle_goal = []( + const GoalUUID &, std::shared_ptr) + { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; + + using GoalHandle = rclcpp_action::ServerGoalHandle; + + std::shared_ptr received_handle; + auto handle_accepted = [&received_handle](std::shared_ptr handle) + { + received_handle = handle; + }; + + auto as = rclcpp_action::create_server( + node, "fibonacci", + handle_goal, + [](std::shared_ptr) { + return rclcpp_action::CancelResponse::REJECT; + }, + handle_accepted); + (void)as; + + auto request = send_goal_request(node, uuid); + + ASSERT_TRUE(received_handle); + ASSERT_TRUE(received_handle->is_active()); + EXPECT_EQ(uuid, received_handle->get_goal_id()); + EXPECT_EQ(request->goal, *(received_handle->get_goal())); +} + +TEST_F(TestServer, handle_cancel_called) +{ + auto node = std::make_shared("handle_cancel_node", "/rclcpp_action/handle_cancel"); + const GoalUUID uuid{{10, 20, 30, 40, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}}; + + auto handle_goal = []( + const GoalUUID &, std::shared_ptr) + { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; + + using GoalHandle = rclcpp_action::ServerGoalHandle; + + auto handle_cancel = [](std::shared_ptr) + { + return rclcpp_action::CancelResponse::ACCEPT; + }; + + std::shared_ptr received_handle; + auto handle_accepted = [&received_handle](std::shared_ptr handle) + { + received_handle = handle; + }; + + auto as = rclcpp_action::create_server( + node, "fibonacci", + handle_goal, + handle_cancel, + handle_accepted); + (void)as; + + send_goal_request(node, uuid); + + ASSERT_TRUE(received_handle); + EXPECT_EQ(uuid, received_handle->get_goal_id()); + EXPECT_FALSE(received_handle->is_canceling()); + + send_cancel_request(node, uuid); + EXPECT_TRUE(received_handle->is_canceling()); +} + +TEST_F(TestServer, handle_cancel_reject) +{ + auto node = std::make_shared("handle_cancel_node", "/rclcpp_action/handle_cancel"); + const GoalUUID uuid{{10, 20, 30, 40, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}}; + + auto handle_goal = []( + const GoalUUID &, std::shared_ptr) + { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; + + using GoalHandle = rclcpp_action::ServerGoalHandle; + + auto handle_cancel = [](std::shared_ptr) + { + return rclcpp_action::CancelResponse::REJECT; + }; + + std::shared_ptr received_handle; + auto handle_accepted = [&received_handle](std::shared_ptr handle) + { + received_handle = handle; + }; + + auto as = rclcpp_action::create_server( + node, "fibonacci", + handle_goal, + handle_cancel, + handle_accepted); + (void)as; + + send_goal_request(node, uuid); + + ASSERT_TRUE(received_handle); + EXPECT_EQ(uuid, received_handle->get_goal_id()); + EXPECT_FALSE(received_handle->is_canceling()); + + auto response_ptr = send_cancel_request(node, uuid); + EXPECT_FALSE(received_handle->is_canceling()); + EXPECT_EQ(CancelResponse::ERROR_REJECTED, response_ptr->return_code); +} + +TEST_F(TestServer, handle_cancel_unknown_goal) +{ + auto node = std::make_shared("handle_cancel_node", "/rclcpp_action/handle_cancel"); + const GoalUUID uuid{{10, 20, 30, 40, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}}; + const GoalUUID unknown_uuid{{11, 22, 33, 44, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17}}; + + auto handle_goal = []( + const GoalUUID &, std::shared_ptr) + { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; + + using GoalHandle = rclcpp_action::ServerGoalHandle; + + auto handle_cancel = [](std::shared_ptr) + { + return rclcpp_action::CancelResponse::ACCEPT; + }; + + std::shared_ptr received_handle; + auto handle_accepted = [&received_handle](std::shared_ptr handle) + { + received_handle = handle; + }; + + auto as = rclcpp_action::create_server( + node, "fibonacci", + handle_goal, + handle_cancel, + handle_accepted); + (void)as; + + send_goal_request(node, uuid); + + ASSERT_TRUE(received_handle); + EXPECT_EQ(uuid, received_handle->get_goal_id()); + EXPECT_FALSE(received_handle->is_canceling()); + + auto response_ptr = send_cancel_request(node, unknown_uuid); + EXPECT_FALSE(received_handle->is_canceling()); + EXPECT_EQ(CancelResponse::ERROR_UNKNOWN_GOAL_ID, response_ptr->return_code); +} + +TEST_F(TestServer, handle_cancel_terminated_goal) +{ + auto node = std::make_shared("handle_cancel_node", "/rclcpp_action/handle_cancel"); + const GoalUUID uuid{{10, 20, 30, 40, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}}; + + auto handle_goal = []( + const GoalUUID &, std::shared_ptr) + { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; + + using GoalHandle = rclcpp_action::ServerGoalHandle; + + auto handle_cancel = [](std::shared_ptr) + { + return rclcpp_action::CancelResponse::ACCEPT; + }; + + std::shared_ptr received_handle; + auto handle_accepted = [&received_handle](std::shared_ptr handle) + { + received_handle = handle; + handle->succeed(std::make_shared()); + }; + + auto as = rclcpp_action::create_server( + node, "fibonacci", + handle_goal, + handle_cancel, + handle_accepted); + (void)as; + + send_goal_request(node, uuid); + + ASSERT_TRUE(received_handle); + EXPECT_EQ(uuid, received_handle->get_goal_id()); + EXPECT_FALSE(received_handle->is_canceling()); + + auto response_ptr = send_cancel_request(node, uuid); + EXPECT_FALSE(received_handle->is_canceling()); + EXPECT_EQ(CancelResponse::ERROR_GOAL_TERMINATED, response_ptr->return_code); +} + +TEST_F(TestServer, publish_status_accepted) +{ + auto node = std::make_shared("status_accept_node", "/rclcpp_action/status_accept"); + const GoalUUID uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 100, 110, 120, 13, 14, 15, 16}}; + + auto handle_goal = []( + const GoalUUID &, std::shared_ptr) + { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; + + using GoalHandle = rclcpp_action::ServerGoalHandle; + + auto handle_cancel = [](std::shared_ptr) + { + return rclcpp_action::CancelResponse::REJECT; + }; + + std::shared_ptr received_handle; + auto handle_accepted = [&received_handle](std::shared_ptr handle) + { + received_handle = handle; + }; + + auto as = rclcpp_action::create_server( + node, "fibonacci", + handle_goal, + handle_cancel, + handle_accepted); + (void)as; + + // Subscribe to status messages + std::vector received_msgs; + auto subscriber = node->create_subscription( + "fibonacci/_action/status", 10, + [&received_msgs](action_msgs::msg::GoalStatusArray::ConstSharedPtr list) + { + received_msgs.push_back(list); + }); + + send_goal_request(node, uuid); + + // 10 seconds + const size_t max_tries = 10 * 1000 / 100; + for (size_t retry = 0; retry < max_tries && received_msgs.size() != 1u; ++retry) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + rclcpp::spin_some(node); + } + + ASSERT_LT(0u, received_msgs.size()); + // Not sure whether accepted will come through because not sure when subscriber will match + for (auto & msg : received_msgs) { + ASSERT_EQ(1u, msg->status_list.size()); + EXPECT_EQ(uuid, msg->status_list.at(0).goal_info.goal_id.uuid); + auto status = msg->status_list.at(0).status; + if (action_msgs::msg::GoalStatus::STATUS_ACCEPTED == status) { + EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_ACCEPTED, status); + } else { + EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_EXECUTING, status); + } + } +} + +TEST_F(TestServer, publish_status_canceling) +{ + auto node = std::make_shared("status_cancel_node", "/rclcpp_action/status_cancel"); + const GoalUUID uuid{{1, 2, 3, 40, 5, 6, 7, 80, 9, 10, 11, 120, 13, 14, 15, 160}}; + + auto handle_goal = []( + const GoalUUID &, std::shared_ptr) + { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; + + using GoalHandle = rclcpp_action::ServerGoalHandle; + + auto handle_cancel = [](std::shared_ptr) + { + return rclcpp_action::CancelResponse::ACCEPT; + }; + + std::shared_ptr received_handle; + auto handle_accepted = [&received_handle](std::shared_ptr handle) + { + received_handle = handle; + }; + + auto as = rclcpp_action::create_server( + node, "fibonacci", + handle_goal, + handle_cancel, + handle_accepted); + (void)as; + + // Subscribe to status messages + std::vector received_msgs; + auto subscriber = node->create_subscription( + "fibonacci/_action/status", 10, + [&received_msgs](action_msgs::msg::GoalStatusArray::ConstSharedPtr list) + { + received_msgs.push_back(list); + }); + + send_goal_request(node, uuid); + send_cancel_request(node, uuid); + + // 10 seconds + const size_t max_tries = 10 * 1000 / 100; + for (size_t retry = 0; retry < max_tries && received_msgs.size() < 2u; ++retry) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + rclcpp::spin_some(node); + } + + ASSERT_LT(0u, received_msgs.size()); + auto & msg = received_msgs.back(); + ASSERT_EQ(1u, msg->status_list.size()); + EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_CANCELING, msg->status_list.at(0).status); + EXPECT_EQ(uuid, msg->status_list.at(0).goal_info.goal_id.uuid); +} + +TEST_F(TestServer, publish_status_canceled) +{ + auto node = std::make_shared("status_canceled", "/rclcpp_action/status_canceled"); + const GoalUUID uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}}; + + auto handle_goal = []( + const GoalUUID &, std::shared_ptr) + { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; + + using GoalHandle = rclcpp_action::ServerGoalHandle; + + auto handle_cancel = [](std::shared_ptr) + { + return rclcpp_action::CancelResponse::ACCEPT; + }; + + std::shared_ptr received_handle; + auto handle_accepted = [&received_handle](std::shared_ptr handle) + { + received_handle = handle; + }; + + auto as = rclcpp_action::create_server( + node, "fibonacci", + handle_goal, + handle_cancel, + handle_accepted); + (void)as; + + // Subscribe to status messages + std::vector received_msgs; + auto subscriber = node->create_subscription( + "fibonacci/_action/status", 10, + [&received_msgs](action_msgs::msg::GoalStatusArray::ConstSharedPtr list) + { + received_msgs.push_back(list); + }); + + send_goal_request(node, uuid); + send_cancel_request(node, uuid); + + received_handle->canceled(std::make_shared()); + + // 10 seconds + const size_t max_tries = 10 * 1000 / 100; + for (size_t retry = 0; retry < max_tries && received_msgs.size() < 3u; ++retry) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + rclcpp::spin_some(node); + } + + ASSERT_LT(0u, received_msgs.size()); + auto & msg = received_msgs.back(); + ASSERT_EQ(1u, msg->status_list.size()); + EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_CANCELED, msg->status_list.at(0).status); + EXPECT_EQ(uuid, msg->status_list.at(0).goal_info.goal_id.uuid); +} + +TEST_F(TestServer, publish_status_succeeded) +{ + auto node = std::make_shared("status_succeeded", "/rclcpp_action/status_succeeded"); + const GoalUUID uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}}; + + auto handle_goal = []( + const GoalUUID &, std::shared_ptr) + { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; + + using GoalHandle = rclcpp_action::ServerGoalHandle; + + auto handle_cancel = [](std::shared_ptr) + { + return rclcpp_action::CancelResponse::REJECT; + }; + + std::shared_ptr received_handle; + auto handle_accepted = [&received_handle](std::shared_ptr handle) + { + received_handle = handle; + }; + + auto as = rclcpp_action::create_server( + node, "fibonacci", + handle_goal, + handle_cancel, + handle_accepted); + (void)as; + + // Subscribe to status messages + std::vector received_msgs; + auto subscriber = node->create_subscription( + "fibonacci/_action/status", 10, + [&received_msgs](action_msgs::msg::GoalStatusArray::ConstSharedPtr list) + { + received_msgs.push_back(list); + }); + + send_goal_request(node, uuid); + received_handle->succeed(std::make_shared()); + + // 10 seconds + const size_t max_tries = 10 * 1000 / 100; + for (size_t retry = 0; retry < max_tries && received_msgs.size() < 2u; ++retry) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + rclcpp::spin_some(node); + } + + ASSERT_LT(0u, received_msgs.size()); + auto & msg = received_msgs.back(); + ASSERT_EQ(1u, msg->status_list.size()); + EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_SUCCEEDED, msg->status_list.at(0).status); + EXPECT_EQ(uuid, msg->status_list.at(0).goal_info.goal_id.uuid); +} + +TEST_F(TestServer, publish_status_aborted) +{ + auto node = std::make_shared("status_aborted", "/rclcpp_action/status_aborted"); + const GoalUUID uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}}; + + auto handle_goal = []( + const GoalUUID &, std::shared_ptr) + { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; + + using GoalHandle = rclcpp_action::ServerGoalHandle; + + auto handle_cancel = [](std::shared_ptr) + { + return rclcpp_action::CancelResponse::REJECT; + }; + + std::shared_ptr received_handle; + auto handle_accepted = [&received_handle](std::shared_ptr handle) + { + received_handle = handle; + }; + + auto as = rclcpp_action::create_server( + node, "fibonacci", + handle_goal, + handle_cancel, + handle_accepted); + (void)as; + + // Subscribe to status messages + std::vector received_msgs; + auto subscriber = node->create_subscription( + "fibonacci/_action/status", 10, + [&received_msgs](action_msgs::msg::GoalStatusArray::ConstSharedPtr list) + { + received_msgs.push_back(list); + }); + + send_goal_request(node, uuid); + received_handle->abort(std::make_shared()); + + // 10 seconds + const size_t max_tries = 10 * 1000 / 100; + for (size_t retry = 0; retry < max_tries && received_msgs.size() < 2u; ++retry) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + rclcpp::spin_some(node); + } + + ASSERT_LT(0u, received_msgs.size()); + auto & msg = received_msgs.back(); + ASSERT_EQ(1u, msg->status_list.size()); + EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_ABORTED, msg->status_list.at(0).status); + EXPECT_EQ(uuid, msg->status_list.at(0).goal_info.goal_id.uuid); +} + +TEST_F(TestServer, publish_feedback) +{ + auto node = std::make_shared("pub_feedback", "/rclcpp_action/pub_feedback"); + const GoalUUID uuid{{1, 20, 30, 4, 5, 6, 70, 8, 9, 1, 11, 120, 13, 14, 15, 160}}; + + auto handle_goal = []( + const GoalUUID &, std::shared_ptr) + { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; + + using GoalHandle = rclcpp_action::ServerGoalHandle; + + auto handle_cancel = [](std::shared_ptr) + { + return rclcpp_action::CancelResponse::REJECT; + }; + + std::shared_ptr received_handle; + auto handle_accepted = [&received_handle](std::shared_ptr handle) + { + received_handle = handle; + }; + + auto as = rclcpp_action::create_server( + node, "fibonacci", + handle_goal, + handle_cancel, + handle_accepted); + (void)as; + + // Subscribe to feedback messages + using FeedbackT = Fibonacci::Impl::FeedbackMessage; + std::vector received_msgs; + auto subscriber = node->create_subscription( + "fibonacci/_action/feedback", 10, [&received_msgs](FeedbackT::ConstSharedPtr msg) + { + received_msgs.push_back(msg); + }); + + send_goal_request(node, uuid); + + auto sent_message = std::make_shared(); + sent_message->sequence = {1, 1, 2, 3, 5}; + received_handle->publish_feedback(sent_message); + + // 10 seconds + const size_t max_tries = 10 * 1000 / 100; + for (size_t retry = 0; retry < max_tries && received_msgs.size() < 1u; ++retry) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + rclcpp::spin_some(node); + } + + ASSERT_EQ(1u, received_msgs.size()); + auto & msg = received_msgs.back(); + ASSERT_EQ(sent_message->sequence, msg->feedback.sequence); +} + +TEST_F(TestServer, get_result) +{ + auto node = std::make_shared("get_result", "/rclcpp_action/get_result"); + const GoalUUID uuid{{1, 2, 3, 4, 5, 6, 7, 80, 90, 10, 11, 12, 13, 14, 15, 160}}; + + auto handle_goal = []( + const GoalUUID &, std::shared_ptr) + { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; + + using GoalHandle = rclcpp_action::ServerGoalHandle; + + auto handle_cancel = [](std::shared_ptr) + { + return rclcpp_action::CancelResponse::REJECT; + }; + + std::shared_ptr received_handle; + auto handle_accepted = [&received_handle](std::shared_ptr handle) + { + received_handle = handle; + }; + + const std::chrono::milliseconds result_timeout{50}; + + rcl_action_server_options_t options = rcl_action_server_get_default_options(); + options.result_timeout.nanoseconds = RCL_MS_TO_NS(result_timeout.count()); + auto as = rclcpp_action::create_server( + node, "fibonacci", + handle_goal, + handle_cancel, + handle_accepted, + options); + (void)as; + + send_goal_request(node, uuid); + + // Send result request + auto result_client = node->create_client( + "fibonacci/_action/get_result"); + if (!result_client->wait_for_service(std::chrono::seconds(20))) { + throw std::runtime_error("get result service didn't become available"); + } + auto request = std::make_shared(); + request->goal_id.uuid = uuid; + auto future = result_client->async_send_request(request); + + // Send a result + auto result = std::make_shared(); + result->sequence = {5, 8, 13, 21}; + received_handle->succeed(result); + + // Wait for the result request to be received + ASSERT_EQ( + rclcpp::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future)); + + auto response = future.get(); + EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_SUCCEEDED, response->status); + EXPECT_EQ(result->sequence, response->result.sequence); + + // Wait for goal expiration + rclcpp::sleep_for(2 * result_timeout); + + // Allow for expiration to take place + rclcpp::spin_some(node); + + // Send and wait for another result request + future = result_client->async_send_request(request); + ASSERT_EQ( + rclcpp::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future)); + + response = future.get(); + EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_UNKNOWN, response->status); +} + +TEST_F(TestServer, get_result_deferred) +{ + auto node = std::make_shared("get_result", "/rclcpp_action/get_result"); + const GoalUUID uuid{{1, 2, 3, 4, 5, 6, 7, 80, 90, 10, 11, 12, 13, 14, 15, 160}}; + + auto handle_goal = []( + const GoalUUID &, std::shared_ptr) + { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; + + using GoalHandle = rclcpp_action::ServerGoalHandle; + + auto handle_cancel = [](std::shared_ptr) + { + return rclcpp_action::CancelResponse::REJECT; + }; + + std::shared_ptr received_handle; + auto handle_accepted = [&received_handle](std::shared_ptr handle) + { + received_handle = handle; + }; + + auto as = rclcpp_action::create_server( + node, "fibonacci", + handle_goal, + handle_cancel, + handle_accepted); + (void)as; + + send_goal_request(node, uuid); + + // Send result request + auto result_client = node->create_client( + "fibonacci/_action/get_result"); + if (!result_client->wait_for_service(std::chrono::seconds(20))) { + throw std::runtime_error("get result service didn't become available"); + } + auto request = std::make_shared(); + request->goal_id.uuid = uuid; + auto future = result_client->async_send_request(request); + + // Process request first + rclcpp::sleep_for(std::chrono::milliseconds(10)); // Give a chance for the request to be served + rclcpp::spin_some(node); + + // Send a result + auto result = std::make_shared(); + result->sequence = {5, 8, 13, 21}; + received_handle->succeed(result); + + // Wait for the result request to be received + ASSERT_EQ( + rclcpp::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, future)); + + auto response = future.get(); + EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_SUCCEEDED, response->status); + EXPECT_EQ(result->sequence, response->result.sequence); +} + +TEST_F(TestServer, deferred_execution) +{ + auto node = std::make_shared("defer_exec", "/rclcpp_action/defer_exec"); + const GoalUUID uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}}; + + auto handle_goal = []( + const GoalUUID &, std::shared_ptr) + { + return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER; + }; + + using GoalHandle = rclcpp_action::ServerGoalHandle; + + auto handle_cancel = [](std::shared_ptr) + { + return rclcpp_action::CancelResponse::REJECT; + }; + + std::shared_ptr received_handle; + auto handle_accepted = [&received_handle](std::shared_ptr handle) + { + received_handle = handle; + }; + + auto as = rclcpp_action::create_server( + node, "fibonacci", + handle_goal, + handle_cancel, + handle_accepted); + (void)as; + + send_goal_request(node, uuid); + + EXPECT_TRUE(received_handle->is_active()); + EXPECT_FALSE(received_handle->is_executing()); + received_handle->execute(); + EXPECT_TRUE(received_handle->is_executing()); +} + +class TestBasicServer : public TestServer +{ +public: + void SetUp() + { + node_ = std::make_shared("goal_request", "/rclcpp_action/goal_request"); + uuid_ = {{1, 2, 3, 4, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}}; + action_server_ = rclcpp_action::create_server( + node_, "fibonacci", + [](const GoalUUID &, std::shared_ptr) { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }, + [](std::shared_ptr) { + return rclcpp_action::CancelResponse::ACCEPT; + }, + [this](std::shared_ptr handle) { + goal_handle_ = handle; + }); + } + + void SendClientGoalRequest( + std::chrono::milliseconds timeout = std::chrono::milliseconds(-1)) + { + send_goal_request(node_, uuid_, timeout); + auto result_client = node_->create_client( + "fibonacci/_action/get_result"); + if (!result_client->wait_for_service(std::chrono::seconds(20))) { + throw std::runtime_error("get result service didn't become available"); + } + auto request = std::make_shared(); + request->goal_id.uuid = uuid_; + auto future = result_client->async_send_request(request); + + // Send a result + auto result = std::make_shared(); + result->sequence = {5, 8, 13, 21}; + goal_handle_->succeed(result); + + // Wait for the result request to be received + ASSERT_EQ( + rclcpp::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node_, future)); + + auto response = future.get(); + EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_SUCCEEDED, response->status); + EXPECT_EQ(result->sequence, response->result.sequence); + + // Wait for goal expiration + rclcpp::sleep_for(std::chrono::milliseconds(100)); + + // Allow for expiration to take place + rclcpp::spin_some(node_); + + // Send and wait for another result request + future = result_client->async_send_request(request); + ASSERT_EQ( + rclcpp::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node_, future)); + } + +protected: + GoalUUID uuid_; + std::shared_ptr node_; + std::shared_ptr> action_server_; + + using GoalHandle = rclcpp_action::ServerGoalHandle; + std::shared_ptr goal_handle_; +}; + +class TestGoalRequestServer : public TestBasicServer {}; + +TEST_F(TestGoalRequestServer, execute_goal_request_received_take_goal) +{ + EXPECT_NO_THROW(SendClientGoalRequest()); +} + +class TestCancelRequestServer : public TestBasicServer +{ +public: + void SendClientCancelRequest( + std::chrono::milliseconds timeout = std::chrono::milliseconds(-1)) + { + send_goal_request(node_, uuid_, timeout); + send_cancel_request(node_, uuid_, timeout); + } +}; + +TEST_F(TestCancelRequestServer, execute_goal_request_received_take_goal) +{ + EXPECT_NO_THROW(SendClientCancelRequest()); +} + +TEST_F(TestGoalRequestServer, is_ready_rcl_error) { + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + auto rcl_context = node_->get_node_base_interface()->get_context()->get_rcl_context().get(); + ASSERT_EQ( + RCL_RET_OK, + rcl_wait_set_init(&wait_set, 10, 10, 10, 10, 10, 10, rcl_context, allocator)); + RCPPUTILS_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)); + }); + EXPECT_NO_THROW(action_server_->add_to_wait_set(wait_set)); + + EXPECT_TRUE(action_server_->is_ready(wait_set)); + + EXPECT_NO_THROW(action_server_->take_data()); + + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", rcl_action_server_wait_set_get_entities_ready, RCL_RET_ERROR); + EXPECT_THROW(action_server_->is_ready(wait_set), rclcpp::exceptions::RCLError); +} + +TEST_F(TestGoalRequestServer, execute_goal_request_received_take_goal_request_errors) +{ + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", rcl_action_take_goal_request, RCL_RET_ERROR); + + EXPECT_THROW(SendClientGoalRequest(), rclcpp::exceptions::RCLError); +} + +TEST_F(TestGoalRequestServer, execute_goal_request_received_send_goal_response_errors) +{ + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", rcl_action_send_goal_response, RCL_RET_ERROR); + + EXPECT_THROW(SendClientGoalRequest(), rclcpp::exceptions::RCLError); +} + +TEST_F(TestGoalRequestServer, execute_goal_request_received_accept_new_goal_errors) +{ + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", rcl_action_accept_new_goal, nullptr); + + EXPECT_THROW(SendClientGoalRequest(), std::runtime_error); +} + +TEST_F(TestGoalRequestServer, execute_goal_request_received_update_goal_state_errors) +{ + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", rcl_action_update_goal_state, RCL_RET_ERROR); + + EXPECT_THROW(SendClientGoalRequest(), rclcpp::exceptions::RCLError); +} + +TEST_F(TestGoalRequestServer, publish_status_server_get_goal_handles_errors) +{ + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", rcl_action_server_get_goal_handles, RCL_RET_ERROR); + + EXPECT_THROW(SendClientGoalRequest(), rclcpp::exceptions::RCLError); +} + +TEST_F(TestGoalRequestServer, publish_status_get_goal_status_array_errors) +{ + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", rcl_action_get_goal_status_array, RCL_RET_ERROR); + + EXPECT_THROW(SendClientGoalRequest(), rclcpp::exceptions::RCLError); +} + +TEST_F(TestGoalRequestServer, publish_status_publish_status_errors) +{ + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", rcl_action_publish_status, RCL_RET_ERROR); + + EXPECT_THROW(SendClientGoalRequest(), std::runtime_error); +} + +TEST_F(TestGoalRequestServer, execute_goal_request_received_take_failed) +{ + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", rcl_action_take_goal_request, RCL_RET_ACTION_SERVER_TAKE_FAILED); + try { + SendClientGoalRequest(std::chrono::milliseconds(100)); + ADD_FAILURE() << "SetupActionServerAndSpin did not throw, but was expected to"; + } catch (const std::runtime_error & e) { + EXPECT_STREQ("send goal future timed out", e.what()); + } catch (...) { + ADD_FAILURE() << "SetupActionServerAndSpin threw, but not the expected std::runtime_error"; + } +} + +TEST_F(TestGoalRequestServer, get_result_rcl_errors) +{ + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", rcl_action_take_result_request, RCL_RET_ERROR); + + EXPECT_THROW(SendClientGoalRequest(), rclcpp::exceptions::RCLError); +} + +TEST_F(TestGoalRequestServer, send_result_rcl_errors) +{ + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", rcl_action_send_result_response, RCL_RET_ERROR); + + EXPECT_THROW(SendClientGoalRequest(), rclcpp::exceptions::RCLError); +} + +TEST_F(TestCancelRequestServer, execute_cancel_request_received_take_cancel_request_errors) +{ + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", rcl_action_take_cancel_request, RCL_RET_ERROR); + + EXPECT_THROW(SendClientCancelRequest(), rclcpp::exceptions::RCLError); +} + +TEST_F(TestCancelRequestServer, execute_cancel_request_received_take_failed) +{ + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", rcl_action_take_cancel_request, RCL_RET_ACTION_SERVER_TAKE_FAILED); + try { + SendClientCancelRequest(std::chrono::milliseconds(100)); + ADD_FAILURE() << "SetupActionServerAndSpin did not throw, but it was expected to"; + } catch (const std::runtime_error & e) { + EXPECT_STREQ("cancel request future timed out", e.what()); + } catch (...) { + ADD_FAILURE() << "SetupActionServerAndSpin threw, but not the expected std::runtime_error"; + } +} + +TEST_F(TestCancelRequestServer, publish_status_rcl_errors) +{ + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", rcl_action_process_cancel_request, RCL_RET_ERROR); + + EXPECT_THROW(SendClientCancelRequest(), rclcpp::exceptions::RCLError); +} + +TEST_F(TestCancelRequestServer, publish_status_send_cancel_response_errors) +{ + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", rcl_action_send_cancel_response, RCL_RET_ERROR); + + EXPECT_THROW(SendClientCancelRequest(), std::runtime_error); +} + +class TestDeadlockServer : public TestServer +{ +public: + void SetUp() + { + node_ = std::make_shared("goal_request", "/rclcpp_action/goal_request"); + uuid1_ = {{1, 2, 3, 4, 5, 6, 70, 80, 9, 1, 11, 120, 13, 140, 15, 160}}; + uuid2_ = {{2, 2, 3, 4, 5, 6, 70, 80, 9, 1, 11, 120, 13, 140, 15, 160}}; + action_server_ = rclcpp_action::create_server( + node_, "fibonacci", + [this](const GoalUUID &, std::shared_ptr) { + // instead of making a deadlock, check if it can acquire the lock in a second + std::unique_lock lock(server_mutex_, std::defer_lock); + this->TryLockFor(lock, std::chrono::milliseconds(1000)); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }, + [this](std::shared_ptr) { + // instead of making a deadlock, check if it can acquire the lock in a second + std::unique_lock lock(server_mutex_, std::defer_lock); + this->TryLockFor(lock, std::chrono::milliseconds(1000)); + return rclcpp_action::CancelResponse::ACCEPT; + }, + [this](std::shared_ptr handle) { + // instead of making a deadlock, check if it can acquire the lock in a second + std::unique_lock lock(server_mutex_, std::defer_lock); + this->TryLockFor(lock, std::chrono::milliseconds(1000)); + goal_handle_ = handle; + }); + } + + void GoalSucceeded() + { + std::lock_guard lock(server_mutex_); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + auto result = std::make_shared(); + result->sequence = {5, 8, 13, 21}; + goal_handle_->succeed(result); + } + + void GoalCanceled() + { + std::lock_guard lock(server_mutex_); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + auto result = std::make_shared(); + goal_handle_->canceled(result); + } + + void TryLockFor( + std::unique_lock & lock, + std::chrono::milliseconds timeout + ) + { + ASSERT_TRUE(lock.try_lock_for(timeout)); + } + +protected: + std::recursive_timed_mutex server_mutex_; + GoalUUID uuid1_, uuid2_; + std::shared_ptr node_; + std::shared_ptr> action_server_; + + using GoalHandle = rclcpp_action::ServerGoalHandle; + std::shared_ptr goal_handle_; +}; + +TEST_F(TestDeadlockServer, deadlock_while_succeed) +{ + send_goal_request(node_, uuid1_); + // this will lock wrapper's mutex and intentionally wait 100ms for calling succeed + // to try to acquire the lock of rclcpp_action mutex + std::thread t(&TestDeadlockServer::GoalSucceeded, this); + // after the wrapper's mutex is locked and before succeed is called + rclcpp::sleep_for(std::chrono::milliseconds(50)); + // call next goal request to intentionally reproduce deadlock + // this first locks rclcpp_action mutex and then call callback to lock wrapper's mutex + send_goal_request(node_, uuid2_); + t.join(); +} + +TEST_F(TestDeadlockServer, deadlock_while_canceled) +{ + send_goal_request(node_, uuid1_); + send_cancel_request(node_, uuid1_); + std::thread t(&TestDeadlockServer::GoalCanceled, this); + rclcpp::sleep_for(std::chrono::milliseconds(50)); + send_goal_request(node_, uuid2_); // deadlock here + t.join(); +} + +TEST_F(TestDeadlockServer, deadlock_while_succeed_and_canceled) +{ + send_goal_request(node_, uuid1_); + std::thread t(&TestDeadlockServer::GoalSucceeded, this); + rclcpp::sleep_for(std::chrono::milliseconds(50)); + auto response_ptr = send_cancel_request(node_, uuid1_); + + // current goal handle is not cancelable, so it returns ERROR_REJECTED + EXPECT_EQ(CancelResponse::ERROR_REJECTED, response_ptr->return_code); + t.join(); +} diff --git a/rclcpp_action/test/test_server_goal_handle.cpp b/rclcpp_action/test/test_server_goal_handle.cpp new file mode 100644 index 0000000000..eb392c8bf7 --- /dev/null +++ b/rclcpp_action/test/test_server_goal_handle.cpp @@ -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. + +#include +#include + +#include "gtest/gtest.h" + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/rclcpp.hpp" +#include "test_msgs/action/fibonacci.hpp" + +#include "action_msgs/msg/goal_info.h" +#include "rclcpp_action/server_goal_handle.hpp" +#include "mocking_utils/patch.hpp" + +class FibonacciServerGoalHandle + : public rclcpp_action::ServerGoalHandle +{ +public: + FibonacciServerGoalHandle( + std::shared_ptr rcl_handle, + rclcpp_action::GoalUUID uuid, + std::shared_ptr goal, + std::function)> on_terminal_state, + std::function on_executing, + std::function)> publish_feedback) + : rclcpp_action::ServerGoalHandle( + rcl_handle, uuid, goal, on_terminal_state, on_executing, publish_feedback) {} + + bool try_cancel() {return try_canceling();} + + void cancel_goal() {_cancel_goal();} +}; + +class TestServerGoalHandle : public ::testing::Test +{ +public: + TestServerGoalHandle() + : handle_(nullptr) {} + + void SetUp() + { + std::shared_ptr rcl_handle = + std::shared_ptr( + new rcl_action_goal_handle_t, + [](rcl_action_goal_handle_t * p) { + if (nullptr == p) { + return; + } + rcl_ret_t ret = rcl_action_goal_handle_fini(p); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + delete p; + }); + *rcl_handle.get() = rcl_action_get_zero_initialized_goal_handle(); + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info(); + ASSERT_EQ(RCL_RET_OK, rcl_action_goal_handle_init(rcl_handle.get(), &goal_info, allocator)); + rclcpp_action::GoalUUID uuid; + std::shared_ptr goal = + std::make_shared(); + auto on_terminal_state = [](const rclcpp_action::GoalUUID &, std::shared_ptr) {}; + auto on_executing = [](const rclcpp_action::GoalUUID &) {}; + auto publish_feedback = + [](std::shared_ptr) {}; + handle_ = std::make_unique( + rcl_handle, uuid, goal, on_terminal_state, on_executing, publish_feedback); + } + +protected: + std::unique_ptr handle_; +}; + +TEST_F(TestServerGoalHandle, construct_destruct) { + EXPECT_FALSE(handle_->is_canceling()); + EXPECT_TRUE(handle_->is_active()); + EXPECT_FALSE(handle_->is_executing()); +} + +TEST_F(TestServerGoalHandle, cancel) { + handle_->execute(); + EXPECT_TRUE(handle_->try_cancel()); + EXPECT_FALSE(handle_->is_canceling()); + EXPECT_FALSE(handle_->is_active()); + EXPECT_FALSE(handle_->is_executing()); + + { + auto mock_get_status = mocking_utils::patch_and_return( + "lib:rclcpp_action", rcl_action_goal_handle_get_status, RCL_RET_ERROR); + EXPECT_FALSE(handle_->try_cancel()); + } + + { + auto mock_update_status = mocking_utils::patch_and_return( + "lib:rclcpp_action", rcl_action_update_goal_state, RCL_RET_ERROR); + auto mock_is_cancelable = mocking_utils::patch_and_return( + "lib:rclcpp_action", rcl_action_goal_handle_is_cancelable, true); + EXPECT_FALSE(handle_->try_cancel()); + EXPECT_THROW(handle_->cancel_goal(), rclcpp::exceptions::RCLError); + + test_msgs::action::Fibonacci::Result::SharedPtr result = + std::make_shared(); + EXPECT_THROW(handle_->canceled(result), rclcpp::exceptions::RCLError); + } +} + +TEST_F(TestServerGoalHandle, abort) { + handle_->execute(); + test_msgs::action::Fibonacci::Result::SharedPtr result = + std::make_shared(); + handle_->abort(result); + EXPECT_FALSE(handle_->is_canceling()); + EXPECT_FALSE(handle_->is_active()); + EXPECT_FALSE(handle_->is_executing()); + + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", rcl_action_update_goal_state, RCL_RET_ERROR); + EXPECT_THROW(handle_->abort(result), rclcpp::exceptions::RCLError); +} + +TEST_F(TestServerGoalHandle, succeed) { + handle_->execute(); + test_msgs::action::Fibonacci::Result::SharedPtr result = + std::make_shared(); + handle_->succeed(result); + EXPECT_FALSE(handle_->is_canceling()); + EXPECT_FALSE(handle_->is_active()); + EXPECT_FALSE(handle_->is_executing()); + + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", rcl_action_update_goal_state, RCL_RET_ERROR); + EXPECT_THROW(handle_->succeed(result), rclcpp::exceptions::RCLError); +} + + +TEST_F(TestServerGoalHandle, execute) { + handle_->execute(); + EXPECT_FALSE(handle_->is_canceling()); + EXPECT_TRUE(handle_->is_active()); + EXPECT_TRUE(handle_->is_executing()); + + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", rcl_action_update_goal_state, RCL_RET_ERROR); + EXPECT_THROW(handle_->execute(), rclcpp::exceptions::RCLError); +} + +TEST_F(TestServerGoalHandle, rcl_action_goal_handle_get_status_error) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_action", rcl_action_goal_handle_get_status, RCL_RET_ERROR); + + EXPECT_THROW(handle_->is_canceling(), rclcpp::exceptions::RCLError); + EXPECT_NO_THROW(handle_->is_active()); + EXPECT_THROW(handle_->is_executing(), rclcpp::exceptions::RCLError); +} diff --git a/rclcpp_action/test/test_traits.cpp b/rclcpp_action/test/test_traits.cpp new file mode 100644 index 0000000000..38016c16dd --- /dev/null +++ b/rclcpp_action/test/test_traits.cpp @@ -0,0 +1,99 @@ +// 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 + +using rosidl_generator_traits::is_message; +using rosidl_generator_traits::is_service; +using rosidl_generator_traits::is_action; +using rosidl_generator_traits::is_action_goal; +using rosidl_generator_traits::is_action_result; +using rosidl_generator_traits::is_action_feedback; + +TEST(TestActionTraits, is_action) { + using Fibonacci = test_msgs::action::Fibonacci; + + // Top level definition is an action + EXPECT_FALSE(is_message()); + EXPECT_FALSE(is_service()); + EXPECT_TRUE(is_action()); + EXPECT_FALSE(is_action_goal()); + EXPECT_FALSE(is_action_result()); + EXPECT_FALSE(is_action_feedback()); + + // Goal is an action_goal as well as a message + EXPECT_TRUE(is_message()); + EXPECT_FALSE(is_service()); + EXPECT_FALSE(is_action()); + EXPECT_TRUE(is_action_goal()); + EXPECT_FALSE(is_action_result()); + EXPECT_FALSE(is_action_feedback()); + + // Result is an action_result as well as a message + EXPECT_TRUE(is_message()); + EXPECT_FALSE(is_service()); + EXPECT_FALSE(is_action()); + EXPECT_FALSE(is_action_goal()); + EXPECT_TRUE(is_action_result()); + EXPECT_FALSE(is_action_feedback()); + + // Feedback is an action_feedback as well as a message + EXPECT_TRUE(is_message()); + EXPECT_FALSE(is_service()); + EXPECT_FALSE(is_action()); + EXPECT_FALSE(is_action_goal()); + EXPECT_FALSE(is_action_result()); + EXPECT_TRUE(is_action_feedback()); +} + +TEST(TestActionTraits, is_action_impl) { + using Fibonacci = test_msgs::action::Fibonacci; + + // Test traits on some of the internal implementation of actions + EXPECT_FALSE(is_message()); + EXPECT_TRUE(is_service()); + EXPECT_FALSE(is_action()); + EXPECT_FALSE(is_action_goal()); + EXPECT_FALSE(is_action_result()); + EXPECT_FALSE(is_action_feedback()); + + EXPECT_FALSE(is_message()); + EXPECT_TRUE(is_service()); + EXPECT_FALSE(is_action()); + EXPECT_FALSE(is_action_goal()); + EXPECT_FALSE(is_action_result()); + EXPECT_FALSE(is_action_feedback()); + + EXPECT_FALSE(is_message()); + EXPECT_TRUE(is_service()); + EXPECT_FALSE(is_action()); + EXPECT_FALSE(is_action_goal()); + EXPECT_FALSE(is_action_result()); + EXPECT_FALSE(is_action_feedback()); + + EXPECT_TRUE(is_message()); + EXPECT_FALSE(is_service()); + EXPECT_FALSE(is_action()); + EXPECT_FALSE(is_action_goal()); + EXPECT_FALSE(is_action_result()); + EXPECT_FALSE(is_action_feedback()); + + EXPECT_TRUE(is_message()); + EXPECT_FALSE(is_service()); + EXPECT_FALSE(is_action()); + EXPECT_FALSE(is_action_goal()); + EXPECT_FALSE(is_action_result()); + EXPECT_FALSE(is_action_feedback()); +} diff --git a/rclcpp_action/test/test_types.cpp b/rclcpp_action/test/test_types.cpp new file mode 100644 index 0000000000..619a4f7899 --- /dev/null +++ b/rclcpp_action/test/test_types.cpp @@ -0,0 +1,94 @@ +// 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 "rclcpp_action/types.hpp" + +TEST(TestActionTypes, goal_uuid_to_string) { + rclcpp_action::GoalUUID goal_id; + for (uint8_t i = 0; i < UUID_SIZE; ++i) { + goal_id[i] = i; + } + EXPECT_STREQ("00010203-0405-0607-0809-0a0b0c0d0e0f", rclcpp_action::to_string(goal_id).c_str()); + + for (uint8_t i = 0; i < UUID_SIZE; ++i) { + goal_id[i] = static_cast(16u + i); + } + EXPECT_STREQ("10111213-1415-1617-1819-1a1b1c1d1e1f", rclcpp_action::to_string(goal_id).c_str()); + + for (uint8_t i = 0; i < UUID_SIZE; ++i) { + goal_id[i] = static_cast(std::numeric_limits::max() - i); + } + EXPECT_STREQ("fffefdfc-fbfa-f9f8-f7f6-f5f4f3f2f1f0", rclcpp_action::to_string(goal_id).c_str()); +} + +TEST(TestActionTypes, goal_uuid_to_rcl_action_goal_info) { + rclcpp_action::GoalUUID goal_id; + for (uint8_t i = 0; i < UUID_SIZE; ++i) { + goal_id[i] = i; + } + rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info(); + rclcpp_action::convert(goal_id, &goal_info); + for (uint8_t i = 0; i < UUID_SIZE; ++i) { + EXPECT_EQ(goal_info.goal_id.uuid[i], goal_id[i]); + } +} + +TEST(TestActionTypes, rcl_action_goal_info_to_goal_uuid) { + rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info(); + for (uint8_t i = 0; i < UUID_SIZE; ++i) { + goal_info.goal_id.uuid[i] = i; + } + + rclcpp_action::GoalUUID goal_id; + rclcpp_action::convert(goal_info, &goal_id); + for (uint8_t i = 0; i < UUID_SIZE; ++i) { + EXPECT_EQ(goal_info.goal_id.uuid[i], goal_id[i]); + } +} + +TEST(TestActionTypes, goal_uuid_to_hashed_uuid_random) { + // Use std::random_device to seed the generator of goal IDs. + std::random_device rd; + std::independent_bits_engine< + std::default_random_engine, 8, decltype(rd())> random_bytes_generator(rd()); + + std::vector hashed_guuids; + constexpr size_t iterations = 1000; + + for (size_t i = 0; i < iterations; i++) { + rclcpp_action::GoalUUID goal_id; + + // Generate random bytes for each element of the array + for (auto & element : goal_id) { + element = static_cast(random_bytes_generator()); + } + + size_t new_hashed_guuid = std::hash()(goal_id); + + // Search for any prevoius hashed goal_id with the same value + for (auto prev_hashed_guuid : hashed_guuids) { + EXPECT_NE(prev_hashed_guuid, new_hashed_guuid); + if (prev_hashed_guuid == new_hashed_guuid) { + // Fail before the first occurrence of a collision + GTEST_FAIL(); + } + } + + hashed_guuids.push_back(new_hashed_guuid); + } +} diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst new file mode 100644 index 0000000000..cc55217333 --- /dev/null +++ b/rclcpp_components/CHANGELOG.rst @@ -0,0 +1,394 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package rclcpp_components +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +29.2.0 (2024-11-25) +------------------- + +29.1.0 (2024-11-20) +------------------- + +29.0.0 (2024-10-03) +------------------- +* Shutdown the context before context's destructor is invoked in tests (`#2633 `_) +* Fix typo in rclcpp_components benchmark_components (`#2602 `_) +* Contributors: Alejandro Hernández Cordero, Christophe Bedard + +28.3.3 (2024-07-29) +------------------- + +28.3.2 (2024-07-24) +------------------- +* Updated rcpputils path API (`#2579 `_) +* remove deprecated APIs from component_manager.hpp (`#2585 `_) +* Contributors: Alberto Soragna, Alejandro Hernández Cordero + +28.3.1 (2024-06-25) +------------------- + +28.3.0 (2024-06-17) +------------------- + +28.2.0 (2024-04-26) +------------------- + +28.1.0 (2024-04-16) +------------------- +* Remove references to index.ros.org. (`#2504 `_) +* Contributors: Chris Lalancette + +28.0.1 (2024-04-16) +------------------- + +28.0.0 (2024-03-28) +------------------- +* Add EXECUTOR docs (`#2440 `_) +* Update quality declaration documents (`#2427 `_) +* crash on no class found (`#2415 `_) + * crash on no class found + * error on no class found instead of no callback groups + Co-authored-by: Chris Lalancette +* Contributors: Adam Aposhian, Christophe Bedard, Ruddick Lawrence + +27.0.0 (2024-02-07) +------------------- + +26.0.0 (2024-01-24) +------------------- + +25.0.0 (2023-12-26) +------------------- +* Switch to target_link_libraries. (`#2374 `_) +* feat(rclcpp_components): support events executor in node main template (`#2366 `_) +* fix(rclcpp_components): increase the service queue sizes in component_container (`#2363 `_) +* Contributors: Chris Lalancette, Daisuke Nishimatsu, M. Fatih Cırıt + +24.0.0 (2023-11-06) +------------------- + +23.2.0 (2023-10-09) +------------------- + +23.1.0 (2023-10-04) +------------------- +* Add missing header required by the rclcpp::NodeOptions type (`#2324 `_) +* Contributors: Ignacio Vizzo + +23.0.0 (2023-09-08) +------------------- +* Update API docs links in package READMEs (`#2302 `_) +* Contributors: Christophe Bedard + +22.2.0 (2023-09-07) +------------------- + +22.1.0 (2023-08-21) +------------------- + +22.0.0 (2023-07-11) +------------------- + +21.3.0 (2023-06-12) +------------------- + +21.2.0 (2023-06-07) +------------------- + +21.1.1 (2023-05-11) +------------------- + +21.1.0 (2023-04-27) +------------------- + +21.0.0 (2023-04-18) +------------------- + +20.0.0 (2023-04-13) +------------------- +* Update all rclcpp packages to C++17. (`#2121 `_) +* Contributors: Chris Lalancette + +19.3.0 (2023-03-01) +------------------- + +19.2.0 (2023-02-24) +------------------- + +19.1.0 (2023-02-14) +------------------- + +19.0.0 (2023-01-30) +------------------- +* Improve component_manager_isolated shutdown (`#2085 `_) +* Contributors: Michael Carroll + +18.0.0 (2022-12-29) +------------------- +* Update maintainers (`#2043 `_) +* Contributors: Audrow Nash + +17.1.0 (2022-11-02) +------------------- +* use unique ptr and remove unuseful container (`#2013 `_) +* Contributors: Chen Lihui + +17.0.0 (2022-09-13) +------------------- +* Revert "Introduce executors new spin_for method, replace spin_until_future_complete with spin_until_complete. (`#1821 `_) (`#1874 `_)" (`#1956 `_) +* Introduce executors new spin_for method, replace spin_until_future_complete with spin_until_complete. (`#1821 `_) (`#1874 `_) +* Contributors: Hubert Liberacki, William Woodall + +16.2.0 (2022-05-03) +------------------- + +16.1.0 (2022-04-29) +------------------- + +16.0.1 (2022-04-13) +------------------- + +16.0.0 (2022-04-08) +------------------- + +15.4.0 (2022-04-05) +------------------- + +15.3.0 (2022-03-30) +------------------- + +15.2.0 (2022-03-24) +------------------- +* Select executor in node registration (`#1898 `_) +* Fix rosdoc2 issues in rclcpp (`#1897 `_) +* Fix bugprone-exception-escape in node_main.cpp.in (`#1895 `_) +* Contributors: Alberto Soragna, Chris Lalancette, Hirokazu Ishida + +15.1.0 (2022-03-01) +------------------- +* small improvements to node_main.cpp.in +* Install headers to include/${PROJECT_NAME} (`#1888 `_) +* Use spin() in component_manager_isolated.hpp (`#1881 `_) +* add use_global_arguments for node options of component nodes (`#1776 `_) +* Contributors: Alberto Soragna, Shane Loretz, gezp + +15.0.0 (2022-01-14) +------------------- +* Add rclcpp_components::component (`#1855 `_) +* Contributors: Shane Loretz + +14.1.0 (2022-01-05) +------------------- + +14.0.0 (2021-12-17) +------------------- +* Add parameter to configure number of thread (`#1708 `_) +* remove RCLCPP_COMPONENTS_PUBLIC in class ComponentManagerIsolated (`#1843 `_) +* create component_container_isolated (`#1781 `_) +* Remove author by request (`#1818 `_) +* Update maintainers (`#1817 `_) +* Suppress clang dead-store warnings in the benchmarks. (`#1802 `_) +* Contributors: Chris Lalancette, Daisuke Nishimatsu, Jacob Perron, gezp + +13.1.0 (2021-10-18) +------------------- + +13.0.0 (2021-08-23) +------------------- +* Update client API to be able to remove pending requests. (`#1734 `_) +* Contributors: Ivan Santiago Paunovic + +12.0.0 (2021-07-26) +------------------- + +11.2.0 (2021-07-21) +------------------- +* Deprecate method names that use CamelCase in rclcpp_components. (`#1716 `_) +* Contributors: Rebecca Butler + +11.1.0 (2021-07-13) +------------------- +* Added a hook to generate node options in ComponentManager (`#1702 `_) +* Contributors: Rebecca Butler + +11.0.0 (2021-05-18) +------------------- + +10.0.0 (2021-05-11) +------------------- + +9.0.2 (2021-04-14) +------------------ + +9.0.1 (2021-04-12) +------------------ + +9.0.0 (2021-04-06) +------------------ +* updating quality declaration links (re: `ros2/docs.ros2.org#52 `_) (`#1615 `_) +* Contributors: shonigmann + +8.2.0 (2021-03-31) +------------------ + +8.1.0 (2021-03-25) +------------------ + +8.0.0 (2021-03-23) +------------------ + +7.0.1 (2021-03-22) +------------------ + +7.0.0 (2021-03-18) +------------------ + +6.3.1 (2021-02-08) +------------------ + +6.3.0 (2021-01-25) +------------------ + +6.2.0 (2021-01-08) +------------------ +* Use std compliant non-method std::filesystem::exists function (`#1502 `_) +* Fix string literal warnings (`#1442 `_) +* Contributors: Audrow Nash, Josh Langsfeld + +6.1.0 (2020-12-10) +------------------ +* Update QDs to QL 1 (`#1477 `_) +* Add benchmarks for components (`#1476 `_) +* Contributors: Scott K Logan, Stephen Brawner + +6.0.0 (2020-11-18) +------------------ +* Bump rclcpp packages to Quality Level 2 (`#1445 `_) +* Contributors: Louise Poubel + +5.1.0 (2020-11-02) +------------------ +* Update maintainers (`#1384 `_) +* ComponentManager: switch off parameter services and event publisher (`#1333 `_) +* Contributors: Ivan Santiago Paunovic, Martijn Buijs + +5.0.0 (2020-09-18) +------------------ + +4.0.0 (2020-07-09) +------------------ +* Bump to QD to level 3 and fixed links (`#1158 `_) +* Include original exception in ComponentManagerException (`#1157 `_) +* Contributors: Alejandro Hernández Cordero, Martijn Buijs, Tomoya Fujita + +3.0.0 (2020-06-18) +------------------ + +2.0.0 (2020-06-01) +------------------ +* Added missing virtual destructors. (`#1149 `_) +* Add Security Vulnerability Policy pointing to REP-2006. (`#1130 `_) +* Contributors: Chris Lalancette, Ivan Santiago Paunovic + +1.1.0 (2020-05-26) +------------------ +* Improve documentation (`#1106 `_) +* Fixed rep links and added more details to dependencies in quality declaration (`#1116 `_) +* Added dockblock to ComponentManager class (`#1102 `_) +* Update quality declaration to reflect version 1.0 (`#1115 `_) +* Contributors: Alejandro Hernández Cordero, Stephen Brawner + +1.0.0 (2020-05-12) +------------------ +* Increasing test coverage of rclcpp_components (`#1044 `_) + * Increasing test coverage of rclcpp_components + Signed-off-by: Stephen Brawner + * PR fixup + Signed-off-by: Stephen Brawner + * Fixup + Signed-off-by: Stephen Brawner + * Removing throws test for now + Signed-off-by: Stephen Brawner +* Contributors: brawner + +0.9.1 (2020-05-08) +------------------ +* Added Quality declaration: rclcpp, rclpp_action, rclcpp_components andrclcpp_lifecycle (`#1100 `_) +* Contributors: Alejandro Hernández Cordero + +0.9.0 (2020-04-29) +------------------ +* Added rclcpp_components Doxyfile (`#1091 `_) +* Deprecate redundant namespaces (`#1083 `_) +* Export targets in addition to include directories / libraries (`#1088 `_) +* Export component manager (`#1070 `_) +* Install the component_manager library (`#1068 `_) +* Make Component Manager public (`#1065 `_) +* Remove absolute path from installed CMake code (`#948 `_) +* Fix function docblock, check for unparsed arguments (`#945 `_) +* Contributors: Alejandro Hernández Cordero, DensoADAS, Dirk Thomas, Jacob Perron, Karsten Knese, Michael Carroll, William Woodall + +0.8.3 (2019-11-19) +------------------ + +0.8.2 (2019-11-18) +------------------ + +0.8.1 (2019-10-23) +------------------ +* Enable intra-process comm via LoadNode request. (`#871 `_) +* Aggregate all component manager API tests. (`#876 `_) +* Contributors: Michel Hidalgo + +0.8.0 (2019-09-26) +------------------ +* Force explicit --ros-args in NodeOptions::arguments(). (`#845 `_) +* Use of -r/--remap flags where appropriate. (`#834 `_) +* Add line break after first open paren in multiline function call (`#785 `_) +* fix linter issue (`#795 `_) +* Remove non-package from ament_target_dependencies() (`#793 `_) +* fix for multiple nodes not being recognized (`#790 `_) +* Cmake infrastructure for creating components (`#784 `_) +* Contributors: Dan Rose, Michel Hidalgo, Shane Loretz, Siddharth Kucheria + +0.7.5 (2019-05-30) +------------------ + +0.7.4 (2019-05-29) +------------------ +* Rename parameter options (`#745 `_) +* don't use global arguments for components loaded into the manager (`#736 `_) +* Contributors: Dirk Thomas, William Woodall + +0.7.3 (2019-05-20) +------------------ + +0.7.2 (2019-05-08) +------------------ +* Updated to support changes to ``Node::get_node_names()``. (`#698 `_) +* Contributors: jhdcs + +0.7.1 (2019-04-26) +------------------ + +0.7.0 (2019-04-14) +------------------ +* Introduce rclcpp_components to implement composition (`#665 `_) +* Contributors: Michael Carroll + +0.6.2 (2018-12-12) +------------------ + +0.6.1 (2018-12-06) +------------------ + +0.6.0 (2018-11-19) +------------------ + +0.5.1 (2018-06-28) +------------------ + +0.5.0 (2018-06-25) +------------------ + +0.4.0 (2017-12-08) +------------------ diff --git a/rclcpp_components/CMakeLists.txt b/rclcpp_components/CMakeLists.txt new file mode 100644 index 0000000000..7d4135051c --- /dev/null +++ b/rclcpp_components/CMakeLists.txt @@ -0,0 +1,176 @@ +cmake_minimum_required(VERSION 3.5) + +project(rclcpp_components) + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options( + -Wall -Wextra -Wpedantic -Wnon-virtual-dtor -Woverloaded-virtual + -Wformat=2 -Wconversion -Wshadow -Wsign-conversion -Wold-style-cast -Wcast-qual + ) +endif() + +find_package(ament_cmake_ros REQUIRED) +find_package(ament_index_cpp REQUIRED) +find_package(class_loader REQUIRED) +find_package(composition_interfaces REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rcpputils REQUIRED) + +# Add an interface library that can be depended upon by libraries who register components +add_library(component INTERFACE) +target_include_directories(component INTERFACE + "$" + "$") +target_link_libraries(component INTERFACE + class_loader::class_loader + rclcpp::rclcpp) + +add_library( + component_manager + SHARED + src/component_manager.cpp +) +target_include_directories(component_manager PUBLIC + "$" + "$") +target_link_libraries(component_manager PUBLIC + ${composition_interfaces_TARGETS} + rclcpp::rclcpp +) +target_link_libraries(component_manager PRIVATE + ament_index_cpp::ament_index_cpp + class_loader::class_loader + rcpputils::rcpputils +) +target_compile_definitions(component_manager + PRIVATE "RCLCPP_COMPONENTS_BUILDING_LIBRARY") + +add_executable( + component_container + src/component_container.cpp +) +target_link_libraries(component_container component_manager rclcpp::rclcpp) + +set(node_main_template_install_dir "share/${PROJECT_NAME}") +install(FILES + src/node_main.cpp.in + DESTINATION ${node_main_template_install_dir}) + +add_executable( + component_container_mt + src/component_container_mt.cpp +) +target_link_libraries(component_container_mt component_manager rclcpp::rclcpp) + +add_executable( + component_container_isolated + src/component_container_isolated.cpp +) +target_link_libraries(component_container_isolated component_manager rclcpp::rclcpp) + +if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") + target_link_libraries(component_container "stdc++fs") + target_link_libraries(component_container_mt "stdc++fs") + target_link_libraries(component_container_isolated "stdc++fs") +endif() + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + find_package(ament_cmake_google_benchmark REQUIRED) + find_package(benchmark REQUIRED) + # Give cppcheck hints about macro definitions coming from outside this package + get_target_property(ament_cmake_cppcheck_ADDITIONAL_INCLUDE_DIRS benchmark::benchmark INTERFACE_INCLUDE_DIRECTORIES) + ament_lint_auto_find_test_dependencies() + + set(components "") + add_library(test_component SHARED test/components/test_component.cpp) + target_link_libraries(test_component PRIVATE component) + #rclcpp_components_register_nodes(test_component "test_rclcpp_components::TestComponent") + set(components "${components}test_rclcpp_components::TestComponentFoo;$\n") + set(components "${components}test_rclcpp_components::TestComponentBar;$\n") + set(components "${components}test_rclcpp_components::TestComponentNoNode;$\n") + + # A properly formed resource only has one ';', this is used to catch an invalid resource entry + set(invalid_components "test_rclcpp_components::TestComponentFoo;;$\n") + + file(GENERATE + OUTPUT + "${CMAKE_CURRENT_BINARY_DIR}/test_ament_index/$/share/ament_index/resource_index/rclcpp_components/${PROJECT_NAME}" + CONTENT "${components}") + + file(GENERATE + OUTPUT + "${CMAKE_CURRENT_BINARY_DIR}/test_ament_index/$/share/ament_index/resource_index/rclcpp_components/invalid_${PROJECT_NAME}" + CONTENT "${invalid_components}") + + set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR}") + if(WIN32) + set(append_library_dirs "${append_library_dirs}/$") + endif() + + ament_add_gtest(test_component_manager test/test_component_manager.cpp + APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index/$ + APPEND_LIBRARY_DIRS "${append_library_dirs}") + if(TARGET test_component_manager) + target_link_libraries(test_component_manager component_manager) + endif() + + ament_add_gtest(test_component_manager_api test/test_component_manager_api.cpp + APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index/$ + APPEND_LIBRARY_DIRS "${append_library_dirs}") + if(TARGET test_component_manager_api) + target_link_libraries(test_component_manager_api component_manager) + endif() + + ament_add_google_benchmark(benchmark_components + test/benchmark/benchmark_components.cpp + APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index/$ + APPEND_LIBRARY_DIRS "${append_library_dirs}") + if(TARGET benchmark_components) + target_link_libraries(benchmark_components component_manager) + endif() +endif() + +install( + TARGETS component component_manager EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +# Install executables +install( + TARGETS component_container component_container_mt component_container_isolated + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +# Install include directories +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +# Install cmake +install( + DIRECTORY cmake + DESTINATION share/${PROJECT_NAME} +) + +# Export old-style CMake variables +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_libraries(component_manager) + +# Export modern CMake targets +ament_export_targets(export_${PROJECT_NAME}) + +# specific order: dependents before dependencies +ament_export_dependencies(ament_index_cpp) +ament_export_dependencies(class_loader) +ament_export_dependencies(composition_interfaces) +ament_export_dependencies(rclcpp) +ament_package(CONFIG_EXTRAS rclcpp_components-extras.cmake.in) diff --git a/rclcpp_components/Doxyfile b/rclcpp_components/Doxyfile new file mode 100644 index 0000000000..11fe1e178f --- /dev/null +++ b/rclcpp_components/Doxyfile @@ -0,0 +1,33 @@ +# All settings not listed here will use the Doxygen default values. + +PROJECT_NAME = "rclcpp_components" +PROJECT_NUMBER = master +PROJECT_BRIEF = "Package containing tools for dynamically loadable components" + +# Use these lines to include the generated logging.hpp (update install path if needed) +# Otherwise just generate for the local (non-generated header files) +INPUT = ./include + +RECURSIVE = YES +OUTPUT_DIRECTORY = doc_output + +EXTRACT_ALL = YES +SORT_MEMBER_DOCS = NO + +GENERATE_LATEX = NO + +ENABLE_PREPROCESSING = YES +MACRO_EXPANSION = YES +EXPAND_ONLY_PREDEF = YES +PREDEFINED = RCLCPP_COMPONENTS_PUBLIC= + +# Tag files that do not exist will produce a warning and cross-project linking will not work. +#TAGFILES += "../../../../doxygen_tag_files/cppreference-doxygen-web.tag.xml=http://en.cppreference.com/w/" +# Consider changing "latest" to the version you want to reference (e.g. beta1 or 1.0.0) +#TAGFILES += "../../../../doxygen_tag_files/class_loader.tag=http://docs.ros2.org/latest/api/class_loader/" +#TAGFILES += "../../../../doxygen_tag_files/rclcpp.tag=http://docs.ros2.org/latest/api/rclcpp/" +#TAGFILES += "../../../../doxygen_tag_files/rcl.tag=http://docs.ros2.org/latest/api/rcl/" +#TAGFILES += "../../../../doxygen_tag_files/rcutils.tag=http://docs.ros2.org/latest/api/rcutils/" +#TAGFILES += "../../../../doxygen_tag_files/rmw.tag=http://docs.ros2.org/latest/api/rmw/" +# Uncomment to generate tag files for cross-project linking. +GENERATE_TAGFILE = "../../../../doxygen_tag_files/rclcpp_components.tag" diff --git a/rclcpp_components/QUALITY_DECLARATION.md b/rclcpp_components/QUALITY_DECLARATION.md new file mode 100644 index 0000000000..2e38c0410a --- /dev/null +++ b/rclcpp_components/QUALITY_DECLARATION.md @@ -0,0 +1,206 @@ +This document is a declaration of software quality for the `rclcpp_components` package, based on the guidelines in [REP-2004](https://www.ros.org/reps/rep-2004.html). + +# rclcpp_components Quality Declaration + +The package `rclcpp_components` claims to be in the **Quality Level 1** category. + +Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Requirements for Quality Level 1 in REP-2004](https://www.ros.org/reps/rep-2004.html). + +## Version Policy [1] + +### Version Scheme [1.i] + +`rclcpp_components` 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] + +`rclcpp_components` is at a stable version, i.e. `>= 1.0.0`. +The current version can be found in its [package.xml](package.xml), and its change history can be found in its [CHANGELOG](CHANGELOG.rst). + +### Public API Declaration [1.iii] + +All symbols in the installed headers are considered part of the public API. + +All installed headers are in the `include` directory of the package, headers in any other folders are not installed and considered private. + +### API Stability Policy [1.iv] + +`rclcpp_components` 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] + +`rclcpp_components` 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] + +`rclcpp_components` 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] + +`rclcpp_components` follows the recommended guidelines for ROS Core packages in the [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#change-control-process). + +### Change Requests [2.i] + +All changes will occur through a pull request, check [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/rclcpp_components/) +* [linux_release](https://ci.ros2.org/view/nightly/job/nightly_linux_release/lastBuild/testReport/rclcpp_components/) +* [mac_osx_release](https://ci.ros2.org/view/nightly/job/nightly_osx_release/lastBuild/testReport/rclcpp_components/) +* [windows_release](https://ci.ros2.org/view/nightly/job/nightly_win_rel/lastBuild/testReport/rclcpp_components/) + +### Documentation Policy [2.v] + +All pull requests must resolve related documentation changes before merging. + +## Documentation [3] + +### Feature Documentation [3.i] + +`rclcpp_components` has a [feature list](http://docs.ros2.org/latest/api/rclcpp_components/) and each item in the list links to the corresponding feature documentation. There is documentation for all of the features, and new features require documentation before being added. + +### Public API Documentation [3.ii] + +The API is publicly available in its [ROS 2 API documentation](http://docs.ros2.org/latest/api/rclcpp_components/). + +### License [3.iii] + +The license for `rclcpp_components` 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](http://build.ros2.org/view/Rpr/job/Rpr__rclcpp__ubuntu_focal_amd64/lastCompletedBuild/testReport/rclcpp_components/) 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 `rclcpp_components`. + +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](http://build.ros2.org/view/Rpr/job/Rpr__rclcpp__ubuntu_focal_amd64/lastCompletedBuild/testReport/rclcpp_components/copyright). + +## Testing [4] + +### Feature Testing [4.i] + +Each feature in `rclcpp_components` has corresponding tests which simulate typical usage, and they are located in the [`test`](https://github.com/ros2/rclcpp_components/tree/rolling/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/rclcpp_components/) +* [linux_release](https://ci.ros2.org/view/nightly/job/nightly_linux_release/lastBuild/testReport/rclcpp_components/) +* [mac_osx_release](https://ci.ros2.org/view/nightly/job/nightly_osx_release/lastBuild/testReport/rclcpp_components/) +* [windows_release](https://ci.ros2.org/view/nightly/job/nightly_win_rel/lastBuild/testReport/rclcpp_components/) + +### 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. + +### Coverage [4.iii] + +`rclcpp_components` 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 +- achieving and maintaining a reasonable branch line coverage (90-100%) +- no lines are manually skipped in coverage calculations + +Changes are required to make a best effort to keep or increase coverage before being accepted, but decreases are allowed if properly justified and accepted by reviewers. + +Current coverage statistics can be viewed [here](https://ci.ros2.org/job/nightly_linux_coverage/lastCompletedBuild/cobertura/src_ros2_rclcpp_rclcpp_components_src/). 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] + +`rclcpp_components` follows the recommendations for performance testing of C/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. + +The performance tests of `rclcpp_components` are located in the [test/benchmark directory](https://github.com/ros2/rclcpp/tree/rolling/rclcpp_components/test/benchmark). + +Package and system level performance benchmarks that cover features of `rclcpp_components` 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] + +`rclcpp_components` uses and passes all the ROS 2 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/rclcpp_components/) +* [linux_release](https://ci.ros2.org/view/nightly/job/nightly_linux_release/lastBuild/testReport/rclcpp_components/) +* [mac_osx_release](https://ci.ros2.org/view/nightly/job/nightly_osx_release/lastBuild/testReport/rclcpp_components/) +* [windows_release](https://ci.ros2.org/view/nightly/job/nightly_win_rel/lastBuild/testReport/rclcpp_components/) + +## Dependencies [5] + +Below are evaluations of each of `rclcpp_components`'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 and Optional Runtime ROS Dependencies [5.i]/[5.ii] + +`rclcpp_components` has the following runtime ROS dependencies: + +#### `ament_index_cpp` + +The `ament_index_cpp` package provides a C++ API to access the ament resource index. + +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ament/ament_index/blob/rolling/ament_index_cpp/QUALITY_DECLARATION.md). + +#### `class_loader` + +The `class_loader` package provides a ROS-independent package for loading plugins during runtime + +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros/class_loader/blob/ros2/QUALITY_DECLARATION.md). + +#### `composition_interfaces` + +The `composition_interfaces` package contains message and service definitions for managing composable nodes in a container process. + +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcl_interfaces/blob/rolling/composition_interfaces/QUALITY_DECLARATION.md). + +#### `rclcpp` + +The `rclcpp` package provides the ROS client library in C++. + +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rclcpp/blob/rolling/rclcpp/QUALITY_DECLARATION.md). + +#### `rcpputils` + +The `rcpputils` package provides an API which contains common utilities and data structures useful when programming in C++. + +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcpputils/blob/rolling/QUALITY_DECLARATION.md). + +### Direct Runtime non-ROS Dependency [5.iii] + +`rclcpp_components` has no run-time or build-time dependencies that need to be considered for this declaration. + +## Platform Support [6] + +`rclcpp_components` supports all of the tier 1 platforms as described in [REP-2000](https://www.ros.org/reps/rep-2000.html#support-tiers), and tests each change against all of them. + +Currently nightly build status can be seen here: +* [linux-aarch64_release](https://ci.ros2.org/view/nightly/job/nightly_linux-aarch64_release/lastBuild/rclcpp_components/) +* [linux_release](https://ci.ros2.org/view/nightly/job/nightly_linux_release/lastBuild/rclcpp_components/) +* [mac_osx_release](https://ci.ros2.org/view/nightly/job/nightly_osx_release/lastBuild/rclcpp_components/) +* [windows_release](https://ci.ros2.org/view/nightly/job/nightly_win_rel/lastBuild/rclcpp_components/) + +## Security + +### Vulnerability Disclosure Policy [7.i] + +This package conforms to the Vulnerability Disclosure Policy in [REP-2006](https://www.ros.org/reps/rep-2006.html). diff --git a/rclcpp_components/README.md b/rclcpp_components/README.md new file mode 100644 index 0000000000..fcc4c04ea0 --- /dev/null +++ b/rclcpp_components/README.md @@ -0,0 +1,9 @@ +# `rclcpp_components` + +Package containing tools for dynamically loadable components. + +The link to the latest rclcpp_components API documentation, which includes a complete list of its main components and features, can be found on the [rclcpp_components package info page](https://docs.ros.org/en/rolling/p/rclcpp_components). + +## Quality Declaration + +This package claims to be in the **Quality Level 1** category, see the [Quality Declaration](QUALITY_DECLARATION.md) for more details. diff --git a/rclcpp/cmake/rclcpp_package_hook.cmake b/rclcpp_components/cmake/rclcpp_components_package_hook.cmake similarity index 64% rename from rclcpp/cmake/rclcpp_package_hook.cmake rename to rclcpp_components/cmake/rclcpp_components_package_hook.cmake index 9e73620f79..268bc5bedb 100644 --- a/rclcpp/cmake/rclcpp_package_hook.cmake +++ b/rclcpp_components/cmake/rclcpp_components_package_hook.cmake @@ -1,4 +1,4 @@ -# Copyright 2016 Open Source Robotics Foundation, Inc. +# 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. @@ -13,5 +13,9 @@ # limitations under the License. # register node plugins -ament_index_register_resource( - "node_plugin" CONTENT "${_RCLCPP__NODE_PLUGINS}") +list(REMOVE_DUPLICATES _RCLCPP_COMPONENTS_PACKAGE_RESOURCE_INDICES) +foreach(resource_index ${_RCLCPP_COMPONENTS_PACKAGE_RESOURCE_INDICES}) + ament_index_register_resource( + ${resource_index} CONTENT "${_RCLCPP_COMPONENTS_${resource_index}__NODES}") +endforeach() + diff --git a/rclcpp_components/cmake/rclcpp_components_register_node.cmake b/rclcpp_components/cmake/rclcpp_components_register_node.cmake new file mode 100644 index 0000000000..0c5bd33b91 --- /dev/null +++ b/rclcpp_components/cmake/rclcpp_components_register_node.cmake @@ -0,0 +1,83 @@ +# 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. + +# Register an rclcpp component with the ament +# resource index and create an executable. +# +# usage: rclcpp_components_register_node( +# PLUGIN EXECUTABLE ) +# +# :param target: the shared library target +# :type target: string +# :param PLUGIN: the plugin name +# :type PLUGIN: string +# :param EXECUTABLE: the node's executable name +# :type EXECUTABLE: string +# :param EXECUTOR: the C++ class name of the executor to use (blank uses SingleThreadedExecutor) +# :type EXECUTOR: string +# :param RESOURCE_INDEX: the ament resource index to register the components +# :type RESOURCE_INDEX: string +# +macro(rclcpp_components_register_node target) + cmake_parse_arguments(ARGS "" "PLUGIN;EXECUTABLE;EXECUTOR;RESOURCE_INDEX" "" ${ARGN}) + if(ARGS_UNPARSED_ARGUMENTS) + message(FATAL_ERROR "rclcpp_components_register_node() called with unused " + "arguments: ${ARGS_UNPARSED_ARGUMENTS}") + endif() + if("${ARGS_PLUGIN}" STREQUAL "") + message(FATAL_ERROR "rclcpp_components_register_node macro requires a PLUGIN argument for target ${target}") + endif() + if("${ARGS_EXECUTABLE}" STREQUAL "") + message(FATAL_ERROR "rclcpp_components_register_node macro requires a EXECUTABLE argument for target ${target}") + endif() + # default to rclcpp_components if not specified otherwise + set(resource_index "rclcpp_components") + if(NOT "${ARGS_RESOURCE_INDEX}" STREQUAL "") + set(resource_index ${ARGS_RESOURCE_INDEX}) + message(STATUS "Setting component resource index to non-default value ${resource_index}") + endif() + + # default to executor if not specified otherwise + set(executor "SingleThreadedExecutor") + if(NOT "${ARGS_EXECUTOR}" STREQUAL "") + set(executor ${ARGS_EXECUTOR}) + message(STATUS "Setting executor non-default value ${executor}") + endif() + + set(component ${ARGS_PLUGIN}) + set(node ${ARGS_EXECUTABLE}) + _rclcpp_components_register_package_hook() + set(_path "lib") + set(library_name "$") + if(WIN32) + set(_path "bin") + endif() + set(_RCLCPP_COMPONENTS_${resource_index}__NODES + "${_RCLCPP_COMPONENTS_${resource_index}__NODES}${component};${_path}/$\n") + list(APPEND _RCLCPP_COMPONENTS_PACKAGE_RESOURCE_INDICES ${resource_index}) + + configure_file(${rclcpp_components_NODE_TEMPLATE} + ${PROJECT_BINARY_DIR}/rclcpp_components/node_main_configured_${node}.cpp.in) + file(GENERATE OUTPUT ${PROJECT_BINARY_DIR}/rclcpp_components/node_main_${node}.cpp + INPUT ${PROJECT_BINARY_DIR}/rclcpp_components/node_main_configured_${node}.cpp.in) + add_executable(${node} ${PROJECT_BINARY_DIR}/rclcpp_components/node_main_${node}.cpp) + target_link_libraries(${node} + class_loader::class_loader + rclcpp::rclcpp + rclcpp_components::component + ) + install(TARGETS + ${node} + DESTINATION lib/${PROJECT_NAME}) +endmacro() diff --git a/rclcpp/cmake/rclcpp_register_node_plugins.cmake b/rclcpp_components/cmake/rclcpp_components_register_nodes.cmake similarity index 53% rename from rclcpp/cmake/rclcpp_register_node_plugins.cmake rename to rclcpp_components/cmake/rclcpp_components_register_nodes.cmake index f8675f823f..e80550a3ce 100644 --- a/rclcpp/cmake/rclcpp_register_node_plugins.cmake +++ b/rclcpp_components/cmake/rclcpp_components_register_nodes.cmake @@ -1,4 +1,4 @@ -# Copyright 2016 Open Source Robotics Foundation, Inc. +# 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. @@ -13,38 +13,47 @@ # limitations under the License. # -# Register a node plugin with the ament resource index. +# Register an rclcpp component with the ament resource index. # -# The passed library can contain multiple plugins extending the node interface. +# The passed library can contain multiple nodes each registered via macro. # # :param target: the shared library target # :type target: string # :param ARGN: the unique plugin names being exported using class_loader # :type ARGN: list of strings +# :param RESOURCE_INDEX: the ament resource index to register the components +# :type RESOURCE_INDEX: string # -macro(rclcpp_register_node_plugins target) +macro(rclcpp_components_register_nodes target) if(NOT TARGET ${target}) message( FATAL_ERROR - "rclcpp_register_node_plugins() first argument " + "rclcpp_components_register_nodes() first argument " "'${target}' is not a target") endif() + cmake_parse_arguments(ARGS "" "RESOURCE_INDEX" "" ${ARGN}) + # default to rclcpp_components if not specified otherwise + set(resource_index "rclcpp_components") + if(NOT "${ARGS_RESOURCE_INDEX}" STREQUAL "") + set(resource_index ${ARGS_RESOURCE_INDEX}) + message(STATUS "Setting component resource index to non-default value ${resource_index}") + endif() get_target_property(_target_type ${target} TYPE) if(NOT _target_type STREQUAL "SHARED_LIBRARY") message( FATAL_ERROR - "rclcpp_register_node_plugins() first argument " + "rclcpp_components_register_nodes() first argument " "'${target}' is not a shared library target") endif() if(${ARGC} GREATER 0) - _rclcpp_register_package_hook() + _rclcpp_components_register_package_hook() set(_unique_names) - foreach(_arg ${ARGN}) + foreach(_arg ${ARGS_UNPARSED_ARGUMENTS}) if(_arg IN_LIST _unique_names) message( FATAL_ERROR - "rclcpp_register_node_plugins() the plugin names " + "rclcpp_components_register_nodes() the plugin names " "must be unique (multiple '${_arg}')") endif() list(APPEND _unique_names "${_arg}") @@ -54,8 +63,10 @@ macro(rclcpp_register_node_plugins target) else() set(_path "lib") endif() - set(_RCLCPP__NODE_PLUGINS - "${_RCLCPP__NODE_PLUGINS}${_arg};${_path}/$\n") + set(_RCLCPP_COMPONENTS_${resource_index}__NODES + "${_RCLCPP_COMPONENTS_${resource_index}__NODES}${_arg};${_path}/$\n") + list(APPEND _RCLCPP_COMPONENTS_PACKAGE_RESOURCE_INDICES ${resource_index}) endforeach() endif() endmacro() + diff --git a/rclcpp_components/include/rclcpp_components/component_manager.hpp b/rclcpp_components/include/rclcpp_components/component_manager.hpp new file mode 100644 index 0000000000..9d98bd89d4 --- /dev/null +++ b/rclcpp_components/include/rclcpp_components/component_manager.hpp @@ -0,0 +1,231 @@ +// 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. + +/** \mainpage rclcpp_components: Package containing tools for dynamically loadable components. + * + * - ComponentManager: Node to manage components. It has the services to load, unload and list + * current components. + * - rclcpp_components/component_manager.hpp) + * - Node factory: The NodeFactory interface is used by the class loader to instantiate components. + * - rclcpp_components/node_factory.hpp) + * - It allows for classes not derived from `rclcpp::Node` to be used as components. + * - It allows derived constructors to be called when components are loaded. + * + * Some useful abstractions and utilities: + * - [RCLCPP_COMPONENTS_REGISTER_NODE: Register a component that can be dynamically loaded + * at runtime. + * - (include/rclcpp_components/register_node_macro.hpp) + * + * Some useful internal abstractions and utilities: + * - Macros for controlling symbol visibility on the library + * - rclcpp_components/visibility_control.h + * + * Package containing CMake tools for register components: + * - `rclcpp_components_register_node` Register an rclcpp component with the ament resource index + * and create an executable. + * - `rclcpp_components_register_nodes` Register an rclcpp component with the ament resource index. + * The passed library can contain multiple nodes each registered via macro. + */ + +#ifndef RCLCPP_COMPONENTS__COMPONENT_MANAGER_HPP__ +#define RCLCPP_COMPONENTS__COMPONENT_MANAGER_HPP__ + +#include +#include +#include +#include +#include + +#include "composition_interfaces/srv/load_node.hpp" +#include "composition_interfaces/srv/unload_node.hpp" +#include "composition_interfaces/srv/list_nodes.hpp" + +#include "rclcpp/executor.hpp" +#include "rclcpp/node_options.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "rclcpp_components/node_factory.hpp" +#include "rclcpp_components/visibility_control.hpp" + +namespace class_loader +{ +class ClassLoader; +} // namespace class_loader + +namespace rclcpp_components +{ + +/// Thrown when an error happens in the component Manager class. +class ComponentManagerException : public std::runtime_error +{ +public: + explicit ComponentManagerException(const std::string & error_desc) + : std::runtime_error(error_desc) {} +}; + +/// ComponentManager handles the services to load, unload, and get the list of loaded components. +class ComponentManager : public rclcpp::Node +{ +public: + using LoadNode = composition_interfaces::srv::LoadNode; + using UnloadNode = composition_interfaces::srv::UnloadNode; + using ListNodes = composition_interfaces::srv::ListNodes; + + /// Represents a component resource. + /** + * Is a pair of class name (for class loader) and library path (absolute) + */ + using ComponentResource = std::pair; + + /// Default constructor + /** + * Initializes the component manager. It creates the services: load node, unload node + * and list nodes. + * + * \param executor the executor which will spin the node. + * \param node_name the name of the node that the data originates from. + * \param node_options additional options to control creation of the node. + */ + RCLCPP_COMPONENTS_PUBLIC + ComponentManager( + std::weak_ptr executor = + std::weak_ptr(), + std::string node_name = "ComponentManager", + const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions() + .start_parameter_services(false) + .start_parameter_event_publisher(false)); + + RCLCPP_COMPONENTS_PUBLIC + virtual ~ComponentManager(); + + /// Return a list of valid loadable components in a given package. + /** + * \param package_name name of the package + * \param resource_index name of the executable + * \throws ComponentManagerException if the resource was not found or a invalid resource entry + * \return a list of component resources + */ + RCLCPP_COMPONENTS_PUBLIC + virtual std::vector + get_component_resources( + const std::string & package_name, + const std::string & resource_index = "rclcpp_components") const; + + /// Instantiate a component from a dynamic library. + /** + * \param resource a component resource (class name + library path) + * \return a NodeFactory interface + */ + RCLCPP_COMPONENTS_PUBLIC + virtual std::shared_ptr + create_component_factory(const ComponentResource & resource); + + /// Member function to set a executor in the component + /** + * \param executor executor to be set + */ + RCLCPP_COMPONENTS_PUBLIC + virtual void + set_executor(const std::weak_ptr executor); + +protected: + /// Create node options for loaded component + /** + * \param request information with the node to load + * \return node options + */ + RCLCPP_COMPONENTS_PUBLIC + virtual rclcpp::NodeOptions + create_node_options(const std::shared_ptr request); + + /// Add component node to executor model, it's invoked in on_load_node() + /** + * \param node_id node_id of loaded component node in node_wrappers_ + */ + RCLCPP_COMPONENTS_PUBLIC + virtual void + add_node_to_executor(uint64_t node_id); + + /// Remove component node from executor model, it's invoked in on_unload_node() + /** + * \param node_id node_id of loaded component node in node_wrappers_ + */ + RCLCPP_COMPONENTS_PUBLIC + virtual void + remove_node_from_executor(uint64_t node_id); + + /// Service callback to load a new node in the component + /** + * This function allows to add parameters, remap rules, a specific node, name a namespace + * and/or additional arguments. + * + * \param request_header unused + * \param request information with the node to load + * \param response + * \throws std::overflow_error if node_id suffers an overflow. Very unlikely to happen at 1 kHz + * (very optimistic rate). it would take 585 years. + * \throws ComponentManagerException In the case that the component constructor throws an + * exception, rethrow into the following catch block. + */ + RCLCPP_COMPONENTS_PUBLIC + virtual void + on_load_node( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + + /// Service callback to unload a node in the component + /** + * \param request_header unused + * \param request unique identifier to remove from the component + * \param response true on the success field if the node unload was succefully, otherwise false + * and the error_message field contains the error. + */ + RCLCPP_COMPONENTS_PUBLIC + virtual void + on_unload_node( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + + /// Service callback to get the list of nodes in the component + /** + * Return a two list: one with the unique identifiers and other with full name of the nodes. + * + * \param request_header unused + * \param request unused + * \param response list with the unique ids and full node names + */ + RCLCPP_COMPONENTS_PUBLIC + virtual void + on_list_nodes( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + +protected: + std::weak_ptr executor_; + + uint64_t unique_id_ {1}; + std::map> loaders_; + std::map node_wrappers_; + + rclcpp::Service::SharedPtr loadNode_srv_; + rclcpp::Service::SharedPtr unloadNode_srv_; + rclcpp::Service::SharedPtr listNodes_srv_; +}; + +} // namespace rclcpp_components + +#endif // RCLCPP_COMPONENTS__COMPONENT_MANAGER_HPP__ diff --git a/rclcpp_components/include/rclcpp_components/component_manager_isolated.hpp b/rclcpp_components/include/rclcpp_components/component_manager_isolated.hpp new file mode 100644 index 0000000000..e2061e4da2 --- /dev/null +++ b/rclcpp_components/include/rclcpp_components/component_manager_isolated.hpp @@ -0,0 +1,136 @@ +// 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 RCLCPP_COMPONENTS__COMPONENT_MANAGER_ISOLATED_HPP__ +#define RCLCPP_COMPONENTS__COMPONENT_MANAGER_ISOLATED_HPP__ + +#include +#include +#include +#include +#include +#include + +#include "rclcpp_components/component_manager.hpp" + + +namespace rclcpp_components +{ +/// ComponentManagerIsolated uses dedicated single-threaded executors for each components. +template +class ComponentManagerIsolated : public rclcpp_components::ComponentManager +{ + using rclcpp_components::ComponentManager::ComponentManager; + + struct DedicatedExecutorWrapper + { + std::shared_ptr executor; + std::thread thread; + std::atomic_bool thread_initialized; + + /// Constructor for the wrapper. + /// This is necessary as atomic variables don't have copy/move operators + /// implemented so this structure is not copyable/movable by default + explicit DedicatedExecutorWrapper(std::shared_ptr exec) + : executor(exec), + thread_initialized(false) + { + } + }; + +public: + ~ComponentManagerIsolated() + { + if (node_wrappers_.size()) { + for (auto & executor_wrapper : dedicated_executor_wrappers_) { + cancel_executor(executor_wrapper.second); + } + node_wrappers_.clear(); + } + } + +protected: + /// Add component node to executor model, it's invoked in on_load_node() + /** + * \param node_id node_id of loaded component node in node_wrappers_ + */ + void + add_node_to_executor(uint64_t node_id) override + { + auto exec = std::make_shared(); + exec->add_node(node_wrappers_[node_id].get_node_base_interface()); + + // Emplace rather than std::move since move operations are deleted for atomics + auto result = dedicated_executor_wrappers_.emplace(std::make_pair(node_id, exec)); + DedicatedExecutorWrapper & wrapper = result.first->second; + wrapper.executor = exec; + auto & thread_initialized = wrapper.thread_initialized; + wrapper.thread = std::thread( + [exec, &thread_initialized]() { + thread_initialized = true; + exec->spin(); + }); + } + /// Remove component node from executor model, it's invoked in on_unload_node() + /** + * \param node_id node_id of loaded component node in node_wrappers_ + */ + void + remove_node_from_executor(uint64_t node_id) override + { + auto executor_wrapper = dedicated_executor_wrappers_.find(node_id); + if (executor_wrapper != dedicated_executor_wrappers_.end()) { + cancel_executor(executor_wrapper->second); + dedicated_executor_wrappers_.erase(executor_wrapper); + } + } + +private: + /// Stops a spinning executor avoiding race conditions. + /** + * @param executor_wrapper executor to stop and its associated thread + */ + void cancel_executor(DedicatedExecutorWrapper & executor_wrapper) + { + // Verify that the executor thread has begun spinning. + // If it has not, then wait until the thread starts to ensure + // that cancel() will fully stop the execution + // + // This prevents a previous race condition that occurs between the + // creation of the executor spin thread and cancelling an executor + + if (!executor_wrapper.thread_initialized) { + auto context = this->get_node_base_interface()->get_context(); + + // Guarantee that either the executor is spinning or we are shutting down. + while (!executor_wrapper.executor->is_spinning() && rclcpp::ok(context)) { + // This is an arbitrarily small delay to avoid busy looping + rclcpp::sleep_for(std::chrono::milliseconds(1)); + } + } + + // After the while loop we are sure that the executor is now spinning, so + // the call to cancel() will work. + executor_wrapper.executor->cancel(); + // Wait for the thread task to return + executor_wrapper.thread.join(); + } + + std::unordered_map dedicated_executor_wrappers_; +}; + +} // namespace rclcpp_components + +#endif // RCLCPP_COMPONENTS__COMPONENT_MANAGER_ISOLATED_HPP__ diff --git a/rclcpp_components/include/rclcpp_components/node_factory.hpp b/rclcpp_components/include/rclcpp_components/node_factory.hpp new file mode 100644 index 0000000000..7b1f2dcae6 --- /dev/null +++ b/rclcpp_components/include/rclcpp_components/node_factory.hpp @@ -0,0 +1,47 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP_COMPONENTS__NODE_FACTORY_HPP__ +#define RCLCPP_COMPONENTS__NODE_FACTORY_HPP__ + +#include "rclcpp/node_options.hpp" +#include "rclcpp_components/node_instance_wrapper.hpp" + +namespace rclcpp_components +{ + +/// The NodeFactory interface is used by the class loader to instantiate components. +/** + * The NodeFactory interface serves two purposes: + * * It allows for classes not derived from `rclcpp::Node` to be used as components. + * * It allows derived constructors to be called when components are loaded. + */ +class NodeFactory +{ +public: + NodeFactory() = default; + + virtual ~NodeFactory() = default; + + /// Create an instance of a component + /** + * \param[in] options Additional options used in the construction of the component. + */ + virtual + NodeInstanceWrapper + create_node_instance(const rclcpp::NodeOptions & options) = 0; +}; +} // namespace rclcpp_components + +#endif // RCLCPP_COMPONENTS__NODE_FACTORY_HPP__ diff --git a/rclcpp_components/include/rclcpp_components/node_factory_template.hpp b/rclcpp_components/include/rclcpp_components/node_factory_template.hpp new file mode 100644 index 0000000000..988b8036bb --- /dev/null +++ b/rclcpp_components/include/rclcpp_components/node_factory_template.hpp @@ -0,0 +1,53 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP_COMPONENTS__NODE_FACTORY_TEMPLATE_HPP__ +#define RCLCPP_COMPONENTS__NODE_FACTORY_TEMPLATE_HPP__ + +#include +#include + +#include "rclcpp_components/node_factory.hpp" + +namespace rclcpp_components +{ + +/// NodeFactoryTemplate is a convenience class for instantiating components. +/** + * The NodeFactoryTemplate class can be used to provide the NodeFactory interface for + * components that implement a single-argument constructor and `get_node_base_interface`. + */ +template +class NodeFactoryTemplate : public NodeFactory +{ +public: + NodeFactoryTemplate() = default; + virtual ~NodeFactoryTemplate() = default; + + /// Create an instance of a component + /** + * \param[in] options Additional options used in the construction of the component. + */ + NodeInstanceWrapper + create_node_instance(const rclcpp::NodeOptions & options) override + { + auto node = std::make_shared(options); + + return NodeInstanceWrapper( + node, std::bind(&NodeT::get_node_base_interface, node)); + } +}; +} // namespace rclcpp_components + +#endif // RCLCPP_COMPONENTS__NODE_FACTORY_TEMPLATE_HPP__ diff --git a/rclcpp_components/include/rclcpp_components/node_instance_wrapper.hpp b/rclcpp_components/include/rclcpp_components/node_instance_wrapper.hpp new file mode 100644 index 0000000000..2705e4e8b3 --- /dev/null +++ b/rclcpp_components/include/rclcpp_components/node_instance_wrapper.hpp @@ -0,0 +1,71 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP_COMPONENTS__NODE_INSTANCE_WRAPPER_HPP__ +#define RCLCPP_COMPONENTS__NODE_INSTANCE_WRAPPER_HPP__ + +#include +#include + +#include "rclcpp/node_interfaces/node_base_interface.hpp" + +namespace rclcpp_components +{ +/// The NodeInstanceWrapper encapsulates the node instance. +class NodeInstanceWrapper +{ +public: + using NodeBaseInterfaceGetter = std::function< + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr(const std::shared_ptr &)>; + + NodeInstanceWrapper() + : node_instance_(nullptr) + {} + + NodeInstanceWrapper( + std::shared_ptr node_instance, + NodeBaseInterfaceGetter node_base_interface_getter) + : node_instance_(node_instance), node_base_interface_getter_(node_base_interface_getter) + {} + + /// Get a type-erased pointer to the original Node instance + /** + * This is only for debugging and special cases. + * For most cases `get_node_base_interface` will be sufficient. + * + * \return Shared pointer to the encapsulated Node instance. + */ + const std::shared_ptr + get_node_instance() const + { + return node_instance_; + } + + /// Get NodeBaseInterface pointer for the encapsulated Node Instance. + /** + * \return Shared NodeBaseInterface pointer of the encapsulated Node instance. + */ + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr + get_node_base_interface() + { + return node_base_interface_getter_(node_instance_); + } + +private: + std::shared_ptr node_instance_; + NodeBaseInterfaceGetter node_base_interface_getter_; +}; +} // namespace rclcpp_components + +#endif // RCLCPP_COMPONENTS__NODE_INSTANCE_WRAPPER_HPP__ diff --git a/rclcpp_components/include/rclcpp_components/register_node_macro.hpp b/rclcpp_components/include/rclcpp_components/register_node_macro.hpp new file mode 100644 index 0000000000..340d8021bb --- /dev/null +++ b/rclcpp_components/include/rclcpp_components/register_node_macro.hpp @@ -0,0 +1,38 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP_COMPONENTS__REGISTER_NODE_MACRO_HPP__ +#define RCLCPP_COMPONENTS__REGISTER_NODE_MACRO_HPP__ + +#include "class_loader/class_loader.hpp" +#include "rclcpp_components/node_factory_template.hpp" + +/// Register a component that can be dynamically loaded at runtime. +/** + * The registration macro should appear once per component per library. + * The macro should appear in a single translation unit. + * + * Valid arguments for NodeClass shall: + * * Have a constructor that takes a single argument that is a `rclcpp::NodeOptions` instance. + * * Have a method of of the signature: + * `rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface` + * + * Note: NodeClass does not need to inherit from `rclcpp::Node`, but it is the easiest way. + */ +#define RCLCPP_COMPONENTS_REGISTER_NODE(NodeClass) \ + CLASS_LOADER_REGISTER_CLASS( \ + rclcpp_components::NodeFactoryTemplate, \ + rclcpp_components::NodeFactory) + +#endif // RCLCPP_COMPONENTS__REGISTER_NODE_MACRO_HPP__ diff --git a/rclcpp_components/include/rclcpp_components/visibility_control.hpp b/rclcpp_components/include/rclcpp_components/visibility_control.hpp new file mode 100644 index 0000000000..fff5cdd3a9 --- /dev/null +++ b/rclcpp_components/include/rclcpp_components/visibility_control.hpp @@ -0,0 +1,56 @@ +// 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. + +/* This header must be included by all rclcpp headers which declare symbols + * which are defined in the rclcpp library. When not building the rclcpp + * library, i.e. when using the headers in other package's code, the contents + * of this header change the visibility of certain symbols which the rclcpp + * library cannot have, but the consuming code must have inorder to link. + */ + +#ifndef RCLCPP_COMPONENTS__VISIBILITY_CONTROL_HPP_ +#define RCLCPP_COMPONENTS__VISIBILITY_CONTROL_HPP_ + +// 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 RCLCPP_COMPONENTS_EXPORT __attribute__ ((dllexport)) + #define RCLCPP_COMPONENTS_IMPORT __attribute__ ((dllimport)) + #else + #define RCLCPP_COMPONENTS_EXPORT __declspec(dllexport) + #define RCLCPP_COMPONENTS_IMPORT __declspec(dllimport) + #endif + #ifdef RCLCPP_COMPONENTS_BUILDING_LIBRARY + #define RCLCPP_COMPONENTS_PUBLIC RCLCPP_COMPONENTS_EXPORT + #else + #define RCLCPP_COMPONENTS_PUBLIC RCLCPP_COMPONENTS_IMPORT + #endif + #define RCLCPP_COMPONENTS_PUBLIC_TYPE RCLCPP_COMPONENTS_PUBLIC + #define RCLCPP_COMPONENTS_LOCAL +#else + #define RCLCPP_COMPONENTS_EXPORT __attribute__ ((visibility("default"))) + #define RCLCPP_COMPONENTS_IMPORT + #if __GNUC__ >= 4 + #define RCLCPP_COMPONENTS_PUBLIC __attribute__ ((visibility("default"))) + #define RCLCPP_COMPONENTS_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define RCLCPP_COMPONENTS_PUBLIC + #define RCLCPP_COMPONENTS_LOCAL + #endif + #define RCLCPP_COMPONENTS_PUBLIC_TYPE +#endif + +#endif // RCLCPP_COMPONENTS__VISIBILITY_CONTROL_HPP_ diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml new file mode 100644 index 0000000000..ff9c20a182 --- /dev/null +++ b/rclcpp_components/package.xml @@ -0,0 +1,41 @@ + + + + rclcpp_components + 29.2.0 + Package containing tools for dynamically loadable components + + Ivan Paunovic + Michel Hidalgo + William Woodall + + Apache License 2.0 + + Jacob Perron + Michael Carroll + + ament_cmake_ros + + ament_index_cpp + class_loader + composition_interfaces + rclcpp + rcpputils + + ament_index_cpp + class_loader + composition_interfaces + rclcpp + + ament_cmake_google_benchmark + ament_cmake_gtest + ament_lint_auto + ament_lint_common + launch_testing + std_msgs + + + ament_cmake + + + diff --git a/rclcpp_components/rclcpp_components-extras.cmake.in b/rclcpp_components/rclcpp_components-extras.cmake.in new file mode 100644 index 0000000000..45a4e5ac0d --- /dev/null +++ b/rclcpp_components/rclcpp_components-extras.cmake.in @@ -0,0 +1,32 @@ +# Copyright 2019 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# copied from rclcpp_components/rclcpp_components-extras.cmake + +# register ament_package() hook for node plugins once. +macro(_rclcpp_components_register_package_hook) + if(NOT DEFINED _RCLCPP_COMPONENTS_PACKAGE_HOOK_REGISTERED) + set(_RCLCPP_COMPONENTS_PACKAGE_HOOK_REGISTERED TRUE) + + find_package(ament_cmake_core QUIET REQUIRED) + ament_register_extension("ament_package" "rclcpp_components" + "rclcpp_components_package_hook.cmake") + endif() +endmacro() + +get_filename_component(@PROJECT_NAME@_SHARE_DIR "${@PROJECT_NAME@_DIR}" DIRECTORY) +set(@PROJECT_NAME@_NODE_TEMPLATE "${@PROJECT_NAME@_SHARE_DIR}/node_main.cpp.in") + +include("${rclcpp_components_DIR}/rclcpp_components_register_nodes.cmake") +include("${rclcpp_components_DIR}/rclcpp_components_register_node.cmake") diff --git a/rclcpp_components/src/component_container.cpp b/rclcpp_components/src/component_container.cpp new file mode 100644 index 0000000000..64083912de --- /dev/null +++ b/rclcpp_components/src/component_container.cpp @@ -0,0 +1,29 @@ +// 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 "rclcpp/rclcpp.hpp" + +#include "rclcpp_components/component_manager.hpp" + +int main(int argc, char * argv[]) +{ + /// Component container with a single-threaded executor. + rclcpp::init(argc, argv); + auto exec = std::make_shared(); + auto node = std::make_shared(exec); + exec->add_node(node); + exec->spin(); +} diff --git a/rclcpp_components/src/component_container_isolated.cpp b/rclcpp_components/src/component_container_isolated.cpp new file mode 100644 index 0000000000..96ba8b1a03 --- /dev/null +++ b/rclcpp_components/src/component_container_isolated.cpp @@ -0,0 +1,49 @@ +// 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 "rclcpp/rclcpp.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp_components/component_manager_isolated.hpp" + +int main(int argc, char * argv[]) +{ + /// Component container with dedicated single-threaded executors for each components. + rclcpp::init(argc, argv); + // parse arguments + bool use_multi_threaded_executor{false}; + std::vector args = rclcpp::remove_ros_arguments(argc, argv); + for (auto & arg : args) { + if (arg == std::string("--use_multi_threaded_executor")) { + use_multi_threaded_executor = true; + } + } + // create executor and component manager + auto exec = std::make_shared(); + rclcpp::Node::SharedPtr node; + if (use_multi_threaded_executor) { + using ComponentManagerIsolated = + rclcpp_components::ComponentManagerIsolated; + node = std::make_shared(exec); + } else { + using ComponentManagerIsolated = + rclcpp_components::ComponentManagerIsolated; + node = std::make_shared(exec); + } + exec->add_node(node); + exec->spin(); +} diff --git a/rclcpp_components/src/component_container_mt.cpp b/rclcpp_components/src/component_container_mt.cpp new file mode 100644 index 0000000000..9dcbade712 --- /dev/null +++ b/rclcpp_components/src/component_container_mt.cpp @@ -0,0 +1,38 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "rclcpp/rclcpp.hpp" + +#include "rclcpp_components/component_manager.hpp" + +int main(int argc, char * argv[]) +{ + /// Component container with a multi-threaded executor. + rclcpp::init(argc, argv); + + auto exec = std::make_shared(); + auto node = std::make_shared(); + if (node->has_parameter("thread_num")) { + const auto thread_num = node->get_parameter("thread_num").as_int(); + exec = std::make_shared( + rclcpp::ExecutorOptions{}, thread_num); + node->set_executor(exec); + } else { + node->set_executor(exec); + } + exec->add_node(node); + exec->spin(); +} diff --git a/rclcpp_components/src/component_manager.cpp b/rclcpp_components/src/component_manager.cpp new file mode 100644 index 0000000000..f73cd8954a --- /dev/null +++ b/rclcpp_components/src/component_manager.cpp @@ -0,0 +1,326 @@ +// 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 "rclcpp_components/component_manager.hpp" + +#include +#include +#include +#include +#include +#include + +#include "ament_index_cpp/get_resource.hpp" +#include "class_loader/class_loader.hpp" +#include "rcpputils/filesystem_helper.hpp" +#include "rcpputils/split.hpp" + +using namespace std::placeholders; + +namespace rclcpp_components +{ + +ComponentManager::ComponentManager( + std::weak_ptr executor, + std::string node_name, + const rclcpp::NodeOptions & node_options) +: Node(std::move(node_name), node_options), + executor_(executor) +{ + loadNode_srv_ = create_service( + "~/_container/load_node", + std::bind(&ComponentManager::on_load_node, this, _1, _2, _3), + rclcpp::ServicesQoS().keep_last(200)); + unloadNode_srv_ = create_service( + "~/_container/unload_node", + std::bind(&ComponentManager::on_unload_node, this, _1, _2, _3), + rclcpp::ServicesQoS().keep_last(200)); + listNodes_srv_ = create_service( + "~/_container/list_nodes", + std::bind(&ComponentManager::on_list_nodes, this, _1, _2, _3)); + + { + rcl_interfaces::msg::ParameterDescriptor desc{}; + desc.description = "Number of thread"; + rcl_interfaces::msg::IntegerRange range{}; + range.from_value = 1; + range.to_value = std::thread::hardware_concurrency(); + desc.integer_range.push_back(range); + desc.read_only = true; + this->declare_parameter( + "thread_num", static_cast(std::thread::hardware_concurrency()), desc); + } +} + +ComponentManager::~ComponentManager() +{ + if (node_wrappers_.size()) { + RCLCPP_DEBUG(get_logger(), "Removing components from executor"); + if (auto exec = executor_.lock()) { + for (auto & wrapper : node_wrappers_) { + exec->remove_node(wrapper.second.get_node_base_interface()); + } + } + } +} + +std::vector +ComponentManager::get_component_resources( + const std::string & package_name, const std::string & resource_index) const +{ + std::string content; + std::string base_path; + if ( + !ament_index_cpp::get_resource( + resource_index, package_name, content, &base_path)) + { + throw ComponentManagerException("Could not find requested resource in ament index"); + } + + std::vector resources; + std::vector lines = rcpputils::split(content, '\n', true); + for (const auto & line : lines) { + std::vector parts = rcpputils::split(line, ';'); + if (parts.size() != 2) { + throw ComponentManagerException("Invalid resource entry"); + } + + std::filesystem::path library_path = parts[1]; + if (!library_path.is_absolute()) { + library_path = (base_path / library_path); + } + resources.push_back({parts[0], library_path.string()}); + } + return resources; +} + +std::shared_ptr +ComponentManager::create_component_factory(const ComponentResource & resource) +{ + std::string library_path = resource.second; + std::string class_name = resource.first; + std::string fq_class_name = "rclcpp_components::NodeFactoryTemplate<" + class_name + ">"; + + class_loader::ClassLoader * loader; + if (loaders_.find(library_path) == loaders_.end()) { + RCLCPP_INFO(get_logger(), "Load Library: %s", library_path.c_str()); + try { + loaders_[library_path] = std::make_unique(library_path); + } catch (const std::exception & ex) { + throw ComponentManagerException("Failed to load library: " + std::string(ex.what())); + } catch (...) { + throw ComponentManagerException("Failed to load library"); + } + } + loader = loaders_[library_path].get(); + + auto classes = loader->getAvailableClasses(); + for (const auto & clazz : classes) { + RCLCPP_INFO(get_logger(), "Found class: %s", clazz.c_str()); + if (clazz == class_name || clazz == fq_class_name) { + RCLCPP_INFO(get_logger(), "Instantiate class: %s", clazz.c_str()); + return loader->createInstance(clazz); + } + } + return {}; +} + +rclcpp::NodeOptions +ComponentManager::create_node_options(const std::shared_ptr request) +{ + std::vector parameters; + for (const auto & p : request->parameters) { + parameters.push_back(rclcpp::Parameter::from_parameter_msg(p)); + } + + std::vector remap_rules; + remap_rules.reserve(request->remap_rules.size() * 2 + 1); + remap_rules.push_back("--ros-args"); + for (const std::string & rule : request->remap_rules) { + remap_rules.push_back("-r"); + remap_rules.push_back(rule); + } + + if (!request->node_name.empty()) { + remap_rules.push_back("-r"); + remap_rules.push_back("__node:=" + request->node_name); + } + + if (!request->node_namespace.empty()) { + remap_rules.push_back("-r"); + remap_rules.push_back("__ns:=" + request->node_namespace); + } + + auto options = rclcpp::NodeOptions() + .use_global_arguments(false) + .parameter_overrides(parameters) + .arguments(remap_rules); + + for (const auto & a : request->extra_arguments) { + const rclcpp::Parameter extra_argument = rclcpp::Parameter::from_parameter_msg(a); + if (extra_argument.get_name() == "use_intra_process_comms") { + if (extra_argument.get_type() != rclcpp::ParameterType::PARAMETER_BOOL) { + throw ComponentManagerException( + "Extra component argument 'use_intra_process_comms' must be a boolean"); + } + options.use_intra_process_comms(extra_argument.get_value()); + } else if (extra_argument.get_name() == "forward_global_arguments") { + if (extra_argument.get_type() != rclcpp::ParameterType::PARAMETER_BOOL) { + throw ComponentManagerException( + "Extra component argument 'forward_global_arguments' must be a boolean"); + } + options.use_global_arguments(extra_argument.get_value()); + if (extra_argument.get_value()) { + RCLCPP_WARN( + get_logger(), "forward_global_arguments is true by default in nodes, but is not " + "recommended in a component manager. If true, this will cause this node's behavior " + "to be influenced by global arguments, not only those targeted at this node."); + } + } + } + + return options; +} + +void +ComponentManager::set_executor(const std::weak_ptr executor) +{ + executor_ = executor; +} + +void +ComponentManager::add_node_to_executor(uint64_t node_id) +{ + if (auto exec = executor_.lock()) { + exec->add_node(node_wrappers_[node_id].get_node_base_interface(), true); + } +} + +void +ComponentManager::remove_node_from_executor(uint64_t node_id) +{ + if (auto exec = executor_.lock()) { + exec->remove_node(node_wrappers_[node_id].get_node_base_interface()); + } +} + +void +ComponentManager::on_load_node( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response) +{ + (void) request_header; + + try { + auto resources = get_component_resources(request->package_name); + + for (const auto & resource : resources) { + if (resource.first != request->plugin_name) { + continue; + } + auto factory = create_component_factory(resource); + + if (factory == nullptr) { + continue; + } + + auto options = create_node_options(request); + auto node_id = unique_id_++; + + if (0 == node_id) { + // This puts a technical limit on the number of times you can add a component. + // But even if you could add (and remove) them at 1 kHz (very optimistic rate) + // it would still be a very long time before you could exhaust the pool of id's: + // 2^64 / 1000 times per sec / 60 sec / 60 min / 24 hours / 365 days = 584,942,417 years + // So around 585 million years. Even at 1 GHz, it would take 585 years. + // I think it's safe to avoid trying to handle overflow. + // If we roll over then it's most likely a bug. + throw std::overflow_error("exhausted the unique ids for components in this process"); + } + + try { + node_wrappers_[node_id] = factory->create_node_instance(options); + } catch (const std::exception & ex) { + // In the case that the component constructor throws an exception, + // rethrow into the following catch block. + throw ComponentManagerException( + "Component constructor threw an exception: " + std::string(ex.what())); + } catch (...) { + // In the case that the component constructor throws an exception, + // rethrow into the following catch block. + throw ComponentManagerException("Component constructor threw an exception"); + } + + add_node_to_executor(node_id); + + auto node = node_wrappers_[node_id].get_node_base_interface(); + response->full_node_name = node->get_fully_qualified_name(); + response->unique_id = node_id; + response->success = true; + return; + } + RCLCPP_ERROR( + get_logger(), "Failed to find class with the requested plugin name '%s' in " + "the loaded library", + request->plugin_name.c_str()); + response->error_message = "Failed to find class with the requested plugin name."; + response->success = false; + } catch (const ComponentManagerException & ex) { + RCLCPP_ERROR(get_logger(), "%s", ex.what()); + response->error_message = ex.what(); + response->success = false; + } +} + +void +ComponentManager::on_unload_node( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response) +{ + (void) request_header; + + auto wrapper = node_wrappers_.find(request->unique_id); + + if (wrapper == node_wrappers_.end()) { + response->success = false; + std::stringstream ss; + ss << "No node found with unique_id: " << request->unique_id; + response->error_message = ss.str(); + RCLCPP_WARN(get_logger(), "%s", ss.str().c_str()); + } else { + remove_node_from_executor(request->unique_id); + node_wrappers_.erase(wrapper); + response->success = true; + } +} + +void +ComponentManager::on_list_nodes( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response) +{ + (void) request_header; + (void) request; + + for (auto & wrapper : node_wrappers_) { + response->unique_ids.push_back(wrapper.first); + response->full_node_names.push_back( + wrapper.second.get_node_base_interface()->get_fully_qualified_name()); + } +} + +} // namespace rclcpp_components diff --git a/rclcpp_components/src/node_main.cpp.in b/rclcpp_components/src/node_main.cpp.in new file mode 100644 index 0000000000..7d621aac9f --- /dev/null +++ b/rclcpp_components/src/node_main.cpp.in @@ -0,0 +1,81 @@ +// 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 "class_loader/class_loader.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_components/node_factory.hpp" +#include "rclcpp_components/node_factory_template.hpp" + +#define NODE_MAIN_LOGGER_NAME "@node@" + +using namespace rclcpp::executors; +using namespace rclcpp::experimental::executors; + +int main(int argc, char * argv[]) +{ + auto args = rclcpp::init_and_remove_ros_arguments(argc, argv); + rclcpp::Logger logger = rclcpp::get_logger(NODE_MAIN_LOGGER_NAME); + @executor@ exec; + rclcpp::NodeOptions options; + options.arguments(args); + + std::string library_name = "@library_name@"; + std::string class_name = "rclcpp_components::NodeFactoryTemplate<@component@>"; + + RCLCPP_DEBUG(logger, "Load library %s", library_name.c_str()); + auto loader = std::make_unique(library_name); + std::vector classes = loader->getAvailableClasses(); + + if (std::find( + classes.begin(), + classes.end(), + class_name) == classes.end()) { + RCLCPP_ERROR( + logger, + "Class %s not found in library %s", + class_name.c_str(), + library_name.c_str()); + return 1; + } + RCLCPP_DEBUG(logger, "Instantiate class %s", class_name.c_str()); + std::shared_ptr node_factory = nullptr; + try { + node_factory = loader->createInstance(class_name); + } catch (const std::exception & ex) { + RCLCPP_ERROR(logger, "Failed to load library %s", ex.what()); + return 1; + } catch (...) { + RCLCPP_ERROR(logger, "Failed to load library"); + return 1; + } + // Scope to destruct node_wrapper before shutdown + { + rclcpp_components::NodeInstanceWrapper node_wrapper = node_factory->create_node_instance(options); + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node = node_wrapper.get_node_base_interface(); + exec.add_node(node); + + exec.spin(); + + exec.remove_node(node_wrapper.get_node_base_interface()); + } + + rclcpp::shutdown(); + + return 0; +} diff --git a/rclcpp_components/test/benchmark/benchmark_components.cpp b/rclcpp_components/test/benchmark/benchmark_components.cpp new file mode 100644 index 0000000000..26d607f5fb --- /dev/null +++ b/rclcpp_components/test/benchmark/benchmark_components.cpp @@ -0,0 +1,124 @@ +// 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 "benchmark/benchmark.h" + +#include + +#include +#include +#include + +#include "rclcpp_components/component_manager.hpp" + +class ComponentTest : public benchmark::Fixture +{ +public: + ComponentTest() + : component_manager_name("my_manager") + { + } + +#ifdef __GNUC__ +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Woverloaded-virtual" +#endif + void SetUp(benchmark::State &) override + { + rcutils_logging_set_default_logger_level(RCUTILS_LOG_SEVERITY_WARN); + + context = std::make_shared(); + context->init(0, nullptr, rclcpp::InitOptions().auto_initialize_logging(false)); + + rclcpp::ExecutorOptions exec_options; + exec_options.context = context; + + executor = std::make_shared(exec_options); + + manager = std::make_shared( + executor, component_manager_name, rclcpp::NodeOptions().context(context)); + executor->add_node(manager); + } + + void TearDown(benchmark::State &) override + { + context->shutdown("Test is complete"); + + manager.reset(); + executor.reset(); + context.reset(); + } +#ifdef __GNUC__ +#pragma GCC diagnostic pop +#endif + + const std::string component_manager_name; + +protected: + rclcpp::Context::SharedPtr context; + rclcpp::executors::SingleThreadedExecutor::SharedPtr executor; + std::shared_ptr manager; +}; + +BENCHMARK_F(ComponentTest, get_component_resources)(benchmark::State & state) +{ + for (auto _ : state) { + (void)_; + std::vector resources = + manager->get_component_resources("rclcpp_components"); + if (resources.size() != 3) { + state.SkipWithError("Wrong number of components found"); + break; + } + } +} + +BENCHMARK_F(ComponentTest, create_component_factory)(benchmark::State & state) +{ + const std::vector resources = + manager->get_component_resources("rclcpp_components"); + if (resources.size() != 3) { + state.SkipWithError("Wrong number of components found"); + return; + } + + for (auto _ : state) { + (void)_; + manager->create_component_factory(resources[0]).reset(); + } +} + +BENCHMARK_F(ComponentTest, create_node_instance)(benchmark::State & state) +{ + const std::vector resources = + manager->get_component_resources("rclcpp_components"); + if (resources.size() != 3) { + state.SkipWithError("Wrong number of components found"); + return; + } + + // Choosing resource 0 - the other two test components were shown empirically to yield + // the same performance characteristics, so they shouldn't need their own benchmarks. + const std::shared_ptr factory = + manager->create_component_factory(resources[0]); + + const rclcpp::NodeOptions options = rclcpp::NodeOptions().context(context); + + for (auto _ : state) { + (void)_; + rclcpp_components::NodeInstanceWrapper node = factory->create_node_instance(options); + benchmark::DoNotOptimize(node); + benchmark::ClobberMemory(); + } +} diff --git a/rclcpp_components/test/components/test_component.cpp b/rclcpp_components/test/components/test_component.cpp new file mode 100644 index 0000000000..7193ed7e1f --- /dev/null +++ b/rclcpp_components/test/components/test_component.cpp @@ -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. + +#include "rclcpp/rclcpp.hpp" + +namespace test_rclcpp_components +{ +/// Simple test component +class TestComponentFoo : public rclcpp::Node +{ +public: + explicit TestComponentFoo(rclcpp::NodeOptions options) + : rclcpp::Node("test_component_foo", options) + { + } +}; + +/// Simple test component +class TestComponentBar : public rclcpp::Node +{ +public: + explicit TestComponentBar(rclcpp::NodeOptions options) + : rclcpp::Node("test_component_bar", options) + { + } +}; + +/// Simple test component that doesn't inherit from rclcpp::Node +class TestComponentNoNode +{ +public: + explicit TestComponentNoNode(rclcpp::NodeOptions options) + : node_("test_component_no_node", options) + { + } + + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr + get_node_base_interface() + { + return node_.get_node_base_interface(); + } + +private: + rclcpp::Node node_; +}; + +} // namespace test_rclcpp_components + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader. +// This acts as a sort of entry point, allowing the component to be discoverable when its library +// is being loaded into a running process. +RCLCPP_COMPONENTS_REGISTER_NODE(test_rclcpp_components::TestComponentFoo) +RCLCPP_COMPONENTS_REGISTER_NODE(test_rclcpp_components::TestComponentBar) +RCLCPP_COMPONENTS_REGISTER_NODE(test_rclcpp_components::TestComponentNoNode) diff --git a/rclcpp_components/test/test_component_manager.cpp b/rclcpp_components/test/test_component_manager.cpp new file mode 100644 index 0000000000..486e1c6a28 --- /dev/null +++ b/rclcpp_components/test/test_component_manager.cpp @@ -0,0 +1,103 @@ +// 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 "rclcpp_components/component_manager.hpp" + +#include "rcpputils/filesystem_helper.hpp" + +class TestComponentManager : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestComponentManager, get_component_resources_invalid) +{ + auto exec = std::make_shared(); + auto manager = std::make_shared(exec); + + EXPECT_THROW( + manager->get_component_resources("invalid_package"), + rclcpp_components::ComponentManagerException); +} + +TEST_F(TestComponentManager, get_component_resources_valid) +{ + auto exec = std::make_shared(); + auto manager = std::make_shared(exec); + + auto resources = manager->get_component_resources("rclcpp_components"); + EXPECT_EQ(3u, resources.size()); + + EXPECT_EQ("test_rclcpp_components::TestComponentFoo", resources[0].first); + EXPECT_EQ("test_rclcpp_components::TestComponentBar", resources[1].first); + EXPECT_EQ("test_rclcpp_components::TestComponentNoNode", resources[2].first); + + namespace fs = std::filesystem; + EXPECT_TRUE(fs::exists(fs::path(resources[0].second))); + EXPECT_TRUE(fs::exists(fs::path(resources[1].second))); + EXPECT_TRUE(fs::exists(fs::path(resources[2].second))); +} + +TEST_F(TestComponentManager, create_component_factory_valid) +{ + auto exec = std::make_shared(); + auto manager = std::make_shared(exec); + + auto resources = manager->get_component_resources("rclcpp_components"); + EXPECT_EQ(3u, resources.size()); + + // Repeated loading should reuse existing class loader and not throw. + EXPECT_NO_THROW(auto factory = manager->create_component_factory(resources[0]);); + EXPECT_NO_THROW(auto factory = manager->create_component_factory(resources[0]);); + + for (const auto & resource : resources) { + auto factory = manager->create_component_factory(resource); + EXPECT_NE(nullptr, factory); + } +} + +TEST_F(TestComponentManager, create_component_factory_invalid) +{ + auto exec = std::make_shared(); + auto manager = std::make_shared(exec); + + // Test invalid library + EXPECT_THROW( + manager->create_component_factory({"foo_class", "invalid_library.so"}), + rclcpp_components::ComponentManagerException); + + // Test valid library with invalid class + auto component_resources = manager->get_component_resources("rclcpp_components"); + auto factory = manager->create_component_factory({"foo_class", component_resources[0].second}); + EXPECT_EQ(nullptr, factory); + + // Test improperly formed resources file + EXPECT_THROW( + auto resources = manager->get_component_resources("invalid_rclcpp_components"), + rclcpp_components::ComponentManagerException); +} diff --git a/rclcpp_components/test/test_component_manager_api.cpp b/rclcpp_components/test/test_component_manager_api.cpp new file mode 100644 index 0000000000..71f32b480c --- /dev/null +++ b/rclcpp_components/test/test_component_manager_api.cpp @@ -0,0 +1,395 @@ +// 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 "composition_interfaces/srv/load_node.hpp" +#include "composition_interfaces/srv/unload_node.hpp" +#include "composition_interfaces/srv/list_nodes.hpp" + +#include "rclcpp_components/component_manager.hpp" +#include "rclcpp_components/component_manager_isolated.hpp" + +using namespace std::chrono_literals; + +class TestComponentManager : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } +}; + +// TODO(hidmic): split up tests once Node bring up/tear down races +// are solved https://github.com/ros2/rclcpp/issues/863 +void test_components_api(bool use_dedicated_executor) +{ + auto exec = std::make_shared(); + auto node = rclcpp::Node::make_shared("test_component_manager"); + std::shared_ptr manager; + if (use_dedicated_executor) { + using ComponentManagerIsolated = + rclcpp_components::ComponentManagerIsolated; + manager = std::make_shared(exec); + } else { + manager = std::make_shared(exec); + } + + exec->add_node(manager); + exec->add_node(node); + + auto composition_client = node->create_client( + "/ComponentManager/_container/load_node"); + + if (!composition_client->wait_for_service(20s)) { + ASSERT_TRUE(false) << "service not available after waiting"; + } + + { + auto request = std::make_shared(); + request->package_name = "rclcpp_components"; + request->plugin_name = "test_rclcpp_components::TestComponentFoo"; + + auto future = composition_client->async_send_request(request); + auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. + auto result = future.get(); + EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); + EXPECT_EQ(result->success, true); + EXPECT_EQ(result->error_message, ""); + EXPECT_EQ(result->full_node_name, "/test_component_foo"); + EXPECT_EQ(result->unique_id, 1u); + } + + { + auto request = std::make_shared(); + request->package_name = "rclcpp_components"; + request->plugin_name = "test_rclcpp_components::TestComponentBar"; + + auto future = composition_client->async_send_request(request); + auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. + auto result = future.get(); + EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); + EXPECT_EQ(result->success, true); + EXPECT_EQ(result->error_message, ""); + EXPECT_EQ(result->full_node_name, "/test_component_bar"); + EXPECT_EQ(result->unique_id, 2u); + } + + // Test remapping the node name + { + auto request = std::make_shared(); + request->package_name = "rclcpp_components"; + request->plugin_name = "test_rclcpp_components::TestComponentFoo"; + request->node_name = "test_component_baz"; + + auto future = composition_client->async_send_request(request); + auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. + auto result = future.get(); + EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); + EXPECT_EQ(result->success, true); + EXPECT_EQ(result->error_message, ""); + EXPECT_EQ(result->full_node_name, "/test_component_baz"); + EXPECT_EQ(result->unique_id, 3u); + } + + // Test remapping the node namespace + { + auto request = std::make_shared(); + request->package_name = "rclcpp_components"; + request->plugin_name = "test_rclcpp_components::TestComponentFoo"; + request->node_namespace = "/ns"; + request->node_name = "test_component_bing"; + + auto future = composition_client->async_send_request(request); + auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. + auto result = future.get(); + EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); + EXPECT_EQ(result->success, true); + EXPECT_EQ(result->error_message, ""); + EXPECT_EQ(result->full_node_name, "/ns/test_component_bing"); + EXPECT_EQ(result->unique_id, 4u); + } + + { + // Valid package, but invalid class name. + auto request = std::make_shared(); + request->package_name = "rclcpp_components"; + request->plugin_name = "test_rclcpp_components::TestComponent"; + + auto future = composition_client->async_send_request(request); + auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. + EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); + auto result = future.get(); + EXPECT_EQ(result->success, false); + EXPECT_EQ(result->error_message, "Failed to find class with the requested plugin name."); + EXPECT_EQ(result->full_node_name, ""); + EXPECT_EQ(result->unique_id, 0u); + } + + { + // Invalid package, but valid class name. + auto request = std::make_shared(); + request->package_name = "rclcpp_components_foo"; + request->plugin_name = "test_rclcpp_components::TestComponentFoo"; + + auto future = composition_client->async_send_request(request); + auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. + EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); + auto result = future.get(); + EXPECT_EQ(result->success, false); + EXPECT_EQ(result->error_message, "Could not find requested resource in ament index"); + EXPECT_EQ(result->full_node_name, ""); + EXPECT_EQ(result->unique_id, 0u); + } + + { + // Remap rules + auto request = std::make_shared(); + request->package_name = "rclcpp_components"; + request->plugin_name = "test_rclcpp_components::TestComponentFoo"; + request->node_name = "test_component_remap"; + request->remap_rules.push_back("alice:=bob"); + + auto future = composition_client->async_send_request(request); + auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. + auto result = future.get(); + EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); + EXPECT_EQ(result->success, true); + EXPECT_EQ(result->error_message, ""); + EXPECT_EQ(result->full_node_name, "/test_component_remap"); + EXPECT_EQ(result->unique_id, 5u); + } + + { + // use_intra_process_comms + auto request = std::make_shared(); + request->package_name = "rclcpp_components"; + request->plugin_name = "test_rclcpp_components::TestComponentFoo"; + request->node_name = "test_component_intra_process"; + rclcpp::Parameter use_intraprocess_comms("use_intra_process_comms", + rclcpp::ParameterValue(true)); + request->extra_arguments.push_back(use_intraprocess_comms.to_parameter_msg()); + + auto future = composition_client->async_send_request(request); + auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. + auto result = future.get(); + EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); + EXPECT_EQ(result->success, true); + EXPECT_EQ(result->error_message, ""); + std::cout << result->full_node_name << std::endl; + EXPECT_EQ(result->full_node_name, "/test_component_intra_process"); + EXPECT_EQ(result->unique_id, 6u); + } + + { + // use_intra_process_comms is not a bool type parameter + auto request = std::make_shared(); + request->package_name = "rclcpp_components"; + request->plugin_name = "test_rclcpp_components::TestComponentFoo"; + request->node_name = "test_component_intra_process_str"; + + rclcpp::Parameter use_intraprocess_comms("use_intra_process_comms", + rclcpp::ParameterValue("hello")); + request->extra_arguments.push_back(use_intraprocess_comms.to_parameter_msg()); + + auto future = composition_client->async_send_request(request); + auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. + auto result = future.get(); + EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); + EXPECT_EQ(result->success, false); + EXPECT_EQ( + result->error_message, + "Extra component argument 'use_intra_process_comms' must be a boolean"); + EXPECT_EQ(result->full_node_name, ""); + EXPECT_EQ(result->unique_id, 0u); + } + + { + // forward_global_arguments + auto request = std::make_shared(); + request->package_name = "rclcpp_components"; + request->plugin_name = "test_rclcpp_components::TestComponentFoo"; + request->node_name = "test_component_global_arguments"; + rclcpp::Parameter forward_global_arguments("forward_global_arguments", + rclcpp::ParameterValue(true)); + request->extra_arguments.push_back(forward_global_arguments.to_parameter_msg()); + + auto future = composition_client->async_send_request(request); + auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. + auto result = future.get(); + EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); + EXPECT_EQ(result->success, true); + EXPECT_EQ(result->error_message, ""); + EXPECT_EQ(result->full_node_name, "/test_component_global_arguments"); + EXPECT_EQ(result->unique_id, 7u); + } + + { + // forward_global_arguments is not a bool type parameter + auto request = std::make_shared(); + request->package_name = "rclcpp_components"; + request->plugin_name = "test_rclcpp_components::TestComponentFoo"; + request->node_name = "test_component_global_arguments_str"; + + rclcpp::Parameter forward_global_arguments("forward_global_arguments", + rclcpp::ParameterValue("hello")); + request->extra_arguments.push_back(forward_global_arguments.to_parameter_msg()); + + auto future = composition_client->async_send_request(request); + auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. + auto result = future.get(); + EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); + EXPECT_EQ(result->success, false); + EXPECT_EQ( + result->error_message, + "Extra component argument 'forward_global_arguments' must be a boolean"); + EXPECT_EQ(result->full_node_name, ""); + EXPECT_EQ(result->unique_id, 0u); + } + + auto node_names = node->get_node_names(); + + auto find_in_nodes = [node_names](std::string name) { + return std::find(node_names.begin(), node_names.end(), name) != node_names.end(); + }; + + EXPECT_TRUE(find_in_nodes("/test_component_foo")); + EXPECT_TRUE(find_in_nodes("/test_component_bar")); + EXPECT_TRUE(find_in_nodes("/test_component_baz")); + EXPECT_TRUE(find_in_nodes("/ns/test_component_bing")); + + { + auto client = node->create_client( + "/ComponentManager/_container/list_nodes"); + + if (!client->wait_for_service(20s)) { + ASSERT_TRUE(false) << "service not available after waiting"; + } + + { + auto request = std::make_shared(); + auto future = client->async_send_request(request); + auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. + EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); + auto result = future.get(); + auto result_node_names = result->full_node_names; + auto result_unique_ids = result->unique_ids; + + EXPECT_EQ(result_node_names.size(), 7u); + EXPECT_EQ(result_node_names[0], "/test_component_foo"); + EXPECT_EQ(result_node_names[1], "/test_component_bar"); + EXPECT_EQ(result_node_names[2], "/test_component_baz"); + EXPECT_EQ(result_node_names[3], "/ns/test_component_bing"); + EXPECT_EQ(result_node_names[4], "/test_component_remap"); + EXPECT_EQ(result_node_names[5], "/test_component_intra_process"); + EXPECT_EQ(result_node_names[6], "/test_component_global_arguments"); + EXPECT_EQ(result_unique_ids.size(), 7u); + EXPECT_EQ(result_unique_ids[0], 1u); + EXPECT_EQ(result_unique_ids[1], 2u); + EXPECT_EQ(result_unique_ids[2], 3u); + EXPECT_EQ(result_unique_ids[3], 4u); + EXPECT_EQ(result_unique_ids[4], 5u); + EXPECT_EQ(result_unique_ids[5], 6u); + EXPECT_EQ(result_unique_ids[6], 7u); + } + } + + { + auto client = node->create_client( + "/ComponentManager/_container/unload_node"); + + if (!client->wait_for_service(20s)) { + ASSERT_TRUE(false) << "service not available after waiting"; + } + + { + auto request = std::make_shared(); + request->unique_id = 1; + + auto future = client->async_send_request(request); + auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. + auto result = future.get(); + EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); + EXPECT_EQ(result->success, true); + EXPECT_EQ(result->error_message, ""); + } + + { + auto request = std::make_shared(); + request->unique_id = 1; + + auto future = client->async_send_request(request); + auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. + auto result = future.get(); + EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); + EXPECT_EQ(result->success, false); + EXPECT_EQ(result->error_message, "No node found with unique_id: 1"); + } + } + + { + auto client = node->create_client( + "/ComponentManager/_container/list_nodes"); + + if (!client->wait_for_service(20s)) { + ASSERT_TRUE(false) << "service not available after waiting"; + } + + { + auto request = std::make_shared(); + auto future = client->async_send_request(request); + auto ret = exec->spin_until_future_complete(future, 5s); // Wait for the result. + EXPECT_EQ(ret, rclcpp::FutureReturnCode::SUCCESS); + auto result = future.get(); + auto result_node_names = result->full_node_names; + auto result_unique_ids = result->unique_ids; + + EXPECT_EQ(result_node_names.size(), 6u); + EXPECT_EQ(result_node_names[0], "/test_component_bar"); + EXPECT_EQ(result_node_names[1], "/test_component_baz"); + EXPECT_EQ(result_node_names[2], "/ns/test_component_bing"); + EXPECT_EQ(result_node_names[3], "/test_component_remap"); + EXPECT_EQ(result_node_names[4], "/test_component_intra_process"); + EXPECT_EQ(result_node_names[5], "/test_component_global_arguments"); + EXPECT_EQ(result_unique_ids.size(), 6u); + EXPECT_EQ(result_unique_ids[0], 2u); + EXPECT_EQ(result_unique_ids[1], 3u); + EXPECT_EQ(result_unique_ids[2], 4u); + EXPECT_EQ(result_unique_ids[3], 5u); + EXPECT_EQ(result_unique_ids[4], 6u); + EXPECT_EQ(result_unique_ids[5], 7u); + } + } +} + +TEST_F(TestComponentManager, components_api) +{ + { + SCOPED_TRACE("ComponentManager"); + test_components_api(false); + } + { + SCOPED_TRACE("ComponentManagerIsolated"); + test_components_api(true); + } +} diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst new file mode 100644 index 0000000000..0cf68a5891 --- /dev/null +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -0,0 +1,500 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package rclcpp_lifecycle +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + + +29.2.0 (2024-11-25) +------------------- + +29.1.0 (2024-11-20) +------------------- +* Fix error message in rclcpp_lifecycle::State::reset() (`#2647 `_) +* Contributors: Christophe Bedard + +29.0.0 (2024-10-03) +------------------- +* Shutdown the context before context's destructor is invoked in tests (`#2633 `_) +* LifecycleNode bugfix and add test cases (`#2562 `_) +* Properly test get_service_names_and_types_by_node in rclcpp_lifecycle (`#2599 `_) +* Contributors: Alejandro Hernández Cordero, Christophe Bedard, Tomoya Fujita + +28.3.3 (2024-07-29) +------------------- + +28.3.2 (2024-07-24) +------------------- +* Removed deprecated methods and classes (`#2575 `_) +* Fix the lifecycle tests on RHEL-9. (`#2583 `_) + * Fix the lifecycle tests on RHEL-9. + The full explanation is in the comment, but basically since + RHEL doesn't support mocking_utils::inject_on_return, we have + to split out certain tests to make sure resources within a + process don't collide. + Co-authored-by: Alejandro Hernández Cordero +* Contributors: Alejandro Hernández Cordero, Chris Lalancette + +28.3.1 (2024-06-25) +------------------- + +28.3.0 (2024-06-17) +------------------- +* revert call shutdown in LifecycleNode destructor (`#2557 `_) +* LifecycleNode shutdown on dtor only with valid context. (`#2545 `_) +* call shutdown in LifecycleNode dtor to avoid leaving the device in unknown state (2nd) (`#2528 `_) +* rclcpp::shutdown should not be called before LifecycleNode dtor. (`#2527 `_) +* Revert "call shutdown in LifecycleNode dtor to avoid leaving the device in un… (`#2450 `_)" (`#2522 `_) +* Add 'mimick' label to tests which use Mimick (`#2516 `_) +* Contributors: Chris Lalancette, Scott K Logan, Tomoya Fujita + +28.2.0 (2024-04-26) +------------------- + +28.1.0 (2024-04-16) +------------------- +* Remove references to index.ros.org. (`#2504 `_) +* Contributors: Chris Lalancette + +28.0.1 (2024-04-16) +------------------- +* call shutdown in LifecycleNode dtor to avoid leaving the device in un… (`#2450 `_) + * call shutdown in LifecycleNode dtor to avoid leaving the device in unknown state. + * add test to verify LifecycleNode::shutdown is called on destructor. + --------- +* Contributors: Tomoya Fujita + +28.0.0 (2024-03-28) +------------------- +* Update quality declaration documents (`#2427 `_) +* Contributors: Christophe Bedard + +27.0.0 (2024-02-07) +------------------- + +26.0.0 (2024-01-24) +------------------- +* Increase timeout for rclcpp_lifecycle to 360 (`#2395 `_) +* Contributors: Jorge Perez + +25.0.0 (2023-12-26) +------------------- + +24.0.0 (2023-11-06) +------------------- +* Fix rclcpp_lifecycle inclusion on Windows. (`#2331 `_) +* Contributors: Chris Lalancette + +23.2.0 (2023-10-09) +------------------- +* add clients & services count (`#2072 `_) +* Contributors: Minju, Lee + +23.1.0 (2023-10-04) +------------------- + +23.0.0 (2023-09-08) +------------------- +* Update API docs links in package READMEs (`#2302 `_) +* Contributors: Christophe Bedard + +22.2.0 (2023-09-07) +------------------- +* add logger level service to lifecycle node. (`#2277 `_) +* Contributors: Tomoya Fujita + +22.1.0 (2023-08-21) +------------------- +* Stop using constref signature of benchmark DoNotOptimize. (`#2238 `_) +* Contributors: Chris Lalancette + +22.0.0 (2023-07-11) +------------------- +* Implement get_node_type_descriptions_interface for lifecyclenode and add smoke test for it (`#2237 `_) +* Switch lifecycle to use the RCLCPP macros. (`#2233 `_) +* Add new node interface TypeDescriptionsInterface to provide GetTypeDescription service (`#2224 `_) +* Contributors: Chris Lalancette, Emerson Knapp + +21.3.0 (2023-06-12) +------------------- + +21.2.0 (2023-06-07) +------------------- + +21.1.1 (2023-05-11) +------------------- + +21.1.0 (2023-04-27) +------------------- + +21.0.0 (2023-04-18) +------------------- +* Add support for logging service. (`#2122 `_) +* Support publishing loaned messages in LifecyclePublisher (`#2159 `_) +* Contributors: Lei Liu, Michael Babenko + +20.0.0 (2023-04-13) +------------------- +* Fixes to silence some clang warnings. (`#2127 `_) +* Update all rclcpp packages to C++17. (`#2121 `_) +* Use the correct macro for LifecycleNode::get_fully_qualified_name (`#2117 `_) +* add get_fully_qualified_name to rclcpp_lifecycle (`#2115 `_) +* Contributors: Chris Lalancette, Steve Macenski + +19.3.0 (2023-03-01) +------------------- + +19.2.0 (2023-02-24) +------------------- + +19.1.0 (2023-02-14) +------------------- + +19.0.0 (2023-01-30) +------------------- + +18.0.0 (2022-12-29) +------------------- +* Implement Unified Node Interface (NodeInterfaces class) (`#2041 `_) +* Add clock type to node_options (`#1982 `_) +* Update maintainers (`#2043 `_) +* Contributors: Audrow Nash, Jeffery Hsu, methylDragon + +17.1.0 (2022-11-02) +------------------- +* LifecycleNode on_configure doc fix. (`#2034 `_) +* Bugfix 20210810 get current state (`#1756 `_) +* Make lifecycle impl get_current_state() const. (`#2031 `_) +* Cleanup the lifecycle implementation (`#2027 `_) +* Cleanup the rclcpp_lifecycle dependencies. (`#2021 `_) +* Contributors: Chris Lalancette, Tomoya Fujita + +17.0.0 (2022-09-13) +------------------- +* Revert "Revert "Add a create_timer method to Node and `LifecycleNode` classes (`#1975 `_)" (`#2009 `_) (`#2010 `_) +* Revert "Add a `create_timer` method to `Node` and `LifecycleNode` classes (`#1975 `_)" (`#2009 `_) +* Add a `create_timer` method to `Node` and `LifecycleNode` classes (`#1975 `_) +* Support pre-set and post-set parameter callbacks in addition to on-set-parameter-callback. (`#1947 `_) +* Make create_service accept rclcpp::QoS (`#1969 `_) +* Make create_client accept rclcpp::QoS (`#1964 `_) +* Contributors: Andrew Symington, Deepanshu Bansal, Ivan Santiago Paunovic, Shane Loretz + +16.2.0 (2022-05-03) +------------------- + +16.1.0 (2022-04-29) +------------------- + +16.0.1 (2022-04-13) +------------------- + +16.0.0 (2022-04-08) +------------------- +* remove things that were deprecated during galactic (`#1913 `_) +* Contributors: William Woodall + +15.4.0 (2022-04-05) +------------------- + +15.3.0 (2022-03-30) +------------------- + +15.2.0 (2022-03-24) +------------------- +* Fix rosdoc2 issues (`#1897 `_) +* Contributors: Chris Lalancette + +15.1.0 (2022-03-01) +------------------- +* Install headers to include/${PROJECT_NAME} (`#1888 `_) +* LifecycleNode::on_deactivate deactivate all managed entities. (`#1885 `_) +* Contributors: Shane Loretz, Tomoya Fujita + +15.0.0 (2022-01-14) +------------------- +* Automatically transition lifecycle entities when node transitions (`#1863 `_) +* Contributors: Ivan Santiago Paunovic + +14.1.0 (2022-01-05) +------------------- + +14.0.0 (2021-12-17) +------------------- +* Remove author by request (`#1818 `_) +* Update maintainers (`#1817 `_) +* Suppress clang dead-store warnings in the benchmarks. (`#1802 `_) +* Contributors: Chris Lalancette, Jacob Perron + +13.1.0 (2021-10-18) +------------------- +* Update forward declarations of `rcl_lifecycle` types (`#1788 `_) +* Deprecate the `void shared_ptr` subscription callback signatures (`#1713 `_) +* Contributors: Abrar Rahman Protyasha, Michel Hidalgo + +13.0.0 (2021-08-23) +------------------- +* Update client API to be able to remove pending requests. (`#1734 `_) +* Change log level for lifecycle_publisher. (`#1715 `_) +* Fix: RCLCPP_PUBLIC -> RCLCPP_LIFECYCLE_PUBLIC (`#1732 `_) +* Use rcpputils/scope_exit.hpp and remove rclcpp/scope_exit.hpp (`#1727 `_) +* Contributors: Alberto Soragna, Christophe Bedard, Ivan Santiago Paunovic, Shane Loretz + +12.0.0 (2021-07-26) +------------------- +* Remove unsafe get_callback_groups API. + Callers should change to using for_each_callback_group(), or + store the callback groups they need internally. +* Add in callback_groups_for_each. + The main reason to add this method in is to make accesses to the + callback_groups\_ vector thread-safe. By having a + callback_groups_for_each that accepts a std::function, we can + just have the callers give us the callback they are interested + in, and we can take care of the locking. + The rest of this fairly large PR is cleaning up all of the places + that use get_callback_groups() to instead use + callback_groups_for_each(). +* Contributors: Chris Lalancette + +11.2.0 (2021-07-21) +------------------- + +11.1.0 (2021-07-13) +------------------- + +11.0.0 (2021-05-18) +------------------- +* Fix destruction order in lifecycle benchmark (`#1675 `_) +* Contributors: Scott K Logan + +10.0.0 (2021-05-11) +------------------- +* [rclcpp] Type Adaptation feature (`#1557 `_) +* Contributors: Audrow Nash, William Woodall + +9.0.2 (2021-04-14) +------------------ + +9.0.1 (2021-04-12) +------------------ + +9.0.0 (2021-04-06) +------------------ +* Add generic publisher and generic subscription for serialized messages (`#1452 `_) +* updating quality declaration links (re: `ros2/docs.ros2.org#52 `_) (`#1615 `_) +* Contributors: Nikolai Morin, shonigmann + +8.2.0 (2021-03-31) +------------------ +* Fix flaky lifecycle node tests (`#1606 `_) +* Clock subscription callback group spins in its own thread (`#1556 `_) +* Delete debug messages (`#1602 `_) +* add automatically_add_executor_with_node option (`#1594 `_) +* Contributors: BriceRenaudeau, Ivan Santiago Paunovic, Jacob Perron, anaelle-sw + +8.1.0 (2021-03-25) +------------------ + +8.0.0 (2021-03-23) +------------------ +* make rcl_lifecyle_com_interface optional in lifecycle nodes (`#1507 `_) +* Contributors: Karsten Knese + +7.0.1 (2021-03-22) +------------------ + +7.0.0 (2021-03-18) +------------------ +* Add support for rmw_connextdds (`#1574 `_) +* Fix SEGV caused by order of destruction of Node sub-interfaces (`#1469 `_) +* Enforce static parameter types (`#1522 `_) +* Contributors: Andrea Sorbini, Colin MacKenzie, Ivan Santiago Paunovic + +6.3.1 (2021-02-08) +------------------ + +6.3.0 (2021-01-25) +------------------ + +6.2.0 (2021-01-08) +------------------ + +6.1.0 (2020-12-10) +------------------ +* add LifecycleNode::get_transition_graph to match services. (`#1472 `_) +* Update QDs to QL 1 (`#1477 `_) +* Benchmark lifecycle features (`#1462 `_) +* Contributors: Stephen Brawner, brawner, tomoya + +6.0.0 (2020-11-18) +------------------ +* Reserve vector capacities and use emplace_back for constructing vectors (`#1464 `_) +* [rclcpp_lifecycle] Change uint8_t iterator variables to size_t (`#1461 `_) +* Bump rclcpp packages to Quality Level 2 (`#1445 `_) +* Contributors: Louise Poubel, brawner + +5.1.0 (2020-11-02) +------------------ +* Increase test timeouts of slow running tests with rmw_connext_cpp (`#1400 `_) +* Update maintainers (`#1384 `_) +* Add clock qos to node options (`#1375 `_) +* Contributors: Ivan Santiago Paunovic, brawner + +5.0.0 (2020-09-18) +------------------ +* Increase test coverage of rclcpp_lifecycle to 96% (`#1298 `_) +* Log error instead of throwing exception in Transition and State reset(), mark no except (`#1297 `_) +* Remove unused private function (rclcpp::Node and rclcpp_lifecycle::Node) (`#1294 `_) +* Remove rmw-dependent unit-test checks (`#1293 `_) +* Added missing tests for rclcpp lifecycle (`#1240 `_) +* Warn about unused result of add_on_set_parameters_callback (`#1238 `_) +* Contributors: Alejandro Hernández Cordero, Jacob Perron, Stephen Brawner + +4.0.0 (2020-07-09) +------------------ +* Remove deprecated set_on_parameters_set_callback function (`#1199 `_) +* Bump to QD to level 3 and fixed links (`#1158 `_) +* Fix race in test_lifecycle_service_client (`#1204 `_) +* Contributors: Alejandro Hernández Cordero, Claire Wang, Dirk Thomas + +3.0.0 (2020-06-18) +------------------ +* Fix doxygen warnings (`#1163 `_) +* Contributors: Alejandro Hernández Cordero + +2.0.0 (2020-06-01) +------------------ +* Added missing virtual destructors. (`#1149 `_) +* Add Security Vulnerability Policy pointing to REP-2006. (`#1130 `_) +* Fixed ``test_lifecycle_node.cpp:check_parameters`` (`#1136 `_) +* Contributors: Chris Lalancette, Ivan Santiago Paunovic + +1.1.0 (2020-05-26) +------------------ +* Deprecate set_on_parameters_set_callback (`#1123 `_) +* Add missing parameter callback functions to lifecycle node (`#1134 `_) +* Expose get_service_names_and_types_by_node from rcl in rclcpp (`#1131 `_) +* Improve documentation (`#1106 `_) +* Fixed rep links and added more details to dependencies in quality declaration (`#1116 `_) +* Update quality declaration to reflect version 1.0 (`#1115 `_) +* Contributors: Alejandro Hernández Cordero, Claire Wang, Dirk Thomas, Stephen Brawner + +1.0.0 (2020-05-12) +------------------ +* Avoid callback_group deprecation (`#1108 `_) +* Contributors: Karsten Knese + +0.9.1 (2020-05-08) +------------------ +* Added rclcpp lifecycle Doxyfile (`#1089 `_) +* Added Quality declaration: rclcpp, rclpp_action, rclcpp_components andrclcpp_lifecycle (`#1100 `_) +* Increasing test coverage of rclcpp_lifecycle (`#1045 `_) +* Contributors: Alejandro Hernández Cordero, brawner + +0.9.0 (2020-04-29) +------------------ +* Export targets in addition to include directories / libraries (`#1096 `_) +* Deprecate redundant namespaces (`#1083 `_) +* Integrate topic statistics (`#1072 `_) +* Reflect changes in rclcpp API (`#1079 `_) +* Fix unknown macro errors reported by cppcheck 1.90 (`#1000 `_) +* Rremoved rmw_implementation from package.xml (`#991 `_) +* Implement functions to get publisher and subcription informations like QoS policies from topic name (`#960 `_) +* Create node clock calls const (`#922 `_) +* Type conversions fixes (`#901 `_) +* Contributors: Alejandro Hernández Cordero, Barry Xu, Devin Bonnie, Dirk Thomas, Jacob Perron, Monika Idzik, Prajakta Gokhale, Steven Macenski, William Woodall + +0.8.3 (2019-11-19) +------------------ + +0.8.2 (2019-11-18) +------------------ + +0.8.1 (2019-10-23) +------------------ +* New Intra-Process Communication (`#778 `_) +* Contributors: Alberto Soragna + +0.8.0 (2019-09-26) +------------------ +* clean up publisher and subscription creation logic (`#867 `_) +* reset error message before setting a new one, embed the original one (`#854 `_) +* remove features and related code which were deprecated in dashing (`#852 `_) +* Fix typo in deprecated warning. (`#848 `_) +* Add line break after first open paren in multiline function call (`#785 `_) +* Fixe error messages not printing to terminal (`#777 `_) +* Add default value to options in LifecycleNode construnctor. Update API documentation. (`#775 `_) +* Contributors: Dan Rose, Dirk Thomas, Esteve Fernandez, Luca Della Vedova, William Woodall, Yathartha Tuladhar + +0.7.5 (2019-05-30) +------------------ + +0.7.4 (2019-05-29) +------------------ +* Rename parameter options (`#745 `_) +* Contributors: William Woodall + +0.7.3 (2019-05-20) +------------------ +* Added missing template functionality to lifecycle_node. (`#707 `_) +* Contributors: Michael Jeronimo + +0.7.2 (2019-05-08) +------------------ +* Added new way to specify QoS settings for publishers and subscriptions. (`#713 `_) +* Deprecated ``shared_ptr`` and raw pointer versions of ``Publisher::publish()``. (`#709 `_) +* Implemented API to set callbacks for liveliness and deadline QoS events for publishers and subscriptions. (`#695 `_) +* Changed the ``IntraProcessManager`` to be capable of storing ``shared_ptr`` in addition to ``unique_ptr``. (`#690 `_) +* Contributors: M. M, William Woodall, ivanpauno + +0.7.1 (2019-04-26) +------------------ +* Added read only parameters. (`#495 `_) +* Contributors: Shane Loretz, William Woodall + +0.7.0 (2019-04-14) +------------------ +* Fixed linter errors in rclcpp_lifecycle. (`#672 `_) +* Added parameter-related templates to LifecycleNode. (`#645 `_) +* Fixed use_sim_time issue on LifeCycleNode. (`#651 `_) +* Updated to use ament_target_dependencies where possible. (`#659 `_) +* Fixed hard-coded duration type representation so int64_t isn't assumed. (`#648 `_) +* Added a method to the LifecycleNode class to get the logging interface. (`#652 `_) +* Set Parameter Event Publisher settings `#591 `_ (`#614 `_) +* Replaced node constructor arguments with NodeOptions. (`#622 `_) +* Removed dependency on rclpy. (`#626 `_) +* Contributors: Emerson Knapp, Karsten Knese, Michael Carroll, Michael Jeronimo, Vinnam Kim, William Woodall, ivanpauno, rarvolt + +0.6.2 (2018-12-13) +------------------ + +0.6.1 (2018-12-07) +------------------ +* Added node path and time stamp to parameter event message (`#584 `_) +* Refactored init to allow for non-global init (`#587 `_) +* Add class Waitable (`#589 `_) +* Contributors: Dirk Thomas, Jacob Perron, William Woodall, bpwilcox + +0.6.0 (2018-11-19) +------------------ +* Updated to use new error handling API from rcutils (`#577 `_) +* Deleted TRANSITION_SHUTDOWN (`#576 `_) +* Added a warning when publishing if publisher is not active (`#574 `_) +* Added SMART_PTRS_DEF to LifecyclePublisher (`#569 `_) +* Added service for transition graph (`#555 `_) +* Added semicolons to all RCLCPP and RCUTILS macros. (`#565 `_) +* Fixed and improved documentation (`#546 `_) +* Removed unneeded dependency on std_msgs (`#513 `_) +* Removed use of uninitialized CMake var (`#511 `_) +* Added get_node_names API from node. (`#508 `_) +* Fixed rosidl dependencies (`#507 `_) +* Contributors: Chris Lalancette, Dirk Thomas, Francisco Martín Rico, Karsten Knese, Mikael Arguedas, Sriram Raghunathan, William Woodall, cho3 + +0.5.0 (2018-06-25) +------------------ +* Added functions that allow you to publish serialized messages and received serialized messages in your subscription callback. (`#388 `_) +* Added ability to initialize parameter values in a node with an argument to the Node constructor. (`#486 `_) +* Nodes now autostart the ROS parameter services which let you get, set, and list parameters in a node. (`#478 `_) +* Fixed a bug that occurred when mixing ``std::shared_ptr`` and ``std::bind``. (`#470 `_) +* Added ability to pass command line arguments to the Node constructor. (`#461 `_) +* Changed library export order for static linking. (`#446 `_) +* Now depends on ``ament_cmake_ros``. (`#444 `_) +* Updaed code to use logging macros rather than ``fprintf()``. (`#439 `_) +* Contributors: Dirk Thomas, Guillaume Autran, Karsten Knese, Michael Carroll, Mikael Arguedas, Shane Loretz, dhood diff --git a/rclcpp_lifecycle/CMakeLists.txt b/rclcpp_lifecycle/CMakeLists.txt index 98a3db2286..9a7335b1bf 100644 --- a/rclcpp_lifecycle/CMakeLists.txt +++ b/rclcpp_lifecycle/CMakeLists.txt @@ -2,111 +2,177 @@ cmake_minimum_required(VERSION 3.5) project(rclcpp_lifecycle) -# Default to C++14 +# Default to C++17 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic -Wnon-virtual-dtor -Woverloaded-virtual) endif() -find_package(ament_cmake REQUIRED) +find_package(ament_cmake_ros REQUIRED) +find_package(lifecycle_msgs REQUIRED) +find_package(rcl REQUIRED) find_package(rclcpp REQUIRED) +find_package(rcl_interfaces REQUIRED) find_package(rcl_lifecycle REQUIRED) -find_package(std_msgs REQUIRED) -find_package(lifecycle_msgs REQUIRED) - -include_directories(include) +find_package(rcutils REQUIRED) +find_package(rosidl_typesupport_cpp REQUIRED) ### CPP High level library add_library(rclcpp_lifecycle - SHARED src/lifecycle_node.cpp + src/lifecycle_node_interface_impl.cpp + src/managed_entity.cpp src/node_interfaces/lifecycle_node_interface.cpp src/state.cpp src/transition.cpp ) -ament_target_dependencies(rclcpp_lifecycle - "lifecycle_msgs" - "rcl_lifecycle" - "rclcpp") +target_include_directories(${PROJECT_NAME} + PUBLIC + "$" + "$") +target_link_libraries(${PROJECT_NAME} + ${lifecycle_msgs_TARGETS} + rcl::rcl + rclcpp::rclcpp + ${rcl_interfaces_TARGETS} + rcl_lifecycle::rcl_lifecycle + rcutils::rcutils + rosidl_typesupport_cpp::rosidl_typesupport_cpp +) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(rclcpp_lifecycle PRIVATE "RCLCPP_LIFECYCLE_BUILDING_DLL") install(TARGETS - rclcpp_lifecycle + rclcpp_lifecycle EXPORT rclcpp_lifecycle ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin) +install(DIRECTORY include/ + DESTINATION include/${PROJECT_NAME}) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) + # Give cppcheck hints about macro definitions coming from outside this package + set(ament_cmake_cppcheck_ADDITIONAL_INCLUDE_DIRS ${rclcpp_INCLUDE_DIRS}) + list(APPEND AMENT_LINT_AUTO_EXCLUDE "ament_cmake_cppcheck") ament_lint_auto_find_test_dependencies() - ament_add_gtest(test_lifecycle_node test/test_lifecycle_node.cpp) + find_package(ament_cmake_cppcheck REQUIRED) + ament_cppcheck() + set_tests_properties(cppcheck PROPERTIES TIMEOUT 360) + + find_package(performance_test_fixture REQUIRED) + + add_performance_test( + benchmark_lifecycle_client + test/benchmark/benchmark_lifecycle_client.cpp) + if(TARGET benchmark_lifecycle_client) + target_link_libraries(benchmark_lifecycle_client ${PROJECT_NAME} rclcpp::rclcpp) + endif() + add_performance_test( + benchmark_lifecycle_node + test/benchmark/benchmark_lifecycle_node.cpp) + if(TARGET benchmark_lifecycle_node) + target_link_libraries(benchmark_lifecycle_node ${PROJECT_NAME} rclcpp::rclcpp) + endif() + add_performance_test( + benchmark_state + test/benchmark/benchmark_state.cpp) + if(TARGET benchmark_state) + target_link_libraries(benchmark_state ${PROJECT_NAME}) + endif() + add_performance_test( + benchmark_transition + test/benchmark/benchmark_transition.cpp) + if(TARGET benchmark_transition) + target_link_libraries(benchmark_transition ${PROJECT_NAME}) + endif() + + ament_add_gtest(test_lifecycle_node test/test_lifecycle_node.cpp TIMEOUT 120) + ament_add_test_label(test_lifecycle_node mimick) if(TARGET test_lifecycle_node) - target_include_directories(test_lifecycle_node PUBLIC - ${rcl_lifecycle_INCLUDE_DIRS} - ${rclcpp_INCLUDE_DIRS} - ${rclcpp_lifecycle_INCLUDE_DIRS} - ) - target_link_libraries(test_lifecycle_node ${PROJECT_NAME}) + target_link_libraries(test_lifecycle_node ${PROJECT_NAME} mimick rcl_lifecycle::rcl_lifecycle rclcpp::rclcpp rcutils::rcutils) + endif() + + ament_add_gtest(test_lifecycle_node_errors test/test_lifecycle_node_errors.cpp) + ament_add_test_label(test_lifecycle_node_errors mimick) + if(TARGET test_lifecycle_node_errors) + target_link_libraries(test_lifecycle_node_errors ${PROJECT_NAME} mimick rcl_lifecycle::rcl_lifecycle) + endif() + + ament_add_gtest(test_lifecycle_publisher test/test_lifecycle_publisher.cpp) + if(TARGET test_lifecycle_publisher) + target_link_libraries(test_lifecycle_publisher ${PROJECT_NAME} rcl_lifecycle::rcl_lifecycle rclcpp::rclcpp ${test_msgs_TARGETS}) + endif() + ament_add_gtest(test_lifecycle_service_client test/test_lifecycle_service_client.cpp TIMEOUT 120) + ament_add_test_label(test_lifecycle_service_client mimick) + if(TARGET test_lifecycle_service_client) + target_link_libraries(test_lifecycle_service_client + ${PROJECT_NAME} + mimick + rcl_lifecycle::rcl_lifecycle + rclcpp::rclcpp + rcpputils::rcpputils + rcutils::rcutils) + endif() + ament_add_gtest(test_client test/test_client.cpp TIMEOUT 120) + ament_add_test_label(test_client mimick) + if(TARGET test_client) + target_link_libraries(test_client + ${PROJECT_NAME} + mimick + ${rcl_interfaces_TARGETS} + rclcpp::rclcpp) + endif() + ament_add_gtest(test_service test/test_service.cpp TIMEOUT 120) + ament_add_test_label(test_service mimick) + if(TARGET test_service) + target_link_libraries(test_service + ${PROJECT_NAME} + mimick + ${test_msgs_TARGETS} + rclcpp::rclcpp) endif() ament_add_gtest(test_state_machine_info test/test_state_machine_info.cpp) if(TARGET test_state_machine_info) - target_include_directories(test_state_machine_info PUBLIC - ${rcl_lifecycle_INCLUDE_DIRS} - ${rclcpp_INCLUDE_DIRS} - ${rclcpp_lifecycle_INCLUDE_DIRS} - ) - target_link_libraries(test_state_machine_info ${PROJECT_NAME}) + target_link_libraries(test_state_machine_info ${PROJECT_NAME} rcl_lifecycle::rcl_lifecycle rclcpp::rclcpp) endif() ament_add_gtest(test_register_custom_callbacks test/test_register_custom_callbacks.cpp) if(TARGET test_register_custom_callbacks) - target_include_directories(test_register_custom_callbacks PUBLIC - ${rcl_lifecycle_INCLUDE_DIRS} - ${rclcpp_INCLUDE_DIRS} - ${rclcpp_lifecycle_INCLUDE_DIRS} - ) - target_link_libraries(test_register_custom_callbacks ${PROJECT_NAME}) + target_link_libraries(test_register_custom_callbacks ${PROJECT_NAME} rcl_lifecycle::rcl_lifecycle rclcpp::rclcpp) endif() ament_add_gtest(test_callback_exceptions test/test_callback_exceptions.cpp) if(TARGET test_callback_exceptions) - target_include_directories(test_callback_exceptions PUBLIC - ${rcl_lifecycle_INCLUDE_DIRS} - ${rclcpp_INCLUDE_DIRS} - ${rclcpp_lifecycle_INCLUDE_DIRS} - ) - target_link_libraries(test_callback_exceptions ${PROJECT_NAME}) + target_link_libraries(test_callback_exceptions ${PROJECT_NAME} rcl_lifecycle::rcl_lifecycle rclcpp::rclcpp) endif() ament_add_gtest(test_state_wrapper test/test_state_wrapper.cpp) if(TARGET test_state_wrapper) - target_include_directories(test_state_wrapper PUBLIC - ${rcl_lifecycle_INCLUDE_DIRS} - ${rclcpp_INCLUDE_DIRS} - ${rclcpp_lifecycle_INCLUDE_DIRS} - ) - target_link_libraries(test_state_wrapper ${PROJECT_NAME}) + target_link_libraries(test_state_wrapper ${PROJECT_NAME} rcl_lifecycle::rcl_lifecycle rclcpp::rclcpp) endif() ament_add_gtest(test_transition_wrapper test/test_transition_wrapper.cpp) + ament_add_test_label(test_transition_wrapper mimick) if(TARGET test_transition_wrapper) - target_include_directories(test_transition_wrapper PUBLIC - ${rcl_lifecycle_INCLUDE_DIRS} - ${rclcpp_INCLUDE_DIRS} - ${rclcpp_lifecycle_INCLUDE_DIRS} + target_link_libraries(test_transition_wrapper ${PROJECT_NAME} mimick rcl_lifecycle::rcl_lifecycle rclcpp::rclcpp rcutils::rcutils) + target_compile_definitions(test_transition_wrapper + PUBLIC RCUTILS_ENABLE_FAULT_INJECTION ) - target_link_libraries(test_transition_wrapper ${PROJECT_NAME}) endif() endif() -ament_export_dependencies(rclcpp) -ament_export_dependencies(rcl_lifecycle) -ament_export_dependencies(lifecycle_msgs) -ament_export_include_directories(include) +# Export old-style CMake variables +ament_export_include_directories("include/${PROJECT_NAME}") ament_export_libraries(${PROJECT_NAME}) -ament_package() -install(DIRECTORY include/ - DESTINATION include) +# Export modern CMake targets +ament_export_targets(${PROJECT_NAME}) + +# Export dependencies +ament_export_dependencies(lifecycle_msgs rcl rclcpp rcl_interfaces rcl_lifecycle rcutils rosidl_typesupport_cpp) + +ament_package() diff --git a/rclcpp_lifecycle/Doxyfile b/rclcpp_lifecycle/Doxyfile new file mode 100644 index 0000000000..3f7644b82e --- /dev/null +++ b/rclcpp_lifecycle/Doxyfile @@ -0,0 +1,34 @@ +# All settings not listed here will use the Doxygen default values. + +PROJECT_NAME = "rclcpp_lifecycle" +PROJECT_NUMBER = master +PROJECT_BRIEF = "C++ ROS Lifecycle Library API" + +# Use these lines to include the generated logging.hpp (update install path if needed) +# Otherwise just generate for the local (non-generated header files) + + +INPUT = ./include + +RECURSIVE = YES +OUTPUT_DIRECTORY = doc_output + +EXTRACT_ALL = YES +SORT_MEMBER_DOCS = NO + +GENERATE_LATEX = NO + +ENABLE_PREPROCESSING = YES +MACRO_EXPANSION = YES +EXPAND_ONLY_PREDEF = YES +PREDEFINED = RCLCPP_LIFECYCLE_PUBLIC= +PREDEFINED += RCUTILS_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/rclcpp.tag=http://docs.ros2.org/latest/api/rclcpp/" +#TAGFILES += "../../../../doxygen_tag_files/rcl_lifecycle.tag=http://docs.ros2.org/latest/api/rcl_lifecycle/" +#TAGFILES += "../../../../doxygen_tag_files/rmw.tag=http://docs.ros2.org/latest/api/rmw/" +# Uncomment to generate tag files for cross-project linking. +GENERATE_TAGFILE = "../../../../doxygen_tag_files/rclcpp_lifecycle.tag" diff --git a/rclcpp_lifecycle/QUALITY_DECLARATION.md b/rclcpp_lifecycle/QUALITY_DECLARATION.md new file mode 100644 index 0000000000..73be7c6c9a --- /dev/null +++ b/rclcpp_lifecycle/QUALITY_DECLARATION.md @@ -0,0 +1,206 @@ +This document is a declaration of software quality for the `rclcpp_lifecycle` package, based on the guidelines in [REP-2004](https://www.ros.org/reps/rep-2004.html). + +# rclcpp_lifecycle Quality Declaration + +The package `rclcpp_lifecycle` claims to be in the **Quality Level 1** category when 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 Requirements for Quality Level 1 in REP-2004](https://www.ros.org/reps/rep-2004.html). + +## Version Policy [1] + +### Version Scheme [1.i] + +`rclcpp_lifecycle` 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] + +`rclcpp_lifecycle` is at a stable version, i.e. `>= 1.0.0`. +The current version can be found in its [package.xml](package.xml), and its change history can be found in its [CHANGELOG](CHANGELOG.rst). + +### Public API Declaration [1.iii] + +All symbols in the installed headers are considered part of the public API. + +All installed headers are in the `include` directory of the package, headers in any other folders are not installed and considered private. + +### API Stability Policy [1.iv] + +`rclcpp_lifecycle` 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] + +`rclcpp_lifecycle` 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] + +`rclcpp_lifecycle` 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] + +`rclcpp_lifecycle` follows the recommended guidelines for ROS Core packages in the [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#change-control-process). + +### Change Requests [2.i] + +All changes will occur through a pull request, check [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/rclcpp_lifecycle/) +* [linux_release](https://ci.ros2.org/view/nightly/job/nightly_linux_release/lastBuild/testReport/rclcpp_lifecycle/) +* [mac_osx_release](https://ci.ros2.org/view/nightly/job/nightly_osx_release/lastBuild/testReport/rclcpp_lifecycle/) +* [windows_release](https://ci.ros2.org/view/nightly/job/nightly_win_rel/lastBuild/testReport/rclcpp_lifecycle/) + +### Documentation Policy [2.v] + +All pull requests must resolve related documentation changes before merging. + +## Documentation [3] + +### Feature Documentation [3.i] + +`rclcpp_lifecycle` has a [feature list](http://docs.ros2.org/latest/api/rclcpp_lifecycle/index.html) and each item in the list links to the corresponding feature documentation. There is documentation for all of the features, and new features require documentation before being added. + +### Public API Documentation [3.ii] + +The API is publicly available in its [ROS 2 API documentation](http://docs.ros2.org/latest/api/rclcpp_lyfecycle/). + +### License [3.iii] + +The license for `rclcpp_lifecycle` 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](http://build.ros2.org/view/Rpr/job/Rpr__rclcpp__ubuntu_focal_amd64/lastCompletedBuild/testReport/rclcpp_lifecycle/) 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 `rclcpp_lifecycle`. + +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](http://build.ros2.org/view/Rpr/job/Rpr__rclcpp__ubuntu_focal_amd64/lastCompletedBuild/testReport/rclcpp_lifecycle/copyright/). + +## Testing [4] + +### Feature Testing [4.i] + +Each feature in `rclcpp_lifecycle` has corresponding tests which simulate typical usage, and they are located in the [`test`](https://github.com/ros2/rclcpp_lifecycle/tree/rolling/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/rclcpp_lifecycle/) +* [linux_release](https://ci.ros2.org/view/nightly/job/nightly_linux_release/lastBuild/testReport/rclcpp_lifecycle/) +* [mac_osx_release](https://ci.ros2.org/view/nightly/job/nightly_osx_release/lastBuild/testReport/rclcpp_lifecycle/) +* [windows_release](https://ci.ros2.org/view/nightly/job/nightly_win_rel/lastBuild/testReport/rclcpp_lifecycle/) + +### 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. + +### Coverage [4.iii] + +`rclcpp_lifecycle` 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 +- achieving and maintaining a reasonable branch line coverage (90-100%) +- no lines are manually skipped in coverage calculations + +Changes are required to make a best effort to keep or increase coverage before being accepted, but decreases are allowed if properly justified and accepted by reviewers. + +Current coverage statistics can be viewed [here](https://ci.ros2.org/job/nightly_linux_coverage/lastCompletedBuild/cobertura/src_ros2_rclcpp_rclcpp_lifecycle_src/). 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] + +`rclcpp_lifecycle` follows the recommendations for performance testing of C/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. + +The performance tests of `rclcpp_lifecycle` are located in the [test/benchmark directory](https://github.com/ros2/rclcpp/tree/rolling/rclcpp_lifecycle/test/benchmark). + +Package and system level performance benchmarks that cover features of `rclcpp_lifecycle` 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] + +`rclcpp_lifecycle` uses and passes all the ROS 2 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/rclcpp_lifecycle/) +* [linux_release](https://ci.ros2.org/view/nightly/job/nightly_linux_release/lastBuild/testReport/rclcpp_lifecycle/) +* [mac_osx_release](https://ci.ros2.org/view/nightly/job/nightly_osx_release/lastBuild/testReport/rclcpp_lifecycle/) +* [windows_release](https://ci.ros2.org/view/nightly/job/nightly_win_rel/lastBuild/testReport/rclcpp_lifecycle/) + +## Dependencies [5] + +Below are evaluations of each of `rclcpp_lifecycle`'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 and Optional Runtime ROS Dependencies [5.i]/[5.ii] + +`rclcpp_lifecycle` has the following runtime ROS dependencies: + +#### `lifecycle_msgs` + +The `lifecycle_msgs` package contains message and service definitions for managing lifecycle nodes. + +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcl_interfaces/blob/rolling/lifecycle_msgs/QUALITY_DECLARATION.md). + +#### `rclcpp` + +The `rclcpp` package provides the ROS client library in C++. + +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rclcpp/blob/rolling/rclcpp/QUALITY_DECLARATION.md). + +#### `rcl_lifecycle` + +The `rcl_lifecycle` package provides functionality for ROS 2 lifecycle nodes in C. + +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcl/blob/rolling/rcl_lifecycle/QUALITY_DECLARATION.md). + +#### `rosidl_typesupport_cpp` + +The `rosidl_typesupport_cpp` package generates the type support for C++ messages. + +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rosidl_typesupport/blob/rolling/rosidl_typesupport_cpp/QUALITY_DECLARATION.md). + +#### `rmw` + +`rmw` is the ROS 2 middleware library. + +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rmw/blob/rolling/rmw/QUALITY_DECLARATION.md). + +### Direct Runtime non-ROS Dependency [5.iii] + +`rclcpp_lifecycle` has no run-time or build-time dependencies that need to be considered for this declaration. + +## Platform Support [6] + +`rclcpp_lifecycle` supports all of the tier 1 platforms as described in [REP-2000](https://www.ros.org/reps/rep-2000.html#support-tiers), and tests each change against all of them. + +Currently nightly build status can be seen here: +* [linux-aarch64_release](https://ci.ros2.org/view/nightly/job/nightly_linux-aarch64_release/lastBuild/rclcpp_lifecycle/) +* [linux_release](https://ci.ros2.org/view/nightly/job/nightly_linux_release/lastBuild/rclcpp_lifecycle/) +* [mac_osx_release](https://ci.ros2.org/view/nightly/job/nightly_osx_release/lastBuild/rclcpp_lifecycle/) +* [windows_release](https://ci.ros2.org/view/nightly/job/nightly_win_rel/lastBuild/rclcpp_lifecycle/) + +## Security + +### Vulnerability Disclosure Policy [7.i] + +This package conforms to the Vulnerability Disclosure Policy in [REP-2006](https://www.ros.org/reps/rep-2006.html). diff --git a/rclcpp_lifecycle/README.md b/rclcpp_lifecycle/README.md new file mode 100644 index 0000000000..72bb50fbd2 --- /dev/null +++ b/rclcpp_lifecycle/README.md @@ -0,0 +1,10 @@ +# `rclcpp_lifecycle` + +Package containing a prototype for lifecycle implementation. + +The link to the latest rclcpp_lifecycle API documentation, which includes a complete list of its main components and features, can be found on the [rclcpp_lifecycle package info page](https://docs.ros.org/en/rolling/p/rclcpp_lifecycle). +For more information about LifeCycle in ROS 2, see the [design document](http://design.ros2.org/articles/node_lifecycle.html). + +## Quality Declaration + +This package claims to be in the **Quality Level 1** category, see the [Quality Declaration](QUALITY_DECLARATION.md) for more details. diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 12137e5d7b..d63cc726f9 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -12,45 +12,80 @@ // See the License for the specific language governing permissions and // limitations under the License. +/** \mainpage rclcpp_lifecycle: Package containing a prototype for lifecycle implementation. + * + * - Lifecycle states: Define the State class. There are 4 primary states: Unconfigured, Inactive, + * Active and Finalized. There are also 6 transition states which are intermediate states during + * a requested transition. Configuring, CleaningUp, ShuttingDown, Activating, Deactivating and + * ErrorProcessing. + * - rclcpp_lifecycle/state.hpp + * - Lifecycle transitions Define the Transition class. There are 7 transitions exposed to a + * supervisory process, they are: create, configure, cleanup, activate, deactivate, shutdown and + * destroy. + * - rclcpp_lifecycle/transition.hpp + * - Lifecycle publisher creates a publisher that allows enabling and disabling message publication. + * - rclcpp_lifecycle/publisher.hpp + * - Lifecycle node: An optional interface class for life cycle node implementations. + * - rclcpp_lifecycle/lifecycle_node.hpp + * + * Some useful internal abstractions and utilities: + * - Macros for controlling symbol visibility on the library + * - rclcpp_lifecycle/visibility_control.h + */ + #ifndef RCLCPP_LIFECYCLE__LIFECYCLE_NODE_HPP_ #define RCLCPP_LIFECYCLE__LIFECYCLE_NODE_HPP_ -#include +#include +#include #include -#include #include +#include +#include +#include + +#include "rcutils/macros.h" #include "rcl/error_handling.h" #include "rcl/node.h" #include "rcl_interfaces/msg/list_parameters_result.hpp" #include "rcl_interfaces/msg/parameter_descriptor.hpp" -#include "rcl_interfaces/msg/parameter_event.hpp" #include "rcl_interfaces/msg/set_parameters_result.hpp" #include "rclcpp/callback_group.hpp" #include "rclcpp/client.hpp" #include "rclcpp/clock.hpp" -#include "rclcpp/context.hpp" #include "rclcpp/event.hpp" +#include "rclcpp/generic_publisher.hpp" +#include "rclcpp/generic_subscription.hpp" #include "rclcpp/logger.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/message_memory_strategy.hpp" +#include "rclcpp/node_options.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/node_interfaces/node_clock_interface.hpp" #include "rclcpp/node_interfaces/node_graph_interface.hpp" #include "rclcpp/node_interfaces/node_logging_interface.hpp" #include "rclcpp/node_interfaces/node_parameters_interface.hpp" #include "rclcpp/node_interfaces/node_services_interface.hpp" +#include "rclcpp/node_interfaces/node_time_source_interface.hpp" #include "rclcpp/node_interfaces/node_timers_interface.hpp" #include "rclcpp/node_interfaces/node_topics_interface.hpp" +#include "rclcpp/node_interfaces/node_type_descriptions_interface.hpp" +#include "rclcpp/node_interfaces/node_waitables_interface.hpp" #include "rclcpp/parameter.hpp" #include "rclcpp/publisher.hpp" +#include "rclcpp/publisher_options.hpp" +#include "rclcpp/qos.hpp" #include "rclcpp/service.hpp" #include "rclcpp/subscription.hpp" +#include "rclcpp/subscription_options.hpp" #include "rclcpp/time.hpp" #include "rclcpp/timer.hpp" +#include "rmw/types.h" + #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/lifecycle_publisher.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -60,6 +95,29 @@ namespace rclcpp_lifecycle { +// include these here to work around an esoteric Windows error where the namespace +// cannot be used in the function declaration below without getting an error like: +// 'rclcpp::SubscriptionOptionsWithAllocator': +// no appropriate default constructor available +template +using PublisherOptionsWithAllocator = rclcpp::PublisherOptionsWithAllocator; +template +using SubscriptionOptionsWithAllocator = rclcpp::SubscriptionOptionsWithAllocator; + +template +PublisherOptionsWithAllocator +create_default_publisher_options() +{ + return rclcpp::PublisherOptionsWithAllocator(); +} + +template +SubscriptionOptionsWithAllocator +create_default_subscription_options() +{ + return rclcpp::SubscriptionOptionsWithAllocator(); +} + /// LifecycleNode for creating lifecycle components /** * has lifecycle nodeinterface for configuring this node. @@ -73,240 +131,568 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, /// Create a new lifecycle node with the specified name. /** * \param[in] node_name Name of the node. - * \param[in] node_name Namespace of the node. - * \param[in] use_intra_process_comms True to use the optimized intra-process communication - * pipeline to pass messages between nodes in the same process using shared memory. + * \param[in] options Additional options to control creation of the node. + * \param[in] enable_communication_interface Deciding whether the communication interface of the underlying rcl_lifecycle_node shall be enabled. */ RCLCPP_LIFECYCLE_PUBLIC explicit LifecycleNode( const std::string & node_name, - const std::string & namespace_ = "", - bool use_intra_process_comms = false); + const rclcpp::NodeOptions & options = rclcpp::NodeOptions(), + bool enable_communication_interface = true); - /// Create a node based on the node name and a rclcpp::Context. + /// Create a node based on the node name /** * \param[in] node_name Name of the node. - * \param[in] node_name Namespace of the node. - * \param[in] context The context for the node (usually represents the state of a process). - * \param[in] use_intra_process_comms True to use the optimized intra-process communication - * pipeline to pass messages between nodes in the same process using shared memory. + * \param[in] namespace_ Namespace of the node. + * \param[in] options Additional options to control creation of the node. + * \param[in] enable_communication_interface Deciding whether the communication interface of the underlying rcl_lifecycle_node shall be enabled. */ RCLCPP_LIFECYCLE_PUBLIC LifecycleNode( const std::string & node_name, const std::string & namespace_, - rclcpp::Context::SharedPtr context, - bool use_intra_process_comms = false); + const rclcpp::NodeOptions & options = rclcpp::NodeOptions(), + bool enable_communication_interface = true); RCLCPP_LIFECYCLE_PUBLIC virtual ~LifecycleNode(); /// Get the name of the node. - // \return The name of the node. + /** + * \return The name of the node. + */ RCLCPP_LIFECYCLE_PUBLIC const char * get_name() const; - /// Get the namespace of the node. - // \return The namespace of the node. + /// Get the namespace of the node + /** + * \return The namespace of the node. + */ RCLCPP_LIFECYCLE_PUBLIC const char * get_namespace() const; + /// Get the fully-qualified name of the node. + /** + * The fully-qualified name includes the local namespace and name of the node. + * \return fully-qualified name of the node. + */ + RCLCPP_LIFECYCLE_PUBLIC + const char * + get_fully_qualified_name() const; + /// Get the logger of the node. - /** \return The logger of the node. */ + /** + * \return The logger of the node. + */ RCLCPP_LIFECYCLE_PUBLIC rclcpp::Logger get_logger() const; /// Create and return a callback group. + /** + * \param[in] group_type callback group type to create by this method. + * \param[in] automatically_add_to_executor_with_node A boolean that + * determines whether a callback group is automatically added to an executor + * with the node with which it is associated. + * \return a callback group + */ RCLCPP_LIFECYCLE_PUBLIC - rclcpp::callback_group::CallbackGroup::SharedPtr - create_callback_group(rclcpp::callback_group::CallbackGroupType group_type); + rclcpp::CallbackGroup::SharedPtr + create_callback_group( + rclcpp::CallbackGroupType group_type, + bool automatically_add_to_executor_with_node = true); - /// Return the list of callback groups in the node. + /// Iterate over the callback groups in the node, calling func on each valid one. RCLCPP_LIFECYCLE_PUBLIC - const std::vector & - get_callback_groups() const; + void + for_each_callback_group( + const rclcpp::node_interfaces::NodeBaseInterface::CallbackGroupFunction & func); /// Create and return a Publisher. /** * \param[in] topic_name The topic for this publisher to publish on. - * \param[in] qos_history_depth The depth of the publisher message queue. - * \return Shared pointer to the created publisher. + * \param[in] qos The Quality of Service settings for this publisher. + * \param[in] options The publisher options for this publisher. + * \return Shared pointer to the created lifecycle publisher. */ - template> - std::shared_ptr> - create_publisher( - const std::string & topic_name, size_t qos_history_depth, - std::shared_ptr allocator = nullptr); - - /// Create and return a LifecyclePublisher. - /** - * \param[in] topic_name The topic for this publisher to publish on. - * \param[in] qos_history_depth The depth of the publisher message queue. - * \return Shared pointer to the created publisher. - */ - template> - std::shared_ptr> + template> + std::shared_ptr> create_publisher( const std::string & topic_name, - const rmw_qos_profile_t & qos_profile = rmw_qos_profile_default, - std::shared_ptr allocator = nullptr); + const rclcpp::QoS & qos, + const PublisherOptionsWithAllocator & options = ( + create_default_publisher_options() + ) + ); /// Create and return a Subscription. /** * \param[in] topic_name The topic to subscribe on. * \param[in] callback The user-defined callback function. - * \param[in] qos_profile The quality of service profile to pass on to the rmw implementation. - * \param[in] group The callback group for this subscription. NULL for no callback group. - * \param[in] ignore_local_publications True to ignore local publications. + * \param[in] qos The quality of service for this subscription. + * \param[in] options The subscription options for this subscription. * \param[in] msg_mem_strat The message memory strategy to use for allocating messages. * \return Shared pointer to the created subscription. */ - /* TODO(jacquelinekay): - Windows build breaks when static member function passed as default - argument to msg_mem_strat, nullptr is a workaround. - */ template< typename MessageT, typename CallbackT, - typename Alloc = std::allocator, - typename SubscriptionT = rclcpp::Subscription> + typename AllocatorT = std::allocator, + typename SubscriptionT = rclcpp::Subscription, + typename MessageMemoryStrategyT = typename SubscriptionT::MessageMemoryStrategyType> std::shared_ptr create_subscription( const std::string & topic_name, + const rclcpp::QoS & qos, CallbackT && callback, - const rmw_qos_profile_t & qos_profile = rmw_qos_profile_default, - rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr, - bool ignore_local_publications = false, - typename rclcpp::message_memory_strategy::MessageMemoryStrategy::SharedPtr - msg_mem_strat = nullptr, - std::shared_ptr allocator = nullptr); - - /// Create and return a Subscription. + const SubscriptionOptionsWithAllocator & options = + create_default_subscription_options(), + typename MessageMemoryStrategyT::SharedPtr msg_mem_strat = ( + MessageMemoryStrategyT::create_default() + ) + ); + + /// Create a timer that uses the wall clock to drive the callback. /** - * \param[in] topic_name The topic to subscribe on. - * \param[in] qos_history_depth The depth of the subscription's incoming message queue. - * \param[in] callback The user-defined callback function. - * \param[in] group The callback group for this subscription. NULL for no callback group. - * \param[in] ignore_local_publications True to ignore local publications. - * \param[in] msg_mem_strat The message memory strategy to use for allocating messages. - * \return Shared pointer to the created subscription. - */ - /* TODO(jacquelinekay): - Windows build breaks when static member function passed as default - argument to msg_mem_strat, nullptr is a workaround. + * \param[in] period Time interval between triggers of the callback. + * \param[in] callback User-defined callback function. + * \param[in] group Callback group to execute this timer's callback in. */ - template< - typename MessageT, - typename CallbackT, - typename Alloc = std::allocator, - typename SubscriptionT = rclcpp::Subscription> - std::shared_ptr - create_subscription( - const std::string & topic_name, - size_t qos_history_depth, - CallbackT && callback, - rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr, - bool ignore_local_publications = false, - typename rclcpp::message_memory_strategy::MessageMemoryStrategy::SharedPtr - msg_mem_strat = nullptr, - std::shared_ptr allocator = nullptr); + template + typename rclcpp::WallTimer::SharedPtr + create_wall_timer( + std::chrono::duration period, + CallbackT callback, + rclcpp::CallbackGroup::SharedPtr group = nullptr); - /// Create a timer. + /// Create a timer that uses the node clock to drive the callback. /** * \param[in] period Time interval between triggers of the callback. * \param[in] callback User-defined callback function. * \param[in] group Callback group to execute this timer's callback in. */ - template - typename rclcpp::WallTimer::SharedPtr - create_wall_timer( - std::chrono::duration period, + template + typename rclcpp::GenericTimer::SharedPtr + create_timer( + std::chrono::duration period, CallbackT callback, - rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); + rclcpp::CallbackGroup::SharedPtr group = nullptr); - /* Create and return a Client. */ + /// Create and return a Client. + /** + * \param[in] service_name The name on which the service is accessible. + * \param[in] qos Quality of service profile for client. + * \param[in] group Callback group to handle the reply to service calls. + * \return Shared pointer to the created client. + */ template typename rclcpp::Client::SharedPtr create_client( const std::string & service_name, - const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default, - rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); + const rclcpp::QoS & qos = rclcpp::ServicesQoS(), + rclcpp::CallbackGroup::SharedPtr group = nullptr); - /* Create and return a Service. */ + /// Create and return a Service. + /** + * \sa rclcpp::Node::create_service + */ template typename rclcpp::Service::SharedPtr create_service( const std::string & service_name, CallbackT && callback, - const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default, - rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); + const rclcpp::QoS & qos = rclcpp::ServicesQoS(), + rclcpp::CallbackGroup::SharedPtr group = nullptr); + /// Create and return a GenericPublisher. + /** + * \sa rclcpp::Node::create_generic_publisher + */ + template> + std::shared_ptr create_generic_publisher( + const std::string & topic_name, + const std::string & topic_type, + const rclcpp::QoS & qos, + const rclcpp::PublisherOptionsWithAllocator & options = ( + rclcpp::PublisherOptionsWithAllocator() + ) + ); + + /// Create and return a GenericSubscription. + /** + * \sa rclcpp::Node::create_generic_subscription + */ + template> + std::shared_ptr create_generic_subscription( + const std::string & topic_name, + const std::string & topic_type, + const rclcpp::QoS & qos, + std::function)> callback, + const rclcpp::SubscriptionOptionsWithAllocator & options = ( + rclcpp::SubscriptionOptionsWithAllocator() + ) + ); + + /// Declare and initialize a parameter, return the effective value. + /** + * \sa rclcpp::Node::declare_parameter + */ RCLCPP_LIFECYCLE_PUBLIC - std::vector - set_parameters(const std::vector & parameters); + const rclcpp::ParameterValue & + declare_parameter( + const std::string & name, + const rclcpp::ParameterValue & default_value, + const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor = + rcl_interfaces::msg::ParameterDescriptor(), + bool ignore_override = false); + + /// Declare and initialize a parameter, return the effective value. + /** + * \sa rclcpp::Node::declare_parameter + */ + RCLCPP_LIFECYCLE_PUBLIC + const rclcpp::ParameterValue & + declare_parameter( + const std::string & name, + rclcpp::ParameterType type, + const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor = + rcl_interfaces::msg::ParameterDescriptor{}, + bool ignore_override = false); + + /// Declare and initialize a parameter with a type. + /** + * \sa rclcpp::Node::declare_parameter + */ + template + auto + declare_parameter( + const std::string & name, + const ParameterT & default_value, + const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor = + rcl_interfaces::msg::ParameterDescriptor(), + bool ignore_override = false); + + /// Declare and initialize a parameter with a type. + /** + * See the non-templated declare_parameter() on this class for details. + */ + template + auto + declare_parameter( + const std::string & name, + const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor = + rcl_interfaces::msg::ParameterDescriptor(), + bool ignore_override = false); + + /// Declare and initialize several parameters with the same namespace and type. + /** + * \sa rclcpp::Node::declare_parameters + */ + template + std::vector + declare_parameters( + const std::string & namespace_, + const std::map & parameters); + + /// Declare and initialize several parameters with the same namespace and type. + /** + * \sa rclcpp::Node::declare_parameters + */ + template + std::vector + declare_parameters( + const std::string & namespace_, + const std::map< + std::string, + std::pair + > & parameters); + /// Undeclare a previously declared parameter. + /** + * \sa rclcpp::Node::undeclare_parameter + */ + RCLCPP_LIFECYCLE_PUBLIC + void + undeclare_parameter(const std::string & name); + + /// Return true if a given parameter is declared. + /** + * \sa rclcpp::Node::has_parameter + */ + RCLCPP_LIFECYCLE_PUBLIC + bool + has_parameter(const std::string & name) const; + + /// Set a single parameter. + /** + * \sa rclcpp::Node::set_parameter + */ RCLCPP_LIFECYCLE_PUBLIC rcl_interfaces::msg::SetParametersResult - set_parameters_atomically(const std::vector & parameters); + set_parameter(const rclcpp::Parameter & parameter); + /// Set one or more parameters, one at a time. + /** + * \sa rclcpp::Node::set_parameters + */ RCLCPP_LIFECYCLE_PUBLIC - std::vector - get_parameters(const std::vector & names) const; + std::vector + set_parameters(const std::vector & parameters); + /// Set one or more parameters, all at once. + /** + * \sa rclcpp::Node::set_parameters_atomically + */ + RCLCPP_LIFECYCLE_PUBLIC + rcl_interfaces::msg::SetParametersResult + set_parameters_atomically(const std::vector & parameters); + + /// Return the parameter by the given name. + /** + * \sa rclcpp::Node::get_parameter + */ RCLCPP_LIFECYCLE_PUBLIC - rclcpp::parameter::ParameterVariant + rclcpp::Parameter get_parameter(const std::string & name) const; + /// Get the value of a parameter by the given name, and return true if it was set. + /** + * \sa rclcpp::Node::get_parameter + */ RCLCPP_LIFECYCLE_PUBLIC bool get_parameter( const std::string & name, - rclcpp::parameter::ParameterVariant & parameter) const; + rclcpp::Parameter & parameter) const; + /// Get the value of a parameter by the given name, and return true if it was set. + /** + * \sa rclcpp::Node::get_parameter + */ template bool get_parameter(const std::string & name, ParameterT & parameter) const; + /// Get the parameter value, or the "alternative_value" if not set, and assign it to "parameter". + /** + * \sa rclcpp::Node::get_parameter_or + */ + template + bool + get_parameter_or( + const std::string & name, + ParameterT & value, + const ParameterT & alternative_value) const; + + /// Return the parameters by the given parameter names. + /** + * \sa rclcpp::Node::get_parameters + */ + RCLCPP_LIFECYCLE_PUBLIC + std::vector + get_parameters(const std::vector & names) const; + + /// Get the parameter values for all parameters that have a given prefix. + /** + * \sa rclcpp::Node::get_parameters + */ + template + bool + get_parameters( + const std::string & prefix, + std::map & values) const; + + /// Return the parameter descriptor for the given parameter name. + /** + * \sa rclcpp::Node::describe_parameter + */ + RCLCPP_LIFECYCLE_PUBLIC + rcl_interfaces::msg::ParameterDescriptor + describe_parameter(const std::string & name) const; + + /// Return a vector of parameter descriptors, one for each of the given names. + /** + * \sa rclcpp::Node::describe_parameters + */ RCLCPP_LIFECYCLE_PUBLIC std::vector describe_parameters(const std::vector & names) const; + /// Return a vector of parameter types, one for each of the given names. + /** + * \sa rclcpp::Node::get_parameter_types + */ RCLCPP_LIFECYCLE_PUBLIC std::vector get_parameter_types(const std::vector & names) const; + /// Return a list of parameters with any of the given prefixes, up to the given depth. + /** + * \sa rclcpp::Node::list_parameters + */ RCLCPP_LIFECYCLE_PUBLIC rcl_interfaces::msg::ListParametersResult list_parameters(const std::vector & prefixes, uint64_t depth) const; - /// Register the callback for parameter changes + using PreSetParametersCallbackHandle = + rclcpp::node_interfaces::PreSetParametersCallbackHandle; + using PreSetParametersCallbackType = + rclcpp::node_interfaces::NodeParametersInterface::PreSetParametersCallbackType; + + using OnSetParametersCallbackHandle = + rclcpp::node_interfaces::OnSetParametersCallbackHandle; + using OnSetParametersCallbackType = + rclcpp::node_interfaces::NodeParametersInterface::OnSetParametersCallbackType; + + using PostSetParametersCallbackHandle = + rclcpp::node_interfaces::PostSetParametersCallbackHandle; + using PostSetParametersCallbackType = + rclcpp::node_interfaces::NodeParametersInterface::PostSetParametersCallbackType; + + /// Add a callback that gets triggered before parameters are validated. + /** + * \sa rclcpp::Node::add_pre_set_parameters_callback + */ + RCLCPP_LIFECYCLE_PUBLIC + RCUTILS_WARN_UNUSED + rclcpp_lifecycle::LifecycleNode::PreSetParametersCallbackHandle::SharedPtr + add_pre_set_parameters_callback( + rclcpp_lifecycle::LifecycleNode::PreSetParametersCallbackType callback); + + /// Add a callback for when parameters are being set. + /** + * \sa rclcpp::Node::add_on_set_parameters_callback + */ + RCLCPP_LIFECYCLE_PUBLIC + RCUTILS_WARN_UNUSED + rclcpp_lifecycle::LifecycleNode::OnSetParametersCallbackHandle::SharedPtr + add_on_set_parameters_callback( + rclcpp_lifecycle::LifecycleNode::OnSetParametersCallbackType callback); + + /// Add a callback that gets triggered after parameters are set successfully. + /** + * \sa rclcpp::Node::add_post_set_parameters_callback + */ + RCLCPP_LIFECYCLE_PUBLIC + RCUTILS_WARN_UNUSED + rclcpp_lifecycle::LifecycleNode::PostSetParametersCallbackHandle::SharedPtr + add_post_set_parameters_callback( + rclcpp_lifecycle::LifecycleNode::PostSetParametersCallbackType callback); + + /// Remove a callback registered with `add_pre_set_parameters_callback`. + /** + * \sa rclcpp::Node::remove_pre_set_parameters_callback + */ + RCLCPP_LIFECYCLE_PUBLIC + void + remove_pre_set_parameters_callback( + const rclcpp_lifecycle::LifecycleNode::PreSetParametersCallbackHandle * const handler); + + /// Remove a callback registered with `add_on_set_parameters_callback`. /** - * \param[in] User defined callback function, It is expected to atomically set parameters. - * \note Repeated invocations of this function will overwrite previous callbacks + * \sa rclcpp::Node::remove_on_set_parameters_callback */ - template + RCLCPP_LIFECYCLE_PUBLIC + void + remove_on_set_parameters_callback( + const rclcpp_lifecycle::LifecycleNode::OnSetParametersCallbackHandle * const handler); + + /// Remove a callback registered with `add_post_set_parameters_callback`. + /** + * \sa rclcpp::Node::remove_post_set_parameters_callback + */ + RCLCPP_LIFECYCLE_PUBLIC void - register_param_change_callback(CallbackT && callback); + remove_post_set_parameters_callback( + const rclcpp_lifecycle::LifecycleNode::PostSetParametersCallbackHandle * const handler); + /// Return a vector of existing node names (string). + /** + * \sa rclcpp::Node::get_node_names + */ + RCLCPP_LIFECYCLE_PUBLIC + std::vector + get_node_names() const; + + /// Return a map of existing topic names to list of topic types. + /** + * \sa rclcpp::Node::get_topic_names_and_types + */ RCLCPP_LIFECYCLE_PUBLIC std::map> get_topic_names_and_types(bool no_demangle = false) const; + /// Return a map of existing service names to list of topic types. + /** + * \sa rclcpp::Node::get_service_names_and_types + */ RCLCPP_LIFECYCLE_PUBLIC std::map> get_service_names_and_types() const; + /// Return a map of existing service names to list of service types for a specific node. + /** + * This function only considers services - not clients. + * + * \param[in] node_name name of the node + * \param[in] namespace_ namespace of the node + */ + RCLCPP_LIFECYCLE_PUBLIC + std::map> + get_service_names_and_types_by_node( + const std::string & node_name, + const std::string & namespace_) const; + + /// Return the number of publishers that are advertised on a given topic. + /** + * \sa rclcpp::Node::count_publishers + */ RCLCPP_LIFECYCLE_PUBLIC size_t count_publishers(const std::string & topic_name) const; + /// Return the number of subscribers who have created a subscription for a given topic. + /** + * \sa rclcpp::Node::count_subscribers + */ RCLCPP_LIFECYCLE_PUBLIC size_t count_subscribers(const std::string & topic_name) const; + /// Return the number of clients created for a given service. + /** + * \sa rclcpp::Node::count_clients + */ + RCLCPP_LIFECYCLE_PUBLIC + size_t + count_clients(const std::string & service_name) const; + + /// Return the number of services created for a given service. + /** + * \sa rclcpp::Node::count_services + */ + RCLCPP_LIFECYCLE_PUBLIC + size_t + count_services(const std::string & service_name) const; + + /// Return the topic endpoint information about publishers on a given topic. + /** + * \sa rclcpp::Node::get_publishers_info_by_topic + */ + RCLCPP_LIFECYCLE_PUBLIC + std::vector + get_publishers_info_by_topic(const std::string & topic_name, bool no_mangle = false) const; + + /// Return the topic endpoint information about subscriptions on a given topic. + /** + * \sa rclcpp::Node::get_subscriptions_info_by_topic + */ + RCLCPP_LIFECYCLE_PUBLIC + std::vector + get_subscriptions_info_by_topic(const std::string & topic_name, bool no_mangle = false) const; + /// Return a graph event, which will be set anytime a graph change occurs. /* The graph Event object is a loan which must be returned. * The Event object is scoped and therefore to return the load just let it go @@ -329,154 +715,355 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, rclcpp::Event::SharedPtr event, std::chrono::nanoseconds timeout); + /// Get a clock as a non-const shared pointer which is managed by the node. + /** + * \sa rclcpp::node_interfaces::NodeClock::get_clock + */ RCLCPP_LIFECYCLE_PUBLIC rclcpp::Clock::SharedPtr get_clock(); + /// Get a clock as a const shared pointer which is managed by the node. + /** + * \sa rclcpp::node_interfaces::NodeClock::get_clock + */ + RCLCPP_LIFECYCLE_PUBLIC + rclcpp::Clock::ConstSharedPtr + get_clock() const; + + /// Returns current time from the time source specified by clock_type. + /** + * \sa rclcpp::Clock::now + */ RCLCPP_LIFECYCLE_PUBLIC rclcpp::Time - now(); + now() const; /// Return the Node's internal NodeBaseInterface implementation. + /** + * \sa rclcpp::Node::get_node_base_interface + */ RCLCPP_LIFECYCLE_PUBLIC rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface(); /// Return the Node's internal NodeClockInterface implementation. + /** + * \sa rclcpp::Node::get_node_clock_interface + */ RCLCPP_LIFECYCLE_PUBLIC rclcpp::node_interfaces::NodeClockInterface::SharedPtr get_node_clock_interface(); /// Return the Node's internal NodeGraphInterface implementation. + /** + * \sa rclcpp::Node::get_node_graph_interface + */ RCLCPP_LIFECYCLE_PUBLIC rclcpp::node_interfaces::NodeGraphInterface::SharedPtr get_node_graph_interface(); + /// Return the Node's internal NodeLoggingInterface implementation. + /** + * \sa rclcpp::Node::get_node_logging_interface + */ + RCLCPP_LIFECYCLE_PUBLIC + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr + get_node_logging_interface(); + /// Return the Node's internal NodeTimersInterface implementation. + /** + * \sa rclcpp::Node::get_node_timers_interface + */ RCLCPP_LIFECYCLE_PUBLIC rclcpp::node_interfaces::NodeTimersInterface::SharedPtr get_node_timers_interface(); /// Return the Node's internal NodeTopicsInterface implementation. + /** + * \sa rclcpp::Node::get_node_topics_interface + */ RCLCPP_LIFECYCLE_PUBLIC rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr get_node_topics_interface(); /// Return the Node's internal NodeServicesInterface implementation. + /** + * \sa rclcpp::Node::get_node_services_interface + */ RCLCPP_LIFECYCLE_PUBLIC rclcpp::node_interfaces::NodeServicesInterface::SharedPtr get_node_services_interface(); /// Return the Node's internal NodeParametersInterface implementation. + /** + * \sa rclcpp::Node::get_node_parameters_interface + */ RCLCPP_LIFECYCLE_PUBLIC rclcpp::node_interfaces::NodeParametersInterface::SharedPtr get_node_parameters_interface(); + /// Return the Node's internal NodeParametersInterface implementation. + /** + * \sa rclcpp::Node::get_node_time_source_interface + */ + RCLCPP_LIFECYCLE_PUBLIC + rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr + get_node_time_source_interface(); + + /// Return the Node's internal NodeTypeDescriptionsInterface implementation. + /** + * \sa rclcpp::Node::get_node_type_descriptions_interface + */ + RCLCPP_LIFECYCLE_PUBLIC + rclcpp::node_interfaces::NodeTypeDescriptionsInterface::SharedPtr + get_node_type_descriptions_interface(); + + /// Return the Node's internal NodeWaitablesInterface implementation. + /** + * \sa rclcpp::Node::get_node_waitables_interface + */ + RCLCPP_LIFECYCLE_PUBLIC + rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr + get_node_waitables_interface(); + + /// Return the NodeOptions used when creating this node. + /** + * \sa rclcpp::Node::get_node_options + */ + RCLCPP_LIFECYCLE_PUBLIC + const rclcpp::NodeOptions & + get_node_options() const; + // // LIFECYCLE COMPONENTS // + /// Return the current State. + /** + * \return the current state + */ RCLCPP_LIFECYCLE_PUBLIC const State & - get_current_state(); + get_current_state() const; + /// Return a list with the available states. + /** + * \return list with the available states. + */ RCLCPP_LIFECYCLE_PUBLIC std::vector - get_available_states(); + get_available_states() const; + + /// Return a list with the current available transitions. + /** + * \return list with the current available transitions. + */ + RCLCPP_LIFECYCLE_PUBLIC + std::vector + get_available_transitions() const; + /// Return a list with all the transitions. + /** + * \return list with all the transitions in the transition graph. + */ RCLCPP_LIFECYCLE_PUBLIC std::vector - get_available_transitions(); + get_transition_graph() const; - /// trigger the specified transition + /// Trigger the specified transition. /* - * return the new state after this transition + * \return the new state after this transition */ RCLCPP_LIFECYCLE_PUBLIC const State & trigger_transition(const Transition & transition); + /// Trigger the specified transition and get the callback return code. + /* + * \param[out] cb_return_code transition callback return code + * \return the new state after this transition + */ RCLCPP_LIFECYCLE_PUBLIC const State & trigger_transition( - const Transition & transition, rcl_lifecycle_transition_key_t & cb_return_code); + const Transition & transition, LifecycleNodeInterface::CallbackReturn & cb_return_code); + /// Trigger the specified transition based on an id. + /* + * \return the new state after this transition + */ RCLCPP_LIFECYCLE_PUBLIC const State & trigger_transition(uint8_t transition_id); + /// Trigger the specified transition based on an id and get the callback return code. + /* + * \param[out] cb_return_code transition callback return code + * \return the new state after this transition + */ RCLCPP_LIFECYCLE_PUBLIC const State & trigger_transition( - uint8_t transition_id, rcl_lifecycle_transition_key_t & cb_return_code); + uint8_t transition_id, LifecycleNodeInterface::CallbackReturn & cb_return_code); + /// Trigger the configure transition + /* + * \param[out] cb_return_code transition callback return code. + * \return the new state after this transition + */ RCLCPP_LIFECYCLE_PUBLIC const State & configure(); + /// Trigger the configure transition and get the callback return code. + /* + * \param[out] cb_return_code transition callback return code + * \return the new state after this transition + */ RCLCPP_LIFECYCLE_PUBLIC const State & - configure(rcl_lifecycle_transition_key_t & cb_return_code); + configure(LifecycleNodeInterface::CallbackReturn & cb_return_code); + /// Trigger the cleanup transition. + /* + * \return the new state after this transition + */ RCLCPP_LIFECYCLE_PUBLIC const State & cleanup(); + /// Trigger the cleanup transition and get the callback return code. + /* + * \param[out] cb_return_code transition callback return code + * \return the new state after this transition + */ RCLCPP_LIFECYCLE_PUBLIC const State & - cleanup(rcl_lifecycle_transition_key_t & cb_return_code); + cleanup(LifecycleNodeInterface::CallbackReturn & cb_return_code); + /// Trigger the activate transition. + /* + * \return the new state after this transition + */ RCLCPP_LIFECYCLE_PUBLIC const State & activate(); + /// Trigger the activate transition and get the callback return code. + /* + * \param[out] cb_return_code transition callback return code + * \return the new state after this transition + */ RCLCPP_LIFECYCLE_PUBLIC const State & - activate(rcl_lifecycle_transition_key_t & cb_return_code); + activate(LifecycleNodeInterface::CallbackReturn & cb_return_code); + /// Trigger the deactivate transition + /* + * \return the new state after this transition + */ RCLCPP_LIFECYCLE_PUBLIC const State & deactivate(); + /// Trigger the deactivate transition and get the callback return code. + /* + * \param[out] cb_return_code transition callback return code + * \return the new state after this transition + */ RCLCPP_LIFECYCLE_PUBLIC const State & - deactivate(rcl_lifecycle_transition_key_t & cb_return_code); + deactivate(LifecycleNodeInterface::CallbackReturn & cb_return_code); + /// Trigger the shutdown transition + /* + * \return the new state after this transition + */ RCLCPP_LIFECYCLE_PUBLIC const State & shutdown(); + /// Trigger the shutdown transition and get the callback return code. + /* + * \param[out] cb_return_code transition callback return code + * \return the new state after this transition + */ RCLCPP_LIFECYCLE_PUBLIC const State & - shutdown(rcl_lifecycle_transition_key_t & cb_return_code); + shutdown(LifecycleNodeInterface::CallbackReturn & cb_return_code); + /// Register the configure callback + /** + * This callback will be called when the transition to this state is triggered + * \param[in] fcn callback function to call + * \return always true + */ RCLCPP_LIFECYCLE_PUBLIC bool - register_on_configure(std::function fcn); + register_on_configure(std::function fcn); + /// Register the cleanup callback + /** + * This callback will be called when the transition to this state is triggered + * \param[in] fcn callback function to call + * \return always true + */ RCLCPP_LIFECYCLE_PUBLIC bool - register_on_cleanup(std::function fcn); + register_on_cleanup(std::function fcn); + /// Register the shutdown callback + /** + * This callback will be called when the transition to this state is triggered + * \param[in] fcn callback function to call + * \return always true + */ RCLCPP_LIFECYCLE_PUBLIC bool - register_on_shutdown(std::function fcn); + register_on_shutdown(std::function fcn); + /// Register the activate callback + /** + * This callback will be called when the transition to this state is triggered + * \param[in] fcn callback function to call + * \return always true + */ RCLCPP_LIFECYCLE_PUBLIC bool - register_on_activate(std::function fcn); + register_on_activate(std::function fcn); + /// Register the deactivate callback + /** + * This callback will be called when the transition to this state is triggered + * \param[in] fcn callback function to call + * \return always true + */ RCLCPP_LIFECYCLE_PUBLIC bool - register_on_deactivate(std::function fcn); + register_on_deactivate(std::function fcn); + /// Register the error callback + /** + * This callback will be called when the transition to this state is triggered + * \param[in] fcn callback function to call + * \return always true + */ RCLCPP_LIFECYCLE_PUBLIC bool - register_on_error(std::function fcn); + register_on_error(std::function fcn); + + RCLCPP_LIFECYCLE_PUBLIC + CallbackReturn + on_activate(const State & previous_state) override; + + RCLCPP_LIFECYCLE_PUBLIC + CallbackReturn + on_deactivate(const State & previous_state) override; protected: RCLCPP_LIFECYCLE_PUBLIC void - add_publisher_handle(std::shared_ptr pub); + add_managed_entity(std::weak_ptr managed_entity); RCLCPP_LIFECYCLE_PUBLIC void @@ -485,20 +1072,19 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, private: RCLCPP_DISABLE_COPY(LifecycleNode) - RCLCPP_LIFECYCLE_PUBLIC - bool - group_in_node(rclcpp::callback_group::CallbackGroup::SharedPtr group); - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_; rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_; rclcpp::node_interfaces::NodeTimersInterface::SharedPtr node_timers_; rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_; rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_; - rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_; rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_; + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_; + rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr node_time_source_; + rclcpp::node_interfaces::NodeTypeDescriptionsInterface::SharedPtr node_type_descriptions_; + rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_; - bool use_intra_process_comms_; + const rclcpp::NodeOptions node_options_; class LifecycleNodeInterfaceImpl; std::unique_ptr impl_; diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp index 37a55afd9d..b1e73b6c64 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp @@ -15,157 +15,121 @@ #ifndef RCLCPP_LIFECYCLE__LIFECYCLE_NODE_IMPL_HPP_ #define RCLCPP_LIFECYCLE__LIFECYCLE_NODE_IMPL_HPP_ +#include +#include +#include +#include #include #include #include +#include -#include "rclcpp/contexts/default_context.hpp" -#include "rclcpp/intra_process_manager.hpp" -#include "rclcpp/parameter.hpp" +#include "rcl_interfaces/msg/parameter_descriptor.hpp" + +#include "rclcpp/callback_group.hpp" +#include "rclcpp/create_client.hpp" +#include "rclcpp/create_generic_publisher.hpp" +#include "rclcpp/create_generic_subscription.hpp" #include "rclcpp/create_publisher.hpp" +#include "rclcpp/create_service.hpp" #include "rclcpp/create_subscription.hpp" +#include "rclcpp/parameter.hpp" +#include "rclcpp/publisher_options.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/subscription_options.hpp" #include "rclcpp/type_support_decl.hpp" -#include "lifecycle_publisher.hpp" -#include "rclcpp_lifecycle/visibility_control.h" +#include "rmw/types.h" #include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "rclcpp_lifecycle/lifecycle_publisher.hpp" +#include "rclcpp_lifecycle/visibility_control.h" namespace rclcpp_lifecycle { -template -std::shared_ptr> -LifecycleNode::create_publisher( - const std::string & topic_name, size_t qos_history_depth, - std::shared_ptr allocator) -{ - if (!allocator) { - allocator = std::make_shared(); - } - rmw_qos_profile_t qos = rmw_qos_profile_default; - qos.depth = qos_history_depth; - return this->create_publisher(topic_name, qos, allocator); -} - -template -std::shared_ptr> +template +std::shared_ptr> LifecycleNode::create_publisher( const std::string & topic_name, - const rmw_qos_profile_t & qos_profile, - std::shared_ptr allocator) + const rclcpp::QoS & qos, + const rclcpp::PublisherOptionsWithAllocator & options) { - using PublisherT = rclcpp_lifecycle::LifecyclePublisher; - - // create regular publisher in rclcpp::Node - return rclcpp::create_publisher( - this->node_topics_.get(), + using PublisherT = rclcpp_lifecycle::LifecyclePublisher; + auto pub = rclcpp::create_publisher( + *this, topic_name, - qos_profile, - use_intra_process_comms_, - allocator); + qos, + options); + this->add_managed_entity(pub); + return pub; } // TODO(karsten1987): Create LifecycleSubscriber -template -std::shared_ptr -LifecycleNode::create_subscription( - const std::string & topic_name, - CallbackT && callback, - const rmw_qos_profile_t & qos_profile, - rclcpp::callback_group::CallbackGroup::SharedPtr group, - bool ignore_local_publications, - typename rclcpp::message_memory_strategy::MessageMemoryStrategy::SharedPtr - msg_mem_strat, - std::shared_ptr allocator) -{ - if (!allocator) { - allocator = std::make_shared(); - } - - if (!msg_mem_strat) { - using rclcpp::message_memory_strategy::MessageMemoryStrategy; - msg_mem_strat = MessageMemoryStrategy::create_default(); - } - - return rclcpp::create_subscription< - MessageT, CallbackT, Alloc, - rclcpp::Subscription>( - this->node_topics_.get(), - topic_name, - std::forward(callback), - qos_profile, - group, - ignore_local_publications, - use_intra_process_comms_, - msg_mem_strat, - allocator); -} - template< typename MessageT, typename CallbackT, - typename Alloc, - typename SubscriptionT> + typename AllocatorT, + typename SubscriptionT, + typename MessageMemoryStrategyT> std::shared_ptr LifecycleNode::create_subscription( const std::string & topic_name, - size_t qos_history_depth, + const rclcpp::QoS & qos, CallbackT && callback, - rclcpp::callback_group::CallbackGroup::SharedPtr group, - bool ignore_local_publications, - typename rclcpp::message_memory_strategy::MessageMemoryStrategy::SharedPtr - msg_mem_strat, - std::shared_ptr allocator) -{ - rmw_qos_profile_t qos = rmw_qos_profile_default; - qos.depth = qos_history_depth; - return this->create_subscription( + const rclcpp::SubscriptionOptionsWithAllocator & options, + typename MessageMemoryStrategyT::SharedPtr msg_mem_strat) +{ + return rclcpp::create_subscription( + *this, topic_name, - std::forward(callback), qos, - group, - ignore_local_publications, - msg_mem_strat, - allocator); + std::forward(callback), + options, + msg_mem_strat); } -template +template typename rclcpp::WallTimer::SharedPtr LifecycleNode::create_wall_timer( - std::chrono::duration period, + std::chrono::duration period, CallbackT callback, - rclcpp::callback_group::CallbackGroup::SharedPtr group) + rclcpp::CallbackGroup::SharedPtr group) { - auto timer = rclcpp::WallTimer::make_shared( - std::chrono::duration_cast(period), - std::move(callback)); - node_timers_->add_timer(timer, group); - return timer; + return rclcpp::create_wall_timer( + period, + std::move(callback), + group, + this->node_base_.get(), + this->node_timers_.get()); +} + +template +typename rclcpp::GenericTimer::SharedPtr +LifecycleNode::create_timer( + std::chrono::duration period, + CallbackT callback, + rclcpp::CallbackGroup::SharedPtr group) +{ + return rclcpp::create_timer( + this->get_clock(), + period, + std::move(callback), + group, + this->node_base_.get(), + this->node_timers_.get()); } template typename rclcpp::Client::SharedPtr LifecycleNode::create_client( const std::string & service_name, - const rmw_qos_profile_t & qos_profile, - rclcpp::callback_group::CallbackGroup::SharedPtr group) + const rclcpp::QoS & qos, + rclcpp::CallbackGroup::SharedPtr group) { - rcl_client_options_t options = rcl_client_get_default_options(); - options.qos = qos_profile; - - using rclcpp::Client; - using rclcpp::ClientBase; - - auto cli = Client::make_shared( - node_base_.get(), - node_graph_, - service_name, - options); - - auto cli_base_ptr = std::dynamic_pointer_cast(cli); - node_services_->add_client(cli_base_ptr, group); - return cli; + return rclcpp::create_client( + node_base_, node_graph_, node_services_, + service_name, qos, group); } template @@ -173,39 +137,173 @@ typename rclcpp::Service::SharedPtr LifecycleNode::create_service( const std::string & service_name, CallbackT && callback, - const rmw_qos_profile_t & qos_profile, - rclcpp::callback_group::CallbackGroup::SharedPtr group) + const rclcpp::QoS & qos, + rclcpp::CallbackGroup::SharedPtr group) +{ + return rclcpp::create_service( + node_base_, node_services_, + service_name, std::forward(callback), qos, group); +} + +template +std::shared_ptr +LifecycleNode::create_generic_publisher( + const std::string & topic_name, + const std::string & topic_type, + const rclcpp::QoS & qos, + const rclcpp::PublisherOptionsWithAllocator & options) +{ + return rclcpp::create_generic_publisher( + node_topics_, + // TODO(karsten1987): LifecycleNode is currently not supporting subnamespaces + // see https://github.com/ros2/rclcpp/issues/1614 + topic_name, + topic_type, + qos, + options + ); +} + +template +std::shared_ptr +LifecycleNode::create_generic_subscription( + const std::string & topic_name, + const std::string & topic_type, + const rclcpp::QoS & qos, + std::function)> callback, + const rclcpp::SubscriptionOptionsWithAllocator & options) { - rclcpp::AnyServiceCallback any_service_callback; - any_service_callback.set(std::forward(callback)); + return rclcpp::create_generic_subscription( + node_topics_, + // TODO(karsten1987): LifecycleNode is currently not supporting subnamespaces + // see https://github.com/ros2/rclcpp/issues/1614 + topic_name, + topic_type, + qos, + std::move(callback), + options + ); +} - rcl_service_options_t service_options = rcl_service_get_default_options(); - service_options.qos = qos_profile; +template +auto +LifecycleNode::declare_parameter( + const std::string & name, + const ParameterT & default_value, + const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor, + bool ignore_override) +{ + return this->declare_parameter( + name, + rclcpp::ParameterValue(default_value), + parameter_descriptor, + ignore_override + ).get(); +} - auto serv = rclcpp::Service::make_shared( - node_base_->get_shared_rcl_node_handle(), - service_name, any_service_callback, service_options); - auto serv_base_ptr = std::dynamic_pointer_cast(serv); - node_services_->add_service(serv_base_ptr, group); - return serv; +template +auto +LifecycleNode::declare_parameter( + const std::string & name, + const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor, + bool ignore_override) +{ + // get advantage of parameter value template magic to get + // the correct rclcpp::ParameterType from ParameterT + rclcpp::ParameterValue value{ParameterT{}}; + return this->declare_parameter( + name, + value.get_type(), + parameter_descriptor, + ignore_override + ).get(); +} + +template +std::vector +LifecycleNode::declare_parameters( + const std::string & namespace_, + const std::map & parameters) +{ + std::vector result; + std::string normalized_namespace = namespace_.empty() ? "" : (namespace_ + "."); + std::transform( + parameters.begin(), parameters.end(), std::back_inserter(result), + [this, &normalized_namespace](auto element) { + return this->declare_parameter(normalized_namespace + element.first, element.second); + } + ); + return result; +} + +template +std::vector +LifecycleNode::declare_parameters( + const std::string & namespace_, + const std::map< + std::string, + std::pair + > & parameters) +{ + std::vector result; + std::string normalized_namespace = namespace_.empty() ? "" : (namespace_ + "."); + std::transform( + parameters.begin(), parameters.end(), std::back_inserter(result), + [this, &normalized_namespace](auto element) { + return static_cast( + this->declare_parameter( + normalized_namespace + element.first, + element.second.first, + element.second.second) + ); + } + ); + return result; } template bool LifecycleNode::get_parameter(const std::string & name, ParameterT & parameter) const { - rclcpp::parameter::ParameterVariant parameter_variant(name, parameter); - bool result = get_parameter(name, parameter_variant); - parameter = parameter_variant.get_value(); + rclcpp::Parameter param(name, parameter); + bool result = get_parameter(name, param); + parameter = param.get_value(); return result; } -template -void -LifecycleNode::register_param_change_callback(CallbackT && callback) +// this is a partially-specialized version of get_parameter above, +// where our concrete type for ParameterT is std::map, but the to-be-determined +// type is the value in the map. +template +bool +LifecycleNode::get_parameters( + const std::string & prefix, + std::map & values) const { - this->node_parameters_->register_param_change_callback(std::forward(callback)); + std::map params; + bool result = node_parameters_->get_parameters_by_prefix(prefix, params); + if (result) { + for (const auto & param : params) { + values[param.first] = param.second.get_value(); + } + } + + return result; +} + +template +bool +LifecycleNode::get_parameter_or( + const std::string & name, + ParameterT & value, + const ParameterT & alternative_value) const +{ + bool got_parameter = get_parameter(name, value); + if (!got_parameter) { + value = alternative_value; + } + return got_parameter; } } // namespace rclcpp_lifecycle diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp index a9e8df87cb..ccc60c71d4 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp @@ -17,37 +17,30 @@ #include #include +#include +#include "rclcpp/logging.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/publisher.hpp" +#include "rclcpp/publisher_options.hpp" +#include "rclcpp/qos.hpp" + +#include "rclcpp_lifecycle/managed_entity.hpp" + namespace rclcpp_lifecycle { -/// base class with only -/** - * pure virtual functions. A managed - * node can then deactivate or activate - * the publishing. - * It is more a convenient interface class - * than a necessary base class. - */ -class LifecyclePublisherInterface -{ -public: - virtual void on_activate() = 0; - virtual void on_deactivate() = 0; - virtual bool is_activated() = 0; -}; - /// brief child class of rclcpp Publisher class. /** - * Overrides all publisher functions to check for - * enabled/disabled state. + * Overrides all publisher functions to check for enabled/disabled state. */ template> -class LifecyclePublisher : public LifecyclePublisherInterface, +class LifecyclePublisher : public SimpleManagedEntity, public rclcpp::Publisher { public: + RCLCPP_SMART_PTR_DEFINITIONS(LifecyclePublisher) + using MessageAllocTraits = rclcpp::allocator::AllocRebind; using MessageAlloc = typename MessageAllocTraits::allocator_type; using MessageDeleter = rclcpp::allocator::Deleter; @@ -56,12 +49,13 @@ class LifecyclePublisher : public LifecyclePublisherInterface, LifecyclePublisher( rclcpp::node_interfaces::NodeBaseInterface * node_base, const std::string & topic, - const rcl_publisher_options_t & publisher_options, - std::shared_ptr allocator) - : rclcpp::Publisher( - node_base, topic, publisher_options, allocator), - enabled_(false) - {} + const rclcpp::QoS & qos, + const rclcpp::PublisherOptionsWithAllocator & options) + : rclcpp::Publisher(node_base, topic, qos, options), + should_log_(true), + logger_(rclcpp::get_logger("LifecyclePublisher")) + { + } ~LifecyclePublisher() {} @@ -72,12 +66,13 @@ class LifecyclePublisher : public LifecyclePublisherInterface, * to the actual rclcpp Publisher base class */ virtual void - publish(std::unique_ptr & msg) + publish(std::unique_ptr msg) { - if (!enabled_) { + if (!this->is_activated()) { + log_publisher_not_enabled(); return; } - rclcpp::Publisher::publish(msg); + rclcpp::Publisher::publish(std::move(msg)); } /// LifecyclePublisher publish function @@ -87,24 +82,10 @@ class LifecyclePublisher : public LifecyclePublisherInterface, * to the actual rclcpp Publisher base class */ virtual void - publish(const std::shared_ptr & msg) - { - if (!enabled_) { - return; - } - rclcpp::Publisher::publish(msg); - } - - /// LifecyclePublisher publish function - /** - * The publish function checks whether the communication - * was enabled or disabled and forwards the message - * to the actual rclcpp Publisher base class - */ - virtual void - publish(std::shared_ptr msg) + publish(const MessageT & msg) { - if (!enabled_) { + if (!this->is_activated()) { + log_publisher_not_enabled(); return; } rclcpp::Publisher::publish(msg); @@ -117,58 +98,49 @@ class LifecyclePublisher : public LifecyclePublisherInterface, * to the actual rclcpp Publisher base class */ virtual void - publish(const MessageT & msg) + publish( + rclcpp::LoanedMessage::ROSMessageType, Alloc> && loaned_msg) { - if (!enabled_) { + if (!this->is_activated()) { + log_publisher_not_enabled(); return; } - rclcpp::Publisher::publish(msg); + rclcpp::Publisher::publish(std::move(loaned_msg)); } - virtual void - publish(const MessageT * msg) + void + on_activate() override { - if (!msg) { - throw std::runtime_error("msg argument is nullptr"); - } - this->publish(*msg); + SimpleManagedEntity::on_activate(); + should_log_ = true; } - /// LifecyclePublisher publish function +private: + /// LifecyclePublisher log helper function /** - * The publish function checks whether the communication - * was enabled or disabled and forwards the message - * to the actual rclcpp Publisher base class + * Helper function that logs a message saying that publisher can't publish + * because it's not enabled. */ - virtual void - publish(std::shared_ptr & msg) + void log_publisher_not_enabled() { - if (!enabled_) { + // Nothing to do if we are not meant to log + if (!should_log_) { return; } - rclcpp::Publisher::publish(msg); - } - virtual void - on_activate() - { - enabled_ = true; - } + // Log the message + RCLCPP_WARN( + logger_, + "Trying to publish message on the topic '%s', but the publisher is not activated", + this->get_topic_name()); - virtual void - on_deactivate() - { - enabled_ = false; + // We stop logging until the flag gets enabled again + should_log_ = false; } - virtual bool - is_activated() - { - return enabled_; - } - -private: - bool enabled_ = false; + bool should_log_ = true; + rclcpp::Logger logger_; }; } // namespace rclcpp_lifecycle diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/managed_entity.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/managed_entity.hpp new file mode 100644 index 0000000000..4f83411e80 --- /dev/null +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/managed_entity.hpp @@ -0,0 +1,68 @@ +// Copyright 2022 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP_LIFECYCLE__MANAGED_ENTITY_HPP_ +#define RCLCPP_LIFECYCLE__MANAGED_ENTITY_HPP_ + +#include + +#include "rclcpp_lifecycle/visibility_control.h" + +namespace rclcpp_lifecycle +{ + +/// Base class for lifecycle entities, like `LifecyclePublisher`. +class ManagedEntityInterface +{ +public: + RCLCPP_LIFECYCLE_PUBLIC + virtual + ~ManagedEntityInterface() {} + + RCLCPP_LIFECYCLE_PUBLIC + virtual + void + on_activate() = 0; + + RCLCPP_LIFECYCLE_PUBLIC + virtual + void + on_deactivate() = 0; +}; + +/// A simple implementation of `ManagedEntityInterface`, which toogles a flag. +class SimpleManagedEntity : public ManagedEntityInterface +{ +public: + RCLCPP_LIFECYCLE_PUBLIC + ~SimpleManagedEntity() override = default; + + RCLCPP_LIFECYCLE_PUBLIC + void + on_activate() override; + + RCLCPP_LIFECYCLE_PUBLIC + void + on_deactivate() override; + + RCLCPP_LIFECYCLE_PUBLIC + bool + is_activated() const; + +private: + std::atomic activated_ = false; +}; + +} // namespace rclcpp_lifecycle +#endif // RCLCPP_LIFECYCLE__MANAGED_ENTITY_HPP_ diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp index 3c2339f7be..45748ea55d 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp @@ -15,10 +15,19 @@ #ifndef RCLCPP_LIFECYCLE__NODE_INTERFACES__LIFECYCLE_NODE_INTERFACE_HPP_ #define RCLCPP_LIFECYCLE__NODE_INTERFACES__LIFECYCLE_NODE_INTERFACE_HPP_ -#include "rcl_lifecycle/data_types.h" +#include "lifecycle_msgs/msg/transition.hpp" #include "rclcpp_lifecycle/state.hpp" #include "rclcpp_lifecycle/visibility_control.h" +#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp" + +// When windows.h is included, ERROR is defined as a macro. So the use of it later in this file, +// even as an enum, causes compilation errors. Work around this by undefining the macro here, +// and then redefining when this header is finished being included. +#if defined(_WIN32) +#pragma push_macro("ERROR") +#undef ERROR +#endif namespace rclcpp_lifecycle { @@ -44,55 +53,74 @@ class LifecycleNodeInterface LifecycleNodeInterface() {} public: + enum class CallbackReturn : uint8_t + { + SUCCESS = lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS, + FAILURE = lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_FAILURE, + ERROR = lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_ERROR + }; + /// Callback function for configure transition /* - * \return true by default + * \return SUCCESS by default */ RCLCPP_LIFECYCLE_PUBLIC - virtual rcl_lifecycle_transition_key_t + virtual CallbackReturn on_configure(const State & previous_state); /// Callback function for cleanup transition /* - * \return true by default + * \return SUCCESS by default */ RCLCPP_LIFECYCLE_PUBLIC - virtual rcl_lifecycle_transition_key_t + virtual CallbackReturn on_cleanup(const State & previous_state); /// Callback function for shutdown transition /* - * \return true by default + * \return SUCCESS by default */ RCLCPP_LIFECYCLE_PUBLIC - virtual rcl_lifecycle_transition_key_t + virtual CallbackReturn on_shutdown(const State & previous_state); /// Callback function for activate transition /* - * \return true by default + * \return SUCCESS by default */ RCLCPP_LIFECYCLE_PUBLIC - virtual rcl_lifecycle_transition_key_t + virtual CallbackReturn on_activate(const State & previous_state); /// Callback function for deactivate transition /* - * \return true by default + * \return SUCCESS by default */ RCLCPP_LIFECYCLE_PUBLIC - virtual rcl_lifecycle_transition_key_t + virtual CallbackReturn on_deactivate(const State & previous_state); /// Callback function for errorneous transition /* - * \return false by default + * \return SUCCESS by default */ RCLCPP_LIFECYCLE_PUBLIC - virtual rcl_lifecycle_transition_key_t + virtual CallbackReturn on_error(const State & previous_state); + + RCLCPP_LIFECYCLE_PUBLIC + virtual + ~LifecycleNodeInterface() {} }; } // namespace node_interfaces } // namespace rclcpp_lifecycle + +#if defined(_WIN32) +#pragma pop_macro("ERROR") +#endif + +RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT( + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface, lifecycle_node) + #endif // RCLCPP_LIFECYCLE__NODE_INTERFACES__LIFECYCLE_NODE_INTERFACE_HPP_ diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp index eecbf43fc4..62e1c51fb3 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp @@ -15,30 +15,45 @@ #ifndef RCLCPP_LIFECYCLE__STATE_HPP_ #define RCLCPP_LIFECYCLE__STATE_HPP_ +#include #include +#include "rcl_lifecycle/data_types.h" + #include "rclcpp_lifecycle/visibility_control.h" #include "rcutils/allocator.h" -// forward declare rcl_state_t -typedef struct rcl_lifecycle_state_t rcl_lifecycle_state_t; - namespace rclcpp_lifecycle { +/// Abstract class for the Lifecycle's states. +/** + * There are 4 primary states: Unconfigured, Inactive, Active and Finalized. + */ class State { public: RCLCPP_LIFECYCLE_PUBLIC explicit State(rcutils_allocator_t allocator = rcutils_get_default_allocator()); + /// State constructor. + /** + * \param[in] id of the state + * \param[in] label of the state + * \param[in] allocator a valid allocator used to initialized the state. + */ RCLCPP_LIFECYCLE_PUBLIC State( uint8_t id, const std::string & label, rcutils_allocator_t allocator = rcutils_get_default_allocator()); + /// State constructor. + /** + * \param[in] rcl_lifecycle_state_handle structure with the state details + * \param[in] allocator a valid allocator used to initialized the state. + */ RCLCPP_LIFECYCLE_PUBLIC explicit State( const rcl_lifecycle_state_t * rcl_lifecycle_state_handle, @@ -53,10 +68,18 @@ class State RCLCPP_LIFECYCLE_PUBLIC State & operator=(const State & rhs); + /// Return the id. + /** + * \return id of the state + */ RCLCPP_LIFECYCLE_PUBLIC uint8_t id() const; + /// Return the label. + /** + * \return label of state + */ RCLCPP_LIFECYCLE_PUBLIC std::string label() const; @@ -64,12 +87,13 @@ class State protected: RCLCPP_LIFECYCLE_PUBLIC void - reset(); + reset() noexcept; rcutils_allocator_t allocator_; bool owns_rcl_state_handle_; + mutable std::recursive_mutex state_handle_mutex_; rcl_lifecycle_state_t * state_handle_; }; diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp index a566a22264..4093579e04 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp @@ -17,35 +17,58 @@ #include +#include "rcl_lifecycle/data_types.h" + #include "rclcpp_lifecycle/state.hpp" #include "rclcpp_lifecycle/visibility_control.h" #include "rcutils/allocator.h" -// forward declare rcl_transition_t -typedef struct rcl_lifecycle_transition_t rcl_lifecycle_transition_t; - namespace rclcpp_lifecycle { +/// The Transition class abstract the Lifecycle's states. +/** + * There are 7 transitions exposed to a supervisory process, they are: create, configure, + * cleanup, activate, deactivate, shutdown and destroy. + */ class Transition { public: RCLCPP_LIFECYCLE_PUBLIC Transition() = delete; + /// Transition constructor. + /** + * \param[in] id of the transition + * \param[in] label of the transition + * \param[in] allocator a valid allocator used to initialized the state. + */ RCLCPP_LIFECYCLE_PUBLIC explicit Transition( uint8_t id, const std::string & label = "", rcutils_allocator_t allocator = rcutils_get_default_allocator()); + /// Transition constructor. + /** + * \param[in] id of the transition + * \param[in] label of the transition + * \param[in] start state of the transition + * \param[in] goal state of the transition + * \param[in] allocator a valid allocator used to initialized the state. + */ RCLCPP_LIFECYCLE_PUBLIC Transition( uint8_t id, const std::string & label, State && start, State && goal, rcutils_allocator_t allocator = rcutils_get_default_allocator()); + /// Transition constructor. + /** + * \param[in] rcl_lifecycle_transition_handle structure with the transition details + * \param[in] allocator a valid allocator used to initialized the state. + */ RCLCPP_LIFECYCLE_PUBLIC explicit Transition( const rcl_lifecycle_transition_t * rcl_lifecycle_transition_handle, @@ -60,18 +83,34 @@ class Transition RCLCPP_LIFECYCLE_PUBLIC Transition & operator=(const Transition & rhs); + /// Return the id. + /** + * \return id of the state + */ RCLCPP_LIFECYCLE_PUBLIC uint8_t id() const; + /// Return the label. + /** + * \return label of the transition + */ RCLCPP_LIFECYCLE_PUBLIC std::string label() const; + /// Return the start state of the transition. + /** + * \return start state of the transition. + */ RCLCPP_LIFECYCLE_PUBLIC State start_state() const; + /// Return the goal state of the transition. + /** + * \return goal state of the transition. + */ RCLCPP_LIFECYCLE_PUBLIC State goal_state() const; @@ -79,7 +118,7 @@ class Transition protected: RCLCPP_LIFECYCLE_PUBLIC void - reset(); + reset() noexcept; rcutils_allocator_t allocator_; diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 028f49c547..065992fdc8 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,32 +2,45 @@ rclcpp_lifecycle - 0.4.0 + 29.2.0 Package containing a prototype for lifecycle implementation - Karsten Knese + + Ivan Paunovic + Michel Hidalgo + William Woodall + Apache License 2.0 - ament_cmake - rosidl_default_generators + Jacob Perron + Karsten Knese + ament_cmake_ros + + lifecycle_msgs + rcl rclcpp + rcl_interfaces rcl_lifecycle - rmw_implementation - rosidl_default_generators - std_msgs - lifecycle_msgs + rcutils + rmw + rosidl_typesupport_cpp + lifecycle_msgs + rcl rclcpp + rcl_interfaces rcl_lifecycle - rclpy - rmw_implementation - rosidl_default_runtime - std_msgs - lifecycle_msgs + rcutils + rmw + rosidl_typesupport_cpp ament_cmake_gtest ament_lint_auto ament_lint_common + mimick_vendor + performance_test_fixture + rcpputils + test_msgs ament_cmake diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index c03175ff93..03ece2e58b 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -14,15 +14,22 @@ #include "rclcpp_lifecycle/lifecycle_node.hpp" -#include +#include +#include #include #include -#include +#include +#include #include +#include #include "lifecycle_msgs/msg/state.hpp" #include "lifecycle_msgs/msg/transition.hpp" +#include "rcl_interfaces/msg/list_parameters_result.hpp" +#include "rcl_interfaces/msg/parameter_descriptor.hpp" +#include "rcl_interfaces/msg/set_parameters_result.hpp" + #include "rclcpp/exceptions.hpp" #include "rclcpp/graph_listener.hpp" #include "rclcpp/logger.hpp" @@ -33,8 +40,13 @@ #include "rclcpp/node_interfaces/node_logging.hpp" #include "rclcpp/node_interfaces/node_parameters.hpp" #include "rclcpp/node_interfaces/node_services.hpp" +#include "rclcpp/node_interfaces/node_time_source.hpp" #include "rclcpp/node_interfaces/node_timers.hpp" #include "rclcpp/node_interfaces/node_topics.hpp" +#include "rclcpp/node_interfaces/node_type_descriptions.hpp" +#include "rclcpp/node_interfaces/node_waitables.hpp" +#include "rclcpp/parameter_service.hpp" +#include "rclcpp/qos.hpp" #include "lifecycle_node_interface_impl.hpp" // implementation @@ -43,55 +55,126 @@ namespace rclcpp_lifecycle LifecycleNode::LifecycleNode( const std::string & node_name, - const std::string & namespace_, - bool use_intra_process_comms) + const rclcpp::NodeOptions & options, + bool enable_communication_interface) : LifecycleNode( node_name, - namespace_, - rclcpp::contexts::default_context::get_global_default_context(), - use_intra_process_comms) + "", + options, + enable_communication_interface) {} LifecycleNode::LifecycleNode( const std::string & node_name, const std::string & namespace_, - rclcpp::Context::SharedPtr context, - bool use_intra_process_comms) -: node_base_(new rclcpp::node_interfaces::NodeBase(node_name, namespace_, context)), + const rclcpp::NodeOptions & options, + bool enable_communication_interface) +: node_base_(new rclcpp::node_interfaces::NodeBase( + node_name, + namespace_, + options.context(), + *(options.get_rcl_node_options()), + options.use_intra_process_comms(), + options.enable_topic_statistics())), node_graph_(new rclcpp::node_interfaces::NodeGraph(node_base_.get())), - node_logging_(new rclcpp::node_interfaces::NodeLogging(node_base_.get())), + node_logging_(new rclcpp::node_interfaces::NodeLogging(node_base_)), node_timers_(new rclcpp::node_interfaces::NodeTimers(node_base_.get())), - node_topics_(new rclcpp::node_interfaces::NodeTopics(node_base_.get())), + node_topics_(new rclcpp::node_interfaces::NodeTopics(node_base_.get(), node_timers_.get())), node_services_(new rclcpp::node_interfaces::NodeServices(node_base_.get())), + node_clock_(new rclcpp::node_interfaces::NodeClock( + node_base_, + node_topics_, + node_graph_, + node_services_, + node_logging_, + options.clock_type() + )), node_parameters_(new rclcpp::node_interfaces::NodeParameters( - node_topics_.get(), - use_intra_process_comms + node_base_, + node_logging_, + node_topics_, + node_services_, + node_clock_, + options.parameter_overrides(), + options.start_parameter_services(), + options.start_parameter_event_publisher(), + options.parameter_event_qos(), + options.parameter_event_publisher_options(), + options.allow_undeclared_parameters(), + options.automatically_declare_parameters_from_overrides() )), - node_clock_(new rclcpp::node_interfaces::NodeClock( + node_time_source_(new rclcpp::node_interfaces::NodeTimeSource( node_base_, node_topics_, node_graph_, + node_services_, + node_logging_, + node_clock_, + node_parameters_, + options.clock_qos(), + options.use_clock_thread() + )), + node_type_descriptions_(new rclcpp::node_interfaces::NodeTypeDescriptions( + node_base_, + node_logging_, + node_parameters_, node_services_ )), - use_intra_process_comms_(use_intra_process_comms), - impl_(new LifecycleNodeInterfaceImpl(node_base_, node_services_)) + node_waitables_(new rclcpp::node_interfaces::NodeWaitables(node_base_.get())), + node_options_(options), + impl_(new LifecycleNodeInterfaceImpl(node_base_, node_services_, node_logging_)) { - impl_->init(); + impl_->init(enable_communication_interface); - register_on_configure(std::bind(&LifecycleNodeInterface::on_configure, this, - std::placeholders::_1)); + register_on_configure( + std::bind( + &LifecycleNodeInterface::on_configure, this, + std::placeholders::_1)); register_on_cleanup(std::bind(&LifecycleNodeInterface::on_cleanup, this, std::placeholders::_1)); - register_on_shutdown(std::bind(&LifecycleNodeInterface::on_shutdown, this, - std::placeholders::_1)); - register_on_activate(std::bind(&LifecycleNodeInterface::on_activate, this, - std::placeholders::_1)); - register_on_deactivate(std::bind(&LifecycleNodeInterface::on_deactivate, this, - std::placeholders::_1)); + register_on_shutdown( + std::bind( + &LifecycleNodeInterface::on_shutdown, this, + std::placeholders::_1)); + register_on_activate( + std::bind( + &LifecycleNodeInterface::on_activate, this, + std::placeholders::_1)); + register_on_deactivate( + std::bind( + &LifecycleNodeInterface::on_deactivate, this, + std::placeholders::_1)); register_on_error(std::bind(&LifecycleNodeInterface::on_error, this, std::placeholders::_1)); + + if (options.enable_logger_service()) { + node_logging_->create_logger_services(node_services_); + } } LifecycleNode::~LifecycleNode() -{} +{ + auto current_state = LifecycleNode::get_current_state().id(); + if (current_state != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) { + // This might be leaving sensors and devices without shutting down unintentionally. + // It is user's responsibility to call shutdown to avoid leaving them unknow states. + RCLCPP_WARN( + rclcpp::get_logger("rclcpp_lifecycle"), + "LifecycleNode is not shut down: Node still in state(%u) in destructor", + current_state); + } + + // release sub-interfaces in an order that allows them to consult with node_base during tear-down + node_waitables_.reset(); + node_type_descriptions_.reset(); + node_time_source_.reset(); + node_parameters_.reset(); + node_clock_.reset(); + node_services_.reset(); + node_topics_.reset(); + node_timers_.reset(); + node_logging_.reset(); + node_graph_.reset(); + node_base_.reset(); +} const char * LifecycleNode::get_name() const @@ -105,59 +188,117 @@ LifecycleNode::get_namespace() const return node_base_->get_namespace(); } +const char * +LifecycleNode::get_fully_qualified_name() const +{ + return node_base_->get_fully_qualified_name(); +} + rclcpp::Logger LifecycleNode::get_logger() const { return node_logging_->get_logger(); } -rclcpp::callback_group::CallbackGroup::SharedPtr +rclcpp::CallbackGroup::SharedPtr LifecycleNode::create_callback_group( - rclcpp::callback_group::CallbackGroupType group_type) + rclcpp::CallbackGroupType group_type, + bool automatically_add_to_executor_with_node) +{ + return node_base_->create_callback_group(group_type, automatically_add_to_executor_with_node); +} + +const rclcpp::ParameterValue & +LifecycleNode::declare_parameter( + const std::string & name, + const rclcpp::ParameterValue & default_value, + const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor, + bool ignore_override) +{ + return this->node_parameters_->declare_parameter( + name, default_value, parameter_descriptor, ignore_override); +} + +const rclcpp::ParameterValue & +LifecycleNode::declare_parameter( + const std::string & name, + rclcpp::ParameterType type, + const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor, + bool ignore_override) +{ + return this->node_parameters_->declare_parameter( + name, + type, + parameter_descriptor, + ignore_override); +} + +void +LifecycleNode::undeclare_parameter(const std::string & name) { - return node_base_->create_callback_group(group_type); + this->node_parameters_->undeclare_parameter(name); } bool -LifecycleNode::group_in_node(rclcpp::callback_group::CallbackGroup::SharedPtr group) +LifecycleNode::has_parameter(const std::string & name) const { - return node_base_->callback_group_in_node(group); + return this->node_parameters_->has_parameter(name); +} + +rcl_interfaces::msg::SetParametersResult +LifecycleNode::set_parameter(const rclcpp::Parameter & parameter) +{ + return this->set_parameters_atomically({parameter}); } std::vector LifecycleNode::set_parameters( - const std::vector & parameters) + const std::vector & parameters) { return node_parameters_->set_parameters(parameters); } rcl_interfaces::msg::SetParametersResult LifecycleNode::set_parameters_atomically( - const std::vector & parameters) + const std::vector & parameters) { return node_parameters_->set_parameters_atomically(parameters); } -std::vector +std::vector LifecycleNode::get_parameters( const std::vector & names) const { return node_parameters_->get_parameters(names); } -rclcpp::parameter::ParameterVariant +rclcpp::Parameter LifecycleNode::get_parameter(const std::string & name) const { return node_parameters_->get_parameter(name); } -bool LifecycleNode::get_parameter( +bool +LifecycleNode::get_parameter( const std::string & name, - rclcpp::parameter::ParameterVariant & parameter) const + rclcpp::Parameter & parameter) const { return node_parameters_->get_parameter(name, parameter); } +rcl_interfaces::msg::ParameterDescriptor +LifecycleNode::describe_parameter(const std::string & name) const +{ + auto result = node_parameters_->describe_parameters({name}); + if (0 == result.size()) { + throw rclcpp::exceptions::ParameterNotDeclaredException(name); + } + if (result.size() > 1) { + throw std::runtime_error("number of described parameters unexpectedly more than one"); + } + return result.front(); +} + std::vector LifecycleNode::describe_parameters( const std::vector & names) const @@ -179,6 +320,51 @@ LifecycleNode::list_parameters( return node_parameters_->list_parameters(prefixes, depth); } +rclcpp::Node::PreSetParametersCallbackHandle::SharedPtr +LifecycleNode::add_pre_set_parameters_callback(PreSetParametersCallbackType callback) +{ + return node_parameters_->add_pre_set_parameters_callback(callback); +} + +rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr +LifecycleNode::add_on_set_parameters_callback(OnSetParametersCallbackType callback) +{ + return node_parameters_->add_on_set_parameters_callback(callback); +} + +rclcpp::Node::PostSetParametersCallbackHandle::SharedPtr +LifecycleNode::add_post_set_parameters_callback(PostSetParametersCallbackType callback) +{ + return node_parameters_->add_post_set_parameters_callback(callback); +} + +void +LifecycleNode::remove_pre_set_parameters_callback( + const PreSetParametersCallbackHandle * const callback) +{ + node_parameters_->remove_pre_set_parameters_callback(callback); +} + +void +LifecycleNode::remove_on_set_parameters_callback( + const OnSetParametersCallbackHandle * const callback) +{ + node_parameters_->remove_on_set_parameters_callback(callback); +} + +void +LifecycleNode::remove_post_set_parameters_callback( + const PostSetParametersCallbackHandle * const callback) +{ + node_parameters_->remove_post_set_parameters_callback(callback); +} + +std::vector +LifecycleNode::get_node_names() const +{ + return node_graph_->get_node_names(); +} + std::map> LifecycleNode::get_topic_names_and_types(bool no_demangle) const { @@ -191,6 +377,15 @@ LifecycleNode::get_service_names_and_types() const return node_graph_->get_service_names_and_types(); } +std::map> +LifecycleNode::get_service_names_and_types_by_node( + const std::string & node_name, + const std::string & namespace_) const +{ + return node_graph_->get_service_names_and_types_by_node( + node_name, namespace_); +} + size_t LifecycleNode::count_publishers(const std::string & topic_name) const { @@ -203,10 +398,35 @@ LifecycleNode::count_subscribers(const std::string & topic_name) const return node_graph_->count_subscribers(topic_name); } -const std::vector & -LifecycleNode::get_callback_groups() const +size_t +LifecycleNode::count_clients(const std::string & service_name) const +{ + return node_graph_->count_clients(service_name); +} + +size_t +LifecycleNode::count_services(const std::string & service_name) const +{ + return node_graph_->count_services(service_name); +} + +std::vector +LifecycleNode::get_publishers_info_by_topic(const std::string & topic_name, bool no_mangle) const +{ + return node_graph_->get_publishers_info_by_topic(topic_name, no_mangle); +} + +std::vector +LifecycleNode::get_subscriptions_info_by_topic(const std::string & topic_name, bool no_mangle) const +{ + return node_graph_->get_subscriptions_info_by_topic(topic_name, no_mangle); +} + +void +LifecycleNode::for_each_callback_group( + const rclcpp::node_interfaces::NodeBaseInterface::CallbackGroupFunction & func) { - return node_base_->get_callback_groups(); + node_base_->for_each_callback_group(func); } rclcpp::Event::SharedPtr @@ -229,8 +449,14 @@ LifecycleNode::get_clock() return node_clock_->get_clock(); } +rclcpp::Clock::ConstSharedPtr +LifecycleNode::get_clock() const +{ + return node_clock_->get_clock(); +} + rclcpp::Time -LifecycleNode::now() +LifecycleNode::now() const { return node_clock_->get_clock()->now(); } @@ -253,6 +479,18 @@ LifecycleNode::get_node_graph_interface() return node_graph_; } +rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr +LifecycleNode::get_node_logging_interface() +{ + return node_logging_; +} + +rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr +LifecycleNode::get_node_time_source_interface() +{ + return node_time_source_; +} + rclcpp::node_interfaces::NodeTimersInterface::SharedPtr LifecycleNode::get_node_timers_interface() { @@ -265,6 +503,12 @@ LifecycleNode::get_node_topics_interface() return node_topics_; } +rclcpp::node_interfaces::NodeTypeDescriptionsInterface::SharedPtr +LifecycleNode::get_node_type_descriptions_interface() +{ + return node_type_descriptions_; +} + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr LifecycleNode::get_node_services_interface() { @@ -277,11 +521,22 @@ LifecycleNode::get_node_parameters_interface() return node_parameters_; } +rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr +LifecycleNode::get_node_waitables_interface() +{ + return node_waitables_; +} + +const rclcpp::NodeOptions & +LifecycleNode::get_node_options() const +{ + return node_options_; +} //// bool LifecycleNode::register_on_configure( - std::function fcn) + std::function fcn) { return impl_->register_callback( lifecycle_msgs::msg::State::TRANSITION_STATE_CONFIGURING, fcn); @@ -289,7 +544,7 @@ LifecycleNode::register_on_configure( bool LifecycleNode::register_on_cleanup( - std::function fcn) + std::function fcn) { return impl_->register_callback( lifecycle_msgs::msg::State::TRANSITION_STATE_CLEANINGUP, fcn); @@ -297,7 +552,7 @@ LifecycleNode::register_on_cleanup( bool LifecycleNode::register_on_shutdown( - std::function fcn) + std::function fcn) { return impl_->register_callback( lifecycle_msgs::msg::State::TRANSITION_STATE_SHUTTINGDOWN, fcn); @@ -305,7 +560,7 @@ LifecycleNode::register_on_shutdown( bool LifecycleNode::register_on_activate( - std::function fcn) + std::function fcn) { return impl_->register_callback( lifecycle_msgs::msg::State::TRANSITION_STATE_ACTIVATING, fcn); @@ -313,7 +568,7 @@ LifecycleNode::register_on_activate( bool LifecycleNode::register_on_deactivate( - std::function fcn) + std::function fcn) { return impl_->register_callback( lifecycle_msgs::msg::State::TRANSITION_STATE_DEACTIVATING, fcn); @@ -321,30 +576,36 @@ LifecycleNode::register_on_deactivate( bool LifecycleNode::register_on_error( - std::function fcn) + std::function fcn) { return impl_->register_callback( lifecycle_msgs::msg::State::TRANSITION_STATE_ERRORPROCESSING, fcn); } const State & -LifecycleNode::get_current_state() +LifecycleNode::get_current_state() const { return impl_->get_current_state(); } std::vector -LifecycleNode::get_available_states() +LifecycleNode::get_available_states() const { return impl_->get_available_states(); } std::vector -LifecycleNode::get_available_transitions() +LifecycleNode::get_available_transitions() const { return impl_->get_available_transitions(); } +std::vector +LifecycleNode::get_transition_graph() const +{ + return impl_->get_transition_graph(); +} + const State & LifecycleNode::trigger_transition(const Transition & transition) { @@ -353,7 +614,7 @@ LifecycleNode::trigger_transition(const Transition & transition) const State & LifecycleNode::trigger_transition( - const Transition & transition, rcl_lifecycle_transition_key_t & cb_return_code) + const Transition & transition, LifecycleNodeInterface::CallbackReturn & cb_return_code) { return trigger_transition(transition.id(), cb_return_code); } @@ -366,7 +627,7 @@ LifecycleNode::trigger_transition(uint8_t transition_id) const State & LifecycleNode::trigger_transition( - uint8_t transition_id, rcl_lifecycle_transition_key_t & cb_return_code) + uint8_t transition_id, LifecycleNodeInterface::CallbackReturn & cb_return_code) { return impl_->trigger_transition(transition_id, cb_return_code); } @@ -379,7 +640,7 @@ LifecycleNode::configure() } const State & -LifecycleNode::configure(rcl_lifecycle_transition_key_t & cb_return_code) +LifecycleNode::configure(LifecycleNodeInterface::CallbackReturn & cb_return_code) { return impl_->trigger_transition( lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE, cb_return_code); @@ -393,7 +654,7 @@ LifecycleNode::cleanup() } const State & -LifecycleNode::cleanup(rcl_lifecycle_transition_key_t & cb_return_code) +LifecycleNode::cleanup(LifecycleNodeInterface::CallbackReturn & cb_return_code) { return impl_->trigger_transition( lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP, cb_return_code); @@ -407,7 +668,7 @@ LifecycleNode::activate() } const State & -LifecycleNode::activate(rcl_lifecycle_transition_key_t & cb_return_code) +LifecycleNode::activate(LifecycleNodeInterface::CallbackReturn & cb_return_code) { return impl_->trigger_transition( lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE, cb_return_code); @@ -421,7 +682,7 @@ LifecycleNode::deactivate() } const State & -LifecycleNode::deactivate(rcl_lifecycle_transition_key_t & cb_return_code) +LifecycleNode::deactivate(LifecycleNodeInterface::CallbackReturn & cb_return_code) { return impl_->trigger_transition( lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE, cb_return_code); @@ -431,21 +692,35 @@ const State & LifecycleNode::shutdown() { return impl_->trigger_transition( - lifecycle_msgs::msg::Transition::TRANSITION_SHUTDOWN); + rcl_lifecycle_shutdown_label); } const State & -LifecycleNode::shutdown(rcl_lifecycle_transition_key_t & cb_return_code) +LifecycleNode::shutdown(LifecycleNodeInterface::CallbackReturn & cb_return_code) { return impl_->trigger_transition( - lifecycle_msgs::msg::Transition::TRANSITION_SHUTDOWN, cb_return_code); + rcl_lifecycle_shutdown_label, cb_return_code); +} + +node_interfaces::LifecycleNodeInterface::CallbackReturn +LifecycleNode::on_activate(const State &) +{ + impl_->on_activate(); + return LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +node_interfaces::LifecycleNodeInterface::CallbackReturn +LifecycleNode::on_deactivate(const State &) +{ + impl_->on_deactivate(); + return LifecycleNodeInterface::CallbackReturn::SUCCESS; } void -LifecycleNode::add_publisher_handle( - std::shared_ptr pub) +LifecycleNode::add_managed_entity( + std::weak_ptr managed_entity) { - impl_->add_publisher_handle(pub); + impl_->add_managed_entity(managed_entity); } void diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp new file mode 100644 index 0000000000..9074c9cc6e --- /dev/null +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp @@ -0,0 +1,598 @@ +// 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 +#include +#include +#include +#include + +#include "lifecycle_msgs/msg/transition_description.hpp" +#include "lifecycle_msgs/msg/transition_event.h" // for getting the c-typesupport +#include "lifecycle_msgs/msg/transition_event.hpp" +#include "lifecycle_msgs/srv/change_state.hpp" +#include "lifecycle_msgs/srv/get_state.hpp" +#include "lifecycle_msgs/srv/get_available_states.hpp" +#include "lifecycle_msgs/srv/get_available_transitions.hpp" + +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_logging_interface.hpp" +#include "rclcpp/node_interfaces/node_services_interface.hpp" + +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +#include "rcl/error_handling.h" +#include "rcl/node.h" + +#include "rcl_lifecycle/rcl_lifecycle.h" +#include "rcl_lifecycle/transition_map.h" + +#include "rcutils/logging_macros.h" + +#include "rmw/types.h" + +#include "lifecycle_node_interface_impl.hpp" + +namespace rclcpp_lifecycle +{ + +LifecycleNode::LifecycleNodeInterfaceImpl::LifecycleNodeInterfaceImpl( + std::shared_ptr node_base_interface, + std::shared_ptr node_services_interface, + std::shared_ptr node_logging_interface) +: node_base_interface_(node_base_interface), + node_services_interface_(node_services_interface), + node_logging_interface_(node_logging_interface) +{ +} + +LifecycleNode::LifecycleNodeInterfaceImpl::~LifecycleNodeInterfaceImpl() +{ + rcl_node_t * node_handle = node_base_interface_->get_rcl_node_handle(); + rcl_ret_t ret; + { + std::lock_guard lock(state_machine_mutex_); + ret = rcl_lifecycle_state_machine_fini(&state_machine_, node_handle); + } + if (ret != RCL_RET_OK) { + RCLCPP_FATAL( + node_logging_interface_->get_logger(), + "failed to destroy rcl_state_machine"); + } +} + +void +LifecycleNode::LifecycleNodeInterfaceImpl::init(bool enable_communication_interface) +{ + rcl_node_t * node_handle = node_base_interface_->get_rcl_node_handle(); + const rcl_node_options_t * node_options = + rcl_node_get_options(node_base_interface_->get_rcl_node_handle()); + auto state_machine_options = rcl_lifecycle_get_default_state_machine_options(); + state_machine_options.enable_com_interface = enable_communication_interface; + state_machine_options.allocator = node_options->allocator; + + // The call to initialize the state machine takes + // currently five different typesupports for all publishers/services + // created within the RCL_LIFECYCLE structure. + // The publisher takes a C-Typesupport since the publishing (i.e. creating + // the message) is done fully in RCL. + // Services are handled in C++, so that it needs a C++ typesupport structure. + std::lock_guard lock(state_machine_mutex_); + state_machine_ = rcl_lifecycle_get_zero_initialized_state_machine(); + rcl_ret_t ret = rcl_lifecycle_state_machine_init( + &state_machine_, + node_handle, + ROSIDL_GET_MSG_TYPE_SUPPORT(lifecycle_msgs, msg, TransitionEvent), + rosidl_typesupport_cpp::get_service_type_support_handle(), + rosidl_typesupport_cpp::get_service_type_support_handle(), + rosidl_typesupport_cpp::get_service_type_support_handle(), + rosidl_typesupport_cpp::get_service_type_support_handle(), + rosidl_typesupport_cpp::get_service_type_support_handle(), + &state_machine_options); + if (ret != RCL_RET_OK) { + throw std::runtime_error( + std::string("Couldn't initialize state machine for node ") + + node_base_interface_->get_name()); + } + + current_state_ = State(state_machine_.current_state); + + if (enable_communication_interface) { + { // change_state + auto cb = std::bind( + &LifecycleNode::LifecycleNodeInterfaceImpl::on_change_state, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); + rclcpp::AnyServiceCallback any_cb; + any_cb.set(std::move(cb)); + + srv_change_state_ = std::make_shared>( + node_base_interface_->get_shared_rcl_node_handle(), + &state_machine_.com_interface.srv_change_state, + any_cb); + node_services_interface_->add_service( + std::dynamic_pointer_cast(srv_change_state_), + nullptr); + } + + { // get_state + auto cb = std::bind( + &LifecycleNode::LifecycleNodeInterfaceImpl::on_get_state, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); + rclcpp::AnyServiceCallback any_cb; + any_cb.set(std::move(cb)); + + srv_get_state_ = std::make_shared>( + node_base_interface_->get_shared_rcl_node_handle(), + &state_machine_.com_interface.srv_get_state, + any_cb); + node_services_interface_->add_service( + std::dynamic_pointer_cast(srv_get_state_), + nullptr); + } + + { // get_available_states + auto cb = std::bind( + &LifecycleNode::LifecycleNodeInterfaceImpl::on_get_available_states, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); + rclcpp::AnyServiceCallback any_cb; + any_cb.set(std::move(cb)); + + srv_get_available_states_ = std::make_shared>( + node_base_interface_->get_shared_rcl_node_handle(), + &state_machine_.com_interface.srv_get_available_states, + any_cb); + node_services_interface_->add_service( + std::dynamic_pointer_cast(srv_get_available_states_), + nullptr); + } + + { // get_available_transitions + auto cb = std::bind( + &LifecycleNode::LifecycleNodeInterfaceImpl::on_get_available_transitions, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); + rclcpp::AnyServiceCallback any_cb; + any_cb.set(std::move(cb)); + + srv_get_available_transitions_ = + std::make_shared>( + node_base_interface_->get_shared_rcl_node_handle(), + &state_machine_.com_interface.srv_get_available_transitions, + any_cb); + node_services_interface_->add_service( + std::dynamic_pointer_cast(srv_get_available_transitions_), + nullptr); + } + + { // get_transition_graph + auto cb = std::bind( + &LifecycleNode::LifecycleNodeInterfaceImpl::on_get_transition_graph, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); + rclcpp::AnyServiceCallback any_cb; + any_cb.set(std::move(cb)); + + srv_get_transition_graph_ = + std::make_shared>( + node_base_interface_->get_shared_rcl_node_handle(), + &state_machine_.com_interface.srv_get_transition_graph, + any_cb); + node_services_interface_->add_service( + std::dynamic_pointer_cast(srv_get_transition_graph_), + nullptr); + } + } +} + +bool +LifecycleNode::LifecycleNodeInterfaceImpl::register_callback( + std::uint8_t lifecycle_transition, + std::function & cb) +{ + cb_map_[lifecycle_transition] = cb; + return true; +} + +void +LifecycleNode::LifecycleNodeInterfaceImpl::on_change_state( + const std::shared_ptr header, + const std::shared_ptr req, + std::shared_ptr resp) +{ + (void)header; + std::uint8_t transition_id; + { + std::lock_guard lock(state_machine_mutex_); + if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { + throw std::runtime_error("Can't get state. State machine is not initialized."); + } + + transition_id = req->transition.id; + // if there's a label attached to the request, + // we check the transition attached to this label. + // we further can't compare the id of the looked up transition + // because ros2 service call defaults all intergers to zero. + // that means if we call ros2 service call ... {transition: {label: shutdown}} + // the id of the request is 0 (zero) whereas the id from the lookup up transition + // can be different. + // the result of this is that the label takes presedence of the id. + if (req->transition.label.size() != 0) { + auto rcl_transition = rcl_lifecycle_get_transition_by_label( + state_machine_.current_state, req->transition.label.c_str()); + if (rcl_transition == nullptr) { + resp->success = false; + return; + } + transition_id = static_cast(rcl_transition->id); + } + } + + node_interfaces::LifecycleNodeInterface::CallbackReturn cb_return_code; + auto ret = change_state(transition_id, cb_return_code); + (void) ret; + // TODO(karsten1987): Lifecycle msgs have to be extended to keep both returns + // 1. return is the actual transition + // 2. return is whether an error occurred or not + resp->success = + (cb_return_code == node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); +} + +void +LifecycleNode::LifecycleNodeInterfaceImpl::on_get_state( + const std::shared_ptr header, + const std::shared_ptr req, + std::shared_ptr resp) const +{ + (void)header; + (void)req; + std::lock_guard lock(state_machine_mutex_); + if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { + throw std::runtime_error( + "Can't get state. State machine is not initialized."); + } + resp->current_state.id = static_cast(state_machine_.current_state->id); + resp->current_state.label = state_machine_.current_state->label; +} + +void +LifecycleNode::LifecycleNodeInterfaceImpl::on_get_available_states( + const std::shared_ptr header, + const std::shared_ptr req, + std::shared_ptr resp) const +{ + (void)header; + (void)req; + std::lock_guard lock(state_machine_mutex_); + if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { + throw std::runtime_error( + "Can't get available states. State machine is not initialized."); + } + + resp->available_states.resize(state_machine_.transition_map.states_size); + for (unsigned int i = 0; i < state_machine_.transition_map.states_size; ++i) { + resp->available_states[i].id = + static_cast(state_machine_.transition_map.states[i].id); + resp->available_states[i].label = + static_cast(state_machine_.transition_map.states[i].label); + } +} + +void +LifecycleNode::LifecycleNodeInterfaceImpl::on_get_available_transitions( + const std::shared_ptr header, + const std::shared_ptr req, + std::shared_ptr resp) const +{ + (void)header; + (void)req; + std::lock_guard lock(state_machine_mutex_); + if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { + throw std::runtime_error( + "Can't get available transitions. State machine is not initialized."); + } + + resp->available_transitions.resize(state_machine_.current_state->valid_transition_size); + for (unsigned int i = 0; i < state_machine_.current_state->valid_transition_size; ++i) { + lifecycle_msgs::msg::TransitionDescription & trans_desc = resp->available_transitions[i]; + + auto rcl_transition = state_machine_.current_state->valid_transitions[i]; + trans_desc.transition.id = static_cast(rcl_transition.id); + trans_desc.transition.label = rcl_transition.label; + trans_desc.start_state.id = static_cast(rcl_transition.start->id); + trans_desc.start_state.label = rcl_transition.start->label; + trans_desc.goal_state.id = static_cast(rcl_transition.goal->id); + trans_desc.goal_state.label = rcl_transition.goal->label; + } +} + +void +LifecycleNode::LifecycleNodeInterfaceImpl::on_get_transition_graph( + const std::shared_ptr header, + const std::shared_ptr req, + std::shared_ptr resp) const +{ + (void)header; + (void)req; + std::lock_guard lock(state_machine_mutex_); + if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { + throw std::runtime_error( + "Can't get available transitions. State machine is not initialized."); + } + + resp->available_transitions.resize(state_machine_.transition_map.transitions_size); + for (unsigned int i = 0; i < state_machine_.transition_map.transitions_size; ++i) { + lifecycle_msgs::msg::TransitionDescription & trans_desc = resp->available_transitions[i]; + + auto rcl_transition = state_machine_.transition_map.transitions[i]; + trans_desc.transition.id = static_cast(rcl_transition.id); + trans_desc.transition.label = rcl_transition.label; + trans_desc.start_state.id = static_cast(rcl_transition.start->id); + trans_desc.start_state.label = rcl_transition.start->label; + trans_desc.goal_state.id = static_cast(rcl_transition.goal->id); + trans_desc.goal_state.label = rcl_transition.goal->label; + } +} + +const State & +LifecycleNode::LifecycleNodeInterfaceImpl::get_current_state() const +{ + return current_state_; +} + +std::vector +LifecycleNode::LifecycleNodeInterfaceImpl::get_available_states() const +{ + std::vector states; + std::lock_guard lock(state_machine_mutex_); + states.reserve(state_machine_.transition_map.states_size); + + for (unsigned int i = 0; i < state_machine_.transition_map.states_size; ++i) { + states.emplace_back(&state_machine_.transition_map.states[i]); + } + return states; +} + +std::vector +LifecycleNode::LifecycleNodeInterfaceImpl::get_available_transitions() const +{ + std::vector transitions; + std::lock_guard lock(state_machine_mutex_); + transitions.reserve(state_machine_.current_state->valid_transition_size); + + for (unsigned int i = 0; i < state_machine_.current_state->valid_transition_size; ++i) { + transitions.emplace_back(&state_machine_.current_state->valid_transitions[i]); + } + return transitions; +} + +std::vector +LifecycleNode::LifecycleNodeInterfaceImpl::get_transition_graph() const +{ + std::vector transitions; + std::lock_guard lock(state_machine_mutex_); + transitions.reserve(state_machine_.transition_map.transitions_size); + + for (unsigned int i = 0; i < state_machine_.transition_map.transitions_size; ++i) { + transitions.emplace_back(&state_machine_.transition_map.transitions[i]); + } + return transitions; +} + +rcl_ret_t +LifecycleNode::LifecycleNodeInterfaceImpl::change_state( + std::uint8_t transition_id, + node_interfaces::LifecycleNodeInterface::CallbackReturn & cb_return_code) +{ + constexpr bool publish_update = true; + State initial_state; + unsigned int current_state_id; + + { + std::lock_guard lock(state_machine_mutex_); + if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { + RCLCPP_ERROR( + node_logging_interface_->get_logger(), + "Unable to change state for state machine for %s: %s", + node_base_interface_->get_name(), rcl_get_error_string().str); + return RCL_RET_ERROR; + } + + // keep the initial state to pass to a transition callback + initial_state = State(state_machine_.current_state); + + if ( + rcl_lifecycle_trigger_transition_by_id( + &state_machine_, transition_id, publish_update) != RCL_RET_OK) + { + RCLCPP_ERROR( + node_logging_interface_->get_logger(), + "Unable to start transition %u from current state %s: %s", + transition_id, state_machine_.current_state->label, rcl_get_error_string().str); + rcutils_reset_error(); + return RCL_RET_ERROR; + } + current_state_id = state_machine_.current_state->id; + } + + // Update the internal current_state_ + current_state_ = State(state_machine_.current_state); + + auto get_label_for_return_code = + [](node_interfaces::LifecycleNodeInterface::CallbackReturn cb_return_code) -> const char *{ + auto cb_id = static_cast(cb_return_code); + if (cb_id == lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS) { + return rcl_lifecycle_transition_success_label; + } else if (cb_id == lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_FAILURE) { + return rcl_lifecycle_transition_failure_label; + } + return rcl_lifecycle_transition_error_label; + }; + + cb_return_code = execute_callback(current_state_id, initial_state); + auto transition_label = get_label_for_return_code(cb_return_code); + + { + std::lock_guard lock(state_machine_mutex_); + if ( + rcl_lifecycle_trigger_transition_by_label( + &state_machine_, transition_label, publish_update) != RCL_RET_OK) + { + RCLCPP_ERROR( + node_logging_interface_->get_logger(), + "Failed to finish transition %u. Current state is now: %s (%s)", + transition_id, state_machine_.current_state->label, rcl_get_error_string().str); + rcutils_reset_error(); + return RCL_RET_ERROR; + } + current_state_id = state_machine_.current_state->id; + } + + // Update the internal current_state_ + current_state_ = State(state_machine_.current_state); + + // error handling ?! + // TODO(karsten1987): iterate over possible ret value + if (cb_return_code == node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR) { + RCLCPP_WARN( + node_logging_interface_->get_logger(), + "Error occurred while doing error handling."); + + auto error_cb_code = execute_callback(current_state_id, initial_state); + auto error_cb_label = get_label_for_return_code(error_cb_code); + std::lock_guard lock(state_machine_mutex_); + if ( + rcl_lifecycle_trigger_transition_by_label( + &state_machine_, error_cb_label, publish_update) != RCL_RET_OK) + { + RCLCPP_ERROR( + node_logging_interface_->get_logger(), + "Failed to call cleanup on error state: %s", rcl_get_error_string().str); + rcutils_reset_error(); + return RCL_RET_ERROR; + } + } + + // Update the internal current_state_ + current_state_ = State(state_machine_.current_state); + + // This true holds in both cases where the actual callback + // was successful or not, since at this point we have a valid transistion + // to either a new primary state or error state + return RCL_RET_OK; +} + +node_interfaces::LifecycleNodeInterface::CallbackReturn +LifecycleNode::LifecycleNodeInterfaceImpl::execute_callback( + unsigned int cb_id, const State & previous_state) const +{ + // in case no callback was attached, we forward directly + auto cb_success = node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + + auto it = cb_map_.find(static_cast(cb_id)); + if (it != cb_map_.end()) { + auto callback = it->second; + try { + cb_success = callback(State(previous_state)); + } catch (const std::exception & e) { + RCLCPP_ERROR( + node_logging_interface_->get_logger(), + "Caught exception in callback for transition %d", it->first); + RCLCPP_ERROR( + node_logging_interface_->get_logger(), + "Original error: %s", e.what()); + cb_success = node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + } + } + return cb_success; +} + +const State & LifecycleNode::LifecycleNodeInterfaceImpl::trigger_transition( + const char * transition_label) +{ + node_interfaces::LifecycleNodeInterface::CallbackReturn error; + return trigger_transition(transition_label, error); +} + +const State & LifecycleNode::LifecycleNodeInterfaceImpl::trigger_transition( + const char * transition_label, + node_interfaces::LifecycleNodeInterface::CallbackReturn & cb_return_code) +{ + const rcl_lifecycle_transition_t * transition; + { + std::lock_guard lock(state_machine_mutex_); + + transition = + rcl_lifecycle_get_transition_by_label(state_machine_.current_state, transition_label); + } + if (transition) { + change_state(static_cast(transition->id), cb_return_code); + } + return get_current_state(); +} + +const State & +LifecycleNode::LifecycleNodeInterfaceImpl::trigger_transition(uint8_t transition_id) +{ + node_interfaces::LifecycleNodeInterface::CallbackReturn error; + change_state(transition_id, error); + (void) error; + return get_current_state(); +} + +const State & +LifecycleNode::LifecycleNodeInterfaceImpl::trigger_transition( + uint8_t transition_id, + node_interfaces::LifecycleNodeInterface::CallbackReturn & cb_return_code) +{ + change_state(transition_id, cb_return_code); + return get_current_state(); +} + +void +LifecycleNode::LifecycleNodeInterfaceImpl::add_managed_entity( + std::weak_ptr managed_entity) +{ + weak_managed_entities_.push_back(managed_entity); +} + +void +LifecycleNode::LifecycleNodeInterfaceImpl::add_timer_handle( + std::shared_ptr timer) +{ + weak_timers_.push_back(timer); +} + +void +LifecycleNode::LifecycleNodeInterfaceImpl::on_activate() const +{ + for (const auto & weak_entity : weak_managed_entities_) { + auto entity = weak_entity.lock(); + if (entity) { + entity->on_activate(); + } + } +} + +void +LifecycleNode::LifecycleNodeInterfaceImpl::on_deactivate() const +{ + for (const auto & weak_entity : weak_managed_entities_) { + auto entity = weak_entity.lock(); + if (entity) { + entity->on_deactivate(); + } + } +} + +} // namespace rclcpp_lifecycle diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp index 3d1a9802b4..5cf5bdaacf 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp @@ -20,32 +20,29 @@ #include #include #include -#include #include -#include -#include "lifecycle_msgs/msg/transition_description.hpp" -#include "lifecycle_msgs/msg/transition_event.h" // for getting the c-typesupport #include "lifecycle_msgs/msg/transition_event.hpp" #include "lifecycle_msgs/srv/change_state.hpp" #include "lifecycle_msgs/srv/get_state.hpp" #include "lifecycle_msgs/srv/get_available_states.hpp" #include "lifecycle_msgs/srv/get_available_transitions.hpp" -#include "rcl/error_handling.h" - #include "rcl_lifecycle/rcl_lifecycle.h" -#include "rcl_lifecycle/transition_map.h" +#include "rclcpp/macros.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_logging_interface.hpp" #include "rclcpp/node_interfaces/node_services_interface.hpp" -#include "rcutils/logging_macros.h" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +#include "rmw/types.h" namespace rclcpp_lifecycle { -class LifecycleNode::LifecycleNodeInterfaceImpl +class LifecycleNode::LifecycleNodeInterfaceImpl final { using ChangeStateSrv = lifecycle_msgs::srv::ChangeState; using GetStateSrv = lifecycle_msgs::srv::GetState; @@ -56,370 +53,128 @@ class LifecycleNode::LifecycleNodeInterfaceImpl public: LifecycleNodeInterfaceImpl( std::shared_ptr node_base_interface, - std::shared_ptr node_services_interface) - : node_base_interface_(node_base_interface), - node_services_interface_(node_services_interface) - {} - - ~LifecycleNodeInterfaceImpl() - { - rcl_node_t * node_handle = node_base_interface_->get_rcl_node_handle(); - const rcl_node_options_t * node_options = rcl_node_get_options(node_handle); - auto ret = rcl_lifecycle_state_machine_fini( - &state_machine_, node_handle, &node_options->allocator); - if (ret != RCL_RET_OK) { - fprintf(stderr, "FATAL: failed to destroy rcl_state_machine\n"); - } - } + std::shared_ptr node_services_interface, + std::shared_ptr node_logging_interface); + + ~LifecycleNodeInterfaceImpl(); void - init() - { - rcl_node_t * node_handle = node_base_interface_->get_rcl_node_handle(); - const rcl_node_options_t * node_options = - rcl_node_get_options(node_base_interface_->get_rcl_node_handle()); - state_machine_ = rcl_lifecycle_get_zero_initialized_state_machine(); - // The call to initialize the state machine takes - // currently five different typesupports for all publishers/services - // created within the RCL_LIFECYCLE structure. - // The publisher takes a C-Typesupport since the publishing (i.e. creating - // the message) is done fully in RCL. - // Services are handled in C++, so that it needs a C++ typesupport structure. - rcl_ret_t ret = rcl_lifecycle_state_machine_init( - &state_machine_, - node_handle, - ROSIDL_GET_MSG_TYPE_SUPPORT(lifecycle_msgs, msg, TransitionEvent), - rosidl_typesupport_cpp::get_service_type_support_handle(), - rosidl_typesupport_cpp::get_service_type_support_handle(), - rosidl_typesupport_cpp::get_service_type_support_handle(), - rosidl_typesupport_cpp::get_service_type_support_handle(), - true, - &node_options->allocator); - if (ret != RCL_RET_OK) { - throw std::runtime_error( - std::string("Couldn't initialize state machine for node ") + - node_base_interface_->get_name()); - } - - { // change_state - auto cb = std::bind(&LifecycleNodeInterfaceImpl::on_change_state, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); - rclcpp::AnyServiceCallback any_cb; - any_cb.set(std::move(cb)); - - srv_change_state_ = std::make_shared>( - node_base_interface_->get_shared_rcl_node_handle(), - &state_machine_.com_interface.srv_change_state, - any_cb); - node_services_interface_->add_service( - std::dynamic_pointer_cast(srv_change_state_), - nullptr); - } - - { // get_state - auto cb = std::bind(&LifecycleNodeInterfaceImpl::on_get_state, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); - rclcpp::AnyServiceCallback any_cb; - any_cb.set(std::move(cb)); - - srv_get_state_ = std::make_shared>( - node_base_interface_->get_shared_rcl_node_handle(), - &state_machine_.com_interface.srv_get_state, - any_cb); - node_services_interface_->add_service( - std::dynamic_pointer_cast(srv_get_state_), - nullptr); - } - - { // get_available_states - auto cb = std::bind(&LifecycleNodeInterfaceImpl::on_get_available_states, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); - rclcpp::AnyServiceCallback any_cb; - any_cb.set(std::move(cb)); - - srv_get_available_states_ = std::make_shared>( - node_base_interface_->get_shared_rcl_node_handle(), - &state_machine_.com_interface.srv_get_available_states, - any_cb); - node_services_interface_->add_service( - std::dynamic_pointer_cast(srv_get_available_states_), - nullptr); - } - - { // get_available_transitions - auto cb = std::bind(&LifecycleNodeInterfaceImpl::on_get_available_transitions, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); - rclcpp::AnyServiceCallback any_cb; - any_cb.set(std::move(cb)); - - srv_get_available_transitions_ = - std::make_shared>( - node_base_interface_->get_shared_rcl_node_handle(), - &state_machine_.com_interface.srv_get_available_transitions, - any_cb); - node_services_interface_->add_service( - std::dynamic_pointer_cast(srv_get_available_transitions_), - nullptr); - } - } + init(bool enable_communication_interface = true); bool register_callback( - std::uint8_t lifecycle_transition, std::function & cb) - { - cb_map_[lifecycle_transition] = cb; - return true; - } + std::uint8_t lifecycle_transition, + std::function & cb); + + const State & + get_current_state() const; + + std::vector + get_available_states() const; + + std::vector + get_available_transitions() const; + + std::vector + get_transition_graph() const; + + const State & + trigger_transition(uint8_t transition_id); + + const State & + trigger_transition( + uint8_t transition_id, + node_interfaces::LifecycleNodeInterface::CallbackReturn & cb_return_code); + + const State & trigger_transition(const char * transition_label); + + const State & trigger_transition( + const char * transition_label, + node_interfaces::LifecycleNodeInterface::CallbackReturn & cb_return_code); + + void + on_activate() const; + + void + on_deactivate() const; + + void + add_managed_entity(std::weak_ptr managed_entity); + + void + add_timer_handle(std::shared_ptr timer); + +private: + RCLCPP_DISABLE_COPY(LifecycleNodeInterfaceImpl) void on_change_state( const std::shared_ptr header, const std::shared_ptr req, - std::shared_ptr resp) - { - (void)header; - if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { - throw std::runtime_error( - "Can't get state. State machine is not initialized."); - } - rcl_lifecycle_transition_key_t cb_return_code; - auto ret = change_state(req->transition.id, cb_return_code); - (void) ret; - // TODO(karsten1987): Lifecycle msgs have to be extended to keep both returns - // 1. return is the actual transition - // 2. return is whether an error occurred or not - resp->success = - (cb_return_code == lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS); - } + std::shared_ptr resp); void on_get_state( const std::shared_ptr header, const std::shared_ptr req, - std::shared_ptr resp) - { - (void)header; - (void)req; - if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { - throw std::runtime_error( - "Can't get state. State machine is not initialized."); - } - resp->current_state.id = static_cast(state_machine_.current_state->id); - resp->current_state.label = state_machine_.current_state->label; - } + std::shared_ptr resp) const; void on_get_available_states( const std::shared_ptr header, const std::shared_ptr req, - std::shared_ptr resp) - { - (void)header; - (void)req; - if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { - throw std::runtime_error( - "Can't get available states. State machine is not initialized."); - } - for (uint8_t i = 0; i < state_machine_.transition_map.states_size; ++i) { - lifecycle_msgs::msg::State state; - state.id = static_cast(state_machine_.transition_map.states[i].id); - state.label = static_cast(state_machine_.transition_map.states[i].label); - resp->available_states.push_back(state); - } - } + std::shared_ptr resp) const; void on_get_available_transitions( const std::shared_ptr header, const std::shared_ptr req, - std::shared_ptr resp) - { - (void)header; - (void)req; - if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { - throw std::runtime_error( - "Can't get available transitions. State machine is not initialized."); - } - - for (uint8_t i = 0; i < state_machine_.transition_map.transitions_size; ++i) { - rcl_lifecycle_transition_t & rcl_transition = state_machine_.transition_map.transitions[i]; - lifecycle_msgs::msg::TransitionDescription trans_desc; - trans_desc.transition.id = rcl_transition.id; - trans_desc.transition.label = rcl_transition.label; - trans_desc.start_state.id = rcl_transition.start->id; - trans_desc.start_state.label = rcl_transition.start->label; - trans_desc.goal_state.id = rcl_transition.goal->id; - trans_desc.goal_state.label = rcl_transition.goal->label; - resp->available_transitions.push_back(trans_desc); - } - } + std::shared_ptr resp) const; - const State & - get_current_state() - { - current_state_ = State(state_machine_.current_state); - return current_state_; - } - - std::vector - get_available_states() - { - std::vector states; - for (uint8_t i = 0; i < state_machine_.transition_map.states_size; ++i) { - State state(&state_machine_.transition_map.states[i]); - states.push_back(state); - } - return states; - } - - std::vector - get_available_transitions() - { - std::vector transitions; - - for (uint8_t i = 0; i < state_machine_.transition_map.transitions_size; ++i) { - Transition transition( - &state_machine_.transition_map.transitions[i]); - transitions.push_back(transition); - } - return transitions; - } + void + on_get_transition_graph( + const std::shared_ptr header, + const std::shared_ptr req, + std::shared_ptr resp) const; rcl_ret_t - change_state(std::uint8_t lifecycle_transition, rcl_lifecycle_transition_key_t & cb_return_code) - { - if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { - RCUTILS_LOG_ERROR("Unable to change state for state machine for %s: %s", - node_base_interface_->get_name(), rcl_get_error_string_safe()) - return RCL_RET_ERROR; - } - - // keep the initial state to pass to a transition callback - State initial_state(state_machine_.current_state); - - uint8_t transition_id = lifecycle_transition; - if (rcl_lifecycle_trigger_transition(&state_machine_, transition_id, true) != RCL_RET_OK) { - RCUTILS_LOG_ERROR("Unable to start transition %u from current state %s: %s", - transition_id, state_machine_.current_state->label, rcl_get_error_string_safe()) - return RCL_RET_ERROR; - } - - cb_return_code = execute_callback( - state_machine_.current_state->id, initial_state); - - if (rcl_lifecycle_trigger_transition( - &state_machine_, cb_return_code, true) != RCL_RET_OK) - { - RCUTILS_LOG_ERROR("Failed to finish transition %u. Current state is now: %s", - transition_id, state_machine_.current_state->label) - return RCL_RET_ERROR; - } - - // error handling ?! - // TODO(karsten1987): iterate over possible ret value - if (cb_return_code == lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_ERROR) { - RCUTILS_LOG_WARN("Error occurred while doing error handling.") - rcl_lifecycle_transition_key_t error_resolved = execute_callback( - state_machine_.current_state->id, initial_state); - if (error_resolved == lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS) { - // We call cleanup on the error state - if (rcl_lifecycle_trigger_transition(&state_machine_, error_resolved, true) != RCL_RET_OK) { - RCUTILS_LOG_ERROR("Failed to call cleanup on error state") - return RCL_RET_ERROR; - } - } else { - // We call shutdown on the error state - if (rcl_lifecycle_trigger_transition(&state_machine_, error_resolved, true) != RCL_RET_OK) { - RCUTILS_LOG_ERROR("Failed to call cleanup on error state") - return RCL_RET_ERROR; - } - } - } - // This true holds in both cases where the actual callback - // was successful or not, since at this point we have a valid transistion - // to either a new primary state or error state - return RCL_RET_OK; - } - - rcl_lifecycle_transition_key_t - execute_callback(unsigned int cb_id, const State & previous_state) - { - // in case no callback was attached, we forward directly - rcl_lifecycle_transition_key_t cb_success = - lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS; - - auto it = cb_map_.find(cb_id); - if (it != cb_map_.end()) { - auto callback = it->second; - try { - cb_success = callback(State(previous_state)); - } catch (const std::exception &) { - // TODO(karsten1987): Windows CI doens't let me print the msg here - // the todo is to forward the exception to the on_error callback - // fprintf(stderr, "Caught exception in callback for transition %d\n", - // it->first); - // fprintf(stderr, "Original error msg: %s\n", e.what()); - // maybe directly go for error handling here - // and pass exception along with it - cb_success = lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_ERROR; - } - } - return cb_success; - } - - const State & - trigger_transition(uint8_t transition_id) - { - rcl_lifecycle_transition_key_t error; - change_state(transition_id, error); - (void) error; - return get_current_state(); - } + change_state( + std::uint8_t transition_id, + node_interfaces::LifecycleNodeInterface::CallbackReturn & cb_return_code); - const State & - trigger_transition(uint8_t transition_id, rcl_lifecycle_transition_key_t & cb_return_code) - { - change_state(transition_id, cb_return_code); - return get_current_state(); - } - - void - add_publisher_handle(std::shared_ptr pub) - { - weak_pubs_.push_back(pub); - } - - void - add_timer_handle(std::shared_ptr timer) - { - weak_timers_.push_back(timer); - } + node_interfaces::LifecycleNodeInterface::CallbackReturn + execute_callback(unsigned int cb_id, const State & previous_state) const; + mutable std::recursive_mutex state_machine_mutex_; rcl_lifecycle_state_machine_t state_machine_; State current_state_; std::map< std::uint8_t, - std::function> cb_map_; + std::function> cb_map_; using NodeBasePtr = std::shared_ptr; using NodeServicesPtr = std::shared_ptr; + using NodeLoggingPtr = std::shared_ptr; using ChangeStateSrvPtr = std::shared_ptr>; using GetStateSrvPtr = std::shared_ptr>; using GetAvailableStatesSrvPtr = - std::shared_ptr>; + std::shared_ptr>; using GetAvailableTransitionsSrvPtr = - std::shared_ptr>; + std::shared_ptr>; + using GetTransitionGraphSrvPtr = + std::shared_ptr>; NodeBasePtr node_base_interface_; NodeServicesPtr node_services_interface_; + NodeLoggingPtr node_logging_interface_; ChangeStateSrvPtr srv_change_state_; GetStateSrvPtr srv_get_state_; GetAvailableStatesSrvPtr srv_get_available_states_; GetAvailableTransitionsSrvPtr srv_get_available_transitions_; + GetTransitionGraphSrvPtr srv_get_transition_graph_; // to controllable things - std::vector> weak_pubs_; + std::vector> weak_managed_entities_; std::vector> weak_timers_; }; diff --git a/rclcpp/src/rclcpp/intra_process_manager_impl.cpp b/rclcpp_lifecycle/src/managed_entity.cpp similarity index 59% rename from rclcpp/src/rclcpp/intra_process_manager_impl.cpp rename to rclcpp_lifecycle/src/managed_entity.cpp index 2fa1d2f969..8cb8fb46ec 100644 --- a/rclcpp/src/rclcpp/intra_process_manager_impl.cpp +++ b/rclcpp_lifecycle/src/managed_entity.cpp @@ -1,4 +1,4 @@ -// Copyright 2015 Open Source Robotics Foundation, Inc. +// 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. @@ -12,12 +12,24 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "rclcpp/intra_process_manager_impl.hpp" +#include "rclcpp_lifecycle/managed_entity.hpp" -#include +namespace rclcpp_lifecycle +{ + +void SimpleManagedEntity::on_activate() +{ + activated_.store(true); +} -rclcpp::intra_process_manager::IntraProcessManagerImplBase::SharedPtr -rclcpp::intra_process_manager::create_default_impl() +void SimpleManagedEntity::on_deactivate() { - return std::make_shared>(); + activated_.store(false); } + +bool SimpleManagedEntity::is_activated() const +{ + return activated_.load(); +} + +} // namespace rclcpp_lifecycle diff --git a/rclcpp_lifecycle/src/node_interfaces/lifecycle_node_interface.cpp b/rclcpp_lifecycle/src/node_interfaces/lifecycle_node_interface.cpp index df596aa3b9..06b889be05 100644 --- a/rclcpp_lifecycle/src/node_interfaces/lifecycle_node_interface.cpp +++ b/rclcpp_lifecycle/src/node_interfaces/lifecycle_node_interface.cpp @@ -12,48 +12,49 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "lifecycle_msgs/msg/transition.hpp" - #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" + namespace rclcpp_lifecycle { namespace node_interfaces { -rcl_lifecycle_transition_key_t + +LifecycleNodeInterface::CallbackReturn LifecycleNodeInterface::on_configure(const State &) { - return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS; + return LifecycleNodeInterface::CallbackReturn::SUCCESS; } -rcl_lifecycle_transition_key_t +LifecycleNodeInterface::CallbackReturn LifecycleNodeInterface::on_cleanup(const State &) { - return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS; + return LifecycleNodeInterface::CallbackReturn::SUCCESS; } -rcl_lifecycle_transition_key_t +LifecycleNodeInterface::CallbackReturn LifecycleNodeInterface::on_shutdown(const State &) { - return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS; + return LifecycleNodeInterface::CallbackReturn::SUCCESS; } -rcl_lifecycle_transition_key_t +LifecycleNodeInterface::CallbackReturn LifecycleNodeInterface::on_activate(const State &) { - return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS; + return LifecycleNodeInterface::CallbackReturn::SUCCESS; } -rcl_lifecycle_transition_key_t +LifecycleNodeInterface::CallbackReturn LifecycleNodeInterface::on_deactivate(const State &) { - return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS; + return LifecycleNodeInterface::CallbackReturn::SUCCESS; } -rcl_lifecycle_transition_key_t +LifecycleNodeInterface::CallbackReturn LifecycleNodeInterface::on_error(const State &) { - return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_FAILURE; + return LifecycleNodeInterface::CallbackReturn::SUCCESS; } } // namespace node_interfaces diff --git a/rclcpp_lifecycle/src/state.cpp b/rclcpp_lifecycle/src/state.cpp index a84f1d301f..85d59d4af6 100644 --- a/rclcpp_lifecycle/src/state.cpp +++ b/rclcpp_lifecycle/src/state.cpp @@ -14,6 +14,7 @@ #include "rclcpp_lifecycle/state.hpp" +#include #include #include "lifecycle_msgs/msg/state.hpp" @@ -21,6 +22,7 @@ #include "rcl_lifecycle/rcl_lifecycle.h" #include "rclcpp/exceptions.hpp" +#include "rclcpp/logging.hpp" #include "rcutils/allocator.h" @@ -69,6 +71,7 @@ State::State( if (!rcl_lifecycle_state_handle) { throw std::runtime_error("rcl_lifecycle_state_handle is null"); } + state_handle_ = const_cast(rcl_lifecycle_state_handle); } @@ -92,6 +95,8 @@ State::operator=(const State & rhs) return *this; } + // hold the lock until state_handle_ is reconstructed + std::lock_guard lock(state_handle_mutex_); // reset all currently used resources reset(); @@ -127,15 +132,17 @@ State::operator=(const State & rhs) uint8_t State::id() const { + std::lock_guard lock(state_handle_mutex_); if (!state_handle_) { throw std::runtime_error("Error in state! Internal state_handle is NULL."); } - return state_handle_->id; + return static_cast(state_handle_->id); } std::string State::label() const { + std::lock_guard lock(state_handle_mutex_); if (!state_handle_) { throw std::runtime_error("Error in state! Internal state_handle is NULL."); } @@ -143,8 +150,9 @@ State::label() const } void -State::reset() +State::reset() noexcept { + std::lock_guard lock(state_handle_mutex_); if (!owns_rcl_state_handle_) { state_handle_ = nullptr; } @@ -157,7 +165,9 @@ State::reset() allocator_.deallocate(state_handle_, allocator_.state); state_handle_ = nullptr; if (ret != RCL_RET_OK) { - rclcpp::exceptions::throw_from_rcl_error(ret); + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp_lifecycle"), + "rcl_lifecycle_state_fini did not complete successfully, leaking memory"); } } diff --git a/rclcpp_lifecycle/src/transition.cpp b/rclcpp_lifecycle/src/transition.cpp index d7bae6c280..360c1cfb13 100644 --- a/rclcpp_lifecycle/src/transition.cpp +++ b/rclcpp_lifecycle/src/transition.cpp @@ -14,6 +14,7 @@ #include "rclcpp_lifecycle/transition.hpp" +#include #include #include "lifecycle_msgs/msg/transition.hpp" @@ -21,6 +22,7 @@ #include "rcl_lifecycle/rcl_lifecycle.h" #include "rclcpp/exceptions.hpp" +#include "rclcpp/logging.hpp" #include "rcutils/allocator.h" @@ -213,7 +215,7 @@ Transition::id() const if (!transition_handle_) { throw std::runtime_error("internal transition_handle is null"); } - return transition_handle_->id; + return static_cast(transition_handle_->id); } std::string @@ -246,7 +248,7 @@ Transition::goal_state() const } void -Transition::reset() +Transition::reset() noexcept { // can't free anything which is not owned if (!owns_rcl_transition_handle_) { @@ -261,7 +263,9 @@ Transition::reset() allocator_.deallocate(transition_handle_, allocator_.state); transition_handle_ = nullptr; if (ret != RCL_RET_OK) { - rclcpp::exceptions::throw_from_rcl_error(ret); + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp_lifecycle"), + "rcl_lifecycle_transition_fini did not complete successfully, leaking memory"); } } } // namespace rclcpp_lifecycle diff --git a/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_client.cpp b/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_client.cpp new file mode 100644 index 0000000000..59ed24489e --- /dev/null +++ b/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_client.cpp @@ -0,0 +1,316 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include + +#include "lifecycle_msgs/msg/state.hpp" +#include "lifecycle_msgs/srv/change_state.hpp" +#include "lifecycle_msgs/srv/get_available_states.hpp" +#include "lifecycle_msgs/srv/get_available_transitions.hpp" +#include "lifecycle_msgs/srv/get_state.hpp" +#include "performance_test_fixture/performance_test_fixture.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +/** + * Benchmarks for measuring performance of a lifecycle_node client. + * + * A service client of a lifecycle node is not a class that's explicitly part of ros2 core, + * but it is the expected interface for interacting with a lifecycle node, so for that reason + * this benchmark exists. + */ + +using namespace std::chrono_literals; +constexpr char const * lifecycle_node_name = "lc_talker"; + +constexpr char const * node_get_state_topic = "/lc_talker/get_state"; +constexpr char const * node_change_state_topic = "/lc_talker/change_state"; +constexpr char const * node_get_available_states_topic = "/lc_talker/get_available_states"; +constexpr char const * node_get_available_transitions_topic = + "/lc_talker/get_available_transitions"; +constexpr char const * node_get_transition_graph_topic = + "/lc_talker/get_transition_graph"; +const lifecycle_msgs::msg::State unknown_state = lifecycle_msgs::msg::State(); +class LifecycleServiceClient : public rclcpp::Node +{ +public: + explicit LifecycleServiceClient(std::string node_name) + : Node(node_name) + { + client_get_available_states_ = this->create_client( + node_get_available_states_topic); + client_get_available_transitions_ = + this->create_client( + node_get_available_transitions_topic); + client_get_transition_graph_ = + this->create_client( + node_get_transition_graph_topic); + client_get_state_ = this->create_client( + node_get_state_topic); + client_change_state_ = this->create_client( + node_change_state_topic); + } + + lifecycle_msgs::msg::State + get_state(std::chrono::seconds time_out = 1s) + { + auto request = std::make_shared(); + + if (!client_get_state_->wait_for_service(time_out)) { + throw std::runtime_error("Wait for service timed out"); + } + + auto future_result = client_get_state_->async_send_request(request); + auto future_status = future_result.wait_for(time_out); + + if (future_status != std::future_status::ready) { + throw std::runtime_error("Get state request failed"); + } + + if (!future_result.valid()) { + throw std::runtime_error("Future result was not valid"); + } + return future_result.get()->current_state; + } + + bool + change_state(std::uint8_t transition, std::chrono::seconds time_out = 1s) + { + auto request = std::make_shared(); + request->transition.id = transition; + + if (!client_change_state_->wait_for_service(time_out)) { + throw std::runtime_error("Wait for service timed out"); + } + + auto future_result = client_change_state_->async_send_request(request); + auto future_status = future_result.wait_for(time_out); + + if (future_status != std::future_status::ready) { + throw std::runtime_error("Change state request failed"); + } + + if (!future_result.valid()) { + throw std::runtime_error("Future result was not valid"); + } + + return future_result.get()->success; + } + + std::vector + get_available_states(std::chrono::seconds time_out = 1s) + { + auto request = std::make_shared(); + + if (!client_get_available_states_->wait_for_service(time_out)) { + throw std::runtime_error("Wait for service timed out"); + } + + auto future_result = client_get_available_states_->async_send_request(request); + auto future_status = future_result.wait_for(time_out); + + if (future_status != std::future_status::ready) { + throw std::runtime_error("Get available states request failed"); + } + + if (!future_result.valid()) { + throw std::runtime_error("Future result was not valid"); + } + + return future_result.get()->available_states; + } + + std::vector + get_available_transitions(std::chrono::seconds time_out = 1s) + { + auto request = std::make_shared(); + + if (!client_get_available_transitions_->wait_for_service(time_out)) { + throw std::runtime_error("Wait for service timed out"); + } + + auto future_result = client_get_available_transitions_->async_send_request(request); + auto future_status = future_result.wait_for(time_out); + + if (future_status != std::future_status::ready) { + throw std::runtime_error("Get available transitions request failed"); + } + + if (!future_result.valid()) { + throw std::runtime_error("Future result was not valid"); + } + + return future_result.get()->available_transitions; + } + + std::vector + get_transition_graph(std::chrono::seconds time_out = 1s) + { + auto request = std::make_shared(); + + if (!client_get_transition_graph_->wait_for_service(time_out)) { + throw std::runtime_error("Wait for service timed out"); + } + + auto future_result = client_get_transition_graph_->async_send_request(request); + auto future_status = future_result.wait_for(time_out); + + if (future_status != std::future_status::ready) { + throw std::runtime_error("Get transition graph request failed"); + } + + if (!future_result.valid()) { + throw std::runtime_error("Future result was not valid"); + } + + return future_result.get()->available_transitions; + } + +private: + std::shared_ptr> + client_get_available_states_; + std::shared_ptr> + client_get_available_transitions_; + std::shared_ptr> + client_get_transition_graph_; + std::shared_ptr> client_get_state_; + std::shared_ptr> client_change_state_; +}; + +class BenchmarkLifecycleClient : public performance_test_fixture::PerformanceTest +{ +public: + void SetUp(benchmark::State & state) + { + rclcpp::init(0, nullptr); + lifecycle_node = std::make_shared(lifecycle_node_name); + lifecycle_client = std::make_shared("lifecycle_client"); + executor = std::make_shared(); + executor->add_node(lifecycle_node->get_node_base_interface()); + executor->add_node(lifecycle_client->get_node_base_interface()); + spinner_ = std::thread(&rclcpp::executors::SingleThreadedExecutor::spin, executor); + performance_test_fixture::PerformanceTest::SetUp(state); + } + + void TearDown(benchmark::State & state) + { + performance_test_fixture::PerformanceTest::TearDown(state); + executor->cancel(); + spinner_.join(); + executor.reset(); + lifecycle_client.reset(); + lifecycle_node.reset(); + rclcpp::shutdown(); + } + +protected: + std::shared_ptr lifecycle_node; + std::shared_ptr lifecycle_client; + std::shared_ptr executor; + std::thread spinner_; +}; + +BENCHMARK_F(BenchmarkLifecycleClient, get_state)(benchmark::State & state) { + for (auto _ : state) { + (void)_; + + lifecycle_msgs::msg::State lifecycle_state = lifecycle_client->get_state(); + if (lifecycle_state.id != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { + const std::string msg = + std::string("Expected state did not match actual: ") + + std::to_string(lifecycle_state.id); + state.SkipWithError(msg.c_str()); + } + benchmark::DoNotOptimize(lifecycle_state); + benchmark::ClobberMemory(); + } +} + +BENCHMARK_F(BenchmarkLifecycleClient, change_state)(benchmark::State & state) { + bool success = + lifecycle_client->change_state(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); + if (!success) { + state.SkipWithError("Transition to configured state failed"); + } + + reset_heap_counters(); + for (auto _ : state) { + (void)_; + success = + lifecycle_client->change_state(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); + if (!success) { + state.SkipWithError("Transition to active state failed"); + } + + success = + lifecycle_client->change_state(lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE); + if (!success) { + state.SkipWithError("Transition to inactive state failed"); + } + } +} + +BENCHMARK_F(BenchmarkLifecycleClient, get_available_states)(benchmark::State & state) { + for (auto _ : state) { + (void)_; + constexpr size_t expected_states = 11u; + std::vector states = lifecycle_client->get_available_states(); + if (states.size() != expected_states) { + const std::string msg = + std::string("Expected number of states did not match actual: ") + + std::to_string(states.size()); + state.SkipWithError(msg.c_str()); + } + benchmark::DoNotOptimize(states); + benchmark::ClobberMemory(); + } +} + +BENCHMARK_F(BenchmarkLifecycleClient, get_available_transitions)(benchmark::State & state) { + for (auto _ : state) { + (void)_; + constexpr size_t expected_transitions = 2u; + std::vector transitions = + lifecycle_client->get_available_transitions(); + if (transitions.size() != expected_transitions) { + const std::string msg = + std::string("Expected number of transitions did not match actual: ") + + std::to_string(transitions.size()); + state.SkipWithError(msg.c_str()); + } + benchmark::DoNotOptimize(transitions); + benchmark::ClobberMemory(); + } +} + +BENCHMARK_F(BenchmarkLifecycleClient, get_transition_graph)(benchmark::State & state) { + for (auto _ : state) { + (void)_; + constexpr size_t expected_transitions = 25u; + std::vector transitions = + lifecycle_client->get_transition_graph(); + if (transitions.size() != expected_transitions) { + const std::string msg = + std::string("Expected number of transitions did not match actual: ") + + std::to_string(transitions.size()); + state.SkipWithError(msg.c_str()); + } + benchmark::DoNotOptimize(transitions); + benchmark::ClobberMemory(); + } +} diff --git a/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_node.cpp b/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_node.cpp new file mode 100644 index 0000000000..4be9ad13af --- /dev/null +++ b/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_node.cpp @@ -0,0 +1,183 @@ +// 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 "lifecycle_msgs/msg/state.hpp" +#include "performance_test_fixture/performance_test_fixture.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +/** + * Benchmarks for evaluating rclcpp_lifecycle::LifecycleNode. + * + * Much of the lifecycle node API calls rclcpp::Node and node_interface functions, which are + * likely to be inlined and are not of much value to benchmark. + */ + +class BenchmarkLifecycleNodeConstruction : public performance_test_fixture::PerformanceTest +{ +public: + void SetUp(benchmark::State & state) + { + rclcpp::init(0, nullptr); + performance_test_fixture::PerformanceTest::SetUp(state); + } + + void TearDown(benchmark::State & state) + { + performance_test_fixture::PerformanceTest::TearDown(state); + rclcpp::shutdown(); + } +}; + +BENCHMARK_F(BenchmarkLifecycleNodeConstruction, construct_lifecycle_node)( + benchmark::State & state) +{ + for (auto _ : state) { + (void)_; + auto node = std::make_shared("node", "ns"); + PERFORMANCE_TEST_FIXTURE_PAUSE_MEASUREMENTS( + state, + { + node.reset(); + }); + } +} + +BENCHMARK_F(BenchmarkLifecycleNodeConstruction, destroy_lifecycle_node)(benchmark::State & state) { + for (auto _ : state) { + (void)_; + std::shared_ptr node(nullptr); + PERFORMANCE_TEST_FIXTURE_PAUSE_MEASUREMENTS( + state, + { + node = std::make_shared("node", "ns"); + }); + node.reset(); + } +} + +class BenchmarkLifecycleNode : public performance_test_fixture::PerformanceTest +{ +public: + void SetUp(benchmark::State & state) + { + rclcpp::init(0, nullptr); + node = std::make_shared("node", "ns"); + performance_test_fixture::PerformanceTest::SetUp(state); + } + + void TearDown(benchmark::State & state) + { + performance_test_fixture::PerformanceTest::TearDown(state); + node.reset(); + rclcpp::shutdown(); + } + +protected: + std::shared_ptr node; +}; + +// This is a simple getter, but it crosses over into the rcl library. +BENCHMARK_F(BenchmarkLifecycleNode, get_current_state)(benchmark::State & state) { + for (auto _ : state) { + (void)_; + const rclcpp_lifecycle::State & lifecycle_state = node->get_current_state(); + if (lifecycle_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { + const std::string message = + std::string("Node's current state is: ") + std::to_string(lifecycle_state.id()); + state.SkipWithError(message.c_str()); + } + // Google benchmark 1.8.2 warns that the constref DoNotOptimize signature may be optimized away + // by the compiler. Cast const away to ensure we don't get that problem (and warning). + benchmark::DoNotOptimize(const_cast(lifecycle_state)); + benchmark::ClobberMemory(); + } +} + +BENCHMARK_F(BenchmarkLifecycleNode, get_available_states)(benchmark::State & state) { + for (auto _ : state) { + (void)_; + constexpr size_t expected_states = 11u; + std::vector lifecycle_states = node->get_available_states(); + if (lifecycle_states.size() != expected_states) { + const std::string msg = std::to_string(lifecycle_states.size()); + state.SkipWithError(msg.c_str()); + } + benchmark::DoNotOptimize(lifecycle_states); + benchmark::ClobberMemory(); + } +} + +BENCHMARK_F(BenchmarkLifecycleNode, get_available_transitions)(benchmark::State & state) { + for (auto _ : state) { + (void)_; + constexpr size_t expected_transitions = 2u; + std::vector transitions = node->get_available_transitions(); + if (transitions.size() != expected_transitions) { + const std::string msg = std::to_string(transitions.size()); + state.SkipWithError(msg.c_str()); + } + benchmark::DoNotOptimize(transitions); + benchmark::ClobberMemory(); + } +} + +BENCHMARK_F(BenchmarkLifecycleNode, get_transition_graph)(benchmark::State & state) { + for (auto _ : state) { + (void)_; + constexpr size_t expected_transitions = 25u; + std::vector transitions = node->get_transition_graph(); + if (transitions.size() != expected_transitions) { + const std::string msg = + std::string("Expected number of transitions did not match actual: ") + + std::to_string(transitions.size()); + state.SkipWithError(msg.c_str()); + } + benchmark::DoNotOptimize(transitions); + benchmark::ClobberMemory(); + } +} + +BENCHMARK_F(BenchmarkLifecycleNode, transition_valid_state)(benchmark::State & state) { + const auto & configured = + node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); + if (configured.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { + state.SkipWithError("Transition to configured state failed"); + } + + reset_heap_counters(); + for (auto _ : state) { + (void)_; + const rclcpp_lifecycle::State & active = + node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); + if (active.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { + state.SkipWithError("Transition to active state failed"); + } + const rclcpp_lifecycle::State & inactive = + node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE); + if (inactive.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { + state.SkipWithError("Transition to inactive state failed"); + } + // Google benchmark 1.8.2 warns that the constref DoNotOptimize signature may be optimized away + // by the compiler. Cast const away to ensure we don't get that problem (and warning). + benchmark::DoNotOptimize(const_cast(active)); + benchmark::DoNotOptimize(const_cast(inactive)); + benchmark::ClobberMemory(); + } +} diff --git a/rclcpp_lifecycle/test/benchmark/benchmark_state.cpp b/rclcpp_lifecycle/test/benchmark/benchmark_state.cpp new file mode 100644 index 0000000000..8a1003bfbc --- /dev/null +++ b/rclcpp_lifecycle/test/benchmark/benchmark_state.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 "performance_test_fixture/performance_test_fixture.hpp" +#include "rclcpp_lifecycle/transition.hpp" + +using PerformanceTest = performance_test_fixture::PerformanceTest; + +// These are relatively quick benchmarks, so it would be difficult to separate construction from +// destruction +BENCHMARK_F(PerformanceTest, construct_destruct_state)(benchmark::State & state) +{ + for (auto _ : state) { + (void)_; + rclcpp_lifecycle::State lifecycle_state(1, "state"); + benchmark::DoNotOptimize(lifecycle_state); + benchmark::ClobberMemory(); + } +} + +BENCHMARK_F(PerformanceTest, copy_destruct_state)(benchmark::State & state) +{ + rclcpp_lifecycle::State lifecycle_state(1, "state"); + for (auto _ : state) { + (void)_; + rclcpp_lifecycle::State state_copy(lifecycle_state); + benchmark::DoNotOptimize(state_copy); + benchmark::ClobberMemory(); + } +} diff --git a/rclcpp_lifecycle/test/benchmark/benchmark_transition.cpp b/rclcpp_lifecycle/test/benchmark/benchmark_transition.cpp new file mode 100644 index 0000000000..6ac6761aed --- /dev/null +++ b/rclcpp_lifecycle/test/benchmark/benchmark_transition.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 "performance_test_fixture/performance_test_fixture.hpp" +#include "rclcpp_lifecycle/transition.hpp" + +using PerformanceTest = performance_test_fixture::PerformanceTest; + +// These are relatively quick benchmarks, so it would be difficult to separate construction from +// destruction +BENCHMARK_F(PerformanceTest, construct_destruct_transition)(benchmark::State & state) +{ + for (auto _ : state) { + (void)_; + rclcpp_lifecycle::Transition transition(1, "transition"); + benchmark::DoNotOptimize(transition); + benchmark::ClobberMemory(); + } +} + +BENCHMARK_F(PerformanceTest, copy_destruct_transition)(benchmark::State & state) +{ + rclcpp_lifecycle::Transition transition(1, "transition"); + for (auto _ : state) { + (void)_; + rclcpp_lifecycle::Transition transition_copy(transition); + benchmark::DoNotOptimize(transition_copy); + benchmark::ClobberMemory(); + } +} diff --git a/rclcpp_lifecycle/test/mocking_utils/patch.hpp b/rclcpp_lifecycle/test/mocking_utils/patch.hpp new file mode 100644 index 0000000000..a6249da3af --- /dev/null +++ b/rclcpp_lifecycle/test/mocking_utils/patch.hpp @@ -0,0 +1,550 @@ +// 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); +}; + +/// Traits specialization for +/// ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7, ArgT8) +/// 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, ReturnT, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7, ArgT8); +}; + +/// Traits specialization for void(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7) +/// 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, ReturnT, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7, ArgT8, ArgT9); +}; + +/// 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/rclcpp_lifecycle/test/test_callback_exceptions.cpp b/rclcpp_lifecycle/test/test_callback_exceptions.cpp index 5eb4f452a4..cd6683459f 100644 --- a/rclcpp_lifecycle/test/test_callback_exceptions.cpp +++ b/rclcpp_lifecycle/test/test_callback_exceptions.cpp @@ -34,6 +34,11 @@ class TestCallbackExceptions : public ::testing::Test { rclcpp::init(0, nullptr); } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } }; class PositiveCallbackExceptionNode : public rclcpp_lifecycle::LifecycleNode @@ -46,18 +51,18 @@ class PositiveCallbackExceptionNode : public rclcpp_lifecycle::LifecycleNode size_t number_of_callbacks = 0; protected: - rcl_lifecycle_transition_key_t + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure(const rclcpp_lifecycle::State &) { ++number_of_callbacks; throw std::runtime_error("custom exception raised in configure callback"); } - rcl_lifecycle_transition_key_t + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_error(const rclcpp_lifecycle::State &) { ++number_of_callbacks; - return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS; + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } }; @@ -65,19 +70,21 @@ TEST_F(TestCallbackExceptions, positive_on_error) { auto test_node = std::make_shared("testnode"); EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); - EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition( + EXPECT_EQ( + State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition( rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); // check if all callbacks were successfully overwritten - EXPECT_EQ(static_cast(2), test_node->number_of_callbacks); + EXPECT_EQ(2u, test_node->number_of_callbacks); } TEST_F(TestCallbackExceptions, positive_on_error_with_code) { auto test_node = std::make_shared("testnode"); EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); - rcl_lifecycle_transition_key_t ret = lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn ret = + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; test_node->configure(ret); - EXPECT_EQ(lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_ERROR, ret); + EXPECT_EQ(rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR, ret); } class NegativeCallbackExceptionNode : public rclcpp_lifecycle::LifecycleNode @@ -90,18 +97,18 @@ class NegativeCallbackExceptionNode : public rclcpp_lifecycle::LifecycleNode size_t number_of_callbacks = 0; protected: - rcl_lifecycle_transition_key_t + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure(const rclcpp_lifecycle::State &) { ++number_of_callbacks; throw std::runtime_error("custom exception raised in configure callback"); } - rcl_lifecycle_transition_key_t + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_error(const rclcpp_lifecycle::State &) { ++number_of_callbacks; - return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_FAILURE; + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } }; @@ -109,17 +116,18 @@ TEST_F(TestCallbackExceptions, negative_on_error) { auto test_node = std::make_shared("testnode"); EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); - EXPECT_EQ(State::PRIMARY_STATE_FINALIZED, test_node->trigger_transition( + EXPECT_EQ( + State::PRIMARY_STATE_FINALIZED, test_node->trigger_transition( rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); // check if all callbacks were successfully overwritten - EXPECT_EQ(static_cast(2), test_node->number_of_callbacks); + EXPECT_EQ(2u, test_node->number_of_callbacks); } TEST_F(TestCallbackExceptions, negative_on_error_with_code) { auto test_node = std::make_shared("testnode"); EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); - rcl_lifecycle_transition_key_t ret = RCL_RET_OK; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn ret; test_node->configure(ret); - EXPECT_EQ(lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_ERROR, ret); + EXPECT_EQ(rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR, ret); } diff --git a/rclcpp_lifecycle/test/test_client.cpp b/rclcpp_lifecycle/test/test_client.cpp new file mode 100644 index 0000000000..f34d71ebfc --- /dev/null +++ b/rclcpp_lifecycle/test/test_client.cpp @@ -0,0 +1,101 @@ +// 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 +#include +#include + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "rcl_interfaces/srv/list_parameters.hpp" + +#include "rmw/qos_profiles.h" + +class TestClient : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node_ = std::make_shared("my_lifecycle_node", "/ns"); + } + + void TearDown() + { + node_.reset(); + } + + rclcpp_lifecycle::LifecycleNode::SharedPtr node_; +}; + +/* + Testing client construction and destruction. + */ +TEST_F(TestClient, construction_and_destruction) { + using rcl_interfaces::srv::ListParameters; + { + auto client = node_->create_client("service"); + EXPECT_TRUE(client); + } + { + auto client = node_->create_client( + "service", rclcpp::ServicesQoS()); + EXPECT_TRUE(client); + } + + { + ASSERT_THROW( + { + auto client = node_->create_client("invalid_service?"); + }, rclcpp::exceptions::InvalidServiceNameError); + } +} + +TEST_F(TestClient, construction_with_free_function) { + { + auto client = rclcpp::create_client( + node_->get_node_base_interface(), + node_->get_node_graph_interface(), + node_->get_node_services_interface(), + "service", + rclcpp::ServicesQoS(), + nullptr); + EXPECT_TRUE(client); + } + { + ASSERT_THROW( + { + auto client = rclcpp::create_client( + node_->get_node_base_interface(), + node_->get_node_graph_interface(), + node_->get_node_services_interface(), + "invalid_?service", + rclcpp::ServicesQoS(), + nullptr); + }, rclcpp::exceptions::InvalidServiceNameError); + } +} diff --git a/rclcpp_lifecycle/test/test_lifecycle_node.cpp b/rclcpp_lifecycle/test/test_lifecycle_node.cpp index 01153e353a..70993af6d5 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_node.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_node.cpp @@ -14,29 +14,111 @@ #include +#include #include +#include #include +#include #include #include "lifecycle_msgs/msg/state.hpp" #include "lifecycle_msgs/msg/transition.hpp" +#include "rcl_lifecycle/rcl_lifecycle.h" +#include "rcl_interfaces/srv/get_logger_levels.hpp" +#include "rcl_interfaces/srv/set_logger_levels.hpp" + #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "./mocking_utils/patch.hpp" + using lifecycle_msgs::msg::State; using lifecycle_msgs::msg::Transition; -struct GoodMood +using namespace std::chrono_literals; + +static const std::chrono::nanoseconds DEFAULT_EVENT_TIMEOUT = std::chrono::seconds(3); +static const std::chrono::nanoseconds DEFAULT_EVENT_SLEEP_PERIOD = std::chrono::milliseconds(100); + +static +bool wait_for_event( + std::shared_ptr node, + std::function predicate, + std::chrono::nanoseconds timeout, + std::chrono::nanoseconds sleep_period) { - static constexpr rcl_lifecycle_transition_key_t cb_ret = - lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS; -}; -struct BadMood + auto start = std::chrono::steady_clock::now(); + std::chrono::microseconds time_slept(0); + + bool predicate_result; + while (!(predicate_result = predicate()) && + time_slept < std::chrono::duration_cast(timeout)) + { + rclcpp::Event::SharedPtr graph_event = node->get_graph_event(); + node->wait_for_graph_change(graph_event, sleep_period); + time_slept = std::chrono::duration_cast( + std::chrono::steady_clock::now() - start); + } + return predicate_result; +} + +static +bool wait_for_topic( + std::shared_ptr node, + const std::string & topic, + std::chrono::nanoseconds timeout = DEFAULT_EVENT_TIMEOUT, + std::chrono::nanoseconds sleep_period = DEFAULT_EVENT_SLEEP_PERIOD) { - static constexpr rcl_lifecycle_transition_key_t cb_ret = - lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_FAILURE; -}; + return wait_for_event( + node, + [node, topic]() + { + auto topic_names_and_types = node->get_topic_names_and_types(); + return topic_names_and_types.end() != topic_names_and_types.find(topic); + }, + timeout, + sleep_period); +} + +static +bool wait_for_service( + std::shared_ptr node, + const std::string & service, + std::chrono::nanoseconds timeout = DEFAULT_EVENT_TIMEOUT, + std::chrono::nanoseconds sleep_period = DEFAULT_EVENT_SLEEP_PERIOD) +{ + return wait_for_event( + node, + [node, service]() + { + auto service_names_and_types = node->get_service_names_and_types(); + return service_names_and_types.end() != service_names_and_types.find(service); + }, + timeout, + sleep_period); +} + +static +bool wait_for_service_by_node( + std::shared_ptr node, + const std::string & node_name, + const std::string & service, + std::chrono::nanoseconds timeout = DEFAULT_EVENT_TIMEOUT, + std::chrono::nanoseconds sleep_period = DEFAULT_EVENT_SLEEP_PERIOD) +{ + return wait_for_event( + node, + [node, node_name, service]() + { + auto service_names_and_types_by_node = node->get_service_names_and_types_by_node( + node_name, ""); + return service_names_and_types_by_node.end() != service_names_and_types_by_node.find( + service); + }, + timeout, + sleep_period); +} class TestDefaultStateMachine : public ::testing::Test { @@ -45,16 +127,35 @@ class TestDefaultStateMachine : public ::testing::Test { rclcpp::init(0, nullptr); } + static void TearDownTestCase() + { + rclcpp::shutdown(); + } }; class EmptyLifecycleNode : public rclcpp_lifecycle::LifecycleNode { public: - explicit EmptyLifecycleNode(std::string node_name) - : rclcpp_lifecycle::LifecycleNode(std::move(node_name)) + explicit EmptyLifecycleNode(const std::string & node_name) + : rclcpp_lifecycle::LifecycleNode(node_name) {} }; +struct GoodMood +{ + using CallbackReturnT = + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + static constexpr CallbackReturnT cb_ret = static_cast( + lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS); +}; +struct BadMood +{ + using CallbackReturnT = + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + static constexpr CallbackReturnT cb_ret = static_cast( + lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_FAILURE); +}; + template class MoodyLifecycleNode : public rclcpp_lifecycle::LifecycleNode { @@ -66,7 +167,7 @@ class MoodyLifecycleNode : public rclcpp_lifecycle::LifecycleNode size_t number_of_callbacks = 0; protected: - rcl_lifecycle_transition_key_t + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure(const rclcpp_lifecycle::State &) { EXPECT_EQ(State::TRANSITION_STATE_CONFIGURING, get_current_state().id()); @@ -74,7 +175,7 @@ class MoodyLifecycleNode : public rclcpp_lifecycle::LifecycleNode return Mood::cb_ret; } - rcl_lifecycle_transition_key_t + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate(const rclcpp_lifecycle::State &) { EXPECT_EQ(State::TRANSITION_STATE_ACTIVATING, get_current_state().id()); @@ -82,7 +183,7 @@ class MoodyLifecycleNode : public rclcpp_lifecycle::LifecycleNode return Mood::cb_ret; } - rcl_lifecycle_transition_key_t + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) { EXPECT_EQ(State::TRANSITION_STATE_DEACTIVATING, get_current_state().id()); @@ -90,7 +191,7 @@ class MoodyLifecycleNode : public rclcpp_lifecycle::LifecycleNode return Mood::cb_ret; } - rcl_lifecycle_transition_key_t + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &) { EXPECT_EQ(State::TRANSITION_STATE_CLEANINGUP, get_current_state().id()); @@ -98,7 +199,7 @@ class MoodyLifecycleNode : public rclcpp_lifecycle::LifecycleNode return Mood::cb_ret; } - rcl_lifecycle_transition_key_t + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_shutdown(const rclcpp_lifecycle::State &) { EXPECT_EQ(State::TRANSITION_STATE_SHUTTINGDOWN, get_current_state().id()); @@ -106,100 +207,651 @@ class MoodyLifecycleNode : public rclcpp_lifecycle::LifecycleNode return Mood::cb_ret; } - rcl_lifecycle_transition_key_t + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_error(const rclcpp_lifecycle::State &); }; template<> -rcl_lifecycle_transition_key_t +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn MoodyLifecycleNode::on_error(const rclcpp_lifecycle::State &) { EXPECT_EQ(State::TRANSITION_STATE_ERRORPROCESSING, get_current_state().id()); ADD_FAILURE(); - return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_ERROR; + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } + template<> -rcl_lifecycle_transition_key_t +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn MoodyLifecycleNode::on_error(const rclcpp_lifecycle::State &) { EXPECT_EQ(State::TRANSITION_STATE_ERRORPROCESSING, get_current_state().id()); ++number_of_callbacks; - return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS; + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } TEST_F(TestDefaultStateMachine, empty_initializer) { auto test_node = std::make_shared("testnode"); EXPECT_STREQ("testnode", test_node->get_name()); EXPECT_STREQ("/", test_node->get_namespace()); + EXPECT_STREQ("/testnode", test_node->get_fully_qualified_name()); EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); } +TEST_F(TestDefaultStateMachine, check_logger_services_exist) { + // Logger level services are disabled + { + rclcpp::NodeOptions options = rclcpp::NodeOptions(); + options.enable_logger_service(false); + auto node = std::make_shared( + "test_logger_service", "/test", options); + auto get_client = node->create_client( + "/test/test_logger_service/get_logger_levels"); + ASSERT_FALSE(get_client->wait_for_service(2s)); + auto set_client = node->create_client( + "/test/test_logger_service/set_logger_levels"); + ASSERT_FALSE(set_client->wait_for_service(2s)); + } + // Logger level services are enabled + { + rclcpp::NodeOptions options = rclcpp::NodeOptions(); + options.enable_logger_service(true); + auto node = std::make_shared( + "test_logger_service", "/test", options); + auto get_client = node->create_client( + "/test/test_logger_service/get_logger_levels"); + ASSERT_TRUE(get_client->wait_for_service(2s)); + auto set_client = node->create_client( + "/test/test_logger_service/set_logger_levels"); + ASSERT_TRUE(set_client->wait_for_service(2s)); + } +} + TEST_F(TestDefaultStateMachine, trigger_transition) { auto test_node = std::make_shared("testnode"); EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); - EXPECT_EQ(State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( + ASSERT_EQ( + State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); - EXPECT_EQ(State::PRIMARY_STATE_ACTIVE, test_node->trigger_transition( + ASSERT_EQ( + State::PRIMARY_STATE_ACTIVE, test_node->trigger_transition( rclcpp_lifecycle::Transition(Transition::TRANSITION_ACTIVATE)).id()); - EXPECT_EQ(State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( + ASSERT_EQ( + State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( rclcpp_lifecycle::Transition(Transition::TRANSITION_DEACTIVATE)).id()); - EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition( + ASSERT_EQ( + State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition( rclcpp_lifecycle::Transition(Transition::TRANSITION_CLEANUP)).id()); - EXPECT_EQ(State::PRIMARY_STATE_FINALIZED, test_node->trigger_transition( - rclcpp_lifecycle::Transition(Transition::TRANSITION_SHUTDOWN)).id()); + ASSERT_EQ( + State::PRIMARY_STATE_FINALIZED, test_node->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_UNCONFIGURED_SHUTDOWN)).id()); +} + +TEST_F(TestDefaultStateMachine, trigger_transition_rcl_errors) { + auto test_node = std::make_shared("testnode"); + + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_lifecycle", rcl_lifecycle_state_machine_is_initialized, RCL_RET_ERROR); + EXPECT_EQ( + State::PRIMARY_STATE_UNCONFIGURED, + test_node->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); + } + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_lifecycle", rcl_lifecycle_trigger_transition_by_id, RCL_RET_ERROR); + EXPECT_EQ( + State::PRIMARY_STATE_UNCONFIGURED, + test_node->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); + } + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_lifecycle", rcl_lifecycle_trigger_transition_by_label, RCL_RET_ERROR); + EXPECT_EQ( + State::TRANSITION_STATE_CONFIGURING, + test_node->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); + } } TEST_F(TestDefaultStateMachine, trigger_transition_with_error_code) { auto test_node = std::make_shared("testnode"); + auto success = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + auto reset_key = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + auto ret = reset_key; + + EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); + test_node->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE), ret); + ASSERT_EQ(success, ret); + ret = reset_key; + + test_node->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_ACTIVATE), ret); + ASSERT_EQ(success, ret); + ret = reset_key; + + test_node->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_DEACTIVATE), ret); + ASSERT_EQ(success, ret); + ret = reset_key; + + test_node->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_CLEANUP), ret); + ASSERT_EQ(success, ret); + ret = reset_key; + + test_node->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_UNCONFIGURED_SHUTDOWN), ret); + ASSERT_EQ(success, ret); +} + +TEST_F(TestDefaultStateMachine, call_transitions_with_error_code) { + auto test_node = std::make_shared("testnode"); + + auto success = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + auto reset_key = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + auto ret = reset_key; - rcl_lifecycle_transition_key_t ret = lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_ERROR; test_node->configure(ret); - EXPECT_EQ(lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS, ret); - ret = lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_ERROR; + + EXPECT_EQ(success, ret); + ret = reset_key; test_node->activate(ret); - EXPECT_EQ(lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS, ret); - ret = lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_ERROR; + EXPECT_EQ(success, ret); + ret = reset_key; test_node->deactivate(ret); - EXPECT_EQ(lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS, ret); - ret = lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_ERROR; + EXPECT_EQ(success, ret); + ret = reset_key; test_node->cleanup(ret); - EXPECT_EQ(lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS, ret); - ret = lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_ERROR; + EXPECT_EQ(success, ret); + ret = reset_key; test_node->shutdown(ret); - EXPECT_EQ(lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS, ret); + EXPECT_EQ(success, ret); +} + +TEST_F(TestDefaultStateMachine, call_transitions_without_code) { + auto test_node = std::make_shared("testnode"); + + auto configured = test_node->configure(); + EXPECT_EQ(configured.id(), State::PRIMARY_STATE_INACTIVE); + + auto activated = test_node->activate(); + EXPECT_EQ(activated.id(), State::PRIMARY_STATE_ACTIVE); + + auto deactivated = test_node->deactivate(); + EXPECT_EQ(deactivated.id(), State::PRIMARY_STATE_INACTIVE); + + auto unconfigured = test_node->cleanup(); + EXPECT_EQ(unconfigured.id(), State::PRIMARY_STATE_UNCONFIGURED); + + auto finalized = test_node->shutdown(); + EXPECT_EQ(finalized.id(), State::PRIMARY_STATE_FINALIZED); } TEST_F(TestDefaultStateMachine, good_mood) { auto test_node = std::make_shared>("testnode"); EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); - EXPECT_EQ(State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( + EXPECT_EQ( + State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); - EXPECT_EQ(State::PRIMARY_STATE_ACTIVE, test_node->trigger_transition( + EXPECT_EQ( + State::PRIMARY_STATE_ACTIVE, test_node->trigger_transition( rclcpp_lifecycle::Transition(Transition::TRANSITION_ACTIVATE)).id()); - EXPECT_EQ(State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( + EXPECT_EQ( + State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( rclcpp_lifecycle::Transition(Transition::TRANSITION_DEACTIVATE)).id()); - EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition( + EXPECT_EQ( + State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition( rclcpp_lifecycle::Transition(Transition::TRANSITION_CLEANUP)).id()); - EXPECT_EQ(State::PRIMARY_STATE_FINALIZED, test_node->trigger_transition( - rclcpp_lifecycle::Transition(Transition::TRANSITION_SHUTDOWN)).id()); + EXPECT_EQ( + State::PRIMARY_STATE_FINALIZED, test_node->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_UNCONFIGURED_SHUTDOWN)).id()); // check if all callbacks were successfully overwritten - EXPECT_EQ(static_cast(5), test_node->number_of_callbacks); + EXPECT_EQ(5u, test_node->number_of_callbacks); } TEST_F(TestDefaultStateMachine, bad_mood) { auto test_node = std::make_shared>("testnode"); EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); - EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition( + EXPECT_EQ( + State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition( rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); // check if all callbacks were successfully overwritten - EXPECT_EQ(static_cast(1), test_node->number_of_callbacks); + EXPECT_EQ(1u, test_node->number_of_callbacks); +} + +TEST_F(TestDefaultStateMachine, shutdown_from_each_primary_state) { + auto success = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + auto reset_key = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + + // PRIMARY_STATE_UNCONFIGURED to shutdown + { + auto ret = reset_key; + auto test_node = std::make_shared("testnode"); + auto finalized = test_node->shutdown(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(finalized.id(), State::PRIMARY_STATE_FINALIZED); + } + + // PRIMARY_STATE_INACTIVE to shutdown + { + auto ret = reset_key; + auto test_node = std::make_shared("testnode"); + auto configured = test_node->configure(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(configured.id(), State::PRIMARY_STATE_INACTIVE); + ret = reset_key; + auto finalized = test_node->shutdown(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(finalized.id(), State::PRIMARY_STATE_FINALIZED); + } + + // PRIMARY_STATE_ACTIVE to shutdown + { + auto ret = reset_key; + auto test_node = std::make_shared("testnode"); + auto configured = test_node->configure(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(configured.id(), State::PRIMARY_STATE_INACTIVE); + ret = reset_key; + auto activated = test_node->activate(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(activated.id(), State::PRIMARY_STATE_ACTIVE); + ret = reset_key; + auto finalized = test_node->shutdown(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(finalized.id(), State::PRIMARY_STATE_FINALIZED); + } + + // PRIMARY_STATE_FINALIZED to shutdown + { + auto ret = reset_key; + auto test_node = std::make_shared("testnode"); + auto configured = test_node->configure(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(configured.id(), State::PRIMARY_STATE_INACTIVE); + ret = reset_key; + auto activated = test_node->activate(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(activated.id(), State::PRIMARY_STATE_ACTIVE); + ret = reset_key; + auto finalized = test_node->shutdown(ret); + EXPECT_EQ(success, ret); + EXPECT_EQ(finalized.id(), State::PRIMARY_STATE_FINALIZED); + ret = reset_key; + auto finalized_again = test_node->shutdown(ret); + EXPECT_EQ(reset_key, ret); + EXPECT_EQ(finalized_again.id(), State::PRIMARY_STATE_FINALIZED); + } +} + +TEST_F(TestDefaultStateMachine, lifecycle_subscriber) { + auto test_node = std::make_shared>("testnode"); + + auto cb = [](const std::shared_ptr msg) {(void) msg;}; + auto lifecycle_sub = + test_node->create_subscription("~/empty", 10, cb); + + SUCCEED(); +} + +// Parameters are tested more thoroughly in rclcpp's test_node.cpp +// These are provided for coverage of lifecycle node's API +TEST_F(TestDefaultStateMachine, declare_parameters) { + // "start_type_description_service" and "use_sim_time" + const uint64_t builtin_param_count = 2; + const uint64_t expected_param_count = 6 + builtin_param_count; + auto test_node = std::make_shared("testnode"); + + auto list_result = test_node->list_parameters({}, 0u); + EXPECT_EQ(list_result.names.size(), builtin_param_count); + EXPECT_STREQ(list_result.names[0].c_str(), "start_type_description_service"); + EXPECT_STREQ(list_result.names[1].c_str(), "use_sim_time"); + + const std::string bool_name = "test_boolean"; + const std::string int_name = "test_int"; + + // Default descriptor overload + test_node->declare_parameter(bool_name, rclcpp::ParameterValue(false)); + + // Explicit descriptor overload + rcl_interfaces::msg::ParameterDescriptor int_descriptor; + int_descriptor.name = int_name; + int_descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + int_descriptor.description = "Example integer parameter"; + test_node->declare_parameter(int_name, rclcpp::ParameterValue(42), int_descriptor); + + std::map str_parameters; + str_parameters["str_one"] = "stringy_string"; + str_parameters["str_two"] = "stringy_string_string"; + + // Default descriptor overload + test_node->declare_parameters("test_string", str_parameters); + + std::map> double_parameters; + rcl_interfaces::msg::ParameterDescriptor double_descriptor_one; + double_descriptor_one.name = "double_one"; + double_descriptor_one.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + double_parameters["double_one"] = std::make_pair(1.0, double_descriptor_one); + + rcl_interfaces::msg::ParameterDescriptor double_descriptor_two; + double_descriptor_two.name = "double_two"; + double_descriptor_two.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + double_parameters["double_two"] = std::make_pair(2.0, double_descriptor_two); + + // Explicit descriptor overload + test_node->declare_parameters("test_double", double_parameters); + + list_result = test_node->list_parameters({}, 0u); + EXPECT_EQ(list_result.names.size(), expected_param_count); + + // The order of these names is not controlled by lifecycle_node, doing set equality + std::set expected_names = { + "start_type_description_service", + "test_boolean", + "test_double.double_one", + "test_double.double_two", + "test_int", + "test_string.str_one", + "test_string.str_two", + "use_sim_time", + }; + std::set actual_names(list_result.names.begin(), list_result.names.end()); + + EXPECT_EQ(expected_names, actual_names); +} + +TEST_F(TestDefaultStateMachine, check_parameters) { + const uint64_t builtin_param_count = 2; + auto test_node = std::make_shared("testnode"); + + auto list_result = test_node->list_parameters({}, 0u); + EXPECT_EQ(list_result.names.size(), builtin_param_count); + EXPECT_STREQ(list_result.names[0].c_str(), "start_type_description_service"); + EXPECT_STREQ(list_result.names[1].c_str(), "use_sim_time"); + + const std::string bool_name = "test_boolean"; + const std::string int_name = "test_int"; + std::vector parameter_names = {bool_name, int_name}; + + EXPECT_FALSE(test_node->has_parameter(bool_name)); + EXPECT_FALSE(test_node->has_parameter(int_name)); + EXPECT_THROW( + test_node->get_parameters(parameter_names), + rclcpp::exceptions::ParameterNotDeclaredException); + + // Default descriptor overload + rcl_interfaces::msg::ParameterDescriptor bool_descriptor; + bool_descriptor.dynamic_typing = true; + test_node->declare_parameter(bool_name, rclcpp::ParameterValue(true), bool_descriptor); + + // Explicit descriptor overload + rcl_interfaces::msg::ParameterDescriptor int_descriptor; + int_descriptor.name = int_name; + int_descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + int_descriptor.description = "Example integer parameter"; + test_node->declare_parameter(int_name, rclcpp::ParameterValue(42), int_descriptor); + + // describe parameters + auto descriptors = test_node->describe_parameters(parameter_names); + EXPECT_EQ(descriptors.size(), parameter_names.size()); + + // This actually throws inside NodeParameters::describe_parameters(), so it's not currently + // possible to cover this method 100%. + EXPECT_THROW( + test_node->describe_parameter("not_a_real_parameter"), + rclcpp::exceptions::ParameterNotDeclaredException); + + // describe parameter matches explicit descriptor + auto descriptor = test_node->describe_parameter(int_name); + EXPECT_STREQ(descriptor.name.c_str(), int_descriptor.name.c_str()); + EXPECT_EQ(descriptor.type, int_descriptor.type); + EXPECT_STREQ(descriptor.description.c_str(), int_descriptor.description.c_str()); + + // bool parameter exists and value matches + EXPECT_TRUE(test_node->has_parameter(bool_name)); + EXPECT_EQ(test_node->get_parameter(bool_name).as_bool(), true); + + // int parameter exists and value matches + EXPECT_TRUE(test_node->has_parameter(int_name)); + EXPECT_EQ(test_node->get_parameter(int_name).as_int(), 42); + + // Get multiple parameters at a time + auto parameters = test_node->get_parameters(parameter_names); + EXPECT_EQ(parameters.size(), parameter_names.size()); + EXPECT_EQ(parameters[0].as_bool(), true); + EXPECT_EQ(parameters[1].as_int(), 42); + + // Get multiple parameters at a time with map + std::map parameter_map; + EXPECT_TRUE(test_node->get_parameters({}, parameter_map)); + + EXPECT_EQ(parameter_map.size(), parameter_names.size() + builtin_param_count); + + // Check parameter types + auto parameter_types = test_node->get_parameter_types(parameter_names); + EXPECT_EQ(parameter_types.size(), parameter_names.size()); + EXPECT_EQ(parameter_types[0], rcl_interfaces::msg::ParameterType::PARAMETER_BOOL); + EXPECT_EQ(parameter_types[1], rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER); + + // Setting parameters + size_t parameters_set = 0; + auto callback = [¶meters_set](const std::vector & parameters) { + parameters_set += parameters.size(); + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + return result; + }; + + // Hold callback handle. Callback is valid during the lifetime of this object. + auto callback_handle = test_node->add_on_set_parameters_callback(callback); + rclcpp::Parameter bool_parameter(bool_name, rclcpp::ParameterValue(false)); + EXPECT_TRUE(test_node->set_parameter(bool_parameter).successful); + EXPECT_EQ(parameters_set, 1u); + + rclcpp::Parameter int_parameter(int_name, rclcpp::ParameterValue(7)); + test_node->set_parameters({int_parameter}); + EXPECT_EQ(parameters_set, 2u); + + test_node->remove_on_set_parameters_callback(callback_handle.get()); + rclcpp::Parameter bool_parameter2(bool_name, rclcpp::ParameterValue(true)); + EXPECT_TRUE(test_node->set_parameter(bool_parameter2).successful); + EXPECT_EQ(parameters_set, 2u); + + + // List parameters + list_result = test_node->list_parameters({}, 0u); + EXPECT_EQ(list_result.names.size(), parameter_names.size() + builtin_param_count); + size_t index = 0; + EXPECT_STREQ(list_result.names[index++].c_str(), "start_type_description_service"); + EXPECT_STREQ(list_result.names[index++].c_str(), parameter_names[0].c_str()); + EXPECT_STREQ(list_result.names[index++].c_str(), parameter_names[1].c_str()); + EXPECT_STREQ(list_result.names[index++].c_str(), "use_sim_time"); + + // Undeclare parameter + test_node->undeclare_parameter(bool_name); + EXPECT_FALSE(test_node->has_parameter(bool_name)); + rclcpp::Parameter parameter; + EXPECT_FALSE(test_node->get_parameter(bool_name, parameter)); + + // Bool parameter has been undeclared, atomic setting should fail + parameters = { + rclcpp::Parameter(bool_name, rclcpp::ParameterValue(true)), + rclcpp::Parameter(int_name, rclcpp::ParameterValue(0))}; + EXPECT_THROW( + test_node->set_parameters_atomically(parameters), + rclcpp::exceptions::ParameterNotDeclaredException); + + // Since setting parameters failed, this should remain the same + EXPECT_EQ(test_node->get_parameter(int_name).as_int(), 7); + + // Bool parameter no longer exists, using "or" value + EXPECT_FALSE( + test_node->get_parameter_or( + bool_name, parameter, rclcpp::Parameter(bool_name, rclcpp::ParameterValue(true)))); + EXPECT_TRUE(parameter.as_bool()); +} + +TEST_F(TestDefaultStateMachine, test_getters) { + auto test_node = std::make_shared("testnode"); + auto options = test_node->get_node_options(); + EXPECT_EQ(0u, options.arguments().size()); + EXPECT_NE(nullptr, test_node->get_node_base_interface()); + EXPECT_NE(nullptr, test_node->get_node_clock_interface()); + EXPECT_NE(nullptr, test_node->get_node_graph_interface()); + EXPECT_NE(nullptr, test_node->get_node_logging_interface()); + EXPECT_NE(nullptr, test_node->get_node_time_source_interface()); + EXPECT_NE(nullptr, test_node->get_node_timers_interface()); + EXPECT_NE(nullptr, test_node->get_node_topics_interface()); + EXPECT_NE(nullptr, test_node->get_node_services_interface()); + EXPECT_NE(nullptr, test_node->get_node_parameters_interface()); + EXPECT_NE(nullptr, test_node->get_node_waitables_interface()); + EXPECT_NE(nullptr, test_node->get_graph_event()); + EXPECT_NE(nullptr, test_node->get_clock()); + EXPECT_LT(0u, test_node->now().nanoseconds()); + EXPECT_STREQ("testnode", test_node->get_logger().get_name()); + EXPECT_NE(nullptr, const_cast(test_node.get())->get_clock()); + EXPECT_NE(nullptr, test_node->get_node_type_descriptions_interface()); +} + +TEST_F(TestDefaultStateMachine, test_graph_topics) { + auto test_node = std::make_shared("testnode"); + auto names = test_node->get_node_names(); + + ASSERT_NE(names.end(), std::find(names.begin(), names.end(), std::string("/testnode"))); + + // Other topics may exist for an rclcpp::Node, but just checking the lifecycle one exists + ASSERT_TRUE(wait_for_topic(test_node, "/testnode/transition_event")); + auto topic_names_and_types = test_node->get_topic_names_and_types(); + EXPECT_STREQ( + topic_names_and_types["/testnode/transition_event"][0].c_str(), + "lifecycle_msgs/msg/TransitionEvent"); + + EXPECT_EQ(1u, test_node->count_publishers("/testnode/transition_event")); + EXPECT_EQ(0u, test_node->count_subscribers("/testnode/transition_event")); + + auto publishers_info = test_node->get_publishers_info_by_topic("/testnode/transition_event"); + EXPECT_EQ(1u, publishers_info.size()); + auto subscriptions_info = + test_node->get_subscriptions_info_by_topic("/testnode/transition_event"); + EXPECT_EQ(0u, subscriptions_info.size()); +} + +TEST_F(TestDefaultStateMachine, test_graph_services) { + auto test_node = std::make_shared("testnode"); + + // These are specific to lifecycle nodes, other services are provided by rclcpp::Node + ASSERT_TRUE(wait_for_service(test_node, "/testnode/change_state")); + ASSERT_TRUE(wait_for_service(test_node, "/testnode/get_available_states")); + ASSERT_TRUE(wait_for_service(test_node, "/testnode/get_available_transitions")); + ASSERT_TRUE(wait_for_service(test_node, "/testnode/get_state")); + ASSERT_TRUE(wait_for_service(test_node, "/testnode/get_transition_graph")); + + auto service_names_and_types = test_node->get_service_names_and_types(); + EXPECT_STREQ( + service_names_and_types["/testnode/change_state"][0].c_str(), + "lifecycle_msgs/srv/ChangeState"); + EXPECT_STREQ( + service_names_and_types["/testnode/get_available_states"][0].c_str(), + "lifecycle_msgs/srv/GetAvailableStates"); + EXPECT_STREQ( + service_names_and_types["/testnode/get_available_transitions"][0].c_str(), + "lifecycle_msgs/srv/GetAvailableTransitions"); + EXPECT_STREQ( + service_names_and_types["/testnode/get_state"][0].c_str(), + "lifecycle_msgs/srv/GetState"); + EXPECT_STREQ( + service_names_and_types["/testnode/get_transition_graph"][0].c_str(), + "lifecycle_msgs/srv/GetAvailableTransitions"); + + EXPECT_EQ(0u, test_node->count_clients("/testnode/change_state")); + EXPECT_EQ(0u, test_node->count_clients("/testnode/get_available_states")); + EXPECT_EQ(0u, test_node->count_clients("/testnode/get_available_transitions")); + EXPECT_EQ(0u, test_node->count_clients("/testnode/get_state")); + EXPECT_EQ(0u, test_node->count_clients("/testnode/get_transition_graph")); + EXPECT_EQ(1u, test_node->count_services("/testnode/change_state")); + EXPECT_EQ(1u, test_node->count_services("/testnode/get_available_states")); + EXPECT_EQ(1u, test_node->count_services("/testnode/get_available_transitions")); + EXPECT_EQ(1u, test_node->count_services("/testnode/get_state")); + EXPECT_EQ(1u, test_node->count_services("/testnode/get_transition_graph")); +} + +TEST_F(TestDefaultStateMachine, test_graph_services_by_node) { + auto test_node = std::make_shared("testnode"); + + // These are specific to lifecycle nodes, other services are provided by rclcpp::Node + ASSERT_TRUE(wait_for_service_by_node(test_node, "testnode", "/testnode/change_state")); + ASSERT_TRUE(wait_for_service_by_node(test_node, "testnode", "/testnode/get_available_states")); + ASSERT_TRUE( + wait_for_service_by_node(test_node, "testnode", "/testnode/get_available_transitions")); + ASSERT_TRUE(wait_for_service_by_node(test_node, "testnode", "/testnode/get_state")); + ASSERT_TRUE(wait_for_service_by_node(test_node, "testnode", "/testnode/get_transition_graph")); + + auto service_names_and_types_by_node = + test_node->get_service_names_and_types_by_node("testnode", ""); + EXPECT_STREQ( + service_names_and_types_by_node["/testnode/change_state"][0].c_str(), + "lifecycle_msgs/srv/ChangeState"); + EXPECT_STREQ( + service_names_and_types_by_node["/testnode/get_available_states"][0].c_str(), + "lifecycle_msgs/srv/GetAvailableStates"); + EXPECT_STREQ( + service_names_and_types_by_node["/testnode/get_available_transitions"][0].c_str(), + "lifecycle_msgs/srv/GetAvailableTransitions"); + EXPECT_STREQ( + service_names_and_types_by_node["/testnode/get_state"][0].c_str(), + "lifecycle_msgs/srv/GetState"); + EXPECT_STREQ( + service_names_and_types_by_node["/testnode/get_transition_graph"][0].c_str(), + "lifecycle_msgs/srv/GetAvailableTransitions"); +} + +TEST_F(TestDefaultStateMachine, test_callback_groups) { + auto test_node = std::make_shared("testnode"); + size_t num_groups = 0; + test_node->for_each_callback_group( + [&num_groups](rclcpp::CallbackGroup::SharedPtr group_ptr) + { + (void)group_ptr; + num_groups++; + }); + EXPECT_EQ(num_groups, 1u); + + auto group = test_node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, true); + EXPECT_NE(nullptr, group); + + num_groups = 0; + test_node->for_each_callback_group( + [&num_groups](rclcpp::CallbackGroup::SharedPtr group_ptr) + { + (void)group_ptr; + num_groups++; + }); + EXPECT_EQ(num_groups, 2u); +} + +TEST_F(TestDefaultStateMachine, wait_for_graph_change) +{ + auto test_node = std::make_shared("testnode"); + EXPECT_THROW( + test_node->wait_for_graph_change(nullptr, std::chrono::milliseconds(1)), + rclcpp::exceptions::InvalidEventError); + + auto event = std::make_shared(); + EXPECT_THROW( + test_node->wait_for_graph_change(event, std::chrono::milliseconds(0)), + rclcpp::exceptions::EventNotRegisteredError); } diff --git a/rclcpp_lifecycle/test/test_lifecycle_node_errors.cpp b/rclcpp_lifecycle/test/test_lifecycle_node_errors.cpp new file mode 100644 index 0000000000..0653b813b9 --- /dev/null +++ b/rclcpp_lifecycle/test/test_lifecycle_node_errors.cpp @@ -0,0 +1,69 @@ +// Copyright 2024 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_lifecycle/rcl_lifecycle.h" + +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "./mocking_utils/patch.hpp" + +class TestDefaultStateMachine : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + static void TearDownTestCase() + { + rclcpp::shutdown(); + } +}; + +class EmptyLifecycleNode : public rclcpp_lifecycle::LifecycleNode +{ +public: + explicit EmptyLifecycleNode(const std::string & node_name) + : rclcpp_lifecycle::LifecycleNode(node_name) + {} +}; + +// This test is split out of test_lifecycle_node.cpp for an esoteric reason. When running on +// RedHat-based distributions (like Fedora or RHEL), the way that glibc is compiled does not +// allow mocking_utils::inject_on_return to work. Thus the test has to patch_and_return(). +// Unfortunately, this means that the resources are not actually cleaned up, and thus other tests +// may return incorrect results. By having it in a separate process we ensure that the resources +// will at least be cleaned up by the process dying. +TEST_F(TestDefaultStateMachine, empty_initializer_rcl_errors) +{ + { + auto patch = mocking_utils::patch_and_return( + "lib:rclcpp_lifecycle", rcl_lifecycle_state_machine_init, RCL_RET_ERROR); + EXPECT_THROW( + std::make_shared("testnode").reset(), + std::runtime_error); + } + { + auto test_node = std::make_shared("testnode"); + auto patch = mocking_utils::patch_and_return( + "lib:rclcpp_lifecycle", rcl_lifecycle_state_machine_fini, RCL_RET_ERROR); + EXPECT_NO_THROW(test_node.reset()); + } +} diff --git a/rclcpp_lifecycle/test/test_lifecycle_publisher.cpp b/rclcpp_lifecycle/test/test_lifecycle_publisher.cpp new file mode 100644 index 0000000000..b98de77238 --- /dev/null +++ b/rclcpp_lifecycle/test/test_lifecycle_publisher.cpp @@ -0,0 +1,196 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + + +#include +#include +#include +#include + +#include "lifecycle_msgs/msg/state.hpp" +#include "lifecycle_msgs/msg/transition.hpp" + +#include "test_msgs/msg/empty.hpp" + +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "rclcpp_lifecycle/lifecycle_publisher.hpp" + +using lifecycle_msgs::msg::State; +using lifecycle_msgs::msg::Transition; + +class TestDefaultStateMachine : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + static void TearDownTestCase() + { + rclcpp::shutdown(); + } +}; + +/// We want to test everything for both the wall and generic timer. +enum class TimerType +{ + WALL_TIMER, + GENERIC_TIMER, +}; + +class EmptyLifecycleNode : public rclcpp_lifecycle::LifecycleNode +{ +public: + explicit EmptyLifecycleNode(const std::string & node_name, const TimerType & timer_type) + : rclcpp_lifecycle::LifecycleNode(node_name) + { + // For coverage this is being added here + switch (timer_type) { + case TimerType::WALL_TIMER: + { + auto timer = create_wall_timer(std::chrono::seconds(1), []() {}); + add_timer_handle(timer); + break; + } + case TimerType::GENERIC_TIMER: + { + auto timer = create_timer(std::chrono::seconds(1), []() {}); + add_timer_handle(timer); + break; + } + } + } +}; + +class TestLifecyclePublisher : public ::testing::TestWithParam +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + } + + void TearDown() + { + rclcpp::shutdown(); + } +}; + +TEST_P(TestLifecyclePublisher, publish_managed_by_node) { + auto node = std::make_shared("node", GetParam()); + + rclcpp::PublisherOptionsWithAllocator> options; + std::shared_ptr> publisher = + node->create_publisher(std::string("topic"), rclcpp::QoS(10), options); + + // transition via LifecycleNode + auto success = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + auto reset_key = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + auto ret = reset_key; + + EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, node->get_current_state().id()); + node->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE), ret); + ASSERT_EQ(success, ret); + ret = reset_key; + node->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_ACTIVATE), ret); + ASSERT_EQ(success, ret); + ret = reset_key; + EXPECT_TRUE(publisher->is_activated()); + { + auto msg_ptr = std::make_unique(); + EXPECT_NO_THROW(publisher->publish(*msg_ptr)); + } + { + auto msg_ptr = std::make_unique(); + EXPECT_NO_THROW(publisher->publish(std::move(msg_ptr))); + } + { + auto loaned_msg = publisher->borrow_loaned_message(); + EXPECT_NO_THROW(publisher->publish(std::move(loaned_msg))); + } + node->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_DEACTIVATE), ret); + ASSERT_EQ(success, ret); + ret = reset_key; + (void)ret; // Just to make clang happy + EXPECT_FALSE(publisher->is_activated()); + { + auto msg_ptr = std::make_unique(); + EXPECT_NO_THROW(publisher->publish(*msg_ptr)); + } + { + auto msg_ptr = std::make_unique(); + EXPECT_NO_THROW(publisher->publish(std::move(msg_ptr))); + } + { + auto loaned_msg = publisher->borrow_loaned_message(); + EXPECT_NO_THROW(publisher->publish(std::move(loaned_msg))); + } +} + +TEST_P(TestLifecyclePublisher, publish) { + auto node = std::make_shared("node", GetParam()); + + rclcpp::PublisherOptionsWithAllocator> options; + std::shared_ptr> publisher = + node->create_publisher(std::string("topic"), rclcpp::QoS(10), options); + + // transition via LifecyclePublisher + publisher->on_deactivate(); + EXPECT_FALSE(publisher->is_activated()); + { + auto msg_ptr = std::make_unique(); + EXPECT_NO_THROW(publisher->publish(*msg_ptr)); + } + { + auto msg_ptr = std::make_unique(); + EXPECT_NO_THROW(publisher->publish(std::move(msg_ptr))); + } + { + auto loaned_msg = publisher->borrow_loaned_message(); + EXPECT_NO_THROW(publisher->publish(std::move(loaned_msg))); + } + publisher->on_activate(); + EXPECT_TRUE(publisher->is_activated()); + { + auto msg_ptr = std::make_unique(); + EXPECT_NO_THROW(publisher->publish(*msg_ptr)); + } + { + auto msg_ptr = std::make_unique(); + EXPECT_NO_THROW(publisher->publish(std::move(msg_ptr))); + } + { + auto loaned_msg = publisher->borrow_loaned_message(); + EXPECT_NO_THROW(publisher->publish(std::move(loaned_msg))); + } +} + +INSTANTIATE_TEST_SUITE_P( + PerTimerType, TestLifecyclePublisher, + ::testing::Values(TimerType::WALL_TIMER, TimerType::GENERIC_TIMER), + [](const ::testing::TestParamInfo & info) -> std::string { + switch (info.param) { + case TimerType::WALL_TIMER: + return std::string("wall_timer"); + case TimerType::GENERIC_TIMER: + return std::string("generic_timer"); + default: + break; + } + return std::string("unknown"); + } +); diff --git a/rclcpp_lifecycle/test/test_lifecycle_service_client.cpp b/rclcpp_lifecycle/test/test_lifecycle_service_client.cpp new file mode 100644 index 0000000000..fc3f86e573 --- /dev/null +++ b/rclcpp_lifecycle/test/test_lifecycle_service_client.cpp @@ -0,0 +1,488 @@ +// 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. + +/** + * Service client test was adopted from: + * https://github.com/ros2/demos/blob/master/lifecycle/src/lifecycle_service_client.cpp + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "lifecycle_msgs/msg/state.hpp" +#include "lifecycle_msgs/msg/transition.hpp" +#include "lifecycle_msgs/msg/transition_description.hpp" +#include "lifecycle_msgs/srv/change_state.hpp" +#include "lifecycle_msgs/srv/get_available_states.hpp" +#include "lifecycle_msgs/srv/get_available_transitions.hpp" +#include "lifecycle_msgs/srv/get_state.hpp" + +#include "rcl_lifecycle/rcl_lifecycle.h" + +#include "rclcpp/node_interfaces/node_graph.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "rcpputils/scope_exit.hpp" + +#include "./mocking_utils/patch.hpp" + +using namespace std::chrono_literals; + +constexpr char const * lifecycle_node_name = "lc_talker"; + +constexpr char const * node_get_state_topic = "/lc_talker/get_state"; +constexpr char const * node_change_state_topic = "/lc_talker/change_state"; +constexpr char const * node_get_available_states_topic = "/lc_talker/get_available_states"; +constexpr char const * node_get_available_transitions_topic = + "/lc_talker/get_available_transitions"; +constexpr char const * node_get_transition_graph_topic = + "/lc_talker/get_transition_graph"; +const lifecycle_msgs::msg::State unknown_state = lifecycle_msgs::msg::State(); + +class EmptyLifecycleNode : public rclcpp_lifecycle::LifecycleNode +{ +public: + EmptyLifecycleNode() + : rclcpp_lifecycle::LifecycleNode(lifecycle_node_name) + {} +}; + +class LifecycleServiceClient : public rclcpp::Node +{ +public: + explicit LifecycleServiceClient(std::string node_name) + : Node(node_name) + { + client_get_available_states_ = this->create_client( + node_get_available_states_topic); + client_get_available_transitions_ = + this->create_client( + node_get_available_transitions_topic); + client_get_transition_graph_ = + this->create_client( + node_get_transition_graph_topic); + client_get_state_ = this->create_client( + node_get_state_topic); + client_change_state_ = this->create_client( + node_change_state_topic); + } + + lifecycle_msgs::msg::State + get_state(std::chrono::seconds time_out = 1s) + { + auto request = std::make_shared(); + + if (!client_get_state_->wait_for_service(time_out)) { + return unknown_state; + } + + auto future_result = client_get_state_->async_send_request(request); + auto future_status = future_result.wait_for(time_out); + + if (future_status != std::future_status::ready) { + return unknown_state; + } + + auto result = future_result.get(); + if (result) { + return result->current_state; + } else { + return unknown_state; + } + } + + bool + change_state(std::uint8_t transition, std::chrono::seconds time_out = 1s) + { + auto request = std::make_shared(); + request->transition.id = transition; + + if (!client_change_state_->wait_for_service(time_out)) { + return false; + } + + auto future_result = client_change_state_->async_send_request(request); + auto future_status = future_result.wait_for(time_out); + + if (future_status != std::future_status::ready) { + return false; + } + + return future_result.get()->success; + } + + std::vector + get_available_states(std::chrono::seconds time_out = 1s) + { + auto request = std::make_shared(); + + if (!client_get_available_states_->wait_for_service(time_out)) { + return std::vector(); + } + + auto future_result = client_get_available_states_->async_send_request(request); + auto future_status = future_result.wait_for(time_out); + + if (future_status != std::future_status::ready) { + return std::vector(); + } + auto result = future_result.get(); + if (result) { + return result->available_states; + } + + return std::vector(); + } + + std::vector + get_available_transitions(std::chrono::seconds time_out = 1s) + { + auto request = std::make_shared(); + + if (!client_get_available_transitions_->wait_for_service(time_out)) { + return std::vector(); + } + + auto future_result = client_get_available_transitions_->async_send_request(request); + auto future_status = future_result.wait_for(time_out); + + if (future_status != std::future_status::ready) { + return std::vector(); + } + + auto result = future_result.get(); + if (result) { + return result->available_transitions; + } + + return std::vector(); + } + + std::vector + get_transition_graph(std::chrono::seconds time_out = 1s) + { + auto request = std::make_shared(); + + if (!client_get_transition_graph_->wait_for_service(time_out)) { + return std::vector(); + } + + auto future_result = client_get_transition_graph_->async_send_request(request); + auto future_status = future_result.wait_for(time_out); + + if (future_status != std::future_status::ready) { + return std::vector(); + } + + auto result = future_result.get(); + if (result) { + return result->available_transitions; + } + + return std::vector(); + } + +private: + std::shared_ptr> + client_get_available_states_; + std::shared_ptr> + client_get_available_transitions_; + std::shared_ptr> + client_get_transition_graph_; + std::shared_ptr> client_get_state_; + std::shared_ptr> client_change_state_; +}; + + +class TestLifecycleServiceClient : public ::testing::Test +{ +protected: + EmptyLifecycleNode * lifecycle_node() {return lifecycle_node_.get();} + LifecycleServiceClient * lifecycle_client() {return lifecycle_client_.get();} + +private: + void SetUp() override + { + rclcpp::init(0, nullptr); + lifecycle_node_ = std::make_shared(); + lifecycle_client_ = std::make_shared("client"); + spinner_ = std::thread(&TestLifecycleServiceClient::spin, this); + } + + void TearDown() override + { + { + std::lock_guard guard(shutdown_mutex_); + rclcpp::shutdown(); + } + spinner_.join(); + } + + void spin() + { + while (true) { + { + std::lock_guard guard(shutdown_mutex_); + if (!rclcpp::ok()) { + break; + } + rclcpp::spin_some(lifecycle_node_->get_node_base_interface()); + rclcpp::spin_some(lifecycle_client_); + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + } + + std::shared_ptr lifecycle_node_; + std::shared_ptr lifecycle_client_; + std::mutex shutdown_mutex_; + std::thread spinner_; +}; + + +TEST_F(TestLifecycleServiceClient, construct_destruct) { + EXPECT_NE(nullptr, lifecycle_client()); + EXPECT_NE(nullptr, lifecycle_node()); +} + +TEST_F(TestLifecycleServiceClient, available_states) { + auto states = lifecycle_client()->get_available_states(); + EXPECT_EQ(states.size(), 11u); + EXPECT_EQ(states[0].id, lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN); + EXPECT_EQ(states[1].id, lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + EXPECT_EQ(states[2].id, lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ(states[3].id, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + EXPECT_EQ(states[4].id, lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); + EXPECT_EQ(states[5].id, lifecycle_msgs::msg::State::TRANSITION_STATE_CONFIGURING); + EXPECT_EQ(states[6].id, lifecycle_msgs::msg::State::TRANSITION_STATE_CLEANINGUP); + EXPECT_EQ(states[7].id, lifecycle_msgs::msg::State::TRANSITION_STATE_SHUTTINGDOWN); + EXPECT_EQ(states[8].id, lifecycle_msgs::msg::State::TRANSITION_STATE_ACTIVATING); + EXPECT_EQ(states[9].id, lifecycle_msgs::msg::State::TRANSITION_STATE_DEACTIVATING); + EXPECT_EQ(states[10].id, lifecycle_msgs::msg::State::TRANSITION_STATE_ERRORPROCESSING); +} + +TEST_F(TestLifecycleServiceClient, transition_graph) { + auto transitions = lifecycle_client()->get_transition_graph(); + EXPECT_EQ(transitions.size(), 25u); +} + +TEST_F(TestLifecycleServiceClient, available_transitions) { + auto transitions = lifecycle_client()->get_available_transitions(); + EXPECT_EQ(transitions.size(), 2u); + EXPECT_EQ(transitions[0].transition.id, lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); + EXPECT_EQ( + transitions[1].transition.id, + lifecycle_msgs::msg::Transition::TRANSITION_UNCONFIGURED_SHUTDOWN); +} + +TEST_F(TestLifecycleServiceClient, lifecycle_transitions) { + EXPECT_EQ( + lifecycle_client()->get_state().id, lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + + auto transitions = lifecycle_client()->get_available_transitions(); + EXPECT_EQ(transitions.size(), 2u); + EXPECT_EQ(transitions[0].transition.id, lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); + EXPECT_EQ( + transitions[1].transition.id, + lifecycle_msgs::msg::Transition::TRANSITION_UNCONFIGURED_SHUTDOWN); + + EXPECT_TRUE( + lifecycle_client()->change_state( + lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE)); + EXPECT_EQ( + lifecycle_client()->get_state().id, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + transitions = lifecycle_client()->get_available_transitions(); + EXPECT_EQ(transitions.size(), 3u); + EXPECT_EQ(transitions[0].transition.id, lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP); + EXPECT_EQ(transitions[1].transition.id, lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); + EXPECT_EQ( + transitions[2].transition.id, + lifecycle_msgs::msg::Transition::TRANSITION_INACTIVE_SHUTDOWN); + + EXPECT_TRUE( + lifecycle_client()->change_state( + lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE)); + EXPECT_EQ(lifecycle_client()->get_state().id, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + transitions = lifecycle_client()->get_available_transitions(); + EXPECT_EQ(transitions.size(), 2u); + EXPECT_EQ(transitions[0].transition.id, lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE); + EXPECT_EQ( + transitions[1].transition.id, + lifecycle_msgs::msg::Transition::TRANSITION_ACTIVE_SHUTDOWN); + + EXPECT_TRUE( + lifecycle_client()->change_state( + lifecycle_msgs::msg::Transition:: + TRANSITION_DEACTIVATE)); + EXPECT_EQ( + lifecycle_client()->get_state().id, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + transitions = lifecycle_client()->get_available_transitions(); + EXPECT_EQ(transitions.size(), 3u); + EXPECT_EQ(transitions[0].transition.id, lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP); + EXPECT_EQ(transitions[1].transition.id, lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); + EXPECT_EQ( + transitions[2].transition.id, + lifecycle_msgs::msg::Transition::TRANSITION_INACTIVE_SHUTDOWN); + + EXPECT_TRUE( + lifecycle_client()->change_state( + lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP)); + EXPECT_EQ( + lifecycle_client()->get_state().id, lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + transitions = lifecycle_client()->get_available_transitions(); + EXPECT_EQ(transitions.size(), 2u); + EXPECT_EQ(transitions[0].transition.id, lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); + EXPECT_EQ( + transitions[1].transition.id, + lifecycle_msgs::msg::Transition::TRANSITION_UNCONFIGURED_SHUTDOWN); + + EXPECT_TRUE( + lifecycle_client()->change_state( + lifecycle_msgs::msg::Transition:: + TRANSITION_UNCONFIGURED_SHUTDOWN)); + EXPECT_EQ( + lifecycle_client()->get_state().id, + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); + transitions = lifecycle_client()->get_available_transitions(); + EXPECT_EQ(transitions.size(), 0u); +} + +TEST_F(TestLifecycleServiceClient, get_service_names_and_types_by_node) +{ + EXPECT_THROW( + lifecycle_node()->get_service_names_and_types_by_node("not_a_node", "not_absolute_namespace"), + std::runtime_error); + auto service_names_and_types = + lifecycle_node()->get_service_names_and_types_by_node(lifecycle_node_name, "/"); + auto start = std::chrono::steady_clock::now(); + while (0 == service_names_and_types.size() || + (std::chrono::steady_clock::now() - start) < std::chrono::seconds(1)) + { + service_names_and_types = + lifecycle_node()->get_service_names_and_types_by_node(lifecycle_node_name, "/"); + } + const std::array services = { + std::make_pair(node_get_state_topic, "lifecycle_msgs/srv/GetState"), + std::make_pair(node_change_state_topic, "lifecycle_msgs/srv/ChangeState"), + std::make_pair(node_get_available_states_topic, "lifecycle_msgs/srv/GetAvailableStates"), + std::make_pair( + node_get_available_transitions_topic, "lifecycle_msgs/srv/GetAvailableTransitions"), + std::make_pair(node_get_transition_graph_topic, "lifecycle_msgs/srv/GetAvailableTransitions"), + }; + for (const auto & [service_name, service_type] : services) { + ASSERT_TRUE(service_names_and_types.find(service_name) != service_names_and_types.end()) + << service_name; + const auto service_types = service_names_and_types.at(service_name); + EXPECT_TRUE( + std::find(service_types.cbegin(), service_types.cend(), service_type) != service_types.cend()) + << service_name; + } +} + +TEST_F(TestLifecycleServiceClient, declare_parameter_with_no_initial_values) +{ + auto node1 = std::make_shared("client1"); + + auto on_set_parameters = + [](const std::vector &) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + return result; + }; + + auto handler = node1->add_on_set_parameters_callback(on_set_parameters); + RCPPUTILS_SCOPE_EXIT( + {node1->remove_on_set_parameters_callback(handler.get());}); // always reset +} + +TEST_F(TestLifecycleServiceClient, wait_for_graph_change) +{ + auto node = std::make_shared("client_wait_for_graph_change"); + auto node_graph = node->get_node_graph_interface(); + ASSERT_NE(nullptr, node_graph); + + EXPECT_NO_THROW(node_graph->notify_graph_change()); + EXPECT_THROW( + node_graph->wait_for_graph_change(nullptr, std::chrono::milliseconds(1)), + rclcpp::exceptions::InvalidEventError); + + auto event = std::make_shared(); + EXPECT_THROW( + node_graph->wait_for_graph_change(event, std::chrono::milliseconds(0)), + rclcpp::exceptions::EventNotRegisteredError); +} + +class TestLifecycleServiceClientRCLErrors : public ::testing::Test +{ +protected: + void SetUp() override + { + rclcpp::init(0, nullptr); + } + + void TearDown() override + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestLifecycleServiceClientRCLErrors, call_services_rcl_errors) { + auto lifecycle_node = std::make_shared(); + auto lifecycle_client = std::make_shared("client_with_errors"); + + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_lifecycle", rcl_lifecycle_state_machine_is_initialized, RCL_RET_ERROR); + + // on_change_state + lifecycle_client->change_state( + lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); + rclcpp::spin_some(lifecycle_client); + EXPECT_THROW( + rclcpp::spin_some(lifecycle_node->get_node_base_interface()), std::runtime_error); + + // on_get_state + lifecycle_client->get_state(); + rclcpp::spin_some(lifecycle_client); + EXPECT_THROW( + rclcpp::spin_some(lifecycle_node->get_node_base_interface()), std::runtime_error); + + // on_get_avilable_states + lifecycle_client->get_available_states(); + rclcpp::spin_some(lifecycle_client); + EXPECT_THROW( + rclcpp::spin_some(lifecycle_node->get_node_base_interface()), std::runtime_error); + + // on_get_available_transitions + lifecycle_client->get_available_transitions(); + rclcpp::spin_some(lifecycle_client); + EXPECT_THROW( + rclcpp::spin_some(lifecycle_node->get_node_base_interface()), std::runtime_error); + + // on_get_transition_graph + lifecycle_client->get_transition_graph(); + rclcpp::spin_some(lifecycle_client); + EXPECT_THROW( + rclcpp::spin_some(lifecycle_node->get_node_base_interface()), std::runtime_error); +} diff --git a/rclcpp_lifecycle/test/test_register_custom_callbacks.cpp b/rclcpp_lifecycle/test/test_register_custom_callbacks.cpp index 8168914a8b..731e89234a 100644 --- a/rclcpp_lifecycle/test/test_register_custom_callbacks.cpp +++ b/rclcpp_lifecycle/test/test_register_custom_callbacks.cpp @@ -34,6 +34,11 @@ class TestRegisterCustomCallbacks : public ::testing::Test { rclcpp::init(0, nullptr); } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } }; class CustomLifecycleNode : public rclcpp_lifecycle::LifecycleNode @@ -46,91 +51,91 @@ class CustomLifecycleNode : public rclcpp_lifecycle::LifecycleNode size_t number_of_callbacks = 0; protected: - rcl_lifecycle_transition_key_t + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure(const rclcpp_lifecycle::State &) { ADD_FAILURE(); ++number_of_callbacks; - return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS; + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } - rcl_lifecycle_transition_key_t + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate(const rclcpp_lifecycle::State &) { ADD_FAILURE(); ++number_of_callbacks; - return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS; + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } - rcl_lifecycle_transition_key_t + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) { ADD_FAILURE(); ++number_of_callbacks; - return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS; + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } - rcl_lifecycle_transition_key_t + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &) { ADD_FAILURE(); ++number_of_callbacks; - return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS; + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } - rcl_lifecycle_transition_key_t + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_shutdown(const rclcpp_lifecycle::State &) { ADD_FAILURE(); ++number_of_callbacks; - return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS; + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } // Custom callbacks public: - rcl_lifecycle_transition_key_t + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_custom_configure(const rclcpp_lifecycle::State & previous_state) { EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, previous_state.id()); EXPECT_EQ(State::TRANSITION_STATE_CONFIGURING, get_current_state().id()); ++number_of_callbacks; - return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS; + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } - rcl_lifecycle_transition_key_t + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_custom_activate(const rclcpp_lifecycle::State & previous_state) { EXPECT_EQ(State::PRIMARY_STATE_INACTIVE, previous_state.id()); EXPECT_EQ(State::TRANSITION_STATE_ACTIVATING, get_current_state().id()); ++number_of_callbacks; - return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS; + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } - rcl_lifecycle_transition_key_t + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_custom_deactivate(const rclcpp_lifecycle::State & previous_state) { EXPECT_EQ(State::PRIMARY_STATE_ACTIVE, previous_state.id()); EXPECT_EQ(State::TRANSITION_STATE_DEACTIVATING, get_current_state().id()); ++number_of_callbacks; - return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS; + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } - rcl_lifecycle_transition_key_t + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_custom_cleanup(const rclcpp_lifecycle::State & previous_state) { EXPECT_EQ(State::PRIMARY_STATE_INACTIVE, previous_state.id()); EXPECT_EQ(State::TRANSITION_STATE_CLEANINGUP, get_current_state().id()); ++number_of_callbacks; - return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS; + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } - rcl_lifecycle_transition_key_t + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_custom_shutdown(const rclcpp_lifecycle::State &) { EXPECT_EQ(State::TRANSITION_STATE_SHUTTINGDOWN, get_current_state().id()); ++number_of_callbacks; - return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS; + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } }; @@ -138,28 +143,43 @@ TEST_F(TestRegisterCustomCallbacks, custom_callbacks) { auto test_node = std::make_shared("testnode"); test_node->register_on_configure( - std::bind(&CustomLifecycleNode::on_custom_configure, test_node, std::placeholders::_1)); - test_node->register_on_cleanup(std::bind(&CustomLifecycleNode::on_custom_cleanup, test_node, - std::placeholders::_1)); - test_node->register_on_shutdown(std::bind(&CustomLifecycleNode::on_custom_shutdown, test_node, - std::placeholders::_1)); - test_node->register_on_activate(std::bind(&CustomLifecycleNode::on_custom_activate, test_node, - std::placeholders::_1)); - test_node->register_on_deactivate(std::bind(&CustomLifecycleNode::on_custom_deactivate, - test_node, std::placeholders::_1)); + std::bind( + &CustomLifecycleNode::on_custom_configure, + test_node.get(), std::placeholders::_1)); + test_node->register_on_cleanup( + std::bind( + &CustomLifecycleNode::on_custom_cleanup, + test_node.get(), std::placeholders::_1)); + test_node->register_on_shutdown( + std::bind( + &CustomLifecycleNode::on_custom_shutdown, + test_node.get(), std::placeholders::_1)); + test_node->register_on_activate( + std::bind( + &CustomLifecycleNode::on_custom_activate, + test_node.get(), std::placeholders::_1)); + test_node->register_on_deactivate( + std::bind( + &CustomLifecycleNode::on_custom_deactivate, + test_node.get(), std::placeholders::_1)); EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); - EXPECT_EQ(State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( + EXPECT_EQ( + State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); - EXPECT_EQ(State::PRIMARY_STATE_ACTIVE, test_node->trigger_transition( + EXPECT_EQ( + State::PRIMARY_STATE_ACTIVE, test_node->trigger_transition( rclcpp_lifecycle::Transition(Transition::TRANSITION_ACTIVATE)).id()); - EXPECT_EQ(State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( + EXPECT_EQ( + State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( rclcpp_lifecycle::Transition(Transition::TRANSITION_DEACTIVATE)).id()); - EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition( + EXPECT_EQ( + State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition( rclcpp_lifecycle::Transition(Transition::TRANSITION_CLEANUP)).id()); - EXPECT_EQ(State::PRIMARY_STATE_FINALIZED, test_node->trigger_transition( - rclcpp_lifecycle::Transition(Transition::TRANSITION_SHUTDOWN)).id()); + EXPECT_EQ( + State::PRIMARY_STATE_FINALIZED, test_node->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_UNCONFIGURED_SHUTDOWN)).id()); // check if all callbacks were successfully overwritten - EXPECT_EQ(static_cast(5), test_node->number_of_callbacks); + EXPECT_EQ(5u, test_node->number_of_callbacks); } diff --git a/rclcpp/test/test_service.cpp b/rclcpp_lifecycle/test/test_service.cpp similarity index 54% rename from rclcpp/test/test_service.cpp rename to rclcpp_lifecycle/test/test_service.cpp index 4531d2b388..13bc25761d 100644 --- a/rclcpp/test/test_service.cpp +++ b/rclcpp_lifecycle/test/test_service.cpp @@ -1,4 +1,4 @@ -// Copyright 2017 Open Source Robotics Foundation, Inc. +// 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. @@ -16,11 +16,17 @@ #include #include +#include #include "rclcpp/exceptions.hpp" #include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "rcl_interfaces/srv/list_parameters.hpp" +#include "rmw/qos_profiles.h" + +#include "test_msgs/srv/empty.hpp" + +using namespace std::chrono_literals; class TestService : public ::testing::Test { @@ -30,9 +36,14 @@ class TestService : public ::testing::Test rclcpp::init(0, nullptr); } + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + void SetUp() { - node = std::make_shared("my_node", "/ns"); + node = std::make_shared("my_lifecycle_node", "/ns"); } void TearDown() @@ -40,24 +51,27 @@ class TestService : public ::testing::Test node.reset(); } - rclcpp::Node::SharedPtr node; + rclcpp_lifecycle::LifecycleNode::SharedPtr node; }; /* Testing service construction and destruction. */ TEST_F(TestService, construction_and_destruction) { - using rcl_interfaces::srv::ListParameters; + using test_msgs::srv::Empty; auto callback = - [](const ListParameters::Request::SharedPtr, ListParameters::Response::SharedPtr) { + [](const Empty::Request::SharedPtr, Empty::Response::SharedPtr) { }; { - auto service = node->create_service("service", callback); + auto service = node->create_service("service", callback); + EXPECT_NE(nullptr, service->get_service_handle()); + const rclcpp::ServiceBase * const_service_base = service.get(); + EXPECT_NE(nullptr, const_service_base->get_service_handle()); } - { - ASSERT_THROW({ - auto service = node->create_service("invalid_service?", callback); + ASSERT_THROW( + { + auto service = node->create_service("invalid_service?", callback); }, rclcpp::exceptions::InvalidServiceNameError); } } diff --git a/rclcpp_lifecycle/test/test_state_machine_info.cpp b/rclcpp_lifecycle/test/test_state_machine_info.cpp index b14441e8cf..addf7d12f1 100644 --- a/rclcpp_lifecycle/test/test_state_machine_info.cpp +++ b/rclcpp_lifecycle/test/test_state_machine_info.cpp @@ -18,9 +18,15 @@ #include #include +#include "lifecycle_msgs/msg/state.hpp" +#include "lifecycle_msgs/msg/transition.hpp" + #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" +using lifecycle_msgs::msg::State; +using lifecycle_msgs::msg::Transition; + class TestStateMachineInfo : public ::testing::Test { protected: @@ -28,6 +34,11 @@ class TestStateMachineInfo : public ::testing::Test { rclcpp::init(0, nullptr); } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } }; TEST_F(TestStateMachineInfo, available_states) { @@ -53,19 +64,107 @@ TEST_F(TestStateMachineInfo, available_states) { } TEST_F(TestStateMachineInfo, available_transitions) { + auto test_node = std::make_shared("testnode"); + std::vector current_available_transitions; + + // PRIMARY_STATE_UNCONFIGURED + current_available_transitions = test_node->get_available_transitions(); + EXPECT_EQ(2u, current_available_transitions.size()); + EXPECT_EQ( + current_available_transitions[0].id(), + Transition::TRANSITION_CONFIGURE); + EXPECT_EQ( + current_available_transitions[1].id(), + Transition::TRANSITION_UNCONFIGURED_SHUTDOWN); + current_available_transitions.clear(); + + // PRIMARY_STATE_INACTIVE + ASSERT_EQ( + State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); + current_available_transitions = test_node->get_available_transitions(); + EXPECT_EQ(3u, current_available_transitions.size()); + EXPECT_EQ( + current_available_transitions[0].id(), + Transition::TRANSITION_CLEANUP); + EXPECT_EQ( + current_available_transitions[1].id(), + Transition::TRANSITION_ACTIVATE); + EXPECT_EQ( + current_available_transitions[2].id(), + Transition::TRANSITION_INACTIVE_SHUTDOWN); + current_available_transitions.clear(); + + // PRIMARY_STATE_ACTIVE + ASSERT_EQ( + State::PRIMARY_STATE_ACTIVE, test_node->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_ACTIVATE)).id()); + current_available_transitions = test_node->get_available_transitions(); + EXPECT_EQ(2u, current_available_transitions.size()); + EXPECT_EQ( + current_available_transitions[0].id(), + Transition::TRANSITION_DEACTIVATE); + EXPECT_EQ( + current_available_transitions[1].id(), + Transition::TRANSITION_ACTIVE_SHUTDOWN); + current_available_transitions.clear(); + + // PRIMARY_STATE_INACTIVE + ASSERT_EQ( + State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_DEACTIVATE)).id()); + current_available_transitions = test_node->get_available_transitions(); + EXPECT_EQ(3u, current_available_transitions.size()); + EXPECT_EQ( + current_available_transitions[0].id(), + Transition::TRANSITION_CLEANUP); + EXPECT_EQ( + current_available_transitions[1].id(), + Transition::TRANSITION_ACTIVATE); + EXPECT_EQ( + current_available_transitions[2].id(), + Transition::TRANSITION_INACTIVE_SHUTDOWN); + current_available_transitions.clear(); + + // PRIMARY_STATE_UNCONFIGURED + ASSERT_EQ( + State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_CLEANUP)).id()); + current_available_transitions = test_node->get_available_transitions(); + EXPECT_EQ(2u, current_available_transitions.size()); + EXPECT_EQ( + current_available_transitions[0].id(), + Transition::TRANSITION_CONFIGURE); + EXPECT_EQ( + current_available_transitions[1].id(), + Transition::TRANSITION_UNCONFIGURED_SHUTDOWN); + current_available_transitions.clear(); + + // PRIMARY_STATE_FINALIZED + ASSERT_EQ( + State::PRIMARY_STATE_FINALIZED, test_node->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_UNCONFIGURED_SHUTDOWN)).id()); + current_available_transitions = test_node->get_available_transitions(); + EXPECT_EQ(0u, current_available_transitions.size()); + current_available_transitions.clear(); +} + +TEST_F(TestStateMachineInfo, transition_graph) { auto test_node = std::make_shared("testnode"); std::vector available_transitions = - test_node->get_available_transitions(); + test_node->get_transition_graph(); EXPECT_EQ(25u, available_transitions.size()); for (rclcpp_lifecycle::Transition & transition : available_transitions) { EXPECT_FALSE(transition.label().empty()); - EXPECT_TRUE(transition.start_state().id() <= 4 || + EXPECT_TRUE( + transition.start_state().id() <= 4 || (transition.start_state().id() >= 10 && (transition.start_state().id() <= 15))); EXPECT_FALSE(transition.start_state().label().empty()); - EXPECT_TRUE(transition.goal_state().id() <= 4 || + EXPECT_TRUE( + transition.goal_state().id() <= 4 || (transition.goal_state().id() >= 10 && (transition.goal_state().id() <= 15))); EXPECT_FALSE(transition.goal_state().label().empty()); diff --git a/rclcpp_lifecycle/test/test_state_wrapper.cpp b/rclcpp_lifecycle/test/test_state_wrapper.cpp index 393cef15e2..599ec9562e 100644 --- a/rclcpp_lifecycle/test/test_state_wrapper.cpp +++ b/rclcpp_lifecycle/test/test_state_wrapper.cpp @@ -28,6 +28,17 @@ class TestStateWrapper : public ::testing::Test } }; +class StateDerived : public rclcpp_lifecycle::State +{ +public: + StateDerived(uint8_t id, const std::string & label) + : State(id, label) {} + void expose_reset() + { + reset(); + } +}; + TEST_F(TestStateWrapper, wrapper) { { rclcpp_lifecycle::State state(1, "my_state"); @@ -44,7 +55,7 @@ TEST_F(TestStateWrapper, wrapper) { } { - rcl_lifecycle_state_t lc_state = {"my_c_state", 2, NULL, NULL, 0}; + rcl_lifecycle_state_t lc_state = {"my_c_state", 2, NULL, 0}; rclcpp_lifecycle::State c_state(lc_state.id, lc_state.label); EXPECT_EQ(2, c_state.id()); EXPECT_FALSE(c_state.label().empty()); @@ -52,7 +63,7 @@ TEST_F(TestStateWrapper, wrapper) { } { - rcl_lifecycle_state_t lc_state = {"my_c_state", 2, NULL, NULL, 0}; + rcl_lifecycle_state_t lc_state = {"my_c_state", 2, NULL, 0}; rclcpp_lifecycle::State c_state(&lc_state); EXPECT_EQ(2, c_state.id()); EXPECT_FALSE(c_state.label().empty()); @@ -61,7 +72,7 @@ TEST_F(TestStateWrapper, wrapper) { { rcl_lifecycle_state_t * lc_state = - new rcl_lifecycle_state_t {"my_c_state", 3, NULL, NULL, 0}; + new rcl_lifecycle_state_t {"my_c_state", 3, NULL, 0}; rclcpp_lifecycle::State c_state(lc_state->id, lc_state->label); EXPECT_EQ(3, c_state.id()); EXPECT_FALSE(c_state.label().empty()); @@ -95,6 +106,10 @@ TEST_F(TestStateWrapper, copy_constructor) { TEST_F(TestStateWrapper, assignment_operator) { auto a = std::make_shared(1, "one"); + *a = *a; + EXPECT_EQ(1, a->id()); + EXPECT_STREQ("one", a->label().c_str()); + auto b = std::make_shared(2, "two"); *b = *a; @@ -107,12 +122,12 @@ TEST_F(TestStateWrapper, assignment_operator) { TEST_F(TestStateWrapper, assignment_operator2) { // Non-owning State rcl_lifecycle_state_t * lc_state1 = - new rcl_lifecycle_state_t{"my_c_state1", 1, NULL, NULL, 0}; + new rcl_lifecycle_state_t{"my_c_state1", 1, NULL, 0}; auto non_owning_state1 = std::make_shared(lc_state1); // Non-owning State rcl_lifecycle_state_t * lc_state2 = - new rcl_lifecycle_state_t{"my_c_state2", 2, NULL, NULL, 0}; + new rcl_lifecycle_state_t{"my_c_state2", 2, NULL, 0}; auto non_owning_state2 = std::make_shared(lc_state2); *non_owning_state2 = *non_owning_state1; @@ -130,7 +145,7 @@ TEST_F(TestStateWrapper, assignment_operator2) { TEST_F(TestStateWrapper, assignment_operator3) { // Non-owning State rcl_lifecycle_state_t * lc_state1 = - new rcl_lifecycle_state_t{"my_c_state1", 1, NULL, NULL, 0}; + new rcl_lifecycle_state_t{"my_c_state1", 1, NULL, 0}; auto non_owning_state1 = std::make_shared(lc_state1); // owning State @@ -150,7 +165,7 @@ TEST_F(TestStateWrapper, assignment_operator3) { TEST_F(TestStateWrapper, assignment_operator4) { // Non-owning State rcl_lifecycle_state_t * lc_state1 = - new rcl_lifecycle_state_t{"my_c_state1", 1, NULL, NULL, 0}; + new rcl_lifecycle_state_t{"my_c_state1", 1, NULL, 0}; auto non_owning_state1 = std::make_shared(lc_state1); // owning State @@ -166,3 +181,16 @@ TEST_F(TestStateWrapper, assignment_operator4) { delete lc_state1; } + +TEST_F(TestStateWrapper, exceptions) { + EXPECT_THROW((void)rclcpp_lifecycle::State(0, ""), std::runtime_error); + + const rcl_lifecycle_state_t * null_handle = nullptr; + EXPECT_THROW((void)rclcpp_lifecycle::State(null_handle), std::runtime_error); + + auto reset_state = std::make_shared(1, "one"); + reset_state->expose_reset(); + + EXPECT_THROW(reset_state->id(), std::runtime_error); + EXPECT_THROW(reset_state->label(), std::runtime_error); +} diff --git a/rclcpp_lifecycle/test/test_transition_wrapper.cpp b/rclcpp_lifecycle/test/test_transition_wrapper.cpp index b920c74135..39791d5da2 100644 --- a/rclcpp_lifecycle/test/test_transition_wrapper.cpp +++ b/rclcpp_lifecycle/test/test_transition_wrapper.cpp @@ -18,8 +18,14 @@ #include #include +#include "rcl_lifecycle/rcl_lifecycle.h" + #include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "rcutils/testing/fault_injection.h" + +#include "./mocking_utils/patch.hpp" + class TestTransitionWrapper : public ::testing::Test { protected: @@ -28,6 +34,22 @@ class TestTransitionWrapper : public ::testing::Test } }; +class TransitionDerived : public rclcpp_lifecycle::Transition +{ +public: + TransitionDerived(const uint8_t id, const std::string & label) + : Transition(id, label) {} + + TransitionDerived( + const uint8_t id, const std::string & label, + rclcpp_lifecycle::State && start, rclcpp_lifecycle::State && goal) + : Transition(id, label, std::move(start), std::move(goal)) {} + void expose_reset() + { + reset(); + } +}; + TEST_F(TestTransitionWrapper, empty_transition) { auto a = std::make_shared(1, "my_transition"); EXPECT_NO_THROW(a.reset()); @@ -75,7 +97,15 @@ TEST_F(TestTransitionWrapper, copy_constructor) { } TEST_F(TestTransitionWrapper, assignment_operator) { - auto a = std::make_shared(1, "one"); + rclcpp_lifecycle::State start_state(1, "start_state"); + rclcpp_lifecycle::State goal_state(2, "goal_state"); + auto a = std::make_shared( + 1, "one", std::move(start_state), + std::move(goal_state)); + *a = *a; + EXPECT_EQ(1, a->id()); + EXPECT_STREQ("one", a->label().c_str()); + auto b = std::make_shared(2, "two"); *b = *a; @@ -83,4 +113,101 @@ TEST_F(TestTransitionWrapper, assignment_operator) { EXPECT_EQ(1, b->id()); EXPECT_STREQ("one", b->label().c_str()); + EXPECT_STREQ("start_state", b->start_state().label().c_str()); + EXPECT_STREQ("goal_state", b->goal_state().label().c_str()); + EXPECT_EQ(1, b->start_state().id()); + EXPECT_EQ(2, b->goal_state().id()); +} + +TEST_F(TestTransitionWrapper, exceptions) { + rcl_lifecycle_transition_t * null_handle = nullptr; + EXPECT_THROW((void)rclcpp_lifecycle::Transition(null_handle), std::runtime_error); + + rclcpp_lifecycle::State start_state(1, "start_state"); + rclcpp_lifecycle::State goal_state(2, "goal_state"); + auto a = std::make_shared( + 1, "one", std::move(start_state), + std::move(goal_state)); + + a->expose_reset(); + EXPECT_THROW(a->start_state(), std::runtime_error); + EXPECT_THROW(a->goal_state(), std::runtime_error); + EXPECT_THROW(a->id(), std::runtime_error); + EXPECT_THROW(a->label(), std::runtime_error); + + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_lifecycle", rcl_lifecycle_transition_init, RCL_RET_ERROR); + + EXPECT_THROW( + std::make_shared(1, "one").reset(), + std::runtime_error); + + rclcpp_lifecycle::State state1(1, "start_state"); + rclcpp_lifecycle::State state2(2, "goal_state"); + EXPECT_THROW( + std::make_shared( + 2, "two", std::move(start_state), std::move(goal_state)).reset(), + std::runtime_error); + } + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp_lifecycle", rcl_lifecycle_transition_fini, RCL_RET_ERROR); + auto transition1 = std::make_shared(1, "one"); + EXPECT_NO_THROW(transition1->expose_reset()); + + rclcpp_lifecycle::State state1(1, "start_state"); + rclcpp_lifecycle::State state2(2, "goal_state"); + auto transition2 = + std::make_shared(2, "two", std::move(start_state), std::move(goal_state)); + EXPECT_NO_THROW(transition2->expose_reset()); + } + + RCUTILS_FAULT_INJECTION_TEST( + { + std::shared_ptr transition = nullptr; + try { + transition = std::make_shared(1, "one"); + } catch (...) { + } + if (nullptr != transition) { + EXPECT_NO_THROW(transition->expose_reset()); + } + }); + + RCUTILS_FAULT_INJECTION_TEST( + { + std::shared_ptr transition = nullptr; + try { + { + // These will fail due to failed allocations + rclcpp_lifecycle::State state1(1, "start_state"); + rclcpp_lifecycle::State state2(2, "goal_state"); + + // Failed allocations and failed rcl init functions + transition = std::make_shared( + 2, "two", std::move(state1), std::move(state2)); + } + } catch (...) { + } + + if (nullptr != transition) { + EXPECT_NO_THROW(transition->expose_reset()); + } + }); + + RCUTILS_FAULT_INJECTION_TEST( + { + try { + // These will fail due to failed allocations + rclcpp_lifecycle::State state1(1, "start_state"); + rclcpp_lifecycle::State state2(2, "goal_state"); + + // Failed allocations and failed rcl init functions + auto a = std::make_shared(2, "two", std::move(state1), std::move(state2)); + auto b = std::make_shared(3, "three"); + *b = *a; + } catch (...) { + } + }); }