diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 32830939c8..c02354634b 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,178 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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) ------------------- diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index e22f3f240a..372038e9f3 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.12) +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) @@ -42,7 +45,9 @@ 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 @@ -64,12 +69,12 @@ set(${PROJECT_NAME}_SRCS 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_subscription.cpp src/rclcpp/graph_listener.cpp @@ -92,6 +97,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 @@ -106,6 +112,7 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/qos.cpp src/rclcpp/event_handler.cpp src/rclcpp/qos_overriding_options.cpp + src/rclcpp/rate.cpp src/rclcpp/serialization.cpp src/rclcpp/serialized_message.cpp src/rclcpp/service.cpp @@ -122,6 +129,21 @@ 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 @@ -200,22 +222,28 @@ 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_interfaces" - "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, @@ -237,20 +265,23 @@ ament_export_libraries(${PROJECT_NAME}) # Export modern CMake targets ament_export_targets(${PROJECT_NAME}) -# specific order: dependents before dependencies -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) @@ -268,7 +299,7 @@ install( if(TEST cppcheck) # must set the property after ament_package() - set_tests_properties(cppcheck PROPERTIES TIMEOUT 500) + set_tests_properties(cppcheck PROPERTIES TIMEOUT 600) endif() if(TEST cpplint) 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..d3c390b449 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, at the [ROS Index](https://index.ros.org/p/rclcpp/). ## Quality Declaration 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 f8c2592fc5..918d8e5a29 100644 --- a/rclcpp/include/rclcpp/any_service_callback.hpp +++ b/rclcpp/include/rclcpp/any_service_callback.hpp @@ -156,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. @@ -182,7 +182,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; } @@ -191,9 +191,9 @@ class AnyServiceCallback #ifndef TRACETOOLS_DISABLED std::visit( [this](auto && arg) { - if (TRACEPOINT_ENABLED(rclcpp_callback_register)) { + if (TRACETOOLS_TRACEPOINT_ENABLED(rclcpp_callback_register)) { char * symbol = tracetools::get_symbol(arg); - DO_TRACEPOINT( + TRACETOOLS_DO_TRACEPOINT( rclcpp_callback_register, static_cast(this), symbol); diff --git a/rclcpp/include/rclcpp/any_subscription_callback.hpp b/rclcpp/include/rclcpp/any_subscription_callback.hpp index 65b29d8535..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< @@ -372,58 +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 -#if !defined(RCLCPP_AVOID_DEPRECATIONS_FOR_UNIT_TESTS) - // suppress deprecation warnings in `test_any_subscription_callback.cpp` - [[deprecated("use 'void(std::shared_ptr)' instead")]] -#endif - void - set_deprecated(std::function)> callback) - { - callback_variant_ = callback; - } - - /// Function for shared_ptr to non-const MessageT with MessageInfo, which is deprecated. - template -#if !defined(RCLCPP_AVOID_DEPRECATIONS_FOR_UNIT_TESTS) - // suppress deprecation warnings in `test_any_subscription_callback.cpp` - [[deprecated( - "use 'void(std::shared_ptr, const rclcpp::MessageInfo &)' instead" - )]] -#endif - 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) @@ -487,12 +462,14 @@ class AnySubscriptionCallback } // 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) { @@ -580,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) { @@ -660,10 +637,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)); } void @@ -671,7 +648,7 @@ class AnySubscriptionCallback 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) { @@ -790,10 +767,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)); } void @@ -801,7 +778,7 @@ class AnySubscriptionCallback 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) { @@ -924,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 @@ -965,9 +942,9 @@ class AnySubscriptionCallback #ifndef TRACETOOLS_DISABLED std::visit( [this](auto && callback) { - if (TRACEPOINT_ENABLED(rclcpp_callback_register)) { + if (TRACETOOLS_TRACEPOINT_ENABLED(rclcpp_callback_register)) { char * symbol = tracetools::get_symbol(callback); - DO_TRACEPOINT( + TRACETOOLS_DO_TRACEPOINT( rclcpp_callback_register, static_cast(this), symbol); diff --git a/rclcpp/include/rclcpp/callback_group.hpp b/rclcpp/include/rclcpp/callback_group.hpp index 43c7daa888..d8c529d56f 100644 --- a/rclcpp/include/rclcpp/callback_group.hpp +++ b/rclcpp/include/rclcpp/callback_group.hpp @@ -129,8 +129,7 @@ class CallbackGroup * added to the executor in either case. * * \param[in] group_type The type of the callback group. - * \param[in] get_node_context Lambda to retrieve the node context when - * checking that the creating node is valid and using the guard condition. + * \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. @@ -138,7 +137,7 @@ class CallbackGroup RCLCPP_PUBLIC explicit CallbackGroup( CallbackGroupType group_type, - std::function get_node_context, + rclcpp::Context::WeakPtr context, bool automatically_add_to_executor_with_node = true); /// Default destructor. @@ -185,18 +184,41 @@ class CallbackGroup * \return the number of entities in the callback group. */ RCLCPP_PUBLIC - size_t size() const; + size_t + size() const; + /// Return a reference to the 'can be taken' atomic boolean. + /** + * The resulting bool will be true in the case that no executor is currently + * using an executable entity from this group. + * The resulting bool will be false in the case that an executor is currently + * using an executable entity from this group, and the group policy doesn't + * allow a second take (eg mutual exclusion) + * \return a reference to the flag + */ RCLCPP_PUBLIC std::atomic_bool & can_be_taken_from(); + /// Get the group type. + /** + * \return the group type + */ RCLCPP_PUBLIC const CallbackGroupType & type() const; + /// Collect all of the entity pointers contained in this callback group. + /** + * \param[in] sub_func Function to execute for each subscription + * \param[in] service_func Function to execute for each service + * \param[in] client_func Function to execute for each client + * \param[in] timer_func Function to execute for each timer + * \param[in] waitable_fuinc Function to execute for each waitable + */ RCLCPP_PUBLIC - void collect_all_ptrs( + void + collect_all_ptrs( std::function sub_func, std::function service_func, std::function client_func, @@ -228,16 +250,6 @@ class CallbackGroup bool automatically_add_to_executor_with_node() const; - /// Retrieve the guard condition used to signal changes to this callback group. - /** - * \param[in] context_ptr context to use when creating the guard condition - * \return guard condition if it is valid, otherwise nullptr. - */ - [[deprecated("Use get_notify_guard_condition() without arguments")]] - RCLCPP_PUBLIC - rclcpp::GuardCondition::SharedPtr - get_notify_guard_condition(const rclcpp::Context::SharedPtr context_ptr); - /// Retrieve the guard condition used to signal changes to this callback group. /** * \return guard condition if it is valid, otherwise nullptr. @@ -297,7 +309,7 @@ class CallbackGroup std::shared_ptr notify_guard_condition_ = nullptr; std::recursive_mutex notify_guard_condition_mutex_; - std::function get_context_; + rclcpp::Context::WeakPtr context_; private: template diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index f3346a067c..f69ab0b28f 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -20,13 +20,13 @@ #include #include #include -#include // NOLINT, cpplint doesn't think this is a cpp std header +#include #include #include #include #include #include -#include // NOLINT +#include #include #include "rcl/client.h" @@ -115,6 +115,29 @@ struct FutureAndRequestId /// 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 @@ -363,12 +386,16 @@ class ClientBase 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}; - - std::recursive_mutex callback_mutex_; - std::function on_new_response_callback_{nullptr}; }; template @@ -767,19 +794,11 @@ class Client : public ClientBase 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(); + return detail::prune_requests_older_than_impl( + pending_requests_, + pending_requests_mutex_, + time_point, + pruned_requests); } /// Configure client introspection. diff --git a/rclcpp/include/rclcpp/context.hpp b/rclcpp/include/rclcpp/context.hpp index 45c70b9af2..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" 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_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_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_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 d371466f0d..64d5b8e322 100644 --- a/rclcpp/include/rclcpp/create_timer.hpp +++ b/rclcpp/include/rclcpp/create_timer.hpp @@ -90,7 +90,8 @@ 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( clock, @@ -98,7 +99,8 @@ create_timer( std::forward(callback), group, node_base.get(), - node_timers.get()); + node_timers.get(), + autostart); } /// Create a timer with a given clock @@ -109,7 +111,8 @@ 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( clock, @@ -117,7 +120,8 @@ create_timer( std::forward(callback), group, rclcpp::node_interfaces::get_node_base_interface(node).get(), - rclcpp::node_interfaces::get_node_timers_interface(node).get()); + rclcpp::node_interfaces::get_node_timers_interface(node).get(), + autostart); } /// Convenience method to create a general timer with node resources. @@ -132,6 +136,7 @@ create_timer( * \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 @@ -144,7 +149,8 @@ create_timer( 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"}; @@ -160,7 +166,7 @@ create_timer( // Add a new generic timer. auto timer = rclcpp::GenericTimer::make_shared( - std::move(clock), period_ns, std::move(callback), node_base->get_context()); + std::move(clock), period_ns, std::move(callback), node_base->get_context(), autostart); node_timers->add_timer(timer, group); return timer; } @@ -187,7 +193,8 @@ create_wall_timer( 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 (node_base == nullptr) { throw std::invalid_argument{"input node_base cannot be null"}; @@ -201,7 +208,7 @@ create_wall_timer( // 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; } 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/duration.hpp b/rclcpp/include/rclcpp/duration.hpp index 537e2a9bf6..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" @@ -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/event_handler.hpp b/rclcpp/include/rclcpp/event_handler.hpp index 3f41de469c..887c571d16 100644 --- a/rclcpp/include/rclcpp/event_handler.hpp +++ b/rclcpp/include/rclcpp/event_handler.hpp @@ -117,12 +117,12 @@ class EventHandlerBase : public Waitable /// Add the Waitable to a wait set. RCLCPP_PUBLIC void - add_to_wait_set(rcl_wait_set_t * wait_set) override; + 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; + is_ready(const rcl_wait_set_t & wait_set) override; /// Set a callback to be called when each new event instance occurs. /** @@ -260,6 +260,16 @@ class EventHandler : public EventHandlerBase } } + ~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 @@ -284,7 +294,7 @@ class EventHandler : public EventHandlerBase /// Execute any entities of the Waitable that are ready. void - execute(std::shared_ptr & data) override + execute(const std::shared_ptr & data) override { if (!data) { throw std::runtime_error("'data' is empty"); diff --git a/rclcpp/include/rclcpp/exceptions/exceptions.hpp b/rclcpp/include/rclcpp/exceptions/exceptions.hpp index ecb2a09905..b3a53373ed 100644 --- a/rclcpp/include/rclcpp/exceptions/exceptions.hpp +++ b/rclcpp/include/rclcpp/exceptions/exceptions.hpp @@ -206,6 +206,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 { @@ -222,6 +230,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 { diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 2d5ca2149a..cfd7ea81fd 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -29,26 +29,24 @@ #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; @@ -282,21 +280,43 @@ class Executor 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 + 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 + 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 @@ -403,17 +423,6 @@ class Executor void cancel(); - /// Support dynamic switching of the memory strategy. - /** - * 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 - */ - RCLCPP_PUBLIC - void - set_memory_strategy(memory_strategy::MemoryStrategy::SharedPtr memory_strategy); - /// Returns true if the executor is currently spinning. /** * This function can be called asynchronously from any thread. @@ -498,6 +507,11 @@ class Executor 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. @@ -509,62 +523,6 @@ class Executor void wait_for_work(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); - /// Find node associated with a callback group - /** - * \param[in] weak_groups_to_nodes map of callback groups to nodes - * \param[in] group callback group to find assocatiated node - * \return Pointer to associated node if found, else nullptr - */ - 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. - /** - * \param[in] node_ptr a shared pointer that points to a node base interface - * \param[in] weak_groups_to_nodes map to nodes to lookup - * \return true if the node is associated with the executor, otherwise false - */ - RCLCPP_PUBLIC - bool - has_node( - const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) const; - - /// Find the callback group associated with a timer - /** - * \param[in] timer Timer to find associated callback group - * \return Pointer to callback group node if found, else nullptr - */ - 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_); - - /// Remove a callback group from the executor. - /** - * \see rclcpp::Executor::remove_callback_group - */ - 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_); - /// Check for executable in ready state and populate union structure. /** * \param[out] any_executable populated union structure of ready executable @@ -575,33 +533,6 @@ class Executor bool get_next_ready_executable(AnyExecutable & any_executable); - /// Check for executable in ready state and populate union structure. - /** - * This is the implementation of get_next_ready_executable that takes into - * account the current state of callback groups' association with nodes and - * executors. - * - * This checks in a particular order for available work: - * * Timers - * * Subscriptions - * * Services - * * Clients - * * Waitable - * - * If the next executable is not associated with this executor/node pair, - * then this method will return false. - * - * \param[out] any_executable populated union structure of ready executable - * \param[in] weak_groups_to_nodes mapping of callback groups to nodes - * \return true if an executable was ready and any_executable was populated, - * otherwise false - */ - RCLCPP_PUBLIC - bool - get_next_ready_executable_from_map( - AnyExecutable & any_executable, - const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); - /// Wait for executable in ready state and populate union structure. /** * If an executable is ready, it will return immediately, otherwise @@ -619,21 +550,6 @@ class Executor AnyExecutable & any_executable, std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); - /// Add all callback groups that can be automatically added from associated nodes. - /** - * 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. - */ - RCLCPP_PUBLIC - virtual void - add_callback_groups_from_nodes_associated_to_executor() RCPPUTILS_TSA_REQUIRES(mutex_); - /// Spinning state, used to prevent multi threaded calls to spin and to cancel blocking spins. std::atomic_bool spinning; @@ -643,16 +559,8 @@ class Executor /// 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_; @@ -662,39 +570,31 @@ class Executor virtual void spin_once_impl(std::chrono::nanoseconds timeout); - typedef std::map> - WeakNodesToGuardConditionsMap; - - typedef std::map> - WeakCallbackGroupsToGuardConditionsMap; - - /// maps nodes to guard conditions - WeakNodesToGuardConditionsMap - weak_nodes_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + /// 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 callback groups to guard conditions - WeakCallbackGroupsToGuardConditionsMap - weak_groups_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_; diff --git a/rclcpp/include/rclcpp/executors.hpp b/rclcpp/include/rclcpp/executors.hpp index f7ff06055f..55a38709a7 100644 --- a/rclcpp/include/rclcpp/executors.hpp +++ b/rclcpp/include/rclcpp/executors.hpp @@ -29,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 diff --git a/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp b/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp index 166bb99119..517894a2a2 100644 --- a/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp +++ b/rclcpp/include/rclcpp/executors/executor_entities_collection.hpp @@ -178,6 +178,12 @@ struct ExecutorEntitiesCollection /// 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 diff --git a/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp b/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp index eee8e59793..0de01fd4b0 100644 --- a/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp +++ b/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp @@ -48,11 +48,11 @@ class ExecutorNotifyWaitable : public rclcpp::Waitable ~ExecutorNotifyWaitable() override = default; RCLCPP_PUBLIC - ExecutorNotifyWaitable(const ExecutorNotifyWaitable & other); + ExecutorNotifyWaitable(ExecutorNotifyWaitable & other); RCLCPP_PUBLIC - ExecutorNotifyWaitable & operator=(const ExecutorNotifyWaitable & other); + ExecutorNotifyWaitable & operator=(ExecutorNotifyWaitable & other); /// Add conditions to the wait set /** @@ -60,7 +60,7 @@ class ExecutorNotifyWaitable : public rclcpp::Waitable */ RCLCPP_PUBLIC void - add_to_wait_set(rcl_wait_set_t * wait_set) override; + add_to_wait_set(rcl_wait_set_t & wait_set) override; /// Check conditions against the wait set /** @@ -69,7 +69,7 @@ class ExecutorNotifyWaitable : public rclcpp::Waitable */ RCLCPP_PUBLIC bool - is_ready(rcl_wait_set_t * wait_set) override; + is_ready(const rcl_wait_set_t & wait_set) override; /// Perform work associated with the waitable. /** @@ -78,7 +78,7 @@ class ExecutorNotifyWaitable : public rclcpp::Waitable */ RCLCPP_PUBLIC void - execute(std::shared_ptr & data) override; + execute(const std::shared_ptr & data) override; /// Retrieve data to be used in the next execute call. /** 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 f9fd2ff672..0000000000 --- a/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp +++ /dev/null @@ -1,357 +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. - * \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); - - /// 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 - void - 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] node_ptr a node base interface shared pointer - * \param[in] weak_groups_to_nodes map to nodes to lookup - * \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_; - - // Mutex to protect vector of new nodes. - std::mutex new_nodes_mutex_; - std::vector new_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 5294605eaf..6f22909caf 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 { @@ -65,7 +54,7 @@ class StaticSingleThreadedExecutor : public rclcpp::Executor explicit StaticSingleThreadedExecutor( const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions()); - /// Default destrcutor. + /// Default destructor. RCLCPP_PUBLIC virtual ~StaticSingleThreadedExecutor(); @@ -116,105 +105,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 05092bb23b..268c3f6649 100644 --- a/rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp +++ b/rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include "rclcpp/allocator/allocator_common.hpp" #include "rclcpp/allocator/allocator_deleter.hpp" @@ -44,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< @@ -65,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< @@ -95,7 +100,7 @@ class TypedIntraProcessBuffer : public IntraProcessBuffer(buffer_.get()), static_cast(this)); @@ -128,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(); @@ -143,6 +158,11 @@ class TypedIntraProcessBuffer : public IntraProcessBuffer::value; } + size_t available_capacity() const override + { + return buffer_->available_capacity(); + } + private: std::unique_ptr> buffer_; @@ -237,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 2c06ea6cbe..b8fe79a5ff 100644 --- a/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp +++ b/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp @@ -15,6 +15,7 @@ #ifndef RCLCPP__EXPERIMENTAL__BUFFERS__RING_BUFFER_IMPLEMENTATION_HPP_ #define RCLCPP__EXPERIMENTAL__BUFFERS__RING_BUFFER_IMPLEMENTATION_HPP_ +#include #include #include #include @@ -52,7 +53,10 @@ class RingBufferImplementation : public BufferImplementationBase if (capacity == 0) { throw std::invalid_argument("capacity must be a positive, non-zero value"); } - TRACEPOINT(rclcpp_construct_ring_buffer, static_cast(this), capacity_); + TRACETOOLS_TRACEPOINT( + rclcpp_construct_ring_buffer, + static_cast(this), + capacity_); } virtual ~RingBufferImplementation() {} @@ -63,13 +67,13 @@ 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); - TRACEPOINT( + TRACETOOLS_TRACEPOINT( rclcpp_ring_buffer_enqueue, static_cast(this), write_index_, @@ -89,7 +93,7 @@ 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_); @@ -98,7 +102,7 @@ class RingBufferImplementation : public BufferImplementationBase } auto request = std::move(ring_buffer_[read_index_]); - TRACEPOINT( + TRACETOOLS_TRACEPOINT( rclcpp_ring_buffer_dequeue, static_cast(this), read_index_, @@ -110,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. @@ -129,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_(); @@ -148,9 +163,21 @@ 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 { - TRACEPOINT(rclcpp_ring_buffer_clear, static_cast(this)); + std::lock_guard lock(mutex_); + return available_capacity_(); + } + + void clear() override + { + TRACETOOLS_TRACEPOINT(rclcpp_ring_buffer_clear, static_cast(this)); } private: @@ -189,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/executors/events_executor/events_executor.hpp b/rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor.hpp index 148ede66ea..dd5b1ebe63 100644 --- a/rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor.hpp +++ b/rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor.hpp @@ -243,6 +243,11 @@ class EventsExecutor : public rclcpp::Executor 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 @@ -269,9 +274,12 @@ class EventsExecutor : public rclcpp::Executor rclcpp::experimental::executors::EventsQueue::UniquePtr events_queue_; std::shared_ptr entities_collector_; - std::shared_ptr current_entities_collection_; std::shared_ptr notify_waitable_; + /// Mutex to protect the current_entities_collection_ + std::recursive_mutex collection_mutex_; + std::shared_ptr current_entities_collection_; + /// Flag used to reduce the number of unnecessary waitable events std::atomic notify_waitable_event_pushed_ {false}; diff --git a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp index 11f2dda6a4..ffd77cb99c 100644 --- a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp +++ b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp @@ -28,6 +28,7 @@ #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" @@ -112,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. /** @@ -131,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. /** @@ -292,6 +331,34 @@ class IntraProcessManager } } + 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 @@ -306,6 +373,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 { @@ -319,6 +391,9 @@ class IntraProcessManager using PublisherMap = std::unordered_map; + using PublisherBufferMap = + std::unordered_map; + using PublisherToSubscriptionIdsMap = std::unordered_map; @@ -337,6 +412,54 @@ 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, @@ -462,7 +585,7 @@ class IntraProcessManager auto ptr = MessageAllocTraits::allocate(allocator, 1); MessageAllocTraits::construct(allocator, ptr, *message); - subscription->provide_intra_process_data(std::move(MessageUniquePtr(ptr, deleter))); + subscription->provide_intra_process_data(MessageUniquePtr(ptr, deleter)); } continue; @@ -481,13 +604,13 @@ class IntraProcessManager "subscription use different allocator types, which is not supported"); } - if constexpr (rclcpp::TypeAdapter::is_specialized::value) { + if constexpr (rclcpp::TypeAdapter::is_specialized::value) { ROSMessageTypeAllocator ros_message_alloc(allocator); - auto ptr = ros_message_alloc.allocate(1); - ros_message_alloc.construct(ptr); + 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); + 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 { @@ -505,7 +628,7 @@ class IntraProcessManager MessageAllocTraits::construct(allocator, ptr, *message); ros_message_subscription->provide_intra_process_message( - std::move(MessageUniquePtr(ptr, deleter))); + MessageUniquePtr(ptr, deleter)); } } } @@ -515,6 +638,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/subscription_intra_process.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp index ec89ebc5ef..5c562a61ff 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp @@ -87,7 +87,7 @@ class SubscriptionIntraProcess buffer_type), any_callback_(callback) { - TRACEPOINT( + TRACETOOLS_TRACEPOINT( rclcpp_subscription_callback_added, static_cast(this), static_cast(&any_callback_)); @@ -132,7 +132,7 @@ class SubscriptionIntraProcess ); } - void execute(std::shared_ptr & data) override + void execute(const std::shared_ptr & data) override { execute_impl(data); } @@ -140,15 +140,14 @@ class SubscriptionIntraProcess 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) { return; diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp index 6583e74ae7..74792e8751 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp @@ -60,10 +60,19 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable RCLCPP_PUBLIC void - add_to_wait_set(rcl_wait_set_t * wait_set) override; + add_to_wait_set(rcl_wait_set_t & wait_set) override; + + RCLCPP_PUBLIC + virtual + size_t + available_capacity() const = 0; + + RCLCPP_PUBLIC + bool + is_durability_transient_local() const; bool - is_ready(rcl_wait_set_t * wait_set) override = 0; + is_ready(const rcl_wait_set_t & wait_set) override = 0; std::shared_ptr take_data() override = 0; @@ -76,7 +85,7 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable } void - execute(std::shared_ptr & data) override = 0; + execute(const std::shared_ptr & data) override = 0; virtual bool diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp index 30debed83a..2f384f351c 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp @@ -30,6 +30,7 @@ #include "rclcpp/experimental/ros_message_intra_process_buffer.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/type_support_decl.hpp" +#include "rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp" #include "tracetools/tracetools.h" @@ -93,14 +94,23 @@ class SubscriptionIntraProcessBuffer : public SubscriptionROSMsgIntraProcessBuff buffer_type, qos_profile, std::make_shared(subscribed_type_allocator_)); - TRACEPOINT( + TRACETOOLS_TRACEPOINT( rclcpp_ipb_to_subscription, static_cast(buffer_.get()), static_cast(this)); } + void + add_to_wait_set(rcl_wait_set_t & wait_set) override + { + if (this->buffer_->has_data()) { + this->trigger_guard_condition(); + } + detail::add_guard_condition_to_rcl_wait_set(wait_set, this->gc_); + } + bool - is_ready(rcl_wait_set_t * wait_set) override + is_ready(const rcl_wait_set_t & wait_set) override { (void) wait_set; return buffer_->has_data(); @@ -169,6 +179,11 @@ class SubscriptionIntraProcessBuffer : public SubscriptionROSMsgIntraProcessBuff return buffer_->use_take_shared_method(); } + size_t available_capacity() const override + { + return buffer_->available_capacity(); + } + protected: void trigger_guard_condition() override diff --git a/rclcpp/include/rclcpp/experimental/timers_manager.hpp b/rclcpp/include/rclcpp/experimental/timers_manager.hpp index e8830c01a0..197397e8b8 100644 --- a/rclcpp/include/rclcpp/experimental/timers_manager.hpp +++ b/rclcpp/include/rclcpp/experimental/timers_manager.hpp @@ -22,10 +22,10 @@ #include #include #include +#include #include #include #include - #include "rclcpp/context.hpp" #include "rclcpp/timer.hpp" @@ -172,13 +172,14 @@ class TimersManager * @brief Get the amount of time before the next timer triggers. * This function is thread safe. * - * @return std::chrono::nanoseconds to wait, + * @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::chrono::nanoseconds get_head_timeout(); + std::optional get_head_timeout(); private: RCLCPP_DISABLE_COPY(TimersManager) @@ -512,12 +513,13 @@ class TimersManager * @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::chrono::nanoseconds to wait, + * @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::chrono::nanoseconds get_head_timeout_unsafe(); + std::optional get_head_timeout_unsafe(); /** * @brief Executes all the timers currently ready when the function is invoked @@ -527,7 +529,7 @@ class TimersManager void execute_ready_timers_unsafe(); // Callback to be called when timer is ready - std::function on_ready_callback_ = nullptr; + std::function on_ready_callback_; // Thread used to run the timers execution task std::thread timers_thread_; diff --git a/rclcpp/include/rclcpp/generic_client.hpp b/rclcpp/include/rclcpp/generic_client.hpp new file mode 100644 index 0000000000..d6073decfc --- /dev/null +++ b/rclcpp/include/rclcpp/generic_client.hpp @@ -0,0 +1,207 @@ +// 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 "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 *; // Serialized data pointer of request message + using Response = void *; // Serialized 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; + + 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; + }; + + 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 `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. + */ + RCLCPP_PUBLIC + FutureAndRequestId + async_send_request(const Request request); + + /// 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); + } + + RCLCPP_PUBLIC + size_t + prune_pending_requests(); + + RCLCPP_PUBLIC + bool + remove_pending_request( + int64_t request_id); + + /// 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 CallbackInfoVariant = std::variant< + std::promise>; // Use variant for extension + + 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 7cd2d8bc39..292e6900d3 100644 --- a/rclcpp/include/rclcpp/generic_publisher.hpp +++ b/rclcpp/include/rclcpp/generic_publisher.hpp @@ -77,7 +77,7 @@ class GenericPublisher : public rclcpp::PublisherBase : rclcpp::PublisherBase( node_base, topic_name, - *rclcpp::get_typesupport_handle(topic_type, "rosidl_typesupport_cpp", *ts_lib), + *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, diff --git a/rclcpp/include/rclcpp/generic_subscription.hpp b/rclcpp/include/rclcpp/generic_subscription.hpp index 975a9d0d0d..dd0e8be94d 100644 --- a/rclcpp/include/rclcpp/generic_subscription.hpp +++ b/rclcpp/include/rclcpp/generic_subscription.hpp @@ -74,20 +74,32 @@ 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.to_rcl_subscription_options(qos), options.event_callbacks, options.use_default_callbacks, DeliveredMessageKind::SERIALIZED_MESSAGE), - callback_(callback), + any_callback_(callback), ts_lib_(ts_lib) - {} + { + TRACETOOLS_TRACEPOINT( + rclcpp_subscription_init, + static_cast(get_subscription_handle().get()), + static_cast(this)); + TRACETOOLS_TRACEPOINT( + rclcpp_subscription_callback_added, + static_cast(this), + static_cast(&any_callback_)); + +#ifndef TRACETOOLS_DISABLED + any_callback_.register_callback_for_tracing(); +#endif + } RCLCPP_PUBLIC virtual ~GenericSubscription() = default; @@ -150,8 +162,7 @@ class GenericSubscription : public rclcpp::SubscriptionBase 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/guard_condition.hpp b/rclcpp/include/rclcpp/guard_condition.hpp index 350f306010..594234657c 100644 --- a/rclcpp/include/rclcpp/guard_condition.hpp +++ b/rclcpp/include/rclcpp/guard_condition.hpp @@ -48,7 +48,7 @@ class GuardCondition */ RCLCPP_PUBLIC explicit GuardCondition( - rclcpp::Context::SharedPtr 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()); @@ -57,11 +57,6 @@ class GuardCondition virtual ~GuardCondition(); - /// Return the context used when creating this guard condition. - RCLCPP_PUBLIC - rclcpp::Context::SharedPtr - get_context() const; - /// Return the underlying rcl guard condition structure. RCLCPP_PUBLIC rcl_guard_condition_t & @@ -105,7 +100,7 @@ class GuardCondition */ RCLCPP_PUBLIC void - add_to_wait_set(rcl_wait_set_t * wait_set); + add_to_wait_set(rcl_wait_set_t & wait_set); /// Set a callback to be called whenever the guard condition is triggered. /** @@ -128,13 +123,14 @@ class GuardCondition 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}; - rcl_wait_set_t * wait_set_{nullptr}; + // 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/logger.hpp b/rclcpp/include/rclcpp/logger.hpp index 77f7f8d670..3b8e8a1625 100644 --- a/rclcpp/include/rclcpp/logger.hpp +++ b/rclcpp/include/rclcpp/logger.hpp @@ -126,9 +126,6 @@ class Logger 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 diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 7ecb67e9b1..35863abba4 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -42,6 +42,7 @@ #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_subscription.hpp" #include "rclcpp/logger.hpp" @@ -56,6 +57,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" @@ -232,13 +234,15 @@ class Node : public std::enable_shared_from_this * \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); + rclcpp::CallbackGroup::SharedPtr group = nullptr, + bool autostart = true); /// Create a timer that uses the node clock to drive the callback. /** @@ -317,6 +321,22 @@ class Node : public std::enable_shared_from_this 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 GenericPublisher. /** * The returned pointer will never be empty, but this function can throw various exceptions, for @@ -355,12 +375,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() ) @@ -969,7 +991,16 @@ 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 @@ -1302,6 +1333,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 @@ -1454,6 +1505,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 @@ -1586,11 +1642,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 d6b090d4d1..d55a23f9c1 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -110,14 +110,16 @@ 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()); + this->node_timers_.get(), + autostart); } template @@ -219,13 +221,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( @@ -233,7 +235,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 ); } diff --git a/rclcpp/include/rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp b/rclcpp/include/rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp index b999344ca9..a243e9611a 100644 --- a/rclcpp/include/rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp @@ -167,6 +167,7 @@ init_tuple(NodeT & n) * 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 \ @@ -189,7 +190,7 @@ init_tuple(NodeT & n) /* Perfect forwarding constructor to get arguments down to StorageClassT (eventually). */ \ template \ explicit NodeInterfacesSupports(ArgsT && ... args) \ - : NodeInterfacesSupports( \ + : NodeInterfacesSupports( \ std::forward(args) ...) \ {} \ \ @@ -200,6 +201,7 @@ init_tuple(NodeT & n) } \ }; \ } // namespace rclcpp::node_interfaces::detail +// *INDENT-ON* } // namespace detail } // namespace node_interfaces diff --git a/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp b/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp index d167515784..863dbee1bf 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp @@ -113,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 5967997ac7..80abc308c1 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp @@ -305,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 diff --git a/rclcpp/include/rclcpp/node_interfaces/node_interfaces.hpp b/rclcpp/include/rclcpp/node_interfaces/node_interfaces.hpp index bb4886d592..80a20fb4a3 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_interfaces.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_interfaces.hpp @@ -30,6 +30,7 @@ rclcpp::node_interfaces::NodeTimeSourceInterface, \ rclcpp::node_interfaces::NodeTimersInterface, \ rclcpp::node_interfaces::NodeTopicsInterface, \ + rclcpp::node_interfaces::NodeTypeDescriptionsInterface, \ rclcpp::node_interfaces::NodeWaitablesInterface @@ -118,6 +119,7 @@ class NodeInterfaces * - 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 @@ -125,7 +127,9 @@ class NodeInterfaces * the RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT macro. * * Usage example: - * ```RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeBaseInterface, base)``` + * ```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: 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_options.hpp b/rclcpp/include/rclcpp/node_options.hpp index a30ce4cf11..71b0d997cf 100644 --- a/rclcpp/include/rclcpp/node_options.hpp +++ b/rclcpp/include/rclcpp/node_options.hpp @@ -43,6 +43,7 @@ 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 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_value.hpp b/rclcpp/include/rclcpp/parameter_value.hpp index f74c36a866..fac896ea46 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 diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index f474a67192..c6f16643c6 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -32,6 +32,9 @@ #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" @@ -109,6 +112,12 @@ class Publisher : public PublisherBase [[deprecated("use std::shared_ptr")]] = std::shared_ptr; + using BufferSharedPtr = typename rclcpp::experimental::buffers::IntraProcessBuffer< + ROSMessageType, + ROSMessageTypeAllocator, + ROSMessageTypeDeleter + >::SharedPtr; + RCLCPP_SMART_PTR_DEFINITIONS(Publisher) /// Default constructor. @@ -171,11 +180,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); @@ -242,9 +254,18 @@ class Publisher : public PublisherBase if (inter_process_publish_needed) { auto shared_msg = this->do_intra_process_ros_message_publish_and_return_shared(std::move(msg)); + if (buffer_) { + buffer_->add_shared(shared_msg); + } this->do_inter_process_publish(*shared_msg); } else { - this->do_intra_process_ros_message_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)); + } } } @@ -269,8 +290,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. @@ -297,26 +318,31 @@ class Publisher : public PublisherBase > publish(std::unique_ptr msg) { - // Avoid allocating when not using intra process. if (!intra_process_is_enabled_) { // In this case we're not using intra process. - ROSMessageType ros_msg; - rclcpp::TypeAdapter::convert_to_ros_message(*msg, ros_msg); - 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; } bool inter_process_publish_needed = get_subscription_count() > get_intra_process_subscription_count(); if (inter_process_publish_needed) { - ROSMessageType ros_msg; - // TODO(clalancette): This is unnecessarily doing an additional conversion - // that may have already been done in do_intra_process_publish_and_return_shared(). - // We should just reuse that effort. - rclcpp::TypeAdapter::convert_to_ros_message(*msg, ros_msg); + 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); + 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)); } } @@ -339,13 +365,12 @@ class Publisher : public PublisherBase > publish(const T & msg) { - // Avoid double 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 and pass it along. @@ -390,7 +415,7 @@ 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. @@ -421,7 +446,7 @@ class Publisher : public PublisherBase void do_inter_process_publish(const ROSMessageType & msg) { - TRACEPOINT(rclcpp_publish, nullptr, 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) { @@ -456,6 +481,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) { @@ -484,7 +510,7 @@ class Publisher : public PublisherBase if (!msg) { throw std::runtime_error("cannot publish msg which is a null pointer"); } - TRACEPOINT( + TRACETOOLS_TRACEPOINT( rclcpp_intra_publish, static_cast(publisher_handle_.get()), msg.get()); @@ -506,7 +532,7 @@ class Publisher : public PublisherBase if (!msg) { throw std::runtime_error("cannot publish msg which is a null pointer"); } - TRACEPOINT( + TRACETOOLS_TRACEPOINT( rclcpp_intra_publish, static_cast(publisher_handle_.get()), msg.get()); @@ -529,7 +555,7 @@ class Publisher : public PublisherBase if (!msg) { throw std::runtime_error("cannot publish msg which is a null pointer"); } - TRACEPOINT( + TRACETOOLS_TRACEPOINT( rclcpp_intra_publish, static_cast(publisher_handle_.get()), msg.get()); @@ -580,6 +606,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 d9181bea43..9a6c398eeb 100644 --- a/rclcpp/include/rclcpp/publisher_base.hpp +++ b/rclcpp/include/rclcpp/publisher_base.hpp @@ -139,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 @@ -215,6 +221,17 @@ 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 diff --git a/rclcpp/include/rclcpp/publisher_options.hpp b/rclcpp/include/rclcpp/publisher_options.hpp index 3501834dd1..01fd314f49 100644 --- a/rclcpp/include/rclcpp/publisher_options.hpp +++ b/rclcpp/include/rclcpp/publisher_options.hpp @@ -24,6 +24,7 @@ #include "rclcpp/allocator/allocator_common.hpp" #include "rclcpp/detail/rmw_implementation_specific_publisher_payload.hpp" +#include "rclcpp/intra_process_buffer_type.hpp" #include "rclcpp/intra_process_setting.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/event_handler.hpp" @@ -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; diff --git a/rclcpp/include/rclcpp/qos_event.hpp b/rclcpp/include/rclcpp/qos_event.hpp deleted file mode 100644 index efc0aa5739..0000000000 --- a/rclcpp/include/rclcpp/qos_event.hpp +++ /dev/null @@ -1,22 +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_ - -#warning This header is obsolete, please include rclcpp/event_handler.hpp instead - -#include "rclcpp/event_handler.hpp" - -#endif // RCLCPP__QOS_EVENT_HPP_ diff --git a/rclcpp/include/rclcpp/rate.hpp b/rclcpp/include/rclcpp/rate.hpp index 55d3bbcb85..5b04b72b81 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,20 @@ class RateBase public: RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(RateBase) + RCLCPP_PUBLIC virtual ~RateBase() {} + + RCLCPP_PUBLIC virtual bool sleep() = 0; + + [[deprecated("use get_type() instead")]] + RCLCPP_PUBLIC virtual bool is_steady() const = 0; + + RCLCPP_PUBLIC + virtual rcl_clock_type_t get_type() const = 0; + + RCLCPP_PUBLIC virtual void reset() = 0; }; @@ -42,14 +55,13 @@ using std::chrono::duration_cast; using std::chrono::nanoseconds; template -class GenericRate : public RateBase +class [[deprecated("use rclcpp::Rate class instead of GenericRate")]] GenericRate : public RateBase { public: RCLCPP_SMART_PTR_DEFINITIONS(GenericRate) explicit GenericRate(double rate) - : GenericRate( - duration_cast(duration(1.0 / rate))) + : period_(duration_cast(duration(1.0 / rate))), last_interval_(Clock::now()) {} explicit GenericRate(std::chrono::nanoseconds period) : period_(period), last_interval_(Clock::now()) @@ -87,12 +99,18 @@ class GenericRate : public RateBase return true; } + [[deprecated("use get_type() instead")]] virtual bool is_steady() const { return Clock::is_steady; } + virtual rcl_clock_type_t get_type() const + { + return Clock::is_steady ? RCL_STEADY_TIME : RCL_SYSTEM_TIME; + } + virtual void reset() { @@ -112,8 +130,59 @@ class GenericRate : public RateBase std::chrono::time_point last_interval_; }; -using Rate = GenericRate; -using WallRate = GenericRate; +class Rate : public RateBase +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(Rate) + + RCLCPP_PUBLIC + explicit Rate( + const double rate, + Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME)); + + RCLCPP_PUBLIC + explicit Rate( + const Duration & period, + Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME)); + + RCLCPP_PUBLIC + virtual bool + sleep(); + + [[deprecated("use get_type() instead")]] + RCLCPP_PUBLIC + virtual bool + is_steady() const; + + RCLCPP_PUBLIC + virtual rcl_clock_type_t + get_type() const; + + RCLCPP_PUBLIC + virtual void + reset(); + + RCLCPP_PUBLIC + std::chrono::nanoseconds + period() const; + +private: + RCLCPP_DISABLE_COPY(Rate) + + Clock::SharedPtr clock_; + Duration period_; + Time last_interval_; +}; + +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 ef587578e2..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: * @@ -164,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/service.hpp b/rclcpp/include/rclcpp/service.hpp index 3cfc11e9ca..9e08dc235d 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -265,15 +265,19 @@ class ServiceBase 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}; - - std::recursive_mutex callback_mutex_; - std::function on_new_request_callback_{nullptr}; }; template @@ -348,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_)); @@ -383,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_)); @@ -420,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_)); @@ -482,6 +486,14 @@ 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"); } diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index 46379744a1..28e929c495 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -121,7 +121,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_triggered_handles_.emplace_back(std::move(waitable_handles_[i])); } } @@ -235,7 +235,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } for (const std::shared_ptr & waitable : waitable_handles_) { - waitable->add_to_wait_set(wait_set); + waitable->add_to_wait_set(*wait_set); } return true; } 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 eb1a980dd3..f8a10a5c27 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -104,7 +104,7 @@ class Subscription : public SubscriptionBase private: using SubscriptionTopicStatisticsSharedPtr = - std::shared_ptr>; + std::shared_ptr; public: RCLCPP_SMART_PTR_DEFINITIONS(Subscription) @@ -127,6 +127,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, @@ -148,6 +149,7 @@ class Subscription : public SubscriptionBase any_callback_(callback), options_(options), message_memory_strategy_(message_memory_strategy) + // *INDENT-ON* { // Setup intra process publishing if requested. if (rclcpp::detail::resolve_use_intra_process(options_, *node_base)) { @@ -163,10 +165,6 @@ 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, @@ -185,7 +183,7 @@ class Subscription : public SubscriptionBase 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( + TRACETOOLS_TRACEPOINT( rclcpp_subscription_init, static_cast(get_subscription_handle().get()), static_cast(subscription_intra_process_.get())); @@ -193,7 +191,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); } @@ -201,11 +200,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_)); @@ -316,7 +315,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); } } @@ -325,8 +324,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 @@ -357,7 +368,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); } } diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index f4957824a5..615f3852b6 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -260,13 +260,13 @@ class SubscriptionBase : public std::enable_shared_from_this bool is_serialized() const; - /// Return the type of the subscription. + /// Return the delivered message kind. /** * \return `DeliveredMessageKind`, which adjusts how messages are received and delivered. */ RCLCPP_PUBLIC DeliveredMessageKind - get_subscription_type() const; + get_delivered_message_kind() const; /// Get matching publisher count. /** \return The number of publishers on this topic. */ @@ -645,6 +645,14 @@ class SubscriptionBase : public std::enable_shared_from_this 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_; @@ -663,15 +671,12 @@ class SubscriptionBase : public std::enable_shared_from_this RCLCPP_DISABLE_COPY(SubscriptionBase) rosidl_message_type_support_t type_support_; - DeliveredMessageKind delivered_message_type_; + 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_; - - std::recursive_mutex callback_mutex_; - std::function on_new_message_callback_{nullptr}; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/subscription_factory.hpp b/rclcpp/include/rclcpp/subscription_factory.hpp index a1727eab5a..0e9d9fefe5 100644 --- a/rclcpp/include/rclcpp/subscription_factory.hpp +++ b/rclcpp/include/rclcpp/subscription_factory.hpp @@ -75,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 43b9ec034c..71f90fe15d 100644 --- a/rclcpp/include/rclcpp/subscription_options.hpp +++ b/rclcpp/include/rclcpp/subscription_options.hpp @@ -77,6 +77,10 @@ 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. + rclcpp::QoS qos = SystemDefaultsQoS(); }; TopicStatisticsOptions topic_stats_options; diff --git a/rclcpp/include/rclcpp/time.hpp b/rclcpp/include/rclcpp/time.hpp index 15533f39ef..a931f3ac9c 100644 --- a/rclcpp/include/rclcpp/time.hpp +++ b/rclcpp/include/rclcpp/time.hpp @@ -57,6 +57,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 +88,7 @@ class Time operator builtin_interfaces::msg::Time() const; /** + * Copy assignment operator * \throws std::runtime_error if seconds are negative */ RCLCPP_PUBLIC @@ -100,6 +105,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 +201,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 +234,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/timer.hpp b/rclcpp/include/rclcpp/timer.hpp index 91b1705985..6060d8bd78 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/rclcpp/include/rclcpp/timer.hpp @@ -53,12 +53,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 @@ -216,21 +221,22 @@ 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()), reinterpret_cast(&callback_)); #ifndef TRACETOOLS_DISABLED - if (TRACEPOINT_ENABLED(rclcpp_callback_register)) { + if (TRACETOOLS_TRACEPOINT_ENABLED(rclcpp_callback_register)) { char * symbol = tracetools::get_symbol(callback_); - DO_TRACEPOINT( + TRACETOOLS_DO_TRACEPOINT( rclcpp_callback_register, reinterpret_cast(&callback_), symbol); @@ -269,9 +275,9 @@ class GenericTimer : public TimerBase void execute_callback() override { - TRACEPOINT(callback_start, reinterpret_cast(&callback_), false); + TRACETOOLS_TRACEPOINT(callback_start, reinterpret_cast(&callback_), false); execute_callback_delegate<>(); - TRACEPOINT(callback_end, reinterpret_cast(&callback_)); + TRACETOOLS_TRACEPOINT(callback_end, reinterpret_cast(&callback_)); } // void specialization @@ -330,13 +336,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..781e2c86fc 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()); } } diff --git a/rclcpp/include/rclcpp/typesupport_helpers.hpp b/rclcpp/include/rclcpp/typesupport_helpers.hpp index 2fad84cf3b..c93b318440 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" @@ -40,11 +41,15 @@ get_typesupport_library(const std::string & type, const std::string & typesuppor /// Extract the 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. + * + * \deprecated Use get_message_typesupport_handle() instead + * * \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 */ +[[deprecated("Use `get_message_typesupport_handle` instead")]] RCLCPP_PUBLIC const rosidl_message_type_support_t * get_typesupport_handle( @@ -52,6 +57,40 @@ get_typesupport_handle( const std::string & typesupport_identifier, rcpputils::SharedLibrary & 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 + * \throws std::runtime_error if the symbol of type not found in the library. + * \return A message type support handle + */ +RCLCPP_PUBLIC +const rosidl_message_type_support_t * +get_message_typesupport_handle( + const std::string & type, + const std::string & typesupport_identifier, + rcpputils::SharedLibrary & library); + +/// Extract the service type support handle from the library. +/** + * The library needs to match the topic type. The shared library must stay loaded for the lifetime of the result. + * + * \param[in] type The service type, e.g. "std_srvs/srv/Empty" + * \param[in] typesupport_identifier Type support identifier, typically "rosidl_typesupport_cpp" + * \param[in] library The shared type support library + * \throws std::runtime_error if the symbol of type not found in the library. + * \return A service type support handle + */ +RCLCPP_PUBLIC +const rosidl_service_type_support_t * +get_service_typesupport_handle( + const std::string & type, + const std::string & typesupport_identifier, + rcpputils::SharedLibrary & library); + } // namespace rclcpp #endif // RCLCPP__TYPESUPPORT_HELPERS_HPP_ diff --git a/rclcpp/include/rclcpp/wait_result.hpp b/rclcpp/include/rclcpp/wait_result.hpp index e879043d04..74d3d2183c 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,8 +375,7 @@ class StoragePolicyCommon needs_pruning_ = true; continue; } - rclcpp::Waitable & waitable = *waitable_ptr_pair.second; - waitable.add_to_wait_set(&rcl_wait_set_); + waitable_entry.waitable->add_to_wait_set(rcl_wait_set_); } } @@ -405,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/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 be2e569c40..4afc2a1b27 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/sequential_synchronization.hpp @@ -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_template.hpp b/rclcpp/include/rclcpp/wait_set_template.hpp index b3f41f8ed5..ce69da6bf2 100644 --- a/rclcpp/include/rclcpp/wait_set_template.hpp +++ b/rclcpp/include/rclcpp/wait_set_template.hpp @@ -153,6 +153,7 @@ 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 key_event_pair : inner_subscription->get_event_handlers()) { @@ -164,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) { @@ -180,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;} } } }); @@ -224,6 +227,7 @@ 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 key_event_pair : inner_subscription->get_event_handlers()) { @@ -231,6 +235,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli 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) { @@ -239,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;} } } }); @@ -289,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;} }); } @@ -326,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;} }); } @@ -357,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;} }); } @@ -384,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;} }); } @@ -415,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;} }); } @@ -442,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;} }); } @@ -473,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;} }); } @@ -500,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;} }); } @@ -551,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;} }); } @@ -578,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;} }); } @@ -715,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 @@ -734,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 @@ -741,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 1a0d8b61f1..e834765a08 100644 --- a/rclcpp/include/rclcpp/waitable.hpp +++ b/rclcpp/include/rclcpp/waitable.hpp @@ -101,6 +101,23 @@ class Waitable size_t get_number_of_ready_guard_conditions(); +#ifdef __GNUC__ +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Woverloaded-virtual" +#endif + /// Deprecated. + /** + * \sa add_to_wait_set(rcl_wait_set_t &) + */ + [[deprecated("Use add_to_wait_set(rcl_wait_set_t & wait_set) signature")]] + RCLCPP_PUBLIC + virtual + void + add_to_wait_set(rcl_wait_set_t * wait_set); +#ifdef __GNUC__ +#pragma GCC diagnostic pop +#endif + /// Add the Waitable to a wait set. /** * \param[in] wait_set A handle to the wait set to add the Waitable to. @@ -109,7 +126,24 @@ class Waitable RCLCPP_PUBLIC virtual void - add_to_wait_set(rcl_wait_set_t * wait_set) = 0; + add_to_wait_set(rcl_wait_set_t & wait_set); + +#ifdef __GNUC__ +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Woverloaded-virtual" +#endif + /// Deprecated. + /** + * \sa is_ready(const rcl_wait_set_t &) + */ + [[deprecated("Use is_ready(const rcl_wait_set_t & wait_set) signature")]] + RCLCPP_PUBLIC + virtual + bool + is_ready(rcl_wait_set_t * wait_set); +#ifdef __GNUC__ +#pragma GCC diagnostic pop +#endif /// Check if the Waitable is ready. /** @@ -124,7 +158,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); /// Take the data so that it can be consumed with `execute`. /** @@ -178,6 +212,23 @@ class Waitable std::shared_ptr take_data_by_entity_id(size_t id); +#ifdef __GNUC__ +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Woverloaded-virtual" +#endif + /// Deprecated. + /** + * \sa execute(const std::shared_ptr &) + */ + [[deprecated("Use execute(const std::shared_ptr & data) signature")]] + RCLCPP_PUBLIC + virtual + void + execute(std::shared_ptr & data); +#ifdef __GNUC__ +#pragma GCC diagnostic pop +#endif + /// Execute data that is passed in. /** * Before calling this method, the Waitable should be added to a wait set, @@ -203,7 +254,7 @@ class Waitable RCLCPP_PUBLIC virtual void - execute(std::shared_ptr & data) = 0; + execute(const std::shared_ptr & data); /// Exchange the "in use by wait set" state for this timer. /** diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 1823f4d463..103b1c1c33 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 21.1.0 + 28.0.0 The ROS client library in C++. Ivan Paunovic @@ -22,6 +22,7 @@ builtin_interfaces rcl_interfaces rosgraph_msgs + rosidl_runtime_c rosidl_runtime_cpp rosidl_typesupport_c rosidl_typesupport_cpp @@ -29,12 +30,14 @@ 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 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 77f6c87bd9..bcacaabebe 100644 --- a/rclcpp/src/rclcpp/callback_group.cpp +++ b/rclcpp/src/rclcpp/callback_group.cpp @@ -31,12 +31,12 @@ using rclcpp::CallbackGroupType; CallbackGroup::CallbackGroup( CallbackGroupType group_type, - std::function get_context, + 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), - get_context_(get_context) + context_(context) {} CallbackGroup::~CallbackGroup() @@ -66,6 +66,7 @@ CallbackGroup::size() const timer_ptrs_.size() + waitable_ptrs_.size(); } + void CallbackGroup::collect_all_ptrs( std::function sub_func, std::function service_func, @@ -123,40 +124,12 @@ CallbackGroup::automatically_add_to_executor_with_node() const return automatically_add_to_executor_with_node_; } -// \TODO(mjcarroll) Deprecated, remove on tock -rclcpp::GuardCondition::SharedPtr -CallbackGroup::get_notify_guard_condition(const rclcpp::Context::SharedPtr context_ptr) -{ - std::lock_guard lock(notify_guard_condition_mutex_); - if (notify_guard_condition_ && context_ptr != notify_guard_condition_->get_context()) { - if (associated_with_executor_) { - trigger_notify_guard_condition(); - } - notify_guard_condition_ = nullptr; - } - - if (!notify_guard_condition_) { - notify_guard_condition_ = std::make_shared(context_ptr); - } - - return notify_guard_condition_; -} - rclcpp::GuardCondition::SharedPtr CallbackGroup::get_notify_guard_condition() { std::lock_guard lock(notify_guard_condition_mutex_); - if (!this->get_context_) { - throw std::runtime_error("Callback group was created without context and not passed context"); - } - auto context_ptr = this->get_context_(); + rclcpp::Context::SharedPtr context_ptr = context_.lock(); if (context_ptr && context_ptr->is_valid()) { - if (notify_guard_condition_ && context_ptr != notify_guard_condition_->get_context()) { - if (associated_with_executor_) { - trigger_notify_guard_condition(); - } - notify_guard_condition_ = nullptr; - } if (!notify_guard_condition_) { notify_guard_condition_ = std::make_shared(context_ptr); } diff --git a/rclcpp/src/rclcpp/client.cpp b/rclcpp/src/rclcpp/client.cpp index e33db71ce2..8388ee1888 100644 --- a/rclcpp/src/rclcpp/client.cpp +++ b/rclcpp/src/rclcpp/client.cpp @@ -125,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(); @@ -138,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() diff --git a/rclcpp/src/rclcpp/context.cpp b/rclcpp/src/rclcpp/context.cpp index 35a11730ab..bbc1d29d0f 100644 --- a/rclcpp/src/rclcpp/context.cpp +++ b/rclcpp/src/rclcpp/context.cpp @@ -462,7 +462,7 @@ template std::vector Context::get_shutdown_callback() const { - const auto get_callback_vector = [this](auto & mutex, auto & callback_set) { + const auto get_callback_vector = [](auto & mutex, auto & callback_set) { const std::lock_guard lock(mutex); std::vector callbacks; for (auto & callback : callback_set) { @@ -496,7 +496,7 @@ Context::sleep_for(const std::chrono::nanoseconds & 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); + 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()); 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/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/duration.cpp b/rclcpp/src/rclcpp/duration.cpp index 2eac7f6b58..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 { @@ -316,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/event_handler.cpp b/rclcpp/src/rclcpp/event_handler.cpp index 40ae6c030d..6232e5b0d9 100644 --- a/rclcpp/src/rclcpp/event_handler.cpp +++ b/rclcpp/src/rclcpp/event_handler.cpp @@ -39,13 +39,6 @@ UnsupportedEventTypeException::UnsupportedEventTypeException( EventHandlerBase::~EventHandlerBase() { - // Since the rmw event listener holds a reference to - // this 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(); - if (rcl_event_fini(&event_handle_) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( "rclcpp", @@ -63,9 +56,9 @@ EventHandlerBase::get_number_of_ready_events() /// Add the Waitable to a wait set. void -EventHandlerBase::add_to_wait_set(rcl_wait_set_t * wait_set) +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"); } @@ -73,9 +66,9 @@ EventHandlerBase::add_to_wait_set(rcl_wait_set_t * wait_set) /// Check if the Waitable is ready. bool -EventHandlerBase::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_; + return wait_set.events[wait_set_event_index_] == &event_handle_; } void diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index a7a2b8278b..a54d71e21b 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -13,6 +13,8 @@ // limitations under the License. #include +#include +#include #include #include #include @@ -22,13 +24,14 @@ #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" @@ -38,21 +41,29 @@ using namespace std::chrono_literals; -using rclcpp::exceptions::throw_from_rcl_error; using rclcpp::Executor; +/// Mask to indicate to the waitset to only add the subscription. +/// The events and intraprocess waitable are already added via the callback group. +static constexpr rclcpp::SubscriptionWaitSetMask kDefaultSubscriptionMask = {true, false, false}; + class rclcpp::ExecutorImplementation {}; Executor::Executor(const 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; - shutdown_callback_handle_ = context_->add_on_shutdown_callback( [weak_gc = std::weak_ptr{shutdown_guard_condition_}]() { auto strong_gc = weak_gc.lock(); @@ -61,74 +72,46 @@ 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()); - - // Put the executor's guard condition in - memory_strategy_->add_guard_condition(*interrupt_guard_condition_.get()); - rcl_allocator_t allocator = memory_strategy_->get_allocator(); + notify_waitable_->set_on_ready_callback( + [this](auto, auto) { + this->entities_need_rebuild_.store(true); + }); - rcl_ret_t 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(); - throw_from_rcl_error(ret, "Failed to create wait set in Executor constructor"); - } + notify_waitable_->add_guard_condition(interrupt_guard_condition_); + notify_waitable_->add_guard_condition(shutdown_guard_condition_); } 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_groups_to_guard_conditions_) { - auto guard_condition = pair.second; - memory_strategy_->remove_guard_condition(guard_condition); - } - weak_groups_to_guard_conditions_.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(); + current_collection_.clients.update( + {}, {}, + [this](auto client) {wait_set_.remove_client(client);}); - // 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(); - } - // Remove and release the sigint guard condition - memory_strategy_->remove_guard_condition(shutdown_guard_condition_.get()); - memory_strategy_->remove_guard_condition(interrupt_guard_condition_.get()); + 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_)) { @@ -142,95 +125,39 @@ Executor::~Executor() std::vector Executor::get_all_callback_groups() { - 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); - } - for (auto const & group_node_ptr : weak_groups_to_nodes_associated_with_executor_) { - groups.push_back(group_node_ptr.first); - } - return groups; + this->collector_.update_collections(); + return this->collector_.get_all_callback_groups(); } std::vector Executor::get_manually_added_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_manually_added_callback_groups(); } std::vector Executor::get_automatically_added_callback_groups_from_nodes() { - 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; + this->collector_.update_collections(); + return this->collector_.get_automatically_added_callback_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); - } - }); - } - } -} - -void -Executor::add_callback_group_to_map( +Executor::add_callback_group( rclcpp::CallbackGroup::SharedPtr group_ptr, rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, - rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & weak_groups_to_nodes, bool notify) { - // 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."); - } - - 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)); + (void) node_ptr; + this->collector_.add_callback_group(group_ptr); - if (node_ptr->get_context()->is_valid()) { - auto callback_group_guard_condition = group_ptr->get_notify_guard_condition(); - weak_groups_to_guard_conditions_[weak_group_ptr] = callback_group_guard_condition.get(); - // Add the callback_group's notify condition to the guard condition handles - memory_strategy_->add_guard_condition(*callback_group_guard_condition); + if (!spinning.load()) { + std::lock_guard guard(mutex_); + this->collect_entities(); } if (notify) { - // Interrupt waiting to handle new node try { interrupt_guard_condition_->trigger(); } catch (const rclcpp::exceptions::RCLError & ex) { @@ -241,91 +168,23 @@ Executor::add_callback_group_to_map( } } -void -Executor::add_callback_group( - rclcpp::CallbackGroup::SharedPtr group_ptr, - 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 Executor::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify) { - // If the node already has an executor - std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic(); - if (has_executor.exchange(true)) { - throw std::runtime_error( - 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); - } - }); + this->collector_.add_node(node_ptr); - const auto gc = node_ptr->get_shared_notify_guard_condition(); - weak_nodes_to_guard_conditions_[node_ptr] = gc.get(); - // Add the node's notify condition to the guard condition handles - memory_strategy_->add_guard_condition(*gc); - weak_nodes_.push_back(node_ptr); -} - -void -Executor::remove_callback_group_from_map( - rclcpp::CallbackGroup::SharedPtr group_ptr, - rclcpp::memory_strategy::MemoryStrategy::WeakCallbackGroupsToNodesMap & weak_groups_to_nodes, - 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 (!spinning.load()) { + std::lock_guard guard(mutex_); + this->collect_entities(); } - // 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_)) - { - auto iter = weak_groups_to_guard_conditions_.find(weak_group_ptr); - if (iter != weak_groups_to_guard_conditions_.end()) { - memory_strategy_->remove_guard_condition(iter->second); - } - weak_groups_to_guard_conditions_.erase(weak_group_ptr); - - if (notify) { - 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 remove: ") + ex.what()); - } + + if (notify) { + try { + interrupt_guard_condition_->trigger(); + } catch (const rclcpp::exceptions::RCLError & ex) { + throw std::runtime_error( + std::string( + "Failed to trigger guard condition on node add: ") + ex.what()); } } } @@ -335,11 +194,21 @@ 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); + + if (!spinning.load()) { + std::lock_guard guard(mutex_); + this->collect_entities(); + } + if (notify) { + 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 remove: ") + ex.what()); + } + } } void @@ -351,48 +220,22 @@ 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."); + if (!spinning.load()) { + std::lock_guard guard(mutex_); + this->collect_entities(); } - 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); + if (notify) { + try { + interrupt_guard_condition_->trigger(); + } catch (const rclcpp::exceptions::RCLError & ex) { + throw std::runtime_error( + std::string( + "Failed to trigger guard condition on node remove: ") + ex.what()); } } - - memory_strategy_->remove_guard_condition(node_ptr->get_shared_notify_guard_condition().get()); - weak_nodes_to_guard_conditions_.erase(node_ptr); - - std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic(); - has_executor.store(false); } void @@ -431,6 +274,22 @@ 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) { @@ -459,20 +318,25 @@ Executor::spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive) throw std::runtime_error("spin_some() called while already spinning"); } RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); - bool work_available = false; + while (rclcpp::ok(context_) && spinning.load() && max_duration_not_elapsed()) { - AnyExecutable any_exec; - if (!work_available) { - wait_for_work(std::chrono::milliseconds::zero()); + if (!wait_result_.has_value()) { + wait_for_work(std::chrono::milliseconds(0)); } + + AnyExecutable any_exec; if (get_next_ready_executable(any_exec)) { execute_any_executable(any_exec); - work_available = true; } else { - if (!work_available || !exhaustive) { - break; - } - work_available = false; + // If nothing is ready, reset the result to signal we are + // ready to wait again + wait_result_.reset(); + } + + if (!wait_result_.has_value() && !exhaustive) { + // In the case of spin some, then we can exit + // In the case of spin all, then we will allow ourselves to wait again. + break; } } } @@ -508,30 +372,21 @@ Executor::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."); - } - std::lock_guard guard{mutex_}; - memory_strategy_ = memory_strategy; -} - void Executor::execute_any_executable(AnyExecutable & any_exec) { if (!spinning.load()) { return; } + if (any_exec.timer) { - TRACEPOINT( + TRACETOOLS_TRACEPOINT( rclcpp_executor_execute, static_cast(any_exec.timer->get_timer_handle().get())); execute_timer(any_exec.timer); } if (any_exec.subscription) { - TRACEPOINT( + TRACETOOLS_TRACEPOINT( rclcpp_executor_execute, static_cast(any_exec.subscription->get_subscription_handle().get())); execute_subscription(any_exec.subscription); @@ -543,18 +398,13 @@ 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. - try { - interrupt_guard_condition_->trigger(); - } catch (const rclcpp::exceptions::RCLError & ex) { - throw std::runtime_error( - std::string( - "Failed to trigger guard condition from execute_any_executable: ") + ex.what()); + if (any_exec.callback_group) { + any_exec.callback_group->can_be_taken_from().store(true); } } @@ -603,7 +453,7 @@ Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription) rclcpp::MessageInfo message_info; message_info.get_rmw_message_info().from_intra_process = false; - switch (subscription->get_subscription_type()) { + switch (subscription->get_delivered_message_kind()) { // Deliver ROS message case rclcpp::DeliveredMessageKind::ROS_MESSAGE: { @@ -653,6 +503,11 @@ Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription) subscription->get_topic_name(), [&]() {return subscription->take_type_erased(message.get(), message_info);}, [&]() {subscription->handle_message(message, message_info);}); + // TODO(clalancette): In the case that the user is using the MessageMemoryPool, + // and they take a shared_ptr reference to the message in the callback, this can + // inadvertently return the message to the pool when the user is still using it. + // This is a bug that needs to be fixed in the pool, and we should probably have + // a custom deleter for the message that actually does the return_message(). subscription->return_message(message); } break; @@ -689,7 +544,6 @@ Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription) throw std::runtime_error("Delivered message kind is not supported"); } } - return; } void @@ -711,8 +565,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(); @@ -724,227 +577,213 @@ Executor::execute_client( } void -Executor::wait_for_work(std::chrono::nanoseconds timeout) -{ - TRACEPOINT(rclcpp_executor_wait_for_work, timeout.count()); - { - std::lock_guard guard(mutex_); - - // 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); - } - auto callback_guard_pair = weak_groups_to_guard_conditions_.find(group_ptr); - if (callback_guard_pair != weak_groups_to_guard_conditions_.end()) { - auto guard_condition = callback_guard_pair->second; - weak_groups_to_guard_conditions_.erase(group_ptr); - memory_strategy_->remove_guard_condition(guard_condition); - } - weak_groups_to_nodes_.erase(group_ptr); - }); - } +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); + }); - // 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_.clients.update( + collection.clients, + [this](auto client) {wait_set_.add_client(client);}, + [this](auto client) {wait_set_.remove_client(client);}); - // 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_.services.update( + collection.services, + [this](auto service) {wait_set_.add_service(service);}, + [this](auto service) {wait_set_.remove_service(service);}); - if (!memory_strategy_->add_handles_to_wait_set(&wait_set_)) { - throw std::runtime_error("Couldn't fill 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);}); - 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"); - } + current_collection_.waitables.update( + collection.waitables, + [this](auto waitable) {wait_set_.add_waitable(waitable);}, + [this](auto waitable) {wait_set_.remove_waitable(waitable);}); - // 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(); + this->entities_need_rebuild_.store(false); } -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; - } - 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; + { + std::lock_guard guard(mutex_); + if (current_collection_.empty() || this->entities_need_rebuild_.load()) { + this->collect_entities(); } } - return nullptr; + 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."); + } } 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) -{ - TRACEPOINT(rclcpp_executor_get_next_ready); - 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()) { + 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. + if (!timer->call()) { + continue; + } + any_executable.timer = timer; + any_executable.callback_group = callback_group; + valid_executable = true; + break; + } } } - 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 @@ -967,22 +806,6 @@ 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(); -} - bool Executor::is_spinning() { diff --git a/rclcpp/src/rclcpp/executors.cpp b/rclcpp/src/rclcpp/executors.cpp index 0a900c07da..3ea8d658ad 100644 --- a/rclcpp/src/rclcpp/executors.cpp +++ b/rclcpp/src/rclcpp/executors.cpp @@ -14,6 +14,21 @@ #include "rclcpp/executors.hpp" +void +rclcpp::spin_all( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, + std::chrono::nanoseconds max_duration) +{ + rclcpp::executors::SingleThreadedExecutor exec; + 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) { diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp index 88a878824a..765002b2ef 100644 --- a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp +++ b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp @@ -39,6 +39,30 @@ void ExecutorEntitiesCollection::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, @@ -203,12 +227,12 @@ ready_executables( } } - for (auto & [handle, entry] : collection.waitables) { + for (const auto & [handle, entry] : collection.waitables) { auto waitable = entry.entity.lock(); if (!waitable) { continue; } - if (!waitable->is_ready(&rcl_wait_set)) { + if (!waitable->is_ready(rcl_wait_set)) { continue; } auto group_info = group_cache(entry.callback_group); @@ -218,13 +242,10 @@ ready_executables( rclcpp::AnyExecutable exec; exec.waitable = waitable; exec.callback_group = group_info; - exec.data = waitable->take_data(); 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 index 84ada64925..702716a758 100644 --- a/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp +++ b/rclcpp/src/rclcpp/executors/executor_entities_collector.cpp @@ -61,6 +61,12 @@ ExecutorEntitiesCollector::~ExecutorEntitiesCollector() 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(); @@ -105,7 +111,8 @@ void ExecutorEntitiesCollector::remove_node( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr) { - if (!node_ptr->get_associated_with_executor_atomic().load()) { + 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."); @@ -143,6 +150,11 @@ ExecutorEntitiesCollector::add_callback_group(rclcpp::CallbackGroup::SharedPtr g } 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 @@ -161,7 +173,6 @@ ExecutorEntitiesCollector::remove_callback_group(rclcpp::CallbackGroup::SharedPt 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; @@ -314,7 +325,11 @@ ExecutorEntitiesCollector::process_queues() if (node_it != weak_nodes_.end()) { remove_weak_node(node_it); } else { - throw std::runtime_error("Node needs to be associated with this executor."); + // 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(); @@ -337,6 +352,13 @@ ExecutorEntitiesCollector::process_queues() 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(); diff --git a/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp b/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp index 15a31cd60d..a95bc3797c 100644 --- a/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp +++ b/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp @@ -27,15 +27,17 @@ ExecutorNotifyWaitable::ExecutorNotifyWaitable(std::function on_exec { } -ExecutorNotifyWaitable::ExecutorNotifyWaitable(const ExecutorNotifyWaitable & other) -: ExecutorNotifyWaitable(other.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=(const ExecutorNotifyWaitable & other) +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_; } @@ -43,43 +45,40 @@ ExecutorNotifyWaitable & ExecutorNotifyWaitable::operator=(const ExecutorNotifyW } void -ExecutorNotifyWaitable::add_to_wait_set(rcl_wait_set_t * wait_set) +ExecutorNotifyWaitable::add_to_wait_set(rcl_wait_set_t & wait_set) { std::lock_guard lock(guard_condition_mutex_); - for (auto weak_guard_condition : this->notify_guard_conditions_) { auto guard_condition = weak_guard_condition.lock(); - if (guard_condition) { - auto rcl_guard_condition = &guard_condition->get_rcl_guard_condition(); + if (!guard_condition) {continue;} - rcl_ret_t ret = rcl_wait_set_add_guard_condition( - wait_set, - rcl_guard_condition, NULL); + 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"); - } + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "failed to add guard condition to wait set"); } } } bool -ExecutorNotifyWaitable::is_ready(rcl_wait_set_t * wait_set) +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) { - auto rcl_guard_condition = wait_set->guard_conditions[ii]; + 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 (auto weak_guard_condition : this->notify_guard_conditions_) { + 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; } } } @@ -87,7 +86,7 @@ ExecutorNotifyWaitable::is_ready(rcl_wait_set_t * wait_set) } void -ExecutorNotifyWaitable::execute(std::shared_ptr & data) +ExecutorNotifyWaitable::execute(const std::shared_ptr & data) { (void) data; this->execute_callback_(); diff --git a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp index 507d47f913..2d5c76e817 100644 --- a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp @@ -55,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(this->spinning.store(false);); std::vector threads; size_t thread_id = 0; { @@ -99,6 +99,18 @@ MultiThreadedExecutor::run(size_t this_thread_number) 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..975733b497 100644 --- a/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp @@ -31,6 +31,11 @@ SingleThreadedExecutor::spin() throw std::runtime_error("spin() called while already spinning"); } RCPPUTILS_SCOPE_EXIT(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 6fd0b56a85..0000000000 --- a/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp +++ /dev/null @@ -1,524 +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" -#include "rclcpp/detail/add_guard_condition_to_rcl_wait_set.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) -{ - // 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; - - // 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(); - // Add new nodes guard conditions to map - std::lock_guard guard{new_nodes_mutex_}; - for (const auto & weak_node : new_nodes_) { - if (auto node_ptr = weak_node.lock()) { - weak_nodes_to_guard_conditions_[node_ptr] = - node_ptr->get_shared_notify_guard_condition().get(); - } - } - new_nodes_.clear(); -} - -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"); - } -} - -void -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; - detail::add_guard_condition_to_rcl_wait_set(*wait_set, *gc); - } -} - -size_t StaticExecutorEntitiesCollector::get_number_of_ready_guard_conditions() -{ - std::lock_guard guard{new_nodes_mutex_}; - return weak_nodes_to_guard_conditions_.size() + new_nodes_.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) { - std::lock_guard guard{new_nodes_mutex_}; - new_nodes_.push_back(node_ptr); - 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 { - const rcl_guard_condition_t & rcl_gc = pair.second->get_rcl_guard_condition(); - return &rcl_gc == 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 3c14b37b45..831076c61c 100644 --- a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp @@ -12,31 +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 - +#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() @@ -46,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_); - + // 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)); } } @@ -64,7 +50,6 @@ 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); } @@ -80,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_); - } - 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; + } } } } @@ -117,163 +98,95 @@ 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_); - } - 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); + 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_callback_group( - rclcpp::CallbackGroup::SharedPtr group_ptr, - 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_callback_group(group_ptr, node_ptr); - if (is_new_node && notify) { - // Interrupt waiting to handle new node - interrupt_guard_condition_->trigger(); + if (current_collection_.empty() || this->entities_need_rebuild_.load()) { + this->collect_entities(); } -} - -void -StaticSingleThreadedExecutor::add_node( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify) -{ - bool is_new_node = entities_collector_->add_node(node_ptr); - if (is_new_node && notify) { - // Interrupt waiting to handle new node - interrupt_guard_condition_->trigger(); + 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 {}; } + return wait_result; } -void -StaticSingleThreadedExecutor::add_node(std::shared_ptr 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) { - 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) { - interrupt_guard_condition_->trigger(); + bool any_ready_executable = false; + if (wait_result.kind() != rclcpp::WaitResultKind::Ready) { + return any_ready_executable; } -} -void -StaticSingleThreadedExecutor::remove_node( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify) -{ - bool node_removed = entities_collector_->remove_node(node_ptr); - if (!node_removed) { - throw std::runtime_error("Node needs to be associated with this executor."); - } - // If the node was matched and removed, interrupt waiting - if (notify) { - interrupt_guard_condition_->trigger(); + 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()) { - auto timer = entities_collector_->get_timer(i); - timer->call(); - execute_timer(std::move(timer)); - if (spin_once) { - return 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); + if (timer->call()) { + execute_timer(timer); 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 index 094ff21282..f7ba6da2df 100644 --- a/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp +++ b/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp @@ -50,6 +50,9 @@ EventsExecutor::EventsExecutor( timers_manager_ = std::make_shared(context_, timer_on_ready_cb); + this->current_entities_collection_ = + std::make_shared(); + notify_waitable_ = std::make_shared( [this]() { // This callback is invoked when: @@ -61,6 +64,10 @@ EventsExecutor::EventsExecutor( this->refresh_current_collection_from_callback_groups(); }); + // Make sure that the notify waitable is immediately added to the collection + // to avoid missing events + this->add_notify_waitable_to_collection(current_entities_collection_->waitables); + notify_waitable_->add_guard_condition(interrupt_guard_condition_); notify_waitable_->add_guard_condition(shutdown_guard_condition_); @@ -87,9 +94,6 @@ EventsExecutor::EventsExecutor( this->entities_collector_ = std::make_shared(notify_waitable_); - - this->current_entities_collection_ = - std::make_shared(); } EventsExecutor::~EventsExecutor() @@ -202,11 +206,12 @@ EventsExecutor::spin_once_impl(std::chrono::nanoseconds timeout) timeout = std::chrono::nanoseconds::max(); } - // Select the smallest between input timeout and timer timeout + // 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 < timeout) { - timeout = next_timer_timeout; + if (next_timer_timeout.has_value() && next_timer_timeout.value() < timeout) { + timeout = next_timer_timeout.value(); is_timer_timeout = true; } @@ -269,10 +274,13 @@ EventsExecutor::execute_event(const ExecutorEvent & event) switch (event.type) { case ExecutorEventType::CLIENT_EVENT: { - auto client = this->retrieve_entity( - static_cast(event.entity_key), - current_entities_collection_->clients); - + rclcpp::ClientBase::SharedPtr client; + { + std::lock_guard lock(collection_mutex_); + client = this->retrieve_entity( + static_cast(event.entity_key), + current_entities_collection_->clients); + } if (client) { for (size_t i = 0; i < event.num_events; i++) { execute_client(client); @@ -283,9 +291,13 @@ EventsExecutor::execute_event(const ExecutorEvent & event) } case ExecutorEventType::SUBSCRIPTION_EVENT: { - auto subscription = this->retrieve_entity( - static_cast(event.entity_key), - current_entities_collection_->subscriptions); + rclcpp::SubscriptionBase::SharedPtr subscription; + { + std::lock_guard lock(collection_mutex_); + subscription = this->retrieve_entity( + static_cast(event.entity_key), + current_entities_collection_->subscriptions); + } if (subscription) { for (size_t i = 0; i < event.num_events; i++) { execute_subscription(subscription); @@ -295,10 +307,13 @@ EventsExecutor::execute_event(const ExecutorEvent & event) } case ExecutorEventType::SERVICE_EVENT: { - auto service = this->retrieve_entity( - static_cast(event.entity_key), - current_entities_collection_->services); - + rclcpp::ServiceBase::SharedPtr service; + { + std::lock_guard lock(collection_mutex_); + service = this->retrieve_entity( + static_cast(event.entity_key), + current_entities_collection_->services); + } if (service) { for (size_t i = 0; i < event.num_events; i++) { execute_service(service); @@ -315,12 +330,16 @@ EventsExecutor::execute_event(const ExecutorEvent & event) } case ExecutorEventType::WAITABLE_EVENT: { - auto waitable = this->retrieve_entity( - static_cast(event.entity_key), - current_entities_collection_->waitables); + rclcpp::Waitable::SharedPtr waitable; + { + std::lock_guard lock(collection_mutex_); + waitable = this->retrieve_entity( + static_cast(event.entity_key), + current_entities_collection_->waitables); + } if (waitable) { for (size_t i = 0; i < event.num_events; i++) { - auto data = waitable->take_data_by_entity_id(event.waitable_data); + const auto data = waitable->take_data_by_entity_id(event.waitable_data); waitable->execute(data); } } @@ -382,6 +401,7 @@ EventsExecutor::get_automatically_added_callback_groups_from_nodes() void EventsExecutor::refresh_current_collection_from_callback_groups() { + // Build the new collection this->entities_collector_->update_collections(); auto callback_groups = this->entities_collector_->get_all_callback_groups(); rclcpp::executors::ExecutorEntitiesCollection new_collection; @@ -395,18 +415,11 @@ EventsExecutor::refresh_current_collection_from_callback_groups() // retrieved in the "standard" way. // To do it, we need to add the notify waitable as an entry in both the new and // current collections such that it's neither added or removed. - rclcpp::CallbackGroup::WeakPtr weak_group_ptr; - new_collection.waitables.insert( - { - this->notify_waitable_.get(), - {this->notify_waitable_, weak_group_ptr} - }); + this->add_notify_waitable_to_collection(new_collection.waitables); - this->current_entities_collection_->waitables.insert( - { - this->notify_waitable_.get(), - {this->notify_waitable_, weak_group_ptr} - }); + // Acquire lock before modifying the current collection + std::lock_guard lock(collection_mutex_); + this->add_notify_waitable_to_collection(current_entities_collection_->waitables); this->refresh_current_collection(new_collection); } @@ -415,6 +428,9 @@ void EventsExecutor::refresh_current_collection( const rclcpp::executors::ExecutorEntitiesCollection & new_collection) { + // Acquire lock before modifying the current collection + std::lock_guard lock(collection_mutex_); + current_entities_collection_->timers.update( new_collection.timers, [this](rclcpp::TimerBase::SharedPtr timer) {timers_manager_->add_timer(timer);}, @@ -486,3 +502,16 @@ EventsExecutor::create_waitable_callback(const rclcpp::Waitable * entity_key) }; 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 index f4ecd16b76..39924afa56 100644 --- a/rclcpp/src/rclcpp/experimental/timers_manager.cpp +++ b/rclcpp/src/rclcpp/experimental/timers_manager.cpp @@ -28,9 +28,9 @@ using rclcpp::experimental::TimersManager; TimersManager::TimersManager( std::shared_ptr context, std::function on_ready_callback) +: on_ready_callback_(on_ready_callback), + context_(context) { - context_ = context; - on_ready_callback_ = on_ready_callback; } TimersManager::~TimersManager() @@ -100,7 +100,7 @@ void TimersManager::stop() } } -std::chrono::nanoseconds TimersManager::get_head_timeout() +std::optional TimersManager::get_head_timeout() { // Do not allow to interfere with the thread running if (running_) { @@ -169,7 +169,7 @@ void TimersManager::execute_ready_timer(const rclcpp::TimerBase * timer_id) } } -std::chrono::nanoseconds TimersManager::get_head_timeout_unsafe() +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()) { @@ -191,7 +191,9 @@ std::chrono::nanoseconds TimersManager::get_head_timeout_unsafe() } head_timer = locked_heap.front(); } - + if (head_timer->is_canceled()) { + return std::nullopt; + } return head_timer->time_until_trigger(); } @@ -242,17 +244,34 @@ void TimersManager::run_timers() // Lock mutex std::unique_lock lock(timers_mutex_); - std::chrono::nanoseconds time_to_sleep = get_head_timeout_unsafe(); + 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(); + } - // No need to wait if a timer is already available - if (time_to_sleep > std::chrono::nanoseconds::zero()) { - if (time_to_sleep != std::chrono::nanoseconds::max()) { - // Wait until timeout or notification that timers have been updated - timers_cv_.wait_for(lock, time_to_sleep, [this]() {return timers_updated_;}); - } else { - // Wait until notification that timers have been updated - timers_cv_.wait(lock, [this]() {return timers_updated_;}); - } + // 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 diff --git a/rclcpp/src/rclcpp/generic_client.cpp b/rclcpp/src/rclcpp/generic_client.cpp new file mode 100644 index 0000000000..fdcfc70aab --- /dev/null +++ b/rclcpp/src/rclcpp/generic_client.cpp @@ -0,0 +1,164 @@ +// 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) +{ + ts_lib_ = get_typesupport_library( + service_type, "rosidl_typesupport_cpp"); + + auto 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); + + 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)); + } +} + +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; +} + +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 87ace1eb19..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); @@ -68,6 +72,7 @@ void GenericPublisher::deserialize_message( 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); diff --git a/rclcpp/src/rclcpp/generic_subscription.cpp b/rclcpp/src/rclcpp/generic_subscription.cpp index e6e61add24..ae28354b98 100644 --- a/rclcpp/src/rclcpp/generic_subscription.cpp +++ b/rclcpp/src/rclcpp/generic_subscription.cpp @@ -49,9 +49,9 @@ 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 diff --git a/rclcpp/src/rclcpp/guard_condition.cpp b/rclcpp/src/rclcpp/guard_condition.cpp index 627644e602..700985f620 100644 --- a/rclcpp/src/rclcpp/guard_condition.cpp +++ b/rclcpp/src/rclcpp/guard_condition.cpp @@ -23,16 +23,17 @@ namespace rclcpp { GuardCondition::GuardCondition( - rclcpp::Context::SharedPtr context, + const rclcpp::Context::SharedPtr & context, rcl_guard_condition_options_t guard_condition_options) -: context_(context), rcl_guard_condition_{rcl_get_zero_initialized_guard_condition()} +: rcl_guard_condition_{rcl_get_zero_initialized_guard_condition()} { - if (!context_) { + if (!context) { throw std::invalid_argument("context argument unexpectedly nullptr"); } + rcl_ret_t ret = rcl_guard_condition_init( &this->rcl_guard_condition_, - context_->get_rcl_context().get(), + context->get_rcl_context().get(), guard_condition_options); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret, "failed to create guard condition"); @@ -53,12 +54,6 @@ GuardCondition::~GuardCondition() } } -rclcpp::Context::SharedPtr -GuardCondition::get_context() const -{ - return context_; -} - rcl_guard_condition_t & GuardCondition::get_rcl_guard_condition() { @@ -97,19 +92,19 @@ GuardCondition::exchange_in_use_by_wait_set_state(bool in_use_state) } void -GuardCondition::add_to_wait_set(rcl_wait_set_t * wait_set) +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_) { + if (&wait_set != wait_set_) { throw std::runtime_error("guard condition has already been added to a wait set."); } } else { - wait_set_ = wait_set; + wait_set_ = &wait_set; } - rcl_ret_t ret = rcl_wait_set_add_guard_condition(wait_set, &this->rcl_guard_condition_, NULL); + 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"); 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 874858b180..2e83c07013 100644 --- a/rclcpp/src/rclcpp/logger.cpp +++ b/rclcpp/src/rclcpp/logger.cpp @@ -17,6 +17,7 @@ #include #include "rcl_logging_interface/rcl_logging_interface.h" +#include "rcl/error_handling.h" #include "rcl/logging_rosout.h" #include "rclcpp/exceptions.hpp" @@ -80,10 +81,12 @@ Logger::get_child(const std::string & suffix) { std::lock_guard guard(*logging_mutex); rcl_ret = rcl_logging_rosout_add_sublogger((*name_).c_str(), suffix.c_str()); - if (RCL_RET_OK != rcl_ret) { + 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", - rcutils_get_error_state(), rcutils_reset_error); + rcl_get_error_state(), rcl_reset_error); } } @@ -98,9 +101,7 @@ Logger::get_child(const std::string & suffix) logger_sublogger_pairname_ptr->second.c_str()); delete logger_sublogger_pairname_ptr; if (RCL_RET_OK != rcl_ret) { - exceptions::throw_from_rcl_error( - rcl_ret, "failed to call rcl_logging_rosout_remove_sublogger", - rcutils_get_error_state(), rcutils_reset_error); + rcl_reset_error(); } }); } 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/node.cpp b/rclcpp/src/rclcpp/node.cpp index 53e77dd796..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" @@ -109,6 +111,22 @@ 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) @@ -206,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_(""), @@ -246,7 +270,8 @@ Node::Node( 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; @@ -498,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 { @@ -591,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() { @@ -634,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 6544d69214..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" @@ -54,17 +57,12 @@ NodeBase::NodeBase( std::shared_ptr logging_mutex = get_global_logging_mutex(); rcl_ret_t ret; - { - 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); - } + + // 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 @@ -114,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", @@ -204,14 +218,9 @@ NodeBase::create_callback_group( rclcpp::CallbackGroupType group_type, bool automatically_add_to_executor_with_node) { - auto weak_context = this->get_context()->weak_from_this(); - auto get_node_context = [weak_context]() -> rclcpp::Context::SharedPtr { - return weak_context.lock(); - }; - auto group = std::make_shared( group_type, - get_node_context, + context_->weak_from_this(), automatically_add_to_executor_with_node); std::lock_guard lock(callback_groups_mutex_); callback_groups_.push_back(group); diff --git a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp index 1228703cb6..f6e9e1fda0 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp @@ -498,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 { diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index c8bb62e238..922ce9e4d1 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -1038,37 +1038,50 @@ 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; } diff --git a/rclcpp/src/rclcpp/node_interfaces/node_services.cpp b/rclcpp/src/rclcpp/node_interfaces/node_services.cpp index e9c4a5266e..fdd4e83780 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_services.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_services.cpp @@ -32,8 +32,7 @@ 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"); } } else { group = node_base_->get_default_callback_group(); @@ -58,8 +57,7 @@ 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"); } } else { group = node_base_->get_default_callback_group(); diff --git a/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp b/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp index 96097def22..29d3125e1f 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp @@ -34,8 +34,7 @@ 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"); } } else { callback_group = node_base_->get_default_callback_group(); @@ -50,7 +49,7 @@ NodeTimers::add_timer( 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 659129d62c..ce71036b93 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp @@ -58,7 +58,7 @@ 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(); @@ -97,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(); 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 02a9de82b0..96eb8df9cf 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp @@ -32,8 +32,7 @@ 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"); } } else { group = node_base_->get_default_callback_group(); 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 698db2d559..111246ed96 100644 --- a/rclcpp/src/rclcpp/publisher_base.cpp +++ b/rclcpp/src/rclcpp/publisher_base.cpp @@ -270,6 +270,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 { @@ -384,3 +391,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/rate.cpp b/rclcpp/src/rclcpp/rate.cpp new file mode 100644 index 0000000000..7c98d6d4fe --- /dev/null +++ b/rclcpp/src/rclcpp/rate.cpp @@ -0,0 +1,106 @@ +// 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; +} + +bool +Rate::is_steady() const +{ + return clock_->get_clock_type() == RCL_STEADY_TIME; +} + +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/signal_handler.cpp b/rclcpp/src/rclcpp/signal_handler.cpp index 7085c63bdf..cf26d06df4 100644 --- a/rclcpp/src/rclcpp/signal_handler.cpp +++ b/rclcpp/src/rclcpp/signal_handler.cpp @@ -113,7 +113,7 @@ SignalHandler::get_logger() SignalHandler & SignalHandler::get_global_signal_handler() { - static SignalHandler signal_handler; + static SignalHandler & signal_handler = *new SignalHandler(); return signal_handler; } diff --git a/rclcpp/src/rclcpp/signal_handler.hpp b/rclcpp/src/rclcpp/signal_handler.hpp index 0d3c399ccb..db608b0d10 100644 --- a/rclcpp/src/rclcpp/signal_handler.hpp +++ b/rclcpp/src/rclcpp/signal_handler.hpp @@ -75,7 +75,7 @@ class SignalHandler final bool install(SignalHandlerOptions signal_handler_options = SignalHandlerOptions::All); - /// Uninstall the signal handler for SIGINT/SIGTERM and join the dedicated singal handling + /// Uninstall the signal handler for SIGINT/SIGTERM and join the dedicated signal handling /// thread. /** * Also restores the previous signal handler. @@ -189,7 +189,7 @@ class SignalHandler final // Whether or not a signal has been received. std::atomic_bool signal_received_ = false; - // A thread to which singal handling tasks are deferred. + // A thread to which signal handling tasks are deferred. std::thread signal_handler_thread_; // A mutex used to synchronize the install() and uninstall() methods. diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index 18ccab0eb0..f7c238a910 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -52,7 +52,7 @@ SubscriptionBase::SubscriptionBase( intra_process_subscription_id_(0), event_callbacks_(event_callbacks), type_support_(type_support_handle), - delivered_message_type_(delivered_message_kind) + delivered_message_kind_(delivered_message_kind) { auto custom_deletor = [node_handle = this->node_handle_](rcl_subscription_t * rcl_subs) { @@ -218,7 +218,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 ); - TRACEPOINT(rclcpp_take, static_cast(message_out)); + TRACETOOLS_TRACEPOINT(rclcpp_take, static_cast(message_out)); if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret) { return false; } else if (RCL_RET_OK != ret) { @@ -244,6 +244,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) { @@ -261,13 +264,13 @@ SubscriptionBase::get_message_type_support_handle() const bool SubscriptionBase::is_serialized() const { - return delivered_message_type_ == rclcpp::DeliveredMessageKind::SERIALIZED_MESSAGE; + return delivered_message_kind_ == rclcpp::DeliveredMessageKind::SERIALIZED_MESSAGE; } rclcpp::DeliveredMessageKind -SubscriptionBase::get_subscription_type() const +SubscriptionBase::get_delivered_message_kind() const { - return delivered_message_type_; + return delivered_message_kind_; } size_t @@ -298,7 +301,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 diff --git a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp index b13123b65b..f9d19da8c5 100644 --- a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp +++ b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp @@ -18,9 +18,9 @@ using rclcpp::experimental::SubscriptionIntraProcessBase; void -SubscriptionIntraProcessBase::add_to_wait_set(rcl_wait_set_t * wait_set) +SubscriptionIntraProcessBase::add_to_wait_set(rcl_wait_set_t & wait_set) { - detail::add_guard_condition_to_rcl_wait_set(*wait_set, gc_); + detail::add_guard_condition_to_rcl_wait_set(wait_set, gc_); } const char * @@ -34,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..978651516c 100644 --- a/rclcpp/src/rclcpp/time.cpp +++ b/rclcpp/src/rclcpp/time.cpp @@ -65,6 +65,8 @@ Time::Time(int64_t nanoseconds, rcl_clock_type_t clock_type) 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 +86,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 +103,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 { @@ -276,10 +269,25 @@ Time::operator-=(const rclcpp::Duration & rhs) } 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 7f3b3b9536..465ceaf5a7 100644 --- a/rclcpp/src/rclcpp/time_source.cpp +++ b/rclcpp/src/rclcpp/time_source.cpp @@ -14,6 +14,7 @@ #include #include +#include #include #include @@ -54,9 +55,7 @@ class ClocksState final ros_time_active_ = true; // Update all attached clocks to zero or last recorded time - for (auto it = associated_clocks_.begin(); it != associated_clocks_.end(); ++it) { - set_clock(last_time_msg_, true, *it); - } + set_all_clocks(last_time_msg_, true); } // An internal method to use in the clock callback that iterates and disables all clocks @@ -71,11 +70,8 @@ class ClocksState final 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); - } + auto msg = std::make_shared(); + set_all_clocks(msg, false); } // Check if ROS time is active @@ -95,7 +91,7 @@ class ClocksState final } } std::lock_guard guard(clock_list_lock_); - associated_clocks_.push_back(clock); + 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); } @@ -104,10 +100,8 @@ class ClocksState final void detachClock(rclcpp::Clock::SharedPtr 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 { + auto removed = associated_clocks_.erase(clock); + if (removed == 0) { RCLCPP_ERROR(logger_, "failed to remove clock"); } } @@ -184,8 +178,8 @@ class ClocksState final // 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_; + // 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. @@ -242,6 +236,7 @@ class TimeSource::NodeState final 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; @@ -286,17 +281,14 @@ class TimeSource::NodeState final parameter_subscription_ = rclcpp::AsyncParametersClient::on_parameter_event( node_topics_, [this](std::shared_ptr event) { - if (node_base_ != nullptr) { - this->on_parameter_event(event); - } - // Do nothing if node_base_ is nullptr because it means the TimeSource is now - // without an attached node + this->on_parameter_event(event); }); } // Detach the attached node void detachNode() { + std::lock_guard guard(node_base_lock_); // 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(); @@ -333,6 +325,7 @@ class TimeSource::NodeState final 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}; @@ -470,6 +463,14 @@ class TimeSource::NodeState final // 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; diff --git a/rclcpp/src/rclcpp/timer.cpp b/rclcpp/src/rclcpp/timer.cpp index 59ddd5e8dd..0dceb6b8d7 100644 --- a/rclcpp/src/rclcpp/timer.cpp +++ b/rclcpp/src/rclcpp/timer.cpp @@ -32,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) { @@ -64,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"); } 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/waitable.cpp b/rclcpp/src/rclcpp/waitable.cpp index 6c1284cb22..43b562481c 100644 --- a/rclcpp/src/rclcpp/waitable.cpp +++ b/rclcpp/src/rclcpp/waitable.cpp @@ -86,3 +86,82 @@ Waitable::clear_on_ready_callback() "Custom waitables should override clear_on_ready_callback if they " "want to use it and make sure to call it on the waitable destructor."); } + +void +Waitable::add_to_wait_set(rcl_wait_set_t * wait_set) +{ + this->add_to_wait_set(*wait_set); +} + +void +Waitable::add_to_wait_set(rcl_wait_set_t & wait_set) +{ +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif + this->add_to_wait_set(&wait_set); +// remove warning suppression +#if !defined(_WIN32) +# pragma GCC diagnostic pop +#else // !defined(_WIN32) +# pragma warning(pop) +#endif +} + +bool +Waitable::is_ready(rcl_wait_set_t * wait_set) +{ + const rcl_wait_set_t & const_wait_set_ref = *wait_set; + return this->is_ready(const_wait_set_ref); +} + +bool +Waitable::is_ready(const rcl_wait_set_t & wait_set) +{ +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif + // note this const cast is only required to support a deprecated function + return this->is_ready(&const_cast(wait_set)); +// remove warning suppression +#if !defined(_WIN32) +# pragma GCC diagnostic pop +#else // !defined(_WIN32) +# pragma warning(pop) +#endif +} + +void +Waitable::execute(std::shared_ptr & data) +{ + const std::shared_ptr & const_data = data; + this->execute(const_data); +} + +void +Waitable::execute(const std::shared_ptr & data) +{ +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif + // note this const cast is only required to support a deprecated function + this->execute(const_cast &>(data)); +// remove warning suppression +#if !defined(_WIN32) +# pragma GCC diagnostic pop +#else // !defined(_WIN32) +# pragma warning(pop) +#endif +} 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_executor.cpp b/rclcpp/test/benchmark/benchmark_executor.cpp index 652007b589..65bb3a1007 100644 --- a/rclcpp/test/benchmark/benchmark_executor.cpp +++ b/rclcpp/test/benchmark/benchmark_executor.cpp @@ -362,42 +362,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); - - entities_collector_->init(&wait_set, memory_strategy); - RCPPUTILS_SCOPE_EXIT(entities_collector_->fini()); - - reset_heap_counters(); - - for (auto _ : st) { - (void)_; - std::shared_ptr data = entities_collector_->take_data(); - entities_collector_->execute(data); - } -} 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 dcb1d36d8e..c2e6b2bfe4 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -6,6 +6,7 @@ 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 @@ -34,71 +35,60 @@ 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) 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_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_generic_client test_generic_client.cpp) +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_client_common test_client_common.cpp) +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}) - ament_target_dependencies(test_create_subscription - "test_msgs" - ) + target_link_libraries(test_create_subscription ${PROJECT_NAME} ${test_msgs_TARGETS}) endif() function(test_add_callback_groups_to_executor_for_rmw_implementation) set(rmw_implementation_env_var RMW_IMPLEMENTATION=${rmw_implementation}) @@ -107,32 +97,17 @@ function(test_add_callback_groups_to_executor_for_rmw_implementation) TIMEOUT 120 ) if(TARGET test_add_callback_groups_to_executor${target_suffix}) - target_link_libraries(test_add_callback_groups_to_executor${target_suffix} ${PROJECT_NAME}) - ament_target_dependencies(test_add_callback_groups_to_executor${target_suffix} - "test_msgs" - ) + target_link_libraries(test_add_callback_groups_to_executor${target_suffix} ${PROJECT_NAME} ${test_msgs_TARGETS}) endif() endfunction() call_for_each_rmw_implementation(test_add_callback_groups_to_executor_for_rmw_implementation) ament_add_gtest(test_expand_topic_or_service_name test_expand_topic_or_service_name.cpp) 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 @@ -142,72 +117,33 @@ 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) +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) 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 @@ -218,7 +154,7 @@ endif() ament_add_gtest(test_node_interfaces__node_base node_interfaces/test_node_base.cpp) 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) @@ -229,43 +165,42 @@ ament_add_gtest(test_node_interfaces__node_graph node_interfaces/test_node_graph.cpp TIMEOUT 120) 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} mimick) + 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) 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) 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) 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) 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) 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) @@ -296,82 +231,43 @@ 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) 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) 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_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) 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}") @@ -401,32 +297,22 @@ ament_add_gtest(test_subscription_publisher_with_same_type_adapter test_subscrip APPEND_LIBRARY_DIRS "${append_library_dirs}" ) if(TARGET test_subscription_publisher_with_same_type_adapter) - ament_target_dependencies(test_subscription_publisher_with_same_type_adapter - "statistics_msgs" - ) target_link_libraries(test_subscription_publisher_with_same_type_adapter ${PROJECT_NAME} - ${cpp_typesupport_target}) + ${cpp_typesupport_target} + ${statistics_msgs_TARGETS} + ) endif() ament_add_gtest(test_publisher_subscription_count_api test_publisher_subscription_count_api.cpp) if(TARGET test_publisher_subscription_count_api) - 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) @@ -435,12 +321,7 @@ function(test_generic_pubsub_for_rmw_implementation) 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" - ) + target_link_libraries(test_generic_pubsub${target_suffix} ${PROJECT_NAME} rcl::rcl ${test_msgs_TARGETS}) endif() endfunction() call_for_each_rmw_implementation(test_generic_pubsub_for_rmw_implementation) @@ -451,153 +332,79 @@ function(test_qos_event_for_rmw_implementation) ENV ${rmw_implementation_env_var} ) if(TARGET test_qos_event${target_suffix}) - target_link_libraries(test_qos_event${target_suffix} ${PROJECT_NAME} mimick) - ament_target_dependencies(test_qos_event${target_suffix} - "rmw" - "rosidl_typesupport_cpp" - "test_msgs" - ) + target_link_libraries(test_qos_event${target_suffix} ${PROJECT_NAME} mimick rcutils::rcutils rmw::rmw ${test_msgs_TARGETS}) endif() endfunction() call_for_each_rmw_implementation(test_qos_event_for_rmw_implementation) 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) 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) -if(TARGET test_service) - ament_target_dependencies(test_service_introspection - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - "test_msgs" - ) - target_link_libraries(test_service_introspection ${PROJECT_NAME} 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) 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}) @@ -605,134 +412,108 @@ 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}") 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) - ament_target_dependencies(test_timers_manager - "rcl") - target_link_libraries(test_timers_manager ${PROJECT_NAME} mimick) + 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}") 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) - ament_target_dependencies(test_logger_service - "rcl_interfaces") - target_link_libraries(test_logger_service ${PROJECT_NAME}) + 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_executors executors/test_executors.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}" TIMEOUT 180) 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) + target_link_libraries(test_executors_timer_cancel_behavior ${PROJECT_NAME} ${rosgraph_msgs_TARGETS}) +endif() + +ament_add_gtest( + test_executors_intraprocess + executors/test_executors_intraprocess.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}" + TIMEOUT 180) +if(TARGET test_executors) + target_link_libraries(test_executors_intraprocess ${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}") 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 - 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) -endif() - ament_add_gtest(test_entities_collector executors/test_entities_collector.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}" TIMEOUT 120) if(TARGET test_entities_collector) - ament_target_dependencies(test_entities_collector - "rcl" - "test_msgs") - target_link_libraries(test_entities_collector ${PROJECT_NAME} mimick) + 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) if(TARGET test_executor_notify_waitable) - ament_target_dependencies(test_executor_notify_waitable - "rcl" - "test_msgs") - target_link_libraries(test_executor_notify_waitable ${PROJECT_NAME} mimick) + 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 5) if(TARGET test_events_executor) - ament_target_dependencies(test_events_executor - "test_msgs") - target_link_libraries(test_events_executor ${PROJECT_NAME}) + 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) - ament_target_dependencies(test_events_queue - "test_msgs") target_link_libraries(test_events_queue ${PROJECT_NAME}) endif() @@ -745,76 +526,60 @@ 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") target_link_libraries(test_subscription_topic_statistics ${PROJECT_NAME} - ${cpp_typesupport_target}) + 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) 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_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) - ament_target_dependencies(test_rosout_subscription "rcl") - target_link_libraries(test_rosout_subscription ${PROJECT_NAME}) + 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) if(TARGET test_executor) - ament_target_dependencies(test_executor "rcl") target_link_libraries(test_executor ${PROJECT_NAME} mimick) endif() @@ -831,12 +596,7 @@ function(test_subscription_content_filter_for_rmw_implementation) TIMEOUT 120 ) if(TARGET test_subscription_content_filter${target_suffix}) - target_link_libraries(test_subscription_content_filter${target_suffix} ${PROJECT_NAME} mimick) - ament_target_dependencies(test_subscription_content_filter${target_suffix} - "rcpputils" - "rosidl_typesupport_cpp" - "test_msgs" - ) + target_link_libraries(test_subscription_content_filter${target_suffix} ${PROJECT_NAME} mimick ${test_msgs_TARGETS}) endif() endfunction() call_for_each_rmw_implementation(test_subscription_content_filter_for_rmw_implementation) diff --git a/rclcpp/test/rclcpp/executors/executor_types.hpp b/rclcpp/test/rclcpp/executors/executor_types.hpp new file mode 100644 index 0000000000..0218a9b547 --- /dev/null +++ b/rclcpp/test/rclcpp/executors/executor_types.hpp @@ -0,0 +1,70 @@ +// 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" + +using ExecutorTypes = + ::testing::Types< + rclcpp::executors::SingleThreadedExecutor, + rclcpp::executors::MultiThreadedExecutor, + rclcpp::executors::StaticSingleThreadedExecutor, + rclcpp::experimental::executors::EventsExecutor>; + +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"; + } + + 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_events_executor.cpp b/rclcpp/test/rclcpp/executors/test_events_executor.cpp index fbe83fcddc..13092b7067 100644 --- a/rclcpp/test/rclcpp/executors/test_events_executor.cpp +++ b/rclcpp/test/rclcpp/executors/test_events_executor.cpp @@ -43,11 +43,6 @@ class TestEventsExecutor : public ::testing::Test TEST_F(TestEventsExecutor, run_pub_sub) { - // rmw_connextdds doesn't support events-executor - if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { - GTEST_SKIP(); - } - auto node = std::make_shared("node"); bool msg_received = false; @@ -65,7 +60,7 @@ TEST_F(TestEventsExecutor, run_pub_sub) executor.add_node(node); bool spin_exited = false; - std::thread spinner([&spin_exited, &executor, this]() { + std::thread spinner([&spin_exited, &executor]() { executor.spin(); spin_exited = true; }); @@ -80,8 +75,6 @@ TEST_F(TestEventsExecutor, run_pub_sub) !spin_exited && (std::chrono::high_resolution_clock::now() - start < 1s)) { - auto time = std::chrono::high_resolution_clock::now() - start; - auto time_msec = std::chrono::duration_cast(time); std::this_thread::sleep_for(25ms); } @@ -95,11 +88,6 @@ TEST_F(TestEventsExecutor, run_pub_sub) TEST_F(TestEventsExecutor, run_clients_servers) { - // rmw_connextdds doesn't support events-executor - if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { - GTEST_SKIP(); - } - auto node = std::make_shared("node"); bool request_received = false; @@ -119,7 +107,7 @@ TEST_F(TestEventsExecutor, run_clients_servers) executor.add_node(node); bool spin_exited = false; - std::thread spinner([&spin_exited, &executor, this]() { + std::thread spinner([&spin_exited, &executor]() { executor.spin(); spin_exited = true; }); @@ -153,11 +141,6 @@ TEST_F(TestEventsExecutor, run_clients_servers) TEST_F(TestEventsExecutor, spin_once_max_duration_timeout) { - // rmw_connextdds doesn't support events-executor - if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { - GTEST_SKIP(); - } - auto node = std::make_shared("node"); EventsExecutor executor; @@ -190,11 +173,6 @@ TEST_F(TestEventsExecutor, spin_once_max_duration_timeout) TEST_F(TestEventsExecutor, spin_once_max_duration_timer) { - // rmw_connextdds doesn't support events-executor - if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { - GTEST_SKIP(); - } - auto node = std::make_shared("node"); EventsExecutor executor; @@ -226,11 +204,6 @@ TEST_F(TestEventsExecutor, spin_once_max_duration_timer) TEST_F(TestEventsExecutor, spin_some_max_duration) { - // rmw_connextdds doesn't support events-executor - if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { - GTEST_SKIP(); - } - { auto node = std::make_shared("node"); @@ -277,11 +250,6 @@ TEST_F(TestEventsExecutor, spin_some_max_duration) TEST_F(TestEventsExecutor, spin_some_zero_duration) { - // rmw_connextdds doesn't support events-executor - if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { - GTEST_SKIP(); - } - auto node = std::make_shared("node"); size_t t_runs = 0; @@ -303,11 +271,6 @@ TEST_F(TestEventsExecutor, spin_some_zero_duration) TEST_F(TestEventsExecutor, spin_all_max_duration) { - // rmw_connextdds doesn't support events-executor - if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { - GTEST_SKIP(); - } - { auto node = std::make_shared("node"); @@ -358,11 +321,6 @@ TEST_F(TestEventsExecutor, spin_all_max_duration) TEST_F(TestEventsExecutor, cancel_while_timers_running) { - // rmw_connextdds doesn't support events-executor - if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { - GTEST_SKIP(); - } - auto node = std::make_shared("node"); EventsExecutor executor; @@ -388,7 +346,7 @@ TEST_F(TestEventsExecutor, cancel_while_timers_running) }); - std::thread spinner([&executor, this]() {executor.spin();}); + std::thread spinner([&executor]() {executor.spin();}); std::this_thread::sleep_for(10ms); // Call cancel while t1 callback is still being executed @@ -402,11 +360,6 @@ TEST_F(TestEventsExecutor, cancel_while_timers_running) TEST_F(TestEventsExecutor, cancel_while_timers_waiting) { - // rmw_connextdds doesn't support events-executor - if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { - GTEST_SKIP(); - } - auto node = std::make_shared("node"); size_t t1_runs = 0; @@ -420,7 +373,7 @@ TEST_F(TestEventsExecutor, cancel_while_timers_waiting) executor.add_node(node); auto start = std::chrono::steady_clock::now(); - std::thread spinner([&executor, this]() {executor.spin();}); + std::thread spinner([&executor]() {executor.spin();}); std::this_thread::sleep_for(10ms); executor.cancel(); @@ -435,11 +388,6 @@ TEST_F(TestEventsExecutor, destroy_entities) // This test fails on Windows! We skip it for now GTEST_SKIP(); - // rmw_connextdds doesn't support events-executor - if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { - 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)); @@ -447,7 +395,7 @@ TEST_F(TestEventsExecutor, destroy_entities) 2ms, [&]() {publisher->publish(std::make_unique());}); EventsExecutor executor_pub; executor_pub.add_node(node_pub); - std::thread spinner([&executor_pub, this]() {executor_pub.spin();}); + 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"); @@ -485,11 +433,6 @@ std::string * g_sub_log_msg; std::promise * g_log_msgs_promise; TEST_F(TestEventsExecutor, test_default_incompatible_qos_callbacks) { - // rmw_connextdds doesn't support events-executor - if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { - GTEST_SKIP(); - } - auto node = std::make_shared("node"); rcutils_logging_output_handler_t original_output_handler = rcutils_logging_get_output_handler(); diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index 38b6ddf870..3434f473c6 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -15,17 +15,19 @@ /** * 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" @@ -34,27 +36,22 @@ #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 "./executor_types.hpp" + using namespace std::chrono_literals; 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(); @@ -75,6 +72,8 @@ class TestExecutors : public ::testing::Test publisher.reset(); subscription.reset(); node.reset(); + + rclcpp::shutdown(); } rclcpp::Node::SharedPtr node; @@ -83,70 +82,17 @@ 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, - rclcpp::experimental::executors::EventsExecutor>; - -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"; - } - - if (std::is_same()) { - return "EventsExecutor"; - } - - 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, - rclcpp::experimental::executors::EventsExecutor>; TYPED_TEST_SUITE(TestExecutorsStable, StandardExecutors, ExecutorTypeNames); // Make sure that executors detach from nodes when destructing TYPED_TEST(TestExecutors, detachOnDestruction) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } - { ExecutorType executor; executor.add_node(this->node); @@ -158,19 +104,8 @@ 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; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } - ExecutorType executor; { @@ -191,14 +126,6 @@ TYPED_TEST(TestExecutorsStable, addTemporaryNode) TYPED_TEST(TestExecutors, emptyExecutor) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } - ExecutorType executor; std::thread spinner([&]() {EXPECT_NO_THROW(executor.spin());}); std::this_thread::sleep_for(50ms); @@ -210,14 +137,6 @@ TYPED_TEST(TestExecutors, emptyExecutor) TYPED_TEST(TestExecutors, addNodeTwoExecutors) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } - ExecutorType executor1; ExecutorType executor2; EXPECT_NO_THROW(executor1.add_node(this->node)); @@ -229,14 +148,6 @@ TYPED_TEST(TestExecutors, addNodeTwoExecutors) TYPED_TEST(TestExecutors, spinWithTimer) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } - ExecutorType executor; bool timer_completed = false; @@ -260,25 +171,20 @@ TYPED_TEST(TestExecutors, spinWithTimer) TYPED_TEST(TestExecutors, spinWhileAlreadySpinning) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } - 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); } @@ -295,14 +201,6 @@ TYPED_TEST(TestExecutors, spinWhileAlreadySpinning) TYPED_TEST(TestExecutors, testSpinUntilFutureComplete) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } - ExecutorType executor; executor.add_node(this->node); @@ -326,14 +224,6 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureComplete) TYPED_TEST(TestExecutors, testSpinUntilSharedFutureComplete) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } - ExecutorType executor; executor.add_node(this->node); @@ -358,14 +248,6 @@ TYPED_TEST(TestExecutors, testSpinUntilSharedFutureComplete) TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteNoTimeout) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } - ExecutorType executor; executor.add_node(this->node); @@ -413,14 +295,6 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteNoTimeout) TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteWithTimeout) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } - ExecutorType executor; executor.add_node(this->node); @@ -463,21 +337,33 @@ class TestWaitable : public rclcpp::Waitable TestWaitable() = default; void - add_to_wait_set(rcl_wait_set_t * wait_set) override + add_to_wait_set(rcl_wait_set_t & wait_set) override { - rclcpp::detail::add_guard_condition_to_rcl_wait_set(*wait_set, gc_); + 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 trigger() { + trigger_count_++; gc_.trigger(); } bool - is_ready(rcl_wait_set_t * wait_set) override + is_ready(const rcl_wait_set_t & wait_set) override { - (void)wait_set; - return true; + 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 @@ -494,11 +380,24 @@ class TestWaitable : public rclcpp::Waitable } void - execute(std::shared_ptr & data) override + execute(const std::shared_ptr &) override { - (void) data; + trigger_count_--; count_++; - std::this_thread::sleep_for(3ms); + 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 + set_on_execute_callback(std::function on_execute_callback) + { + on_execute_callback_ = on_execute_callback; } void @@ -520,27 +419,21 @@ class TestWaitable : public rclcpp::Waitable get_number_of_ready_guard_conditions() override {return 1;} size_t - get_count() + get_count() const { return count_; } private: - size_t count_ = 0; + std::atomic trigger_count_ = 0; + std::atomic count_ = 0; rclcpp::GuardCondition gc_; + std::function on_execute_callback_ = nullptr; }; TYPED_TEST(TestExecutors, spinAll) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } - ExecutorType executor; auto waitable_interfaces = this->node->get_node_waitables_interface(); auto my_waitable = std::make_shared(); @@ -580,67 +473,188 @@ 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; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } - ExecutorType executor; + // Use an isolated callback group to avoid interference from any housekeeping + // items that may be in the default callback group of the node. + constexpr bool automatically_add_to_executor_with_node = false; + auto isolated_callback_group = this->node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + automatically_add_to_executor_with_node); + + // Check that spin_some() returns quickly when there is no work to be done. + // This can be a false positive if there is somehow some work for the executor + // to do that has not been considered, but the isolated callback group should + // avoid that. + { + ExecutorType executor; + executor.add_callback_group(isolated_callback_group, this->node->get_node_base_interface()); + + auto start = std::chrono::steady_clock::now(); + // spin_some with some non-trival "max_duration" and check that it does not + // take anywhere near that long to execute. + constexpr auto max_duration = 10s; + executor.spin_some(max_duration); + EXPECT_LT( + to_nanoseconds_helper(std::chrono::steady_clock::now() - start), + to_nanoseconds_helper(max_duration / 2)) + << "spin_some() took a long time to execute when it should have done " + << "nothing and should not have blocked either, but this could be a " + << "false negative if the computer is really slow"; + } + + // Check that having one thing ready gets executed by spin_some(). auto waitable_interfaces = this->node->get_node_waitables_interface(); - auto my_waitable = std::make_shared(); - waitable_interfaces->add_waitable(my_waitable, nullptr); - executor.add_node(this->node); + auto my_waitable1 = std::make_shared(); + waitable_interfaces->add_waitable(my_waitable1, isolated_callback_group); + { + ExecutorType executor; + executor.add_callback_group(isolated_callback_group, this->node->get_node_base_interface()); - // 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; - }); + my_waitable1->trigger(); - // 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)) - { - my_waitable->trigger(); - this->publisher->publish(test_msgs::msg::Empty()); - std::this_thread::sleep_for(1ms); + // 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"; } - // The count of "execute" depends on whether the executor starts spinning before (1) or after (0) - // the first iteration of the while loop - EXPECT_LE(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(); - spinner.join(); + // 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"; + } } -// Check spin_node_until_future_complete with node base pointer -TYPED_TEST(TestExecutors, testSpinNodeUntilFutureCompleteNodeBasePtr) +// The purpose of this test is to check that the ExecutorT.spin_some() method: +// - does not continue executing after max_duration has elapsed +TYPED_TEST(TestExecutors, spin_some_max_duration) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor + + // 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 if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) + std::is_same()) { GTEST_SKIP(); } + // Use an isolated callback group to avoid interference from any housekeeping + // items that may be in the default callback group of the node. + constexpr bool automatically_add_to_executor_with_node = false; + auto isolated_callback_group = this->node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + automatically_add_to_executor_with_node); + + // Set up a situation with two waitables that take time to execute, such that + // the time it takes to execute two waitables far exceeds the max_duration + // given to spin_some(), which should result in spin_some() starting to + // execute one of them, have the max duration elapse, finish executing one + // of them, then returning before starting on the second. + constexpr auto max_duration = 100ms; // relatively short because we expect to exceed it + constexpr auto waitable_callback_duration = max_duration * 2; + auto long_running_callback = [&waitable_callback_duration]() { + std::this_thread::sleep_for(waitable_callback_duration); + }; + + auto waitable_interfaces = this->node->get_node_waitables_interface(); + + auto my_waitable1 = std::make_shared(); + my_waitable1->set_on_execute_callback(long_running_callback); + waitable_interfaces->add_waitable(my_waitable1, isolated_callback_group); + + auto my_waitable2 = std::make_shared(); + my_waitable2->set_on_execute_callback(long_running_callback); + waitable_interfaces->add_waitable(my_waitable2, isolated_callback_group); + + my_waitable1->trigger(); + my_waitable2->trigger(); + + ExecutorType executor; + executor.add_callback_group(isolated_callback_group, this->node->get_node_base_interface()); + + auto start = std::chrono::steady_clock::now(); + // spin_some and check that it does not take longer than two of waitable_callback_duration, + // nor significantly less than a single waitable_callback_duration. + executor.spin_some(max_duration); + auto spin_some_run_time = std::chrono::steady_clock::now() - start; + EXPECT_GT( + to_nanoseconds_helper(spin_some_run_time), + to_nanoseconds_helper(waitable_callback_duration / 2)) + << "spin_some() took less than half the expected time to execute a single " + << "waitable, which implies it did not actually execute one when it was " + << "expected to"; + EXPECT_LT( + to_nanoseconds_helper(spin_some_run_time), + to_nanoseconds_helper(waitable_callback_duration * 2)) + << "spin_some() took longer than expected to execute by a significant margin, but " + << "this could be a false positive on a very slow computer"; + + // check that exactly one of the waitables were executed (do not depend on a specific order) + size_t number_of_waitables_executed = my_waitable1->get_count() + my_waitable2->get_count(); + EXPECT_EQ(number_of_waitables_executed, 1u) + << "expected exactly one of the two waitables to be executed, but " + << "my_waitable1->get_count(): " << my_waitable1->get_count() << " and " + << "my_waitable2->get_count(): " << my_waitable2->get_count(); +} + +// Check spin_node_until_future_complete with node base pointer +TYPED_TEST(TestExecutors, testSpinNodeUntilFutureCompleteNodeBasePtr) +{ + using ExecutorType = TypeParam; ExecutorType executor; std::promise promise; @@ -657,14 +671,6 @@ TYPED_TEST(TestExecutors, testSpinNodeUntilFutureCompleteNodeBasePtr) TYPED_TEST(TestExecutors, testSpinNodeUntilFutureCompleteNodePtr) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } - ExecutorType executor; std::promise promise; @@ -681,14 +687,6 @@ TYPED_TEST(TestExecutors, testSpinNodeUntilFutureCompleteNodePtr) TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteInterrupted) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } - ExecutorType executor; executor.add_node(this->node); @@ -729,6 +727,78 @@ 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 `notify_waitable_event_pushed_` 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; + // rmw_connextdds doesn't support events-executor + if ( + std::is_same() && + std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) + { + GTEST_SKIP(); + } + + // 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 + auto executor = std::make_shared(); + // 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 spin_until_future_complete with node base pointer (instantiates its own executor) TEST(TestExecutors, testSpinUntilFutureCompleteNodeBasePtr) { @@ -769,106 +839,3 @@ TEST(TestExecutors, testSpinUntilFutureCompleteNodePtr) rclcpp::shutdown(); } - -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 = 0; - - 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(1); - }; - - 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_int 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. - const size_t kNumMessages = 100; - - using ExecutorType = TypeParam; - ExecutorType executor; - executor.add_node(this->node); - - EXPECT_EQ(0, 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(0); - - 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, &kNumMessages]() { - 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_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..7fd1676c5d --- /dev/null +++ b/rclcpp/test/rclcpp/executors/test_executors_timer_cancel_behavior.cpp @@ -0,0 +1,430 @@ +// 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() {return cnt1_;} + int GetTimer2Cnt() {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() + { + cnt1_++; + RCLCPP_DEBUG(this->get_logger(), "Timer 1! (%d)", cnt1_); + } + + void Timer2Callback() + { + cnt2_++; + RCLCPP_DEBUG(this->get_logger(), "Timer 2! (%d)", cnt2_); + } + + 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(); + } + } + + 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()); + rostime_ += ros_update_duration_; + } + } + +private: + void RunTimer() + { + while (running_) { + PublishClock(); + std::this_thread::sleep_for(realtime_clock_step_.to_chrono()); + } + } + + void PublishClock() + { + const std::lock_guard lock(mutex_); + auto message = rosgraph_msgs::msg::Clock(); + 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(); + }); + } + + 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; +}; + +using MainExecutorTypes = + ::testing::Types< + rclcpp::executors::SingleThreadedExecutor, + rclcpp::executors::MultiThreadedExecutor, + rclcpp::executors::StaticSingleThreadedExecutor>; + +// 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_static_executor_entities_collector.cpp b/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp index 1ea91029f4..1c8c5b3abe 100644 --- a/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp +++ b/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp @@ -230,9 +230,9 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_out_of_scope) { class TestWaitable : public rclcpp::Waitable { public: - void add_to_wait_set(rcl_wait_set_t *) override {} + 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 @@ -240,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) { @@ -513,11 +510,6 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_to_wait_set_nullptr) { entities_collector_->init(&wait_set, memory_strategy); RCPPUTILS_SCOPE_EXIT(entities_collector_->fini()); - EXPECT_THROW( - entities_collector_->add_to_wait_set(nullptr), - std::invalid_argument); - rcl_reset_error(); - EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface())); } diff --git a/rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp b/rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp index 5ca6c1c25a..1a0f3f88c4 100644 --- a/rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp +++ b/rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp @@ -56,7 +56,7 @@ 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 trigger guard condition on callback group add: error not set")); } } @@ -69,7 +69,7 @@ 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 trigger guard condition on node add: error not set")); } } @@ -86,7 +86,8 @@ 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 trigger guard condition on callback group remove: error not set")); } } @@ -99,7 +100,7 @@ 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.")); } } @@ -114,7 +115,7 @@ 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 trigger guard condition on node remove: error not set")); } } diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp index 70ccb5622d..3882d2b71c 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp @@ -129,6 +129,9 @@ TEST_F(TestNodeGraph, construct_from_node) 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 @@ -534,6 +537,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()); diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp index 58bf010767..d262c67a9a 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp @@ -77,9 +77,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); @@ -95,15 +95,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), @@ -116,6 +116,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) diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp index ea55a1aac2..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) @@ -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) diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp index d038a4b44d..dc92dee610 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp @@ -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) 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 a1f35c0cdc..aa34a71af4 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp @@ -28,8 +28,8 @@ class TestWaitable : public rclcpp::Waitable { public: - void add_to_wait_set(rcl_wait_set_t *) override {} - 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 @@ -37,10 +37,7 @@ class TestWaitable : public rclcpp::Waitable return nullptr; } - void execute(std::shared_ptr & data) override - { - (void) data; - } + void execute(const std::shared_ptr &) override {} }; 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)); diff --git a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp index 2adf907bc5..d25b3490c2 100644 --- a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp +++ b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp @@ -39,14 +39,14 @@ static bool test_waitable_result = false; class TestWaitable : public rclcpp::Waitable { public: - void add_to_wait_set(rcl_wait_set_t *) override + void add_to_wait_set(rcl_wait_set_t &) override { if (!test_waitable_result) { throw std::runtime_error("TestWaitable add_to_wait_set failed"); } } - bool is_ready(rcl_wait_set_t *) override + bool is_ready(const rcl_wait_set_t &) override { return test_waitable_result; } @@ -57,10 +57,7 @@ class TestWaitable : public rclcpp::Waitable return nullptr; } - void execute(std::shared_ptr & data) override - { - (void) data; - } + void execute(const std::shared_ptr &) override {} }; static bool test_waitable_result2 = false; @@ -82,12 +79,12 @@ class TestWaitable2 : public rclcpp::Waitable EXPECT_EQ(rcl_event_fini(&pub_event_), RCL_RET_OK); } - void add_to_wait_set(rcl_wait_set_t * wait_set) override + void add_to_wait_set(rcl_wait_set_t & wait_set) override { - EXPECT_EQ(rcl_wait_set_add_event(wait_set, &pub_event_, &wait_set_event_index_), RCL_RET_OK); + EXPECT_EQ(rcl_wait_set_add_event(&wait_set, &pub_event_, &wait_set_event_index_), RCL_RET_OK); } - bool is_ready(rcl_wait_set_t *) override + bool is_ready(const rcl_wait_set_t &) override { return test_waitable_result2; } @@ -98,7 +95,7 @@ class TestWaitable2 : public rclcpp::Waitable return nullptr; } - void execute(std::shared_ptr & data) override + void execute(const std::shared_ptr & data) override { (void) data; } @@ -200,9 +197,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 = @@ -949,9 +946,9 @@ 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), rclcpp::ServicesQoS(), callback_group); diff --git a/rclcpp/test/rclcpp/strategies/test_message_pool_memory_strategy.cpp b/rclcpp/test/rclcpp/strategies/test_message_pool_memory_strategy.cpp index b68bc96ab6..69198adf59 100644 --- a/rclcpp/test/rclcpp/strategies/test_message_pool_memory_strategy.cpp +++ b/rclcpp/test/rclcpp/strategies/test_message_pool_memory_strategy.cpp @@ -56,18 +56,28 @@ TEST_F(TestMessagePoolMemoryStrategy, borrow_too_many) { // Size is 1, borrowing second time should fail RCLCPP_EXPECT_THROW_EQ( message_memory_strategy_->borrow_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 49131638db..fefec6c8fa 100644 --- a/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp +++ b/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp @@ -99,13 +99,6 @@ TYPED_TEST_SUITE(TestAddCallbackGroupsToExecutorStable, StandardExecutors, Execu TYPED_TEST(TestAddCallbackGroupsToExecutor, add_callback_groups) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } auto node = std::make_shared("my_node", "/ns"); auto timer_callback = []() {}; @@ -155,13 +148,6 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, add_callback_groups) TYPED_TEST(TestAddCallbackGroupsToExecutor, remove_callback_groups) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } auto node = std::make_shared("my_node", "/ns"); auto timer_callback = []() {}; @@ -193,13 +179,6 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, remove_callback_groups) TYPED_TEST(TestAddCallbackGroupsToExecutor, add_duplicate_callback_groups) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } ExecutorType executor; auto node = std::make_shared("my_node", "/ns"); @@ -220,30 +199,32 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, add_duplicate_callback_groups) TYPED_TEST(TestAddCallbackGroupsToExecutor, add_callback_groups_after_add_node_to_executor) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } + + 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++; + printf("in timer_callback(%zu)\n", cur_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 = []() {}; @@ -255,6 +236,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); } /* @@ -263,13 +245,6 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, add_callback_groups_after_add_node_t TYPED_TEST(TestAddCallbackGroupsToExecutor, add_unallowable_callback_groups) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } ExecutorType executor; auto node = std::make_shared("my_node", "/ns"); @@ -307,13 +282,6 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, add_unallowable_callback_groups) TYPED_TEST(TestAddCallbackGroupsToExecutor, one_node_many_callback_groups_many_executors) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } ExecutorType timer_executor; ExecutorType sub_executor; @@ -355,13 +323,6 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, one_node_many_callback_groups_many_e TYPED_TEST(TestAddCallbackGroupsToExecutorStable, subscriber_triggered_to_receive_message) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } auto node = std::make_shared("my_node", "/ns"); @@ -428,13 +389,6 @@ TYPED_TEST(TestAddCallbackGroupsToExecutorStable, subscriber_triggered_to_receiv TYPED_TEST(TestAddCallbackGroupsToExecutorStable, callback_group_create_after_spin) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } auto node = std::make_shared("my_node", "/ns"); @@ -481,13 +435,6 @@ TYPED_TEST(TestAddCallbackGroupsToExecutorStable, callback_group_create_after_sp TYPED_TEST(TestAddCallbackGroupsToExecutor, remove_callback_group) { using ExecutorType = TypeParam; - // rmw_connextdds doesn't support events-executor - if ( - std::is_same() && - std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) - { - GTEST_SKIP(); - } ExecutorType executor; auto node = std::make_shared("my_node", "/ns"); diff --git a/rclcpp/test/rclcpp/test_any_service_callback.cpp b/rclcpp/test/rclcpp/test_any_service_callback.cpp index 6a70b8b5da..b6e49097c3 100644 --- a/rclcpp/test/rclcpp/test_any_service_callback.cpp +++ b/rclcpp/test/rclcpp/test_any_service_callback.cpp @@ -51,8 +51,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 +67,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 +83,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 +97,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 45fe091f07..a98271e931 100644 --- a/rclcpp/test/rclcpp/test_any_subscription_callback.cpp +++ b/rclcpp/test/rclcpp/test_any_subscription_callback.cpp @@ -19,8 +19,6 @@ #include #include -// TODO(aprotyas): Figure out better way to suppress deprecation warnings. -#define RCLCPP_AVOID_DEPRECATIONS_FOR_UNIT_TESTS 1 #include "rclcpp/any_subscription_callback.hpp" #include "test_msgs/msg/empty.hpp" #include "test_msgs/msg/empty.h" diff --git a/rclcpp/test/rclcpp/test_client.cpp b/rclcpp/test/rclcpp/test_client.cpp index 9070e1caa9..5c7e5046ab 100644 --- a/rclcpp/test/rclcpp/test_client.cpp +++ b/rclcpp/test/rclcpp/test_client.cpp @@ -24,12 +24,9 @@ #include "rcl_interfaces/srv/list_parameters.hpp" #include "../mocking_utils/patch.hpp" -#include "../utils/rclcpp_gtest_macros.hpp" #include "test_msgs/srv/empty.hpp" -using namespace std::chrono_literals; - class TestClient : public ::testing::Test { protected: @@ -219,385 +216,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 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; - } - - 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 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)); -} - -TEST_F(TestClientWithServer, test_client_remove_pending_request) { - auto client = node->create_client("no_service_server_available_here"); - auto request = std::make_shared(); - auto future = client->async_send_request(request); - - EXPECT_TRUE(client->remove_pending_request(future)); -} - -TEST_F(TestClientWithServer, prune_requests_older_than_no_pruned) { - auto client = node->create_client(service_name); - auto request = std::make_shared(); - auto future = client->async_send_request(request); - auto time = std::chrono::system_clock::now() + 1s; - - EXPECT_EQ(1u, client->prune_requests_older_than(time)); -} - -TEST_F(TestClientWithServer, prune_requests_older_than_with_pruned) { - auto client = node->create_client(service_name); - auto request = std::make_shared(); - auto future = client->async_send_request(request); - auto time = std::chrono::system_clock::now() + 1s; - - 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]); -} - -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); - } -} - -/* - Testing on_new_response callbacks. - */ -TEST_F(TestClient, on_new_response_callback) { - auto client_node = std::make_shared("client_node", "ns"); - auto server_node = std::make_shared("server_node", "ns"); - - rclcpp::ServicesQoS client_qos; - client_qos.keep_last(3); - auto client = client_node->create_client("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); - - client->async_send_request(request); - auto start = std::chrono::steady_clock::now(); - while (server_requests_count == 0 && - (std::chrono::steady_clock::now() - start) < 10s) - { - rclcpp::spin_some(server_node); - } - - ASSERT_EQ(server_requests_count, 1u); - - 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;}; - client->set_on_new_response_callback(increase_c2_cb); - - client->async_send_request(request); - start = std::chrono::steady_clock::now(); - while (server_requests_count == 1 && - (std::chrono::steady_clock::now() - start) < 10s) - { - rclcpp::spin_some(server_node); - } - - ASSERT_EQ(server_requests_count, 2u); - - 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); - EXPECT_EQ(c2.load(), 1u); - - client->clear_on_new_response_callback(); - - client->async_send_request(request); - client->async_send_request(request); - client->async_send_request(request); - start = std::chrono::steady_clock::now(); - while (server_requests_count < 5 && - (std::chrono::steady_clock::now() - start) < 10s) - { - 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(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(client->set_on_new_response_callback(invalid_cb), std::invalid_argument); -} - -TEST_F(TestClient, rcl_client_request_publisher_get_actual_qos_error) { - auto mock = mocking_utils::patch_and_return( - "lib:rclcpp", rcl_client_request_publisher_get_actual_qos, nullptr); - auto client = node->create_client("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")); -} - -TEST_F(TestClient, rcl_client_response_subscription_get_actual_qos_error) { - auto mock = mocking_utils::patch_and_return( - "lib:rclcpp", rcl_client_response_subscription_get_actual_qos, nullptr); - auto client = node->create_client("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")); -} - -TEST_F(TestClient, client_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 client = - node->create_client("client", 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); -} - -TEST_F(TestClient, client_qos_depth) { - 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()); -} 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_context.cpp b/rclcpp/test/rclcpp/test_context.cpp index 9d736f9db2..c8779371fe 100644 --- a/rclcpp/test/rclcpp/test_context.cpp +++ b/rclcpp/test/rclcpp/test_context.cpp @@ -214,3 +214,25 @@ TEST(TestContext, check_on_shutdown_callback_order_after_del) { 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_timer.cpp b/rclcpp/test/rclcpp/test_create_timer.cpp index f253af4838..c4dc1f7e61 100644 --- a/rclcpp/test/rclcpp/test_create_timer.cpp +++ b/rclcpp/test/rclcpp/test_create_timer.cpp @@ -193,3 +193,29 @@ TEST(TestCreateTimer, timer_function_pointer) 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 d5908c7d4b..2347514d7a 100644 --- a/rclcpp/test/rclcpp/test_duration.cpp +++ b/rclcpp/test/rclcpp/test_duration.cpp @@ -79,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 bdbb0a1079..668ab96797 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) { @@ -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) { @@ -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) { @@ -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"); diff --git a/rclcpp/test/rclcpp/test_generic_client.cpp b/rclcpp/test/rclcpp/test_generic_client.cpp new file mode 100644 index 0000000000..be65ea1f53 --- /dev/null +++ b/rclcpp/test/rclcpp/test_generic_client.cpp @@ -0,0 +1,230 @@ +// 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/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" + +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"); + }, std::runtime_error); + } +} + +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); + } +} diff --git a/rclcpp/test/rclcpp/test_generic_pubsub.cpp b/rclcpp/test/rclcpp/test_generic_pubsub.cpp index f4cef0b757..79fbfcc33a 100644 --- a/rclcpp/test/rclcpp/test_generic_pubsub.cpp +++ b/rclcpp/test/rclcpp/test_generic_pubsub.cpp @@ -69,7 +69,7 @@ class RclcppGenericNodeFixture : public Test size_t counter = 0; auto subscription = node_->create_generic_subscription( topic_name, type, rclcpp::QoS(1), - [&counter, &messages, this](std::shared_ptr message) { + [&counter, &messages, this](const std::shared_ptr message) { T2 deserialized_message; rclcpp::Serialization serializer; serializer.deserialize_message(message.get(), &deserialized_message); @@ -236,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(); }; @@ -263,3 +263,49 @@ TEST_F(RclcppGenericNodeFixture, generic_publisher_uses_qos) // 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_guard_condition.cpp b/rclcpp/test/rclcpp/test_guard_condition.cpp index 1e72264869..ed54074a4e 100644 --- a/rclcpp/test/rclcpp/test_guard_condition.cpp +++ b/rclcpp/test/rclcpp/test_guard_condition.cpp @@ -66,16 +66,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. */ @@ -115,19 +105,11 @@ TEST_F(TestGuardCondition, add_to_wait_set) { "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)); + 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); - } - - { - auto mock = mocking_utils::patch_and_return( - "lib:rclcpp", rcl_wait_set_add_guard_condition, RCL_RET_ERROR); - - auto gd = std::make_shared(); - EXPECT_THROW(gd->add_to_wait_set(nullptr), rclcpp::exceptions::RCLError); + EXPECT_THROW(gc->add_to_wait_set(wait_set_2), std::runtime_error); } } } 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 45d916b004..046a5228da 100644 --- a/rclcpp/test/rclcpp/test_intra_process_manager.cpp +++ b/rclcpp/test/rclcpp/test_intra_process_manager.cpp @@ -31,6 +31,111 @@ // 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; + } + + // 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 @@ -80,6 +185,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 { @@ -117,6 +228,9 @@ class Publisher : public PublisherBase { 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 @@ -124,65 +238,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 @@ -221,6 +282,16 @@ class SubscriptionIntraProcessBase 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; + rclcpp::QoS qos_profile; std::string topic_name; }; @@ -275,11 +346,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; @@ -311,12 +388,14 @@ 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 @@ -348,28 +427,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; @@ -388,7 +475,7 @@ TEST(TestIntraProcessManager, add_pub_sub) { 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); @@ -404,7 +491,7 @@ TEST(TestIntraProcessManager, add_pub_sub) { auto s2 = std::make_shared(rclcpp::QoS(10).reliable()); - auto s2_id = ipm->add_subscription(s2); + auto s2_id = ipm->template add_subscription(s2); auto p3_id = ipm->add_publisher(p3); p1_subs = ipm->get_subscription_count(p1_id); @@ -424,14 +511,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; @@ -447,7 +534,7 @@ TEST(TestIntraProcessManager, single_subscription) { auto s1 = std::make_shared(); 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()); @@ -458,7 +545,7 @@ TEST(TestIntraProcessManager, single_subscription) { ipm->remove_subscription(s1_id); auto s2 = std::make_shared(); 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(); @@ -477,15 +564,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; @@ -501,11 +588,11 @@ TEST(TestIntraProcessManager, multiple_subscriptions_same_type) { auto s1 = std::make_shared(); 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(); 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()); @@ -521,11 +608,11 @@ TEST(TestIntraProcessManager, multiple_subscriptions_same_type) { auto s3 = std::make_shared(); 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(); 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()); @@ -540,11 +627,11 @@ TEST(TestIntraProcessManager, multiple_subscriptions_same_type) { auto s5 = std::make_shared(); 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(); 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()); @@ -560,12 +647,12 @@ TEST(TestIntraProcessManager, multiple_subscriptions_same_type) { auto s7 = std::make_shared(); 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(); 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(); @@ -578,20 +665,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; @@ -607,11 +694,11 @@ TEST(TestIntraProcessManager, multiple_subscriptions_different_type) { auto s1 = std::make_shared(); 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(); 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()); @@ -626,15 +713,15 @@ TEST(TestIntraProcessManager, multiple_subscriptions_different_type) { auto s3 = std::make_shared(); 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(); 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(); 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()); @@ -658,19 +745,19 @@ TEST(TestIntraProcessManager, multiple_subscriptions_different_type) { auto s6 = std::make_shared(); 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(); 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(); 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(); 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()); @@ -696,12 +783,12 @@ TEST(TestIntraProcessManager, multiple_subscriptions_different_type) { auto s10 = std::make_shared(); 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(); 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(); @@ -712,3 +799,170 @@ 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(rclcpp::QoS(history_depth).best_effort()); + + auto s1 = std::make_shared(rclcpp::QoS(history_depth).best_effort()); + auto s2 = std::make_shared(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(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(rclcpp::QoS(history_depth).transient_local()); + + auto s1 = + std::make_shared(rclcpp::QoS(history_depth).transient_local()); + auto s2 = + std::make_shared(rclcpp::QoS(history_depth).transient_local()); + auto s3 = + std::make_shared(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_memory_strategy.cpp b/rclcpp/test/rclcpp/test_memory_strategy.cpp index e96729a2a1..348b7f0143 100644 --- a/rclcpp/test/rclcpp/test_memory_strategy.cpp +++ b/rclcpp/test/rclcpp/test_memory_strategy.cpp @@ -35,10 +35,10 @@ typedef std::map take_data() override {return nullptr;} - void execute(std::shared_ptr & data) override {(void)data;} + void execute(const std::shared_ptr &) override {} }; class TestMemoryStrategy : public ::testing::Test @@ -143,9 +143,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_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( diff --git a/rclcpp/test/rclcpp/test_node.cpp b/rclcpp/test/rclcpp/test_node.cpp index 512c2c1384..ad73aadc2a 100644 --- a/rclcpp/test/rclcpp/test_node.cpp +++ b/rclcpp/test/rclcpp/test_node.cpp @@ -78,6 +78,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()); } { @@ -3310,6 +3311,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( diff --git a/rclcpp/test/rclcpp/test_parameter_client.cpp b/rclcpp/test/rclcpp/test_parameter_client.cpp index 2ce414d327..afc48a652b 100644 --- a/rclcpp/test/rclcpp/test_parameter_client.cpp +++ b/rclcpp/test/rclcpp/test_parameter_client.cpp @@ -59,6 +59,8 @@ class TestParameterClient : public ::testing::Test node_with_option.reset(); } + // "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; }; @@ -925,6 +927,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", @@ -944,12 +947,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", @@ -964,13 +968,14 @@ TEST_F(TestParameterClient, sync_parameter_load_parameters) { 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", @@ -990,7 +995,7 @@ TEST_F(TestParameterClient, async_parameter_load_parameters_complicated_regex) { 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(6)); + 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); @@ -1020,6 +1025,7 @@ TEST_F(TestParameterClient, async_parameter_load_no_valid_parameter) { 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", @@ -1068,7 +1074,7 @@ TEST_F(TestParameterClient, async_parameter_load_parameters_from_map) { 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(6)); + 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); diff --git a/rclcpp/test/rclcpp/test_publisher.cpp b/rclcpp/test/rclcpp/test_publisher.cpp index 20a46194fc..4284ea48bb 100644 --- a/rclcpp/test/rclcpp/test_publisher.cpp +++ b/rclcpp/test/rclcpp/test_publisher.cpp @@ -196,11 +196,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()), @@ -482,7 +478,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 @@ -530,7 +526,7 @@ TEST_F(TestPublisher, run_event_handlers) { for (const auto & key_event_pair : publisher->get_event_handlers()) { auto handler = key_event_pair.second; - std::shared_ptr data = handler->take_data(); + const std::shared_ptr data = handler->take_data(); handler->execute(data); } } @@ -629,6 +625,41 @@ TEST_P(TestPublisherWaitForAllAcked, check_wait_for_all_acked_with_QosPolicy) { 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, @@ -639,3 +670,96 @@ INSTANTIATE_TEST_SUITE_P( 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 c041d86aee..9367a89014 100644 --- a/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp +++ b/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. - #include +#include #include #include #include @@ -25,6 +25,7 @@ #include "rclcpp/loaned_message.hpp" #include "rclcpp/rclcpp.hpp" +#include "rclcpp/msg/large_message.hpp" #include "rclcpp/msg/string.hpp" @@ -105,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 /* @@ -152,33 +179,54 @@ TEST_F(TestPublisher, conversion_exception_is_passed_up) { } } +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( - TestPublisher, +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::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); - }; - 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; @@ -239,6 +287,14 @@ 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. */ @@ -307,3 +363,45 @@ 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(); + message_data->reserve(length); + std::fill(message_data->begin(), message_data->begin() + 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; + message_data.reserve(length); + std::fill(message_data.begin(), message_data.begin() + length, '#'); + pub->publish(message_data); +} diff --git a/rclcpp/test/rclcpp/test_qos_event.cpp b/rclcpp/test/rclcpp/test_qos_event.cpp index 6b522d7ea2..ddd6b8db1c 100644 --- a/rclcpp/test/rclcpp/test_qos_event.cpp +++ b/rclcpp/test/rclcpp/test_qos_event.cpp @@ -301,24 +301,19 @@ 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_NO_THROW(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) { - // rmw_connextdds doesn't support rmw_event_set_callback() interface - if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { - GTEST_SKIP(); - } - auto offered_deadline = rclcpp::Duration(std::chrono::milliseconds(1)); auto requested_deadline = rclcpp::Duration(std::chrono::milliseconds(2)); @@ -360,11 +355,6 @@ TEST_F(TestQosEvent, test_on_new_event_callback) TEST_F(TestQosEvent, test_invalid_on_new_event_callback) { - // rmw_connextdds doesn't support rmw_event_set_callback() interface - if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { - GTEST_SKIP(); - } - 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;}; @@ -439,11 +429,6 @@ TEST_F(TestQosEvent, test_invalid_on_new_event_callback) TEST_F(TestQosEvent, test_pub_matched_event_by_set_event_callback) { - // rmw_connextdds doesn't support rmw_event_set_callback() interface - if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { - GTEST_SKIP(); - } - std::atomic_size_t matched_count = 0; rclcpp::PublisherOptions pub_options; @@ -451,8 +436,10 @@ TEST_F(TestQosEvent, test_pub_matched_event_by_set_event_callback) auto pub = node->create_publisher( topic_name, 10, pub_options); - auto matched_event_callback = [&matched_count](size_t count) { + 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); @@ -460,34 +447,32 @@ TEST_F(TestQosEvent, test_pub_matched_event_by_set_event_callback) rclcpp::executors::SingleThreadedExecutor ex; ex.add_node(node->get_node_base_interface()); - const auto timeout = std::chrono::milliseconds(200); + const auto timeout = std::chrono::seconds(10); { auto sub1 = node->create_subscription(topic_name, 10, message_callback); - ex.spin_some(timeout); + 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_some(timeout); + ex.spin_until_future_complete(prom.get_future(), timeout); + prom = {}; EXPECT_EQ(matched_count, static_cast(2)); } - ex.spin_some(timeout); + ex.spin_until_future_complete(prom.get_future(), timeout); + prom = {}; EXPECT_EQ(matched_count, static_cast(3)); } - ex.spin_some(timeout); + 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) { - // rmw_connextdds doesn't support rmw_event_set_callback() interface - if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) { - GTEST_SKIP(); - } - std::atomic_size_t matched_count = 0; rclcpp::SubscriptionOptions sub_options; @@ -495,8 +480,10 @@ TEST_F(TestQosEvent, test_sub_matched_event_by_set_event_callback) auto sub = node->create_subscription( topic_name, 10, message_callback, sub_options); - auto matched_event_callback = [&matched_count](size_t count) { + 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); @@ -504,39 +491,44 @@ TEST_F(TestQosEvent, test_sub_matched_event_by_set_event_callback) rclcpp::executors::SingleThreadedExecutor ex; ex.add_node(node->get_node_base_interface()); - const auto timeout = std::chrono::milliseconds(200); + const auto timeout = std::chrono::seconds(10000); { auto pub1 = node->create_publisher(topic_name, 10); - ex.spin_some(timeout); + 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_some(timeout); + ex.spin_until_future_complete(prom.get_future(), timeout); + prom = {}; EXPECT_EQ(matched_count, static_cast(2)); } - ex.spin_some(timeout); + ex.spin_until_future_complete(prom.get_future(), timeout); + prom = {}; EXPECT_EQ(matched_count, static_cast(3)); } - ex.spin_some(timeout); + 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](rmw_matched_status_t & s) { + [&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( @@ -551,11 +543,12 @@ TEST_F(TestQosEvent, test_pub_matched_event_by_option_event_callback) matched_expected_result.current_count = 1; matched_expected_result.current_count_change = 1; - const auto timeout = std::chrono::milliseconds(200); + const auto timeout = std::chrono::seconds(10); { auto sub = node->create_subscription(topic_name, 10, message_callback); - ex.spin_some(timeout); + ex.spin_until_future_complete(prom.get_future(), timeout); + prom = {}; // destroy a connected subscription matched_expected_result.total_count = 1; @@ -563,20 +556,22 @@ TEST_F(TestQosEvent, test_pub_matched_event_by_option_event_callback) matched_expected_result.current_count = 0; matched_expected_result.current_count_change = -1; } - ex.spin_some(timeout); + 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](rmw_matched_status_t & s) { + [&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); @@ -590,10 +585,11 @@ TEST_F(TestQosEvent, test_sub_matched_event_by_option_event_callback) matched_expected_result.current_count = 1; matched_expected_result.current_count_change = 1; - const auto timeout = std::chrono::milliseconds(200); + const auto timeout = std::chrono::seconds(10); { auto pub1 = node->create_publisher(topic_name, 10); - ex.spin_some(timeout); + ex.spin_until_future_complete(prom.get_future(), timeout); + prom = {}; // destroy a connected publisher matched_expected_result.total_count = 1; @@ -601,5 +597,5 @@ TEST_F(TestQosEvent, test_sub_matched_event_by_option_event_callback) matched_expected_result.current_count = 0; matched_expected_result.current_count_change = -1; } - ex.spin_some(timeout); + ex.spin_until_future_complete(prom.get_future(), timeout); } diff --git a/rclcpp/test/rclcpp/test_rate.cpp b/rclcpp/test/rclcpp/test_rate.cpp index d6608d59f6..b80789c853 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,23 @@ TEST(TestRate, rate_basics) { auto start = std::chrono::system_clock::now(); rclcpp::Rate r(period); - EXPECT_EQ(period, r.period()); + EXPECT_EQ(rclcpp::Duration(period), r.period()); +// suppress deprecated warnings +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif ASSERT_FALSE(r.is_steady()); +// remove warning suppression +#if !defined(_WIN32) +# pragma GCC diagnostic pop +#else // !defined(_WIN32) +# pragma warning(pop) +#endif + 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 +93,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 +101,25 @@ TEST(TestRate, wall_rate_basics) { auto start = std::chrono::steady_clock::now(); rclcpp::WallRate r(period); - EXPECT_EQ(period, r.period()); + EXPECT_EQ(rclcpp::Duration(period), r.period()); +// suppress deprecated warnings +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif ASSERT_TRUE(r.is_steady()); +// suppress deprecated warnings +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif + 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 +150,221 @@ TEST(TestRate, wall_rate_basics) { EXPECT_GT(epsilon, delta); } -TEST(TestRate, from_double) { +/* + Basic test for the deprecated GenericRate class. + */ +TEST_F(TestRate, generic_rate) { + auto period = std::chrono::milliseconds(100); + auto offset = std::chrono::milliseconds(50); + auto epsilon = std::chrono::milliseconds(1); + double overrun_ratio = 1.5; + + { + auto start = std::chrono::system_clock::now(); + +// suppress deprecated warnings +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif + + rclcpp::GenericRate gr(period); + ASSERT_FALSE(gr.is_steady()); + +// remove warning suppression +#if !defined(_WIN32) +# pragma GCC diagnostic pop +#else // !defined(_WIN32) +# pragma warning(pop) +#endif + + ASSERT_EQ(gr.get_type(), RCL_SYSTEM_TIME); + EXPECT_EQ(period, gr.period()); + ASSERT_TRUE(gr.sleep()); + auto one = std::chrono::system_clock::now(); + auto delta = one - start; + EXPECT_LT(period, delta + epsilon); + EXPECT_GT(period * overrun_ratio, delta); + + rclcpp::sleep_for(offset); + ASSERT_TRUE(gr.sleep()); + auto two = std::chrono::system_clock::now(); + delta = two - start; + EXPECT_LT(2 * period, delta); + EXPECT_GT(2 * period * overrun_ratio, delta); + + rclcpp::sleep_for(offset); + auto two_offset = std::chrono::system_clock::now(); + gr.reset(); + ASSERT_TRUE(gr.sleep()); + auto three = std::chrono::system_clock::now(); + delta = three - two_offset; + EXPECT_LT(period, delta + epsilon); + EXPECT_GT(period * overrun_ratio, delta); + + rclcpp::sleep_for(offset + period); + auto four = std::chrono::system_clock::now(); + ASSERT_FALSE(gr.sleep()); + auto five = std::chrono::system_clock::now(); + delta = five - four; + ASSERT_TRUE(epsilon > delta); + } + + { + auto start = std::chrono::steady_clock::now(); + +// suppress deprecated warnings +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif + + rclcpp::GenericRate gr(period); + ASSERT_TRUE(gr.is_steady()); + +// remove warning suppression +#if !defined(_WIN32) +# pragma GCC diagnostic pop +#else // !defined(_WIN32) +# pragma warning(pop) +#endif + + ASSERT_EQ(gr.get_type(), RCL_STEADY_TIME); + EXPECT_EQ(period, gr.period()); + ASSERT_TRUE(gr.sleep()); + auto one = std::chrono::steady_clock::now(); + auto delta = one - start; + EXPECT_LT(period, delta); + EXPECT_GT(period * overrun_ratio, delta); + + rclcpp::sleep_for(offset); + ASSERT_TRUE(gr.sleep()); + auto two = std::chrono::steady_clock::now(); + delta = two - start; + EXPECT_LT(2 * period, delta + epsilon); + EXPECT_GT(2 * period * overrun_ratio, delta); + + rclcpp::sleep_for(offset); + auto two_offset = std::chrono::steady_clock::now(); + gr.reset(); + ASSERT_TRUE(gr.sleep()); + auto three = std::chrono::steady_clock::now(); + delta = three - two_offset; + EXPECT_LT(period, delta); + EXPECT_GT(period * overrun_ratio, delta); + + rclcpp::sleep_for(offset + period); + auto four = std::chrono::steady_clock::now(); + ASSERT_FALSE(gr.sleep()); + auto five = std::chrono::steady_clock::now(); + delta = five - four; + EXPECT_GT(epsilon, delta); + } +} + +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)); +// suppress deprecated warnings +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif + EXPECT_FALSE(rate.is_steady()); +// remove warning suppression +#if !defined(_WIN32) +# pragma GCC diagnostic pop +#else // !defined(_WIN32) +# pragma warning(pop) +#endif + EXPECT_EQ(RCL_SYSTEM_TIME, rate.get_type()); + } + { + rclcpp::Rate rate(1.0, std::make_shared(RCL_STEADY_TIME)); +// suppress deprecated warnings +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif + EXPECT_TRUE(rate.is_steady()); +// remove warning suppression +#if !defined(_WIN32) +# pragma GCC diagnostic pop +#else // !defined(_WIN32) +# pragma warning(pop) +#endif + EXPECT_EQ(RCL_STEADY_TIME, rate.get_type()); + } + { + rclcpp::Rate rate(1.0, std::make_shared(RCL_ROS_TIME)); +// suppress deprecated warnings +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif + EXPECT_FALSE(rate.is_steady()); +// remove warning suppression +#if !defined(_WIN32) +# pragma GCC diagnostic pop +#else // !defined(_WIN32) +# pragma warning(pop) +#endif + 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 index ea761fdc0c..08786a3fcb 100644 --- a/rclcpp/test/rclcpp/test_rosout_subscription.cpp +++ b/rclcpp/test/rclcpp/test_rosout_subscription.cpp @@ -70,7 +70,7 @@ TEST_F(TestRosoutSubscription, test_rosoutsubscription_getchild) { // before calling get_child of Logger { RCLCPP_INFO( - rclcpp::get_logger(logger_name), this->rosout_msg_data.c_str()); + 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); @@ -83,7 +83,7 @@ TEST_F(TestRosoutSubscription, test_rosoutsubscription_getchild) { // after calling get_child of Logger // 1. use child_logger directly { - RCLCPP_INFO(child_logger, this->rosout_msg_data.c_str()); + 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); @@ -93,7 +93,7 @@ TEST_F(TestRosoutSubscription, test_rosoutsubscription_getchild) { // 2. use rclcpp::get_logger { - RCLCPP_INFO(rclcpp::get_logger(logger_name), this->rosout_msg_data.c_str()); + 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); @@ -104,7 +104,7 @@ TEST_F(TestRosoutSubscription, test_rosoutsubscription_getchild) { // `child_logger` is end of life, there is no sublogger { - RCLCPP_INFO(rclcpp::get_logger(logger_name), this->rosout_msg_data.c_str()); + 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); @@ -119,7 +119,7 @@ TEST_F(TestRosoutSubscription, test_rosoutsubscription_parent_log) { rclcpp::Logger logger = this->node->get_logger(); ASSERT_EQ(logger.get_name(), logger_name); - RCLCPP_INFO(logger, this->rosout_msg_data.c_str()); + 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); @@ -133,14 +133,14 @@ TEST_F(TestRosoutSubscription, test_rosoutsubscription_child_log) { this->rosout_msg_name = logger_name; rclcpp::Logger logger = this->node->get_logger(); - RCLCPP_INFO(logger, this->rosout_msg_data.c_str()); + 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, this->rosout_msg_data.c_str()); + 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); @@ -148,14 +148,14 @@ TEST_F(TestRosoutSubscription, test_rosoutsubscription_child_log) { received_msg_promise = {}; logger = this->node->get_logger().get_child("child2"); - RCLCPP_INFO(logger, this->rosout_msg_data.c_str()); + 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, this->rosout_msg_data.c_str()); + 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); @@ -171,10 +171,20 @@ TEST_F(TestRosoutSubscription, test_rosoutsubscription_getchild_hierarchy) { 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, this->rosout_msg_data.c_str()); + 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 0a020bb7c0..67a2d9b2be 100644 --- a/rclcpp/test/rclcpp/test_serialized_message.cpp +++ b/rclcpp/test/rclcpp/test_serialized_message.cpp @@ -145,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 a3c361cfde..1a00ceb527 100644 --- a/rclcpp/test/rclcpp/test_service.cpp +++ b/rclcpp/test/rclcpp/test_service.cpp @@ -88,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()); @@ -102,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); } } @@ -111,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 = node->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); @@ -149,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); @@ -189,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(); @@ -217,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); { @@ -243,9 +240,9 @@ TEST_F(TestService, send_response) { 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();}; + 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( @@ -315,9 +312,8 @@ TEST_F(TestService, on_new_request_callback) { 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 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(), @@ -327,9 +323,8 @@ TEST_F(TestService, rcl_service_response_publisher_get_actual_qos_error) { 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 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(), @@ -345,11 +340,10 @@ TEST_F(TestService, server_qos) { 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 callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, test_msgs::srv::Empty::Response::SharedPtr) {}; - auto server = node->create_service( - "service", callback, qos_profile); + 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(); @@ -360,8 +354,6 @@ TEST_F(TestService, server_qos) { } TEST_F(TestService, server_qos_depth) { - using namespace std::literals::chrono_literals; - uint64_t server_cb_count_ = 0; auto server_callback = [&]( const test_msgs::srv::Empty::Request::SharedPtr, @@ -380,8 +372,8 @@ TEST_F(TestService, server_qos_depth) { ::testing::AssertionResult request_result = ::testing::AssertionSuccess(); auto request = std::make_shared(); - using SharedFuture = rclcpp::Client::SharedFuture; - auto client_callback = [&request_result](SharedFuture future_response) { + auto client_callback = [&request_result]( + rclcpp::Client::SharedFuture future_response) { if (nullptr == future_response.get()) { request_result = ::testing::AssertionFailure() << "Future response was null"; } diff --git a/rclcpp/test/rclcpp/test_subscription.cpp b/rclcpp/test/rclcpp/test_subscription.cpp index 6facada4dd..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; @@ -448,13 +367,13 @@ TEST_F(TestSubscription, on_new_message_callback) { using test_msgs::msg::Empty; auto do_nothing = [](std::shared_ptr) {FAIL();}; - auto sub = node->create_subscription("~/test_take", 10, do_nothing); + 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 pub = node_->create_publisher("~/test_take", 3); { test_msgs::msg::Empty msg; pub->publish(msg); @@ -518,13 +437,13 @@ TEST_F(TestSubscription, on_new_intra_process_message_callback) { using test_msgs::msg::Empty; auto do_nothing = [](std::shared_ptr) {FAIL();}; - auto sub = node->create_subscription("~/test_take", 10, do_nothing); + auto sub = node_->create_subscription("~/test_take", 10, do_nothing); std::atomic c1 {0}; auto increase_c1_cb = [&c1](size_t count_msgs) {c1 += count_msgs;}; sub->set_on_new_intra_process_message_callback(increase_c1_cb); - auto pub = node->create_publisher("~/test_take", 1); + auto pub = node_->create_publisher("~/test_take", 1); { test_msgs::msg::Empty msg; pub->publish(msg); @@ -580,21 +499,146 @@ TEST_F(TestSubscription, on_new_intra_process_message_callback) { EXPECT_THROW(sub->set_on_new_intra_process_message_callback(invalid_cb), std::invalid_argument); } +TEST_F(TestSubscription, get_network_flow_endpoints_errors) { + initialize(); + const rclcpp::QoS subscription_qos(1); + auto subscription_callback = [](test_msgs::msg::Empty::ConstSharedPtr msg) { + (void)msg; + }; + auto subscription = node_->create_subscription( + "topic", subscription_qos, subscription_callback); + + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_subscription_get_network_flow_endpoints, RCL_RET_ERROR); + auto mock_network_flow_endpoint_array_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_network_flow_endpoint_array_fini, RCL_RET_ERROR); + EXPECT_THROW( + subscription->get_network_flow_endpoints(), + rclcpp::exceptions::RCLError); + } + { + auto mock_network_flow_endpoint_array_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_network_flow_endpoint_array_fini, RCL_RET_ERROR); + EXPECT_THROW( + subscription->get_network_flow_endpoints(), + rclcpp::exceptions::RCLError); + } + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_subscription_get_network_flow_endpoints, RCL_RET_OK); + auto mock_network_flow_endpoint_array_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_network_flow_endpoint_array_fini, RCL_RET_OK); + EXPECT_NO_THROW(subscription->get_network_flow_endpoints()); + } +} + +class TestSubscriptionSub : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node_ = std::make_shared("test_subscription", "/ns"); + subnode_ = node_->create_sub_node("sub_ns"); + } + + rclcpp::Node::SharedPtr node_; + rclcpp::Node::SharedPtr subnode_; +}; + +/* + Testing subscription construction and destruction for subnodes. + */ +TEST_F(TestSubscriptionSub, construction_and_destruction) { + auto callback = [](test_msgs::msg::Empty::ConstSharedPtr msg) { + (void)msg; + }; + { + auto sub = subnode_->create_subscription("topic", 1, callback); + EXPECT_STREQ(sub->get_topic_name(), "/ns/sub_ns/topic"); + } + + { + auto sub = subnode_->create_subscription("/topic", 1, callback); + EXPECT_STREQ(sub->get_topic_name(), "/topic"); + } + + { + auto sub = subnode_->create_subscription("~/topic", 1, callback); + std::string expected_topic_name = + std::string(node_->get_namespace()) + "/" + node_->get_name() + "/topic"; + EXPECT_STREQ(sub->get_topic_name(), expected_topic_name.c_str()); + } + + { + ASSERT_THROW( + { + auto sub = node_->create_subscription("invalid_topic?", 1, callback); + }, rclcpp::exceptions::InvalidTopicNameError); + } +} + +struct TestParameters final +{ + TestParameters(rclcpp::QoS qos, std::string description) + : qos(qos), description(description) {} + rclcpp::QoS qos; + std::string description; +}; + +std::ostream & operator<<(std::ostream & out, const TestParameters & params) +{ + out << params.description; + return out; +} + +class TestSubscriptionInvalidIntraprocessQos + : public TestSubscription, + public ::testing::WithParamInterface +{}; + +static std::vector invalid_qos_profiles() +{ + std::vector parameters; + + parameters.reserve(1); + parameters.push_back( + TestParameters( + rclcpp::QoS(rclcpp::KeepAll()), + "keep_all_qos")); + + return parameters; +} + +INSTANTIATE_TEST_SUITE_P( + TestSubscriptionThrows, TestSubscriptionInvalidIntraprocessQos, + ::testing::ValuesIn(invalid_qos_profiles()), + ::testing::PrintToStringParamName()); + /* Testing subscription with intraprocess enabled and invalid QoS */ TEST_P(TestSubscriptionInvalidIntraprocessQos, test_subscription_throws) { initialize(rclcpp::NodeOptions().use_intra_process_comms(true)); rclcpp::QoS qos = GetParam().qos; - using test_msgs::msg::Empty; { auto callback = std::bind( - &TestSubscriptionInvalidIntraprocessQos::OnMessage, + &TestSubscriptionInvalidIntraprocessQos::on_message, this, std::placeholders::_1); ASSERT_THROW( - {auto subscription = node->create_subscription( + {auto subscription = node_->create_subscription( "topic", qos, callback);}, @@ -612,71 +656,15 @@ TEST_P(TestSubscriptionInvalidIntraprocessQos, test_subscription_throws_intrapro initialize(); rclcpp::QoS qos = GetParam().qos; auto callback = std::bind( - &TestSubscriptionInvalidIntraprocessQos::OnMessage, + &TestSubscriptionInvalidIntraprocessQos::on_message, this, std::placeholders::_1); RCLCPP_EXPECT_THROW_EQ( - {auto subscription = node->create_subscription( + {auto subscription = node_->create_subscription( "topic", qos, callback, options);}, std::runtime_error("Unrecognized IntraProcessSetting value")); } - -static std::vector invalid_qos_profiles() -{ - std::vector parameters; - - 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")); - - return parameters; -} - -INSTANTIATE_TEST_SUITE_P( - TestSubscriptionThrows, TestSubscriptionInvalidIntraprocessQos, - ::testing::ValuesIn(invalid_qos_profiles()), - ::testing::PrintToStringParamName()); - -TEST_F(TestSubscription, get_network_flow_endpoints_errors) { - initialize(); - const rclcpp::QoS subscription_qos(1); - auto subscription_callback = [](test_msgs::msg::Empty::ConstSharedPtr msg) { - (void)msg; - }; - auto subscription = node->create_subscription( - "topic", subscription_qos, subscription_callback); - - { - auto mock = mocking_utils::patch_and_return( - "lib:rclcpp", rcl_subscription_get_network_flow_endpoints, RCL_RET_ERROR); - auto mock_network_flow_endpoint_array_fini = mocking_utils::patch_and_return( - "lib:rclcpp", rcl_network_flow_endpoint_array_fini, RCL_RET_ERROR); - EXPECT_THROW( - subscription->get_network_flow_endpoints(), - rclcpp::exceptions::RCLError); - } - { - auto mock_network_flow_endpoint_array_fini = mocking_utils::patch_and_return( - "lib:rclcpp", rcl_network_flow_endpoint_array_fini, RCL_RET_ERROR); - EXPECT_THROW( - subscription->get_network_flow_endpoints(), - rclcpp::exceptions::RCLError); - } - { - auto mock = mocking_utils::patch_and_return( - "lib:rclcpp", rcl_subscription_get_network_flow_endpoints, RCL_RET_OK); - auto mock_network_flow_endpoint_array_fini = mocking_utils::patch_and_return( - "lib:rclcpp", rcl_network_flow_endpoint_array_fini, RCL_RET_OK); - EXPECT_NO_THROW(subscription->get_network_flow_endpoints()); - } -} diff --git a/rclcpp/test/rclcpp/test_subscription_options.cpp b/rclcpp/test/rclcpp/test_subscription_options.cpp index c4cfb6b4c4..d1122333bc 100644 --- a/rclcpp/test/rclcpp/test_subscription_options.cpp +++ b/rclcpp/test/rclcpp/test_subscription_options.cpp @@ -60,14 +60,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()); 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_time.cpp b/rclcpp/test/rclcpp/test_time.cpp index f3969d3886..7d656fb9c4 100644 --- a/rclcpp/test/rclcpp/test_time.cpp +++ b/rclcpp/test/rclcpp/test_time.cpp @@ -363,10 +363,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) { diff --git a/rclcpp/test/rclcpp/test_time_source.cpp b/rclcpp/test/rclcpp/test_time_source.cpp index 26d7cb2192..1437e47126 100644 --- a/rclcpp/test/rclcpp/test_time_source.cpp +++ b/rclcpp/test/rclcpp/test_time_source.cpp @@ -305,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()); diff --git a/rclcpp/test/rclcpp/test_timer.cpp b/rclcpp/test/rclcpp/test_timer.cpp index 59a1218519..6d26bc54b8 100644 --- a/rclcpp/test/rclcpp/test_timer.cpp +++ b/rclcpp/test/rclcpp/test_timer.cpp @@ -73,6 +73,20 @@ class TestTimer : public ::testing::TestWithParam EXPECT_FALSE(timer->is_steady()); break; } + timer_without_autostart = test_node->create_wall_timer( + 100ms, + [this]() -> void + { + this->has_timer_run.store(true); + + if (this->cancel_timer.load()) { + this->timer->cancel(); + } + // prevent any tests running timer from blocking + this->executor->cancel(); + }, nullptr, false); + EXPECT_TRUE(timer_without_autostart->is_steady()); + executor->add_node(test_node); // don't start spinning, let the test dictate when } @@ -93,6 +107,7 @@ class TestTimer : public ::testing::TestWithParam std::atomic cancel_timer; rclcpp::Node::SharedPtr test_node; std::shared_ptr timer; + std::shared_ptr timer_without_autostart; std::shared_ptr executor; }; @@ -270,20 +285,14 @@ TEST_P(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( - { - switch (timer_type) { - case TimerType::WALL_TIMER: - timer_to_test_destructor = - test_node->create_wall_timer(std::chrono::milliseconds(0), [](void) {}); - break; - case TimerType::GENERIC_TIMER: - timer_to_test_destructor = - test_node->create_timer(std::chrono::milliseconds(0), [](void) {}); - break; - } - timer_to_test_destructor.reset(); - }); + if (timer_type == TimerType::WALL_TIMER) { + timer_to_test_destructor = + test_node->create_wall_timer(std::chrono::milliseconds(0), [](void) {}); + } else { + timer_to_test_destructor = + test_node->create_timer(std::chrono::milliseconds(0), [](void) {}); + } + timer_to_test_destructor.reset(); } { auto mock = mocking_utils::patch_and_return( @@ -334,3 +343,18 @@ INSTANTIATE_TEST_SUITE_P( 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 index 635ec322c0..90d321c188 100644 --- a/rclcpp/test/rclcpp/test_timers_manager.cpp +++ b/rclcpp/test/rclcpp/test_timers_manager.cpp @@ -15,6 +15,7 @@ #include #include +#include #include #include @@ -65,8 +66,10 @@ TEST_F(TestTimersManager, empty_manager) TEST_F(TestTimersManager, add_run_remove_timer) { size_t t_runs = 0; + std::chrono::milliseconds timer_period(10); + auto t = TimerT::make_shared( - 10ms, + timer_period, [&t_runs]() { t_runs++; }, @@ -79,7 +82,7 @@ TEST_F(TestTimersManager, add_run_remove_timer) timers_manager->add_timer(t); // Sleep for more 3 times the timer period - std::this_thread::sleep_for(30ms); + 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); @@ -191,67 +194,6 @@ TEST_F(TestTimersManager, head_not_ready) EXPECT_EQ(0u, t_runs); } -TEST_F(TestTimersManager, timers_order) -{ - auto timers_manager = std::make_shared( - rclcpp::contexts::get_global_default_context()); - - size_t t1_runs = 0; - auto t1 = TimerT::make_shared( - 10ms, - [&t1_runs]() { - t1_runs++; - }, - rclcpp::contexts::get_global_default_context()); - - size_t t2_runs = 0; - auto t2 = TimerT::make_shared( - 30ms, - [&t2_runs]() { - t2_runs++; - }, - rclcpp::contexts::get_global_default_context()); - - size_t t3_runs = 0; - auto t3 = TimerT::make_shared( - 100ms, - [&t3_runs]() { - t3_runs++; - }, - rclcpp::contexts::get_global_default_context()); - - // Add timers in a random order - timers_manager->add_timer(t2); - timers_manager->add_timer(t3); - timers_manager->add_timer(t1); - - std::this_thread::sleep_for(10ms); - execute_all_ready_timers(timers_manager); - EXPECT_EQ(1u, t1_runs); - EXPECT_EQ(0u, t2_runs); - EXPECT_EQ(0u, t3_runs); - - std::this_thread::sleep_for(30ms); - execute_all_ready_timers(timers_manager); - EXPECT_EQ(2u, t1_runs); - EXPECT_EQ(1u, t2_runs); - EXPECT_EQ(0u, t3_runs); - - std::this_thread::sleep_for(100ms); - execute_all_ready_timers(timers_manager); - EXPECT_EQ(3u, t1_runs); - EXPECT_EQ(2u, t2_runs); - EXPECT_EQ(1u, t3_runs); - - timers_manager->remove_timer(t1); - - std::this_thread::sleep_for(30ms); - execute_all_ready_timers(timers_manager); - EXPECT_EQ(3u, t1_runs); - EXPECT_EQ(3u, t2_runs); - EXPECT_EQ(1u, t3_runs); -} - TEST_F(TestTimersManager, start_stop_timers_thread) { auto timers_manager = std::make_shared( @@ -274,7 +216,7 @@ TEST_F(TestTimersManager, timers_thread) auto timers_manager = std::make_shared( rclcpp::contexts::get_global_default_context()); - size_t t1_runs = 0; + int t1_runs = 0; auto t1 = TimerT::make_shared( 1ms, [&t1_runs]() { @@ -282,7 +224,7 @@ TEST_F(TestTimersManager, timers_thread) }, rclcpp::contexts::get_global_default_context()); - size_t t2_runs = 0; + int t2_runs = 0; auto t2 = TimerT::make_shared( 1ms, [&t2_runs]() { @@ -296,12 +238,12 @@ TEST_F(TestTimersManager, timers_thread) // Run timers thread for a while timers_manager->start(); - std::this_thread::sleep_for(5ms); + std::this_thread::sleep_for(50ms); timers_manager->stop(); EXPECT_LT(1u, t1_runs); EXPECT_LT(1u, t2_runs); - EXPECT_EQ(t1_runs, t2_runs); + EXPECT_LE(std::abs(t1_runs - t2_runs), 1); } TEST_F(TestTimersManager, destructor) @@ -365,13 +307,13 @@ TEST_F(TestTimersManager, add_remove_while_thread_running) timers_manager->start(); // After a while remove t1 and add t2 - std::this_thread::sleep_for(5ms); + 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(5ms); + std::this_thread::sleep_for(50ms); timers_manager->stop(); // t1 has stopped running @@ -417,3 +359,81 @@ TEST_F(TestTimersManager, infinite_loop) 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, cancel_iter]() { + 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_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/topic_statistics/test_subscription_topic_statistics.cpp b/rclcpp/test/rclcpp/topic_statistics/test_subscription_topic_statistics.cpp index ce6887c631..9166272207 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; - -private: - rclcpp::Subscription::SharedPtr subscription_; -}; + ~SubscriberWithTopicStatistics() override = default; -/** - * 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()); @@ -524,6 +355,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 +363,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 9afb97536a..12bd2f884e 100644 --- a/rclcpp/test/rclcpp/wait_set_policies/test_dynamic_storage.cpp +++ b/rclcpp/test/rclcpp/wait_set_policies/test_dynamic_storage.cpp @@ -51,14 +51,14 @@ class TestWaitable : public rclcpp::Waitable TestWaitable() : is_ready_(false) {} - void add_to_wait_set(rcl_wait_set_t *) override {} + void add_to_wait_set(rcl_wait_set_t &) override {} - bool is_ready(rcl_wait_set_t *) override {return is_ready_;} + 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;} + execute(const std::shared_ptr &) override {} void set_is_ready(bool value) {is_ready_ = value;} 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 470d0de361..55573cc11b 100644 --- a/rclcpp/test/rclcpp/wait_set_policies/test_static_storage.cpp +++ b/rclcpp/test/rclcpp/wait_set_policies/test_static_storage.cpp @@ -50,14 +50,14 @@ class TestWaitable : public rclcpp::Waitable public: TestWaitable() : is_ready_(false) {} - void add_to_wait_set(rcl_wait_set_t *) override {} + void add_to_wait_set(rcl_wait_set_t &) override {} - bool is_ready(rcl_wait_set_t *) override {return is_ready_;} + 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;} + execute(const std::shared_ptr &) override {} void set_is_ready(bool value) {is_ready_ = value;} 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 d217286da1..cd44918236 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,19 @@ class TestWaitable : public rclcpp::Waitable public: TestWaitable() : is_ready_(false), add_to_wait_set_(false) {} - void add_to_wait_set(rcl_wait_set_t *) override + void add_to_wait_set(rcl_wait_set_t &) override { if (!add_to_wait_set_) { throw std::runtime_error("waitable unexpectedly failed to be added to wait set"); } } - bool is_ready(rcl_wait_set_t *) override {return is_ready_;} + 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;} + execute(const std::shared_ptr & data) override {(void)data;} void set_is_ready(bool value) {is_ready_ = value;} 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 347714bbf7..7554f56270 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,14 +50,14 @@ class TestWaitable : public rclcpp::Waitable public: TestWaitable() : is_ready_(false) {} - void add_to_wait_set(rcl_wait_set_t *) override {} + void add_to_wait_set(rcl_wait_set_t &) override {} - bool is_ready(rcl_wait_set_t *) override {return is_ready_;} + 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;} + execute(const std::shared_ptr &) override {} void set_is_ready(bool value) {is_ready_ = value;} diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 26f00ce559..b7b7a7e4c7 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,68 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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) ------------------- diff --git a/rclcpp_action/CMakeLists.txt b/rclcpp_action/CMakeLists.txt index 785ed0e5e1..647a820969 100644 --- a/rclcpp_action/CMakeLists.txt +++ b/rclcpp_action/CMakeLists.txt @@ -5,6 +5,7 @@ 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) @@ -21,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, @@ -69,12 +68,7 @@ ament_export_libraries(${PROJECT_NAME}) # Export modern CMake targets ament_export_targets(${PROJECT_NAME}) -# specific order: dependents before dependencies -ament_export_dependencies(ament_cmake) -ament_export_dependencies(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) @@ -87,58 +81,52 @@ if(BUILD_TESTING) ament_add_gtest(test_client test/test_client.cpp TIMEOUT 180) 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) 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) 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() 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..dbb91a6ba3 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, at the [ROS Index](https://index.ros.org/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 a9bf2a5008..94cf4aab31 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -122,12 +122,12 @@ class ClientBase : public rclcpp::Waitable /// \internal RCLCPP_ACTION_PUBLIC void - add_to_wait_set(rcl_wait_set_t * wait_set) override; + 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 @@ -142,7 +142,7 @@ class ClientBase : public rclcpp::Waitable /// \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 diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp index dd3a40671c..73987ec887 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp @@ -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..d12b4fc5b3 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,7 +70,7 @@ 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_) { @@ -82,7 +82,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 +90,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 +98,7 @@ template int8_t ClientGoalHandle::get_status() { - std::lock_guard guard(handle_mutex_); + std::lock_guard guard(handle_mutex_); return status_; } @@ -106,7 +106,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 +114,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 +122,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 +130,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 +140,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 +168,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/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index 892de5743b..7461de2867 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -119,13 +119,13 @@ class ServerBase : public rclcpp::Waitable /// \internal RCLCPP_ACTION_PUBLIC void - add_to_wait_set(rcl_wait_set_t * wait_set) override; + 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 @@ -139,7 +139,7 @@ class ServerBase : public rclcpp::Waitable /// \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 @@ -279,19 +279,19 @@ class ServerBase : public rclcpp::Waitable /// \internal RCLCPP_ACTION_PUBLIC void - execute_goal_request_received(std::shared_ptr & data); + execute_goal_request_received(const std::shared_ptr & data); /// 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(const std::shared_ptr & data); /// 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(const std::shared_ptr & data); /// Handle a timeout indicating a completed goal should be forgotten by the server /// \internal diff --git a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp index ac9dd49492..7d17819189 100644 --- a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp @@ -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. * diff --git a/rclcpp_action/include/rclcpp_action/types.hpp b/rclcpp_action/include/rclcpp_action/types.hpp index 829b6cd2c1..cf359dfaa9 100644 --- a/rclcpp_action/include/rclcpp_action/types.hpp +++ b/rclcpp_action/include/rclcpp_action/types.hpp @@ -69,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 bfda7c1343..8c1dd30127 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 21.1.0 + 28.0.0 Adds action APIs for C++. Ivan Paunovic @@ -23,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 2d5018d5af..fc5b3eeb90 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -163,7 +163,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 +171,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 @@ -253,20 +253,21 @@ ClientBase::get_number_of_ready_services() } void -ClientBase::add_to_wait_set(rcl_wait_set_t * wait_set) +ClientBase::add_to_wait_set(rcl_wait_set_t & wait_set) { rcl_ret_t ret = rcl_action_wait_set_add_action_client( - wait_set, pimpl_->client_handle.get(), nullptr, nullptr); + &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(), + &wait_set, + pimpl_->client_handle.get(), &pimpl_->is_feedback_ready, &pimpl_->is_status_ready, &pimpl_->is_goal_response_ready, @@ -619,7 +620,7 @@ ClientBase::take_data_by_entity_id(size_t id) } void -ClientBase::execute(std::shared_ptr & data) +ClientBase::execute(const std::shared_ptr & data) { if (!data) { throw std::runtime_error("'data' is empty"); diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index c0fa96a88e..565aaa4f3b 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -165,18 +165,18 @@ ServerBase::get_number_of_ready_guard_conditions() } void -ServerBase::add_to_wait_set(rcl_wait_set_t * wait_set) +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); + &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; @@ -186,7 +186,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, @@ -287,7 +287,7 @@ ServerBase::take_data_by_entity_id(size_t id) } void -ServerBase::execute(std::shared_ptr & data) +ServerBase::execute(const std::shared_ptr & data) { if (!data && !pimpl_->goal_expired_.load()) { throw std::runtime_error("'data' is empty"); @@ -307,7 +307,7 @@ ServerBase::execute(std::shared_ptr & data) } void -ServerBase::execute_goal_request_received(std::shared_ptr & data) +ServerBase::execute_goal_request_received(const std::shared_ptr & data) { auto shared_ptr = std::static_pointer_cast >>(data); @@ -344,7 +344,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; @@ -396,11 +405,10 @@ 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(const std::shared_ptr & data) { auto shared_ptr = std::static_pointer_cast , @@ -483,14 +491,22 @@ 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(const std::shared_ptr & data) { auto shared_ptr = std::static_pointer_cast , rmw_request_id_t>>(data); @@ -538,11 +554,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 @@ -671,7 +694,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); } } 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/test_client.cpp b/rclcpp_action/test/test_client.cpp index b94a82d500..08093cb873 100644 --- a/rclcpp_action/test/test_client.cpp +++ b/rclcpp_action/test/test_client.cpp @@ -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_NO_THROW(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; @@ -545,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) { @@ -852,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); @@ -860,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; @@ -902,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 8687f90fbe..9ffa31797f 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -1107,12 +1107,12 @@ TEST_F(TestGoalRequestServer, is_ready_rcl_error) { { EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)); }); - EXPECT_NO_THROW(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_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_types.cpp b/rclcpp_action/test/test_types.cpp index 4922c52930..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) { @@ -59,3 +60,35 @@ TEST(TestActionTypes, rcl_action_goal_info_to_goal_uuid) { 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 144faaf2a5..ad7db5096e 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,63 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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) ------------------- diff --git a/rclcpp_components/CMakeLists.txt b/rclcpp_components/CMakeLists.txt index 121ef434c2..7d4135051c 100644 --- a/rclcpp_components/CMakeLists.txt +++ b/rclcpp_components/CMakeLists.txt @@ -21,7 +21,7 @@ find_package(composition_interfaces REQUIRED) find_package(rclcpp REQUIRED) find_package(rcpputils REQUIRED) -# Add an interface library that can be dependend upon by libraries who register components +# Add an interface library that can be depended upon by libraries who register components add_library(component INTERFACE) target_include_directories(component INTERFACE "$" @@ -38,12 +38,14 @@ 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") @@ -52,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 @@ -66,20 +65,13 @@ 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) -ament_target_dependencies(component_container_isolated - "rclcpp" -) - +target_link_libraries(component_container_isolated component_manager rclcpp::rclcpp) if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") target_link_libraries(component_container "stdc++fs") 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..0b0e0a7aec 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, at the [ROS Index](https://index.ros.org/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 c5b87af667..0c5bd33b91 100644 --- a/rclcpp_components/cmake/rclcpp_components_register_node.cmake +++ b/rclcpp_components/cmake/rclcpp_components_register_node.cmake @@ -24,6 +24,8 @@ # :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 # @@ -70,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/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 5a63923544..e309698b84 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 21.1.0 + 28.0.0 Package containing tools for dynamically loadable components Ivan Paunovic diff --git a/rclcpp_components/src/component_manager.cpp b/rclcpp_components/src/component_manager.cpp index 036350a64f..7b77af9c92 100644 --- a/rclcpp_components/src/component_manager.cpp +++ b/rclcpp_components/src/component_manager.cpp @@ -39,10 +39,12 @@ 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)); diff --git a/rclcpp_components/src/node_main.cpp.in b/rclcpp_components/src/node_main.cpp.in index 71754d1f82..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,48 +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::@executor@ exec; + @executor@ exec; rclcpp::NodeOptions options; options.arguments(args); - 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 = std::make_unique(library_name); - auto classes = loader->getAvailableClasses(); - for (const auto & clazz : classes) { - std::string name = clazz.c_str(); - if (name.compare(class_name) == 0) { - RCLCPP_DEBUG(logger, "Instantiate class %s", clazz.c_str()); - std::shared_ptr node_factory = nullptr; - try { - node_factory = loader->createInstance(clazz); - } 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; - } - auto wrapper = node_factory->create_node_instance(options); - auto node = wrapper.get_node_base_interface(); - node_wrappers.push_back(wrapper); - exec.add_node(node); - } + 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_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 9ab0142005..0e8c604097 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,66 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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) ------------------- diff --git a/rclcpp_lifecycle/CMakeLists.txt b/rclcpp_lifecycle/CMakeLists.txt index f46f993b21..66b1990ec5 100644 --- a/rclcpp_lifecycle/CMakeLists.txt +++ b/rclcpp_lifecycle/CMakeLists.txt @@ -60,8 +60,13 @@ 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( 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..e182390136 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, at the [ROS Index](https://index.ros.org/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 197ecf5c19..657ddddc34 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.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" @@ -689,6 +690,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 @@ -823,6 +840,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 @@ -1085,6 +1110,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/node_interfaces/lifecycle_node_interface.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp index 272c4def27..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 @@ -21,6 +21,14 @@ #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 { namespace node_interfaces @@ -108,6 +116,10 @@ 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) diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index ac710d1431..6a2157c002 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 21.1.0 + 28.0.0 Package containing a prototype for lifecycle implementation Ivan Paunovic diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 4c0b94cb42..0c448cf5e6 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -43,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" @@ -113,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); @@ -137,6 +144,10 @@ 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() @@ -375,6 +386,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 { @@ -468,6 +491,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() { diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp index 5c5f7797e1..9074c9cc6e 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp @@ -29,6 +29,7 @@ #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" @@ -50,9 +51,11 @@ namespace rclcpp_lifecycle LifecycleNode::LifecycleNodeInterfaceImpl::LifecycleNodeInterfaceImpl( std::shared_ptr node_base_interface, - std::shared_ptr node_services_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_services_interface_(node_services_interface), + node_logging_interface_(node_logging_interface) { } @@ -65,8 +68,8 @@ LifecycleNode::LifecycleNodeInterfaceImpl::~LifecycleNodeInterfaceImpl() ret = rcl_lifecycle_state_machine_fini(&state_machine_, node_handle); } if (ret != RCL_RET_OK) { - RCUTILS_LOG_FATAL_NAMED( - "rclcpp_lifecycle", + RCLCPP_FATAL( + node_logging_interface_->get_logger(), "failed to destroy rcl_state_machine"); } } @@ -398,7 +401,8 @@ LifecycleNode::LifecycleNodeInterfaceImpl::change_state( { std::lock_guard lock(state_machine_mutex_); if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { - RCUTILS_LOG_ERROR( + 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; @@ -411,7 +415,8 @@ LifecycleNode::LifecycleNodeInterfaceImpl::change_state( rcl_lifecycle_trigger_transition_by_id( &state_machine_, transition_id, publish_update) != RCL_RET_OK) { - RCUTILS_LOG_ERROR( + 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(); @@ -443,7 +448,8 @@ LifecycleNode::LifecycleNodeInterfaceImpl::change_state( rcl_lifecycle_trigger_transition_by_label( &state_machine_, transition_label, publish_update) != RCL_RET_OK) { - RCUTILS_LOG_ERROR( + 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(); @@ -458,7 +464,9 @@ LifecycleNode::LifecycleNodeInterfaceImpl::change_state( // 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."); + 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); @@ -467,7 +475,9 @@ LifecycleNode::LifecycleNodeInterfaceImpl::change_state( 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); + 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; } @@ -495,8 +505,12 @@ LifecycleNode::LifecycleNodeInterfaceImpl::execute_callback( 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()); + 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; } } diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp index d09f44831c..5cf5bdaacf 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp @@ -32,6 +32,7 @@ #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 "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" @@ -52,7 +53,8 @@ class LifecycleNode::LifecycleNodeInterfaceImpl final public: LifecycleNodeInterfaceImpl( std::shared_ptr node_base_interface, - std::shared_ptr node_services_interface); + std::shared_ptr node_services_interface, + std::shared_ptr node_logging_interface); ~LifecycleNodeInterfaceImpl(); @@ -152,6 +154,7 @@ class LifecycleNode::LifecycleNodeInterfaceImpl final 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 = @@ -163,6 +166,7 @@ class LifecycleNode::LifecycleNodeInterfaceImpl final 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_; diff --git a/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_client.cpp b/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_client.cpp index c8d166ef66..59ed24489e 100644 --- a/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_client.cpp +++ b/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_client.cpp @@ -228,7 +228,8 @@ class BenchmarkLifecycleClient : public performance_test_fixture::PerformanceTes BENCHMARK_F(BenchmarkLifecycleClient, get_state)(benchmark::State & state) { for (auto _ : state) { (void)_; - const auto lifecycle_state = lifecycle_client->get_state(); + + 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: ") + @@ -268,7 +269,7 @@ BENCHMARK_F(BenchmarkLifecycleClient, get_available_states)(benchmark::State & s 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: ") + @@ -284,7 +285,8 @@ BENCHMARK_F(BenchmarkLifecycleClient, get_available_transitions)(benchmark::Stat 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: ") + @@ -300,7 +302,8 @@ BENCHMARK_F(BenchmarkLifecycleClient, get_transition_graph)(benchmark::State & s 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 0bf3c48ab9..4be9ad13af 100644 --- a/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_node.cpp +++ b/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_node.cpp @@ -97,13 +97,15 @@ class BenchmarkLifecycleNode : public performance_test_fixture::PerformanceTest BENCHMARK_F(BenchmarkLifecycleNode, get_current_state)(benchmark::State & state) { for (auto _ : state) { (void)_; - const auto & lifecycle_state = node->get_current_state(); + 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(); } } @@ -112,7 +114,7 @@ BENCHMARK_F(BenchmarkLifecycleNode, get_available_states)(benchmark::State & sta 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()); @@ -126,7 +128,7 @@ BENCHMARK_F(BenchmarkLifecycleNode, get_available_transitions)(benchmark::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()); @@ -140,7 +142,7 @@ BENCHMARK_F(BenchmarkLifecycleNode, get_transition_graph)(benchmark::State & sta 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: ") + @@ -162,18 +164,20 @@ BENCHMARK_F(BenchmarkLifecycleNode, transition_valid_state)(benchmark::State & s reset_heap_counters(); for (auto _ : state) { (void)_; - const auto & active = + 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/test_lifecycle_node.cpp b/rclcpp_lifecycle/test/test_lifecycle_node.cpp index 31850c589b..fdb9be6153 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); @@ -249,6 +253,35 @@ TEST_F(TestDefaultStateMachine, empty_initializer_rcl_errors) { } } +TEST_F(TestDefaultStateMachine, check_logger_services_exist) { + // Logger level services are disabled + { + rclcpp::NodeOptions options = rclcpp::NodeOptions(); + options.enable_logger_service(false); + auto node = std::make_shared( + "test_logger_service", "/test", options); + auto get_client = node->create_client( + "/test/test_logger_service/get_logger_levels"); + ASSERT_FALSE(get_client->wait_for_service(2s)); + auto set_client = node->create_client( + "/test/test_logger_service/set_logger_levels"); + ASSERT_FALSE(set_client->wait_for_service(2s)); + } + // Logger level services are enabled + { + rclcpp::NodeOptions options = rclcpp::NodeOptions(); + options.enable_logger_service(true); + auto node = std::make_shared( + "test_logger_service", "/test", options); + auto get_client = node->create_client( + "/test/test_logger_service/get_logger_levels"); + ASSERT_TRUE(get_client->wait_for_service(2s)); + auto set_client = node->create_client( + "/test/test_logger_service/set_logger_levels"); + ASSERT_TRUE(set_client->wait_for_service(2s)); + } +} + TEST_F(TestDefaultStateMachine, trigger_transition) { auto test_node = std::make_shared("testnode"); @@ -427,11 +460,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"; @@ -469,10 +506,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", @@ -487,11 +525,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"; @@ -549,8 +589,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); @@ -585,10 +624,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); @@ -633,6 +674,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) { @@ -684,6 +726,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) {