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/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/README.md b/README.md index 0226a75f0a..baf82c8520 100644 --- a/README.md +++ b/README.md @@ -8,7 +8,8 @@ rclcpp provides the standard C++ API for interacting with ROS 2. `#include "rclcpp/rclcpp.hpp"` allows use of the most common elements of the ROS 2 system. -Visit the [rclcpp API documentation](http://docs.ros2.org/latest/api/rclcpp/) for a complete list of its main components. +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 diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 350624dc87..07f7d2b4d2 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,686 @@ 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. diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index c1cb8bf3e2..8712b48856 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.20) project(rclcpp) @@ -10,11 +10,14 @@ 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(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) @@ -25,6 +28,7 @@ find_package(tracetools 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") # About -Wno-sign-conversion: With Clang, -Wconversion implies -Wsign-conversion. There are a number of @@ -41,24 +45,39 @@ 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/exceptions.cpp src/rclcpp/executable_list.cpp src/rclcpp/executor.cpp + src/rclcpp/executor_options.cpp src/rclcpp/executors.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_executor_entities_collector.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 @@ -80,6 +99,7 @@ set(${PROJECT_NAME}_SRCS 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 @@ -91,8 +111,9 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/parameter_value.cpp src/rclcpp/publisher_base.cpp src/rclcpp/qos.cpp - src/rclcpp/qos_event.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 @@ -109,6 +130,23 @@ set(${PROJECT_NAME}_SRCS 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" @@ -122,7 +160,7 @@ set(python_code_logging 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_logging}" + COMMAND Python3::Interpreter ARGS -c "${python_code_logging}" DEPENDS "${CMAKE_CURRENT_BINARY_DIR}/logging.hpp.em.watch" COMMENT "Expanding logging.hpp.em" VERBATIM @@ -146,7 +184,7 @@ foreach(interface_file ${interface_files}) 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 ${PYTHON_EXECUTABLE} ARGS -c "${python_${interface_name}_traits}" + 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 @@ -166,7 +204,7 @@ foreach(interface_file ${interface_files}) 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 ${PYTHON_EXECUTABLE} ARGS -c "${python_get_${interface_name}}" + 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 @@ -184,22 +222,29 @@ endif() target_include_directories(${PROJECT_NAME} PUBLIC "$" "$" - "$") -target_link_libraries(${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT}) -# specific order: dependents before dependencies -ament_target_dependencies(${PROJECT_NAME} - "ament_index_cpp" - "libstatistics_collector" - "rcl" - "rcl_yaml_param_parser" - "rcpputils" - "rcutils" - "builtin_interfaces" - "rosgraph_msgs" - "rosidl_typesupport_cpp" - "rosidl_runtime_cpp" - "statistics_msgs" - "tracetools" + "$") +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, @@ -214,24 +259,30 @@ install( RUNTIME DESTINATION bin ) -# specific order: dependents before dependencies -ament_export_include_directories(include) +# 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(ament_index_cpp) -ament_export_dependencies(libstatistics_collector) -ament_export_dependencies(rcl) -ament_export_dependencies(rcpputils) -ament_export_dependencies(rcutils) -ament_export_dependencies(builtin_interfaces) -ament_export_dependencies(rosgraph_msgs) -ament_export_dependencies(rosidl_typesupport_cpp) -ament_export_dependencies(rosidl_typesupport_c) -ament_export_dependencies(rosidl_runtime_cpp) -ament_export_dependencies(rcl_yaml_param_parser) -ament_export_dependencies(statistics_msgs) -ament_export_dependencies(tracetools) +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) @@ -244,10 +295,16 @@ 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 500) + 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 index e0913167c2..53c2908f7c 100644 --- a/rclcpp/QUALITY_DECLARATION.md +++ b/rclcpp/QUALITY_DECLARATION.md @@ -4,7 +4,7 @@ This document is a declaration of software quality for the `rclcpp` package, bas 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 Quality Categories in REP-2004](https://www.ros.org/reps/rep-2004.html) of the ROS2 developer guide. +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] @@ -94,7 +94,7 @@ There is an automated test which runs a linter that ensures each file has at lea ### 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/master/test) directory. +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: @@ -129,7 +129,7 @@ Current coverage statistics can be viewed [here](https://ci.ros2.org/job/nightly `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/master/rclcpp/test/benchmark). +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/) @@ -139,7 +139,7 @@ Changes that introduce regressions in performance must be adequately justified i ### Linters and Static Analysis [4.v] -`rclcpp` uses and passes all the ROS2 standard linters and static analysis tools for a C++ package as described in the [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#linters-and-static-analysis). Passing implies there are no linter/static errors when testing against CI of supported platforms. +`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/) @@ -163,49 +163,49 @@ It also has several test dependencies, which do not affect the resulting quality 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/master/QUALITY_DECLARATION.md). +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/master/rcl/QUALITY_DECLARATION.md). +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/master/rcl_yaml_param_parser/QUALITY_DECLARATION.md). +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/master/QUALITY_DECLARATION.md). +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/master/QUALITY_DECLARATION.md). +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/master/rmw/QUALITY_DECLARATION.md). +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/master/statistics_msgs/QUALITY_DECLARATION.md). +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://gitlab.com/ros-tracing/ros2_tracing/-/blob/master/tracetools/QUALITY_DECLARATION.md). +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] diff --git a/rclcpp/README.md b/rclcpp/README.md index 55c25f4780..8ec47772f1 100644 --- a/rclcpp/README.md +++ b/rclcpp/README.md @@ -2,7 +2,7 @@ The ROS client library in C++. -Visit the [rclcpp API documentation](http://docs.ros2.org/latest/api/rclcpp/) for a complete list of its main components and features. +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 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 d117376517..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) { @@ -73,6 +90,7 @@ 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; diff --git a/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp b/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp index cfc1e15922..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(); } diff --git a/rclcpp/include/rclcpp/any_executable.hpp b/rclcpp/include/rclcpp/any_executable.hpp index 5d4064f452..e4e9eaecb0 100644 --- a/rclcpp/include/rclcpp/any_executable.hpp +++ b/rclcpp/include/rclcpp/any_executable.hpp @@ -45,9 +45,9 @@ struct AnyExecutable rclcpp::ClientBase::SharedPtr client; rclcpp::Waitable::SharedPtr waitable; // These are used to keep the scope on the containing items - rclcpp::CallbackGroup::SharedPtr callback_group; - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base; - std::shared_ptr data; + rclcpp::CallbackGroup::SharedPtr callback_group {nullptr}; + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base {nullptr}; + std::shared_ptr data {nullptr}; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/any_service_callback.hpp b/rclcpp/include/rclcpp/any_service_callback.hpp index 56c07d4c66..5d6f3ee7b7 100644 --- a/rclcpp/include/rclcpp/any_service_callback.hpp +++ b/rclcpp/include/rclcpp/any_service_callback.hpp @@ -40,9 +40,14 @@ struct can_be_nullptr : std::false_type {}; // 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 // Forward declare @@ -151,7 +156,7 @@ class AnyServiceCallback const std::shared_ptr & request_header, std::shared_ptr request) { - TRACEPOINT(callback_start, static_cast(this), false); + 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. @@ -160,11 +165,13 @@ class AnyServiceCallback 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(); @@ -177,7 +184,7 @@ class AnyServiceCallback const auto & cb = std::get(callback_); cb(request_header, std::move(request), response); } - TRACEPOINT(callback_end, static_cast(this)); + TRACETOOLS_TRACEPOINT(callback_end, static_cast(this)); return response; } @@ -186,10 +193,14 @@ class AnyServiceCallback #ifndef TRACETOOLS_DISABLED std::visit( [this](auto && arg) { - TRACEPOINT( - rclcpp_callback_register, - static_cast(this), - tracetools::get_symbol(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 } diff --git a/rclcpp/include/rclcpp/any_subscription_callback.hpp b/rclcpp/include/rclcpp/any_subscription_callback.hpp index bb45d6a07d..da5abe6c53 100644 --- a/rclcpp/include/rclcpp/any_subscription_callback.hpp +++ b/rclcpp/include/rclcpp/any_subscription_callback.hpp @@ -20,7 +20,7 @@ #include #include #include -#include // NOLINT[build/include_order] +#include #include "rosidl_runtime_cpp/traits.hpp" #include "tracetools/tracetools.h" @@ -30,19 +30,19 @@ #include "rclcpp/detail/subscription_callback_type_helper.hpp" #include "rclcpp/function_traits.hpp" #include "rclcpp/message_info.hpp" +#include "rclcpp/serialization.hpp" #include "rclcpp/serialized_message.hpp" #include "rclcpp/type_adapter.hpp" - -template -inline constexpr bool always_false_v = false; - namespace rclcpp { namespace detail { +template +inline constexpr bool always_false_v = false; + template struct MessageDeleterHelper { @@ -158,13 +158,14 @@ struct AnySubscriptionCallbackPossibleTypes template< typename MessageT, typename AllocatorT, - bool is_adapted_type = rclcpp::TypeAdapter::is_specialized::value + 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 +struct AnySubscriptionCallbackHelper { using CallbackTypes = AnySubscriptionCallbackPossibleTypes; @@ -194,7 +195,7 @@ struct AnySubscriptionCallbackHelper /// Specialization for when MessageT is a TypeAdapter. template -struct AnySubscriptionCallbackHelper +struct AnySubscriptionCallbackHelper { using CallbackTypes = AnySubscriptionCallbackPossibleTypes; @@ -232,6 +233,26 @@ struct AnySubscriptionCallbackHelper >; }; +/// 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< @@ -354,12 +375,6 @@ class AnySubscriptionCallback allocator::set_allocator_for_deleter(&ros_message_type_deleter_, &ros_message_type_allocator_); } - [[deprecated("use AnySubscriptionCallback(const AllocatorT & allocator) instead")]] - explicit - AnySubscriptionCallback(std::shared_ptr allocator) // NOLINT[runtime/explicit] - : AnySubscriptionCallback(*NotNull(allocator.get(), "invalid allocator").pointer) - {} - AnySubscriptionCallback(const AnySubscriptionCallback &) = default; /// Generic function for setting the callback. @@ -378,56 +393,12 @@ class AnySubscriptionCallback // converted to one another, e.g. shared_ptr and unique_ptr. using scbth = detail::SubscriptionCallbackTypeHelper; - // Determine if the given CallbackT is a deprecated signature or not. - constexpr auto is_deprecated = - rclcpp::function_traits::same_arguments< - typename scbth::callback_type, - std::function)> - >::value - || - rclcpp::function_traits::same_arguments< - typename scbth::callback_type, - std::function, const rclcpp::MessageInfo &)> - >::value; - - // Use the discovered type to force the type of callback when assigning - // into the variant. - if constexpr (is_deprecated) { - // If deprecated, call sub-routine that is deprecated. - set_deprecated(static_cast(callback)); - } else { - // Otherwise just assign it. - callback_variant_ = static_cast(callback); - } + callback_variant_ = static_cast(callback); // Return copy of self for easier testing, normally will be compiled out. return *this; } - /// Function for shared_ptr to non-const MessageT, which is deprecated. - template - // TODO(wjwwood): enable this deprecation after Galactic - // [[deprecated( - // "use 'void (std::shared_ptr)' instead" - // )]] - void - set_deprecated(std::function)> callback) - { - callback_variant_ = callback; - } - - /// Function for shared_ptr to non-const MessageT with MessageInfo, which is deprecated. - template - // TODO(wjwwood): enable this deprecation after Galactic - // [[deprecated( - // "use 'void (std::shared_ptr, const rclcpp::MessageInfo &)' instead" - // )]] - void - set_deprecated(std::function, const rclcpp::MessageInfo &)> callback) - { - callback_variant_ = callback; - } - std::unique_ptr create_ros_unique_ptr_from_ros_shared_ptr_message( const std::shared_ptr & message) @@ -474,13 +445,31 @@ class AnySubscriptionCallback } } + 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 { + 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. - void + template + typename std::enable_if::value, + void>::type dispatch( std::shared_ptr message, const rclcpp::MessageInfo & message_info) { - TRACEPOINT(callback_start, static_cast(this), false); + 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) { @@ -568,19 +557,19 @@ class AnySubscriptionCallback } // condition to catch unhandled callback types else { // NOLINT[readability/braces] - static_assert(always_false_v, "unhandled callback type"); + static_assert(detail::always_false_v, "unhandled callback type"); } }, callback_variant_); - TRACEPOINT(callback_end, static_cast(this)); + 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, + std::shared_ptr serialized_message, const rclcpp::MessageInfo & message_info) { - TRACEPOINT(callback_start, static_cast(this), false); + 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) { @@ -648,18 +637,18 @@ class AnySubscriptionCallback } // condition to catch unhandled callback types else { // NOLINT[readability/braces] - static_assert(always_false_v, "unhandled callback type"); + static_assert(detail::always_false_v, "unhandled callback type"); } }, callback_variant_); - TRACEPOINT(callback_end, static_cast(this)); + TRACETOOLS_TRACEPOINT(callback_end, static_cast(this)); } void dispatch_intra_process( - std::shared_ptr message, + std::shared_ptr message, const rclcpp::MessageInfo & message_info) { - TRACEPOINT(callback_start, static_cast(this), true); + 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) { @@ -675,65 +664,89 @@ class AnySubscriptionCallback // conditions for custom type if constexpr (is_ta && std::is_same_v) { - auto local_message = convert_ros_message_to_custom_type_unique_ptr(*message); - callback(*local_message); + callback(*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); + callback(*message, message_info); } else if constexpr ( // NOLINT[readability/braces] is_ta && ( std::is_same_v|| std::is_same_v )) { - callback(convert_ros_message_to_custom_type_unique_ptr(*message)); + 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(convert_ros_message_to_custom_type_unique_ptr(*message), message_info); + 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(convert_ros_message_to_custom_type_unique_ptr(*message)); + callback(message); } else if constexpr ( // NOLINT[readability/braces] is_ta && ( std::is_same_v|| std::is_same_v )) { - callback(convert_ros_message_to_custom_type_unique_ptr(*message), message_info); + callback(message, message_info); } // conditions for ros message type - 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) { // 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) { - callback(create_ros_unique_ptr_from_ros_shared_ptr_message(message)); + 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) { - callback(create_ros_unique_ptr_from_ros_shared_ptr_message(message), message_info); + 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) { - callback(message); + 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) { - callback(message, message_info); + 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] @@ -754,18 +767,18 @@ class AnySubscriptionCallback } // condition to catch unhandled callback types else { // NOLINT[readability/braces] - static_assert(always_false_v, "unhandled callback type"); + static_assert(detail::always_false_v, "unhandled callback type"); } }, callback_variant_); - TRACEPOINT(callback_end, static_cast(this)); + TRACETOOLS_TRACEPOINT(callback_end, static_cast(this)); } void dispatch_intra_process( - std::unique_ptr message, + std::unique_ptr message, const rclcpp::MessageInfo & message_info) { - TRACEPOINT(callback_start, static_cast(this), true); + 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) { @@ -776,70 +789,98 @@ class 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) { - auto local_message = convert_ros_message_to_custom_type_unique_ptr(*message); - callback(*local_message); + callback(*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); + callback(*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)); + callback(std::move(message)); } else if constexpr ( // NOLINT[readability/braces] is_ta && ( std::is_same_v|| std::is_same_v )) { - callback(convert_ros_message_to_custom_type_unique_ptr(*message), message_info); + callback(std::move(message), message_info); } else if constexpr ( // NOLINT[readability/braces] is_ta && ( std::is_same_v|| std::is_same_v )) { - callback(convert_ros_message_to_custom_type_unique_ptr(*message)); + callback(std::move(message)); } else if constexpr ( // NOLINT[readability/braces] is_ta && ( std::is_same_v|| std::is_same_v )) { - callback(convert_ros_message_to_custom_type_unique_ptr(*message), message_info); + callback(std::move(message), message_info); } // conditions for ros message type - 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) { // 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) { - callback(std::move(message)); + 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) { - callback(std::move(message), message_info); + 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) { - callback(std::move(message)); + 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) { - callback(std::move(message), message_info); + 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] @@ -860,10 +901,10 @@ class AnySubscriptionCallback } // condition to catch unhandled callback types else { // NOLINT[readability/braces] - static_assert(always_false_v, "unhandled callback type"); + static_assert(detail::always_false_v, "unhandled callback type"); } }, callback_variant_); - TRACEPOINT(callback_end, static_cast(this)); + TRACETOOLS_TRACEPOINT(callback_end, static_cast(this)); } constexpr @@ -886,7 +927,13 @@ class AnySubscriptionCallback 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 @@ -895,10 +942,14 @@ class AnySubscriptionCallback #ifndef TRACETOOLS_DISABLED std::visit( [this](auto && callback) { - TRACEPOINT( - rclcpp_callback_register, - static_cast(this), - tracetools::get_symbol(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 } diff --git a/rclcpp/include/rclcpp/callback_group.hpp b/rclcpp/include/rclcpp/callback_group.hpp index efa8352206..b4a4d4e9a6 100644 --- a/rclcpp/include/rclcpp/callback_group.hpp +++ b/rclcpp/include/rclcpp/callback_group.hpp @@ -16,11 +16,14 @@ #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_base.hpp" @@ -86,6 +89,7 @@ class CallbackGroup * 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. @@ -93,8 +97,13 @@ class CallbackGroup RCLCPP_PUBLIC explicit CallbackGroup( CallbackGroupType group_type, + rclcpp::Context::WeakPtr context, bool automatically_add_to_executor_with_node = true); + /// Default destructor. + RCLCPP_PUBLIC + ~CallbackGroup(); + template rclcpp::SubscriptionBase::SharedPtr find_subscription_ptrs_if(Function func) const @@ -130,14 +139,52 @@ class CallbackGroup 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 + 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 @@ -163,6 +210,19 @@ class CallbackGroup 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) @@ -205,6 +265,11 @@ class CallbackGroup 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 diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index bd2d326012..ff90dcb346 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -16,35 +16,122 @@ #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 "rcutils/logging_macros.h" - #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; @@ -61,7 +148,7 @@ class ClientBase 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. /** @@ -150,6 +237,123 @@ class ClientBase 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) @@ -165,10 +369,22 @@ 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_; 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}; @@ -178,6 +394,9 @@ 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; @@ -187,6 +406,7 @@ 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>; @@ -195,6 +415,55 @@ class Client : public ClientBase 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. @@ -204,22 +473,20 @@ class Client : public ClientBase * \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 subscription. + * \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) + : 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( 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) { @@ -292,34 +559,89 @@ class Client : public ClientBase std::shared_ptr request_header, 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) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Received invalid sequence number. Ignoring..."); + 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< @@ -329,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().get(), 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< @@ -355,29 +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()); - - auto wrapping_cb = [future_with_request, promise, request, - cb = std::forward(cb)](SharedFuture future) { - auto response = future.get(); - promise->set_value(std::make_pair(request, response)); - cb(future_with_request); - }; + 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}; + } - async_send_request(request, wrapping_cb); + /// 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; + } - 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 FutureAndRequestId & 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 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 9d2ef45b4d..2050bf53e6 100644 --- a/rclcpp/include/rclcpp/clock.hpp +++ b/rclcpp/include/rclcpp/clock.hpp @@ -19,6 +19,7 @@ #include #include +#include "rclcpp/contexts/default_context.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/time.hpp" #include "rclcpp/visibility_control.hpp" @@ -76,7 +77,110 @@ class Clock */ 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. @@ -89,6 +193,16 @@ class Clock 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 * @@ -103,13 +217,13 @@ class Clock std::mutex & get_clock_mutex() noexcept; - // Add a callback to invoke if the jump threshold is exceeded. + /// 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 then the time jump; + * 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. * @@ -118,7 +232,7 @@ class Clock * \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 - * then the threshold. + * 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 diff --git a/rclcpp/include/rclcpp/context.hpp b/rclcpp/include/rclcpp/context.hpp index 5d71ffe06c..4417f4d675 100644 --- a/rclcpp/include/rclcpp/context.hpp +++ b/rclcpp/include/rclcpp/context.hpp @@ -26,6 +26,7 @@ #include #include #include +#include #include "rcl/context.h" #include "rcl/guard_condition.h" @@ -48,22 +49,28 @@ class ContextAlreadyInitialized : public std::runtime_error /// Forward declare WeakContextsWrapper class WeakContextsWrapper; -class OnShutdownCallbackHandle +class ShutdownCallbackHandle { friend class Context; public: - using OnShutdownCallbackType = std::function; + using ShutdownCallbackType = std::function; private: - std::weak_ptr callback; + 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. - * It is often used in conjunction with rclcpp::init, or rclcpp::init_local, - * and rclcpp::shutdown. + * 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 { @@ -75,7 +82,7 @@ class Context : public std::enable_shared_from_this * 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_sigint option in the InitOptions class. + * See the shutdown_on_signal option in the InitOptions class. */ RCLCPP_PUBLIC Context(); @@ -93,7 +100,7 @@ class Context : public std::enable_shared_from_this * 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_sigint + * 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. * @@ -124,7 +131,7 @@ class Context : public std::enable_shared_from_this void init( int argc, - char const * const argv[], + char const * const * argv, const rclcpp::InitOptions & init_options = rclcpp::InitOptions()); /// Return true if the context is valid, otherwise false. @@ -180,6 +187,11 @@ class Context : public std::enable_shared_from_this * * 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 @@ -189,7 +201,7 @@ class Context : public std::enable_shared_from_this bool shutdown(const std::string & reason); - using OnShutdownCallback = OnShutdownCallbackHandle::OnShutdownCallbackType; + using OnShutdownCallback = OnShutdownCallbackHandle::ShutdownCallbackType; /// Add a on_shutdown callback to be called when shutdown is called for this context. /** @@ -197,7 +209,7 @@ class Context : public std::enable_shared_from_this * to last step in shutdown(). * * When shutdown occurs due to the signal handler, these callbacks are run - * asynchronoulsy in the dedicated singal handling thread. + * 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. @@ -221,7 +233,7 @@ class Context : public std::enable_shared_from_this * to last step in shutdown(). * * When shutdown occurs due to the signal handler, these callbacks are run - * asynchronously in the dedicated singal handling thread. + * 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. @@ -249,6 +261,33 @@ class Context : public std::enable_shared_from_this 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. @@ -257,6 +296,14 @@ class Context : public std::enable_shared_from_this 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 @@ -268,7 +315,7 @@ class Context : public std::enable_shared_from_this * * - this context is shutdown() * - this context is destructed (resulting in shutdown) - * - this context has shutdown_on_sigint=true and SIGINT occurs (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. @@ -280,7 +327,6 @@ class Context : public std::enable_shared_from_this /// Interrupt any blocking sleep_for calls, causing them to return immediately and return true. RCLCPP_PUBLIC - virtual void interrupt_all_sleep_for(); @@ -313,7 +359,6 @@ class Context : public std::enable_shared_from_this // Called by constructor and destructor to clean up by finalizing the // shutdown rcl context and preparing for a new init cycle. RCLCPP_PUBLIC - virtual void clean_up(); @@ -335,9 +380,12 @@ class Context : public std::enable_shared_from_this // attempt to acquire another sub context. std::recursive_mutex sub_contexts_mutex_; - std::unordered_set> on_shutdown_callbacks_; + 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. @@ -345,6 +393,31 @@ class Context : public std::enable_shared_from_this /// 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. 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 index 1a960b8d8f..8cc73c7500 100644 --- a/rclcpp/include/rclcpp/create_client.hpp +++ b/rclcpp/include/rclcpp/create_client.hpp @@ -20,13 +20,24 @@ #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. -/// \internal +/** + * \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( @@ -34,11 +45,11 @@ create_client( std::shared_ptr node_graph, std::shared_ptr node_services, const std::string & service_name, - const rmw_qos_profile_t & qos_profile, - rclcpp::CallbackGroup::SharedPtr group) + const rclcpp::QoS & qos = rclcpp::ServicesQoS(), + rclcpp::CallbackGroup::SharedPtr group = nullptr) { rcl_client_options_t options = rcl_client_get_default_options(); - options.qos = qos_profile; + options.qos = qos.get_rmw_qos_profile(); auto cli = rclcpp::Client::make_shared( node_base.get(), @@ -50,7 +61,6 @@ create_client( 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_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 index f5281cc673..c2549721b5 100644 --- a/rclcpp/include/rclcpp/create_generic_subscription.hpp +++ b/rclcpp/include/rclcpp/create_generic_subscription.hpp @@ -45,13 +45,15 @@ namespace rclcpp * Not all publisher options are currently respected, the only relevant options for this * publisher are `event_callbacks`, `use_default_callbacks`, and `%callback_group`. */ -template> +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, - std::function)> callback, + CallbackT && callback, const rclcpp::SubscriptionOptionsWithAllocator & options = ( rclcpp::SubscriptionOptionsWithAllocator() ) @@ -60,13 +62,20 @@ std::shared_ptr create_generic_subscription( 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, - callback, + any_subscription_callback, options); topics_interface->add_subscription(subscription, options.callback_group); diff --git a/rclcpp/include/rclcpp/create_publisher.hpp b/rclcpp/include/rclcpp/create_publisher.hpp index c633c74e2e..f6088a33c3 100644 --- a/rclcpp/include/rclcpp/create_publisher.hpp +++ b/rclcpp/include/rclcpp/create_publisher.hpp @@ -92,7 +92,7 @@ template< typename NodeT> std::shared_ptr create_publisher( - NodeT & node, + NodeT && node, const std::string & topic_name, const rclcpp::QoS & qos, const rclcpp::PublisherOptionsWithAllocator & options = ( diff --git a/rclcpp/include/rclcpp/create_service.hpp b/rclcpp/include/rclcpp/create_service.hpp index 9aaa02a1ed..42c253a526 100644 --- a/rclcpp/include/rclcpp/create_service.hpp +++ b/rclcpp/include/rclcpp/create_service.hpp @@ -26,6 +26,32 @@ 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 diff --git a/rclcpp/include/rclcpp/create_subscription.hpp b/rclcpp/include/rclcpp/create_subscription.hpp index 5b84930ff7..3a1e4b1a13 100644 --- a/rclcpp/include/rclcpp/create_subscription.hpp +++ b/rclcpp/include/rclcpp/create_subscription.hpp @@ -50,8 +50,8 @@ template< typename SubscriptionT, typename MessageMemoryStrategyT, typename NodeParametersT, - typename NodeTopicsT, - typename ROSMessageType = typename SubscriptionT::ROSMessageType> + typename NodeTopicsT +> typename std::shared_ptr create_subscription( NodeParametersT & node_parameters, @@ -70,7 +70,7 @@ create_subscription( using rclcpp::node_interfaces::get_node_topics_interface; auto node_topics_interface = get_node_topics_interface(node_topics); - std::shared_ptr> + std::shared_ptr subscription_topic_stats = nullptr; if (rclcpp::detail::resolve_enable_topic_statistics( @@ -80,8 +80,7 @@ create_subscription( 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::to_string(options.topic_stats_options.publish_period.count()) + " ms"); } std::shared_ptr> @@ -89,14 +88,14 @@ create_subscription( node_parameters, node_topics_interface, options.topic_stats_options.publish_topic, - qos); + options.topic_stats_options.qos); - subscription_topic_stats = std::make_shared< - rclcpp::topic_statistics::SubscriptionTopicStatistics - >(node_topics_interface->get_node_base_interface()->get_name(), publisher); + subscription_topic_stats = + std::make_shared( + node_topics_interface->get_node_base_interface()->get_name(), publisher); std::weak_ptr< - rclcpp::topic_statistics::SubscriptionTopicStatistics + 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(); diff --git a/rclcpp/include/rclcpp/create_timer.hpp b/rclcpp/include/rclcpp/create_timer.hpp index 345f43fcbb..64d5b8e322 100644 --- a/rclcpp/include/rclcpp/create_timer.hpp +++ b/rclcpp/include/rclcpp/create_timer.hpp @@ -23,12 +23,63 @@ #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 @@ -39,16 +90,17 @@ create_timer( rclcpp::Clock::SharedPtr clock, rclcpp::Duration period, CallbackT && callback, - rclcpp::CallbackGroup::SharedPtr group = nullptr) + rclcpp::CallbackGroup::SharedPtr group = nullptr, + bool autostart = true) { - auto timer = rclcpp::GenericTimer::make_shared( + return create_timer( clock, period.to_chrono(), std::forward(callback), - node_base->get_context()); - - node_timers->add_timer(timer, group); - return timer; + group, + node_base.get(), + node_timers.get(), + autostart); } /// Create a timer with a given clock @@ -59,85 +111,107 @@ create_timer( rclcpp::Clock::SharedPtr clock, rclcpp::Duration period, CallbackT && callback, - rclcpp::CallbackGroup::SharedPtr group = nullptr) + rclcpp::CallbackGroup::SharedPtr group = nullptr, + bool autostart = true) { return create_timer( - rclcpp::node_interfaces::get_node_base_interface(node), - rclcpp::node_interfaces::get_node_timers_interface(node), clock, - period, + period.to_chrono(), std::forward(callback), - group); + 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 timer with node resources. +/// 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 - * \param node_base - * \param node_timers - * \return - * \throws std::invalid argument if either node_base or node_timers - * are null, or period is negative or too large + * \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::WallTimer::SharedPtr -create_wall_timer( +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) + 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"}; } - if (period < std::chrono::duration::zero()) { - throw std::invalid_argument{"timer period cannot be negative"}; - } + const std::chrono::nanoseconds period_ns = detail::safe_cast_to_period_in_ns(period); - // 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); + // 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; +} - // 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()"}; +/// 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"}; } - 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."}; + 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()); + 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/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 index cd8d9ffef7..651e58e7d2 100644 --- a/rclcpp/include/rclcpp/detail/qos_parameters.hpp +++ b/rclcpp/include/rclcpp/detail/qos_parameters.hpp @@ -104,6 +104,7 @@ declare_parameter_or_get( } } +#ifdef DOXYGEN_ONLY /// \internal Declare QoS parameters for the given entity. /** * \tparam NodeT Node pointer or reference type. @@ -116,12 +117,23 @@ declare_parameter_or_get( * \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< + (rclcpp::node_interfaces::has_node_parameters_interface< decltype(std::declval::type>())>::value || std::is_same, - rclcpp::node_interfaces::NodeParametersInterface::SharedPtr>::value, + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr>::value), rclcpp::QoS> declare_qos_parameters( const ::rclcpp::QosOverridingOptions & options, @@ -204,6 +216,8 @@ declare_qos_parameters( 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) \ diff --git a/rclcpp/include/rclcpp/detail/resolve_intra_process_buffer_type.hpp b/rclcpp/include/rclcpp/detail/resolve_intra_process_buffer_type.hpp index e7196a1e11..234316a8f0 100644 --- a/rclcpp/include/rclcpp/detail/resolve_intra_process_buffer_type.hpp +++ b/rclcpp/include/rclcpp/detail/resolve_intra_process_buffer_type.hpp @@ -47,6 +47,11 @@ resolve_intra_process_buffer_type( return resolved_buffer_type; } +RCLCPP_PUBLIC +rclcpp::IntraProcessBufferType +resolve_intra_process_buffer_type( + const rclcpp::IntraProcessBufferType buffer_type); + } // namespace detail } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/detail/subscription_callback_type_helper.hpp b/rclcpp/include/rclcpp/detail/subscription_callback_type_helper.hpp index ec24038d79..0ebe6efd66 100644 --- a/rclcpp/include/rclcpp/detail/subscription_callback_type_helper.hpp +++ b/rclcpp/include/rclcpp/detail/subscription_callback_type_helper.hpp @@ -38,6 +38,7 @@ namespace detail * * This issue, with the lambda's, can be demonstrated with this minimal program: * + * \code{.cpp} * #include * #include * @@ -52,6 +53,7 @@ namespace detail * 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. 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 index 62012ba0c8..d1c5d2549a 100644 --- a/rclcpp/include/rclcpp/detail/utilities.hpp +++ b/rclcpp/include/rclcpp/detail/utilities.hpp @@ -30,7 +30,7 @@ namespace detail std::vector get_unparsed_ros_arguments( - int argc, char const * const argv[], + int argc, char const * const * argv, rcl_arguments_t * arguments, rcl_allocator_t allocator); diff --git a/rclcpp/include/rclcpp/duration.hpp b/rclcpp/include/rclcpp/duration.hpp index c6b790fc7a..6b48f441e3 100644 --- a/rclcpp/include/rclcpp/duration.hpp +++ b/rclcpp/include/rclcpp/duration.hpp @@ -18,6 +18,7 @@ #include #include "builtin_interfaces/msg/duration.hpp" +#include "builtin_interfaces/msg/time.hpp" #include "rcl/time.h" #include "rclcpp/visibility_control.hpp" @@ -38,13 +39,6 @@ class RCLCPP_PUBLIC Duration */ Duration(int32_t seconds, uint32_t nanoseconds); - /// Construct duration from the specified nanoseconds. - [[deprecated( - "Use Duration::from_nanoseconds instead or std::chrono_literals. For example:" - "rclcpp::Duration::from_nanoseconds(int64_variable);" - "rclcpp::Duration(0ns);")]] - explicit Duration(rcl_duration_value_t nanoseconds); - /// Construct duration from the specified std::chrono::nanoseconds. explicit Duration(std::chrono::nanoseconds nanoseconds); @@ -99,9 +93,13 @@ class RCLCPP_PUBLIC Duration Duration operator+(const rclcpp::Duration & rhs) const; + Duration & operator+=(const rclcpp::Duration & rhs); + Duration operator-(const rclcpp::Duration & rhs) const; + Duration & operator-=(const rclcpp::Duration & rhs); + /// Get the maximum representable value. /** * \return the maximum representable value @@ -113,6 +111,9 @@ class RCLCPP_PUBLIC Duration Duration operator*(double scale) const; + Duration & + operator*=(double scale); + /// Get duration in nanosecods /** * \return the duration in nanoseconds as a rcl_duration_value_t. @@ -158,6 +159,14 @@ class RCLCPP_PUBLIC 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_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/exceptions.hpp b/rclcpp/include/rclcpp/exceptions/exceptions.hpp index 524ccf73ea..08c6b88250 100644 --- a/rclcpp/include/rclcpp/exceptions/exceptions.hpp +++ b/rclcpp/include/rclcpp/exceptions/exceptions.hpp @@ -100,6 +100,15 @@ class InvalidServiceNameError : public NameValidationError {} }; +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: @@ -109,6 +118,8 @@ class UnimplementedError : public std::runtime_error : 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 @@ -129,7 +140,7 @@ throw_from_rcl_error [[noreturn]] ( rcl_ret_t ret, const std::string & prefix = "", const rcl_error_state_t * error_state = nullptr, - void (* reset_error)() = rcl_reset_error); + reset_error_function_t reset_error = rcl_reset_error); /* *INDENT-ON* */ class RCLErrorBase @@ -204,6 +215,14 @@ class UnknownROSArgsError : public std::runtime_error 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 { @@ -220,6 +239,14 @@ class EventNotRegisteredError : public std::runtime_error : 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 { @@ -254,6 +281,23 @@ class InvalidParameterTypeException : public std::runtime_error {} }; +/// 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 { @@ -289,7 +333,6 @@ class ParameterUninitializedException : public std::runtime_error /// Construct an instance. /** * \param[in] name the name of the parameter. - * \param[in] message custom exception message. */ explicit ParameterUninitializedException(const std::string & name) : std::runtime_error("parameter '" + name + "' is not initialized") diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index ca00f1309e..ae2087bbc5 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -29,28 +29,27 @@ #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/memory_strategies.hpp" -#include "rclcpp/memory_strategy.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp/visibility_control.hpp" +#include "rclcpp/wait_set.hpp" namespace rclcpp { -typedef std::map> WeakCallbackGroupsToNodesMap; - // Forward declaration is used in convenience method signature. class Node; +class ExecutorImplementation; /// Coordinate the order and timing of available communication tasks. /** @@ -273,29 +272,51 @@ 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); - /// Collect work once and execute all available work, optionally within a duration. + /// 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 callbacks will cause this function - * to block (which may have unintended consequences). + * 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. - * Note that spin_some() may take longer than this time as it only returns once max_duration has - * been exceeded. */ 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 @@ -305,7 +326,9 @@ class Executor * 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 positive. + * \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. */ @@ -313,6 +336,16 @@ class Executor virtual void 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)); @@ -333,52 +366,12 @@ class Executor 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; - - if (spinning.exchange(true)) { - throw std::runtime_error("spin_until_future_complete() called while already spinning"); - } - RCPPUTILS_SCOPE_EXIT(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 = future.wait_for(std::chrono::seconds(0)); - if (status == std::future_status::ready) { - return FutureReturnCode::SUCCESS; + return spin_until_future_complete_impl( + std::chrono::duration_cast(timeout), + [&future](std::chrono::nanoseconds wait_time) { + return future.wait_for(wait_time); } - // 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; + ); } /// Cancel any running spin* function, causing it to return. @@ -387,27 +380,67 @@ class Executor * \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. - * \throws std::runtime_error if memory_strategy is null + * 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); @@ -422,123 +455,107 @@ class Executor void 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_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 + 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 void wait_for_work(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); - RCLCPP_PUBLIC - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr - get_node_by_group( - const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes, - rclcpp::CallbackGroup::SharedPtr group); - - /// Return true if the node has been added to this executor. + /// Check for executable in ready state and populate union structure. /** - * \param[in] node_ptr a shared pointer that points to a node base interface - * \return true if the node is associated with the executor, otherwise false + * \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 bool - has_node( - const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) const; - - RCLCPP_PUBLIC - rclcpp::CallbackGroup::SharedPtr - get_group_by_timer(rclcpp::TimerBase::SharedPtr timer); - - /// Add a callback group to an executor - /** - * \see rclcpp::Executor::add_callback_group - */ - RCLCPP_PUBLIC - virtual void - add_callback_group_to_map( - rclcpp::CallbackGroup::SharedPtr group_ptr, - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - WeakCallbackGroupsToNodesMap & weak_groups_to_nodes, - bool notify = true) RCPPUTILS_TSA_REQUIRES(mutex_); + get_next_ready_executable(AnyExecutable & any_executable); - /// Remove a callback group from the executor. + /// Wait for executable in ready state and populate union structure. /** - * \see rclcpp::Executor::remove_callback_group + * 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 - virtual void - remove_callback_group_from_map( - rclcpp::CallbackGroup::SharedPtr group_ptr, - WeakCallbackGroupsToNodesMap & weak_groups_to_nodes, - bool notify = true) RCPPUTILS_TSA_REQUIRES(mutex_); - - RCLCPP_PUBLIC - bool - get_next_ready_executable(AnyExecutable & any_executable); - - RCLCPP_PUBLIC - bool - get_next_ready_executable_from_map( - AnyExecutable & any_executable, - const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); - - RCLCPP_PUBLIC bool get_next_executable( AnyExecutable & any_executable, std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); - /// Add all callback groups that can be automatically added from associated nodes. + /// This function triggers a recollect of all entities that are registered to the executor. /** - * The executor, before collecting entities, verifies if any callback group from - * nodes associated with the executor, which is not already associated to an executor, - * can be automatically added to this executor. - * This takes care of any callback group that has been added to a node but not explicitly added - * to the executor. - * It is important to note that in order for the callback groups to be automatically added to an - * executor through this function, the node of the callback groups needs to have been added - * through the `add_node` method. + * Calling this function is thread safe. + * + * \param[in] notify if true will execute a trigger that will wake up a waiting executor */ RCLCPP_PUBLIC virtual void - add_callback_groups_from_nodes_associated_to_executor() RCPPUTILS_TSA_REQUIRES(mutex_); + 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(); - - // Mutex to protect the subsequent memory_strategy_. mutable std::mutex mutex_; - /// The memory strategy: an interface for handling user-defined memory allocation strategies. - memory_strategy::MemoryStrategy::SharedPtr - memory_strategy_ RCPPUTILS_TSA_PT_GUARDED_BY(mutex_); - /// The context associated with this executor. std::shared_ptr context_; @@ -548,33 +565,37 @@ class Executor virtual void spin_once_impl(std::chrono::nanoseconds timeout); - typedef std::map> - WeakNodesToGuardConditionsMap; + /// 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_; - /// maps nodes to guard conditions - WeakNodesToGuardConditionsMap - weak_nodes_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + std::atomic_bool entities_need_rebuild_; - /// maps callback groups associated to nodes - WeakCallbackGroupsToNodesMap - weak_groups_associated_with_executor_to_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + /// Collector used to associate executable entities from nodes and guard conditions + rclcpp::executors::ExecutorEntitiesCollector collector_; - /// maps callback groups to nodes associated with executor - WeakCallbackGroupsToNodesMap - weak_groups_to_nodes_associated_with_executor_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + /// WaitSet to be waited on. + rclcpp::WaitSet wait_set_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + std::optional> wait_result_ RCPPUTILS_TSA_GUARDED_BY(mutex_); - /// maps all callback groups to nodes - WeakCallbackGroupsToNodesMap - weak_groups_to_nodes_ 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_); - /// nodes that are associated with the executor - std::list - weak_nodes_ 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 rclcpp diff --git a/rclcpp/include/rclcpp/executor_options.hpp b/rclcpp/include/rclcpp/executor_options.hpp index 066f29eb4e..a8fd5b73bf 100644 --- a/rclcpp/include/rclcpp/executor_options.hpp +++ b/rclcpp/include/rclcpp/executor_options.hpp @@ -15,6 +15,8 @@ #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" @@ -24,18 +26,30 @@ namespace rclcpp { +class ExecutorOptionsImplementation; + /// Options to be passed to the executor constructor. struct ExecutorOptions { - ExecutorOptions() - : memory_strategy(rclcpp::memory_strategies::create_default_strategy()), - context(rclcpp::contexts::get_global_default_context()), - max_conditions(0) - {} + 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 diff --git a/rclcpp/include/rclcpp/executors.hpp b/rclcpp/include/rclcpp/executors.hpp index 36fb0d63cf..adb9c6d6c6 100644 --- a/rclcpp/include/rclcpp/executors.hpp +++ b/rclcpp/include/rclcpp/executors.hpp @@ -21,6 +21,7 @@ #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" @@ -28,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 @@ -107,7 +120,9 @@ spin_until_future_complete( 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); } 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 a1befba300..119013ebfb 100644 --- a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp @@ -47,12 +47,12 @@ class MultiThreadedExecutor : public rclcpp::Executor * * \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 instead + * 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( + explicit MultiThreadedExecutor( const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions(), size_t number_of_threads = 0, bool yield_before_execute = false, diff --git a/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp b/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp deleted file mode 100644 index 695b35a226..0000000000 --- a/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp +++ /dev/null @@ -1,354 +0,0 @@ -// 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__EXECUTORS__STATIC_EXECUTOR_ENTITIES_COLLECTOR_HPP_ -#define RCLCPP__EXECUTORS__STATIC_EXECUTOR_ENTITIES_COLLECTOR_HPP_ - -#include -#include -#include -#include -#include - -#include "rcl/guard_condition.h" -#include "rcl/wait.h" - -#include "rclcpp/experimental/executable_list.hpp" -#include "rclcpp/macros.hpp" -#include "rclcpp/memory_strategy.hpp" -#include "rclcpp/visibility_control.hpp" -#include "rclcpp/waitable.hpp" - -namespace rclcpp -{ -namespace executors -{ -typedef std::map> WeakCallbackGroupsToNodesMap; - -class StaticExecutorEntitiesCollector final - : public rclcpp::Waitable, - public std::enable_shared_from_this -{ -public: - RCLCPP_SMART_PTR_DEFINITIONS(StaticExecutorEntitiesCollector) - - // Constructor - RCLCPP_PUBLIC - StaticExecutorEntitiesCollector() = default; - - // Destructor - RCLCPP_PUBLIC - ~StaticExecutorEntitiesCollector(); - - /// Initialize StaticExecutorEntitiesCollector - /** - * \param p_wait_set A reference to the wait set to be used in the executor - * \param memory_strategy Shared pointer to the memory strategy to set. - * \param executor_guard_condition executor's guard condition - * \throws std::runtime_error if memory strategy is null - */ - RCLCPP_PUBLIC - void - init( - rcl_wait_set_t * p_wait_set, - rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy, - rcl_guard_condition_t * executor_guard_condition); - - /// Finalize StaticExecutorEntitiesCollector to clear resources - RCLCPP_PUBLIC - bool - is_init() {return initialized_;} - - RCLCPP_PUBLIC - void - fini(); - - /// Execute the waitable. - RCLCPP_PUBLIC - void - execute(std::shared_ptr & data) override; - - /// Take the data so that it can be consumed with `execute`. - /** - * For `StaticExecutorEntitiesCollector`, this always return `nullptr`. - * \sa rclcpp::Waitable::take_data() - */ - RCLCPP_PUBLIC - std::shared_ptr - take_data() override; - - /// Function to add_handles_to_wait_set and wait for work and - /** - * block until the wait set is ready or until the timeout has been exceeded. - * \throws std::runtime_error if wait set couldn't be cleared or filled. - * \throws any rcl errors from rcl_wait, \see rclcpp::exceptions::throw_from_rcl_error() - */ - RCLCPP_PUBLIC - void - refresh_wait_set(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); - - /** - * \throws std::runtime_error if it couldn't add guard condition to wait set - */ - RCLCPP_PUBLIC - bool - add_to_wait_set(rcl_wait_set_t * wait_set) override; - - RCLCPP_PUBLIC - size_t - get_number_of_ready_guard_conditions() override; - - /// Add a callback group to an executor. - /** - * \see rclcpp::Executor::add_callback_group - */ - RCLCPP_PUBLIC - bool - add_callback_group( - rclcpp::CallbackGroup::SharedPtr group_ptr, - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr); - - /// Add a callback group to an executor. - /** - * \see rclcpp::Executor::add_callback_group - * \return boolean whether the node from the callback group is new - */ - RCLCPP_PUBLIC - bool - add_callback_group( - rclcpp::CallbackGroup::SharedPtr group_ptr, - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); - - /// Remove a callback group from the executor. - /** - * \see rclcpp::Executor::remove_callback_group - */ - RCLCPP_PUBLIC - bool - remove_callback_group( - rclcpp::CallbackGroup::SharedPtr group_ptr); - - /// Remove a callback group from the executor. - /** - * \see rclcpp::Executor::remove_callback_group_from_map - */ - RCLCPP_PUBLIC - bool - remove_callback_group_from_map( - rclcpp::CallbackGroup::SharedPtr group_ptr, - WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); - - /** - * \see rclcpp::Executor::add_node() - * \throw std::runtime_error if node was already added - */ - RCLCPP_PUBLIC - bool - add_node( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr); - - /** - * \see rclcpp::Executor::remove_node() - * \throw std::runtime_error if no guard condition is associated with node. - */ - RCLCPP_PUBLIC - bool - remove_node( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr); - - RCLCPP_PUBLIC - std::vector - get_all_callback_groups(); - - /// Get callback groups that belong to executor. - /** - * \see rclcpp::Executor::get_manually_added_callback_groups() - */ - RCLCPP_PUBLIC - std::vector - get_manually_added_callback_groups(); - - /// Get callback groups that belong to executor. - /** - * \see rclcpp::Executor::get_automatically_added_callback_groups_from_nodes() - */ - RCLCPP_PUBLIC - std::vector - get_automatically_added_callback_groups_from_nodes(); - - /// Complete all available queued work without blocking. - /** - * This function checks if after the guard condition was triggered - * (or a spurious wakeup happened) we are really ready to execute - * i.e. re-collect entities - */ - RCLCPP_PUBLIC - bool - is_ready(rcl_wait_set_t * wait_set) override; - - /// Return number of timers - /** - * \return number of timers - */ - RCLCPP_PUBLIC - size_t - get_number_of_timers() {return exec_list_.number_of_timers;} - - /// Return number of subscriptions - /** - * \return number of subscriptions - */ - RCLCPP_PUBLIC - size_t - get_number_of_subscriptions() {return exec_list_.number_of_subscriptions;} - - /// Return number of services - /** - * \return number of services - */ - RCLCPP_PUBLIC - size_t - get_number_of_services() {return exec_list_.number_of_services;} - - /// Return number of clients - /** - * \return number of clients - */ - RCLCPP_PUBLIC - size_t - get_number_of_clients() {return exec_list_.number_of_clients;} - - /// Return number of waitables - /** - * \return number of waitables - */ - RCLCPP_PUBLIC - size_t - get_number_of_waitables() {return exec_list_.number_of_waitables;} - - /** Return a SubscritionBase Sharedptr by index. - * \param[in] i The index of the SubscritionBase - * \return a SubscritionBase shared pointer - * \throws std::out_of_range if the argument is higher than the size of the structrue. - */ - RCLCPP_PUBLIC - rclcpp::SubscriptionBase::SharedPtr - get_subscription(size_t i) {return exec_list_.subscription[i];} - - /** Return a TimerBase Sharedptr by index. - * \param[in] i The index of the TimerBase - * \return a TimerBase shared pointer - * \throws std::out_of_range if the argument is higher than the size. - */ - RCLCPP_PUBLIC - rclcpp::TimerBase::SharedPtr - get_timer(size_t i) {return exec_list_.timer[i];} - - /** Return a ServiceBase Sharedptr by index. - * \param[in] i The index of the ServiceBase - * \return a ServiceBase shared pointer - * \throws std::out_of_range if the argument is higher than the size. - */ - RCLCPP_PUBLIC - rclcpp::ServiceBase::SharedPtr - get_service(size_t i) {return exec_list_.service[i];} - - /** Return a ClientBase Sharedptr by index - * \param[in] i The index of the ClientBase - * \return a ClientBase shared pointer - * \throws std::out_of_range if the argument is higher than the size. - */ - RCLCPP_PUBLIC - rclcpp::ClientBase::SharedPtr - get_client(size_t i) {return exec_list_.client[i];} - - /** Return a Waitable Sharedptr by index - * \param[in] i The index of the Waitable - * \return a Waitable shared pointer - * \throws std::out_of_range if the argument is higher than the size. - */ - RCLCPP_PUBLIC - rclcpp::Waitable::SharedPtr - get_waitable(size_t i) {return exec_list_.waitable[i];} - -private: - /// Function to reallocate space for entities in the wait set. - /** - * \throws std::runtime_error if wait set couldn't be cleared or resized. - */ - void - prepare_wait_set(); - - void - fill_executable_list(); - - void - fill_memory_strategy(); - - /// Return true if the node belongs to the collector - /** - * \param[in] group_ptr a node base interface shared pointer - * \return boolean whether a node belongs the collector - */ - bool - has_node( - const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) const; - - /// Add all callback groups that can be automatically added by any executor - /// and is not already associated with an executor from nodes - /// that are associated with executor - /** - * \see rclcpp::Executor::add_callback_groups_from_nodes_associated_to_executor() - */ - void - add_callback_groups_from_nodes_associated_to_executor(); - - void - fill_executable_list_from_map(const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); - - /// Memory strategy: an interface for handling user-defined memory allocation strategies. - rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy_; - - // maps callback groups to nodes. - WeakCallbackGroupsToNodesMap weak_groups_associated_with_executor_to_nodes_; - // maps callback groups to nodes. - WeakCallbackGroupsToNodesMap weak_groups_to_nodes_associated_with_executor_; - - typedef std::map> - WeakNodesToGuardConditionsMap; - WeakNodesToGuardConditionsMap weak_nodes_to_guard_conditions_; - - /// List of weak nodes registered in the static executor - std::list weak_nodes_; - - /// Wait set for managing entities that the rmw layer waits on. - rcl_wait_set_t * p_wait_set_ = nullptr; - - /// Executable list: timers, subscribers, clients, services and waitables - rclcpp::experimental::ExecutableList exec_list_; - - /// Bool to check if the entities collector has been initialized - bool initialized_ = false; -}; - -} // namespace executors -} // namespace rclcpp - -#endif // RCLCPP__EXECUTORS__STATIC_EXECUTOR_ENTITIES_COLLECTOR_HPP_ diff --git a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp index 61da15e125..33674465a8 100644 --- a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp @@ -15,24 +15,13 @@ #ifndef RCLCPP__EXECUTORS__STATIC_SINGLE_THREADED_EXECUTOR_HPP_ #define RCLCPP__EXECUTORS__STATIC_SINGLE_THREADED_EXECUTOR_HPP_ +#include #include -#include -#include #include -#include -#include - -#include "rmw/rmw.h" #include "rclcpp/executor.hpp" -#include "rclcpp/executors/static_executor_entities_collector.hpp" -#include "rclcpp/experimental/executable_list.hpp" -#include "rclcpp/macros.hpp" -#include "rclcpp/memory_strategies.hpp" -#include "rclcpp/node.hpp" -#include "rclcpp/rate.hpp" -#include "rclcpp/utilities.hpp" -#include "rclcpp/visibility_control.hpp" +#include "rclcpp/executors/executor_entities_collection.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" namespace rclcpp { @@ -42,9 +31,15 @@ namespace executors /// Static executor implementation /** * This executor is a static version of the original single threaded executor. - * It's static because it doesn't reconstruct the executable list for every iteration. + * 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; @@ -55,7 +50,8 @@ namespace executors * exec.spin(); * exec.remove_node(node); */ -class StaticSingleThreadedExecutor : public rclcpp::Executor +class [[deprecated("Use rclcpp::executors::SingleThreadedExecutor")]] StaticSingleThreadedExecutor + : public rclcpp::Executor { public: RCLCPP_SMART_PTR_DEFINITIONS(StaticSingleThreadedExecutor) @@ -65,7 +61,7 @@ class StaticSingleThreadedExecutor : public rclcpp::Executor explicit StaticSingleThreadedExecutor( const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions()); - /// Default destrcutor. + /// Default destructor. RCLCPP_PUBLIC virtual ~StaticSingleThreadedExecutor(); @@ -99,9 +95,10 @@ class StaticSingleThreadedExecutor : public rclcpp::Executor /// Static executor implementation of spin all /** - * This non-blocking function will execute entities until - * timeout or no more work available. If new entities get ready - * while executing work available, they will be executed + * 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: @@ -115,105 +112,31 @@ class StaticSingleThreadedExecutor : public rclcpp::Executor void spin_all(std::chrono::nanoseconds max_duration) override; - /// Add a callback group to an executor. - /** - * \sa rclcpp::Executor::add_callback_group - */ - RCLCPP_PUBLIC - void - add_callback_group( - rclcpp::CallbackGroup::SharedPtr group_ptr, - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - bool notify = true) override; - - /// Remove callback group from the executor - /** - * \sa rclcpp::Executor::remove_callback_group - */ - RCLCPP_PUBLIC - void - remove_callback_group( - rclcpp::CallbackGroup::SharedPtr group_ptr, - bool notify = true) override; - - /// Add a node to the executor. - /** - * \sa rclcpp::Executor::add_node - */ - RCLCPP_PUBLIC - void - add_node( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - bool notify = true) override; - - /// Convenience function which takes Node and forwards NodeBaseInterface. - /** - * \sa rclcpp::StaticSingleThreadedExecutor::add_node - */ - RCLCPP_PUBLIC - void - add_node(std::shared_ptr node_ptr, bool notify = true) override; - - /// Remove a node from the executor. - /** - * \sa rclcpp::Executor::remove_node - */ - RCLCPP_PUBLIC - void - remove_node( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - bool notify = true) override; - - /// Convenience function which takes Node and forwards NodeBaseInterface. - /** - * \sa rclcpp::Executor::remove_node - */ - RCLCPP_PUBLIC - void - remove_node(std::shared_ptr node_ptr, bool notify = true) override; - - RCLCPP_PUBLIC - std::vector - get_all_callback_groups() override; - - /// Get callback groups that belong to executor. - /** - * \sa rclcpp::Executor::get_manually_added_callback_groups() - */ - RCLCPP_PUBLIC - std::vector - get_manually_added_callback_groups() override; - - /// Get callback groups that belong to executor. - /** - * \sa rclcpp::Executor::get_automatically_added_callback_groups_from_nodes() - */ - RCLCPP_PUBLIC - std::vector - get_automatically_added_callback_groups_from_nodes() 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. */ - RCLCPP_PUBLIC bool - execute_ready_executables(bool spin_once = false); + execute_ready_executables( + const rclcpp::executors::ExecutorEntitiesCollection & collection, + rclcpp::WaitResult & wait_result, + bool spin_once); - RCLCPP_PUBLIC void spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive); - RCLCPP_PUBLIC void spin_once_impl(std::chrono::nanoseconds timeout) override; + std::optional> + collect_and_wait(std::chrono::nanoseconds timeout); + private: RCLCPP_DISABLE_COPY(StaticSingleThreadedExecutor) - - StaticExecutorEntitiesCollector::SharedPtr entities_collector_; }; } // namespace executors diff --git a/rclcpp/include/rclcpp/experimental/buffers/buffer_implementation_base.hpp b/rclcpp/include/rclcpp/experimental/buffers/buffer_implementation_base.hpp index 1e5346116a..1d50b1659e 100644 --- a/rclcpp/include/rclcpp/experimental/buffers/buffer_implementation_base.hpp +++ b/rclcpp/include/rclcpp/experimental/buffers/buffer_implementation_base.hpp @@ -15,6 +15,8 @@ #ifndef RCLCPP__EXPERIMENTAL__BUFFERS__BUFFER_IMPLEMENTATION_BASE_HPP_ #define RCLCPP__EXPERIMENTAL__BUFFERS__BUFFER_IMPLEMENTATION_BASE_HPP_ +#include + namespace rclcpp { namespace experimental @@ -31,8 +33,11 @@ class 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 diff --git a/rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp index aef38a028d..268c3f6649 100644 --- a/rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp +++ b/rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp @@ -16,13 +16,16 @@ #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 { @@ -42,6 +45,7 @@ class IntraProcessBufferBase virtual bool has_data() const = 0; virtual bool use_take_shared_method() const = 0; + virtual size_t available_capacity() const = 0; }; template< @@ -63,6 +67,9 @@ class IntraProcessBuffer : public IntraProcessBufferBase 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< @@ -93,6 +100,10 @@ class TypedIntraProcessBuffer : public IntraProcessBuffer(buffer_.get()), + static_cast(this)); if (!allocator) { message_allocator_ = std::make_shared(); } else { @@ -122,6 +133,16 @@ class TypedIntraProcessBuffer : public IntraProcessBuffer(); } + 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(); @@ -137,6 +158,11 @@ class TypedIntraProcessBuffer : public IntraProcessBuffer::value; } + size_t available_capacity() const override + { + return buffer_->available_capacity(); + } + private: std::unique_ptr> buffer_; @@ -231,6 +257,71 @@ class TypedIntraProcessBuffer : public IntraProcessBufferdequeue(); } + + // 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 diff --git a/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp b/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp index ad01946193..b8fe79a5ff 100644 --- a/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp +++ b/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp @@ -15,9 +15,6 @@ #ifndef RCLCPP__EXPERIMENTAL__BUFFERS__RING_BUFFER_IMPLEMENTATION_HPP_ #define RCLCPP__EXPERIMENTAL__BUFFERS__RING_BUFFER_IMPLEMENTATION_HPP_ -#include -#include -#include #include #include #include @@ -29,6 +26,7 @@ #include "rclcpp/logging.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/visibility_control.hpp" +#include "tracetools/tracetools.h" namespace rclcpp { @@ -55,6 +53,10 @@ class RingBufferImplementation : public BufferImplementationBase 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() {} @@ -65,12 +67,18 @@ class RingBufferImplementation : public BufferImplementationBase * * \param request the element to be stored in the ring buffer */ - void enqueue(BufferT request) + 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_); @@ -85,16 +93,20 @@ class RingBufferImplementation : public BufferImplementationBase * * \return the element that is being removed from the ring buffer */ - BufferT dequeue() + BufferT dequeue() override { std::lock_guard lock(mutex_); if (!has_data_()) { - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Calling dequeue on empty intra-process buffer"); - throw std::runtime_error("Calling dequeue on empty intra-process buffer"); + 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_--; @@ -102,6 +114,17 @@ class RingBufferImplementation : public BufferImplementationBase 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. @@ -121,7 +144,7 @@ class RingBufferImplementation : public BufferImplementationBase * * \return `true` if there is data and `false` otherwise */ - inline bool has_data() const + inline bool has_data() const override { std::lock_guard lock(mutex_); return has_data_(); @@ -140,7 +163,22 @@ class RingBufferImplementation : public BufferImplementationBase return is_full_(); } - void clear() {} + /// 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 @@ -178,6 +216,82 @@ class RingBufferImplementation : public BufferImplementationBase 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_; diff --git a/rclcpp/include/rclcpp/experimental/create_intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/create_intra_process_buffer.hpp index 1d232d14fe..4d7668b964 100644 --- a/rclcpp/include/rclcpp/experimental/create_intra_process_buffer.hpp +++ b/rclcpp/include/rclcpp/experimental/create_intra_process_buffer.hpp @@ -16,11 +16,9 @@ #define RCLCPP__EXPERIMENTAL__CREATE_INTRA_PROCESS_BUFFER_HPP_ #include -#include +#include #include -#include "rcl/subscription.h" - #include "rclcpp/experimental/buffers/intra_process_buffer.hpp" #include "rclcpp/experimental/buffers/ring_buffer_implementation.hpp" #include "rclcpp/intra_process_buffer_type.hpp" diff --git a/rclcpp/include/rclcpp/experimental/executable_list.hpp b/rclcpp/include/rclcpp/experimental/executable_list.hpp index e1c70db3de..887dd6cd8f 100644 --- a/rclcpp/include/rclcpp/experimental/executable_list.hpp +++ b/rclcpp/include/rclcpp/experimental/executable_list.hpp @@ -15,7 +15,6 @@ #ifndef RCLCPP__EXPERIMENTAL__EXECUTABLE_LIST_HPP_ #define RCLCPP__EXPERIMENTAL__EXECUTABLE_LIST_HPP_ -#include #include #include "rclcpp/client.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 index a927a81856..a403c20c8b 100644 --- a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp +++ b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp @@ -19,18 +19,17 @@ #include -#include -#include -#include -#include -#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" @@ -38,6 +37,7 @@ #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 @@ -113,9 +113,40 @@ class IntraProcessManager * \param subscription the SubscriptionIntraProcess to register. * \return an unsigned 64-bit integer which is the subscription's unique id. */ - RCLCPP_PUBLIC + template< + typename ROSMessageType, + typename Alloc = std::allocator + > uint64_t - add_subscription(rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr subscription); + 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. /** @@ -132,14 +163,21 @@ class IntraProcessManager * 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); + 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. /** @@ -173,11 +211,14 @@ class IntraProcessManager * * \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 Alloc = std::allocator, - typename Deleter = std::default_delete> + typename ROSMessageType, + typename Alloc, + typename Deleter = std::default_delete + > void do_intra_process_publish( uint64_t intra_process_publisher_id, @@ -203,7 +244,7 @@ class IntraProcessManager // 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( + 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) @@ -212,13 +253,13 @@ class IntraProcessManager // 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); + 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( + this->template add_owned_msg_to_buffers( std::move(message), concatenated_vector, allocator); @@ -229,17 +270,19 @@ class IntraProcessManager // for the buffers that do not require ownership auto shared_msg = std::allocate_shared(allocator, *message); - this->template add_shared_msg_to_buffers( + this->template add_shared_msg_to_buffers( shared_msg, sub_ids.take_shared_subscriptions); - this->template add_owned_msg_to_buffers( + this->template add_owned_msg_to_buffers( std::move(message), sub_ids.take_ownership_subscriptions, allocator); } } template< typename MessageT, - typename Alloc = std::allocator, - typename Deleter = std::default_delete> + 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, @@ -265,7 +308,7 @@ class IntraProcessManager // 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( + this->template add_shared_msg_to_buffers( shared_msg, sub_ids.take_shared_subscriptions); } return shared_msg; @@ -275,21 +318,48 @@ class IntraProcessManager auto shared_msg = std::allocate_shared(allocator, *message); if (!sub_ids.take_shared_subscriptions.empty()) { - this->template add_shared_msg_to_buffers( + 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( + 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 @@ -304,6 +374,11 @@ class IntraProcessManager 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 { @@ -317,6 +392,9 @@ class IntraProcessManager using PublisherMap = std::unordered_map; + using PublisherBufferMap = + std::unordered_map; + using PublisherToSubscriptionIdsMap = std::unordered_map; @@ -335,44 +413,134 @@ class IntraProcessManager 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 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) { - auto subscription = std::dynamic_pointer_cast< - rclcpp::experimental::SubscriptionIntraProcessBuffer - >(subscription_base); - if (nullptr == subscription) { - throw std::runtime_error( - "failed to dynamic cast SubscriptionIntraProcessBase to " - "SubscriptionIntraProcessBuffer, which " - "can happen when the publisher and subscription use different " - "allocator types, which is not supported"); - } + 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; + } - subscription->provide_intra_process_message(message); + 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 { - subscriptions_.erase(id); + 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 = std::allocator, - typename Deleter = std::default_delete> + typename Alloc, + typename Deleter, + typename ROSMessageType> void add_owned_msg_to_buffers( std::unique_ptr message, @@ -382,39 +550,88 @@ class IntraProcessManager 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) { - auto subscription = std::dynamic_pointer_cast< - rclcpp::experimental::SubscriptionIntraProcessBuffer - >(subscription_base); - if (nullptr == subscription) { - throw std::runtime_error( - "failed to dynamic cast SubscriptionIntraProcessBase to " - "SubscriptionIntraProcessBuffer, which " - "can happen when the publisher and subscription use different " - "allocator types, which is not supported"); - } + 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_message(std::move(message)); + 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 - MessageUniquePtr copy_message; Deleter deleter = message.get_deleter(); auto ptr = MessageAllocTraits::allocate(allocator, 1); MessageAllocTraits::construct(allocator, ptr, *message); - copy_message = MessageUniquePtr(ptr, deleter); - subscription->provide_intra_process_message(std::move(copy_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 { - subscriptions_.erase(subscription_it); + 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)); + } + } } } } @@ -422,6 +639,7 @@ class IntraProcessManager PublisherToSubscriptionIdsMap pub_to_subs_; SubscriptionMap subscriptions_; PublisherMap publishers_; + PublisherBufferMap publisher_buffers_; mutable std::shared_timed_mutex mutex_; }; 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 index f174cd5c07..0624f92c62 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp @@ -15,25 +15,22 @@ #ifndef RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_HPP_ #define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_HPP_ -#include +#include -#include -#include #include #include #include +#include #include -#include "rcl/error_handling.h" +#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/create_intra_process_buffer.hpp" -#include "rclcpp/experimental/subscription_intra_process_base.hpp" #include "rclcpp/experimental/subscription_intra_process_buffer.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/type_support_decl.hpp" -#include "rclcpp/waitable.hpp" #include "tracetools/tracetools.h" namespace rclcpp @@ -43,47 +40,54 @@ namespace experimental template< typename MessageT, - typename Alloc = std::allocator, - typename Deleter = std::default_delete, - typename CallbackMessageT = MessageT> + typename SubscribedType, + typename SubscribedTypeAlloc = std::allocator, + typename SubscribedTypeDeleter = std::default_delete, + typename ROSMessageType = SubscribedType, + typename Alloc = std::allocator +> class SubscriptionIntraProcess : public SubscriptionIntraProcessBuffer< - MessageT, - Alloc, - Deleter + SubscribedType, + SubscribedTypeAlloc, + SubscribedTypeDeleter, + ROSMessageType > { using SubscriptionIntraProcessBufferT = SubscriptionIntraProcessBuffer< - MessageT, - Alloc, - Deleter + SubscribedType, + SubscribedTypeAlloc, + SubscribedTypeDeleter, + ROSMessageType >; public: RCLCPP_SMART_PTR_DEFINITIONS(SubscriptionIntraProcess) - using MessageAllocTraits = typename SubscriptionIntraProcessBufferT::MessageAllocTraits; - using MessageAlloc = typename SubscriptionIntraProcessBufferT::MessageAlloc; - using ConstMessageSharedPtr = typename SubscriptionIntraProcessBufferT::ConstMessageSharedPtr; - using MessageUniquePtr = typename SubscriptionIntraProcessBufferT::MessageUniquePtr; + 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, + 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( - allocator, + : SubscriptionIntraProcessBuffer( + std::make_shared(*allocator), context, topic_name, qos_profile, buffer_type), any_callback_(callback) { - TRACEPOINT( + TRACETOOLS_TRACEPOINT( rclcpp_subscription_callback_added, static_cast(this), static_cast(&any_callback_)); @@ -97,17 +101,47 @@ class SubscriptionIntraProcess 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() + 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( @@ -115,26 +149,25 @@ class SubscriptionIntraProcess ); } - void execute(std::shared_ptr & data) + void execute(const std::shared_ptr & data) override { - execute_impl(data); + execute_impl(data); } protected: template typename std::enable_if::value, void>::type - execute_impl(std::shared_ptr & data) + execute_impl(const std::shared_ptr &) { - (void)data; throw std::runtime_error("Subscription intra-process can't handle serialized messages"); } template typename std::enable_if::value, void>::type - execute_impl(std::shared_ptr & data) + execute_impl(const std::shared_ptr & data) { - if (!data) { - throw std::runtime_error("'data' is empty"); + if (nullptr == data) { + return; } rmw_message_info_t msg_info; @@ -154,7 +187,7 @@ class SubscriptionIntraProcess shared_ptr.reset(); } - AnySubscriptionCallback any_callback_; + AnySubscriptionCallback any_callback_; }; } // namespace experimental diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp index ae57521999..74792e8751 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp @@ -15,18 +15,17 @@ #ifndef RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_ #define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_ -#include - -#include +#include #include #include #include -#include -#include "rcl/error_handling.h" +#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/type_support_decl.hpp" #include "rclcpp/waitable.hpp" namespace rclcpp @@ -39,34 +38,57 @@ 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) - : topic_name_(topic_name), qos_profile_(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() {return 1;} + 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 - add_to_wait_set(rcl_wait_set_t * wait_set); + is_durability_transient_local() const; + + bool + is_ready(const rcl_wait_set_t & wait_set) override = 0; - virtual bool - is_ready(rcl_wait_set_t * wait_set) = 0; + std::shared_ptr + take_data() override = 0; - virtual std::shared_ptr - take_data() = 0; + take_data_by_entity_id(size_t id) override + { + (void)id; + return take_data(); + } - virtual void - execute(std::shared_ptr & data) = 0; + void + execute(const std::shared_ptr & data) override = 0; - virtual bool + virtual + bool use_take_shared_method() const = 0; RCLCPP_PUBLIC @@ -77,14 +99,108 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable 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 reentrant_mutex_; - rcl_guard_condition_t gc_; + std::recursive_mutex callback_mutex_; + std::function on_new_message_callback_ {nullptr}; + size_t unread_count_{0}; + rclcpp::GuardCondition gc_; -private: 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_; }; diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp index 05a830633c..2f384f351c 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp @@ -15,24 +15,23 @@ #ifndef RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BUFFER_HPP_ #define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BUFFER_HPP_ -#include - -#include -#include #include -#include #include +#include #include #include "rcl/error_handling.h" +#include "rcl/guard_condition.h" +#include "rcl/wait.h" -#include "rclcpp/any_subscription_callback.hpp" #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/waitable.hpp" +#include "rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp" + #include "tracetools/tracetools.h" namespace rclcpp @@ -41,24 +40,39 @@ namespace experimental { template< - typename MessageT, - typename Alloc = std::allocator, - typename Deleter = std::default_delete + 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 SubscriptionIntraProcessBase +class SubscriptionIntraProcessBuffer : public SubscriptionROSMsgIntraProcessBuffer::allocator_type, + allocator::Deleter::allocator_type, + ROSMessageType>> { public: RCLCPP_SMART_PTR_DEFINITIONS(SubscriptionIntraProcessBuffer) - using MessageAllocTraits = allocator::AllocRebind; - using MessageAlloc = typename MessageAllocTraits::allocator_type; - using ConstMessageSharedPtr = std::shared_ptr; - using MessageUniquePtr = std::unique_ptr; + 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< - MessageT, + SubscribedType, Alloc, - Deleter + SubscribedTypeDeleter >::UniquePtr; SubscriptionIntraProcessBuffer( @@ -67,74 +81,119 @@ class SubscriptionIntraProcessBuffer : public SubscriptionIntraProcessBase const std::string & topic_name, const rclcpp::QoS & qos_profile, rclcpp::IntraProcessBufferType buffer_type) - : SubscriptionIntraProcessBase(topic_name, qos_profile) + : 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_ = rclcpp::experimental::create_intra_process_buffer( buffer_type, qos_profile, - allocator); + std::make_shared(subscribed_type_allocator_)); + TRACETOOLS_TRACEPOINT( + rclcpp_ipb_to_subscription, + static_cast(buffer_.get()), + static_cast(this)); + } - // Create the guard condition. - rcl_guard_condition_options_t guard_condition_options = - rcl_guard_condition_get_default_options(); + 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_); + } - gc_ = rcl_get_zero_initialized_guard_condition(); - rcl_ret_t ret = rcl_guard_condition_init( - &gc_, context->get_rcl_context().get(), guard_condition_options); + bool + is_ready(const rcl_wait_set_t & wait_set) override + { + (void) wait_set; + return buffer_->has_data(); + } - if (RCL_RET_OK != ret) { + 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( - "SubscriptionIntraProcessBuffer init error initializing guard condition"); + "convert_ros_message_to_subscribed_type_unique_ptr " + "unexpectedly called without TypeAdapter"); } } - virtual ~SubscriptionIntraProcessBuffer() + void + provide_intra_process_message(ConstMessageSharedPtr message) override { - if (rcl_guard_condition_fini(&gc_) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Failed to destroy guard condition: %s", - rcutils_get_error_string().str); + 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(); } - bool - is_ready(rcl_wait_set_t * wait_set) + void + provide_intra_process_message(MessageUniquePtr message) override { - (void) wait_set; - return buffer_->has_data(); + 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_message(ConstMessageSharedPtr message) + provide_intra_process_data(ConstDataSharedPtr message) { buffer_->add_shared(std::move(message)); trigger_guard_condition(); + this->invoke_on_new_message(); } void - provide_intra_process_message(MessageUniquePtr message) + 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 + 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() + trigger_guard_condition() override { - rcl_ret_t ret = rcl_trigger_guard_condition(&gc_); - (void)ret; + this->gc_.trigger(); } BufferUniquePtr buffer_; + SubscribedTypeAllocator subscribed_type_allocator_; + SubscribedTypeDeleter subscribed_type_deleter_; }; } // namespace experimental 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 4004476842..b6af0aeb47 100644 --- a/rclcpp/include/rclcpp/function_traits.hpp +++ b/rclcpp/include/rclcpp/function_traits.hpp @@ -80,10 +80,12 @@ struct function_traits: function_traits -#if defined _LIBCPP_VERSION // libc++ (Clang) +#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 @@ -97,10 +99,12 @@ struct function_traits< // std::bind for object const methods template -#if defined _LIBCPP_VERSION // libc++ (Clang) +#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 @@ -114,7 +118,9 @@ struct function_traits< // std::bind for free functions template -#if defined _LIBCPP_VERSION // libc++ (Clang) +#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> 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 index 7cadda5d1d..292e6900d3 100644 --- a/rclcpp/include/rclcpp/generic_publisher.hpp +++ b/rclcpp/include/rclcpp/generic_publisher.hpp @@ -31,6 +31,8 @@ #include "rclcpp/typesupport_helpers.hpp" #include "rclcpp/visibility_control.hpp" +#include "rmw/rmw.h" + namespace rclcpp { @@ -60,7 +62,6 @@ class GenericPublisher : public rclcpp::PublisherBase * \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`. @@ -76,39 +77,13 @@ class GenericPublisher : public rclcpp::PublisherBase : rclcpp::PublisherBase( node_base, topic_name, - *rclcpp::get_typesupport_handle(topic_type, "rosidl_typesupport_cpp", *ts_lib), - options.template to_rcl_publisher_options(qos)), + *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) - { - // This is unfortunately duplicated with the code in publisher.hpp. - // TODO(nnmm): Deduplicate by moving this into PublisherBase. - if (options.event_callbacks.deadline_callback) { - this->add_event_handler( - options.event_callbacks.deadline_callback, - RCL_PUBLISHER_OFFERED_DEADLINE_MISSED); - } - if (options.event_callbacks.liveliness_callback) { - this->add_event_handler( - options.event_callbacks.liveliness_callback, - RCL_PUBLISHER_LIVELINESS_LOST); - } - if (options.event_callbacks.incompatible_qos_callback) { - this->add_event_handler( - options.event_callbacks.incompatible_qos_callback, - RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS); - } else if (options.use_default_callbacks) { - // Register default callback when not specified - try { - this->add_event_handler( - [this](QOSOfferedIncompatibleQoSInfo & info) { - this->default_incompatible_qos_callback(info); - }, - RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS); - } catch (UnsupportedEventTypeException & /*exc*/) { - // pass - } - } - } + {} RCLCPP_PUBLIC virtual ~GenericPublisher() = default; @@ -117,9 +92,24 @@ class GenericPublisher : public rclcpp::PublisherBase 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 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 index 673712eedb..dd0e8be94d 100644 --- a/rclcpp/include/rclcpp/generic_subscription.hpp +++ b/rclcpp/include/rclcpp/generic_subscription.hpp @@ -74,51 +74,31 @@ class GenericSubscription : public rclcpp::SubscriptionBase const std::string & topic_name, const std::string & topic_type, const rclcpp::QoS & qos, - // TODO(nnmm): Add variant for callback with message info. See issue #1604. - std::function)> callback, + AnySubscriptionCallback callback, const rclcpp::SubscriptionOptionsWithAllocator & options) : SubscriptionBase( node_base, - *rclcpp::get_typesupport_handle(topic_type, "rosidl_typesupport_cpp", *ts_lib), + *rclcpp::get_message_typesupport_handle(topic_type, "rosidl_typesupport_cpp", *ts_lib), topic_name, - options.template to_rcl_subscription_options(qos), - true), - callback_(callback), + options.to_rcl_subscription_options(qos), + options.event_callbacks, + options.use_default_callbacks, + DeliveredMessageKind::SERIALIZED_MESSAGE), + any_callback_(callback), ts_lib_(ts_lib) { - // This is unfortunately duplicated with the code in subscription.hpp. - // TODO(nnmm): Deduplicate by moving this into SubscriptionBase. - if (options.event_callbacks.deadline_callback) { - this->add_event_handler( - options.event_callbacks.deadline_callback, - RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED); - } - if (options.event_callbacks.liveliness_callback) { - this->add_event_handler( - options.event_callbacks.liveliness_callback, - RCL_SUBSCRIPTION_LIVELINESS_CHANGED); - } - if (options.event_callbacks.incompatible_qos_callback) { - this->add_event_handler( - options.event_callbacks.incompatible_qos_callback, - RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS); - } else if (options.use_default_callbacks) { - // Register default callback when not specified - try { - this->add_event_handler( - [this](QOSRequestedIncompatibleQoSInfo & info) { - this->default_incompatible_qos_callback(info); - }, - RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS); - } catch (UnsupportedEventTypeException & /*exc*/) { - // pass - } - } - if (options.event_callbacks.message_lost_callback) { - this->add_event_handler( - options.event_callbacks.message_lost_callback, - RCL_SUBSCRIPTION_MESSAGE_LOST); - } + 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 @@ -155,10 +135,34 @@ class GenericSubscription : public rclcpp::SubscriptionBase 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) - - std::function)> callback_; + AnySubscriptionCallback> any_callback_; // The type support library should stay loaded, so it is stored in the GenericSubscription std::shared_ptr ts_lib_; }; diff --git a/rclcpp/include/rclcpp/get_message_type_support_handle.hpp b/rclcpp/include/rclcpp/get_message_type_support_handle.hpp index 2f19528baf..cbc2e6035b 100644 --- a/rclcpp/include/rclcpp/get_message_type_support_handle.hpp +++ b/rclcpp/include/rclcpp/get_message_type_support_handle.hpp @@ -28,7 +28,17 @@ namespace rclcpp { -/// Specialization for when MessageT is an actual ROS message type. +#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< @@ -44,7 +54,6 @@ get_message_type_support_handle() return *handle; } -/// Specialization for when MessageT is an adapted type using rclcpp::TypeAdapter. template constexpr typename std::enable_if_t< @@ -63,12 +72,9 @@ get_message_type_support_handle() return *handle; } -/// Specialization for when MessageT is not a ROS message nor an adapted type. -/** - * 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. - */ +// 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 > @@ -85,6 +91,8 @@ 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 30305ad2ff..7d7f787fe8 100644 --- a/rclcpp/include/rclcpp/graph_listener.hpp +++ b/rclcpp/include/rclcpp/graph_listener.hpp @@ -24,6 +24,7 @@ #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" @@ -127,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 @@ -187,7 +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(); + 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 index 010c127a07..594234657c 100644 --- a/rclcpp/include/rclcpp/guard_condition.hpp +++ b/rclcpp/include/rclcpp/guard_condition.hpp @@ -40,30 +40,34 @@ class GuardCondition * 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( - rclcpp::Context::SharedPtr context = - rclcpp::contexts::get_global_default_context()); + 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 context used when creating this guard condition. + /// Return the underlying rcl guard condition structure. RCLCPP_PUBLIC - rclcpp::Context::SharedPtr - get_context() const; + 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; - /// Notify the wait set waiting on this condition, if any, that the condition had been met. + /// 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. @@ -89,10 +93,44 @@ class GuardCondition 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: - rclcpp::Context::SharedPtr context_; 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 diff --git a/rclcpp/include/rclcpp/init_options.hpp b/rclcpp/include/rclcpp/init_options.hpp index 6f87904f25..ab1346db32 100644 --- a/rclcpp/include/rclcpp/init_options.hpp +++ b/rclcpp/include/rclcpp/init_options.hpp @@ -29,7 +29,7 @@ class InitOptions { public: /// If true, the context will be shutdown on SIGINT by the signal handler (if it was installed). - bool shutdown_on_sigint = true; + bool shutdown_on_signal = true; /// Constructor /** diff --git a/rclcpp/include/rclcpp/loaned_message.hpp b/rclcpp/include/rclcpp/loaned_message.hpp index 4dd396e033..34a1633ef4 100644 --- a/rclcpp/include/rclcpp/loaned_message.hpp +++ b/rclcpp/include/rclcpp/loaned_message.hpp @@ -31,21 +31,20 @@ namespace rclcpp template> class LoanedMessage { +public: using MessageAllocatorTraits = rclcpp::allocator::AllocRebind; using MessageAllocator = typename MessageAllocatorTraits::allocator_type; -public: /// Constructor of the LoanedMessage class. /** * The constructor of this class allocates memory for a given message type * and associates this with a given publisher. * - * Given the publisher instance, a case differentiation is being performaned - * which decides whether the underlying middleware is able to allocate the appropriate - * memory for this message type or not. - * In the case that the middleware can not loan messages, the passed in allocator instance - * is being used to allocate the message within the scope of this class. - * Otherwise, the allocator is being ignored and the allocation is solely performaned + * 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. @@ -53,12 +52,12 @@ class LoanedMessage * 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 can not allocate messages + * \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, - std::allocator allocator) + MessageAllocator allocator) : pub_(pub), message_(nullptr), message_allocator_(std::move(allocator)) @@ -82,42 +81,14 @@ class LoanedMessage } } - /// Constructor of the LoanedMessage class. - /** - * The constructor of this class allocates memory for a given message type - * and associates this with a given publisher. - * - * Given the publisher instance, a case differentiation is being performaned - * which decides whether the underlying middleware is able to allocate the appropriate - * memory for this message type or not. - * In the case that the middleware can not loan messages, the passed in allocator instance - * is being used to allocate the message within the scope of this class. - * Otherwise, the allocator is being ignored and the allocation is solely performaned - * 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 can not allocate messages - * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw. - */ - [[ - deprecated("used the LoanedMessage constructor that does not use a shared_ptr to the allocator") - ]] - LoanedMessage( - const rclcpp::PublisherBase * pub, - std::shared_ptr> allocator) - : LoanedMessage(*pub, *allocator) - {} - /// 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. /** diff --git a/rclcpp/include/rclcpp/logger.hpp b/rclcpp/include/rclcpp/logger.hpp index e1ab4945f3..ee244fd988 100644 --- a/rclcpp/include/rclcpp/logger.hpp +++ b/rclcpp/include/rclcpp/logger.hpp @@ -15,8 +15,10 @@ #ifndef RCLCPP__LOGGER_HPP_ #define RCLCPP__LOGGER_HPP_ +#include #include #include +#include #include "rclcpp/visibility_control.hpp" @@ -76,18 +78,46 @@ 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 \ref rcl_logging_get_logging_directory. + * 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: @@ -122,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 @@ -157,13 +185,7 @@ 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. /** @@ -174,6 +196,24 @@ class Logger 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/memory_strategy.hpp b/rclcpp/include/rclcpp/memory_strategy.hpp index da79643e4e..fb5ba2a63f 100644 --- a/rclcpp/include/rclcpp/memory_strategy.hpp +++ b/rclcpp/include/rclcpp/memory_strategy.hpp @@ -64,9 +64,11 @@ class RCLCPP_PUBLIC MemoryStrategy virtual void clear_handles() = 0; virtual void remove_null_handles(rcl_wait_set_t * wait_set) = 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( @@ -98,52 +100,52 @@ class RCLCPP_PUBLIC MemoryStrategy static rclcpp::SubscriptionBase::SharedPtr get_subscription_by_handle( - std::shared_ptr subscriber_handle, + const std::shared_ptr & subscriber_handle, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); static rclcpp::ServiceBase::SharedPtr get_service_by_handle( - std::shared_ptr service_handle, + const std::shared_ptr & service_handle, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); static rclcpp::ClientBase::SharedPtr get_client_by_handle( - std::shared_ptr client_handle, + const std::shared_ptr & client_handle, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); static rclcpp::TimerBase::SharedPtr get_timer_by_handle( - std::shared_ptr timer_handle, + const std::shared_ptr & timer_handle, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); static rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_by_group( - rclcpp::CallbackGroup::SharedPtr group, + const rclcpp::CallbackGroup::SharedPtr & group, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); static rclcpp::CallbackGroup::SharedPtr get_group_by_subscription( - rclcpp::SubscriptionBase::SharedPtr subscription, + const rclcpp::SubscriptionBase::SharedPtr & subscription, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); static rclcpp::CallbackGroup::SharedPtr get_group_by_service( - rclcpp::ServiceBase::SharedPtr service, + const rclcpp::ServiceBase::SharedPtr & service, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); static rclcpp::CallbackGroup::SharedPtr get_group_by_client( - rclcpp::ClientBase::SharedPtr client, + const rclcpp::ClientBase::SharedPtr & client, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); static rclcpp::CallbackGroup::SharedPtr get_group_by_timer( - rclcpp::TimerBase::SharedPtr timer, + const rclcpp::TimerBase::SharedPtr & timer, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); static rclcpp::CallbackGroup::SharedPtr get_group_by_waitable( - rclcpp::Waitable::SharedPtr waitable, + const rclcpp::Waitable::SharedPtr & waitable, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); }; diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 7e5f402f8a..930bf419f1 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -42,7 +42,9 @@ #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" @@ -56,6 +58,7 @@ #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" @@ -227,38 +230,53 @@ class Node : public std::enable_shared_from_this ) ); - /// Create a timer. + /// Create a wall timer that uses the wall 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. + * \param[in] autostart The state of the clock on initialization. */ 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 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::GenericTimer::SharedPtr + create_timer( std::chrono::duration period, CallbackT callback, rclcpp::CallbackGroup::SharedPtr group = nullptr); /// Create and return a Client. /** - * \param[in] service_name The topic to service on. - * \param[in] qos_profile rmw_qos_profile_t Quality of service profile for client. - * \param[in] group Callback group to call the service. + * \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, + const rclcpp::QoS & qos = rclcpp::ServicesQoS(), rclcpp::CallbackGroup::SharedPtr group = nullptr); /// Create and return a Service. /** * \param[in] service_name The topic to service on. * \param[in] callback User-defined callback function. - * \param[in] qos_profile rmw_qos_profile_t Quality of service profile for client. + * \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. */ @@ -267,7 +285,41 @@ class Node : public std::enable_shared_from_this create_service( const std::string & service_name, CallbackT && callback, - const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default, + 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 + 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. @@ -308,12 +360,14 @@ class Node : public std::enable_shared_from_this * `%callback_group`. * \return Shared pointer to the created generic subscription. */ - template> + 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, - std::function)> callback, + CallbackT && callback, const rclcpp::SubscriptionOptionsWithAllocator & options = ( rclcpp::SubscriptionOptionsWithAllocator() ) @@ -336,11 +390,21 @@ class Node : public std::enable_shared_from_this * * If `ignore_override` is `true`, the parameter override will be ignored. * - * This method, if successful, will result in any callback registered with - * add_on_set_parameters_callback to be called. + * 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. * @@ -395,22 +459,6 @@ class Node : public std::enable_shared_from_this rcl_interfaces::msg::ParameterDescriptor{}, bool ignore_override = false); - /// Declare a parameter - [[deprecated( - "declare_parameter() with only a name is deprecated and will be deleted in the future.\n" \ - "If you want to declare a parameter that won't change type without a default value use:\n" \ - "`node->declare_parameter(name)`, where e.g. ParameterT=int64_t.\n\n" \ - "If you want to declare a parameter that can dynamically change type use:\n" \ - "```\n" \ - "rcl_interfaces::msg::ParameterDescriptor descriptor;\n" \ - "descriptor.dynamic_typing = true;\n" \ - "node->declare_parameter(name, rclcpp::ParameterValue{}, descriptor);\n" \ - "```" - )]] - RCLCPP_PUBLIC - const rclcpp::ParameterValue & - declare_parameter(const std::string & name); - /// Declare and initialize a parameter with a type. /** * See the non-templated declare_parameter() on this class for details. @@ -473,11 +521,22 @@ class Node : public std::enable_shared_from_this * 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. + * `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. @@ -515,8 +574,9 @@ class Node : public std::enable_shared_from_this /// Undeclare a previously declared parameter. /** - * This method will not cause a callback registered with - * add_on_set_parameters_callback to be called. + * 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 @@ -550,11 +610,24 @@ class Node : public std::enable_shared_from_this * Parameter overrides are ignored by set_parameter. * * This method will result in any callback registered with - * add_on_set_parameters_callback to be called. + * `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. @@ -591,11 +664,25 @@ class Node : public std::enable_shared_from_this * corresponding SetParametersResult in the vector returned by this function. * * This method will result in any callback registered with - * add_on_set_parameters_callback to be called, once for each parameter. + * `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. * @@ -622,11 +709,25 @@ class Node : public std::enable_shared_from_this * If the exception is thrown then none of the parameters will have been set. * * This method will result in any callback registered with - * add_on_set_parameters_callback to be called, just one time. + * `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. * @@ -646,17 +747,21 @@ class Node : public std::enable_shared_from_this /** * 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 an exception, and instead return a default initialized - * rclcpp::Parameter, which has a type of + * 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 @@ -720,14 +825,32 @@ class Node : public std::enable_shared_from_this 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_parameters(), this method may throw the + * 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. + * not allowed, and may throw the rclcpp::exceptions::ParameterUninitializedException exception. * - * Also like get_parameters(), if undeclared parameters are allowed and the + * 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. @@ -737,6 +860,8 @@ class Node : public std::enable_shared_from_this * \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 @@ -851,18 +976,98 @@ class Node : public std::enable_shared_from_this /// Return a list of parameters with any of the given prefixes, up to the given depth. /** - * \todo: properly document and test this method. + * 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; + using PreSetParametersCallbackHandle = + rclcpp::node_interfaces::PreSetParametersCallbackHandle; + using PreSetParametersCallbackType = + rclcpp::node_interfaces::NodeParametersInterface::PreSetParametersCallbackType; + using OnSetParametersCallbackHandle = rclcpp::node_interfaces::OnSetParametersCallbackHandle; - using OnParametersSetCallbackType = - rclcpp::node_interfaces::NodeParametersInterface::OnParametersSetCallbackType; + 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. + /** + * 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. + */ + RCLCPP_PUBLIC + RCUTILS_WARN_UNUSED + PreSetParametersCallbackHandle::SharedPtr + add_pre_set_parameters_callback(PreSetParametersCallbackType callback); - /// Add a callback for when parameters are being set. + /// 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 @@ -870,6 +1075,9 @@ class Node : public std::enable_shared_from_this * 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 @@ -901,6 +1109,8 @@ class Node : public std::enable_shared_from_this * 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 @@ -929,7 +1139,80 @@ class Node : public std::enable_shared_from_this RCLCPP_PUBLIC RCUTILS_WARN_UNUSED OnSetParametersCallbackHandle::SharedPtr - add_on_set_parameters_callback(OnParametersSetCallbackType callback); + 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`. /** @@ -958,6 +1241,18 @@ class Node : public std::enable_shared_from_this 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 + 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. @@ -1021,6 +1316,26 @@ class Node : public std::enable_shared_from_this 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 @@ -1173,6 +1488,11 @@ class Node : public std::enable_shared_from_this rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr get_node_time_source_interface(); + /// Return the Node's internal NodeTypeDescriptionsInterface implementation. + RCLCPP_PUBLIC + 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 @@ -1305,11 +1625,18 @@ class Node : public std::enable_shared_from_this 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_; + + 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 df6039cf49..5b81bdcba4 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -40,6 +40,7 @@ #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" @@ -110,9 +111,27 @@ typename rclcpp::WallTimer::SharedPtr Node::create_wall_timer( std::chrono::duration period, CallbackT callback, - rclcpp::CallbackGroup::SharedPtr group) + rclcpp::CallbackGroup::SharedPtr group, + bool autostart) { return rclcpp::create_wall_timer( + period, + std::move(callback), + group, + this->node_base_.get(), + this->node_timers_.get(), + autostart); +} + +template +typename rclcpp::GenericTimer::SharedPtr +Node::create_timer( + std::chrono::duration period, + CallbackT callback, + rclcpp::CallbackGroup::SharedPtr group) +{ + return rclcpp::create_timer( + this->get_clock(), period, std::move(callback), group, @@ -124,7 +143,7 @@ template typename Client::SharedPtr Node::create_client( const std::string & service_name, - const rmw_qos_profile_t & qos_profile, + const rclcpp::QoS & qos, rclcpp::CallbackGroup::SharedPtr group) { return rclcpp::create_client( @@ -132,7 +151,7 @@ Node::create_client( node_graph_, node_services_, extend_name_with_sub_namespace(service_name, this->get_sub_namespace()), - qos_profile, + qos, group); } @@ -141,7 +160,7 @@ typename rclcpp::Service::SharedPtr Node::create_service( const std::string & service_name, CallbackT && callback, - const rmw_qos_profile_t & qos_profile, + const rclcpp::QoS & qos, rclcpp::CallbackGroup::SharedPtr group) { return rclcpp::create_service( @@ -149,7 +168,26 @@ Node::create_service( node_services_, extend_name_with_sub_namespace(service_name, this->get_sub_namespace()), std::forward(callback), - qos_profile, + 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); } @@ -170,13 +208,13 @@ Node::create_generic_publisher( ); } -template +template std::shared_ptr Node::create_generic_subscription( const std::string & topic_name, const std::string & topic_type, const rclcpp::QoS & qos, - std::function)> callback, + CallbackT && callback, const rclcpp::SubscriptionOptionsWithAllocator & options) { return rclcpp::create_generic_subscription( @@ -184,7 +222,7 @@ Node::create_generic_subscription( extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()), topic_type, qos, - std::move(callback), + std::forward(callback), options ); } @@ -220,12 +258,16 @@ Node::declare_parameter( // 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(); + try { + return this->declare_parameter( + name, + value.get_type(), + parameter_descriptor, + ignore_override + ).get(); + } catch (const ParameterTypeException &) { + throw exceptions::UninitializedStaticallyTypedParameterException(name); + } } template @@ -309,6 +351,17 @@ Node::get_parameter_or( 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. 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 8a19a6c9df..6173a08d50 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp @@ -15,7 +15,7 @@ #ifndef RCLCPP__NODE_INTERFACES__NODE_BASE_HPP_ #define RCLCPP__NODE_INTERFACES__NODE_BASE_HPP_ -#include +#include #include #include #include @@ -34,11 +34,18 @@ 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(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, @@ -46,7 +53,8 @@ class NodeBase : public NodeBaseInterface rclcpp::Context::SharedPtr context, const rcl_node_options_t & rcl_node_options, bool use_intra_process_default, - bool enable_topic_statistics_default); + bool enable_topic_statistics_default, + rclcpp::CallbackGroup::SharedPtr default_callback_group = nullptr); RCLCPP_PUBLIC virtual @@ -113,13 +121,18 @@ class NodeBase : public NodeBaseInterface std::atomic_bool & get_associated_with_executor_atomic() override; + [[deprecated("Use get_shared_notify_guard_condition or trigger_notify_guard_condition instead")]] RCLCPP_PUBLIC - rcl_guard_condition_t * + rclcpp::GuardCondition & get_notify_guard_condition() override; RCLCPP_PUBLIC - std::unique_lock - acquire_notify_guard_condition_lock() const override; + rclcpp::GuardCondition::SharedPtr + get_shared_notify_guard_condition() override; + + RCLCPP_PUBLIC + void + trigger_notify_guard_condition() override; RCLCPP_PUBLIC bool @@ -149,7 +162,7 @@ class NodeBase : public NodeBaseInterface /// 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 ebe7b1b98e..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 @@ -142,23 +144,36 @@ 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 + 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 - std::unique_lock - acquire_notify_guard_condition_lock() const = 0; + void + trigger_notify_guard_condition() = 0; /// Return the default preference for using intra process communication. RCLCPP_PUBLIC @@ -183,4 +198,6 @@ class NodeBaseInterface } // 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 fd9a34a907..fb2e0670a7 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_clock.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_clock.hpp @@ -15,6 +15,7 @@ #ifndef RCLCPP__NODE_INTERFACES__NODE_CLOCK_HPP_ #define RCLCPP__NODE_INTERFACES__NODE_CLOCK_HPP_ +#include "rcl/time.h" #include "rclcpp/clock.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" @@ -42,7 +43,8 @@ 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::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + rcl_clock_type_t clock_type); RCLCPP_PUBLIC virtual @@ -67,7 +69,7 @@ class NodeClock : public NodeClockInterface rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_; rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_; - rclcpp::Clock::SharedPtr ros_clock_; + 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 8965a371d8..744eef4ce6 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_clock_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_clock_interface.hpp @@ -17,6 +17,7 @@ #include "rclcpp/clock.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp" #include "rclcpp/visibility_control.hpp" namespace rclcpp @@ -50,4 +51,6 @@ class NodeClockInterface } // 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 4036b1b754..863dbee1bf 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp @@ -15,11 +15,14 @@ #ifndef RCLCPP__NODE_INTERFACES__NODE_GRAPH_HPP_ #define RCLCPP__NODE_INTERFACES__NODE_GRAPH_HPP_ +#include #include +#include #include #include #include #include +#include #include #include @@ -70,10 +73,34 @@ class NodeGraph : public NodeGraphInterface 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 std::vector 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; @@ -86,6 +113,14 @@ class NodeGraph : public NodeGraphInterface size_t 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 const rcl_guard_condition_t * get_graph_guard_condition() const override; diff --git a/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp index 2234c91826..80abc308c1 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include @@ -28,6 +29,7 @@ #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" @@ -55,7 +57,8 @@ class TopicEndpointInfo 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) + 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()); } @@ -120,6 +123,16 @@ class TopicEndpointInfo 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_; @@ -127,6 +140,7 @@ class TopicEndpointInfo rclcpp::EndpointType endpoint_type_; std::array endpoint_gid_; rclcpp::QoS qos_profile_; + rosidl_type_hash_t topic_type_hash_; }; namespace node_interfaces @@ -146,7 +160,9 @@ class NodeGraphInterface /** * 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 used and do not have remap rules applied. + * 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 */ @@ -160,7 +176,9 @@ 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 used and do not have remap rules applied. + * 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 @@ -170,7 +188,9 @@ class NodeGraphInterface /// 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. + * 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 @@ -182,18 +202,85 @@ class NodeGraphInterface 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 used and do not have remap rules applied. + * 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 used and do not have remap rules applied. + * The returned names are the actual names after remap rules applied. */ RCLCPP_PUBLIC virtual @@ -218,6 +305,24 @@ class NodeGraphInterface 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 @@ -283,6 +388,8 @@ class NodeGraphInterface /// 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 @@ -293,6 +400,8 @@ class NodeGraphInterface /// 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 @@ -304,4 +413,6 @@ class NodeGraphInterface } // 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 ea91064be7..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,7 +38,7 @@ 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 @@ -49,13 +52,21 @@ class NodeLogging : public NodeLoggingInterface 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 669da55ee6..7f9776cf36 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_logging_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_logging_interface.hpp @@ -19,6 +19,8 @@ #include "rclcpp/logger.hpp" #include "rclcpp/macros.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 @@ -37,21 +39,34 @@ class NodeLoggingInterface ~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. */ + /** + * \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 552fbc6979..ffbb400c11 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp @@ -15,9 +15,10 @@ #ifndef RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_HPP_ #define RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_HPP_ +#include #include #include -#include +#include #include #include @@ -84,6 +85,16 @@ 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( const node_interfaces::NodeBaseInterface::SharedPtr node_base, @@ -103,25 +114,6 @@ class NodeParameters : public NodeParametersInterface virtual ~NodeParameters(); -// This is overriding a deprecated method, so we need to ignore the deprecation warning here. -// Users of the method will still get a warning! -#ifndef _WIN32 -# pragma GCC diagnostic push -# pragma GCC diagnostic ignored "-Wdeprecated-declarations" -#else -# pragma warning(push) -# pragma warning(disable: 4996) -#endif - [[deprecated(RCLCPP_INTERNAL_NODE_PARAMETERS_INTERFACE_DEPRECATE_DECLARE)]] - RCLCPP_PUBLIC - const rclcpp::ParameterValue & - declare_parameter(const std::string & name) override; -#ifndef _WIN32 -# pragma GCC diagnostic pop -#else -# pragma warning(pop) -#endif - RCLCPP_PUBLIC const rclcpp::ParameterValue & declare_parameter( @@ -190,20 +182,48 @@ class NodeParameters : public NodeParametersInterface rcl_interfaces::msg::ListParametersResult 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(OnParametersSetCallbackType callback) override; + 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 void 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 CallbacksContainerType = std::list; + 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) @@ -215,9 +235,11 @@ class NodeParameters : public NodeParametersInterface // declare_parameter, etc). In those cases, this will be set to false. bool parameter_modification_enabled_{true}; - OnParametersSetCallbackType on_parameters_set_callback_ = nullptr; + PreSetCallbacksHandleContainer pre_set_parameters_callback_container_; + + OnSetCallbacksHandleContainer on_set_parameters_callback_container_; - CallbacksContainerType on_parameters_set_callback_container_; + PostSetCallbacksHandleContainer post_set_parameters_callback_container_; std::map parameters_; diff --git a/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp index d9981516c7..1cf10c1a97 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp @@ -15,8 +15,8 @@ #ifndef RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_INTERFACE_HPP_ #define RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_INTERFACE_HPP_ +#include #include -#include #include #include @@ -25,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" @@ -33,28 +34,37 @@ 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 OnParametersSetCallbackType = + using OnSetParametersCallbackType = std::function< rcl_interfaces::msg::SetParametersResult( const std::vector &)>; - OnParametersSetCallbackType callback; + OnSetParametersCallbackType callback; }; -#define RCLCPP_INTERNAL_NODE_PARAMETERS_INTERFACE_DEPRECATE_DECLARE \ - "declare_parameter() with only a name is deprecated and will be deleted in the future.\n" \ - "If you want to declare a parameter that won't change type without a default value use:\n" \ - "`node_params->declare_parameter(name, type)`, with e.g. type=rclcpp::PARAMETER_INTEGER.\n\n" \ - "If you want to declare a parameter that can dynamically change type use:\n" \ - "```\n" \ - "rcl_interfaces::msg::ParameterDescriptor descriptor;\n" \ - "descriptor.dynamic_typing = true;\n" \ - "node_params->declare_parameter(name, rclcpp::ParameterValue{}, descriptor);\n" \ - "```" +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 @@ -66,15 +76,6 @@ class NodeParametersInterface virtual ~NodeParametersInterface() = default; - /// Declare a parameter. - /** - * \sa rclcpp::Node::declare_parameter - */ - [[deprecated(RCLCPP_INTERNAL_NODE_PARAMETERS_INTERFACE_DEPRECATE_DECLARE)]] - virtual - const rclcpp::ParameterValue & - declare_parameter(const std::string & name) = 0; - /// Declare and initialize a parameter. /** * \sa rclcpp::Node::declare_parameter @@ -205,16 +206,46 @@ class NodeParametersInterface rcl_interfaces::msg::ListParametersResult list_parameters(const std::vector & prefixes, uint64_t depth) const = 0; - using OnParametersSetCallbackType = OnSetParametersCallbackHandle::OnParametersSetCallbackType; + using OnSetParametersCallbackType = OnSetParametersCallbackHandle::OnSetParametersCallbackType; + using PostSetParametersCallbackType = + PostSetParametersCallbackHandle::PostSetParametersCallbackType; + using PreSetParametersCallbackType = PreSetParametersCallbackHandle::PreSetParametersCallbackType; - /// Add a callback for when parameters are being set. + /// 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(OnParametersSetCallbackType callback) = 0; + 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`. /** @@ -225,6 +256,15 @@ class NodeParametersInterface 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 + 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 @@ -235,4 +275,6 @@ class NodeParametersInterface } // 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_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp index ab6cdab42e..ff58ef36b6 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp @@ -20,6 +20,7 @@ #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" @@ -62,4 +63,6 @@ class NodeServicesInterface } // 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 index 1c9afebe1f..1594d5f8e5 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_time_source.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_time_source.hpp @@ -48,7 +48,7 @@ class NodeTimeSource : public NodeTimeSourceInterface 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::RosoutQoS(), + const rclcpp::QoS & qos = rclcpp::ClockQoS(), bool use_clock_thread = true ); diff --git a/rclcpp/include/rclcpp/node_interfaces/node_time_source_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_time_source_interface.hpp index 5b7065193e..3783e5d83a 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_time_source_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_time_source_interface.hpp @@ -16,6 +16,7 @@ #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 @@ -37,4 +38,6 @@ class NodeTimeSourceInterface } // 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_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_timers_interface.hpp index 4573d3d02b..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" @@ -47,4 +48,6 @@ class NodeTimersInterface } // 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 b4d8c5b2f6..33a87732d6 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_topics.hpp @@ -20,12 +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 diff --git a/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp index ca69e86e73..aa5530f8dd 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp @@ -15,8 +15,6 @@ #ifndef RCLCPP__NODE_INTERFACES__NODE_TOPICS_INTERFACE_HPP_ #define RCLCPP__NODE_INTERFACES__NODE_TOPICS_INTERFACE_HPP_ -#include -#include #include #include "rcl/publisher.h" @@ -24,6 +22,7 @@ #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" @@ -97,4 +96,6 @@ class NodeTopicsInterface } // 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_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_waitables_interface.hpp index 0faae1da62..fd0029a89b 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_waitables_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_waitables_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/visibility_control.hpp" #include "rclcpp/waitable.hpp" @@ -54,4 +55,6 @@ class NodeWaitablesInterface } // 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 index 73a2c701af..71b0d997cf 100644 --- a/rclcpp/include/rclcpp/node_options.hpp +++ b/rclcpp/include/rclcpp/node_options.hpp @@ -19,6 +19,7 @@ #include #include +#include "rcl/time.h" #include "rcl/node_options.h" #include "rclcpp/context.hpp" #include "rclcpp/contexts/default_context.hpp" @@ -42,12 +43,15 @@ class NodeOptions * - 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 @@ -230,6 +234,24 @@ class NodeOptions 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 @@ -246,6 +268,19 @@ class NodeOptions 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 & @@ -349,6 +384,9 @@ class NodeOptions * 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. */ @@ -397,10 +435,14 @@ class NodeOptions 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) ); diff --git a/rclcpp/include/rclcpp/parameter.hpp b/rclcpp/include/rclcpp/parameter.hpp index 68147ed731..85f088d025 100644 --- a/rclcpp/include/rclcpp/parameter.hpp +++ b/rclcpp/include/rclcpp/parameter.hpp @@ -264,6 +264,7 @@ get_value_helper(const rclcpp::Parameter * parameter) } // namespace detail +/// \cond template decltype(auto) Parameter::get_value() const @@ -275,6 +276,7 @@ Parameter::get_value() const throw exceptions::InvalidParameterTypeException(this->name_, ex.what()); } } +/// \endcond } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/parameter_client.hpp b/rclcpp/include/rclcpp/parameter_client.hpp index 4a15a35141..67fbae5054 100644 --- a/rclcpp/include/rclcpp/parameter_client.hpp +++ b/rclcpp/include/rclcpp/parameter_client.hpp @@ -39,6 +39,7 @@ #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" @@ -58,7 +59,7 @@ class AsyncParametersClient * \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 rmw qos profile to use to subscribe + * \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 @@ -68,21 +69,20 @@ 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, + const rclcpp::QoS & qos_profile = rclcpp::ParametersQoS(), rclcpp::CallbackGroup::SharedPtr group = nullptr); - /// Constructor /** - * \param[in] node The async paramters client will be added to this node. + * \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 rmw qos profile to use to subscribe + * \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 - AsyncParametersClient( + explicit AsyncParametersClient( const std::shared_ptr 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(), @@ -94,18 +94,17 @@ class AsyncParametersClient group) {} - /// Constructor /** - * \param[in] node The async paramters client will be added to this node. + * \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 rmw qos profile to use to subscribe + * \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 - AsyncParametersClient( + 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(), @@ -185,7 +184,10 @@ class AsyncParametersClient /** * This function filters the parameters to be set based on the node name. * - * \param yaml_filename the full name of the yaml file + * 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 */ @@ -208,9 +210,7 @@ class AsyncParametersClient typename rclcpp::Subscription::SharedPtr on_parameter_event( CallbackT && callback, - const rclcpp::QoS & qos = ( - rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_parameter_events)) - ), + const rclcpp::QoS & qos = rclcpp::ParameterEventsQoS(), const rclcpp::SubscriptionOptionsWithAllocator & options = ( rclcpp::SubscriptionOptionsWithAllocator() )) @@ -235,9 +235,7 @@ class AsyncParametersClient on_parameter_event( NodeT && node, CallbackT && callback, - const rclcpp::QoS & qos = ( - rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_parameter_events)) - ), + const rclcpp::QoS & qos = rclcpp::ParameterEventsQoS(), const rclcpp::SubscriptionOptionsWithAllocator & options = ( rclcpp::SubscriptionOptionsWithAllocator() )) @@ -308,7 +306,7 @@ class SyncParametersClient explicit SyncParametersClient( std::shared_ptr 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( std::make_shared(), node, @@ -321,7 +319,7 @@ class SyncParametersClient rclcpp::Executor::SharedPtr executor, std::shared_ptr 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(), @@ -333,10 +331,10 @@ class SyncParametersClient {} template - SyncParametersClient( + explicit SyncParametersClient( 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( std::make_shared(), node, @@ -349,7 +347,7 @@ class 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(), @@ -368,7 +366,7 @@ class SyncParametersClient 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_ = diff --git a/rclcpp/include/rclcpp/parameter_event_handler.hpp b/rclcpp/include/rclcpp/parameter_event_handler.hpp index c7b781f0ec..589fb1144c 100644 --- a/rclcpp/include/rclcpp/parameter_event_handler.hpp +++ b/rclcpp/include/rclcpp/parameter_event_handler.hpp @@ -67,46 +67,57 @@ struct ParameterEventCallbackHandle * The first step is to instantiate a ParameterEventHandler, providing a ROS node to use * to create any required subscriptions: * - * auto param_handler = std::make_shared(node); + * ```cpp + * auto param_handler = std::make_shared(node); + * ``` * * Next, you can supply a callback to the add_parameter_callback method, as follows: * - * 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); + * ```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: * - * 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"); + * ```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, call remove_parameter_callback, passing the handle returned - * from add_parameter_callback: + * To remove a parameter callback, reset the callback handle smart pointer or call + * remove_parameter_callback, passing the handle returned from add_parameter_callback: * - * param_handler->remove_parameter_callback(handle2); + * ```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. @@ -114,47 +125,54 @@ struct ParameterEventCallbackHandle * is convenient to use a regular expression on the node names or namespaces of interest. * For example: * - * 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::ParameterEventsSubscriber::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()); - * } + * ```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::ParameterEventsSubscriber::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()); - * } + * // 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); + * } + * }; + * 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). * - * To remove a parameter event callback, use: + * Note: the callback handle returned from add_parameter_event_callback must be captured or + * the callback will immediately be unregistered. * - * param_handler->remove_event_parameter_callback(handle); + * To remove a parameter event callback, reset the callback smart pointer or use: + * + * ```cpp + * param_handler->remove_event_parameter_callback(handle3); + * ``` */ class ParameterEventHandler { @@ -165,17 +183,21 @@ class ParameterEventHandler * \param[in] qos The QoS settings to use for any subscriptions. */ template - ParameterEventHandler( + 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)) { - 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, - std::bind(&ParameterEventHandler::event_callback, this, std::placeholders::_1)); + [callbacks = callbacks_](const rcl_interfaces::msg::ParameterEvent & event) { + callbacks->event_callback(event); + }); } using ParameterEventCallbackType = @@ -185,10 +207,14 @@ class ParameterEventHandler /** * 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); @@ -208,12 +234,17 @@ class ParameterEventHandler /** * 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, @@ -249,8 +280,8 @@ class ParameterEventHandler get_parameter_from_event( const rcl_interfaces::msg::ParameterEvent & event, rclcpp::Parameter & parameter, - const std::string parameter_name, - const std::string node_name = ""); + const std::string & parameter_name, + const std::string & node_name = ""); /// Get an rclcpp::Parameter from parameter event /** @@ -264,13 +295,14 @@ class ParameterEventHandler * \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 = ""); + const std::string & parameter_name, + const std::string & node_name = ""); /// Get all rclcpp::Parameter values from a parameter event /** @@ -285,17 +317,6 @@ class ParameterEventHandler using CallbacksContainerType = std::list; protected: - /// Callback for parameter events subscriptions. - RCLCPP_PUBLIC - void - event_callback(const rcl_interfaces::msg::ParameterEvent & event); - - // 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_; - // *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 @@ -319,18 +340,34 @@ class ParameterEventHandler }; // *INDENT-ON* - // Map container for registered parameters - std::unordered_map< - std::pair, - CallbacksContainerType, - StringPairHash - > parameter_callbacks_; + struct Callbacks + { + std::recursive_mutex mutex_; - rclcpp::Subscription::SharedPtr event_subscription_; + // Map container for registered parameters + std::unordered_map< + std::pair, + CallbacksContainerType, + StringPairHash + > parameter_callbacks_; - std::list event_callbacks_; + std::list event_callbacks_; + + /// Callback for parameter events subscriptions. + RCLCPP_PUBLIC + void + event_callback(const rcl_interfaces::msg::ParameterEvent & event); + }; - std::recursive_mutex mutex_; + 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 diff --git a/rclcpp/include/rclcpp/parameter_events_filter.hpp b/rclcpp/include/rclcpp/parameter_events_filter.hpp index 3aa70d8a85..6960a1bccf 100644 --- a/rclcpp/include/rclcpp/parameter_events_filter.hpp +++ b/rclcpp/include/rclcpp/parameter_events_filter.hpp @@ -48,7 +48,7 @@ class ParameterEventsFilter * * Example Usage: * - * If you have recieved a parameter event and are only interested in parameters foo and + * 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 diff --git a/rclcpp/include/rclcpp/parameter_map.hpp b/rclcpp/include/rclcpp/parameter_map.hpp index 9cfcdaaacf..17e2128a7b 100644 --- a/rclcpp/include/rclcpp/parameter_map.hpp +++ b/rclcpp/include/rclcpp/parameter_map.hpp @@ -35,11 +35,13 @@ 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); +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. @@ -51,11 +53,20 @@ 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); +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 diff --git a/rclcpp/include/rclcpp/parameter_service.hpp b/rclcpp/include/rclcpp/parameter_service.hpp index 143a5223c8..54353e9579 100644 --- a/rclcpp/include/rclcpp/parameter_service.hpp +++ b/rclcpp/include/rclcpp/parameter_service.hpp @@ -28,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" @@ -40,11 +41,11 @@ class ParameterService RCLCPP_SMART_PTR_DEFINITIONS(ParameterService) RCLCPP_PUBLIC - explicit ParameterService( + ParameterService( const std::shared_ptr node_base, const std::shared_ptr node_services, rclcpp::node_interfaces::NodeParametersInterface * node_params, - const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters); + const rclcpp::QoS & qos_profile = rclcpp::ParametersQoS()); private: rclcpp::Service::SharedPtr get_parameters_service_; diff --git a/rclcpp/include/rclcpp/parameter_value.hpp b/rclcpp/include/rclcpp/parameter_value.hpp index f74c36a866..549429aa85 100644 --- a/rclcpp/include/rclcpp/parameter_value.hpp +++ b/rclcpp/include/rclcpp/parameter_value.hpp @@ -24,6 +24,7 @@ #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 @@ -358,7 +359,7 @@ class ParameterValue /// Return the value of a parameter as a string RCLCPP_PUBLIC std::string -to_string(const ParameterValue & type); +to_string(const ParameterValue & value); } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 346a87f3f1..22825daf89 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -20,16 +20,21 @@ #include #include #include +#include #include #include "rcl/error_handling.h" #include "rcl/publisher.h" #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" @@ -81,7 +86,6 @@ class Publisher : public PublisherBase /// MessageT::custom_type if MessageT is a TypeAdapter, otherwise just MessageT. using PublishedType = 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 PublishedTypeAllocatorTraits = allocator::AllocRebind; @@ -92,21 +96,11 @@ class Publisher : public PublisherBase using ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type; using ROSMessageTypeDeleter = allocator::Deleter; - using MessageAllocatorTraits - [[deprecated("use PublishedTypeAllocatorTraits")]] = - PublishedTypeAllocatorTraits; - using MessageAllocator - [[deprecated("use PublishedTypeAllocator")]] = - PublishedTypeAllocator; - using MessageDeleter - [[deprecated("use PublishedTypeDeleter")]] = - PublishedTypeDeleter; - using MessageUniquePtr - [[deprecated("use std::unique_ptr")]] = - std::unique_ptr; - using MessageSharedPtr - [[deprecated("use std::shared_ptr")]] = - std::shared_ptr; + using BufferSharedPtr = typename rclcpp::experimental::buffers::IntraProcessBuffer< + ROSMessageType, + ROSMessageTypeAllocator, + ROSMessageTypeDeleter + >::SharedPtr; RCLCPP_SMART_PTR_DEFINITIONS(Publisher) @@ -118,8 +112,8 @@ class Publisher : public PublisherBase * * \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 Subcription. - * \param[in] options Options for the subscription. + * \param[in] qos QoS profile for the publisher. + * \param[in] options Options for the publisher. */ Publisher( rclcpp::node_interfaces::NodeBaseInterface * node_base, @@ -130,40 +124,16 @@ class Publisher : public PublisherBase node_base, topic, rclcpp::get_message_type_support_handle(), - options.template to_rcl_publisher_options(qos)), + 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(&published_type_deleter_, &published_type_allocator_); allocator::set_allocator_for_deleter(&ros_message_type_deleter_, &ros_message_type_allocator_); - - if (options_.event_callbacks.deadline_callback) { - this->add_event_handler( - options_.event_callbacks.deadline_callback, - RCL_PUBLISHER_OFFERED_DEADLINE_MISSED); - } - if (options_.event_callbacks.liveliness_callback) { - this->add_event_handler( - options_.event_callbacks.liveliness_callback, - RCL_PUBLISHER_LIVELINESS_LOST); - } - if (options_.event_callbacks.incompatible_qos_callback) { - this->add_event_handler( - options_.event_callbacks.incompatible_qos_callback, - RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS); - } else if (options_.use_default_callbacks) { - // Register default callback when not specified - try { - this->add_event_handler( - [this](QOSOfferedIncompatibleQoSInfo & info) { - this->default_incompatible_qos_callback(info); - }, - RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS); - } catch (UnsupportedEventTypeException & /*exc*/) { - // pass - } - } // Setup continues in the post construction method, post_init_setup(). } @@ -194,11 +164,14 @@ class Publisher : public PublisherBase throw std::invalid_argument( "intraprocess communication is not allowed with a zero qos history depth value"); } - if (qos.durability() != rclcpp::DurabilityPolicy::Volatile) { - throw std::invalid_argument( - "intraprocess communication allowed only with volatile durability"); + 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()); + uint64_t intra_process_publisher_id = ipm->add_publisher(this->shared_from_this(), buffer_); this->setup_intra_process( intra_process_publisher_id, ipm); @@ -214,7 +187,7 @@ class Publisher : public PublisherBase * 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 \sa `publish` the LoanedMessage instance is being returned to the middleware + * 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 @@ -263,10 +236,20 @@ class Publisher : public PublisherBase get_subscription_count() > get_intra_process_subscription_count(); if (inter_process_publish_needed) { - auto shared_msg = this->do_intra_process_publish_and_return_shared(std::move(msg)); + 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 { - this->do_intra_process_publish(std::move(msg)); + 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)); + } } } @@ -291,8 +274,8 @@ class Publisher : public PublisherBase { // Avoid allocating when not using intra process. if (!intra_process_is_enabled_) { - // In this case we're not using intra process. - return this->do_inter_process_publish(msg); + this->do_inter_process_publish(msg); + return; } // Otherwise we have to allocate memory in a unique_ptr and pass it along. // As the message is not const, a copy should be made. @@ -319,13 +302,33 @@ class Publisher : public PublisherBase > publish(std::unique_ptr msg) { - // TODO(wjwwood): later update this to give the unique_ptr to the intra - // process manager and let it decide if it needs to be converted or not. - // For now, convert it unconditionally and pass it the ROSMessageType - // publish function specialization. - auto unique_ros_msg = this->create_ros_message_unique_ptr(); - rclcpp::TypeAdapter::convert_to_ros_message(*msg, *unique_ros_msg); - this->publish(std::move(unique_ros_msg)); + if (!intra_process_is_enabled_) { + // In this case we're not using intra process. + 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)); + } } /// Publish a message on the topic. @@ -346,26 +349,19 @@ class Publisher : public PublisherBase > publish(const T & msg) { - // TODO(wjwwood): later update this to give the unique_ptr to the intra - // process manager and let it decide if it needs to be converted or not. - // For now, convert it unconditionally and pass it the ROSMessageType - // publish function specialization. - - // Avoid allocating when not using intra process. if (!intra_process_is_enabled_) { // Convert to the ROS message equivalent and publish it. - ROSMessageType ros_msg; - rclcpp::TypeAdapter::convert_to_ros_message(msg, ros_msg); - // In this case we're not using intra process. - return this->do_inter_process_publish(ros_msg); + 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, convert it, - // and pass it along. + + // Otherwise we have to allocate memory in a unique_ptr and pass it along. // As the message is not const, a copy should be made. // A shared_ptr could also be constructed here. - auto unique_ros_msg = this->create_ros_message_unique_ptr(); - rclcpp::TypeAdapter::convert_to_ros_message(msg, *unique_ros_msg); - this->publish(std::move(unique_ros_msg)); + auto unique_msg = this->duplicate_type_adapt_message_as_unique_ptr(msg); + this->publish(std::move(unique_msg)); } void @@ -394,10 +390,6 @@ class Publisher : public PublisherBase if (!loaned_msg.is_valid()) { throw std::runtime_error("loaned message is not valid"); } - if (intra_process_is_enabled_) { - // TODO(Karsten1987): support loaned message passed by intraprocess - throw std::runtime_error("storing loaned messages in intra process is not supported yet"); - } // verify that publisher supports loaned messages // TODO(Karsten1987): This case separation has to be done in rclcpp @@ -407,21 +399,14 @@ class Publisher : public PublisherBase 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(std::move(loaned_msg.release())); + 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->do_inter_process_publish(loaned_msg.get()); + this->publish(loaned_msg.get()); } } - [[deprecated("use get_published_type_allocator() or get_ros_message_type_allocator() instead")]] - std::shared_ptr - get_allocator() const - { - return std::make_shared(published_type_allocator_); - } - PublishedTypeAllocator get_published_type_allocator() const { @@ -438,10 +423,7 @@ class Publisher : public PublisherBase void do_inter_process_publish(const ROSMessageType & msg) { - TRACEPOINT( - rclcpp_publish, - static_cast(publisher_handle_.get()), - static_cast(&msg)); + TRACETOOLS_TRACEPOINT(rclcpp_publish, nullptr, static_cast(&msg)); auto status = rcl_publish(publisher_handle_.get(), &msg, nullptr); if (RCL_RET_PUBLISHER_INVALID == status) { @@ -476,6 +458,7 @@ class Publisher : public PublisherBase 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) { @@ -494,7 +477,7 @@ class Publisher : public PublisherBase } void - do_intra_process_publish(std::unique_ptr msg) + do_intra_process_publish(std::unique_ptr msg) { auto ipm = weak_ipm_.lock(); if (!ipm) { @@ -504,15 +487,41 @@ class Publisher : public PublisherBase 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( + ipm->template do_intra_process_publish( + intra_process_publisher_id_, + std::move(msg), + published_type_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_publish_and_return_shared( + do_intra_process_ros_message_publish_and_return_shared( std::unique_ptr msg) { auto ipm = weak_ipm_.lock(); @@ -523,14 +532,19 @@ class Publisher : public PublisherBase 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_sharedtemplate 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() @@ -549,6 +563,15 @@ class Publisher : public PublisherBase 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 @@ -560,6 +583,8 @@ class Publisher : public PublisherBase PublishedTypeDeleter published_type_deleter_; ROSMessageTypeAllocator ros_message_type_allocator_; ROSMessageTypeDeleter ros_message_type_deleter_; + + BufferSharedPtr buffer_{nullptr}; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/publisher_base.hpp b/rclcpp/include/rclcpp/publisher_base.hpp index c67a0439e0..9a6c398eeb 100644 --- a/rclcpp/include/rclcpp/publisher_base.hpp +++ b/rclcpp/include/rclcpp/publisher_base.hpp @@ -18,11 +18,14 @@ #include #include +#include #include #include #include #include #include +#include +#include #include #include "rcl/publisher.h" @@ -30,9 +33,10 @@ #include "rclcpp/macros.hpp" #include "rclcpp/network_flow_endpoint.hpp" #include "rclcpp/qos.hpp" -#include "rclcpp/qos_event.hpp" +#include "rclcpp/event_handler.hpp" #include "rclcpp/type_support_decl.hpp" #include "rclcpp/visibility_control.hpp" +#include "rcpputils/time.hpp" namespace rclcpp { @@ -74,11 +78,18 @@ class PublisherBase : public std::enable_shared_from_this 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 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 @@ -110,9 +121,10 @@ class PublisherBase : public std::enable_shared_from_this get_publisher_handle() const; /// Get all the QoS event handlers associated with this publisher. - /** \return The vector of QoS event handlers. */ + /** \return The map of QoS event handlers. */ RCLCPP_PUBLIC - const std::vector> & + const + std::unordered_map> & get_event_handlers() const; /// Get subscription count @@ -127,6 +139,12 @@ class PublisherBase : public std::enable_shared_from_this 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 @@ -203,6 +221,122 @@ class PublisherBase : public std::enable_shared_from_this 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 @@ -210,23 +344,27 @@ class PublisherBase : public std::enable_shared_from_this 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_.emplace_back(handler); + 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::vector> event_handlers_; + std::unordered_map> event_handlers_; using IntraProcessManagerWeakPtr = std::weak_ptr; @@ -235,6 +373,10 @@ class PublisherBase : public std::enable_shared_from_this 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 diff --git a/rclcpp/include/rclcpp/publisher_options.hpp b/rclcpp/include/rclcpp/publisher_options.hpp index 48250307e9..01fd314f49 100644 --- a/rclcpp/include/rclcpp/publisher_options.hpp +++ b/rclcpp/include/rclcpp/publisher_options.hpp @@ -24,9 +24,10 @@ #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/qos_event.hpp" +#include "rclcpp/event_handler.hpp" #include "rclcpp/qos_overriding_options.hpp" namespace rclcpp @@ -40,6 +41,9 @@ 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; @@ -72,7 +76,7 @@ struct PublisherOptionsWithAllocator : public PublisherOptionsBase /// Optional custom allocator. std::shared_ptr allocator = nullptr; - PublisherOptionsWithAllocator() {} + PublisherOptionsWithAllocator() {} /// Constructor using base class as input. explicit PublisherOptionsWithAllocator(const PublisherOptionsBase & publisher_options_base) diff --git a/rclcpp/include/rclcpp/qos.hpp b/rclcpp/include/rclcpp/qos.hpp index 99d5acbd33..2ad49487c5 100644 --- a/rclcpp/include/rclcpp/qos.hpp +++ b/rclcpp/include/rclcpp/qos.hpp @@ -44,6 +44,7 @@ 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, }; @@ -52,6 +53,7 @@ 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, }; @@ -60,6 +62,7 @@ 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, }; @@ -77,7 +80,9 @@ struct RCLCPP_PUBLIC QoSInitialization 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); + 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 @@ -94,7 +99,7 @@ struct RCLCPP_PUBLIC KeepAll : public rclcpp::QoSInitialization /// 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); + explicit KeepLast(size_t depth, bool print_depth_warning = true); }; /// Encapsulation of Quality of Service settings. @@ -180,6 +185,10 @@ class RCLCPP_PUBLIC QoS 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); @@ -199,6 +208,10 @@ class RCLCPP_PUBLIC QoS QoS & transient_local(); + /// Set the durability setting to best available. + QoS & + durability_best_available(); + /// Set the deadline setting. QoS & deadline(rmw_time_t deadline); @@ -488,6 +501,36 @@ class RCLCPP_PUBLIC SystemDefaultsQoS : public QoS )); }; +/** + * 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_event.hpp b/rclcpp/include/rclcpp/qos_event.hpp deleted file mode 100644 index b3bf61cea5..0000000000 --- a/rclcpp/include/rclcpp/qos_event.hpp +++ /dev/null @@ -1,173 +0,0 @@ -// 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_EVENT_HPP_ -#define RCLCPP__QOS_EVENT_HPP_ - -#include -#include -#include -#include - -#include "rcl/error_handling.h" -#include "rmw/incompatible_qos_events_statuses.h" - -#include "rcutils/logging_macros.h" - -#include "rclcpp/exceptions.hpp" -#include "rclcpp/function_traits.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 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; - -/// 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; -}; - -/// 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; -}; - -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 QOSEventHandlerBase : public Waitable -{ -public: - RCLCPP_PUBLIC - virtual ~QOSEventHandlerBase(); - - /// 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 - bool - add_to_wait_set(rcl_wait_set_t * wait_set) override; - - /// Check if the Waitable is ready. - RCLCPP_PUBLIC - bool - is_ready(rcl_wait_set_t * wait_set) override; - -protected: - rcl_event_t event_handle_; - size_t wait_set_event_index_; -}; - -template -class QOSEventHandler : public QOSEventHandlerBase -{ -public: - template - QOSEventHandler( - const EventCallbackT & callback, - InitFuncT init_func, - ParentHandleT parent_handle, - EventTypeEnum event_type) - : event_callback_(callback) - { - parent_handle_ = parent_handle; - 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"); - } - } - } - - /// 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)); - } - - /// Execute any entities of the Waitable that are ready. - void - execute(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__QOS_EVENT_HPP_ diff --git a/rclcpp/include/rclcpp/rate.hpp b/rclcpp/include/rclcpp/rate.hpp index 55d3bbcb85..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,9 +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; }; @@ -41,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 f1d751ff3f..50af3f1a89 100644 --- a/rclcpp/include/rclcpp/rclcpp.hpp +++ b/rclcpp/include/rclcpp/rclcpp.hpp @@ -54,6 +54,7 @@ * - rclcpp::ParameterValue * - rclcpp::AsyncParametersClient * - rclcpp::SyncParametersClient + * - rclcpp::copy_all_parameter_values() * - rclcpp/parameter.hpp * - rclcpp/parameter_value.hpp * - rclcpp/parameter_client.hpp @@ -95,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: * @@ -117,6 +121,18 @@ * - 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 @@ -152,6 +168,7 @@ #include #include +#include "rclcpp/copy_all_parameter_values.hpp" #include "rclcpp/executors.hpp" #include "rclcpp/guard_condition.hpp" #include "rclcpp/logging.hpp" diff --git a/rclcpp/include/rclcpp/scope_exit.hpp b/rclcpp/include/rclcpp/scope_exit.hpp deleted file mode 100644 index 8b8e3ea49b..0000000000 --- a/rclcpp/include/rclcpp/scope_exit.hpp +++ /dev/null @@ -1,56 +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_ - -// TODO(christophebedard) remove this header completely in I-turtle - -#warning rclcpp/scope_exit.hpp has been deprecated, please use rcpputils/scope_exit.hpp instead - -#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/service.hpp b/rclcpp/include/rclcpp/service.hpp index 78fc5e048f..8296cb5962 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -19,22 +19,31 @@ #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 "rclcpp/logging.hpp" -#include "rmw/error_handling.h" -#include "rmw/rmw.h" -#include "tracetools/tracetools.h" namespace rclcpp { @@ -48,7 +57,7 @@ class ServiceBase 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. */ @@ -121,6 +130,124 @@ class ServiceBase 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) @@ -132,11 +259,24 @@ 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_; + 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}; }; @@ -167,18 +307,16 @@ class 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 subscription. + * \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), 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_ = std::shared_ptr( new rcl_service_t, [handle = node_handle_, service_name](rcl_service_t * service) @@ -197,7 +335,7 @@ class Service rcl_ret_t ret = rcl_service_init( 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) { @@ -214,7 +352,7 @@ class Service rclcpp::exceptions::throw_from_rcl_error(ret, "could not create service"); } - TRACEPOINT( + TRACETOOLS_TRACEPOINT( rclcpp_service_callback_added, static_cast(get_service_handle().get()), static_cast(&any_callback_)); @@ -237,8 +375,8 @@ class Service std::shared_ptr node_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 if (!rcl_service_is_valid(service_handle.get())) { @@ -249,7 +387,7 @@ class Service } service_handle_ = service_handle; - TRACEPOINT( + TRACETOOLS_TRACEPOINT( rclcpp_service_callback_added, static_cast(get_service_handle().get()), static_cast(&any_callback_)); @@ -272,8 +410,8 @@ class Service std::shared_ptr node_handle, rcl_service_t * 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 if (!rcl_service_is_valid(service_handle)) { @@ -286,7 +424,7 @@ class Service // In this case, rcl owns the service handle memory service_handle_ = std::shared_ptr(new rcl_service_t); service_handle_->impl = service_handle->impl; - TRACEPOINT( + TRACETOOLS_TRACEPOINT( rclcpp_service_callback_added, static_cast(get_service_handle().get()), static_cast(&any_callback_)); @@ -348,15 +486,52 @@ class Service { 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"); } } + /// 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"); + } + } + private: 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 43ef71682e..28eff94aed 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -21,6 +21,7 @@ #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" @@ -61,17 +62,17 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy allocator_ = std::make_shared(); } - void add_guard_condition(const rcl_guard_condition_t * guard_condition) override + 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) override + 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) { @@ -119,7 +120,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } } for (size_t i = 0; i < waitable_handles_.size(); ++i) { - if (!waitable_handles_[i]->is_ready(wait_set)) { + if (!waitable_handles_[i]->is_ready(*wait_set)) { waitable_handles_[i].reset(); } } @@ -163,30 +164,22 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy if (!group || !group->can_be_taken_from().load()) { continue; } - group->find_subscription_ptrs_if( + + group->collect_all_ptrs( [this](const rclcpp::SubscriptionBase::SharedPtr & subscription) { subscription_handles_.push_back(subscription->get_subscription_handle()); - return false; - }); - group->find_service_ptrs_if( + }, [this](const rclcpp::ServiceBase::SharedPtr & service) { service_handles_.push_back(service->get_service_handle()); - return false; - }); - group->find_client_ptrs_if( + }, [this](const rclcpp::ClientBase::SharedPtr & client) { client_handles_.push_back(client->get_client_handle()); - return false; - }); - group->find_timer_ptrs_if( + }, [this](const rclcpp::TimerBase::SharedPtr & timer) { timer_handles_.push_back(timer->get_timer_handle()); - return false; - }); - group->find_waitable_ptrs_if( + }, [this](const rclcpp::Waitable::SharedPtr & waitable) { waitable_handles_.push_back(waitable); - return false; }); } @@ -203,7 +196,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy bool add_handles_to_wait_set(rcl_wait_set_t * wait_set) override { - for (auto subscription : subscription_handles_) { + 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", @@ -212,7 +205,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } } - for (auto client : client_handles_) { + 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", @@ -221,7 +214,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } } - for (auto service : service_handles_) { + 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", @@ -230,7 +223,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } } - for (auto timer : timer_handles_) { + 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", @@ -240,22 +233,11 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } for (auto guard_condition : guard_conditions_) { - if (rcl_wait_set_add_guard_condition(wait_set, guard_condition, NULL) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Couldn't add guard_condition to wait set: %s", - rcl_get_error_string().str); - return false; - } + detail::add_guard_condition_to_rcl_wait_set(*wait_set, *guard_condition); } - for (auto waitable : waitable_handles_) { - if (!waitable->add_to_wait_set(wait_set)) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Couldn't add waitable to wait set: %s", rcl_get_error_string().str); - return false; - } + for (const std::shared_ptr & waitable : waitable_handles_) { + waitable->add_to_wait_set(*wait_set); } return true; } @@ -388,7 +370,8 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy ++it; continue; } - if (!timer->call()) { + auto data = timer->call(); + if (!data) { // timer was cancelled, skip it. ++it; continue; @@ -397,6 +380,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy 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; } @@ -412,7 +396,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy { auto it = waitable_handles_.begin(); while (it != waitable_handles_.end()) { - auto waitable = *it; + 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); @@ -448,7 +432,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy size_t number_of_ready_subscriptions() const override { size_t number_of_subscriptions = subscription_handles_.size(); - for (auto waitable : waitable_handles_) { + for (const std::shared_ptr & waitable : waitable_handles_) { number_of_subscriptions += waitable->get_number_of_ready_subscriptions(); } return number_of_subscriptions; @@ -457,7 +441,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy size_t number_of_ready_services() const override { size_t number_of_services = service_handles_.size(); - for (auto waitable : waitable_handles_) { + for (const std::shared_ptr & waitable : waitable_handles_) { number_of_services += waitable->get_number_of_ready_services(); } return number_of_services; @@ -466,7 +450,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy size_t number_of_ready_events() const override { size_t number_of_events = 0; - for (auto waitable : waitable_handles_) { + for (const std::shared_ptr & waitable : waitable_handles_) { number_of_events += waitable->get_number_of_ready_events(); } return number_of_events; @@ -475,7 +459,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy size_t number_of_ready_clients() const override { size_t number_of_clients = client_handles_.size(); - for (auto waitable : waitable_handles_) { + for (const std::shared_ptr & waitable : waitable_handles_) { number_of_clients += waitable->get_number_of_ready_clients(); } return number_of_clients; @@ -484,7 +468,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy size_t number_of_guard_conditions() const override { size_t number_of_guard_conditions = guard_conditions_.size(); - for (auto waitable : waitable_handles_) { + for (const std::shared_ptr & waitable : waitable_handles_) { number_of_guard_conditions += waitable->get_number_of_ready_guard_conditions(); } return number_of_guard_conditions; @@ -493,7 +477,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy size_t number_of_ready_timers() const override { size_t number_of_timers = timer_handles_.size(); - for (auto waitable : waitable_handles_) { + for (const std::shared_ptr & waitable : waitable_handles_) { number_of_timers += waitable->get_number_of_ready_timers(); } return number_of_timers; @@ -509,7 +493,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy using VectorRebind = std::vector::template rebind_alloc>; - VectorRebind guard_conditions_; + VectorRebind guard_conditions_; VectorRebind> subscription_handles_; VectorRebind> service_handles_; diff --git a/rclcpp/include/rclcpp/strategies/message_pool_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/message_pool_memory_strategy.hpp index 0e7d4366e5..703066fa3f 100644 --- a/rclcpp/include/rclcpp/strategies/message_pool_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/message_pool_memory_strategy.hpp @@ -15,10 +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" @@ -50,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); } } @@ -68,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 0b42549978..4e552eb1df 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -90,21 +90,9 @@ class Subscription : public SubscriptionBase using ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type; using ROSMessageTypeDeleter = allocator::Deleter; - using MessageAllocatorTraits [[deprecated("use ROSMessageTypeAllocatorTraits")]] = - ROSMessageTypeAllocatorTraits; - using MessageAllocator [[deprecated("use ROSMessageTypeAllocator")]] = - ROSMessageTypeAllocator; - using MessageDeleter [[deprecated("use ROSMessageTypeDeleter")]] = - ROSMessageTypeDeleter; - - using ConstMessageSharedPtr [[deprecated]] = std::shared_ptr; - using MessageUniquePtr - [[deprecated("use std::unique_ptr instead")]] = - std::unique_ptr; - private: using SubscriptionTopicStatisticsSharedPtr = - std::shared_ptr>; + std::shared_ptr; public: RCLCPP_SMART_PTR_DEFINITIONS(Subscription) @@ -127,6 +115,7 @@ class Subscription : public SubscriptionBase * 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( rclcpp::node_interfaces::NodeBaseInterface * node_base, const rosidl_message_type_support_t & type_support_handle, @@ -140,46 +129,18 @@ class Subscription : public SubscriptionBase node_base, type_support_handle, topic_name, - options.template to_rcl_subscription_options(qos), - callback.is_serialized_message_callback()), + 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), options_(options), message_memory_strategy_(message_memory_strategy) + // *INDENT-ON* { - if (options.event_callbacks.deadline_callback) { - this->add_event_handler( - options.event_callbacks.deadline_callback, - RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED); - } - if (options.event_callbacks.liveliness_callback) { - this->add_event_handler( - options.event_callbacks.liveliness_callback, - RCL_SUBSCRIPTION_LIVELINESS_CHANGED); - } - if (options.event_callbacks.incompatible_qos_callback) { - this->add_event_handler( - options.event_callbacks.incompatible_qos_callback, - RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS); - } else if (options_.use_default_callbacks) { - // Register default callback when not specified - try { - this->add_event_handler( - [this](QOSRequestedIncompatibleQoSInfo & info) { - this->default_incompatible_qos_callback(info); - }, - RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS); - } catch (UnsupportedEventTypeException & /*exc*/) { - // pass - } - } - if (options.event_callbacks.message_lost_callback) { - this->add_event_handler( - options.event_callbacks.message_lost_callback, - RCL_SUBSCRIPTION_MESSAGE_LOST); - } - // Setup intra process publishing if requested. - if (rclcpp::detail::resolve_use_intra_process(options, *node_base)) { + 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. @@ -192,21 +153,25 @@ class Subscription : public SubscriptionBase throw std::invalid_argument( "intraprocess communication is not allowed with 0 depth qos policy"); } - if (qos_profile.durability() != rclcpp::DurabilityPolicy::Volatile) { - throw std::invalid_argument( - "intraprocess communication allowed only with volatile durability"); - } + + 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(), + 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)); - TRACEPOINT( + 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())); @@ -214,7 +179,8 @@ class Subscription : public SubscriptionBase // Add it to the intra process manager. using rclcpp::experimental::IntraProcessManager; auto ipm = context->get_sub_context(); - uint64_t intra_process_subscription_id = ipm->add_subscription(subscription_intra_process_); + uint64_t intra_process_subscription_id = ipm->template add_subscription< + ROSMessageType, ROSMessageTypeAllocator>(subscription_intra_process_); this->setup_intra_process(intra_process_subscription_id, ipm); } @@ -222,11 +188,11 @@ class Subscription : public SubscriptionBase this->subscription_topic_statistics_ = std::move(subscription_topic_statistics); } - TRACEPOINT( + TRACETOOLS_TRACEPOINT( rclcpp_subscription_init, static_cast(get_subscription_handle().get()), static_cast(this)); - TRACEPOINT( + TRACETOOLS_TRACEPOINT( rclcpp_subscription_callback_added, static_cast(this), static_cast(&any_callback_)); @@ -276,7 +242,7 @@ class Subscription : public SubscriptionBase /// Take the next message from the inter-process subscription. /** - * This verison takes a SubscribedType which is different frmo the + * This version takes a SubscribedType which is different from the * ROSMessageType when a rclcpp::TypeAdapter is in used. * * \sa take(ROSMessageType &, rclcpp::MessageInfo &) @@ -337,7 +303,7 @@ class Subscription : public SubscriptionBase 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(*typed_message, time); + subscription_topic_statistics_->handle_message(message_info.get_rmw_message_info(), time); } } @@ -346,8 +312,20 @@ class Subscription : public SubscriptionBase const std::shared_ptr & serialized_message, const rclcpp::MessageInfo & message_info) override { - // TODO(wjwwood): enable topic statistics for serialized messages + 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 @@ -355,11 +333,31 @@ class Subscription : public SubscriptionBase void * loaned_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 = 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); + } } /// Return the borrowed message. @@ -389,6 +387,57 @@ class Subscription : public SubscriptionBase return any_callback_.use_take_shared_method(); } + // DYNAMIC TYPE ================================================================================== + // TODO(methylDragon): Reorder later + // TODO(methylDragon): Implement later... + rclcpp::dynamic_typesupport::DynamicMessageType::SharedPtr + get_shared_dynamic_message_type() override + { + throw rclcpp::exceptions::UnimplementedError( + "get_shared_dynamic_message_type is not implemented for Subscription"); + } + + rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr + get_shared_dynamic_message() override + { + throw rclcpp::exceptions::UnimplementedError( + "get_shared_dynamic_message is not implemented for Subscription"); + } + + 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"); + } + + rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr + create_dynamic_message() override + { + 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) @@ -404,13 +453,6 @@ class Subscription : public SubscriptionBase /// Component which computes and publishes topic statistics for this subscriber SubscriptionTopicStatisticsSharedPtr subscription_topic_statistics_{nullptr}; - - using SubscriptionIntraProcessT = rclcpp::experimental::SubscriptionIntraProcess< - ROSMessageType, - AllocatorT, - ROSMessageTypeDeleter, - MessageT>; - std::shared_ptr subscription_intra_process_; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index 7653292b5e..615f3852b6 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -17,24 +17,32 @@ #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/qos_event.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" @@ -55,6 +63,27 @@ namespace experimental 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 @@ -71,7 +100,8 @@ class SubscriptionBase : public std::enable_shared_from_this * \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] is_serialized is true if the message will be delivered still serialized + * \param[in] delivered_message_kind Enum flag to change how the message will be received and + * delivered */ RCLCPP_PUBLIC SubscriptionBase( @@ -79,12 +109,20 @@ class SubscriptionBase : public std::enable_shared_from_this const rosidl_message_type_support_t & type_support_handle, const std::string & topic_name, const rcl_subscription_options_t & subscription_options, - bool is_serialized = false); + 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 * @@ -99,15 +137,16 @@ class SubscriptionBase : public std::enable_shared_from_this get_subscription_handle() const; /// Get all the QoS event handlers associated with this subscription. - /** \return The vector of QoS event handlers. */ + /** \return The map of QoS event handlers. */ RCLCPP_PUBLIC - const std::vector> & + 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 publisher, and it + * 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. @@ -221,6 +260,14 @@ class SubscriptionBase : public std::enable_shared_from_this 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 @@ -283,6 +330,287 @@ class SubscriptionBase : public std::enable_shared_from_this 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 @@ -290,44 +618,64 @@ class SubscriptionBase : public std::enable_shared_from_this 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_.emplace_back(handler); + 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::vector> event_handlers_; + 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_; - bool is_serialized_; + 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_; }; 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 4da6c236bf..0e9d9fefe5 100644 --- a/rclcpp/include/rclcpp/subscription_factory.hpp +++ b/rclcpp/include/rclcpp/subscription_factory.hpp @@ -26,14 +26,13 @@ #include "rclcpp/any_subscription_callback.hpp" #include "rclcpp/get_message_type_support_handle.hpp" -#include "rclcpp/intra_process_buffer_type.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/visibility_control.hpp" #include "rclcpp/topic_statistics/subscription_topic_statistics.hpp" +#include "rclcpp/visibility_control.hpp" namespace rclcpp { @@ -76,15 +75,14 @@ template< typename CallbackT, typename AllocatorT, typename SubscriptionT = rclcpp::Subscription, - typename MessageMemoryStrategyT = typename SubscriptionT::MessageMemoryStrategyType, - typename ROSMessageType = typename SubscriptionT::ROSMessageType + typename MessageMemoryStrategyT = typename SubscriptionT::MessageMemoryStrategyType > SubscriptionFactory create_subscription_factory( CallbackT && callback, const rclcpp::SubscriptionOptionsWithAllocator & options, typename MessageMemoryStrategyT::SharedPtr msg_mem_strat, - std::shared_ptr> + std::shared_ptr subscription_topic_stats = nullptr ) { diff --git a/rclcpp/include/rclcpp/subscription_options.hpp b/rclcpp/include/rclcpp/subscription_options.hpp index d6d2d4c60d..0dd738ee60 100644 --- a/rclcpp/include/rclcpp/subscription_options.hpp +++ b/rclcpp/include/rclcpp/subscription_options.hpp @@ -26,8 +26,9 @@ #include "rclcpp/intra_process_buffer_type.hpp" #include "rclcpp/intra_process_setting.hpp" #include "rclcpp/qos.hpp" -#include "rclcpp/qos_event.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" @@ -76,11 +77,18 @@ struct SubscriptionOptionsBase // 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. @@ -94,7 +102,7 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase /// Optional custom allocator. std::shared_ptr allocator = nullptr; - SubscriptionOptionsWithAllocator() {} + SubscriptionOptionsWithAllocator() {} /// Constructor using base class as input. explicit SubscriptionOptionsWithAllocator( @@ -107,7 +115,6 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase * \param qos QoS profile for subcription. * \return rcl_subscription_options_t structure based on the rclcpp::QoS */ - template rcl_subscription_options_t to_rcl_subscription_options(const rclcpp::QoS & qos) const { @@ -123,6 +130,21 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase 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; } diff --git a/rclcpp/include/rclcpp/time.hpp b/rclcpp/include/rclcpp/time.hpp index 15533f39ef..7ebf9f21b8 100644 --- a/rclcpp/include/rclcpp/time.hpp +++ b/rclcpp/include/rclcpp/time.hpp @@ -49,6 +49,7 @@ class Time /** * \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_type = RCL_SYSTEM_TIME); @@ -57,6 +58,10 @@ class Time 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 @@ -84,6 +89,7 @@ class Time operator builtin_interfaces::msg::Time() const; /** + * Copy assignment operator * \throws std::runtime_error if seconds are negative */ RCLCPP_PUBLIC @@ -100,6 +106,13 @@ class Time 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 */ @@ -189,7 +202,7 @@ class Time */ RCLCPP_PUBLIC static Time - max(); + max(rcl_clock_type_t clock_type = RCL_SYSTEM_TIME); // NOLINT /// Get the seconds since epoch /** @@ -222,6 +235,15 @@ 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 0a40e60201..0de9b368e8 100644 --- a/rclcpp/include/rclcpp/time_source.hpp +++ b/rclcpp/include/rclcpp/time_source.hpp @@ -56,6 +56,7 @@ class TimeSource * * \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( @@ -68,12 +69,21 @@ class TimeSource * 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 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 @@ -116,89 +126,33 @@ class TimeSource RCLCPP_PUBLIC void attachClock(rclcpp::Clock::SharedPtr clock); - /// Detach a clock to the time source + /// 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(); -protected: - // Dedicated thread for clock subscription. - bool use_clock_thread_; - std::thread clock_executor_thread_; - 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_; - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_; - rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_; - rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_; - - // 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 MessageT = rosgraph_msgs::msg::Clock; - using Alloc = std::allocator; - 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); - - // Create the subscription for the clock topic - void create_clock_sub(); - - // Destroy the subscription for the clock topic - void destroy_clock_sub(); - - // 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(std::shared_ptr 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 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_{false}; - // Last set message to be passed to newly registered clocks - std::shared_ptr 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_; - // A handler for the use_sim_time parameter callback. - rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr sim_time_cb_handler_{nullptr}; + 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 52df423d83..0ed62007d4 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/rclcpp/include/rclcpp/timer.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include #include @@ -43,6 +44,12 @@ namespace rclcpp { +struct TimerInfo +{ + Time expected_call_time; + Time actual_call_time; +}; + class TimerBase { public: @@ -53,12 +60,17 @@ class TimerBase * \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( Clock::SharedPtr clock, std::chrono::nanoseconds period, - rclcpp::Context::SharedPtr context); + rclcpp::Context::SharedPtr context, + bool autostart = true); /// TimerBase destructor RCLCPP_PUBLIC @@ -96,16 +108,20 @@ class TimerBase * The multithreaded executor takes advantage of this to avoid scheduling * the callback multiple times. * - * \return `true` if the callback should be executed, `false` if the timer was canceled. + * \return a valid shared_ptr if the callback should be executed, + * an invalid shared_ptr (nullptr) if the timer was canceled. */ RCLCPP_PUBLIC - virtual bool + 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 std::shared_ptr @@ -113,7 +129,8 @@ class TimerBase /// 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 @@ -148,23 +165,61 @@ class TimerBase 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: + 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_; std::atomic in_use_by_wait_set_{false}; -}; + RCLCPP_PUBLIC + void + set_on_reset_callback(rcl_event_callback_t callback, const void * user_data); +}; using VoidCallbackType = std::function; using TimerCallbackType = std::function; +using TimerInfoCallbackType = std::function; /// Generic timer. Periodically executes a user-specified callback. 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 || + rclcpp::function_traits::same_arguments::value >::type * = nullptr > class GenericTimer : public TimerBase @@ -178,21 +233,28 @@ class GenericTimer : public TimerBase * \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 */ explicit GenericTimer( Clock::SharedPtr clock, std::chrono::nanoseconds period, FunctorT && callback, - rclcpp::Context::SharedPtr context + rclcpp::Context::SharedPtr context, bool autostart = true ) - : TimerBase(clock, period, context), callback_(std::forward(callback)) + : TimerBase(clock, period, context, autostart), callback_(std::forward(callback)) { - TRACEPOINT( + TRACETOOLS_TRACEPOINT( rclcpp_timer_callback_added, static_cast(get_timer_handle().get()), - static_cast(&callback_)); - TRACEPOINT( - rclcpp_callback_register, - static_cast(&callback_), - tracetools::get_symbol(callback_)); + 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. @@ -206,28 +268,29 @@ class GenericTimer : public TimerBase * \sa rclcpp::TimerBase::call * \throws std::runtime_error if it failed to notify timer that callback will occurr */ - bool + std::shared_ptr call() override { - rcl_ret_t ret = rcl_timer_call(timer_handle_.get()); + 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 false; + return nullptr; } if (ret != RCL_RET_OK) { throw std::runtime_error("Failed to notify timer that callback occurred"); } - return true; + return timer_call_info_; } /** * \sa rclcpp::TimerBase::execute_callback */ void - execute_callback() override + execute_callback(const std::shared_ptr & data) override { - TRACEPOINT(callback_start, static_cast(&callback_), false); - execute_callback_delegate<>(); - TRACEPOINT(callback_end, static_cast(&callback_)); + TRACETOOLS_TRACEPOINT(callback_start, reinterpret_cast(&callback_), false); + execute_callback_delegate<>(*static_cast(data.get())); + TRACETOOLS_TRACEPOINT(callback_end, reinterpret_cast(&callback_)); } // void specialization @@ -238,7 +301,7 @@ class GenericTimer : public TimerBase >::type * = nullptr > void - execute_callback_delegate() + execute_callback_delegate(const rcl_timer_call_info_t &) { callback_(); } @@ -250,11 +313,26 @@ class GenericTimer : public TimerBase >::type * = nullptr > void - execute_callback_delegate() + execute_callback_delegate(const rcl_timer_call_info_t &) { callback_(*this); } + + 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) + { + 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 @@ -273,7 +351,8 @@ 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 || + rclcpp::function_traits::same_arguments::value >::type * = nullptr > class WallTimer : public GenericTimer @@ -286,13 +365,15 @@ class WallTimer : public GenericTimer * \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) + rclcpp::Context::SharedPtr context, + bool autostart = true) : GenericTimer( - std::make_shared(RCL_STEADY_TIME), period, std::move(callback), context) + std::make_shared(RCL_STEADY_TIME), period, std::move(callback), context, autostart) {} protected: diff --git a/rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp b/rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp index 4b9221406f..7a6db13502 100644 --- a/rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp +++ b/rclcpp/include/rclcpp/topic_statistics/subscription_topic_statistics.hpp @@ -48,21 +48,12 @@ 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. - * - * \tparam CallbackMessageT the subscribed message type - */ -template + */ class SubscriptionTopicStatistics { - using TopicStatsCollector = - libstatistics_collector::topic_statistics_collector::TopicStatisticsCollector< - CallbackMessageT>; - using ReceivedMessageAge = - libstatistics_collector::topic_statistics_collector::ReceivedMessageAgeCollector< - CallbackMessageT>; - using ReceivedMessagePeriod = - libstatistics_collector::topic_statistics_collector::ReceivedMessagePeriodCollector< - CallbackMessageT>; + using TopicStatsCollector = libstatistics_collector::TopicStatisticsCollector; + using ReceivedMessageAge = libstatistics_collector::ReceivedMessageAgeCollector; + using ReceivedMessagePeriod = libstatistics_collector::ReceivedMessagePeriodCollector; public: /// Construct a SubscriptionTopicStatistics object. @@ -101,16 +92,16 @@ class SubscriptionTopicStatistics /** * This method acquires a lock to prevent race conditions to collectors list. * - * \param received_message the message received by the subscription + * \param message_info the message info corresponding to the received message * \param now_nanoseconds current time in nanoseconds */ virtual void handle_message( - const CallbackMessageT & received_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(received_message, now_nanoseconds.nanoseconds()); + collector->OnMessageReceived(message_info, now_nanoseconds.nanoseconds()); } } @@ -181,12 +172,11 @@ class SubscriptionTopicStatistics { auto received_message_age = std::make_unique(); received_message_age->Start(); - subscriber_statistics_collectors_.emplace_back(std::move(received_message_age)); - 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)); } diff --git a/rclcpp/include/rclcpp/typesupport_helpers.hpp b/rclcpp/include/rclcpp/typesupport_helpers.hpp index 2fad84cf3b..7f75a8052b 100644 --- a/rclcpp/include/rclcpp/typesupport_helpers.hpp +++ b/rclcpp/include/rclcpp/typesupport_helpers.hpp @@ -22,6 +22,7 @@ #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" @@ -37,17 +38,36 @@ RCLCPP_PUBLIC std::shared_ptr get_typesupport_library(const std::string & type, const std::string & typesupport_identifier); -/// Extract the type support handle from the library. +/// 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 - * \return A type support handle + * \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_typesupport_handle( +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); diff --git a/rclcpp/include/rclcpp/utilities.hpp b/rclcpp/include/rclcpp/utilities.hpp index 5011a12c6a..5b4ea86a6d 100644 --- a/rclcpp/include/rclcpp/utilities.hpp +++ b/rclcpp/include/rclcpp/utilities.hpp @@ -42,6 +42,20 @@ 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. /** * Initializes the global context which is accessible via the function @@ -50,10 +64,19 @@ namespace rclcpp * 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 const * const argv[], const InitOptions & init_options = InitOptions()); +init( + int argc, + char const * const * argv, + const InitOptions & init_options = InitOptions(), + SignalHandlerOptions signal_handler_options = SignalHandlerOptions::All); /// Install the global signal handler for rclcpp. /** @@ -67,17 +90,26 @@ init(int argc, char const * const argv[], const InitOptions & init_options = Ini * * 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 -install_signal_handlers(); +install_signal_handlers(SignalHandlerOptions signal_handler_options = SignalHandlerOptions::All); /// Return true if the signal handlers are installed, otherwise false. RCLCPP_PUBLIC bool signal_handlers_installed(); +/// Get the current signal handler options. +/** + * If no signal handler is installed, SignalHandlerOptions::None is returned. + */ +RCLCPP_PUBLIC +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 @@ -107,7 +139,7 @@ RCLCPP_PUBLIC std::vector init_and_remove_ros_arguments( int argc, - char const * const argv[], + char const * const * argv, const InitOptions & init_options = InitOptions()); /// Remove ROS-specific arguments from argument vector. @@ -125,7 +157,7 @@ init_and_remove_ros_arguments( */ RCLCPP_PUBLIC std::vector -remove_ros_arguments(int argc, char const * const argv[]); +remove_ros_arguments(int argc, char const * const * argv); /// Check rclcpp's status. /** @@ -137,7 +169,7 @@ remove_ros_arguments(int argc, char const * const argv[]); * the context initialized by rclcpp::init(). * * \param[in] context Optional check for shutdown of this Context. - * \return true if shutdown has been called, false otherwise + * \return false if shutdown has been called, true otherwise */ RCLCPP_PUBLIC bool @@ -289,6 +321,15 @@ 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 index f5f3adc9a4..25c45ad782 100644 --- a/rclcpp/include/rclcpp/wait_for_message.hpp +++ b/rclcpp/include/rclcpp/wait_for_message.hpp @@ -18,6 +18,8 @@ #include #include +#include "rcpputils/scope_exit.hpp" + #include "rclcpp/node.hpp" #include "rclcpp/visibility_control.hpp" #include "rclcpp/wait_set.hpp" @@ -54,6 +56,7 @@ bool wait_for_message( 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) { @@ -90,7 +93,7 @@ bool wait_for_message( 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) {}); + 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); } diff --git a/rclcpp/include/rclcpp/wait_result.hpp b/rclcpp/include/rclcpp/wait_result.hpp index e879043d04..3384a7846a 100644 --- a/rclcpp/include/rclcpp/wait_result.hpp +++ b/rclcpp/include/rclcpp/wait_result.hpp @@ -17,13 +17,21 @@ #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 { @@ -134,6 +142,151 @@ class WaitResult final } } + /// 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) @@ -151,12 +304,25 @@ class WaitResult final // 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). - wait_set_pointer_->wait_result_acquire(); + this->get_wait_set().wait_result_acquire(); } - const WaitResultKind wait_result_kind_; + /// 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 diff --git a/rclcpp/include/rclcpp/wait_result_kind.hpp b/rclcpp/include/rclcpp/wait_result_kind.hpp index 3ce65bf4f3..7980d1d127 100644 --- a/rclcpp/include/rclcpp/wait_result_kind.hpp +++ b/rclcpp/include/rclcpp/wait_result_kind.hpp @@ -26,6 +26,7 @@ enum RCLCPP_PUBLIC WaitResultKind Ready, //get_subscription_handle().get(), + subscription_entry.subscription->get_subscription_handle().get(), nullptr); if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't fill wait set"); } } @@ -271,8 +269,7 @@ class StoragePolicyCommon [this](const auto & inner_guard_conditions) { for (const auto & guard_condition : inner_guard_conditions) { - auto guard_condition_ptr_pair = get_raw_pointer_from_smart_pointer(guard_condition); - if (nullptr == guard_condition_ptr_pair.second) { + 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 @@ -285,10 +282,10 @@ class StoragePolicyCommon } rcl_ret_t ret = rcl_wait_set_add_guard_condition( &rcl_wait_set_, - &guard_condition_ptr_pair.second->get_rcl_guard_condition(), + &guard_condition->get_rcl_guard_condition(), nullptr); if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't fill wait set"); } } }; @@ -301,8 +298,7 @@ class StoragePolicyCommon // Add timers. for (const auto & timer : timers) { - auto timer_ptr_pair = get_raw_pointer_from_smart_pointer(timer); - if (nullptr == timer_ptr_pair.second) { + 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 @@ -315,17 +311,16 @@ class StoragePolicyCommon } rcl_ret_t ret = rcl_wait_set_add_timer( &rcl_wait_set_, - timer_ptr_pair.second->get_timer_handle().get(), + timer->get_timer_handle().get(), nullptr); if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't fill wait set"); } } // Add clients. for (const auto & client : clients) { - auto client_ptr_pair = get_raw_pointer_from_smart_pointer(client); - if (nullptr == client_ptr_pair.second) { + 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 @@ -338,7 +333,7 @@ class StoragePolicyCommon } rcl_ret_t ret = rcl_wait_set_add_client( &rcl_wait_set_, - client_ptr_pair.second->get_client_handle().get(), + client->get_client_handle().get(), nullptr); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); @@ -347,8 +342,7 @@ class StoragePolicyCommon // Add services. for (const auto & service : services) { - auto service_ptr_pair = get_raw_pointer_from_smart_pointer(service); - if (nullptr == service_ptr_pair.second) { + 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 @@ -361,17 +355,16 @@ class StoragePolicyCommon } rcl_ret_t ret = rcl_wait_set_add_service( &rcl_wait_set_, - service_ptr_pair.second->get_service_handle().get(), + service->get_service_handle().get(), nullptr); if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't fill wait set"); } } // Add waitables. for (auto & waitable_entry : waitables) { - auto waitable_ptr_pair = get_raw_pointer_from_smart_pointer(waitable_entry.waitable); - if (nullptr == waitable_ptr_pair.second) { + 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 @@ -382,11 +375,7 @@ class StoragePolicyCommon needs_pruning_ = true; continue; } - rclcpp::Waitable & waitable = *waitable_ptr_pair.second; - bool successful = waitable.add_to_wait_set(&rcl_wait_set_); - if (!successful) { - throw std::runtime_error("waitable unexpectedly failed to be added to wait set"); - } + waitable_entry.waitable->add_to_wait_set(rcl_wait_set_); } } @@ -408,6 +397,32 @@ class StoragePolicyCommon 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_; 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 index 90b9df0178..933bbf6895 100644 --- 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 @@ -191,7 +191,7 @@ class WritePreferringReadWriteLock final WritePreferringReadWriteLock & parent_lock_; - friend WritePreferringReadWriteLock; + friend class WritePreferringReadWriteLock; }; /// Write mutex for the WritePreferringReadWriteLock. @@ -212,7 +212,7 @@ class WritePreferringReadWriteLock final WritePreferringReadWriteLock & parent_lock_; - friend WritePreferringReadWriteLock; + friend class WritePreferringReadWriteLock; }; /// Return read mutex which can be used with standard constructs like std::lock_guard. diff --git a/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp b/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp index 4cec85f39a..8f97596218 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/dynamic_storage.hpp @@ -204,15 +204,19 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo void storage_rebuild_rcl_wait_set(const ArrayOfExtraGuardConditions & extra_guard_conditions) { + this->storage_acquire_ownerships(); + this->storage_rebuild_rcl_wait_set_with_sets( - subscriptions_, - guard_conditions_, + shared_subscriptions_, + shared_guard_conditions_, extra_guard_conditions, - timers_, - clients_, - services_, - waitables_ + shared_timers_, + shared_clients_, + shared_services_, + shared_waitables_ ); + + this->storage_release_ownerships(); } template @@ -382,6 +386,8 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo 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()); @@ -407,6 +413,7 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo } }; // 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_); @@ -438,6 +445,7 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo shared_ptr.reset(); } }; + reset_all(shared_subscriptions_); reset_all(shared_guard_conditions_); reset_all(shared_timers_); reset_all(shared_clients_); @@ -445,6 +453,61 @@ class DynamicStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCo 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_; diff --git a/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp b/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp index 5ebf32bb72..4afc2a1b27 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp @@ -203,7 +203,7 @@ class SequentialSynchronization : public detail::SynchronizationPolicyCommon std::shared_ptr && waitable, std::shared_ptr && associated_entity, std::function< - void(std::shared_ptr&&, std::shared_ptr&&) + void(std::shared_ptr&&, std::shared_ptr &&) > add_waitable_function) { // Explicitly no thread synchronization. @@ -290,7 +290,7 @@ class SequentialSynchronization : public detail::SynchronizationPolicyCommon return create_wait_result(WaitResultKind::Empty); } else { // Some other error case, throw. - rclcpp::exceptions::throw_from_rcl_error(ret); + rclcpp::exceptions::throw_from_rcl_error(ret, "rcl_wait() failed"); } } while (should_loop()); diff --git a/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp b/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp index 434947c19f..7f5cad74ad 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/static_storage.hpp @@ -188,6 +188,61 @@ class StaticStorage : public rclcpp::wait_set_policies::detail::StoragePolicyCom // 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_; diff --git a/rclcpp/include/rclcpp/wait_set_policies/thread_safe_synchronization.hpp b/rclcpp/include/rclcpp/wait_set_policies/thread_safe_synchronization.hpp index ea74769ad4..4a4fb16547 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/thread_safe_synchronization.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/thread_safe_synchronization.hpp @@ -216,7 +216,7 @@ class ThreadSafeSynchronization : public detail::SynchronizationPolicyCommon std::shared_ptr && waitable, std::shared_ptr && associated_entity, std::function< - void(std::shared_ptr&&, std::shared_ptr&&) + void(std::shared_ptr&&, std::shared_ptr &&) > add_waitable_function) { using rclcpp::wait_set_policies::detail::WritePreferringReadWriteLock; diff --git a/rclcpp/include/rclcpp/wait_set_template.hpp b/rclcpp/include/rclcpp/wait_set_template.hpp index 3adee41164..ce69da6bf2 100644 --- a/rclcpp/include/rclcpp/wait_set_template.hpp +++ b/rclcpp/include/rclcpp/wait_set_template.hpp @@ -153,9 +153,11 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli 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 event : inner_subscription->get_event_handlers()) { + 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); @@ -163,6 +165,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli 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) { @@ -179,6 +182,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli 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;} } } }); @@ -223,12 +227,15 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli 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 event : inner_subscription->get_event_handlers()) { + 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) { @@ -237,6 +244,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli // 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;} } } }); @@ -287,6 +295,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli // 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;} }); } @@ -324,6 +333,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli // 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;} }); } @@ -355,6 +365,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli // 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;} }); } @@ -382,6 +393,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli // 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;} }); } @@ -413,6 +425,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli // 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;} }); } @@ -440,6 +453,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli // 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;} }); } @@ -471,6 +485,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli // 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;} }); } @@ -498,6 +513,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli // 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;} }); } @@ -514,7 +530,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli * 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::QOSEventHandlerBase are just a special case of + * 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. @@ -549,6 +565,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli // 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;} }); } @@ -576,6 +593,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli // 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;} }); } @@ -713,6 +731,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli 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 @@ -732,6 +751,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli 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 @@ -739,6 +759,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli } bool wait_result_holding_ = false; + bool wait_result_dirty_ = false; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/waitable.hpp b/rclcpp/include/rclcpp/waitable.hpp index 85deabda85..c803629dcd 100644 --- a/rclcpp/include/rclcpp/waitable.hpp +++ b/rclcpp/include/rclcpp/waitable.hpp @@ -16,6 +16,7 @@ #define RCLCPP__WAITABLE_HPP_ #include +#include #include #include "rclcpp/macros.hpp" @@ -103,13 +104,12 @@ class Waitable /// Add the Waitable to a wait set. /** * \param[in] wait_set A handle to the wait set to add the Waitable to. - * \return `true` if the Waitable is added successfully, `false` otherwise. * \throws rclcpp::execptions::RCLError from rcl_wait_set_add_*() */ RCLCPP_PUBLIC virtual - bool - add_to_wait_set(rcl_wait_set_t * wait_set) = 0; + void + add_to_wait_set(rcl_wait_set_t & wait_set) = 0; /// Check if the Waitable is ready. /** @@ -124,7 +124,7 @@ class Waitable RCLCPP_PUBLIC virtual bool - is_ready(rcl_wait_set_t * wait_set) = 0; + is_ready(const rcl_wait_set_t & wait_set) = 0; /// Take the data so that it can be consumed with `execute`. /** @@ -145,8 +145,7 @@ class Waitable * ```cpp * // ... create a wait set and a Waitable * // Add the Waitable to the wait set - * bool add_ret = waitable.add_to_wait_set(wait_set); - * // ... error handling + * waitable.add_to_wait_set(wait_set); * // Wait * rcl_ret_t wait_ret = rcl_wait(wait_set); * // ... error handling @@ -161,6 +160,24 @@ class Waitable 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, @@ -172,8 +189,7 @@ class Waitable * ```cpp * // ... create a wait set and a Waitable * // Add the Waitable to the wait set - * bool add_ret = waitable.add_to_wait_set(wait_set); - * // ... error handling + * waitable.add_to_wait_set(wait_set); * // Wait * rcl_ret_t wait_ret = rcl_wait(wait_set); * // ... error handling @@ -187,7 +203,7 @@ class Waitable RCLCPP_PUBLIC virtual void - execute(std::shared_ptr & data) = 0; + execute(const std::shared_ptr & data) = 0; /// Exchange the "in use by wait set" state for this timer. /** @@ -203,6 +219,45 @@ class Waitable 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 diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 56726b9490..abbd14f8dc 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,20 +2,27 @@ rclcpp - 12.0.0 + 29.2.0 The ROS client library in C++. + Ivan Paunovic - Mabel Zhang + Michel Hidalgo William Woodall + Apache License 2.0 + Dirk Thomas + Jacob Perron ament_cmake_ros + ament_cmake_gen_version_h + python3 ament_index_cpp builtin_interfaces rcl_interfaces rosgraph_msgs + rosidl_runtime_c rosidl_runtime_cpp rosidl_typesupport_c rosidl_typesupport_cpp @@ -23,20 +30,24 @@ builtin_interfaces rcl_interfaces rosgraph_msgs + rosidl_runtime_c rosidl_runtime_cpp rosidl_typesupport_c rosidl_typesupport_cpp libstatistics_collector rcl + 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 diff --git a/rclcpp/resource/logging.hpp.em b/rclcpp/resource/logging.hpp.em index e8009678c7..01087e5dd0 100644 --- a/rclcpp/resource/logging.hpp.em +++ b/rclcpp/resource/logging.hpp.em @@ -114,7 +114,6 @@ def get_rclcpp_suffix_from_features(features): @[ end for]@ @[ if 'stream' not in feature_combination]@ * \param ... The format string, followed by the variable arguments for the format string. - * It also accepts a single argument of type std::string. @[ end if]@ */ @{params = rclcpp_feature_combinations[feature_combination].params.keys()}@ @@ -125,7 +124,7 @@ def get_rclcpp_suffix_from_features(features): ) \ do { \ static_assert( \ - ::std::is_same::type>::type, \ + ::std::is_same>, \ typename ::rclcpp::Logger>::value, \ "First argument to logging macros must be an rclcpp::Logger"); \ @[ if 'throttle' in feature_combination]@ \ @@ -141,19 +140,19 @@ def get_rclcpp_suffix_from_features(features): }; \ @[ end if] \ @[ if 'stream' in feature_combination]@ - std::stringstream ss; \ - ss << @(stream_arg); \ + 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 if p != stream_arg]))@ @[ end if]@ - logger.get_name(), \ + (logger).get_name(), \ @[ if 'stream' not in feature_combination]@ __VA_ARGS__); \ @[ else]@ - "%s", ss.str().c_str()); \ + "%s", rclcpp_stream_ss_.str().c_str()); \ @[ end if]@ } while (0) diff --git a/rclcpp/src/rclcpp/any_executable.cpp b/rclcpp/src/rclcpp/any_executable.cpp index 769deacb11..2449cbe1f7 100644 --- a/rclcpp/src/rclcpp/any_executable.cpp +++ b/rclcpp/src/rclcpp/any_executable.cpp @@ -21,6 +21,7 @@ AnyExecutable::AnyExecutable() 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 6b38578ba9..bcacaabebe 100644 --- a/rclcpp/src/rclcpp/callback_group.cpp +++ b/rclcpp/src/rclcpp/callback_group.cpp @@ -12,21 +12,37 @@ // 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::CallbackGroup; using rclcpp::CallbackGroupType; 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) + automatically_add_to_executor_with_node_(automatically_add_to_executor_with_node), + context_(context) {} +CallbackGroup::~CallbackGroup() +{ + trigger_notify_guard_condition(); +} std::atomic_bool & CallbackGroup::can_be_taken_from() @@ -40,6 +56,62 @@ CallbackGroup::type() const return type_; } +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_); + + 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::get_associated_with_executor_atomic() { @@ -52,6 +124,29 @@ CallbackGroup::automatically_add_to_executor_with_node() const 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 CallbackGroup::add_subscription( const rclcpp::SubscriptionBase::SharedPtr subscription_ptr) diff --git a/rclcpp/src/rclcpp/client.cpp b/rclcpp/src/rclcpp/client.cpp index 6a1d3934d4..8388ee1888 100644 --- a/rclcpp/src/rclcpp/client.cpp +++ b/rclcpp/src/rclcpp/client.cpp @@ -23,9 +23,11 @@ #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" @@ -38,7 +40,8 @@ ClientBase::ClientBase( rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph) : node_graph_(node_graph), node_handle_(node_base->get_shared_rcl_node_handle()), - context_(node_base->get_context()) + 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; @@ -64,12 +67,6 @@ ClientBase::ClientBase( }); } -ClientBase::~ClientBase() -{ - // Make sure the client handle is destructed as early as possible and before the node handle - client_handle_.reset(); -} - bool ClientBase::take_type_erased_response(void * response_out, rmw_request_id_t & request_header_out) { @@ -128,7 +125,6 @@ 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(); @@ -141,6 +137,7 @@ 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() @@ -198,3 +195,54 @@ 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 e9207e6f31..5c13f19d13 100644 --- a/rclcpp/src/rclcpp/clock.cpp +++ b/rclcpp/src/rclcpp/clock.cpp @@ -14,11 +14,14 @@ #include "rclcpp/clock.hpp" +#include #include #include #include "rclcpp/exceptions.hpp" +#include "rclcpp/utilities.hpp" +#include "rcpputils/scope_exit.hpp" #include "rcutils/logging_macros.h" namespace rclcpp @@ -46,6 +49,10 @@ class Clock::Impl 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_; }; @@ -64,7 +71,7 @@ Clock::Clock(rcl_clock_type_t clock_type) Clock::~Clock() {} Time -Clock::now() +Clock::now() const { Time now(0, 0, impl_->rcl_clock_.type); @@ -76,6 +83,197 @@ Clock::now() return now; } +void +Clock::cancel_sleep_or_wait() +{ + { + std::unique_lock lock(impl_->wait_mutex_); + impl_->stop_sleeping_ = true; + } + impl_->cv_.notify_one(); +} + +bool +Clock::sleep_until( + Time until, + Context::SharedPtr context) +{ + 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; + + // 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; + } + } + + 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() { diff --git a/rclcpp/src/rclcpp/context.cpp b/rclcpp/src/rclcpp/context.cpp index 580689d694..a9b40b733e 100644 --- a/rclcpp/src/rclcpp/context.cpp +++ b/rclcpp/src/rclcpp/context.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include "rcl/init.h" @@ -31,8 +32,6 @@ #include "rcutils/error_handling.h" #include "rcutils/macros.h" -#include "rmw/impl/cpp/demangle.hpp" - #include "./logging_mutex.hpp" using rclcpp::Context; @@ -155,7 +154,9 @@ Context::~Context() // this will not prevent errors, but will maybe make them easier to reproduce std::lock_guard lock(init_mutex_); try { - this->shutdown("context destructor was called while still not shutdown"); + // 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(); @@ -191,7 +192,7 @@ __delete_context(rcl_context_t * context) void Context::init( int argc, - char const * const argv[], + char const * const * argv, const rclcpp::InitOptions & init_options) { std::lock_guard init_lock(init_mutex_); @@ -211,28 +212,27 @@ Context::init( } rcl_context_.reset(context, __delete_context); - 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) { - rcl_context_.reset(); - rclcpp::exceptions::throw_from_rcl_error(ret, "failed to configure logging"); + 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"); } - } else { - RCLCPP_WARN( - rclcpp::get_logger("rclcpp"), - "logging was initialized more than once"); + ++count; } - ++count; - } - try { 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()) { @@ -307,6 +307,15 @@ Context::shutdown(const std::string & reason) // 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) { @@ -355,41 +364,120 @@ Context::on_shutdown(OnShutdownCallback callback) rclcpp::OnShutdownCallbackHandle Context::add_on_shutdown_callback(OnShutdownCallback callback) { - auto callback_shared_ptr = std::make_shared(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(callback_shared_ptr); + on_shutdown_callbacks_.emplace_back(callback_shared_ptr); } - OnShutdownCallbackHandle callback_handle; + ShutdownCallbackHandle callback_handle; callback_handle.callback = callback_shared_ptr; return callback_handle; } +template bool -Context::remove_on_shutdown_callback(const OnShutdownCallbackHandle & callback_handle) +Context::remove_shutdown_callback( + const ShutdownCallbackHandle & callback_handle) { - std::lock_guard lock(on_shutdown_callbacks_mutex_); - auto callback_shared_ptr = callback_handle.callback.lock(); + const auto callback_shared_ptr = callback_handle.callback.lock(); if (callback_shared_ptr == nullptr) { return false; } - return on_shutdown_callbacks_.erase(callback_shared_ptr) == 1; + + 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 { - std::vector callbacks; + return get_shutdown_callback(); +} - { - std::lock_guard lock(on_shutdown_callbacks_mutex_); - for (auto & iter : on_shutdown_callbacks_) { - callbacks.emplace_back(*iter); - } - } +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); - return callbacks; + 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 @@ -402,16 +490,15 @@ bool Context::sleep_for(const std::chrono::nanoseconds & nanoseconds) { std::chrono::nanoseconds time_left = nanoseconds; - { - 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, nanoseconds); - time_left -= std::chrono::steady_clock::now() - start; - } - if (time_left > std::chrono::nanoseconds::zero() && this->is_valid()) { - return sleep_for(time_left); - } + 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(); } 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 index 15f567f7c5..3959e64882 100644 --- a/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.cpp +++ b/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "./resolve_parameter_overrides.hpp" +#include "resolve_parameter_overrides.hpp" #include #include @@ -51,18 +51,13 @@ rclcpp::detail::resolve_parameter_overrides( [params]() { rcl_yaml_node_struct_fini(params); }); - rclcpp::ParameterMap initial_map = rclcpp::parameter_map_from(params); + rclcpp::ParameterMap initial_map = rclcpp::parameter_map_from(params, node_fqn.c_str()); - // Enforce wildcard matching precedence - // TODO(cottsay) implement further wildcard matching - const std::array node_matching_names{"/**", node_fqn}; - for (const auto & node_name : node_matching_names) { - if (initial_map.count(node_name) > 0) { - // Combine parameter yaml files, overwriting values in older ones - for (const rclcpp::Parameter & param : initial_map.at(node_name)) { - result[param.get_name()] = - rclcpp::ParameterValue(param.get_value_message()); - } + 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()); } } } diff --git a/rclcpp/src/rclcpp/detail/utilities.cpp b/rclcpp/src/rclcpp/detail/utilities.cpp index 179756f6a3..0166f2dceb 100644 --- a/rclcpp/src/rclcpp/detail/utilities.cpp +++ b/rclcpp/src/rclcpp/detail/utilities.cpp @@ -31,7 +31,7 @@ namespace detail std::vector get_unparsed_ros_arguments( - int argc, char const * const argv[], + int argc, char const * const * argv, rcl_arguments_t * arguments, rcl_allocator_t allocator) { diff --git a/rclcpp/src/rclcpp/duration.cpp b/rclcpp/src/rclcpp/duration.cpp index 6c20757d46..7cb678456e 100644 --- a/rclcpp/src/rclcpp/duration.cpp +++ b/rclcpp/src/rclcpp/duration.cpp @@ -28,6 +28,8 @@ #include "rcutils/logging_macros.h" +#include "rclcpp/utilities.hpp" + namespace rclcpp { @@ -37,11 +39,6 @@ Duration::Duration(int32_t seconds, uint32_t nanoseconds) rcl_duration_.nanoseconds += nanoseconds; } -Duration::Duration(rcl_duration_value_t nanoseconds) -{ - rcl_duration_.nanoseconds = nanoseconds; -} - Duration::Duration(std::chrono::nanoseconds nanoseconds) { rcl_duration_.nanoseconds = nanoseconds.count(); @@ -166,6 +163,13 @@ Duration::operator+(const rclcpp::Duration & rhs) const 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) { @@ -195,6 +199,13 @@ Duration::operator-(const rclcpp::Duration & rhs) const 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) { @@ -227,6 +238,13 @@ Duration::operator*(double scale) const static_cast(rcl_duration_.nanoseconds) * scale_ld)); } +Duration & +Duration::operator*=(double scale) +{ + *this = *this * scale; + return *this; +} + rcl_duration_value_t Duration::nanoseconds() const { @@ -300,4 +318,50 @@ Duration::from_nanoseconds(rcl_duration_value_t 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/qos_event.cpp b/rclcpp/src/rclcpp/event_handler.cpp similarity index 64% rename from rclcpp/src/rclcpp/qos_event.cpp rename to rclcpp/src/rclcpp/event_handler.cpp index 8af3918c48..630bc26d33 100644 --- a/rclcpp/src/rclcpp/qos_event.cpp +++ b/rclcpp/src/rclcpp/event_handler.cpp @@ -12,9 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include -#include "rclcpp/qos_event.hpp" +#include "rcl/error_handling.h" +#include "rcl/event.h" + +#include "rclcpp/event_handler.hpp" +#include "rclcpp/exceptions/exceptions.hpp" namespace rclcpp { @@ -33,7 +38,7 @@ UnsupportedEventTypeException::UnsupportedEventTypeException( std::runtime_error(prefix + (prefix.empty() ? "" : ": ") + base_exc.formatted_message) {} -QOSEventHandlerBase::~QOSEventHandlerBase() +EventHandlerBase::~EventHandlerBase() { if (rcl_event_fini(&event_handle_) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( @@ -45,27 +50,42 @@ QOSEventHandlerBase::~QOSEventHandlerBase() /// Get the number of ready events. size_t -QOSEventHandlerBase::get_number_of_ready_events() +EventHandlerBase::get_number_of_ready_events() { return 1; } /// Add the Waitable to a wait set. -bool -QOSEventHandlerBase::add_to_wait_set(rcl_wait_set_t * 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_); + 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"); } - return true; } /// Check if the Waitable is ready. bool -QOSEventHandlerBase::is_ready(rcl_wait_set_t * wait_set) +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) { - return wait_set->events[wait_set_event_index_] == &event_handle_; + 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/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 8256f28790..69131cc111 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -13,6 +13,9 @@ // limitations under the License. #include +#include +#include +#include #include #include #include @@ -22,40 +25,54 @@ #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/memory_strategy.hpp" #include "rclcpp/node.hpp" #include "rclcpp/utilities.hpp" #include "rcutils/logging_macros.h" +#include "tracetools/tracetools.h" + using namespace std::chrono_literals; -using rclcpp::exceptions::throw_from_rcl_error; -using rclcpp::AnyExecutable; using rclcpp::Executor; -using rclcpp::ExecutorOptions; -using rclcpp::FutureReturnCode; + +/// 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), + entities_need_rebuild_(true), + collector_(nullptr), + wait_set_({}, {}, {}, {}, {}, {}, context) +{ +} Executor::Executor(const rclcpp::ExecutorOptions & options) : spinning(false), + interrupt_guard_condition_(std::make_shared(options.context)), shutdown_guard_condition_(std::make_shared(options.context)), - memory_strategy_(options.memory_strategy) + 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()) { - // Store the context for later use. - context_ = options.context; - - rcl_guard_condition_options_t guard_condition_options = rcl_guard_condition_get_default_options(); - rcl_ret_t ret = rcl_guard_condition_init( - &interrupt_guard_condition_, context_->get_rcl_context().get(), guard_condition_options); - if (RCL_RET_OK != ret) { - throw_from_rcl_error(ret, "Failed to create interrupt guard condition in Executor constructor"); - } - shutdown_callback_handle_ = context_->add_on_shutdown_callback( [weak_gc = std::weak_ptr{shutdown_guard_condition_}]() { auto strong_gc = weak_gc.lock(); @@ -64,80 +81,43 @@ Executor::Executor(const rclcpp::ExecutorOptions & options) } }); - // 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_) - memory_strategy_->add_guard_condition(&shutdown_guard_condition_->get_rcl_guard_condition()); + 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(); - - ret = rcl_wait_set_init( - &wait_set_, - 0, 2, 0, 0, 0, 0, - context_->get_rcl_context().get(), - allocator); - if (RCL_RET_OK != ret) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "failed to create wait set: %s", rcl_get_error_string().str); - rcl_reset_error(); - if (rcl_guard_condition_fini(&interrupt_guard_condition_) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "failed to destroy guard condition: %s", rcl_get_error_string().str); - rcl_reset_error(); - } - throw_from_rcl_error(ret, "Failed to create wait set in Executor constructor"); - } + wait_set_.add_waitable(notify_waitable_); } Executor::~Executor() { - // Disassociate all callback groups. - for (auto & pair : weak_groups_to_nodes_) { - auto group = pair.first.lock(); - if (group) { - std::atomic_bool & has_executor = group->get_associated_with_executor_atomic(); - has_executor.store(false); - } - } - // Disassociate all nodes. - std::for_each( - weak_nodes_.begin(), weak_nodes_.end(), [] - (rclcpp::node_interfaces::NodeBaseInterface::WeakPtr weak_node_ptr) { - auto shared_node_ptr = weak_node_ptr.lock(); - if (shared_node_ptr) { - std::atomic_bool & has_executor = shared_node_ptr->get_associated_with_executor_atomic(); - has_executor.store(false); - } + 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); }); - weak_nodes_.clear(); - weak_groups_associated_with_executor_to_nodes_.clear(); - weak_groups_to_nodes_associated_with_executor_.clear(); - weak_groups_to_nodes_.clear(); - for (const auto & pair : weak_nodes_to_guard_conditions_) { - auto & guard_condition = pair.second; - memory_strategy_->remove_guard_condition(guard_condition); - } - weak_nodes_to_guard_conditions_.clear(); - - // Finalize the wait set. - if (rcl_wait_set_fini(&wait_set_) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "failed to destroy wait set: %s", rcl_get_error_string().str); - rcl_reset_error(); - } - // Finalize the interrupt guard condition. - if (rcl_guard_condition_fini(&interrupt_guard_condition_) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "failed to destroy guard condition: %s", rcl_get_error_string().str); - rcl_reset_error(); - } - // Remove and release the sigint guard condition - memory_strategy_->remove_guard_condition(&shutdown_guard_condition_->get_rcl_guard_condition()); + + 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_)) { @@ -148,102 +128,40 @@ Executor::~Executor() } } -std::vector -Executor::get_all_callback_groups() +void +Executor::handle_updated_entities(bool notify) { - std::vector groups; - std::lock_guard guard{mutex_}; - for (const auto & group_node_ptr : weak_groups_associated_with_executor_to_nodes_) { - groups.push_back(group_node_ptr.first); + this->entities_need_rebuild_.store(true); + + if (!spinning.load() && entities_need_rebuild_.exchange(false)) { + std::lock_guard guard(mutex_); + this->collect_entities(); } - for (auto const & group_node_ptr : weak_groups_to_nodes_associated_with_executor_) { - groups.push_back(group_node_ptr.first); + + if (notify) { + interrupt_guard_condition_->trigger(); } - return groups; } std::vector -Executor::get_manually_added_callback_groups() +Executor::get_all_callback_groups() { - std::vector groups; - std::lock_guard guard{mutex_}; - for (auto const & group_node_ptr : weak_groups_associated_with_executor_to_nodes_) { - groups.push_back(group_node_ptr.first); - } - return groups; + this->collector_.update_collections(); + return this->collector_.get_all_callback_groups(); } std::vector -Executor::get_automatically_added_callback_groups_from_nodes() +Executor::get_manually_added_callback_groups() { - std::vector groups; - std::lock_guard guard{mutex_}; - for (auto const & group_node_ptr : weak_groups_to_nodes_associated_with_executor_) { - groups.push_back(group_node_ptr.first); - } - return groups; -} - -void -Executor::add_callback_groups_from_nodes_associated_to_executor() -{ - for (auto & weak_node : weak_nodes_) { - auto node = weak_node.lock(); - if (node) { - node->for_each_callback_group( - [this, node](rclcpp::CallbackGroup::SharedPtr shared_group_ptr) - { - if ( - shared_group_ptr->automatically_add_to_executor_with_node() && - !shared_group_ptr->get_associated_with_executor_atomic().load()) - { - this->add_callback_group_to_map( - shared_group_ptr, - node, - weak_groups_to_nodes_associated_with_executor_, - true); - } - }); - } - } + this->collector_.update_collections(); + return this->collector_.get_manually_added_callback_groups(); } -void -Executor::add_callback_group_to_map( - rclcpp::CallbackGroup::SharedPtr group_ptr, - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & weak_groups_to_nodes, - bool notify) +std::vector +Executor::get_automatically_added_callback_groups_from_nodes() { - // If the callback_group already has an executor - 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."); - } - bool is_new_node = !has_node(node_ptr, weak_groups_to_nodes_associated_with_executor_) && - !has_node(node_ptr, weak_groups_associated_with_executor_to_nodes_); - rclcpp::CallbackGroup::WeakPtr weak_group_ptr = group_ptr; - auto insert_info = - weak_groups_to_nodes.insert(std::make_pair(weak_group_ptr, node_ptr)); - bool was_inserted = insert_info.second; - if (!was_inserted) { - throw std::runtime_error("Callback group was already added to executor."); - } - // Also add to the map that contains all callback groups - weak_groups_to_nodes_.insert(std::make_pair(weak_group_ptr, node_ptr)); - if (is_new_node) { - rclcpp::node_interfaces::NodeBaseInterface::WeakPtr node_weak_ptr(node_ptr); - weak_nodes_to_guard_conditions_[node_weak_ptr] = node_ptr->get_notify_guard_condition(); - if (notify) { - // Interrupt waiting to handle new node - rcl_ret_t ret = rcl_trigger_guard_condition(&interrupt_guard_condition_); - if (ret != RCL_RET_OK) { - throw_from_rcl_error(ret, "Failed to trigger guard condition on callback group add"); - } - } - // Add the node's notify condition to the guard condition handles - memory_strategy_->add_guard_condition(node_ptr->get_notify_guard_condition()); - } + this->collector_.update_collections(); + return this->collector_.get_automatically_added_callback_groups(); } void @@ -252,76 +170,29 @@ Executor::add_callback_group( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify) { - std::lock_guard guard{mutex_}; - this->add_callback_group_to_map( - group_ptr, - node_ptr, - weak_groups_associated_with_executor_to_nodes_, - notify); -} + (void) node_ptr; + this->collector_.add_callback_group(group_ptr); -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)) { + try { + this->handle_updated_entities(notify); + } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( - std::string("Node '") + node_ptr->get_fully_qualified_name() + - "' has already been added to an executor."); - } - std::lock_guard guard{mutex_}; - node_ptr->for_each_callback_group( - [this, node_ptr, notify](rclcpp::CallbackGroup::SharedPtr group_ptr) - { - if (!group_ptr->get_associated_with_executor_atomic().load() && - group_ptr->automatically_add_to_executor_with_node()) - { - this->add_callback_group_to_map( - group_ptr, - node_ptr, - weak_groups_to_nodes_associated_with_executor_, - notify); - } - }); - - weak_nodes_.push_back(node_ptr); + std::string( + "Failed to handle entities update on callback group add: ") + ex.what()); + } } void -Executor::remove_callback_group_from_map( - rclcpp::CallbackGroup::SharedPtr group_ptr, - rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & weak_groups_to_nodes, - bool notify) +Executor::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify) { - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr; - rclcpp::CallbackGroup::WeakPtr weak_group_ptr = group_ptr; - auto iter = weak_groups_to_nodes.find(weak_group_ptr); - if (iter != weak_groups_to_nodes.end()) { - node_ptr = iter->second.lock(); - if (node_ptr == nullptr) { - throw std::runtime_error("Node must not be deleted before its callback group(s)."); - } - weak_groups_to_nodes.erase(iter); - weak_groups_to_nodes_.erase(group_ptr); - std::atomic_bool & has_executor = group_ptr->get_associated_with_executor_atomic(); - has_executor.store(false); - } else { - throw std::runtime_error("Callback group needs to be associated with executor."); - } - // If the node was matched and removed, interrupt waiting. - if (!has_node(node_ptr, weak_groups_to_nodes_associated_with_executor_) && - !has_node(node_ptr, weak_groups_associated_with_executor_to_nodes_)) - { - rclcpp::node_interfaces::NodeBaseInterface::WeakPtr node_weak_ptr(node_ptr); - weak_nodes_to_guard_conditions_.erase(node_weak_ptr); - if (notify) { - rcl_ret_t ret = rcl_trigger_guard_condition(&interrupt_guard_condition_); - if (ret != RCL_RET_OK) { - throw_from_rcl_error(ret, "Failed to trigger guard condition on callback group remove"); - } - } - memory_strategy_->remove_guard_condition(node_ptr->get_notify_guard_condition()); + 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()); } } @@ -330,11 +201,15 @@ Executor::remove_callback_group( rclcpp::CallbackGroup::SharedPtr group_ptr, bool notify) { - std::lock_guard guard{mutex_}; - this->remove_callback_group_from_map( - group_ptr, - weak_groups_associated_with_executor_to_nodes_, - 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()); + } } void @@ -346,45 +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) { - if (!node_ptr->get_associated_with_executor_atomic().load()) { - throw std::runtime_error("Node needs to be associated with an executor."); - } + this->collector_.remove_node(node_ptr); - std::lock_guard guard{mutex_}; - bool found_node = false; - auto node_it = weak_nodes_.begin(); - while (node_it != weak_nodes_.end()) { - bool matched = (node_it->lock() == node_ptr); - if (matched) { - found_node = true; - node_it = weak_nodes_.erase(node_it); - } else { - ++node_it; - } - } - if (!found_node) { - throw std::runtime_error("Node needs to be associated with this executor."); - } - - for (auto it = weak_groups_to_nodes_associated_with_executor_.begin(); - it != weak_groups_to_nodes_associated_with_executor_.end(); ) - { - auto weak_node_ptr = it->second; - auto shared_node_ptr = weak_node_ptr.lock(); - auto group_ptr = it->first.lock(); - - // Increment iterator before removing in case it's invalidated - it++; - if (shared_node_ptr == node_ptr) { - remove_callback_group_from_map( - group_ptr, - weak_groups_to_nodes_associated_with_executor_, - notify); - } + 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()); } - - std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic(); - has_executor.store(false); } void @@ -404,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) { @@ -423,10 +321,26 @@ void Executor::spin_some(std::chrono::nanoseconds max_duration) return this->spin_some_impl(max_duration, false); } +void +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 positive"); + 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); } @@ -450,21 +364,54 @@ Executor::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); ); - bool work_available = false; + 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 (!work_available) { - wait_for_work(std::chrono::milliseconds::zero()); - } if (get_next_ready_executable(any_exec)) { execute_any_executable(any_exec); - work_available = true; + just_waited = false; } else { - if (!work_available || !exhaustive) { + // 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; } - work_available = false; } } } @@ -484,7 +431,7 @@ Executor::spin_once(std::chrono::nanoseconds timeout) if (spinning.exchange(true)) { throw std::runtime_error("spin_once() called while already spinning"); } - RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); + RCPPUTILS_SCOPE_EXIT(wait_result_.reset();this->spinning.store(false);); spin_once_impl(timeout); } @@ -492,20 +439,12 @@ void Executor::cancel() { spinning.store(false); - rcl_ret_t ret = rcl_trigger_guard_condition(&interrupt_guard_condition_); - if (ret != RCL_RET_OK) { - throw_from_rcl_error(ret, "Failed to trigger guard condition in cancel"); - } -} - -void -Executor::set_memory_strategy(rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy) -{ - if (memory_strategy == nullptr) { - throw std::runtime_error("Received NULL memory strategy in executor."); + 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()); } - std::lock_guard guard{mutex_}; - memory_strategy_ = memory_strategy; } void @@ -514,10 +453,21 @@ Executor::execute_any_executable(AnyExecutable & any_exec) if (!spinning.load()) { return; } + + assert( + (void("cannot execute an AnyExecutable without a valid callback group"), + any_exec.callback_group)); + if (any_exec.timer) { - execute_timer(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) { + TRACETOOLS_TRACEPOINT( + rclcpp_executor_execute, + static_cast(any_exec.subscription->get_subscription_handle().get())); execute_subscription(any_exec.subscription); } if (any_exec.service) { @@ -527,25 +477,22 @@ Executor::execute_any_executable(AnyExecutable & any_exec) execute_client(any_exec.client); } if (any_exec.waitable) { - any_exec.waitable->execute(any_exec.data); + 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. - rcl_ret_t ret = rcl_trigger_guard_condition(&interrupt_guard_condition_); - if (ret != RCL_RET_OK) { - throw_from_rcl_error(ret, "Failed to trigger guard condition from execute_any_executable"); - } } +template static void take_and_do_error_handling( const char * action_description, const char * topic_or_service_name, - std::function take_action, - std::function handle_action) + Taker take_action, + Handler handle_action) { bool taken = false; try { @@ -578,76 +525,108 @@ take_and_do_error_handling( void 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; - if (subscription->is_serialized()) { - // 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);}, - [&]() + switch (subscription->get_delivered_message_kind()) { + // Deliver ROS message + case rclcpp::DeliveredMessageKind::ROS_MESSAGE: { - subscription->handle_serialized_message(serialized_msg, message_info); - }); - subscription->return_serialized_message(serialized_msg); - } else 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); + 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); } - 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); + 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"); } - 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);}); - subscription->return_message(message); } } 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 @@ -663,8 +642,7 @@ Executor::execute_service(rclcpp::ServiceBase::SharedPtr service) } 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(); @@ -676,219 +654,223 @@ Executor::execute_client( } void -Executor::wait_for_work(std::chrono::nanoseconds timeout) -{ - { - std::lock_guard guard(mutex_); +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); + }); - // Check weak_nodes_ to find any callback group that is not owned - // by an executor and add it to the list of callbackgroups for - // collect entities. Also exchange to false so it is not - // allowed to add to another executor - add_callback_groups_from_nodes_associated_to_executor(); - - // Collect the subscriptions and timers to be waited on - memory_strategy_->clear_handles(); - bool has_invalid_weak_groups_or_nodes = - memory_strategy_->collect_entities(weak_groups_to_nodes_); - - if (has_invalid_weak_groups_or_nodes) { - std::vector invalid_group_ptrs; - for (auto pair : weak_groups_to_nodes_) { - auto weak_group_ptr = pair.first; - auto weak_node_ptr = pair.second; - if (weak_group_ptr.expired() || weak_node_ptr.expired()) { - invalid_group_ptrs.push_back(weak_group_ptr); - auto node_guard_pair = weak_nodes_to_guard_conditions_.find(weak_node_ptr); - if (node_guard_pair != weak_nodes_to_guard_conditions_.end()) { - auto guard_condition = node_guard_pair->second; - weak_nodes_to_guard_conditions_.erase(weak_node_ptr); - memory_strategy_->remove_guard_condition(guard_condition); - } - } - } - std::for_each( - invalid_group_ptrs.begin(), invalid_group_ptrs.end(), - [this](rclcpp::CallbackGroup::WeakPtr group_ptr) { - if (weak_groups_to_nodes_associated_with_executor_.find(group_ptr) != - weak_groups_to_nodes_associated_with_executor_.end()) - { - weak_groups_to_nodes_associated_with_executor_.erase(group_ptr); - } - if (weak_groups_associated_with_executor_to_nodes_.find(group_ptr) != - weak_groups_associated_with_executor_to_nodes_.end()) - { - weak_groups_associated_with_executor_to_nodes_.erase(group_ptr); - } - weak_groups_to_nodes_.erase(group_ptr); - }); - } + current_collection_.clients.update( + collection.clients, + [this](auto client) {wait_set_.add_client(client);}, + [this](auto client) {wait_set_.remove_client(client);}); - // clear wait set - rcl_ret_t ret = rcl_wait_set_clear(&wait_set_); - if (ret != RCL_RET_OK) { - throw_from_rcl_error(ret, "Couldn't clear wait set"); - } + current_collection_.services.update( + collection.services, + [this](auto service) {wait_set_.add_service(service);}, + [this](auto service) {wait_set_.remove_service(service);}); - // The size of waitables are accounted for in size of the other entities - ret = rcl_wait_set_resize( - &wait_set_, memory_strategy_->number_of_ready_subscriptions(), - memory_strategy_->number_of_guard_conditions(), memory_strategy_->number_of_ready_timers(), - memory_strategy_->number_of_ready_clients(), memory_strategy_->number_of_ready_services(), - memory_strategy_->number_of_ready_events()); - if (RCL_RET_OK != ret) { - throw_from_rcl_error(ret, "Couldn't resize the wait set"); - } + 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);}); - if (!memory_strategy_->add_handles_to_wait_set(&wait_set_)) { - throw std::runtime_error("Couldn't fill wait set"); - } - } + current_collection_.waitables.update( + collection.waitables, + [this](auto waitable) {wait_set_.add_waitable(waitable);}, + [this](auto waitable) {wait_set_.remove_waitable(waitable);}); - rcl_ret_t status = - rcl_wait(&wait_set_, std::chrono::duration_cast(timeout).count()); - if (status == RCL_RET_WAIT_SET_EMPTY) { - RCUTILS_LOG_WARN_NAMED( - "rclcpp", - "empty wait set received in rcl_wait(). This should never happen."); - } 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"); - } - - // check the null handles in the wait set and remove them from the handles in memory strategy - // for callback-based entities - std::lock_guard guard(mutex_); - memory_strategy_->remove_null_handles(&wait_set_); + // 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(); } -rclcpp::node_interfaces::NodeBaseInterface::SharedPtr -Executor::get_node_by_group( - const rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & - weak_groups_to_nodes, - rclcpp::CallbackGroup::SharedPtr group) +void +Executor::wait_for_work(std::chrono::nanoseconds timeout) { - if (!group) { - return nullptr; - } - 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; -} + TRACETOOLS_TRACEPOINT(rclcpp_executor_wait_for_work, timeout.count()); -rclcpp::CallbackGroup::SharedPtr -Executor::get_group_by_timer(rclcpp::TimerBase::SharedPtr timer) -{ - std::lock_guard guard{mutex_}; - for (const auto & pair : weak_groups_associated_with_executor_to_nodes_) { - auto group = pair.first.lock(); - if (!group) { - continue; - } - auto timer_ref = group->find_timer_ptrs_if( - [timer](const rclcpp::TimerBase::SharedPtr & timer_ptr) -> bool { - return timer_ptr == timer; - }); - if (timer_ref) { - return group; - } - } + // Clear any previous wait result + this->wait_result_.reset(); - for (const auto & pair : weak_groups_to_nodes_associated_with_executor_) { - auto group = pair.first.lock(); - if (!group) { - continue; + { + std::lock_guard guard(mutex_); + if (this->entities_need_rebuild_.exchange(false) || current_collection_.empty()) { + this->collect_entities(); } - auto timer_ref = group->find_timer_ptrs_if( - [timer](const rclcpp::TimerBase::SharedPtr & timer_ptr) -> bool { - return timer_ptr == timer; - }); - if (timer_ref) { - return group; + } + 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()); + } } } - return nullptr; } bool Executor::get_next_ready_executable(AnyExecutable & any_executable) { - bool success = get_next_ready_executable_from_map(any_executable, weak_groups_to_nodes_); - return success; -} + TRACETOOLS_TRACEPOINT(rclcpp_executor_get_next_ready); -bool -Executor::get_next_ready_executable_from_map( - AnyExecutable & any_executable, - const rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & - weak_groups_to_nodes) -{ - bool success = false; - std::lock_guard guard{mutex_}; - // Check the timers to see if there are any that are ready - memory_strategy_->get_next_timer(any_executable, weak_groups_to_nodes); - if (any_executable.timer) { - success = true; + bool valid_executable = false; + + if (!wait_result_.has_value() || wait_result_->kind() != rclcpp::WaitResultKind::Ready) { + return false; } - if (!success) { - // Check the subscriptions to see if there are any that are ready - memory_strategy_->get_next_subscription(any_executable, weak_groups_to_nodes); - if (any_executable.subscription) { - success = true; + + 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++; } } - if (!success) { - // Check the services to see if there are any that are ready - memory_strategy_->get_next_service(any_executable, weak_groups_to_nodes); - if (any_executable.service) { - success = true; + + 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; + } } } - if (!success) { - // Check the clients to see if there are any that are ready - memory_strategy_->get_next_client(any_executable, weak_groups_to_nodes); - if (any_executable.client) { - success = true; + + 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; + } } } - if (!success) { - // Check the waitables to see if there are any that are ready - memory_strategy_->get_next_waitable(any_executable, weak_groups_to_nodes); - if (any_executable.waitable) { - any_executable.data = any_executable.waitable->take_data(); - success = true; + + 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; + } } } - // At this point any_executable should be valid with either a valid subscription - // or a valid timer, or it should be a null shared_ptr - if (success) { - rclcpp::CallbackGroup::WeakPtr weak_group_ptr = any_executable.callback_group; - auto iter = weak_groups_to_nodes.find(weak_group_ptr); - if (iter == weak_groups_to_nodes.end()) { - success = false; + + 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; + } } } - if (success) { - // If it is valid, check to see if the group is mutually exclusive or - // not, then mark it accordingly ..Check if the callback_group belongs to this executor - if (any_executable.callback_group && any_executable.callback_group->type() == \ - CallbackGroupType::MutuallyExclusive) - { - // It should not have been taken otherwise + if (any_executable.callback_group) { + if (any_executable.callback_group->type() == CallbackGroupType::MutuallyExclusive) { assert(any_executable.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_executable is executed or when the - // any_executable is destructued any_executable.callback_group->can_be_taken_from().store(false); } } - // If there is no ready executable, return false - return success; + + + return valid_executable; } bool @@ -911,18 +893,8 @@ Executor::get_next_executable(AnyExecutable & any_executable, std::chrono::nanos return success; } -// Returns true iff the weak_groups_to_nodes map has node_ptr as the value in any of its entry. bool -Executor::has_node( - const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - const rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & - weak_groups_to_nodes) const -{ - return std::find_if( - weak_groups_to_nodes.begin(), - weak_groups_to_nodes.end(), - [&](const WeakCallbackGroupsToNodesMap::value_type & other) -> bool { - auto other_ptr = other.second.lock(); - return other_ptr == node_ptr; - }) != weak_groups_to_nodes.end(); +Executor::is_spinning() +{ + 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 849815fed5..130b4d953f 100644 --- a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp @@ -21,6 +21,7 @@ #include "rcpputils/scope_exit.hpp" +#include "rclcpp/logging.hpp" #include "rclcpp/utilities.hpp" using rclcpp::executors::MultiThreadedExecutor; @@ -34,9 +35,15 @@ MultiThreadedExecutor::MultiThreadedExecutor( yield_before_execute_(yield_before_execute), next_exec_timeout_(next_exec_timeout) { - number_of_threads_ = number_of_threads ? 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."); } } @@ -48,7 +55,7 @@ MultiThreadedExecutor::spin() if (spinning.exchange(true)) { throw std::runtime_error("spin() called while already spinning"); } - RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); + RCPPUTILS_SCOPE_EXIT(wait_result_.reset();this->spinning.store(false);); std::vector threads; size_t thread_id = 0; { @@ -72,8 +79,9 @@ MultiThreadedExecutor::get_number_of_threads() } void -MultiThreadedExecutor::run(size_t) +MultiThreadedExecutor::run(size_t this_thread_number) { + (void)this_thread_number; while (rclcpp::ok(this->context_) && spinning.load()) { rclcpp::AnyExecutable any_exec; { @@ -91,6 +99,18 @@ MultiThreadedExecutor::run(size_t) 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 e7f311c147..689bbae398 100644 --- a/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp @@ -30,7 +30,12 @@ SingleThreadedExecutor::spin() if (spinning.exchange(true)) { throw std::runtime_error("spin() called while already spinning"); } - RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); + 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)) { diff --git a/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp b/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp deleted file mode 100644 index c0b7fb0e03..0000000000 --- a/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp +++ /dev/null @@ -1,519 +0,0 @@ -// 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/executors/static_executor_entities_collector.hpp" - -#include -#include -#include -#include -#include -#include - -#include "rclcpp/memory_strategy.hpp" -#include "rclcpp/executors/static_single_threaded_executor.hpp" - -using rclcpp::executors::StaticExecutorEntitiesCollector; - -StaticExecutorEntitiesCollector::~StaticExecutorEntitiesCollector() -{ - // Disassociate all callback groups and thus nodes. - for (const auto & pair : weak_groups_associated_with_executor_to_nodes_) { - auto group = pair.first.lock(); - if (group) { - std::atomic_bool & has_executor = group->get_associated_with_executor_atomic(); - has_executor.store(false); - } - } - for (const auto & pair : weak_groups_to_nodes_associated_with_executor_) { - auto group = pair.first.lock(); - if (group) { - std::atomic_bool & has_executor = group->get_associated_with_executor_atomic(); - has_executor.store(false); - } - } - // Disassociate all nodes - for (const 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); - } - } - weak_groups_associated_with_executor_to_nodes_.clear(); - weak_groups_to_nodes_associated_with_executor_.clear(); - exec_list_.clear(); - weak_nodes_.clear(); - weak_nodes_to_guard_conditions_.clear(); -} - -void -StaticExecutorEntitiesCollector::init( - rcl_wait_set_t * p_wait_set, - rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy, - rcl_guard_condition_t * executor_guard_condition) -{ - // Empty initialize executable list - exec_list_ = rclcpp::experimental::ExecutableList(); - // Get executor's wait_set_ pointer - p_wait_set_ = p_wait_set; - // Get executor's memory strategy ptr - if (memory_strategy == nullptr) { - throw std::runtime_error("Received NULL memory strategy in executor waitable."); - } - memory_strategy_ = memory_strategy; - - // Add executor's guard condition - memory_strategy_->add_guard_condition(executor_guard_condition); - - // Get memory strategy and executable list. Prepare wait_set_ - std::shared_ptr shared_ptr; - execute(shared_ptr); - - // The entities collector is now initialized - initialized_ = true; -} - -void -StaticExecutorEntitiesCollector::fini() -{ - memory_strategy_->clear_handles(); - exec_list_.clear(); -} - -std::shared_ptr -StaticExecutorEntitiesCollector::take_data() -{ - return nullptr; -} - -void -StaticExecutorEntitiesCollector::execute(std::shared_ptr & data) -{ - (void) data; - // Fill memory strategy with entities coming from weak_nodes_ - fill_memory_strategy(); - // Fill exec_list_ with entities coming from weak_nodes_ (same as memory strategy) - fill_executable_list(); - // Resize the wait_set_ based on memory_strategy handles (rcl_wait_set_resize) - prepare_wait_set(); -} - -void -StaticExecutorEntitiesCollector::fill_memory_strategy() -{ - memory_strategy_->clear_handles(); - bool has_invalid_weak_groups_or_nodes = - memory_strategy_->collect_entities(weak_groups_to_nodes_associated_with_executor_); - // Clean up any invalid nodes, if they were detected - if (has_invalid_weak_groups_or_nodes) { - std::vector invalid_group_ptrs; - for (const auto & pair : weak_groups_to_nodes_associated_with_executor_) { - auto & weak_group_ptr = pair.first; - auto & weak_node_ptr = pair.second; - if (weak_group_ptr.expired() || weak_node_ptr.expired()) { - invalid_group_ptrs.push_back(weak_group_ptr); - } - } - std::for_each( - invalid_group_ptrs.begin(), invalid_group_ptrs.end(), - [this](rclcpp::CallbackGroup::WeakPtr group_ptr) { - weak_groups_to_nodes_associated_with_executor_.erase(group_ptr); - }); - } - has_invalid_weak_groups_or_nodes = - memory_strategy_->collect_entities(weak_groups_associated_with_executor_to_nodes_); - // Clean up any invalid nodes, if they were detected - if (has_invalid_weak_groups_or_nodes) { - std::vector invalid_group_ptrs; - for (const auto & pair : weak_groups_associated_with_executor_to_nodes_) { - auto & weak_group_ptr = pair.first; - const auto & weak_node_ptr = pair.second; - if (weak_group_ptr.expired() || weak_node_ptr.expired()) { - invalid_group_ptrs.push_back(weak_group_ptr); - } - } - std::for_each( - invalid_group_ptrs.begin(), invalid_group_ptrs.end(), - [this](rclcpp::CallbackGroup::WeakPtr group_ptr) { - weak_groups_associated_with_executor_to_nodes_.erase(group_ptr); - }); - } - - // Add the static executor waitable to the memory strategy - memory_strategy_->add_waitable_handle(this->shared_from_this()); -} - -void -StaticExecutorEntitiesCollector::fill_executable_list() -{ - exec_list_.clear(); - add_callback_groups_from_nodes_associated_to_executor(); - fill_executable_list_from_map(weak_groups_associated_with_executor_to_nodes_); - fill_executable_list_from_map(weak_groups_to_nodes_associated_with_executor_); - // Add the executor's waitable to the executable list - exec_list_.add_waitable(shared_from_this()); -} -void -StaticExecutorEntitiesCollector::fill_executable_list_from_map( - const rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & - weak_groups_to_nodes) -{ - for (const auto & pair : weak_groups_to_nodes) { - auto group = pair.first.lock(); - auto node = pair.second.lock(); - if (!node || !group || !group->can_be_taken_from().load()) { - continue; - } - group->find_timer_ptrs_if( - [this](const rclcpp::TimerBase::SharedPtr & timer) { - if (timer) { - exec_list_.add_timer(timer); - } - return false; - }); - group->find_subscription_ptrs_if( - [this](const rclcpp::SubscriptionBase::SharedPtr & subscription) { - if (subscription) { - exec_list_.add_subscription(subscription); - } - return false; - }); - group->find_service_ptrs_if( - [this](const rclcpp::ServiceBase::SharedPtr & service) { - if (service) { - exec_list_.add_service(service); - } - return false; - }); - group->find_client_ptrs_if( - [this](const rclcpp::ClientBase::SharedPtr & client) { - if (client) { - exec_list_.add_client(client); - } - return false; - }); - group->find_waitable_ptrs_if( - [this](const rclcpp::Waitable::SharedPtr & waitable) { - if (waitable) { - exec_list_.add_waitable(waitable); - } - return false; - }); - } -} - -void -StaticExecutorEntitiesCollector::prepare_wait_set() -{ - // clear wait set - if (rcl_wait_set_clear(p_wait_set_) != RCL_RET_OK) { - throw std::runtime_error("Couldn't clear wait set"); - } - - // The size of waitables are accounted for in size of the other entities - rcl_ret_t ret = rcl_wait_set_resize( - p_wait_set_, memory_strategy_->number_of_ready_subscriptions(), - memory_strategy_->number_of_guard_conditions(), memory_strategy_->number_of_ready_timers(), - memory_strategy_->number_of_ready_clients(), memory_strategy_->number_of_ready_services(), - memory_strategy_->number_of_ready_events()); - - if (RCL_RET_OK != ret) { - throw std::runtime_error( - std::string("Couldn't resize the wait set: ") + rcl_get_error_string().str); - } -} - -void -StaticExecutorEntitiesCollector::refresh_wait_set(std::chrono::nanoseconds timeout) -{ - // clear wait set (memset to '0' all wait_set_ entities - // but keeps the wait_set_ number of entities) - if (rcl_wait_set_clear(p_wait_set_) != RCL_RET_OK) { - throw std::runtime_error("Couldn't clear wait set"); - } - - if (!memory_strategy_->add_handles_to_wait_set(p_wait_set_)) { - throw std::runtime_error("Couldn't fill wait set"); - } - - rcl_ret_t status = - rcl_wait(p_wait_set_, std::chrono::duration_cast(timeout).count()); - - if (status == RCL_RET_WAIT_SET_EMPTY) { - RCUTILS_LOG_WARN_NAMED( - "rclcpp", - "empty wait set received in rcl_wait(). This should never happen."); - } 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"); - } -} - -bool -StaticExecutorEntitiesCollector::add_to_wait_set(rcl_wait_set_t * wait_set) -{ - // Add waitable guard conditions (one for each registered node) into the wait set. - for (const auto & pair : weak_nodes_to_guard_conditions_) { - auto & gc = pair.second; - rcl_ret_t ret = rcl_wait_set_add_guard_condition(wait_set, gc, NULL); - if (ret != RCL_RET_OK) { - throw std::runtime_error("Executor waitable: couldn't add guard condition to wait set"); - } - } - return true; -} - -size_t StaticExecutorEntitiesCollector::get_number_of_ready_guard_conditions() -{ - return weak_nodes_to_guard_conditions_.size(); -} - -bool -StaticExecutorEntitiesCollector::add_node( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr) -{ - bool is_new_node = false; - // 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."); - } - node_ptr->for_each_callback_group( - [this, node_ptr, &is_new_node](rclcpp::CallbackGroup::SharedPtr group_ptr) - { - if ( - !group_ptr->get_associated_with_executor_atomic().load() && - group_ptr->automatically_add_to_executor_with_node()) - { - is_new_node = (add_callback_group( - group_ptr, - node_ptr, - weak_groups_to_nodes_associated_with_executor_) || - is_new_node); - } - }); - weak_nodes_.push_back(node_ptr); - return is_new_node; -} - -bool -StaticExecutorEntitiesCollector::add_callback_group( - rclcpp::CallbackGroup::SharedPtr group_ptr, - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) -{ - // If the callback_group already has an executor - 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."); - } - bool is_new_node = !has_node(node_ptr, weak_groups_associated_with_executor_to_nodes_) && - !has_node(node_ptr, weak_groups_to_nodes_associated_with_executor_); - rclcpp::CallbackGroup::WeakPtr weak_group_ptr = group_ptr; - auto insert_info = weak_groups_to_nodes.insert( - std::make_pair(weak_group_ptr, node_ptr)); - bool was_inserted = insert_info.second; - if (!was_inserted) { - throw std::runtime_error("Callback group was already added to executor."); - } - if (is_new_node) { - weak_nodes_to_guard_conditions_[node_ptr] = node_ptr->get_notify_guard_condition(); - return true; - } - return false; -} - -bool -StaticExecutorEntitiesCollector::add_callback_group( - rclcpp::CallbackGroup::SharedPtr group_ptr, - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr) -{ - return add_callback_group(group_ptr, node_ptr, weak_groups_associated_with_executor_to_nodes_); -} - -bool -StaticExecutorEntitiesCollector::remove_callback_group( - rclcpp::CallbackGroup::SharedPtr group_ptr) -{ - return this->remove_callback_group_from_map( - group_ptr, - weak_groups_associated_with_executor_to_nodes_); -} - -bool -StaticExecutorEntitiesCollector::remove_callback_group_from_map( - rclcpp::CallbackGroup::SharedPtr group_ptr, - rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) -{ - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr; - rclcpp::CallbackGroup::WeakPtr weak_group_ptr = group_ptr; - auto iter = weak_groups_to_nodes.find(weak_group_ptr); - if (iter != weak_groups_to_nodes.end()) { - node_ptr = iter->second.lock(); - if (node_ptr == nullptr) { - throw std::runtime_error("Node must not be deleted before its callback group(s)."); - } - weak_groups_to_nodes.erase(iter); - } else { - throw std::runtime_error("Callback group needs to be associated with executor."); - } - // If the node was matched and removed, interrupt waiting. - if (!has_node(node_ptr, weak_groups_associated_with_executor_to_nodes_) && - !has_node(node_ptr, weak_groups_to_nodes_associated_with_executor_)) - { - rclcpp::node_interfaces::NodeBaseInterface::WeakPtr node_weak_ptr(node_ptr); - weak_nodes_to_guard_conditions_.erase(node_weak_ptr); - return true; - } - return false; -} - -bool -StaticExecutorEntitiesCollector::remove_node( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr) -{ - if (!node_ptr->get_associated_with_executor_atomic().load()) { - return false; - } - bool node_found = false; - auto node_it = weak_nodes_.begin(); - while (node_it != weak_nodes_.end()) { - bool matched = (node_it->lock() == node_ptr); - if (matched) { - weak_nodes_.erase(node_it); - node_found = true; - break; - } - ++node_it; - } - if (!node_found) { - return false; - } - std::vector found_group_ptrs; - std::for_each( - weak_groups_to_nodes_associated_with_executor_.begin(), - weak_groups_to_nodes_associated_with_executor_.end(), - [&found_group_ptrs, node_ptr](std::pair key_value_pair) { - auto & weak_node_ptr = key_value_pair.second; - auto shared_node_ptr = weak_node_ptr.lock(); - auto group_ptr = key_value_pair.first.lock(); - if (shared_node_ptr == node_ptr) { - found_group_ptrs.push_back(group_ptr); - } - }); - std::for_each( - found_group_ptrs.begin(), found_group_ptrs.end(), [this] - (rclcpp::CallbackGroup::SharedPtr group_ptr) { - this->remove_callback_group_from_map( - group_ptr, - weak_groups_to_nodes_associated_with_executor_); - }); - std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic(); - has_executor.store(false); - return true; -} - -bool -StaticExecutorEntitiesCollector::is_ready(rcl_wait_set_t * p_wait_set) -{ - // Check wait_set guard_conditions for added/removed entities to/from a node - for (size_t i = 0; i < p_wait_set->size_of_guard_conditions; ++i) { - if (p_wait_set->guard_conditions[i] != NULL) { - auto found_guard_condition = std::find_if( - weak_nodes_to_guard_conditions_.begin(), weak_nodes_to_guard_conditions_.end(), - [&](std::pair pair) -> bool { - return pair.second == p_wait_set->guard_conditions[i]; - }); - if (found_guard_condition != weak_nodes_to_guard_conditions_.end()) { - return true; - } - } - } - // None of the guard conditions triggered belong to a registered node - return false; -} - -// Returns true iff the weak_groups_to_nodes map has node_ptr as the value in any of its entry. -bool -StaticExecutorEntitiesCollector::has_node( - const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - const rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & - weak_groups_to_nodes) const -{ - return std::find_if( - weak_groups_to_nodes.begin(), - weak_groups_to_nodes.end(), - [&](const WeakCallbackGroupsToNodesMap::value_type & other) -> bool { - auto other_ptr = other.second.lock(); - return other_ptr == node_ptr; - }) != weak_groups_to_nodes.end(); -} - -void -StaticExecutorEntitiesCollector::add_callback_groups_from_nodes_associated_to_executor() -{ - for (const auto & weak_node : weak_nodes_) { - auto node = weak_node.lock(); - if (node) { - node->for_each_callback_group( - [this, node](rclcpp::CallbackGroup::SharedPtr shared_group_ptr) - { - if (shared_group_ptr->automatically_add_to_executor_with_node() && - !shared_group_ptr->get_associated_with_executor_atomic().load()) - { - add_callback_group( - shared_group_ptr, - node, - weak_groups_to_nodes_associated_with_executor_); - } - }); - } - } -} - -std::vector -StaticExecutorEntitiesCollector::get_all_callback_groups() -{ - std::vector groups; - for (const auto & group_node_ptr : weak_groups_associated_with_executor_to_nodes_) { - groups.push_back(group_node_ptr.first); - } - for (const auto & group_node_ptr : weak_groups_to_nodes_associated_with_executor_) { - groups.push_back(group_node_ptr.first); - } - return groups; -} - -std::vector -StaticExecutorEntitiesCollector::get_manually_added_callback_groups() -{ - std::vector groups; - for (const auto & group_node_ptr : weak_groups_associated_with_executor_to_nodes_) { - groups.push_back(group_node_ptr.first); - } - return groups; -} - -std::vector -StaticExecutorEntitiesCollector::get_automatically_added_callback_groups_from_nodes() -{ - std::vector groups; - for (const auto & group_node_ptr : weak_groups_to_nodes_associated_with_executor_) { - groups.push_back(group_node_ptr.first); - } - return groups; -} diff --git a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp index bc27bdf464..d517ccafd0 100644 --- a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp @@ -12,30 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "rclcpp/executors/static_single_threaded_executor.hpp" - -#include -#include -#include - +#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; -using rclcpp::experimental::ExecutableList; -StaticSingleThreadedExecutor::StaticSingleThreadedExecutor( - const rclcpp::ExecutorOptions & options) +StaticSingleThreadedExecutor::StaticSingleThreadedExecutor(const rclcpp::ExecutorOptions & options) : rclcpp::Executor(options) { - entities_collector_ = std::make_shared(); } -StaticSingleThreadedExecutor::~StaticSingleThreadedExecutor() -{ - if (entities_collector_->is_init()) { - entities_collector_->fini(); - } -} +StaticSingleThreadedExecutor::~StaticSingleThreadedExecutor() {} void StaticSingleThreadedExecutor::spin() @@ -45,14 +35,11 @@ StaticSingleThreadedExecutor::spin() } RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); - // Set memory_strategy_ and exec_list_ based on weak_nodes_ - // Prepare wait_set_ based on memory_strategy_ - entities_collector_->init(&wait_set_, memory_strategy_, &interrupt_guard_condition_); - + // 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()) { - // Refresh wait set and wait for work - entities_collector_->refresh_wait_set(); - execute_ready_executables(); + this->spin_once_impl(std::chrono::nanoseconds(-1)); } } @@ -63,15 +50,14 @@ StaticSingleThreadedExecutor::spin_some(std::chrono::nanoseconds max_duration) 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 positive"); + 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); } @@ -79,36 +65,32 @@ StaticSingleThreadedExecutor::spin_all(std::chrono::nanoseconds max_duration) void StaticSingleThreadedExecutor::spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive) { - // Make sure the entities collector has been initialized - if (!entities_collector_->is_init()) { - entities_collector_->init(&wait_set_, memory_strategy_, &interrupt_guard_condition_); - } - 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; + 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); ); + RCPPUTILS_SCOPE_EXIT(this->spinning.store(false);); while (rclcpp::ok(context_) && spinning.load() && max_duration_not_elapsed()) { // Get executables that are ready now - entities_collector_->refresh_wait_set(std::chrono::milliseconds::zero()); - // Execute ready executables - bool work_available = execute_ready_executables(); - if (!work_available || !exhaustive) { - break; + 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; + } } } } @@ -116,169 +98,106 @@ StaticSingleThreadedExecutor::spin_some_impl(std::chrono::nanoseconds max_durati void StaticSingleThreadedExecutor::spin_once_impl(std::chrono::nanoseconds timeout) { - // Make sure the entities collector has been initialized - if (!entities_collector_->is_init()) { - entities_collector_->init(&wait_set_, memory_strategy_, &interrupt_guard_condition_); - } - if (rclcpp::ok(context_) && spinning.load()) { - // Wait until we have a ready entity or timeout expired - entities_collector_->refresh_wait_set(timeout); - // Execute ready executables - execute_ready_executables(true); - } -} - -void -StaticSingleThreadedExecutor::add_callback_group( - rclcpp::CallbackGroup::SharedPtr group_ptr, - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - bool notify) -{ - bool is_new_node = entities_collector_->add_callback_group(group_ptr, node_ptr); - if (is_new_node && 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().str); + 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); } } } -void -StaticSingleThreadedExecutor::add_node( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify) +std::optional> +StaticSingleThreadedExecutor::collect_and_wait(std::chrono::nanoseconds timeout) { - bool is_new_node = entities_collector_->add_node(node_ptr); - if (is_new_node && 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().str); - } + if (this->entities_need_rebuild_.exchange(false) || current_collection_.empty()) { + this->collect_entities(); } -} - -void -StaticSingleThreadedExecutor::add_node(std::shared_ptr node_ptr, bool notify) -{ - this->add_node(node_ptr->get_node_base_interface(), notify); -} - -void -StaticSingleThreadedExecutor::remove_callback_group( - rclcpp::CallbackGroup::SharedPtr group_ptr, bool notify) -{ - bool node_removed = entities_collector_->remove_callback_group(group_ptr); - // If the node was matched and removed, interrupt waiting - if (node_removed && notify) { - if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) { - throw std::runtime_error(rcl_get_error_string().str); + 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; } -void -StaticSingleThreadedExecutor::remove_node( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify) +// 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 node_removed = entities_collector_->remove_node(node_ptr); - if (!node_removed) { - throw std::runtime_error("Node needs to be associated with this executor."); + bool any_ready_executable = false; + if (wait_result.kind() != rclcpp::WaitResultKind::Ready) { + return any_ready_executable; } - // If the node was matched and removed, interrupt waiting - if (notify) { - if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) { - throw std::runtime_error(rcl_get_error_string().str); + + 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;} } } -} - -std::vector -StaticSingleThreadedExecutor::get_all_callback_groups() -{ - return entities_collector_->get_all_callback_groups(); -} - -std::vector -StaticSingleThreadedExecutor::get_manually_added_callback_groups() -{ - return entities_collector_->get_manually_added_callback_groups(); -} - -std::vector -StaticSingleThreadedExecutor::get_automatically_added_callback_groups_from_nodes() -{ - return entities_collector_->get_automatically_added_callback_groups_from_nodes(); -} - -void -StaticSingleThreadedExecutor::remove_node(std::shared_ptr node_ptr, bool notify) -{ - this->remove_node(node_ptr->get_node_base_interface(), notify); -} -bool -StaticSingleThreadedExecutor::execute_ready_executables(bool spin_once) -{ - bool any_ready_executable = false; - - // Execute all the ready subscriptions - for (size_t i = 0; i < wait_set_.size_of_subscriptions; ++i) { - if (i < entities_collector_->get_number_of_subscriptions()) { - if (wait_set_.subscriptions[i]) { - execute_subscription(entities_collector_->get_subscription(i)); - if (spin_once) { - return true; - } - any_ready_executable = true; - } + 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; } - } - // Execute all the ready timers - for (size_t i = 0; i < wait_set_.size_of_timers; ++i) { - if (i < entities_collector_->get_number_of_timers()) { - if (wait_set_.timers[i] && entities_collector_->get_timer(i)->is_ready()) { - execute_timer(entities_collector_->get_timer(i)); - if (spin_once) { - return true; - } - any_ready_executable = true; + 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;} } } - // Execute all the ready services - for (size_t i = 0; i < wait_set_.size_of_services; ++i) { - if (i < entities_collector_->get_number_of_services()) { - if (wait_set_.services[i]) { - execute_service(entities_collector_->get_service(i)); - if (spin_once) { - return true; - } - any_ready_executable = true; - } + + 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;} } } - // Execute all the ready clients - for (size_t i = 0; i < wait_set_.size_of_clients; ++i) { - if (i < entities_collector_->get_number_of_clients()) { - if (wait_set_.clients[i]) { - execute_client(entities_collector_->get_client(i)); - if (spin_once) { - return true; - } - any_ready_executable = true; - } + + 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;} } } - // Execute all the ready waitables - for (size_t i = 0; i < entities_collector_->get_number_of_waitables(); ++i) { - auto waitable = entities_collector_->get_waitable(i); - if (waitable->is_ready(&wait_set_)) { - auto data = waitable->take_data(); + + 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); - if (spin_once) { - return true; - } any_ready_executable = true; + if (spin_once) {return any_ready_executable;} } } return any_ready_executable; 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/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 index 733aa5dd5c..d526a6fac1 100644 --- a/rclcpp/src/rclcpp/generic_publisher.cpp +++ b/rclcpp/src/rclcpp/generic_publisher.cpp @@ -23,6 +23,10 @@ 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); @@ -31,4 +35,50 @@ void GenericPublisher::publish(const rclcpp::SerializedMessage & 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 index cc50955773..ae28354b98 100644 --- a/rclcpp/src/rclcpp/generic_subscription.cpp +++ b/rclcpp/src/rclcpp/generic_subscription.cpp @@ -25,17 +25,20 @@ namespace rclcpp { -std::shared_ptr GenericSubscription::create_message() +std::shared_ptr +GenericSubscription::create_message() { return create_serialized_message(); } -std::shared_ptr GenericSubscription::create_serialized_message() +std::shared_ptr +GenericSubscription::create_serialized_message() { return std::make_shared(0); } -void GenericSubscription::handle_message( +void +GenericSubscription::handle_message( std::shared_ptr &, const rclcpp::MessageInfo &) { @@ -46,12 +49,13 @@ void GenericSubscription::handle_message( void GenericSubscription::handle_serialized_message( const std::shared_ptr & message, - const rclcpp::MessageInfo &) + const rclcpp::MessageInfo & message_info) { - callback_(message); + any_callback_.dispatch(message, message_info); } -void GenericSubscription::handle_loaned_message( +void +GenericSubscription::handle_loaned_message( void * message, const rclcpp::MessageInfo & message_info) { (void) message; @@ -60,16 +64,69 @@ void GenericSubscription::handle_loaned_message( "handle_loaned_message is not implemented for GenericSubscription"); } -void GenericSubscription::return_message(std::shared_ptr & message) +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( +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 0af47acc5b..d7ea42117a 100644 --- a/rclcpp/src/rclcpp/graph_listener.cpp +++ b/rclcpp/src/rclcpp/graph_listener.cpp @@ -22,6 +22,7 @@ #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" @@ -40,24 +41,14 @@ 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) + is_shutdown_(false), + interrupt_guard_condition_(parent_context) { - // TODO(wjwwood): make a guard condition class in rclcpp so this can be tracked - // automatically with the rcl guard condition - // hold on to this context to prevent it from going out of scope while this - // guard condition is using it. - rcl_ret_t ret = rcl_guard_condition_init( - &interrupt_guard_condition_, - rcl_parent_context_.get(), - rcl_guard_condition_get_default_options()); - if (RCL_RET_OK != ret) { - throw_from_rcl_error(ret, "failed to create interrupt guard condition"); - } } GraphListener::~GraphListener() { - this->shutdown(std::nothrow); + GraphListener::shutdown(std::nothrow); } void GraphListener::init_wait_set() @@ -159,10 +150,7 @@ GraphListener::run_loop() 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_, NULL); - if (RCL_RET_OK != ret) { - throw_from_rcl_error(ret, "failed to add interrupt 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. std::vector graph_gc_indexes(node_graph_interfaces_size, 0u); @@ -211,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 @@ -351,10 +336,6 @@ 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 (is_started_) { cleanup_wait_set(); } diff --git a/rclcpp/src/rclcpp/guard_condition.cpp b/rclcpp/src/rclcpp/guard_condition.cpp index 22ca3f3223..700985f620 100644 --- a/rclcpp/src/rclcpp/guard_condition.cpp +++ b/rclcpp/src/rclcpp/guard_condition.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include + #include "rclcpp/guard_condition.hpp" #include "rclcpp/exceptions.hpp" @@ -20,19 +22,21 @@ namespace rclcpp { -GuardCondition::GuardCondition(rclcpp::Context::SharedPtr context) -: context_(context), rcl_guard_condition_{rcl_get_zero_initialized_guard_condition()} +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_) { + if (!context) { throw std::invalid_argument("context argument unexpectedly nullptr"); } - rcl_guard_condition_options_t guard_condition_options = rcl_guard_condition_get_default_options(); + rcl_ret_t ret = rcl_guard_condition_init( &this->rcl_guard_condition_, - context_->get_rcl_context().get(), + context->get_rcl_context().get(), guard_condition_options); if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to create guard condition"); } } @@ -45,15 +49,15 @@ GuardCondition::~GuardCondition() } catch (const std::exception & exception) { RCLCPP_ERROR( rclcpp::get_logger("rclcpp"), - "Error in destruction of rcl guard condition: %s", exception.what()); + "failed to finalize guard condition: %s", exception.what()); } } } -rclcpp::Context::SharedPtr -GuardCondition::get_context() const +rcl_guard_condition_t & +GuardCondition::get_rcl_guard_condition() { - return context_; + return rcl_guard_condition_; } const rcl_guard_condition_t & @@ -69,6 +73,16 @@ GuardCondition::trigger() 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 @@ -77,4 +91,41 @@ 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 index 49b408b8b4..5b8452c09a 100644 --- a/rclcpp/src/rclcpp/init_options.cpp +++ b/rclcpp/src/rclcpp/init_options.cpp @@ -43,7 +43,7 @@ InitOptions::InitOptions(const rcl_init_options_t & init_options) InitOptions::InitOptions(const InitOptions & other) : InitOptions(*other.get_rcl_init_options()) { - shutdown_on_sigint = other.shutdown_on_sigint; + shutdown_on_signal = other.shutdown_on_signal; initialize_logging_ = other.initialize_logging_; } @@ -70,7 +70,7 @@ InitOptions::operator=(const InitOptions & other) if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret, "failed to copy rcl init options"); } - this->shutdown_on_sigint = other.shutdown_on_sigint; + this->shutdown_on_signal = other.shutdown_on_signal; this->initialize_logging_ = other.initialize_logging_; } return *this; diff --git a/rclcpp/src/rclcpp/intra_process_manager.cpp b/rclcpp/src/rclcpp/intra_process_manager.cpp index efce4afeaf..831ffdf2ca 100644 --- a/rclcpp/src/rclcpp/intra_process_manager.cpp +++ b/rclcpp/src/rclcpp/intra_process_manager.cpp @@ -32,13 +32,24 @@ IntraProcessManager::~IntraProcessManager() {} uint64_t -IntraProcessManager::add_publisher(rclcpp::PublisherBase::SharedPtr publisher) +IntraProcessManager::add_publisher( + rclcpp::PublisherBase::SharedPtr publisher, + rclcpp::experimental::buffers::IntraProcessBufferBase::SharedPtr buffer) { 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(); @@ -58,30 +69,6 @@ IntraProcessManager::add_publisher(rclcpp::PublisherBase::SharedPtr publisher) return pub_id; } -uint64_t -IntraProcessManager::add_subscription(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()); - } - } - - return sub_id; -} - void IntraProcessManager::remove_subscription(uint64_t intra_process_subscription_id) { @@ -112,6 +99,7 @@ IntraProcessManager::remove_publisher(uint64_t 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); } @@ -225,5 +213,52 @@ IntraProcessManager::can_communicate( 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 b58edd4c80..788ae31d31 100644 --- a/rclcpp/src/rclcpp/logger.cpp +++ b/rclcpp/src/rclcpp/logger.cpp @@ -12,14 +12,21 @@ // 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 { @@ -48,6 +55,14 @@ get_node_logger(const rcl_node_t * node) 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() { @@ -61,6 +76,66 @@ get_logging_directory() 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) @@ -80,4 +155,18 @@ Logger::set_level(Level level) } } +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 index 308a21fe73..bbbe9bbeed 100644 --- a/rclcpp/src/rclcpp/logging_mutex.cpp +++ b/rclcpp/src/rclcpp/logging_mutex.cpp @@ -14,6 +14,7 @@ #include #include +#include #include "rcutils/macros.h" diff --git a/rclcpp/src/rclcpp/memory_strategy.cpp b/rclcpp/src/rclcpp/memory_strategy.cpp index bd32bdb169..cb69dc0d26 100644 --- a/rclcpp/src/rclcpp/memory_strategy.cpp +++ b/rclcpp/src/rclcpp/memory_strategy.cpp @@ -19,7 +19,7 @@ using rclcpp::memory_strategy::MemoryStrategy; rclcpp::SubscriptionBase::SharedPtr MemoryStrategy::get_subscription_by_handle( - std::shared_ptr subscriber_handle, + const std::shared_ptr & subscriber_handle, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { for (const auto & pair : weak_groups_to_nodes) { @@ -40,7 +40,7 @@ MemoryStrategy::get_subscription_by_handle( rclcpp::ServiceBase::SharedPtr MemoryStrategy::get_service_by_handle( - std::shared_ptr service_handle, + const std::shared_ptr & service_handle, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { for (const auto & pair : weak_groups_to_nodes) { @@ -61,7 +61,7 @@ MemoryStrategy::get_service_by_handle( rclcpp::ClientBase::SharedPtr MemoryStrategy::get_client_by_handle( - std::shared_ptr client_handle, + const std::shared_ptr & client_handle, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { for (const auto & pair : weak_groups_to_nodes) { @@ -82,7 +82,7 @@ MemoryStrategy::get_client_by_handle( rclcpp::TimerBase::SharedPtr MemoryStrategy::get_timer_by_handle( - std::shared_ptr timer_handle, + const std::shared_ptr & timer_handle, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { for (const auto & pair : weak_groups_to_nodes) { @@ -103,7 +103,7 @@ MemoryStrategy::get_timer_by_handle( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr MemoryStrategy::get_node_by_group( - rclcpp::CallbackGroup::SharedPtr group, + const rclcpp::CallbackGroup::SharedPtr & group, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { if (!group) { @@ -121,7 +121,7 @@ MemoryStrategy::get_node_by_group( rclcpp::CallbackGroup::SharedPtr MemoryStrategy::get_group_by_subscription( - rclcpp::SubscriptionBase::SharedPtr subscription, + const rclcpp::SubscriptionBase::SharedPtr & subscription, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { for (const auto & pair : weak_groups_to_nodes) { @@ -143,7 +143,7 @@ MemoryStrategy::get_group_by_subscription( rclcpp::CallbackGroup::SharedPtr MemoryStrategy::get_group_by_service( - rclcpp::ServiceBase::SharedPtr service, + const rclcpp::ServiceBase::SharedPtr & service, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { for (const auto & pair : weak_groups_to_nodes) { @@ -165,7 +165,7 @@ MemoryStrategy::get_group_by_service( rclcpp::CallbackGroup::SharedPtr MemoryStrategy::get_group_by_client( - rclcpp::ClientBase::SharedPtr client, + const rclcpp::ClientBase::SharedPtr & client, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { for (const auto & pair : weak_groups_to_nodes) { @@ -187,7 +187,7 @@ MemoryStrategy::get_group_by_client( rclcpp::CallbackGroup::SharedPtr MemoryStrategy::get_group_by_timer( - rclcpp::TimerBase::SharedPtr timer, + const rclcpp::TimerBase::SharedPtr & timer, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { for (const auto & pair : weak_groups_to_nodes) { @@ -209,7 +209,7 @@ MemoryStrategy::get_group_by_timer( rclcpp::CallbackGroup::SharedPtr MemoryStrategy::get_group_by_waitable( - rclcpp::Waitable::SharedPtr waitable, + const rclcpp::Waitable::SharedPtr & waitable, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { for (const auto & pair : weak_groups_to_nodes) { diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index c9ed1e3ac2..1a68c7f108 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -23,6 +23,7 @@ #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" @@ -36,6 +37,7 @@ #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" @@ -47,6 +49,9 @@ 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) @@ -54,8 +59,14 @@ extend_sub_namespace(const std::string & existing_sub_namespace, const std::stri // Assumption is that the existing_sub_namespace does not need checking // because it would be checked already when it was set with this function. - // check if the new sub-namespace extension is absolute - if (extension.front() == '/') { + 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(), @@ -70,7 +81,7 @@ extend_sub_namespace(const std::string & existing_sub_namespace, const std::stri new_sub_namespace = existing_sub_namespace + "/" + extension; } - // remove any trailing `/` so that new extensions do no result in `//` + // 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); } @@ -86,7 +97,11 @@ create_effective_namespace(const std::string & node_namespace, const std::string // and do not need trimming of `/` and other things, as they were validated // in other functions already. - if (node_namespace.back() == '/') { + // 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 { @@ -94,6 +109,24 @@ create_effective_namespace(const std::string & node_namespace, const std::string } } +} // 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 NodeOptions & options) @@ -152,7 +185,7 @@ Node::Node( 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_timers_.get())), node_services_(new rclcpp::node_interfaces::NodeServices(node_base_.get())), @@ -161,7 +194,8 @@ Node::Node( node_topics_, node_graph_, node_services_, - node_logging_ + node_logging_, + options.clock_type() )), node_parameters_(new rclcpp::node_interfaces::NodeParameters( node_base_, @@ -190,6 +224,12 @@ Node::Node( options.clock_qos(), options.use_clock_thread() )), + node_type_descriptions_(new rclcpp::node_interfaces::NodeTypeDescriptions( + node_base_, + node_logging_, + node_parameters_, + node_services_ + )), node_waitables_(new rclcpp::node_interfaces::NodeWaitables(node_base_.get())), node_options_(options), sub_namespace_(""), @@ -209,6 +249,10 @@ Node::Node( 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( @@ -222,9 +266,12 @@ Node::Node( 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_)) + effective_namespace_(create_effective_namespace(other.get_namespace(), sub_namespace_)), + hidden_impl_(other.hidden_impl_) { // Validate new effective namespace. int validation_result; @@ -293,24 +340,6 @@ Node::create_callback_group( return node_base_->create_callback_group(group_type, automatically_add_to_executor_with_node); } -const rclcpp::ParameterValue & -Node::declare_parameter(const std::string & name) -{ -#ifndef _WIN32 -# pragma GCC diagnostic push -# pragma GCC diagnostic ignored "-Wdeprecated-declarations" -#else -# pragma warning(push) -# pragma warning(disable: 4996) -#endif - return this->node_parameters_->declare_parameter(name); -#ifndef _WIN32 -# pragma GCC diagnostic pop -#else -# pragma warning(pop) -#endif -} - const rclcpp::ParameterValue & Node::declare_parameter( const std::string & name, @@ -354,7 +383,7 @@ Node::has_parameter(const std::string & name) const rcl_interfaces::msg::SetParametersResult Node::set_parameter(const rclcpp::Parameter & parameter) { - return this->set_parameters_atomically({parameter}); + return node_parameters_->set_parameters_atomically({parameter}); } std::vector @@ -419,16 +448,40 @@ Node::list_parameters(const std::vector & prefixes, uint64_t depth) 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(OnParametersSetCallbackType callback) +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 callback) +Node::remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const handler) { - return node_parameters_->remove_on_set_parameters_callback(callback); + 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 @@ -470,6 +523,18 @@ Node::count_subscribers(const std::string & topic_name) const return node_graph_->count_subscribers(topic_name); } +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 { @@ -563,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() { @@ -606,3 +677,20 @@ 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 5240c8c818..5648290654 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp @@ -20,6 +20,9 @@ #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 "rcutils/logging_macros.h" #include "rmw/validate_namespace.h" @@ -37,52 +40,30 @@ NodeBase::NodeBase( rclcpp::Context::SharedPtr context, const rcl_node_options_t & rcl_node_options, bool use_intra_process_default, - bool enable_topic_statistics_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_, context_->get_rcl_context().get(), 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) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "failed to destroy guard condition: %s", rcl_get_error_string().str); - } - }; - // Create the rcl node and store it in a shared_ptr with a custom destructor. std::unique_ptr rcl_node(new rcl_node_t(rcl_get_zero_initialized_node())); std::shared_ptr logging_mutex = get_global_logging_mutex(); - { - std::lock_guard guard(*logging_mutex); - // TODO(ivanpauno): /rosout Qos should be reconfigurable. - // TODO(ivanpauno): Instead of mutually excluding rcl_node_init with the global logger mutex, - // rcl_logging_rosout_init_publisher_for_node could be decoupled from there and be called - // here directly. - 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) { - // Finalize the interrupt guard condition. - finalize_notify_guard_condition(); + 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; @@ -131,13 +112,29 @@ 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.release(), - [logging_mutex](rcl_node_t * node) -> void { - std::lock_guard guard(*logging_mutex); - // TODO(ivanpauno): Instead of mutually excluding rcl_node_fini with the global logger mutex, - // rcl_logging_rosout_fini_publisher_for_node could be decoupled from there and be called - // here directly. + [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) { RCUTILS_LOG_ERROR_NAMED( "rclcpp", @@ -146,9 +143,14 @@ NodeBase::NodeBase( delete node; }); - // Create the default callback group. - using rclcpp::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; @@ -160,11 +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) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "failed to destroy guard condition: %s", rcl_get_error_string().str); - } } } @@ -207,13 +204,13 @@ 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::CallbackGroup::SharedPtr @@ -223,9 +220,20 @@ NodeBase::create_callback_group( { 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; } @@ -265,20 +273,34 @@ 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_; } -std::unique_lock -NodeBase::acquire_notify_guard_condition_lock() const +void +NodeBase::trigger_notify_guard_condition() { - return std::unique_lock(notify_guard_condition_mutex_); + 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 diff --git a/rclcpp/src/rclcpp/node_interfaces/node_clock.cpp b/rclcpp/src/rclcpp/node_interfaces/node_clock.cpp index 734d7bdb37..a37c65b71b 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_clock.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_clock.cpp @@ -24,13 +24,14 @@ NodeClock::NodeClock( 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::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), node_logging_(node_logging), - ros_clock_(std::make_shared(RCL_ROS_TIME)) + clock_(std::make_shared(clock_type)) {} NodeClock::~NodeClock() @@ -39,11 +40,11 @@ NodeClock::~NodeClock() rclcpp::Clock::SharedPtr NodeClock::get_clock() { - return ros_clock_; + return clock_; } rclcpp::Clock::ConstSharedPtr NodeClock::get_clock() const { - return ros_clock_; + return clock_; } diff --git a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp index 415b6277ec..f6e9e1fda0 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include #include @@ -27,6 +28,7 @@ #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; @@ -178,6 +180,115 @@ NodeGraph::get_service_names_and_types_by_node( 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 { @@ -206,6 +317,81 @@ NodeGraph::get_node_names() const 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 { @@ -237,7 +423,6 @@ NodeGraph::get_node_names_and_namespaces() const throw std::runtime_error(error_msg); } - std::vector> node_names; node_names.reserve(node_names_c.size); for (size_t i = 0; i < node_names_c.size; ++i) { @@ -313,6 +498,50 @@ NodeGraph::count_subscribers(const std::string & topic_name) const 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; +} + const rcl_guard_condition_t * NodeGraph::get_graph_guard_condition() const { @@ -348,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()); } } @@ -604,3 +832,15 @@ 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 269f14dac1..4f7476a09e 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_logging.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_logging.cpp @@ -12,14 +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) { - logger_ = rclcpp::get_logger(this->get_logger_name()); + logger_ = rclcpp::get_logger(NodeLogging::get_logger_name()); } NodeLogging::~NodeLogging() @@ -37,3 +39,55 @@ 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 92d42bd8e7..922ce9e4d1 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -39,6 +39,31 @@ 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( const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, @@ -101,20 +126,43 @@ NodeParameters::NodeParameters( // 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) { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.dynamic_typing = true; - for (const auto & pair : this->get_parameter_overrides()) { - if (!this->has_parameter(pair.first)) { - this->declare_parameter( - pair.first, - pair.second, - descriptor, - true); + 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); } - } + ); } } +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() {} @@ -236,6 +284,9 @@ __check_parameters( } 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; @@ -254,19 +305,54 @@ __check_parameters( return result; } -using OnParametersSetCallbackType = - rclcpp::node_interfaces::NodeParametersInterface::OnParametersSetCallbackType; -using CallbacksContainerType = - rclcpp::node_interfaces::NodeParameters::CallbacksContainerType; +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_parameters_set_callbacks( +__call_on_set_parameters_callbacks( const std::vector & parameters, - CallbacksContainerType & callback_container, - const OnParametersSetCallbackType & callback) + OnSetCallbacksHandleContainer & callback_container) { rcl_interfaces::msg::SetParametersResult result; result.successful = true; @@ -283,19 +369,37 @@ __call_on_parameters_set_callbacks( it = callback_container.erase(it); } } - if (callback) { - result = callback(parameters); - } 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, - CallbacksContainerType & callback_container, - const OnParametersSetCallbackType & callback, + OnSetCallbacksHandleContainer & on_set_callback_container, + PostSetCallbacksHandleContainer & post_set_callback_container, bool allow_undeclared = false) { // Check if the value being set complies with the descriptor. @@ -304,9 +408,9 @@ __set_parameters_atomically_common( if (!result.successful) { return result; } - // Call the user callback to see if the new value(s) are allowed. + // Call the user callbacks to see if the new value(s) are allowed. result = - __call_on_parameters_set_callbacks(parameters, callback_container, callback); + __call_on_set_parameters_callbacks(parameters, on_set_callback_container); if (!result.successful) { return result; } @@ -318,6 +422,8 @@ __set_parameters_atomically_common( 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. @@ -332,8 +438,8 @@ __declare_parameter_common( const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor, std::map & parameters_out, const std::map & overrides, - CallbacksContainerType & callback_container, - const OnParametersSetCallbackType & callback, + OnSetCallbacksHandleContainer & on_set_callback_container, + PostSetCallbacksHandleContainer & post_set_callback_container, rcl_interfaces::msg::ParameterEvent * parameter_event_out, bool ignore_override = false) { @@ -363,14 +469,15 @@ __declare_parameter_common( return result; } - // Check with the user's callback to see if the initial value can be set. + // 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, - callback_container, - callback); + on_set_callback_container, + post_set_callback_container + ); if (!result.successful) { return result; @@ -397,8 +504,8 @@ declare_parameter_helper( bool ignore_override, std::map & parameters, const std::map & overrides, - CallbacksContainerType & callback_container, - const OnParametersSetCallbackType & callback, + 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) @@ -434,8 +541,8 @@ declare_parameter_helper( parameter_descriptor, parameters, overrides, - callback_container, - callback, + on_set_callback_container, + post_set_callback_container, ¶meter_event, ignore_override); @@ -464,14 +571,6 @@ declare_parameter_helper( return parameters.at(name).value; } -const rclcpp::ParameterValue & -NodeParameters::declare_parameter(const std::string & name) -{ - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.dynamic_typing = true; - return this->declare_parameter(name, rclcpp::ParameterValue{}, descriptor, false); -} - const rclcpp::ParameterValue & NodeParameters::declare_parameter( const std::string & name, @@ -490,8 +589,8 @@ NodeParameters::declare_parameter( ignore_override, parameters_, parameter_overrides_, - on_parameters_set_callback_container_, - on_parameters_set_callback_, + on_set_parameters_callback_container_, + post_set_parameters_callback_container_, events_publisher_.get(), combined_name_, *node_clock_); @@ -526,8 +625,8 @@ NodeParameters::declare_parameter( ignore_override, parameters_, parameter_overrides_, - on_parameters_set_callback_container_, - on_parameters_set_callback_, + on_set_parameters_callback_container_, + post_set_parameters_callback_container_, events_publisher_.get(), combined_name_, *node_clock_); @@ -552,7 +651,7 @@ NodeParameters::undeclare_parameter(const std::string & name) } if (!parameter_info->second.descriptor.dynamic_typing) { throw rclcpp::exceptions::InvalidParameterTypeException{ - name, "cannot undeclare an statically typed parameter"}; + name, "cannot undeclare a statically typed parameter"}; } parameters_.erase(parameter_info); @@ -601,12 +700,27 @@ NodeParameters::set_parameters_atomically(const std::vector & rcl_interfaces::msg::SetParametersResult result; + // 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) { + 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. @@ -641,12 +755,13 @@ NodeParameters::set_parameters_atomically(const std::vector & // 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 callback here, so that it may be called once, with + // 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_; - CallbacksContainerType empty_callback_container; + OnSetCallbacksHandleContainer empty_on_set_callback_container; + PostSetCallbacksHandleContainer empty_post_set_callback_container; // Implicit declare uses dynamic type descriptor. rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -661,8 +776,8 @@ NodeParameters::set_parameters_atomically(const std::vector & staged_parameter_changes, parameter_overrides_, // Only call callbacks once below - empty_callback_container, // callback_container is explicitly empty - nullptr, // callback is explicitly null. + 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) { @@ -675,12 +790,14 @@ NodeParameters::set_parameters_atomically(const std::vector & // 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; + 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, staged_parameter_change.first); + 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. @@ -690,7 +807,7 @@ NodeParameters::set_parameters_atomically(const std::vector & } } if (any_initial_values_used) { - parameters_copy = parameters; + 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); @@ -707,7 +824,7 @@ NodeParameters::set_parameters_atomically(const std::vector & 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 an statically typed parameter"; + result.reason = "cannot undeclare a statically typed parameter"; result.successful = false; return result; } @@ -722,11 +839,10 @@ NodeParameters::set_parameters_atomically(const std::vector & *parameters_to_be_set, // they are actually set on the official parameter storage parameters_, - // this will get called once, with all the parameters to be set - on_parameters_set_callback_container_, // These callbacks are called once. When a callback returns an unsuccessful result, - // the remaining aren't called. - on_parameters_set_callback_, + // 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. @@ -782,29 +898,18 @@ NodeParameters::set_parameters_atomically(const std::vector & parameter_event_msg.stamp = node_clock_->get_clock()->now(); events_publisher_->publish(parameter_event_msg); } - return result; } std::vector NodeParameters::get_parameters(const std::vector & names) const { - std::lock_guard lock(mutex_); std::vector results; results.reserve(names.size()); + std::lock_guard lock(mutex_); for (auto & name : names) { - auto found_parameter = parameters_.find(name); - if (found_parameter != parameters_.cend()) { - // found - results.emplace_back(name, found_parameter->second.value); - } else if (this->allow_undeclared_) { - // not found, but undeclared allowed - results.emplace_back(name, rclcpp::ParameterValue()); - } else { - // not found, and undeclared are not allowed - throw rclcpp::exceptions::ParameterNotDeclaredException(name); - } + results.emplace_back(this->get_parameter(name)); } return results; } @@ -815,18 +920,18 @@ NodeParameters::get_parameter(const std::string & name) const std::lock_guard lock(mutex_); auto param_iter = parameters_.find(name); - if ( - parameters_.end() != param_iter && - (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}; + 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{}; - } else if (parameters_.end() == param_iter) { - throw rclcpp::exceptions::ParameterNotDeclaredException(name); + return rclcpp::Parameter{name}; } else { - throw rclcpp::exceptions::ParameterUninitializedException(name); + throw rclcpp::exceptions::ParameterNotDeclaredException(name); } } @@ -933,41 +1038,74 @@ NodeParameters::list_parameters(const std::vector & prefixes, uint6 // 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::remove_pre_set_parameters_callback( + const PreSetParametersCallbackHandle * const handle) +{ + 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"); + } +} + void NodeParameters::remove_on_set_parameters_callback( const OnSetParametersCallbackHandle * const handle) @@ -976,20 +1114,53 @@ NodeParameters::remove_on_set_parameters_callback( ParameterMutationRecursionGuard guard(parameter_modification_enabled_); auto it = std::find_if( - on_parameters_set_callback_container_.begin(), - on_parameters_set_callback_container_.end(), + 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 != on_parameters_set_callback_container_.end()) { - on_parameters_set_callback_container_.erase(it); + if (it != post_set_parameters_callback_container_.end()) { + post_set_parameters_callback_container_.erase(it); } else { - throw std::runtime_error("Callback doesn't exist"); + 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(OnParametersSetCallbackType callback) +NodeParameters::add_on_set_parameters_callback(OnSetParametersCallbackType callback) { std::lock_guard lock(mutex_); ParameterMutationRecursionGuard guard(parameter_modification_enabled_); @@ -997,7 +1168,21 @@ NodeParameters::add_on_set_parameters_callback(OnParametersSetCallbackType callb auto handle = std::make_shared(); handle->callback = callback; // the last callback registered is executed first. - on_parameters_set_callback_container_.emplace_front(handle); + 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; } diff --git a/rclcpp/src/rclcpp/node_interfaces/node_services.cpp b/rclcpp/src/rclcpp/node_interfaces/node_services.cpp index dee39cb403..fdd4e83780 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_services.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_services.cpp @@ -32,23 +32,21 @@ NodeServices::add_service( { 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().str - ); - } + 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()); } } @@ -59,23 +57,21 @@ NodeServices::add_client( { 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().str - ); - } + 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()); } } diff --git a/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp b/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp index 6936e26759..29d3125e1f 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp @@ -34,19 +34,22 @@ NodeTimers::add_timer( { 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().str); + std::string("failed to notify wait set on timer creation: ") + ex.what()); } - TRACEPOINT( + + 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 c57fbcee5d..ce71036b93 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp @@ -14,9 +14,18 @@ #include "rclcpp/node_interfaces/node_topics.hpp" +#include #include +#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; @@ -49,24 +58,24 @@ NodeTopics::add_publisher( // Assign to a group. if (callback_group) { if (!node_base_->callback_group_in_node(callback_group)) { - throw std::runtime_error("Cannot create publisher, callback group not in node."); + throw rclcpp::exceptions::MissingGroupNodeException("publisher"); } } else { callback_group = node_base_->get_default_callback_group(); } - for (auto & publisher_event : publisher->get_event_handlers()) { + 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. - { - 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().str); - } + 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()); } } @@ -88,8 +97,7 @@ NodeTopics::add_subscription( // 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"); } } else { callback_group = node_base_->get_default_callback_group(); @@ -97,7 +105,8 @@ NodeTopics::add_subscription( callback_group->add_subscription(subscription); - for (auto & subscription_event : subscription->get_event_handlers()) { + for (auto & key_event_pair : subscription->get_event_handlers()) { + auto subscription_event = key_event_pair.second; callback_group->add_waitable(subscription_event); } @@ -108,13 +117,12 @@ NodeTopics::add_subscription( } // 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(); - auto ret = rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()); - if (ret != RCL_RET_OK) { - using rclcpp::exceptions::throw_from_rcl_error; - throw_from_rcl_error(ret, "failed to notify wait set on subscription creation"); - } + 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()); } } 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 index 0fd083788a..96eb8df9cf 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp @@ -32,23 +32,21 @@ NodeWaitables::add_waitable( { if (group) { if (!node_base_->callback_group_in_node(group)) { - // TODO(jacobperron): use custom exception - throw std::runtime_error("Cannot create waitable, group not in node."); + throw rclcpp::exceptions::MissingGroupNodeException("waitable"); } - group->add_waitable(waitable_ptr); } else { - node_base_->get_default_callback_group()->add_waitable(waitable_ptr); + 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. - { - 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 waitable creation: ") + - rmw_get_error_string().str - ); - } + 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()); } } diff --git a/rclcpp/src/rclcpp/node_options.cpp b/rclcpp/src/rclcpp/node_options.cpp index 06feffd1e6..ca58154eb4 100644 --- a/rclcpp/src/rclcpp/node_options.cpp +++ b/rclcpp/src/rclcpp/node_options.cpp @@ -77,8 +77,10 @@ NodeOptions::operator=(const NodeOptions & other) 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_; @@ -247,6 +249,19 @@ NodeOptions::start_parameter_services(bool 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 { @@ -260,6 +275,19 @@ NodeOptions::start_parameter_event_publisher(bool start_parameter_event_publishe 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 { diff --git a/rclcpp/src/rclcpp/parameter_client.cpp b/rclcpp/src/rclcpp/parameter_client.cpp index 38ced0e1a5..b60585ef00 100644 --- a/rclcpp/src/rclcpp/parameter_client.cpp +++ b/rclcpp/src/rclcpp/parameter_client.cpp @@ -35,7 +35,7 @@ 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) { @@ -46,7 +46,7 @@ AsyncParametersClient::AsyncParametersClient( } 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; @@ -290,28 +290,24 @@ std::shared_future> AsyncParametersClient::load_parameters( const std::string & yaml_filename) { - rclcpp::ParameterMap parameter_map = rclcpp::parameter_map_from_yaml_file(yaml_filename); - return this->load_parameters(parameter_map); + 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; - std::string remote_name = remote_node_name_.substr(remote_node_name_.substr(1).find("/") + 2); - for (const auto & params : parameter_map) { - std::string node_full_name = params.first; - std::string node_name = node_full_name.substr(node_full_name.find("/*/") + 3); - if (node_full_name == remote_node_name_ || - node_full_name == "/**" || - (node_name == remote_name)) - { - for (const auto & param : params.second) { - parameters.push_back(param); - } - } - } + 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"); diff --git a/rclcpp/src/rclcpp/parameter_event_handler.cpp b/rclcpp/src/rclcpp/parameter_event_handler.cpp index 232a32f887..b1b36b663e 100644 --- a/rclcpp/src/rclcpp/parameter_event_handler.cpp +++ b/rclcpp/src/rclcpp/parameter_event_handler.cpp @@ -29,10 +29,10 @@ ParameterEventCallbackHandle::SharedPtr ParameterEventHandler::add_parameter_event_callback( ParameterEventCallbackType callback) { - std::lock_guard lock(mutex_); + std::lock_guard lock(callbacks_->mutex_); auto handle = std::make_shared(); handle->callback = callback; - event_callbacks_.emplace_front(handle); + callbacks_->event_callbacks_.emplace_front(handle); return handle; } @@ -41,15 +41,15 @@ void ParameterEventHandler::remove_parameter_event_callback( ParameterEventCallbackHandle::SharedPtr callback_handle) { - std::lock_guard lock(mutex_); + std::lock_guard lock(callbacks_->mutex_); auto it = std::find_if( - event_callbacks_.begin(), - event_callbacks_.end(), + callbacks_->event_callbacks_.begin(), + callbacks_->event_callbacks_.end(), [callback_handle](const auto & weak_handle) { return callback_handle.get() == weak_handle.lock().get(); }); - if (it != event_callbacks_.end()) { - event_callbacks_.erase(it); + if (it != callbacks_->event_callbacks_.end()) { + callbacks_->event_callbacks_.erase(it); } else { throw std::runtime_error("Callback doesn't exist"); } @@ -61,7 +61,7 @@ ParameterEventHandler::add_parameter_callback( ParameterCallbackType callback, const std::string & node_name) { - std::lock_guard lock(mutex_); + std::lock_guard lock(callbacks_->mutex_); auto full_node_name = resolve_path(node_name); auto handle = std::make_shared(); @@ -69,7 +69,7 @@ ParameterEventHandler::add_parameter_callback( handle->parameter_name = parameter_name; handle->node_name = full_node_name; // the last callback registered is executed first. - parameter_callbacks_[{parameter_name, full_node_name}].emplace_front(handle); + callbacks_->parameter_callbacks_[{parameter_name, full_node_name}].emplace_front(handle); return handle; } @@ -78,9 +78,9 @@ void ParameterEventHandler::remove_parameter_callback( ParameterCallbackHandle::SharedPtr callback_handle) { - std::lock_guard lock(mutex_); + std::lock_guard lock(callbacks_->mutex_); auto handle = callback_handle.get(); - auto & container = parameter_callbacks_[{handle->parameter_name, handle->node_name}]; + auto & container = callbacks_->parameter_callbacks_[{handle->parameter_name, handle->node_name}]; auto it = std::find_if( container.begin(), container.end(), @@ -90,7 +90,7 @@ ParameterEventHandler::remove_parameter_callback( if (it != container.end()) { container.erase(it); if (container.empty()) { - parameter_callbacks_.erase({handle->parameter_name, handle->node_name}); + callbacks_->parameter_callbacks_.erase({handle->parameter_name, handle->node_name}); } } else { throw std::runtime_error("Callback doesn't exist"); @@ -101,8 +101,8 @@ 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) + const std::string & parameter_name, + const std::string & node_name) { if (event.node != node_name) { return false; @@ -128,14 +128,18 @@ ParameterEventHandler::get_parameter_from_event( rclcpp::Parameter ParameterEventHandler::get_parameter_from_event( const rcl_interfaces::msg::ParameterEvent & event, - const std::string parameter_name, - const std::string node_name) + const std::string & parameter_name, + const std::string & node_name) { rclcpp::Parameter p; if (!get_parameter_from_event(event, p, parameter_name, node_name)) { - throw std::runtime_error( - "Parameter '" + parameter_name + "' of node '" + node_name + - "' is not part of parameter event"); + 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; } @@ -158,7 +162,7 @@ ParameterEventHandler::get_parameters_from_event( } void -ParameterEventHandler::event_callback(const rcl_interfaces::msg::ParameterEvent & event) +ParameterEventHandler::Callbacks::event_callback(const rcl_interfaces::msg::ParameterEvent & event) { std::lock_guard lock(mutex_); diff --git a/rclcpp/src/rclcpp/parameter_map.cpp b/rclcpp/src/rclcpp/parameter_map.cpp index e5e3da019c..5ed67daae6 100644 --- a/rclcpp/src/rclcpp/parameter_map.cpp +++ b/rclcpp/src/rclcpp/parameter_map.cpp @@ -13,8 +13,11 @@ // 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; @@ -22,8 +25,15 @@ 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) +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"); @@ -49,6 +59,15 @@ rclcpp::parameter_map_from(const rcl_params_t * const c_params) 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]; @@ -65,6 +84,7 @@ rclcpp::parameter_map_from(const rcl_params_t * const c_params) params_node.emplace_back(c_param_name, parameter_value_from(c_param_value)); } } + return parameters; } @@ -128,13 +148,31 @@ rclcpp::parameter_value_from(const rcl_variant_t * const c_param_value) } ParameterMap -rclcpp::parameter_map_from_yaml_file(const std::string & yaml_filename) +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); + + 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 5c30917499..0923798339 100644 --- a/rclcpp/src/rclcpp/parameter_service.cpp +++ b/rclcpp/src/rclcpp/parameter_service.cpp @@ -29,7 +29,7 @@ ParameterService::ParameterService( const std::shared_ptr node_base, const std::shared_ptr node_services, rclcpp::node_interfaces::NodeParametersInterface * node_params, - const rmw_qos_profile_t & qos_profile) + const rclcpp::QoS & qos_profile) { const std::string node_name = node_base->get_name(); @@ -48,6 +48,8 @@ ParameterService::ParameterService( } } 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, nullptr); diff --git a/rclcpp/src/rclcpp/parameter_value.cpp b/rclcpp/src/rclcpp/parameter_value.cpp index e88b225479..ee46e77673 100644 --- a/rclcpp/src/rclcpp/parameter_value.cpp +++ b/rclcpp/src/rclcpp/parameter_value.cpp @@ -129,8 +129,7 @@ ParameterValue::ParameterValue(const rcl_interfaces::msg::ParameterValue & value case PARAMETER_NOT_SET: break; default: - // TODO(wjwwood): use custom exception - throw std::runtime_error("Unknown type: " + std::to_string(value.type)); + throw rclcpp::exceptions::UnknownTypeError(std::to_string(value.type)); } } diff --git a/rclcpp/src/rclcpp/publisher_base.cpp b/rclcpp/src/rclcpp/publisher_base.cpp index 17d72594d5..0dc9b01a7d 100644 --- a/rclcpp/src/rclcpp/publisher_base.cpp +++ b/rclcpp/src/rclcpp/publisher_base.cpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include "rcutils/logging_macros.h" @@ -36,7 +37,7 @@ #include "rclcpp/macros.hpp" #include "rclcpp/network_flow_endpoint.hpp" #include "rclcpp/node.hpp" -#include "rclcpp/qos_event.hpp" +#include "rclcpp/event_handler.hpp" using rclcpp::PublisherBase; @@ -44,9 +45,14 @@ 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 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) + 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) { @@ -95,6 +101,8 @@ PublisherBase::PublisherBase( rmw_reset_error(); throw std::runtime_error(msg); } + + bind_event_callbacks(event_callbacks_, use_default_callbacks); } PublisherBase::~PublisherBase() @@ -123,6 +131,85 @@ 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 { @@ -154,7 +241,8 @@ PublisherBase::get_publisher_handle() const return publisher_handle_; } -const std::vector> & +const +std::unordered_map> & PublisherBase::get_event_handlers() const { return event_handlers_; @@ -202,6 +290,13 @@ PublisherBase::get_intra_process_subscription_count() const 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 { @@ -224,7 +319,7 @@ PublisherBase::assert_liveliness() const bool PublisherBase::can_loan_messages() const { - return rcl_publisher_can_loan_messages(publisher_handle_.get()); + return !intra_process_is_enabled_ && rcl_publisher_can_loan_messages(publisher_handle_.get()); } bool @@ -270,6 +365,17 @@ PublisherBase::default_incompatible_qos_callback( 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(); @@ -305,3 +411,22 @@ std::vector PublisherBase::get_network_flow_endpoin 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 index 8b912de07f..2453149aa4 100644 --- a/rclcpp/src/rclcpp/qos.cpp +++ b/rclcpp/src/rclcpp/qos.cpp @@ -16,6 +16,8 @@ #include +#include "rclcpp/logging.hpp" + #include "rmw/error_handling.h" #include "rmw/types.h" #include "rmw/qos_profiles.h" @@ -43,9 +45,19 @@ std::string qos_policy_name_from_kind(rmw_qos_policy_kind_t policy_kind) } } -QoSInitialization::QoSInitialization(rmw_qos_history_policy_t history_policy_arg, size_t depth_arg) +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) @@ -53,8 +65,9 @@ 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_KEEP_LAST: 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); @@ -65,9 +78,10 @@ KeepAll::KeepAll() : QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_ALL, 0) {} -KeepLast::KeepLast(size_t depth) -: QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_LAST, depth) -{} +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, @@ -111,6 +125,14 @@ QoS::history(HistoryPolicy history) 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; @@ -150,6 +172,12 @@ 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) { @@ -176,6 +204,12 @@ 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) { @@ -380,4 +414,8 @@ SystemDefaultsQoS::SystemDefaultsQoS(const QoSInitialization & qos_initializatio : 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/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/service.cpp b/rclcpp/src/rclcpp/service.cpp index 827062bdd3..9c246e4b6b 100644 --- a/rclcpp/src/rclcpp/service.cpp +++ b/rclcpp/src/rclcpp/service.cpp @@ -22,17 +22,17 @@ #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) -: node_handle_(node_handle) +: node_handle_(node_handle), + node_logger_(rclcpp::get_node_logger(node_handle_.get())) {} -ServiceBase::~ServiceBase() -{} bool ServiceBase::take_type_erased_request(void * request_out, rmw_request_id_t & request_id_out) @@ -84,3 +84,55 @@ 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 index eda8585002..cf26d06df4 100644 --- a/rclcpp/src/rclcpp/signal_handler.cpp +++ b/rclcpp/src/rclcpp/signal_handler.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "./signal_handler.hpp" +#include "signal_handler.hpp" #include #include @@ -30,80 +30,131 @@ #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; -// initialize static storage in SignalHandler class -SignalHandler::signal_handler_type SignalHandler::old_signal_handler_; -std::atomic_bool SignalHandler::signal_received_ = ATOMIC_VAR_INIT(false); -std::atomic_bool SignalHandler::wait_for_signal_is_setup_ = ATOMIC_VAR_INIT(false); -#if defined(_WIN32) -HANDLE SignalHandler::signal_handler_sem_; -#elif defined(__APPLE__) -dispatch_semaphore_t SignalHandler::signal_handler_sem_; -#else // posix -sem_t SignalHandler::signal_handler_sem_; +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; +} -// The logger must be initialized before the local static variable signal_handler, -// from the method get_global_signal_handler(), so that it is destructed after -// it, because the destructor of SignalHandler uses this logger object. -static rclcpp::Logger g_logger = rclcpp::get_logger("rclcpp"); +// 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 g_logger; + return SignalHandler::get_global_signal_handler().logger_; } SignalHandler & SignalHandler::get_global_signal_handler() { - // This is initialized after the g_logger static global, ensuring - // SignalHandler::get_logger() may be called from the destructor of - // SignalHandler, according to this: - // - // Variables declared at block scope with the specifier static have static - // storage duration but are initialized the first time control passes - // through their declaration (unless their initialization is zero- or - // constant-initialization, which can be performed before the block is - // first entered). On all further calls, the declaration is skipped. - // - // -- https://en.cppreference.com/w/cpp/language/storage_duration#Static_local_variables - // - // Which is guaranteed to occur after static initialization for global (see: - // https://en.cppreference.com/w/cpp/language/initialization#Static_initialization), - // which is when g_logger will be initialized. - // And destruction will occur in the reverse order. - static SignalHandler signal_handler; + static SignalHandler & signal_handler = *new SignalHandler(); return signal_handler; } bool -SignalHandler::install() +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 signal_handler_argument; + SignalHandler::signal_handler_type handler_argument; #if defined(RCLCPP_HAS_SIGACTION) - memset(&signal_handler_argument, 0, sizeof(signal_handler_argument)); - sigemptyset(&signal_handler_argument.sa_mask); - signal_handler_argument.sa_sigaction = signal_handler; - signal_handler_argument.sa_flags = SA_SIGINFO; + 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 - signal_handler_argument = signal_handler; + 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); + } - old_signal_handler_ = SignalHandler::set_signal_handler(SIGINT, signal_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 (...) { @@ -125,10 +176,24 @@ SignalHandler::uninstall() 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. - set_signal_handler(SIGINT, old_signal_handler_); + 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(); - signal_handler_thread_.join(); + if (signal_handler_thread_.joinable()) { + signal_handler_thread_.join(); + } teardown_wait_for_signal(); } catch (...) { installed_.exchange(true); @@ -151,98 +216,57 @@ SignalHandler::~SignalHandler() } catch (const std::exception & exc) { RCLCPP_ERROR( get_logger(), - "caught %s exception when uninstalling the sigint handler in rclcpp::~SignalHandler: %s", + "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 the sigint handler in rclcpp::~SignalHandler"); + "caught unknown exception when uninstalling signal handlers in rclcpp::~SignalHandler"); } } SignalHandler::signal_handler_type -SignalHandler::set_signal_handler( - int signal_value, - const SignalHandler::signal_handler_type & signal_handler) +SignalHandler::get_old_signal_handler(int signum) { - bool signal_handler_install_failed; - SignalHandler::signal_handler_type old_signal_handler; + if (SIGINT == signum) { + return old_sigint_handler_; + } else if (SIGTERM == signum) { + return old_sigterm_handler_; + } #if defined(RCLCPP_HAS_SIGACTION) - ssize_t ret = sigaction(signal_value, &signal_handler, &old_signal_handler); - signal_handler_install_failed = (ret == -1); + SignalHandler::signal_handler_type ret; + memset(&ret, 0, sizeof(ret)); + sigemptyset(&ret.sa_mask); + ret.sa_handler = SIG_DFL; + return ret; #else - old_signal_handler = std::signal(signal_value, signal_handler); - signal_handler_install_failed = (old_signal_handler == SIG_ERR); + return SIG_DFL; #endif - if (signal_handler_install_failed) { - char error_string[1024]; - rcutils_strerror(error_string, sizeof(error_string)); - auto msg = - "Failed to set SIGINT signal handler (" + std::to_string(errno) + "): " + error_string; - throw std::runtime_error(msg); - } - - return old_signal_handler; } void SignalHandler::signal_handler_common() { - signal_received_.store(true); + auto & instance = SignalHandler::get_global_signal_handler(); + instance.signal_received_.store(true); RCLCPP_DEBUG( get_logger(), - "signal_handler(): SIGINT received, notifying deferred signal handler"); - notify_signal_handler(); + "signal_handler(): notifying deferred signal handler"); + instance.notify_signal_handler(); } -#if defined(RCLCPP_HAS_SIGACTION) -void -SignalHandler::signal_handler(int signal_value, siginfo_t * siginfo, void * context) -{ - RCLCPP_INFO(get_logger(), "signal_handler(signal_value=%d)", signal_value); - - if (old_signal_handler_.sa_flags & SA_SIGINFO) { - if (old_signal_handler_.sa_sigaction != NULL) { - old_signal_handler_.sa_sigaction(signal_value, 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(signal_value); - } - } - - signal_handler_common(); -} -#else -void -SignalHandler::signal_handler(int signal_value) -{ - RCLCPP_INFO(get_logger(), "signal_handler(signal_value=%d)", signal_value); - - if (old_signal_handler_) { - old_signal_handler_(signal_value); - } - - signal_handler_common(); -} -#endif - void SignalHandler::deferred_signal_handler() { while (true) { if (signal_received_.exchange(false)) { - RCLCPP_DEBUG(get_logger(), "deferred_signal_handler(): SIGINT received, shutting down"); + RCLCPP_DEBUG(get_logger(), "deferred_signal_handler(): shutting down"); for (auto context_ptr : rclcpp::get_contexts()) { - if (context_ptr->get_init_options().shutdown_on_sigint) { + 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_sigint == true", + "shutting down rclcpp::Context @ %p, because it had shutdown_on_signal == true", static_cast(context_ptr.get())); context_ptr->shutdown("signal handler"); } @@ -252,9 +276,11 @@ SignalHandler::deferred_signal_handler() RCLCPP_DEBUG(get_logger(), "deferred_signal_handler(): signal handling uninstalled"); break; } - RCLCPP_DEBUG(get_logger(), "deferred_signal_handler(): waiting for SIGINT or uninstall"); + 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 or uninstall"); + RCLCPP_DEBUG( + get_logger(), "deferred_signal_handler(): woken up due to SIGINT/SIGTERM or uninstall"); } } @@ -356,3 +382,9 @@ SignalHandler::notify_signal_handler() noexcept } #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 index 81cb6e180c..db608b0d10 100644 --- a/rclcpp/src/rclcpp/signal_handler.hpp +++ b/rclcpp/src/rclcpp/signal_handler.hpp @@ -21,6 +21,7 @@ #include #include "rclcpp/logging.hpp" +#include "rclcpp/utilities.hpp" // includes for semaphore notification code #if defined(_WIN32) @@ -39,14 +40,14 @@ namespace rclcpp { -/// Responsible for manaaging the SIGINT signal handling. +/// Responsible for managing the SIGINT/SIGTERM signal handling. /** * This class is responsible for: * - * - installing the signal handler for SIGINT - * - uninstalling the signal handler for SIGINT - * - creating a thread to execute "on sigint" work outside of the signal handler - * - safely notifying the dedicated signal handling thread when receiving SIGINT + * - 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 @@ -64,15 +65,18 @@ class SignalHandler final rclcpp::Logger & get_logger(); - /// Install the signal handler for SIGINT and start the dedicated signal handling thread. + /// Install the signal handler for SIGINT/SIGTERM and start the dedicated signal handling thread. /** - * Also stores the current signal handler to be called on SIGINT and to + * 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(); + install(SignalHandlerOptions signal_handler_options = SignalHandlerOptions::All); - /// Uninstall the signal handler for SIGINT and join the dedicated singal handling thread. + /// Uninstall the signal handler for SIGINT/SIGTERM and join the dedicated signal handling + /// thread. /** * Also restores the previous signal handler. */ @@ -83,26 +87,34 @@ class SignalHandler final bool is_installed(); -private: - SignalHandler() = default; - - ~SignalHandler(); + /// 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 - // POSIX signal handler structure storage for the existing signal handler. - static SignalHandler::signal_handler_type old_signal_handler_; - /// Set the signal handler function. - static - SignalHandler::signal_handler_type - set_signal_handler(int signal_value, const SignalHandler::signal_handler_type & signal_handler); + + 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. - static void signal_handler_common(); @@ -127,7 +139,6 @@ class SignalHandler final * This must be called before wait_for_signal() or notify_signal_handler(). * This is not thread-safe. */ - static void setup_wait_for_signal(); @@ -137,7 +148,6 @@ class SignalHandler final * * This is not thread-safe. */ - static void teardown_wait_for_signal() noexcept; @@ -147,7 +157,6 @@ class SignalHandler final * * This is not thread-safe. */ - static void wait_for_signal(); @@ -158,29 +167,45 @@ class SignalHandler final * * This is thread-safe. */ - static 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. - static std::atomic_bool signal_received_; - // A thread to which singal handling tasks are deferred. + 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}; + std::atomic_bool installed_ = false; // Whether or not the semaphore for wait_for_signal is setup. - static std::atomic_bool wait_for_signal_is_setup_; + std::atomic_bool wait_for_signal_is_setup_; // Storage for the wait_for_signal semaphore. #if defined(_WIN32) - static HANDLE signal_handler_sem_; + HANDLE signal_handler_sem_; #elif defined(__APPLE__) - static dispatch_semaphore_t signal_handler_sem_; + dispatch_semaphore_t signal_handler_sem_; #else // posix - static sem_t signal_handler_sem_; + sem_t signal_handler_sem_; #endif }; diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index ba76e5f9bc..7dca16a1a9 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -17,18 +17,24 @@ #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/qos_event.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( @@ -36,13 +42,17 @@ SubscriptionBase::SubscriptionBase( const rosidl_message_type_support_t & type_support_handle, const std::string & topic_name, const rcl_subscription_options_t & subscription_options, - bool is_serialized) + 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), - is_serialized_(is_serialized) + delivered_message_kind_(delivered_message_kind) { auto custom_deletor = [node_handle = this->node_handle_](rcl_subscription_t * rcl_subs) { @@ -76,9 +86,10 @@ SubscriptionBase::SubscriptionBase( 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() @@ -97,6 +108,98 @@ SubscriptionBase::~SubscriptionBase() 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 { @@ -115,7 +218,8 @@ SubscriptionBase::get_subscription_handle() const return subscription_handle_; } -const std::vector> & +const +std::unordered_map> & SubscriptionBase::get_event_handlers() const { return event_handlers_; @@ -143,6 +247,7 @@ SubscriptionBase::take_type_erased(void * message_out, rclcpp::MessageInfo & mes &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) { @@ -168,6 +273,9 @@ SubscriptionBase::take_serialized( &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) { @@ -185,7 +293,13 @@ SubscriptionBase::get_message_type_support_handle() const bool SubscriptionBase::is_serialized() const { - return is_serialized_; + return delivered_message_kind_ == rclcpp::DeliveredMessageKind::SERIALIZED_MESSAGE; +} + +rclcpp::DeliveredMessageKind +SubscriptionBase::get_delivered_message_kind() const +{ + return delivered_message_kind_; } size_t @@ -216,7 +330,20 @@ SubscriptionBase::setup_intra_process( bool SubscriptionBase::can_loan_messages() const { - return rcl_subscription_can_loan_messages(subscription_handle_.get()); + 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 @@ -252,6 +379,17 @@ SubscriptionBase::default_incompatible_qos_callback( 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 { @@ -281,7 +419,8 @@ SubscriptionBase::exchange_in_use_by_wait_set_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 & qos_event : event_handlers_) { + 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); } @@ -289,7 +428,8 @@ SubscriptionBase::exchange_in_use_by_wait_set_state( throw std::runtime_error("given pointer_to_subscription_part does not match any part"); } -std::vector SubscriptionBase::get_network_flow_endpoints() const +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 = @@ -325,3 +465,117 @@ std::vector SubscriptionBase::get_network_flow_endp 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 index 2738161675..f9d19da8c5 100644 --- a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp +++ b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp @@ -13,16 +13,14 @@ // 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; -bool -SubscriptionIntraProcessBase::add_to_wait_set(rcl_wait_set_t * wait_set) +void +SubscriptionIntraProcessBase::add_to_wait_set(rcl_wait_set_t & wait_set) { - std::lock_guard lock(reentrant_mutex_); - - rcl_ret_t ret = rcl_wait_set_add_guard_condition(wait_set, &gc_, NULL); - return RCL_RET_OK == ret; + detail::add_guard_condition_to_rcl_wait_set(wait_set, gc_); } const char * @@ -36,3 +34,9 @@ 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 556a5e69ad..e2780c04d8 100644 --- a/rclcpp/src/rclcpp/time.cpp +++ b/rclcpp/src/rclcpp/time.cpp @@ -60,11 +60,17 @@ 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) = default; +Time::Time(Time && rhs) noexcept = default; + Time::Time( const builtin_interfaces::msg::Time & time_msg, rcl_clock_type_t clock_type) @@ -84,23 +90,11 @@ 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; - constexpr rcl_time_point_value_t kRemainder = RCL_S_TO_NS(1); - const auto result = std::div(rcl_time_.nanoseconds, kRemainder); - if (result.rem >= 0) { - msg_time.sec = static_cast(result.quot); - msg_time.nanosec = static_cast(result.rem); - } else { - msg_time.sec = static_cast(result.quot - 1); - msg_time.nanosec = static_cast(kRemainder + result.rem); - } - return msg_time; + return convert_rcl_time_to_sec_nanos(rcl_time_.nanoseconds); } Time & @@ -113,6 +107,9 @@ Time::operator=(const builtin_interfaces::msg::Time & time_msg) return *this; } +Time & +Time::operator=(Time && rhs) noexcept = default; + bool Time::operator==(const rclcpp::Time & rhs) const { @@ -256,6 +253,9 @@ Time::operator+=(const rclcpp::Duration & rhs) } 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; } @@ -271,15 +271,33 @@ Time::operator-=(const rclcpp::Duration & rhs) } 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() +Time::max(rcl_clock_type_t clock_type) { - return Time(std::numeric_limits::max(), 999999999); + 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 140b5531d2..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 @@ -33,29 +34,507 @@ namespace rclcpp { +class ClocksState final +{ +public: + ClocksState() + : logger_(rclcpp::get_logger("rclcpp")), + last_time_msg_(std::make_shared()) + { + } + + // 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; + } + + // Local storage + ros_time_active_ = true; + + // Update all attached clocks to zero or last recorded time + set_all_clocks(last_time_msg_, true); + } + + // 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; + } + + // Local storage + ros_time_active_ = false; + + // Update all attached clocks + auto msg = std::make_shared(); + set_all_clocks(msg, false); + } + + // Check if ROS time is active + bool is_ros_time_active() const + { + return ros_time_active_; + } + + // 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); + } + + // 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"); + } + } + + // 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 +{ +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 { + use_sim_time_param = node_parameters_->get_parameter(use_sim_time_name).get_parameter_value(); + } + 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 { + 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); + }); + } + + // 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(); + } + + void attachClock(std::shared_ptr clock) + { + clocks_state_.attachClock(std::move(clock)); + } + + void detachClock(std::shared_ptr clock) + { + clocks_state_.detachClock(std::move(clock)); + } + +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) + { + if (!clocks_state_.is_ros_time_active() && SET_TRUE == this->parameter_state_) { + clocks_state_.enable_ros_time(); + } + // 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); + + if (SET_TRUE == this->parameter_state_) { + clocks_state_.set_all_clocks(time_msg, true); + } + } + + // 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 + ); + } + + // 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_); + } + 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; + } + + // 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) -: use_clock_thread_(use_clock_thread), - logger_(rclcpp::get_logger("rclcpp")), - qos_(qos) +: TimeSource(qos, use_clock_thread) { - this->attachNode(node); + attachNode(node); } TimeSource::TimeSource( const rclcpp::QoS & qos, bool use_clock_thread) -: use_clock_thread_(use_clock_thread), - logger_(rclcpp::get_logger("rclcpp")), - qos_(qos) +: constructed_use_clock_thread_(use_clock_thread), + constructed_qos_(qos) { + node_state_ = std::make_shared(qos, use_clock_thread); } void TimeSource::attachNode(rclcpp::Node::SharedPtr node) { - use_clock_thread_ = node->get_node_options().use_clock_thread(); + node_state_->set_use_clock_thread(node->get_node_options().use_clock_thread()); attachNode( node->get_node_base_interface(), node->get_node_topics_interface(), @@ -75,298 +554,51 @@ void TimeSource::attachNode( rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_interface, rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_interface) { - 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), - rcl_interfaces::msg::ParameterDescriptor()); - } else { - use_sim_time_param = node_parameters_->get_parameter(use_sim_time_name).get_parameter_value(); - } - if (use_sim_time_param.get_type() == rclcpp::PARAMETER_BOOL) { - if (use_sim_time_param.get()) { - parameter_state_ = SET_TRUE; - enable_ros_time(); - create_clock_sub(); - } - } else { - 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()); - } - sim_time_cb_handler_ = node_parameters_->add_on_set_parameters_callback( - [use_sim_time_name](const std::vector & parameters) { - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; - for (const auto & parameter : parameters) { - if ( - parameter.get_name() == use_sim_time_name && - parameter.get_type() != rclcpp::PARAMETER_BOOL) - { - result.successful = false; - result.reason = "'" + use_sim_time_name + "' must be a bool"; - break; - } - } - return result; - }); - - // TODO(tfoote) use parameters interface not subscribe to events via topic ticketed #609 - parameter_subscription_ = rclcpp::AsyncParametersClient::on_parameter_event( - node_topics_, - std::bind(&TimeSource::on_parameter_event, this, std::placeholders::_1)); + 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)); } void TimeSource::detachNode() { - this->ros_time_active_ = false; - destroy_clock_sub(); - parameter_subscription_.reset(); - node_base_.reset(); - node_topics_.reset(); - node_graph_.reset(); - node_services_.reset(); - node_logging_.reset(); - node_clock_.reset(); - if (sim_time_cb_handler_ && node_parameters_) { - node_parameters_->remove_on_set_parameters_callback(sim_time_cb_handler_.get()); - } - sim_time_cb_handler_.reset(); - node_parameters_.reset(); - disable_ros_time(); + node_state_.reset(); + node_state_ = std::make_shared( + constructed_qos_, + constructed_use_clock_thread_); } 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"); - } - - std::lock_guard guard(clock_list_lock_); - associated_clocks_.push_back(clock); - // Set the clock to zero unless there's a recently received message - auto time_msg = std::make_shared(); - if (last_msg_set_) { - time_msg = std::make_shared(last_msg_set_->clock); - } - set_clock(time_msg, ros_time_active_, clock); + node_state_->attachClock(std::move(clock)); } 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 { - RCLCPP_ERROR(logger_, "failed to remove clock"); - } -} - -TimeSource::~TimeSource() -{ - if ( - node_base_ || node_topics_ || node_graph_ || node_services_ || - node_logging_ || node_clock_ || node_parameters_) - { - this->detachNode(); - } -} - -void TimeSource::set_clock( - const builtin_interfaces::msg::Time::SharedPtr msg, bool set_ros_time_enabled, - std::shared_ptr clock) -{ - std::lock_guard clock_guard(clock->get_clock_mutex()); - - // 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"); - } -} - -void TimeSource::clock_cb(std::shared_ptr msg) -{ - if (!this->ros_time_active_ && SET_TRUE == this->parameter_state_) { - enable_ros_time(); - } - // Cache the last message in case a new clock is attached. - last_msg_set_ = msg; - auto time_msg = std::make_shared(msg->clock); - - if (SET_TRUE == this->parameter_state_) { - std::lock_guard guard(clock_list_lock_); - for (auto it = associated_clocks_.begin(); it != associated_clocks_.end(); ++it) { - set_clock(time_msg, true, *it); - } - } -} - -void TimeSource::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", - rclcpp::QoS(KeepLast(1)).best_effort(), - std::bind(&TimeSource::clock_cb, this, std::placeholders::_1), - options - ); + node_state_->detachClock(std::move(clock)); } -void TimeSource::destroy_clock_sub() +bool TimeSource::get_use_clock_thread() { - 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_); - } - clock_subscription_.reset(); + return node_state_->get_use_clock_thread(); } -void TimeSource::on_parameter_event( - std::shared_ptr event) +void TimeSource::set_use_clock_thread(bool use_clock_thread) { - // 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; - enable_ros_time(); - create_clock_sub(); - } else { - parameter_state_ = SET_FALSE; - disable_ros_time(); - destroy_clock_sub(); - } - } - // 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; - } + node_state_->set_use_clock_thread(use_clock_thread); } -void TimeSource::enable_ros_time() +bool TimeSource::clock_thread_is_joinable() { - if (ros_time_active_) { - // already enabled no-op - return; - } - - // Local storage - ros_time_active_ = true; - - // Update all attached clocks to zero or last recorded time - std::lock_guard guard(clock_list_lock_); - auto time_msg = std::make_shared(); - if (last_msg_set_) { - time_msg = std::make_shared(last_msg_set_->clock); - } - for (auto it = associated_clocks_.begin(); it != associated_clocks_.end(); ++it) { - set_clock(time_msg, true, *it); - } + return node_state_->clock_thread_is_joinable(); } -void TimeSource::disable_ros_time() +TimeSource::~TimeSource() { - if (!ros_time_active_) { - // already disabled no-op - return; - } - - // Local storage - ros_time_active_ = false; - - // 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); - } } } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/timer.cpp b/rclcpp/src/rclcpp/timer.cpp index 70414f38c6..0dceb6b8d7 100644 --- a/rclcpp/src/rclcpp/timer.cpp +++ b/rclcpp/src/rclcpp/timer.cpp @@ -19,9 +19,12 @@ #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; @@ -29,7 +32,8 @@ using rclcpp::TimerBase; TimerBase::TimerBase( rclcpp::Clock::SharedPtr clock, std::chrono::nanoseconds period, - rclcpp::Context::SharedPtr context) + rclcpp::Context::SharedPtr context, + bool autostart) : clock_(clock), timer_handle_(nullptr) { if (nullptr == context) { @@ -61,9 +65,9 @@ TimerBase::TimerBase( rcl_clock_t * clock_handle = clock_->get_clock_handle(); { std::lock_guard clock_guard(clock_->get_clock_mutex()); - rcl_ret_t ret = rcl_timer_init( - timer_handle_.get(), clock_handle, rcl_context.get(), period.count(), nullptr, - rcl_get_default_allocator()); + 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"); } @@ -71,7 +75,9 @@ TimerBase::TimerBase( } TimerBase::~TimerBase() -{} +{ + clear_on_reset_callback(); +} void TimerBase::cancel() @@ -96,7 +102,11 @@ TimerBase::is_canceled() void TimerBase::reset() { - rcl_ret_t ret = rcl_timer_reset(timer_handle_.get()); + 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"); } @@ -119,7 +129,9 @@ TimerBase::time_until_trigger() int64_t time_until_next_call = 0; rcl_ret_t ret = rcl_timer_get_time_until_next_call( timer_handle_.get(), &time_until_next_call); - if (ret != RCL_RET_OK) { + 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); @@ -136,3 +148,73 @@ 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/typesupport_helpers.cpp b/rclcpp/src/rclcpp/typesupport_helpers.cpp index 7286c35baa..04dd8c4c9f 100644 --- a/rclcpp/src/rclcpp/typesupport_helpers.cpp +++ b/rclcpp/src/rclcpp/typesupport_helpers.cpp @@ -91,20 +91,12 @@ extract_type_identifier(const std::string & full_type) return std::make_tuple(package_name, middle_module, type_name); } -} // 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 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; @@ -112,19 +104,23 @@ get_typesupport_handle( std::string type_name; std::tie(package_name, middle_module, type_name) = extract_type_identifier(type); - auto mk_error = [&package_name, &type_name](auto reason) { + 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 message type " << package_name << + "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 + "__get_message_type_support_handle__" + - package_name + "__" + (middle_module.empty() ? "msg" : middle_module) + "__" + type_name; - - const rosidl_message_type_support_t * (* get_ts)() = nullptr; + 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(); @@ -133,4 +129,52 @@ get_typesupport_handle( } } +} // 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 c3e3915d79..f300060010 100644 --- a/rclcpp/src/rclcpp/utilities.cpp +++ b/rclcpp/src/rclcpp/utilities.cpp @@ -31,18 +31,22 @@ namespace rclcpp { void -init(int argc, char const * const argv[], const InitOptions & init_options) +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(); + install_signal_handlers(signal_handler_options); } bool -install_signal_handlers() +install_signal_handlers(SignalHandlerOptions signal_handler_options) { - return SignalHandler::get_global_signal_handler().install(); + return SignalHandler::get_global_signal_handler().install(signal_handler_options); } bool @@ -51,6 +55,13 @@ signal_handlers_installed() return SignalHandler::get_global_signal_handler().is_installed(); } +SignalHandlerOptions +get_current_signal_handler_options() +{ + return SignalHandler::get_global_signal_handler().get_current_signal_handler_options(); +} + + bool uninstall_signal_handlers() { @@ -60,7 +71,7 @@ uninstall_signal_handlers() static std::vector _remove_ros_arguments( - char const * const argv[], + char const * const * argv, const rcl_arguments_t * args, rcl_allocator_t alloc) { @@ -101,7 +112,7 @@ _remove_ros_arguments( std::vector init_and_remove_ros_arguments( int argc, - char const * const argv[], + char const * const * argv, const InitOptions & init_options) { init(argc, argv, init_options); @@ -112,7 +123,7 @@ init_and_remove_ros_arguments( } std::vector -remove_ros_arguments(int argc, char const * const argv[]) +remove_ros_arguments(int argc, char const * const * argv) { rcl_allocator_t alloc = rcl_get_default_allocator(); rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); @@ -203,4 +214,17 @@ 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 cstrings; +} + } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/waitable.cpp b/rclcpp/src/rclcpp/waitable.cpp index b76c7215e0..ef3a50a8d9 100644 --- a/rclcpp/src/rclcpp/waitable.cpp +++ b/rclcpp/src/rclcpp/waitable.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include + #include "rclcpp/waitable.hpp" using rclcpp::Waitable; diff --git a/rclcpp/test/benchmark/CMakeLists.txt b/rclcpp/test/benchmark/CMakeLists.txt index a88c43e3b2..6b93711df2 100644 --- a/rclcpp/test/benchmark/CMakeLists.txt +++ b/rclcpp/test/benchmark/CMakeLists.txt @@ -7,14 +7,12 @@ find_package(performance_test_fixture REQUIRED) add_performance_test(benchmark_client benchmark_client.cpp) if(TARGET benchmark_client) - target_link_libraries(benchmark_client ${PROJECT_NAME}) - ament_target_dependencies(benchmark_client test_msgs rcl_interfaces) + 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}) - ament_target_dependencies(benchmark_executor test_msgs) + target_link_libraries(benchmark_executor ${PROJECT_NAME} ${test_msgs_TARGETS}) endif() add_performance_test(benchmark_init_shutdown benchmark_init_shutdown.cpp) @@ -39,6 +37,5 @@ endif() add_performance_test(benchmark_service benchmark_service.cpp) if(TARGET benchmark_service) - target_link_libraries(benchmark_service ${PROJECT_NAME}) - ament_target_dependencies(benchmark_service test_msgs rcl_interfaces) + 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 index 05608623a0..26ee58b633 100644 --- a/rclcpp/test/benchmark/benchmark_client.cpp +++ b/rclcpp/test/benchmark/benchmark_client.cpp @@ -62,6 +62,7 @@ BENCHMARK_F(ClientPerformanceTest, construct_client_no_service)(benchmark::State reset_heap_counters(); for (auto _ : state) { + (void)_; auto client = node->create_client("not_an_existing_service"); benchmark::DoNotOptimize(client); benchmark::ClobberMemory(); @@ -79,6 +80,7 @@ BENCHMARK_F(ClientPerformanceTest, construct_client_empty_srv)(benchmark::State reset_heap_counters(); for (auto _ : state) { + (void)_; auto client = node->create_client(empty_service_name); benchmark::DoNotOptimize(client); benchmark::ClobberMemory(); @@ -96,6 +98,7 @@ BENCHMARK_F(ClientPerformanceTest, destroy_client_empty_srv)(benchmark::State & reset_heap_counters(); for (auto _ : state) { + (void)_; state.PauseTiming(); auto client = node->create_client(empty_service_name); state.ResumeTiming(); @@ -109,6 +112,7 @@ BENCHMARK_F(ClientPerformanceTest, destroy_client_empty_srv)(benchmark::State & 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 @@ -132,6 +136,7 @@ BENCHMARK_F(ClientPerformanceTest, async_send_request_only)(benchmark::State & s reset_heap_counters(); for (auto _ : state) { + (void)_; auto future = client->async_send_request(shared_request); benchmark::DoNotOptimize(future); benchmark::ClobberMemory(); @@ -144,6 +149,7 @@ BENCHMARK_F(ClientPerformanceTest, async_send_request_and_response)(benchmark::S 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)); diff --git a/rclcpp/test/benchmark/benchmark_executor.cpp b/rclcpp/test/benchmark/benchmark_executor.cpp index 85815b3b50..ea7afa8696 100644 --- a/rclcpp/test/benchmark/benchmark_executor.cpp +++ b/rclcpp/test/benchmark/benchmark_executor.cpp @@ -78,6 +78,7 @@ BENCHMARK_F(PerformanceTestExecutor, single_thread_executor_spin_some)(benchmark reset_heap_counters(); for (auto _ : st) { + (void)_; st.PauseTiming(); for (unsigned int i = 0u; i < kNumberOfNodes; i++) { publishers[i]->publish(empty_msgs); @@ -104,6 +105,7 @@ BENCHMARK_F(PerformanceTestExecutor, multi_thread_executor_spin_some)(benchmark: reset_heap_counters(); for (auto _ : st) { + (void)_; st.PauseTiming(); for (unsigned int i = 0u; i < kNumberOfNodes; i++) { publishers[i]->publish(empty_msgs); @@ -142,6 +144,7 @@ BENCHMARK_F(PerformanceTestExecutorSimple, single_thread_executor_add_node)(benc { rclcpp::executors::SingleThreadedExecutor executor; for (auto _ : st) { + (void)_; executor.add_node(node); st.PauseTiming(); executor.remove_node(node); @@ -154,6 +157,7 @@ BENCHMARK_F( { rclcpp::executors::SingleThreadedExecutor executor; for (auto _ : st) { + (void)_; st.PauseTiming(); executor.add_node(node); st.ResumeTiming(); @@ -165,6 +169,7 @@ BENCHMARK_F(PerformanceTestExecutorSimple, multi_thread_executor_add_node)(bench { rclcpp::executors::MultiThreadedExecutor executor; for (auto _ : st) { + (void)_; executor.add_node(node); st.PauseTiming(); executor.remove_node(node); @@ -176,6 +181,7 @@ BENCHMARK_F(PerformanceTestExecutorSimple, multi_thread_executor_remove_node)(be { rclcpp::executors::MultiThreadedExecutor executor; for (auto _ : st) { + (void)_; st.PauseTiming(); executor.add_node(node); st.ResumeTiming(); @@ -183,68 +189,6 @@ BENCHMARK_F(PerformanceTestExecutorSimple, multi_thread_executor_remove_node)(be } } -BENCHMARK_F( - PerformanceTestExecutorSimple, - static_single_thread_executor_add_node)(benchmark::State & st) -{ - rclcpp::executors::StaticSingleThreadedExecutor executor; - for (auto _ : st) { - executor.add_node(node); - st.PauseTiming(); - executor.remove_node(node); - st.ResumeTiming(); - } -} - -BENCHMARK_F( - PerformanceTestExecutorSimple, - static_single_thread_executor_remove_node)(benchmark::State & st) -{ - rclcpp::executors::StaticSingleThreadedExecutor executor; - for (auto _ : st) { - st.PauseTiming(); - executor.add_node(node); - st.ResumeTiming(); - executor.remove_node(node); - } -} - -BENCHMARK_F( - PerformanceTestExecutorSimple, - static_single_thread_executor_spin_until_future_complete)(benchmark::State & st) -{ - rclcpp::executors::StaticSingleThreadedExecutor 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 = executor.spin_until_future_complete(shared_future, 100ms); - if (ret != rclcpp::FutureReturnCode::SUCCESS) { - st.SkipWithError(rcutils_get_error_string().str); - } - - reset_heap_counters(); - - for (auto _ : st) { - // static_single_thread_executor has a special design. We need to add/remove the node each - // time you call spin - st.PauseTiming(); - executor.add_node(node); - st.ResumeTiming(); - - ret = executor.spin_until_future_complete(shared_future, 100ms); - if (ret != rclcpp::FutureReturnCode::SUCCESS) { - st.SkipWithError(rcutils_get_error_string().str); - break; - } - st.PauseTiming(); - executor.remove_node(node); - st.ResumeTiming(); - } -} - BENCHMARK_F( PerformanceTestExecutorSimple, single_thread_executor_spin_node_until_future_complete)(benchmark::State & st) @@ -265,6 +209,7 @@ BENCHMARK_F( 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) { @@ -294,6 +239,7 @@ BENCHMARK_F( 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) { @@ -303,29 +249,6 @@ BENCHMARK_F( } } -BENCHMARK_F( - PerformanceTestExecutorSimple, - static_single_thread_executor_spin_node_until_future_complete)(benchmark::State & st) -{ - rclcpp::executors::StaticSingleThreadedExecutor 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(); - - reset_heap_counters(); - - for (auto _ : st) { - 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); - break; - } - } -} - BENCHMARK_F(PerformanceTestExecutorSimple, spin_until_future_complete)(benchmark::State & st) { // test success of an immediately finishing future @@ -342,6 +265,7 @@ BENCHMARK_F(PerformanceTestExecutorSimple, spin_until_future_complete)(benchmark 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); @@ -349,42 +273,3 @@ BENCHMARK_F(PerformanceTestExecutorSimple, spin_until_future_complete)(benchmark } } } - -BENCHMARK_F( - PerformanceTestExecutorSimple, - static_executor_entities_collector_execute)(benchmark::State & st) -{ - rclcpp::executors::StaticExecutorEntitiesCollector::SharedPtr entities_collector_ = - std::make_shared(); - 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(); - rcl_ret_t ret = rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator); - if (ret != RCL_RET_OK) { - st.SkipWithError(rcutils_get_error_string().str); - } - RCPPUTILS_SCOPE_EXIT( - { - rcl_ret_t ret = rcl_wait_set_fini(&wait_set); - if (ret != RCL_RET_OK) { - st.SkipWithError(rcutils_get_error_string().str); - } - }); - - auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); - rclcpp::GuardCondition guard_condition(shared_context); - rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); - - entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); - RCPPUTILS_SCOPE_EXIT(entities_collector_->fini()); - - reset_heap_counters(); - - for (auto _ : st) { - std::shared_ptr data = entities_collector_->take_data(); - entities_collector_->execute(data); - } -} diff --git a/rclcpp/test/benchmark/benchmark_init_shutdown.cpp b/rclcpp/test/benchmark/benchmark_init_shutdown.cpp index 6c3ae8b72e..c2a60f693a 100644 --- a/rclcpp/test/benchmark/benchmark_init_shutdown.cpp +++ b/rclcpp/test/benchmark/benchmark_init_shutdown.cpp @@ -26,6 +26,7 @@ BENCHMARK_F(PerformanceTest, rclcpp_init)(benchmark::State & state) reset_heap_counters(); for (auto _ : state) { + (void)_; rclcpp::init(0, nullptr); state.PauseTiming(); @@ -43,6 +44,7 @@ BENCHMARK_F(PerformanceTest, rclcpp_shutdown)(benchmark::State & state) reset_heap_counters(); for (auto _ : state) { + (void)_; state.PauseTiming(); rclcpp::init(0, nullptr); state.ResumeTiming(); diff --git a/rclcpp/test/benchmark/benchmark_node.cpp b/rclcpp/test/benchmark/benchmark_node.cpp index 2a1d80674c..a5d7f4c80f 100644 --- a/rclcpp/test/benchmark/benchmark_node.cpp +++ b/rclcpp/test/benchmark/benchmark_node.cpp @@ -44,6 +44,7 @@ BENCHMARK_F(NodePerformanceTest, create_node)(benchmark::State & state) 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__ @@ -66,6 +67,7 @@ BENCHMARK_F(NodePerformanceTest, destroy_node)(benchmark::State & state) reset_heap_counters(); for (auto _ : state) { + (void)_; // Using pointer to separate construction and destruction in timing state.PauseTiming(); auto node = std::make_shared("node"); diff --git a/rclcpp/test/benchmark/benchmark_node_parameters_interface.cpp b/rclcpp/test/benchmark/benchmark_node_parameters_interface.cpp index be447b7190..04c7071d64 100644 --- a/rclcpp/test/benchmark/benchmark_node_parameters_interface.cpp +++ b/rclcpp/test/benchmark/benchmark_node_parameters_interface.cpp @@ -71,6 +71,7 @@ class NodeParametersInterfaceTest : public performance_test_fixture::Performance 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); } @@ -79,6 +80,7 @@ BENCHMARK_F(NodeParametersInterfaceTest, declare_undeclare)(benchmark::State & s 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; @@ -89,6 +91,7 @@ BENCHMARK_F(NodeParametersInterfaceTest, has_parameter_hit)(benchmark::State & s 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; @@ -112,6 +115,7 @@ BENCHMARK_F(NodeParametersInterfaceTest, set_parameters_bool)(benchmark::State & reset_heap_counters(); for (auto _ : state) { + (void)_; node->set_parameters(param_values2); node->set_parameters(param_values1); } @@ -133,6 +137,7 @@ BENCHMARK_F(NodeParametersInterfaceTest, set_parameters_atomically_bool)(benchma reset_heap_counters(); for (auto _ : state) { + (void)_; node->set_parameters_atomically(param_values2); node->set_parameters_atomically(param_values1); } @@ -164,6 +169,7 @@ BENCHMARK_F(NodeParametersInterfaceTest, set_parameters_callback_bool)(benchmark reset_heap_counters(); for (auto _ : state) { + (void)_; node->set_parameters(param_values2); node->set_parameters(param_values1); } @@ -191,6 +197,7 @@ BENCHMARK_F(NodeParametersInterfaceTest, set_parameters_string)(benchmark::State reset_heap_counters(); for (auto _ : state) { + (void)_; node->set_parameters(param_values2); node->set_parameters(param_values1); } @@ -212,6 +219,7 @@ BENCHMARK_F(NodeParametersInterfaceTest, set_parameters_array)(benchmark::State reset_heap_counters(); for (auto _ : state) { + (void)_; node->set_parameters(param_values2); node->set_parameters(param_values1); } @@ -224,6 +232,7 @@ BENCHMARK_F(NodeParametersInterfaceTest, get_parameter)(benchmark::State & state reset_heap_counters(); for (auto _ : state) { + (void)_; node->get_parameter(param1_name, param1_value); } } @@ -239,6 +248,7 @@ BENCHMARK_F(NodeParametersInterfaceTest, list_parameters_hit)(benchmark::State & 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"); @@ -258,6 +268,7 @@ BENCHMARK_F(NodeParametersInterfaceTest, list_parameters_miss)(benchmark::State 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"); diff --git a/rclcpp/test/benchmark/benchmark_parameter_client.cpp b/rclcpp/test/benchmark/benchmark_parameter_client.cpp index c0bdcf2932..0d9a6fd9c9 100644 --- a/rclcpp/test/benchmark/benchmark_parameter_client.cpp +++ b/rclcpp/test/benchmark/benchmark_parameter_client.cpp @@ -136,6 +136,7 @@ static bool result_is_successful(rcl_interfaces::msg::SetParametersResult result 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()) { @@ -148,6 +149,7 @@ BENCHMARK_F(ParameterClientTest, create_destroy_client)(benchmark::State & state 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; @@ -158,6 +160,7 @@ BENCHMARK_F(ParameterClientTest, has_parameter_hit)(benchmark::State & state) 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; @@ -179,6 +182,7 @@ BENCHMARK_F(ParameterClientTest, set_parameters_bool)(benchmark::State & state) }; for (auto _ : state) { + (void)_; std::vector results = params_client->set_parameters(param_values2); if (!std::all_of(results.begin(), results.end(), result_is_successful)) { @@ -208,6 +212,7 @@ BENCHMARK_F(ParameterClientTest, set_parameters_atomically_bool)(benchmark::Stat }; for (auto _ : state) { + (void)_; rcl_interfaces::msg::SetParametersResult result = params_client->set_parameters_atomically(param_values2); if (!result.successful) { @@ -237,6 +242,7 @@ BENCHMARK_F(ParameterClientTest, set_parameters_string)(benchmark::State & state }; for (auto _ : state) { + (void)_; std::vector results = params_client->set_parameters(param_values2); if (!std::all_of(results.begin(), results.end(), result_is_successful)) { @@ -266,6 +272,7 @@ BENCHMARK_F(ParameterClientTest, set_parameters_array)(benchmark::State & state) }; for (auto _ : state) { + (void)_; std::vector results = params_client->set_parameters(param_values2); if (!std::all_of(results.begin(), results.end(), result_is_successful)) { @@ -284,6 +291,7 @@ BENCHMARK_F(ParameterClientTest, set_parameters_array)(benchmark::State & state) 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)"); @@ -300,6 +308,7 @@ BENCHMARK_F(ParameterClientTest, list_parameters_hit)(benchmark::State & state) }; for (auto _ : state) { + (void)_; rcl_interfaces::msg::ListParametersResult param_list = params_client->list_parameters(prefixes, 10); if (param_list.names.size() != 2) { @@ -317,6 +326,7 @@ BENCHMARK_F(ParameterClientTest, list_parameters_miss)(benchmark::State & state) }; for (auto _ : state) { + (void)_; rcl_interfaces::msg::ListParametersResult param_list = params_client->list_parameters(prefixes, 10); if (param_list.names.size() != 0) { diff --git a/rclcpp/test/benchmark/benchmark_service.cpp b/rclcpp/test/benchmark/benchmark_service.cpp index f1d9aca68b..a42723da90 100644 --- a/rclcpp/test/benchmark/benchmark_service.cpp +++ b/rclcpp/test/benchmark/benchmark_service.cpp @@ -69,6 +69,7 @@ BENCHMARK_F(ServicePerformanceTest, construct_service_no_client)(benchmark::Stat reset_heap_counters(); for (auto _ : state) { + (void)_; auto service = node->create_service("not_a_service", callback); benchmark::DoNotOptimize(service); benchmark::ClobberMemory(); @@ -87,6 +88,7 @@ BENCHMARK_F(ServicePerformanceTest, construct_service_empty_srv)(benchmark::Stat reset_heap_counters(); for (auto _ : state) { + (void)_; auto service = node->create_service(empty_service_name, callback); benchmark::DoNotOptimize(service); benchmark::ClobberMemory(); @@ -105,6 +107,7 @@ BENCHMARK_F(ServicePerformanceTest, destroy_service_empty_srv)(benchmark::State reset_heap_counters(); for (auto _ : state) { + (void)_; state.PauseTiming(); auto service = node->create_service(empty_service_name, callback); state.ResumeTiming(); @@ -123,6 +126,7 @@ BENCHMARK_F(ServicePerformanceTest, async_send_response)(benchmark::State & stat reset_heap_counters(); for (auto _ : state) { + (void)_; state.PauseTiming(); // Clear executor queue rclcpp::spin_some(node->get_node_base_interface()); 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/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index 35d9b4ea4e..35fa8fdd32 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -6,12 +6,15 @@ 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 @@ -28,104 +31,94 @@ 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() -# Increasing timeout because connext can take a long time to destroy nodes -# TODO(brawner) remove when destroying Node for Connext is resolved. See: -# https://github.com/ros2/rclcpp/issues/1250 -ament_add_gtest( - test_allocator_memory_strategy - strategies/test_allocator_memory_strategy.cpp - TIMEOUT 360) +ament_add_gtest(test_allocator_memory_strategy strategies/test_allocator_memory_strategy.cpp) if(TARGET test_allocator_memory_strategy) - ament_target_dependencies(test_allocator_memory_strategy - "rcl" - "test_msgs" - ) - target_link_libraries(test_allocator_memory_strategy ${PROJECT_NAME}) + 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) - ament_target_dependencies(test_message_pool_memory_strategy - "rcl" - "test_msgs" - ) - target_link_libraries(test_message_pool_memory_strategy ${PROJECT_NAME}) + 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) - ament_target_dependencies(test_any_service_callback - "test_msgs" - ) - target_link_libraries(test_any_service_callback ${PROJECT_NAME}) + 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) - ament_target_dependencies(test_any_subscription_callback - "test_msgs" - ) - target_link_libraries(test_any_subscription_callback ${PROJECT_NAME}) + 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) - ament_target_dependencies(test_client - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - "test_msgs" - ) - target_link_libraries(test_client ${PROJECT_NAME} mimick) + 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) - ament_target_dependencies(test_create_timer - "rcl_interfaces" - "rmw" - "rcl" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - ) target_link_libraries(test_create_timer ${PROJECT_NAME}) target_include_directories(test_create_timer PRIVATE ./) endif() -ament_add_gtest(test_create_subscription test_create_subscription.cpp) -if(TARGET test_create_subscription) - target_link_libraries(test_create_subscription ${PROJECT_NAME}) - ament_target_dependencies(test_create_subscription - "test_msgs" +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_add_callback_groups_to_executor - test_add_callback_groups_to_executor.cpp - TIMEOUT 120) -if(TARGET test_add_callback_groups_to_executor) - target_link_libraries(test_add_callback_groups_to_executor ${PROJECT_NAME}) - ament_target_dependencies(test_add_callback_groups_to_executor - "test_msgs" +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) - ament_target_dependencies(test_expand_topic_or_service_name - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - ) - target_link_libraries(test_expand_topic_or_service_name ${PROJECT_NAME} mimick) + 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_include_directories(test_function_traits PUBLIC ../../include) - ament_target_dependencies(test_function_traits - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - ) + target_link_libraries(test_function_traits ${PROJECT_NAME}) endif() ament_add_gtest( test_future_return_code @@ -135,72 +128,35 @@ if(TARGET test_future_return_code) endif() ament_add_gmock(test_intra_process_manager test_intra_process_manager.cpp) if(TARGET test_intra_process_manager) - ament_target_dependencies(test_intra_process_manager - "rcl" - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - ) - target_link_libraries(test_intra_process_manager ${PROJECT_NAME}) + 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) - ament_target_dependencies(test_intra_process_manager_with_allocators - "test_msgs" - ) - target_link_libraries(test_intra_process_manager_with_allocators ${PROJECT_NAME}) + 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) - ament_target_dependencies(test_ring_buffer_implementation - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - ) 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) - ament_target_dependencies(test_intra_process_buffer - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - ) target_link_libraries(test_intra_process_buffer ${PROJECT_NAME}) endif() ament_add_gtest(test_loaned_message test_loaned_message.cpp) -ament_target_dependencies(test_loaned_message - "test_msgs" -) -target_link_libraries(test_loaned_message ${PROJECT_NAME} mimick) +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) -ament_target_dependencies(test_memory_strategy - "test_msgs" -) -target_link_libraries(test_memory_strategy ${PROJECT_NAME}) +target_link_libraries(test_memory_strategy ${PROJECT_NAME} ${test_msgs_TARGETS}) ament_add_gtest(test_message_memory_strategy test_message_memory_strategy.cpp) -ament_target_dependencies(test_message_memory_strategy - "test_msgs" -) -target_link_libraries(test_message_memory_strategy ${PROJECT_NAME}) +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) - ament_target_dependencies(test_node - "rcl_interfaces" - "rcpputils" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - "test_msgs" - ) - target_link_libraries(test_node ${PROJECT_NAME} mimick) + 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 @@ -210,8 +166,9 @@ if(TARGET test_node_interfaces__get_node_interfaces) 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) + 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) @@ -221,39 +178,54 @@ 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) - ament_target_dependencies( - test_node_interfaces__node_graph - "test_msgs") - target_link_libraries(test_node_interfaces__node_graph ${PROJECT_NAME} mimick) + 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) + 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) + 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) + 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) - ament_target_dependencies( - test_node_interfaces__node_topics - "test_msgs") - target_link_libraries(test_node_interfaces__node_topics ${PROJECT_NAME} mimick) + 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) + 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 @@ -279,82 +251,46 @@ endif() ament_add_gtest(test_node_global_args test_node_global_args.cpp) if(TARGET test_node_global_args) - ament_target_dependencies(test_node_global_args - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - ) 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) - ament_target_dependencies(test_node_options "rcl") - target_link_libraries(test_node_options ${PROJECT_NAME} mimick) + 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) - ament_target_dependencies(test_init_options "rcl") - target_link_libraries(test_init_options ${PROJECT_NAME} mimick) + target_link_libraries(test_init_options ${PROJECT_NAME} mimick rcl::rcl) endif() -ament_add_gtest(test_parameter_client test_parameter_client.cpp) +ament_add_gmock(test_parameter_client test_parameter_client.cpp) if(TARGET test_parameter_client) - ament_target_dependencies(test_parameter_client - "rcl_interfaces" - ) - target_link_libraries(test_parameter_client ${PROJECT_NAME}) + 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) - ament_target_dependencies(test_parameter_service - "rcl_interfaces" - ) 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) - ament_target_dependencies(test_parameter_events_filter - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - ) - target_link_libraries(test_parameter_events_filter ${PROJECT_NAME}) + 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) - ament_target_dependencies(test_parameter - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - ) 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) - ament_target_dependencies(test_parameter_event_handler - "rcl_interfaces" - "rmw" - "rosidl_generator_cpp" - "rosidl_typesupport_cpp" - ) 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}) + 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) - ament_target_dependencies(test_publisher - "rcl" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - "test_msgs" - ) - target_link_libraries(test_publisher ${PROJECT_NAME} mimick) + target_link_libraries(test_publisher ${PROJECT_NAME} mimick rcl::rcl rcutils::rcutils ${test_msgs_TARGETS}) endif() set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR}") @@ -366,295 +302,273 @@ ament_add_gtest(test_publisher_with_type_adapter test_publisher_with_type_adapte APPEND_LIBRARY_DIRS "${append_library_dirs}" ) if(TARGET test_publisher_with_type_adapter) - ament_target_dependencies(test_publisher_with_type_adapter - "rcutils" - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - "test_msgs" - ) - rosidl_target_interfaces(test_publisher_with_type_adapter ${PROJECT_NAME}_test_msgs "rosidl_typesupport_cpp") - target_link_libraries(test_publisher_with_type_adapter ${PROJECT_NAME} mimick) + 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) - ament_target_dependencies(test_subscription_with_type_adapter - "rcutils" - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - "test_msgs" + 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} ) - rosidl_target_interfaces(test_subscription_with_type_adapter ${PROJECT_NAME}_test_msgs "rosidl_typesupport_cpp") - target_link_libraries(test_subscription_with_type_adapter ${PROJECT_NAME} mimick) endif() ament_add_gtest(test_publisher_subscription_count_api test_publisher_subscription_count_api.cpp) if(TARGET test_publisher_subscription_count_api) - ament_target_dependencies(test_publisher_subscription_count_api - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - "test_msgs" - ) - target_link_libraries(test_publisher_subscription_count_api ${PROJECT_NAME}) + 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) - ament_target_dependencies(test_qos - "rmw" - ) target_link_libraries(test_qos ${PROJECT_NAME} + rmw::rmw ) endif() -function(test_generic_pubsub_for_rmw_implementation) - set(rmw_implementation_env_var RMW_IMPLEMENTATION=${rmw_implementation}) - ament_add_gmock(test_generic_pubsub${target_suffix} test_generic_pubsub.cpp - ENV ${rmw_implementation_env_var} - ) - if(TARGET test_generic_pubsub${target_suffix}) - target_link_libraries(test_generic_pubsub${target_suffix} ${PROJECT_NAME}) - ament_target_dependencies(test_generic_pubsub${target_suffix} - "rcpputils" - "rosidl_typesupport_cpp" - "test_msgs" - ) - endif() -endfunction() -call_for_each_rmw_implementation(test_generic_pubsub_for_rmw_implementation) -ament_add_gtest(test_qos_event test_qos_event.cpp) -if(TARGET test_qos_event) - ament_target_dependencies(test_qos_event - "rmw" - "test_msgs" - ) - target_link_libraries(test_qos_event - ${PROJECT_NAME} - mimick - ) -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} - ) + 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} - ) + target_link_libraries(test_qos_parameters ${PROJECT_NAME}) endif() ament_add_gtest(test_rate test_rate.cpp) if(TARGET test_rate) - ament_target_dependencies(test_rate - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - ) - target_link_libraries(test_rate - ${PROJECT_NAME} - ) + 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) - ament_target_dependencies(test_serialized_message_allocator - test_msgs - ) - target_link_libraries(test_serialized_message_allocator - ${PROJECT_NAME} - ) + 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) - ament_target_dependencies(test_serialized_message - test_msgs - ) - target_link_libraries(test_serialized_message - ${PROJECT_NAME} - ) + 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) - ament_target_dependencies(test_service - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - "test_msgs" - ) - target_link_libraries(test_service ${PROJECT_NAME} mimick) + 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) - ament_target_dependencies(test_subscription - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - "test_msgs" - ) - target_link_libraries(test_subscription ${PROJECT_NAME} mimick) + 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) - ament_target_dependencies(test_subscription_publisher_count_api - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - "test_msgs" - ) - target_link_libraries(test_subscription_publisher_count_api ${PROJECT_NAME}) + 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) - ament_target_dependencies(test_subscription_traits - "rcl" - "test_msgs" - ) - target_link_libraries(test_subscription_traits ${PROJECT_NAME}) + 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) - ament_target_dependencies(test_type_support - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - "test_msgs" - ) - target_link_libraries(test_type_support ${PROJECT_NAME}) + 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}) + 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) - ament_target_dependencies(test_find_weak_nodes - "rcl" - ) target_link_libraries(test_find_weak_nodes ${PROJECT_NAME}) endif() ament_add_gtest(test_externally_defined_services test_externally_defined_services.cpp) -ament_target_dependencies(test_externally_defined_services - "rcl" - "test_msgs" -) -target_link_libraries(test_externally_defined_services ${PROJECT_NAME}) +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) - ament_target_dependencies(test_duration - "rcl") - target_link_libraries(test_duration ${PROJECT_NAME}) + 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}) +target_link_libraries(test_logger ${PROJECT_NAME} rcutils::rcutils) ament_add_gmock(test_logging test_logging.cpp) -target_link_libraries(test_logging ${PROJECT_NAME}) +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) - ament_target_dependencies(test_time - "rcl") - target_link_libraries(test_time ${PROJECT_NAME}) + 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) - ament_target_dependencies(test_timer - "rcl") - target_link_libraries(test_timer ${PROJECT_NAME} mimick) + 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) - ament_target_dependencies(test_time_source - "rcl") - target_link_libraries(test_time_source ${PROJECT_NAME}) + 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) - ament_target_dependencies(test_utilities - "rcl") - target_link_libraries(test_utilities ${PROJECT_NAME} mimick) + 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) - ament_target_dependencies(test_wait_for_message - "test_msgs") - target_link_libraries(test_wait_for_message ${PROJECT_NAME}) + 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) - ament_target_dependencies(test_interface_traits - "rcl") target_link_libraries(test_interface_traits ${PROJECT_NAME}) endif() -# TODO(brawner) remove when destroying Node for Connext is resolved. See: -# https://github.com/ros2/rclcpp/issues/1250 +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) - ament_target_dependencies(test_executors - "rcl" - "test_msgs") - target_link_libraries(test_executors ${PROJECT_NAME}) + 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) - ament_target_dependencies(test_static_single_threaded_executor - "test_msgs") - target_link_libraries(test_static_single_threaded_executor ${PROJECT_NAME} mimick) + 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) - ament_target_dependencies(test_multi_threaded_executor - "rcl") target_link_libraries(test_multi_threaded_executor ${PROJECT_NAME}) endif() -ament_add_gtest(test_static_executor_entities_collector executors/test_static_executor_entities_collector.cpp +ament_add_gtest(test_entities_collector executors/test_entities_collector.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}" TIMEOUT 120) -if(TARGET test_static_executor_entities_collector) - ament_target_dependencies(test_static_executor_entities_collector - "rcl" - "test_msgs") - target_link_libraries(test_static_executor_entities_collector ${PROJECT_NAME} mimick) +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() @@ -662,73 +576,121 @@ endif() ament_add_gtest(test_wait_set test_wait_set.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}") if(TARGET test_wait_set) - ament_target_dependencies(test_wait_set "test_msgs") - target_link_libraries(test_wait_set ${PROJECT_NAME}) + 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) - ament_target_dependencies(test_subscription_topic_statistics - "builtin_interfaces" - "libstatistics_collector" - "rcl_interfaces" - "rcutils" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - "statistics_msgs" - "test_msgs") - rosidl_target_interfaces(test_subscription_topic_statistics ${PROJECT_NAME}_test_msgs "rosidl_typesupport_cpp") - target_link_libraries(test_subscription_topic_statistics ${PROJECT_NAME}) + 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) - ament_target_dependencies(test_subscription_options "rcl") 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) - ament_target_dependencies(test_dynamic_storage "rcl" "test_msgs") - target_link_libraries(test_dynamic_storage ${PROJECT_NAME}) + 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) - ament_target_dependencies(test_storage_policy_common "rcl" "test_msgs") - target_link_libraries(test_storage_policy_common ${PROJECT_NAME} mimick) + 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) - ament_target_dependencies(test_static_storage "rcl" "test_msgs") - target_link_libraries(test_static_storage ${PROJECT_NAME}) + 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) - ament_target_dependencies(test_thread_safe_synchronization "rcl" "test_msgs") - target_link_libraries(test_thread_safe_synchronization ${PROJECT_NAME}) + 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) - ament_target_dependencies(test_rosout_qos "rcl") - target_link_libraries(test_rosout_qos ${PROJECT_NAME}) + 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) - ament_target_dependencies(test_executor "rcl") - target_link_libraries(test_executor ${PROJECT_NAME} mimick) + 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 index 341846a3eb..4619b7665d 100644 --- a/rclcpp/test/rclcpp/allocator/test_allocator_common.cpp +++ b/rclcpp/test/rclcpp/allocator/test_allocator_common.cpp @@ -51,6 +51,53 @@ TEST(TestAllocatorCommon, retyped_allocate) { 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); 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 index 1cf90d9901..cea0900a39 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -15,25 +15,34 @@ /** * 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; @@ -41,18 +50,10 @@ template class TestExecutors : public ::testing::Test { public: - static void SetUpTestCase() + void SetUp() { 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(); @@ -73,6 +74,8 @@ class TestExecutors : public ::testing::Test publisher.reset(); subscription.reset(); node.reset(); + + rclcpp::shutdown(); } rclcpp::Node::SharedPtr node; @@ -81,54 +84,16 @@ class TestExecutors : public ::testing::Test int callback_count; }; -// spin_all and spin_some are not implemented correctly in StaticSingleThreadedExecutor, see: -// https://github.com/ros2/rclcpp/issues/1219 for tracking template class TestExecutorsStable : public TestExecutors {}; -using ExecutorTypes = - ::testing::Types< - rclcpp::executors::SingleThreadedExecutor, - rclcpp::executors::MultiThreadedExecutor, - rclcpp::executors::StaticSingleThreadedExecutor>; - -class ExecutorTypeNames -{ -public: - template - static std::string GetName(int idx) - { - (void)idx; - if (std::is_same()) { - return "SingleThreadedExecutor"; - } - - if (std::is_same()) { - return "MultiThreadedExecutor"; - } - - if (std::is_same()) { - return "StaticSingleThreadedExecutor"; - } - - return ""; - } -}; - -// TYPED_TEST_SUITE is deprecated as of gtest 1.9, use TYPED_TEST_SUITE when gtest dependency -// is updated. TYPED_TEST_SUITE(TestExecutors, ExecutorTypes, ExecutorTypeNames); -// 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>; TYPED_TEST_SUITE(TestExecutorsStable, StandardExecutors, ExecutorTypeNames); // Make sure that executors detach from nodes when destructing -TYPED_TEST(TestExecutors, detachOnDestruction) { +TYPED_TEST(TestExecutors, detachOnDestruction) +{ using ExecutorType = TypeParam; { ExecutorType executor; @@ -141,9 +106,7 @@ TYPED_TEST(TestExecutors, detachOnDestruction) { } // Make sure that the executor can automatically remove expired nodes correctly -// Currently fails for StaticSingleThreadedExecutor so it is being skipped, see: -// https://github.com/ros2/rclcpp/issues/1231 -TYPED_TEST(TestExecutorsStable, addTemporaryNode) { +TYPED_TEST(TestExecutors, addTemporaryNode) { using ExecutorType = TypeParam; ExecutorType executor; @@ -161,8 +124,20 @@ TYPED_TEST(TestExecutorsStable, addTemporaryNode) { 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) { +TYPED_TEST(TestExecutors, addNodeTwoExecutors) +{ using ExecutorType = TypeParam; ExecutorType executor1; ExecutorType executor2; @@ -172,7 +147,8 @@ TYPED_TEST(TestExecutors, addNodeTwoExecutors) { } // Check simple spin example -TYPED_TEST(TestExecutors, spinWithTimer) { +TYPED_TEST(TestExecutors, spinWithTimer) +{ using ExecutorType = TypeParam; ExecutorType executor; @@ -194,19 +170,23 @@ TYPED_TEST(TestExecutors, spinWithTimer) { executor.remove_node(this->node, true); } -TYPED_TEST(TestExecutors, spinWhileAlreadySpinning) { +TYPED_TEST(TestExecutors, spinWhileAlreadySpinning) +{ using ExecutorType = TypeParam; ExecutorType executor; - executor.add_node(this->node); - bool timer_completed = false; - auto timer = this->node->create_wall_timer(1ms, [&]() {timer_completed = true;}); + 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. + // 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 && (std::chrono::steady_clock::now() - start) < 10s) { + while (!timer_completed.load() && (std::chrono::steady_clock::now() - start) < 10s) { std::this_thread::sleep_for(1ms); } @@ -220,7 +200,8 @@ TYPED_TEST(TestExecutors, spinWhileAlreadySpinning) { } // Check executor exits immediately if future is complete. -TYPED_TEST(TestExecutors, testSpinUntilFutureComplete) { +TYPED_TEST(TestExecutors, testSpinUntilFutureComplete) +{ using ExecutorType = TypeParam; ExecutorType executor; executor.add_node(this->node); @@ -242,7 +223,8 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureComplete) { } // Same test, but uses a shared future. -TYPED_TEST(TestExecutors, testSpinUntilSharedFutureComplete) { +TYPED_TEST(TestExecutors, testSpinUntilSharedFutureComplete) +{ using ExecutorType = TypeParam; ExecutorType executor; executor.add_node(this->node); @@ -265,7 +247,8 @@ TYPED_TEST(TestExecutors, testSpinUntilSharedFutureComplete) { } // For a longer running future that should require several iterations of spin_once -TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteNoTimeout) { +TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteNoTimeout) +{ using ExecutorType = TypeParam; ExecutorType executor; executor.add_node(this->node); @@ -311,7 +294,8 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteNoTimeout) { } // Check spin_until_future_complete timeout works as expected -TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteWithTimeout) { +TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteWithTimeout) +{ using ExecutorType = TypeParam; ExecutorType executor; executor.add_node(this->node); @@ -349,81 +333,8 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteWithTimeout) { spinner.join(); } -class TestWaitable : public rclcpp::Waitable +TYPED_TEST(TestExecutors, spinAll) { -public: - TestWaitable() - { - rcl_guard_condition_options_t guard_condition_options = - rcl_guard_condition_get_default_options(); - - gc_ = rcl_get_zero_initialized_guard_condition(); - rcl_ret_t ret = rcl_guard_condition_init( - &gc_, - rclcpp::contexts::get_global_default_context()->get_rcl_context().get(), - guard_condition_options); - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); - } - } - - ~TestWaitable() - { - rcl_ret_t ret = rcl_guard_condition_fini(&gc_); - if (RCL_RET_OK != ret) { - fprintf(stderr, "failed to call rcl_guard_condition_fini\n"); - } - } - - bool - add_to_wait_set(rcl_wait_set_t * wait_set) override - { - rcl_ret_t ret = rcl_wait_set_add_guard_condition(wait_set, &gc_, NULL); - return RCL_RET_OK == ret; - } - - bool trigger() - { - rcl_ret_t ret = rcl_trigger_guard_condition(&gc_); - return RCL_RET_OK == ret; - } - - bool - is_ready(rcl_wait_set_t * wait_set) override - { - (void)wait_set; - return true; - } - - std::shared_ptr - take_data() override - { - return nullptr; - } - - void - execute(std::shared_ptr & data) override - { - (void) data; - count_++; - std::this_thread::sleep_for(3ms); - } - - size_t - get_number_of_ready_guard_conditions() override {return 1;} - - size_t - get_count() - { - return count_; - } - -private: - size_t count_ = 0; - rcl_guard_condition_t gc_; -}; - -TYPED_TEST(TestExecutors, spinAll) { using ExecutorType = TypeParam; ExecutorType executor; auto waitable_interfaces = this->node->get_node_waitables_interface(); @@ -464,47 +375,181 @@ TYPED_TEST(TestExecutors, spinAll) { spinner.join(); } -TYPED_TEST(TestExecutors, spinSome) { +// 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; - 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, doesn't block test from finishing because spin_some should exit after the - // first one completes. - bool spin_exited = false; - std::thread spinner([&spin_exited, &executor, this]() { - executor.spin_some(1s); - executor.remove_node(this->node, true); - spin_exited = true; - }); + // 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"; + } - // Do some work until sufficient calls to the waitable occur, but keep going until either - // count becomes too large, spin exits, or the 1 second timeout completes. - auto start = std::chrono::steady_clock::now(); - while ( - my_waitable->get_count() <= 1 && - !spin_exited && - (std::chrono::steady_clock::now() - start < 1s)) + // 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); { - my_waitable->trigger(); - this->publisher->publish(test_msgs::msg::Empty()); - std::this_thread::sleep_for(1ms); + 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"; } - EXPECT_EQ(1u, my_waitable->get_count()); - waitable_interfaces->remove_waitable(my_waitable, nullptr); - EXPECT_TRUE(spin_exited); - // Cancel if it hasn't exited already. - executor.cancel(); + // 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"; + } +} - spinner.join(); +// 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) { +TYPED_TEST(TestExecutors, testSpinNodeUntilFutureCompleteNodeBasePtr) +{ using ExecutorType = TypeParam; ExecutorType executor; @@ -519,7 +564,8 @@ TYPED_TEST(TestExecutors, testSpinNodeUntilFutureCompleteNodeBasePtr) { } // Check spin_node_until_future_complete with node pointer -TYPED_TEST(TestExecutors, testSpinNodeUntilFutureCompleteNodePtr) { +TYPED_TEST(TestExecutors, testSpinNodeUntilFutureCompleteNodePtr) +{ using ExecutorType = TypeParam; ExecutorType executor; @@ -534,7 +580,8 @@ TYPED_TEST(TestExecutors, testSpinNodeUntilFutureCompleteNodePtr) { } // Check spin_until_future_complete can be properly interrupted. -TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteInterrupted) { +TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteInterrupted) +{ using ExecutorType = TypeParam; ExecutorType executor; executor.add_node(this->node); @@ -576,8 +623,135 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteInterrupted) { 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) { +TEST(TestExecutors, testSpinUntilFutureCompleteNodeBasePtr) +{ rclcpp::init(0, nullptr); { @@ -597,7 +771,8 @@ TEST(TestExecutors, testSpinUntilFutureCompleteNodeBasePtr) { } // Check spin_until_future_complete with node pointer (instantiates its own executor) -TEST(TestExecutors, testSpinUntilFutureCompleteNodePtr) { +TEST(TestExecutors, testSpinUntilFutureCompleteNodePtr) +{ rclcpp::init(0, nullptr); { @@ -614,3 +789,57 @@ TEST(TestExecutors, testSpinUntilFutureCompleteNodePtr) { 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 index b6fd2c3fda..09dfa03f90 100644 --- a/rclcpp/test/rclcpp/executors/test_multi_threaded_executor.cpp +++ b/rclcpp/test/rclcpp/executors/test_multi_threaded_executor.cpp @@ -32,6 +32,11 @@ class TestMultiThreadedExecutor : public ::testing::Test { rclcpp::init(0, nullptr); } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } }; constexpr std::chrono::milliseconds PERIOD_MS = 1000ms; 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 index 51278af9f9..1c8c5b3abe 100644 --- a/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp +++ b/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp @@ -143,12 +143,11 @@ TEST_F(TestStaticExecutorEntitiesCollector, init_bad_arguments) { RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));}); rclcpp::GuardCondition guard_condition(shared_context); - rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); // Check memory strategy is nullptr rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy = nullptr; EXPECT_THROW( - entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition), + entities_collector_->init(&wait_set, memory_strategy), std::runtime_error); } @@ -169,9 +168,8 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_basic_node) { auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); rclcpp::GuardCondition guard_condition(shared_context); - rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); - entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + entities_collector_->init(&wait_set, memory_strategy); RCPPUTILS_SCOPE_EXIT(entities_collector_->fini()); EXPECT_EQ( expected_number_of_entities->subscriptions, @@ -185,7 +183,7 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_basic_node) { entities_collector_->get_number_of_waitables()); EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface())); - entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + 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()); @@ -216,10 +214,9 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_out_of_scope) { auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); rclcpp::GuardCondition guard_condition(shared_context); - rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); // Expect weak_node pointers to be cleaned up and used - entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + 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()); @@ -233,9 +230,9 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_out_of_scope) { class TestWaitable : public rclcpp::Waitable { public: - bool add_to_wait_set(rcl_wait_set_t *) override {return true;} + void add_to_wait_set(rcl_wait_set_t &) override {} - bool is_ready(rcl_wait_set_t *) override {return true;} + bool is_ready(const rcl_wait_set_t &) override {return true;} std::shared_ptr take_data() override @@ -243,10 +240,7 @@ class TestWaitable : public rclcpp::Waitable return nullptr; } void - execute(std::shared_ptr & data) override - { - (void) data; - } + execute(const std::shared_ptr &) override {} }; TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_with_entities) { @@ -288,9 +282,8 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_with_entities) { auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); rclcpp::GuardCondition guard_condition(shared_context); - rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); - entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + entities_collector_->init(&wait_set, memory_strategy); RCPPUTILS_SCOPE_EXIT(entities_collector_->fini()); EXPECT_EQ( @@ -310,7 +303,7 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_with_entities) { entities_collector_->get_number_of_waitables()); entities_collector_->remove_node(node->get_node_base_interface()); - entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + 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()); @@ -367,9 +360,8 @@ TEST_F(TestStaticExecutorEntitiesCollector, prepare_wait_set_rcl_wait_set_clear_ auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); rclcpp::GuardCondition guard_condition(shared_context); - rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); - entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + entities_collector_->init(&wait_set, memory_strategy); RCPPUTILS_SCOPE_EXIT(entities_collector_->fini()); { @@ -398,9 +390,8 @@ TEST_F(TestStaticExecutorEntitiesCollector, prepare_wait_set_rcl_wait_set_resize auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); rclcpp::GuardCondition guard_condition(shared_context); - rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); - entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + entities_collector_->init(&wait_set, memory_strategy); RCPPUTILS_SCOPE_EXIT(entities_collector_->fini()); { @@ -436,9 +427,8 @@ TEST_F(TestStaticExecutorEntitiesCollector, refresh_wait_set_rcl_wait_failed) { auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); rclcpp::GuardCondition guard_condition(shared_context); - rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); - entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + entities_collector_->init(&wait_set, memory_strategy); RCPPUTILS_SCOPE_EXIT(entities_collector_->fini()); { @@ -485,9 +475,8 @@ TEST_F(TestStaticExecutorEntitiesCollector, refresh_wait_set_add_handles_to_wait auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); rclcpp::GuardCondition guard_condition(shared_context); - rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); - entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + entities_collector_->init(&wait_set, memory_strategy); RCPPUTILS_SCOPE_EXIT(entities_collector_->fini()); { @@ -517,16 +506,10 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_to_wait_set_nullptr) { auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); rclcpp::GuardCondition guard_condition(shared_context); - rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); - entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + entities_collector_->init(&wait_set, memory_strategy); RCPPUTILS_SCOPE_EXIT(entities_collector_->fini()); - RCLCPP_EXPECT_THROW_EQ( - entities_collector_->add_to_wait_set(nullptr), - std::runtime_error("Executor waitable: couldn't add guard condition to wait set")); - rcl_reset_error(); - EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface())); } @@ -545,7 +528,6 @@ TEST_F(TestStaticExecutorEntitiesCollector, fill_memory_strategy_invalid_group) auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); rclcpp::GuardCondition guard_condition(shared_context); - rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive); @@ -555,7 +537,7 @@ TEST_F(TestStaticExecutorEntitiesCollector, fill_memory_strategy_invalid_group) cb_group.reset(); - entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + entities_collector_->init(&wait_set, memory_strategy); RCPPUTILS_SCOPE_EXIT(entities_collector_->fini()); ASSERT_EQ(entities_collector_->get_all_callback_groups().size(), 1u); @@ -623,9 +605,8 @@ TEST_F( auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); rclcpp::GuardCondition guard_condition(shared_context); - rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); - entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + entities_collector_->init(&wait_set, memory_strategy); RCPPUTILS_SCOPE_EXIT(entities_collector_->fini()); cb_group->get_associated_with_executor_atomic().exchange(false); diff --git a/rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp b/rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp index 5ca6c1c25a..8e330cbebc 100644 --- a/rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp +++ b/rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp @@ -20,12 +20,13 @@ #include #include "rclcpp/exceptions.hpp" +#include "rclcpp/executors.hpp" #include "rclcpp/node.hpp" #include "rclcpp/rclcpp.hpp" -#include "rclcpp/executors.hpp" #include "test_msgs/srv/empty.hpp" +#include "./executor_types.hpp" #include "../../mocking_utils/patch.hpp" #include "../../utils/rclcpp_gtest_macros.hpp" @@ -46,7 +47,15 @@ class TestStaticSingleThreadedExecutor : public ::testing::Test }; TEST_F(TestStaticSingleThreadedExecutor, add_callback_group_trigger_guard_failed) { - rclcpp::executors::StaticSingleThreadedExecutor executor; +#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); @@ -56,12 +65,20 @@ TEST_F(TestStaticSingleThreadedExecutor, add_callback_group_trigger_guard_failed "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("error not set")); + std::runtime_error("Failed to handle entities update on callback group add: error not set")); } } TEST_F(TestStaticSingleThreadedExecutor, add_node_trigger_guard_failed) { - rclcpp::executors::StaticSingleThreadedExecutor executor; +#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"); { @@ -69,12 +86,20 @@ TEST_F(TestStaticSingleThreadedExecutor, add_node_trigger_guard_failed) { "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); RCLCPP_EXPECT_THROW_EQ( executor.add_node(node), - std::runtime_error("error not set")); + std::runtime_error("Failed to handle entities update on node add: error not set")); } } TEST_F(TestStaticSingleThreadedExecutor, remove_callback_group_trigger_guard_failed) { - rclcpp::executors::StaticSingleThreadedExecutor executor; +#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); @@ -86,12 +111,21 @@ TEST_F(TestStaticSingleThreadedExecutor, remove_callback_group_trigger_guard_fai "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); RCLCPP_EXPECT_THROW_EQ( executor.remove_callback_group(cb_group, true), - std::runtime_error("error not set")); + std::runtime_error( + "Failed to handle entities update on callback group remove: error not set")); } } TEST_F(TestStaticSingleThreadedExecutor, remove_node_failed) { - rclcpp::executors::StaticSingleThreadedExecutor executor; +#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"); { @@ -99,12 +133,20 @@ TEST_F(TestStaticSingleThreadedExecutor, remove_node_failed) { "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); RCLCPP_EXPECT_THROW_EQ( executor.remove_node(node, true), - std::runtime_error("Node needs to be associated with this executor.")); + std::runtime_error("Node '/ns/node' needs to be associated with an executor.")); } } TEST_F(TestStaticSingleThreadedExecutor, remove_node_trigger_guard_failed) { - rclcpp::executors::StaticSingleThreadedExecutor executor; +#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); @@ -114,12 +156,20 @@ TEST_F(TestStaticSingleThreadedExecutor, remove_node_trigger_guard_failed) { "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); RCLCPP_EXPECT_THROW_EQ( executor.remove_node(node, true), - std::runtime_error("error not set")); + std::runtime_error("Failed to handle entities update on node remove: error not set")); } } TEST_F(TestStaticSingleThreadedExecutor, execute_service) { - rclcpp::executors::StaticSingleThreadedExecutor executor; +#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); 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/test_get_node_interfaces.cpp b/rclcpp/test/rclcpp/node_interfaces/test_get_node_interfaces.cpp index d91570e3be..d890f74d08 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_get_node_interfaces.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_get_node_interfaces.cpp @@ -38,6 +38,7 @@ class TestGetNodeInterfaces : public ::testing::Test { node.reset(); wrapped_node.reset(); + rclcpp::shutdown(); } static rclcpp::Node::SharedPtr node; diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp index 6a44d7abf7..a2f19aa82b 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp @@ -28,6 +28,7 @@ #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" @@ -46,9 +47,13 @@ constexpr char absolute_namespace[] = "/ns"; class TestNodeGraph : public ::testing::Test { public: - void SetUp() + 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 @@ -58,7 +63,7 @@ class TestNodeGraph : public ::testing::Test ASSERT_NE(nullptr, node_graph_); } - void TearDown() + static void TearDownTestCase() { rclcpp::shutdown(); } @@ -122,8 +127,15 @@ TEST_F(TestNodeGraph, construct_from_node) 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 @@ -211,7 +223,7 @@ TEST_F(TestNodeGraph, get_service_names_and_types_by_node) 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(1)) { + 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 = @@ -225,6 +237,40 @@ TEST_F(TestNodeGraph, get_service_names_and_types_by_node) 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 = []( @@ -244,6 +290,21 @@ TEST_F(TestNodeGraph, get_service_names_and_types_by_node_rcl_errors) " 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 = []( @@ -259,12 +320,163 @@ TEST_F(TestNodeGraph, get_service_names_and_types_by_node_names_and_types_fini_e 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( @@ -278,6 +490,20 @@ TEST_F(TestNodeGraph, get_node_names_and_namespaces_rcl_errors) " 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( @@ -287,6 +513,18 @@ TEST_F(TestNodeGraph, get_node_names_and_namespaces_fini_errors) 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); @@ -303,6 +541,22 @@ TEST_F(TestNodeGraph, count_subscribers_rcl_error) 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()); @@ -326,9 +580,9 @@ TEST_F(TestNodeGraph, notify_graph_change_rcl_error) { auto mock = mocking_utils::patch_and_return( "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); - EXPECT_THROW( + RCLCPP_EXPECT_THROW_EQ( node()->get_node_graph_interface()->notify_graph_change(), - rclcpp::exceptions::RCLError); + std::runtime_error("failed to notify wait set on graph change: error not set")); } TEST_F(TestNodeGraph, get_info_by_topic) @@ -369,6 +623,18 @@ TEST_F(TestNodeGraph, get_info_by_topic) 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; 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 index 31b755b4a7..0cc06792a8 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp @@ -21,6 +21,7 @@ #include #include +#include #include #include #include @@ -31,6 +32,8 @@ #include "../../mocking_utils/patch.hpp" #include "../../utils/rclcpp_gtest_macros.hpp" +#include "rcpputils/filesystem_helper.hpp" + class TestNodeParameters : public ::testing::Test { public: @@ -47,6 +50,7 @@ class TestNodeParameters : public ::testing::Test dynamic_cast( node->get_node_parameters_interface().get()); ASSERT_NE(nullptr, node_parameters); + test_resources_path /= "test_node_parameters"; } void TearDown() @@ -57,6 +61,8 @@ class TestNodeParameters : public ::testing::Test 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) { @@ -72,9 +78,9 @@ TEST_F(TestNodeParameters, list_parameters) std::vector prefixes; const auto list_result = node_parameters->list_parameters(prefixes, 1u); - // Currently the only default parameter is 'use_sim_time', but that may change. + // Currently the default parameters are 'use_sim_time' and 'start_type_description_service' size_t number_of_parameters = list_result.names.size(); - EXPECT_GE(1u, number_of_parameters); + EXPECT_GE(2u, number_of_parameters); const std::string parameter_name = "new_parameter"; const rclcpp::ParameterValue value(true); @@ -90,15 +96,15 @@ TEST_F(TestNodeParameters, list_parameters) std::find(list_result2.names.begin(), list_result2.names.end(), parameter_name), list_result2.names.end()); - // Check prefixes + // 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(value.get(), added_parameter_value.get()); + EXPECT_EQ(value2.get(), added_parameter_value2.get()); prefixes = {"prefix"}; - auto list_result3 = node_parameters->list_parameters(prefixes, 2u); + 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), @@ -111,6 +117,13 @@ TEST_F(TestNodeParameters, list_parameters) 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) @@ -170,7 +183,7 @@ TEST_F(TestNodeParameters, set_parameters) { EXPECT_TRUE(result[0].successful); } -TEST_F(TestNodeParameters, add_remove_parameters_callback) { +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; @@ -197,5 +210,248 @@ TEST_F(TestNodeParameters, add_remove_parameters_callback) { RCLCPP_EXPECT_THROW_EQ( node_parameters->remove_on_set_parameters_callback(handle.get()), - std::runtime_error("Callback doesn't exist")); + 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 index a7607f5d76..7b00fea972 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp @@ -92,7 +92,7 @@ TEST_F(TestNodeService, add_service) different_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); RCLCPP_EXPECT_THROW_EQ( node_services->add_service(service, callback_group_in_different_node), - std::runtime_error("Cannot create service, group not in node.")); + rclcpp::exceptions::MissingGroupNodeException("service")); } TEST_F(TestNodeService, add_service_rcl_trigger_guard_condition_error) @@ -103,7 +103,7 @@ TEST_F(TestNodeService, add_service_rcl_trigger_guard_condition_error) "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")); + std::runtime_error("failed to notify wait set on service creation: error not set")); } TEST_F(TestNodeService, add_client) @@ -119,7 +119,7 @@ TEST_F(TestNodeService, add_client) different_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); RCLCPP_EXPECT_THROW_EQ( node_services->add_client(client, callback_group_in_different_node), - std::runtime_error("Cannot create client, group not in node.")); + rclcpp::exceptions::MissingGroupNodeException("client")); } TEST_F(TestNodeService, add_client_rcl_trigger_guard_condition_error) @@ -130,7 +130,7 @@ TEST_F(TestNodeService, add_client_rcl_trigger_guard_condition_error) "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")); + std::runtime_error("failed to notify wait set on client creation: error not set")); } TEST_F(TestNodeService, resolve_service_name) diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp index d368e0e875..ce6343fb5c 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp @@ -32,8 +32,8 @@ class TestTimer : public rclcpp::TimerBase : TimerBase(node->get_clock(), std::chrono::nanoseconds(1), node->get_node_base_interface()->get_context()) {} - bool call() override {return true;} - void execute_callback() override {} + std::shared_ptr call() override {return nullptr;} + void execute_callback(const std::shared_ptr &) override {} bool is_steady() override {return false;} }; @@ -75,7 +75,7 @@ TEST_F(TestNodeTimers, add_timer) different_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); RCLCPP_EXPECT_THROW_EQ( node_timers->add_timer(timer, callback_group_in_different_node), - std::runtime_error("Cannot create timer, group not in node.")); + rclcpp::exceptions::MissingGroupNodeException("timer")); } TEST_F(TestNodeTimers, add_timer_rcl_trigger_guard_condition_error) @@ -87,5 +87,5 @@ TEST_F(TestNodeTimers, add_timer_rcl_trigger_guard_condition_error) "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")); + 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 index bb59c83bb2..e8f873f693 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp @@ -28,6 +28,11 @@ #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 { @@ -36,16 +41,14 @@ const rosidl_message_type_support_t EmptyTypeSupport() return *rosidl_typesupport_cpp::get_message_type_support_handle(); } -const rcl_publisher_options_t PublisherOptions() +const rclcpp::PublisherOptionsWithAllocator> PublisherOptions() { - return rclcpp::PublisherOptionsWithAllocator>().template - to_rcl_publisher_options(rclcpp::QoS(10)); + return rclcpp::PublisherOptionsWithAllocator>(); } -const rcl_subscription_options_t SubscriptionOptions() +const rclcpp::SubscriptionOptionsWithAllocator> SubscriptionOptions() { - return rclcpp::SubscriptionOptionsWithAllocator>().template - to_rcl_subscription_options(rclcpp::QoS(10)); + return rclcpp::SubscriptionOptionsWithAllocator>(); } } // namespace @@ -55,7 +58,9 @@ class TestPublisher : public rclcpp::PublisherBase public: explicit TestPublisher(rclcpp::Node * node) : rclcpp::PublisherBase( - node->get_node_base_interface().get(), "topic", EmptyTypeSupport(), PublisherOptions()) {} + 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 @@ -63,7 +68,9 @@ class TestSubscription : public rclcpp::SubscriptionBase public: explicit TestSubscription(rclcpp::Node * node) : rclcpp::SubscriptionBase( - node->get_node_base_interface().get(), EmptyTypeSupport(), "topic", SubscriptionOptions()) {} + 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 @@ -75,6 +82,18 @@ class TestSubscription : public rclcpp::SubscriptionBase 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 @@ -129,7 +148,7 @@ TEST_F(TestNodeTopics, add_publisher_rcl_trigger_guard_condition_error) "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")); + std::runtime_error("failed to notify wait set on publisher creation: error not set")); } TEST_F(TestNodeTopics, add_subscription) 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 index a12ef36eab..2b6b1092be 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp @@ -28,19 +28,16 @@ class TestWaitable : public rclcpp::Waitable { public: - bool add_to_wait_set(rcl_wait_set_t *) override {return false;} - bool is_ready(rcl_wait_set_t *) override {return false;} + 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; - } + std::shared_ptr take_data() override {return nullptr;} + void execute(const std::shared_ptr &) override {} - void execute(std::shared_ptr & data) override - { - (void) data; - } + 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 @@ -78,7 +75,7 @@ TEST_F(TestNodeWaitables, add_remove_waitable) node_waitables->add_waitable(waitable, callback_group1)); RCLCPP_EXPECT_THROW_EQ( node_waitables->add_waitable(waitable, callback_group2), - std::runtime_error("Cannot create waitable, group not in node.")); + rclcpp::exceptions::MissingGroupNodeException("waitable")); EXPECT_NO_THROW(node_waitables->remove_waitable(waitable, callback_group1)); EXPECT_NO_THROW(node_waitables->remove_waitable(waitable, callback_group2)); @@ -96,5 +93,5 @@ TEST_F(TestNodeWaitables, add_waitable_rcl_trigger_guard_condition_error) "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")); + 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 index 09279ed024..27c2711916 100644 --- a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp +++ b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp @@ -39,26 +39,25 @@ static bool test_waitable_result = false; class TestWaitable : public rclcpp::Waitable { public: - bool add_to_wait_set(rcl_wait_set_t *) override + void add_to_wait_set(rcl_wait_set_t &) override { - return test_waitable_result; + if (!test_waitable_result) { + throw std::runtime_error("TestWaitable add_to_wait_set failed"); + } } - bool is_ready(rcl_wait_set_t *) override + bool is_ready(const rcl_wait_set_t &) override { return test_waitable_result; } - std::shared_ptr - take_data() override - { - return nullptr; - } + std::shared_ptr take_data() override {return nullptr;} + void execute(const std::shared_ptr &) override {} - void execute(std::shared_ptr & data) override - { - (void) data; - } + 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 @@ -147,9 +146,9 @@ class TestAllocatorMemoryStrategy : public ::testing::Test 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 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 = @@ -160,7 +159,7 @@ class TestAllocatorMemoryStrategy : public ::testing::Test services_.push_back( node_with_service->create_service( - "service", std::move(service_callback), rmw_qos_profile_services_default, callback_group)); + "service", std::move(service_callback), rclcpp::ServicesQoS(), callback_group)); return node_with_service; } @@ -175,7 +174,7 @@ class TestAllocatorMemoryStrategy : public ::testing::Test clients_.push_back( node_with_client->create_client( - "service", rmw_qos_profile_services_default, callback_group)); + "service", rclcpp::ServicesQoS(), callback_group)); return node_with_client; } @@ -453,19 +452,19 @@ TEST_F(TestAllocatorMemoryStrategy, construct_destruct) { } TEST_F(TestAllocatorMemoryStrategy, add_remove_guard_conditions) { - rcl_guard_condition_t guard_condition1 = rcl_get_zero_initialized_guard_condition(); - rcl_guard_condition_t guard_condition2 = rcl_get_zero_initialized_guard_condition(); - rcl_guard_condition_t guard_condition3 = rcl_get_zero_initialized_guard_condition(); + 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_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_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)); @@ -495,14 +494,8 @@ TEST_F(TestAllocatorMemoryStrategy, add_remove_waitables) { TEST_F(TestAllocatorMemoryStrategy, number_of_entities_with_subscription) { RclWaitSetSizes expected_sizes = {}; expected_sizes.size_of_subscriptions = 1; - const std::string implementation_identifier = rmw_get_implementation_identifier(); - if (implementation_identifier == "rmw_cyclonedds_cpp" || - implementation_identifier == "rmw_connextdds") - { - // For cyclonedds and connext, a subscription will also add an event and waitable - expected_sizes.size_of_events += 1; - expected_sizes.size_of_waitables += 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)); } @@ -569,24 +562,17 @@ TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_client) { TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_guard_condition) { auto node = create_node_with_disabled_callback_groups("node"); - rcl_guard_condition_t guard_condition = rcl_get_zero_initialized_guard_condition(); auto context = node->get_node_base_interface()->get_context(); - rcl_context_t * rcl_context = context->get_rcl_context().get(); - rcl_guard_condition_options_t guard_condition_options = { - rcl_get_default_allocator()}; - - EXPECT_EQ( - RCL_RET_OK, - rcl_guard_condition_init(&guard_condition, rcl_context, guard_condition_options)); - RCPPUTILS_SCOPE_EXIT( - { - EXPECT_EQ(RCL_RET_OK, rcl_guard_condition_fini(&guard_condition)); - }); - allocator_memory_strategy()->add_guard_condition(&guard_condition); + 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_TRUE(TestAddHandlesToWaitSet(node, insufficient_capacities)); + EXPECT_THROW(TestAddHandlesToWaitSet(node, insufficient_capacities), std::runtime_error); } TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_timer) { @@ -607,7 +593,9 @@ TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_waitable) { EXPECT_TRUE(allocator_memory_strategy()->add_handles_to_wait_set(nullptr)); test_waitable_result = false; - EXPECT_FALSE(allocator_memory_strategy()->add_handles_to_wait_set(nullptr)); + 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()); @@ -798,11 +786,11 @@ TEST_F(TestAllocatorMemoryStrategy, get_next_service_out_of_scope) { 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_callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}; auto service = node->create_service( - "service", std::move(service_callback), rmw_qos_profile_services_default, callback_group); + "service", std::move(service_callback), rclcpp::ServicesQoS(), callback_group); node->for_each_callback_group( [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) @@ -840,7 +828,7 @@ TEST_F(TestAllocatorMemoryStrategy, get_next_client_out_of_scope) { node->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive); auto client = node->create_client( - "service", rmw_qos_profile_services_default, callback_group); + "service", rclcpp::ServicesQoS(), callback_group); weak_groups_to_nodes.insert( std::pairborrow_message(), - std::runtime_error("Tried to access message that was still in use! Abort.")); + std::runtime_error("No more free slots in the pool")); EXPECT_NO_THROW(message_memory_strategy_->return_message(message)); } -TEST_F(TestMessagePoolMemoryStrategy, return_unrecognized) { - auto message = message_memory_strategy_->borrow_message(); - ASSERT_NE(nullptr, message); +TEST_F(TestMessagePoolMemoryStrategy, borrow_hold_reference) { + { + auto message = message_memory_strategy_->borrow_message(); + ASSERT_NE(nullptr, message); - auto unrecognized = std::make_shared(); - // Unrecognized does not belong to pool - RCLCPP_EXPECT_THROW_EQ( - message_memory_strategy_->return_message(unrecognized), - std::runtime_error("Unrecognized message ptr in return_message.")); - EXPECT_NO_THROW(message_memory_strategy_->return_message(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 index fa636b7157..2c628e5cf8 100644 --- a/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp +++ b/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp @@ -23,13 +23,14 @@ #include "rclcpp/node.hpp" #include "test_msgs/msg/empty.hpp" -#include "test_msgs/msg/empty.h" #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 @@ -46,42 +47,20 @@ class TestAddCallbackGroupsToExecutor : public ::testing::Test } }; -using ExecutorTypes = - ::testing::Types< - rclcpp::executors::SingleThreadedExecutor, - rclcpp::executors::MultiThreadedExecutor, - rclcpp::executors::StaticSingleThreadedExecutor>; - -class ExecutorTypeNames -{ -public: - template - static std::string GetName(int idx) - { - (void)idx; - if (std::is_same()) { - return "SingleThreadedExecutor"; - } - - if (std::is_same()) { - return "MultiThreadedExecutor"; - } - - if (std::is_same()) { - return "StaticSingleThreadedExecutor"; - } - - return ""; - } -}; +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) { +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( @@ -127,8 +106,10 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, add_callback_groups) { /* * Test removing callback groups. */ -TYPED_TEST(TestAddCallbackGroupsToExecutor, remove_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( @@ -158,7 +139,9 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, remove_callback_groups) { */ TYPED_TEST(TestAddCallbackGroupsToExecutor, add_duplicate_callback_groups) { - rclcpp::executors::MultiThreadedExecutor executor; + 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( @@ -176,22 +159,32 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, add_duplicate_callback_groups) */ TYPED_TEST(TestAddCallbackGroupsToExecutor, add_callback_groups_after_add_node_to_executor) { - rclcpp::executors::MultiThreadedExecutor 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(), 1u); - std::atomic_int timer_count {0}; + 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]() { - if (timer_count > 0) { - ASSERT_EQ(executor.get_all_callback_groups().size(), 3u); + auto cur_timer_count = timer_count++; + if (cur_timer_count > 0) { executor.cancel(); } - timer_count++; }; 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); + 1s, timer_callback, cb_grp); rclcpp::CallbackGroup::SharedPtr cb_grp2 = node->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive, false); auto timer2_callback = []() {}; @@ -203,6 +196,7 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, add_callback_groups_after_add_node_t rclcpp::TimerBase::SharedPtr timer3_ = node->create_wall_timer( 2s, timer3_callback, cb_grp3); executor.spin(); + ASSERT_GT(timer_count.load(), 0u); } /* @@ -210,13 +204,15 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, add_callback_groups_after_add_node_t */ 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); - rclcpp::executors::MultiThreadedExecutor executor; executor.add_callback_group(cb_grp, node->get_node_base_interface()); ASSERT_EQ(executor.get_all_callback_groups().size(), 1u); @@ -245,14 +241,16 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, add_unallowable_callback_groups) */ 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); - rclcpp::executors::MultiThreadedExecutor timer_executor; - rclcpp::executors::MultiThreadedExecutor sub_executor; timer_executor.add_callback_group(cb_grp, node->get_node_base_interface()); const rclcpp::QoS qos(10); auto options = rclcpp::SubscriptionOptions(); @@ -276,12 +274,127 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, one_node_many_callback_groups_many_e 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) { - rclcpp::executors::MultiThreadedExecutor executor; + 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( diff --git a/rclcpp/test/rclcpp/test_any_service_callback.cpp b/rclcpp/test/rclcpp/test_any_service_callback.cpp index 6a70b8b5da..ef46155d99 100644 --- a/rclcpp/test/rclcpp/test_any_service_callback.cpp +++ b/rclcpp/test/rclcpp/test_any_service_callback.cpp @@ -24,7 +24,6 @@ #include "rclcpp/any_service_callback.hpp" #include "rclcpp/service.hpp" #include "test_msgs/srv/empty.hpp" -#include "test_msgs/srv/empty.h" class TestAnyServiceCallback : public ::testing::Test { @@ -51,8 +50,10 @@ TEST_F(TestAnyServiceCallback, no_set_and_dispatch_throw) { TEST_F(TestAnyServiceCallback, set_and_dispatch_no_header) { int callback_calls = 0; - auto callback = [&callback_calls](const std::shared_ptr, - std::shared_ptr) { + auto callback = [&callback_calls]( + const std::shared_ptr, + std::shared_ptr) + { callback_calls++; }; @@ -65,10 +66,11 @@ TEST_F(TestAnyServiceCallback, set_and_dispatch_no_header) { 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) { + auto callback_with_header = [&callback_with_header_calls]( + const std::shared_ptr, + const std::shared_ptr, + std::shared_ptr) + { callback_with_header_calls++; }; @@ -80,9 +82,9 @@ TEST_F(TestAnyServiceCallback, set_and_dispatch_header) { 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) { + auto callback_with_header = [&callback_with_header_calls]( + const std::shared_ptr, const std::shared_ptr) + { callback_with_header_calls++; }; @@ -94,10 +96,10 @@ TEST_F(TestAnyServiceCallback, set_and_dispatch_defered) { 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) + auto callback_with_header = [&callback_with_header_calls]( + std::shared_ptr>, + const std::shared_ptr, + const std::shared_ptr) { callback_with_header_calls++; }; diff --git a/rclcpp/test/rclcpp/test_any_subscription_callback.cpp b/rclcpp/test/rclcpp/test_any_subscription_callback.cpp index 8e44eb71bf..58a9211937 100644 --- a/rclcpp/test/rclcpp/test_any_subscription_callback.cpp +++ b/rclcpp/test/rclcpp/test_any_subscription_callback.cpp @@ -21,7 +21,29 @@ #include "rclcpp/any_subscription_callback.hpp" #include "test_msgs/msg/empty.hpp" -#include "test_msgs/msg/empty.h" + +// 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 { @@ -41,34 +63,23 @@ class TestAnySubscriptionCallback : public ::testing::Test rclcpp::MessageInfo message_info_; }; -void construct_with_null_allocator() +class TestAnySubscriptionCallbackTA : public ::testing::Test { -// suppress deprecated function warning -#if !defined(_WIN32) -# pragma GCC diagnostic push -# pragma GCC diagnostic ignored "-Wdeprecated-declarations" -#else // !defined(_WIN32) -# pragma warning(push) -# pragma warning(disable: 4996) -#endif - - // We need to wrap this in a function because `EXPECT_THROW` is a macro, and thinks - // that the comma in here splits macro arguments, not the template arguments. - rclcpp::AnySubscriptionCallback any_subscription_callback(nullptr); - -// remove warning suppression -#if !defined(_WIN32) -# pragma GCC diagnostic pop -#else // !defined(_WIN32) -# pragma warning(pop) -#endif -} +public: + TestAnySubscriptionCallbackTA() {} -TEST(AnySubscriptionCallback, null_allocator) { - EXPECT_THROW( - construct_with_null_allocator(), - std::invalid_argument); -} + 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. @@ -79,6 +90,111 @@ TEST_F(TestAnySubscriptionCallback, construct_destruct) { 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_), @@ -95,29 +211,6 @@ TEST_F(TestAnySubscriptionCallback, unset_dispatch_throw) { // Parameterized test to test across all callback types and dispatch types. // -// 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; - template class InstanceContextImpl { @@ -175,7 +268,7 @@ class DispatchTests {}; class DispatchTestsWithTA - : public TestAnySubscriptionCallback, + : public TestAnySubscriptionCallbackTA, public ::testing::WithParamInterface> {}; @@ -191,27 +284,35 @@ format_parameter_with_ta(const ::testing::TestParamInfo as input */ \ - TEST_P(DispatchTests_name, 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_name, 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_name, 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(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_); +} -PARAMETERIZED_TESTS(DispatchTests) -PARAMETERIZED_TESTS(DispatchTestsWithTA) +/* 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 diff --git a/rclcpp/test/rclcpp/test_client.cpp b/rclcpp/test/rclcpp/test_client.cpp index ea64340f03..506d981dd6 100644 --- a/rclcpp/test/rclcpp/test_client.cpp +++ b/rclcpp/test/rclcpp/test_client.cpp @@ -89,7 +89,10 @@ TEST_F(TestClient, construction_and_destruction) { { auto client = node->create_client("service"); } - + { + auto client = node->create_client( + "service", rclcpp::ServicesQoS()); + } { ASSERT_THROW( { @@ -105,7 +108,7 @@ TEST_F(TestClient, construction_with_free_function) { node->get_node_graph_interface(), node->get_node_services_interface(), "service", - rmw_qos_profile_services_default, + rclcpp::ServicesQoS(), nullptr); } { @@ -116,7 +119,7 @@ TEST_F(TestClient, construction_with_free_function) { node->get_node_graph_interface(), node->get_node_services_interface(), "invalid_?service", - rmw_qos_profile_services_default, + rclcpp::ServicesQoS(), nullptr); }, rclcpp::exceptions::InvalidServiceNameError); } @@ -171,160 +174,3 @@ TEST_F(TestClientSub, construction_and_destruction) { }, rclcpp::exceptions::InvalidServiceNameError); } } - -class TestClientWithServer : public ::testing::Test -{ -protected: - static void SetUpTestCase() - { - rclcpp::init(0, nullptr); - } - - static void TearDownTestCase() - { - rclcpp::shutdown(); - } - - void SetUp() - { - node = std::make_shared("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)); - } - - ::testing::AssertionResult SendEmptyRequestAndWait( - 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 future = 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"; - } - - return request_result; - } - - std::shared_ptr node; - std::shared_ptr> service; - const std::string service_name{"empty_service"}; -}; - -TEST_F(TestClientWithServer, async_send_request) { - EXPECT_TRUE(SendEmptyRequestAndWait()); -} - -TEST_F(TestClientWithServer, async_send_request_callback_with_request) { - 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 future = 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); -} - -TEST_F(TestClientWithServer, async_send_request_rcl_send_request_error) { - // Checking rcl_send_request in rclcpp::Client::async_send_request() - auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_send_request, RCL_RET_ERROR); - EXPECT_THROW(SendEmptyRequestAndWait(), rclcpp::exceptions::RCLError); -} - -TEST_F(TestClientWithServer, async_send_request_rcl_service_server_is_available_error) { - { - // Checking rcl_service_server_is_available in rclcpp::ClientBase::service_is_ready - auto client = node->create_client(service_name); - 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 = node->create_client(service_name); - 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 = node->create_client(service_name); - 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); - } -} - -TEST_F(TestClientWithServer, take_response) { - auto client = node->create_client(service_name); - 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; - - client->async_send_request(request); - EXPECT_FALSE(client->take_response(response, *request_header.get())); - - { - // 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(client->take_response(response, *request_header.get())); - } - { - // 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(client->take_response(response, *request_header.get())); - } - { - // 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( - client->take_response(response, *request_header.get()), - rclcpp::exceptions::RCLError); - } -} 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 index fd947485e2..06536116a1 100644 --- a/rclcpp/test/rclcpp/test_create_subscription.cpp +++ b/rclcpp/test/rclcpp/test_create_subscription.cpp @@ -20,7 +20,6 @@ #include "rclcpp/create_subscription.hpp" #include "rclcpp/node.hpp" #include "test_msgs/msg/empty.hpp" -#include "test_msgs/msg/empty.h" using namespace std::chrono_literals; diff --git a/rclcpp/test/rclcpp/test_create_timer.cpp b/rclcpp/test/rclcpp/test_create_timer.cpp index f6acd79b24..c4dc1f7e61 100644 --- a/rclcpp/test/rclcpp/test_create_timer.cpp +++ b/rclcpp/test/rclcpp/test_create_timer.cpp @@ -62,7 +62,7 @@ TEST(TestCreateTimer, call_with_node_wrapper_compiles) rclcpp::shutdown(); } -TEST(TestCreateTimer, call_wall_timer_with_bad_arguments) +TEST(TestCreateWallTimer, call_wall_timer_with_bad_arguments) { rclcpp::init(0, nullptr); NodeWrapper node("test_create_wall_timers_with_bad_arguments"); @@ -116,3 +116,106 @@ TEST(TestCreateTimer, call_wall_timer_with_bad_arguments) 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 index 319ce06601..2347514d7a 100644 --- a/rclcpp/test/rclcpp/test_duration.cpp +++ b/rclcpp/test/rclcpp/test_duration.cpp @@ -52,9 +52,23 @@ TEST_F(TestDuration, operators) { 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); @@ -65,6 +79,35 @@ TEST_F(TestDuration, operators) { 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); diff --git a/rclcpp/test/rclcpp/test_executor.cpp b/rclcpp/test/rclcpp/test_executor.cpp index fce0369edd..21a19cee18 100644 --- a/rclcpp/test/rclcpp/test_executor.cpp +++ b/rclcpp/test/rclcpp/test_executor.cpp @@ -46,23 +46,6 @@ class DummyExecutor : public rclcpp::Executor { spin_node_once_nanoseconds(node, std::chrono::milliseconds(100)); } - - rclcpp::memory_strategy::MemoryStrategy * memory_strategy_ptr() - { - return memory_strategy_.get(); - } - - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr local_get_node_by_group( - rclcpp::CallbackGroup::SharedPtr group) - { - std::lock_guard guard_{mutex_}; // only to make the TSA happy - return get_node_by_group(weak_groups_to_nodes_, group); - } - - rclcpp::CallbackGroup::SharedPtr local_get_group_by_timer(rclcpp::TimerBase::SharedPtr timer) - { - return get_group_by_timer(timer); - } }; class TestExecutor : public ::testing::Test @@ -130,7 +113,7 @@ 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 in Executor constructor: error not set")); + std::runtime_error("Failed to create wait set: error not set")); } TEST_F(TestExecutor, add_callback_group_twice) { @@ -142,7 +125,7 @@ TEST_F(TestExecutor, add_callback_group_twice) { 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 was already added to executor.")); + std::runtime_error("Callback group has already been added to this executor.")); } TEST_F(TestExecutor, add_callback_group_failed_trigger_guard_condition) { @@ -155,7 +138,7 @@ TEST_F(TestExecutor, add_callback_group_failed_trigger_guard_condition) { "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 trigger guard condition on callback group add: error not set")); + std::runtime_error("Failed to handle entities update on callback group add: error not set")); } TEST_F(TestExecutor, remove_callback_group_null_node) { @@ -168,9 +151,15 @@ TEST_F(TestExecutor, remove_callback_group_null_node) { 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) { @@ -186,7 +175,7 @@ TEST_F(TestExecutor, remove_callback_group_failed_trigger_guard_condition) { RCLCPP_EXPECT_THROW_EQ( dummy.remove_callback_group(cb_group, true), std::runtime_error( - "Failed to trigger guard condition on callback group remove: error not set")); + "Failed to handle entities update on callback group remove: error not set")); } TEST_F(TestExecutor, remove_node_not_associated) { @@ -197,7 +186,7 @@ TEST_F(TestExecutor, remove_node_not_associated) { RCLCPP_EXPECT_THROW_EQ( dummy.remove_node(node->get_node_base_interface(), false), - std::runtime_error("Node needs to be associated with an executor.")); + std::runtime_error("Node '/ns/node' needs to be associated with an executor.")); } TEST_F(TestExecutor, remove_node_associated_with_different_executor) { @@ -211,7 +200,7 @@ TEST_F(TestExecutor, remove_node_associated_with_different_executor) { RCLCPP_EXPECT_THROW_EQ( dummy2.remove_node(node1->get_node_base_interface(), false), - std::runtime_error("Node needs to be associated with this executor.")); + std::runtime_error("Node '/ns/node1' needs to be associated with this executor.")); } TEST_F(TestExecutor, spin_node_once_nanoseconds) { @@ -248,7 +237,7 @@ TEST_F(TestExecutor, spin_all_invalid_duration) { RCLCPP_EXPECT_THROW_EQ( dummy.spin_all(std::chrono::nanoseconds(-1)), - std::invalid_argument("max_duration must be positive")); + std::invalid_argument("max_duration must be greater than or equal to 0")); } TEST_F(TestExecutor, spin_some_in_spin_some) { @@ -328,42 +317,14 @@ TEST_F(TestExecutor, cancel_failed_trigger_guard_condition) { std::runtime_error("Failed to trigger guard condition in cancel: error not set")); } -TEST_F(TestExecutor, set_memory_strategy_nullptr) { - DummyExecutor dummy; - - RCLCPP_EXPECT_THROW_EQ( - dummy.set_memory_strategy(nullptr), - std::runtime_error("Received NULL memory strategy in executor.")); -} - -TEST_F(TestExecutor, set_memory_strategy) { - DummyExecutor dummy; - rclcpp::memory_strategy::MemoryStrategy::SharedPtr strategy = - std::make_shared< - rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy<>>(); - - dummy.set_memory_strategy(strategy); - EXPECT_EQ(dummy.memory_strategy_ptr(), strategy.get()); -} - -TEST_F(TestExecutor, spin_once_failed_trigger_guard_condition) { - DummyExecutor dummy; - auto node = std::make_shared("node", "ns"); - auto timer = - node->create_wall_timer(std::chrono::milliseconds(1), [&]() {}); - - dummy.add_node(node); - // Wait for the wall timer to have expired. - std::this_thread::sleep_for(std::chrono::milliseconds(50)); - auto mock = mocking_utils::patch_and_return( - "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); +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( - dummy.spin_once(std::chrono::milliseconds(1)), - std::runtime_error( - "Failed to trigger guard condition from execute_any_executable: error not set")); + DummyExecutor dummy, + std::runtime_error("Couldn't clear the wait set: error not set")); } -TEST_F(TestExecutor, spin_some_fail_wait_set_clear) { +TEST_F(TestExecutor, spin_all_fail_wait_set_clear) { DummyExecutor dummy; auto node = std::make_shared("node", "ns"); auto timer = @@ -371,9 +332,10 @@ TEST_F(TestExecutor, spin_some_fail_wait_set_clear) { 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_some(std::chrono::milliseconds(1)), - std::runtime_error("Couldn't clear wait set: error not set")); + 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) { @@ -401,7 +363,7 @@ TEST_F(TestExecutor, spin_some_fail_add_handles_to_wait_set) { RCL_RET_ERROR); RCLCPP_EXPECT_THROW_EQ( dummy.spin_some(std::chrono::milliseconds(1)), - std::runtime_error("Couldn't fill wait set")); + std::runtime_error("Couldn't fill wait set: error not set")); } TEST_F(TestExecutor, spin_some_fail_wait) { @@ -417,71 +379,6 @@ TEST_F(TestExecutor, spin_some_fail_wait) { std::runtime_error("rcl_wait() failed: error not set")); } -TEST_F(TestExecutor, get_node_by_group_null_group) { - DummyExecutor dummy; - ASSERT_EQ(nullptr, dummy.local_get_node_by_group(nullptr)); -} - -TEST_F(TestExecutor, get_node_by_group) { - 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); - ASSERT_EQ(node->get_node_base_interface().get(), dummy.local_get_node_by_group(cb_group).get()); -} - -TEST_F(TestExecutor, get_node_by_group_not_found) { - DummyExecutor dummy; - auto node = std::make_shared("node", "ns"); - rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive); - ASSERT_EQ(nullptr, dummy.local_get_node_by_group(cb_group).get()); -} - -TEST_F(TestExecutor, get_group_by_timer_nullptr) { - DummyExecutor dummy; - ASSERT_EQ(nullptr, dummy.local_get_group_by_timer(nullptr)); -} - -TEST_F(TestExecutor, get_group_by_timer) { - DummyExecutor dummy; - auto node = std::make_shared("node", "ns"); - rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive); - auto timer = - node->create_wall_timer(std::chrono::milliseconds(1), [&]() {}, cb_group); - dummy.add_node(node); - - ASSERT_EQ(cb_group.get(), dummy.local_get_group_by_timer(timer).get()); -} - -TEST_F(TestExecutor, get_group_by_timer_with_deleted_group) { - DummyExecutor dummy; - auto node = std::make_shared("node", "ns"); - rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive); - auto timer = - node->create_wall_timer(std::chrono::milliseconds(1), [&]() {}, cb_group); - dummy.add_node(node); - - cb_group.reset(); - - ASSERT_EQ(nullptr, dummy.local_get_group_by_timer(timer).get()); -} - -TEST_F(TestExecutor, get_group_by_timer_add_callback_group) { - DummyExecutor dummy; - auto node = std::make_shared("node", "ns"); - rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive); - auto timer = - node->create_wall_timer(std::chrono::milliseconds(1), [&]() {}, cb_group); - dummy.add_callback_group(cb_group, node->get_node_base_interface(), false); - - ASSERT_EQ(cb_group.get(), dummy.local_get_group_by_timer(timer).get()); -} - TEST_F(TestExecutor, spin_until_future_complete_in_spin_until_future_complete) { DummyExecutor dummy; auto node = std::make_shared("node", "ns"); @@ -546,6 +443,40 @@ TEST_F(TestExecutor, 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"); @@ -556,3 +487,24 @@ TEST_F(TestExecutor, spin_until_future_complete_future_already_complete) { 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_externally_defined_services.cpp b/rclcpp/test/rclcpp/test_externally_defined_services.cpp index 563f7f6ec3..21a7d11142 100644 --- a/rclcpp/test/rclcpp/test_externally_defined_services.cpp +++ b/rclcpp/test/rclcpp/test_externally_defined_services.cpp @@ -34,6 +34,11 @@ class TestExternallyDefinedServices : public ::testing::Test { rclcpp::init(0, nullptr); } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } }; void diff --git a/rclcpp/test/rclcpp/test_find_weak_nodes.cpp b/rclcpp/test/rclcpp/test_find_weak_nodes.cpp index d402f02a11..739a247b39 100644 --- a/rclcpp/test/rclcpp/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) { diff --git a/rclcpp/test/rclcpp/test_function_traits.cpp b/rclcpp/test/rclcpp/test_function_traits.cpp index a9e6e8d261..20c9521c4e 100644 --- a/rclcpp/test/rclcpp/test_function_traits.cpp +++ b/rclcpp/test/rclcpp/test_function_traits.cpp @@ -393,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< @@ -402,6 +403,7 @@ TEST(TestFunctionTraits, argument_types) { 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< @@ -413,6 +415,7 @@ TEST(TestFunctionTraits, argument_types) { 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< @@ -429,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< @@ -447,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< @@ -455,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< @@ -470,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< @@ -530,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, @@ -572,6 +582,7 @@ 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( @@ -580,6 +591,7 @@ TEST(TestFunctionTraits, check_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( @@ -745,6 +757,7 @@ 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 + +#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 index 71a60a77fb..79fbfcc33a 100644 --- a/rclcpp/test/rclcpp/test_generic_pubsub.cpp +++ b/rclcpp/test/rclcpp/test_generic_pubsub.cpp @@ -61,34 +61,37 @@ class RclcppGenericNodeFixture : public Test publishers_.push_back(publisher); } - std::vector subscribe_raw_messages( - size_t expected_messages_number, const std::string & topic_name, const std::string & type) + template + std::vector subscribe_raw_messages( + size_t expected_recv_msg_count, const std::string & topic_name, const std::string & type) { - std::vector messages; + std::vector messages; size_t counter = 0; auto subscription = node_->create_generic_subscription( topic_name, type, rclcpp::QoS(1), - [&counter, &messages](std::shared_ptr message) { - test_msgs::msg::Strings string_message; - rclcpp::Serialization serializer; - serializer.deserialize_message(message.get(), &string_message); - messages.push_back(string_message.string_value); + [&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_messages_number) { + while (counter < expected_recv_msg_count) { rclcpp::spin_some(node_); } return messages; } - rclcpp::SerializedMessage serialize_string_message(const std::string & message) + template + rclcpp::SerializedMessage serialize_message(const T1 & data) { - test_msgs::msg::Strings string_message; - string_message.string_value = message; - rclcpp::Serialization ser; + T2 message; + write_message(data, message); + + rclcpp::Serialization ser; SerializedMessage result; - ser.serialize_message(&string_message, &result); + ser.serialize_message(&message, &result); return result; } @@ -115,6 +118,27 @@ class RclcppGenericNodeFixture : public Test 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; + } }; @@ -130,7 +154,7 @@ TEST_F(RclcppGenericNodeFixture, publisher_and_subscriber_work) auto subscriber_future_ = std::async( std::launch::async, [this, topic_name, type] { - return subscribe_raw_messages(1, topic_name, type); + return subscribe_raw_messages(1, topic_name, type); }); // TODO(karsten1987): Port 'wait_for_sub' to rclcpp @@ -147,7 +171,7 @@ TEST_F(RclcppGenericNodeFixture, publisher_and_subscriber_work) ASSERT_TRUE(success); for (const auto & message : test_messages) { - publisher->publish(serialize_string_message(message)); + publisher->publish(serialize_message(message)); } auto subscribed_messages = subscriber_future_.get(); @@ -155,6 +179,51 @@ TEST_F(RclcppGenericNodeFixture, publisher_and_subscriber_work) 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, @@ -167,7 +236,7 @@ TEST_F(RclcppGenericNodeFixture, generic_subscription_uses_qos) auto publisher = node_->create_publisher(topic_name, qos); auto subscription = node_->create_generic_subscription( topic_name, topic_type, qos, - [](std::shared_ptr/* message */) {}); + [](std::shared_ptr/* message */) {}); auto connected = [publisher, subscription]() -> bool { return publisher->get_subscription_count() && subscription->get_publisher_count(); }; @@ -187,10 +256,56 @@ TEST_F(RclcppGenericNodeFixture, generic_publisher_uses_qos) auto publisher = node_->create_generic_publisher(topic_name, topic_type, qos); auto subscription = node_->create_subscription( topic_name, qos, - [](std::shared_ptr/* message */) {}); + [](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 index bd66c686d7..24c370e65d 100644 --- a/rclcpp/test/rclcpp/test_graph_listener.cpp +++ b/rclcpp/test/rclcpp/test_graph_listener.cpp @@ -87,7 +87,7 @@ TEST_F(TestGraphListener, error_construct_graph_listener) { auto graph_listener_error = std::make_shared(get_global_default_context()); graph_listener_error.reset(); - }, std::runtime_error("failed to create interrupt guard condition: error not set")); + }, std::runtime_error("failed to create guard condition: error not set")); } // Required for mocking_utils below @@ -169,7 +169,7 @@ TEST_F(TestGraphListener, error_run_graph_listener_mock_wait_set_add_guard_condi "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 interrupt guard condition to wait set: error not set")); + std::runtime_error("failed to add guard condition to wait set: error not set")); } TEST_F(TestGraphListener, error_run_graph_listener_mock_wait_error) { @@ -292,9 +292,7 @@ TEST_F(TestGraphListener, test_graph_listener_shutdown_guard_fini_error_throw) { auto mock_wait_set_fini = mocking_utils::patch_and_return( "lib:rclcpp", rcl_guard_condition_fini, RCL_RET_ERROR); - RCLCPP_EXPECT_THROW_EQ( - graph_listener_test->shutdown(), - std::runtime_error("failed to finalize interrupt guard condition: error not set")); + 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 index 8100cf1c9b..c9fc138ccc 100644 --- a/rclcpp/test/rclcpp/test_guard_condition.cpp +++ b/rclcpp/test/rclcpp/test_guard_condition.cpp @@ -27,6 +27,11 @@ class TestGuardCondition : public ::testing::Test { rclcpp::init(0, nullptr); } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } }; /* @@ -66,16 +71,6 @@ TEST_F(TestGuardCondition, construction_and_destruction) { } } -/* - * Testing context accessor. - */ -TEST_F(TestGuardCondition, get_context) { - { - auto gc = std::make_shared(); - gc->get_context(); - } -} - /* * Testing rcl guard condition accessor. */ @@ -102,3 +97,75 @@ TEST_F(TestGuardCondition, trigger) { } } } + +/* + * 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_intra_process_buffer.cpp b/rclcpp/test/rclcpp/test_intra_process_buffer.cpp index 16c457c96f..fa427de74b 100644 --- a/rclcpp/test/rclcpp/test_intra_process_buffer.cpp +++ b/rclcpp/test/rclcpp/test_intra_process_buffer.cpp @@ -190,6 +190,33 @@ TEST(TestIntraProcessBuffer, shared_buffer_consume) { 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())); } /* @@ -237,4 +264,103 @@ TEST(TestIntraProcessBuffer, unique_buffer_consume) { 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 index 59437aa560..6a4bfe56d8 100644 --- a/rclcpp/test/rclcpp/test_intra_process_manager.cpp +++ b/rclcpp/test/rclcpp/test_intra_process_manager.cpp @@ -22,6 +22,7 @@ #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" @@ -30,6 +31,112 @@ // 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 @@ -52,9 +159,9 @@ class PublisherBase public: RCLCPP_SMART_PTR_DEFINITIONS(PublisherBase) - explicit PublisherBase(rclcpp::QoS qos = rclcpp::QoS(10)) - : qos_profile(qos), - topic_name("topic") + explicit PublisherBase(const std::string & topic, const rclcpp::QoS & qos) + : topic_name(topic), + qos_profile(qos) {} virtual ~PublisherBase() @@ -79,6 +186,12 @@ class PublisherBase return qos_profile; } + bool + is_durability_transient_local() const + { + return qos_profile.durability() == rclcpp::DurabilityPolicy::TransientLocal; + } + bool operator==(const rmw_gid_t & gid) const { @@ -93,10 +206,12 @@ class PublisherBase return false; } - rclcpp::QoS qos_profile; - std::string topic_name; uint64_t intra_process_publisher_id_; IntraProcessManagerWeakPtr weak_ipm_; + +private: + std::string topic_name; + rclcpp::QoS qos_profile; }; template> @@ -111,11 +226,14 @@ class Publisher : public PublisherBase RCLCPP_SMART_PTR_DEFINITIONS(Publisher) - explicit Publisher(rclcpp::QoS qos = rclcpp::QoS(10)) - : PublisherBase(qos) + 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 @@ -123,65 +241,12 @@ class Publisher : public PublisherBase 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 buffers -{ -namespace mock -{ -template< - typename MessageT, - typename Alloc = std::allocator, - typename MessageDeleter = std::default_delete> -class IntraProcessBuffer -{ -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; - } - - void add(MessageUniquePtr msg) - { - message_ptr = reinterpret_cast(msg.get()); - unique_msg = std::move(msg); - } - - void pop(std::uintptr_t & msg_ptr) - { - msg_ptr = message_ptr; - message_ptr = 0; - } - - // need to store the messages somewhere otherwise the memory address will be reused - ConstMessageSharedPtr shared_msg; - MessageUniquePtr unique_msg; - - std::uintptr_t message_ptr; -}; - -} // namespace mock -} // namespace buffers -} // namespace experimental -} // namespace rclcpp - - namespace rclcpp { namespace experimental @@ -194,9 +259,14 @@ class SubscriptionIntraProcessBase public: RCLCPP_SMART_PTR_ALIASES_ONLY(SubscriptionIntraProcessBase) - explicit SubscriptionIntraProcessBase(rclcpp::QoS qos = rclcpp::QoS(10)) - : qos_profile(qos), topic_name("topic") - {} + explicit SubscriptionIntraProcessBase( + rclcpp::Context::SharedPtr context, + const std::string & topic, + const rclcpp::QoS & qos) + : topic_name(topic), qos_profile(qos) + { + (void)context; + } virtual ~SubscriptionIntraProcessBase() {} @@ -212,24 +282,36 @@ class SubscriptionIntraProcessBase const char * get_topic_name() { - return 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; - const char * topic_name; }; template< typename MessageT, typename Alloc = std::allocator, - typename Deleter = std::default_delete> + typename Deleter = std::default_delete, + typename ROSMessageType = MessageT +> class SubscriptionIntraProcessBuffer : public SubscriptionIntraProcessBase { public: RCLCPP_SMART_PTR_DEFINITIONS(SubscriptionIntraProcessBuffer) - explicit SubscriptionIntraProcessBuffer(rclcpp::QoS qos) - : SubscriptionIntraProcessBase(qos), take_shared_method(false) + explicit SubscriptionIntraProcessBuffer(const std::string & topic, const rclcpp::QoS & qos) + : SubscriptionIntraProcessBase(nullptr, topic, qos), take_shared_method(false) { buffer = std::make_unique>(); } @@ -246,6 +328,18 @@ class SubscriptionIntraProcessBuffer : public SubscriptionIntraProcessBase 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() { @@ -255,11 +349,17 @@ class SubscriptionIntraProcessBuffer : public SubscriptionIntraProcessBase } bool - use_take_shared_method() const + 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; @@ -267,7 +367,7 @@ class SubscriptionIntraProcessBuffer : public SubscriptionIntraProcessBase template< typename MessageT, - typename Alloc = std::allocator, + typename Alloc = std::allocator, typename Deleter = std::default_delete> class SubscriptionIntraProcess : public SubscriptionIntraProcessBuffer< MessageT, @@ -278,8 +378,8 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBuffer< public: RCLCPP_SMART_PTR_DEFINITIONS(SubscriptionIntraProcess) - explicit SubscriptionIntraProcess(rclcpp::QoS qos = rclcpp::QoS(10)) - : SubscriptionIntraProcessBuffer(qos) + explicit SubscriptionIntraProcess(const std::string & topic, const rclcpp::QoS & qos) + : SubscriptionIntraProcessBuffer(topic, qos) { } }; @@ -291,17 +391,19 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBuffer< // 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" +#include "../src/rclcpp/intra_process_manager.cpp" // NOLINT #undef Publisher #undef PublisherBase #undef IntraProcessBuffer @@ -328,28 +430,36 @@ void Publisher::publish(MessageUniquePtr msg) throw std::runtime_error("cannot publish msg which is a null pointer"); } - ipm->template do_intra_process_publish( - intra_process_publisher_id_, - std::move(msg), - *message_allocator_); + 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. + * 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; @@ -359,38 +469,55 @@ TEST(TestIntraProcessManager, add_pub_sub) { auto ipm = std::make_shared(); - auto p1 = std::make_shared(rclcpp::QoS(10).best_effort()); + auto p1 = std::make_shared("topic", rclcpp::QoS(10).best_effort()); - auto p2 = std::make_shared(rclcpp::QoS(10).best_effort()); - p2->topic_name = "different_topic_name"; + auto p2 = std::make_shared("different_topic_name", rclcpp::QoS(10).best_effort()); - auto s1 = std::make_shared(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->add_subscription(s1); + 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(rclcpp::QoS(10).reliable()); + auto p3 = std::make_shared("topic", rclcpp::QoS(10).reliable()); - auto s2 = std::make_shared(rclcpp::QoS(10).reliable()); + auto s2 = std::make_shared("topic", rclcpp::QoS(10).reliable()); - auto s2_id = ipm->add_subscription(s2); + // 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); - ASSERT_EQ(1u, p1_subs); + + 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); @@ -404,14 +531,14 @@ TEST(TestIntraProcessManager, add_pub_sub) { } /* - 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. + * 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; @@ -421,13 +548,13 @@ TEST(TestIntraProcessManager, single_subscription) { auto ipm = std::make_shared(); - auto p1 = 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(); + auto s1 = std::make_shared("topic", rclcpp::QoS(10)); s1->take_shared_method = false; - auto s1_id = ipm->add_subscription(s1); + auto s1_id = ipm->template add_subscription(s1); auto unique_msg = std::make_unique(); auto original_message_pointer = reinterpret_cast(unique_msg.get()); @@ -436,9 +563,9 @@ TEST(TestIntraProcessManager, single_subscription) { ASSERT_EQ(original_message_pointer, received_message_pointer_1); ipm->remove_subscription(s1_id); - auto s2 = std::make_shared(); + auto s2 = std::make_shared("topic", rclcpp::QoS(10)); s2->take_shared_method = true; - auto s2_id = ipm->add_subscription(s2); + auto s2_id = ipm->template add_subscription(s2); (void)s2_id; unique_msg = std::make_unique(); @@ -457,15 +584,15 @@ TEST(TestIntraProcessManager, single_subscription) { } /* - 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. + * 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; @@ -475,17 +602,17 @@ TEST(TestIntraProcessManager, multiple_subscriptions_same_type) { auto ipm = std::make_shared(); - auto p1 = 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(); + auto s1 = std::make_shared("topic", rclcpp::QoS(10)); s1->take_shared_method = false; - auto s1_id = ipm->add_subscription(s1); + auto s1_id = ipm->template add_subscription(s1); - auto s2 = std::make_shared(); + auto s2 = std::make_shared("topic", rclcpp::QoS(10)); s2->take_shared_method = false; - auto s2_id = ipm->add_subscription(s2); + auto s2_id = ipm->template add_subscription(s2); auto unique_msg = std::make_unique(); auto original_message_pointer = reinterpret_cast(unique_msg.get()); @@ -499,13 +626,13 @@ TEST(TestIntraProcessManager, multiple_subscriptions_same_type) { ipm->remove_subscription(s1_id); ipm->remove_subscription(s2_id); - auto s3 = std::make_shared(); + auto s3 = std::make_shared("topic", rclcpp::QoS(10)); s3->take_shared_method = true; - auto s3_id = ipm->add_subscription(s3); + auto s3_id = ipm->template add_subscription(s3); - auto s4 = std::make_shared(); + auto s4 = std::make_shared("topic", rclcpp::QoS(10)); s4->take_shared_method = true; - auto s4_id = ipm->add_subscription(s4); + auto s4_id = ipm->template add_subscription(s4); unique_msg = std::make_unique(); original_message_pointer = reinterpret_cast(unique_msg.get()); @@ -518,13 +645,13 @@ TEST(TestIntraProcessManager, multiple_subscriptions_same_type) { ipm->remove_subscription(s3_id); ipm->remove_subscription(s4_id); - auto s5 = std::make_shared(); + auto s5 = std::make_shared("topic", rclcpp::QoS(10)); s5->take_shared_method = false; - auto s5_id = ipm->add_subscription(s5); + auto s5_id = ipm->template add_subscription(s5); - auto s6 = std::make_shared(); + auto s6 = std::make_shared("topic", rclcpp::QoS(10)); s6->take_shared_method = false; - auto s6_id = ipm->add_subscription(s6); + auto s6_id = ipm->template add_subscription(s6); unique_msg = std::make_unique(); original_message_pointer = reinterpret_cast(unique_msg.get()); @@ -538,14 +665,14 @@ TEST(TestIntraProcessManager, multiple_subscriptions_same_type) { ipm->remove_subscription(s5_id); ipm->remove_subscription(s6_id); - auto s7 = std::make_shared(); + auto s7 = std::make_shared("topic", rclcpp::QoS(10)); s7->take_shared_method = true; - auto s7_id = ipm->add_subscription(s7); + auto s7_id = ipm->template add_subscription(s7); (void)s7_id; - auto s8 = std::make_shared(); + auto s8 = std::make_shared("topic", rclcpp::QoS(10)); s8->take_shared_method = true; - auto s8_id = ipm->add_subscription(s8); + auto s8_id = ipm->template add_subscription(s8); (void)s8_id; unique_msg = std::make_unique(); @@ -558,20 +685,20 @@ TEST(TestIntraProcessManager, multiple_subscriptions_same_type) { } /* - 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 + * 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; @@ -581,17 +708,17 @@ TEST(TestIntraProcessManager, multiple_subscriptions_different_type) { auto ipm = std::make_shared(); - auto p1 = 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(); + auto s1 = std::make_shared("topic", rclcpp::QoS(10)); s1->take_shared_method = true; - auto s1_id = ipm->add_subscription(s1); + auto s1_id = ipm->template add_subscription(s1); - auto s2 = std::make_shared(); + auto s2 = std::make_shared("topic", rclcpp::QoS(10)); s2->take_shared_method = false; - auto s2_id = ipm->add_subscription(s2); + auto s2_id = ipm->template add_subscription(s2); auto unique_msg = std::make_unique(); auto original_message_pointer = reinterpret_cast(unique_msg.get()); @@ -604,17 +731,17 @@ TEST(TestIntraProcessManager, multiple_subscriptions_different_type) { ipm->remove_subscription(s1_id); ipm->remove_subscription(s2_id); - auto s3 = std::make_shared(); + auto s3 = std::make_shared("topic", rclcpp::QoS(10)); s3->take_shared_method = false; - auto s3_id = ipm->add_subscription(s3); + auto s3_id = ipm->template add_subscription(s3); - auto s4 = std::make_shared(); + auto s4 = std::make_shared("topic", rclcpp::QoS(10)); s4->take_shared_method = false; - auto s4_id = ipm->add_subscription(s4); + auto s4_id = ipm->template add_subscription(s4); - auto s5 = std::make_shared(); + auto s5 = std::make_shared("topic", rclcpp::QoS(10)); s5->take_shared_method = true; - auto s5_id = ipm->add_subscription(s5); + auto s5_id = ipm->template add_subscription(s5); unique_msg = std::make_unique(); original_message_pointer = reinterpret_cast(unique_msg.get()); @@ -636,21 +763,21 @@ TEST(TestIntraProcessManager, multiple_subscriptions_different_type) { ipm->remove_subscription(s4_id); ipm->remove_subscription(s5_id); - auto s6 = std::make_shared(); + auto s6 = std::make_shared("topic", rclcpp::QoS(10)); s6->take_shared_method = true; - auto s6_id = ipm->add_subscription(s6); + auto s6_id = ipm->template add_subscription(s6); - auto s7 = std::make_shared(); + auto s7 = std::make_shared("topic", rclcpp::QoS(10)); s7->take_shared_method = true; - auto s7_id = ipm->add_subscription(s7); + auto s7_id = ipm->template add_subscription(s7); - auto s8 = std::make_shared(); + auto s8 = std::make_shared("topic", rclcpp::QoS(10)); s8->take_shared_method = false; - auto s8_id = ipm->add_subscription(s8); + auto s8_id = ipm->template add_subscription(s8); - auto s9 = std::make_shared(); + auto s9 = std::make_shared("topic", rclcpp::QoS(10)); s9->take_shared_method = false; - auto s9_id = ipm->add_subscription(s9); + auto s9_id = ipm->template add_subscription(s9); unique_msg = std::make_unique(); original_message_pointer = reinterpret_cast(unique_msg.get()); @@ -674,14 +801,14 @@ TEST(TestIntraProcessManager, multiple_subscriptions_different_type) { ipm->remove_subscription(s8_id); ipm->remove_subscription(s9_id); - auto s10 = std::make_shared(); + auto s10 = std::make_shared("topic", rclcpp::QoS(10)); s10->take_shared_method = false; - auto s10_id = ipm->add_subscription(s10); + auto s10_id = ipm->template add_subscription(s10); (void)s10_id; - auto s11 = std::make_shared(); + auto s11 = std::make_shared("topic", rclcpp::QoS(10)); s11->take_shared_method = true; - auto s11_id = ipm->add_subscription(s11); + auto s11_id = ipm->template add_subscription(s11); (void)s11_id; unique_msg = std::make_unique(); @@ -692,3 +819,172 @@ TEST(TestIntraProcessManager, multiple_subscriptions_different_type) { 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_loaned_message.cpp b/rclcpp/test/rclcpp/test_loaned_message.cpp index 01284a27c1..0623210079 100644 --- a/rclcpp/test/rclcpp/test_loaned_message.cpp +++ b/rclcpp/test/rclcpp/test_loaned_message.cpp @@ -14,6 +14,7 @@ #include #include +#include #include "rclcpp/loaned_message.hpp" #include "rclcpp/rclcpp.hpp" @@ -39,53 +40,6 @@ class TestLoanedMessage : public ::testing::Test } }; -TEST_F(TestLoanedMessage, initialize) { - auto node = std::make_shared("loaned_message_test_node"); - auto pub = node->create_publisher("loaned_message_test_topic", 1); - -// suppress deprecated function warning -#if !defined(_WIN32) -# pragma GCC diagnostic push -# pragma GCC diagnostic ignored "-Wdeprecated-declarations" -#else // !defined(_WIN32) -# pragma warning(push) -# pragma warning(disable: 4996) -#endif - - auto pub_allocator = pub->get_allocator(); - -// remove warning suppression -#if !defined(_WIN32) -# pragma GCC diagnostic pop -#else // !defined(_WIN32) -# pragma warning(pop) -#endif - -// suppress deprecated function warning -#if !defined(_WIN32) -# pragma GCC diagnostic push -# pragma GCC diagnostic ignored "-Wdeprecated-declarations" -#else // !defined(_WIN32) -# pragma warning(push) -# pragma warning(disable: 4996) -#endif - - auto loaned_msg = rclcpp::LoanedMessage(pub.get(), pub_allocator); - -// remove warning suppression -#if !defined(_WIN32) -# pragma GCC diagnostic pop -#else // !defined(_WIN32) -# pragma warning(pop) -#endif - - ASSERT_TRUE(loaned_msg.is_valid()); - loaned_msg.get().float32_value = 42.0f; - ASSERT_EQ(42.0f, loaned_msg.get().float32_value); - - SUCCEED(); -} - 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); @@ -175,3 +129,18 @@ TEST_F(TestLoanedMessage, construct_with_loaned_message_publisher) { 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 index 1869b8f478..35118cf2df 100644 --- a/rclcpp/test/rclcpp/test_logger.cpp +++ b/rclcpp/test/rclcpp/test_logger.cpp @@ -14,6 +14,7 @@ #include +#include #include #include @@ -160,21 +161,57 @@ TEST(TestLogger, set_level) { 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_logging_directory(); - auto expected_path = rcpputils::fs::path{"/fake_home_dir"} / ".ros" / "log"; - - // TODO(ivanpauno): Add operator== to rcpputils::fs::path - auto it = path.cbegin(); - auto eit = expected_path.cbegin(); - for (; it != path.cend() && eit != expected_path.cend(); ++it, ++eit) { - EXPECT_EQ(*eit, *it); - } - EXPECT_EQ(it, path.cend()); - EXPECT_EQ(eit, expected_path.cend()); + 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 index 7fce7db39a..8e2214f8e6 100644 --- a/rclcpp/test/rclcpp/test_logging.cpp +++ b/rclcpp/test/rclcpp/test_logging.cpp @@ -229,6 +229,12 @@ TEST_F(TestLoggingMacros, test_throttle) { } } +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"); diff --git a/rclcpp/test/rclcpp/test_memory_strategy.cpp b/rclcpp/test/rclcpp/test_memory_strategy.cpp index f20926bafa..7ceb4e7be4 100644 --- a/rclcpp/test/rclcpp/test_memory_strategy.cpp +++ b/rclcpp/test/rclcpp/test_memory_strategy.cpp @@ -35,10 +35,16 @@ typedef std::map take_data() override {return nullptr;} - void execute(std::shared_ptr & data) override {(void)data;} + 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 @@ -143,9 +149,9 @@ TEST_F(TestMemoryStrategy, get_service_by_handle) { { 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_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::paircreate_service( "service", std::move(service_callback), - rmw_qos_profile_services_default, callback_group); + rclcpp::ServicesQoS(), callback_group); service_handle = service->get_service_handle(); @@ -197,7 +203,7 @@ TEST_F(TestMemoryStrategy, get_client_by_handle) { node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); { auto client = node->create_client( - "service", rmw_qos_profile_services_default, callback_group); + "service", rclcpp::ServicesQoS(), callback_group); client_handle = client->get_client_handle(); weak_groups_to_nodes.insert( @@ -307,6 +313,8 @@ TEST_F(TestMemoryStrategy, get_node_by_group) { 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( @@ -363,8 +371,9 @@ TEST_F(TestMemoryStrategy, get_group_by_subscription) { 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( - nullptr, + callback_group, memory_strategy()->get_group_by_subscription(subscription, weak_groups_to_nodes)); } @@ -386,14 +395,14 @@ TEST_F(TestMemoryStrategy, get_group_by_service) { { 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_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), - rmw_qos_profile_services_default, callback_group); + rclcpp::ServicesQoS(), callback_group); weak_groups_to_nodes.insert( std::pair( @@ -432,7 +441,7 @@ TEST_F(TestMemoryStrategy, get_group_by_client) { node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); client = node->create_client( - "service", rmw_qos_profile_services_default, callback_group); + "service", rclcpp::ServicesQoS(), callback_group); weak_groups_to_nodes.insert( std::pair( diff --git a/rclcpp/test/rclcpp/test_node.cpp b/rclcpp/test/rclcpp/test_node.cpp index 70b207b03e..c956b273be 100644 --- a/rclcpp/test/rclcpp/test_node.cpp +++ b/rclcpp/test/rclcpp/test_node.cpp @@ -15,6 +15,7 @@ #include #include +#include #include #include #include @@ -56,7 +57,7 @@ class TestNode : public ::testing::Test test_resources_path /= "test_node"; } - rcpputils::fs::path test_resources_path{TEST_RESOURCES_DIRECTORY}; + std::filesystem::path test_resources_path{TEST_RESOURCES_DIRECTORY}; }; /* @@ -78,6 +79,7 @@ TEST_F(TestNode, construction_and_destruction) { 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()); } { @@ -95,6 +97,99 @@ TEST_F(TestNode, construction_and_destruction) { (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) { @@ -102,6 +197,7 @@ 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()); } { @@ -116,30 +212,35 @@ 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 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()); } { @@ -278,6 +379,13 @@ TEST_F(TestNode, subnode_construction_and_destruction) { 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) { @@ -677,6 +785,12 @@ TEST_F(TestNode, declare_parameter_with_overrides) { "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{}; @@ -1400,6 +1514,36 @@ TEST_F(TestNode, set_parameter_undeclared_parameters_not_allowed) { 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) { @@ -1437,6 +1581,36 @@ TEST_F(TestNode, set_parameter_undeclared_parameters_allowed) { 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) { @@ -1581,6 +1755,51 @@ TEST_F(TestNode, set_parameters_undeclared_parameters_not_allowed) { 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 @@ -1629,6 +1848,48 @@ TEST_F(TestNode, set_parameters_undeclared_parameters_allowed) { 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) { @@ -1771,6 +2032,48 @@ TEST_F(TestNode, set_parameters_atomically_undeclared_parameters_not_allowed) { 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 @@ -1859,6 +2162,45 @@ TEST_F(TestNode, set_parameters_atomically_undeclared_parameters_allowed) { 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 @@ -2031,6 +2373,33 @@ TEST_F(TestNode, get_parameter_or_undeclared_parameters_allowed) { } } +// 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( @@ -2576,6 +2945,163 @@ TEST_F(TestNode, declare_parameter_with_vector) { } } +// 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) { @@ -2786,6 +3312,9 @@ TEST_F(TestNode, get_entity_names) { 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( @@ -2890,6 +3419,9 @@ TEST_F(TestNode, static_and_dynamic_typing) { 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); @@ -2910,20 +3442,35 @@ TEST_F(TestNode, static_and_dynamic_typing) { "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); { -#ifndef _WIN32 -# pragma GCC diagnostic push -# pragma GCC diagnostic ignored "-Wdeprecated-declarations" -#else -# pragma warning(push) -# pragma warning(disable: 4996) -#endif - auto param = node->declare_parameter("deprecated_way_dynamic_typing"); - EXPECT_EQ(param, rclcpp::ParameterValue{}); -#ifndef _WIN32 -# pragma GCC diagnostic pop -#else -# pragma warning(pop) -#endif + 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 index febef6bb37..c3b0726d66 100644 --- a/rclcpp/test/rclcpp/test_node_global_args.cpp +++ b/rclcpp/test/rclcpp/test_node_global_args.cpp @@ -32,6 +32,11 @@ class TestNodeWithGlobalArgs : public ::testing::Test const int argc = sizeof(args) / sizeof(const char *); rclcpp::init(argc, args); } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } }; TEST_F(TestNodeWithGlobalArgs, local_arguments_before_global) { diff --git a/rclcpp/test/rclcpp/test_node_options.cpp b/rclcpp/test/rclcpp/test_node_options.cpp index 3fd0a0d52b..7abc36f38e 100644 --- a/rclcpp/test/rclcpp/test_node_options.cpp +++ b/rclcpp/test/rclcpp/test_node_options.cpp @@ -200,6 +200,57 @@ TEST(TestNodeOptions, copy) { 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) { @@ -266,6 +317,11 @@ TEST(TestNodeOptions, bool_setters_and_getters) { 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) { @@ -316,3 +372,12 @@ TEST(TestNodeOptions, set_get_allocator) { // 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 index 0d798dc451..a2e0548089 100644 --- a/rclcpp/test/rclcpp/test_parameter.cpp +++ b/rclcpp/test/rclcpp/test_parameter.cpp @@ -31,6 +31,11 @@ class TestParameter : public ::testing::Test { rclcpp::init(0, nullptr); } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } }; TEST_F(TestParameter, construct_destruct) { diff --git a/rclcpp/test/rclcpp/test_parameter_client.cpp b/rclcpp/test/rclcpp/test_parameter_client.cpp index 4cd9e671be..a709d8cc68 100644 --- a/rclcpp/test/rclcpp/test_parameter_client.cpp +++ b/rclcpp/test/rclcpp/test_parameter_client.cpp @@ -12,9 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include +#include #include #include #include @@ -59,6 +60,13 @@ class TestParameterClient : public ::testing::Test 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; }; @@ -440,6 +448,7 @@ TEST_F(TestParameterClient, sync_parameter_get_parameter_types_allow_undeclared) 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); { @@ -448,6 +457,14 @@ TEST_F(TestParameterClient, sync_parameter_get_parameters) { 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); @@ -487,6 +504,7 @@ TEST_F(TestParameterClient, sync_parameter_get_parameters) { 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); { @@ -495,6 +513,14 @@ TEST_F(TestParameterClient, sync_parameter_get_parameters_allow_undeclared) { 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); @@ -907,6 +933,7 @@ TEST_F(TestParameterClient, sync_parameter_delete_parameters) { 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", @@ -914,7 +941,7 @@ TEST_F(TestParameterClient, async_parameter_load_parameters) { auto asynchronous_client = std::make_shared(load_node, "/namespace/load_node"); // load parameters - rcpputils::fs::path test_resources_path{TEST_RESOURCES_DIRECTORY}; + 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); @@ -926,12 +953,13 @@ TEST_F(TestParameterClient, async_parameter_load_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(), static_cast(5)); + 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", @@ -939,12 +967,149 @@ TEST_F(TestParameterClient, sync_parameter_load_parameters) { auto synchronous_client = std::make_shared(load_node); // load parameters - rcpputils::fs::path test_resources_path{TEST_RESOURCES_DIRECTORY}; + 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(5)); + 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 index 097b0ee391..0cf4a7a445 100644 --- a/rclcpp/test/rclcpp/test_parameter_event_handler.cpp +++ b/rclcpp/test/rclcpp/test_parameter_event_handler.cpp @@ -29,17 +29,17 @@ class TestParameterEventHandler : public rclcpp::ParameterEventHandler void test_event(rcl_interfaces::msg::ParameterEvent::ConstSharedPtr event) { - event_callback(*event); + callbacks_->event_callback(*event); } size_t num_event_callbacks() { - return event_callbacks_.size(); + return callbacks_->event_callbacks_.size(); } size_t num_parameter_callbacks() { - return parameter_callbacks_.size(); + return callbacks_->parameter_callbacks_.size(); } }; @@ -107,6 +107,11 @@ class TestNode : public ::testing::Test 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; @@ -202,9 +207,17 @@ TEST_F(TestNode, GetParameterFromEvent) EXPECT_THROW( ParameterEventHandler::get_parameter_from_event(*multiple, "my_int", wrong_name), std::runtime_error); - // Throws if parameter not part of event + + // 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(*diff_ns_bool, "my_int", node_name), + ParameterEventHandler::get_parameter_from_event(*multiple, "my_notset", wrong_name), std::runtime_error); } diff --git a/rclcpp/test/rclcpp/test_parameter_map.cpp b/rclcpp/test/rclcpp/test_parameter_map.cpp index 3f54e4c879..0158b7c7e7 100644 --- a/rclcpp/test/rclcpp/test_parameter_map.cpp +++ b/rclcpp/test/rclcpp/test_parameter_map.cpp @@ -19,6 +19,7 @@ #include #include +#include #include #include "rclcpp/parameter_map.hpp" @@ -353,3 +354,132 @@ TEST(Test_parameter_map_from, string_array_param_value) 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_publisher.cpp b/rclcpp/test/rclcpp/test_publisher.cpp index bec8bbb17a..e6e09bcb7b 100644 --- a/rclcpp/test/rclcpp/test_publisher.cpp +++ b/rclcpp/test/rclcpp/test_publisher.cpp @@ -14,8 +14,9 @@ #include -#include +#include #include +#include #include #include @@ -24,19 +25,20 @@ #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() { - if (!rclcpp::ok()) { - rclcpp::init(0, nullptr); - } + rclcpp::init(0, nullptr); } protected: @@ -50,6 +52,11 @@ class TestPublisher : public ::testing::Test node.reset(); } + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + rclcpp::Node::SharedPtr node; }; @@ -77,6 +84,7 @@ class TestPublisherSub : public ::testing::Test protected: static void SetUpTestCase() { + rclcpp::init(0, nullptr); } void SetUp() @@ -90,6 +98,11 @@ class TestPublisherSub : public ::testing::Test node.reset(); } + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + rclcpp::Node::SharedPtr node; rclcpp::Node::SharedPtr subnode; }; @@ -154,6 +167,17 @@ TEST_F(TestPublisher, various_creation_signatures) { 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(); @@ -181,11 +205,7 @@ static std::vector invalid_qos_profiles() { std::vector parameters; - parameters.reserve(2); - parameters.push_back( - TestParameters( - rclcpp::QoS(rclcpp::KeepLast(10)).transient_local(), - "transient_local_qos")); + parameters.reserve(1); parameters.push_back( TestParameters( rclcpp::QoS(rclcpp::KeepAll()), @@ -230,10 +250,9 @@ const rosidl_message_type_support_t EmptyTypeSupport() return *rosidl_typesupport_cpp::get_message_type_support_handle(); } -const rcl_publisher_options_t PublisherOptions() +const rclcpp::PublisherOptionsWithAllocator> PublisherOptions() { - return rclcpp::PublisherOptionsWithAllocator>().template - to_rcl_publisher_options(rclcpp::QoS(10)); + return rclcpp::PublisherOptionsWithAllocator>(); } class TestPublisherBase : public rclcpp::PublisherBase @@ -241,7 +260,9 @@ class TestPublisherBase : public rclcpp::PublisherBase public: explicit TestPublisherBase(rclcpp::Node * node) : rclcpp::PublisherBase( - node->get_node_base_interface().get(), "topic", EmptyTypeSupport(), PublisherOptions()) {} + node->get_node_base_interface().get(), "topic", EmptyTypeSupport(), + PublisherOptions().to_rcl_publisher_options(rclcpp::QoS(10)), + PublisherOptions().event_callbacks, PublisherOptions().use_default_callbacks) {} }; /* @@ -393,17 +414,23 @@ TEST_F(TestPublisher, intra_process_publish_failures) { std::allocator allocator; { rclcpp::LoanedMessage loaned_msg(*publisher, allocator); - RCLCPP_EXPECT_THROW_EQ( - publisher->publish(std::move(loaned_msg)), - std::runtime_error("storing loaned messages in intra process is not supported yet")); + EXPECT_NO_THROW(publisher->publish(std::move(loaned_msg))); } { rclcpp::LoanedMessage loaned_msg(*publisher, allocator); - loaned_msg.release(); + 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( @@ -460,7 +487,7 @@ class TestPublisherProtectedMethods : public rclcpp::Publisher && loaned_msg) { - this->do_loaned_message_publish(std::move(loaned_msg.release())); + this->do_loaned_message_publish(loaned_msg.release()); } void call_default_incompatible_qos_callback(rclcpp::QOSOfferedIncompatibleQoSInfo & event) const @@ -472,6 +499,11 @@ class TestPublisherProtectedMethods : public rclcpp::Publisher>; + // 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); @@ -501,8 +533,9 @@ TEST_F(TestPublisher, run_event_handlers) { initialize(); auto publisher = node->create_publisher("topic", 10); - for (const auto & handler : publisher->get_event_handlers()) { - std::shared_ptr data = handler->take_data(); + 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); } } @@ -536,3 +569,206 @@ TEST_F(TestPublisher, get_network_flow_endpoints_errors) { 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_with_type_adapter.cpp b/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp index 2f5d540568..d70b0e382d 100644 --- a/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp +++ b/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp @@ -12,36 +12,23 @@ // 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/loaned_message.hpp" #include "rclcpp/rclcpp.hpp" -#include "../mocking_utils/patch.hpp" -#include "../utils/rclcpp_gtest_macros.hpp" - -#include "test_msgs/msg/empty.hpp" +#include "rclcpp/msg/large_message.hpp" #include "rclcpp/msg/string.hpp" -#ifdef RMW_IMPLEMENTATION -# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX -# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) -#else -# define CLASSNAME(NAME, SUFFIX) NAME -#endif - - using namespace std::chrono_literals; static const int g_max_loops = 200; @@ -58,28 +45,6 @@ class TestPublisher : public ::testing::Test } } -protected: - void initialize(const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions()) - { - node = std::make_shared("my_node", "/ns", node_options); - } - - void TearDown() - { - node.reset(); - } - - rclcpp::Node::SharedPtr node; -}; - -class CLASSNAME (test_intra_process_within_one_node, RMW_IMPLEMENTATION) : public ::testing::Test -{ -public: - static void SetUpTestCase() - { - rclcpp::init(0, nullptr); - } - static void TearDownTestCase() { rclcpp::shutdown(); @@ -128,7 +93,7 @@ struct TypeAdapter { (void) source; (void) destination; - throw std::runtime_error("This should happen"); + throw std::runtime_error("This should not happen"); } static void @@ -141,6 +106,32 @@ struct TypeAdapter } }; +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 /* @@ -150,7 +141,7 @@ TEST_F(TestPublisher, various_creation_signatures) { for (auto is_intra_process : {true, false}) { rclcpp::NodeOptions options; options.use_intra_process_comms(is_intra_process); - initialize(options); + auto node = std::make_shared("my_node", "/ns", options); { using StringTypeAdapter = rclcpp::TypeAdapter; auto publisher = node->create_publisher("topic", 42); @@ -172,39 +163,70 @@ TEST_F(TestPublisher, conversion_exception_is_passed_up) { for (auto is_intra_process : {true, false}) { rclcpp::NodeOptions options; options.use_intra_process_comms(is_intra_process); - initialize(options); + + 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_F( - CLASSNAME(test_intra_process_within_one_node, RMW_IMPLEMENTATION), +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 callback = - [message_data, &is_received]( - const rclcpp::msg::String::SharedPtr 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 node = rclcpp::Node::make_shared( "test_intra_process", rclcpp::NodeOptions().use_intra_process_comms(true)); auto pub = node->create_publisher(topic_name, 10); - auto sub = node->create_subscription(topic_name, 1, callback); + 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; @@ -265,13 +287,21 @@ TEST_F( } } +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; - initialize(); + auto node = std::make_shared("my_node", "/ns", rclcpp::NodeOptions()); const std::string message_data = "Message Data"; const std::string topic_name = "topic_name"; @@ -333,3 +363,41 @@ TEST_F(TestPublisher, check_type_adapted_message_is_sent_and_received) { 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 index 47d1d10b07..eecfaf97a7 100644 --- a/rclcpp/test/rclcpp/test_qos.cpp +++ b/rclcpp/test/rclcpp/test_qos.cpp @@ -17,6 +17,7 @@ #include #include "rclcpp/qos.hpp" +#include "rmw/rmw.h" #include "rmw/types.h" @@ -93,6 +94,9 @@ TEST(TestQoS, setters_and_getters) { 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()); @@ -102,6 +106,9 @@ TEST(TestQoS, setters_and_getters) { 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()); @@ -183,6 +190,9 @@ TEST(TestQoS, DerivedTypes) { 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) { @@ -232,13 +242,20 @@ TEST(TestQoS, qos_check_compatible) // 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); - EXPECT_EQ(ret.compatibility, rclcpp::QoSCompatibility::Error); - EXPECT_FALSE(ret.reason.empty()); + 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 @@ -246,7 +263,12 @@ TEST(TestQoS, qos_check_compatible) rclcpp::SystemDefaultsQoS pub_qos; rclcpp::QoS sub_qos = rclcpp::QoS(1).reliable(); rclcpp::QoSCheckCompatibleResult ret = rclcpp::qos_check_compatible(pub_qos, sub_qos); - EXPECT_EQ(ret.compatibility, rclcpp::QoSCompatibility::Warning); - EXPECT_FALSE(ret.reason.empty()); + 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 index 960ffdcb4c..634d4837a5 100644 --- a/rclcpp/test/rclcpp/test_qos_event.cpp +++ b/rclcpp/test/rclcpp/test_qos_event.cpp @@ -14,6 +14,7 @@ #include +#include #include #include #include @@ -27,18 +28,18 @@ #include "../mocking_utils/patch.hpp" +using namespace std::chrono_literals; + class TestQosEvent : public ::testing::Test { protected: - static void SetUpTestCase() + 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); - } - void SetUp() - { - is_fastrtps = - std::string(rmw_get_implementation_identifier()).find("rmw_fastrtps") != std::string::npos; + rmw_implementation_str = std::string(rmw_get_implementation_identifier()); node = std::make_shared("test_qos_event", "/ns"); @@ -50,11 +51,12 @@ class TestQosEvent : public ::testing::Test void TearDown() { node.reset(); + rclcpp::shutdown(); } + std::string rmw_implementation_str; static constexpr char topic_name[] = "test_topic"; rclcpp::Node::SharedPtr node; - bool is_fastrtps; std::function message_callback; }; @@ -71,28 +73,29 @@ TEST_F(TestQosEvent, test_publisher_constructor) auto publisher = node->create_publisher( topic_name, 10, options); - // 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); + 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) { @@ -101,12 +104,8 @@ TEST_F(TestQosEvent, test_publisher_constructor) "Offered incompatible qos - total %d (delta %d), last_policy_kind: %d", event.total_count, event.total_count_change, event.last_policy_kind); }; - try { - publisher = node->create_publisher( - topic_name, 10, options); - } catch (const rclcpp::UnsupportedEventTypeException & /*exc*/) { - EXPECT_TRUE(is_fastrtps); - } + publisher = node->create_publisher( + topic_name, 10, options); } /* @@ -114,35 +113,38 @@ TEST_F(TestQosEvent, test_publisher_constructor) */ 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); - // 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); + 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) { @@ -151,12 +153,8 @@ TEST_F(TestQosEvent, test_subscription_constructor) "Requested incompatible qos - total %d (delta %d), last_policy_kind: %d", event.total_count, event.total_count_change, event.last_policy_kind); }; - try { - subscription = node->create_subscription( - topic_name, 10, message_callback, options); - } catch (const rclcpp::UnsupportedEventTypeException & /*exc*/) { - EXPECT_TRUE(is_fastrtps); - } + subscription = node->create_subscription( + topic_name, 10, message_callback, options); } /* @@ -210,12 +208,12 @@ TEST_F(TestQosEvent, test_default_incompatible_qos_callbacks) ex.add_node(node->get_node_base_interface()); // This future won't complete on fastrtps, so just timeout immediately - const auto timeout = (is_fastrtps) ? std::chrono::milliseconds(5) : std::chrono::seconds(10); + const auto timeout = std::chrono::seconds(10); ex.spin_until_future_complete(log_msgs_future, timeout); - if (is_fastrtps) { - EXPECT_EQ("", pub_log_msg); - EXPECT_EQ("", sub_log_msg); + 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. " @@ -237,7 +235,8 @@ TEST_F(TestQosEvent, construct_destruct_rcl_error) { // This callback requires some type of parameter, but it could be anything auto callback = [](int) {}; - const rcl_publisher_event_type_t event_type = RCL_PUBLISHER_OFFERED_DEADLINE_MISSED; + 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 @@ -247,7 +246,7 @@ TEST_F(TestQosEvent, construct_destruct_rcl_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::QOSEventHandler>>( + 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 @@ -263,7 +262,7 @@ TEST_F(TestQosEvent, construct_destruct_rcl_error) { auto throwing_statement = [callback, rcl_handle, event_type]() { // reset() is needed for this exception std::make_shared< - rclcpp::QOSEventHandler>>( + rclcpp::EventHandler>>( callback, rcl_publisher_event_init, rcl_handle, event_type).reset(); }; @@ -274,15 +273,18 @@ TEST_F(TestQosEvent, construct_destruct_rcl_error) { } 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;}; - rcl_publisher_event_type_t event_type = RCL_PUBLISHER_OFFERED_DEADLINE_MISSED; + const rcl_publisher_event_type_t event_type = RCL_PUBLISHER_OFFERED_DEADLINE_MISSED; - rclcpp::QOSEventHandler handler( + rclcpp::EventHandler handler( callback, rcl_publisher_event_init, rcl_handle, event_type); std::shared_ptr data = handler.take_data(); @@ -306,8 +308,9 @@ TEST_F(TestQosEvent, add_to_wait_set) { // This callback requires some type of parameter, but it could be anything auto callback = [](int) {}; - rcl_publisher_event_type_t event_type = RCL_PUBLISHER_OFFERED_DEADLINE_MISSED; - rclcpp::QOSEventHandler handler( + 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()); @@ -315,13 +318,309 @@ TEST_F(TestQosEvent, add_to_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_OK); - EXPECT_TRUE(handler.add_to_wait_set(&wait_set)); + 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); + 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_parameters.cpp b/rclcpp/test/rclcpp/test_qos_parameters.cpp index 97d524d176..be9a12cb5e 100644 --- a/rclcpp/test/rclcpp/test_qos_parameters.cpp +++ b/rclcpp/test/rclcpp/test_qos_parameters.cpp @@ -255,3 +255,9 @@ TEST(TestQosParameters, internal_functions_failure_modes) { 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 index d6608d59f6..86d22cbd94 100644 --- a/rclcpp/test/rclcpp/test_rate.cpp +++ b/rclcpp/test/rclcpp/test_rate.cpp @@ -14,14 +14,31 @@ #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(TestRate, rate_basics) { +TEST_F(TestRate, rate_basics) { auto period = std::chrono::milliseconds(1000); auto offset = std::chrono::milliseconds(500); auto epsilon = std::chrono::milliseconds(100); @@ -29,8 +46,8 @@ TEST(TestRate, rate_basics) { auto start = std::chrono::system_clock::now(); rclcpp::Rate r(period); - EXPECT_EQ(period, r.period()); - ASSERT_FALSE(r.is_steady()); + 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; @@ -61,7 +78,7 @@ TEST(TestRate, rate_basics) { ASSERT_TRUE(epsilon > delta); } -TEST(TestRate, wall_rate_basics) { +TEST_F(TestRate, wall_rate_basics) { auto period = std::chrono::milliseconds(100); auto offset = std::chrono::milliseconds(50); auto epsilon = std::chrono::milliseconds(1); @@ -69,8 +86,8 @@ TEST(TestRate, wall_rate_basics) { auto start = std::chrono::steady_clock::now(); rclcpp::WallRate r(period); - EXPECT_EQ(period, r.period()); - ASSERT_TRUE(r.is_steady()); + 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; @@ -101,21 +118,58 @@ TEST(TestRate, wall_rate_basics) { EXPECT_GT(epsilon, delta); } -TEST(TestRate, from_double) { +TEST_F(TestRate, from_double) { { - rclcpp::WallRate rate(1.0); - EXPECT_EQ(std::chrono::seconds(1), rate.period()); + rclcpp::Rate rate(1.0); + EXPECT_EQ(rclcpp::Duration(std::chrono::seconds(1)), rate.period()); } { - rclcpp::WallRate rate(2.0); - EXPECT_EQ(std::chrono::milliseconds(500), rate.period()); + rclcpp::Rate rate(2.0); + EXPECT_EQ(rclcpp::Duration(std::chrono::milliseconds(500)), rate.period()); } { rclcpp::WallRate rate(0.5); - EXPECT_EQ(std::chrono::seconds(2), rate.period()); + EXPECT_EQ(rclcpp::Duration(std::chrono::seconds(2)), rate.period()); } { rclcpp::WallRate rate(4.0); - EXPECT_EQ(std::chrono::milliseconds(250), rate.period()); + 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 index 07dfd8d584..0abd9b1a89 100644 --- a/rclcpp/test/rclcpp/test_ring_buffer_implementation.cpp +++ b/rclcpp/test/rclcpp/test_ring_buffer_implementation.cpp @@ -22,7 +22,7 @@ #include "rclcpp/experimental/buffers/ring_buffer_implementation.hpp" /* - Construtctor + * Construtctor */ TEST(TestRingBufferImplementation, constructor) { // Cannot create a buffer of size zero. @@ -37,10 +37,11 @@ TEST(TestRingBufferImplementation, constructor) { } /* - Basic usage - - insert data and check that it has data - - extract data - - overwrite old data writing over the buffer capacity + * 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); @@ -64,6 +65,12 @@ TEST(TestRingBufferImplementation, basic_usage) { 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()); @@ -79,3 +86,56 @@ TEST(TestRingBufferImplementation, basic_usage) { 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_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 index c3e65bcb08..67a2d9b2be 100644 --- a/rclcpp/test/rclcpp/test_serialized_message.cpp +++ b/rclcpp/test/rclcpp/test_serialized_message.cpp @@ -67,8 +67,6 @@ TEST(TestSerializedMessage, various_constructors) { 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_EQ(0u, other_serialized_message.capacity()); - EXPECT_EQ(0u, other_serialized_message.size()); 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()); @@ -147,11 +145,6 @@ TEST(TestSerializedMessage, reserve) { // Resize using reserve method serialized_msg.reserve(15); EXPECT_EQ(15u, serialized_msg.capacity()); - - // Use invalid value throws exception - EXPECT_THROW( - {serialized_msg.reserve(-1);}, - rclcpp::exceptions::RCLBadAlloc); } TEST(TestSerializedMessage, serialization) { diff --git a/rclcpp/test/rclcpp/test_service.cpp b/rclcpp/test/rclcpp/test_service.cpp index 16ab31cf1b..c6b3d3ace1 100644 --- a/rclcpp/test/rclcpp/test_service.cpp +++ b/rclcpp/test/rclcpp/test_service.cpp @@ -22,11 +22,14 @@ #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: @@ -85,12 +88,11 @@ class TestServiceSub : public ::testing::Test Testing service construction and destruction. */ TEST_F(TestService, construction_and_destruction) { - using rcl_interfaces::srv::ListParameters; - auto callback = - [](const ListParameters::Request::SharedPtr, ListParameters::Response::SharedPtr) { - }; + auto callback = []( + const rcl_interfaces::srv::ListParameters::Request::SharedPtr, + rcl_interfaces::srv::ListParameters::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()); @@ -99,7 +101,8 @@ TEST_F(TestService, construction_and_destruction) { { ASSERT_THROW( { - auto service = node->create_service("invalid_service?", callback); + auto service = node->create_service( + "invalid_service?", callback); }, rclcpp::exceptions::InvalidServiceNameError); } } @@ -108,27 +111,27 @@ TEST_F(TestService, construction_and_destruction) { Testing service construction and destruction for subnodes. */ TEST_F(TestServiceSub, construction_and_destruction) { - using rcl_interfaces::srv::ListParameters; - auto callback = - [](const ListParameters::Request::SharedPtr, ListParameters::Response::SharedPtr) { - }; + auto callback = []( + const rcl_interfaces::srv::ListParameters::Request::SharedPtr, + rcl_interfaces::srv::ListParameters::Response::SharedPtr) {}; { - auto service = subnode->create_service("service", callback); + auto service = subnode->create_service( + "service", callback); EXPECT_STREQ(service->get_service_name(), "/ns/sub_ns/service"); } { ASSERT_THROW( { - auto service = node->create_service("invalid_service?", callback); + 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 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); @@ -146,11 +149,10 @@ TEST_F(TestService, construction_and_destruction_rcl_errors) { /* Testing basic getters */ TEST_F(TestService, basic_public_getters) { - using rcl_interfaces::srv::ListParameters; - auto callback = - [](const ListParameters::Request::SharedPtr, ListParameters::Response::SharedPtr) { - }; - auto service = node->create_service("service", callback); + 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); @@ -186,9 +188,8 @@ TEST_F(TestService, basic_public_getters) { } TEST_F(TestService, take_request) { - auto callback = - [](const test_msgs::srv::Empty::Request::SharedPtr, - test_msgs::srv::Empty::Response::SharedPtr) {}; + 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(); @@ -214,9 +215,8 @@ TEST_F(TestService, take_request) { } TEST_F(TestService, send_response) { - auto callback = - [](const test_msgs::srv::Empty::Request::SharedPtr, - test_msgs::srv::Empty::Response::SharedPtr) {}; + auto callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, test_msgs::srv::Empty::Response::SharedPtr) {}; auto server = node->create_service("service", callback); { @@ -235,3 +235,167 @@ TEST_F(TestService, send_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 index 2895865c1f..06f6f9785b 100644 --- a/rclcpp/test/rclcpp/test_subscription.cpp +++ b/rclcpp/test/rclcpp/test_subscription.cpp @@ -17,6 +17,8 @@ #include #include #include +#include +#include #include #include #include @@ -34,53 +36,7 @@ using namespace std::chrono_literals; class TestSubscription : public ::testing::Test { public: - void OnMessage(test_msgs::msg::Empty::ConstSharedPtr msg) - { - (void)msg; - } - - static void SetUpTestCase() - { - rclcpp::init(0, nullptr); - } - -protected: - void initialize(const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions()) - { - node = std::make_shared("test_subscription", "/ns", node_options); - } - - void TearDown() - { - node.reset(); - } - - rclcpp::Node::SharedPtr node; -}; - -struct TestParameters -{ - 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 -{}; - -class TestSubscriptionSub : public ::testing::Test -{ -public: - void OnMessage(test_msgs::msg::Empty::ConstSharedPtr msg) + void on_message(test_msgs::msg::Empty::ConstSharedPtr msg) { (void)msg; } @@ -88,60 +44,20 @@ class TestSubscriptionSub : public ::testing::Test protected: static void SetUpTestCase() { + rclcpp::init(0, nullptr); } - void SetUp() - { - node = std::make_shared("test_subscription", "/ns"); - subnode = node->create_sub_node("sub_ns"); - } - - void TearDown() - { - node.reset(); - } - - rclcpp::Node::SharedPtr node; - rclcpp::Node::SharedPtr subnode; -}; - -class SubscriptionClassNodeInheritance : public rclcpp::Node -{ -public: - SubscriptionClassNodeInheritance() - : Node("subscription_class_node_inheritance") - { - } - - void OnMessage(test_msgs::msg::Empty::ConstSharedPtr msg) - { - (void)msg; - } - - void CreateSubscription() + static void TearDownTestCase() { - auto callback = std::bind( - &SubscriptionClassNodeInheritance::OnMessage, this, std::placeholders::_1); - using test_msgs::msg::Empty; - auto sub = this->create_subscription("topic", 10, callback); + rclcpp::shutdown(); } -}; -class SubscriptionClass -{ -public: - void OnMessage(test_msgs::msg::Empty::ConstSharedPtr msg) + void initialize(const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions()) { - (void)msg; + node_ = std::make_shared("test_subscription", "/ns", node_options); } - void CreateSubscription() - { - auto node = std::make_shared("test_subscription_member_callback", "/ns"); - auto callback = std::bind(&SubscriptionClass::OnMessage, this, std::placeholders::_1); - using test_msgs::msg::Empty; - auto sub = node->create_subscription("topic", 10, callback); - } + rclcpp::Node::SharedPtr node_; }; /* @@ -155,7 +71,7 @@ TEST_F(TestSubscription, construction_and_destruction) { }; { constexpr size_t depth = 10u; - auto sub = node->create_subscription("topic", depth, callback); + 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 @@ -172,40 +88,7 @@ TEST_F(TestSubscription, construction_and_destruction) { { ASSERT_THROW( { - auto sub = node->create_subscription("invalid_topic?", 10, callback); - }, rclcpp::exceptions::InvalidTopicNameError); - } -} - -/* - Testing subscription construction and destruction for subnodes. - */ -TEST_F(TestSubscriptionSub, construction_and_destruction) { - using test_msgs::msg::Empty; - auto callback = [](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); + auto sub = node_->create_subscription("invalid_topic?", 10, callback); }, rclcpp::exceptions::InvalidTopicNameError); } } @@ -218,31 +101,31 @@ TEST_F(TestSubscription, various_creation_signatures) { using test_msgs::msg::Empty; auto cb = [](test_msgs::msg::Empty::ConstSharedPtr) {}; { - auto sub = node->create_subscription("topic", 1, cb); + auto sub = node_->create_subscription("topic", 1, cb); (void)sub; } { - auto sub = node->create_subscription("topic", rclcpp::QoS(1), cb); + auto sub = node_->create_subscription("topic", rclcpp::QoS(1), cb); (void)sub; } { auto sub = - node->create_subscription("topic", rclcpp::QoS(rclcpp::KeepLast(1)), cb); + node_->create_subscription("topic", rclcpp::QoS(rclcpp::KeepLast(1)), cb); (void)sub; } { auto sub = - node->create_subscription("topic", rclcpp::QoS(rclcpp::KeepAll()), cb); + node_->create_subscription("topic", rclcpp::QoS(rclcpp::KeepAll()), cb); (void)sub; } { - auto sub = node->create_subscription( + auto sub = node_->create_subscription( "topic", 42, cb, rclcpp::SubscriptionOptions()); (void)sub; } { auto sub = rclcpp::create_subscription( - node, "topic", 42, cb, rclcpp::SubscriptionOptions()); + node_, "topic", 42, cb, rclcpp::SubscriptionOptions()); (void)sub; } { @@ -250,40 +133,78 @@ TEST_F(TestSubscription, various_creation_signatures) { options.allocator = std::make_shared>(); EXPECT_NE(nullptr, options.get_allocator()); auto sub = rclcpp::create_subscription( - node, "topic", 42, cb, options); + 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); + 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(); - using test_msgs::msg::Empty; { // Member callback for plain class SubscriptionClass subscription_object; - subscription_object.CreateSubscription(); + subscription_object.custom_create_subscription(); } { // Member callback for class inheriting from rclcpp::Node SubscriptionClassNodeInheritance subscription_object; - subscription_object.CreateSubscription(); + 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::OnMessage, this, std::placeholders::_1); - auto sub = node->create_subscription("topic", 1, callback); + auto callback = std::bind(&TestSubscription::on_message, this, std::placeholders::_1); + auto sub = node_->create_subscription("topic", 1, callback); } } @@ -292,10 +213,9 @@ TEST_F(TestSubscription, callback_bind) { */ TEST_F(TestSubscription, take) { initialize(); - using test_msgs::msg::Empty; auto do_nothing = [](std::shared_ptr) {FAIL();}; { - auto sub = node->create_subscription("~/test_take", 1, do_nothing); + 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)); @@ -303,23 +223,23 @@ TEST_F(TestSubscription, take) { { rclcpp::SubscriptionOptions so; so.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; - auto sub = node->create_subscription("~/test_take", 1, do_nothing, so); + 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); + 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_recieved = false; + bool message_received = false; auto start = std::chrono::steady_clock::now(); do { - message_recieved = sub->take(msg, msg_info); + message_received = sub->take(msg, msg_info); std::this_thread::sleep_for(100ms); - } while (!message_recieved && std::chrono::steady_clock::now() - start < 10s); - EXPECT_TRUE(message_recieved); + } 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. } @@ -329,10 +249,9 @@ TEST_F(TestSubscription, take) { */ TEST_F(TestSubscription, take_serialized) { initialize(); - using test_msgs::msg::Empty; auto do_nothing = [](std::shared_ptr) {FAIL();}; { - auto sub = node->create_subscription("~/test_take", 1, do_nothing); + 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)); @@ -340,23 +259,23 @@ TEST_F(TestSubscription, take_serialized) { { rclcpp::SubscriptionOptions so; so.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; - auto sub = node->create_subscription("~/test_take", 1, do_nothing, so); + 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); + 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_recieved = false; + bool message_received = false; auto start = std::chrono::steady_clock::now(); do { - message_recieved = sub->take_serialized(*msg, msg_info); + message_received = sub->take_serialized(*msg, msg_info); std::this_thread::sleep_for(100ms); - } while (!message_recieved && std::chrono::steady_clock::now() - start < 10s); - EXPECT_TRUE(message_recieved); + } while (!message_received && std::chrono::steady_clock::now() - start < 10s); + EXPECT_TRUE(message_received); } } @@ -368,7 +287,7 @@ TEST_F(TestSubscription, rcl_subscription_init_error) { // reset() is not needed for triggering exception, just to avoid an unused return value warning EXPECT_THROW( - node->create_subscription("topic", 10, callback).reset(), + node_->create_subscription("topic", 10, callback).reset(), rclcpp::exceptions::RCLError); } @@ -380,7 +299,7 @@ TEST_F(TestSubscription, rcl_subscription_fini_error) { // Cleanup just fails, no exception expected EXPECT_NO_THROW( - node->create_subscription("topic", 10, callback).reset()); + node_->create_subscription("topic", 10, callback).reset()); } TEST_F(TestSubscription, rcl_subscription_get_actual_qos_error) { @@ -389,7 +308,7 @@ TEST_F(TestSubscription, rcl_subscription_get_actual_qos_error) { auto mock = mocking_utils::patch_and_return( "lib:rclcpp", rcl_subscription_get_actual_qos, nullptr); - auto sub = node->create_subscription("topic", 10, callback); + 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")); } @@ -400,7 +319,7 @@ TEST_F(TestSubscription, rcl_take_type_erased_error) { auto mock = mocking_utils::patch_and_return( "lib:rclcpp", rcl_take, RCL_RET_ERROR); - auto sub = node->create_subscription("topic", 10, callback); + auto sub = node_->create_subscription("topic", 10, callback); test_msgs::msg::Empty msg; rclcpp::MessageInfo message_info; @@ -413,7 +332,7 @@ TEST_F(TestSubscription, rcl_take_serialized_message_error) { auto mock = mocking_utils::patch_and_return( "lib:rclcpp", rcl_take_serialized_message, RCL_RET_ERROR); - auto sub = node->create_subscription("topic", 10, callback); + auto sub = node_->create_subscription("topic", 10, callback); rclcpp::SerializedMessage msg; rclcpp::MessageInfo message_info; @@ -426,14 +345,14 @@ TEST_F(TestSubscription, rcl_subscription_get_publisher_count_error) { 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); + 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); + auto sub = node_->create_subscription("topic", 10, callback); test_msgs::msg::Empty msg; rclcpp::MessageInfo message_info; @@ -441,79 +360,152 @@ TEST_F(TestSubscription, handle_loaned_message) { } /* - Testing subscription with intraprocess enabled and invalid QoS + Testing on_new_message callbacks. */ -TEST_P(TestSubscriptionInvalidIntraprocessQos, test_subscription_throws) { - initialize(rclcpp::NodeOptions().use_intra_process_comms(true)); - rclcpp::QoS qos = GetParam().qos; +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); { - auto callback = std::bind( - &TestSubscriptionInvalidIntraprocessQos::OnMessage, - this, - std::placeholders::_1); + test_msgs::msg::Empty msg; + pub->publish(msg); + } - ASSERT_THROW( - {auto subscription = node->create_subscription( - "topic", - qos, - callback);}, - std::invalid_argument); + 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 subscription with invalid use_intra_process_comm + Testing on_new_intra_process_message callbacks. */ -TEST_P(TestSubscriptionInvalidIntraprocessQos, test_subscription_throws_intraprocess) { - rclcpp::SubscriptionOptionsWithAllocator> options; - options.use_intra_process_comm = static_cast(5); +TEST_F(TestSubscription, on_new_intra_process_message_callback) { + initialize(rclcpp::NodeOptions().use_intra_process_comms(true)); + using test_msgs::msg::Empty; - initialize(); - rclcpp::QoS qos = GetParam().qos; - auto callback = std::bind( - &TestSubscriptionInvalidIntraprocessQos::OnMessage, - this, - std::placeholders::_1); + auto do_nothing = [](std::shared_ptr) {FAIL();}; + auto sub = node_->create_subscription("~/test_take", 10, do_nothing); - RCLCPP_EXPECT_THROW_EQ( - {auto subscription = node->create_subscription( - "topic", - qos, - callback, - options);}, - std::runtime_error("Unrecognized IntraProcessSetting value")); -} + 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); -static std::vector invalid_qos_profiles() -{ - std::vector parameters; + auto pub = node_->create_publisher("~/test_take", 1); + { + test_msgs::msg::Empty msg; + pub->publish(msg); + } - parameters.reserve(3); - parameters.push_back( - TestParameters( - rclcpp::QoS(rclcpp::KeepLast(10)).transient_local(), - "transient_local_qos")); - parameters.push_back( - TestParameters( - rclcpp::QoS(rclcpp::KeepAll()), - "keep_all_qos")); + auto start = std::chrono::steady_clock::now(); + do { + std::this_thread::sleep_for(100ms); + } while (c1 == 0 && std::chrono::steady_clock::now() - start < 10s); - return parameters; -} + EXPECT_EQ(c1.load(), 1u); -INSTANTIATE_TEST_SUITE_P( - TestSubscriptionThrows, TestSubscriptionInvalidIntraprocessQos, - ::testing::ValuesIn(invalid_qos_profiles()), - ::testing::PrintToStringParamName()); + 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::SharedPtr msg) { + auto subscription_callback = [](test_msgs::msg::Empty::ConstSharedPtr msg) { (void)msg; }; - auto subscription = node->create_subscription( + auto subscription = node_->create_subscription( "topic", subscription_qos, subscription_callback); { @@ -540,3 +532,139 @@ TEST_F(TestSubscription, get_network_flow_endpoints_errors) { 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 index c4cfb6b4c4..1bcde41130 100644 --- a/rclcpp/test/rclcpp/test_subscription_options.cpp +++ b/rclcpp/test/rclcpp/test_subscription_options.cpp @@ -51,6 +51,11 @@ class TestSubscriptionOptions : public ::testing::Test node.reset(); } + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + rclcpp::Node::SharedPtr node; }; @@ -60,14 +65,17 @@ TEST_F(TestSubscriptionOptions, topic_statistics_options_default_and_set) { 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) { 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_with_type_adapter.cpp b/rclcpp/test/rclcpp/test_subscription_with_type_adapter.cpp index 54c11bf1b1..8e74592f9e 100644 --- a/rclcpp/test/rclcpp/test_subscription_with_type_adapter.cpp +++ b/rclcpp/test/rclcpp/test_subscription_with_type_adapter.cpp @@ -16,32 +16,16 @@ #include #include -#include #include #include #include -#include -#include #include "rclcpp/exceptions.hpp" -#include "rclcpp/loaned_message.hpp" #include "rclcpp/rclcpp.hpp" -#include "../mocking_utils/patch.hpp" -#include "../utils/rclcpp_gtest_macros.hpp" - -#include "test_msgs/msg/empty.hpp" #include "rclcpp/msg/string.hpp" -#ifdef RMW_IMPLEMENTATION -# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX -# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) -#else -# define CLASSNAME(NAME, SUFFIX) NAME -#endif - - using namespace std::chrono_literals; static const int g_max_loops = 200; @@ -58,28 +42,6 @@ class TestSubscription : public ::testing::Test } } -protected: - void initialize(const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions()) - { - node = std::make_shared("my_node", "/ns", node_options); - } - - void TearDown() - { - node.reset(); - } - - rclcpp::Node::SharedPtr node; -}; - -class CLASSNAME (test_intra_process_within_one_node, RMW_IMPLEMENTATION) : public ::testing::Test -{ -public: - static void SetUpTestCase() - { - rclcpp::init(0, nullptr); - } - static void TearDownTestCase() { rclcpp::shutdown(); @@ -128,11 +90,25 @@ void wait_for_message_to_be_received( 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) { - initialize(); + auto node = std::make_shared("my_node", "/ns", rclcpp::NodeOptions()); { using StringTypeAdapter = rclcpp::TypeAdapter; auto sub = @@ -151,7 +127,7 @@ TEST_F(TestSubscription, various_creation_signatures) { * Testing that subscriber receives type adapted types and ROS message types with intra proccess communications. */ TEST_F( - CLASSNAME(test_intra_process_within_one_node, RMW_IMPLEMENTATION), + TestSubscription, check_type_adapted_messages_are_received_by_intra_process_subscription) { using StringTypeAdapter = rclcpp::TypeAdapter; const std::string message_data = "Message Data"; @@ -175,6 +151,7 @@ TEST_F( 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); @@ -191,6 +168,7 @@ TEST_F( 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); @@ -206,6 +184,7 @@ TEST_F( 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); @@ -222,6 +201,7 @@ TEST_F( 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); @@ -237,6 +217,7 @@ TEST_F( 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); @@ -253,6 +234,7 @@ TEST_F( 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); @@ -268,6 +250,7 @@ TEST_F( 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); @@ -284,6 +267,7 @@ TEST_F( 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); @@ -299,6 +283,7 @@ TEST_F( 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); @@ -315,6 +300,7 @@ TEST_F( 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); @@ -330,6 +316,7 @@ TEST_F( 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); @@ -346,6 +333,7 @@ TEST_F( 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); @@ -358,7 +346,7 @@ TEST_F( * Testing that subscriber receives type adapted types and ROS message types with inter proccess communications. */ TEST_F( - CLASSNAME(test_intra_process_within_one_node, RMW_IMPLEMENTATION), + TestSubscription, check_type_adapted_messages_are_received_by_inter_process_subscription) { using StringTypeAdapter = rclcpp::TypeAdapter; const std::string message_data = "Message Data"; @@ -381,6 +369,7 @@ TEST_F( 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); @@ -398,6 +387,7 @@ TEST_F( 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); @@ -413,6 +403,7 @@ TEST_F( 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); @@ -430,6 +421,7 @@ TEST_F( 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); @@ -445,6 +437,7 @@ TEST_F( 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); @@ -461,6 +454,7 @@ TEST_F( 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); @@ -476,6 +470,7 @@ TEST_F( 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); @@ -492,6 +487,7 @@ TEST_F( 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); @@ -507,6 +503,7 @@ TEST_F( 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); @@ -523,6 +520,7 @@ TEST_F( 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); @@ -538,6 +536,7 @@ TEST_F( 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); @@ -554,6 +553,7 @@ TEST_F( 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); diff --git a/rclcpp/test/rclcpp/test_time.cpp b/rclcpp/test/rclcpp/test_time.cpp index f7889e96f6..8406de4efb 100644 --- a/rclcpp/test/rclcpp/test_time.cpp +++ b/rclcpp/test/rclcpp/test_time.cpp @@ -17,14 +17,18 @@ #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" @@ -91,8 +95,8 @@ TEST_F(TestTime, time_sources) { EXPECT_NE(0u, steady_now.nanosec); } -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 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) { @@ -134,6 +138,8 @@ TEST_F(TestTime, conversions) { EXPECT_ANY_THROW(rclcpp::Time(-1, 1)); + EXPECT_ANY_THROW(rclcpp::Time(-1)); + EXPECT_ANY_THROW( { rclcpp::Time assignment(1, 2); @@ -164,48 +170,6 @@ TEST_F(TestTime, conversions) { EXPECT_EQ(time_msg.nanosec, HALF_SEC_IN_NS); EXPECT_EQ(rclcpp::Time(time_msg).nanoseconds(), ONE_AND_HALF_SEC_IN_NS); } - - { - // Can rclcpp::Time be negative or not? The following constructor works: - rclcpp::Time time(-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); - - // The opposite conversion throws... - EXPECT_ANY_THROW( - { - rclcpp::Time negative_time(time_msg); - }); - } - - { - // Can rclcpp::Time be negative or not? The following constructor works: - rclcpp::Time time(-ONE_SEC_IN_NS); - auto time_msg = static_cast(time); - EXPECT_EQ(time_msg.sec, -1); - EXPECT_EQ(time_msg.nanosec, 0u); - - // The opposite conversion throws... - EXPECT_ANY_THROW( - { - rclcpp::Time negative_time(time_msg); - }); - } - - { - // Can rclcpp::Time be negative or not? The following constructor works: - rclcpp::Time time(-ONE_AND_HALF_SEC_IN_NS); - auto time_msg = static_cast(time); - EXPECT_EQ(time_msg.sec, -2); - EXPECT_EQ(time_msg.nanosec, HALF_SEC_IN_NS); - - // The opposite conversion throws... - EXPECT_ANY_THROW( - { - rclcpp::Time negative_time(time_msg); - }); - } } TEST_F(TestTime, operators) { @@ -322,31 +286,18 @@ TEST_F(TestTime, overflow_detectors) { TEST_F(TestTime, overflows) { rclcpp::Time max_time(std::numeric_limits::max()); - rclcpp::Time min_time(std::numeric_limits::min()); rclcpp::Duration one(1ns); rclcpp::Duration two(2ns); - // Cross min/max + // Cross 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_THROW(rclcpp::Time(max_time) += one, std::overflow_error); - EXPECT_THROW(rclcpp::Time(min_time) -= one, std::underflow_error); EXPECT_NO_THROW(max_time - max_time); - EXPECT_NO_THROW(min_time - min_time); - // Cross zero in both directions + // Cross zero rclcpp::Time one_time(1); - EXPECT_NO_THROW(one_time - two); - EXPECT_NO_THROW(rclcpp::Time(one_time) -= two); - - rclcpp::Time minus_one_time(-1); - EXPECT_NO_THROW(minus_one_time + two); - EXPECT_NO_THROW(rclcpp::Time(minus_one_time) += two); - - EXPECT_NO_THROW(one_time - minus_one_time); - EXPECT_NO_THROW(minus_one_time - one_time); + 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); @@ -359,10 +310,27 @@ TEST_F(TestTime, seconds) { } TEST_F(TestTime, test_max) { - const rclcpp::Time time_max = rclcpp::Time::max(); - const rclcpp::Time max_time(std::numeric_limits::max(), 999999999); - EXPECT_DOUBLE_EQ(max_time.seconds(), time_max.seconds()); - EXPECT_EQ(max_time.nanoseconds(), time_max.nanoseconds()); + // 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) { @@ -411,39 +379,489 @@ TEST_F(TestTime, test_overflow_underflow_throws) { 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_MIN) + rclcpp::Duration(-1ns), - std::underflow_error("addition leads to int64_t underflow")); RCLCPP_EXPECT_THROW_EQ( test_time = rclcpp::Time(INT64_MAX) - rclcpp::Duration(-1ns), std::overflow_error("time subtraction leads to int64_t overflow")); - RCLCPP_EXPECT_THROW_EQ( - test_time = rclcpp::Time(INT64_MIN) - rclcpp::Duration(1ns), - std::underflow_error("time subtraction leads to int64_t underflow")); 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_MIN); - RCLCPP_EXPECT_THROW_EQ( - test_time += rclcpp::Duration(-1ns), - std::underflow_error("addition leads to int64_t underflow")); 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")); - test_time = rclcpp::Time(INT64_MIN); - RCLCPP_EXPECT_THROW_EQ( - test_time -= rclcpp::Duration(1ns), - std::underflow_error("time subtraction leads to int64_t underflow")); 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( - test_time = rclcpp::Duration::from_nanoseconds(INT64_MIN) + rclcpp::Time(-1), - std::underflow_error("addition leads to int64_t underflow")); + 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 index 3b64e9e8b1..1437e47126 100644 --- a/rclcpp/test/rclcpp/test_time_source.cpp +++ b/rclcpp/test/rclcpp/test_time_source.cpp @@ -27,6 +27,8 @@ #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 @@ -106,7 +108,7 @@ void spin_until_ros_time_updated( executor.add_node(node); auto start = std::chrono::system_clock::now(); - while (std::chrono::system_clock::now() < (start + 1s)) { + while (std::chrono::system_clock::now() < (start + 2s)) { if (!rclcpp::ok()) { break; // Break for ctrl-c } @@ -246,11 +248,54 @@ TEST_F(TestTimeSource, ROS_time_valid_sim_time) { } TEST_F(TestTimeSource, ROS_invalid_sim_time) { - rclcpp::TimeSource ts; - ts.attachNode(node); + 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); @@ -260,7 +305,7 @@ TEST_F(TestTimeSource, clock) { trigger_clock_changes(node, ros_clock, false); - // Even now that we've recieved a message, ROS time should still not be active since the + // 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()); @@ -304,8 +349,8 @@ class CallbackObject TEST_F(TestTimeSource, callbacks) { CallbackObject cbo; rcl_jump_threshold_t jump_threshold; - jump_threshold.min_forward.nanoseconds = 0; - jump_threshold.min_backward.nanoseconds = 0; + jump_threshold.min_forward.nanoseconds = 1; + jump_threshold.min_backward.nanoseconds = -1; jump_threshold.on_clock_change = true; rclcpp::TimeSource ts(node); @@ -396,8 +441,8 @@ TEST_F(TestTimeSource, callbacks) { TEST_F(TestTimeSource, callback_handler_erasure) { CallbackObject cbo; rcl_jump_threshold_t jump_threshold; - jump_threshold.min_forward.nanoseconds = 0; - jump_threshold.min_backward.nanoseconds = 0; + jump_threshold.min_forward.nanoseconds = 1; + jump_threshold.min_backward.nanoseconds = -1; jump_threshold.on_clock_change = true; rclcpp::TimeSource ts(node); @@ -530,12 +575,12 @@ class ClockThreadTestingTimeSource : public rclcpp::TimeSource bool GetUseClockThreadOption() { - return this->use_clock_thread_; + return this->get_use_clock_thread(); } bool IsClockThreadJoinable() { - return this->clock_executor_thread_.joinable(); + return this->clock_thread_is_joinable(); } }; @@ -614,7 +659,8 @@ class SimClockPublisherNode : public rclcpp::Node { public: SimClockPublisherNode() - : rclcpp::Node("sim_clock_publisher_node") + : rclcpp::Node("sim_clock_publisher_node"), + pub_time_(0, 0) { // Create a clock publisher clock_pub_ = this->create_publisher( @@ -629,10 +675,6 @@ class SimClockPublisherNode : public rclcpp::Node &SimClockPublisherNode::timer_callback, this) ); - - // Init clock msg to zero - clock_msg_.clock.sec = 0; - clock_msg_.clock.nanosec = 0; } ~SimClockPublisherNode() @@ -655,13 +697,15 @@ class SimClockPublisherNode : public rclcpp::Node private: void timer_callback() { - // Increment clock msg and publish it - clock_msg_.clock.nanosec += 1000000; + // 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; @@ -679,7 +723,7 @@ class ClockThreadTestingNode : public rclcpp::Node this->set_parameter(rclcpp::Parameter("use_sim_time", true)); // Create a 100ms timer - timer_ = this->create_wall_timer( + timer_ = this->create_timer( std::chrono::milliseconds(100), std::bind( &ClockThreadTestingNode::timer_callback, @@ -719,26 +763,62 @@ class ClockThreadTestingNode : public rclcpp::Node bool is_callback_frozen_ = true; }; -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 +// 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(); - // 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) + 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 { - rclcpp::spin_some(clock_thread_testing_node.get_node_base_interface()); + 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); } - // Node should have get out of timer callback - ASSERT_FALSE(clock_thread_testing_node.GetIsCallbackFrozen()); + 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 index 0928fcaa5d..55c0722ae1 100644 --- a/rclcpp/test/rclcpp/test_timer.cpp +++ b/rclcpp/test/rclcpp/test_timer.cpp @@ -30,8 +30,15 @@ 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::Test +class TestTimer : public ::testing::TestWithParam { protected: void SetUp() override @@ -44,7 +51,29 @@ class TestTimer : public ::testing::Test test_node = std::make_shared("test_timer_node"); - timer = test_node->create_wall_timer( + 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 { @@ -55,9 +84,8 @@ class TestTimer : public ::testing::Test } // prevent any tests running timer from blocking this->executor->cancel(); - } - ); - EXPECT_TRUE(timer->is_steady()); + }, nullptr, false); + EXPECT_TRUE(timer_without_autostart->is_steady()); executor->add_node(test_node); // don't start spinning, let the test dictate when @@ -72,12 +100,14 @@ class TestTimer : public ::testing::Test } // 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; }; @@ -91,7 +121,7 @@ void test_initial_conditions( } /// Simple test -TEST_F(TestTimer, test_simple_cancel) +TEST_P(TestTimer, test_simple_cancel) { // expect clean state, don't run otherwise test_initial_conditions(timer, has_timer_run); @@ -104,18 +134,20 @@ TEST_F(TestTimer, test_simple_cancel) } /// Test state when using reset -TEST_F(TestTimer, test_is_canceled_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(); @@ -127,7 +159,7 @@ TEST_F(TestTimer, test_is_canceled_reset) } /// Run and check state, cancel the executor -TEST_F(TestTimer, test_run_cancel_executor) +TEST_P(TestTimer, test_run_cancel_executor) { // expect clean state, don't run otherwise test_initial_conditions(timer, has_timer_run); @@ -144,7 +176,7 @@ TEST_F(TestTimer, test_run_cancel_executor) } /// Run and check state, cancel the timer -TEST_F(TestTimer, test_run_cancel_timer) +TEST_P(TestTimer, test_run_cancel_timer) { // expect clean state, don't run otherwise test_initial_conditions(timer, has_timer_run); @@ -157,7 +189,7 @@ TEST_F(TestTimer, test_run_cancel_timer) EXPECT_TRUE(timer->is_canceled()); } -TEST_F(TestTimer, test_bad_arguments) { +TEST_P(TestTimer, test_bad_arguments) { auto node_base = rclcpp::node_interfaces::get_node_base_interface(test_node); auto context = node_base->get_context(); @@ -196,13 +228,19 @@ TEST_F(TestTimer, test_bad_arguments) { rclcpp::exceptions::RCLError); } -TEST_F(TestTimer, callback_with_timer) { +TEST_P(TestTimer, callback_with_timer) { rclcpp::TimerBase * timer_ptr = nullptr; - timer = test_node->create_wall_timer( - std::chrono::milliseconds(1), - [&timer_ptr](rclcpp::TimerBase & timer) { + 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)) @@ -214,13 +252,41 @@ TEST_F(TestTimer, callback_with_timer) { EXPECT_FALSE(timer_ptr->is_ready()); } -TEST_F(TestTimer, callback_with_period_zero) { +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; - timer = test_node->create_wall_timer( - std::chrono::milliseconds(0), - [&timer_ptr](rclcpp::TimerBase & timer) { + 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)) @@ -233,7 +299,7 @@ TEST_F(TestTimer, callback_with_period_zero) { } /// Test internal failures using mocks -TEST_F(TestTimer, test_failures_with_exceptions) +TEST_P(TestTimer, test_failures_with_exceptions) { // expect clean state, don't run otherwise test_initial_conditions(timer, has_timer_run); @@ -241,12 +307,14 @@ TEST_F(TestTimer, test_failures_with_exceptions) 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); - EXPECT_NO_THROW( - { + if (timer_type == TimerType::WALL_TIMER) { timer_to_test_destructor = - test_node->create_wall_timer(std::chrono::milliseconds(0), [](void) {}); - timer_to_test_destructor.reset(); - }); + 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( @@ -281,3 +349,34 @@ TEST_F(TestTimer, test_failures_with_exceptions) 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 index 1732031029..682907e2a9 100644 --- a/rclcpp/test/rclcpp/test_type_support.cpp +++ b/rclcpp/test/rclcpp/test_type_support.cpp @@ -58,10 +58,9 @@ class TestTypeSupport : public ::testing::Test rclcpp::Node::SharedPtr node = std::make_shared("my_node", "/ns"); }; -const rcl_publisher_options_t PublisherOptions() +const rclcpp::PublisherOptionsWithAllocator> PublisherOptions() { - return rclcpp::PublisherOptionsWithAllocator>().template - to_rcl_publisher_options(rclcpp::QoS(10)); + return rclcpp::PublisherOptionsWithAllocator>(); } // Auxiliary classes used to test rosidl_message_type_support_t getters @@ -77,7 +76,8 @@ class TestTSParameterEvent : public rclcpp::PublisherBase node->get_node_base_interface().get(), "topicTSParameterEvent", *ts_parameter_event, - PublisherOptions()) {} + 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 = @@ -91,7 +91,8 @@ class TestTSSetParameterResult : public rclcpp::PublisherBase node->get_node_base_interface().get(), "topicTSSetParameterResult", *ts_set_parameter_result, - PublisherOptions()) {} + PublisherOptions().to_rcl_publisher_options(rclcpp::QoS(10)), + PublisherOptions().event_callbacks, PublisherOptions().use_default_callbacks) {} }; const rosidl_message_type_support_t * ts_parameter_descriptor = @@ -105,7 +106,8 @@ class TestTSParameterDescriptor : public rclcpp::PublisherBase node->get_node_base_interface().get(), "topicTSParameterDescriptor", *ts_parameter_descriptor, - PublisherOptions()) {} + 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 = @@ -119,7 +121,8 @@ class TestTSListParametersResult : public rclcpp::PublisherBase node->get_node_base_interface().get(), "topicTSListParametersResult", *ts_list_parameter_result, - PublisherOptions()) {} + PublisherOptions().to_rcl_publisher_options(rclcpp::QoS(10)), + PublisherOptions().event_callbacks, PublisherOptions().use_default_callbacks) {} }; /* diff --git a/rclcpp/test/rclcpp/test_typesupport_helpers.cpp b/rclcpp/test/rclcpp/test_typesupport_helpers.cpp index 8cdcfc19c0..2117b89455 100644 --- a/rclcpp/test/rclcpp/test_typesupport_helpers.cpp +++ b/rclcpp/test/rclcpp/test_typesupport_helpers.cpp @@ -50,7 +50,7 @@ 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_typesupport_handle( + auto string_typesupport = rclcpp::get_message_typesupport_handle( "test_msgs/BasicTypes", "rosidl_typesupport_cpp", *library); EXPECT_THAT( @@ -65,7 +65,7 @@ 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_typesupport_handle( + auto string_typesupport = rclcpp::get_message_typesupport_handle( "test_msgs/msg/BasicTypes", "rosidl_typesupport_cpp", *library); EXPECT_THAT( @@ -75,3 +75,52 @@ TEST(TypesupportHelpersTest, returns_c_type_info_for_valid_library) { 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 index c57fcb9fc3..b64b0a31c6 100644 --- a/rclcpp/test/rclcpp/test_utilities.cpp +++ b/rclcpp/test/rclcpp/test_utilities.cpp @@ -25,7 +25,20 @@ #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) { @@ -111,10 +124,42 @@ TEST(TestUtilities, multi_init) { 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()); } @@ -123,6 +168,7 @@ TEST(TestUtilities, test_context_basic_access_const_methods) { 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, ==) @@ -132,11 +178,11 @@ MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, <) TEST(TestUtilities, test_context_init_shutdown_fails) { { - auto context = std::make_shared(); 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()); } { @@ -144,6 +190,7 @@ TEST(TestUtilities, test_context_init_shutdown_fails) { 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()); } { diff --git a/rclcpp/test/rclcpp/test_wait_for_message.cpp b/rclcpp/test/rclcpp/test_wait_for_message.cpp index f0071698ca..dadc06f2da 100644 --- a/rclcpp/test/rclcpp/test_wait_for_message.cpp +++ b/rclcpp/test/rclcpp/test_wait_for_message.cpp @@ -72,3 +72,38 @@ TEST(TestUtilities, wait_for_message_indefinitely) { 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 index f9d97a5759..78e102f390 100644 --- a/rclcpp/test/rclcpp/test_wait_set.cpp +++ b/rclcpp/test/rclcpp/test_wait_set.cpp @@ -31,6 +31,11 @@ class TestWaitSet : public ::testing::Test { rclcpp::init(0, nullptr); } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } }; /* @@ -257,7 +262,7 @@ TEST_F(TestWaitSet, add_guard_condition_to_two_different_wait_set) { 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()[0]; + auto qos_event = pub->get_event_handlers().begin()->second; wait_set1.add_waitable(qos_event, pub); ASSERT_THROW( { @@ -301,7 +306,7 @@ TEST_F(TestWaitSet, add_remove_wait) { [](rclcpp::QOSDeadlineOfferedInfo &) {}; auto pub = node->create_publisher( "~/test", 1, publisher_options); - auto qos_event = pub->get_event_handlers()[0]; + auto qos_event = pub->get_event_handlers().begin()->second; // Subscription mask is required here for coverage. wait_set.add_subscription(sub, {true, true, true}); diff --git a/rclcpp/test/rclcpp/topic_statistics/test_subscription_topic_statistics.cpp b/rclcpp/test/rclcpp/topic_statistics/test_subscription_topic_statistics.cpp index ce6887c631..a6fbbcbc74 100644 --- a/rclcpp/test/rclcpp/topic_statistics/test_subscription_topic_statistics.cpp +++ b/rclcpp/test/rclcpp/topic_statistics/test_subscription_topic_statistics.cpp @@ -14,7 +14,6 @@ #include -#include #include #include #include @@ -22,12 +21,12 @@ #include #include #include +#include #include #include "libstatistics_collector/moving_average_statistics/types.hpp" #include "rclcpp/create_publisher.hpp" -#include "rclcpp/msg/message_with_header.hpp" #include "rclcpp/node.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/rclcpp.hpp" @@ -36,10 +35,10 @@ #include "rclcpp/topic_statistics/subscription_topic_statistics.hpp" #include "statistics_msgs/msg/metrics_message.hpp" -#include "statistics_msgs/msg/statistic_data_point.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" @@ -67,7 +66,6 @@ constexpr const std::chrono::seconds kUnstableMessageAgeWindowDuration{ constexpr const std::chrono::seconds kUnstableMessageAgeOffset{std::chrono::seconds{1}}; } // namespace -using rclcpp::msg::MessageWithHeader; using test_msgs::msg::Empty; using rclcpp::topic_statistics::SubscriptionTopicStatistics; using statistics_msgs::msg::MetricsMessage; @@ -76,114 +74,73 @@ using statistics_msgs::msg::StatisticDataType; using libstatistics_collector::moving_average_statistics::StatisticData; /** - * Wrapper class to test and expose parts of the SubscriptionTopicStatistics class. - * \tparam CallbackMessageT + * Wrapper class to test and expose parts of the SubscriptionTopicStatistics class. */ -template -class TestSubscriptionTopicStatistics : public SubscriptionTopicStatistics +class TestSubscriptionTopicStatistics : public SubscriptionTopicStatistics { public: TestSubscriptionTopicStatistics( const std::string & node_name, rclcpp::Publisher::SharedPtr publisher) - : SubscriptionTopicStatistics(node_name, publisher) + : SubscriptionTopicStatistics(node_name, std::move(publisher)) { } - virtual ~TestSubscriptionTopicStatistics() = default; + ~TestSubscriptionTopicStatistics() override = default; /// Exposed for testing - std::vector get_current_collector_data() const - { - return SubscriptionTopicStatistics::get_current_collector_data(); - } + using SubscriptionTopicStatistics::get_current_collector_data; }; /** - * Empty publisher node: used to publish empty messages + * PublisherNode wrapper: used to create publisher node */ -class EmptyPublisher : public rclcpp::Node +template +class PublisherNode : public rclcpp::Node { public: - EmptyPublisher( + 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); + publisher_ = create_publisher(topic, 10); publish_timer_ = this->create_wall_timer( publish_period, [this]() { this->publish_message(); }); } - virtual ~EmptyPublisher() = default; + ~PublisherNode() override = default; private: void publish_message() { - auto msg = Empty{}; + auto msg = MessageT{}; publisher_->publish(msg); } - rclcpp::Publisher::SharedPtr publisher_; + typename rclcpp::Publisher::SharedPtr publisher_; rclcpp::TimerBase::SharedPtr publish_timer_; }; /** - * MessageWithHeader publisher node: used to publish MessageWithHeader with `header` value set - */ -class MessageWithHeaderPublisher : public rclcpp::Node -{ -public: - MessageWithHeaderPublisher( - 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(); - }); - uniform_dist_ = std::uniform_int_distribution{1000000, 100000000}; - } - - virtual ~MessageWithHeaderPublisher() = default; - -private: - void publish_message() - { - std::random_device rd; - std::mt19937 gen{rd()}; - uint32_t d = uniform_dist_(gen); - auto msg = MessageWithHeader{}; - // Subtract ~1 second (add some noise for a non-zero standard deviation) - // so the received message age calculation is always > 0 - msg.header.stamp = this->now() - rclcpp::Duration{1, d}; - publisher_->publish(msg); - } - - rclcpp::Publisher::SharedPtr publisher_; - rclcpp::TimerBase::SharedPtr publish_timer_; - std::uniform_int_distribution uniform_dist_; -}; - -/** - * TransitionMessageStamp publisher node : used to publish MessageWithHeader with `header` value set + * 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. */ - -class TransitionMessageStampPublisher : public rclcpp::Node +template +class TransitionMessageStampPublisherEmulator : public rclcpp::Node { public: - TransitionMessageStampPublisher( - const std::string & name, const std::string & topic, + 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) + : Node(name), transition_duration_(transition_duration), message_age_offset_(message_age_offset), + subscription_(std::move(subscription)) { - publisher_ = create_publisher(topic, 10); publish_timer_ = this->create_wall_timer(publish_period, [this]() {this->publish_message();}); start_time_ = this->now(); } @@ -191,84 +148,66 @@ class TransitionMessageStampPublisher : public rclcpp::Node private: void publish_message() { - auto msg = MessageWithHeader{}; + 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. - msg.header.stamp = now - message_age_offset_; + rmw_message_info.source_timestamp = (now - message_age_offset_).nanoseconds(); } else { - msg.header.stamp = now; + rmw_message_info.source_timestamp = now.nanoseconds(); } - publisher_->publish(msg); + 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::Publisher::SharedPtr publisher_; rclcpp::TimerBase::SharedPtr publish_timer_; }; /** - * Empty subscriber node: used to create subscriber topic statistics requirements + * Message subscriber node: used to create subscriber with enabled topic statistics collectors + * */ -class EmptySubscriber : public rclcpp::Node +template +class SubscriberWithTopicStatistics : public rclcpp::Node { public: - EmptySubscriber(const std::string & name, const std::string & topic) + SubscriberWithTopicStatistics( + const std::string & name, const std::string & topic, + std::chrono::milliseconds publish_period = defaultStatisticsPublishPeriod) : Node(name) { - // manually enable topic statistics via options + // 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 = [](Empty::UniquePtr msg) { + auto callback = [](typename MessageT::UniquePtr msg) { (void) msg; }; - subscription_ = create_subscription>( + subscription_ = create_subscription>( topic, rclcpp::QoS(rclcpp::KeepAll()), callback, options); } - virtual ~EmptySubscriber() = default; + ~SubscriberWithTopicStatistics() override = default; -private: - rclcpp::Subscription::SharedPtr subscription_; -}; - -/** - * MessageWithHeader subscriber node: used to create subscriber topic statistics requirements - */ -class MessageWithHeaderSubscriber : public rclcpp::Node -{ -public: - MessageWithHeaderSubscriber(const std::string & name, const std::string & topic) - : Node(name) + typename rclcpp::Subscription::SharedPtr get_subscription() { - // manually enable topic statistics via options - auto options = rclcpp::SubscriptionOptions(); - options.topic_stats_options.state = rclcpp::TopicStatisticsState::Enable; - options.topic_stats_options.publish_period = defaultStatisticsPublishPeriod; - - auto callback = [](MessageWithHeader::UniquePtr msg) { - (void) msg; - }; - subscription_ = create_subscription>( - topic, - rclcpp::QoS(rclcpp::KeepAll()), - callback, - options); + return subscription_; } - virtual ~MessageWithHeaderSubscriber() = default; private: - rclcpp::Subscription::SharedPtr subscription_; + typename rclcpp::Subscription::SharedPtr subscription_; }; /** @@ -277,43 +216,17 @@ class MessageWithHeaderSubscriber : public rclcpp::Node class TestSubscriptionTopicStatisticsFixture : public ::testing::Test { protected: - void SetUp() + void SetUp() override { rclcpp::init(0 /* argc */, nullptr /* argv */); } - void TearDown() + void TearDown() override { rclcpp::shutdown(); } }; -/** - * Check if a received statistics message is empty (no data was observed) - * \param message_to_check - */ -void check_if_statistics_message_is_empty(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_EQ(0, stats_point.data) << "unexpected sample count" << stats_point.data; - break; - case StatisticDataType::STATISTICS_DATA_TYPE_AVERAGE: - case StatisticDataType::STATISTICS_DATA_TYPE_MINIMUM: - case StatisticDataType::STATISTICS_DATA_TYPE_MAXIMUM: - case StatisticDataType::STATISTICS_DATA_TYPE_STDDEV: - EXPECT_TRUE(std::isnan(stats_point.data)) << "unexpected value" << stats_point.data << - " for type:" << type; - break; - default: - FAIL() << "received unknown statistics type: " << std::dec << - static_cast(type); - } - } -} - /** * Check if a received statistics message observed data and contains some calculation * \param message_to_check @@ -348,28 +261,13 @@ void check_if_statistic_message_is_populated(const MetricsMessage & message_to_c /** * Test an invalid argument is thrown for a bad input publish period. */ -TEST(TestSubscriptionTopicStatistics, test_invalid_publish_period) +TEST_F(TestSubscriptionTopicStatisticsFixture, test_invalid_publish_period) { - rclcpp::init(0 /* argc */, nullptr /* argv */); - - auto node = std::make_shared("test_period_node"); - - auto options = rclcpp::SubscriptionOptions(); - options.topic_stats_options.state = rclcpp::TopicStatisticsState::Enable; - options.topic_stats_options.publish_period = std::chrono::milliseconds(0); - - auto callback = [](Empty::UniquePtr msg) { - (void) msg; - }; - ASSERT_THROW( - (node->create_subscription>( - "should_throw_invalid_arg", - rclcpp::QoS(rclcpp::KeepAll()), - callback, - options)), std::invalid_argument); - - rclcpp::shutdown(); + SubscriberWithTopicStatistics( + "test_period_node", "should_throw_invalid_arg", std::chrono::milliseconds(0) + ), + std::invalid_argument); } /** @@ -378,7 +276,7 @@ TEST(TestSubscriptionTopicStatistics, test_invalid_publish_period) */ TEST_F(TestSubscriptionTopicStatisticsFixture, test_manual_construction) { - auto empty_subscriber = std::make_shared( + auto empty_subscriber = std::make_shared>( kTestSubNodeName, kTestSubStatsEmptyTopic); @@ -389,7 +287,7 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_manual_construction) 10); // Construct a separate instance - auto sub_topic_stats = std::make_unique>( + auto sub_topic_stats = std::make_unique( empty_subscriber->get_name(), topic_stats_publisher); @@ -410,7 +308,7 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_manual_construction) TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_no_header) { // Create an empty publisher - auto empty_publisher = std::make_shared( + auto empty_publisher = std::make_shared>( kTestPubNodeName, kTestSubStatsEmptyTopic); // empty_subscriber has a topic statistics instance as part of its subscription @@ -422,7 +320,7 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_no "/statistics", kNumExpectedMessages); - auto empty_subscriber = std::make_shared( + auto empty_subscriber = std::make_shared>( kTestSubNodeName, kTestSubStatsEmptyTopic); @@ -432,74 +330,7 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_no 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}; - - std::set received_metrics; - for (const auto & msg : received_messages) { - if (msg.metrics_source == "message_age") { - message_age_count++; - } - if (msg.metrics_source == "message_period") { - message_period_count++; - } - } - EXPECT_EQ(kNumExpectedMessageAgeMessages, message_age_count); - EXPECT_EQ(kNumExpectedMessagePeriodMessages, message_period_count); - - // Check the collected statistics for message period. - // Message age statistics will not be calculated because Empty messages - // don't have a `header` with timestamp. This means that we expect to receive a `message_age` - // and `message_period` message for each empty message published. - for (const auto & msg : received_messages) { - if (msg.metrics_source == kMessageAgeSourceLabel) { - check_if_statistics_message_is_empty(msg); - } else if (msg.metrics_source == kMessagePeriodSourceLabel) { - check_if_statistic_message_is_populated(msg); - } - } -} - -TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_with_header) -{ - // Create a MessageWithHeader publisher - auto msg_with_header_publisher = std::make_shared( - kTestPubNodeName, - kTestSubStatsTopic); - // 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_stats_for_message_with_header", - "/statistics", - kNumExpectedMessages); - - auto msg_with_header_subscriber = std::make_shared( - kTestSubNodeName, - kTestSubStatsTopic); - - rclcpp::executors::SingleThreadedExecutor ex; - ex.add_node(msg_with_header_publisher); - ex.add_node(statistics_listener); - ex.add_node(msg_with_header_subscriber); - - // Spin and get future - ex.spin_until_future_complete( - statistics_listener->GetFuture(), - kTestTimeout); + 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()); @@ -512,7 +343,6 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_wi uint64_t message_age_count{0}; uint64_t message_period_count{0}; - std::set received_metrics; for (const auto & msg : received_messages) { if (msg.metrics_source == kMessageAgeSourceLabel) { message_age_count++; @@ -524,6 +354,7 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_wi 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); } @@ -531,23 +362,27 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_wi TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_include_window_reset) { - // Create a MessageWithHeader publisher - auto msg_with_header_publisher = std::make_shared( - kTestPubNodeName, kTestSubStatsTopic, kUnstableMessageAgeWindowDuration, - kUnstableMessageAgeOffset); - - // msg_with_header_subscriber has a topic statistics instance as part of its + // msg_subscriber_with_topic_statistics has a topic statistics instance as part of its // subscription this will listen to and generate statistics - auto msg_with_header_subscriber = - std::make_shared(kTestSubNodeName, kTestSubStatsTopic); + 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_with_header_publisher); + ex.add_node(msg_publisher); ex.add_node(statistics_listener); - ex.add_node(msg_with_header_subscriber); + ex.add_node(msg_subscriber_with_topic_statistics); // Spin and get future ex.spin_until_future_complete(statistics_listener->GetFuture(), kTestTimeout); diff --git a/rclcpp/test/rclcpp/wait_set_policies/test_dynamic_storage.cpp b/rclcpp/test/rclcpp/wait_set_policies/test_dynamic_storage.cpp index 7a15e2cf49..e69d3480d9 100644 --- a/rclcpp/test/rclcpp/wait_set_policies/test_dynamic_storage.cpp +++ b/rclcpp/test/rclcpp/wait_set_policies/test_dynamic_storage.cpp @@ -51,17 +51,19 @@ class TestWaitable : public rclcpp::Waitable TestWaitable() : is_ready_(false) {} - bool add_to_wait_set(rcl_wait_set_t *) override {return true;} - - bool is_ready(rcl_wait_set_t *) override {return is_ready_;} + 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(std::shared_ptr & data) override {(void)data;} + 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_; }; diff --git a/rclcpp/test/rclcpp/wait_set_policies/test_static_storage.cpp b/rclcpp/test/rclcpp/wait_set_policies/test_static_storage.cpp index 1e4d5f805e..ed3336e123 100644 --- a/rclcpp/test/rclcpp/wait_set_policies/test_static_storage.cpp +++ b/rclcpp/test/rclcpp/wait_set_policies/test_static_storage.cpp @@ -50,17 +50,20 @@ class TestWaitable : public rclcpp::Waitable public: TestWaitable() : is_ready_(false) {} - bool add_to_wait_set(rcl_wait_set_t *) override {return true;} - bool is_ready(rcl_wait_set_t *) override {return is_ready_;} + 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(std::shared_ptr & data) override {(void)data;} + 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_; }; 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 index ba0f4d16e8..f7ae12c2f3 100644 --- a/rclcpp/test/rclcpp/wait_set_policies/test_storage_policy_common.cpp +++ b/rclcpp/test/rclcpp/wait_set_policies/test_storage_policy_common.cpp @@ -50,19 +50,28 @@ class TestWaitable : public rclcpp::Waitable public: TestWaitable() : is_ready_(false), add_to_wait_set_(false) {} - bool add_to_wait_set(rcl_wait_set_t *) override {return add_to_wait_set_;} - bool is_ready(rcl_wait_set_t *) override {return is_ready_;} + 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"); + } + } - std::shared_ptr take_data() override {return nullptr;} + bool is_ready(const rcl_wait_set_t &) override {return is_ready_;} - void - execute(std::shared_ptr & data) override {(void)data;} + 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_; 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 index 99d07f1a19..12c8838188 100644 --- a/rclcpp/test/rclcpp/wait_set_policies/test_thread_safe_synchronization.cpp +++ b/rclcpp/test/rclcpp/wait_set_policies/test_thread_safe_synchronization.cpp @@ -50,17 +50,20 @@ class TestWaitable : public rclcpp::Waitable public: TestWaitable() : is_ready_(false) {} - bool add_to_wait_set(rcl_wait_set_t *) override {return true;} - bool is_ready(rcl_wait_set_t *) override {return is_ready_;} + 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(std::shared_ptr & data) override {(void)data;} + 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_; }; 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/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_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/utils/rclcpp_gtest_macros.hpp b/rclcpp/test/utils/rclcpp_gtest_macros.hpp index a7b1b97d77..7dd708f974 100644 --- a/rclcpp/test/utils/rclcpp_gtest_macros.hpp +++ b/rclcpp/test/utils/rclcpp_gtest_macros.hpp @@ -162,7 +162,7 @@ ::testing::AssertionResult AreThrowableContentsEqual( #define RCLCPP_EXPECT_THROW_EQ(throwing_statement, expected_exception) \ do { \ ::testing::AssertionResult \ - is_the_result_of_the_throwing_expression_equal_to_the_expected_throwable = \ + is_the_result_of_the_throwing_expression_equal_to_the_expected_throwable = \ ::testing::AssertionSuccess(); \ CHECK_THROW_EQ_IMPL( \ throwing_statement, \ @@ -183,7 +183,7 @@ ::testing::AssertionResult AreThrowableContentsEqual( #define RCLCPP_ASSERT_THROW_EQ(throwing_statement, expected_exception) \ do { \ ::testing::AssertionResult \ - is_the_result_of_the_throwing_expression_equal_to_the_expected_throwable = \ + is_the_result_of_the_throwing_expression_equal_to_the_expected_throwable = \ ::testing::AssertionSuccess(); \ CHECK_THROW_EQ_IMPL( \ throwing_statement, \ diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 76b806d276..a8a7e3899d 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,235 @@ 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) ------------------- diff --git a/rclcpp_action/CMakeLists.txt b/rclcpp_action/CMakeLists.txt index e9bd327643..0fc1065d55 100644 --- a/rclcpp_action/CMakeLists.txt +++ b/rclcpp_action/CMakeLists.txt @@ -5,13 +5,15 @@ 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++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( @@ -20,28 +22,26 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") ) endif() -set(${PROJECT_NAME}_SRCS +add_library(${PROJECT_NAME} src/client.cpp src/qos.cpp src/server.cpp src/server_goal_handle.cpp src/types.cpp ) - -add_library(${PROJECT_NAME} - ${${PROJECT_NAME}_SRCS}) - target_include_directories(${PROJECT_NAME} PUBLIC "$" - "$") - -ament_target_dependencies(${PROJECT_NAME} - "action_msgs" - "rcl_action" - "rclcpp" - "rcpputils" - "rosidl_runtime_c" + "$") +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, @@ -51,7 +51,7 @@ target_compile_definitions(${PROJECT_NAME} install( DIRECTORY include/ - DESTINATION include) + DESTINATION include/${PROJECT_NAME}) install( TARGETS ${PROJECT_NAME} @@ -61,16 +61,14 @@ install( RUNTIME DESTINATION bin ) -# specific order: dependents before dependencies -ament_export_include_directories(include) +# 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(ament_cmake) -ament_export_dependencies(action_msgs) -ament_export_dependencies(rclcpp) -ament_export_dependencies(rcl_action) -ament_export_dependencies(rosidl_runtime_c) +ament_export_dependencies(action_msgs rcl_action rclcpp rosidl_runtime_c) if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) @@ -82,61 +80,63 @@ if(BUILD_TESTING) 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) - ament_target_dependencies(test_client - "rcutils" - "test_msgs" - ) 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) - ament_target_dependencies(test_server - "rcpputils" - "rcutils" - "test_msgs" - ) 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) - ament_target_dependencies(test_server_goal_handle - "rcutils" - "test_msgs" - ) 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) - ament_target_dependencies(test_traits - "test_msgs" - ) target_link_libraries(test_traits ${PROJECT_NAME} + ${test_msgs_TARGETS} ) endif() ament_add_gtest(test_types test/test_types.cpp) if(TARGET test_types) - ament_target_dependencies(test_types - "test_msgs" - ) 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 index 4b97902c6c..e7c9fedb59 100644 --- a/rclcpp_action/Doxyfile +++ b/rclcpp_action/Doxyfile @@ -21,15 +21,15 @@ GENERATE_LATEX = NO ENABLE_PREPROCESSING = YES MACRO_EXPANSION = YES EXPAND_ONLY_PREDEF = YES -PREDEFINED = RCLCPP_PUBLIC= +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/" +#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/" +#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 index 7512ad3312..5812429610 100644 --- a/rclcpp_action/QUALITY_DECLARATION.md +++ b/rclcpp_action/QUALITY_DECLARATION.md @@ -4,7 +4,7 @@ This document is a declaration of software quality for the `rclcpp_action` packa 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 Quality Categories in REP-2004](https://www.ros.org/reps/rep-2004.html) of the ROS2 developer guide. +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] @@ -92,7 +92,7 @@ There is an automated test which runs a linter that ensures each file has at lea ### 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/master/test) directory. +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: @@ -125,7 +125,7 @@ Current coverage statistics can be viewed [here](https://ci.ros2.org/job/nightly `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/master/rclcpp_action/test/benchmark). +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/) @@ -135,7 +135,7 @@ Changes that introduce regressions in performance must be adequately justified i ### Linters and Static Analysis [4.v] -`rclcpp_action` uses and passes all the ROS2 standard linters and static analysis tools for a C++ package as described in the [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#linters-and-static-analysis). Passing implies there are no linter/static errors when testing against CI of supported platforms. +`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/) @@ -159,19 +159,19 @@ It also has several test dependencies, which do not affect the resulting quality `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/master/action_msgs/QUALITY_DECLARATION.md). +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/master/rclcpp/QUALITY_DECLARATION.md). +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/master/rcl_action/QUALITY_DECLARATION.md). +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] diff --git a/rclcpp_action/README.md b/rclcpp_action/README.md index c98987c922..bc3b85e6bd 100644 --- a/rclcpp_action/README.md +++ b/rclcpp_action/README.md @@ -2,7 +2,8 @@ Adds action APIs for C++. -Visit the [rclcpp_action API documentation](http://docs.ros2.org/latest/api/rclcpp_action/) for a complete list of its main components and features. For more information about Actions in ROS 2, see the [design document](http://design.ros2.org/articles/actions.html). +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 diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index 8294f2be68..40a326702a 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -15,18 +15,6 @@ #ifndef RCLCPP_ACTION__CLIENT_HPP_ #define RCLCPP_ACTION__CLIENT_HPP_ -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - #include #include #include @@ -35,8 +23,23 @@ #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" @@ -59,9 +62,21 @@ class ClientBaseImpl; 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 @@ -108,23 +123,61 @@ class ClientBase : public rclcpp::Waitable /// \internal RCLCPP_ACTION_PUBLIC - bool - add_to_wait_set(rcl_wait_set_t * wait_set) override; + void + add_to_wait_set(rcl_wait_set_t & wait_set) override; /// \internal RCLCPP_ACTION_PUBLIC bool - is_ready(rcl_wait_set_t * wait_set) override; + 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(std::shared_ptr & data) override; + 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 // ----------------- @@ -244,8 +297,31 @@ class ClientBase : public rclcpp::Waitable // 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 @@ -268,107 +344,13 @@ class Client : public ClientBase 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; - /// Compatibility wrapper for `goal_response_callback`. - class GoalResponseCallback - { -public: - using NewSignature = std::function; - using OldSignature = std::function)>; - - GoalResponseCallback() = default; - - GoalResponseCallback(std::nullptr_t) {} // NOLINT, intentionally implicit. - - // implicit constructor - [[deprecated( - "Use new goal response callback signature " - "`std::function::GoalHandle::SharedPtr)>` " - "instead of the old " - "`std::function::GoalHandle::SharedPtr>)>`.\n" - "e.g.:\n" - "```cpp\n" - "Client::SendGoalOptions options;\n" - "options.goal_response_callback = [](Client::GoalHandle::SharedPtr goal) {\n" - " // do something with `goal` here\n" - "};")]] - GoalResponseCallback(OldSignature old_callback) // NOLINT, intentionally implicit. - : old_callback_(std::move(old_callback)) {} - - GoalResponseCallback(NewSignature new_callback) // NOLINT, intentionally implicit. - : new_callback_(std::move(new_callback)) {} - - GoalResponseCallback & - operator=(OldSignature old_callback) {old_callback_ = std::move(old_callback); return *this;} - - GoalResponseCallback & - operator=(NewSignature new_callback) {new_callback_ = std::move(new_callback); return *this;} - - void - operator()(typename GoalHandle::SharedPtr goal_handle) const - { - if (new_callback_) { - new_callback_(std::move(goal_handle)); - return; - } - if (old_callback_) { - throw std::runtime_error{ - "Cannot call GoalResponseCallback(GoalHandle::SharedPtr) " - "if using the old goal response callback signature."}; - } - throw std::bad_function_call{}; - } - - [[deprecated( - "Calling " - "`void goal_response_callback(" - " std::shared_future::GoalHandle::SharedPtr> goal_handle_shared_future)`" - " is deprecated.")]] - void - operator()(std::shared_future goal_handle_future) const - { - if (old_callback_) { - old_callback_(std::move(goal_handle_future)); - return; - } - if (new_callback_) { - new_callback_(std::move(goal_handle_future).get_future().share()); - return; - } - throw std::bad_function_call{}; - } - - explicit operator bool() const noexcept { - return new_callback_ || old_callback_; - } - -private: - friend class Client; - void - operator()( - typename GoalHandle::SharedPtr goal_handle, - std::shared_future goal_handle_future) const - { - if (new_callback_) { - new_callback_(std::move(goal_handle)); - return; - } - if (old_callback_) { - old_callback_(std::move(goal_handle_future)); - return; - } - throw std::bad_function_call{}; - } - - NewSignature new_callback_; - OldSignature old_callback_; - }; - /// Options for sending a goal. /** * This struct is used to pass parameters to the function `async_send_goal`. @@ -425,12 +407,22 @@ class Client : public ClientBase /// 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 `ClientGoalHandle`. + * 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 returned goal handle is used to monitor the status of the goal and get the final result. - * It is valid as long as you hold a reference to the shared pointer or until the - * rclcpp_action::Client is destroyed at which point the goal status will become UNKNOWN. + * 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 @@ -450,14 +442,14 @@ class Client : public ClientBase goal_request->goal = goal; this->send_goal_request( std::static_pointer_cast(goal_request), - [this, goal_request, options, promise, future](std::shared_ptr response) mutable + [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, future); + options.goal_response_callback(nullptr); } return; } @@ -468,12 +460,12 @@ class Client : public ClientBase std::shared_ptr goal_handle( new GoalHandle(goal_info, options.feedback_callback, options.result_callback)); { - std::lock_guard guard(goal_handles_mutex_); + 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, future); + options.goal_response_callback(goal_handle); } if (options.result_callback) { @@ -486,7 +478,7 @@ class Client : public ClientBase // 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_); + 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()) { @@ -516,7 +508,7 @@ class Client : public ClientBase typename GoalHandle::SharedPtr goal_handle, ResultCallback result_callback = nullptr) { - std::lock_guard lock(goal_handles_mutex_); + std::lock_guard lock(goal_handles_mutex_); if (goal_handles_.count(goal_handle->get_goal_id()) == 0) { throw exceptions::UnknownGoalHandleError(); } @@ -551,7 +543,7 @@ class Client : public ClientBase typename GoalHandle::SharedPtr goal_handle, CancelCallback cancel_callback = nullptr) { - std::lock_guard lock(goal_handles_mutex_); + std::lock_guard lock(goal_handles_mutex_); if (goal_handles_.count(goal_handle->get_goal_id()) == 0) { throw exceptions::UnknownGoalHandleError(); } @@ -582,6 +574,63 @@ class Client : public ClientBase 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. @@ -610,7 +659,7 @@ class Client : public ClientBase virtual ~Client() { - std::lock_guard guard(goal_handles_mutex_); + 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(); @@ -657,7 +706,7 @@ class Client : public ClientBase void handle_feedback_message(std::shared_ptr message) override { - std::lock_guard guard(goal_handles_mutex_); + std::lock_guard guard(goal_handles_mutex_); using FeedbackMessage = typename ActionT::Impl::FeedbackMessage; typename FeedbackMessage::SharedPtr feedback_message = std::static_pointer_cast(message); @@ -694,7 +743,7 @@ class Client : public ClientBase void handle_status_message(std::shared_ptr message) override { - std::lock_guard guard(goal_handles_mutex_); + 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) { @@ -743,7 +792,7 @@ class Client : public ClientBase 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_); + std::lock_guard lock(goal_handles_mutex_); goal_handles_.erase(goal_handle->get_goal_id()); }); } catch (rclcpp::exceptions::RCLError & ex) { @@ -775,7 +824,7 @@ class Client : public ClientBase } std::map goal_handles_; - std::mutex goal_handles_mutex_; + std::recursive_mutex goal_handles_mutex_; }; } // namespace rclcpp_action diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp index e2861e3e79..73987ec887 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp @@ -15,17 +15,17 @@ #ifndef RCLCPP_ACTION__CLIENT_GOAL_HANDLE_HPP_ #define RCLCPP_ACTION__CLIENT_GOAL_HANDLE_HPP_ -#include - -#include -#include -#include - #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" @@ -62,7 +62,7 @@ class ClientGoalHandle RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ClientGoalHandle) // A wrapper that defines the result of an action - typedef struct WrappedResult + struct WrappedResult { /// The unique identifier of the goal GoalUUID goal_id; @@ -70,7 +70,7 @@ class ClientGoalHandle ResultCode code; /// User defined fields sent back with an action typename ActionT::Result::SharedPtr result; - } WrappedResult; + }; using Feedback = typename ActionT::Feedback; using Result = typename ActionT::Result; @@ -104,7 +104,7 @@ class ClientGoalHandle private: // The templated Client creates goal handles - friend Client; + friend class Client; ClientGoalHandle( const GoalInfo & info, @@ -163,7 +163,7 @@ class ClientGoalHandle ResultCallback result_callback_{nullptr}; int8_t status_{GoalStatus::STATUS_ACCEPTED}; - std::mutex handle_mutex_; + std::recursive_mutex handle_mutex_; }; } // namespace rclcpp_action diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp index 0c25e52433..58b1d7f248 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp @@ -59,7 +59,7 @@ template std::shared_future::WrappedResult> ClientGoalHandle::async_get_result() { - std::lock_guard guard(handle_mutex_); + std::lock_guard guard(handle_mutex_); if (!is_result_aware_) { throw exceptions::UnawareGoalHandleError(); } @@ -70,11 +70,12 @@ template void ClientGoalHandle::set_result(const WrappedResult & wrapped_result) { - std::lock_guard guard(handle_mutex_); + 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(); } } @@ -82,7 +83,7 @@ template void ClientGoalHandle::set_feedback_callback(FeedbackCallback callback) { - std::lock_guard guard(handle_mutex_); + std::lock_guard guard(handle_mutex_); feedback_callback_ = callback; } @@ -90,7 +91,7 @@ template void ClientGoalHandle::set_result_callback(ResultCallback callback) { - std::lock_guard guard(handle_mutex_); + std::lock_guard guard(handle_mutex_); result_callback_ = callback; } @@ -98,7 +99,7 @@ template int8_t ClientGoalHandle::get_status() { - std::lock_guard guard(handle_mutex_); + std::lock_guard guard(handle_mutex_); return status_; } @@ -106,7 +107,7 @@ template void ClientGoalHandle::set_status(int8_t status) { - std::lock_guard guard(handle_mutex_); + std::lock_guard guard(handle_mutex_); status_ = status; } @@ -114,7 +115,7 @@ template bool ClientGoalHandle::is_feedback_aware() { - std::lock_guard guard(handle_mutex_); + std::lock_guard guard(handle_mutex_); return feedback_callback_ != nullptr; } @@ -122,7 +123,7 @@ template bool ClientGoalHandle::is_result_aware() { - std::lock_guard guard(handle_mutex_); + std::lock_guard guard(handle_mutex_); return is_result_aware_; } @@ -130,7 +131,7 @@ template bool ClientGoalHandle::set_result_awareness(bool awareness) { - std::lock_guard guard(handle_mutex_); + std::lock_guard guard(handle_mutex_); bool previous = is_result_aware_; is_result_aware_ = awareness; return previous; @@ -140,7 +141,7 @@ template void ClientGoalHandle::invalidate(const exceptions::UnawareGoalHandleError & ex) { - std::lock_guard guard(handle_mutex_); + std::lock_guard guard(handle_mutex_); // Guard against multiple calls if (is_invalidated()) { return; @@ -168,7 +169,7 @@ ClientGoalHandle::call_feedback_callback( RCLCPP_ERROR(rclcpp::get_logger("rclcpp_action"), "Sent feedback to wrong goal handle."); return; } - std::lock_guard guard(handle_mutex_); + 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."); diff --git a/rclcpp_action/include/rclcpp_action/create_client.hpp b/rclcpp_action/include/rclcpp_action/create_client.hpp index 8538a8205e..f594bca78d 100644 --- a/rclcpp_action/include/rclcpp_action/create_client.hpp +++ b/rclcpp_action/include/rclcpp_action/create_client.hpp @@ -15,11 +15,11 @@ #ifndef RCLCPP_ACTION__CREATE_CLIENT_HPP_ #define RCLCPP_ACTION__CREATE_CLIENT_HPP_ -#include - #include #include +#include "rclcpp/node.hpp" + #include "rclcpp_action/client.hpp" #include "rclcpp_action/visibility_control.hpp" diff --git a/rclcpp_action/include/rclcpp_action/create_server.hpp b/rclcpp_action/include/rclcpp_action/create_server.hpp index 9116fc05ee..c333a60581 100644 --- a/rclcpp_action/include/rclcpp_action/create_server.hpp +++ b/rclcpp_action/include/rclcpp_action/create_server.hpp @@ -15,17 +15,17 @@ #ifndef RCLCPP_ACTION__CREATE_SERVER_HPP_ #define RCLCPP_ACTION__CREATE_SERVER_HPP_ -#include - -#include -#include -#include -#include -#include - #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" @@ -116,7 +116,7 @@ create_server( * * \sa Server::Server() for more information. * - * \param[in] node] The action server will be added to this node. + * \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. diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index f9488fc1d5..a885383614 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -15,14 +15,6 @@ #ifndef RCLCPP_ACTION__SERVER_HPP_ #define RCLCPP_ACTION__SERVER_HPP_ -#include -#include -#include -#include -#include -#include -#include - #include #include #include @@ -30,6 +22,16 @@ #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" @@ -70,6 +72,17 @@ enum class CancelResponse : int8_t 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(); @@ -109,24 +122,61 @@ class ServerBase : public rclcpp::Waitable /// Add all entities to a wait set. /// \internal RCLCPP_ACTION_PUBLIC - bool - add_to_wait_set(rcl_wait_set_t * wait_set) override; + 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(rcl_wait_set_t *) override; + 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(std::shared_ptr & data) override; + 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 // ----------------- @@ -233,19 +283,29 @@ class ServerBase : public rclcpp::Waitable /// \internal RCLCPP_ACTION_PUBLIC void - execute_goal_request_received(std::shared_ptr & data); + 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(std::shared_ptr & data); + 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(std::shared_ptr & data); + 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 @@ -256,6 +316,29 @@ class ServerBase : public rclcpp::Waitable /// 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 @@ -276,7 +359,8 @@ class Server : public ServerBase, public std::enable_shared_from_this)>; + const GoalUUID &, + std::shared_ptr)>; /// 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. @@ -386,7 +470,8 @@ class Server : public ServerBase, public std::enable_shared_from_this rcl_goal_handle, - GoalUUID uuid, std::shared_ptr goal_request_message) override + GoalUUID uuid, + std::shared_ptr goal_request_message) override { std::shared_ptr> goal_handle; std::weak_ptr> weak_this = this->shared_from_this(); diff --git a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp index d41b32eb4f..07873f4c71 100644 --- a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp @@ -15,15 +15,15 @@ #ifndef RCLCPP_ACTION__SERVER_GOAL_HANDLE_HPP_ #define RCLCPP_ACTION__SERVER_GOAL_HANDLE_HPP_ -#include -#include - -#include - #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" @@ -43,7 +43,7 @@ class ServerGoalHandleBase { public: /// Indicate if client has requested this goal be cancelled. - /// \return true if a cancelation request has been accepted for this goal. + /// \return true if a cancellation request has been accepted for this goal. RCLCPP_ACTION_PUBLIC bool is_canceling() const; @@ -128,7 +128,7 @@ class Server; * accepted. * A `Server` will create an instance and give it to the user in their `handle_accepted` callback. * - * Internally, this class is responsible for coverting between the C++ action type and generic + * Internally, this class is responsible for converting between the C++ action type and generic * types for `rclcpp_action::ServerGoalHandleBase`. */ template @@ -196,7 +196,7 @@ class ServerGoalHandle : public ServerGoalHandleBase /// Indicate that a goal has been canceled. /** - * Only call this if the goal is executing or pending, but 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. * @@ -273,7 +273,7 @@ class ServerGoalHandle : public ServerGoalHandleBase /// A unique id for the goal request. const GoalUUID uuid_; - friend Server; + friend class Server; std::function)> on_terminal_state_; std::function on_executing_; diff --git a/rclcpp_action/include/rclcpp_action/types.hpp b/rclcpp_action/include/rclcpp_action/types.hpp index addf4a8307..cf359dfaa9 100644 --- a/rclcpp_action/include/rclcpp_action/types.hpp +++ b/rclcpp_action/include/rclcpp_action/types.hpp @@ -15,15 +15,16 @@ #ifndef RCLCPP_ACTION__TYPES_HPP_ #define RCLCPP_ACTION__TYPES_HPP_ -#include - -#include -#include - +#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 @@ -33,7 +34,7 @@ using GoalUUID = std::array; using GoalStatus = action_msgs::msg::GoalStatus; using GoalInfo = action_msgs::msg::GoalInfo; -/// Convert a goal id to a human readable string. +/// Convert a goal id to a human readable RFC-4122 compliant string. RCLCPP_ACTION_PUBLIC std::string to_string(const GoalUUID & goal_id); @@ -68,14 +69,13 @@ struct hash { size_t operator()(const rclcpp_action::GoalUUID & uuid) const noexcept { - // TODO(sloretz) Use someone else's hash function and cite it - size_t result = 0; - for (size_t i = 0; i < uuid.size(); ++i) { - for (size_t b = 0; b < sizeof(size_t); ++b) { - size_t part = uuid[i]; - part <<= CHAR_BIT * b; - result ^= part; - } + // 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; } diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index 21a210296c..6fb522f1fe 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,13 +2,17 @@ rclcpp_action - 12.0.0 + 29.2.0 Adds action APIs for C++. + Ivan Paunovic - Mabel Zhang + Michel Hidalgo William Woodall + Apache License 2.0 + Dirk Thomas + Jacob Perron ament_cmake_ros @@ -19,6 +23,7 @@ action_msgs rclcpp rcl_action + rcl rcpputils ament_cmake diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index 5839a4f932..3ea9b1fb1a 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -12,11 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include -#include - #include #include #include @@ -24,6 +19,12 @@ #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" @@ -31,6 +32,67 @@ 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: @@ -44,7 +106,7 @@ class ClientBaseImpl : 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{} ()) + random_bytes_generator(std::random_device{}()) { std::weak_ptr weak_node_handle(node_handle); client_handle = std::shared_ptr( @@ -94,11 +156,11 @@ class ClientBaseImpl size_t num_clients{0u}; size_t num_services{0u}; - 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}; + // 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_; @@ -142,6 +204,7 @@ 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(), @@ -163,7 +226,6 @@ bool ClientBase::wait_for_action_server_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 = pimpl_->node_graph_.lock(); if (!node_ptr) { throw rclcpp::exceptions::InvalidNodeError(); @@ -172,6 +234,7 @@ ClientBase::wait_for_action_server_nanoseconds(std::chrono::nanoseconds timeout) 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 @@ -252,34 +315,70 @@ ClientBase::get_number_of_ready_services() return pimpl_->num_services; } -bool -ClientBase::add_to_wait_set(rcl_wait_set_t * wait_set) +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); - return RCL_RET_OK == ret; + &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(rcl_wait_set_t * wait_set) +ClientBase::is_ready(const rcl_wait_set_t & wait_set) { - rcl_ret_t ret = rcl_action_client_wait_set_get_entities_ready( - wait_set, pimpl_->client_handle.get(), - &pimpl_->is_feedback_ready, - &pimpl_->is_status_ready, - &pimpl_->is_goal_response_ready, - &pimpl_->is_cancel_response_ready, - &pimpl_->is_result_response_ready); - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error( - ret, "failed to check for any ready entities"); + 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; } - return - pimpl_->is_feedback_ready || - pimpl_->is_status_ready || - pimpl_->is_goal_response_ready || - pimpl_->is_cancel_response_ready || - pimpl_->is_result_response_ready; + + if (is_cancel_response_ready) { + pimpl_->next_ready_event = static_cast(EntityType::CancelClient); + return true; + } + + return false; } void @@ -316,14 +415,19 @@ ClientBase::handle_result_response( const rmw_request_id_t & response_header, std::shared_ptr 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; + 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); } - pimpl_->pending_result_responses[sequence_number](response); - pimpl_->pending_result_responses.erase(sequence_number); + auto & response_callback = pending_result_response.mapped(); + response_callback(response); } void @@ -383,118 +487,314 @@ ClientBase::generate_goal_id() 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() { - if (pimpl_->is_feedback_ready) { - std::shared_ptr feedback_message = this->create_feedback_message(); - rcl_ret_t ret = rcl_action_take_feedback( - pimpl_->client_handle.get(), feedback_message.get()); - return std::static_pointer_cast( - std::make_shared>>( - ret, feedback_message)); - } else if (pimpl_->is_status_ready) { - std::shared_ptr status_message = this->create_status_message(); - rcl_ret_t ret = rcl_action_take_status( - pimpl_->client_handle.get(), status_message.get()); - return std::static_pointer_cast( - std::make_shared>>( - ret, status_message)); - } else if (pimpl_->is_goal_response_ready) { - rmw_request_id_t response_header; - std::shared_ptr goal_response = this->create_goal_response(); - rcl_ret_t ret = rcl_action_take_goal_response( - pimpl_->client_handle.get(), &response_header, goal_response.get()); - return std::static_pointer_cast( - std::make_shared>>( - ret, response_header, goal_response)); - } else if (pimpl_->is_result_response_ready) { - rmw_request_id_t response_header; - std::shared_ptr result_response = this->create_result_response(); - rcl_ret_t ret = rcl_action_take_result_response( - pimpl_->client_handle.get(), &response_header, result_response.get()); - return std::static_pointer_cast( - std::make_shared>>( - ret, response_header, result_response)); - } else if (pimpl_->is_cancel_response_ready) { - rmw_request_id_t response_header; - std::shared_ptr cancel_response = this->create_cancel_response(); - rcl_ret_t ret = rcl_action_take_cancel_response( - pimpl_->client_handle.get(), &response_header, cancel_response.get()); - return std::static_pointer_cast( - std::make_shared>>( - ret, response_header, cancel_response)); - } else { + // 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); } -void -ClientBase::execute(std::shared_ptr & data) +std::shared_ptr +ClientBase::take_data_by_entity_id(size_t id) { - if (!data) { - throw std::runtime_error("'data' is empty"); + 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; } - if (pimpl_->is_feedback_ready) { - auto shared_ptr = std::static_pointer_cast>>(data); - auto ret = std::get<0>(*shared_ptr); - pimpl_->is_feedback_ready = false; - if (RCL_RET_OK == ret) { - auto feedback_message = std::get<1>(*shared_ptr); - this->handle_feedback_message(feedback_message); - } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret, "error taking feedback"); - } - } else if (pimpl_->is_status_ready) { - auto shared_ptr = std::static_pointer_cast>>(data); - auto ret = std::get<0>(*shared_ptr); - pimpl_->is_status_ready = false; - if (RCL_RET_OK == ret) { - auto status_message = std::get<1>(*shared_ptr); - this->handle_status_message(status_message); - } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret, "error taking status"); - } - } else if (pimpl_->is_goal_response_ready) { - auto shared_ptr = std::static_pointer_cast< - std::tuple>>(data); - auto ret = std::get<0>(*shared_ptr); - pimpl_->is_goal_response_ready = false; - if (RCL_RET_OK == ret) { - auto response_header = std::get<1>(*shared_ptr); - auto goal_response = std::get<2>(*shared_ptr); - this->handle_goal_response(response_header, goal_response); - } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret, "error taking goal response"); - } - } else if (pimpl_->is_result_response_ready) { - auto shared_ptr = std::static_pointer_cast< - std::tuple>>(data); - auto ret = std::get<0>(*shared_ptr); - pimpl_->is_result_response_ready = false; - if (RCL_RET_OK == ret) { - auto response_header = std::get<1>(*shared_ptr); - auto result_response = std::get<2>(*shared_ptr); - this->handle_result_response(response_header, result_response); - } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret, "error taking result response"); - } - } else if (pimpl_->is_cancel_response_ready) { - auto shared_ptr = std::static_pointer_cast< - std::tuple>>(data); - auto ret = std::get<0>(*shared_ptr); - pimpl_->is_cancel_response_ready = false; - if (RCL_RET_OK == ret) { - auto response_header = std::get<1>(*shared_ptr); - auto cancel_response = std::get<2>(*shared_ptr); - this->handle_cancel_response(response_header, cancel_response); - } else if (RCL_RET_ACTION_CLIENT_TAKE_FAILED != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret, "error taking cancel response"); - } - } else { - throw std::runtime_error("Executing action client but nothing is ready"); + 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/server.cpp b/rclcpp_action/src/server.cpp index d1477300f1..fefc02d6ad 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -12,29 +12,64 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include - -#include - -#include -#include -#include -#include - #include #include #include #include #include #include +#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: @@ -60,11 +95,6 @@ class ServerBaseImpl size_t num_services_ = 0; size_t num_guard_conditions_ = 0; - std::atomic goal_request_ready_{false}; - std::atomic cancel_request_ready_{false}; - std::atomic result_request_ready_{false}; - std::atomic goal_expired_{false}; - // Lock for unordered_maps std::recursive_mutex unordered_map_mutex_; @@ -75,8 +105,12 @@ class ServerBaseImpl // 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( @@ -164,17 +198,19 @@ ServerBase::get_number_of_ready_guard_conditions() return pimpl_->num_guard_conditions_; } -bool -ServerBase::add_to_wait_set(rcl_wait_set_t * wait_set) +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); - return RCL_RET_OK == ret; + &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(rcl_wait_set_t * wait_set) +ServerBase::is_ready(const rcl_wait_set_t & wait_set) { bool goal_request_ready; bool cancel_request_ready; @@ -184,7 +220,7 @@ ServerBase::is_ready(rcl_wait_set_t * wait_set) { std::lock_guard lock(pimpl_->action_server_reentrant_mutex_); ret = rcl_action_server_wait_set_get_entities_ready( - wait_set, + &wait_set, pimpl_->action_server_.get(), &goal_request_ready, &cancel_request_ready, @@ -192,105 +228,153 @@ ServerBase::is_ready(rcl_wait_set_t * wait_set) &goal_expired); } - pimpl_->goal_request_ready_ = goal_request_ready; - pimpl_->cancel_request_ready_ = cancel_request_ready; - pimpl_->result_request_ready_ = result_request_ready; - pimpl_->goal_expired_ = goal_expired; - if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } - return pimpl_->goal_request_ready_.load() || - pimpl_->cancel_request_ready_.load() || - pimpl_->result_request_ready_.load() || - pimpl_->goal_expired_.load(); + 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() { - if (pimpl_->goal_request_ready_.load()) { - rcl_ret_t ret; - rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info(); - rmw_request_id_t request_header; + size_t next_ready_event = pimpl_->next_ready_event.exchange(std::numeric_limits::max()); - std::lock_guard lock(pimpl_->action_server_reentrant_mutex_); + if (next_ready_event == std::numeric_limits::max()) { + throw std::runtime_error("ServerBase::take_data() called but no data is ready"); + } - std::shared_ptr message = create_goal_request(); - ret = rcl_action_take_goal_request( - pimpl_->action_server_.get(), - &request_header, - message.get()); - - return std::static_pointer_cast( - std::make_shared - >>( - ret, - goal_info, - request_header, message)); - } else if (pimpl_->cancel_request_ready_.load()) { - rcl_ret_t ret; - rmw_request_id_t request_header; + return take_data_by_entity_id(next_ready_event); +} - // Initialize cancel request - auto request = std::make_shared(); +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_); - ret = rcl_action_take_cancel_request( - pimpl_->action_server_.get(), - &request_header, - request.get()); + std::lock_guard lock(pimpl_->action_server_reentrant_mutex_); - return std::static_pointer_cast( - std::make_shared - , - rmw_request_id_t>>(ret, request, request_header)); - } else if (pimpl_->result_request_ready_.load()) { - 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()); - - return std::static_pointer_cast( - std::make_shared, rmw_request_id_t>>( - ret, result_request, request_header)); - } else if (pimpl_->goal_expired_.load()) { - return nullptr; - } else { - throw std::runtime_error("Taking data from action server but nothing is ready"); + 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(std::shared_ptr & data) +ServerBase::execute(const std::shared_ptr & data_in) { - if (!data && !pimpl_->goal_expired_.load()) { - throw std::runtime_error("'data' is empty"); - } - - if (pimpl_->goal_request_ready_.load()) { - execute_goal_request_received(data); - } else if (pimpl_->cancel_request_ready_.load()) { - execute_cancel_request_received(data); - } else if (pimpl_->result_request_ready_.load()) { - execute_result_request_received(data); - } else if (pimpl_->goal_expired_.load()) { - execute_check_expired_goals(); - } else { - throw std::runtime_error("Executing action server but nothing is ready"); + 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(std::shared_ptr & data) +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) { - auto shared_ptr = std::static_pointer_cast - >>(data); - rcl_ret_t ret = std::get<0>(*shared_ptr); 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 @@ -299,14 +383,6 @@ ServerBase::execute_goal_request_received(std::shared_ptr & data) } else if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } - rcl_action_goal_info_t goal_info = std::get<1>(*shared_ptr); - rmw_request_id_t request_header = std::get<2>(*shared_ptr); - std::shared_ptr message = std::get<3>(*shared_ptr); - - bool expected = true; - if (!pimpl_->goal_request_ready_.compare_exchange_strong(expected, false)) { - return; - } GoalUUID uuid = get_goal_id_from_goal_request(message.get()); convert(uuid, &goal_info); @@ -323,7 +399,16 @@ ServerBase::execute_goal_request_received(std::shared_ptr & data) } if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(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; @@ -375,16 +460,14 @@ ServerBase::execute_goal_request_received(std::shared_ptr & data) // Tell user to start executing action call_goal_accepted_callback(handle, uuid, message); } - data.reset(); } void -ServerBase::execute_cancel_request_received(std::shared_ptr & data) +ServerBase::execute_cancel_request_received( + rcl_ret_t ret, + std::shared_ptr request, + rmw_request_id_t request_header) { - auto shared_ptr = std::static_pointer_cast - , - rmw_request_id_t>>(data); - auto ret = std::get<0>(*shared_ptr); 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 @@ -393,8 +476,6 @@ ServerBase::execute_cancel_request_received(std::shared_ptr & data) } else if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } - auto request = std::get<1>(*shared_ptr); - auto request_header = std::get<2>(*shared_ptr); // Convert c++ message to C message rcl_action_cancel_request_t cancel_request = rcl_action_get_zero_initialized_cancel_request(); @@ -461,18 +542,26 @@ ServerBase::execute_cancel_request_received(std::shared_ptr & data) 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); } - data.reset(); } void -ServerBase::execute_result_request_received(std::shared_ptr & data) +ServerBase::execute_result_request_received( + rcl_ret_t ret, + std::shared_ptr result_request, + rmw_request_id_t request_header) { - auto shared_ptr = std::static_pointer_cast - , rmw_request_id_t>>(data); - auto ret = std::get<0>(*shared_ptr); 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 @@ -481,10 +570,7 @@ ServerBase::execute_result_request_received(std::shared_ptr & data) } else if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } - auto result_request = std::get<1>(*shared_ptr); - auto request_header = std::get<2>(*shared_ptr); - pimpl_->result_request_ready_ = false; std::shared_ptr result_response; // check if the goal exists @@ -516,11 +602,18 @@ ServerBase::execute_result_request_received(std::shared_ptr & data) 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); } } - data.reset(); } void @@ -649,7 +742,13 @@ ServerBase::publish_result(const GoalUUID & uuid, std::shared_ptr result_m 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 (RCL_RET_OK != ret) { + 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); } } @@ -676,3 +775,138 @@ ServerBase::publish_feedback(std::shared_ptr feedback_msg) 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 index f0709e19cb..7df0ebf0e1 100644 --- a/rclcpp_action/src/server_goal_handle.cpp +++ b/rclcpp_action/src/server_goal_handle.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include +#include -#include -#include +#include "rcl_action/action_server.h" +#include "rcl_action/goal_handle.h" -#include +#include "rclcpp_action/server_goal_handle.hpp" +#include "rclcpp/exceptions.hpp" namespace rclcpp_action { diff --git a/rclcpp_action/src/types.cpp b/rclcpp_action/src/types.cpp index 773702789e..7f2a71d688 100644 --- a/rclcpp_action/src/types.cpp +++ b/rclcpp_action/src/types.cpp @@ -15,25 +15,33 @@ #include "rclcpp_action/types.hpp" #include -#include namespace rclcpp_action { std::string to_string(const GoalUUID & goal_id) { - std::stringstream stream; - stream << std::hex; - for (const auto & element : goal_id) { - stream << static_cast(element); + 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 stream.str(); + return result; } void convert(const GoalUUID & goal_id, rcl_action_goal_info_t * info) { - for (size_t i = 0; i < 16; ++i) { + for (size_t i = 0; i < UUID_SIZE; ++i) { info->goal_id.uuid[i] = goal_id[i]; } } @@ -41,7 +49,7 @@ convert(const GoalUUID & goal_id, rcl_action_goal_info_t * info) void convert(const rcl_action_goal_info_t & info, GoalUUID * goal_id) { - for (size_t i = 0; i < 16; ++i) { + for (size_t i = 0; i < UUID_SIZE; ++i) { (*goal_id)[i] = info.goal_id.uuid[i]; } } diff --git a/rclcpp_action/test/benchmark/CMakeLists.txt b/rclcpp_action/test/benchmark/CMakeLists.txt index dc87c553af..0ce6c6245c 100644 --- a/rclcpp_action/test/benchmark/CMakeLists.txt +++ b/rclcpp_action/test/benchmark/CMakeLists.txt @@ -9,8 +9,7 @@ add_performance_test( benchmark_action_client.cpp TIMEOUT 240) if(TARGET benchmark_action_client) - target_link_libraries(benchmark_action_client ${PROJECT_NAME}) - ament_target_dependencies(benchmark_action_client rclcpp test_msgs) + target_link_libraries(benchmark_action_client ${PROJECT_NAME} rclcpp::rclcpp ${test_msgs_TARGETS}) endif() add_performance_test( @@ -18,6 +17,5 @@ add_performance_test( benchmark_action_server.cpp TIMEOUT 120) if(TARGET benchmark_action_server) - target_link_libraries(benchmark_action_server ${PROJECT_NAME}) - ament_target_dependencies(benchmark_action_server rclcpp test_msgs) + 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 index 72d221883e..8b11935117 100644 --- a/rclcpp_action/test/benchmark/benchmark_action_client.cpp +++ b/rclcpp_action/test/benchmark/benchmark_action_client.cpp @@ -122,6 +122,7 @@ BENCHMARK_F(ActionClientPerformanceTest, construct_client_without_server)(benchm { 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 @@ -136,6 +137,7 @@ BENCHMARK_F(ActionClientPerformanceTest, construct_client_with_server)(benchmark 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 @@ -148,6 +150,7 @@ BENCHMARK_F(ActionClientPerformanceTest, construct_client_with_server)(benchmark 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); @@ -170,6 +173,7 @@ BENCHMARK_F(ActionClientPerformanceTest, async_send_goal_only)(benchmark::State reset_heap_counters(); for (auto _ : state) { + (void)_; auto future_goal_handle = client->async_send_goal(goal); } } @@ -188,6 +192,7 @@ BENCHMARK_F(ActionClientPerformanceTest, async_send_goal_rejected)(benchmark::St 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()) { @@ -215,6 +220,7 @@ BENCHMARK_F(ActionClientPerformanceTest, async_send_goal_get_accepted_response)( 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)); @@ -246,6 +252,7 @@ BENCHMARK_F(ActionClientPerformanceTest, async_get_result)(benchmark::State & st 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); @@ -298,6 +305,7 @@ BENCHMARK_F(ActionClientPerformanceTest, async_cancel_goal)(benchmark::State & s reset_heap_counters(); for (auto _ : state) { + (void)_; state.PauseTiming(); auto future_goal_handle = client->async_send_goal(goal); @@ -333,6 +341,7 @@ BENCHMARK_F(ActionClientPerformanceTest, async_cancel_all_goals)(benchmark::Stat 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); diff --git a/rclcpp_action/test/benchmark/benchmark_action_server.cpp b/rclcpp_action/test/benchmark/benchmark_action_server.cpp index 3ac7fc5812..6817f86b14 100644 --- a/rclcpp_action/test/benchmark/benchmark_action_server.cpp +++ b/rclcpp_action/test/benchmark/benchmark_action_server.cpp @@ -67,6 +67,7 @@ BENCHMARK_F(ActionServerPerformanceTest, construct_server_without_client)(benchm { 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) { @@ -88,6 +89,7 @@ BENCHMARK_F(ActionServerPerformanceTest, construct_server_without_client)(benchm 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) { @@ -109,6 +111,7 @@ BENCHMARK_F(ActionServerPerformanceTest, construct_server_with_client)(benchmark 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, @@ -144,6 +147,7 @@ BENCHMARK_F(ActionServerPerformanceTest, action_server_accept_goal)(benchmark::S reset_heap_counters(); for (auto _ : state) { + (void)_; state.PauseTiming(); auto client_goal_handle_future = AsyncSendGoalOfOrder(1); state.ResumeTiming(); @@ -176,6 +180,7 @@ BENCHMARK_F(ActionServerPerformanceTest, action_server_cancel_goal)(benchmark::S 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 @@ -212,6 +217,7 @@ BENCHMARK_F(ActionServerPerformanceTest, action_server_execute_goal)(benchmark:: reset_heap_counters(); for (auto _ : state) { + (void)_; state.PauseTiming(); auto client_goal_handle_future = AsyncSendGoalOfOrder(1); @@ -257,6 +263,7 @@ BENCHMARK_F(ActionServerPerformanceTest, action_server_set_success)(benchmark::S reset_heap_counters(); for (auto _ : state) { + (void)_; state.PauseTiming(); auto client_goal_handle_future = AsyncSendGoalOfOrder(goal_order); @@ -301,6 +308,7 @@ BENCHMARK_F(ActionServerPerformanceTest, action_server_abort)(benchmark::State & reset_heap_counters(); for (auto _ : state) { + (void)_; state.PauseTiming(); auto client_goal_handle_future = AsyncSendGoalOfOrder(goal_order); diff --git a/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp index ae01964fd5..08093cb873 100644 --- a/rclcpp_action/test/test_client.cpp +++ b/rclcpp_action/test/test_client.cpp @@ -12,27 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - -#include -#include -#include - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include - #include #include #include @@ -41,13 +20,34 @@ #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" +#include "mocking_utils/patch.hpp" using namespace std::chrono_literals; @@ -397,13 +397,13 @@ TEST_F(TestClient, is_ready) { ASSERT_EQ( RCL_RET_OK, rcl_wait_set_init(&wait_set, 10, 10, 10, 10, 10, 10, rcl_context, allocator)); - ASSERT_TRUE(action_client->add_to_wait_set(&wait_set)); - EXPECT_TRUE(action_client->is_ready(&wait_set)); + 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); + EXPECT_THROW(action_client->is_ready(wait_set), rclcpp::exceptions::RCLError); } client_node.reset(); // Drop node before action client } @@ -499,8 +499,7 @@ TEST_F(TestClientAgainstServer, async_send_goal_with_goal_response_callback_wait 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) + [&goal_response_received](typename ActionGoalHandle::SharedPtr goal_handle) { if (goal_handle) { goal_response_received = true; @@ -536,64 +535,6 @@ TEST_F(TestClientAgainstServer, async_send_goal_with_goal_response_callback_wait } } -TEST_F(TestClientAgainstServer, async_send_goal_with_deprecated_goal_response_callback) -{ - 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(); - -#if !defined(_WIN32) -# pragma GCC diagnostic push -# pragma GCC diagnostic ignored "-Wdeprecated-declarations" -#else -# pragma warning(push) -# pragma warning(disable: 4996) -#endif - send_goal_ops.goal_response_callback = - [&goal_response_received] - (std::shared_future goal_future) - { - if (goal_future.get()) { - goal_response_received = true; - } - }; -#if !defined(_WIN32) -# pragma GCC diagnostic pop -#else -# pragma warning(pop) -#endif - - { - 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); @@ -603,8 +544,7 @@ TEST_F(TestClientAgainstServer, async_send_goal_with_feedback_callback_wait_for_ goal.order = 4; int feedback_count = 0; auto send_goal_ops = rclcpp_action::Client::SendGoalOptions(); - send_goal_ops.feedback_callback = - [&feedback_count]( + send_goal_ops.feedback_callback = [&feedback_count]( typename ActionGoalHandle::SharedPtr goal_handle, const std::shared_ptr feedback) { @@ -910,6 +850,86 @@ TEST_F(TestClientAgainstServer, async_cancel_some_goals_with_callback) 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); @@ -918,9 +938,8 @@ TEST_F(TestClientAgainstServer, send_rcl_errors) 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) {}; + send_goal_ops.feedback_callback = []( + typename ActionGoalHandle::SharedPtr, const std::shared_ptr) {}; { ActionGoal goal; @@ -960,9 +979,8 @@ TEST_F(TestClientAgainstServer, execute_rcl_errors) 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) {}; + send_goal_ops.feedback_callback = []( + typename ActionGoalHandle::SharedPtr, const std::shared_ptr) {}; { ActionGoal goal; diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index 6f8d874359..c63d1dfdfc 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -12,22 +12,22 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include -#include -#include - -#include - #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" +#include "mocking_utils/patch.hpp" using Fibonacci = test_msgs::action::Fibonacci; using CancelResponse = typename Fibonacci::Impl::CancelGoalService::Response; @@ -205,6 +205,27 @@ TEST_F(TestServer, construction_and_destruction_wait_set_error) }, 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"); @@ -485,10 +506,10 @@ TEST_F(TestServer, publish_status_accepted) (void)as; // Subscribe to status messages - std::vector received_msgs; + std::vector received_msgs; auto subscriber = node->create_subscription( "fibonacci/_action/status", 10, - [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list) + [&received_msgs](action_msgs::msg::GoalStatusArray::ConstSharedPtr list) { received_msgs.push_back(list); }); @@ -548,10 +569,10 @@ TEST_F(TestServer, publish_status_canceling) (void)as; // Subscribe to status messages - std::vector received_msgs; + std::vector received_msgs; auto subscriber = node->create_subscription( "fibonacci/_action/status", 10, - [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list) + [&received_msgs](action_msgs::msg::GoalStatusArray::ConstSharedPtr list) { received_msgs.push_back(list); }); @@ -605,10 +626,10 @@ TEST_F(TestServer, publish_status_canceled) (void)as; // Subscribe to status messages - std::vector received_msgs; + std::vector received_msgs; auto subscriber = node->create_subscription( "fibonacci/_action/status", 10, - [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list) + [&received_msgs](action_msgs::msg::GoalStatusArray::ConstSharedPtr list) { received_msgs.push_back(list); }); @@ -664,10 +685,10 @@ TEST_F(TestServer, publish_status_succeeded) (void)as; // Subscribe to status messages - std::vector received_msgs; + std::vector received_msgs; auto subscriber = node->create_subscription( "fibonacci/_action/status", 10, - [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list) + [&received_msgs](action_msgs::msg::GoalStatusArray::ConstSharedPtr list) { received_msgs.push_back(list); }); @@ -721,10 +742,10 @@ TEST_F(TestServer, publish_status_aborted) (void)as; // Subscribe to status messages - std::vector received_msgs; + std::vector received_msgs; auto subscriber = node->create_subscription( "fibonacci/_action/status", 10, - [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list) + [&received_msgs](action_msgs::msg::GoalStatusArray::ConstSharedPtr list) { received_msgs.push_back(list); }); @@ -779,9 +800,9 @@ TEST_F(TestServer, publish_feedback) // Subscribe to feedback messages using FeedbackT = Fibonacci::Impl::FeedbackMessage; - std::vector received_msgs; + std::vector received_msgs; auto subscriber = node->create_subscription( - "fibonacci/_action/feedback", 10, [&received_msgs](FeedbackT::SharedPtr msg) + "fibonacci/_action/feedback", 10, [&received_msgs](FeedbackT::ConstSharedPtr msg) { received_msgs.push_back(msg); }); @@ -1086,12 +1107,15 @@ TEST_F(TestGoalRequestServer, is_ready_rcl_error) { { EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)); }); - ASSERT_TRUE(action_server_->add_to_wait_set(&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()); - EXPECT_TRUE(action_server_->is_ready(&wait_set)); 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); + EXPECT_THROW(action_server_->is_ready(wait_set), rclcpp::exceptions::RCLError); } TEST_F(TestGoalRequestServer, execute_goal_request_received_take_goal_request_errors) diff --git a/rclcpp_action/test/test_server_goal_handle.cpp b/rclcpp_action/test/test_server_goal_handle.cpp index cd56ba4a8f..eb392c8bf7 100644 --- a/rclcpp_action/test/test_server_goal_handle.cpp +++ b/rclcpp_action/test/test_server_goal_handle.cpp @@ -12,19 +12,19 @@ // 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 "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" +#include "mocking_utils/patch.hpp" class FibonacciServerGoalHandle : public rclcpp_action::ServerGoalHandle diff --git a/rclcpp_action/test/test_types.cpp b/rclcpp_action/test/test_types.cpp index 7c652aaad6..619a4f7899 100644 --- a/rclcpp_action/test/test_types.cpp +++ b/rclcpp_action/test/test_types.cpp @@ -15,6 +15,7 @@ #include #include +#include #include "rclcpp_action/types.hpp" TEST(TestActionTypes, goal_uuid_to_string) { @@ -22,17 +23,17 @@ TEST(TestActionTypes, goal_uuid_to_string) { for (uint8_t i = 0; i < UUID_SIZE; ++i) { goal_id[i] = i; } - EXPECT_STREQ("0123456789abcdef", rclcpp_action::to_string(goal_id).c_str()); + 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("101112131415161718191a1b1c1d1e1f", rclcpp_action::to_string(goal_id).c_str()); + 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("fffefdfcfbfaf9f8f7f6f5f4f3f2f1f0", rclcpp_action::to_string(goal_id).c_str()); + EXPECT_STREQ("fffefdfc-fbfa-f9f8-f7f6-f5f4f3f2f1f0", rclcpp_action::to_string(goal_id).c_str()); } TEST(TestActionTypes, goal_uuid_to_rcl_action_goal_info) { @@ -54,8 +55,40 @@ TEST(TestActionTypes, rcl_action_goal_info_to_goal_uuid) { } rclcpp_action::GoalUUID goal_id; - rclcpp_action::convert(goal_id, &goal_info); + 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 index 3fd10b6d81..cc55217333 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,201 @@ 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) ------------------- diff --git a/rclcpp_components/CMakeLists.txt b/rclcpp_components/CMakeLists.txt index 5098fd8631..7d4135051c 100644 --- a/rclcpp_components/CMakeLists.txt +++ b/rclcpp_components/CMakeLists.txt @@ -2,9 +2,10 @@ cmake_minimum_required(VERSION 3.5) project(rclcpp_components) -# 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( @@ -20,6 +21,15 @@ 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 @@ -27,13 +37,15 @@ add_library( ) target_include_directories(component_manager PUBLIC "$" - "$") -ament_target_dependencies(component_manager - "ament_index_cpp" - "class_loader" - "composition_interfaces" - "rclcpp" - "rcpputils" + "$") +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") @@ -42,10 +54,7 @@ add_executable( component_container src/component_container.cpp ) -target_link_libraries(component_container component_manager) -ament_target_dependencies(component_container - "rclcpp" -) +target_link_libraries(component_container component_manager rclcpp::rclcpp) set(node_main_template_install_dir "share/${PROJECT_NAME}") install(FILES @@ -56,14 +65,18 @@ add_executable( component_container_mt src/component_container_mt.cpp ) -target_link_libraries(component_container_mt component_manager) -ament_target_dependencies(component_container_mt - "rclcpp" +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) @@ -76,10 +89,7 @@ if(BUILD_TESTING) set(components "") add_library(test_component SHARED test/components/test_component.cpp) - target_include_directories(test_component PUBLIC include) - ament_target_dependencies(test_component - "class_loader" - "rclcpp") + 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") @@ -127,7 +137,7 @@ if(BUILD_TESTING) endif() install( - TARGETS component_manager EXPORT component_manager + TARGETS component component_manager EXPORT export_${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -135,14 +145,14 @@ install( # Install executables install( - TARGETS component_container component_container_mt + TARGETS component_container component_container_mt component_container_isolated RUNTIME DESTINATION lib/${PROJECT_NAME} ) # Install include directories install( DIRECTORY include/ - DESTINATION include + DESTINATION include/${PROJECT_NAME} ) # Install cmake @@ -151,10 +161,14 @@ install( DESTINATION share/${PROJECT_NAME} ) -# specific order: dependents before dependencies -ament_export_include_directories(include) +# Export old-style CMake variables +ament_export_include_directories("include/${PROJECT_NAME}") ament_export_libraries(component_manager) -ament_export_targets(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) diff --git a/rclcpp_components/Doxyfile b/rclcpp_components/Doxyfile index da33050c4a..11fe1e178f 100644 --- a/rclcpp_components/Doxyfile +++ b/rclcpp_components/Doxyfile @@ -22,12 +22,12 @@ 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/" +#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/" +#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 index 5b8efa934d..2e38c0410a 100644 --- a/rclcpp_components/QUALITY_DECLARATION.md +++ b/rclcpp_components/QUALITY_DECLARATION.md @@ -4,7 +4,7 @@ This document is a declaration of software quality for the `rclcpp_components` p 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 Quality Categories in REP-2004](https://www.ros.org/reps/rep-2004.html) of the ROS2 developer guide. +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] @@ -92,7 +92,7 @@ There is an automated test which runs a linter that ensures each file has at lea ### 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/master/test) directory. +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: @@ -125,7 +125,7 @@ Current coverage statistics can be viewed [here](https://ci.ros2.org/job/nightly `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/master/rclcpp_components/test/benchmark). +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/) @@ -135,7 +135,7 @@ Changes that introduce regressions in performance must be adequately justified i ### Linters and Static Analysis [4.v] -`rclcpp_components` uses and passes all the ROS2 standard linters and static analysis tools for a C++ package as described in the [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#linters-and-static-analysis). Passing implies there are no linter/static errors when testing against CI of supported platforms. +`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/) @@ -159,7 +159,7 @@ It also has several test dependencies, which do not affect the resulting quality 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/master/ament_index_cpp/QUALITY_DECLARATION.md). +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` @@ -171,19 +171,19 @@ It is **Quality Level 1**, see its [Quality Declaration document](https://github 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/master/composition_interfaces/QUALITY_DECLARATION.md). +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/master/rclcpp/QUALITY_DECLARATION.md). +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/master/QUALITY_DECLARATION.md). +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] diff --git a/rclcpp_components/README.md b/rclcpp_components/README.md index 829bcbc888..fcc4c04ea0 100644 --- a/rclcpp_components/README.md +++ b/rclcpp_components/README.md @@ -2,7 +2,7 @@ Package containing tools for dynamically loadable components. -Visit the [rclcpp_components API documentation](http://docs.ros2.org/latest/api/rclcpp_components/) for a complete list of its main components and features. +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 diff --git a/rclcpp_components/cmake/rclcpp_components_register_node.cmake b/rclcpp_components/cmake/rclcpp_components_register_node.cmake index 1a9d0cb1c2..0c5bd33b91 100644 --- a/rclcpp_components/cmake/rclcpp_components_register_node.cmake +++ b/rclcpp_components/cmake/rclcpp_components_register_node.cmake @@ -24,11 +24,13 @@ # :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;RESOURCE_INDEX" "" ${ARGN}) + 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}") @@ -46,6 +48,13 @@ macro(rclcpp_components_register_node target) 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() @@ -63,10 +72,11 @@ macro(rclcpp_components_register_node target) 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) - ament_target_dependencies(${node} - "rclcpp" - "class_loader" - "rclcpp_components") + target_link_libraries(${node} + class_loader::class_loader + rclcpp::rclcpp + rclcpp_components::component + ) install(TARGETS ${node} DESTINATION lib/${PROJECT_NAME}) diff --git a/rclcpp_components/include/rclcpp_components/component_manager.hpp b/rclcpp_components/include/rclcpp_components/component_manager.hpp index b2f1de0827..9d98bd89d4 100644 --- a/rclcpp_components/include/rclcpp_components/component_manager.hpp +++ b/rclcpp_components/include/rclcpp_components/component_manager.hpp @@ -99,7 +99,8 @@ class ComponentManager : public rclcpp::Node */ RCLCPP_COMPONENTS_PUBLIC ComponentManager( - std::weak_ptr executor, + std::weak_ptr executor = + std::weak_ptr(), std::string node_name = "ComponentManager", const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions() .start_parameter_services(false) @@ -130,6 +131,14 @@ class ComponentManager : public rclcpp::Node 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 /** @@ -140,6 +149,22 @@ class ComponentManager : public rclcpp::Node 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 @@ -160,20 +185,6 @@ class ComponentManager : public rclcpp::Node const std::shared_ptr request, std::shared_ptr response); - /** - * \deprecated Use on_load_node() instead - */ - [[deprecated("Use on_load_node() instead")]] - RCLCPP_COMPONENTS_PUBLIC - virtual void - OnLoadNode( - const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response) - { - on_load_node(request_header, request, response); - } - /// Service callback to unload a node in the component /** * \param request_header unused @@ -188,20 +199,6 @@ class ComponentManager : public rclcpp::Node const std::shared_ptr request, std::shared_ptr response); - /** - * \deprecated Use on_unload_node() instead - */ - [[deprecated("Use on_unload_node() instead")]] - RCLCPP_COMPONENTS_PUBLIC - virtual void - OnUnloadNode( - const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response) - { - on_unload_node(request_header, request, 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. @@ -217,21 +214,7 @@ class ComponentManager : public rclcpp::Node const std::shared_ptr request, std::shared_ptr response); - /** - * \deprecated Use on_list_nodes() instead - */ - [[deprecated("Use on_list_nodes() instead")]] - RCLCPP_COMPONENTS_PUBLIC - virtual void - OnListNodes( - const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response) - { - on_list_nodes(request_header, request, response); - } - -private: +protected: std::weak_ptr executor_; uint64_t unique_id_ {1}; 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 index 67e6cd7331..7b1f2dcae6 100644 --- a/rclcpp_components/include/rclcpp_components/node_factory.hpp +++ b/rclcpp_components/include/rclcpp_components/node_factory.hpp @@ -15,6 +15,7 @@ #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 diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index 03613b8e0c..ff9c20a182 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,12 +2,16 @@ rclcpp_components - 12.0.0 + 29.2.0 Package containing tools for dynamically loadable components + Ivan Paunovic - Mabel Zhang + Michel Hidalgo William Woodall + Apache License 2.0 + + Jacob Perron Michael Carroll ament_cmake_ros 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 index aa4784e426..9dcbade712 100644 --- a/rclcpp_components/src/component_container_mt.cpp +++ b/rclcpp_components/src/component_container_mt.cpp @@ -22,8 +22,17 @@ 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(exec); + 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 index cc2157f0d5..f73cd8954a 100644 --- a/rclcpp_components/src/component_manager.cpp +++ b/rclcpp_components/src/component_manager.cpp @@ -14,6 +14,7 @@ #include "rclcpp_components/component_manager.hpp" +#include #include #include #include @@ -39,13 +40,27 @@ ComponentManager::ComponentManager( { loadNode_srv_ = create_service( "~/_container/load_node", - std::bind(&ComponentManager::on_load_node, this, _1, _2, _3)); + 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)); + 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() @@ -81,11 +96,11 @@ ComponentManager::get_component_resources( throw ComponentManagerException("Invalid resource entry"); } - std::string library_path = parts[1]; - if (!rcpputils::fs::path(library_path).is_absolute()) { - library_path = base_path + "/" + library_path; + 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}); + resources.push_back({parts[0], library_path.string()}); } return resources; } @@ -160,12 +175,46 @@ ComponentManager::create_node_options(const std::shared_ptr r "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, @@ -214,10 +263,9 @@ ComponentManager::on_load_node( throw ComponentManagerException("Component constructor threw an exception"); } + add_node_to_executor(node_id); + auto node = node_wrappers_[node_id].get_node_base_interface(); - if (auto exec = executor_.lock()) { - exec->add_node(node, true); - } response->full_node_name = node->get_fully_qualified_name(); response->unique_id = node_id; response->success = true; @@ -253,9 +301,7 @@ ComponentManager::on_unload_node( response->error_message = ss.str(); RCLCPP_WARN(get_logger(), "%s", ss.str().c_str()); } else { - if (auto exec = executor_.lock()) { - exec->remove_node(wrapper->second.get_node_base_interface()); - } + remove_node_from_executor(request->unique_id); node_wrappers_.erase(wrapper); response->success = true; } diff --git a/rclcpp_components/src/node_main.cpp.in b/rclcpp_components/src/node_main.cpp.in index bfebc4aa7d..7d621aac9f 100644 --- a/rclcpp_components/src/node_main.cpp.in +++ b/rclcpp_components/src/node_main.cpp.in @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include #include @@ -23,42 +24,56 @@ #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); - rclcpp::executors::SingleThreadedExecutor exec; + @executor@ exec; rclcpp::NodeOptions options; options.arguments(args); - std::vector loaders; - std::vector node_wrappers; 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 = new class_loader::ClassLoader(library_name); - auto classes = loader->getAvailableClasses(); - for (auto clazz : classes) { - std::string name = clazz.c_str(); - if (!(name.compare(class_name))) { - RCLCPP_DEBUG(logger, "Instantiate class %s", clazz.c_str()); - auto node_factory = loader->createInstance(clazz); - auto wrapper = node_factory->create_node_instance(options); - auto node = wrapper.get_node_base_interface(); - node_wrappers.push_back(wrapper); - exec.add_node(node); - } - } - loaders.push_back(loader); + 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.spin(); - for (auto wrapper : node_wrappers) { - exec.remove_node(wrapper.get_node_base_interface()); + exec.remove_node(node_wrapper.get_node_base_interface()); } - node_wrappers.clear(); rclcpp::shutdown(); diff --git a/rclcpp_components/test/benchmark/benchmark_components.cpp b/rclcpp_components/test/benchmark/benchmark_components.cpp index 6fbe6df107..26d607f5fb 100644 --- a/rclcpp_components/test/benchmark/benchmark_components.cpp +++ b/rclcpp_components/test/benchmark/benchmark_components.cpp @@ -74,6 +74,7 @@ class ComponentTest : public benchmark::Fixture 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) { @@ -93,6 +94,7 @@ BENCHMARK_F(ComponentTest, create_component_factory)(benchmark::State & state) } for (auto _ : state) { + (void)_; manager->create_component_factory(resources[0]).reset(); } } @@ -107,13 +109,14 @@ BENCHMARK_F(ComponentTest, create_node_instance)(benchmark::State & state) } // Choosing resource 0 - the other two test components were shown empirically to yield - // the same performance charactarisitics, so they shouldn't need their own benchmarks. + // 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/test_component_manager.cpp b/rclcpp_components/test/test_component_manager.cpp index ebc86ff6f0..486e1c6a28 100644 --- a/rclcpp_components/test/test_component_manager.cpp +++ b/rclcpp_components/test/test_component_manager.cpp @@ -14,6 +14,7 @@ #include +#include #include #include "rclcpp_components/component_manager.hpp" @@ -27,6 +28,11 @@ class TestComponentManager : public ::testing::Test { rclcpp::init(0, nullptr); } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } }; TEST_F(TestComponentManager, get_component_resources_invalid) @@ -51,7 +57,7 @@ TEST_F(TestComponentManager, get_component_resources_valid) EXPECT_EQ("test_rclcpp_components::TestComponentBar", resources[1].first); EXPECT_EQ("test_rclcpp_components::TestComponentNoNode", resources[2].first); - namespace fs = rcpputils::fs; + 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))); diff --git a/rclcpp_components/test/test_component_manager_api.cpp b/rclcpp_components/test/test_component_manager_api.cpp index 73d4bc3687..71f32b480c 100644 --- a/rclcpp_components/test/test_component_manager_api.cpp +++ b/rclcpp_components/test/test_component_manager_api.cpp @@ -22,6 +22,7 @@ #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; @@ -32,15 +33,27 @@ class TestComponentManager : public ::testing::Test { 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 -TEST_F(TestComponentManager, components_api) +void test_components_api(bool use_dedicated_executor) { auto exec = std::make_shared(); auto node = rclcpp::Node::make_shared("test_component_manager"); - auto manager = std::make_shared(exec); + 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); @@ -57,13 +70,14 @@ TEST_F(TestComponentManager, components_api) request->package_name = "rclcpp_components"; request->plugin_name = "test_rclcpp_components::TestComponentFoo"; - auto result = composition_client->async_send_request(request); - auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result. + 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.get()->success, true); - EXPECT_EQ(result.get()->error_message, ""); - EXPECT_EQ(result.get()->full_node_name, "/test_component_foo"); - EXPECT_EQ(result.get()->unique_id, 1u); + 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); } { @@ -71,13 +85,14 @@ TEST_F(TestComponentManager, components_api) request->package_name = "rclcpp_components"; request->plugin_name = "test_rclcpp_components::TestComponentBar"; - auto result = composition_client->async_send_request(request); - auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result. + 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.get()->success, true); - EXPECT_EQ(result.get()->error_message, ""); - EXPECT_EQ(result.get()->full_node_name, "/test_component_bar"); - EXPECT_EQ(result.get()->unique_id, 2u); + 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 @@ -87,13 +102,14 @@ TEST_F(TestComponentManager, components_api) request->plugin_name = "test_rclcpp_components::TestComponentFoo"; request->node_name = "test_component_baz"; - auto result = composition_client->async_send_request(request); - auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result. + 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.get()->success, true); - EXPECT_EQ(result.get()->error_message, ""); - EXPECT_EQ(result.get()->full_node_name, "/test_component_baz"); - EXPECT_EQ(result.get()->unique_id, 3u); + 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 @@ -104,13 +120,14 @@ TEST_F(TestComponentManager, components_api) request->node_namespace = "/ns"; request->node_name = "test_component_bing"; - auto result = composition_client->async_send_request(request); - auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result. + 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.get()->success, true); - EXPECT_EQ(result.get()->error_message, ""); - EXPECT_EQ(result.get()->full_node_name, "/ns/test_component_bing"); - EXPECT_EQ(result.get()->unique_id, 4u); + 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); } { @@ -119,13 +136,14 @@ TEST_F(TestComponentManager, components_api) request->package_name = "rclcpp_components"; request->plugin_name = "test_rclcpp_components::TestComponent"; - auto result = composition_client->async_send_request(request); - auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result. + 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); - EXPECT_EQ(result.get()->success, false); - EXPECT_EQ(result.get()->error_message, "Failed to find class with the requested plugin name."); - EXPECT_EQ(result.get()->full_node_name, ""); - EXPECT_EQ(result.get()->unique_id, 0u); + 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); } { @@ -134,13 +152,14 @@ TEST_F(TestComponentManager, components_api) request->package_name = "rclcpp_components_foo"; request->plugin_name = "test_rclcpp_components::TestComponentFoo"; - auto result = composition_client->async_send_request(request); - auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result. + 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); - EXPECT_EQ(result.get()->success, false); - EXPECT_EQ(result.get()->error_message, "Could not find requested resource in ament index"); - EXPECT_EQ(result.get()->full_node_name, ""); - EXPECT_EQ(result.get()->unique_id, 0u); + 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); } { @@ -151,13 +170,14 @@ TEST_F(TestComponentManager, components_api) request->node_name = "test_component_remap"; request->remap_rules.push_back("alice:=bob"); - auto result = composition_client->async_send_request(request); - auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result. + 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.get()->success, true); - EXPECT_EQ(result.get()->error_message, ""); - EXPECT_EQ(result.get()->full_node_name, "/test_component_remap"); - EXPECT_EQ(result.get()->unique_id, 5u); + 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); } { @@ -170,14 +190,15 @@ TEST_F(TestComponentManager, components_api) rclcpp::ParameterValue(true)); request->extra_arguments.push_back(use_intraprocess_comms.to_parameter_msg()); - auto result = composition_client->async_send_request(request); - auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result. + 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.get()->success, true); - EXPECT_EQ(result.get()->error_message, ""); - std::cout << result.get()->full_node_name << std::endl; - EXPECT_EQ(result.get()->full_node_name, "/test_component_intra_process"); - EXPECT_EQ(result.get()->unique_id, 6u); + 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); } { @@ -191,15 +212,59 @@ TEST_F(TestComponentManager, components_api) rclcpp::ParameterValue("hello")); request->extra_arguments.push_back(use_intraprocess_comms.to_parameter_msg()); - auto result = composition_client->async_send_request(request); - auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result. + 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.get()->success, false); + EXPECT_EQ(result->success, false); EXPECT_EQ( - result.get()->error_message, + result->error_message, "Extra component argument 'use_intra_process_comms' must be a boolean"); - EXPECT_EQ(result.get()->full_node_name, ""); - EXPECT_EQ(result.get()->unique_id, 0u); + 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(); @@ -223,26 +288,29 @@ TEST_F(TestComponentManager, components_api) { auto request = std::make_shared(); - auto result = client->async_send_request(request); - auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result. + 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_node_names = result.get()->full_node_names; - auto result_unique_ids = result.get()->unique_ids; + 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.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_unique_ids.size(), 6u); + 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); } } @@ -258,22 +326,24 @@ TEST_F(TestComponentManager, components_api) auto request = std::make_shared(); request->unique_id = 1; - auto result = client->async_send_request(request); - auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result. + 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.get()->success, true); - EXPECT_EQ(result.get()->error_message, ""); + EXPECT_EQ(result->success, true); + EXPECT_EQ(result->error_message, ""); } { auto request = std::make_shared(); request->unique_id = 1; - auto result = client->async_send_request(request); - auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result. + 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.get()->success, false); - EXPECT_EQ(result.get()->error_message, "No node found with unique_id: 1"); + EXPECT_EQ(result->success, false); + EXPECT_EQ(result->error_message, "No node found with unique_id: 1"); } } @@ -287,24 +357,39 @@ TEST_F(TestComponentManager, components_api) { auto request = std::make_shared(); - auto result = client->async_send_request(request); - auto ret = exec->spin_until_future_complete(result, 5s); // Wait for the result. + 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_node_names = result.get()->full_node_names; - auto result_unique_ids = result.get()->unique_ids; + 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(), 5u); + 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_unique_ids.size(), 5u); + 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 index 7a46abb7b5..0cf68a5891 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,240 @@ 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. diff --git a/rclcpp_lifecycle/CMakeLists.txt b/rclcpp_lifecycle/CMakeLists.txt index e73a5a3ffd..9a7335b1bf 100644 --- a/rclcpp_lifecycle/CMakeLists.txt +++ b/rclcpp_lifecycle/CMakeLists.txt @@ -2,23 +2,29 @@ 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 -Wnon-virtual-dtor -Woverloaded-virtual) endif() 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(rcutils REQUIRED) find_package(rosidl_typesupport_cpp REQUIRED) -find_package(lifecycle_msgs REQUIRED) ### CPP High level library add_library(rclcpp_lifecycle 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 @@ -26,13 +32,15 @@ add_library(rclcpp_lifecycle target_include_directories(${PROJECT_NAME} PUBLIC "$" - "$") -# specific order: dependents before dependencies -ament_target_dependencies(rclcpp_lifecycle - "rclcpp" - "rcl_lifecycle" - "lifecycle_msgs" - "rosidl_typesupport_cpp" + "$") +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, @@ -45,27 +53,33 @@ install(TARGETS 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() + 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}) - ament_target_dependencies(benchmark_lifecycle_client rclcpp) + 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}) - ament_target_dependencies(benchmark_lifecycle_node rclcpp) + target_link_libraries(benchmark_lifecycle_node ${PROJECT_NAME} rclcpp::rclcpp) endif() add_performance_test( benchmark_state @@ -81,87 +95,84 @@ if(BUILD_TESTING) 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) - ament_target_dependencies(test_lifecycle_node - "rcl_lifecycle" - "rclcpp" - "rcutils" - ) - target_link_libraries(test_lifecycle_node ${PROJECT_NAME} mimick) + 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) - ament_target_dependencies(test_lifecycle_publisher - "rcl_lifecycle" - "rclcpp" - "test_msgs" - ) - target_link_libraries(test_lifecycle_publisher ${PROJECT_NAME}) + 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) - ament_target_dependencies(test_lifecycle_service_client - "rcl_lifecycle" - "rclcpp" - "rcpputils" - "rcutils" - ) - target_link_libraries(test_lifecycle_service_client ${PROJECT_NAME} mimick) + 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) - ament_target_dependencies(test_state_machine_info - "rcl_lifecycle" - "rclcpp" - ) - 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) - ament_target_dependencies(test_register_custom_callbacks - "rcl_lifecycle" - "rclcpp" - ) - 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) - ament_target_dependencies(test_callback_exceptions - "rcl_lifecycle" - "rclcpp" - ) - 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) - ament_target_dependencies(test_state_wrapper - "rcl_lifecycle" - "rclcpp" - ) - 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) - ament_target_dependencies(test_transition_wrapper - "rcl_lifecycle" - "rclcpp" - "rcutils" - ) - target_link_libraries(test_transition_wrapper ${PROJECT_NAME} mimick) + 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 ) endif() endif() -# specific order: dependents before dependencies -ament_export_include_directories(include) +# 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(rclcpp) -ament_export_dependencies(rcl_lifecycle) -ament_export_dependencies(lifecycle_msgs) -ament_package() -install(DIRECTORY include/ - DESTINATION include) +# 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 index 9a83ee5e85..3f7644b82e 100644 --- a/rclcpp_lifecycle/Doxyfile +++ b/rclcpp_lifecycle/Doxyfile @@ -22,12 +22,13 @@ 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/" +#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/" +#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 index 5a37ad395b..73be7c6c9a 100644 --- a/rclcpp_lifecycle/QUALITY_DECLARATION.md +++ b/rclcpp_lifecycle/QUALITY_DECLARATION.md @@ -4,7 +4,7 @@ This document is a declaration of software quality for the `rclcpp_lifecycle` pa 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 Quality Categories in REP-2004](https://www.ros.org/reps/rep-2004.html) of the ROS2 developer guide. +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] @@ -92,7 +92,7 @@ There is an automated test which runs a linter that ensures each file has at lea ### 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/master/test) directory. +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: @@ -125,7 +125,7 @@ Current coverage statistics can be viewed [here](https://ci.ros2.org/job/nightly `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/master/rclcpp_lifecycle/test/benchmark). +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/) @@ -135,7 +135,7 @@ Changes that introduce regressions in performance must be adequately justified i ### Linters and Static Analysis [4.v] -`rclcpp_lifecycle` uses and passes all the ROS2 standard linters and static analysis tools for a C++ package as described in the [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#linters-and-static-analysis). Passing implies there are no linter/static errors when testing against CI of supported platforms. +`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/) @@ -159,31 +159,31 @@ It also has several test dependencies, which do not affect the resulting quality 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/master/lifecycle_msgs/QUALITY_DECLARATION.md). +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/master/rclcpp/QUALITY_DECLARATION.md). +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/master/rcl_lifecycle/QUALITY_DECLARATION.md). +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/master/rosidl_typesupport_cpp/QUALITY_DECLARATION.md). +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/master/rmw/QUALITY_DECLARATION.md). +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] diff --git a/rclcpp_lifecycle/README.md b/rclcpp_lifecycle/README.md index d5bcd96479..72bb50fbd2 100644 --- a/rclcpp_lifecycle/README.md +++ b/rclcpp_lifecycle/README.md @@ -2,7 +2,8 @@ Package containing a prototype for lifecycle implementation. -Visit the [rclcpp_lifecycle API documentation](http://docs.ros2.org/latest/api/rclcpp_lifecycle/) for a complete list of its main components and features. For more information about LifeCycle in ROS 2, see the [design document](http://design.ros2.org/articles/node_lifecycle.html). +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 diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 4442304c0c..d63cc726f9 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -36,6 +36,8 @@ #ifndef RCLCPP_LIFECYCLE__LIFECYCLE_NODE_HPP_ #define RCLCPP_LIFECYCLE__LIFECYCLE_NODE_HPP_ +#include +#include #include #include #include @@ -49,13 +51,11 @@ #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" @@ -72,6 +72,7 @@ #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" @@ -83,6 +84,8 @@ #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" @@ -137,7 +140,7 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, 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] namespace_ Namespace of the node. @@ -170,6 +173,15 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, 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. @@ -242,7 +254,7 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, ) ); - /// Create a timer. + /// Create a timer that uses the wall clock to drive the callback. /** * \param[in] period Time interval between triggers of the callback. * \param[in] callback User-defined callback function. @@ -255,15 +267,31 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, CallbackT callback, rclcpp::CallbackGroup::SharedPtr group = nullptr); + /// 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::GenericTimer::SharedPtr + create_timer( + std::chrono::duration period, + CallbackT callback, + rclcpp::CallbackGroup::SharedPtr group = nullptr); + /// Create and return a Client. /** - * \sa rclcpp::Node::create_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, + const rclcpp::QoS & qos = rclcpp::ServicesQoS(), rclcpp::CallbackGroup::SharedPtr group = nullptr); /// Create and return a Service. @@ -275,7 +303,7 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, create_service( const std::string & service_name, CallbackT && callback, - const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default, + const rclcpp::QoS & qos = rclcpp::ServicesQoS(), rclcpp::CallbackGroup::SharedPtr group = nullptr); /// Create and return a GenericPublisher. @@ -333,22 +361,6 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, rcl_interfaces::msg::ParameterDescriptor{}, bool ignore_override = false); - /// Declare a parameter - [[deprecated( - "declare_parameter() with only a name is deprecated and will be deleted in the future.\n" \ - "If you want to declare a parameter that won't change type without a default value use:\n" \ - "`node->declare_parameter(name)`, where e.g. ParameterT=int64_t.\n\n" \ - "If you want to declare a parameter that can dynamically change type use:\n" \ - "```\n" \ - "rcl_interfaces::msg::ParameterDescriptor descriptor;\n" \ - "descriptor.dynamic_typing = true;\n" \ - "node->declare_parameter(name, rclcpp::ParameterValue{}, descriptor);\n" \ - "```" - )]] - RCLCPP_LIFECYCLE_PUBLIC - const rclcpp::ParameterValue & - declare_parameter(const std::string & name); - /// Declare and initialize a parameter with a type. /** * \sa rclcpp::Node::declare_parameter @@ -524,10 +536,30 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, rcl_interfaces::msg::ListParametersResult list_parameters(const std::vector & prefixes, uint64_t depth) const; + using PreSetParametersCallbackHandle = + rclcpp::node_interfaces::PreSetParametersCallbackHandle; + using PreSetParametersCallbackType = + rclcpp::node_interfaces::NodeParametersInterface::PreSetParametersCallbackType; + using OnSetParametersCallbackHandle = rclcpp::node_interfaces::OnSetParametersCallbackHandle; - using OnParametersSetCallbackType = - rclcpp::node_interfaces::NodeParametersInterface::OnParametersSetCallbackType; + 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. /** @@ -537,7 +569,26 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, RCUTILS_WARN_UNUSED rclcpp_lifecycle::LifecycleNode::OnSetParametersCallbackHandle::SharedPtr add_on_set_parameters_callback( - rclcpp_lifecycle::LifecycleNode::OnParametersSetCallbackType 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`. /** @@ -548,6 +599,15 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, 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 + 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 @@ -601,6 +661,22 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, 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 @@ -735,6 +811,14 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, 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 @@ -760,7 +844,7 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, */ RCLCPP_LIFECYCLE_PUBLIC const State & - get_current_state(); + get_current_state() const; /// Return a list with the available states. /** @@ -768,7 +852,7 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, */ RCLCPP_LIFECYCLE_PUBLIC std::vector - get_available_states(); + get_available_states() const; /// Return a list with the current available transitions. /** @@ -776,7 +860,7 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, */ RCLCPP_LIFECYCLE_PUBLIC std::vector - get_available_transitions(); + get_available_transitions() const; /// Return a list with all the transitions. /** @@ -784,7 +868,7 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, */ RCLCPP_LIFECYCLE_PUBLIC std::vector - get_transition_graph(); + get_transition_graph() const; /// Trigger the specified transition. /* @@ -968,10 +1052,18 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, bool 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 @@ -989,6 +1081,7 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, 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_; diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp index a7f1c686bc..b1e73b6c64 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp @@ -15,28 +15,35 @@ #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 "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/event.hpp" -#include "rclcpp/experimental/intra_process_manager.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 { @@ -49,11 +56,13 @@ LifecycleNode::create_publisher( const rclcpp::PublisherOptionsWithAllocator & options) { using PublisherT = rclcpp_lifecycle::LifecyclePublisher; - return rclcpp::create_publisher( + auto pub = rclcpp::create_publisher( *this, topic_name, qos, options); + this->add_managed_entity(pub); + return pub; } // TODO(karsten1987): Create LifecycleSubscriber @@ -87,35 +96,40 @@ LifecycleNode::create_wall_timer( CallbackT callback, rclcpp::CallbackGroup::SharedPtr group) { - auto timer = rclcpp::WallTimer::make_shared( - std::chrono::duration_cast(period), - std::move(callback), this->node_base_->get_context()); - 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, + 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 @@ -123,12 +137,12 @@ typename rclcpp::Service::SharedPtr LifecycleNode::create_service( const std::string & service_name, CallbackT && callback, - const rmw_qos_profile_t & qos_profile, + const rclcpp::QoS & qos, rclcpp::CallbackGroup::SharedPtr group) { return rclcpp::create_service( node_base_, node_services_, - service_name, std::forward(callback), qos_profile, group); + service_name, std::forward(callback), qos, group); } template diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp index fdf9ba17c9..ccc60c71d4 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp @@ -19,35 +19,23 @@ #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" -#include "rclcpp/logging.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 ~LifecyclePublisherInterface() {} - 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. */ template> -class LifecyclePublisher : public LifecyclePublisherInterface, +class LifecyclePublisher : public SimpleManagedEntity, public rclcpp::Publisher { public: @@ -64,7 +52,7 @@ class LifecyclePublisher : public LifecyclePublisherInterface, const rclcpp::QoS & qos, const rclcpp::PublisherOptionsWithAllocator & options) : rclcpp::Publisher(node_base, topic, qos, options), - enabled_(false), + should_log_(true), logger_(rclcpp::get_logger("LifecyclePublisher")) { } @@ -80,12 +68,8 @@ class LifecyclePublisher : public LifecyclePublisherInterface, virtual void publish(std::unique_ptr msg) { - if (!enabled_) { - RCLCPP_WARN( - logger_, - "Trying to publish message on the topic '%s', but the publisher is not activated", - this->get_topic_name()); - + if (!this->is_activated()) { + log_publisher_not_enabled(); return; } rclcpp::Publisher::publish(std::move(msg)); @@ -100,37 +84,62 @@ class LifecyclePublisher : public LifecyclePublisherInterface, virtual void publish(const MessageT & msg) { - if (!enabled_) { - RCLCPP_WARN( - logger_, - "Trying to publish message on the topic '%s', but the publisher is not activated", - this->get_topic_name()); - + if (!this->is_activated()) { + log_publisher_not_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 - on_activate() + publish( + rclcpp::LoanedMessage::ROSMessageType, Alloc> && loaned_msg) { - enabled_ = true; + if (!this->is_activated()) { + log_publisher_not_enabled(); + return; + } + rclcpp::Publisher::publish(std::move(loaned_msg)); } - virtual void - on_deactivate() + void + on_activate() override { - enabled_ = false; + SimpleManagedEntity::on_activate(); + should_log_ = true; } - virtual bool - is_activated() +private: + /// LifecyclePublisher log helper function + /** + * Helper function that logs a message saying that publisher can't publish + * because it's not enabled. + */ + void log_publisher_not_enabled() { - return enabled_; + // Nothing to do if we are not meant to log + if (!should_log_) { + return; + } + + // Log the message + RCLCPP_WARN( + logger_, + "Trying to publish message on the topic '%s', but the publisher is not activated", + this->get_topic_name()); + + // We stop logging until the flag gets enabled again + should_log_ = false; } -private: - bool enabled_ = false; + bool should_log_ = true; rclcpp::Logger logger_; }; 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 9f2459e296..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 @@ -17,10 +17,17 @@ #include "lifecycle_msgs/msg/transition.hpp" -#include "rcl_lifecycle/rcl_lifecycle.h" - #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 { @@ -55,7 +62,7 @@ class LifecycleNodeInterface /// Callback function for configure transition /* - * \return true by default + * \return SUCCESS by default */ RCLCPP_LIFECYCLE_PUBLIC virtual CallbackReturn @@ -63,7 +70,7 @@ class LifecycleNodeInterface /// Callback function for cleanup transition /* - * \return true by default + * \return SUCCESS by default */ RCLCPP_LIFECYCLE_PUBLIC virtual CallbackReturn @@ -71,7 +78,7 @@ class LifecycleNodeInterface /// Callback function for shutdown transition /* - * \return true by default + * \return SUCCESS by default */ RCLCPP_LIFECYCLE_PUBLIC virtual CallbackReturn @@ -79,7 +86,7 @@ class LifecycleNodeInterface /// Callback function for activate transition /* - * \return true by default + * \return SUCCESS by default */ RCLCPP_LIFECYCLE_PUBLIC virtual CallbackReturn @@ -87,7 +94,7 @@ class LifecycleNodeInterface /// Callback function for deactivate transition /* - * \return true by default + * \return SUCCESS by default */ RCLCPP_LIFECYCLE_PUBLIC virtual CallbackReturn @@ -95,7 +102,7 @@ class LifecycleNodeInterface /// Callback function for errorneous transition /* - * \return false by default + * \return SUCCESS by default */ RCLCPP_LIFECYCLE_PUBLIC virtual CallbackReturn @@ -108,4 +115,12 @@ class 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 f09e390395..62e1c51fb3 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp @@ -15,15 +15,15 @@ #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 { @@ -93,6 +93,7 @@ class State 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 e15ccce7e2..4093579e04 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp @@ -17,14 +17,13 @@ #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 { diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 9eea76121d..065992fdc8 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,35 +2,44 @@ rclcpp_lifecycle - 12.0.0 + 29.2.0 Package containing a prototype for lifecycle implementation + Ivan Paunovic - Mabel Zhang + Michel Hidalgo William Woodall + Apache License 2.0 + + Jacob Perron Karsten Knese ament_cmake_ros lifecycle_msgs + rcl rclcpp + rcl_interfaces rcl_lifecycle + rcutils + rmw rosidl_typesupport_cpp lifecycle_msgs + rcl rclcpp + rcl_interfaces rcl_lifecycle + rcutils + rmw rosidl_typesupport_cpp - rmw - ament_cmake_gtest ament_lint_auto ament_lint_common mimick_vendor performance_test_fixture rcpputils - rcutils test_msgs diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index d2bd49bcf7..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" @@ -36,6 +43,7 @@ #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" @@ -69,7 +77,7 @@ LifecycleNode::LifecycleNode( 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_timers_.get())), node_services_(new rclcpp::node_interfaces::NodeServices(node_base_.get())), @@ -78,7 +86,8 @@ LifecycleNode::LifecycleNode( node_topics_, node_graph_, node_services_, - node_logging_ + node_logging_, + options.clock_type() )), node_parameters_(new rclcpp::node_interfaces::NodeParameters( node_base_, @@ -105,9 +114,15 @@ LifecycleNode::LifecycleNode( options.clock_qos(), options.use_clock_thread() )), + node_type_descriptions_(new rclcpp::node_interfaces::NodeTypeDescriptions( + node_base_, + node_logging_, + node_parameters_, + node_services_ + )), node_waitables_(new rclcpp::node_interfaces::NodeWaitables(node_base_.get())), node_options_(options), - impl_(new LifecycleNodeInterfaceImpl(node_base_, node_services_)) + impl_(new LifecycleNodeInterfaceImpl(node_base_, node_services_, node_logging_)) { impl_->init(enable_communication_interface); @@ -129,12 +144,27 @@ LifecycleNode::LifecycleNode( &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(); @@ -143,6 +173,7 @@ LifecycleNode::~LifecycleNode() node_timers_.reset(); node_logging_.reset(); node_graph_.reset(); + node_base_.reset(); } const char * @@ -157,6 +188,12 @@ 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 { @@ -182,24 +219,6 @@ LifecycleNode::declare_parameter( name, default_value, parameter_descriptor, ignore_override); } -const rclcpp::ParameterValue & -LifecycleNode::declare_parameter(const std::string & name) -{ -#ifndef _WIN32 -# pragma GCC diagnostic push -# pragma GCC diagnostic ignored "-Wdeprecated-declarations" -#else -# pragma warning(push) -# pragma warning(disable: 4996) -#endif - return this->node_parameters_->declare_parameter(name); -#ifndef _WIN32 -# pragma GCC diagnostic pop -#else -# pragma warning(pop) -#endif -} - const rclcpp::ParameterValue & LifecycleNode::declare_parameter( const std::string & name, @@ -301,12 +320,31 @@ 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(OnParametersSetCallbackType callback) +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) @@ -314,6 +352,13 @@ LifecycleNode::remove_on_set_parameters_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 { @@ -353,6 +398,18 @@ LifecycleNode::count_subscribers(const std::string & topic_name) const return node_graph_->count_subscribers(topic_name); } +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 { @@ -446,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() { @@ -520,25 +583,25 @@ LifecycleNode::register_on_error( } 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() +LifecycleNode::get_transition_graph() const { return impl_->get_transition_graph(); } @@ -639,11 +702,25 @@ LifecycleNode::shutdown(LifecycleNodeInterface::CallbackReturn & 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 9f70d2c259..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,470 +53,108 @@ 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(); - auto ret = rcl_lifecycle_state_machine_fini(&state_machine_, node_handle); - if (ret != RCL_RET_OK) { - RCUTILS_LOG_FATAL_NAMED( - "rclcpp_lifecycle", - "failed to destroy rcl_state_machine"); - } - } + std::shared_ptr node_services_interface, + std::shared_ptr node_logging_interface); + + ~LifecycleNodeInterfaceImpl(); void - init(bool enable_communication_interface = true) - { - 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(); - 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. - 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()); - } - - if (enable_communication_interface) { - { // 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); - } - - { // get_transition_graph - auto cb = std::bind( - &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); - } - } - } + 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::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."); - } - - auto 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); - } + 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."); - } - - 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); - } - } + 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."); - } - - 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; - } - } + std::shared_ptr resp) const; void on_get_transition_graph( 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."); - } - - 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 & - get_current_state() - { - current_state_ = State(state_machine_.current_state); - return current_state_; - } - - std::vector - get_available_states() - { - std::vector states; - 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 - get_available_transitions() - { - std::vector transitions; - 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 - get_transition_graph() - { - std::vector transitions; - 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; - } + std::shared_ptr resp) const; rcl_ret_t - change_state(std::uint8_t transition_id, LifecycleNodeInterface::CallbackReturn & 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().str); - return RCL_RET_ERROR; - } - - constexpr bool publish_update = true; - // keep the initial state to pass to a transition callback - State initial_state(state_machine_.current_state); - - if ( - rcl_lifecycle_trigger_transition_by_id( - &state_machine_, transition_id, publish_update) != 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().str); - rcutils_reset_error(); - return RCL_RET_ERROR; - } - - 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(state_machine_.current_state->id, initial_state); - auto transition_label = get_label_for_return_code(cb_return_code); - - if ( - rcl_lifecycle_trigger_transition_by_label( - &state_machine_, transition_label, publish_update) != RCL_RET_OK) - { - RCUTILS_LOG_ERROR( - "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; - } - - // error handling ?! - // TODO(karsten1987): iterate over possible ret value - if (cb_return_code == node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR) { - RCUTILS_LOG_WARN("Error occurred while doing error handling."); - - auto error_cb_code = execute_callback(state_machine_.current_state->id, initial_state); - auto error_cb_label = get_label_for_return_code(error_cb_code); - if ( - rcl_lifecycle_trigger_transition_by_label( - &state_machine_, error_cb_label, publish_update) != RCL_RET_OK) - { - RCUTILS_LOG_ERROR("Failed to call cleanup on error state: %s", rcl_get_error_string().str); - rcutils_reset_error(); - 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; - } - - LifecycleNodeInterface::CallbackReturn - execute_callback(unsigned int cb_id, const State & previous_state) - { - // 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) { - RCUTILS_LOG_ERROR("Caught exception in callback for transition %d", it->first); - RCUTILS_LOG_ERROR("Original error: %s", e.what()); - cb_success = node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; - } - } - return cb_success; - } - - const State & trigger_transition(const char * transition_label) - { - LifecycleNodeInterface::CallbackReturn error; - return trigger_transition(transition_label, error); - } - - const State & trigger_transition( - const char * transition_label, LifecycleNodeInterface::CallbackReturn & cb_return_code) - { - auto 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 & - trigger_transition(uint8_t transition_id) - { - LifecycleNodeInterface::CallbackReturn 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, LifecycleNodeInterface::CallbackReturn & 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 = @@ -531,6 +166,7 @@ class LifecycleNode::LifecycleNodeInterfaceImpl 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_; @@ -538,7 +174,7 @@ class LifecycleNode::LifecycleNodeInterfaceImpl 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_lifecycle/src/managed_entity.cpp b/rclcpp_lifecycle/src/managed_entity.cpp new file mode 100644 index 0000000000..8cb8fb46ec --- /dev/null +++ b/rclcpp_lifecycle/src/managed_entity.cpp @@ -0,0 +1,35 @@ +// 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 "rclcpp_lifecycle/managed_entity.hpp" + +namespace rclcpp_lifecycle +{ + +void SimpleManagedEntity::on_activate() +{ + activated_.store(true); +} + +void SimpleManagedEntity::on_deactivate() +{ + 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 1c8a5a646a..06b889be05 100644 --- a/rclcpp_lifecycle/src/node_interfaces/lifecycle_node_interface.cpp +++ b/rclcpp_lifecycle/src/node_interfaces/lifecycle_node_interface.cpp @@ -14,7 +14,7 @@ #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" -#include "rcl_lifecycle/rcl_lifecycle.h" +#include "rclcpp_lifecycle/state.hpp" namespace rclcpp_lifecycle { diff --git a/rclcpp_lifecycle/src/state.cpp b/rclcpp_lifecycle/src/state.cpp index f7aca2688e..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" @@ -70,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); } @@ -93,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(); @@ -128,6 +132,7 @@ 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."); } @@ -137,6 +142,7 @@ State::id() const 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."); } @@ -146,6 +152,7 @@ State::label() const void State::reset() noexcept { + std::lock_guard lock(state_handle_mutex_); if (!owns_rcl_state_handle_) { state_handle_ = nullptr; } @@ -160,7 +167,7 @@ State::reset() noexcept if (ret != RCL_RET_OK) { RCLCPP_ERROR( rclcpp::get_logger("rclcpp_lifecycle"), - "rcl_lifecycle_transition_fini did not complete successfully, leaking memory"); + "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 50f0b33735..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" diff --git a/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_client.cpp b/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_client.cpp index 27b01df51a..59ed24489e 100644 --- a/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_client.cpp +++ b/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_client.cpp @@ -227,7 +227,9 @@ class BenchmarkLifecycleClient : public performance_test_fixture::PerformanceTes BENCHMARK_F(BenchmarkLifecycleClient, get_state)(benchmark::State & state) { for (auto _ : state) { - const auto lifecycle_state = lifecycle_client->get_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: ") + @@ -248,6 +250,7 @@ BENCHMARK_F(BenchmarkLifecycleClient, change_state)(benchmark::State & state) { reset_heap_counters(); for (auto _ : state) { + (void)_; success = lifecycle_client->change_state(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); if (!success) { @@ -264,8 +267,9 @@ BENCHMARK_F(BenchmarkLifecycleClient, change_state)(benchmark::State & state) { BENCHMARK_F(BenchmarkLifecycleClient, get_available_states)(benchmark::State & state) { for (auto _ : state) { + (void)_; constexpr size_t expected_states = 11u; - const auto states = lifecycle_client->get_available_states(); + 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: ") + @@ -279,8 +283,10 @@ BENCHMARK_F(BenchmarkLifecycleClient, get_available_states)(benchmark::State & s BENCHMARK_F(BenchmarkLifecycleClient, get_available_transitions)(benchmark::State & state) { for (auto _ : state) { + (void)_; constexpr size_t expected_transitions = 2u; - const auto transitions = lifecycle_client->get_available_transitions(); + 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: ") + @@ -294,8 +300,10 @@ BENCHMARK_F(BenchmarkLifecycleClient, get_available_transitions)(benchmark::Stat BENCHMARK_F(BenchmarkLifecycleClient, get_transition_graph)(benchmark::State & state) { for (auto _ : state) { + (void)_; constexpr size_t expected_transitions = 25u; - const auto transitions = lifecycle_client->get_transition_graph(); + 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: ") + diff --git a/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_node.cpp b/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_node.cpp index bc53499554..4be9ad13af 100644 --- a/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_node.cpp +++ b/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_node.cpp @@ -49,6 +49,7 @@ 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, @@ -60,6 +61,7 @@ BENCHMARK_F(BenchmarkLifecycleNodeConstruction, construct_lifecycle_node)( BENCHMARK_F(BenchmarkLifecycleNodeConstruction, destroy_lifecycle_node)(benchmark::State & state) { for (auto _ : state) { + (void)_; std::shared_ptr node(nullptr); PERFORMANCE_TEST_FIXTURE_PAUSE_MEASUREMENTS( state, @@ -94,21 +96,25 @@ class BenchmarkLifecycleNode : public performance_test_fixture::PerformanceTest // 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) { - const auto & lifecycle_state = node->get_current_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()); } - benchmark::DoNotOptimize(lifecycle_state); + // 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; - const auto lifecycle_states = node->get_available_states(); + 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()); @@ -120,8 +126,9 @@ BENCHMARK_F(BenchmarkLifecycleNode, get_available_states)(benchmark::State & sta BENCHMARK_F(BenchmarkLifecycleNode, get_available_transitions)(benchmark::State & state) { for (auto _ : state) { + (void)_; constexpr size_t expected_transitions = 2u; - const auto & transitions = node->get_available_transitions(); + 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()); @@ -133,8 +140,9 @@ BENCHMARK_F(BenchmarkLifecycleNode, get_available_transitions)(benchmark::State BENCHMARK_F(BenchmarkLifecycleNode, get_transition_graph)(benchmark::State & state) { for (auto _ : state) { + (void)_; constexpr size_t expected_transitions = 25u; - const auto & transitions = node->get_transition_graph(); + 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: ") + @@ -155,18 +163,21 @@ BENCHMARK_F(BenchmarkLifecycleNode, transition_valid_state)(benchmark::State & s reset_heap_counters(); for (auto _ : state) { - const auto & active = + (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 auto & inactive = + 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"); } - benchmark::DoNotOptimize(active); - benchmark::DoNotOptimize(inactive); + // 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 index 2e26b08433..8a1003bfbc 100644 --- a/rclcpp_lifecycle/test/benchmark/benchmark_state.cpp +++ b/rclcpp_lifecycle/test/benchmark/benchmark_state.cpp @@ -22,6 +22,7 @@ using PerformanceTest = performance_test_fixture::PerformanceTest; 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(); @@ -32,6 +33,7 @@ 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 index 9403ce663e..6ac6761aed 100644 --- a/rclcpp_lifecycle/test/benchmark/benchmark_transition.cpp +++ b/rclcpp_lifecycle/test/benchmark/benchmark_transition.cpp @@ -22,6 +22,7 @@ using PerformanceTest = performance_test_fixture::PerformanceTest; BENCHMARK_F(PerformanceTest, construct_destruct_transition)(benchmark::State & state) { for (auto _ : state) { + (void)_; rclcpp_lifecycle::Transition transition(1, "transition"); benchmark::DoNotOptimize(transition); benchmark::ClobberMemory(); @@ -32,6 +33,7 @@ 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/test_callback_exceptions.cpp b/rclcpp_lifecycle/test/test_callback_exceptions.cpp index 7c4117ae7b..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 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 7ee883a5f0..70993af6d5 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_node.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_node.cpp @@ -25,6 +25,8 @@ #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" @@ -34,6 +36,8 @@ using lifecycle_msgs::msg::State; using lifecycle_msgs::msg::Transition; +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); @@ -229,22 +233,36 @@ 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, empty_initializer_rcl_errors) { +TEST_F(TestDefaultStateMachine, check_logger_services_exist) { + // Logger level services are disabled { - auto patch = mocking_utils::inject_on_return( - "lib:rclcpp_lifecycle", rcl_lifecycle_state_machine_init, RCL_RET_ERROR); - EXPECT_THROW( - std::make_shared("testnode").reset(), - std::runtime_error); + 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 { - auto test_node = std::make_shared("testnode"); - auto patch = mocking_utils::inject_on_return( - "lib:rclcpp_lifecycle", rcl_lifecycle_state_machine_fini, RCL_RET_ERROR); - EXPECT_NO_THROW(test_node.reset()); + 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)); } } @@ -413,10 +431,75 @@ TEST_F(TestDefaultStateMachine, bad_mood) { 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 cb = [](const std::shared_ptr msg) {(void) msg;}; auto lifecycle_sub = test_node->create_subscription("~/empty", 10, cb); @@ -426,11 +509,15 @@ TEST_F(TestDefaultStateMachine, lifecycle_subscriber) { // 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(), 1u); - EXPECT_STREQ(list_result.names[0].c_str(), "use_sim_time"); + 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"; @@ -468,10 +555,11 @@ TEST_F(TestDefaultStateMachine, declare_parameters) { test_node->declare_parameters("test_double", double_parameters); list_result = test_node->list_parameters({}, 0u); - EXPECT_EQ(list_result.names.size(), 7u); + 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", @@ -486,11 +574,13 @@ TEST_F(TestDefaultStateMachine, declare_parameters) { } 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(), 1u); - EXPECT_STREQ(list_result.names[0].c_str(), "use_sim_time"); + 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"; @@ -548,8 +638,7 @@ TEST_F(TestDefaultStateMachine, check_parameters) { std::map parameter_map; EXPECT_TRUE(test_node->get_parameters({}, parameter_map)); - // int param, bool param, and use_sim_time - EXPECT_EQ(parameter_map.size(), 3u); + EXPECT_EQ(parameter_map.size(), parameter_names.size() + builtin_param_count); // Check parameter types auto parameter_types = test_node->get_parameter_types(parameter_names); @@ -584,10 +673,12 @@ TEST_F(TestDefaultStateMachine, check_parameters) { // List parameters list_result = test_node->list_parameters({}, 0u); - EXPECT_EQ(list_result.names.size(), 3u); - EXPECT_STREQ(list_result.names[0].c_str(), parameter_names[0].c_str()); - EXPECT_STREQ(list_result.names[1].c_str(), parameter_names[1].c_str()); - EXPECT_STREQ(list_result.names[2].c_str(), "use_sim_time"); + 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); @@ -632,6 +723,7 @@ TEST_F(TestDefaultStateMachine, test_getters) { 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) { @@ -683,6 +775,17 @@ TEST_F(TestDefaultStateMachine, test_graph_services) { 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) { 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 index 555354a8c3..b98de77238 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_publisher.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_publisher.cpp @@ -18,11 +18,17 @@ #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: @@ -36,69 +42,155 @@ class TestDefaultStateMachine : public ::testing::Test } }; +/// 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) + explicit EmptyLifecycleNode(const std::string & node_name, const TimerType & timer_type) : rclcpp_lifecycle::LifecycleNode(node_name) { - rclcpp::PublisherOptionsWithAllocator> options; - publisher_ = - std::make_shared>( - get_node_base_interface().get(), std::string("topic"), rclcpp::QoS(10), options); - add_publisher_handle(publisher_); - // For coverage this is being added here - auto timer = create_wall_timer(std::chrono::seconds(1), []() {}); - add_timer_handle(timer); + 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; + } + } } - - std::shared_ptr> publisher() - { - return publisher_; - } - -private: - std::shared_ptr> publisher_; }; -class TestLifecyclePublisher : public ::testing::Test +class TestLifecyclePublisher : public ::testing::TestWithParam { public: void SetUp() { rclcpp::init(0, nullptr); - node_ = std::make_shared("node"); } void TearDown() { rclcpp::shutdown(); } - -protected: - std::shared_ptr node_; }; -TEST_F(TestLifecyclePublisher, publish) { - node_->publisher()->on_deactivate(); - EXPECT_FALSE(node_->publisher()->is_activated()); +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(node_->publisher()->publish(*msg_ptr)); + EXPECT_NO_THROW(publisher->publish(*msg_ptr)); } { auto msg_ptr = std::make_unique(); - EXPECT_NO_THROW(node_->publisher()->publish(std::move(msg_ptr))); + 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_->publisher()->on_activate(); - EXPECT_TRUE(node_->publisher()->is_activated()); + 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(node_->publisher()->publish(*msg_ptr)); + EXPECT_NO_THROW(publisher->publish(*msg_ptr)); } { auto msg_ptr = std::make_unique(); - EXPECT_NO_THROW(node_->publisher()->publish(std::move(msg_ptr))); + 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 index d7188ca350..fc3f86e573 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_service_client.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_service_client.cpp @@ -18,11 +18,14 @@ */ #include +#include +#include #include #include #include #include #include +#include #include #include "lifecycle_msgs/msg/state.hpp" @@ -33,6 +36,8 @@ #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" @@ -98,8 +103,9 @@ class LifecycleServiceClient : public rclcpp::Node return unknown_state; } - if (future_result.get()) { - return future_result.get()->current_state; + auto result = future_result.get(); + if (result) { + return result->current_state; } else { return unknown_state; } @@ -140,9 +146,9 @@ class LifecycleServiceClient : public rclcpp::Node if (future_status != std::future_status::ready) { return std::vector(); } - - if (future_result.get()) { - return future_result.get()->available_states; + auto result = future_result.get(); + if (result) { + return result->available_states; } return std::vector(); @@ -164,8 +170,9 @@ class LifecycleServiceClient : public rclcpp::Node return std::vector(); } - if (future_result.get()) { - return future_result.get()->available_transitions; + auto result = future_result.get(); + if (result) { + return result->available_transitions; } return std::vector(); @@ -187,8 +194,9 @@ class LifecycleServiceClient : public rclcpp::Node return std::vector(); } - if (future_result.get()) { - return future_result.get()->available_transitions; + auto result = future_result.get(); + if (result) { + return result->available_transitions; } return std::vector(); @@ -363,26 +371,34 @@ TEST_F(TestLifecycleServiceClient, lifecycle_transitions) { TEST_F(TestLifecycleServiceClient, get_service_names_and_types_by_node) { - auto node1 = std::make_shared("client1"); - auto node2 = std::make_shared("client2"); - - auto node_graph = node1->get_node_graph_interface(); - ASSERT_NE(nullptr, node_graph); - EXPECT_THROW( - node_graph->get_service_names_and_types_by_node("not_a_node", "not_absolute_namespace"), + lifecycle_node()->get_service_names_and_types_by_node("not_a_node", "not_absolute_namespace"), std::runtime_error); - auto service_names_and_types1 = node_graph->get_service_names_and_types_by_node("client1", "/"); - auto service_names_and_types2 = node_graph->get_service_names_and_types_by_node("client2", "/"); + 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_types1.size() || - service_names_and_types1.size() != service_names_and_types2.size() || + while (0 == service_names_and_types.size() || (std::chrono::steady_clock::now() - start) < std::chrono::seconds(1)) { - service_names_and_types1 = node_graph->get_service_names_and_types_by_node("client1", "/"); - service_names_and_types2 = node_graph->get_service_names_and_types_by_node("client2", "/"); + 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; } - EXPECT_EQ(service_names_and_types1.size(), service_names_and_types2.size()); } TEST_F(TestLifecycleServiceClient, declare_parameter_with_no_initial_values) diff --git a/rclcpp_lifecycle/test/test_register_custom_callbacks.cpp b/rclcpp_lifecycle/test/test_register_custom_callbacks.cpp index 3c146e4207..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 diff --git a/rclcpp_lifecycle/test/test_service.cpp b/rclcpp_lifecycle/test/test_service.cpp new file mode 100644 index 0000000000..13bc25761d --- /dev/null +++ b/rclcpp_lifecycle/test/test_service.cpp @@ -0,0 +1,77 @@ +// 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 "rmw/qos_profiles.h" + +#include "test_msgs/srv/empty.hpp" + +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_lifecycle_node", "/ns"); + } + + void TearDown() + { + node.reset(); + } + + rclcpp_lifecycle::LifecycleNode::SharedPtr node; +}; + +/* + Testing service construction and destruction. + */ +TEST_F(TestService, construction_and_destruction) { + using test_msgs::srv::Empty; + auto callback = + [](const Empty::Request::SharedPtr, Empty::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); + } +} diff --git a/rclcpp_lifecycle/test/test_state_machine_info.cpp b/rclcpp_lifecycle/test/test_state_machine_info.cpp index 452002e563..addf7d12f1 100644 --- a/rclcpp_lifecycle/test/test_state_machine_info.cpp +++ b/rclcpp_lifecycle/test/test_state_machine_info.cpp @@ -34,6 +34,11 @@ class TestStateMachineInfo : public ::testing::Test { rclcpp::init(0, nullptr); } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } }; TEST_F(TestStateMachineInfo, available_states) { diff --git a/rclcpp_lifecycle/test/test_transition_wrapper.cpp b/rclcpp_lifecycle/test/test_transition_wrapper.cpp index 4f72d90bb6..39791d5da2 100644 --- a/rclcpp_lifecycle/test/test_transition_wrapper.cpp +++ b/rclcpp_lifecycle/test/test_transition_wrapper.cpp @@ -18,10 +18,12 @@ #include #include -#include "rcutils/testing/fault_injection.h" +#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