diff --git a/README.md b/README.md index f219245ab5..0226a75f0a 100644 --- a/README.md +++ b/README.md @@ -12,6 +12,6 @@ Visit the [rclcpp API documentation](http://docs.ros2.org/latest/api/rclcpp/) fo ### Examples -The ROS 2 tutorials [Writing a simple publisher and subscriber](https://index.ros.org/doc/ros2/Tutorials/Writing-A-Simple-Cpp-Publisher-And-Subscriber/) -and [Writing a simple service and client](https://index.ros.org/doc/ros2/Tutorials/Writing-A-Simple-Cpp-Service-And-Client/) +The ROS 2 tutorials [Writing a simple publisher and subscriber](https://docs.ros.org/en/rolling/Tutorials/Writing-A-Simple-Cpp-Publisher-And-Subscriber.html). +and [Writing a simple service and client](https://docs.ros.org/en/rolling/Tutorials/Writing-A-Simple-Cpp-Service-And-Client.html) contain some examples of rclcpp APIs in use. diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 118c6504be..0676ae673c 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,103 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +11.0.0 (2021-05-18) +------------------- +* Allow declare uninitialized parameters (`#1673 `_) +* Fix syntax issue with gcc (`#1674 `_) +* [service] Don't use a weak_ptr to avoid leaking (`#1668 `_) +* Contributors: Ivan Santiago Paunovic, Jacob Perron, William Woodall + +10.0.0 (2021-05-11) +------------------- +* Fix doc typo (`#1663 `_) +* [rclcpp] Type Adaptation feature (`#1557 `_) +* Do not attempt to use void allocators for memory allocation. (`#1657 `_) +* Keep custom allocator in publisher and subscription options alive. (`#1647 `_) +* Fix get_publishers_subscriptions_info_by_topic test in test_node.cpp (`#1648 `_) +* Use OnShutdown callback handle instead of OnShutdown callback (`#1639 `_) +* use dynamic_pointer_cast to detect allocator mismatch in intra process manager (`#1643 `_) +* Increase cppcheck timeout to 500s (`#1634 `_) +* Clarify node parameters docs (`#1631 `_) +* Contributors: Audrow Nash, Barry Xu, Jacob Perron, Michel Hidalgo, Shane Loretz, William Woodall + +9.0.2 (2021-04-14) +------------------ +* Avoid returning loan when none was obtained. (`#1629 `_) +* Use a different implementation of mutex two priorities (`#1628 `_) +* Do not test the value of the history policy when testing the get_publishers/subscriptions_info_by_topic() methods (`#1626 `_) +* Check first parameter type and range before calling the user validation callbacks (`#1627 `_) +* Contributors: Ivan Santiago Paunovic, Miguel Company + +9.0.1 (2021-04-12) +------------------ +* Restore test exception for Connext (`#1625 `_) +* Fix race condition in TimeSource clock thread setup (`#1623 `_) +* Contributors: Andrea Sorbini, Michel Hidalgo + +9.0.0 (2021-04-06) +------------------ +* remove deprecated code which was deprecated in foxy and should be removed in galactic (`#1622 `_) +* Change index.ros.org -> docs.ros.org. (`#1620 `_) +* Unique network flows (`#1496 `_) +* Add spin_some support to the StaticSingleThreadedExecutor (`#1338 `_) +* Add publishing instrumentation (`#1600 `_) +* Create load_parameters and delete_parameters methods (`#1596 `_) +* refactor AnySubscriptionCallback and add/deprecate callback signatures (`#1598 `_) +* Add generic publisher and generic subscription for serialized messages (`#1452 `_) +* use context from `node_base\_` for clock executor. (`#1617 `_) +* updating quality declaration links (re: `ros2/docs.ros2.org#52 `_) (`#1615 `_) +* Contributors: Ananya Muddukrishna, BriceRenaudeau, Chris Lalancette, Christophe Bedard, Nikolai Morin, Tomoya Fujita, William Woodall, mauropasse, shonigmann + +8.2.0 (2021-03-31) +------------------ +* Initialize integers in test_parameter_event_handler.cpp to avoid undefined behavior (`#1609 `_) +* Namespace tracetools C++ functions (`#1608 `_) +* Revert "Namespace tracetools C++ functions (`#1603 `_)" (`#1607 `_) +* Namespace tracetools C++ functions (`#1603 `_) +* Clock subscription callback group spins in its own thread (`#1556 `_) +* Contributors: Chris Lalancette, Christophe Bedard, Ivan Santiago Paunovic, anaelle-sw + +8.1.0 (2021-03-25) +------------------ +* Remove rmw_connext_cpp references. (`#1595 `_) +* Add API for checking QoS profile compatibility (`#1554 `_) +* Document misuse of parameters callback (`#1590 `_) +* use const auto & to iterate over parameters (`#1593 `_) +* Contributors: Chris Lalancette, Jacob Perron, Karsten Knese + +8.0.0 (2021-03-23) +------------------ +* Guard against integer overflow in duration conversion (`#1584 `_) +* Contributors: Jacob Perron + +7.0.1 (2021-03-22) +------------------ +* get_parameters service should return empty if undeclared parameters are allowed (`#1514 `_) +* Made 'Context::shutdown_reason' function a const function (`#1578 `_) +* Contributors: Tomoya Fujita, suab321321 + +7.0.0 (2021-03-18) +------------------ +* Document design decisions that were made for statically typed parameters (`#1568 `_) +* Fix doc typo in CallbackGroup constructor (`#1582 `_) +* Enable qos parameter overrides for the /parameter_events topic (`#1532 `_) +* Add support for rmw_connextdds (`#1574 `_) +* Remove 'struct' from the rcl_time_jump_t. (`#1577 `_) +* Add tests for declaring statically typed parameters when undeclared parameters are allowed (`#1575 `_) +* Quiet clang memory leak warning on "DoNotOptimize". (`#1571 `_) +* Add ParameterEventsSubscriber class (`#829 `_) +* When a parameter change is rejected, the parameters map shouldn't be updated. (`#1567 `_) +* Fix when to throw the NoParameterOverrideProvided exception. (`#1567 `_) +* Fix SEGV caused by order of destruction of Node sub-interfaces (`#1469 `_) +* Fix benchmark test failure introduced in `#1522 `_ (`#1564 `_) +* Fix documented example in create_publisher (`#1558 `_) +* Enforce static parameter types (`#1522 `_) +* Allow timers to keep up the intended rate in MultiThreadedExecutor (`#1516 `_) +* Fix UBSAN warnings in any_subscription_callback. (`#1551 `_) +* Fix runtime error: reference binding to null pointer of type (`#1547 `_) +* Contributors: Andrea Sorbini, Chris Lalancette, Colin MacKenzie, Ivan Santiago Paunovic, Jacob Perron, Steven! Ragnarök, bpwilcox, tomoya + 6.3.1 (2021-02-08) ------------------ * Reference test resources directly from source tree (`#1543 `_) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 509335ad0a..b976897dec 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -5,6 +5,7 @@ project(rclcpp) find_package(Threads REQUIRED) find_package(ament_cmake_ros REQUIRED) +find_package(ament_index_cpp REQUIRED) find_package(builtin_interfaces REQUIRED) find_package(libstatistics_collector REQUIRED) find_package(rcl REQUIRED) @@ -20,9 +21,10 @@ find_package(rosidl_typesupport_cpp REQUIRED) find_package(statistics_msgs REQUIRED) find_package(tracetools REQUIRED) -# Default to C++14 +# TODO(wjwwood): remove this when gtest can build on its own, when using target_compile_features() +# Default to C++17 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD 17) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") # About -Wno-sign-conversion: With Clang, -Wconversion implies -Wsign-conversion. There are a number of @@ -40,6 +42,7 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/context.cpp src/rclcpp/contexts/default_context.cpp src/rclcpp/detail/mutex_two_priorities.cpp + src/rclcpp/detail/resolve_parameter_overrides.cpp src/rclcpp/detail/rmw_implementation_specific_payload.cpp src/rclcpp/detail/rmw_implementation_specific_publisher_payload.cpp src/rclcpp/detail/rmw_implementation_specific_subscription_payload.cpp @@ -50,7 +53,6 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/executable_list.cpp src/rclcpp/executor.cpp src/rclcpp/executors.cpp - src/rclcpp/expand_topic_or_service_name.cpp src/rclcpp/executors/events_executor.cpp src/rclcpp/executors/events_executor_entities_collector.cpp src/rclcpp/executors/multi_threaded_executor.cpp @@ -58,7 +60,10 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/executors/static_executor_entities_collector.cpp src/rclcpp/executors/static_single_threaded_executor.cpp src/rclcpp/executors/timers_manager.cpp + src/rclcpp/expand_topic_or_service_name.cpp src/rclcpp/future_return_code.cpp + src/rclcpp/generic_publisher.cpp + src/rclcpp/generic_subscription.cpp src/rclcpp/graph_listener.cpp src/rclcpp/guard_condition.cpp src/rclcpp/init_options.cpp @@ -68,8 +73,8 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/memory_strategies.cpp src/rclcpp/memory_strategy.cpp src/rclcpp/message_info.cpp + src/rclcpp/network_flow_endpoint.cpp src/rclcpp/node.cpp - src/rclcpp/node_options.cpp src/rclcpp/node_interfaces/node_base.cpp src/rclcpp/node_interfaces/node_clock.cpp src/rclcpp/node_interfaces/node_graph.cpp @@ -80,13 +85,14 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/node_interfaces/node_timers.cpp src/rclcpp/node_interfaces/node_topics.cpp src/rclcpp/node_interfaces/node_waitables.cpp + src/rclcpp/node_options.cpp src/rclcpp/parameter.cpp - src/rclcpp/parameter_value.cpp src/rclcpp/parameter_client.cpp src/rclcpp/parameter_event_handler.cpp src/rclcpp/parameter_events_filter.cpp src/rclcpp/parameter_map.cpp src/rclcpp/parameter_service.cpp + src/rclcpp/parameter_value.cpp src/rclcpp/publisher_base.cpp src/rclcpp/qos.cpp src/rclcpp/qos_event.cpp @@ -101,6 +107,7 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/time_source.cpp src/rclcpp/timer.cpp src/rclcpp/type_support.cpp + src/rclcpp/typesupport_helpers.cpp src/rclcpp/utilities.cpp src/rclcpp/wait_set_policies/detail/write_preferring_read_write_lock.cpp src/rclcpp/waitable.cpp @@ -172,8 +179,12 @@ foreach(interface_file ${interface_files}) include/rclcpp/node_interfaces/get_${interface_name}.hpp) endforeach() -add_library(${PROJECT_NAME} - ${${PROJECT_NAME}_SRCS}) +add_library(${PROJECT_NAME} ${${PROJECT_NAME}_SRCS}) +target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17) +# TODO(wjwwood): address all deprecation warnings and then remove this +if(WIN32) + target_compile_definitions(${PROJECT_NAME} PUBLIC "_SILENCE_ALL_CXX17_DEPRECATION_WARNINGS") +endif() target_include_directories(${PROJECT_NAME} PUBLIC "$" "$" @@ -181,6 +192,7 @@ target_include_directories(${PROJECT_NAME} PUBLIC target_link_libraries(${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT}) # specific order: dependents before dependencies ament_target_dependencies(${PROJECT_NAME} + "ament_index_cpp" "libstatistics_collector" "rcl" "rcl_yaml_param_parser" @@ -211,6 +223,7 @@ ament_export_include_directories(include) ament_export_libraries(${PROJECT_NAME}) ament_export_targets(${PROJECT_NAME}) +ament_export_dependencies(ament_index_cpp) ament_export_dependencies(libstatistics_collector) ament_export_dependencies(rcl) ament_export_dependencies(rcpputils) @@ -237,3 +250,8 @@ install( DIRECTORY include/ ${CMAKE_CURRENT_BINARY_DIR}/include/ DESTINATION include ) + +if(TEST cppcheck) + # must set the property after ament_package() + set_tests_properties(cppcheck PROPERTIES TIMEOUT 500) +endif() diff --git a/rclcpp/QUALITY_DECLARATION.md b/rclcpp/QUALITY_DECLARATION.md index dc95fbfcd6..e0913167c2 100644 --- a/rclcpp/QUALITY_DECLARATION.md +++ b/rclcpp/QUALITY_DECLARATION.md @@ -4,13 +4,13 @@ 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://index.ros.org/doc/ros2/Contributing/Developer-Guide/#package-quality-categories) of the ROS2 developer guide. +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. ## Version Policy [1] ### Version Scheme [1.i] -`rclcpp` uses `semver` according to the recommendation for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#versioning). +`rclcpp` uses `semver` according to the recommendation for ROS Core packages in the [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#versioning). ### Version Stability [1.ii] @@ -39,11 +39,11 @@ Headers under the folder `detail` are not considered part of the public API and ## Change Control Process [2] -`rclcpp` follows the recommended guidelines for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#change-control-process). +`rclcpp` follows the recommended guidelines for ROS Core packages in the [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#change-control-process). ### Change Requests [2.i] -All changes will occur through a pull request, check [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#change-control-process) for additional information. +All changes will occur through a pull request, check [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#change-control-process) for additional information. ### Contributor Origin [2.ii] @@ -51,7 +51,7 @@ This package uses DCO as its confirmation of contributor origin policy. More inf ### Peer Review Policy [2.iii] -All pull requests will be peer-reviewed, check [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#change-control-process) for additional information. +All pull requests will be peer-reviewed, check [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#change-control-process) for additional information. ### Continuous Integration [2.iv] @@ -111,7 +111,7 @@ The tests aim to cover both typical usage and corner cases, but are quantified b ### Coverage [4.iii] -`rclcpp` follows the recommendations for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#code-coverage), and opts to use line coverage instead of branch coverage. +`rclcpp` follows the recommendations for ROS Core packages in the [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#code-coverage), and opts to use line coverage instead of branch coverage. This includes: @@ -121,13 +121,13 @@ This includes: Changes are required to make a best effort to keep or increase coverage before being accepted, but decreases are allowed if properly justified and accepted by reviewers. -Current coverage statistics can be viewed [here](https://ci.ros2.org/job/nightly_linux_coverage/lastCompletedBuild/cobertura/src_ros2_rclcpp_rclcpp_src_rclcpp/). A description of how coverage statistics are calculated is summarized in this page ["ROS 2 Onboarding Guide"](https://index.ros.org/doc/ros2/Contributing/ROS-2-On-boarding-Guide/#note-on-coverage-runs). +Current coverage statistics can be viewed [here](https://ci.ros2.org/job/nightly_linux_coverage/lastCompletedBuild/cobertura/src_ros2_rclcpp_rclcpp_src_rclcpp/). A description of how coverage statistics are calculated is summarized in this page ["ROS 2 Onboarding Guide"](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#note-on-coverage-runs). `rclcpp` has a line coverage `>= 95%`, which is calculated over all directories within `rclcpp` with the exception of the `experimental` directory. ### Performance [4.iv] -`rclcpp` follows the recommendations for performance testing of C/C++ code in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#performance), and opts to do performance analysis on each release rather than each change. +`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). @@ -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://index.ros.org/doc/ros2/Contributing/Developer-Guide/#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 ROS2 standard linters and static analysis tools for a C++ package as described in the [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#linters-and-static-analysis). Passing implies there are no linter/static errors when testing against CI of supported platforms. Currently nightly test results can be seen here: * [linux-aarch64_release](https://ci.ros2.org/view/nightly/job/nightly_linux-aarch64_release/lastBuild/testReport/rclcpp/) diff --git a/rclcpp/doc/notes_on_statically_typed_parameters.md b/rclcpp/doc/notes_on_statically_typed_parameters.md new file mode 100644 index 0000000000..b96866c639 --- /dev/null +++ b/rclcpp/doc/notes_on_statically_typed_parameters.md @@ -0,0 +1,141 @@ +# Notes on statically typed parameters + +## Introduction + +Until ROS 2 Foxy, all parameters could change type anytime, except if the user installed a parameter callback to reject a change. +This could generate confusing errors, for example: + +``` +$ ros2 run demo_nodes_py listener & +$ ros2 param set /listener use_sim_time not_a_boolean +[ERROR] [1614712713.233733147] [listener]: use_sim_time parameter set to something besides a bool +Set parameter successful +$ ros2 param get /listener use_sim_time +String value is: not_a_boolean +``` + +For most use cases, having static parameter types is enough. +This article documents some of the decisions that were made when implementing static parameter types enforcement in: + +* https://github.com/ros2/rclcpp/pull/1522 +* https://github.com/ros2/rclpy/pull/683 + +## Allowing dynamically typed parameters + +There might be some scenarios where dynamic typing is desired, so a `dynamic_typing` field was added to the [parameter descriptor](https://github.com/ros2/rcl_interfaces/blob/09b5ed93a733adb9deddc47f9a4a8c6e9f584667/rcl_interfaces/msg/ParameterDescriptor.msg#L25). +It defaults to `false`. + +For example: + +```cpp +rcl_interfaces::msg::ParameterDescriptor descriptor; +descriptor.dynamic_typing = true; + +node->declare_parameter("dynamically_typed_parameter", rclcpp::ParameterValue{}, descriptor); +``` + +```py +rcl_interfaces.msg.ParameterDescriptor descriptor; +descriptor.dynamic_typing = True; + +node.declare_parameter("dynamically_typed_parameter", None, descriptor); +``` + +## How is the parameter type specified? + +The parameter type will be inferred from the default value provided when declaring it. + +## Statically typed parameters when allowing undeclared parameters + +When undeclared parameters are allowed and a parameter is set without a previous declaration, the parameter will be dynamically typed. +This is consistent with other allowed behaviors when undeclared parameters are allowed, e.g. trying to get an undeclared parameter returns "NOT_SET". +Parameter declarations will remain special and dynamic or static typing will be used based on the parameter descriptor (default to static). + +## Declaring a parameter without a default value + +There might be cases were a default value does not make sense and the user must always provide an override. +In those cases, the parameter type can be specified explicitly: + +```cpp +// method signature +template +Node::declare_parameter(std::string name, rcl_interfaces::msg::ParameterDescriptor = rcl_interfaces::msg::ParameterDescriptor{}); +// or alternatively +Node::declare_parameter(std::string name, rclcpp::ParameterType type, rcl_interfaces::msg::ParameterDescriptor = rcl_interfaces::msg::ParameterDescriptor{}); + +// examples +node->declare_parameter("my_integer_parameter"); // declare an integer parameter +node->declare_parameter("another_integer_parameter", rclcpp::ParameterType::PARAMETER_INTEGER); // another way to do the same +``` + +```py +# method signature +Node.declare_parameter(name: str, param_type: rclpy.Parameter.Type, descriptor: rcl_interfaces.msg.ParameterDescriptor = rcl_interfaces.msg.ParameterDescriptor()) + +# example +node.declare_parameter('my_integer_parameter', rclpy.Parameter.Type.INTEGER); # declare an integer parameter +``` + +If the parameter may be unused, but when used requires a parameter override, then you could conditionally declare it: + +```cpp +auto mode = node->declare_parameter("mode", "modeA"); // "mode" parameter is an string +if (mode == "modeB") { + node->declare_parameter("param_needed_when_mode_b"); // when "modeB", the user must provide "param_needed_when_mode_b" +} +``` + +## Other migration notes + +Declaring a parameter with only a name is deprecated: + +```cpp +node->declare_parameter("my_param"); // this generates a build warning +``` + +```py +node.declare_parameter("my_param"); # this generates a python user warning +``` + +Before, you could initialize a parameter with the "NOT SET" value (in cpp `rclcpp::ParameterValue{}`, in python `None`). +Now this will throw an exception in both cases: + +```cpp +node->declare_parameter("my_param", rclcpp::ParameterValue{}); // not valid, will throw exception +``` + +```py +node.declare_parameter("my_param", None); # not valid, will raise error +``` + +## Possible improvements + +### Easier way to declare dynamically typed parameters + +Declaring a dynamically typed parameter in `rclcpp` could be considered to be a bit verbose: + +```cpp +rcl_interfaces::msg::ParameterDescriptor descriptor; +descriptor.dynamic_typing = true; + +node->declare_parameter(name, rclcpp::ParameterValue{}, descriptor); +``` + +the following ways could be supported to make it simpler: + +```cpp +node->declare_parameter(name, rclcpp::PARAMETER_DYNAMIC); +node->declare_parameter(name, default_value, rclcpp::PARAMETER_DYNAMIC); +``` + +or alternatively: + +```cpp +node->declare_parameter(name, default_value, rclcpp::ParameterDescriptor{}.dynamic_typing()); +``` + +For `rclpy`, there's already a short way to do it: + +```py +node.declare_parameter(name, default_value, rclpy.ParameterDescriptor(dynamic_typing=true)); +``` diff --git a/rclcpp/include/rclcpp/any_executable.hpp b/rclcpp/include/rclcpp/any_executable.hpp index ed01d45bc1..5d4064f452 100644 --- a/rclcpp/include/rclcpp/any_executable.hpp +++ b/rclcpp/include/rclcpp/any_executable.hpp @@ -50,12 +50,6 @@ struct AnyExecutable std::shared_ptr data; }; -namespace executor -{ - -using AnyExecutable [[deprecated("use rclcpp::AnyExecutable instead")]] = AnyExecutable; - -} // namespace executor } // namespace rclcpp #endif // RCLCPP__ANY_EXECUTABLE_HPP_ diff --git a/rclcpp/include/rclcpp/any_service_callback.hpp b/rclcpp/include/rclcpp/any_service_callback.hpp index 86d420860d..78c3ed0052 100644 --- a/rclcpp/include/rclcpp/any_service_callback.hpp +++ b/rclcpp/include/rclcpp/any_service_callback.hpp @@ -107,12 +107,12 @@ class AnyServiceCallback TRACEPOINT( rclcpp_callback_register, static_cast(this), - get_symbol(shared_ptr_callback_)); + tracetools::get_symbol(shared_ptr_callback_)); } else if (shared_ptr_with_request_header_callback_) { TRACEPOINT( rclcpp_callback_register, static_cast(this), - get_symbol(shared_ptr_with_request_header_callback_)); + tracetools::get_symbol(shared_ptr_with_request_header_callback_)); } #endif // TRACETOOLS_DISABLED } diff --git a/rclcpp/include/rclcpp/any_subscription_callback.hpp b/rclcpp/include/rclcpp/any_subscription_callback.hpp index 124215bdca..bb45d6a07d 100644 --- a/rclcpp/include/rclcpp/any_subscription_callback.hpp +++ b/rclcpp/include/rclcpp/any_subscription_callback.hpp @@ -15,269 +15,919 @@ #ifndef RCLCPP__ANY_SUBSCRIPTION_CALLBACK_HPP_ #define RCLCPP__ANY_SUBSCRIPTION_CALLBACK_HPP_ -#include - #include #include #include #include #include +#include // NOLINT[build/include_order] + +#include "rosidl_runtime_cpp/traits.hpp" +#include "tracetools/tracetools.h" +#include "tracetools/utils.hpp" #include "rclcpp/allocator/allocator_common.hpp" +#include "rclcpp/detail/subscription_callback_type_helper.hpp" #include "rclcpp/function_traits.hpp" #include "rclcpp/message_info.hpp" -#include "rclcpp/visibility_control.hpp" -#include "tracetools/tracetools.h" -#include "tracetools/utils.hpp" +#include "rclcpp/serialized_message.hpp" +#include "rclcpp/type_adapter.hpp" + + +template +inline constexpr bool always_false_v = false; namespace rclcpp { -template -class AnySubscriptionCallback +namespace detail +{ + +template +struct MessageDeleterHelper { - using MessageAllocTraits = allocator::AllocRebind; - using MessageAlloc = typename MessageAllocTraits::allocator_type; - using MessageDeleter = allocator::Deleter; - using ConstMessageSharedPtr = std::shared_ptr; - using MessageUniquePtr = std::unique_ptr; + using AllocTraits = allocator::AllocRebind; + using Alloc = typename AllocTraits::allocator_type; + using Deleter = allocator::Deleter; +}; - using SharedPtrCallback = std::function)>; +/// Struct which contains all possible callback signatures, with or without a TypeAdapter.s +template +struct AnySubscriptionCallbackPossibleTypes +{ + /// MessageT::custom_type if MessageT is a TypeAdapter, otherwise just MessageT. + using SubscribedType = typename rclcpp::TypeAdapter::custom_type; + /// MessageT::ros_message_type if MessageT is a TypeAdapter, otherwise just MessageT. + using ROSMessageType = typename rclcpp::TypeAdapter::ros_message_type; + + using SubscribedMessageDeleter = + typename MessageDeleterHelper::Deleter; + using ROSMessageDeleter = + typename MessageDeleterHelper::Deleter; + using SerializedMessageDeleter = + typename MessageDeleterHelper::Deleter; + + using ConstRefCallback = + std::function; + using ConstRefROSMessageCallback = + std::function; + using ConstRefWithInfoCallback = + std::function; + using ConstRefWithInfoROSMessageCallback = + std::function; + using ConstRefSerializedMessageCallback = + std::function; + using ConstRefSerializedMessageWithInfoCallback = + std::function; + + using UniquePtrCallback = + std::function)>; + using UniquePtrROSMessageCallback = + std::function)>; + using UniquePtrWithInfoCallback = + std::function, + const rclcpp::MessageInfo &)>; + using UniquePtrWithInfoROSMessageCallback = + std::function, + const rclcpp::MessageInfo &)>; + using UniquePtrSerializedMessageCallback = + std::function)>; + using UniquePtrSerializedMessageWithInfoCallback = + std::function, + const rclcpp::MessageInfo &)>; + + using SharedConstPtrCallback = + std::function)>; + using SharedConstPtrROSMessageCallback = + std::function)>; + using SharedConstPtrWithInfoCallback = + std::function, + const rclcpp::MessageInfo &)>; + using SharedConstPtrWithInfoROSMessageCallback = + std::function, + const rclcpp::MessageInfo &)>; + using SharedConstPtrSerializedMessageCallback = + std::function)>; + using SharedConstPtrSerializedMessageWithInfoCallback = + std::function, + const rclcpp::MessageInfo &)>; + + using ConstRefSharedConstPtrCallback = + std::function &)>; + using ConstRefSharedConstPtrROSMessageCallback = + std::function &)>; + using ConstRefSharedConstPtrWithInfoCallback = + std::function &, + const rclcpp::MessageInfo &)>; + using ConstRefSharedConstPtrWithInfoROSMessageCallback = + std::function &, + const rclcpp::MessageInfo &)>; + using ConstRefSharedConstPtrSerializedMessageCallback = + std::function &)>; + using ConstRefSharedConstPtrSerializedMessageWithInfoCallback = + std::function &, + const rclcpp::MessageInfo &)>; + + // Deprecated signatures: + using SharedPtrCallback = + std::function)>; + using SharedPtrROSMessageCallback = + std::function)>; using SharedPtrWithInfoCallback = - std::function, const rclcpp::MessageInfo &)>; - using ConstSharedPtrCallback = std::function)>; - using ConstSharedPtrWithInfoCallback = - std::function, const rclcpp::MessageInfo &)>; - using UniquePtrCallback = std::function; + std::function, const rclcpp::MessageInfo &)>; + using SharedPtrWithInfoROSMessageCallback = + std::function, + const rclcpp::MessageInfo &)>; + using SharedPtrSerializedMessageCallback = + std::function)>; + using SharedPtrSerializedMessageWithInfoCallback = + std::function, const rclcpp::MessageInfo &)>; +}; + +/// Template helper to select the variant type based on whether or not MessageT is a TypeAdapter. +template< + typename MessageT, + typename AllocatorT, + bool is_adapted_type = rclcpp::TypeAdapter::is_specialized::value +> +struct AnySubscriptionCallbackHelper; + +/// Specialization for when MessageT is not a TypeAdapter. +template +struct AnySubscriptionCallbackHelper +{ + using CallbackTypes = AnySubscriptionCallbackPossibleTypes; + + using variant_type = std::variant< + typename CallbackTypes::ConstRefCallback, + typename CallbackTypes::ConstRefWithInfoCallback, + typename CallbackTypes::ConstRefSerializedMessageCallback, + typename CallbackTypes::ConstRefSerializedMessageWithInfoCallback, + typename CallbackTypes::UniquePtrCallback, + typename CallbackTypes::UniquePtrWithInfoCallback, + typename CallbackTypes::UniquePtrSerializedMessageCallback, + typename CallbackTypes::UniquePtrSerializedMessageWithInfoCallback, + typename CallbackTypes::SharedConstPtrCallback, + typename CallbackTypes::SharedConstPtrWithInfoCallback, + typename CallbackTypes::SharedConstPtrSerializedMessageCallback, + typename CallbackTypes::SharedConstPtrSerializedMessageWithInfoCallback, + typename CallbackTypes::ConstRefSharedConstPtrCallback, + typename CallbackTypes::ConstRefSharedConstPtrWithInfoCallback, + typename CallbackTypes::ConstRefSharedConstPtrSerializedMessageCallback, + typename CallbackTypes::ConstRefSharedConstPtrSerializedMessageWithInfoCallback, + typename CallbackTypes::SharedPtrCallback, + typename CallbackTypes::SharedPtrWithInfoCallback, + typename CallbackTypes::SharedPtrSerializedMessageCallback, + typename CallbackTypes::SharedPtrSerializedMessageWithInfoCallback + >; +}; + +/// Specialization for when MessageT is a TypeAdapter. +template +struct AnySubscriptionCallbackHelper +{ + using CallbackTypes = AnySubscriptionCallbackPossibleTypes; + + using variant_type = std::variant< + typename CallbackTypes::ConstRefCallback, + typename CallbackTypes::ConstRefROSMessageCallback, + typename CallbackTypes::ConstRefWithInfoCallback, + typename CallbackTypes::ConstRefWithInfoROSMessageCallback, + typename CallbackTypes::ConstRefSerializedMessageCallback, + typename CallbackTypes::ConstRefSerializedMessageWithInfoCallback, + typename CallbackTypes::UniquePtrCallback, + typename CallbackTypes::UniquePtrROSMessageCallback, + typename CallbackTypes::UniquePtrWithInfoCallback, + typename CallbackTypes::UniquePtrWithInfoROSMessageCallback, + typename CallbackTypes::UniquePtrSerializedMessageCallback, + typename CallbackTypes::UniquePtrSerializedMessageWithInfoCallback, + typename CallbackTypes::SharedConstPtrCallback, + typename CallbackTypes::SharedConstPtrROSMessageCallback, + typename CallbackTypes::SharedConstPtrWithInfoCallback, + typename CallbackTypes::SharedConstPtrWithInfoROSMessageCallback, + typename CallbackTypes::SharedConstPtrSerializedMessageCallback, + typename CallbackTypes::SharedConstPtrSerializedMessageWithInfoCallback, + typename CallbackTypes::ConstRefSharedConstPtrCallback, + typename CallbackTypes::ConstRefSharedConstPtrROSMessageCallback, + typename CallbackTypes::ConstRefSharedConstPtrWithInfoCallback, + typename CallbackTypes::ConstRefSharedConstPtrWithInfoROSMessageCallback, + typename CallbackTypes::ConstRefSharedConstPtrSerializedMessageCallback, + typename CallbackTypes::ConstRefSharedConstPtrSerializedMessageWithInfoCallback, + typename CallbackTypes::SharedPtrCallback, + typename CallbackTypes::SharedPtrROSMessageCallback, + typename CallbackTypes::SharedPtrWithInfoCallback, + typename CallbackTypes::SharedPtrWithInfoROSMessageCallback, + typename CallbackTypes::SharedPtrSerializedMessageCallback, + typename CallbackTypes::SharedPtrSerializedMessageWithInfoCallback + >; +}; + +} // namespace detail + +template< + typename MessageT, + typename AllocatorT = std::allocator +> +class AnySubscriptionCallback +{ +private: + /// MessageT::custom_type if MessageT is a TypeAdapter, otherwise just MessageT. + using SubscribedType = typename rclcpp::TypeAdapter::custom_type; + /// MessageT::ros_message_type if MessageT is a TypeAdapter, otherwise just MessageT. + using ROSMessageType = typename rclcpp::TypeAdapter::ros_message_type; + + using HelperT = typename rclcpp::detail::AnySubscriptionCallbackHelper; + + using SubscribedTypeDeleterHelper = + rclcpp::detail::MessageDeleterHelper; + using SubscribedTypeAllocatorTraits = typename SubscribedTypeDeleterHelper::AllocTraits; + using SubscribedTypeAllocator = typename SubscribedTypeDeleterHelper::Alloc; + using SubscribedTypeDeleter = typename SubscribedTypeDeleterHelper::Deleter; + + using ROSMessageTypeDeleterHelper = + rclcpp::detail::MessageDeleterHelper; + using ROSMessageTypeAllocatorTraits = typename ROSMessageTypeDeleterHelper::AllocTraits; + using ROSMessageTypeAllocator = typename ROSMessageTypeDeleterHelper::Alloc; + using ROSMessageTypeDeleter = typename ROSMessageTypeDeleterHelper::Deleter; + + using SerializedMessageDeleterHelper = + rclcpp::detail::MessageDeleterHelper; + using SerializedMessageAllocatorTraits = typename SerializedMessageDeleterHelper::AllocTraits; + using SerializedMessageAllocator = typename SerializedMessageDeleterHelper::Alloc; + using SerializedMessageDeleter = typename SerializedMessageDeleterHelper::Deleter; + + // See AnySubscriptionCallbackPossibleTypes for the types of these. + using CallbackTypes = detail::AnySubscriptionCallbackPossibleTypes; + + using ConstRefCallback = + typename CallbackTypes::ConstRefCallback; + using ConstRefROSMessageCallback = + typename CallbackTypes::ConstRefROSMessageCallback; + using ConstRefWithInfoCallback = + typename CallbackTypes::ConstRefWithInfoCallback; + using ConstRefWithInfoROSMessageCallback = + typename CallbackTypes::ConstRefWithInfoROSMessageCallback; + using ConstRefSerializedMessageCallback = + typename CallbackTypes::ConstRefSerializedMessageCallback; + using ConstRefSerializedMessageWithInfoCallback = + typename CallbackTypes::ConstRefSerializedMessageWithInfoCallback; + using UniquePtrCallback = + typename CallbackTypes::UniquePtrCallback; + using UniquePtrROSMessageCallback = + typename CallbackTypes::UniquePtrROSMessageCallback; using UniquePtrWithInfoCallback = - std::function; + typename CallbackTypes::UniquePtrWithInfoCallback; + using UniquePtrWithInfoROSMessageCallback = + typename CallbackTypes::UniquePtrWithInfoROSMessageCallback; + using UniquePtrSerializedMessageCallback = + typename CallbackTypes::UniquePtrSerializedMessageCallback; + using UniquePtrSerializedMessageWithInfoCallback = + typename CallbackTypes::UniquePtrSerializedMessageWithInfoCallback; + using SharedConstPtrCallback = + typename CallbackTypes::SharedConstPtrCallback; + using SharedConstPtrROSMessageCallback = + typename CallbackTypes::SharedConstPtrROSMessageCallback; + using SharedConstPtrWithInfoCallback = + typename CallbackTypes::SharedConstPtrWithInfoCallback; + using SharedConstPtrWithInfoROSMessageCallback = + typename CallbackTypes::SharedConstPtrWithInfoROSMessageCallback; + using SharedConstPtrSerializedMessageCallback = + typename CallbackTypes::SharedConstPtrSerializedMessageCallback; + using SharedConstPtrSerializedMessageWithInfoCallback = + typename CallbackTypes::SharedConstPtrSerializedMessageWithInfoCallback; + using ConstRefSharedConstPtrCallback = + typename CallbackTypes::ConstRefSharedConstPtrCallback; + using ConstRefSharedConstPtrROSMessageCallback = + typename CallbackTypes::ConstRefSharedConstPtrROSMessageCallback; + using ConstRefSharedConstPtrWithInfoCallback = + typename CallbackTypes::ConstRefSharedConstPtrWithInfoCallback; + using ConstRefSharedConstPtrWithInfoROSMessageCallback = + typename CallbackTypes::ConstRefSharedConstPtrWithInfoROSMessageCallback; + using ConstRefSharedConstPtrSerializedMessageCallback = + typename CallbackTypes::ConstRefSharedConstPtrSerializedMessageCallback; + using ConstRefSharedConstPtrSerializedMessageWithInfoCallback = + typename CallbackTypes::ConstRefSharedConstPtrSerializedMessageWithInfoCallback; + using SharedPtrCallback = + typename CallbackTypes::SharedPtrCallback; + using SharedPtrROSMessageCallback = + typename CallbackTypes::SharedPtrROSMessageCallback; + using SharedPtrWithInfoCallback = + typename CallbackTypes::SharedPtrWithInfoCallback; + using SharedPtrWithInfoROSMessageCallback = + typename CallbackTypes::SharedPtrWithInfoROSMessageCallback; + using SharedPtrSerializedMessageCallback = + typename CallbackTypes::SharedPtrSerializedMessageCallback; + using SharedPtrSerializedMessageWithInfoCallback = + typename CallbackTypes::SharedPtrSerializedMessageWithInfoCallback; + + template + struct NotNull + { + NotNull(const T * pointer_in, const char * msg) + : pointer(pointer_in) + { + if (pointer == nullptr) { + throw std::invalid_argument(msg); + } + } - SharedPtrCallback shared_ptr_callback_; - SharedPtrWithInfoCallback shared_ptr_with_info_callback_; - ConstSharedPtrCallback const_shared_ptr_callback_; - ConstSharedPtrWithInfoCallback const_shared_ptr_with_info_callback_; - UniquePtrCallback unique_ptr_callback_; - UniquePtrWithInfoCallback unique_ptr_with_info_callback_; + const T * pointer; + }; public: - explicit AnySubscriptionCallback(std::shared_ptr allocator) - : shared_ptr_callback_(nullptr), shared_ptr_with_info_callback_(nullptr), - const_shared_ptr_callback_(nullptr), const_shared_ptr_with_info_callback_(nullptr), - unique_ptr_callback_(nullptr), unique_ptr_with_info_callback_(nullptr) + explicit + AnySubscriptionCallback(const AllocatorT & allocator = AllocatorT()) // NOLINT[runtime/explicit] + : subscribed_type_allocator_(allocator), + ros_message_type_allocator_(allocator) { - if (allocator == nullptr) { - throw std::runtime_error("invalid allocator"); - } - message_allocator_ = std::make_shared(*allocator.get()); - allocator::set_allocator_for_deleter(&message_deleter_, message_allocator_.get()); + allocator::set_allocator_for_deleter(&subscribed_type_deleter_, &subscribed_type_allocator_); + allocator::set_allocator_for_deleter(&ros_message_type_deleter_, &ros_message_type_allocator_); } + [[deprecated("use AnySubscriptionCallback(const AllocatorT & allocator) instead")]] + explicit + AnySubscriptionCallback(std::shared_ptr allocator) // NOLINT[runtime/explicit] + : AnySubscriptionCallback(*NotNull(allocator.get(), "invalid allocator").pointer) + {} + AnySubscriptionCallback(const AnySubscriptionCallback &) = default; - template< - typename CallbackT, - typename std::enable_if< + /// Generic function for setting the callback. + /** + * There are specializations that overload this in order to deprecate some + * callback signatures, and also to fix ambiguity between shared_ptr and + * unique_ptr callback signatures when using them with lambda functions. + */ + template + AnySubscriptionCallback + set(CallbackT callback) + { + // Use the SubscriptionCallbackTypeHelper to determine the actual type of + // the CallbackT, in terms of std::function<...>, which does not happen + // automatically with lambda functions in cases where the arguments can be + // converted to one another, e.g. shared_ptr and unique_ptr. + using scbth = detail::SubscriptionCallbackTypeHelper; + + // Determine if the given CallbackT is a deprecated signature or not. + constexpr auto is_deprecated = rclcpp::function_traits::same_arguments< - CallbackT, - SharedPtrCallback + typename scbth::callback_type, + std::function)> >::value - >::type * = nullptr - > - void set(CallbackT callback) + || + 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); + } + + // Return copy of self for easier testing, normally will be compiled out. + return *this; + } + + /// Function for shared_ptr to non-const MessageT, which is deprecated. + template + // TODO(wjwwood): enable this deprecation after Galactic + // [[deprecated( + // "use 'void (std::shared_ptr)' instead" + // )]] + void + set_deprecated(std::function)> callback) { - shared_ptr_callback_ = callback; + callback_variant_ = callback; } - template< - typename CallbackT, - typename std::enable_if< - rclcpp::function_traits::same_arguments< - CallbackT, - SharedPtrWithInfoCallback - >::value - >::type * = nullptr - > - void set(CallbackT callback) + /// Function for shared_ptr to non-const MessageT with MessageInfo, which is deprecated. + template + // TODO(wjwwood): enable this deprecation after Galactic + // [[deprecated( + // "use 'void (std::shared_ptr, const rclcpp::MessageInfo &)' instead" + // )]] + void + set_deprecated(std::function, const rclcpp::MessageInfo &)> callback) { - shared_ptr_with_info_callback_ = callback; + callback_variant_ = callback; } - template< - typename CallbackT, - typename std::enable_if< - rclcpp::function_traits::same_arguments< - CallbackT, - ConstSharedPtrCallback - >::value - >::type * = nullptr - > - void set(CallbackT callback) + std::unique_ptr + create_ros_unique_ptr_from_ros_shared_ptr_message( + const std::shared_ptr & message) { - const_shared_ptr_callback_ = callback; + auto ptr = ROSMessageTypeAllocatorTraits::allocate(ros_message_type_allocator_, 1); + ROSMessageTypeAllocatorTraits::construct(ros_message_type_allocator_, ptr, *message); + return std::unique_ptr(ptr, ros_message_type_deleter_); } - template< - typename CallbackT, - typename std::enable_if< - rclcpp::function_traits::same_arguments< - CallbackT, - ConstSharedPtrWithInfoCallback - >::value - >::type * = nullptr - > - void set(CallbackT callback) + std::unique_ptr + create_serialized_message_unique_ptr_from_shared_ptr( + const std::shared_ptr & serialized_message) { - const_shared_ptr_with_info_callback_ = callback; + auto ptr = SerializedMessageAllocatorTraits::allocate(serialized_message_allocator_, 1); + SerializedMessageAllocatorTraits::construct( + serialized_message_allocator_, ptr, *serialized_message); + return std::unique_ptr< + rclcpp::SerializedMessage, + SerializedMessageDeleter + >(ptr, serialized_message_deleter_); } - template< - typename CallbackT, - typename std::enable_if< - rclcpp::function_traits::same_arguments< - CallbackT, - UniquePtrCallback - >::value - >::type * = nullptr - > - void set(CallbackT callback) + std::unique_ptr + create_custom_unique_ptr_from_custom_shared_ptr_message( + const std::shared_ptr & message) { - unique_ptr_callback_ = callback; + auto ptr = SubscribedTypeAllocatorTraits::allocate(subscribed_type_allocator_, 1); + SubscribedTypeAllocatorTraits::construct(subscribed_type_allocator_, ptr, *message); + return std::unique_ptr(ptr, subscribed_type_deleter_); } - template< - typename CallbackT, - typename std::enable_if< - rclcpp::function_traits::same_arguments< - CallbackT, - UniquePtrWithInfoCallback - >::value - >::type * = nullptr - > - void set(CallbackT callback) + std::unique_ptr + convert_ros_message_to_custom_type_unique_ptr(const ROSMessageType & msg) { - unique_ptr_with_info_callback_ = callback; + if constexpr (rclcpp::TypeAdapter::is_specialized::value) { + auto ptr = SubscribedTypeAllocatorTraits::allocate(subscribed_type_allocator_, 1); + SubscribedTypeAllocatorTraits::construct(subscribed_type_allocator_, ptr); + rclcpp::TypeAdapter::convert_to_custom(msg, *ptr); + return std::unique_ptr(ptr, subscribed_type_deleter_); + } else { + throw std::runtime_error( + "convert_ros_message_to_custom_type_unique_ptr " + "unexpectedly called without TypeAdapter"); + } } - void dispatch( - std::shared_ptr message, const rclcpp::MessageInfo & message_info) + // Dispatch when input is a ros message and the output could be anything. + void + dispatch( + std::shared_ptr message, + const rclcpp::MessageInfo & message_info) { TRACEPOINT(callback_start, static_cast(this), false); - if (shared_ptr_callback_) { - shared_ptr_callback_(message); - } else if (shared_ptr_with_info_callback_) { - shared_ptr_with_info_callback_(message, message_info); - } else if (const_shared_ptr_callback_) { - const_shared_ptr_callback_(message); - } else if (const_shared_ptr_with_info_callback_) { - const_shared_ptr_with_info_callback_(message, message_info); - } else if (unique_ptr_callback_) { - auto ptr = MessageAllocTraits::allocate(*message_allocator_.get(), 1); - MessageAllocTraits::construct(*message_allocator_.get(), ptr, *message); - unique_ptr_callback_(MessageUniquePtr(ptr, message_deleter_)); - } else if (unique_ptr_with_info_callback_) { - auto ptr = MessageAllocTraits::allocate(*message_allocator_.get(), 1); - MessageAllocTraits::construct(*message_allocator_.get(), ptr, *message); - unique_ptr_with_info_callback_(MessageUniquePtr(ptr, message_deleter_), message_info); - } else { - throw std::runtime_error("unexpected message without any callback set"); + // Check if the variant is "unset", throw if it is. + if (callback_variant_.index() == 0) { + if (std::get<0>(callback_variant_) == nullptr) { + // This can happen if it is default initialized, or if it is assigned nullptr. + throw std::runtime_error("dispatch called on an unset AnySubscriptionCallback"); + } } + // Dispatch. + std::visit( + [&message, &message_info, this](auto && callback) { + using T = std::decay_t; + static constexpr bool is_ta = rclcpp::TypeAdapter::is_specialized::value; + + // conditions for output is custom message + if constexpr (is_ta && std::is_same_v) { + // TODO(wjwwood): consider avoiding heap allocation for small messages + // maybe something like: + // if constexpr (rosidl_generator_traits::has_fixed_size && sizeof(T) < N) { + // ... on stack + // } + auto local_message = convert_ros_message_to_custom_type_unique_ptr(*message); + callback(*local_message); + } else if constexpr (is_ta && std::is_same_v) { // NOLINT + auto local_message = convert_ros_message_to_custom_type_unique_ptr(*message); + callback(*local_message, message_info); + } else if constexpr (is_ta && std::is_same_v) { + callback(convert_ros_message_to_custom_type_unique_ptr(*message)); + } else if constexpr (is_ta && std::is_same_v) { + callback(convert_ros_message_to_custom_type_unique_ptr(*message), message_info); + } else if constexpr ( // NOLINT[readability/braces] + is_ta && ( + std::is_same_v|| + std::is_same_v|| + std::is_same_v + )) + { + callback(convert_ros_message_to_custom_type_unique_ptr(*message)); + } else if constexpr ( // NOLINT[readability/braces] + is_ta && ( + std::is_same_v|| + std::is_same_v|| + std::is_same_v + )) + { + callback(convert_ros_message_to_custom_type_unique_ptr(*message), message_info); + } + // conditions for output is ros message + else if constexpr (std::is_same_v) { // NOLINT + callback(*message); + } else if constexpr (std::is_same_v) { + callback(*message, message_info); + } else if constexpr (std::is_same_v) { + callback(create_ros_unique_ptr_from_ros_shared_ptr_message(message)); + } else if constexpr (std::is_same_v) { + callback(create_ros_unique_ptr_from_ros_shared_ptr_message(message), message_info); + } else if constexpr ( // NOLINT[readability/braces] + std::is_same_v|| + std::is_same_v|| + std::is_same_v) + { + callback(message); + } else if constexpr ( // NOLINT[readability/braces] + std::is_same_v|| + std::is_same_v|| + std::is_same_v) + { + callback(message, message_info); + } + // condition to catch SerializedMessage types + else if constexpr ( // NOLINT[readability/braces] + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v) + { + throw std::runtime_error( + "Cannot dispatch std::shared_ptr message " + "to rclcpp::SerializedMessage"); + } + // condition to catch unhandled callback types + else { // NOLINT[readability/braces] + static_assert(always_false_v, "unhandled callback type"); + } + }, callback_variant_); TRACEPOINT(callback_end, static_cast(this)); } - void dispatch_intra_process( - ConstMessageSharedPtr message, const rclcpp::MessageInfo & message_info) + // Dispatch when input is a serialized message and the output could be anything. + void + dispatch( + std::shared_ptr serialized_message, + const rclcpp::MessageInfo & message_info) { - TRACEPOINT(callback_start, static_cast(this), true); - - // If the message is not valid, return. - if (!message) { - return; + TRACEPOINT(callback_start, static_cast(this), false); + // Check if the variant is "unset", throw if it is. + if (callback_variant_.index() == 0) { + if (std::get<0>(callback_variant_) == nullptr) { + // This can happen if it is default initialized, or if it is assigned nullptr. + throw std::runtime_error("dispatch called on an unset AnySubscriptionCallback"); + } } + // Dispatch. + std::visit( + [&serialized_message, &message_info, this](auto && callback) { + using T = std::decay_t; - if (const_shared_ptr_callback_) { - const_shared_ptr_callback_(message); - } else if (const_shared_ptr_with_info_callback_) { - const_shared_ptr_with_info_callback_(message, message_info); - } else { - if ( - unique_ptr_callback_ || unique_ptr_with_info_callback_ || - shared_ptr_callback_ || shared_ptr_with_info_callback_) - { - throw std::runtime_error( - "unexpected dispatch_intra_process const shared " - "message call with no const shared_ptr callback"); - } else { - throw std::runtime_error("unexpected message without any callback set"); + // condition to catch SerializedMessage types + if constexpr (std::is_same_v) { + callback(*serialized_message); + } else if constexpr (std::is_same_v) { + callback(*serialized_message, message_info); + } else if constexpr (std::is_same_v) { + callback(create_serialized_message_unique_ptr_from_shared_ptr(serialized_message)); + } else if constexpr (std::is_same_v) { + callback( + create_serialized_message_unique_ptr_from_shared_ptr(serialized_message), + message_info); + } else if constexpr ( // NOLINT[readability/braces] + std::is_same_v|| + std::is_same_v|| + std::is_same_v) + { + callback(create_serialized_message_unique_ptr_from_shared_ptr(serialized_message)); + } else if constexpr ( // NOLINT[readability/braces] + std::is_same_v|| + std::is_same_v|| + std::is_same_v) + { + callback( + create_serialized_message_unique_ptr_from_shared_ptr(serialized_message), + message_info); + } + // conditions for output anything else + else if constexpr ( // NOLINT[whitespace/newline] + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v) + { + throw std::runtime_error( + "cannot dispatch rclcpp::SerializedMessage to " + "non-rclcpp::SerializedMessage callbacks"); + } + // condition to catch unhandled callback types + else { // NOLINT[readability/braces] + static_assert(always_false_v, "unhandled callback type"); + } + }, callback_variant_); + TRACEPOINT(callback_end, static_cast(this)); + } + + void + dispatch_intra_process( + std::shared_ptr message, + const rclcpp::MessageInfo & message_info) + { + TRACEPOINT(callback_start, static_cast(this), true); + // Check if the variant is "unset", throw if it is. + if (callback_variant_.index() == 0) { + if (std::get<0>(callback_variant_) == nullptr) { + // This can happen if it is default initialized, or if it is assigned nullptr. + throw std::runtime_error("dispatch called on an unset AnySubscriptionCallback"); } } + // Dispatch. + std::visit( + [&message, &message_info, this](auto && callback) { + using T = std::decay_t; + static constexpr bool is_ta = rclcpp::TypeAdapter::is_specialized::value; + + // conditions for custom type + if constexpr (is_ta && std::is_same_v) { + auto local_message = convert_ros_message_to_custom_type_unique_ptr(*message); + callback(*local_message); + } else if constexpr (is_ta && std::is_same_v) { // NOLINT + auto local_message = convert_ros_message_to_custom_type_unique_ptr(*message); + callback(*local_message, message_info); + } else if constexpr ( // NOLINT[readability/braces] + is_ta && ( + std::is_same_v|| + std::is_same_v + )) + { + callback(convert_ros_message_to_custom_type_unique_ptr(*message)); + } else if constexpr ( // NOLINT[readability/braces] + is_ta && ( + std::is_same_v|| + std::is_same_v + )) + { + callback(convert_ros_message_to_custom_type_unique_ptr(*message), message_info); + } else if constexpr ( // NOLINT[readability/braces] + is_ta && ( + std::is_same_v|| + std::is_same_v + )) + { + callback(convert_ros_message_to_custom_type_unique_ptr(*message)); + } else if constexpr ( // NOLINT[readability/braces] + is_ta && ( + std::is_same_v|| + std::is_same_v + )) + { + callback(convert_ros_message_to_custom_type_unique_ptr(*message), message_info); + } + // conditions for ros message type + else if constexpr (std::is_same_v) { // NOLINT + callback(*message); + } else if constexpr (std::is_same_v) { + callback(*message, message_info); + } else if constexpr ( // NOLINT[readability/braces] + std::is_same_v|| + std::is_same_v) + { + callback(create_ros_unique_ptr_from_ros_shared_ptr_message(message)); + } else if constexpr ( // NOLINT[readability/braces] + std::is_same_v|| + std::is_same_v) + { + callback(create_ros_unique_ptr_from_ros_shared_ptr_message(message), message_info); + } else if constexpr ( // NOLINT[readability/braces] + std::is_same_v|| + std::is_same_v) + { + callback(message); + } else if constexpr ( // NOLINT[readability/braces] + std::is_same_v|| + std::is_same_v) + { + callback(message, message_info); + } + // condition to catch SerializedMessage types + else if constexpr ( // NOLINT[readability/braces] + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v) + { + throw std::runtime_error( + "Cannot dispatch std::shared_ptr message " + "to rclcpp::SerializedMessage"); + } + // condition to catch unhandled callback types + else { // NOLINT[readability/braces] + static_assert(always_false_v, "unhandled callback type"); + } + }, callback_variant_); TRACEPOINT(callback_end, static_cast(this)); } - void dispatch_intra_process( - MessageUniquePtr message, const rclcpp::MessageInfo & message_info) + void + dispatch_intra_process( + std::unique_ptr message, + const rclcpp::MessageInfo & message_info) { TRACEPOINT(callback_start, static_cast(this), true); - - // If the message is not valid, return. - if (!message) { - return; + // Check if the variant is "unset", throw if it is. + if (callback_variant_.index() == 0) { + if (std::get<0>(callback_variant_) == nullptr) { + // This can happen if it is default initialized, or if it is assigned nullptr. + throw std::runtime_error("dispatch called on an unset AnySubscriptionCallback"); + } } + // Dispatch. + std::visit( + [&message, &message_info, this](auto && callback) { + using T = std::decay_t; + static constexpr bool is_ta = rclcpp::TypeAdapter::is_specialized::value; - if (shared_ptr_callback_) { - typename std::shared_ptr shared_message = std::move(message); - shared_ptr_callback_(shared_message); - } else if (shared_ptr_with_info_callback_) { - typename std::shared_ptr shared_message = std::move(message); - shared_ptr_with_info_callback_(shared_message, message_info); - } else if (unique_ptr_callback_) { - unique_ptr_callback_(std::move(message)); - } else if (unique_ptr_with_info_callback_) { - unique_ptr_with_info_callback_(std::move(message), message_info); - } else if (const_shared_ptr_callback_ || const_shared_ptr_with_info_callback_) { - throw std::runtime_error( - "unexpected dispatch_intra_process unique message call" - " with const shared_ptr callback"); - } else { - throw std::runtime_error("unexpected message without any callback set"); - } + // conditions for custom type + if constexpr (is_ta && std::is_same_v) { + auto local_message = convert_ros_message_to_custom_type_unique_ptr(*message); + callback(*local_message); + } else if constexpr (is_ta && std::is_same_v) { // NOLINT + auto local_message = convert_ros_message_to_custom_type_unique_ptr(*message); + callback(*local_message, message_info); + } else if constexpr ( // NOLINT[readability/braces] + is_ta && ( + std::is_same_v|| + std::is_same_v + )) + { + callback(convert_ros_message_to_custom_type_unique_ptr(*message)); + } else if constexpr ( // NOLINT[readability/braces] + is_ta && ( + std::is_same_v|| + std::is_same_v + )) + { + callback(convert_ros_message_to_custom_type_unique_ptr(*message), message_info); + } else if constexpr ( // NOLINT[readability/braces] + is_ta && ( + std::is_same_v|| + std::is_same_v + )) + { + callback(convert_ros_message_to_custom_type_unique_ptr(*message)); + } else if constexpr ( // NOLINT[readability/braces] + is_ta && ( + std::is_same_v|| + std::is_same_v + )) + { + callback(convert_ros_message_to_custom_type_unique_ptr(*message), message_info); + } + // conditions for ros message type + else if constexpr (std::is_same_v) { // NOLINT + callback(*message); + } else if constexpr (std::is_same_v) { + callback(*message, message_info); + } else if constexpr ( // NOLINT[readability/braces] + std::is_same_v|| + std::is_same_v) + { + callback(std::move(message)); + } else if constexpr ( // NOLINT[readability/braces] + std::is_same_v|| + std::is_same_v) + { + callback(std::move(message), message_info); + } else if constexpr ( // NOLINT[readability/braces] + std::is_same_v|| + std::is_same_v) + { + callback(std::move(message)); + } else if constexpr ( // NOLINT[readability/braces] + std::is_same_v|| + std::is_same_v) + { + callback(std::move(message), message_info); + } + // condition to catch SerializedMessage types + else if constexpr ( // NOLINT[readability/braces] + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v) + { + throw std::runtime_error( + "Cannot dispatch std::unique_ptr message " + "to rclcpp::SerializedMessage"); + } + // condition to catch unhandled callback types + else { // NOLINT[readability/braces] + static_assert(always_false_v, "unhandled callback type"); + } + }, callback_variant_); TRACEPOINT(callback_end, static_cast(this)); } - bool use_take_shared_method() const + constexpr + bool + use_take_shared_method() const + { + return + std::holds_alternative(callback_variant_) || + std::holds_alternative(callback_variant_) || + std::holds_alternative(callback_variant_) || + std::holds_alternative(callback_variant_); + } + + constexpr + bool + is_serialized_message_callback() const { - return const_shared_ptr_callback_ || const_shared_ptr_with_info_callback_; + return + std::holds_alternative(callback_variant_) || + std::holds_alternative(callback_variant_) || + std::holds_alternative(callback_variant_) || + std::holds_alternative(callback_variant_) || + std::holds_alternative(callback_variant_); } - void register_callback_for_tracing() + void + register_callback_for_tracing() { #ifndef TRACETOOLS_DISABLED - if (shared_ptr_callback_) { - TRACEPOINT( - rclcpp_callback_register, - static_cast(this), - get_symbol(shared_ptr_callback_)); - } else if (shared_ptr_with_info_callback_) { - TRACEPOINT( - rclcpp_callback_register, - static_cast(this), - get_symbol(shared_ptr_with_info_callback_)); - } else if (unique_ptr_callback_) { - TRACEPOINT( - rclcpp_callback_register, - static_cast(this), - get_symbol(unique_ptr_callback_)); - } else if (unique_ptr_with_info_callback_) { - TRACEPOINT( - rclcpp_callback_register, - static_cast(this), - get_symbol(unique_ptr_with_info_callback_)); - } + std::visit( + [this](auto && callback) { + TRACEPOINT( + rclcpp_callback_register, + static_cast(this), + tracetools::get_symbol(callback)); + }, callback_variant_); #endif // TRACETOOLS_DISABLED } + typename HelperT::variant_type & + get_variant() + { + return callback_variant_; + } + + const typename HelperT::variant_type & + get_variant() const + { + return callback_variant_; + } + private: - std::shared_ptr message_allocator_; - MessageDeleter message_deleter_; + // TODO(wjwwood): switch to inheriting from std::variant (i.e. HelperT::variant_type) once + // inheriting from std::variant is realistic (maybe C++23?), see: + // http://www.open-std.org/jtc1/sc22/wg21/docs/papers/2020/p2162r0.html + // For now, compose the variant into this class as a private attribute. + typename HelperT::variant_type callback_variant_; + + SubscribedTypeAllocator subscribed_type_allocator_; + SubscribedTypeDeleter subscribed_type_deleter_; + ROSMessageTypeAllocator ros_message_type_allocator_; + ROSMessageTypeDeleter ros_message_type_deleter_; + SerializedMessageAllocator serialized_message_allocator_; + SerializedMessageDeleter serialized_message_deleter_; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/callback_group.hpp b/rclcpp/include/rclcpp/callback_group.hpp index e9de8dc996..efa8352206 100644 --- a/rclcpp/include/rclcpp/callback_group.hpp +++ b/rclcpp/include/rclcpp/callback_group.hpp @@ -85,8 +85,8 @@ class CallbackGroup * group, or after, is irrelevant; the callback group will be automatically * added to the executor in either case. * - * \param[in] group_type They type of the callback group. - * \param[in] automatically_add_to_executor_with_node a boolean which + * \param[in] group_type The type of the 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. */ @@ -222,13 +222,6 @@ class CallbackGroup } }; -namespace callback_group -{ - -using CallbackGroupType [[deprecated("use rclcpp::CallbackGroupType instead")]] = CallbackGroupType; -using CallbackGroup [[deprecated("use rclcpp::CallbackGroup instead")]] = CallbackGroup; - -} // namespace callback_group } // namespace rclcpp #endif // RCLCPP__CALLBACK_GROUP_HPP_ diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index 3da956e583..cbf132c523 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -28,18 +29,19 @@ #include "rcl/error_handling.h" #include "rcl/wait.h" +#include "rclcpp/detail/cpp_callback_trampoline.hpp" #include "rclcpp/exceptions.hpp" +#include "rclcpp/expand_topic_or_service_name.hpp" #include "rclcpp/function_traits.hpp" +#include "rclcpp/logging.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/node_interfaces/node_graph_interface.hpp" #include "rclcpp/type_support_decl.hpp" #include "rclcpp/utilities.hpp" -#include "rclcpp/expand_topic_or_service_name.hpp" #include "rclcpp/visibility_control.hpp" -#include "rcutils/logging_macros.h" - #include "rmw/error_handling.h" +#include "rmw/impl/cpp/demangle.hpp" #include "rmw/rmw.h" namespace rclcpp @@ -150,11 +152,87 @@ class ClientBase bool exchange_in_use_by_wait_set_state(bool in_use_state); - RCLCPP_PUBLIC + /// Set a callback to be called when each new response is received. + /** + * The callback receives a size_t which is the number of responses received + * since the last time this callback was called. + * Normally this is 1, but can be > 1 if responses were received before any + * callback was set. + * + * Since this callback is called from the middleware, you should aim to make + * it fast and not blocking. + * If you need to do a lot of work or wait for some other event, you should + * spin it off to another thread, otherwise you risk blocking the middleware. + * + * Calling it again will clear any previously set callback. + * + * An exception will be thrown if the callback is not callable. + * + * This function is thread-safe. + * + * If you want more information available in the callback, like the client + * or other information, you may use a lambda with captures or std::bind. + * + * \sa rmw_client_set_on_new_response_callback + * \sa rcl_client_set_on_new_response_callback + * + * \param[in] callback functor to be called when a new response is received + */ + void + set_on_new_response_callback(std::function callback) + { + if (!callback) { + throw std::invalid_argument( + "The callback passed to set_on_new_response_callback " + "is not callable."); + } + + auto new_callback = + [callback, this](size_t number_of_responses) { + try { + callback(number_of_responses); + } catch (const std::exception & exception) { + RCLCPP_ERROR_STREAM( + node_logger_, + "rclcpp::ClientBase@" << this << + " caught " << rmw::impl::cpp::demangle(exception) << + " exception in user-provided callback for the 'on new response' callback: " << + exception.what()); + } catch (...) { + RCLCPP_ERROR_STREAM( + node_logger_, + "rclcpp::ClientBase@" << this << + " caught unhandled exception in user-provided callback " << + "for the 'on new response' callback"); + } + }; + + std::lock_guard lock(reentrant_mutex_); + + // Set it temporarily to the new callback, while we replace the old one. + // This two-step setting, prevents a gap where the old std::function has + // been replaced but the middleware hasn't been told about the new one yet. + set_on_new_response_callback( + rclcpp::detail::cpp_callback_trampoline, + static_cast(&new_callback)); + + // Store the std::function to keep it in scope, also overwrites the existing one. + on_new_response_callback_ = new_callback; + + // Set it again, now using the permanent storage. + set_on_new_response_callback( + rclcpp::detail::cpp_callback_trampoline, + static_cast(&on_new_response_callback_)); + } + + /// Unset the callback registered for new responses, if any. void - set_listener_callback( - rmw_listener_callback_t callback, - const void * user_data) const; + clear_on_new_response_callback() + { + std::lock_guard lock(reentrant_mutex_); + set_on_new_response_callback(nullptr, nullptr); + on_new_response_callback_ = nullptr; + } protected: RCLCPP_DISABLE_COPY(ClientBase) @@ -171,13 +249,21 @@ class ClientBase const rcl_node_t * get_rcl_node_handle() const; + RCLCPP_PUBLIC + void + set_on_new_response_callback(rcl_event_callback_t callback, const void * user_data); + rclcpp::node_interfaces::NodeGraphInterface::WeakPtr node_graph_; std::shared_ptr node_handle_; std::shared_ptr context_; + rclcpp::Logger node_logger_; std::shared_ptr client_handle_; std::atomic in_use_by_wait_set_{false}; + + std::recursive_mutex reentrant_mutex_; + std::function on_new_response_callback_{nullptr}; }; template @@ -303,8 +389,8 @@ class Client : public ClientBase int64_t sequence_number = request_header->sequence_number; // TODO(esteve) this should throw instead since it is not expected to happen in the first place if (this->pending_requests_.count(sequence_number) == 0) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", + RCLCPP_ERROR( + node_logger_, "Received invalid sequence number. Ignoring..."); return; } diff --git a/rclcpp/include/rclcpp/context.hpp b/rclcpp/include/rclcpp/context.hpp index c480444f8f..5d71ffe06c 100644 --- a/rclcpp/include/rclcpp/context.hpp +++ b/rclcpp/include/rclcpp/context.hpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -47,6 +48,17 @@ class ContextAlreadyInitialized : public std::runtime_error /// Forward declare WeakContextsWrapper class WeakContextsWrapper; +class OnShutdownCallbackHandle +{ + friend class Context; + +public: + using OnShutdownCallbackType = std::function; + +private: + std::weak_ptr callback; +}; + /// Context which encapsulates shared state between nodes and other similar entities. /** * A context also represents the lifecycle between init and shutdown of rclcpp. @@ -80,7 +92,7 @@ class Context : public std::enable_shared_from_this * * Note that this function does not setup any signal handlers, so if you want * it to be shutdown by the signal handler, then you need to either install - * them manually with rclcpp::install_signal_handers() or use rclcpp::init(). + * them manually with rclcpp::install_signal_handlers() or use rclcpp::init(). * In addition to installing the signal handlers, the shutdown_on_sigint * of the InitOptions needs to be `true` for this context to be shutdown by * the signal handler, otherwise it will be passed over. @@ -150,7 +162,7 @@ class Context : public std::enable_shared_from_this */ RCLCPP_PUBLIC std::string - shutdown_reason(); + shutdown_reason() const; /// Shutdown the context, making it uninitialized and therefore invalid for derived entities. /** @@ -177,7 +189,7 @@ class Context : public std::enable_shared_from_this bool shutdown(const std::string & reason); - using OnShutdownCallback = std::function; + using OnShutdownCallback = OnShutdownCallbackHandle::OnShutdownCallbackType; /// Add a on_shutdown callback to be called when shutdown is called for this context. /** @@ -203,23 +215,47 @@ class Context : public std::enable_shared_from_this OnShutdownCallback on_shutdown(OnShutdownCallback callback); - /// Return the shutdown callbacks as const. + /// Add a on_shutdown callback to be called when shutdown is called for this context. + /** + * These callbacks will be called in the order they are added as the second + * to last step in shutdown(). + * + * When shutdown occurs due to the signal handler, these callbacks are run + * asynchronously in the dedicated singal handling thread. + * + * Also, shutdown() may be called from the destructor of this function. + * Therefore, it is not safe to throw exceptions from these callbacks. + * Instead, log errors or use some other mechanism to indicate an error has + * occurred. + * + * On shutdown callbacks may be registered before init and after shutdown, + * and persist on repeated init's. + * + * \param[in] callback the on_shutdown callback to be registered + * \return the created callback handle + */ + RCLCPP_PUBLIC + virtual + OnShutdownCallbackHandle + add_on_shutdown_callback(OnShutdownCallback callback); + + /// Remove an registered on_shutdown callbacks. /** - * Using the returned reference is not thread-safe with calls that modify - * the list of "on shutdown" callbacks, i.e. on_shutdown(). + * \param[in] callback_handle the on_shutdown callback handle to be removed. + * \return true if the callback is found and removed, otherwise false. */ RCLCPP_PUBLIC - const std::vector & - get_on_shutdown_callbacks() const; + virtual + bool + remove_on_shutdown_callback(const OnShutdownCallbackHandle & callback_handle); /// Return the shutdown callbacks. /** - * Using the returned reference is not thread-safe with calls that modify - * the list of "on shutdown" callbacks, i.e. on_shutdown(). + * Returned callbacks are a copy of the registered callbacks. */ RCLCPP_PUBLIC - std::vector & - get_on_shutdown_callbacks(); + std::vector + get_on_shutdown_callbacks() const; /// Return the internal rcl context. RCLCPP_PUBLIC @@ -286,7 +322,7 @@ class Context : public std::enable_shared_from_this // This mutex is recursive so that the destructor can ensure atomicity // between is_initialized and shutdown. - std::recursive_mutex init_mutex_; + mutable std::recursive_mutex init_mutex_; std::shared_ptr rcl_context_; rclcpp::InitOptions init_options_; std::string shutdown_reason_; @@ -299,8 +335,8 @@ class Context : public std::enable_shared_from_this // attempt to acquire another sub context. std::recursive_mutex sub_contexts_mutex_; - std::vector on_shutdown_callbacks_; - std::mutex on_shutdown_callbacks_mutex_; + std::unordered_set> on_shutdown_callbacks_; + mutable std::mutex on_shutdown_callbacks_mutex_; /// Condition variable for timed sleep (see sleep_for). std::condition_variable interrupt_condition_variable_; diff --git a/rclcpp/include/rclcpp/contexts/default_context.hpp b/rclcpp/include/rclcpp/contexts/default_context.hpp index 0c2bbf8265..7c4ac63ef0 100644 --- a/rclcpp/include/rclcpp/contexts/default_context.hpp +++ b/rclcpp/include/rclcpp/contexts/default_context.hpp @@ -36,22 +36,6 @@ RCLCPP_PUBLIC DefaultContext::SharedPtr get_global_default_context(); -namespace default_context -{ - -using DefaultContext -[[deprecated("use rclcpp::contexts::DefaultContext instead")]] = DefaultContext; - -[[deprecated("use rclcpp::contexts::get_global_default_context() instead")]] -RCLCPP_PUBLIC -inline -DefaultContext::SharedPtr -get_global_default_context() -{ - return rclcpp::contexts::get_global_default_context(); -} - -} // namespace default_context } // namespace contexts } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/create_generic_publisher.hpp b/rclcpp/include/rclcpp/create_generic_publisher.hpp new file mode 100644 index 0000000000..296446f7b9 --- /dev/null +++ b/rclcpp/include/rclcpp/create_generic_publisher.hpp @@ -0,0 +1,69 @@ +// Copyright 2020, Apex.AI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__CREATE_GENERIC_PUBLISHER_HPP_ +#define RCLCPP__CREATE_GENERIC_PUBLISHER_HPP_ + +#include +#include +#include + +#include "rclcpp/generic_publisher.hpp" +#include "rclcpp/node_interfaces/node_topics_interface.hpp" +#include "rclcpp/publisher_options.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/typesupport_helpers.hpp" + +namespace rclcpp +{ + +/// Create and return a GenericPublisher. +/** + * The returned pointer will never be empty, but this function can throw various exceptions, for + * instance when the message's package can not be found on the AMENT_PREFIX_PATH. + * + * \param topics_interface NodeTopicsInterface pointer used in parts of the setup + * \param topic_name Topic name + * \param topic_type Topic type + * \param qos %QoS settings + * \param options %Publisher options. + * Not all publisher options are currently respected, the only relevant options for this + * publisher are `event_callbacks`, `use_default_callbacks`, and `%callback_group`. + */ +template> +std::shared_ptr create_generic_publisher( + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr topics_interface, + const std::string & topic_name, + const std::string & topic_type, + const rclcpp::QoS & qos, + const rclcpp::PublisherOptionsWithAllocator & options = ( + rclcpp::PublisherOptionsWithAllocator() + ) +) +{ + auto ts_lib = rclcpp::get_typesupport_library(topic_type, "rosidl_typesupport_cpp"); + auto pub = std::make_shared( + topics_interface->get_node_base_interface(), + std::move(ts_lib), + topic_name, + topic_type, + qos, + options); + topics_interface->add_publisher(pub, options.callback_group); + return pub; +} + +} // namespace rclcpp + +#endif // RCLCPP__CREATE_GENERIC_PUBLISHER_HPP_ diff --git a/rclcpp/include/rclcpp/create_generic_subscription.hpp b/rclcpp/include/rclcpp/create_generic_subscription.hpp new file mode 100644 index 0000000000..f5281cc673 --- /dev/null +++ b/rclcpp/include/rclcpp/create_generic_subscription.hpp @@ -0,0 +1,79 @@ +// Copyright 2020, Apex.AI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__CREATE_GENERIC_SUBSCRIPTION_HPP_ +#define RCLCPP__CREATE_GENERIC_SUBSCRIPTION_HPP_ + +#include +#include +#include +#include + +#include "rcl/subscription.h" +#include "rclcpp/generic_subscription.hpp" +#include "rclcpp/node_interfaces/node_topics_interface.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/serialized_message.hpp" +#include "rclcpp/subscription_options.hpp" +#include "rclcpp/typesupport_helpers.hpp" + +namespace rclcpp +{ + +/// Create and return a GenericSubscription. +/** + * The returned pointer will never be empty, but this function can throw various exceptions, for + * instance when the message's package can not be found on the AMENT_PREFIX_PATH. + * + * \param topics_interface NodeTopicsInterface pointer used in parts of the setup. + * \param topic_name Topic name + * \param topic_type Topic type + * \param qos %QoS settings + * \param callback Callback for new messages of serialized form + * \param options %Publisher options. + * Not all publisher options are currently respected, the only relevant options for this + * publisher are `event_callbacks`, `use_default_callbacks`, and `%callback_group`. + */ +template> +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, + const rclcpp::SubscriptionOptionsWithAllocator & options = ( + rclcpp::SubscriptionOptionsWithAllocator() + ) +) +{ + auto ts_lib = rclcpp::get_typesupport_library( + topic_type, "rosidl_typesupport_cpp"); + + auto subscription = std::make_shared( + topics_interface->get_node_base_interface(), + std::move(ts_lib), + topic_name, + topic_type, + qos, + callback, + options); + + topics_interface->add_subscription(subscription, options.callback_group); + + return subscription; +} + +} // namespace rclcpp + +#endif // RCLCPP__CREATE_GENERIC_SUBSCRIPTION_HPP_ diff --git a/rclcpp/include/rclcpp/create_subscription.hpp b/rclcpp/include/rclcpp/create_subscription.hpp index cf2458de62..5b84930ff7 100644 --- a/rclcpp/include/rclcpp/create_subscription.hpp +++ b/rclcpp/include/rclcpp/create_subscription.hpp @@ -47,11 +47,11 @@ template< typename MessageT, typename CallbackT, typename AllocatorT, - typename CallbackMessageT, typename SubscriptionT, typename MessageMemoryStrategyT, typename NodeParametersT, - typename NodeTopicsT> + typename NodeTopicsT, + typename ROSMessageType = typename SubscriptionT::ROSMessageType> 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( @@ -92,11 +92,11 @@ create_subscription( qos); subscription_topic_stats = std::make_shared< - rclcpp::topic_statistics::SubscriptionTopicStatistics + rclcpp::topic_statistics::SubscriptionTopicStatistics >(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(); @@ -153,7 +153,6 @@ create_subscription( * \tparam MessageT * \tparam CallbackT * \tparam AllocatorT - * \tparam CallbackMessageT * \tparam SubscriptionT * \tparam MessageMemoryStrategyT * \tparam NodeT @@ -171,13 +170,8 @@ template< typename MessageT, typename CallbackT, typename AllocatorT = std::allocator, - typename CallbackMessageT = - typename rclcpp::subscription_traits::has_message_type::type, - typename SubscriptionT = rclcpp::Subscription, - typename MessageMemoryStrategyT = rclcpp::message_memory_strategy::MessageMemoryStrategy< - CallbackMessageT, - AllocatorT - >, + typename SubscriptionT = rclcpp::Subscription, + typename MessageMemoryStrategyT = typename SubscriptionT::MessageMemoryStrategyType, typename NodeT> typename std::shared_ptr create_subscription( @@ -194,7 +188,7 @@ create_subscription( ) { return rclcpp::detail::create_subscription< - MessageT, CallbackT, AllocatorT, CallbackMessageT, SubscriptionT, MessageMemoryStrategyT>( + MessageT, CallbackT, AllocatorT, SubscriptionT, MessageMemoryStrategyT>( node, node, topic_name, qos, std::forward(callback), options, msg_mem_strat); } @@ -206,13 +200,8 @@ template< typename MessageT, typename CallbackT, typename AllocatorT = std::allocator, - typename CallbackMessageT = - typename rclcpp::subscription_traits::has_message_type::type, - typename SubscriptionT = rclcpp::Subscription, - typename MessageMemoryStrategyT = rclcpp::message_memory_strategy::MessageMemoryStrategy< - CallbackMessageT, - AllocatorT - >> + typename SubscriptionT = rclcpp::Subscription, + typename MessageMemoryStrategyT = typename SubscriptionT::MessageMemoryStrategyType> typename std::shared_ptr create_subscription( rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node_parameters, @@ -229,7 +218,7 @@ create_subscription( ) { return rclcpp::detail::create_subscription< - MessageT, CallbackT, AllocatorT, CallbackMessageT, SubscriptionT, MessageMemoryStrategyT>( + MessageT, CallbackT, AllocatorT, SubscriptionT, MessageMemoryStrategyT>( node_parameters, node_topics, topic_name, qos, std::forward(callback), options, msg_mem_strat); } diff --git a/rclcpp/include/rclcpp/detail/cpp_callback_trampoline.hpp b/rclcpp/include/rclcpp/detail/cpp_callback_trampoline.hpp new file mode 100644 index 0000000000..7ea991c0de --- /dev/null +++ b/rclcpp/include/rclcpp/detail/cpp_callback_trampoline.hpp @@ -0,0 +1,67 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__DETAIL__CPP_CALLBACK_TRAMPOLINE_HPP_ +#define RCLCPP__DETAIL__CPP_CALLBACK_TRAMPOLINE_HPP_ + +#include + +namespace rclcpp +{ + +namespace detail +{ + +/// Trampoline pattern for wrapping std::function into C-style callbacks. +/** + * A common pattern in C is for a function to take a function pointer and a + * void pointer for "user data" which is passed to the function pointer when it + * is called from within C. + * + * It works by using the user data pointer to store a pointer to a + * std::function instance. + * So when called from C, this function will cast the user data to the right + * std::function type and call it. + * + * This should allow you to use free functions, lambdas with and without + * captures, and various kinds of std::bind instances. + * + * The interior of this function is likely to be executed within a C runtime, + * so no exceptins should be thrown at this point, and doing so results in + * undefined behavior. + * + * \tparam UserDataT Deduced type based on what is passed for user data, + * usually this type is either `void *` or `const void *`. + * \tparam Args the arguments being passed to the callback + * \tparam ReturnT the return type of this function and the callback, default void + * \param user_data the function pointer, possibly type erased + * \returns whatever the callback returns, if anything + */ +template< + typename UserDataT, + typename ... Args, + typename ReturnT = void +> +ReturnT +cpp_callback_trampoline(UserDataT user_data, Args ... args) noexcept +{ + auto & actual_callback = *reinterpret_cast *>(user_data); + return actual_callback(args ...); +} + +} // namespace detail + +} // namespace rclcpp + +#endif // RCLCPP__DETAIL__CPP_CALLBACK_TRAMPOLINE_HPP_ diff --git a/rclcpp/include/rclcpp/detail/mutex_two_priorities.hpp b/rclcpp/include/rclcpp/detail/mutex_two_priorities.hpp index 6c5f69e98f..98118b619f 100644 --- a/rclcpp/include/rclcpp/detail/mutex_two_priorities.hpp +++ b/rclcpp/include/rclcpp/detail/mutex_two_priorities.hpp @@ -15,6 +15,7 @@ #ifndef RCLCPP__DETAIL__MUTEX_TWO_PRIORITIES_HPP_ #define RCLCPP__DETAIL__MUTEX_TWO_PRIORITIES_HPP_ +#include #include namespace rclcpp @@ -62,11 +63,11 @@ class MutexTwoPriorities get_low_priority_lockable(); private: - // Implementation detail: the idea here is that only one low priority thread can be - // trying to take the data_ mutex while the others are excluded by the barrier_ mutex. - // All high priority threads are already waiting for the data_ mutex. - std::mutex barrier_; - std::mutex data_; + std::condition_variable hp_cv_; + std::condition_variable lp_cv_; + std::mutex cv_mutex_; + size_t hp_waiting_count_{0u}; + bool data_taken_{false}; }; } // namespace detail diff --git a/rclcpp/include/rclcpp/detail/subscription_callback_type_helper.hpp b/rclcpp/include/rclcpp/detail/subscription_callback_type_helper.hpp new file mode 100644 index 0000000000..ec24038d79 --- /dev/null +++ b/rclcpp/include/rclcpp/detail/subscription_callback_type_helper.hpp @@ -0,0 +1,164 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__DETAIL__SUBSCRIPTION_CALLBACK_TYPE_HELPER_HPP_ +#define RCLCPP__DETAIL__SUBSCRIPTION_CALLBACK_TYPE_HELPER_HPP_ + +#include +#include + +#include "rclcpp/function_traits.hpp" +#include "rclcpp/message_info.hpp" + +namespace rclcpp +{ +namespace detail +{ + +/// Template metaprogramming helper used to resolve the callback argument into a std::function. +/** + * Sometimes the CallbackT is a std::function already, but it could also be a + * function pointer, lambda, bind, or some variant of those. + * In some cases, like a lambda where the arguments can be converted between one + * another, e.g. std::function)> and + * std::function)>, you need to make that not ambiguous + * by checking the arguments independently using function traits rather than + * rely on overloading the two std::function types. + * + * This issue, with the lambda's, can be demonstrated with this minimal program: + * + * #include + * #include + * + * void f(std::function)>) {} + * void f(std::function)>) {} + * + * int main() { + * // Fails to compile with an "ambiguous call" error. + * f([](std::shared_ptr){}); + * + * // Works. + * std::function)> cb = [](std::shared_ptr){}; + * f(cb); + * } + * + * If this program ever starts working in a future version of C++, this class + * may become redundant. + * + * This helper works by using SFINAE with rclcpp::function_traits::same_arguments<> + * to narrow down the exact std::function<> type for the given CallbackT. + */ +template +struct SubscriptionCallbackTypeHelper +{ + using callback_type = typename rclcpp::function_traits::as_std_function::type; +}; + +template +struct SubscriptionCallbackTypeHelper< + MessageT, + CallbackT, + typename std::enable_if_t< + rclcpp::function_traits::same_arguments< + CallbackT, + std::function)> + >::value + > +> +{ + using callback_type = std::function)>; +}; + +template +struct SubscriptionCallbackTypeHelper< + MessageT, + CallbackT, + typename std::enable_if_t< + rclcpp::function_traits::same_arguments< + CallbackT, + std::function, const rclcpp::MessageInfo &)> + >::value + > +> +{ + using callback_type = + std::function, const rclcpp::MessageInfo &)>; +}; + +template +struct SubscriptionCallbackTypeHelper< + MessageT, + CallbackT, + typename std::enable_if_t< + rclcpp::function_traits::same_arguments< + CallbackT, + std::function &)> + >::value + > +> +{ + using callback_type = std::function &)>; +}; + +template +struct SubscriptionCallbackTypeHelper< + MessageT, + CallbackT, + typename std::enable_if_t< + rclcpp::function_traits::same_arguments< + CallbackT, + std::function &, const rclcpp::MessageInfo &)> + >::value + > +> +{ + using callback_type = + std::function &, const rclcpp::MessageInfo &)>; +}; + +template +struct SubscriptionCallbackTypeHelper< + MessageT, + CallbackT, + typename std::enable_if_t< + rclcpp::function_traits::same_arguments< + CallbackT, + std::function)> + >::value + > +> +{ + using callback_type = std::function)>; +}; + +template +struct SubscriptionCallbackTypeHelper< + MessageT, + CallbackT, + typename std::enable_if_t< + rclcpp::function_traits::same_arguments< + CallbackT, + std::function, const rclcpp::MessageInfo &)> + >::value + > +> +{ + using callback_type = + std::function, const rclcpp::MessageInfo &)>; +}; + +} // namespace detail +} // namespace rclcpp + +#endif // RCLCPP__DETAIL__SUBSCRIPTION_CALLBACK_TYPE_HELPER_HPP_ diff --git a/rclcpp/include/rclcpp/exceptions/exceptions.hpp b/rclcpp/include/rclcpp/exceptions/exceptions.hpp index a0c711290c..524ccf73ea 100644 --- a/rclcpp/include/rclcpp/exceptions/exceptions.hpp +++ b/rclcpp/include/rclcpp/exceptions/exceptions.hpp @@ -282,8 +282,8 @@ class ParameterModifiedInCallbackException : public std::runtime_error using std::runtime_error::runtime_error; }; -/// Thrown when a parameter override wasn't provided and one was required. -class NoParameterOverrideProvided : public std::runtime_error +/// Thrown when an uninitialized parameter is accessed. +class ParameterUninitializedException : public std::runtime_error { public: /// Construct an instance. @@ -291,8 +291,8 @@ class NoParameterOverrideProvided : public std::runtime_error * \param[in] name the name of the parameter. * \param[in] message custom exception message. */ - explicit NoParameterOverrideProvided(const std::string & name) - : std::runtime_error("parameter '" + name + "' requires an user provided parameter override") + explicit ParameterUninitializedException(const std::string & name) + : std::runtime_error("parameter '" + name + "' is not initialized") {} }; @@ -303,6 +303,13 @@ class InvalidQosOverridesException : public std::runtime_error using std::runtime_error::runtime_error; }; +/// Thrown if a QoS compatibility check fails. +class QoSCheckCompatibleException : public std::runtime_error +{ + // Inherit constructors from runtime_error. + using std::runtime_error::runtime_error; +}; + } // namespace exceptions } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 9321f679fc..4861337eaf 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -30,6 +30,7 @@ #include "rcl/guard_condition.h" #include "rcl/wait.h" +#include "rclcpp/context.hpp" #include "rclcpp/contexts/default_context.hpp" #include "rclcpp/guard_condition.hpp" #include "rclcpp/executor_options.hpp" @@ -524,7 +525,7 @@ class Executor std::atomic_bool spinning; /// Guard condition for signaling the rmw layer to wake up for special events. - rcl_guard_condition_t interrupt_guard_condition_ = rcl_get_zero_initialized_guard_condition(); + rclcpp::GuardCondition interrupt_guard_condition_; std::shared_ptr shutdown_guard_condition_; @@ -548,7 +549,7 @@ class Executor spin_once_impl(std::chrono::nanoseconds timeout); typedef std::map> WeakNodesToGuardConditionsMap; @@ -571,14 +572,11 @@ class Executor /// nodes that are associated with the executor std::list weak_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_); -}; - -namespace executor -{ -using Executor [[deprecated("use rclcpp::Executor instead")]] = rclcpp::Executor; + /// shutdown callback handle registered to Context + rclcpp::OnShutdownCallbackHandle shutdown_callback_handle_; +}; -} // namespace executor } // namespace rclcpp #endif // RCLCPP__EXECUTOR_HPP_ diff --git a/rclcpp/include/rclcpp/executor_options.hpp b/rclcpp/include/rclcpp/executor_options.hpp index d5ec6d0da9..066f29eb4e 100644 --- a/rclcpp/include/rclcpp/executor_options.hpp +++ b/rclcpp/include/rclcpp/executor_options.hpp @@ -38,20 +38,6 @@ struct ExecutorOptions size_t max_conditions; }; -namespace executor -{ - -using ExecutorArgs [[deprecated("use rclcpp::ExecutorOptions instead")]] = ExecutorOptions; - -[[deprecated("use rclcpp::ExecutorOptions() instead")]] -inline -rclcpp::ExecutorOptions -create_default_executor_arguments() -{ - return rclcpp::ExecutorOptions(); -} - -} // namespace executor } // namespace rclcpp #endif // RCLCPP__EXECUTOR_OPTIONS_HPP_ diff --git a/rclcpp/include/rclcpp/executors/events_executor.hpp b/rclcpp/include/rclcpp/executors/events_executor.hpp index 44de455301..ac136a241c 100644 --- a/rclcpp/include/rclcpp/executors/events_executor.hpp +++ b/rclcpp/include/rclcpp/executors/events_executor.hpp @@ -29,8 +29,6 @@ #include "rclcpp/experimental/buffers/simple_events_queue.hpp" #include "rclcpp/node.hpp" -#include "rmw/listener_callback_type.h" - namespace rclcpp { namespace executors @@ -211,31 +209,6 @@ class EventsExecutor : public rclcpp::Executor private: RCLCPP_DISABLE_COPY(EventsExecutor) - // Executor callback: Push new events into the queue and trigger cv. - // This function is called by the DDS entities when an event happened, - // like a subscription receiving a message. - static void - push_event(const void * event_data, size_t num_events) - { - if (!event_data) { - throw std::runtime_error("Executor event data not valid."); - } - - auto data = static_cast(event_data); - - executors::EventsExecutor * this_executor = data->executor; - ExecutorEvent event = data->event; - event.num_events = num_events; - - // Event queue mutex scope - { - std::unique_lock lock(this_executor->push_mutex_); - this_executor->events_queue_->push(event); - } - // Notify that the event queue has some events in it. - this_executor->events_queue_cv_.notify_one(); - } - // Execute a single event RCLCPP_PUBLIC void diff --git a/rclcpp/include/rclcpp/executors/events_executor_entities_collector.hpp b/rclcpp/include/rclcpp/executors/events_executor_entities_collector.hpp index 2781026492..6e66d32f1d 100644 --- a/rclcpp/include/rclcpp/executors/events_executor_entities_collector.hpp +++ b/rclcpp/include/rclcpp/executors/events_executor_entities_collector.hpp @@ -209,16 +209,16 @@ class EventsExecutorEntitiesCollector final unset_callback_group_entities_callbacks(rclcpp::CallbackGroup::SharedPtr group); void - set_guard_condition_callback(const rcl_guard_condition_t * guard_condition); + set_guard_condition_callback(rclcpp::GuardCondition * guard_condition); void - unset_guard_condition_callback(const rcl_guard_condition_t * guard_condition); + unset_guard_condition_callback(rclcpp::GuardCondition * guard_condition); - void - remove_callback_data(void * entity_id, ExecutorEventType type); + std::function + create_entity_callback(void * exec_entity_id, ExecutorEventType type); - const EventsExecutorCallbackData * - get_callback_data(void * entity_id, ExecutorEventType type); + std::function + create_waitable_callback(void * waitable_id); /// Return true if the node belongs to the collector /** @@ -249,7 +249,7 @@ class EventsExecutorEntitiesCollector final WeakCallbackGroupsToNodesMap weak_groups_to_nodes_associated_with_executor_; typedef std::map> WeakNodesToGuardConditionsMap; WeakNodesToGuardConditionsMap weak_nodes_to_guard_conditions_; @@ -269,17 +269,6 @@ class EventsExecutorEntitiesCollector final EventsExecutor * associated_executor_ = nullptr; /// Instance of the timers manager used by the associated executor TimersManager::SharedPtr timers_manager_; - - /// Callback data objects mapped to the number of listeners sharing the same object. - /// When no more listeners use the object, it can be removed from the map. - /// For example, the entities collector holds every node's guard condition, which - /// share the same EventsExecutorCallbackData object ptr to use as their callback arg: - /// cb_data_object = {executor_ptr, entities_collector_ptr, WAITABLE_EVENT}; - /// Node1->gc(&cb_data_object) - /// Node2->gc(&cb_data_object) - /// So the maps has: (cb_data_object, 2) - /// When both nodes are removed (counter = 0), the cb_data_object can be destroyed. - std::unordered_map callback_data_map_; }; } // namespace executors diff --git a/rclcpp/include/rclcpp/executors/events_executor_event_types.hpp b/rclcpp/include/rclcpp/executors/events_executor_event_types.hpp index a38e0f8e00..c2b9a76c74 100644 --- a/rclcpp/include/rclcpp/executors/events_executor_event_types.hpp +++ b/rclcpp/include/rclcpp/executors/events_executor_event_types.hpp @@ -20,9 +20,6 @@ namespace rclcpp namespace executors { -// forward declaration of EventsExecutor to avoid circular dependency -class EventsExecutor; - enum ExecutorEventType { SUBSCRIPTION_EVENT, @@ -33,47 +30,12 @@ enum ExecutorEventType struct ExecutorEvent { - const void * entity_id; + const void * exec_entity_id; + int gen_entity_id; ExecutorEventType type; size_t num_events; }; -// The EventsExecutorCallbackData struct is what the listeners -// will use as argument when calling their callbacks from the -// RMW implementation. The listeners get a (void *) of this struct, -// and the executor is in charge to cast it back and use the data. -struct EventsExecutorCallbackData -{ - EventsExecutorCallbackData( - EventsExecutor * _executor, - ExecutorEvent _event) - { - executor = _executor; - event = _event; - } - - // Equal operator - bool operator==(const EventsExecutorCallbackData & other) const - { - return event.entity_id == other.event.entity_id; - } - - // Struct members - EventsExecutor * executor; - ExecutorEvent event; -}; - -// To be able to use std::unordered_map with an EventsExecutorCallbackData -// as key, we need a hasher. We use the entity ID as hash, since it is -// unique for each EventsExecutorCallbackData object. -struct KeyHasher -{ - size_t operator()(const EventsExecutorCallbackData & key) const - { - return std::hash()(key.event.entity_id); - } -}; - } // namespace executors } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/executors/events_executor_notify_waitable.hpp b/rclcpp/include/rclcpp/executors/events_executor_notify_waitable.hpp index 2a19e2d9af..d15d9eba28 100644 --- a/rclcpp/include/rclcpp/executors/events_executor_notify_waitable.hpp +++ b/rclcpp/include/rclcpp/executors/events_executor_notify_waitable.hpp @@ -53,27 +53,19 @@ class EventsExecutorNotifyWaitable final : public EventWaitable RCLCPP_PUBLIC void - add_guard_condition(const rcl_guard_condition_t * guard_condition) + add_guard_condition(const rclcpp::GuardCondition * guard_condition) { notify_guard_conditions_.push_back(guard_condition); } RCLCPP_PUBLIC void - set_listener_callback( - rmw_listener_callback_t callback, - const void * user_data) const override + set_on_ready_callback(std::function callback) override { - for (auto gc : notify_guard_conditions_) { - rcl_ret_t ret = rcl_guard_condition_set_listener_callback( - gc, - callback, - user_data); - - if (RCL_RET_OK != ret) { - throw std::runtime_error("Couldn't set guard condition events callback"); - } - } + (void)callback; + // for (auto gc : notify_guard_conditions_) { + // gc->set_listener_callback(); + // } } RCLCPP_PUBLIC @@ -85,7 +77,7 @@ class EventsExecutorNotifyWaitable final : public EventWaitable } private: - std::list notify_guard_conditions_; + std::list notify_guard_conditions_; }; } // namespace executors diff --git a/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp b/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp index 51f80b32c9..3eb1cbbdb3 100644 --- a/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp +++ b/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp @@ -65,9 +65,13 @@ class StaticExecutorEntitiesCollector final init( rcl_wait_set_t * p_wait_set, rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy, - rcl_guard_condition_t * executor_guard_condition); + rclcpp::GuardCondition * executor_guard_condition); /// Finalize StaticExecutorEntitiesCollector to clear resources + RCLCPP_PUBLIC + bool + is_init() {return initialized_;} + RCLCPP_PUBLIC void fini(); @@ -326,7 +330,7 @@ class StaticExecutorEntitiesCollector final WeakCallbackGroupsToNodesMap weak_groups_to_nodes_associated_with_executor_; typedef std::map> WeakNodesToGuardConditionsMap; WeakNodesToGuardConditionsMap weak_nodes_to_guard_conditions_; @@ -339,6 +343,9 @@ class StaticExecutorEntitiesCollector final /// 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 diff --git a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp index cbfcd00a1c..61da15e125 100644 --- a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp @@ -15,6 +15,7 @@ #ifndef RCLCPP__EXECUTORS__STATIC_SINGLE_THREADED_EXECUTOR_HPP_ #define RCLCPP__EXECUTORS__STATIC_SINGLE_THREADED_EXECUTOR_HPP_ +#include #include #include #include @@ -78,6 +79,42 @@ class StaticSingleThreadedExecutor : public rclcpp::Executor void spin() override; + /// Static executor implementation of spin some + /** + * This non-blocking function will execute entities that + * were ready when this API was called, until timeout or no + * more work available. Entities that got ready while + * executing work, won't be taken into account here. + * + * Example: + * while(condition) { + * spin_some(); + * sleep(); // User should have some sync work or + * // sleep to avoid a 100% CPU usage + * } + */ + RCLCPP_PUBLIC + void + spin_some(std::chrono::nanoseconds max_duration = std::chrono::nanoseconds(0)) override; + + /// Static executor implementation of spin all + /** + * This non-blocking function will execute entities until + * timeout or no more work available. If new entities get ready + * while executing work available, they will be executed + * as long as the timeout hasn't expired. + * + * Example: + * while(condition) { + * spin_all(); + * sleep(); // User should have some sync work or + * // sleep to avoid a 100% CPU usage + * } + */ + RCLCPP_PUBLIC + void + spin_all(std::chrono::nanoseconds max_duration) override; + /// Add a callback group to an executor. /** * \sa rclcpp::Executor::add_callback_group @@ -155,113 +192,23 @@ class StaticSingleThreadedExecutor : public rclcpp::Executor std::vector get_automatically_added_callback_groups_from_nodes() override; - /// Spin (blocking) until the future is complete, it times out waiting, or rclcpp is interrupted. +protected: /** - * \param[in] future The future to wait on. If this function returns SUCCESS, the future can be - * accessed without blocking (though it may still throw an exception). - * \param[in] timeout Optional timeout parameter, which gets passed to - * Executor::execute_ready_executables. - * `-1` is block forever, `0` is non-blocking. - * If the time spent inside the blocking loop exceeds this timeout, return a TIMEOUT return - * code. - * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`. - * - * Example usage: - * rclcpp::executors::StaticSingleThreadedExecutor exec; - * // ... other part of code like creating node - * // define future - * exec.add_node(node); - * exec.spin_until_future_complete(future); + * @brief Executes ready executables from wait set. + * @param spin_once if true executes only the first ready executable. + * @return true if any executable was ready. */ - template - rclcpp::FutureReturnCode - spin_until_future_complete( - FutureT & future, - std::chrono::duration timeout = std::chrono::duration(-1)) - { - std::future_status status = future.wait_for(std::chrono::seconds(0)); - if (status == std::future_status::ready) { - return rclcpp::FutureReturnCode::SUCCESS; - } - - auto end_time = std::chrono::steady_clock::now(); - std::chrono::nanoseconds timeout_ns = std::chrono::duration_cast( - timeout); - if (timeout_ns > std::chrono::nanoseconds::zero()) { - end_time += timeout_ns; - } - std::chrono::nanoseconds timeout_left = timeout_ns; - - entities_collector_->init(&wait_set_, memory_strategy_, &interrupt_guard_condition_); - RCLCPP_SCOPE_EXIT(entities_collector_->fini()); - - while (rclcpp::ok(this->context_)) { - // Do one set of work. - entities_collector_->refresh_wait_set(timeout_left); - execute_ready_executables(); - // Check if the future is set, return SUCCESS if it is. - status = future.wait_for(std::chrono::seconds(0)); - if (status == std::future_status::ready) { - return rclcpp::FutureReturnCode::SUCCESS; - } - // If the original timeout is < 0, then this is blocking, never TIMEOUT. - if (timeout_ns < std::chrono::nanoseconds::zero()) { - continue; - } - // Otherwise check if we still have time to wait, return TIMEOUT if not. - auto now = std::chrono::steady_clock::now(); - if (now >= end_time) { - return rclcpp::FutureReturnCode::TIMEOUT; - } - // Subtract the elapsed time from the original timeout. - timeout_left = std::chrono::duration_cast(end_time - now); - } - - // The future did not complete before ok() returned false, return INTERRUPTED. - return rclcpp::FutureReturnCode::INTERRUPTED; - } - - /// Not yet implemented, see https://github.com/ros2/rclcpp/issues/1219 for tracking RCLCPP_PUBLIC - void - spin_some(std::chrono::nanoseconds max_duration = std::chrono::nanoseconds(0)) override - { - (void)max_duration; - throw rclcpp::exceptions::UnimplementedError( - "spin_some is not implemented for StaticSingleThreadedExecutor, use spin or " - "spin_until_future_complete"); - } + bool + execute_ready_executables(bool spin_once = false); - /// Not yet implemented, see https://github.com/ros2/rclcpp/issues/1219 for tracking RCLCPP_PUBLIC void - spin_all(std::chrono::nanoseconds) override - { - throw rclcpp::exceptions::UnimplementedError( - "spin_all is not implemented for StaticSingleThreadedExecutor, use spin or " - "spin_until_future_complete"); - } + spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive); - /// Not yet implemented, see https://github.com/ros2/rclcpp/issues/1219 for tracking - RCLCPP_PUBLIC - void - spin_once(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)) override - { - (void)timeout; - throw rclcpp::exceptions::UnimplementedError( - "spin_once is not implemented for StaticSingleThreadedExecutor, use spin or " - "spin_until_future_complete"); - } - -protected: - /// Check which executables in ExecutableList struct are ready from wait_set and execute them. - /** - * \param[in] exec_list Structure that can hold subscriptionbases, timerbases, etc - * \param[in] timeout Optional timeout parameter. - */ RCLCPP_PUBLIC void - execute_ready_executables(); + spin_once_impl(std::chrono::nanoseconds timeout) override; private: RCLCPP_DISABLE_COPY(StaticSingleThreadedExecutor) diff --git a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp index 9d4b62654c..4d968ba7d8 100644 --- a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp +++ b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp @@ -33,6 +33,7 @@ #include "rclcpp/allocator/allocator_deleter.hpp" #include "rclcpp/experimental/subscription_intra_process.hpp" #include "rclcpp/experimental/subscription_intra_process_base.hpp" +#include "rclcpp/experimental/subscription_intra_process_buffer.hpp" #include "rclcpp/logger.hpp" #include "rclcpp/logging.hpp" #include "rclcpp/macros.hpp" @@ -181,7 +182,7 @@ class IntraProcessManager do_intra_process_publish( uint64_t intra_process_publisher_id, std::unique_ptr message, - std::shared_ptr::allocator_type> allocator) + typename allocator::AllocRebind::allocator_type & allocator) { using MessageAllocTraits = allocator::AllocRebind; using MessageAllocatorT = typename MessageAllocTraits::allocator_type; @@ -202,7 +203,8 @@ class IntraProcessManager // None of the buffers require ownership, so we promote the pointer std::shared_ptr msg = std::move(message); - this->template add_shared_msg_to_buffers(msg, sub_ids.take_shared_subscriptions); + this->template add_shared_msg_to_buffers( + msg, sub_ids.take_shared_subscriptions); } else if (!sub_ids.take_ownership_subscriptions.empty() && // NOLINT sub_ids.take_shared_subscriptions.size() <= 1) { @@ -225,9 +227,9 @@ class IntraProcessManager { // Construct a new shared pointer from the message // for the buffers that do not require ownership - auto shared_msg = std::allocate_shared(*allocator, *message); + auto shared_msg = std::allocate_shared(allocator, *message); - this->template add_shared_msg_to_buffers( + this->template add_shared_msg_to_buffers( shared_msg, sub_ids.take_shared_subscriptions); this->template add_owned_msg_to_buffers( std::move(message), sub_ids.take_ownership_subscriptions, allocator); @@ -242,7 +244,7 @@ class IntraProcessManager do_intra_process_publish_and_return_shared( uint64_t intra_process_publisher_id, std::unique_ptr message, - std::shared_ptr::allocator_type> allocator) + typename allocator::AllocRebind::allocator_type & allocator) { using MessageAllocTraits = allocator::AllocRebind; using MessageAllocatorT = typename MessageAllocTraits::allocator_type; @@ -263,17 +265,17 @@ class IntraProcessManager // If there are no owning, just convert to shared. std::shared_ptr shared_msg = std::move(message); if (!sub_ids.take_shared_subscriptions.empty()) { - this->template add_shared_msg_to_buffers( + this->template add_shared_msg_to_buffers( shared_msg, sub_ids.take_shared_subscriptions); } return shared_msg; } else { // Construct a new shared pointer from the message for the buffers that // do not require ownership and to return. - auto shared_msg = std::allocate_shared(*allocator, *message); + auto shared_msg = std::allocate_shared(allocator, *message); if (!sub_ids.take_shared_subscriptions.empty()) { - this->template add_shared_msg_to_buffers( + this->template add_shared_msg_to_buffers( shared_msg, sub_ids.take_shared_subscriptions); } @@ -350,7 +352,10 @@ class IntraProcessManager bool can_communicate(PublisherInfo pub_info, SubscriptionInfo sub_info) const; - template + template< + typename MessageT, + typename Alloc, + typename Deleter> void add_shared_msg_to_buffers( std::shared_ptr message, @@ -363,9 +368,16 @@ class IntraProcessManager } auto subscription_base = subscription_it->second.subscription.lock(); if (subscription_base) { - auto subscription = std::static_pointer_cast< - rclcpp::experimental::SubscriptionIntraProcess + auto subscription = std::dynamic_pointer_cast< + rclcpp::experimental::SubscriptionIntraProcessBuffer >(subscription_base); + if (nullptr == subscription) { + throw std::runtime_error( + "failed to dynamic cast SubscriptionIntraProcessBase to " + "SubscriptionIntraProcessBuffer, which " + "can happen when the publisher and subscription use different " + "allocator types, which is not supported"); + } subscription->provide_intra_process_message(message); } else { @@ -382,7 +394,7 @@ class IntraProcessManager add_owned_msg_to_buffers( std::unique_ptr message, std::vector subscription_ids, - std::shared_ptr::allocator_type> allocator) + typename allocator::AllocRebind::allocator_type & allocator) { using MessageAllocTraits = allocator::AllocRebind; using MessageUniquePtr = std::unique_ptr; @@ -394,9 +406,16 @@ class IntraProcessManager } auto subscription_base = subscription_it->second.subscription.lock(); if (subscription_base) { - auto subscription = std::static_pointer_cast< - rclcpp::experimental::SubscriptionIntraProcess + auto subscription = std::dynamic_pointer_cast< + rclcpp::experimental::SubscriptionIntraProcessBuffer >(subscription_base); + if (nullptr == subscription) { + throw std::runtime_error( + "failed to dynamic cast SubscriptionIntraProcessBase to " + "SubscriptionIntraProcessBuffer, which " + "can happen when the publisher and subscription use different " + "allocator types, which is not supported"); + } if (std::next(it) == subscription_ids.end()) { // If this is the last subscription, give up ownership @@ -405,8 +424,8 @@ class IntraProcessManager // Copy the message since we have additional subscriptions to serve MessageUniquePtr copy_message; Deleter deleter = message.get_deleter(); - auto ptr = MessageAllocTraits::allocate(*allocator.get(), 1); - MessageAllocTraits::construct(*allocator.get(), ptr, *message); + auto ptr = MessageAllocTraits::allocate(allocator, 1); + MessageAllocTraits::construct(allocator, ptr, *message); copy_message = MessageUniquePtr(ptr, deleter); subscription->provide_intra_process_message(std::move(copy_message)); diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp index 46e019daee..92ee8fde78 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp @@ -30,6 +30,7 @@ #include "rclcpp/experimental/buffers/intra_process_buffer.hpp" #include "rclcpp/experimental/create_intra_process_buffer.hpp" #include "rclcpp/experimental/subscription_intra_process_base.hpp" +#include "rclcpp/experimental/subscription_intra_process_buffer.hpp" #include "rclcpp/type_support_decl.hpp" #include "rclcpp/waitable.hpp" #include "tracetools/tracetools.h" @@ -44,21 +45,27 @@ template< typename Alloc = std::allocator, typename Deleter = std::default_delete, typename CallbackMessageT = MessageT> -class SubscriptionIntraProcess : public SubscriptionIntraProcessBase +class SubscriptionIntraProcess + : public SubscriptionIntraProcessBuffer< + MessageT, + Alloc, + Deleter + > { -public: - RCLCPP_SMART_PTR_DEFINITIONS(SubscriptionIntraProcess) - - using MessageAllocTraits = allocator::AllocRebind; - using MessageAlloc = typename MessageAllocTraits::allocator_type; - using ConstMessageSharedPtr = std::shared_ptr; - using MessageUniquePtr = std::unique_ptr; - - using BufferUniquePtr = typename rclcpp::experimental::buffers::IntraProcessBuffer< + using SubscriptionIntraProcessBufferT = SubscriptionIntraProcessBuffer< MessageT, Alloc, Deleter - >::UniquePtr; + >; + +public: + RCLCPP_SMART_PTR_DEFINITIONS(SubscriptionIntraProcess) + + using MessageAllocTraits = typename SubscriptionIntraProcessBufferT::MessageAllocTraits; + using MessageAlloc = typename SubscriptionIntraProcessBufferT::MessageAlloc; + using ConstMessageSharedPtr = typename SubscriptionIntraProcessBufferT::ConstMessageSharedPtr; + using MessageUniquePtr = typename SubscriptionIntraProcessBufferT::MessageUniquePtr; + using BufferUniquePtr = typename SubscriptionIntraProcessBufferT::BufferUniquePtr; SubscriptionIntraProcess( AnySubscriptionCallback callback, @@ -67,31 +74,14 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBase const std::string & topic_name, rmw_qos_profile_t qos_profile, rclcpp::IntraProcessBufferType buffer_type) - : SubscriptionIntraProcessBase(topic_name, qos_profile), + : SubscriptionIntraProcessBuffer( + allocator, + context, + topic_name, + qos_profile, + buffer_type), any_callback_(callback) { - if (!std::is_same::value) { - throw std::runtime_error("SubscriptionIntraProcess wrong callback type"); - } - - // Create the intra-process buffer. - buffer_ = rclcpp::experimental::create_intra_process_buffer( - buffer_type, - qos_profile, - allocator); - - // Create the guard condition. - rcl_guard_condition_options_t guard_condition_options = - rcl_guard_condition_get_default_options(); - - gc_ = rcl_get_zero_initialized_guard_condition(); - rcl_ret_t ret = rcl_guard_condition_init( - &gc_, context->get_rcl_context().get(), guard_condition_options); - - if (RCL_RET_OK != ret) { - throw std::runtime_error("SubscriptionIntraProcess init error initializing guard condition"); - } - TRACEPOINT( rclcpp_subscription_callback_added, static_cast(this), @@ -104,22 +94,7 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBase #endif } - ~SubscriptionIntraProcess() - { - if (rcl_guard_condition_fini(&gc_) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Failed to destroy guard condition: %s", - rcutils_get_error_string().str); - } - } - - bool - is_ready(rcl_wait_set_t * wait_set) - { - (void) wait_set; - return buffer_->has_data(); - } + virtual ~SubscriptionIntraProcess() = default; std::shared_ptr take_data() @@ -128,9 +103,9 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBase MessageUniquePtr unique_msg; if (any_callback_.use_take_shared_method()) { - shared_msg = buffer_->consume_shared(); + shared_msg = this->buffer_->consume_shared(); } else { - unique_msg = buffer_->consume_unique(); + unique_msg = this->buffer_->consume_unique(); } return std::static_pointer_cast( std::make_shared>( @@ -141,37 +116,10 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBase void execute(std::shared_ptr & data) { - execute_impl(data); - } - - void - provide_intra_process_message(ConstMessageSharedPtr message) - { - buffer_->add_shared(std::move(message)); - trigger_guard_condition(); - } - - void - provide_intra_process_message(MessageUniquePtr message) - { - buffer_->add_unique(std::move(message)); - trigger_guard_condition(); - } - - bool - use_take_shared_method() const - { - return buffer_->use_take_shared_method(); - } - -private: - void - trigger_guard_condition() - { - rcl_ret_t ret = rcl_trigger_guard_condition(&gc_); - (void)ret; + execute_impl(data); } +protected: template typename std::enable_if::value, void>::type execute_impl(std::shared_ptr & data) @@ -206,7 +154,6 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBase } AnySubscriptionCallback any_callback_; - BufferUniquePtr buffer_; }; } // namespace experimental diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp index f40f997762..8c36e3bef6 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp @@ -16,7 +16,7 @@ #define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_ #include - +#include #include #include #include @@ -24,7 +24,10 @@ #include #include "rcl/error_handling.h" +#include "rmw/impl/cpp/demangle.hpp" +#include "rclcpp/guard_condition.hpp" +#include "rclcpp/logging.hpp" #include "rclcpp/type_support_decl.hpp" #include "rclcpp/waitable.hpp" @@ -38,32 +41,39 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable public: RCLCPP_SMART_PTR_ALIASES_ONLY(SubscriptionIntraProcessBase) + enum class EntityType + { + Subscription, + }; + RCLCPP_PUBLIC - SubscriptionIntraProcessBase(const std::string & topic_name, rmw_qos_profile_t qos_profile) - : topic_name_(topic_name), qos_profile_(qos_profile) + SubscriptionIntraProcessBase( + rclcpp::Context::SharedPtr context, + const std::string & topic_name, rmw_qos_profile_t qos_profile) + : gc_(context), topic_name_(topic_name), qos_profile_(qos_profile) {} virtual ~SubscriptionIntraProcessBase() = default; RCLCPP_PUBLIC size_t - get_number_of_ready_guard_conditions() {return 1;} + get_number_of_ready_guard_conditions() override {return 1;} RCLCPP_PUBLIC bool - add_to_wait_set(rcl_wait_set_t * wait_set); + add_to_wait_set(rcl_wait_set_t * wait_set) override; - virtual bool - is_ready(rcl_wait_set_t * wait_set) = 0; + bool + is_ready(rcl_wait_set_t * wait_set) override = 0; - virtual std::shared_ptr - take_data() = 0; + take_data() override = 0; - virtual void - execute(std::shared_ptr & data) = 0; + void + execute(std::shared_ptr & data) override = 0; - virtual bool + virtual + bool use_take_shared_method() const = 0; RCLCPP_PUBLIC @@ -74,15 +84,92 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable rmw_qos_profile_t get_actual_qos() const; - RCLCPP_PUBLIC + /// Set a callback to be called when each new message arrives. + /** + * The callback receives a size_t which is the number of messages received + * since the last time this callback was called. + * Normally this is 1, but can be > 1 if messages were received before any + * callback was set. + * + * The callback also receives an int identifier argument. + * This is needed because a Waitable may be composed of several distinct entities, + * such as subscriptions, services, etc. + * The application should provide a generic callback function that will be then + * forwarded by the waitable to all of its entities. + * Before forwarding, a different value for the identifier argument will be + * bond to the function. + * This implies that the provided callback can use the identifier to behave + * differently depending on which entity triggered the waitable to become ready. + * + * Calling it again will clear any previously set callback. + * + * An exception will be thrown if the callback is not callable. + * + * This function is thread-safe. + * + * If you want more information available in the callback, like the subscription + * or other information, you may use a lambda with captures or std::bind. + * + * \param[in] callback functor to be called when a new message is received. + */ + void + set_on_ready_callback(std::function callback) override + { + if (!callback) { + throw std::invalid_argument( + "The callback passed to set_on_ready_callback " + "is not callable."); + } + + // Note: we bind the int identifier argument to this waitable's entity types + auto new_callback = + [callback, this](size_t number_of_events) { + try { + callback(number_of_events, static_cast(EntityType::Subscription)); + } catch (const std::exception & exception) { + RCLCPP_ERROR_STREAM( + // TODO(wjwwood): get this class access to the node logger it is associated with + rclcpp::get_logger("rclcpp"), + "rclcpp::SubscriptionIntraProcessBase@" << this << + " caught " << rmw::impl::cpp::demangle(exception) << + " exception in user-provided callback for the 'on ready' callback: " << + exception.what()); + } catch (...) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("rclcpp"), + "rclcpp::SubscriptionIntraProcessBase@" << this << + " caught unhandled exception in user-provided callback " << + "for the 'on ready' callback"); + } + }; + + std::lock_guard lock(reentrant_mutex_); + on_new_message_callback_ = new_callback; + + if (unread_count_ > 0) { + if (qos_profile_.history == RMW_QOS_POLICY_HISTORY_KEEP_ALL || qos_profile_.depth == 0) { + on_new_message_callback_(unread_count_); + } else { + // Use qos profile depth as upper bound for unread_count_ + on_new_message_callback_(std::min(unread_count_, qos_profile_.depth)); + } + unread_count_ = 0; + } + } + + /// Unset the callback registered for new messages, if any. void - set_listener_callback( - rmw_listener_callback_t callback, - const void * user_data) const override; + clear_on_ready_callback() override + { + std::lock_guard lock(reentrant_mutex_); + on_new_message_callback_ = nullptr; + } protected: std::recursive_mutex reentrant_mutex_; - rcl_guard_condition_t gc_; + std::function on_new_message_callback_ {nullptr}; + size_t unread_count_{0}; + rclcpp::GuardCondition gc_; private: virtual void diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp new file mode 100644 index 0000000000..546e5b81db --- /dev/null +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp @@ -0,0 +1,132 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BUFFER_HPP_ +#define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BUFFER_HPP_ + +#include + +#include +#include +#include +#include +#include +#include + +#include "rcl/error_handling.h" + +#include "rclcpp/any_subscription_callback.hpp" +#include "rclcpp/experimental/buffers/intra_process_buffer.hpp" +#include "rclcpp/experimental/create_intra_process_buffer.hpp" +#include "rclcpp/experimental/subscription_intra_process_base.hpp" +#include "rclcpp/type_support_decl.hpp" +#include "rclcpp/waitable.hpp" +#include "tracetools/tracetools.h" + +namespace rclcpp +{ +namespace experimental +{ + +template< + typename MessageT, + typename Alloc = std::allocator, + typename Deleter = std::default_delete +> +class SubscriptionIntraProcessBuffer : public SubscriptionIntraProcessBase +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(SubscriptionIntraProcessBuffer) + + using MessageAllocTraits = allocator::AllocRebind; + using MessageAlloc = typename MessageAllocTraits::allocator_type; + using ConstMessageSharedPtr = std::shared_ptr; + using MessageUniquePtr = std::unique_ptr; + + using BufferUniquePtr = typename rclcpp::experimental::buffers::IntraProcessBuffer< + MessageT, + Alloc, + Deleter + >::UniquePtr; + + SubscriptionIntraProcessBuffer( + std::shared_ptr allocator, + rclcpp::Context::SharedPtr context, + const std::string & topic_name, + rmw_qos_profile_t qos_profile, + rclcpp::IntraProcessBufferType buffer_type) + : SubscriptionIntraProcessBase(context, topic_name, qos_profile) + { + // Create the intra-process buffer. + buffer_ = rclcpp::experimental::create_intra_process_buffer( + buffer_type, + qos_profile, + allocator); + } + + bool + is_ready(rcl_wait_set_t * wait_set) + { + (void) wait_set; + return buffer_->has_data(); + } + + void + provide_intra_process_message(ConstMessageSharedPtr message) + { + buffer_->add_shared(std::move(message)); + invoke_callback(); + trigger_guard_condition(); + } + + void + provide_intra_process_message(MessageUniquePtr message) + { + buffer_->add_unique(std::move(message)); + invoke_callback(); + trigger_guard_condition(); + } + + bool + use_take_shared_method() const + { + return buffer_->use_take_shared_method(); + } + +protected: + void + invoke_callback() + { + std::lock_guard lock(reentrant_mutex_); + if (on_new_message_callback_) { + on_new_message_callback_(1); + } else { + gc_.trigger(); + unread_count_++; + } + } + + void + trigger_guard_condition() + { + gc_.trigger(); + } + + BufferUniquePtr buffer_; +}; + +} // namespace experimental +} // namespace rclcpp + +#endif // RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BUFFER_HPP_ diff --git a/rclcpp/include/rclcpp/function_traits.hpp b/rclcpp/include/rclcpp/function_traits.hpp index f5c56b04e9..4004476842 100644 --- a/rclcpp/include/rclcpp/function_traits.hpp +++ b/rclcpp/include/rclcpp/function_traits.hpp @@ -162,6 +162,32 @@ struct same_arguments : std::is_same< > {}; +namespace detail +{ + +template +struct as_std_function_helper; + +template +struct as_std_function_helper> +{ + using type = std::function; +}; + +} // namespace detail + +template< + typename FunctorT, + typename FunctionTraits = function_traits +> +struct as_std_function +{ + using type = typename detail::as_std_function_helper< + typename FunctionTraits::return_type, + typename FunctionTraits::arguments + >::type; +}; + } // namespace function_traits } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/generic_publisher.hpp b/rclcpp/include/rclcpp/generic_publisher.hpp new file mode 100644 index 0000000000..7cadda5d1d --- /dev/null +++ b/rclcpp/include/rclcpp/generic_publisher.hpp @@ -0,0 +1,127 @@ +// Copyright 2018, Bosch Software Innovations GmbH. +// Copyright 2021, Apex.AI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__GENERIC_PUBLISHER_HPP_ +#define RCLCPP__GENERIC_PUBLISHER_HPP_ + +#include +#include + +#include "rcpputils/shared_library.hpp" + +#include "rclcpp/callback_group.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_topics_interface.hpp" +#include "rclcpp/publisher_base.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/serialized_message.hpp" +#include "rclcpp/typesupport_helpers.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ + +/// %Publisher for serialized messages whose type is not known at compile time. +/** + * Since the type is not known at compile time, this is not a template, and the dynamic library + * containing type support information has to be identified and loaded based on the type name. + * + * It does not support intra-process handling. + */ +class GenericPublisher : public rclcpp::PublisherBase +{ +public: + // cppcheck-suppress unknownMacro + RCLCPP_SMART_PTR_DEFINITIONS(GenericPublisher) + + /// Constructor. + /** + * In order to properly publish to a topic, this publisher needs to be added to + * the node_topic_interface of the node passed into this constructor. + * + * \sa rclcpp::Node::create_generic_publisher() or rclcpp::create_generic_publisher() for + * creating an instance of this class and adding it to the node_topic_interface. + * + * \param node_base Pointer to parent node's NodeBaseInterface + * \param ts_lib Type support library, needs to correspond to topic_type + * \param topic_name Topic name + * \param topic_type Topic type + * \param qos %QoS settings + * \param callback Callback for new messages of serialized form + * \param options %Publisher options. + * Not all publisher options are currently respected, the only relevant options for this + * publisher are `event_callbacks`, `use_default_callbacks`, and `%callback_group`. + */ + template> + GenericPublisher( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + std::shared_ptr ts_lib, + const std::string & topic_name, + const std::string & topic_type, + const rclcpp::QoS & qos, + const rclcpp::PublisherOptionsWithAllocator & options) + : rclcpp::PublisherBase( + node_base, + topic_name, + *rclcpp::get_typesupport_handle(topic_type, "rosidl_typesupport_cpp", *ts_lib), + options.template to_rcl_publisher_options(qos)), + ts_lib_(ts_lib) + { + // This is unfortunately duplicated with the code in publisher.hpp. + // TODO(nnmm): Deduplicate by moving this into PublisherBase. + if (options.event_callbacks.deadline_callback) { + this->add_event_handler( + options.event_callbacks.deadline_callback, + RCL_PUBLISHER_OFFERED_DEADLINE_MISSED); + } + if (options.event_callbacks.liveliness_callback) { + this->add_event_handler( + options.event_callbacks.liveliness_callback, + RCL_PUBLISHER_LIVELINESS_LOST); + } + if (options.event_callbacks.incompatible_qos_callback) { + this->add_event_handler( + options.event_callbacks.incompatible_qos_callback, + RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS); + } else if (options.use_default_callbacks) { + // Register default callback when not specified + try { + this->add_event_handler( + [this](QOSOfferedIncompatibleQoSInfo & info) { + this->default_incompatible_qos_callback(info); + }, + RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS); + } catch (UnsupportedEventTypeException & /*exc*/) { + // pass + } + } + } + + RCLCPP_PUBLIC + virtual ~GenericPublisher() = default; + + /// Publish a rclcpp::SerializedMessage. + RCLCPP_PUBLIC + void publish(const rclcpp::SerializedMessage & message); + +private: + // The type support library should stay loaded, so it is stored in the GenericPublisher + std::shared_ptr ts_lib_; +}; + +} // namespace rclcpp + +#endif // RCLCPP__GENERIC_PUBLISHER_HPP_ diff --git a/rclcpp/include/rclcpp/generic_subscription.hpp b/rclcpp/include/rclcpp/generic_subscription.hpp new file mode 100644 index 0000000000..673712eedb --- /dev/null +++ b/rclcpp/include/rclcpp/generic_subscription.hpp @@ -0,0 +1,168 @@ +// Copyright 2018, Bosch Software Innovations GmbH. +// Copyright 2021, Apex.AI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__GENERIC_SUBSCRIPTION_HPP_ +#define RCLCPP__GENERIC_SUBSCRIPTION_HPP_ + +#include +#include +#include + +#include "rcpputils/shared_library.hpp" + +#include "rclcpp/callback_group.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_topics_interface.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/serialized_message.hpp" +#include "rclcpp/subscription_base.hpp" +#include "rclcpp/typesupport_helpers.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ + +/// %Subscription for serialized messages whose type is not known at compile time. +/** + * Since the type is not known at compile time, this is not a template, and the dynamic library + * containing type support information has to be identified and loaded based on the type name. + * + * It does not support intra-process handling. + */ +class GenericSubscription : public rclcpp::SubscriptionBase +{ +public: + // cppcheck-suppress unknownMacro + RCLCPP_SMART_PTR_DEFINITIONS(GenericSubscription) + + /// Constructor. + /** + * In order to properly subscribe to a topic, this subscription needs to be added to + * the node_topic_interface of the node passed into this constructor. + * + * \sa rclcpp::Node::create_generic_subscription() or rclcpp::create_generic_subscription() for + * creating an instance of this class and adding it to the node_topic_interface. + * + * \param node_base Pointer to parent node's NodeBaseInterface + * \param ts_lib Type support library, needs to correspond to topic_type + * \param topic_name Topic name + * \param topic_type Topic type + * \param qos %QoS settings + * \param callback Callback for new messages of serialized form + * \param options %Subscription options. + * Not all subscription options are currently respected, the only relevant options for this + * subscription are `event_callbacks`, `use_default_callbacks`, `ignore_local_publications`, and + * `%callback_group`. + */ + template> + GenericSubscription( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + const std::shared_ptr ts_lib, + const std::string & topic_name, + const std::string & topic_type, + const rclcpp::QoS & qos, + // TODO(nnmm): Add variant for callback with message info. See issue #1604. + std::function)> callback, + const rclcpp::SubscriptionOptionsWithAllocator & options) + : SubscriptionBase( + node_base, + *rclcpp::get_typesupport_handle(topic_type, "rosidl_typesupport_cpp", *ts_lib), + topic_name, + options.template to_rcl_subscription_options(qos), + true), + callback_(callback), + ts_lib_(ts_lib) + { + // This is unfortunately duplicated with the code in subscription.hpp. + // TODO(nnmm): Deduplicate by moving this into SubscriptionBase. + if (options.event_callbacks.deadline_callback) { + this->add_event_handler( + options.event_callbacks.deadline_callback, + RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED); + } + if (options.event_callbacks.liveliness_callback) { + this->add_event_handler( + options.event_callbacks.liveliness_callback, + RCL_SUBSCRIPTION_LIVELINESS_CHANGED); + } + if (options.event_callbacks.incompatible_qos_callback) { + this->add_event_handler( + options.event_callbacks.incompatible_qos_callback, + RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS); + } else if (options.use_default_callbacks) { + // Register default callback when not specified + try { + this->add_event_handler( + [this](QOSRequestedIncompatibleQoSInfo & info) { + this->default_incompatible_qos_callback(info); + }, + RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS); + } catch (UnsupportedEventTypeException & /*exc*/) { + // pass + } + } + if (options.event_callbacks.message_lost_callback) { + this->add_event_handler( + options.event_callbacks.message_lost_callback, + RCL_SUBSCRIPTION_MESSAGE_LOST); + } + } + + RCLCPP_PUBLIC + virtual ~GenericSubscription() = default; + + // Same as create_serialized_message() as the subscription is to serialized_messages only + RCLCPP_PUBLIC + std::shared_ptr create_message() override; + + RCLCPP_PUBLIC + std::shared_ptr create_serialized_message() override; + + /// Cast the message to a rclcpp::SerializedMessage and call the callback. + RCLCPP_PUBLIC + void handle_message( + std::shared_ptr & message, const rclcpp::MessageInfo & message_info) override; + + /// Handle dispatching rclcpp::SerializedMessage to user callback. + RCLCPP_PUBLIC + void + handle_serialized_message( + const std::shared_ptr & serialized_message, + const rclcpp::MessageInfo & message_info) override; + + /// This function is currently not implemented. + RCLCPP_PUBLIC + void handle_loaned_message( + void * loaned_message, const rclcpp::MessageInfo & message_info) override; + + // Same as return_serialized_message() as the subscription is to serialized_messages only + RCLCPP_PUBLIC + void return_message(std::shared_ptr & message) override; + + RCLCPP_PUBLIC + void return_serialized_message(std::shared_ptr & message) override; + +private: + RCLCPP_DISABLE_COPY(GenericSubscription) + + std::function)> callback_; + // The type support library should stay loaded, so it is stored in the GenericSubscription + std::shared_ptr ts_lib_; +}; + +} // namespace rclcpp + +#endif // RCLCPP__GENERIC_SUBSCRIPTION_HPP_ diff --git a/rclcpp/include/rclcpp/get_message_type_support_handle.hpp b/rclcpp/include/rclcpp/get_message_type_support_handle.hpp new file mode 100644 index 0000000000..2f19528baf --- /dev/null +++ b/rclcpp/include/rclcpp/get_message_type_support_handle.hpp @@ -0,0 +1,90 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__GET_MESSAGE_TYPE_SUPPORT_HANDLE_HPP_ +#define RCLCPP__GET_MESSAGE_TYPE_SUPPORT_HANDLE_HPP_ + +#include + +#include "rosidl_runtime_cpp/traits.hpp" +#include "rosidl_runtime_cpp/message_type_support_decl.hpp" +#include "rosidl_typesupport_cpp/message_type_support.hpp" + +#include "rclcpp/type_adapter.hpp" + +/// Versions of rosidl_typesupport_cpp::get_message_type_support_handle that handle adapted types. + +namespace rclcpp +{ + +/// Specialization for when MessageT is an actual ROS message type. +template +constexpr +typename std::enable_if_t< + rosidl_generator_traits::is_message::value, + const rosidl_message_type_support_t & +> +get_message_type_support_handle() +{ + auto handle = rosidl_typesupport_cpp::get_message_type_support_handle(); + if (!handle) { + throw std::runtime_error("Type support handle unexpectedly nullptr"); + } + return *handle; +} + +/// Specialization for when MessageT is an adapted type using rclcpp::TypeAdapter. +template +constexpr +typename std::enable_if_t< + !rosidl_generator_traits::is_message::value && + rclcpp::TypeAdapter::is_specialized::value, + const rosidl_message_type_support_t & +> +get_message_type_support_handle() +{ + auto handle = rosidl_typesupport_cpp::get_message_type_support_handle< + typename TypeAdapter::ros_message_type + >(); + if (!handle) { + throw std::runtime_error("Type support handle unexpectedly nullptr"); + } + return *handle; +} + +/// Specialization for when MessageT is not a ROS message nor an adapted type. +/** + * This specialization is a pass through runtime check, which allows a better + * static_assert to catch this issue further down the line. + * This should never get to be called in practice, and is purely defensive. + */ +template< + typename AdaptedType +> +constexpr +typename std::enable_if_t< + !rosidl_generator_traits::is_message::value && + !TypeAdapter::is_specialized::value, + const rosidl_message_type_support_t & +> +get_message_type_support_handle() +{ + throw std::runtime_error( + "this specialization of rclcpp::get_message_type_support_handle() " + "should never be called"); +} + +} // namespace rclcpp + +#endif // RCLCPP__GET_MESSAGE_TYPE_SUPPORT_HANDLE_HPP_ diff --git a/rclcpp/include/rclcpp/graph_listener.hpp b/rclcpp/include/rclcpp/graph_listener.hpp index 30305ad2ff..79357b952e 100644 --- a/rclcpp/include/rclcpp/graph_listener.hpp +++ b/rclcpp/include/rclcpp/graph_listener.hpp @@ -24,6 +24,7 @@ #include "rcl/guard_condition.h" #include "rcl/wait.h" #include "rclcpp/context.hpp" +#include "rclcpp/guard_condition.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/node_interfaces/node_graph_interface.hpp" #include "rclcpp/visibility_control.hpp" @@ -187,7 +188,7 @@ class GraphListener : public std::enable_shared_from_this mutable std::mutex node_graph_interfaces_mutex_; std::vector node_graph_interfaces_; - rcl_guard_condition_t interrupt_guard_condition_ = rcl_get_zero_initialized_guard_condition(); + rclcpp::GuardCondition interrupt_guard_condition_; rcl_wait_set_t wait_set_ = rcl_get_zero_initialized_wait_set(); }; diff --git a/rclcpp/include/rclcpp/guard_condition.hpp b/rclcpp/include/rclcpp/guard_condition.hpp index 010c127a07..5ad2d5a757 100644 --- a/rclcpp/include/rclcpp/guard_condition.hpp +++ b/rclcpp/include/rclcpp/guard_condition.hpp @@ -47,7 +47,9 @@ class GuardCondition RCLCPP_PUBLIC explicit GuardCondition( rclcpp::Context::SharedPtr context = - rclcpp::contexts::get_global_default_context()); + rclcpp::contexts::get_global_default_context(), + rcl_guard_condition_options_t guard_condition_options = + rcl_guard_condition_get_default_options()); RCLCPP_PUBLIC virtual @@ -89,10 +91,25 @@ class GuardCondition bool exchange_in_use_by_wait_set_state(bool in_use_state); + /// Adds the guard condition to a waitset + /** + * This function is thread-safe. + * \param[in] wait_set pointer to a wait set where to add the guard condition + */ + RCLCPP_PUBLIC + void + add_to_wait_set(rcl_wait_set_t * wait_set) const; + + RCLCPP_PUBLIC + void + set_on_trigger_callback(std::function callback); + protected: rclcpp::Context::SharedPtr context_; rcl_guard_condition_t rcl_guard_condition_; std::atomic in_use_by_wait_set_{false}; + std::function on_trigger_callback_{nullptr}; + size_t unread_count_{0}; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/is_ros_compatible_type.hpp b/rclcpp/include/rclcpp/is_ros_compatible_type.hpp new file mode 100644 index 0000000000..97c6ad6a27 --- /dev/null +++ b/rclcpp/include/rclcpp/is_ros_compatible_type.hpp @@ -0,0 +1,35 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__IS_ROS_COMPATIBLE_TYPE_HPP_ +#define RCLCPP__IS_ROS_COMPATIBLE_TYPE_HPP_ + +#include "rosidl_runtime_cpp/traits.hpp" + +#include "rclcpp/type_adapter.hpp" + +namespace rclcpp +{ + +template +struct is_ros_compatible_type +{ + static constexpr bool value = + rosidl_generator_traits::is_message::value || + rclcpp::TypeAdapter::is_specialized::value; +}; + +} // namespace rclcpp + +#endif // RCLCPP__IS_ROS_COMPATIBLE_TYPE_HPP_ diff --git a/rclcpp/include/rclcpp/loaned_message.hpp b/rclcpp/include/rclcpp/loaned_message.hpp index 1d7b0d5e4f..4dd396e033 100644 --- a/rclcpp/include/rclcpp/loaned_message.hpp +++ b/rclcpp/include/rclcpp/loaned_message.hpp @@ -103,6 +103,9 @@ class LoanedMessage * \param[in] allocator Allocator instance in case middleware can not allocate messages * \throws anything rclcpp::exceptions::throw_from_rcl_error can throw. */ + [[ + deprecated("used the LoanedMessage constructor that does not use a shared_ptr to the allocator") + ]] LoanedMessage( const rclcpp::PublisherBase * pub, std::shared_ptr> allocator) diff --git a/rclcpp/include/rclcpp/memory_strategy.hpp b/rclcpp/include/rclcpp/memory_strategy.hpp index da79643e4e..324a88430c 100644 --- a/rclcpp/include/rclcpp/memory_strategy.hpp +++ b/rclcpp/include/rclcpp/memory_strategy.hpp @@ -64,9 +64,11 @@ class RCLCPP_PUBLIC MemoryStrategy virtual void clear_handles() = 0; virtual void remove_null_handles(rcl_wait_set_t * wait_set) = 0; - virtual void add_guard_condition(const rcl_guard_condition_t * guard_condition) = 0; + virtual void + add_guard_condition(const rclcpp::GuardCondition * guard_condition) = 0; - virtual void remove_guard_condition(const rcl_guard_condition_t * guard_condition) = 0; + virtual void + remove_guard_condition(const rclcpp::GuardCondition * guard_condition) = 0; virtual void get_next_subscription( diff --git a/rclcpp/include/rclcpp/network_flow_endpoint.hpp b/rclcpp/include/rclcpp/network_flow_endpoint.hpp new file mode 100644 index 0000000000..a4f0b1c666 --- /dev/null +++ b/rclcpp/include/rclcpp/network_flow_endpoint.hpp @@ -0,0 +1,115 @@ +// Copyright 2020 Ericsson AB +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__NETWORK_FLOW_ENDPOINT_HPP_ +#define RCLCPP__NETWORK_FLOW_ENDPOINT_HPP_ + +#include +#include +#include + +#include "rcl/network_flow_endpoints.h" + +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ + +/// Forward declaration +class NetworkFlowEndpoint; + +/// Check if two NetworkFlowEndpoint instances are equal +RCLCPP_PUBLIC +bool operator==(const NetworkFlowEndpoint & left, const NetworkFlowEndpoint & right); + +/// Check if two NetworkFlowEndpoint instances are not equal +RCLCPP_PUBLIC +bool operator!=(const NetworkFlowEndpoint & left, const NetworkFlowEndpoint & right); + +/// Streaming helper for NetworkFlowEndpoint +RCLCPP_PUBLIC +std::ostream & operator<<(std::ostream & os, const NetworkFlowEndpoint & network_flow_endpoint); + +/** + * Class describes a network flow endpoint based on the counterpart definition + * in the RMW layer. + */ +class NetworkFlowEndpoint +{ +public: + /// Construct from rcl_network_flow_endpoint_t + RCLCPP_PUBLIC + explicit NetworkFlowEndpoint(const rcl_network_flow_endpoint_t & network_flow_endpoint) + : transport_protocol_( + rcl_network_flow_endpoint_get_transport_protocol_string(network_flow_endpoint. + transport_protocol)), + internet_protocol_( + rcl_network_flow_endpoint_get_internet_protocol_string( + network_flow_endpoint.internet_protocol)), + transport_port_(network_flow_endpoint.transport_port), + flow_label_(network_flow_endpoint.flow_label), + dscp_(network_flow_endpoint.dscp), + internet_address_(network_flow_endpoint.internet_address) + { + } + + /// Get transport protocol + RCLCPP_PUBLIC + const std::string & transport_protocol() const; + + /// Get internet protocol + RCLCPP_PUBLIC + const std::string & internet_protocol() const; + + /// Get transport port + RCLCPP_PUBLIC + uint16_t transport_port() const; + + /// Get flow label + RCLCPP_PUBLIC + uint32_t flow_label() const; + + /// Get DSCP + RCLCPP_PUBLIC + uint8_t dscp() const; + + /// Get internet address + RCLCPP_PUBLIC + const std::string & internet_address() const; + + /// Compare two NetworkFlowEndpoint instances + friend bool rclcpp::operator==( + const NetworkFlowEndpoint & left, + const NetworkFlowEndpoint & right); + friend bool rclcpp::operator!=( + const NetworkFlowEndpoint & left, + const NetworkFlowEndpoint & right); + + /// Streaming helper + friend std::ostream & rclcpp::operator<<( + std::ostream & os, + const NetworkFlowEndpoint & network_flow_endpoint); + +private: + std::string transport_protocol_; + std::string internet_protocol_; + uint16_t transport_port_; + uint32_t flow_label_; + uint8_t dscp_; + std::string internet_address_; +}; + +} // namespace rclcpp + +#endif // RCLCPP__NETWORK_FLOW_ENDPOINT_HPP_ diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 5ff9546ec3..2e44c6c60b 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include #include @@ -41,6 +42,8 @@ #include "rclcpp/clock.hpp" #include "rclcpp/context.hpp" #include "rclcpp/event.hpp" +#include "rclcpp/generic_publisher.hpp" +#include "rclcpp/generic_subscription.hpp" #include "rclcpp/logger.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/message_memory_strategy.hpp" @@ -203,13 +206,8 @@ class Node : public std::enable_shared_from_this typename MessageT, typename CallbackT, typename AllocatorT = std::allocator, - typename CallbackMessageT = - typename rclcpp::subscription_traits::has_message_type::type, - typename SubscriptionT = rclcpp::Subscription, - typename MessageMemoryStrategyT = rclcpp::message_memory_strategy::MessageMemoryStrategy< - CallbackMessageT, - AllocatorT - > + typename SubscriptionT = rclcpp::Subscription, + typename MessageMemoryStrategyT = typename SubscriptionT::MessageMemoryStrategyType > std::shared_ptr create_subscription( @@ -266,6 +264,55 @@ class Node : public std::enable_shared_from_this const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default, rclcpp::CallbackGroup::SharedPtr group = nullptr); + /// Create and return a GenericPublisher. + /** + * The returned pointer will never be empty, but this function can throw various exceptions, for + * instance when the message's package can not be found on the AMENT_PREFIX_PATH. + * + * \param[in] topic_name Topic name + * \param[in] topic_type Topic type + * \param[in] qos %QoS settings + * \param options %Publisher options. + * Not all publisher options are currently respected, the only relevant options for this + * publisher are `event_callbacks`, `use_default_callbacks`, and `%callback_group`. + * \return Shared pointer to the created generic publisher. + */ + template> + std::shared_ptr create_generic_publisher( + const std::string & topic_name, + const std::string & topic_type, + const rclcpp::QoS & qos, + const rclcpp::PublisherOptionsWithAllocator & options = ( + rclcpp::PublisherOptionsWithAllocator() + ) + ); + + /// Create and return a GenericSubscription. + /** + * The returned pointer will never be empty, but this function can throw various exceptions, for + * instance when the message's package can not be found on the AMENT_PREFIX_PATH. + * + * \param[in] topic_name Topic name + * \param[in] topic_type Topic type + * \param[in] qos %QoS settings + * \param[in] callback Callback for new messages of serialized form + * \param[in] options %Subscription options. + * Not all subscription options are currently respected, the only relevant options for this + * subscription are `event_callbacks`, `use_default_callbacks`, `ignore_local_publications`, and + * `%callback_group`. + * \return Shared pointer to the created generic subscription. + */ + template> + std::shared_ptr create_generic_subscription( + const std::string & topic_name, + const std::string & topic_type, + const rclcpp::QoS & qos, + std::function)> callback, + const rclcpp::SubscriptionOptionsWithAllocator & options = ( + rclcpp::SubscriptionOptionsWithAllocator() + ) + ); + /// Declare and initialize a parameter, return the effective value. /** * This method is used to declare that a parameter exists on this node. @@ -840,6 +887,9 @@ class Node : public std::enable_shared_from_this * * This allows the node developer to control which parameters may be changed. * + * It is considered bad practice to reject changes for "unknown" parameters as this prevents + * other parts of the node (that may be aware of these parameters) from handling them. + * * Note that the callback is called when declare_parameter() and its variants * are called, and so you cannot assume the parameter has been set before * this callback, so when checking a new value against the existing one, you diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 8ed078113f..df6039cf49 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -36,10 +36,12 @@ #include "rclcpp/contexts/default_context.hpp" #include "rclcpp/create_client.hpp" +#include "rclcpp/create_generic_publisher.hpp" +#include "rclcpp/create_generic_subscription.hpp" #include "rclcpp/create_publisher.hpp" #include "rclcpp/create_service.hpp" -#include "rclcpp/create_timer.hpp" #include "rclcpp/create_subscription.hpp" +#include "rclcpp/create_timer.hpp" #include "rclcpp/detail/resolve_enable_topic_statistics.hpp" #include "rclcpp/parameter.hpp" #include "rclcpp/qos.hpp" @@ -84,7 +86,6 @@ template< typename MessageT, typename CallbackT, typename AllocatorT, - typename CallbackMessageT, typename SubscriptionT, typename MessageMemoryStrategyT> std::shared_ptr @@ -152,6 +153,43 @@ Node::create_service( group); } +template +std::shared_ptr +Node::create_generic_publisher( + const std::string & topic_name, + const std::string & topic_type, + const rclcpp::QoS & qos, + const rclcpp::PublisherOptionsWithAllocator & options) +{ + return rclcpp::create_generic_publisher( + node_topics_, + extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()), + topic_type, + qos, + options + ); +} + +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, + const rclcpp::SubscriptionOptionsWithAllocator & options) +{ + return rclcpp::create_generic_subscription( + node_topics_, + extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()), + topic_type, + qos, + std::move(callback), + options + ); +} + + template auto Node::declare_parameter( diff --git a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp index 3f8b0021a0..bfa9430de0 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp @@ -104,9 +104,13 @@ class NodeBase : public NodeBaseInterface get_associated_with_executor_atomic() override; RCLCPP_PUBLIC - rcl_guard_condition_t * + const rcl_guard_condition_t * get_notify_guard_condition() override; + RCLCPP_PUBLIC + rclcpp::GuardCondition * + get_notify_rclcpp_guard_condition() override; + RCLCPP_PUBLIC std::unique_lock acquire_notify_guard_condition_lock() const override; @@ -122,6 +126,10 @@ class NodeBase : public NodeBaseInterface resolve_topic_or_service_name( const std::string & name, bool is_service, bool only_expand = false) const override; + RCLCPP_PUBLIC + void + trigger_notify_guard_condition() override; + private: RCLCPP_DISABLE_COPY(NodeBase) @@ -138,7 +146,7 @@ class NodeBase : public NodeBaseInterface /// Guard condition for notifying the Executor of changes to this node. mutable std::recursive_mutex notify_guard_condition_mutex_; - rcl_guard_condition_t notify_guard_condition_ = rcl_get_zero_initialized_guard_condition(); + rclcpp::GuardCondition notify_guard_condition_; bool notify_guard_condition_is_valid_; }; diff --git a/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp index a9c30dd09a..e13b2ceb6a 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp @@ -24,6 +24,7 @@ #include "rclcpp/callback_group.hpp" #include "rclcpp/context.hpp" +#include "rclcpp/guard_condition.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/visibility_control.hpp" @@ -142,9 +143,20 @@ class NodeBaseInterface */ RCLCPP_PUBLIC virtual - rcl_guard_condition_t * + const rcl_guard_condition_t * get_notify_guard_condition() = 0; + /// Return guard condition that should be notified when the internal node state changes. + /** + * For example, this should be notified when a publisher is added or removed. + * + * \return the GuardCondition if it is valid, else nullptr + */ + RCLCPP_PUBLIC + virtual + rclcpp::GuardCondition * + get_notify_rclcpp_guard_condition() = 0; + /// Acquire and return a scoped lock that protects the notify guard condition. /** This should be used when triggering the notify guard condition. */ RCLCPP_PUBLIC @@ -170,6 +182,12 @@ class NodeBaseInterface std::string resolve_topic_or_service_name( const std::string & name, bool is_service, bool only_expand = false) const = 0; + + /// Trigger the node's notify guard condition. + RCLCPP_PUBLIC + virtual + void + trigger_notify_guard_condition() = 0; }; } // namespace node_interfaces diff --git a/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp index a9c054ad7f..d9981516c7 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp @@ -130,7 +130,7 @@ class NodeParametersInterface std::vector set_parameters(const std::vector & parameters) = 0; - /// Set and initialize a parameter, all at once. + /// Set one or more parameters, all at once. /** * \sa rclcpp::Node::set_parameters_atomically */ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_time_source.hpp b/rclcpp/include/rclcpp/node_interfaces/node_time_source.hpp index b30e393d6e..1c9afebe1f 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_time_source.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_time_source.hpp @@ -48,7 +48,8 @@ class NodeTimeSource : public NodeTimeSourceInterface rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock, rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters, - const rclcpp::QoS & qos = rclcpp::RosoutQoS() + const rclcpp::QoS & qos = rclcpp::RosoutQoS(), + bool use_clock_thread = true ); RCLCPP_PUBLIC diff --git a/rclcpp/include/rclcpp/node_options.hpp b/rclcpp/include/rclcpp/node_options.hpp index 7d6944c767..73a2c701af 100644 --- a/rclcpp/include/rclcpp/node_options.hpp +++ b/rclcpp/include/rclcpp/node_options.hpp @@ -47,6 +47,7 @@ class NodeOptions * - start_parameter_services = true * - start_parameter_event_publisher = true * - clock_qos = rclcpp::ClockQoS() + * - use_clock_thread = true * - rosout_qos = rclcpp::RosoutQoS() * - parameter_event_qos = rclcpp::ParameterEventQoS * - with history setting and depth from rmw_qos_profile_parameter_events @@ -258,6 +259,20 @@ class NodeOptions NodeOptions & clock_qos(const rclcpp::QoS & clock_qos); + + /// Return the use_clock_thread flag. + RCLCPP_PUBLIC + bool + use_clock_thread() const; + + /// Set the use_clock_thread flag, return this for parameter idiom. + /** + * If true, a dedicated thread will be used to subscribe to "/clock" topic. + */ + RCLCPP_PUBLIC + NodeOptions & + use_clock_thread(bool use_clock_thread); + /// Return a reference to the parameter_event_qos QoS. RCLCPP_PUBLIC const rclcpp::QoS & @@ -384,6 +399,8 @@ class NodeOptions rclcpp::QoS clock_qos_ = rclcpp::ClockQoS(); + bool use_clock_thread_ {true}; + rclcpp::QoS parameter_event_qos_ = rclcpp::ParameterEventsQoS( rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_parameter_events) ); diff --git a/rclcpp/include/rclcpp/parameter_client.hpp b/rclcpp/include/rclcpp/parameter_client.hpp index c80d30f109..4a15a35141 100644 --- a/rclcpp/include/rclcpp/parameter_client.hpp +++ b/rclcpp/include/rclcpp/parameter_client.hpp @@ -31,11 +31,14 @@ #include "rcl_interfaces/srv/list_parameters.hpp" #include "rcl_interfaces/srv/set_parameters.hpp" #include "rcl_interfaces/srv/set_parameters_atomically.hpp" +#include "rcl_yaml_param_parser/parser.h" +#include "rclcpp/exceptions.hpp" #include "rclcpp/executors.hpp" #include "rclcpp/create_subscription.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/node.hpp" #include "rclcpp/parameter.hpp" +#include "rclcpp/parameter_map.hpp" #include "rclcpp/type_support_decl.hpp" #include "rclcpp/visibility_control.hpp" #include "rmw/rmw.h" @@ -154,6 +157,42 @@ class AsyncParametersClient void(std::shared_future) > callback = nullptr); + /// Delete several parameters at once. + /** + * This function behaves like command-line tool `ros2 param delete` would. + * + * \param parameters_names vector of parameters names + * \return the future of the set_parameter service used to delete the parameters + */ + RCLCPP_PUBLIC + std::shared_future> + delete_parameters( + const std::vector & parameters_names); + + /// Load parameters from yaml file. + /** + * This function behaves like command-line tool `ros2 param load` would. + * + * \param yaml_filename the full name of the yaml file + * \return the future of the set_parameter service used to load the parameters + */ + RCLCPP_PUBLIC + std::shared_future> + load_parameters( + const std::string & yaml_filename); + + /// Load parameters from parameter map. + /** + * This function filters the parameters to be set based on the node name. + * + * \param yaml_filename the full name of the yaml file + * \return the future of the set_parameter service used to load the parameters + * \throw InvalidParametersException if there is no parameter to set + */ + RCLCPP_PUBLIC + std::shared_future> + load_parameters(const rclcpp::ParameterMap & parameter_map); + RCLCPP_PUBLIC std::shared_future list_parameters( @@ -444,6 +483,46 @@ class SyncParametersClient ); } + /// Delete several parameters at once. + /** + * This function behaves like command-line tool `ros2 param delete` would. + * + * \param parameters_names vector of parameters names + * \param timeout for the spin used to make it synchronous + * \return the future of the set_parameter service used to delete the parameters + */ + template + std::vector + delete_parameters( + const std::vector & parameters_names, + std::chrono::duration timeout = std::chrono::duration(-1)) + { + return delete_parameters( + parameters_names, + std::chrono::duration_cast(timeout) + ); + } + + /// Load parameters from yaml file. + /** + * This function behaves like command-line tool `ros2 param load` would. + * + * \param yaml_filename the full name of the yaml file + * \param timeout for the spin used to make it synchronous + * \return the future of the set_parameter service used to load the parameters + */ + template + std::vector + load_parameters( + const std::string & yaml_filename, + std::chrono::duration timeout = std::chrono::duration(-1)) + { + return load_parameters( + yaml_filename, + std::chrono::duration_cast(timeout) + ); + } + template rcl_interfaces::msg::ListParametersResult list_parameters( @@ -524,6 +603,18 @@ class SyncParametersClient const std::vector & parameters, std::chrono::nanoseconds timeout); + RCLCPP_PUBLIC + std::vector + delete_parameters( + const std::vector & parameters_names, + std::chrono::nanoseconds timeout); + + RCLCPP_PUBLIC + std::vector + load_parameters( + const std::string & yaml_filename, + std::chrono::nanoseconds timeout); + RCLCPP_PUBLIC rcl_interfaces::msg::SetParametersResult set_parameters_atomically( diff --git a/rclcpp/include/rclcpp/parameter_event_handler.hpp b/rclcpp/include/rclcpp/parameter_event_handler.hpp index 9a2e65b15e..c7b781f0ec 100644 --- a/rclcpp/include/rclcpp/parameter_event_handler.hpp +++ b/rclcpp/include/rclcpp/parameter_event_handler.hpp @@ -52,7 +52,7 @@ struct ParameterEventCallbackHandle RCLCPP_SMART_PTR_DEFINITIONS(ParameterEventCallbackHandle) using ParameterEventCallbackType = - std::function; + std::function; ParameterEventCallbackType callback; }; @@ -115,16 +115,16 @@ struct ParameterEventCallbackHandle * For example: * * auto cb3 = - * [fqn, remote_param_name, &node](const rcl_interfaces::msg::ParameterEvent::SharedPtr & event) { + * [fqn, remote_param_name, &node](const rcl_interfaces::msg::ParameterEvent & event) { * // Look for any updates to parameters in "/a_namespace" as well as any parameter changes * // to our own node ("this_node") * std::regex re("(/a_namespace/.*)|(/this_node)"); - * if (regex_match(event->node, re)) { + * if (regex_match(event.node, re)) { * // Now that we know the event matches the regular expression we scanned for, we can * // use 'get_parameter_from_event' to get a specific parameter name that we're looking for * rclcpp::Parameter p; * if (rclcpp::ParameterEventsSubscriber::get_parameter_from_event( - * *event, p, remote_param_name, fqn)) + * event, p, remote_param_name, fqn)) * { * RCLCPP_INFO( * node->get_logger(), @@ -136,7 +136,7 @@ struct ParameterEventCallbackHandle * * // You can also use 'get_parameter*s*_from_event' to enumerate all changes that came * // in on this event - * auto params = rclcpp::ParameterEventsSubscriber::get_parameters_from_event(*event); + * auto params = rclcpp::ParameterEventsSubscriber::get_parameters_from_event(event); * for (auto & p : params) { * RCLCPP_INFO( * node->get_logger(), @@ -288,7 +288,7 @@ class ParameterEventHandler /// Callback for parameter events subscriptions. RCLCPP_PUBLIC void - event_callback(const rcl_interfaces::msg::ParameterEvent::SharedPtr event); + event_callback(const rcl_interfaces::msg::ParameterEvent & event); // Utility function for resolving node path. std::string resolve_path(const std::string & path); diff --git a/rclcpp/include/rclcpp/parameter_events_filter.hpp b/rclcpp/include/rclcpp/parameter_events_filter.hpp index d09d287762..3aa70d8a85 100644 --- a/rclcpp/include/rclcpp/parameter_events_filter.hpp +++ b/rclcpp/include/rclcpp/parameter_events_filter.hpp @@ -37,7 +37,7 @@ class ParameterEventsFilter RCLCPP_SMART_PTR_DEFINITIONS(ParameterEventsFilter) enum class EventType {NEW, DELETED, CHANGED}; ///< An enum for the type of event. /// Used for the listed results - using EventPair = std::pair; + using EventPair = std::pair; /// Construct a filtered view of a parameter event. /** @@ -60,7 +60,7 @@ class ParameterEventsFilter */ RCLCPP_PUBLIC ParameterEventsFilter( - rcl_interfaces::msg::ParameterEvent::SharedPtr event, + std::shared_ptr event, const std::vector & names, const std::vector & types); @@ -74,7 +74,7 @@ class ParameterEventsFilter private: // access only allowed via const accessor. std::vector result_; ///< Storage of the resultant vector - rcl_interfaces::msg::ParameterEvent::SharedPtr event_; ///< Keep event in scope + std::shared_ptr event_; ///< Keep event in scope }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/parameter_map.hpp b/rclcpp/include/rclcpp/parameter_map.hpp index 09b59776a6..9cfcdaaacf 100644 --- a/rclcpp/include/rclcpp/parameter_map.hpp +++ b/rclcpp/include/rclcpp/parameter_map.hpp @@ -15,6 +15,7 @@ #ifndef RCLCPP__PARAMETER_MAP_HPP_ #define RCLCPP__PARAMETER_MAP_HPP_ +#include #include #include @@ -48,6 +49,14 @@ RCLCPP_PUBLIC ParameterValue parameter_value_from(const rcl_variant_t * const c_value); +/// Get the ParameterMap from a yaml file. +/// \param[in] yaml_filename full name of the yaml file. +/// \returns an instance of a parameter map +/// \throws from rcl error of rcl_parse_yaml_file() +RCLCPP_PUBLIC +ParameterMap +parameter_map_from_yaml_file(const std::string & yaml_filename); + } // namespace rclcpp #endif // RCLCPP__PARAMETER_MAP_HPP_ diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 14b2f333c1..c7f77420c4 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -15,9 +15,6 @@ #ifndef RCLCPP__PUBLISHER_HPP_ #define RCLCPP__PUBLISHER_HPP_ -#include -#include - #include #include #include @@ -27,19 +24,26 @@ #include "rcl/error_handling.h" #include "rcl/publisher.h" +#include "rmw/error_handling.h" +#include "rmw/rmw.h" #include "rclcpp/allocator/allocator_common.hpp" #include "rclcpp/allocator/allocator_deleter.hpp" #include "rclcpp/detail/resolve_use_intra_process.hpp" #include "rclcpp/experimental/intra_process_manager.hpp" +#include "rclcpp/get_message_type_support_handle.hpp" +#include "rclcpp/is_ros_compatible_type.hpp" #include "rclcpp/loaned_message.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/publisher_base.hpp" #include "rclcpp/publisher_options.hpp" +#include "rclcpp/type_adapter.hpp" #include "rclcpp/type_support_decl.hpp" #include "rclcpp/visibility_control.hpp" +#include "tracetools/tracetools.h" + namespace rclcpp { @@ -47,15 +51,62 @@ template class LoanedMessage; /// A publisher publishes messages of any type to a topic. +/** + * MessageT must be a: + * + * - ROS message type with its own type support (e.g. std_msgs::msgs::String), or a + * - rclcpp::TypeAdapter + * (e.g. rclcpp::TypeAdapter type + * (e.g. TypeAdapter), PublishedType will + * be the custom type, and ROSMessageType will be the ros message type. + * + * This is achieved because of the "identity specialization" for TypeAdapter, + * which returns itself if it is already a TypeAdapter, and the default + * specialization which allows ROSMessageType to be void. + * \sa rclcpp::TypeAdapter for more details. + */ template> class Publisher : public PublisherBase { public: - using MessageAllocatorTraits = allocator::AllocRebind; - using MessageAllocator = typename MessageAllocatorTraits::allocator_type; - using MessageDeleter = allocator::Deleter; - using MessageUniquePtr = std::unique_ptr; - using MessageSharedPtr = std::shared_ptr; + static_assert( + rclcpp::is_ros_compatible_type::value, + "given message type is not compatible with ROS and cannot be used with a Publisher"); + + /// MessageT::custom_type if MessageT is a TypeAdapter, otherwise just MessageT. + using PublishedType = typename rclcpp::TypeAdapter::custom_type; + /// MessageT::ros_message_type if MessageT is a TypeAdapter, otherwise just MessageT. + using ROSMessageType = typename rclcpp::TypeAdapter::ros_message_type; + + using PublishedTypeAllocatorTraits = allocator::AllocRebind; + using PublishedTypeAllocator = typename PublishedTypeAllocatorTraits::allocator_type; + using PublishedTypeDeleter = allocator::Deleter; + + using ROSMessageTypeAllocatorTraits = allocator::AllocRebind; + using ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type; + using ROSMessageTypeDeleter = allocator::Deleter; + + using MessageAllocatorTraits + [[deprecated("use PublishedTypeAllocatorTraits")]] = + PublishedTypeAllocatorTraits; + using MessageAllocator + [[deprecated("use PublishedTypeAllocator")]] = + PublishedTypeAllocator; + using MessageDeleter + [[deprecated("use PublishedTypeDeleter")]] = + PublishedTypeDeleter; + using MessageUniquePtr + [[deprecated("use std::unique_ptr")]] = + std::unique_ptr; + using MessageSharedPtr + [[deprecated("use std::shared_ptr")]] = + std::shared_ptr; RCLCPP_SMART_PTR_DEFINITIONS(Publisher) @@ -78,12 +129,14 @@ class Publisher : public PublisherBase : PublisherBase( node_base, topic, - *rosidl_typesupport_cpp::get_message_type_support_handle(), + rclcpp::get_message_type_support_handle(), options.template to_rcl_publisher_options(qos)), options_(options), - message_allocator_(new MessageAllocator(*options.get_allocator().get())) + published_type_allocator_(*options.get_allocator()), + ros_message_type_allocator_(*options.get_allocator()) { - allocator::set_allocator_for_deleter(&message_deleter_, message_allocator_.get()); + allocator::set_allocator_for_deleter(&published_type_deleter_, &published_type_allocator_); + allocator::set_allocator_for_deleter(&ros_message_type_deleter_, &ros_message_type_allocator_); if (options_.event_callbacks.deadline_callback) { this->add_event_handler( @@ -168,21 +221,33 @@ class Publisher : public PublisherBase * allocator. * \sa rclcpp::LoanedMessage for details of the LoanedMessage class. * - * \return LoanedMessage containing memory for a ROS message of type MessageT + * \return LoanedMessage containing memory for a ROS message of type ROSMessageType */ - rclcpp::LoanedMessage + rclcpp::LoanedMessage borrow_loaned_message() { - return rclcpp::LoanedMessage(this, this->get_allocator()); + return rclcpp::LoanedMessage( + *this, + this->get_ros_message_type_allocator()); } - /// Send a message to the topic for this publisher. + /// Publish a message on the topic. /** - * This function is templated on the input message type, MessageT. - * \param[in] msg A shared pointer to the message to send. + * This signature is enabled if the element_type of the std::unique_ptr is + * a ROS message type, as opposed to the custom_type of a TypeAdapter, and + * that type matches the type given when creating the publisher. + * + * This signature allows the user to give ownership of the message to rclcpp, + * allowing for more efficient intra-process communication optimizations. + * + * \param[in] msg A unique pointer to the message to send. */ - virtual void - publish(std::unique_ptr msg) + template + typename std::enable_if_t< + rosidl_generator_traits::is_message::value && + std::is_same::value + > + publish(std::unique_ptr msg) { if (!intra_process_is_enabled_) { this->do_inter_process_publish(*msg); @@ -205,8 +270,24 @@ class Publisher : public PublisherBase } } - virtual void - publish(const MessageT & msg) + /// Publish a message on the topic. + /** + * This signature is enabled if the object being published is + * a ROS message type, as opposed to the custom_type of a TypeAdapter, and + * that type matches the type given when creating the publisher. + * + * This signature allows the user to give a reference to a message, which is + * copied onto the heap without modification so that a copy can be owned by + * rclcpp and ownership of the copy can be moved later if needed. + * + * \param[in] msg A const reference to the message to send. + */ + template + typename std::enable_if_t< + rosidl_generator_traits::is_message::value && + std::is_same::value + > + publish(const T & msg) { // Avoid allocating when not using intra process. if (!intra_process_is_enabled_) { @@ -216,12 +297,77 @@ class Publisher : public PublisherBase // Otherwise we have to allocate memory in a unique_ptr and pass it along. // As the message is not const, a copy should be made. // A shared_ptr could also be constructed here. - auto ptr = MessageAllocatorTraits::allocate(*message_allocator_.get(), 1); - MessageAllocatorTraits::construct(*message_allocator_.get(), ptr, msg); - MessageUniquePtr unique_msg(ptr, message_deleter_); + auto unique_msg = this->duplicate_ros_message_as_unique_ptr(msg); this->publish(std::move(unique_msg)); } + /// Publish a message on the topic. + /** + * This signature is enabled if this class was created with a TypeAdapter and + * the element_type of the std::unique_ptr matches the custom_type for the + * TypeAdapter used with this class. + * + * This signature allows the user to give ownership of the message to rclcpp, + * allowing for more efficient intra-process communication optimizations. + * + * \param[in] msg A unique pointer to the message to send. + */ + template + typename std::enable_if_t< + rclcpp::TypeAdapter::is_specialized::value && + std::is_same::value + > + publish(std::unique_ptr msg) + { + // TODO(wjwwood): later update this to give the unique_ptr to the intra + // process manager and let it decide if it needs to be converted or not. + // For now, convert it unconditionally and pass it the ROSMessageType + // publish function specialization. + auto unique_ros_msg = this->create_ros_message_unique_ptr(); + rclcpp::TypeAdapter::convert_to_ros_message(*msg, *unique_ros_msg); + this->publish(std::move(unique_ros_msg)); + } + + /// Publish a message on the topic. + /** + * This signature is enabled if this class was created with a TypeAdapter and + * the given type matches the custom_type of the TypeAdapter. + * + * This signature allows the user to give a reference to a message, which is + * copied onto the heap without modification so that a copy can be owned by + * rclcpp and ownership of the copy can be moved later if needed. + * + * \param[in] msg A const reference to the message to send. + */ + template + typename std::enable_if_t< + rclcpp::TypeAdapter::is_specialized::value && + std::is_same::value + > + publish(const T & msg) + { + // TODO(wjwwood): later update this to give the unique_ptr to the intra + // process manager and let it decide if it needs to be converted or not. + // For now, convert it unconditionally and pass it the ROSMessageType + // publish function specialization. + + // Avoid allocating when not using intra process. + if (!intra_process_is_enabled_) { + // Convert to the ROS message equivalent and publish it. + ROSMessageType ros_msg; + rclcpp::TypeAdapter::convert_to_ros_message(msg, ros_msg); + // In this case we're not using intra process. + return this->do_inter_process_publish(ros_msg); + } + // Otherwise we have to allocate memory in a unique_ptr, convert it, + // and pass it along. + // As the message is not const, a copy should be made. + // A shared_ptr could also be constructed here. + auto unique_ros_msg = this->create_ros_message_unique_ptr(); + rclcpp::TypeAdapter::convert_to_ros_message(msg, *unique_ros_msg); + this->publish(std::move(unique_ros_msg)); + } + void publish(const rcl_serialized_message_t & serialized_msg) { @@ -243,7 +389,7 @@ class Publisher : public PublisherBase * \param loaned_msg The LoanedMessage instance to be published. */ void - publish(rclcpp::LoanedMessage && loaned_msg) + publish(rclcpp::LoanedMessage && loaned_msg) { if (!loaned_msg.is_valid()) { throw std::runtime_error("loaned message is not valid"); @@ -269,16 +415,33 @@ class Publisher : public PublisherBase } } - std::shared_ptr + [[deprecated("use get_published_type_allocator() or get_ros_message_type_allocator() instead")]] + std::shared_ptr get_allocator() const { - return message_allocator_; + return std::make_shared(published_type_allocator_); + } + + PublishedTypeAllocator + get_published_type_allocator() const + { + return published_type_allocator_; + } + + ROSMessageTypeAllocator + get_ros_message_type_allocator() const + { + return ros_message_type_allocator_; } protected: void - do_inter_process_publish(const MessageT & msg) + do_inter_process_publish(const ROSMessageType & msg) { + TRACEPOINT( + rclcpp_publish, + static_cast(publisher_handle_.get()), + static_cast(&msg)); auto status = rcl_publish(publisher_handle_.get(), &msg, nullptr); if (RCL_RET_PUBLISHER_INVALID == status) { @@ -310,7 +473,8 @@ class Publisher : public PublisherBase } void - do_loaned_message_publish(std::unique_ptr> msg) + do_loaned_message_publish( + std::unique_ptr> msg) { auto status = rcl_publish_loaned_message(publisher_handle_.get(), msg.get(), nullptr); @@ -330,7 +494,7 @@ class Publisher : public PublisherBase } void - do_intra_process_publish(std::unique_ptr msg) + do_intra_process_publish(std::unique_ptr msg) { auto ipm = weak_ipm_.lock(); if (!ipm) { @@ -341,14 +505,15 @@ class Publisher : public PublisherBase throw std::runtime_error("cannot publish msg which is a null pointer"); } - ipm->template do_intra_process_publish( + ipm->template do_intra_process_publish( intra_process_publisher_id_, std::move(msg), - message_allocator_); + ros_message_type_allocator_); } - std::shared_ptr - do_intra_process_publish_and_return_shared(std::unique_ptr msg) + std::shared_ptr + do_intra_process_publish_and_return_shared( + std::unique_ptr msg) { auto ipm = weak_ipm_.lock(); if (!ipm) { @@ -359,10 +524,29 @@ class Publisher : public PublisherBase throw std::runtime_error("cannot publish msg which is a null pointer"); } - return ipm->template do_intra_process_publish_and_return_shared( + return ipm->template do_intra_process_publish_and_return_shared( intra_process_publisher_id_, std::move(msg), - message_allocator_); + ros_message_type_allocator_); + } + + /// Return a new unique_ptr using the ROSMessageType of the publisher. + std::unique_ptr + create_ros_message_unique_ptr() + { + auto ptr = ROSMessageTypeAllocatorTraits::allocate(ros_message_type_allocator_, 1); + ROSMessageTypeAllocatorTraits::construct(ros_message_type_allocator_, ptr); + return std::unique_ptr(ptr, ros_message_type_deleter_); + } + + /// Duplicate a given ros message as a unique_ptr. + std::unique_ptr + duplicate_ros_message_as_unique_ptr(const ROSMessageType & msg) + { + auto ptr = ROSMessageTypeAllocatorTraits::allocate(ros_message_type_allocator_, 1); + ROSMessageTypeAllocatorTraits::construct(ros_message_type_allocator_, ptr, msg); + return std::unique_ptr(ptr, ros_message_type_deleter_); } /// Copy of original options passed during construction. @@ -372,9 +556,10 @@ class Publisher : public PublisherBase */ const rclcpp::PublisherOptionsWithAllocator options_; - std::shared_ptr message_allocator_; - - MessageDeleter message_deleter_; + PublishedTypeAllocator published_type_allocator_; + PublishedTypeDeleter published_type_deleter_; + ROSMessageTypeAllocator ros_message_type_allocator_; + ROSMessageTypeDeleter ros_message_type_deleter_; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/publisher_base.hpp b/rclcpp/include/rclcpp/publisher_base.hpp index 48ebfea860..847bccff1e 100644 --- a/rclcpp/include/rclcpp/publisher_base.hpp +++ b/rclcpp/include/rclcpp/publisher_base.hpp @@ -23,11 +23,14 @@ #include #include #include +#include +#include #include #include "rcl/publisher.h" #include "rclcpp/macros.hpp" +#include "rclcpp/network_flow_endpoint.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/qos_event.hpp" #include "rclcpp/type_support_decl.hpp" @@ -109,9 +112,10 @@ class PublisherBase : public std::enable_shared_from_this get_publisher_handle() const; /// Get all the QoS event handlers associated with this publisher. - /** \return The vector of QoS event handlers. */ + /** \return The map of QoS event handlers. */ RCLCPP_PUBLIC - const std::vector> & + const + std::unordered_map> & get_event_handlers() const; /// Get subscription count @@ -193,6 +197,82 @@ class PublisherBase : public std::enable_shared_from_this uint64_t intra_process_publisher_id, IntraProcessManagerSharedPtr ipm); + /// Set a callback to be called when each new qos event instance occurs. + /** + * The callback receives a size_t which is the number of events that occurred + * since the last time this callback was called. + * Normally this is 1, but can be > 1 if events occurred before any + * callback was set. + * + * Since this callback is called from the middleware, you should aim to make + * it fast and not blocking. + * If you need to do a lot of work or wait for some other event, you should + * spin it off to another thread, otherwise you risk blocking the middleware. + * + * Calling it again will clear any previously set callback. + * + * An exception will be thrown if the callback is not callable. + * + * This function is thread-safe. + * + * If you want more information available in the callback, like the qos event + * or other information, you may use a lambda with captures or std::bind. + * + * \sa rclcpp::QOSEventHandlerBase::set_on_ready_callback + * + * \param[in] callback functor to be called when a new event occurs + * \param[in] event_type identifier for the qos event we want to attach the callback to + */ + RCLCPP_PUBLIC + void + set_on_new_qos_event_callback( + std::function callback, + rcl_publisher_event_type_t event_type) + { + if (event_handlers_.count(event_type) == 0) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Calling set_on_new_qos_event_callback for non registered event_type"); + return; + } + + if (!callback) { + throw std::invalid_argument( + "The callback passed to set_on_new_qos_event_callback " + "is not callable."); + } + + // The on_ready_callback signature has an extra `int` argument used to disambiguate between + // possible different entities within a generic waitable. + // We hide that detail to users of this method. + std::function new_callback = std::bind(callback, std::placeholders::_1); + event_handlers_[event_type]->set_on_ready_callback(new_callback); + } + + /// Unset the callback registered for new qos events, if any. + RCLCPP_PUBLIC + void + clear_on_new_qos_event_callback(rcl_publisher_event_type_t event_type) + { + if (event_handlers_.count(event_type) == 0) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Calling clear_on_new_qos_event_callback for non registered event_type"); + return; + } + + event_handlers_[event_type]->clear_on_ready_callback(); + } + + /// Get network flow endpoints + /** + * Describes network flow endpoints that this publisher is sending messages out on + * \return vector of NetworkFlowEndpoint + */ + RCLCPP_PUBLIC + std::vector + get_network_flow_endpoints() const; + protected: template void @@ -206,7 +286,7 @@ class PublisherBase : public std::enable_shared_from_this rcl_publisher_event_init, publisher_handle_, event_type); - event_handlers_.emplace_back(handler); + event_handlers_.insert(std::make_pair(event_type, handler)); } RCLCPP_PUBLIC @@ -216,7 +296,8 @@ class PublisherBase : public std::enable_shared_from_this std::shared_ptr publisher_handle_; - std::vector> event_handlers_; + std::unordered_map> event_handlers_; using IntraProcessManagerWeakPtr = std::weak_ptr; diff --git a/rclcpp/include/rclcpp/publisher_options.hpp b/rclcpp/include/rclcpp/publisher_options.hpp index 2e12c12bcb..48250307e9 100644 --- a/rclcpp/include/rclcpp/publisher_options.hpp +++ b/rclcpp/include/rclcpp/publisher_options.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include "rcl/publisher.h" @@ -45,6 +46,11 @@ struct PublisherOptionsBase /// Whether or not to use default callbacks when user doesn't supply any in event_callbacks bool use_default_callbacks = true; + /// Require middleware to generate unique network flow endpoints + /// Disabled by default + rmw_unique_network_flow_endpoints_requirement_t require_unique_network_flow_endpoints = + RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_NOT_REQUIRED; + /// Callback group in which the waitable items from the publisher should be placed. std::shared_ptr callback_group; @@ -59,6 +65,10 @@ struct PublisherOptionsBase template struct PublisherOptionsWithAllocator : public PublisherOptionsBase { + static_assert( + std::is_void_v::value_type>, + "Publisher allocator value type must be void"); + /// Optional custom allocator. std::shared_ptr allocator = nullptr; @@ -75,11 +85,10 @@ struct PublisherOptionsWithAllocator : public PublisherOptionsBase to_rcl_publisher_options(const rclcpp::QoS & qos) const { rcl_publisher_options_t result = rcl_publisher_get_default_options(); - using AllocatorTraits = std::allocator_traits; - using MessageAllocatorT = typename AllocatorTraits::template rebind_alloc; - auto message_alloc = std::make_shared(*this->get_allocator().get()); - result.allocator = rclcpp::allocator::get_rcl_allocator(*message_alloc); + result.allocator = this->get_rcl_allocator(); result.qos = qos.get_rmw_qos_profile(); + result.rmw_publisher_options.require_unique_network_flow_endpoints = + this->require_unique_network_flow_endpoints; // Apply payload to rcl_publisher_options if necessary. if (rmw_implementation_payload && rmw_implementation_payload->has_been_customized()) { @@ -95,10 +104,35 @@ struct PublisherOptionsWithAllocator : public PublisherOptionsBase get_allocator() const { if (!this->allocator) { - return std::make_shared(); + if (!allocator_storage_) { + allocator_storage_ = std::make_shared(); + } + return allocator_storage_; } return this->allocator; } + +private: + using PlainAllocator = + typename std::allocator_traits::template rebind_alloc; + + rcl_allocator_t + get_rcl_allocator() const + { + if (!plain_allocator_storage_) { + plain_allocator_storage_ = + std::make_shared(*this->get_allocator()); + } + return rclcpp::allocator::get_rcl_allocator(*plain_allocator_storage_); + } + + // This is a temporal workaround, to make sure that get_allocator() + // always returns a copy of the same allocator. + mutable std::shared_ptr allocator_storage_; + + // This is a temporal workaround, to keep the plain allocator that backs + // up the rcl allocator returned in rcl_publisher_options_t alive. + mutable std::shared_ptr plain_allocator_storage_; }; using PublisherOptions = PublisherOptionsWithAllocator>; diff --git a/rclcpp/include/rclcpp/qos.hpp b/rclcpp/include/rclcpp/qos.hpp index edc3a58bbe..99d5acbd33 100644 --- a/rclcpp/include/rclcpp/qos.hpp +++ b/rclcpp/include/rclcpp/qos.hpp @@ -18,6 +18,7 @@ #include #include "rclcpp/duration.hpp" +#include "rclcpp/exceptions.hpp" #include "rclcpp/visibility_control.hpp" #include "rcl/logging_rosout.h" #include "rmw/incompatible_qos_events_statuses.h" @@ -62,6 +63,13 @@ enum class LivelinessPolicy Unknown = RMW_QOS_POLICY_LIVELINESS_UNKNOWN, }; +enum class QoSCompatibility +{ + Ok = RMW_QOS_COMPATIBILITY_OK, + Warning = RMW_QOS_COMPATIBILITY_WARNING, + Error = RMW_QOS_COMPATIBILITY_ERROR, +}; + /// QoS initialization values, cannot be created directly, use KeepAll or KeepLast instead. struct RCLCPP_PUBLIC QoSInitialization { @@ -95,8 +103,8 @@ struct RCLCPP_PUBLIC KeepLast : public rclcpp::QoSInitialization * and other entities, and includes things like how data is sent or resent, * how data is buffered on the publishing and subscribing side, and other things. * See: - * - * https://index.ros.org/doc/ros2/Concepts/About-Quality-of-Service-Settings + * + * https://docs.ros.org/en/rolling/Concepts/About-Quality-of-Service-Settings.html * */ class RCLCPP_PUBLIC QoS @@ -273,6 +281,61 @@ bool operator==(const QoS & left, const QoS & right); RCLCPP_PUBLIC bool operator!=(const QoS & left, const QoS & right); +/// Result type for checking QoS compatibility +/** + * \see rclcpp::qos_check_compatible() + */ +struct QoSCheckCompatibleResult +{ + /// Compatibility result. + QoSCompatibility compatibility; + + /// Reason for a (possible) incompatibility. + /** + * Set if compatiblity is QoSCompatibility::Warning or QoSCompatiblity::Error. + * Not set if the QoS profiles are compatible. + */ + std::string reason; +}; + +/// Check if two QoS profiles are compatible. +/** + * Two QoS profiles are compatible if a publisher and subcription + * using the QoS policies can communicate with each other. + * + * If any policies have value "system default" or "unknown" then it is possible that + * compatiblity cannot be determined. + * In this case, the value QoSCompatility::Warning is set as part of + * the returned structure. + * + * Example usage: + * + * ```cpp + * rclcpp::QoSCheckCompatibleResult result = rclcpp::qos_check_compatible( + * publisher_qos, subscription_qos); + * if (rclcpp::QoSCompatibility::Error != result.compatibility) { + * // QoS not compatible ... + * // result.reason contains info about the incompatibility + * } else if (rclcpp::QoSCompatibility::Warning != result.compatibility) { + * // QoS may not be compatible ... + * // result.reason contains info about the possible incompatibility + * } + * ``` + * + * \param[in] publisher_qos: The QoS profile for a publisher. + * \param[in] subscription_qos: The QoS profile for a subscription. + * \return Struct with compatiblity set to QoSCompatibility::Ok if the QoS profiles are + * compatible, or + * \return Struct with compatibility set to QoSCompatibility::Warning if there is a chance + * the QoS profiles are not compatible, or + * \return Struct with compatibility set to QoSCompatibility::Error if the QoS profiles are + * not compatible. + * \throws rclcpp::exceptions::QoSCheckCompatibilityException if an unexpected error occurs. + */ +RCLCPP_PUBLIC +QoSCheckCompatibleResult +qos_check_compatible(const QoS & publisher_qos, const QoS & subscription_qos); + /** * Clock QoS class * - History: Keep last, diff --git a/rclcpp/include/rclcpp/qos_event.hpp b/rclcpp/include/rclcpp/qos_event.hpp index b3841cf2f5..078305037c 100644 --- a/rclcpp/include/rclcpp/qos_event.hpp +++ b/rclcpp/include/rclcpp/qos_event.hpp @@ -17,16 +17,20 @@ #include #include +#include #include #include #include "rcl/error_handling.h" +#include "rmw/impl/cpp/demangle.hpp" #include "rmw/incompatible_qos_events_statuses.h" #include "rcutils/logging_macros.h" +#include "rclcpp/detail/cpp_callback_trampoline.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp/function_traits.hpp" +#include "rclcpp/logging.hpp" #include "rclcpp/waitable.hpp" namespace rclcpp @@ -84,6 +88,11 @@ class UnsupportedEventTypeException : public exceptions::RCLErrorBase, public st class QOSEventHandlerBase : public Waitable { public: + enum class EntityType + { + Event, + }; + RCLCPP_PUBLIC virtual ~QOSEventHandlerBase(); @@ -102,15 +111,109 @@ class QOSEventHandlerBase : public Waitable bool is_ready(rcl_wait_set_t * wait_set) override; - RCLCPP_PUBLIC + /// Set a callback to be called when each new event instance occurs. + /** + * The callback receives a size_t which is the number of events that occurred + * since the last time this callback was called. + * Normally this is 1, but can be > 1 if events occurred before any + * callback was set. + * + * The callback also receives an int identifier argument. + * This is needed because a Waitable may be composed of several distinct entities, + * such as subscriptions, services, etc. + * The application should provide a generic callback function that will be then + * forwarded by the waitable to all of its entities. + * Before forwarding, a different value for the identifier argument will be + * bond to the function. + * This implies that the provided callback can use the identifier to behave + * differently depending on which entity triggered the waitable to become ready. + * + * Since this callback is called from the middleware, you should aim to make + * it fast and not blocking. + * If you need to do a lot of work or wait for some other event, you should + * spin it off to another thread, otherwise you risk blocking the middleware. + * + * Calling it again will clear any previously set callback. + * + * An exception will be thrown if the callback is not callable. + * + * This function is thread-safe. + * + * If you want more information available in the callback, like the qos event + * or other information, you may use a lambda with captures or std::bind. + * + * \sa rmw_event_set_callback + * \sa rcl_event_set_callback + * + * \param[in] callback functor to be called when a new event occurs + */ + void + set_on_ready_callback(std::function callback) override + { + if (!callback) { + throw std::invalid_argument( + "The callback passed to set_on_ready_callback " + "is not callable."); + } + + // Note: we bind the int identifier argument to this waitable's entity types + auto new_callback = + [callback, this](size_t number_of_events) { + try { + callback(number_of_events, static_cast(EntityType::Event)); + } catch (const std::exception & exception) { + RCLCPP_ERROR_STREAM( + // TODO(wjwwood): get this class access to the node logger it is associated with + rclcpp::get_logger("rclcpp"), + "rclcpp::QOSEventHandlerBase@" << this << + " caught " << rmw::impl::cpp::demangle(exception) << + " exception in user-provided callback for the 'on ready' callback: " << + exception.what()); + } catch (...) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("rclcpp"), + "rclcpp::QOSEventHandlerBase@" << this << + " caught unhandled exception in user-provided callback " << + "for the 'on ready' callback"); + } + }; + + std::lock_guard lock(reentrant_mutex_); + + // Set it temporarily to the new callback, while we replace the old one. + // This two-step setting, prevents a gap where the old std::function has + // been replaced but the middleware hasn't been told about the new one yet. + set_on_new_event_callback( + rclcpp::detail::cpp_callback_trampoline, + static_cast(&new_callback)); + + // Store the std::function to keep it in scope, also overwrites the existing one. + on_new_event_callback_ = new_callback; + + // Set it again, now using the permanent storage. + set_on_new_event_callback( + rclcpp::detail::cpp_callback_trampoline, + static_cast(&on_new_event_callback_)); + } + + /// Unset the callback registered for new events, if any. void - set_listener_callback( - rmw_listener_callback_t callback, - const void * user_data) const override; + clear_on_ready_callback() override + { + std::lock_guard lock(reentrant_mutex_); + set_on_new_event_callback(nullptr, nullptr); + on_new_event_callback_ = nullptr; + } protected: + RCLCPP_PUBLIC + void + set_on_new_event_callback(rcl_event_callback_t callback, const void * user_data); + rcl_event_t event_handle_; size_t wait_set_event_index_; + std::recursive_mutex reentrant_mutex_; + std::function on_new_event_callback_{nullptr}; }; template @@ -123,9 +226,8 @@ class QOSEventHandler : public QOSEventHandlerBase InitFuncT init_func, ParentHandleT parent_handle, EventTypeEnum event_type) - : event_callback_(callback) + : parent_handle_(parent_handle), event_callback_(callback) { - parent_handle_ = parent_handle; event_handle_ = rcl_get_zero_initialized_event(); rcl_ret_t ret = init_func(&event_handle_, parent_handle.get(), event_type); if (ret != RCL_RET_OK) { diff --git a/rclcpp/include/rclcpp/rclcpp.hpp b/rclcpp/include/rclcpp/rclcpp.hpp index 56e999f702..7f0498fe49 100644 --- a/rclcpp/include/rclcpp/rclcpp.hpp +++ b/rclcpp/include/rclcpp/rclcpp.hpp @@ -117,6 +117,15 @@ * - Allocator related items: * - rclcpp/allocator/allocator_common.hpp * - rclcpp/allocator/allocator_deleter.hpp + * - Generic publisher + * - rclcpp::Node::create_generic_publisher() + * - rclcpp::GenericPublisher + * - rclcpp::GenericPublisher::publish() + * - rclcpp/generic_publisher.hpp + * - Generic subscription + * - rclcpp::Node::create_generic_subscription() + * - rclcpp::GenericSubscription + * - rclcpp/generic_subscription.hpp * - Memory management tools: * - rclcpp/memory_strategies.hpp * - rclcpp/memory_strategy.hpp @@ -134,6 +143,7 @@ * - rclcpp/scope_exit.hpp * - rclcpp/time.hpp * - rclcpp/utilities.hpp + * - rclcpp/typesupport_helpers.hpp * - rclcpp/visibility_control.hpp */ diff --git a/rclcpp/include/rclcpp/serialized_message.hpp b/rclcpp/include/rclcpp/serialized_message.hpp index 4b90bf3561..b8f68dc77e 100644 --- a/rclcpp/include/rclcpp/serialized_message.hpp +++ b/rclcpp/include/rclcpp/serialized_message.hpp @@ -47,7 +47,7 @@ class RCLCPP_PUBLIC_TYPE SerializedMessage * \param[in] initial_capacity The amount of memory to be allocated. * \param[in] allocator The allocator to be used for the initialization. */ - SerializedMessage( + explicit SerializedMessage( size_t initial_capacity, const rcl_allocator_t & allocator = rcl_get_default_allocator()); diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 283a5058e0..c88e8befcb 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -19,22 +19,27 @@ #include #include #include +#include #include #include #include "rcl/error_handling.h" #include "rcl/service.h" +#include "rmw/error_handling.h" +#include "rmw/impl/cpp/demangle.hpp" +#include "rmw/rmw.h" + +#include "tracetools/tracetools.h" + #include "rclcpp/any_service_callback.hpp" +#include "rclcpp/detail/cpp_callback_trampoline.hpp" #include "rclcpp/exceptions.hpp" +#include "rclcpp/expand_topic_or_service_name.hpp" +#include "rclcpp/logging.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/type_support_decl.hpp" -#include "rclcpp/expand_topic_or_service_name.hpp" #include "rclcpp/visibility_control.hpp" -#include "rclcpp/logging.hpp" -#include "rmw/error_handling.h" -#include "rmw/rmw.h" -#include "tracetools/tracetools.h" namespace rclcpp { @@ -121,11 +126,88 @@ class ServiceBase bool exchange_in_use_by_wait_set_state(bool in_use_state); - RCLCPP_PUBLIC + /// Set a callback to be called when each new request is received. + /** + * The callback receives a size_t which is the number of requests received + * since the last time this callback was called. + * Normally this is 1, but can be > 1 if requests were received before any + * callback was set. + * + * Since this callback is called from the middleware, you should aim to make + * it fast and not blocking. + * If you need to do a lot of work or wait for some other event, you should + * spin it off to another thread, otherwise you risk blocking the middleware. + * + * Calling it again will clear any previously set callback. + * + * + * An exception will be thrown if the callback is not callable. + * + * This function is thread-safe. + * + * If you want more information available in the callback, like the service + * or other information, you may use a lambda with captures or std::bind. + * + * \sa rmw_service_set_on_new_request_callback + * \sa rcl_service_set_on_new_request_callback + * + * \param[in] callback functor to be called when a new request is received + */ + void + set_on_new_request_callback(std::function callback) + { + if (!callback) { + throw std::invalid_argument( + "The callback passed to set_on_new_request_callback " + "is not callable."); + } + + auto new_callback = + [callback, this](size_t number_of_requests) { + try { + callback(number_of_requests); + } catch (const std::exception & exception) { + RCLCPP_ERROR_STREAM( + node_logger_, + "rclcpp::ServiceBase@" << this << + " caught " << rmw::impl::cpp::demangle(exception) << + " exception in user-provided callback for the 'on new request' callback: " << + exception.what()); + } catch (...) { + RCLCPP_ERROR_STREAM( + node_logger_, + "rclcpp::ServiceBase@" << this << + " caught unhandled exception in user-provided callback " << + "for the 'on new request' callback"); + } + }; + + std::lock_guard lock(reentrant_mutex_); + + // Set it temporarily to the new callback, while we replace the old one. + // This two-step setting, prevents a gap where the old std::function has + // been replaced but the middleware hasn't been told about the new one yet. + set_on_new_request_callback( + rclcpp::detail::cpp_callback_trampoline, + static_cast(&new_callback)); + + // Store the std::function to keep it in scope, also overwrites the existing one. + on_new_request_callback_ = new_callback; + + // Set it again, now using the permanent storage. + set_on_new_request_callback( + rclcpp::detail::cpp_callback_trampoline, + static_cast(&on_new_request_callback_)); + } + + /// Unset the callback registered for new requests, if any. void - set_listener_callback( - rmw_listener_callback_t callback, - const void * user_data) const; + clear_on_new_request_callback() + { + std::lock_guard lock(reentrant_mutex_); + set_on_new_request_callback(nullptr, nullptr); + on_new_request_callback_ = nullptr; + } protected: RCLCPP_DISABLE_COPY(ServiceBase) @@ -138,12 +220,21 @@ class ServiceBase const rcl_node_t * get_rcl_node_handle() const; + RCLCPP_PUBLIC + void + set_on_new_request_callback(rcl_event_callback_t callback, const void * user_data); + std::shared_ptr node_handle_; std::shared_ptr service_handle_; bool owns_rcl_handle_ = true; + rclcpp::Logger node_logger_; + std::atomic in_use_by_wait_set_{false}; + + std::recursive_mutex reentrant_mutex_; + std::function on_new_request_callback_{nullptr}; }; template @@ -183,25 +274,16 @@ class Service : public ServiceBase using rosidl_typesupport_cpp::get_service_type_support_handle; auto service_type_support_handle = get_service_type_support_handle(); - std::weak_ptr weak_node_handle(node_handle_); // rcl does the static memory allocation here service_handle_ = std::shared_ptr( - new rcl_service_t, [weak_node_handle, service_name](rcl_service_t * service) + new rcl_service_t, [handle = node_handle_, service_name](rcl_service_t * service) { - auto handle = weak_node_handle.lock(); - if (handle) { - if (rcl_service_fini(service, handle.get()) != RCL_RET_OK) { - RCLCPP_ERROR( - rclcpp::get_node_logger(handle.get()).get_child("rclcpp"), - "Error in destruction of rcl service handle: %s", - rcl_get_error_string().str); - rcl_reset_error(); - } - } else { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("rclcpp"), - "Error in destruction of rcl service handle " << service_name << - ": the Node Handle was destructed too early. You will leak memory"); + if (rcl_service_fini(service, handle.get()) != RCL_RET_OK) { + RCLCPP_ERROR( + rclcpp::get_node_logger(handle.get()).get_child("rclcpp"), + "Error in destruction of rcl service handle: %s", + rcl_get_error_string().str); + rcl_reset_error(); } delete service; }); @@ -355,15 +437,6 @@ class Service : public ServiceBase send_response(*request_header, *response); } - [[deprecated("use the send_response() which takes references instead of shared pointers")]] - void - send_response( - std::shared_ptr req_id, - std::shared_ptr response) - { - send_response(*req_id, *response); - } - void send_response(rmw_request_id_t & req_id, typename ServiceT::Response & response) { diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index 76790b1245..463bc1ce0f 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -61,7 +61,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy allocator_ = std::make_shared(); } - void add_guard_condition(const rcl_guard_condition_t * guard_condition) override + void add_guard_condition(const rclcpp::GuardCondition * guard_condition) override { for (const auto & existing_guard_condition : guard_conditions_) { if (existing_guard_condition == guard_condition) { @@ -71,7 +71,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy guard_conditions_.push_back(guard_condition); } - void remove_guard_condition(const rcl_guard_condition_t * guard_condition) override + void remove_guard_condition(const rclcpp::GuardCondition * guard_condition) override { for (auto it = guard_conditions_.begin(); it != guard_conditions_.end(); ++it) { if (*it == guard_condition) { @@ -240,13 +240,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } for (auto guard_condition : guard_conditions_) { - if (rcl_wait_set_add_guard_condition(wait_set, guard_condition, NULL) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Couldn't add guard_condition to wait set: %s", - rcl_get_error_string().str); - return false; - } + guard_condition->add_to_wait_set(wait_set); } for (auto waitable : waitable_handles_) { @@ -504,7 +498,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy using VectorRebind = std::vector::template rebind_alloc>; - VectorRebind guard_conditions_; + VectorRebind guard_conditions_; VectorRebind> subscription_handles_; VectorRebind> service_handles_; diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index fe98dc5796..dd73bffa46 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -60,10 +60,16 @@ class NodeTopicsInterface; /// Subscription implementation, templated on the type of message this subscription receives. template< - typename CallbackMessageT, + typename MessageT, typename AllocatorT = std::allocator, + /// MessageT::custom_type if MessageT is a TypeAdapter, + /// otherwise just MessageT. + typename SubscribedT = typename rclcpp::TypeAdapter::custom_type, + /// MessageT::ros_message_type if MessageT is a TypeAdapter, + /// otherwise just MessageT. + typename ROSMessageT = typename rclcpp::TypeAdapter::ros_message_type, typename MessageMemoryStrategyT = rclcpp::message_memory_strategy::MessageMemoryStrategy< - CallbackMessageT, + ROSMessageT, AllocatorT >> class Subscription : public SubscriptionBase @@ -71,14 +77,36 @@ class Subscription : public SubscriptionBase friend class rclcpp::node_interfaces::NodeTopicsInterface; public: - using MessageAllocatorTraits = allocator::AllocRebind; - using MessageAllocator = typename MessageAllocatorTraits::allocator_type; - using MessageDeleter = allocator::Deleter; - using ConstMessageSharedPtr = std::shared_ptr; - using MessageUniquePtr = std::unique_ptr; + // Redeclare these here to use outside of the class. + using SubscribedType = SubscribedT; + using ROSMessageType = ROSMessageT; + using MessageMemoryStrategyType = MessageMemoryStrategyT; + + using SubscribedTypeAllocatorTraits = allocator::AllocRebind; + using SubscribedTypeAllocator = typename SubscribedTypeAllocatorTraits::allocator_type; + using SubscribedTypeDeleter = allocator::Deleter; + + using ROSMessageTypeAllocatorTraits = allocator::AllocRebind; + using ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type; + using ROSMessageTypeDeleter = allocator::Deleter; + + using MessageAllocatorTraits [[deprecated("use ROSMessageTypeAllocatorTraits")]] = + ROSMessageTypeAllocatorTraits; + using MessageAllocator [[deprecated("use ROSMessageTypeAllocator")]] = + ROSMessageTypeAllocator; + using MessageDeleter [[deprecated("use ROSMessageTypeDeleter")]] = + ROSMessageTypeDeleter; + + using ConstMessageSharedPtr [[deprecated]] = std::shared_ptr; + using MessageUniquePtr + [[deprecated("use std::unique_ptr instead")]] = + std::unique_ptr; + +private: using SubscriptionTopicStatisticsSharedPtr = - std::shared_ptr>; + std::shared_ptr>; +public: RCLCPP_SMART_PTR_DEFINITIONS(Subscription) /// Default constructor. @@ -104,7 +132,7 @@ class Subscription : public SubscriptionBase const rosidl_message_type_support_t & type_support_handle, const std::string & topic_name, const rclcpp::QoS & qos, - AnySubscriptionCallback callback, + AnySubscriptionCallback callback, const rclcpp::SubscriptionOptionsWithAllocator & options, typename MessageMemoryStrategyT::SharedPtr message_memory_strategy, SubscriptionTopicStatisticsSharedPtr subscription_topic_statistics = nullptr) @@ -112,8 +140,8 @@ class Subscription : public SubscriptionBase node_base, type_support_handle, topic_name, - options.template to_rcl_subscription_options(qos), - rclcpp::subscription_traits::is_serialized_subscription_argument::value), + options.template to_rcl_subscription_options(qos), + callback.is_serialized_message_callback()), any_callback_(callback), options_(options), message_memory_strategy_(message_memory_strategy) @@ -241,11 +269,34 @@ class Subscription : public SubscriptionBase * \throws any rcl errors from rcl_take, \sa rclcpp::exceptions::throw_from_rcl_error() */ bool - take(CallbackMessageT & message_out, rclcpp::MessageInfo & message_info_out) + take(ROSMessageType & message_out, rclcpp::MessageInfo & message_info_out) { return this->take_type_erased(static_cast(&message_out), message_info_out); } + /// Take the next message from the inter-process subscription. + /** + * This verison takes a SubscribedType which is different frmo the + * ROSMessageType when a rclcpp::TypeAdapter is in used. + * + * \sa take(ROSMessageType &, rclcpp::MessageInfo &) + */ + template + std::enable_if_t< + !rosidl_generator_traits::is_message::value && + std::is_same_v, + bool + > + take(TakeT & message_out, rclcpp::MessageInfo & message_info_out) + { + ROSMessageType local_message; + bool taken = this->take_type_erased(static_cast(&local_message), message_info_out); + if (taken) { + rclcpp::TypeAdapter::convert_to_custom(local_message, message_out); + } + return taken; + } + std::shared_ptr create_message() override { @@ -272,7 +323,7 @@ class Subscription : public SubscriptionBase // we should ignore this copy of the message. return; } - auto typed_message = std::static_pointer_cast(message); + auto typed_message = std::static_pointer_cast(message); std::chrono::time_point now; if (subscription_topic_statistics_) { @@ -290,15 +341,24 @@ class Subscription : public SubscriptionBase } } + void + handle_serialized_message( + const std::shared_ptr & serialized_message, + const rclcpp::MessageInfo & message_info) override + { + // TODO(wjwwood): enable topic statistics for serialized messages + any_callback_.dispatch(serialized_message, message_info); + } + void handle_loaned_message( void * loaned_message, const rclcpp::MessageInfo & message_info) override { - auto typed_message = static_cast(loaned_message); + auto typed_message = static_cast(loaned_message); // message is loaned, so we have to make sure that the deleter does not deallocate the message - auto sptr = std::shared_ptr( - typed_message, [](CallbackMessageT * msg) {(void) msg;}); + auto sptr = std::shared_ptr( + typed_message, [](ROSMessageType * msg) {(void) msg;}); any_callback_.dispatch(sptr, message_info); } @@ -309,7 +369,7 @@ class Subscription : public SubscriptionBase void return_message(std::shared_ptr & message) override { - auto typed_message = std::static_pointer_cast(message); + auto typed_message = std::static_pointer_cast(message); message_memory_strategy_->return_message(typed_message); } @@ -332,21 +392,24 @@ class Subscription : public SubscriptionBase private: RCLCPP_DISABLE_COPY(Subscription) - AnySubscriptionCallback any_callback_; + AnySubscriptionCallback any_callback_; /// Copy of original options passed during construction. /** * It is important to save a copy of this so that the rmw payload which it * may contain is kept alive for the duration of the subscription. */ const rclcpp::SubscriptionOptionsWithAllocator options_; - typename message_memory_strategy::MessageMemoryStrategy::SharedPtr + typename message_memory_strategy::MessageMemoryStrategy::SharedPtr message_memory_strategy_; + /// Component which computes and publishes topic statistics for this subscriber SubscriptionTopicStatisticsSharedPtr subscription_topic_statistics_{nullptr}; + using SubscriptionIntraProcessT = rclcpp::experimental::SubscriptionIntraProcess< - CallbackMessageT, + ROSMessageType, AllocatorT, - typename MessageUniquePtr::deleter_type>; + ROSMessageTypeDeleter, + MessageT>; std::shared_ptr subscription_intra_process_; }; diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index d7b7e9abaf..abac6dc670 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include #include @@ -24,13 +25,16 @@ #include "rcl/subscription.h" +#include "rmw/impl/cpp/demangle.hpp" #include "rmw/rmw.h" #include "rclcpp/any_subscription_callback.hpp" +#include "rclcpp/detail/cpp_callback_trampoline.hpp" #include "rclcpp/experimental/intra_process_manager.hpp" #include "rclcpp/experimental/subscription_intra_process_base.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/message_info.hpp" +#include "rclcpp/network_flow_endpoint.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/qos_event.hpp" #include "rclcpp/serialized_message.hpp" @@ -61,8 +65,11 @@ class SubscriptionBase : public std::enable_shared_from_this public: RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(SubscriptionBase) - /// Default constructor. + /// Constructor. /** + * This accepts rcl_subscription_options_t instead of rclcpp::SubscriptionOptions because + * rclcpp::SubscriptionOptions::to_rcl_subscription_options depends on the message type. + * * \param[in] node_base NodeBaseInterface pointer used in parts of the setup. * \param[in] type_support_handle rosidl type support struct, for the Message type of the topic. * \param[in] topic_name Name of the topic to subscribe to. @@ -77,7 +84,7 @@ class SubscriptionBase : public std::enable_shared_from_this const rcl_subscription_options_t & subscription_options, bool is_serialized = false); - /// Default destructor. + /// Destructor. RCLCPP_PUBLIC virtual ~SubscriptionBase(); @@ -95,9 +102,10 @@ class SubscriptionBase : public std::enable_shared_from_this get_subscription_handle() const; /// Get all the QoS event handlers associated with this subscription. - /** \return The vector of QoS event handlers. */ + /** \return The map of QoS event handlers. */ RCLCPP_PUBLIC - const std::vector> & + const + std::unordered_map> & get_event_handlers() const; /// Get the actual QoS settings, after the defaults have been determined. @@ -179,6 +187,13 @@ class SubscriptionBase : public std::enable_shared_from_this void handle_message(std::shared_ptr & message, const rclcpp::MessageInfo & message_info) = 0; + RCLCPP_PUBLIC + virtual + void + handle_serialized_message( + const std::shared_ptr & serialized_message, + const rclcpp::MessageInfo & message_info) = 0; + RCLCPP_PUBLIC virtual void @@ -263,11 +278,220 @@ class SubscriptionBase : public std::enable_shared_from_this bool exchange_in_use_by_wait_set_state(void * pointer_to_subscription_part, bool in_use_state); + /// Set a callback to be called when each new message is received. + /** + * The callback receives a size_t which is the number of messages received + * since the last time this callback was called. + * Normally this is 1, but can be > 1 if messages were received before any + * callback was set. + * + * Since this callback is called from the middleware, you should aim to make + * it fast and not blocking. + * If you need to do a lot of work or wait for some other event, you should + * spin it off to another thread, otherwise you risk blocking the middleware. + * + * Calling it again will clear any previously set callback. + * + * This function is thread-safe. + * + * If you want more information available in the callback, like the subscription + * or other information, you may use a lambda with captures or std::bind. + * + * \sa rmw_subscription_set_on_new_message_callback + * \sa rcl_subscription_set_on_new_message_callback + * + * \param[in] callback functor to be called when a new message is received + */ + RCLCPP_PUBLIC + void + set_on_new_message_callback(std::function callback) + { + if (!callback) { + throw std::invalid_argument( + "The callback passed to set_on_new_message_callback " + "is not callable."); + } + + auto new_callback = + [callback, this](size_t number_of_messages) { + try { + callback(number_of_messages); + } catch (const std::exception & exception) { + RCLCPP_ERROR_STREAM( + node_logger_, + "rclcpp::SubscriptionBase@" << this << + " caught " << rmw::impl::cpp::demangle(exception) << + " exception in user-provided callback for the 'on new message' callback: " << + exception.what()); + } catch (...) { + RCLCPP_ERROR_STREAM( + node_logger_, + "rclcpp::SubscriptionBase@" << this << + " caught unhandled exception in user-provided callback " << + "for the 'on new message' callback"); + } + }; + + std::lock_guard lock(reentrant_mutex_); + + // Set it temporarily to the new callback, while we replace the old one. + // This two-step setting, prevents a gap where the old std::function has + // been replaced but the middleware hasn't been told about the new one yet. + set_on_new_message_callback( + rclcpp::detail::cpp_callback_trampoline, + static_cast(&new_callback)); + + // Store the std::function to keep it in scope, also overwrites the existing one. + on_new_message_callback_ = new_callback; + + // Set it again, now using the permanent storage. + set_on_new_message_callback( + rclcpp::detail::cpp_callback_trampoline, + static_cast(&on_new_message_callback_)); + } + + /// Unset the callback registered for new messages, if any. + RCLCPP_PUBLIC + void + clear_on_new_message_callback() + { + std::lock_guard lock(reentrant_mutex_); + set_on_new_message_callback(nullptr, nullptr); + on_new_message_callback_ = nullptr; + } + + /// Set a callback to be called when each new intra-process message is received. + /** + * The callback receives a size_t which is the number of messages received + * since the last time this callback was called. + * Normally this is 1, but can be > 1 if messages were received before any + * callback was set. + * + * Calling it again will clear any previously set callback. + * + * This function is thread-safe. + * + * If you want more information available in the callback, like the subscription + * or other information, you may use a lambda with captures or std::bind. + * + * \sa rclcpp::SubscriptionIntraProcessBase::set_on_ready_callback + * + * \param[in] callback functor to be called when a new message is received + */ + RCLCPP_PUBLIC + void + set_on_new_intra_process_message_callback(std::function callback) + { + if (!use_intra_process_) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Calling set_on_new_intra_process_message_callback for subscription with IPC disabled"); + return; + } + + if (!callback) { + throw std::invalid_argument( + "The callback passed to set_on_new_intra_process_message_callback " + "is not callable."); + } + + // The on_ready_callback signature has an extra `int` argument used to disambiguate between + // possible different entities within a generic waitable. + // We hide that detail to users of this method. + std::function new_callback = std::bind(callback, std::placeholders::_1); + subscription_intra_process_->set_on_ready_callback(new_callback); + } + + /// Unset the callback registered for new intra-process messages, if any. + RCLCPP_PUBLIC + void + clear_on_new_intra_process_message_callback() + { + if (!use_intra_process_) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Calling clear_on_new_intra_process_message_callback for subscription with IPC disabled"); + return; + } + + subscription_intra_process_->clear_on_ready_callback(); + } + + /// Set a callback to be called when each new qos event instance occurs. + /** + * The callback receives a size_t which is the number of events that occurred + * since the last time this callback was called. + * Normally this is 1, but can be > 1 if events occurred before any + * callback was set. + * + * Since this callback is called from the middleware, you should aim to make + * it fast and not blocking. + * If you need to do a lot of work or wait for some other event, you should + * spin it off to another thread, otherwise you risk blocking the middleware. + * + * Calling it again will clear any previously set callback. + * + * An exception will be thrown if the callback is not callable. + * + * This function is thread-safe. + * + * If you want more information available in the callback, like the qos event + * or other information, you may use a lambda with captures or std::bind. + * + * \sa rclcpp::QOSEventHandlerBase::set_on_ready_callback + * + * \param[in] callback functor to be called when a new event occurs + * \param[in] event_type identifier for the qos event we want to attach the callback to + */ + RCLCPP_PUBLIC + void + set_on_new_qos_event_callback( + std::function callback, + rcl_subscription_event_type_t event_type) + { + if (event_handlers_.count(event_type) == 0) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Calling set_on_new_qos_event_callback for non registered event_type"); + return; + } + + if (!callback) { + throw std::invalid_argument( + "The callback passed to set_on_new_qos_event_callback " + "is not callable."); + } + + // The on_ready_callback signature has an extra `int` argument used to disambiguate between + // possible different entities within a generic waitable. + // We hide that detail to users of this method. + std::function new_callback = std::bind(callback, std::placeholders::_1); + event_handlers_[event_type]->set_on_ready_callback(new_callback); + } + + /// Unset the callback registered for new qos events, if any. RCLCPP_PUBLIC void - set_listener_callback( - rmw_listener_callback_t callback, - const void * user_data) const; + clear_on_new_qos_event_callback(rcl_subscription_event_type_t event_type) + { + if (event_handlers_.count(event_type) == 0) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Calling clear_on_new_qos_event_callback for non registered event_type"); + return; + } + + event_handlers_[event_type]->clear_on_ready_callback(); + } + + /// Get network flow endpoints + /** + * Describes network flow endpoints that this subscription is receiving messages on + * \return vector of NetworkFlowEndpoint + */ + RCLCPP_PUBLIC + std::vector + get_network_flow_endpoints() const; protected: template @@ -283,7 +507,7 @@ class SubscriptionBase : public std::enable_shared_from_this get_subscription_handle(), event_type); qos_events_in_use_by_wait_set_.insert(std::make_pair(handler.get(), false)); - event_handlers_.emplace_back(handler); + event_handlers_.insert(std::make_pair(event_type, handler)); } RCLCPP_PUBLIC @@ -293,17 +517,24 @@ class SubscriptionBase : public std::enable_shared_from_this bool matches_any_intra_process_publishers(const rmw_gid_t * sender_gid) const; + RCLCPP_PUBLIC + void + set_on_new_message_callback(rcl_event_callback_t callback, const void * user_data); + rclcpp::node_interfaces::NodeBaseInterface * const node_base_; std::shared_ptr node_handle_; std::shared_ptr subscription_handle_; std::shared_ptr intra_process_subscription_handle_; + rclcpp::Logger node_logger_; - std::vector> event_handlers_; + std::unordered_map> event_handlers_; bool use_intra_process_; IntraProcessManagerWeakPtr weak_ipm_; uint64_t intra_process_subscription_id_; + std::shared_ptr subscription_intra_process_; private: RCLCPP_DISABLE_COPY(SubscriptionBase) @@ -315,6 +546,9 @@ class SubscriptionBase : public std::enable_shared_from_this 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 reentrant_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 694be60a74..4da6c236bf 100644 --- a/rclcpp/include/rclcpp/subscription_factory.hpp +++ b/rclcpp/include/rclcpp/subscription_factory.hpp @@ -25,6 +25,7 @@ #include "rosidl_typesupport_cpp/message_type_support.hpp" #include "rclcpp/any_subscription_callback.hpp" +#include "rclcpp/get_message_type_support_handle.hpp" #include "rclcpp/intra_process_buffer_type.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/qos.hpp" @@ -74,26 +75,23 @@ template< typename MessageT, typename CallbackT, typename AllocatorT, - typename CallbackMessageT = - typename rclcpp::subscription_traits::has_message_type::type, - typename SubscriptionT = rclcpp::Subscription, - typename MessageMemoryStrategyT = rclcpp::message_memory_strategy::MessageMemoryStrategy< - CallbackMessageT, - AllocatorT - >> + typename SubscriptionT = rclcpp::Subscription, + typename MessageMemoryStrategyT = typename SubscriptionT::MessageMemoryStrategyType, + typename ROSMessageType = typename SubscriptionT::ROSMessageType +> 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 ) { auto allocator = options.get_allocator(); using rclcpp::AnySubscriptionCallback; - AnySubscriptionCallback any_subscription_callback(allocator); + AnySubscriptionCallback any_subscription_callback(*allocator); any_subscription_callback.set(std::forward(callback)); SubscriptionFactory factory { @@ -107,9 +105,9 @@ create_subscription_factory( using rclcpp::Subscription; using rclcpp::SubscriptionBase; - auto sub = Subscription::make_shared( + auto sub = Subscription::make_shared( node_base, - *rosidl_typesupport_cpp::get_message_type_support_handle(), + rclcpp::get_message_type_support_handle(), topic_name, qos, any_subscription_callback, diff --git a/rclcpp/include/rclcpp/subscription_options.hpp b/rclcpp/include/rclcpp/subscription_options.hpp index ade1bb8e9f..d6d2d4c60d 100644 --- a/rclcpp/include/rclcpp/subscription_options.hpp +++ b/rclcpp/include/rclcpp/subscription_options.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include "rclcpp/callback_group.hpp" @@ -45,6 +46,11 @@ struct SubscriptionOptionsBase /// True to ignore local publications. bool ignore_local_publications = false; + /// Require middleware to generate unique network flow endpoints + /// Disabled by default + rmw_unique_network_flow_endpoints_requirement_t require_unique_network_flow_endpoints = + RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_NOT_REQUIRED; + /// The callback group for this subscription. NULL to use the default callback group. rclcpp::CallbackGroup::SharedPtr callback_group = nullptr; @@ -81,6 +87,10 @@ struct SubscriptionOptionsBase template struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase { + static_assert( + std::is_void_v::value_type>, + "Subscription allocator value type must be void"); + /// Optional custom allocator. std::shared_ptr allocator = nullptr; @@ -102,12 +112,11 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase to_rcl_subscription_options(const rclcpp::QoS & qos) const { rcl_subscription_options_t result = rcl_subscription_get_default_options(); - using AllocatorTraits = std::allocator_traits; - using MessageAllocatorT = typename AllocatorTraits::template rebind_alloc; - auto message_alloc = std::make_shared(*this->get_allocator().get()); - result.allocator = allocator::get_rcl_allocator(*message_alloc); + result.allocator = this->get_rcl_allocator(); result.qos = qos.get_rmw_qos_profile(); result.rmw_subscription_options.ignore_local_publications = this->ignore_local_publications; + result.rmw_subscription_options.require_unique_network_flow_endpoints = + this->require_unique_network_flow_endpoints; // Apply payload to rcl_subscription_options if necessary. if (rmw_implementation_payload && rmw_implementation_payload->has_been_customized()) { @@ -117,15 +126,39 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase return result; } - /// Get the allocator, creating one if needed. std::shared_ptr get_allocator() const { if (!this->allocator) { - return std::make_shared(); + if (!allocator_storage_) { + allocator_storage_ = std::make_shared(); + } + return allocator_storage_; } return this->allocator; } + +private: + using PlainAllocator = + typename std::allocator_traits::template rebind_alloc; + + rcl_allocator_t + get_rcl_allocator() const + { + if (!plain_allocator_storage_) { + plain_allocator_storage_ = + std::make_shared(*this->get_allocator()); + } + return rclcpp::allocator::get_rcl_allocator(*plain_allocator_storage_); + } + + // This is a temporal workaround, to make sure that get_allocator() + // always returns a copy of the same allocator. + mutable std::shared_ptr allocator_storage_; + + // This is a temporal workaround, to keep the plain allocator that backs + // up the rcl allocator returned in rcl_subscription_options_t alive. + mutable std::shared_ptr plain_allocator_storage_; }; using SubscriptionOptions = SubscriptionOptionsWithAllocator>; diff --git a/rclcpp/include/rclcpp/subscription_traits.hpp b/rclcpp/include/rclcpp/subscription_traits.hpp index 8eacd3a14b..34f2cc9219 100644 --- a/rclcpp/include/rclcpp/subscription_traits.hpp +++ b/rclcpp/include/rclcpp/subscription_traits.hpp @@ -64,7 +64,7 @@ struct is_serialized_callback template struct extract_message_type { - using type = typename std::remove_cv::type; + using type = typename std::remove_cv_t>; }; template diff --git a/rclcpp/include/rclcpp/time_source.hpp b/rclcpp/include/rclcpp/time_source.hpp index 3f569b49f9..b4eb2aff24 100644 --- a/rclcpp/include/rclcpp/time_source.hpp +++ b/rclcpp/include/rclcpp/time_source.hpp @@ -25,6 +25,7 @@ #include "rcl_interfaces/msg/parameter_event.hpp" #include "rclcpp/node.hpp" +#include "rclcpp/executors.hpp" #include "rclcpp/node_interfaces/node_parameters_interface.hpp" @@ -57,7 +58,10 @@ class TimeSource * \param qos QoS that will be used when creating a `/clock` subscription. */ RCLCPP_PUBLIC - explicit TimeSource(rclcpp::Node::SharedPtr node, const rclcpp::QoS & qos = rclcpp::ClockQoS()); + explicit TimeSource( + rclcpp::Node::SharedPtr node, + const rclcpp::QoS & qos = rclcpp::ClockQoS(), + bool use_clock_thread = true); /// Empty constructor /** @@ -66,7 +70,9 @@ class TimeSource * \param qos QoS that will be used when creating a `/clock` subscription. */ RCLCPP_PUBLIC - explicit TimeSource(const rclcpp::QoS & qos = rclcpp::ClockQoS()); + explicit TimeSource( + const rclcpp::QoS & qos = rclcpp::ClockQoS(), + bool use_clock_thread = true); /// Attack node to the time source. /** @@ -118,6 +124,11 @@ class TimeSource RCLCPP_PUBLIC ~TimeSource(); +protected: + // Dedicated thread for clock subscription. + bool use_clock_thread_; + std::thread clock_executor_thread_; + private: // Preserve the node reference rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; @@ -140,9 +151,12 @@ class TimeSource using SubscriptionT = rclcpp::Subscription; std::shared_ptr clock_subscription_{nullptr}; std::mutex clock_sub_lock_; + rclcpp::CallbackGroup::SharedPtr clock_callback_group_; + rclcpp::executors::SingleThreadedExecutor::SharedPtr clock_executor_; + std::promise cancel_clock_executor_promise_; // The clock callback itself - void clock_cb(const rosgraph_msgs::msg::Clock::SharedPtr msg); + void clock_cb(std::shared_ptr msg); // Create the subscription for the clock topic void create_clock_sub(); @@ -156,7 +170,7 @@ class TimeSource std::shared_ptr parameter_subscription_; // Callback for parameter updates - void on_parameter_event(const rcl_interfaces::msg::ParameterEvent::SharedPtr event); + void on_parameter_event(std::shared_ptr event); // An enum to hold the parameter state enum UseSimTimeParameterState {UNSET, SET_TRUE, SET_FALSE}; @@ -177,7 +191,7 @@ class TimeSource // This is needed when new clocks are added. bool ros_time_active_{false}; // Last set message to be passed to newly registered clocks - rosgraph_msgs::msg::Clock::SharedPtr last_msg_set_; + std::shared_ptr last_msg_set_; // A lock to protect iterating the associated_clocks_ field. std::mutex clock_list_lock_; diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp index a2662f4abe..cc8e5c6ccd 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/rclcpp/include/rclcpp/timer.hpp @@ -181,7 +181,7 @@ class GenericTimer : public TimerBase TRACEPOINT( rclcpp_callback_register, static_cast(&callback_), - get_symbol(callback_)); + tracetools::get_symbol(callback_)); } /// Default destructor. diff --git a/rclcpp/include/rclcpp/type_adapter.hpp b/rclcpp/include/rclcpp/type_adapter.hpp new file mode 100644 index 0000000000..d6834ccc22 --- /dev/null +++ b/rclcpp/include/rclcpp/type_adapter.hpp @@ -0,0 +1,201 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__TYPE_ADAPTER_HPP_ +#define RCLCPP__TYPE_ADAPTER_HPP_ + +#include + +namespace rclcpp +{ + +/// Template structure used to adapt custom, user-defined types to ROS types. +/** + * Adapting a custom, user-defined type to a ROS type allows that custom type + * to be used when publishing and subscribing in ROS. + * + * In order to adapt a custom type to a ROS type, the user must create a + * template specialization of this structure for the custom type. + * In that specialization they must: + * + * - change `is_specialized` to `std::true_type`, + * - specify the custom type with `using custom_type = ...`, + * - specify the ROS type with `using ros_message_type = ...`, + * - provide static convert functions with the signatures: + * - static void convert_to_ros(const custom_type &, ros_message_type &) + * - static void convert_to_custom(const ros_message_type &, custom_type &) + * + * The convert functions must convert from one type to the other. + * + * For example, here is a theoretical example for adapting `std::string` to the + * `std_msgs::msg::String` ROS message type: + * + * template<> + * struct rclcpp::TypeAdapter + * { + * using is_specialized = std::true_type; + * using custom_type = std::string; + * using ros_message_type = std_msgs::msg::String; + * + * static + * void + * convert_to_ros_message( + * const custom_type & source, + * ros_message_type & destination) + * { + * destination.data = source; + * } + * + * static + * void + * convert_to_custom( + * const ros_message_type & source, + * custom_type & destination) + * { + * destination = source.data; + * } + * }; + * + * The adapter can then be used when creating a publisher or subscription, + * e.g.: + * + * using MyAdaptedType = TypeAdapter; + * auto pub = node->create_publisher("topic", 10); + * auto sub = node->create_subscription( + * "topic", + * 10, + * [](const std::string & msg) {...}); + * + * You can also be more declarative by using the adapt_type::as metafunctions, + * which are a bit less ambiguous to read: + * + * using AdaptedType = rclcpp::adapt_type::as; + * auto pub = node->create_publisher(...); + * + * If you wish, you may associate a custom type with a single ROS message type, + * allowing you to be a bit more brief when creating entities, e.g.: + * + * // First you must declare the association, this is similar to how you + * // would avoid using the namespace in C++ by doing `using std::vector;`. + * RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE(std::string, std_msgs::msg::String); + * + * // Then you can create things with just the custom type, and the ROS + * // message type is implied based on the previous statement. + * auto pub = node->create_publisher(...); + */ +template +struct TypeAdapter +{ + using is_specialized = std::false_type; + using custom_type = CustomType; + // In this case, the CustomType is the only thing given, or there is no specialization. + // Assign ros_message_type to CustomType for the former case. + using ros_message_type = CustomType; +}; + +/// Helper template to determine if a type is a TypeAdapter, false specialization. +template +struct is_type_adapter : std::false_type {}; + +/// Helper template to determine if a type is a TypeAdapter, true specialization. +template +struct is_type_adapter>: std::true_type {}; + +/// Identity specialization for TypeAdapter. +template +struct TypeAdapter::value>>: T {}; + +namespace detail +{ + +template +struct assert_type_pair_is_specialized_type_adapter +{ + using type_adapter = TypeAdapter; + static_assert( + type_adapter::is_specialized::value, + "No type adapter for this custom type/ros message type pair"); +}; + +} // namespace detail + +/// Template metafunction that can make the type being adapted explicit. +template +struct adapt_type +{ + template + using as = typename ::rclcpp::detail::assert_type_pair_is_specialized_type_adapter< + CustomType, + ROSMessageType + >::type_adapter; +}; + +/// Implicit type adapter used as a short hand way to create something with just the custom type. +/** + * This is used when creating a publisher or subscription using just the custom + * type in conjunction with RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE(). + * For example: + * + * #include "type_adapter_for_std_string_to_std_msgs_String.hpp" + * + * RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE(std::string, std_msgs::msg::String); + * + * int main(...) { + * // ... + * auto pub = node->create_publisher(...); + * } + * + * \sa TypeAdapter for more examples. + */ +template +struct ImplicitTypeAdapter +{ + using is_specialized = std::false_type; +}; + +/// Specialization of TypeAdapter for ImplicitTypeAdapter. +/** + * This allows for things like this: + * + * RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE(std::string, std_msgs::msg::String); + * auto pub = node->create_publisher("topic", 10); + * + */ +template +struct TypeAdapter::is_specialized::value>> + : ImplicitTypeAdapter +{}; + +/// Assigns the custom type implicitly to the given custom type/ros message type pair. +/** + * Note: this macro needs to be used in the root namespace. + * We cannot use ::rclcpp to protect against this, due to how GCC interprets the + * syntax, see: https://stackoverflow.com/a/2781537 + * + * \sa TypeAdapter + * \sa ImplicitTypeAdapter + */ +#define RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE(CustomType, ROSMessageType) \ + template<> \ + struct rclcpp::ImplicitTypeAdapter \ + : public rclcpp::TypeAdapter \ + { \ + static_assert( \ + is_specialized::value, \ + "Cannot use custom type as ros type when there is no TypeAdapter for that pair"); \ + } + +} // namespace rclcpp + +#endif // RCLCPP__TYPE_ADAPTER_HPP_ diff --git a/rclcpp/include/rclcpp/typesupport_helpers.hpp b/rclcpp/include/rclcpp/typesupport_helpers.hpp new file mode 100644 index 0000000000..2fad84cf3b --- /dev/null +++ b/rclcpp/include/rclcpp/typesupport_helpers.hpp @@ -0,0 +1,57 @@ +// Copyright 2018, Bosch Software Innovations GmbH. +// Copyright 2021, Apex.AI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__TYPESUPPORT_HELPERS_HPP_ +#define RCLCPP__TYPESUPPORT_HELPERS_HPP_ + +#include +#include +#include + +#include "rcpputils/shared_library.hpp" +#include "rosidl_runtime_cpp/message_type_support_decl.hpp" + +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +/// Load the type support library for the given type. +/** + * \param[in] type The topic type, e.g. "std_msgs/msg/String" + * \param[in] typesupport_identifier Type support identifier, typically "rosidl_typesupport_cpp" + * \return A shared library + */ +RCLCPP_PUBLIC +std::shared_ptr +get_typesupport_library(const std::string & type, const std::string & typesupport_identifier); + +/// Extract the type support handle from the library. +/** + * The library needs to match the topic type. The shared library must stay loaded for the lifetime of the result. + * \param[in] type The topic type, e.g. "std_msgs/msg/String" + * \param[in] typesupport_identifier Type support identifier, typically "rosidl_typesupport_cpp" + * \param[in] library The shared type support library + * \return A type support handle + */ +RCLCPP_PUBLIC +const rosidl_message_type_support_t * +get_typesupport_handle( + const std::string & type, + const std::string & typesupport_identifier, + rcpputils::SharedLibrary & library); + +} // namespace rclcpp + +#endif // RCLCPP__TYPESUPPORT_HELPERS_HPP_ diff --git a/rclcpp/include/rclcpp/utilities.hpp b/rclcpp/include/rclcpp/utilities.hpp index 2b6a3db37f..5011a12c6a 100644 --- a/rclcpp/include/rclcpp/utilities.hpp +++ b/rclcpp/include/rclcpp/utilities.hpp @@ -143,21 +143,6 @@ RCLCPP_PUBLIC bool ok(rclcpp::Context::SharedPtr context = nullptr); -/// Return true if init() has already been called for the given context. -/** - * If nullptr is given for the context, then the global context is used, i.e. - * the context initialized by rclcpp::init(). - * - * Deprecated, as it is no longer different from rcl_ok(). - * - * \param[in] context Optional check for initialization of this Context. - * \return true if the context is initialized, and false otherwise - */ -[[deprecated("use the function ok() instead, which has the same usage.")]] -RCLCPP_PUBLIC -bool -is_initialized(rclcpp::Context::SharedPtr context = nullptr); - /// Shutdown rclcpp context, invalidating it for derived entities. /** * If nullptr is given for the context, then the global context is used, i.e. diff --git a/rclcpp/include/rclcpp/wait_set_template.hpp b/rclcpp/include/rclcpp/wait_set_template.hpp index 39c7dd3d4e..51c0a513cd 100644 --- a/rclcpp/include/rclcpp/wait_set_template.hpp +++ b/rclcpp/include/rclcpp/wait_set_template.hpp @@ -155,7 +155,8 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli this->storage_add_subscription(std::move(local_subscription)); } if (mask.include_events) { - for (auto event : inner_subscription->get_event_handlers()) { + for (auto key_event_pair : inner_subscription->get_event_handlers()) { + auto event = key_event_pair.second; auto local_subscription = inner_subscription; bool already_in_use = local_subscription->exchange_in_use_by_wait_set_state(event.get(), true); @@ -225,7 +226,8 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli this->storage_remove_subscription(std::move(local_subscription)); } if (mask.include_events) { - for (auto event : inner_subscription->get_event_handlers()) { + for (auto key_event_pair : inner_subscription->get_event_handlers()) { + auto event = key_event_pair.second; auto local_subscription = inner_subscription; local_subscription->exchange_in_use_by_wait_set_state(event.get(), false); this->storage_remove_waitable(std::move(event)); diff --git a/rclcpp/include/rclcpp/waitable.hpp b/rclcpp/include/rclcpp/waitable.hpp index 9ae79c3508..8318f84266 100644 --- a/rclcpp/include/rclcpp/waitable.hpp +++ b/rclcpp/include/rclcpp/waitable.hpp @@ -16,6 +16,7 @@ #define RCLCPP__WAITABLE_HPP_ #include +#include #include #include "rclcpp/macros.hpp" @@ -203,12 +204,42 @@ class Waitable bool exchange_in_use_by_wait_set_state(bool in_use_state); - RCLCPP_PUBLIC + /// Set a callback to be called whenever the waitable becomes ready. + /** + * The callback receives a size_t which is the number of times the waitable was ready + * since the last time this callback was called. + * Normally this is 1, but can be > 1 if waitable was triggered before any + * callback was set. + * + * The callback also receives an int identifier argument. + * This is needed because a Waitable may be composed of several distinct entities, + * such as subscriptions, services, etc. + * The application should provide a generic callback function that will be then + * forwarded by the waitable to all of its entities. + * Before forwarding, a different value for the identifier argument will be + * bond to the function. + * This implies that the provided callback can use the identifier to behave + * differently depending on which entity triggered the waitable to become ready. + * + * Note: this function must be overridden with a proper implementation + * by the custom classes who inherit from rclcpp::Waitable if they want to use it. + * + * \sa rclcpp::Waitable::clear_on_ready_callback + * + * \param[in] callback functor to be called when the waitable becomes ready + */ + virtual + void + set_on_ready_callback(std::function callback); + + /// Unset any callback registered via set_on_ready_callback. + /** + * Note: this function must be overridden with a proper implementation + * by the custom classes who inherit from rclcpp::Waitable if they want to use it. + */ virtual void - set_listener_callback( - rmw_listener_callback_t callback, - const void * user_data) const; + clear_on_ready_callback(); private: std::atomic in_use_by_wait_set_{false}; diff --git a/rclcpp/package.xml b/rclcpp/package.xml index d876bfb377..28067938e5 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 6.3.1 + 11.0.0 The ROS client library in C++. Ivan Paunovic Mabel Zhang @@ -12,12 +12,14 @@ ament_cmake_ros + ament_index_cpp builtin_interfaces rcl_interfaces rosgraph_msgs rosidl_runtime_cpp rosidl_typesupport_c rosidl_typesupport_cpp + ament_index_cpp builtin_interfaces rcl_interfaces rosgraph_msgs diff --git a/rclcpp/src/rclcpp/client.cpp b/rclcpp/src/rclcpp/client.cpp index c74dd59c8d..e340a7d019 100644 --- a/rclcpp/src/rclcpp/client.cpp +++ b/rclcpp/src/rclcpp/client.cpp @@ -38,7 +38,8 @@ ClientBase::ClientBase( rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph) : node_graph_(node_graph), node_handle_(node_base->get_shared_rcl_node_handle()), - context_(node_base->get_context()) + context_(node_base->get_context()), + node_logger_(rclcpp::get_node_logger(node_handle_.get())) { std::weak_ptr weak_node_handle(node_handle_); rcl_client_t * new_rcl_client = new rcl_client_t; @@ -200,16 +201,15 @@ ClientBase::exchange_in_use_by_wait_set_state(bool in_use_state) } void -ClientBase::set_listener_callback( - rmw_listener_callback_t callback, - const void * user_data) const +ClientBase::set_on_new_response_callback(rcl_event_callback_t callback, const void * user_data) { - rcl_ret_t ret = rcl_client_set_listener_callback( + rcl_ret_t ret = rcl_client_set_on_new_response_callback( client_handle_.get(), callback, user_data); if (RCL_RET_OK != ret) { - throw std::runtime_error("Couldn't set listener callback to client"); + using rclcpp::exceptions::throw_from_rcl_error; + throw_from_rcl_error(ret, "failed to set the on new response callback for client"); } } diff --git a/rclcpp/src/rclcpp/context.cpp b/rclcpp/src/rclcpp/context.cpp index 9e00f7a1eb..0f51d70ce4 100644 --- a/rclcpp/src/rclcpp/context.cpp +++ b/rclcpp/src/rclcpp/context.cpp @@ -31,8 +31,6 @@ #include "rcutils/error_handling.h" #include "rcutils/macros.h" -#include "rmw/impl/cpp/demangle.hpp" - #include "./logging_mutex.hpp" using rclcpp::Context; @@ -291,7 +289,7 @@ Context::get_domain_id() const } std::string -Context::shutdown_reason() +Context::shutdown_reason() const { std::lock_guard lock(init_mutex_); return shutdown_reason_; @@ -315,9 +313,13 @@ Context::shutdown(const std::string & reason) // set shutdown reason shutdown_reason_ = reason; // call each shutdown callback - for (const auto & callback : on_shutdown_callbacks_) { - callback(); + { + std::lock_guard lock(on_shutdown_callbacks_mutex_); + for (const auto & callback : on_shutdown_callbacks_) { + (*callback)(); + } } + // interrupt all blocking sleep_for() and all blocking executors or wait sets this->interrupt_all_sleep_for(); // remove self from the global contexts @@ -344,20 +346,48 @@ Context::shutdown(const std::string & reason) rclcpp::Context::OnShutdownCallback Context::on_shutdown(OnShutdownCallback callback) { - on_shutdown_callbacks_.push_back(callback); + add_on_shutdown_callback(callback); return callback; } -const std::vector & -Context::get_on_shutdown_callbacks() const +rclcpp::OnShutdownCallbackHandle +Context::add_on_shutdown_callback(OnShutdownCallback callback) +{ + auto callback_shared_ptr = std::make_shared(callback); + { + std::lock_guard lock(on_shutdown_callbacks_mutex_); + on_shutdown_callbacks_.emplace(callback_shared_ptr); + } + + OnShutdownCallbackHandle callback_handle; + callback_handle.callback = callback_shared_ptr; + return callback_handle; +} + +bool +Context::remove_on_shutdown_callback(const OnShutdownCallbackHandle & callback_handle) { - return on_shutdown_callbacks_; + std::lock_guard lock(on_shutdown_callbacks_mutex_); + auto callback_shared_ptr = callback_handle.callback.lock(); + if (callback_shared_ptr == nullptr) { + return false; + } + return on_shutdown_callbacks_.erase(callback_shared_ptr) == 1; } -std::vector & -Context::get_on_shutdown_callbacks() +std::vector +Context::get_on_shutdown_callbacks() const { - return on_shutdown_callbacks_; + std::vector callbacks; + + { + std::lock_guard lock(on_shutdown_callbacks_mutex_); + for (auto & iter : on_shutdown_callbacks_) { + callbacks.emplace_back(*iter); + } + } + + return callbacks; } std::shared_ptr diff --git a/rclcpp/src/rclcpp/detail/mutex_two_priorities.cpp b/rclcpp/src/rclcpp/detail/mutex_two_priorities.cpp index e7c7a091e8..8deb864f5f 100644 --- a/rclcpp/src/rclcpp/detail/mutex_two_priorities.cpp +++ b/rclcpp/src/rclcpp/detail/mutex_two_priorities.cpp @@ -31,13 +31,31 @@ HighPriorityLockable::HighPriorityLockable(MutexTwoPriorities & parent) void HighPriorityLockable::lock() { - parent_.data_.lock(); + std::unique_lock guard{parent_.cv_mutex_}; + if (parent_.data_taken_) { + ++parent_.hp_waiting_count_; + while (parent_.data_taken_) { + parent_.hp_cv_.wait(guard); + } + --parent_.hp_waiting_count_; + } + parent_.data_taken_ = true; } void HighPriorityLockable::unlock() { - parent_.data_.unlock(); + bool notify_lp{false}; + { + std::lock_guard guard{parent_.cv_mutex_}; + parent_.data_taken_ = false; + notify_lp = 0u == parent_.hp_waiting_count_; + } + if (notify_lp) { + parent_.lp_cv_.notify_one(); + } else { + parent_.hp_cv_.notify_one(); + } } LowPriorityLockable::LowPriorityLockable(MutexTwoPriorities & parent) @@ -47,16 +65,27 @@ LowPriorityLockable::LowPriorityLockable(MutexTwoPriorities & parent) void LowPriorityLockable::lock() { - std::unique_lock barrier_guard{parent_.barrier_}; - parent_.data_.lock(); - barrier_guard.release(); + std::unique_lock guard{parent_.cv_mutex_}; + while (parent_.data_taken_ || parent_.hp_waiting_count_) { + parent_.lp_cv_.wait(guard); + } + parent_.data_taken_ = true; } void LowPriorityLockable::unlock() { - std::lock_guard barrier_guard{parent_.barrier_, std::adopt_lock}; - parent_.data_.unlock(); + bool notify_lp{false}; + { + std::lock_guard guard{parent_.cv_mutex_}; + parent_.data_taken_ = false; + notify_lp = 0u == parent_.hp_waiting_count_; + } + if (notify_lp) { + parent_.lp_cv_.notify_one(); + } else { + parent_.hp_cv_.notify_one(); + } } HighPriorityLockable diff --git a/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.cpp b/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.cpp new file mode 100644 index 0000000000..bb8b61dca6 --- /dev/null +++ b/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.cpp @@ -0,0 +1,77 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "./resolve_parameter_overrides.hpp" + +#include +#include +#include + +#include "rcl_yaml_param_parser/parser.h" + +#include "rclcpp/scope_exit.hpp" +#include "rclcpp/parameter_map.hpp" + +std::map +rclcpp::detail::resolve_parameter_overrides( + const std::string & node_fqn, + const std::vector & parameter_overrides, + const rcl_arguments_t * local_args, + const rcl_arguments_t * global_args) +{ + std::map result; + + // global before local so that local overwrites global + std::array argument_sources = {global_args, local_args}; + + // Get fully qualified node name post-remapping to use to find node's params in yaml files + + for (const rcl_arguments_t * source : argument_sources) { + if (!source) { + continue; + } + rcl_params_t * params = NULL; + rcl_ret_t ret = rcl_arguments_get_param_overrides(source, ¶ms); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + if (params) { + auto cleanup_params = make_scope_exit( + [params]() { + rcl_yaml_node_struct_fini(params); + }); + rclcpp::ParameterMap initial_map = rclcpp::parameter_map_from(params); + + // Enforce wildcard matching precedence + // TODO(cottsay) implement further wildcard matching + const std::array node_matching_names{"/**", node_fqn}; + for (const auto & node_name : node_matching_names) { + if (initial_map.count(node_name) > 0) { + // Combine parameter yaml files, overwriting values in older ones + for (const rclcpp::Parameter & param : initial_map.at(node_name)) { + result[param.get_name()] = + rclcpp::ParameterValue(param.get_value_message()); + } + } + } + } + } + + // parameter overrides passed to constructor will overwrite overrides from yaml file sources + for (auto & param : parameter_overrides) { + result[param.get_name()] = + rclcpp::ParameterValue(param.get_value_message()); + } + return result; +} diff --git a/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.hpp b/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.hpp new file mode 100644 index 0000000000..5eaeb0a75c --- /dev/null +++ b/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.hpp @@ -0,0 +1,44 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__DETAIL__RESOLVE_PARAMETER_OVERRIDES_HPP_ +#define RCLCPP__DETAIL__RESOLVE_PARAMETER_OVERRIDES_HPP_ + +#include +#include +#include + +#include "rcl/arguments.h" + +#include "rclcpp/parameter.hpp" +#include "rclcpp/parameter_value.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace detail +{ +/// \internal Get the parameter overrides from the arguments. +RCLCPP_LOCAL +std::map +resolve_parameter_overrides( + const std::string & node_name, + const std::vector & parameter_overrides, + const rcl_arguments_t * local_args, + const rcl_arguments_t * global_args); + +} // namespace detail +} // namespace rclcpp + +#endif // RCLCPP__DETAIL__RESOLVE_PARAMETER_OVERRIDES_HPP_ diff --git a/rclcpp/src/rclcpp/duration.cpp b/rclcpp/src/rclcpp/duration.cpp index 317d6b0e90..6c20757d46 100644 --- a/rclcpp/src/rclcpp/duration.cpp +++ b/rclcpp/src/rclcpp/duration.cpp @@ -67,13 +67,27 @@ Duration::operator builtin_interfaces::msg::Duration() const { builtin_interfaces::msg::Duration msg_duration; constexpr rcl_duration_value_t kDivisor = RCL_S_TO_NS(1); + constexpr int32_t max_s = std::numeric_limits::max(); + constexpr int32_t min_s = std::numeric_limits::min(); + constexpr uint32_t max_ns = std::numeric_limits::max(); const auto result = std::div(rcl_duration_.nanoseconds, kDivisor); if (result.rem >= 0) { - msg_duration.sec = static_cast(result.quot); - msg_duration.nanosec = static_cast(result.rem); + // saturate if we will overflow + if (result.quot > max_s) { + msg_duration.sec = max_s; + msg_duration.nanosec = max_ns; + } else { + msg_duration.sec = static_cast(result.quot); + msg_duration.nanosec = static_cast(result.rem); + } } else { - msg_duration.sec = static_cast(result.quot - 1); - msg_duration.nanosec = static_cast(kDivisor + result.rem); + if (result.quot <= min_s) { + msg_duration.sec = min_s; + msg_duration.nanosec = 0u; + } else { + msg_duration.sec = static_cast(result.quot - 1); + msg_duration.nanosec = static_cast(kDivisor + result.rem); + } } return msg_duration; } @@ -238,11 +252,14 @@ Duration::to_rmw_time() const throw std::runtime_error("rmw_time_t cannot be negative"); } - // reuse conversion logic from msg creation - builtin_interfaces::msg::Duration msg = *this; + // Purposefully avoid creating from builtin_interfaces::msg::Duration + // to avoid possible overflow converting from int64_t to int32_t, then back to uint64_t rmw_time_t result; - result.sec = static_cast(msg.sec); - result.nsec = static_cast(msg.nanosec); + constexpr rcl_duration_value_t kDivisor = RCL_S_TO_NS(1); + const auto div_result = std::div(rcl_duration_.nanoseconds, kDivisor); + result.sec = static_cast(div_result.quot); + result.nsec = static_cast(div_result.rem); + return result; } diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index d33df58ee2..5465a76d01 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -43,20 +43,14 @@ using rclcpp::FutureReturnCode; Executor::Executor(const rclcpp::ExecutorOptions & options) : spinning(false), + interrupt_guard_condition_(options.context), shutdown_guard_condition_(std::make_shared(options.context)), memory_strategy_(options.memory_strategy) { // Store the context for later use. context_ = options.context; - rcl_guard_condition_options_t guard_condition_options = rcl_guard_condition_get_default_options(); - rcl_ret_t ret = rcl_guard_condition_init( - &interrupt_guard_condition_, context_->get_rcl_context().get(), guard_condition_options); - if (RCL_RET_OK != ret) { - throw_from_rcl_error(ret, "Failed to create interrupt guard condition in Executor constructor"); - } - - context_->on_shutdown( + shutdown_callback_handle_ = context_->add_on_shutdown_callback( [weak_gc = std::weak_ptr{shutdown_guard_condition_}]() { auto strong_gc = weak_gc.lock(); if (strong_gc) { @@ -66,13 +60,13 @@ Executor::Executor(const rclcpp::ExecutorOptions & options) // The number of guard conditions is always at least 2: 1 for the ctrl-c guard cond, // and one for the executor's guard cond (interrupt_guard_condition_) - memory_strategy_->add_guard_condition(&shutdown_guard_condition_->get_rcl_guard_condition()); + memory_strategy_->add_guard_condition(shutdown_guard_condition_.get()); // Put the executor's guard condition in memory_strategy_->add_guard_condition(&interrupt_guard_condition_); rcl_allocator_t allocator = memory_strategy_->get_allocator(); - ret = rcl_wait_set_init( + rcl_ret_t ret = rcl_wait_set_init( &wait_set_, 0, 2, 0, 0, 0, 0, context_->get_rcl_context().get(), @@ -82,12 +76,6 @@ Executor::Executor(const rclcpp::ExecutorOptions & options) "rclcpp", "failed to create wait set: %s", rcl_get_error_string().str); rcl_reset_error(); - if (rcl_guard_condition_fini(&interrupt_guard_condition_) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "failed to destroy guard condition: %s", rcl_get_error_string().str); - rcl_reset_error(); - } throw_from_rcl_error(ret, "Failed to create wait set in Executor constructor"); } } @@ -129,15 +117,9 @@ Executor::~Executor() "failed to destroy wait set: %s", rcl_get_error_string().str); rcl_reset_error(); } - // Finalize the interrupt guard condition. - if (rcl_guard_condition_fini(&interrupt_guard_condition_) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "failed to destroy guard condition: %s", rcl_get_error_string().str); - rcl_reset_error(); - } // Remove and release the sigint guard condition - memory_strategy_->remove_guard_condition(&shutdown_guard_condition_->get_rcl_guard_condition()); + memory_strategy_->remove_guard_condition(shutdown_guard_condition_.get()); + memory_strategy_->remove_guard_condition(&interrupt_guard_condition_); } std::vector @@ -226,17 +208,14 @@ Executor::add_callback_group_to_map( // Also add to the map that contains all callback groups weak_groups_to_nodes_.insert(std::make_pair(weak_group_ptr, node_ptr)); if (is_new_node) { - rclcpp::node_interfaces::NodeBaseInterface::WeakPtr node_weak_ptr(node_ptr); - weak_nodes_to_guard_conditions_[node_weak_ptr] = node_ptr->get_notify_guard_condition(); + const auto & node_gc = node_ptr->get_notify_rclcpp_guard_condition(); + weak_nodes_to_guard_conditions_[node_ptr] = node_gc; if (notify) { // Interrupt waiting to handle new node - rcl_ret_t ret = rcl_trigger_guard_condition(&interrupt_guard_condition_); - if (ret != RCL_RET_OK) { - throw_from_rcl_error(ret, "Failed to trigger guard condition on callback group add"); - } + interrupt_guard_condition_.trigger(); } // Add the node's notify condition to the guard condition handles - memory_strategy_->add_guard_condition(node_ptr->get_notify_guard_condition()); + memory_strategy_->add_guard_condition(node_gc); } } @@ -303,15 +282,11 @@ Executor::remove_callback_group_from_map( if (!has_node(node_ptr, weak_groups_to_nodes_associated_with_executor_) && !has_node(node_ptr, weak_groups_associated_with_executor_to_nodes_)) { - rclcpp::node_interfaces::NodeBaseInterface::WeakPtr node_weak_ptr(node_ptr); - weak_nodes_to_guard_conditions_.erase(node_weak_ptr); + weak_nodes_to_guard_conditions_.erase(node_ptr); if (notify) { - rcl_ret_t ret = rcl_trigger_guard_condition(&interrupt_guard_condition_); - if (ret != RCL_RET_OK) { - throw_from_rcl_error(ret, "Failed to trigger guard condition on callback group remove"); - } + interrupt_guard_condition_.trigger(); } - memory_strategy_->remove_guard_condition(node_ptr->get_notify_guard_condition()); + memory_strategy_->remove_guard_condition(node_ptr->get_notify_rclcpp_guard_condition()); } } @@ -482,10 +457,7 @@ void Executor::cancel() { spinning.store(false); - rcl_ret_t ret = rcl_trigger_guard_condition(&interrupt_guard_condition_); - if (ret != RCL_RET_OK) { - throw_from_rcl_error(ret, "Failed to trigger guard condition in cancel"); - } + interrupt_guard_condition_.trigger(); } void @@ -523,10 +495,7 @@ Executor::execute_any_executable(AnyExecutable & any_exec) any_exec.callback_group->can_be_taken_from().store(true); // Wake the wait, because it may need to be recalculated or work that // was previously blocked is now available. - rcl_ret_t ret = rcl_trigger_guard_condition(&interrupt_guard_condition_); - if (ret != RCL_RET_OK) { - throw_from_rcl_error(ret, "Failed to trigger guard condition from execute_any_executable"); - } + interrupt_guard_condition_.trigger(); } static @@ -581,8 +550,7 @@ Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription) [&]() {return subscription->take_serialized(*serialized_msg.get(), message_info);}, [&]() { - auto void_serialized_msg = std::static_pointer_cast(serialized_msg); - subscription->handle_message(void_serialized_msg, message_info); + subscription->handle_serialized_message(serialized_msg, message_info); }); subscription->return_serialized_message(serialized_msg); } else if (subscription->can_loan_messages()) { @@ -610,16 +578,18 @@ Executor::execute_subscription(rclcpp::SubscriptionBase::SharedPtr subscription) return true; }, [&]() {subscription->handle_loaned_message(loaned_msg, message_info);}); - rcl_ret_t ret = rcl_return_loaned_message_from_subscription( - subscription->get_subscription_handle().get(), - loaned_msg); - if (RCL_RET_OK != ret) { - RCLCPP_ERROR( - rclcpp::get_logger("rclcpp"), - "rcl_return_loaned_message_from_subscription() failed for subscription on topic '%s': %s", - subscription->get_topic_name(), rcl_get_error_string().str); + if (nullptr != loaned_msg) { + rcl_ret_t ret = rcl_return_loaned_message_from_subscription( + subscription->get_subscription_handle().get(), + loaned_msg); + if (RCL_RET_OK != ret) { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), + "rcl_return_loaned_message_from_subscription() failed for subscription on topic '%s': %s", + subscription->get_topic_name(), rcl_get_error_string().str); + } + loaned_msg = nullptr; } - loaned_msg = nullptr; } else { // This case is taking a copy of the message data from the middleware via // inter-process communication. diff --git a/rclcpp/src/rclcpp/executors/events_executor.cpp b/rclcpp/src/rclcpp/executors/events_executor.cpp index 0ccda41211..1d609913b5 100644 --- a/rclcpp/src/rclcpp/executors/events_executor.cpp +++ b/rclcpp/src/rclcpp/executors/events_executor.cpp @@ -47,7 +47,7 @@ EventsExecutor::EventsExecutor( // Setup the executor notifier to wake up the executor when some guard conditions are tiggered. // The added guard conditions are guaranteed to not go out of scope before the executor itself. executor_notifier_ = std::make_shared(); - executor_notifier_->add_guard_condition(&shutdown_guard_condition_->get_rcl_guard_condition()); + executor_notifier_->add_guard_condition(shutdown_guard_condition_.get()); executor_notifier_->add_guard_condition(&interrupt_guard_condition_); entities_collector_->add_waitable(executor_notifier_); @@ -253,7 +253,7 @@ EventsExecutor::execute_event(const ExecutorEvent & event) switch (event.type) { case SUBSCRIPTION_EVENT: { - auto subscription = entities_collector_->get_subscription(event.entity_id); + auto subscription = entities_collector_->get_subscription(event.exec_entity_id); if (subscription) { for (size_t i = 0; i < event.num_events; i++) { @@ -265,7 +265,7 @@ EventsExecutor::execute_event(const ExecutorEvent & event) case SERVICE_EVENT: { - auto service = entities_collector_->get_service(event.entity_id); + auto service = entities_collector_->get_service(event.exec_entity_id); if (service) { for (size_t i = 0; i < event.num_events; i++) { @@ -277,7 +277,7 @@ EventsExecutor::execute_event(const ExecutorEvent & event) case CLIENT_EVENT: { - auto client = entities_collector_->get_client(event.entity_id); + auto client = entities_collector_->get_client(event.exec_entity_id); if (client) { for (size_t i = 0; i < event.num_events; i++) { @@ -289,7 +289,7 @@ EventsExecutor::execute_event(const ExecutorEvent & event) case WAITABLE_EVENT: { - auto waitable = entities_collector_->get_waitable(event.entity_id); + auto waitable = entities_collector_->get_waitable(event.exec_entity_id); if (waitable) { for (size_t i = 0; i < event.num_events; i++) { diff --git a/rclcpp/src/rclcpp/executors/events_executor_entities_collector.cpp b/rclcpp/src/rclcpp/executors/events_executor_entities_collector.cpp index 1122be96df..1d46b9d187 100644 --- a/rclcpp/src/rclcpp/executors/events_executor_entities_collector.cpp +++ b/rclcpp/src/rclcpp/executors/events_executor_entities_collector.cpp @@ -20,7 +20,7 @@ #include "rclcpp/executors/events_executor.hpp" #include "rclcpp/executors/events_executor_entities_collector.hpp" -using rclcpp::executors::EventsExecutorCallbackData; +using rclcpp::executors::ExecutorEvent; using rclcpp::executors::EventsExecutorEntitiesCollector; EventsExecutorEntitiesCollector::EventsExecutorEntitiesCollector( @@ -67,7 +67,7 @@ EventsExecutorEntitiesCollector::~EventsExecutorEntitiesCollector() for (const auto & pair : weak_nodes_to_guard_conditions_) { auto node = pair.first.lock(); if (node) { - auto node_gc = pair.second; + auto & node_gc = pair.second; unset_guard_condition_callback(node_gc); } } @@ -76,7 +76,6 @@ EventsExecutorEntitiesCollector::~EventsExecutorEntitiesCollector() weak_nodes_.clear(); weak_clients_map_.clear(); weak_services_map_.clear(); - callback_data_map_.clear(); weak_waitables_map_.clear(); weak_subscriptions_map_.clear(); weak_nodes_to_guard_conditions_.clear(); @@ -141,10 +140,10 @@ EventsExecutorEntitiesCollector::add_callback_group( if (is_new_node) { // Set an event callback for the node's notify guard condition, so if new entities are added // or removed to this node we will receive an event. - set_guard_condition_callback(node_ptr->get_notify_guard_condition()); + set_guard_condition_callback(node_ptr->get_notify_rclcpp_guard_condition()); // Store node's notify guard condition - weak_nodes_to_guard_conditions_[node_ptr] = node_ptr->get_notify_guard_condition(); + weak_nodes_to_guard_conditions_[node_ptr] = node_ptr->get_notify_rclcpp_guard_condition(); } // Add callback group to weak_groups_to_node @@ -238,9 +237,8 @@ EventsExecutorEntitiesCollector::set_callback_group_entities_callbacks( if (subscription) { weak_subscriptions_map_.emplace(subscription.get(), subscription); - subscription->set_listener_callback( - &EventsExecutor::push_event, - get_callback_data(subscription.get(), SUBSCRIPTION_EVENT)); + subscription->set_on_new_message_callback( + create_entity_callback(subscription.get(), SUBSCRIPTION_EVENT)); } return false; }); @@ -249,9 +247,8 @@ EventsExecutorEntitiesCollector::set_callback_group_entities_callbacks( if (service) { weak_services_map_.emplace(service.get(), service); - service->set_listener_callback( - &EventsExecutor::push_event, - get_callback_data(service.get(), SERVICE_EVENT)); + service->set_on_new_request_callback( + create_entity_callback(service.get(), SERVICE_EVENT)); } return false; }); @@ -260,9 +257,8 @@ EventsExecutorEntitiesCollector::set_callback_group_entities_callbacks( if (client) { weak_clients_map_.emplace(client.get(), client); - client->set_listener_callback( - &EventsExecutor::push_event, - get_callback_data(client.get(), CLIENT_EVENT)); + client->set_on_new_response_callback( + create_entity_callback(client.get(), CLIENT_EVENT)); } return false; }); @@ -271,9 +267,8 @@ EventsExecutorEntitiesCollector::set_callback_group_entities_callbacks( if (waitable) { weak_waitables_map_.emplace(waitable.get(), waitable); - waitable->set_listener_callback( - &EventsExecutor::push_event, - get_callback_data(waitable.get(), WAITABLE_EVENT)); + waitable->set_on_ready_callback( + create_waitable_callback(waitable.get())); } return false; }); @@ -296,36 +291,32 @@ EventsExecutorEntitiesCollector::unset_callback_group_entities_callbacks( group->find_subscription_ptrs_if( [this](const rclcpp::SubscriptionBase::SharedPtr & subscription) { if (subscription) { - subscription->set_listener_callback(nullptr, nullptr); + subscription->clear_on_new_message_callback(); weak_subscriptions_map_.erase(subscription.get()); - remove_callback_data(subscription.get(), SUBSCRIPTION_EVENT); } return false; }); group->find_service_ptrs_if( [this](const rclcpp::ServiceBase::SharedPtr & service) { if (service) { - service->set_listener_callback(nullptr, nullptr); + service->clear_on_new_request_callback(); weak_services_map_.erase(service.get()); - remove_callback_data(service.get(), SERVICE_EVENT); } return false; }); group->find_client_ptrs_if( [this](const rclcpp::ClientBase::SharedPtr & client) { if (client) { - client->set_listener_callback(nullptr, nullptr); + client->clear_on_new_response_callback(); weak_clients_map_.erase(client.get()); - remove_callback_data(client.get(), CLIENT_EVENT); } return false; }); group->find_waitable_ptrs_if( [this](const rclcpp::Waitable::SharedPtr & waitable) { if (waitable) { - waitable->set_listener_callback(nullptr, nullptr); + waitable->clear_on_ready_callback(); weak_waitables_map_.erase(waitable.get()); - remove_callback_data(waitable.get(), WAITABLE_EVENT); } return false; }); @@ -375,7 +366,7 @@ EventsExecutorEntitiesCollector::remove_callback_group_from_map( // Node doesn't have more callback groups associated to the executor. // Unset the event callback for the node's notify guard condition, to stop // receiving events if entities are added or removed to this node. - unset_guard_condition_callback(node_ptr->get_notify_guard_condition()); + unset_guard_condition_callback(node_ptr->get_notify_rclcpp_guard_condition()); // Remove guard condition from list rclcpp::node_interfaces::NodeBaseInterface::WeakPtr weak_node_ptr(node_ptr); @@ -485,30 +476,30 @@ EventsExecutorEntitiesCollector::get_automatically_added_callback_groups_from_no void EventsExecutorEntitiesCollector::set_guard_condition_callback( - const rcl_guard_condition_t * guard_condition) + rclcpp::GuardCondition * guard_condition) { - rcl_ret_t ret = rcl_guard_condition_set_listener_callback( - guard_condition, - &EventsExecutor::push_event, - get_callback_data(this, WAITABLE_EVENT)); + auto gc_callback = [this](size_t num_events) { + // Override num events (we don't care more than a single event) + num_events = 1; + int gc_id = -1; + ExecutorEvent event = {this, gc_id, WAITABLE_EVENT, num_events}; + // Event queue mutex scope + { + std::unique_lock lock(associated_executor_->push_mutex_); + associated_executor_->events_queue_->push(event); + } + // Notify that the event queue has some events in it. + associated_executor_->events_queue_cv_.notify_one(); + }; - if (ret != RCL_RET_OK) { - throw std::runtime_error("Couldn't set guard condition event callback"); - } + guard_condition->set_on_trigger_callback(gc_callback); } void EventsExecutorEntitiesCollector::unset_guard_condition_callback( - const rcl_guard_condition_t * guard_condition) + rclcpp::GuardCondition * guard_condition) { - rcl_ret_t ret = rcl_guard_condition_set_listener_callback( - guard_condition, nullptr, nullptr); - - if (ret != RCL_RET_OK) { - throw std::runtime_error("Couldn't unset guard condition event callback"); - } - - remove_callback_data(this, WAITABLE_EVENT); + guard_condition->set_on_trigger_callback(nullptr); } rclcpp::SubscriptionBase::SharedPtr @@ -592,56 +583,37 @@ EventsExecutorEntitiesCollector::add_waitable(rclcpp::Waitable::SharedPtr waitab { weak_waitables_map_.emplace(waitable.get(), waitable); - waitable->set_listener_callback( - &EventsExecutor::push_event, - get_callback_data(waitable.get(), WAITABLE_EVENT)); + waitable->set_on_ready_callback( + create_waitable_callback(waitable.get())); } -const EventsExecutorCallbackData * -EventsExecutorEntitiesCollector::get_callback_data( - void * entity_id, ExecutorEventType event_type) +std::function +EventsExecutorEntitiesCollector::create_entity_callback( + void * exec_entity_id, ExecutorEventType event_type) { - // Create an entity callback data object and check if - // we already have stored one like it - ExecutorEvent event = {entity_id, event_type, 0}; - EventsExecutorCallbackData data(associated_executor_, event); - - auto it = callback_data_map_.find(data); - - if (it != callback_data_map_.end()) { - // We found a callback data matching entity ID and type. - // Increment callback data counter and return pointer to data - it->second++; - return &it->first; - } - - // There was no callback data object matching ID and type, - // create one and set counter to 1. - callback_data_map_.emplace(data, 1); - - // Return a pointer to the just added entity callback data. - it = callback_data_map_.find(data); - return &it->first; + return [this, exec_entity_id, event_type](size_t num_events) { + ExecutorEvent event = {exec_entity_id, -1, event_type, num_events}; + // Event queue mutex scope + { + std::unique_lock lock(associated_executor_->push_mutex_); + associated_executor_->events_queue_->push(event); + } + // Notify that the event queue has some events in it. + associated_executor_->events_queue_cv_.notify_one(); + }; } -void -EventsExecutorEntitiesCollector::remove_callback_data( - void * entity_id, ExecutorEventType event_type) +std::function +EventsExecutorEntitiesCollector::create_waitable_callback(void * exec_entity_id) { - // Create an entity callback data object and check if - // we already have stored one like it - ExecutorEvent event = {entity_id, event_type, 0}; - EventsExecutorCallbackData data(associated_executor_, event); - - auto it = callback_data_map_.find(data); - - if (it != callback_data_map_.end()) { - // We found a callback data matching entity ID and type. - // If we have more than 1 decrement counter, otherwise remove it. - if (it->second > 1) { - it->second--; - } else { - callback_data_map_.erase(it); + return [this, exec_entity_id](size_t num_events, int gen_entity_id) { + ExecutorEvent event = {exec_entity_id, gen_entity_id, WAITABLE_EVENT, num_events}; + // Event queue mutex scope + { + std::unique_lock lock(associated_executor_->push_mutex_); + associated_executor_->events_queue_->push(event); } - } + // Notify that the event queue has some events in it. + associated_executor_->events_queue_cv_.notify_one(); + }; } diff --git a/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp b/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp index a0f55d3d5b..21a2386bfa 100644 --- a/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp +++ b/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp @@ -62,7 +62,7 @@ void StaticExecutorEntitiesCollector::init( rcl_wait_set_t * p_wait_set, rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy, - rcl_guard_condition_t * executor_guard_condition) + GuardCondition * executor_guard_condition) { // Empty initialize executable list exec_list_ = rclcpp::experimental::ExecutableList(); @@ -76,9 +76,13 @@ StaticExecutorEntitiesCollector::init( // Add executor's guard condition memory_strategy_->add_guard_condition(executor_guard_condition); + // Get memory strategy and executable list. Prepare wait_set_ std::shared_ptr shared_ptr; execute(shared_ptr); + + // The entities collector is now initialized + initialized_ = true; } void @@ -263,10 +267,7 @@ StaticExecutorEntitiesCollector::add_to_wait_set(rcl_wait_set_t * wait_set) // Add waitable guard conditions (one for each registered node) into the wait set. for (const auto & pair : weak_nodes_to_guard_conditions_) { auto & gc = pair.second; - rcl_ret_t ret = rcl_wait_set_add_guard_condition(wait_set, gc, NULL); - if (ret != RCL_RET_OK) { - throw std::runtime_error("Executor waitable: couldn't add guard condition to wait set"); - } + gc->add_to_wait_set(wait_set); } return true; } @@ -323,7 +324,7 @@ StaticExecutorEntitiesCollector::add_callback_group( throw std::runtime_error("Callback group was already added to executor."); } if (is_new_node) { - weak_nodes_to_guard_conditions_[node_ptr] = node_ptr->get_notify_guard_condition(); + weak_nodes_to_guard_conditions_[node_ptr] = node_ptr->get_notify_rclcpp_guard_condition(); return true; } return false; @@ -429,8 +430,9 @@ StaticExecutorEntitiesCollector::is_ready(rcl_wait_set_t * p_wait_set) auto found_guard_condition = std::find_if( weak_nodes_to_guard_conditions_.begin(), weak_nodes_to_guard_conditions_.end(), [&](std::pair pair) -> bool { - return pair.second == p_wait_set->guard_conditions[i]; + const GuardCondition *> 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; diff --git a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp index 4b5afc308f..680b755b95 100644 --- a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp @@ -14,6 +14,7 @@ #include "rclcpp/executors/static_single_threaded_executor.hpp" +#include #include #include @@ -29,7 +30,12 @@ StaticSingleThreadedExecutor::StaticSingleThreadedExecutor( entities_collector_ = std::make_shared(); } -StaticSingleThreadedExecutor::~StaticSingleThreadedExecutor() {} +StaticSingleThreadedExecutor::~StaticSingleThreadedExecutor() +{ + if (entities_collector_->is_init()) { + entities_collector_->fini(); + } +} void StaticSingleThreadedExecutor::spin() @@ -42,7 +48,6 @@ StaticSingleThreadedExecutor::spin() // Set memory_strategy_ and exec_list_ based on weak_nodes_ // Prepare wait_set_ based on memory_strategy_ entities_collector_->init(&wait_set_, memory_strategy_, &interrupt_guard_condition_); - RCLCPP_SCOPE_EXIT(entities_collector_->fini()); while (rclcpp::ok(this->context_) && spinning.load()) { // Refresh wait set and wait for work @@ -51,6 +56,79 @@ StaticSingleThreadedExecutor::spin() } } +void +StaticSingleThreadedExecutor::spin_some(std::chrono::nanoseconds max_duration) +{ + // In this context a 0 input max_duration means no duration limit + if (std::chrono::nanoseconds(0) == max_duration) { + max_duration = std::chrono::nanoseconds::max(); + } + + return this->spin_some_impl(max_duration, false); +} + +void +StaticSingleThreadedExecutor::spin_all(std::chrono::nanoseconds max_duration) +{ + if (max_duration <= std::chrono::nanoseconds(0)) { + throw std::invalid_argument("max_duration must be positive"); + } + return this->spin_some_impl(max_duration, true); +} + +void +StaticSingleThreadedExecutor::spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive) +{ + // Make sure the entities collector has been initialized + if (!entities_collector_->is_init()) { + entities_collector_->init(&wait_set_, memory_strategy_, &interrupt_guard_condition_); + } + + auto start = std::chrono::steady_clock::now(); + auto max_duration_not_elapsed = [max_duration, start]() { + if (std::chrono::nanoseconds(0) == max_duration) { + // told to spin forever if need be + return true; + } else if (std::chrono::steady_clock::now() - start < max_duration) { + // told to spin only for some maximum amount of time + return true; + } + // spun too long + return false; + }; + + if (spinning.exchange(true)) { + throw std::runtime_error("spin_some() called while already spinning"); + } + RCLCPP_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; + } + } +} + +void +StaticSingleThreadedExecutor::spin_once_impl(std::chrono::nanoseconds timeout) +{ + // Make sure the entities collector has been initialized + if (!entities_collector_->is_init()) { + entities_collector_->init(&wait_set_, memory_strategy_, &interrupt_guard_condition_); + } + + if (rclcpp::ok(context_) && spinning.load()) { + // Wait until we have a ready entity or timeout expired + entities_collector_->refresh_wait_set(timeout); + // Execute ready executables + execute_ready_executables(true); + } +} + void StaticSingleThreadedExecutor::add_callback_group( rclcpp::CallbackGroup::SharedPtr group_ptr, @@ -60,9 +138,7 @@ StaticSingleThreadedExecutor::add_callback_group( bool is_new_node = entities_collector_->add_callback_group(group_ptr, node_ptr); if (is_new_node && notify) { // Interrupt waiting to handle new node - if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) { - throw std::runtime_error(rcl_get_error_string().str); - } + interrupt_guard_condition_.trigger(); } } @@ -73,9 +149,7 @@ StaticSingleThreadedExecutor::add_node( bool is_new_node = entities_collector_->add_node(node_ptr); if (is_new_node && notify) { // Interrupt waiting to handle new node - if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) { - throw std::runtime_error(rcl_get_error_string().str); - } + interrupt_guard_condition_.trigger(); } } @@ -92,9 +166,7 @@ StaticSingleThreadedExecutor::remove_callback_group( bool node_removed = entities_collector_->remove_callback_group(group_ptr); // If the node was matched and removed, interrupt waiting if (node_removed && notify) { - if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) { - throw std::runtime_error(rcl_get_error_string().str); - } + interrupt_guard_condition_.trigger(); } } @@ -108,9 +180,7 @@ StaticSingleThreadedExecutor::remove_node( } // If the node was matched and removed, interrupt waiting if (notify) { - if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) { - throw std::runtime_error(rcl_get_error_string().str); - } + interrupt_guard_condition_.trigger(); } } @@ -138,14 +208,20 @@ StaticSingleThreadedExecutor::remove_node(std::shared_ptr node_ptr this->remove_node(node_ptr->get_node_base_interface(), notify); } -void -StaticSingleThreadedExecutor::execute_ready_executables() +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; } } } @@ -154,6 +230,10 @@ StaticSingleThreadedExecutor::execute_ready_executables() if (i < entities_collector_->get_number_of_timers()) { if (wait_set_.timers[i] && entities_collector_->get_timer(i)->is_ready()) { execute_timer(entities_collector_->get_timer(i)); + if (spin_once) { + return true; + } + any_ready_executable = true; } } } @@ -162,6 +242,10 @@ StaticSingleThreadedExecutor::execute_ready_executables() 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; } } } @@ -170,6 +254,10 @@ StaticSingleThreadedExecutor::execute_ready_executables() 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; } } } @@ -179,6 +267,11 @@ StaticSingleThreadedExecutor::execute_ready_executables() if (waitable->is_ready(&wait_set_)) { auto data = waitable->take_data(); waitable->execute(data); + if (spin_once) { + return true; + } + any_ready_executable = true; } } + return any_ready_executable; } diff --git a/rclcpp/src/rclcpp/generic_publisher.cpp b/rclcpp/src/rclcpp/generic_publisher.cpp new file mode 100644 index 0000000000..733aa5dd5c --- /dev/null +++ b/rclcpp/src/rclcpp/generic_publisher.cpp @@ -0,0 +1,34 @@ +// Copyright 2018, Bosch Software Innovations GmbH. +// Copyright 2021, Apex.AI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp/generic_publisher.hpp" + +#include +#include + +namespace rclcpp +{ + +void GenericPublisher::publish(const rclcpp::SerializedMessage & message) +{ + auto return_code = rcl_publish_serialized_message( + get_publisher_handle().get(), &message.get_rcl_serialized_message(), NULL); + + if (return_code != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(return_code, "failed to publish serialized message"); + } +} + +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/generic_subscription.cpp b/rclcpp/src/rclcpp/generic_subscription.cpp new file mode 100644 index 0000000000..cc50955773 --- /dev/null +++ b/rclcpp/src/rclcpp/generic_subscription.cpp @@ -0,0 +1,75 @@ +// Copyright 2018, Bosch Software Innovations GmbH. +// Copyright 2021, Apex.AI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp/generic_subscription.hpp" + +#include +#include + +#include "rcl/subscription.h" + +#include "rclcpp/exceptions.hpp" + +namespace rclcpp +{ + +std::shared_ptr GenericSubscription::create_message() +{ + return create_serialized_message(); +} + +std::shared_ptr GenericSubscription::create_serialized_message() +{ + return std::make_shared(0); +} + +void GenericSubscription::handle_message( + std::shared_ptr &, + const rclcpp::MessageInfo &) +{ + throw rclcpp::exceptions::UnimplementedError( + "handle_message is not implemented for GenericSubscription"); +} + +void +GenericSubscription::handle_serialized_message( + const std::shared_ptr & message, + const rclcpp::MessageInfo &) +{ + callback_(message); +} + +void GenericSubscription::handle_loaned_message( + void * message, const rclcpp::MessageInfo & message_info) +{ + (void) message; + (void) message_info; + throw rclcpp::exceptions::UnimplementedError( + "handle_loaned_message is not implemented for GenericSubscription"); +} + +void GenericSubscription::return_message(std::shared_ptr & message) +{ + auto typed_message = std::static_pointer_cast(message); + return_serialized_message(typed_message); +} + +void GenericSubscription::return_serialized_message( + std::shared_ptr & message) +{ + message.reset(); +} + +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/graph_listener.cpp b/rclcpp/src/rclcpp/graph_listener.cpp index 0af47acc5b..0589d65cab 100644 --- a/rclcpp/src/rclcpp/graph_listener.cpp +++ b/rclcpp/src/rclcpp/graph_listener.cpp @@ -40,19 +40,9 @@ GraphListener::GraphListener(const std::shared_ptr & parent_context) : weak_parent_context_(parent_context), rcl_parent_context_(parent_context->get_rcl_context()), is_started_(false), - is_shutdown_(false) + is_shutdown_(false), + interrupt_guard_condition_(parent_context) { - // TODO(wjwwood): make a guard condition class in rclcpp so this can be tracked - // automatically with the rcl guard condition - // hold on to this context to prevent it from going out of scope while this - // guard condition is using it. - rcl_ret_t ret = rcl_guard_condition_init( - &interrupt_guard_condition_, - rcl_parent_context_.get(), - rcl_guard_condition_get_default_options()); - if (RCL_RET_OK != ret) { - throw_from_rcl_error(ret, "failed to create interrupt guard condition"); - } } GraphListener::~GraphListener() @@ -159,10 +149,7 @@ GraphListener::run_loop() throw_from_rcl_error(ret, "failed to clear wait set"); } // Put the interrupt guard condition in the wait set. - ret = rcl_wait_set_add_guard_condition(&wait_set_, &interrupt_guard_condition_, NULL); - if (RCL_RET_OK != ret) { - throw_from_rcl_error(ret, "failed to add interrupt guard condition to wait set"); - } + interrupt_guard_condition_.add_to_wait_set(&wait_set_); // Put graph guard conditions for each node into the wait set. std::vector graph_gc_indexes(node_graph_interfaces_size, 0u); @@ -211,19 +198,16 @@ GraphListener::run_loop() } static void -interrupt_(rcl_guard_condition_t * interrupt_guard_condition) +interrupt_(GuardCondition * interrupt_guard_condition) { - rcl_ret_t ret = rcl_trigger_guard_condition(interrupt_guard_condition); - if (RCL_RET_OK != ret) { - throw_from_rcl_error(ret, "failed to trigger the interrupt guard condition"); - } + interrupt_guard_condition->trigger(); } static void acquire_nodes_lock_( std::mutex * node_graph_interfaces_barrier_mutex, std::mutex * node_graph_interfaces_mutex, - rcl_guard_condition_t * interrupt_guard_condition) + GuardCondition * interrupt_guard_condition) { { // Acquire this lock to prevent the run loop from re-locking the @@ -351,10 +335,6 @@ GraphListener::__shutdown() interrupt_(&interrupt_guard_condition_); listener_thread_.join(); } - rcl_ret_t ret = rcl_guard_condition_fini(&interrupt_guard_condition_); - if (RCL_RET_OK != ret) { - throw_from_rcl_error(ret, "failed to finalize interrupt guard condition"); - } if (is_started_) { cleanup_wait_set(); } diff --git a/rclcpp/src/rclcpp/guard_condition.cpp b/rclcpp/src/rclcpp/guard_condition.cpp index 22ca3f3223..3d44638ea7 100644 --- a/rclcpp/src/rclcpp/guard_condition.cpp +++ b/rclcpp/src/rclcpp/guard_condition.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include + #include "rclcpp/guard_condition.hpp" #include "rclcpp/exceptions.hpp" @@ -20,19 +22,20 @@ namespace rclcpp { -GuardCondition::GuardCondition(rclcpp::Context::SharedPtr context) +GuardCondition::GuardCondition( + rclcpp::Context::SharedPtr context, + rcl_guard_condition_options_t guard_condition_options) : context_(context), rcl_guard_condition_{rcl_get_zero_initialized_guard_condition()} { if (!context_) { throw std::invalid_argument("context argument unexpectedly nullptr"); } - rcl_guard_condition_options_t guard_condition_options = rcl_guard_condition_get_default_options(); rcl_ret_t ret = rcl_guard_condition_init( &this->rcl_guard_condition_, context_->get_rcl_context().get(), guard_condition_options); if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to create guard condition"); } } @@ -45,7 +48,7 @@ GuardCondition::~GuardCondition() } catch (const std::exception & exception) { RCLCPP_ERROR( rclcpp::get_logger("rclcpp"), - "Error in destruction of rcl guard condition: %s", exception.what()); + "failed to finalize guard condition: %s", exception.what()); } } } @@ -65,9 +68,14 @@ GuardCondition::get_rcl_guard_condition() const void GuardCondition::trigger() { - rcl_ret_t ret = rcl_trigger_guard_condition(&rcl_guard_condition_); - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); + if (on_trigger_callback_) { + on_trigger_callback_(1); + } else { + rcl_ret_t ret = rcl_trigger_guard_condition(&rcl_guard_condition_); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + unread_count_++; } } @@ -77,4 +85,29 @@ GuardCondition::exchange_in_use_by_wait_set_state(bool in_use_state) return in_use_by_wait_set_.exchange(in_use_state); } +void +GuardCondition::add_to_wait_set(rcl_wait_set_t * wait_set) const +{ + rcl_ret_t ret = rcl_wait_set_add_guard_condition(wait_set, &this->rcl_guard_condition_, NULL); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "failed to add guard condition to wait set"); + } +} + +void +GuardCondition::set_on_trigger_callback(std::function callback) +{ + if (callback) { + on_trigger_callback_ = callback; + + if (unread_count_) { + callback(unread_count_); + unread_count_ = 0; + } + return; + } + + on_trigger_callback_ = nullptr; +} } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/network_flow_endpoint.cpp b/rclcpp/src/rclcpp/network_flow_endpoint.cpp new file mode 100644 index 0000000000..2b85b17123 --- /dev/null +++ b/rclcpp/src/rclcpp/network_flow_endpoint.cpp @@ -0,0 +1,84 @@ +// Copyright 2020 Ericsson AB +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "rclcpp/network_flow_endpoint.hpp" + +namespace rclcpp +{ + +const std::string & +NetworkFlowEndpoint::transport_protocol() const +{ + return transport_protocol_; +} + +const std::string & +NetworkFlowEndpoint::internet_protocol() const +{ + return internet_protocol_; +} + +uint16_t NetworkFlowEndpoint::transport_port() const +{ + return transport_port_; +} + +uint32_t NetworkFlowEndpoint::flow_label() const +{ + return flow_label_; +} + +uint8_t NetworkFlowEndpoint::dscp() const +{ + return dscp_; +} + +const std::string & +NetworkFlowEndpoint::internet_address() const +{ + return internet_address_; +} + +bool operator==(const NetworkFlowEndpoint & left, const NetworkFlowEndpoint & right) +{ + return left.transport_protocol_ == right.transport_protocol_ && + left.internet_protocol_ == right.internet_protocol_ && + left.transport_port_ == right.transport_port_ && + left.flow_label_ == right.flow_label_ && + left.dscp_ == right.dscp_ && + left.internet_address_ == right.internet_address_; +} + +bool operator!=(const NetworkFlowEndpoint & left, const NetworkFlowEndpoint & right) +{ + return !(left == right); +} + +std::ostream & operator<<(std::ostream & os, const NetworkFlowEndpoint & network_flow_endpoint) +{ + // Stream out in JSON-like format + os << "{" << + "\"transportProtocol\": \"" << network_flow_endpoint.transport_protocol_ << "\", " << + "\"internetProtocol\": \"" << network_flow_endpoint.internet_protocol_ << "\", " << + "\"transportPort\": \"" << network_flow_endpoint.transport_port_ << "\", " << + "\"flowLabel\": \"" << std::to_string(network_flow_endpoint.flow_label_) << "\", " << + "\"dscp\": \"" << std::to_string(network_flow_endpoint.dscp_) << "\", " << + "\"internetAddress\": \"" << network_flow_endpoint.internet_address_ << "\"" << + "}"; + return os; +} + +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index abe7b8fc4b..117702760e 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #include #include #include @@ -20,6 +21,9 @@ #include #include +#include "rcl/arguments.h" + +#include "rclcpp/detail/qos_parameters.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp/graph_listener.hpp" #include "rclcpp/node.hpp" @@ -33,9 +37,12 @@ #include "rclcpp/node_interfaces/node_timers.hpp" #include "rclcpp/node_interfaces/node_topics.hpp" #include "rclcpp/node_interfaces/node_waitables.hpp" +#include "rclcpp/qos_overriding_options.hpp" #include "rmw/validate_namespace.h" +#include "./detail/resolve_parameter_overrides.hpp" + using rclcpp::Node; using rclcpp::NodeOptions; using rclcpp::exceptions::throw_from_rcl_error; @@ -94,6 +101,45 @@ Node::Node( { } +static +rclcpp::QoS +get_parameter_events_qos( + rclcpp::node_interfaces::NodeBaseInterface & node_base, + const rclcpp::NodeOptions & options) +{ + auto final_qos = options.parameter_event_qos(); + const rcl_arguments_t * global_args = nullptr; + auto * rcl_options = options.get_rcl_node_options(); + if (rcl_options->use_global_arguments) { + auto context_ptr = node_base.get_context()->get_rcl_context(); + global_args = &(context_ptr->global_arguments); + } + + auto parameter_overrides = rclcpp::detail::resolve_parameter_overrides( + node_base.get_fully_qualified_name(), + options.parameter_overrides(), + &rcl_options->arguments, + global_args); + + auto final_topic_name = node_base.resolve_topic_or_service_name("/parameter_events", false); + auto prefix = "qos_overrides." + final_topic_name + "."; + std::array policies = { + rclcpp::QosPolicyKind::Depth, + rclcpp::QosPolicyKind::Durability, + rclcpp::QosPolicyKind::History, + rclcpp::QosPolicyKind::Reliability, + }; + for (const auto & policy : policies) { + auto param_name = prefix + rclcpp::qos_policy_kind_to_cstr(policy); + auto it = parameter_overrides.find(param_name); + auto value = it != parameter_overrides.end() ? + it->second : + rclcpp::detail::get_default_qos_param_value(policy, options.parameter_event_qos()); + rclcpp::detail::apply_qos_override(policy, value, final_qos); + } + return final_qos; +} + Node::Node( const std::string & node_name, const std::string & namespace_, @@ -126,7 +172,9 @@ Node::Node( options.parameter_overrides(), options.start_parameter_services(), options.start_parameter_event_publisher(), - options.parameter_event_qos(), + // This is needed in order to apply parameter overrides to the qos profile provided in + // options. + get_parameter_events_qos(*node_base_, options), options.parameter_event_publisher_options(), options.allow_undeclared_parameters(), options.automatically_declare_parameters_from_overrides() @@ -139,13 +187,28 @@ Node::Node( node_logging_, node_clock_, node_parameters_, - options.clock_qos() + options.clock_qos(), + options.use_clock_thread() )), node_waitables_(new rclcpp::node_interfaces::NodeWaitables(node_base_.get())), node_options_(options), sub_namespace_(""), effective_namespace_(create_effective_namespace(this->get_namespace(), sub_namespace_)) { + // we have got what we wanted directly from the overrides, + // but declare the parameters anyway so they are visible. + rclcpp::detail::declare_qos_parameters( + rclcpp::QosOverridingOptions + { + QosPolicyKind::Depth, + QosPolicyKind::Durability, + QosPolicyKind::History, + QosPolicyKind::Reliability, + }, + node_parameters_, + node_topics_->resolve_topic_name("/parameter_events"), + options.parameter_event_qos(), + rclcpp::detail::PublisherQosParametersTraits{}); } Node::Node( diff --git a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp index 8fa46abe60..8563a312e8 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp @@ -44,30 +44,15 @@ NodeBase::NodeBase( node_handle_(nullptr), default_callback_group_(nullptr), associated_with_executor_(false), + notify_guard_condition_(context), notify_guard_condition_is_valid_(false) { - // Setup the guard condition that is notified when changes occur in the graph. - rcl_guard_condition_options_t guard_condition_options = rcl_guard_condition_get_default_options(); - rcl_ret_t ret = rcl_guard_condition_init( - ¬ify_guard_condition_, context_->get_rcl_context().get(), guard_condition_options); - if (ret != RCL_RET_OK) { - throw_from_rcl_error(ret, "failed to create interrupt guard condition"); - } - - // Setup a safe exit lamda to clean up the guard condition in case of an error here. - auto finalize_notify_guard_condition = [this]() { - // Finalize the interrupt guard condition. - if (rcl_guard_condition_fini(¬ify_guard_condition_) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "failed to destroy guard condition: %s", rcl_get_error_string().str); - } - }; - // Create the rcl node and store it in a shared_ptr with a custom destructor. std::unique_ptr rcl_node(new rcl_node_t(rcl_get_zero_initialized_node())); std::shared_ptr logging_mutex = get_global_logging_mutex(); + + rcl_ret_t ret; { std::lock_guard guard(*logging_mutex); // TODO(ivanpauno): /rosout Qos should be reconfigurable. @@ -80,9 +65,6 @@ NodeBase::NodeBase( context_->get_rcl_context().get(), &rcl_node_options); } if (ret != RCL_RET_OK) { - // Finalize the interrupt guard condition. - finalize_notify_guard_condition(); - if (ret == RCL_RET_NODE_INVALID_NAME) { rcl_reset_error(); // discard rcl_node_init error int validation_result; @@ -160,11 +142,6 @@ NodeBase::~NodeBase() { std::lock_guard notify_condition_lock(notify_guard_condition_mutex_); notify_guard_condition_is_valid_ = false; - if (rcl_guard_condition_fini(¬ify_guard_condition_) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "failed to destroy guard condition: %s", rcl_get_error_string().str); - } } } @@ -262,8 +239,18 @@ NodeBase::get_associated_with_executor_atomic() return associated_with_executor_; } -rcl_guard_condition_t * +const rcl_guard_condition_t * NodeBase::get_notify_guard_condition() +{ + std::lock_guard notify_condition_lock(notify_guard_condition_mutex_); + if (!notify_guard_condition_is_valid_) { + return nullptr; + } + return ¬ify_guard_condition_.get_rcl_guard_condition(); +} + +rclcpp::GuardCondition * +NodeBase::get_notify_rclcpp_guard_condition() { std::lock_guard notify_condition_lock(notify_guard_condition_mutex_); if (!notify_guard_condition_is_valid_) { @@ -310,3 +297,10 @@ NodeBase::resolve_topic_or_service_name( allocator.deallocate(output_cstr, allocator.state); return output; } + +void +NodeBase::trigger_notify_guard_condition() +{ + std::unique_lock lock(notify_guard_condition_mutex_); + notify_guard_condition_.trigger(); +} diff --git a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp index 415b6277ec..fe479beff5 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_graph.cpp @@ -348,13 +348,7 @@ NodeGraph::notify_graph_change() } } graph_cv_.notify_all(); - { - auto notify_condition_lock = node_base_->acquire_notify_guard_condition_lock(); - rcl_ret_t ret = rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()); - if (RCL_RET_OK != ret) { - throw_from_rcl_error(ret, "failed to trigger notify guard condition"); - } - } + node_base_->trigger_notify_guard_condition(); } void diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index 36abc8e163..3d91943bc5 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -16,6 +16,7 @@ #include +#include #include #include #include @@ -35,6 +36,8 @@ #include "rcutils/logging_macros.h" #include "rmw/qos_profiles.h" +#include "../detail/resolve_parameter_overrides.hpp" + using rclcpp::node_interfaces::NodeParameters; NodeParameters::NodeParameters( @@ -86,50 +89,15 @@ NodeParameters::NodeParameters( throw std::runtime_error("Need valid node options in NodeParameters"); } - std::vector argument_sources; - // global before local so that local overwrites global + const rcl_arguments_t * global_args = nullptr; if (options->use_global_arguments) { auto context_ptr = node_base->get_context()->get_rcl_context(); - argument_sources.push_back(&(context_ptr->global_arguments)); + global_args = &(context_ptr->global_arguments); } - argument_sources.push_back(&options->arguments); - - // Get fully qualified node name post-remapping to use to find node's params in yaml files combined_name_ = node_base->get_fully_qualified_name(); - for (const rcl_arguments_t * source : argument_sources) { - rcl_params_t * params = NULL; - rcl_ret_t ret = rcl_arguments_get_param_overrides(source, ¶ms); - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); - } - if (params) { - auto cleanup_params = make_scope_exit( - [params]() { - rcl_yaml_node_struct_fini(params); - }); - rclcpp::ParameterMap initial_map = rclcpp::parameter_map_from(params); - - // Enforce wildcard matching precedence - // TODO(cottsay) implement further wildcard matching - const std::vector node_matching_names{"/**", combined_name_}; - for (const auto & node_name : node_matching_names) { - if (initial_map.count(node_name) > 0) { - // Combine parameter yaml files, overwriting values in older ones - for (const rclcpp::Parameter & param : initial_map.at(node_name)) { - parameter_overrides_[param.get_name()] = - rclcpp::ParameterValue(param.get_value_message()); - } - } - } - } - } - - // parameter overrides passed to constructor will overwrite overrides from yaml file sources - for (auto & param : parameter_overrides) { - parameter_overrides_[param.get_name()] = - rclcpp::ParameterValue(param.get_value_message()); - } + parameter_overrides_ = rclcpp::detail::resolve_parameter_overrides( + combined_name_, parameter_overrides, &options->arguments, global_args); // If asked, initialize any parameters that ended up in the initial parameter values, // but did not get declared explcitily by this point. @@ -331,14 +299,15 @@ __set_parameters_atomically_common( const OnParametersSetCallbackType & callback, bool allow_undeclared = false) { - // Call the user callback to see if the new value(s) are allowed. - rcl_interfaces::msg::SetParametersResult result = - __call_on_parameters_set_callbacks(parameters, callback_container, callback); + // Check if the value being set complies with the descriptor. + rcl_interfaces::msg::SetParametersResult result = __check_parameters( + parameter_infos, parameters, allow_undeclared); if (!result.successful) { return result; } - // Check if the value being set complies with the descriptor. - result = __check_parameters(parameter_infos, parameters, allow_undeclared); + // Call the user callback to see if the new value(s) are allowed. + result = + __call_on_parameters_set_callbacks(parameters, callback_container, callback); if (!result.successful) { return result; } @@ -380,6 +349,21 @@ __declare_parameter_common( initial_value = &overrides_it->second; } + // If there is no initial value, then skip initialization + if (initial_value->get_type() == rclcpp::PARAMETER_NOT_SET) { + // Add declared parameters to storage (without a value) + parameter_infos[name].descriptor.name = name; + if (parameter_descriptor.dynamic_typing) { + parameter_infos[name].descriptor.type = rclcpp::PARAMETER_NOT_SET; + } else { + parameter_infos[name].descriptor.type = parameter_descriptor.type; + } + parameters_out[name] = parameter_infos.at(name); + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + return result; + } + // Check with the user's callback to see if the initial value can be set. std::vector parameter_wrappers {rclcpp::Parameter(name, *initial_value)}; // This function also takes care of default vs initial value. @@ -444,14 +428,6 @@ declare_parameter_helper( parameter_descriptor.type = static_cast(type); } - if ( - rclcpp::PARAMETER_NOT_SET == default_value.get_type() && - overrides.find(name) == overrides.end() && - parameter_descriptor.dynamic_typing == false) - { - throw rclcpp::exceptions::NoParameterOverrideProvided(name); - } - rcl_interfaces::msg::ParameterEvent parameter_event; auto result = __declare_parameter_common( name, @@ -837,14 +813,21 @@ NodeParameters::get_parameters(const std::vector & names) const rclcpp::Parameter NodeParameters::get_parameter(const std::string & name) const { - rclcpp::Parameter parameter; + std::lock_guard lock(mutex_); - if (get_parameter(name, parameter)) { - return parameter; + auto param_iter = parameters_.find(name); + if ( + parameters_.end() != param_iter && + (param_iter->second.value.get_type() != rclcpp::ParameterType::PARAMETER_NOT_SET || + param_iter->second.descriptor.dynamic_typing)) + { + return rclcpp::Parameter{name, param_iter->second.value}; } else if (this->allow_undeclared_) { - return parameter; - } else { + return rclcpp::Parameter{}; + } else if (parameters_.end() == param_iter) { throw rclcpp::exceptions::ParameterNotDeclaredException(name); + } else { + throw rclcpp::exceptions::ParameterUninitializedException(name); } } @@ -986,22 +969,6 @@ NodeParameters::list_parameters(const std::vector & prefixes, uint6 return result; } -struct HandleCompare - : public std::unary_function -{ - explicit HandleCompare(const OnSetParametersCallbackHandle * const base) - : base_(base) {} - bool operator()(const OnSetParametersCallbackHandle::WeakPtr & handle) - { - auto shared_handle = handle.lock(); - if (base_ == shared_handle.get()) { - return true; - } - return false; - } - const OnSetParametersCallbackHandle * const base_; -}; - void NodeParameters::remove_on_set_parameters_callback( const OnSetParametersCallbackHandle * const handle) @@ -1012,7 +979,9 @@ NodeParameters::remove_on_set_parameters_callback( auto it = std::find_if( on_parameters_set_callback_container_.begin(), on_parameters_set_callback_container_.end(), - HandleCompare(handle)); + [handle](const auto & weak_handle) { + return handle == weak_handle.lock().get(); + }); if (it != on_parameters_set_callback_container_.end()) { on_parameters_set_callback_container_.erase(it); } else { diff --git a/rclcpp/src/rclcpp/node_interfaces/node_services.cpp b/rclcpp/src/rclcpp/node_interfaces/node_services.cpp index dee39cb403..c0c3935fcb 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_services.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_services.cpp @@ -41,15 +41,7 @@ NodeServices::add_service( } // Notify the executor that a new service was created using the parent Node. - { - auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock(); - if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) { - throw std::runtime_error( - std::string("Failed to notify wait set on service creation: ") + - rmw_get_error_string().str - ); - } - } + node_base_->trigger_notify_guard_condition(); } void @@ -68,15 +60,7 @@ NodeServices::add_client( } // Notify the executor that a new client was created using the parent Node. - { - auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock(); - if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) { - throw std::runtime_error( - std::string("Failed to notify wait set on client creation: ") + - rmw_get_error_string().str - ); - } - } + node_base_->trigger_notify_guard_condition(); } std::string diff --git a/rclcpp/src/rclcpp/node_interfaces/node_time_source.cpp b/rclcpp/src/rclcpp/node_interfaces/node_time_source.cpp index 415b986051..2bd3a098b6 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_time_source.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_time_source.cpp @@ -27,7 +27,8 @@ NodeTimeSource::NodeTimeSource( rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock, rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters, - const rclcpp::QoS & qos) + const rclcpp::QoS & qos, + bool use_clock_thread) : node_base_(node_base), node_topics_(node_topics), node_graph_(node_graph), @@ -35,7 +36,7 @@ NodeTimeSource::NodeTimeSource( node_logging_(node_logging), node_clock_(node_clock), node_parameters_(node_parameters), - time_source_(qos) + time_source_(qos, use_clock_thread) { time_source_.attachNode( node_base_, diff --git a/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp b/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp index 6936e26759..f54e64a216 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp @@ -41,11 +41,9 @@ NodeTimers::add_timer( } else { node_base_->get_default_callback_group()->add_timer(timer); } - if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) { - throw std::runtime_error( - std::string("Failed to notify wait set on timer creation: ") + - rmw_get_error_string().str); - } + + node_base_->trigger_notify_guard_condition(); + TRACEPOINT( rclcpp_timer_link_node, static_cast(timer->get_timer_handle().get()), diff --git a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp index c57fbcee5d..2180a58406 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp @@ -55,19 +55,13 @@ NodeTopics::add_publisher( callback_group = node_base_->get_default_callback_group(); } - for (auto & publisher_event : publisher->get_event_handlers()) { + for (auto & key_event_pair : publisher->get_event_handlers()) { + auto publisher_event = key_event_pair.second; callback_group->add_waitable(publisher_event); } // Notify the executor that a new publisher was created using the parent Node. - { - auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock(); - if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) { - throw std::runtime_error( - std::string("Failed to notify wait set on publisher creation: ") + - rmw_get_error_string().str); - } - } + node_base_->trigger_notify_guard_condition(); } rclcpp::SubscriptionBase::SharedPtr @@ -97,7 +91,8 @@ NodeTopics::add_subscription( callback_group->add_subscription(subscription); - for (auto & subscription_event : subscription->get_event_handlers()) { + for (auto & key_event_pair : subscription->get_event_handlers()) { + auto subscription_event = key_event_pair.second; callback_group->add_waitable(subscription_event); } @@ -108,14 +103,7 @@ NodeTopics::add_subscription( } // Notify the executor that a new subscription was created using the parent Node. - { - auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock(); - auto ret = rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()); - if (ret != RCL_RET_OK) { - using rclcpp::exceptions::throw_from_rcl_error; - throw_from_rcl_error(ret, "failed to notify wait set on subscription creation"); - } - } + node_base_->trigger_notify_guard_condition(); } rclcpp::node_interfaces::NodeBaseInterface * diff --git a/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp b/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp index 0fd083788a..f6af1829b1 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp @@ -41,15 +41,7 @@ NodeWaitables::add_waitable( } // Notify the executor that a new waitable was created using the parent Node. - { - auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock(); - if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) { - throw std::runtime_error( - std::string("Failed to notify wait set on waitable creation: ") + - rmw_get_error_string().str - ); - } - } + node_base_->trigger_notify_guard_condition(); } void diff --git a/rclcpp/src/rclcpp/node_options.cpp b/rclcpp/src/rclcpp/node_options.cpp index c76bafd9b8..06feffd1e6 100644 --- a/rclcpp/src/rclcpp/node_options.cpp +++ b/rclcpp/src/rclcpp/node_options.cpp @@ -78,6 +78,7 @@ NodeOptions::operator=(const NodeOptions & other) this->start_parameter_services_ = other.start_parameter_services_; this->start_parameter_event_publisher_ = other.start_parameter_event_publisher_; this->clock_qos_ = other.clock_qos_; + this->use_clock_thread_ = other.use_clock_thread_; this->parameter_event_qos_ = other.parameter_event_qos_; this->rosout_qos_ = other.rosout_qos_; this->parameter_event_publisher_options_ = other.parameter_event_publisher_options_; @@ -272,6 +273,19 @@ NodeOptions::clock_qos(const rclcpp::QoS & clock_qos) return *this; } +bool +NodeOptions::use_clock_thread() const +{ + return this->use_clock_thread_; +} + +NodeOptions & +NodeOptions::use_clock_thread(bool use_clock_thread) +{ + this->use_clock_thread_ = use_clock_thread; + return *this; +} + const rclcpp::QoS & NodeOptions::parameter_event_qos() const { diff --git a/rclcpp/src/rclcpp/parameter_client.cpp b/rclcpp/src/rclcpp/parameter_client.cpp index 4ab824d3a6..38ced0e1a5 100644 --- a/rclcpp/src/rclcpp/parameter_client.cpp +++ b/rclcpp/src/rclcpp/parameter_client.cpp @@ -42,7 +42,7 @@ AsyncParametersClient::AsyncParametersClient( if (remote_node_name != "") { remote_node_name_ = remote_node_name; } else { - remote_node_name_ = node_base_interface->get_name(); + remote_node_name_ = node_base_interface->get_fully_qualified_name(); } rcl_client_options_t options = rcl_client_get_default_options(); @@ -273,6 +273,55 @@ AsyncParametersClient::set_parameters_atomically( return future_result; } +std::shared_future> +AsyncParametersClient::delete_parameters( + const std::vector & parameters_names) +{ + std::vector parameters; + for (const std::string & name : parameters_names) { + parameters.push_back(rclcpp::Parameter(name)); + } + auto future_result = set_parameters(parameters); + + return future_result; +} + +std::shared_future> +AsyncParametersClient::load_parameters( + const std::string & yaml_filename) +{ + rclcpp::ParameterMap parameter_map = rclcpp::parameter_map_from_yaml_file(yaml_filename); + return this->load_parameters(parameter_map); +} + +std::shared_future> +AsyncParametersClient::load_parameters( + const rclcpp::ParameterMap & parameter_map) +{ + std::vector parameters; + std::string remote_name = remote_node_name_.substr(remote_node_name_.substr(1).find("/") + 2); + for (const auto & params : parameter_map) { + std::string node_full_name = params.first; + std::string node_name = node_full_name.substr(node_full_name.find("/*/") + 3); + if (node_full_name == remote_node_name_ || + node_full_name == "/**" || + (node_name == remote_name)) + { + for (const auto & param : params.second) { + parameters.push_back(param); + } + } + } + + if (parameters.size() == 0) { + throw rclcpp::exceptions::InvalidParametersException("No valid parameter"); + } + auto future_result = set_parameters(parameters); + + return future_result; +} + + std::shared_future AsyncParametersClient::list_parameters( const std::vector & prefixes, @@ -420,6 +469,42 @@ SyncParametersClient::set_parameters( return std::vector(); } +std::vector +SyncParametersClient::delete_parameters( + const std::vector & parameters_names, + std::chrono::nanoseconds timeout) +{ + auto f = async_parameters_client_->delete_parameters(parameters_names); + + using rclcpp::executors::spin_node_until_future_complete; + if ( + spin_node_until_future_complete( + *executor_, node_base_interface_, f, + timeout) == rclcpp::FutureReturnCode::SUCCESS) + { + return f.get(); + } + return std::vector(); +} + +std::vector +SyncParametersClient::load_parameters( + const std::string & yaml_filename, + std::chrono::nanoseconds timeout) +{ + auto f = async_parameters_client_->load_parameters(yaml_filename); + + using rclcpp::executors::spin_node_until_future_complete; + if ( + spin_node_until_future_complete( + *executor_, node_base_interface_, f, + timeout) == rclcpp::FutureReturnCode::SUCCESS) + { + return f.get(); + } + return std::vector(); +} + rcl_interfaces::msg::SetParametersResult SyncParametersClient::set_parameters_atomically( const std::vector & parameters, diff --git a/rclcpp/src/rclcpp/parameter_event_handler.cpp b/rclcpp/src/rclcpp/parameter_event_handler.cpp index 9fd5994864..232a32f887 100644 --- a/rclcpp/src/rclcpp/parameter_event_handler.cpp +++ b/rclcpp/src/rclcpp/parameter_event_handler.cpp @@ -37,23 +37,6 @@ ParameterEventHandler::add_parameter_event_callback( return handle; } -template -struct HandleCompare - : public std::unary_function -{ - explicit HandleCompare(const CallbackHandleT * const base) - : base_(base) {} - bool operator()(const typename CallbackHandleT::WeakPtr & handle) - { - auto shared_handle = handle.lock(); - if (base_ == shared_handle.get()) { - return true; - } - return false; - } - const CallbackHandleT * const base_; -}; - void ParameterEventHandler::remove_parameter_event_callback( ParameterEventCallbackHandle::SharedPtr callback_handle) @@ -62,7 +45,9 @@ ParameterEventHandler::remove_parameter_event_callback( auto it = std::find_if( event_callbacks_.begin(), event_callbacks_.end(), - HandleCompare(callback_handle.get())); + [callback_handle](const auto & weak_handle) { + return callback_handle.get() == weak_handle.lock().get(); + }); if (it != event_callbacks_.end()) { event_callbacks_.erase(it); } else { @@ -99,7 +84,9 @@ ParameterEventHandler::remove_parameter_callback( auto it = std::find_if( container.begin(), container.end(), - HandleCompare(handle)); + [handle](const auto & weak_handle) { + return handle == weak_handle.lock().get(); + }); if (it != container.end()) { container.erase(it); if (container.empty()) { @@ -171,14 +158,13 @@ ParameterEventHandler::get_parameters_from_event( } void -ParameterEventHandler::event_callback( - const rcl_interfaces::msg::ParameterEvent::SharedPtr event) +ParameterEventHandler::event_callback(const rcl_interfaces::msg::ParameterEvent & event) { std::lock_guard lock(mutex_); for (auto it = parameter_callbacks_.begin(); it != parameter_callbacks_.end(); ++it) { rclcpp::Parameter p; - if (get_parameter_from_event(*event, p, it->first.first, it->first.second)) { + if (get_parameter_from_event(event, p, it->first.first, it->first.second)) { for (auto cb = it->second.begin(); cb != it->second.end(); ++cb) { auto shared_handle = cb->lock(); if (nullptr != shared_handle) { diff --git a/rclcpp/src/rclcpp/parameter_events_filter.cpp b/rclcpp/src/rclcpp/parameter_events_filter.cpp index 36ef708c35..be9882c85b 100644 --- a/rclcpp/src/rclcpp/parameter_events_filter.cpp +++ b/rclcpp/src/rclcpp/parameter_events_filter.cpp @@ -14,6 +14,7 @@ #include "rclcpp/parameter_events_filter.hpp" +#include #include #include @@ -22,7 +23,7 @@ using EventType = rclcpp::ParameterEventsFilter::EventType; using EventPair = rclcpp::ParameterEventsFilter::EventPair; ParameterEventsFilter::ParameterEventsFilter( - rcl_interfaces::msg::ParameterEvent::SharedPtr event, + std::shared_ptr event, const std::vector & names, const std::vector & types) : event_(event) diff --git a/rclcpp/src/rclcpp/parameter_map.cpp b/rclcpp/src/rclcpp/parameter_map.cpp index 447e50d4b1..e5e3da019c 100644 --- a/rclcpp/src/rclcpp/parameter_map.cpp +++ b/rclcpp/src/rclcpp/parameter_map.cpp @@ -126,3 +126,15 @@ rclcpp::parameter_value_from(const rcl_variant_t * const c_param_value) throw InvalidParameterValueException("No parameter value set"); } + +ParameterMap +rclcpp::parameter_map_from_yaml_file(const std::string & yaml_filename) +{ + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + rcl_params_t * rcl_parameters = rcl_yaml_node_struct_init(allocator); + const char * path = yaml_filename.c_str(); + if (!rcl_parse_yaml_file(path, rcl_parameters)) { + rclcpp::exceptions::throw_from_rcl_error(RCL_RET_ERROR); + } + return rclcpp::parameter_map_from(rcl_parameters); +} diff --git a/rclcpp/src/rclcpp/parameter_service.cpp b/rclcpp/src/rclcpp/parameter_service.cpp index 0cbb9667e4..5c30917499 100644 --- a/rclcpp/src/rclcpp/parameter_service.cpp +++ b/rclcpp/src/rclcpp/parameter_service.cpp @@ -41,12 +41,13 @@ ParameterService::ParameterService( const std::shared_ptr request, std::shared_ptr response) { - for (const auto & name : request->names) { - // Default construct param to NOT_SET - rclcpp::Parameter param; - node_params->get_parameter(name, param); - // push back NOT_SET when get_parameter() call fails - response->values.push_back(param.get_value_message()); + try { + auto parameters = node_params->get_parameters(request->names); + for (const auto & param : parameters) { + response->values.push_back(param.get_value_message()); + } + } catch (const rclcpp::exceptions::ParameterNotDeclaredException & ex) { + RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Failed to get parameters: %s", ex.what()); } }, qos_profile, nullptr); @@ -67,7 +68,7 @@ ParameterService::ParameterService( return static_cast(type); }); } catch (const rclcpp::exceptions::ParameterNotDeclaredException & ex) { - RCLCPP_WARN(rclcpp::get_logger("rclcpp"), "Failed to get parameter types: %s", ex.what()); + RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Failed to get parameter types: %s", ex.what()); } }, qos_profile, nullptr); @@ -88,7 +89,7 @@ ParameterService::ParameterService( result = node_params->set_parameters_atomically( {rclcpp::Parameter::from_parameter_msg(p)}); } catch (const rclcpp::exceptions::ParameterNotDeclaredException & ex) { - RCLCPP_WARN(rclcpp::get_logger("rclcpp"), "Failed to set parameter: %s", ex.what()); + RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Failed to set parameter: %s", ex.what()); result.successful = false; result.reason = ex.what(); } @@ -116,7 +117,7 @@ ParameterService::ParameterService( auto result = node_params->set_parameters_atomically(pvariants); response->result = result; } catch (const rclcpp::exceptions::ParameterNotDeclaredException & ex) { - RCLCPP_WARN( + RCLCPP_DEBUG( rclcpp::get_logger("rclcpp"), "Failed to set parameters atomically: %s", ex.what()); response->result.successful = false; response->result.reason = "One or more parameters were not declared before setting"; @@ -136,7 +137,7 @@ ParameterService::ParameterService( auto descriptors = node_params->describe_parameters(request->names); response->descriptors = descriptors; } catch (const rclcpp::exceptions::ParameterNotDeclaredException & ex) { - RCLCPP_WARN(rclcpp::get_logger("rclcpp"), "Failed to describe parameters: %s", ex.what()); + RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Failed to describe parameters: %s", ex.what()); } }, qos_profile, nullptr); diff --git a/rclcpp/src/rclcpp/publisher_base.cpp b/rclcpp/src/rclcpp/publisher_base.cpp index aadee307b3..d820234842 100644 --- a/rclcpp/src/rclcpp/publisher_base.cpp +++ b/rclcpp/src/rclcpp/publisher_base.cpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include "rcutils/logging_macros.h" @@ -34,6 +35,7 @@ #include "rclcpp/experimental/intra_process_manager.hpp" #include "rclcpp/logging.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/network_flow_endpoint.hpp" #include "rclcpp/node.hpp" #include "rclcpp/qos_event.hpp" @@ -153,7 +155,8 @@ PublisherBase::get_publisher_handle() const return publisher_handle_; } -const std::vector> & +const +std::unordered_map> & PublisherBase::get_event_handlers() const { return event_handlers_; @@ -268,3 +271,39 @@ PublisherBase::default_incompatible_qos_callback( get_topic_name(), policy_name.c_str()); } + +std::vector PublisherBase::get_network_flow_endpoints() const +{ + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + rcl_network_flow_endpoint_array_t network_flow_endpoint_array = + rcl_get_zero_initialized_network_flow_endpoint_array(); + rcl_ret_t ret = rcl_publisher_get_network_flow_endpoints( + publisher_handle_.get(), &allocator, &network_flow_endpoint_array); + if (RCL_RET_OK != ret) { + auto error_msg = std::string("error obtaining network flows of publisher: ") + + rcl_get_error_string().str; + rcl_reset_error(); + if (RCL_RET_OK != + rcl_network_flow_endpoint_array_fini(&network_flow_endpoint_array)) + { + error_msg += std::string(", also error cleaning up network flow array: ") + + rcl_get_error_string().str; + rcl_reset_error(); + } + rclcpp::exceptions::throw_from_rcl_error(ret, error_msg); + } + + std::vector network_flow_endpoint_vector; + for (size_t i = 0; i < network_flow_endpoint_array.size; ++i) { + network_flow_endpoint_vector.push_back( + rclcpp::NetworkFlowEndpoint( + network_flow_endpoint_array.network_flow_endpoint[i])); + } + + ret = rcl_network_flow_endpoint_array_fini(&network_flow_endpoint_array); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "error cleaning up network flow array"); + } + + return network_flow_endpoint_vector; +} diff --git a/rclcpp/src/rclcpp/qos.cpp b/rclcpp/src/rclcpp/qos.cpp index d0f47cd01e..8b912de07f 100644 --- a/rclcpp/src/rclcpp/qos.cpp +++ b/rclcpp/src/rclcpp/qos.cpp @@ -16,7 +16,9 @@ #include +#include "rmw/error_handling.h" #include "rmw/types.h" +#include "rmw/qos_profiles.h" namespace rclcpp { @@ -309,6 +311,45 @@ bool operator!=(const QoS & left, const QoS & right) return !(left == right); } +QoSCheckCompatibleResult +qos_check_compatible(const QoS & publisher_qos, const QoS & subscription_qos) +{ + rmw_qos_compatibility_type_t compatible; + const size_t reason_size = 2048u; + char reason_c_str[reason_size] = ""; + rmw_ret_t ret = rmw_qos_profile_check_compatible( + publisher_qos.get_rmw_qos_profile(), + subscription_qos.get_rmw_qos_profile(), + &compatible, + reason_c_str, + reason_size); + if (RMW_RET_OK != ret) { + std::string error_str(rmw_get_error_string().str); + rmw_reset_error(); + throw rclcpp::exceptions::QoSCheckCompatibleException{error_str}; + } + + QoSCheckCompatibleResult result; + result.reason = std::string(reason_c_str); + + switch (compatible) { + case RMW_QOS_COMPATIBILITY_OK: + result.compatibility = QoSCompatibility::Ok; + break; + case RMW_QOS_COMPATIBILITY_WARNING: + result.compatibility = QoSCompatibility::Warning; + break; + case RMW_QOS_COMPATIBILITY_ERROR: + result.compatibility = QoSCompatibility::Error; + break; + default: + throw rclcpp::exceptions::QoSCheckCompatibleException{ + "Unexpected compatibility value returned by rmw '" + std::to_string(compatible) + + "'"}; + } + return result; +} + ClockQoS::ClockQoS(const QoSInitialization & qos_initialization) // Using `rmw_qos_profile_sensor_data` intentionally. // It's best effort and `qos_initialization` is overriding the depth to 1. diff --git a/rclcpp/src/rclcpp/qos_event.cpp b/rclcpp/src/rclcpp/qos_event.cpp index 0cd1968154..3862cf68c5 100644 --- a/rclcpp/src/rclcpp/qos_event.cpp +++ b/rclcpp/src/rclcpp/qos_event.cpp @@ -69,17 +69,18 @@ QOSEventHandlerBase::is_ready(rcl_wait_set_t * wait_set) } void -QOSEventHandlerBase::set_listener_callback( - rmw_listener_callback_t callback, - const void * user_data) const +QOSEventHandlerBase::set_on_new_event_callback( + rcl_event_callback_t callback, + const void * user_data) { - rcl_ret_t ret = rcl_event_set_listener_callback( + rcl_ret_t ret = rcl_event_set_callback( &event_handle_, callback, user_data); if (RCL_RET_OK != ret) { - throw std::runtime_error("Couldn't set listener callback to QOSEventHandlerBase"); + using rclcpp::exceptions::throw_from_rcl_error; + throw_from_rcl_error(ret, "failed to set the on new message callback for subscription"); } } diff --git a/rclcpp/src/rclcpp/service.cpp b/rclcpp/src/rclcpp/service.cpp index e279668bdf..bfd9aba0c0 100644 --- a/rclcpp/src/rclcpp/service.cpp +++ b/rclcpp/src/rclcpp/service.cpp @@ -28,7 +28,8 @@ using rclcpp::ServiceBase; ServiceBase::ServiceBase(std::shared_ptr node_handle) -: node_handle_(node_handle) +: node_handle_(node_handle), + node_logger_(rclcpp::get_node_logger(node_handle_.get())) {} ServiceBase::~ServiceBase() @@ -86,16 +87,15 @@ ServiceBase::exchange_in_use_by_wait_set_state(bool in_use_state) } void -ServiceBase::set_listener_callback( - rmw_listener_callback_t callback, - const void * user_data) const +ServiceBase::set_on_new_request_callback(rcl_event_callback_t callback, const void * user_data) { - rcl_ret_t ret = rcl_service_set_listener_callback( + rcl_ret_t ret = rcl_service_set_on_new_request_callback( service_handle_.get(), callback, user_data); if (RCL_RET_OK != ret) { - throw std::runtime_error("Couldn't set listener callback to service"); + using rclcpp::exceptions::throw_from_rcl_error; + throw_from_rcl_error(ret, "failed to set the on new request callback for service"); } } diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index 065de5cbbd..010b74e86d 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include #include "rclcpp/exceptions.hpp" @@ -39,6 +40,7 @@ SubscriptionBase::SubscriptionBase( bool is_serialized) : node_base_(node_base), node_handle_(node_base_->get_shared_rcl_node_handle()), + node_logger_(rclcpp::get_node_logger(node_handle_.get())), use_intra_process_(false), intra_process_subscription_id_(0), type_support_(type_support_handle), @@ -115,7 +117,8 @@ SubscriptionBase::get_subscription_handle() const return subscription_handle_; } -const std::vector> & +const +std::unordered_map> & SubscriptionBase::get_event_handlers() const { return event_handlers_; @@ -281,7 +284,8 @@ SubscriptionBase::exchange_in_use_by_wait_set_state( if (get_intra_process_waitable().get() == pointer_to_subscription_part) { return intra_process_subscription_waitable_in_use_by_wait_set_.exchange(in_use_state); } - for (const auto & qos_event : event_handlers_) { + for (const auto & key_event_pair : event_handlers_) { + auto qos_event = key_event_pair.second; if (qos_event.get() == pointer_to_subscription_part) { return qos_events_in_use_by_wait_set_[qos_event.get()].exchange(in_use_state); } @@ -290,16 +294,54 @@ SubscriptionBase::exchange_in_use_by_wait_set_state( } void -SubscriptionBase::set_listener_callback( - rmw_listener_callback_t callback, - const void * user_data) const +SubscriptionBase::set_on_new_message_callback( + rcl_event_callback_t callback, + const void * user_data) { - rcl_ret_t ret = rcl_subscription_set_listener_callback( + rcl_ret_t ret = rcl_subscription_set_on_new_message_callback( subscription_handle_.get(), callback, user_data); if (RCL_RET_OK != ret) { - throw std::runtime_error("Couldn't set listener callback to subscription"); + using rclcpp::exceptions::throw_from_rcl_error; + throw_from_rcl_error(ret, "failed to set the on new message callback for subscription"); } } + +std::vector SubscriptionBase::get_network_flow_endpoints() const +{ + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + rcl_network_flow_endpoint_array_t network_flow_endpoint_array = + rcl_get_zero_initialized_network_flow_endpoint_array(); + rcl_ret_t ret = rcl_subscription_get_network_flow_endpoints( + subscription_handle_.get(), &allocator, &network_flow_endpoint_array); + if (RCL_RET_OK != ret) { + auto error_msg = std::string("Error obtaining network flows of subscription: ") + + rcl_get_error_string().str; + rcl_reset_error(); + if (RCL_RET_OK != + rcl_network_flow_endpoint_array_fini(&network_flow_endpoint_array)) + { + error_msg += std::string(". Also error cleaning up network flow array: ") + + rcl_get_error_string().str; + rcl_reset_error(); + } + rclcpp::exceptions::throw_from_rcl_error(ret, error_msg); + } + + std::vector network_flow_endpoint_vector; + for (size_t i = 0; i < network_flow_endpoint_array.size; ++i) { + network_flow_endpoint_vector.push_back( + rclcpp::NetworkFlowEndpoint( + network_flow_endpoint_array. + network_flow_endpoint[i])); + } + + ret = rcl_network_flow_endpoint_array_fini(&network_flow_endpoint_array); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "error cleaning up network flow array"); + } + + return network_flow_endpoint_vector; +} diff --git a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp index 8a2aa7d521..d1fd1c700e 100644 --- a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp +++ b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include + #include "rclcpp/experimental/subscription_intra_process_base.hpp" using rclcpp::experimental::SubscriptionIntraProcessBase; @@ -21,8 +23,9 @@ SubscriptionIntraProcessBase::add_to_wait_set(rcl_wait_set_t * wait_set) { std::lock_guard lock(reentrant_mutex_); - rcl_ret_t ret = rcl_wait_set_add_guard_condition(wait_set, &gc_, NULL); - return RCL_RET_OK == ret; + gc_.add_to_wait_set(wait_set); + + return true; } const char * @@ -36,18 +39,3 @@ SubscriptionIntraProcessBase::get_actual_qos() const { return qos_profile_; } - -void -SubscriptionIntraProcessBase::set_listener_callback( - rmw_listener_callback_t callback, - const void * user_data) const -{ - rcl_ret_t ret = rcl_guard_condition_set_listener_callback( - &gc_, - callback, - user_data); - - if (RCL_RET_OK != ret) { - throw std::runtime_error("Couldn't set guard condition listener callback"); - } -} diff --git a/rclcpp/src/rclcpp/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp index c8d8270859..140b5531d2 100644 --- a/rclcpp/src/rclcpp/time_source.cpp +++ b/rclcpp/src/rclcpp/time_source.cpp @@ -33,21 +33,29 @@ namespace rclcpp { -TimeSource::TimeSource(std::shared_ptr node, const rclcpp::QoS & qos) -: logger_(rclcpp::get_logger("rclcpp")), +TimeSource::TimeSource( + std::shared_ptr node, + const rclcpp::QoS & qos, + bool use_clock_thread) +: use_clock_thread_(use_clock_thread), + logger_(rclcpp::get_logger("rclcpp")), qos_(qos) { this->attachNode(node); } -TimeSource::TimeSource(const rclcpp::QoS & qos) -: logger_(rclcpp::get_logger("rclcpp")), +TimeSource::TimeSource( + const rclcpp::QoS & qos, + bool use_clock_thread) +: use_clock_thread_(use_clock_thread), + logger_(rclcpp::get_logger("rclcpp")), qos_(qos) { } void TimeSource::attachNode(rclcpp::Node::SharedPtr node) { + use_clock_thread_ = node->get_node_options().use_clock_thread(); attachNode( node->get_node_base_interface(), node->get_node_topics_interface(), @@ -127,7 +135,7 @@ void TimeSource::attachNode( void TimeSource::detachNode() { this->ros_time_active_ = false; - clock_subscription_.reset(); + destroy_clock_sub(); parameter_subscription_.reset(); node_base_.reset(); node_topics_.reset(); @@ -208,7 +216,7 @@ void TimeSource::set_clock( } } -void TimeSource::clock_cb(const rosgraph_msgs::msg::Clock::SharedPtr msg) +void TimeSource::clock_cb(std::shared_ptr msg) { if (!this->ros_time_active_ && SET_TRUE == this->parameter_state_) { enable_ros_time(); @@ -242,6 +250,28 @@ void TimeSource::create_clock_sub() rclcpp::QosPolicyKind::Reliability, }); + if (use_clock_thread_) { + clock_callback_group_ = node_base_->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + false + ); + options.callback_group = clock_callback_group_; + rclcpp::ExecutorOptions exec_options; + exec_options.context = node_base_->get_context(); + clock_executor_ = + std::make_shared(exec_options); + if (!clock_executor_thread_.joinable()) { + cancel_clock_executor_promise_ = std::promise{}; + clock_executor_thread_ = std::thread( + [this]() { + auto future = cancel_clock_executor_promise_.get_future(); + clock_executor_->add_callback_group(clock_callback_group_, node_base_); + clock_executor_->spin_until_future_complete(future); + } + ); + } + } + clock_subscription_ = rclcpp::create_subscription( node_parameters_, node_topics_, @@ -255,10 +285,17 @@ void TimeSource::create_clock_sub() void TimeSource::destroy_clock_sub() { std::lock_guard guard(clock_sub_lock_); + if (clock_executor_thread_.joinable()) { + cancel_clock_executor_promise_.set_value(); + clock_executor_->cancel(); + clock_executor_thread_.join(); + clock_executor_->remove_callback_group(clock_callback_group_); + } clock_subscription_.reset(); } -void TimeSource::on_parameter_event(const rcl_interfaces::msg::ParameterEvent::SharedPtr event) +void TimeSource::on_parameter_event( + std::shared_ptr event) { // Filter out events on 'use_sim_time' parameter instances in other nodes. if (event->node != node_base_->get_fully_qualified_name()) { diff --git a/rclcpp/src/rclcpp/typesupport_helpers.cpp b/rclcpp/src/rclcpp/typesupport_helpers.cpp new file mode 100644 index 0000000000..7286c35baa --- /dev/null +++ b/rclcpp/src/rclcpp/typesupport_helpers.cpp @@ -0,0 +1,136 @@ +// Copyright 2018, Bosch Software Innovations GmbH. +// Copyright 2021, Apex.AI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp/typesupport_helpers.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include "ament_index_cpp/get_package_prefix.hpp" +#include "ament_index_cpp/get_resources.hpp" +#include "rcpputils/shared_library.hpp" +#include "rcpputils/find_library.hpp" +#include "rosidl_runtime_cpp/message_type_support_decl.hpp" + +namespace rclcpp +{ + +namespace +{ + +// Look for the library in the ament prefix paths. +std::string get_typesupport_library_path( + const std::string & package_name, const std::string & typesupport_identifier) +{ + const char * dynamic_library_folder; +#ifdef _WIN32 + dynamic_library_folder = "/bin/"; +#elif __APPLE__ + dynamic_library_folder = "/lib/"; +#else + dynamic_library_folder = "/lib/"; +#endif + + std::string package_prefix; + try { + package_prefix = ament_index_cpp::get_package_prefix(package_name); + } catch (ament_index_cpp::PackageNotFoundError & e) { + throw std::runtime_error(e.what()); + } + + const std::string library_path = rcpputils::path_for_library( + package_prefix + dynamic_library_folder, + package_name + "__" + typesupport_identifier); + if (library_path.empty()) { + throw std::runtime_error( + "Typesupport library for " + package_name + " does not exist in '" + package_prefix + + "'."); + } + return library_path; +} + +std::tuple +extract_type_identifier(const std::string & full_type) +{ + char type_separator = '/'; + auto sep_position_back = full_type.find_last_of(type_separator); + auto sep_position_front = full_type.find_first_of(type_separator); + if (sep_position_back == std::string::npos || + sep_position_back == 0 || + sep_position_back == full_type.length() - 1) + { + throw std::runtime_error( + "Message type is not of the form package/type and cannot be processed"); + } + + std::string package_name = full_type.substr(0, sep_position_front); + std::string middle_module = ""; + if (sep_position_back - sep_position_front > 0) { + middle_module = + full_type.substr(sep_position_front + 1, sep_position_back - sep_position_front - 1); + } + std::string type_name = full_type.substr(sep_position_back + 1); + + return std::make_tuple(package_name, middle_module, type_name); +} + +} // 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) +{ + std::string package_name; + std::string middle_module; + 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) { + std::stringstream rcutils_dynamic_loading_error; + rcutils_dynamic_loading_error << + "Something went wrong loading the typesupport library for message 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; + // This will throw runtime_error if the symbol was not found. + get_ts = reinterpret_cast(library.get_symbol(symbol_name)); + return get_ts(); + } catch (std::runtime_error &) { + throw std::runtime_error{mk_error("Library could not be found.")}; + } +} + +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/waitable.cpp b/rclcpp/src/rclcpp/waitable.cpp index 2e0410d00e..a42968dc95 100644 --- a/rclcpp/src/rclcpp/waitable.cpp +++ b/rclcpp/src/rclcpp/waitable.cpp @@ -61,14 +61,19 @@ Waitable::exchange_in_use_by_wait_set_state(bool in_use_state) } void -Waitable::set_listener_callback( - rmw_listener_callback_t callback, - const void * user_data) const +Waitable::set_on_ready_callback(std::function callback) { (void)callback; - (void)user_data; throw std::runtime_error( - "Custom waitables should override set_listener_callback() " - "if they want to use RMW listeners"); + "Custom waitables should override set_on_ready_callback " + "if they want to use it."); +} + +void +Waitable::clear_on_ready_callback() +{ + throw std::runtime_error( + "Custom waitables should override clear_on_ready_callback " + "if they want to use it."); } diff --git a/rclcpp/test/benchmark/benchmark_executor.cpp b/rclcpp/test/benchmark/benchmark_executor.cpp index 27e6d2ff5b..0f26335aa9 100644 --- a/rclcpp/test/benchmark/benchmark_executor.cpp +++ b/rclcpp/test/benchmark/benchmark_executor.cpp @@ -42,7 +42,7 @@ class PerformanceTestExecutor : public PerformanceTest nodes[i]->create_publisher( "/empty_msgs_" + std::to_string(i), rclcpp::QoS(10))); - auto callback = [this](test_msgs::msg::Empty::SharedPtr) {this->callback_count++;}; + auto callback = [this](test_msgs::msg::Empty::ConstSharedPtr) {this->callback_count++;}; subscriptions.push_back( nodes[i]->create_subscription( "/empty_msgs_" + std::to_string(i), rclcpp::QoS(10), std::move(callback))); @@ -376,9 +376,8 @@ BENCHMARK_F( auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); rclcpp::GuardCondition guard_condition(shared_context); - rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); - entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + entities_collector_->init(&wait_set, memory_strategy, &guard_condition); RCLCPP_SCOPE_EXIT(entities_collector_->fini()); reset_heap_counters(); diff --git a/rclcpp/test/msg/String.msg b/rclcpp/test/msg/String.msg new file mode 100644 index 0000000000..44e5aaf86b --- /dev/null +++ b/rclcpp/test/msg/String.msg @@ -0,0 +1 @@ +string data \ No newline at end of file diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index fba6117904..d7e2d72fe4 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -7,6 +7,7 @@ add_definitions(-DTEST_RESOURCES_DIRECTORY="${TEST_RESOURCES_DIRECTORY}") rosidl_generate_interfaces(${PROJECT_NAME}_test_msgs ../msg/Header.msg ../msg/MessageWithHeader.msg + ../msg/String.msg DEPENDENCIES builtin_interfaces LIBRARY_NAME ${PROJECT_NAME} SKIP_INSTALL @@ -143,6 +144,13 @@ if(TARGET test_intra_process_manager) ) target_link_libraries(test_intra_process_manager ${PROJECT_NAME}) 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}) +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 @@ -348,6 +356,44 @@ if(TARGET test_publisher) ) target_link_libraries(test_publisher ${PROJECT_NAME} mimick) endif() + +set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR}") +if(WIN32) + set(append_library_dirs "${append_library_dirs}/$") +endif() + +ament_add_gtest(test_publisher_with_type_adapter test_publisher_with_type_adapter.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}" +) +if(TARGET test_publisher_with_type_adapter) + ament_target_dependencies(test_publisher_with_type_adapter + "rcutils" + "rcl_interfaces" + "rmw" + "rosidl_runtime_cpp" + "rosidl_typesupport_cpp" + "test_msgs" + ) + rosidl_target_interfaces(test_publisher_with_type_adapter ${PROJECT_NAME}_test_msgs "rosidl_typesupport_cpp") + target_link_libraries(test_publisher_with_type_adapter ${PROJECT_NAME} mimick) +endif() + +ament_add_gtest(test_subscription_with_type_adapter test_subscription_with_type_adapter.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}" +) +if(TARGET test_subscription_with_type_adapter) + ament_target_dependencies(test_subscription_with_type_adapter + "rcutils" + "rcl_interfaces" + "rmw" + "rosidl_runtime_cpp" + "rosidl_typesupport_cpp" + "test_msgs" + ) + rosidl_target_interfaces(test_subscription_with_type_adapter ${PROJECT_NAME}_test_msgs "rosidl_typesupport_cpp") + target_link_libraries(test_subscription_with_type_adapter ${PROJECT_NAME} mimick) +endif() + ament_add_gtest(test_publisher_subscription_count_api test_publisher_subscription_count_api.cpp) if(TARGET test_publisher_subscription_count_api) ament_target_dependencies(test_publisher_subscription_count_api @@ -368,6 +414,21 @@ if(TARGET test_qos) ${PROJECT_NAME} ) endif() +function(test_generic_pubsub_for_rmw_implementation) + set(rmw_implementation_env_var RMW_IMPLEMENTATION=${rmw_implementation}) + ament_add_gmock(test_generic_pubsub${target_suffix} test_generic_pubsub.cpp + ENV ${rmw_implementation_env_var} + ) + if(TARGET test_generic_pubsub${target_suffix}) + target_link_libraries(test_generic_pubsub${target_suffix} ${PROJECT_NAME}) + ament_target_dependencies(test_generic_pubsub${target_suffix} + "rcpputils" + "rosidl_typesupport_cpp" + "test_msgs" + ) + endif() +endfunction() +call_for_each_rmw_implementation(test_generic_pubsub_for_rmw_implementation) ament_add_gtest(test_qos_event test_qos_event.cpp) if(TARGET test_qos_event) ament_target_dependencies(test_qos_event @@ -473,6 +534,10 @@ if(TARGET test_type_support) ) target_link_libraries(test_type_support ${PROJECT_NAME}) endif() +ament_add_gmock(test_typesupport_helpers test_typesupport_helpers.cpp) +if(TARGET test_typesupport_helpers) + target_link_libraries(test_typesupport_helpers ${PROJECT_NAME}) +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 @@ -481,11 +546,6 @@ if(TARGET test_find_weak_nodes) target_link_libraries(test_find_weak_nodes ${PROJECT_NAME}) endif() -set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR}") -if(WIN32) - set(append_library_dirs "${append_library_dirs}/$") -endif() - ament_add_gtest(test_externally_defined_services test_externally_defined_services.cpp) ament_target_dependencies(test_externally_defined_services "rcl" @@ -539,14 +599,6 @@ if(TARGET test_utilities) target_link_libraries(test_utilities ${PROJECT_NAME} mimick) endif() -ament_add_gtest(test_init test_init.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}") -if(TARGET test_init) - ament_target_dependencies(test_init - "rcl") - target_link_libraries(test_init ${PROJECT_NAME}) -endif() - ament_add_gtest(test_interface_traits test_interface_traits.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}") if(TARGET test_interface_traits) diff --git a/rclcpp/test/rclcpp/executors/test_events_executor.cpp b/rclcpp/test/rclcpp/executors/test_events_executor.cpp index 8d11f17037..eb7858a1ad 100644 --- a/rclcpp/test/rclcpp/executors/test_events_executor.cpp +++ b/rclcpp/test/rclcpp/executors/test_events_executor.cpp @@ -52,12 +52,6 @@ TEST_F(TestEventsExecutor, notify_waitable) rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); EXPECT_THROW(notifier->add_to_wait_set(&wait_set), std::runtime_error); EXPECT_THROW(notifier->is_ready(&wait_set), std::runtime_error); - - { - auto mock = mocking_utils::patch_and_return( - "lib:rclcpp", rcl_guard_condition_set_listener_callback, RCL_RET_ERROR); - EXPECT_THROW(std::make_shared(), std::runtime_error); - } } TEST_F(TestEventsExecutor, run_clients_servers) diff --git a/rclcpp/test/rclcpp/executors/test_events_executor_entities_collector.cpp b/rclcpp/test/rclcpp/executors/test_events_executor_entities_collector.cpp index 3f11f983fe..32ed2ecfa6 100644 --- a/rclcpp/test/rclcpp/executors/test_events_executor_entities_collector.cpp +++ b/rclcpp/test/rclcpp/executors/test_events_executor_entities_collector.cpp @@ -156,23 +156,3 @@ TEST_F(TestEventsExecutorEntitiesCollector, remove_node_opposite_order) EXPECT_NO_THROW(entities_collector_->remove_node(node2->get_node_base_interface())); } - -TEST_F(TestEventsExecutorEntitiesCollector, test_rcl_exception) -{ - auto node1 = std::make_shared("node1", "ns"); - auto node2 = std::make_shared("node2", "ns"); - entities_collector_->add_node(node1->get_node_base_interface()); - - { - auto mock = mocking_utils::patch_and_return( - "lib:rclcpp", rcl_guard_condition_set_listener_callback, RCL_RET_ERROR); - - EXPECT_THROW( - entities_collector_->add_node(node2->get_node_base_interface()), - std::runtime_error); - - EXPECT_THROW( - entities_collector_->remove_node(node1->get_node_base_interface()), - std::runtime_error); - } -} diff --git a/rclcpp/test/rclcpp/executors/test_events_queue.cpp b/rclcpp/test/rclcpp/executors/test_events_queue.cpp index 9d600e61f7..a07681698e 100644 --- a/rclcpp/test/rclcpp/executors/test_events_queue.cpp +++ b/rclcpp/test/rclcpp/executors/test_events_queue.cpp @@ -52,14 +52,14 @@ TEST(TestEventsQueue, SimpleQueueTest) EXPECT_TRUE(simple_queue->empty()); // Lets push an event into the queue and get it back - rclcpp::executors::ExecutorEvent push_event = {simple_queue.get(), - rclcpp::executors::ExecutorEventType::SUBSCRIPTION_EVENT}; + rclcpp::executors::ExecutorEvent push_event = {simple_queue.get(), 0, + rclcpp::executors::ExecutorEventType::SUBSCRIPTION_EVENT, 1}; simple_queue->push(push_event); rclcpp::executors::ExecutorEvent front_event = simple_queue->front(); // The events should be equal - EXPECT_EQ(push_event.entity_id, front_event.entity_id); + EXPECT_EQ(push_event.exec_entity_id, front_event.exec_entity_id); EXPECT_EQ(push_event.type, front_event.type); } diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index 7cecdff81a..3a386e3a9f 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -31,6 +31,7 @@ #include "rcl/time.h" #include "rclcpp/clock.hpp" #include "rclcpp/duration.hpp" +#include "rclcpp/guard_condition.hpp" #include "rclcpp/rclcpp.hpp" #include "test_msgs/msg/empty.hpp" @@ -62,7 +63,7 @@ class TestExecutors : public ::testing::Test const std::string topic_name = std::string("topic_") + test_name.str(); publisher = node->create_publisher(topic_name, rclcpp::QoS(10)); - auto callback = [this](test_msgs::msg::Empty::SharedPtr) {this->callback_count++;}; + auto callback = [this](test_msgs::msg::Empty::ConstSharedPtr) {this->callback_count++;}; subscription = node->create_subscription( topic_name, rclcpp::QoS(10), std::move(callback)); @@ -129,9 +130,9 @@ TYPED_TEST_SUITE(TestExecutors, ExecutorTypes, ExecutorTypeNames); using StandardExecutors = ::testing::Types< rclcpp::executors::EventsExecutor, - rclcpp::executors::MultiThreadedExecutor, - rclcpp::executors::SingleThreadedExecutor>; -TYPED_TEST_CASE(TestExecutorsStable, StandardExecutors, ExecutorTypeNames); + rclcpp::executors::SingleThreadedExecutor, + rclcpp::executors::MultiThreadedExecutor>; +TYPED_TEST_SUITE(TestExecutorsStable, StandardExecutors, ExecutorTypeNames); // Make sure that executors detach from nodes when destructing TYPED_TEST(TestExecutors, detachOnDestruction) { @@ -162,7 +163,7 @@ TYPED_TEST(TestExecutorsStable, addTemporaryNode) { // Sleep for a short time to verify executor.spin() is going, and didn't throw. std::thread spinner([&]() {EXPECT_NO_THROW(executor.spin());}); - std::this_thread::sleep_for(200ms); + std::this_thread::sleep_for(50ms); executor.cancel(); spinner.join(); } @@ -355,86 +356,21 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteWithTimeout) { spinner.join(); } -// Check spin_until_future_complete can be properly interrupted. -TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteInterrupted) { - using ExecutorType = TypeParam; - ExecutorType executor; - executor.add_node(this->node); - - bool spin_exited = false; - - // This needs to block longer than it takes to get to the shutdown call below and for - // spin_until_future_complete to return - std::future future = std::async( - std::launch::async, - [&spin_exited]() { - auto start = std::chrono::steady_clock::now(); - while (!spin_exited && (std::chrono::steady_clock::now() - start) < 1s) { - std::this_thread::sleep_for(1ms); - } - }); - - // Long timeout - std::thread spinner([&spin_exited, &executor, &future]() { - auto ret = executor.spin_until_future_complete(future, 1s); - EXPECT_EQ(rclcpp::FutureReturnCode::INTERRUPTED, ret); - spin_exited = true; - }); - - // Do some minimal work - this->publisher->publish(test_msgs::msg::Empty()); - std::this_thread::sleep_for(1ms); - - // Force interruption - rclcpp::shutdown(); - - // Give it time to exit - auto start = std::chrono::steady_clock::now(); - while (!spin_exited && (std::chrono::steady_clock::now() - start) < 1s) { - std::this_thread::sleep_for(1ms); - } - - EXPECT_TRUE(spin_exited); - spinner.join(); -} - class TestWaitable : public rclcpp::Waitable { public: - TestWaitable() - { - rcl_guard_condition_options_t guard_condition_options = - rcl_guard_condition_get_default_options(); - - gc_ = rcl_get_zero_initialized_guard_condition(); - rcl_ret_t ret = rcl_guard_condition_init( - &gc_, - rclcpp::contexts::get_global_default_context()->get_rcl_context().get(), - guard_condition_options); - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); - } - } - - ~TestWaitable() - { - rcl_ret_t ret = rcl_guard_condition_fini(&gc_); - if (RCL_RET_OK != ret) { - fprintf(stderr, "failed to call rcl_guard_condition_fini\n"); - } - } + TestWaitable() = default; bool add_to_wait_set(rcl_wait_set_t * wait_set) override { - rcl_ret_t ret = rcl_wait_set_add_guard_condition(wait_set, &gc_, NULL); - return RCL_RET_OK == ret; + gc_.add_to_wait_set(wait_set); + return true; } - bool trigger() + void trigger() { - rcl_ret_t ret = rcl_trigger_guard_condition(&gc_); - return RCL_RET_OK == ret; + gc_.trigger(); } bool @@ -468,21 +404,13 @@ class TestWaitable : public rclcpp::Waitable } void - set_listener_callback( - rmw_listener_callback_t callback, - const void * user_data) const override + set_on_ready_callback(std::function callback) override { - rcl_ret_t ret = rcl_guard_condition_set_listener_callback( - &gc_, callback, user_data); - - if (RCL_RET_OK != ret) { - throw std::runtime_error(std::string("Couldn't set guard condition callback")); - } } private: size_t count_ = 0; - rcl_guard_condition_t gc_; + rclcpp::GuardCondition gc_; }; TYPED_TEST(TestExecutorsStable, spinAll) { @@ -596,54 +524,46 @@ TYPED_TEST(TestExecutorsStable, testSpinNodeUntilFutureCompleteNodePtr) { EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); } -TYPED_TEST(TestExecutorsStable, testSpinSomeWhileSpinning) { - using ExecutorType = TypeParam; - ExecutorType executor; - - std::thread spinner([&]() {executor.spin();}); - - // Wait to make sure thread started - do { - std::this_thread::sleep_for(5ms); - } while (!spinner.joinable()); - - EXPECT_THROW(executor.spin_some(1s), std::runtime_error); - - executor.cancel(); - spinner.join(); -} - -TYPED_TEST(TestExecutorsStable, testSpinAllWhileSpinning) { +// Check spin_until_future_complete can be properly interrupted. +TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteInterrupted) { using ExecutorType = TypeParam; ExecutorType executor; + executor.add_node(this->node); - std::thread spinner([&]() {executor.spin();}); - - // Wait to make sure thread started - do { - std::this_thread::sleep_for(5ms); - } while (!spinner.joinable()); - - EXPECT_THROW(executor.spin_all(1s), std::runtime_error); + bool spin_exited = false; - executor.cancel(); - spinner.join(); -} + // This needs to block longer than it takes to get to the shutdown call below and for + // spin_until_future_complete to return + std::future future = std::async( + std::launch::async, + [&spin_exited]() { + auto start = std::chrono::steady_clock::now(); + while (!spin_exited && (std::chrono::steady_clock::now() - start) < 1s) { + std::this_thread::sleep_for(1ms); + } + }); -TYPED_TEST(TestExecutorsStable, testSpinOnceWhileSpinning) { - using ExecutorType = TypeParam; - ExecutorType executor; + // Long timeout + std::thread spinner([&spin_exited, &executor, &future]() { + auto ret = executor.spin_until_future_complete(future, 1s); + EXPECT_EQ(rclcpp::FutureReturnCode::INTERRUPTED, ret); + spin_exited = true; + }); - std::thread spinner([&]() {executor.spin();}); + // Do some minimal work + this->publisher->publish(test_msgs::msg::Empty()); + std::this_thread::sleep_for(1ms); - // Wait to make sure thread started - do { - std::this_thread::sleep_for(5ms); - } while (!spinner.joinable()); + // Force interruption + rclcpp::shutdown(); - EXPECT_THROW(executor.spin_once(), std::runtime_error); + // Give it time to exit + auto start = std::chrono::steady_clock::now(); + while (!spin_exited && (std::chrono::steady_clock::now() - start) < 1s) { + std::this_thread::sleep_for(1ms); + } - executor.cancel(); + EXPECT_TRUE(spin_exited); spinner.join(); } 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 e39589c3c6..4628d2472b 100644 --- a/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp +++ b/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp @@ -141,12 +141,11 @@ TEST_F(TestStaticExecutorEntitiesCollector, init_bad_arguments) { RCLCPP_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));}); rclcpp::GuardCondition guard_condition(shared_context); - rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); // Check memory strategy is nullptr rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy = nullptr; EXPECT_THROW( - entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition), + entities_collector_->init(&wait_set, memory_strategy, &guard_condition), std::runtime_error); } @@ -167,9 +166,7 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_basic_node) { auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); rclcpp::GuardCondition guard_condition(shared_context); - rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); - - entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + entities_collector_->init(&wait_set, memory_strategy, &guard_condition); RCLCPP_SCOPE_EXIT(entities_collector_->fini()); EXPECT_EQ( expected_number_of_entities->subscriptions, @@ -183,7 +180,7 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_basic_node) { entities_collector_->get_number_of_waitables()); EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface())); - entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + entities_collector_->init(&wait_set, memory_strategy, &guard_condition); EXPECT_EQ(0u, entities_collector_->get_number_of_subscriptions()); EXPECT_EQ(0u, entities_collector_->get_number_of_timers()); EXPECT_EQ(0u, entities_collector_->get_number_of_services()); @@ -214,10 +211,9 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_out_of_scope) { auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); rclcpp::GuardCondition guard_condition(shared_context); - rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); // Expect weak_node pointers to be cleaned up and used - entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + entities_collector_->init(&wait_set, memory_strategy, &guard_condition); RCLCPP_SCOPE_EXIT(entities_collector_->fini()); EXPECT_EQ(0u, entities_collector_->get_number_of_subscriptions()); EXPECT_EQ(0u, entities_collector_->get_number_of_timers()); @@ -255,7 +251,7 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_with_entities) { // Create 1 of each entity type auto subscription = node->create_subscription( - "topic", rclcpp::QoS(10), [](test_msgs::msg::Empty::SharedPtr) {}); + "topic", rclcpp::QoS(10), [](test_msgs::msg::Empty::ConstSharedPtr) {}); auto timer = node->create_wall_timer(std::chrono::seconds(60), []() {}); auto service = @@ -267,7 +263,7 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_with_entities) { auto client = node->create_client("service"); auto waitable = std::make_shared(); - // Adding a subscription with rmw_connext_cpp adds another waitable, so we need to get the + // Adding a subscription could add another waitable, so we need to get the // current number of waitables just before adding the new waitable. expected_number_of_entities->waitables = get_number_of_default_entities(node)->waitables; node->get_node_waitables_interface()->add_waitable(waitable, nullptr); @@ -286,9 +282,8 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_with_entities) { auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); rclcpp::GuardCondition guard_condition(shared_context); - rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); - entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + entities_collector_->init(&wait_set, memory_strategy, &guard_condition); RCLCPP_SCOPE_EXIT(entities_collector_->fini()); EXPECT_EQ( @@ -308,7 +303,7 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_with_entities) { entities_collector_->get_number_of_waitables()); entities_collector_->remove_node(node->get_node_base_interface()); - entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + entities_collector_->init(&wait_set, memory_strategy, &guard_condition); EXPECT_EQ(0u, entities_collector_->get_number_of_subscriptions()); EXPECT_EQ(0u, entities_collector_->get_number_of_timers()); EXPECT_EQ(0u, entities_collector_->get_number_of_services()); @@ -365,9 +360,8 @@ TEST_F(TestStaticExecutorEntitiesCollector, prepare_wait_set_rcl_wait_set_clear_ auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); rclcpp::GuardCondition guard_condition(shared_context); - rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); - entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + entities_collector_->init(&wait_set, memory_strategy, &guard_condition); RCLCPP_SCOPE_EXIT(entities_collector_->fini()); { @@ -396,9 +390,8 @@ TEST_F(TestStaticExecutorEntitiesCollector, prepare_wait_set_rcl_wait_set_resize auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); rclcpp::GuardCondition guard_condition(shared_context); - rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); - entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + entities_collector_->init(&wait_set, memory_strategy, &guard_condition); RCLCPP_SCOPE_EXIT(entities_collector_->fini()); { @@ -434,9 +427,8 @@ TEST_F(TestStaticExecutorEntitiesCollector, refresh_wait_set_rcl_wait_failed) { auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); rclcpp::GuardCondition guard_condition(shared_context); - rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); - entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + entities_collector_->init(&wait_set, memory_strategy, &guard_condition); RCLCPP_SCOPE_EXIT(entities_collector_->fini()); { @@ -455,7 +447,7 @@ TEST_F(TestStaticExecutorEntitiesCollector, refresh_wait_set_add_handles_to_wait // Create 1 of each entity type auto subscription = node->create_subscription( - "topic", rclcpp::QoS(10), [](test_msgs::msg::Empty::SharedPtr) {}); + "topic", rclcpp::QoS(10), [](test_msgs::msg::Empty::ConstSharedPtr) {}); auto timer = node->create_wall_timer(std::chrono::seconds(60), []() {}); auto service = @@ -483,9 +475,8 @@ TEST_F(TestStaticExecutorEntitiesCollector, refresh_wait_set_add_handles_to_wait auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); rclcpp::GuardCondition guard_condition(shared_context); - rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); - entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + entities_collector_->init(&wait_set, memory_strategy, &guard_condition); RCLCPP_SCOPE_EXIT(entities_collector_->fini()); { @@ -515,14 +506,13 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_to_wait_set_nullptr) { auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); rclcpp::GuardCondition guard_condition(shared_context); - rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); - entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + entities_collector_->init(&wait_set, memory_strategy, &guard_condition); RCLCPP_SCOPE_EXIT(entities_collector_->fini()); - RCLCPP_EXPECT_THROW_EQ( + EXPECT_THROW( entities_collector_->add_to_wait_set(nullptr), - std::runtime_error("Executor waitable: couldn't add guard condition to wait set")); + std::invalid_argument); rcl_reset_error(); EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface())); @@ -543,7 +533,6 @@ TEST_F(TestStaticExecutorEntitiesCollector, fill_memory_strategy_invalid_group) auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); rclcpp::GuardCondition guard_condition(shared_context); - rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive); @@ -553,7 +542,7 @@ TEST_F(TestStaticExecutorEntitiesCollector, fill_memory_strategy_invalid_group) cb_group.reset(); - entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + entities_collector_->init(&wait_set, memory_strategy, &guard_condition); RCLCPP_SCOPE_EXIT(entities_collector_->fini()); ASSERT_EQ(entities_collector_->get_all_callback_groups().size(), 1u); @@ -621,9 +610,8 @@ TEST_F( auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); rclcpp::GuardCondition guard_condition(shared_context); - rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); - entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + entities_collector_->init(&wait_set, memory_strategy, &guard_condition); RCLCPP_SCOPE_EXIT(entities_collector_->fini()); cb_group->get_associated_with_executor_atomic().exchange(false); diff --git a/rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp b/rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp index cd47396f9e..5ca6c1c25a 100644 --- a/rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp +++ b/rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp @@ -45,16 +45,6 @@ class TestStaticSingleThreadedExecutor : public ::testing::Test } }; -TEST_F(TestStaticSingleThreadedExecutor, check_unimplemented) { - rclcpp::executors::StaticSingleThreadedExecutor executor; - auto node = std::make_shared("node", "ns"); - executor.add_node(node); - - EXPECT_THROW(executor.spin_some(), rclcpp::exceptions::UnimplementedError); - EXPECT_THROW(executor.spin_all(0ns), rclcpp::exceptions::UnimplementedError); - EXPECT_THROW(executor.spin_once(0ns), rclcpp::exceptions::UnimplementedError); -} - TEST_F(TestStaticSingleThreadedExecutor, add_callback_group_trigger_guard_failed) { rclcpp::executors::StaticSingleThreadedExecutor executor; auto node = std::make_shared("node", "ns"); diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp index 08717164e2..6a44d7abf7 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp @@ -335,7 +335,7 @@ TEST_F(TestNodeGraph, get_info_by_topic) { const rclcpp::QoS publisher_qos(1); auto publisher = node()->create_publisher("topic", publisher_qos); - auto callback = [](const test_msgs::msg::Empty::SharedPtr) {}; + auto callback = [](test_msgs::msg::Empty::ConstSharedPtr) {}; const rclcpp::QoS subscriber_qos(10); auto subscription = @@ -364,28 +364,10 @@ TEST_F(TestNodeGraph, get_info_by_topic) EXPECT_EQ(rclcpp::EndpointType::Publisher, const_publisher_endpoint_info.endpoint_type()); rclcpp::QoS actual_qos = publisher_endpoint_info.qos_profile(); - switch (actual_qos.get_rmw_qos_profile().history) { - case RMW_QOS_POLICY_HISTORY_KEEP_LAST: - EXPECT_EQ(1u, actual_qos.get_rmw_qos_profile().depth); - break; - case RMW_QOS_POLICY_HISTORY_UNKNOWN: - EXPECT_EQ(0u, actual_qos.get_rmw_qos_profile().depth); - break; - default: - ADD_FAILURE() << "unexpected history"; - } + EXPECT_EQ(actual_qos.reliability(), rclcpp::ReliabilityPolicy::Reliable); rclcpp::QoS const_actual_qos = const_publisher_endpoint_info.qos_profile(); - switch (const_actual_qos.get_rmw_qos_profile().history) { - case RMW_QOS_POLICY_HISTORY_KEEP_LAST: - EXPECT_EQ(1u, const_actual_qos.get_rmw_qos_profile().depth); - break; - case RMW_QOS_POLICY_HISTORY_UNKNOWN: - EXPECT_EQ(0u, const_actual_qos.get_rmw_qos_profile().depth); - break; - default: - ADD_FAILURE() << "unexpected history"; - } + EXPECT_EQ(const_actual_qos.reliability(), rclcpp::ReliabilityPolicy::Reliable); auto endpoint_gid = publisher_endpoint_info.endpoint_gid(); auto const_endpoint_gid = const_publisher_endpoint_info.endpoint_gid(); diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp index a7607f5d76..38388d9308 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp @@ -103,7 +103,7 @@ TEST_F(TestNodeService, add_service_rcl_trigger_guard_condition_error) "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); RCLCPP_EXPECT_THROW_EQ( node_services->add_service(service, callback_group), - std::runtime_error("Failed to notify wait set on service creation: error not set")); + std::runtime_error("error not set")); } TEST_F(TestNodeService, add_client) @@ -130,7 +130,7 @@ TEST_F(TestNodeService, add_client_rcl_trigger_guard_condition_error) "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); RCLCPP_EXPECT_THROW_EQ( node_services->add_client(client, callback_group), - std::runtime_error("Failed to notify wait set on client creation: error not set")); + std::runtime_error("error not set")); } TEST_F(TestNodeService, resolve_service_name) diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp index e206e75b6c..e1d5cd179d 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp @@ -86,5 +86,5 @@ TEST_F(TestNodeTimers, add_timer_rcl_trigger_guard_condition_error) "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); RCLCPP_EXPECT_THROW_EQ( node_timers->add_timer(timer, callback_group), - std::runtime_error("Failed to notify wait set on timer creation: error not set")); + std::runtime_error("error not set")); } diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp index 16f1d92b86..ec69ac3e49 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp @@ -71,6 +71,8 @@ class TestSubscription : public rclcpp::SubscriptionBase void handle_message(std::shared_ptr &, const rclcpp::MessageInfo &) override {} void handle_loaned_message(void *, const rclcpp::MessageInfo &) override {} + void handle_serialized_message( + const std::shared_ptr &, const rclcpp::MessageInfo &) override {} void return_message(std::shared_ptr &) override {} void return_serialized_message(std::shared_ptr &) override {} }; @@ -127,7 +129,7 @@ TEST_F(TestNodeTopics, add_publisher_rcl_trigger_guard_condition_error) "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); RCLCPP_EXPECT_THROW_EQ( node_topics->add_publisher(publisher, callback_group), - std::runtime_error("Failed to notify wait set on publisher creation: error not set")); + std::runtime_error("error not set")); } TEST_F(TestNodeTopics, add_subscription) @@ -155,7 +157,7 @@ TEST_F(TestNodeTopics, add_subscription_rcl_trigger_guard_condition_error) "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); RCLCPP_EXPECT_THROW_EQ( node_topics->add_subscription(subscription, callback_group), - std::runtime_error("failed to notify wait set on subscription creation: error not set")); + std::runtime_error("error not set")); } TEST_F(TestNodeTopics, resolve_topic_name) diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp index a12ef36eab..cb2db50084 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp @@ -96,5 +96,5 @@ TEST_F(TestNodeWaitables, add_waitable_rcl_trigger_guard_condition_error) "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); RCLCPP_EXPECT_THROW_EQ( node_waitables->add_waitable(waitable, callback_group), - std::runtime_error("Failed to notify wait set on waitable creation: error not set")); + std::runtime_error("error not set")); } diff --git a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp index b401d6d046..889ee84983 100644 --- a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp +++ b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp @@ -125,7 +125,7 @@ class TestAllocatorMemoryStrategy : public ::testing::Test std::shared_ptr create_node_with_subscription(const std::string & name) { - auto subscription_callback = [](const test_msgs::msg::Empty::SharedPtr) {}; + auto subscription_callback = [](test_msgs::msg::Empty::ConstSharedPtr) {}; const rclcpp::QoS qos(10); auto node_with_subscription = create_node_with_disabled_callback_groups(name); @@ -468,9 +468,9 @@ TEST_F(TestAllocatorMemoryStrategy, construct_destruct) { } TEST_F(TestAllocatorMemoryStrategy, add_remove_guard_conditions) { - rcl_guard_condition_t guard_condition1 = rcl_get_zero_initialized_guard_condition(); - rcl_guard_condition_t guard_condition2 = rcl_get_zero_initialized_guard_condition(); - rcl_guard_condition_t guard_condition3 = rcl_get_zero_initialized_guard_condition(); + rclcpp::GuardCondition guard_condition1; + rclcpp::GuardCondition guard_condition2; + rclcpp::GuardCondition guard_condition3; EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(&guard_condition1)); EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(&guard_condition2)); @@ -511,11 +511,10 @@ TEST_F(TestAllocatorMemoryStrategy, number_of_entities_with_subscription) { RclWaitSetSizes expected_sizes = {}; expected_sizes.size_of_subscriptions = 1; const std::string implementation_identifier = rmw_get_implementation_identifier(); - // TODO(asorbini): Remove Connext exception once ros2/rmw_connext is deprecated. - if (implementation_identifier == "rmw_connext_cpp" || - implementation_identifier == "rmw_cyclonedds_cpp") + if (implementation_identifier == "rmw_cyclonedds_cpp" || + implementation_identifier == "rmw_connextdds") { - // For cyclonedds, a subscription will also add an event and waitable + // For cyclonedds and connext, a subscription will also add an event and waitable expected_sizes.size_of_events += 1; expected_sizes.size_of_waitables += 1; } @@ -586,24 +585,17 @@ TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_client) { TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_guard_condition) { auto node = create_node_with_disabled_callback_groups("node"); - rcl_guard_condition_t guard_condition = rcl_get_zero_initialized_guard_condition(); auto context = node->get_node_base_interface()->get_context(); - rcl_context_t * rcl_context = context->get_rcl_context().get(); - rcl_guard_condition_options_t guard_condition_options = { - rcl_get_default_allocator()}; - - EXPECT_EQ( - RCL_RET_OK, - rcl_guard_condition_init(&guard_condition, rcl_context, guard_condition_options)); - RCLCPP_SCOPE_EXIT( - { - EXPECT_EQ(RCL_RET_OK, rcl_guard_condition_fini(&guard_condition)); - }); + + rclcpp::GuardCondition guard_condition(context); + + EXPECT_NO_THROW(rclcpp::GuardCondition guard_condition(context);); allocator_memory_strategy()->add_guard_condition(&guard_condition); + RclWaitSetSizes insufficient_capacities = SufficientWaitSetCapacities(); insufficient_capacities.size_of_guard_conditions = 0; - EXPECT_TRUE(TestAddHandlesToWaitSet(node, insufficient_capacities)); + EXPECT_THROW(TestAddHandlesToWaitSet(node, insufficient_capacities), std::runtime_error); } TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_timer) { @@ -781,7 +773,7 @@ TEST_F(TestAllocatorMemoryStrategy, get_next_subscription_out_of_scope) { subscription_options.callback_group = callback_group; - auto subscription_callback = [](const test_msgs::msg::Empty::SharedPtr) {}; + auto subscription_callback = [](test_msgs::msg::Empty::ConstSharedPtr) {}; const rclcpp::QoS qos(10); auto subscription = node->create_subscription< 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 f25da5cb3d..a44ae0fb1d 100644 --- a/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp +++ b/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp @@ -101,7 +101,7 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, add_callback_groups) { const rclcpp::QoS qos(10); auto options = rclcpp::SubscriptionOptions(); - auto callback = [](const test_msgs::msg::Empty::SharedPtr) {}; + auto callback = [](test_msgs::msg::Empty::ConstSharedPtr) {}; rclcpp::CallbackGroup::SharedPtr cb_grp2 = node->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive); options.callback_group = cb_grp2; @@ -144,7 +144,7 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, remove_callback_groups) { executor.add_callback_group(cb_grp, node->get_node_base_interface()); const rclcpp::QoS qos(10); auto options = rclcpp::SubscriptionOptions(); - auto callback = [](const test_msgs::msg::Empty::SharedPtr) {}; + auto callback = [](test_msgs::msg::Empty::ConstSharedPtr) {}; rclcpp::CallbackGroup::SharedPtr cb_grp2 = node->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive); options.callback_group = cb_grp2; @@ -230,7 +230,7 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, add_unallowable_callback_groups) const rclcpp::QoS qos(10); auto options = rclcpp::SubscriptionOptions(); - auto callback = [](const test_msgs::msg::Empty::SharedPtr) {}; + auto callback = [](test_msgs::msg::Empty::ConstSharedPtr) {}; rclcpp::CallbackGroup::SharedPtr cb_grp2 = node->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive, false); options.callback_group = cb_grp2; @@ -265,7 +265,7 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, one_node_many_callback_groups_many_e timer_executor.add_callback_group(cb_grp, node->get_node_base_interface()); const rclcpp::QoS qos(10); auto options = rclcpp::SubscriptionOptions(); - auto callback = [](const test_msgs::msg::Empty::SharedPtr) {}; + auto callback = [](test_msgs::msg::Empty::ConstSharedPtr) {}; rclcpp::CallbackGroup::SharedPtr cb_grp2 = node->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive, false); options.callback_group = cb_grp2; diff --git a/rclcpp/test/rclcpp/test_any_subscription_callback.cpp b/rclcpp/test/rclcpp/test_any_subscription_callback.cpp index 5ed90846b7..8e44eb71bf 100644 --- a/rclcpp/test/rclcpp/test_any_subscription_callback.cpp +++ b/rclcpp/test/rclcpp/test_any_subscription_callback.cpp @@ -16,6 +16,7 @@ #include #include +#include #include #include "rclcpp/any_subscription_callback.hpp" @@ -25,42 +26,57 @@ class TestAnySubscriptionCallback : public ::testing::Test { public: - TestAnySubscriptionCallback() - : allocator_(std::make_shared>()), - any_subscription_callback_(allocator_) {} - void SetUp() + TestAnySubscriptionCallback() {} + + static + std::unique_ptr + get_unique_ptr_msg() { - msg_shared_ptr_ = std::make_shared(); - msg_const_shared_ptr_ = std::make_shared(); - msg_unique_ptr_ = std::make_unique(); + return std::make_unique(); } protected: - std::shared_ptr> allocator_; - rclcpp::AnySubscriptionCallback> - any_subscription_callback_; - - std::shared_ptr msg_shared_ptr_; - std::shared_ptr msg_const_shared_ptr_; - std::unique_ptr msg_unique_ptr_; + rclcpp::AnySubscriptionCallback any_subscription_callback_; + std::shared_ptr msg_shared_ptr_{std::make_shared()}; rclcpp::MessageInfo message_info_; }; void construct_with_null_allocator() { +// suppress deprecated function warning +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif + // We need to wrap this in a function because `EXPECT_THROW` is a macro, and thinks // that the comma in here splits macro arguments, not the template arguments. - rclcpp::AnySubscriptionCallback< - test_msgs::msg::Empty, std::allocator> any_subscription_callback_(nullptr); + rclcpp::AnySubscriptionCallback any_subscription_callback(nullptr); + +// remove warning suppression +#if !defined(_WIN32) +# pragma GCC diagnostic pop +#else // !defined(_WIN32) +# pragma warning(pop) +#endif } -TEST(AnySubscription, null_allocator) { +TEST(AnySubscriptionCallback, null_allocator) { EXPECT_THROW( construct_with_null_allocator(), - std::runtime_error); + std::invalid_argument); } TEST_F(TestAnySubscriptionCallback, construct_destruct) { + // Default constructor. + rclcpp::AnySubscriptionCallback asc1; + + // Constructor with allocator. + std::allocator allocator; + rclcpp::AnySubscriptionCallback asc2(allocator); } TEST_F(TestAnySubscriptionCallback, unset_dispatch_throw) { @@ -68,153 +84,539 @@ TEST_F(TestAnySubscriptionCallback, unset_dispatch_throw) { any_subscription_callback_.dispatch(msg_shared_ptr_, message_info_), std::runtime_error); EXPECT_THROW( - any_subscription_callback_.dispatch_intra_process(msg_const_shared_ptr_, message_info_), + any_subscription_callback_.dispatch_intra_process(msg_shared_ptr_, message_info_), std::runtime_error); EXPECT_THROW( - any_subscription_callback_.dispatch_intra_process(std::move(msg_unique_ptr_), message_info_), + any_subscription_callback_.dispatch_intra_process(get_unique_ptr_msg(), message_info_), std::runtime_error); } -TEST_F(TestAnySubscriptionCallback, set_dispatch_shared_ptr) { - int callback_count = 0; - auto shared_ptr_callback = [&callback_count]( - const std::shared_ptr) { - callback_count++; - }; - - any_subscription_callback_.set(shared_ptr_callback); - EXPECT_NO_THROW(any_subscription_callback_.dispatch(msg_shared_ptr_, message_info_)); - EXPECT_EQ(callback_count, 1); +// +// Parameterized test to test across all callback types and dispatch types. +// - // Can't convert ConstSharedPtr to SharedPtr - EXPECT_THROW( - any_subscription_callback_.dispatch_intra_process(msg_const_shared_ptr_, message_info_), - std::runtime_error); - EXPECT_EQ(callback_count, 1); +// Type adapter to be used in tests. +struct MyEmpty {}; - // Promotes Unique into SharedPtr - EXPECT_NO_THROW( - any_subscription_callback_.dispatch_intra_process(std::move(msg_unique_ptr_), message_info_)); - EXPECT_EQ(callback_count, 2); -} +template<> +struct rclcpp::TypeAdapter +{ + using is_specialized = std::true_type; + using custom_type = MyEmpty; + using ros_message_type = test_msgs::msg::Empty; + + static + void + convert_to_ros_message(const custom_type &, ros_message_type &) + {} + + static + void + convert_to_custom(const ros_message_type &, custom_type &) + {} +}; -TEST_F(TestAnySubscriptionCallback, set_dispatch_shared_ptr_w_info) { - int callback_count = 0; - auto shared_ptr_w_info_callback = [&callback_count]( - const std::shared_ptr, const rclcpp::MessageInfo &) { - callback_count++; - }; +using MyTA = rclcpp::TypeAdapter; - any_subscription_callback_.set(shared_ptr_w_info_callback); +template +class InstanceContextImpl +{ +public: + InstanceContextImpl() = default; + virtual ~InstanceContextImpl() = default; - EXPECT_NO_THROW(any_subscription_callback_.dispatch(msg_shared_ptr_, message_info_)); - EXPECT_EQ(callback_count, 1); + explicit InstanceContextImpl(rclcpp::AnySubscriptionCallback asc) + : any_subscription_callback_(asc) + {} - // Can't convert ConstSharedPtr to SharedPtr - EXPECT_THROW( - any_subscription_callback_.dispatch_intra_process(msg_const_shared_ptr_, message_info_), - std::runtime_error); - EXPECT_EQ(callback_count, 1); + virtual + rclcpp::AnySubscriptionCallback + get_any_subscription_callback_to_test() const + { + return any_subscription_callback_; + } - // Promotes Unique into SharedPtr - EXPECT_NO_THROW( - any_subscription_callback_.dispatch_intra_process(std::move(msg_unique_ptr_), message_info_)); - EXPECT_EQ(callback_count, 2); -} +protected: + rclcpp::AnySubscriptionCallback any_subscription_callback_; +}; -TEST_F(TestAnySubscriptionCallback, set_dispatch_const_shared_ptr) { - int callback_count = 0; - auto const_shared_ptr_callback = [&callback_count]( - std::shared_ptr) { - callback_count++; - }; +template +class InstanceContext +{ +public: + InstanceContext(const std::string & name, std::shared_ptr> impl) + : name(name), impl_(impl) + {} - any_subscription_callback_.set(const_shared_ptr_callback); + InstanceContext( + const std::string & name, + rclcpp::AnySubscriptionCallback asc) + : name(name), impl_(std::make_shared>(asc)) + {} - // Ok to promote shared_ptr to ConstSharedPtr - EXPECT_NO_THROW(any_subscription_callback_.dispatch(msg_shared_ptr_, message_info_)); - EXPECT_EQ(callback_count, 1); + InstanceContext(const InstanceContext & other) + : InstanceContext(other.name, other.impl_) {} - EXPECT_NO_THROW( - any_subscription_callback_.dispatch_intra_process(msg_const_shared_ptr_, message_info_)); - EXPECT_EQ(callback_count, 2); + rclcpp::AnySubscriptionCallback + get_any_subscription_callback_to_test() const + { + return impl_->get_any_subscription_callback_to_test(); + } - // Not allowed to convert unique_ptr to const shared_ptr - EXPECT_THROW( - any_subscription_callback_.dispatch_intra_process(std::move(msg_unique_ptr_), message_info_), - std::runtime_error); - EXPECT_EQ(callback_count, 2); -} + std::string name; -TEST_F(TestAnySubscriptionCallback, set_dispatch_const_shared_ptr_w_info) { - int callback_count = 0; - auto const_shared_ptr_callback = [&callback_count]( - std::shared_ptr, const rclcpp::MessageInfo &) { - callback_count++; - }; +protected: + std::shared_ptr> impl_; +}; - any_subscription_callback_.set( - std::move(const_shared_ptr_callback)); +class DispatchTests + : public TestAnySubscriptionCallback, + public ::testing::WithParamInterface> +{}; - // Ok to promote shared_ptr to ConstSharedPtr - EXPECT_NO_THROW(any_subscription_callback_.dispatch(msg_shared_ptr_, message_info_)); - EXPECT_EQ(callback_count, 1); +class DispatchTestsWithTA + : public TestAnySubscriptionCallback, + public ::testing::WithParamInterface> +{}; - EXPECT_NO_THROW( - any_subscription_callback_.dispatch_intra_process(msg_const_shared_ptr_, message_info_)); - EXPECT_EQ(callback_count, 2); +auto +format_parameter(const ::testing::TestParamInfo & info) +{ + return info.param.name; +} - // Not allowed to convert unique_ptr to const shared_ptr - EXPECT_THROW( - any_subscription_callback_.dispatch_intra_process(std::move(msg_unique_ptr_), message_info_), - std::runtime_error); - EXPECT_EQ(callback_count, 2); +auto +format_parameter_with_ta(const ::testing::TestParamInfo & info) +{ + return info.param.name; } -TEST_F(TestAnySubscriptionCallback, set_dispatch_unique_ptr) { - int callback_count = 0; - auto unique_ptr_callback = [&callback_count]( - std::unique_ptr) { - callback_count++; - }; +#define PARAMETERIZED_TESTS(DispatchTests_name) \ + /* Testing dispatch with shared_ptr as input */ \ + TEST_P(DispatchTests_name, test_inter_shared_dispatch) { \ + auto any_subscription_callback_to_test = GetParam().get_any_subscription_callback_to_test(); \ + any_subscription_callback_to_test.dispatch(msg_shared_ptr_, message_info_); \ + } \ + \ + /* Testing dispatch with shared_ptr as input */ \ + TEST_P(DispatchTests_name, test_intra_shared_dispatch) { \ + auto any_subscription_callback_to_test = GetParam().get_any_subscription_callback_to_test(); \ + any_subscription_callback_to_test.dispatch_intra_process(msg_shared_ptr_, message_info_); \ + } \ + \ + /* Testing dispatch with unique_ptr as input */ \ + TEST_P(DispatchTests_name, test_intra_unique_dispatch) { \ + auto any_subscription_callback_to_test = GetParam().get_any_subscription_callback_to_test(); \ + any_subscription_callback_to_test.dispatch_intra_process(get_unique_ptr_msg(), message_info_); \ + } - any_subscription_callback_.set(unique_ptr_callback); +PARAMETERIZED_TESTS(DispatchTests) +PARAMETERIZED_TESTS(DispatchTestsWithTA) - // Message is copied into unique_ptr - EXPECT_NO_THROW(any_subscription_callback_.dispatch(msg_shared_ptr_, message_info_)); - EXPECT_EQ(callback_count, 1); +// Generic classes for testing callbacks using std::bind to class methods. +template +class BindContextImpl : public InstanceContextImpl +{ + static constexpr size_t number_of_callback_args{sizeof...(CallbackArgs)}; - EXPECT_THROW( - any_subscription_callback_.dispatch_intra_process(msg_const_shared_ptr_, message_info_), - std::runtime_error); - EXPECT_EQ(callback_count, 1); +public: + using InstanceContextImpl::InstanceContextImpl; + virtual ~BindContextImpl() = default; - // Unique_ptr is_moved - EXPECT_NO_THROW( - any_subscription_callback_.dispatch_intra_process(std::move(msg_unique_ptr_), message_info_)); - EXPECT_EQ(callback_count, 2); -} + void on_message(CallbackArgs ...) const {} -TEST_F(TestAnySubscriptionCallback, set_dispatch_unique_ptr_w_info) { - int callback_count = 0; - auto unique_ptr_callback = [&callback_count]( - std::unique_ptr, const rclcpp::MessageInfo &) { - callback_count++; - }; + rclcpp::AnySubscriptionCallback + get_any_subscription_callback_to_test() const override + { + if constexpr (number_of_callback_args == 1) { + return rclcpp::AnySubscriptionCallback().set( + std::bind(&BindContextImpl::on_message, this, std::placeholders::_1) + ); + } else { + return rclcpp::AnySubscriptionCallback().set( + std::bind(&BindContextImpl::on_message, this, std::placeholders::_1, std::placeholders::_2) + ); + } + } +}; - any_subscription_callback_.set(unique_ptr_callback); +template +class BindContext : public InstanceContext +{ +public: + explicit BindContext(const std::string & name) + : InstanceContext(name, std::make_shared>()) + {} +}; - // Message is copied into unique_ptr - EXPECT_NO_THROW(any_subscription_callback_.dispatch(msg_shared_ptr_, message_info_)); - EXPECT_EQ(callback_count, 1); +// +// Versions of `const MessageT &` +// +void const_ref_free_func(const test_msgs::msg::Empty &) {} +void const_ref_w_info_free_func(const test_msgs::msg::Empty &, const rclcpp::MessageInfo &) {} + +INSTANTIATE_TEST_SUITE_P( + ConstRefCallbackTests, + DispatchTests, + ::testing::Values( + // lambda + InstanceContext{"lambda", rclcpp::AnySubscriptionCallback().set( + [](const test_msgs::msg::Empty &) {})}, + InstanceContext{"lambda_with_info", + rclcpp::AnySubscriptionCallback().set( + [](const test_msgs::msg::Empty &, const rclcpp::MessageInfo &) {})}, + // free function + InstanceContext{"free_function", rclcpp::AnySubscriptionCallback().set( + const_ref_free_func)}, + InstanceContext{"free_function_with_info", + rclcpp::AnySubscriptionCallback().set( + const_ref_w_info_free_func)}, + // bind function + BindContext("bind_method"), + BindContext( + "bind_method_with_info") + ), + format_parameter +); + +void const_ref_ta_free_func(const MyEmpty &) {} +void const_ref_ta_w_info_free_func(const MyEmpty &, const rclcpp::MessageInfo &) {} + +INSTANTIATE_TEST_SUITE_P( + ConstRefTACallbackTests, + DispatchTestsWithTA, + ::testing::Values( + // lambda + InstanceContext{"lambda_ta", rclcpp::AnySubscriptionCallback().set( + [](const MyEmpty &) {})}, + InstanceContext{"lambda_ta_with_info", + rclcpp::AnySubscriptionCallback().set( + [](const MyEmpty &, const rclcpp::MessageInfo &) {})}, + InstanceContext{"lambda", rclcpp::AnySubscriptionCallback().set( + [](const test_msgs::msg::Empty &) {})}, + InstanceContext{"lambda_with_info", + rclcpp::AnySubscriptionCallback().set( + [](const test_msgs::msg::Empty &, const rclcpp::MessageInfo &) {})}, + // free function + InstanceContext{"free_function_ta", rclcpp::AnySubscriptionCallback().set( + const_ref_ta_free_func)}, + InstanceContext{"free_function_ta_with_info", + rclcpp::AnySubscriptionCallback().set( + const_ref_ta_w_info_free_func)}, + InstanceContext{"free_function", rclcpp::AnySubscriptionCallback().set( + const_ref_free_func)}, + InstanceContext{"free_function_with_info", + rclcpp::AnySubscriptionCallback().set( + const_ref_w_info_free_func)}, + // bind function + BindContext("bind_method_ta"), + BindContext( + "bind_method_ta_with_info"), + BindContext("bind_method"), + BindContext( + "bind_method_with_info") + ), + format_parameter_with_ta +); - EXPECT_THROW( - any_subscription_callback_.dispatch_intra_process(msg_const_shared_ptr_, message_info_), - std::runtime_error); - EXPECT_EQ(callback_count, 1); +// +// Versions of `std::unique_ptr` +// +void unique_ptr_free_func(std::unique_ptr) {} +void unique_ptr_w_info_free_func( + std::unique_ptr, const rclcpp::MessageInfo &) +{} + +INSTANTIATE_TEST_SUITE_P( + UniquePtrCallbackTests, + DispatchTests, + ::testing::Values( + // lambda + InstanceContext{"lambda", rclcpp::AnySubscriptionCallback().set( + [](std::unique_ptr) {})}, + InstanceContext{"lambda_with_info", + rclcpp::AnySubscriptionCallback().set( + [](std::unique_ptr, const rclcpp::MessageInfo &) {})}, + // free function + InstanceContext{"free_function", rclcpp::AnySubscriptionCallback().set( + unique_ptr_free_func)}, + InstanceContext{"free_function_with_info", + rclcpp::AnySubscriptionCallback().set( + unique_ptr_w_info_free_func)}, + // bind function + BindContext>("bind_method"), + BindContext< + test_msgs::msg::Empty, + std::unique_ptr, + const rclcpp::MessageInfo & + >("bind_method_with_info") + ), + format_parameter +); + +void unique_ptr_ta_free_func(std::unique_ptr) {} +void unique_ptr_ta_w_info_free_func(std::unique_ptr, const rclcpp::MessageInfo &) {} + +INSTANTIATE_TEST_SUITE_P( + UniquePtrCallbackTests, + DispatchTestsWithTA, + ::testing::Values( + // lambda + InstanceContext{"lambda_ta", rclcpp::AnySubscriptionCallback().set( + [](std::unique_ptr) {})}, + InstanceContext{"lambda_ta_with_info", + rclcpp::AnySubscriptionCallback().set( + [](std::unique_ptr, const rclcpp::MessageInfo &) {})}, + InstanceContext{"lambda", rclcpp::AnySubscriptionCallback().set( + [](std::unique_ptr) {})}, + InstanceContext{"lambda_with_info", + rclcpp::AnySubscriptionCallback().set( + [](std::unique_ptr, const rclcpp::MessageInfo &) {})}, + // free function + InstanceContext{"free_function_ta", rclcpp::AnySubscriptionCallback().set( + unique_ptr_ta_free_func)}, + InstanceContext{"free_function_ta_with_info", + rclcpp::AnySubscriptionCallback().set( + unique_ptr_ta_w_info_free_func)}, + InstanceContext{"free_function", rclcpp::AnySubscriptionCallback().set( + unique_ptr_free_func)}, + InstanceContext{"free_function_with_info", + rclcpp::AnySubscriptionCallback().set( + unique_ptr_w_info_free_func)}, + // bind function + BindContext>("bind_method_ta"), + BindContext, const rclcpp::MessageInfo &>( + "bind_method_ta_with_info"), + BindContext>("bind_method"), + BindContext, const rclcpp::MessageInfo &>( + "bind_method_with_info") + ), + format_parameter_with_ta +); - // Unique_ptr is_moved - EXPECT_NO_THROW( - any_subscription_callback_.dispatch_intra_process(std::move(msg_unique_ptr_), message_info_)); - EXPECT_EQ(callback_count, 2); -} +// +// Versions of `std::shared_ptr` +// +void shared_const_ptr_free_func(std::shared_ptr) {} +void shared_const_ptr_w_info_free_func( + std::shared_ptr, const rclcpp::MessageInfo &) +{} + +INSTANTIATE_TEST_SUITE_P( + SharedConstPtrCallbackTests, + DispatchTests, + ::testing::Values( + // lambda + InstanceContext{"lambda", rclcpp::AnySubscriptionCallback().set( + [](std::shared_ptr) {})}, + InstanceContext{"lambda_with_info", + rclcpp::AnySubscriptionCallback().set( + [](std::shared_ptr, const rclcpp::MessageInfo &) {})}, + // free function + InstanceContext{"free_function", rclcpp::AnySubscriptionCallback().set( + shared_const_ptr_free_func)}, + InstanceContext{"free_function_with_info", + rclcpp::AnySubscriptionCallback().set( + shared_const_ptr_w_info_free_func)}, + // bind function + BindContext>("bind_method"), + BindContext, + const rclcpp::MessageInfo &>( + "bind_method_with_info") + ), + format_parameter +); + +void shared_const_ptr_ta_free_func(std::shared_ptr) {} +void shared_const_ptr_ta_w_info_free_func( + std::shared_ptr, const rclcpp::MessageInfo &) +{} + +INSTANTIATE_TEST_SUITE_P( + SharedConstPtrCallbackTests, + DispatchTestsWithTA, + ::testing::Values( + // lambda + InstanceContext{"lambda_ta", rclcpp::AnySubscriptionCallback().set( + [](std::shared_ptr) {})}, + InstanceContext{"lambda_ta_with_info", + rclcpp::AnySubscriptionCallback().set( + [](std::shared_ptr, const rclcpp::MessageInfo &) {})}, + InstanceContext{"lambda", rclcpp::AnySubscriptionCallback().set( + [](std::shared_ptr) {})}, + InstanceContext{"lambda_with_info", + rclcpp::AnySubscriptionCallback().set( + [](std::shared_ptr, const rclcpp::MessageInfo &) {})}, + // free function + InstanceContext{"free_function_ta", rclcpp::AnySubscriptionCallback().set( + shared_const_ptr_ta_free_func)}, + InstanceContext{"free_function_ta_with_info", + rclcpp::AnySubscriptionCallback().set( + shared_const_ptr_ta_w_info_free_func)}, + InstanceContext{"free_function", rclcpp::AnySubscriptionCallback().set( + shared_const_ptr_free_func)}, + InstanceContext{"free_function_with_info", + rclcpp::AnySubscriptionCallback().set( + shared_const_ptr_w_info_free_func)}, + // bind function + BindContext>("bind_method_ta"), + BindContext, const rclcpp::MessageInfo &>( + "bind_method_ta_with_info"), + BindContext>("bind_method"), + BindContext, const rclcpp::MessageInfo &>( + "bind_method_with_info") + ), + format_parameter_with_ta +); + +// +// Versions of `const std::shared_ptr &` +// +void const_ref_shared_const_ptr_free_func(const std::shared_ptr &) {} +void const_ref_shared_const_ptr_w_info_free_func( + const std::shared_ptr &, const rclcpp::MessageInfo &) +{} + +INSTANTIATE_TEST_SUITE_P( + ConstRefSharedConstPtrCallbackTests, + DispatchTests, + ::testing::Values( + // lambda + InstanceContext{"lambda", rclcpp::AnySubscriptionCallback().set( + [](const std::shared_ptr &) {})}, + InstanceContext{"lambda_with_info", + rclcpp::AnySubscriptionCallback().set( + [](const std::shared_ptr &, const rclcpp::MessageInfo &) {})}, + // free function + InstanceContext{"free_function", rclcpp::AnySubscriptionCallback().set( + const_ref_shared_const_ptr_free_func)}, + InstanceContext{"free_function_with_info", + rclcpp::AnySubscriptionCallback().set( + const_ref_shared_const_ptr_w_info_free_func)}, + // bind function + BindContext &>("bind_method"), + BindContext &, + const rclcpp::MessageInfo &>( + "bind_method_with_info") + ), + format_parameter +); + +void const_ref_shared_const_ptr_ta_free_func(const std::shared_ptr &) {} +void const_ref_shared_const_ptr_ta_w_info_free_func( + const std::shared_ptr &, const rclcpp::MessageInfo &) +{} + +INSTANTIATE_TEST_SUITE_P( + ConstRefSharedConstPtrCallbackTests, + DispatchTestsWithTA, + ::testing::Values( + // lambda + InstanceContext{"lambda_ta", rclcpp::AnySubscriptionCallback().set( + [](const std::shared_ptr &) {})}, + InstanceContext{"lambda_ta_with_info", + rclcpp::AnySubscriptionCallback().set( + [](const std::shared_ptr &, const rclcpp::MessageInfo &) {})}, + InstanceContext{"lambda", rclcpp::AnySubscriptionCallback().set( + [](const std::shared_ptr &) {})}, + InstanceContext{"lambda_with_info", + rclcpp::AnySubscriptionCallback().set( + [](const std::shared_ptr &, const rclcpp::MessageInfo &) {})}, + // free function + InstanceContext{"free_function_ta", rclcpp::AnySubscriptionCallback().set( + const_ref_shared_const_ptr_ta_free_func)}, + InstanceContext{"free_function_ta_with_info", + rclcpp::AnySubscriptionCallback().set( + const_ref_shared_const_ptr_ta_w_info_free_func)}, + InstanceContext{"free_function", rclcpp::AnySubscriptionCallback().set( + const_ref_shared_const_ptr_free_func)}, + InstanceContext{"free_function_with_info", + rclcpp::AnySubscriptionCallback().set( + const_ref_shared_const_ptr_w_info_free_func)}, + // bind function + BindContext &>("bind_method_ta"), + BindContext &, const rclcpp::MessageInfo &>( + "bind_method_ta_with_info"), + BindContext &>("bind_method"), + BindContext &, + const rclcpp::MessageInfo &>( + "bind_method_with_info") + ), + format_parameter_with_ta +); + +// +// Versions of `std::shared_ptr` +// +void shared_ptr_free_func(std::shared_ptr) {} +void shared_ptr_w_info_free_func( + std::shared_ptr, const rclcpp::MessageInfo &) +{} + +INSTANTIATE_TEST_SUITE_P( + SharedPtrCallbackTests, + DispatchTests, + ::testing::Values( + // lambda + InstanceContext{"lambda", rclcpp::AnySubscriptionCallback().set( + [](std::shared_ptr) {})}, + InstanceContext{"lambda_with_info", + rclcpp::AnySubscriptionCallback().set( + [](std::shared_ptr, const rclcpp::MessageInfo &) {})}, + // free function + InstanceContext{"free_function", rclcpp::AnySubscriptionCallback().set( + shared_ptr_free_func)}, + InstanceContext{"free_function_with_info", + rclcpp::AnySubscriptionCallback().set( + shared_ptr_w_info_free_func)}, + // bind function + BindContext>("bind_method"), + BindContext, + const rclcpp::MessageInfo &>( + "bind_method_with_info") + ), + format_parameter +); + +void shared_ptr_ta_free_func(std::shared_ptr) {} +void shared_ptr_ta_w_info_free_func( + std::shared_ptr, const rclcpp::MessageInfo &) +{} + +INSTANTIATE_TEST_SUITE_P( + SharedPtrCallbackTests, + DispatchTestsWithTA, + ::testing::Values( + // lambda + InstanceContext{"lambda_ta", rclcpp::AnySubscriptionCallback().set( + [](std::shared_ptr) {})}, + InstanceContext{"lambda_ta_with_info", + rclcpp::AnySubscriptionCallback().set( + [](std::shared_ptr, const rclcpp::MessageInfo &) {})}, + InstanceContext{"lambda", rclcpp::AnySubscriptionCallback().set( + [](std::shared_ptr) {})}, + InstanceContext{"lambda_with_info", + rclcpp::AnySubscriptionCallback().set( + [](std::shared_ptr, const rclcpp::MessageInfo &) {})}, + // free function + InstanceContext{"free_function_ta", rclcpp::AnySubscriptionCallback().set( + shared_ptr_ta_free_func)}, + InstanceContext{"free_function_ta_with_info", + rclcpp::AnySubscriptionCallback().set( + shared_ptr_ta_w_info_free_func)}, + InstanceContext{"free_function", rclcpp::AnySubscriptionCallback().set( + shared_ptr_free_func)}, + InstanceContext{"free_function_with_info", + rclcpp::AnySubscriptionCallback().set( + shared_ptr_w_info_free_func)}, + // bind function + BindContext>("bind_method_ta"), + BindContext, const rclcpp::MessageInfo &>( + "bind_method_ta_with_info"), + BindContext>("bind_method"), + BindContext, const rclcpp::MessageInfo &>( + "bind_method_with_info") + ), + format_parameter_with_ta +); diff --git a/rclcpp/test/rclcpp/test_client.cpp b/rclcpp/test/rclcpp/test_client.cpp index ea64340f03..cde036a762 100644 --- a/rclcpp/test/rclcpp/test_client.cpp +++ b/rclcpp/test/rclcpp/test_client.cpp @@ -27,6 +27,8 @@ #include "test_msgs/srv/empty.hpp" +using namespace std::chrono_literals; + class TestClient : public ::testing::Test { protected: @@ -328,3 +330,92 @@ TEST_F(TestClientWithServer, take_response) { 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"); + + auto client = client_node->create_client("test_service"); + 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); + 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 == 0 && std::chrono::steady_clock::now() - start < 10s); + + EXPECT_EQ(c1.load(), 1u); + EXPECT_EQ(c2.load(), 1u); + EXPECT_EQ(c3.load(), 3u); + + std::function invalid_cb = nullptr; + EXPECT_THROW(client->set_on_new_response_callback(invalid_cb), std::invalid_argument); +} diff --git a/rclcpp/test/rclcpp/test_create_subscription.cpp b/rclcpp/test/rclcpp/test_create_subscription.cpp index 401e6c33ed..fd947485e2 100644 --- a/rclcpp/test/rclcpp/test_create_subscription.cpp +++ b/rclcpp/test/rclcpp/test_create_subscription.cpp @@ -42,7 +42,7 @@ TEST_F(TestCreateSubscription, create) { auto node = std::make_shared("my_node", "/ns"); const rclcpp::QoS qos(10); auto options = rclcpp::SubscriptionOptions(); - auto callback = [](const test_msgs::msg::Empty::SharedPtr) {}; + auto callback = [](test_msgs::msg::Empty::ConstSharedPtr) {}; auto subscription = rclcpp::create_subscription(node, "topic_name", qos, callback, options); @@ -55,7 +55,7 @@ TEST_F(TestCreateSubscription, create_with_overriding_options) { const rclcpp::QoS qos(10); auto options = rclcpp::SubscriptionOptions(); options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); - auto callback = [](const test_msgs::msg::Empty::SharedPtr) {}; + auto callback = [](test_msgs::msg::Empty::ConstSharedPtr) {}; auto subscription = rclcpp::create_subscription(node, "topic_name", qos, callback, options); @@ -67,7 +67,7 @@ TEST_F(TestCreateSubscription, create_separated_node_topics_and_parameters) { auto node = std::make_shared("my_node", "/ns"); const rclcpp::QoS qos(10); auto options = rclcpp::SubscriptionOptions(); - auto callback = [](const test_msgs::msg::Empty::SharedPtr) {}; + auto callback = [](test_msgs::msg::Empty::ConstSharedPtr) {}; auto node_parameters = node->get_node_parameters_interface(); auto node_topics = node->get_node_topics_interface(); @@ -86,7 +86,7 @@ TEST_F(TestCreateSubscription, create_with_statistics) { options.topic_stats_options.publish_topic = "topic_statistics"; options.topic_stats_options.publish_period = 5min; - auto callback = [](const test_msgs::msg::Empty::SharedPtr) {}; + auto callback = [](test_msgs::msg::Empty::ConstSharedPtr) {}; auto subscription = rclcpp::create_subscription(node, "topic_name", qos, callback, options); diff --git a/rclcpp/test/rclcpp/test_duration.cpp b/rclcpp/test/rclcpp/test_duration.cpp index 0d349d0d12..319ce06601 100644 --- a/rclcpp/test/rclcpp/test_duration.cpp +++ b/rclcpp/test/rclcpp/test_duration.cpp @@ -138,6 +138,7 @@ TEST_F(TestDuration, maximum_duration) { static const int64_t HALF_SEC_IN_NS = 500 * 1000 * 1000; static const int64_t ONE_SEC_IN_NS = 1000 * 1000 * 1000; static const int64_t ONE_AND_HALF_SEC_IN_NS = 3 * HALF_SEC_IN_NS; +static const int64_t MAX_NANOSECONDS = std::numeric_limits::max(); TEST_F(TestDuration, from_seconds) { EXPECT_EQ(rclcpp::Duration(0ns), rclcpp::Duration::from_seconds(0.0)); @@ -267,6 +268,34 @@ TEST_F(TestDuration, conversions) { auto chrono_duration = duration.to_chrono(); EXPECT_EQ(chrono_duration.count(), -ONE_AND_HALF_SEC_IN_NS); } + + { + auto duration = rclcpp::Duration::from_nanoseconds(MAX_NANOSECONDS); + + const auto duration_msg = static_cast(duration); + EXPECT_EQ(duration_msg.sec, std::numeric_limits::max()); + EXPECT_EQ(duration_msg.nanosec, std::numeric_limits::max()); + + auto rmw_time = duration.to_rmw_time(); + EXPECT_EQ(rmw_time.sec, 9223372036u); + EXPECT_EQ(rmw_time.nsec, 854775807u); + + auto chrono_duration = duration.to_chrono(); + EXPECT_EQ(chrono_duration.count(), MAX_NANOSECONDS); + } + + { + auto duration = rclcpp::Duration::from_nanoseconds(-MAX_NANOSECONDS); + + const auto duration_msg = static_cast(duration); + EXPECT_EQ(duration_msg.sec, std::numeric_limits::min()); + EXPECT_EQ(duration_msg.nanosec, 0u); + + EXPECT_THROW(duration.to_rmw_time(), std::runtime_error); + + auto chrono_duration = duration.to_chrono(); + EXPECT_EQ(chrono_duration.count(), -MAX_NANOSECONDS); + } } TEST_F(TestDuration, test_some_constructors) { diff --git a/rclcpp/test/rclcpp/test_executor.cpp b/rclcpp/test/rclcpp/test_executor.cpp index fce0369edd..d5818a697f 100644 --- a/rclcpp/test/rclcpp/test_executor.cpp +++ b/rclcpp/test/rclcpp/test_executor.cpp @@ -155,7 +155,7 @@ TEST_F(TestExecutor, add_callback_group_failed_trigger_guard_condition) { "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); RCLCPP_EXPECT_THROW_EQ( dummy.add_callback_group(cb_group, node->get_node_base_interface(), true), - std::runtime_error("Failed to trigger guard condition on callback group add: error not set")); + std::runtime_error("error not set")); } TEST_F(TestExecutor, remove_callback_group_null_node) { @@ -186,7 +186,7 @@ TEST_F(TestExecutor, remove_callback_group_failed_trigger_guard_condition) { RCLCPP_EXPECT_THROW_EQ( dummy.remove_callback_group(cb_group, true), std::runtime_error( - "Failed to trigger guard condition on callback group remove: error not set")); + "error not set")); } TEST_F(TestExecutor, remove_node_not_associated) { @@ -325,7 +325,7 @@ TEST_F(TestExecutor, cancel_failed_trigger_guard_condition) { "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); RCLCPP_EXPECT_THROW_EQ( dummy.cancel(), - std::runtime_error("Failed to trigger guard condition in cancel: error not set")); + std::runtime_error("error not set")); } TEST_F(TestExecutor, set_memory_strategy_nullptr) { @@ -360,7 +360,7 @@ TEST_F(TestExecutor, spin_once_failed_trigger_guard_condition) { 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")); + "error not set")); } TEST_F(TestExecutor, spin_some_fail_wait_set_clear) { diff --git a/rclcpp/test/rclcpp/test_generic_pubsub.cpp b/rclcpp/test/rclcpp/test_generic_pubsub.cpp new file mode 100644 index 0000000000..71a60a77fb --- /dev/null +++ b/rclcpp/test/rclcpp/test_generic_pubsub.cpp @@ -0,0 +1,196 @@ +// Copyright 2018, Bosch Software Innovations GmbH. +// Copyright 2021, Apex.AI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include + +#include "test_msgs/message_fixtures.hpp" +#include "test_msgs/msg/basic_types.hpp" + +#include "rcl/graph.h" + +#include "rclcpp/generic_publisher.hpp" +#include "rclcpp/generic_subscription.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/serialization.hpp" +#include "rclcpp/serialized_message.hpp" + +using namespace ::testing; // NOLINT +using namespace rclcpp; // NOLINT + +class RclcppGenericNodeFixture : public Test +{ +public: + RclcppGenericNodeFixture() + { + node_ = std::make_shared("pubsub"); + publisher_node_ = std::make_shared( + "publisher_node", + rclcpp::NodeOptions().start_parameter_event_publisher(false)); + } + + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void create_publisher(const std::string & topic) + { + auto publisher = publisher_node_->create_publisher(topic, 10); + publishers_.push_back(publisher); + } + + std::vector subscribe_raw_messages( + size_t expected_messages_number, const std::string & topic_name, const std::string & type) + { + std::vector messages; + size_t counter = 0; + auto subscription = node_->create_generic_subscription( + topic_name, type, rclcpp::QoS(1), + [&counter, &messages](std::shared_ptr message) { + test_msgs::msg::Strings string_message; + rclcpp::Serialization serializer; + serializer.deserialize_message(message.get(), &string_message); + messages.push_back(string_message.string_value); + counter++; + }); + + while (counter < expected_messages_number) { + rclcpp::spin_some(node_); + } + return messages; + } + + rclcpp::SerializedMessage serialize_string_message(const std::string & message) + { + test_msgs::msg::Strings string_message; + string_message.string_value = message; + rclcpp::Serialization ser; + SerializedMessage result; + ser.serialize_message(&string_message, &result); + return result; + } + + void sleep_to_allow_topics_discovery() + { + // This is a short sleep to allow the node some time to discover the topic + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + } + + template + bool wait_for(const Condition & condition, const Duration & timeout) + { + using clock = std::chrono::system_clock; + auto start = clock::now(); + while (!condition()) { + if ((clock::now() - start) > timeout) { + return false; + } + rclcpp::spin_some(node_); + } + return true; + } + + std::shared_ptr node_; + rclcpp::Node::SharedPtr publisher_node_; + std::vector> publishers_; +}; + + +TEST_F(RclcppGenericNodeFixture, publisher_and_subscriber_work) +{ + // We currently publish more messages because they can get lost + std::vector test_messages = {"Hello World", "Hello World"}; + std::string topic_name = "/string_topic"; + std::string type = "test_msgs/msg/Strings"; + + auto publisher = node_->create_generic_publisher( + topic_name, type, rclcpp::QoS(1)); + + auto subscriber_future_ = std::async( + std::launch::async, [this, topic_name, type] { + return subscribe_raw_messages(1, topic_name, type); + }); + + // TODO(karsten1987): Port 'wait_for_sub' to rclcpp + auto allocator = node_->get_node_options().allocator(); + auto success = false; + auto ret = rcl_wait_for_subscribers( + node_->get_node_base_interface()->get_rcl_node_handle(), + &allocator, + topic_name.c_str(), + 1u, + static_cast(1e9), + &success); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_TRUE(success); + + for (const auto & message : test_messages) { + publisher->publish(serialize_string_message(message)); + } + + auto subscribed_messages = subscriber_future_.get(); + EXPECT_THAT(subscribed_messages, SizeIs(Not(0))); + EXPECT_THAT(subscribed_messages[0], StrEq("Hello World")); +} + +TEST_F(RclcppGenericNodeFixture, generic_subscription_uses_qos) +{ + // If the GenericSubscription does not use the provided QoS profile, + // its request will be incompatible with the Publisher's offer and no messages will be passed. + using namespace std::chrono_literals; + std::string topic_name = "string_topic"; + std::string topic_type = "test_msgs/msg/Strings"; + rclcpp::QoS qos = rclcpp::SensorDataQoS(); + + auto publisher = node_->create_publisher(topic_name, qos); + auto subscription = node_->create_generic_subscription( + topic_name, topic_type, qos, + [](std::shared_ptr/* message */) {}); + auto connected = [publisher, subscription]() -> bool { + return publisher->get_subscription_count() && subscription->get_publisher_count(); + }; + // It normally takes < 20ms, 5s chosen as "a very long time" + ASSERT_TRUE(wait_for(connected, 5s)); +} + +TEST_F(RclcppGenericNodeFixture, generic_publisher_uses_qos) +{ + // If the GenericPublisher does not use the provided QoS profile, + // its offer will be incompatible with the Subscription's request and no messages will be passed. + using namespace std::chrono_literals; + std::string topic_name = "string_topic"; + std::string topic_type = "test_msgs/msg/Strings"; + rclcpp::QoS qos = rclcpp::QoS(1).transient_local(); + + auto publisher = node_->create_generic_publisher(topic_name, topic_type, qos); + auto subscription = node_->create_subscription( + topic_name, qos, + [](std::shared_ptr/* message */) {}); + auto connected = [publisher, subscription]() -> bool { + return publisher->get_subscription_count() && subscription->get_publisher_count(); + }; + // It normally takes < 20ms, 5s chosen as "a very long time" + ASSERT_TRUE(wait_for(connected, 5s)); +} diff --git a/rclcpp/test/rclcpp/test_graph_listener.cpp b/rclcpp/test/rclcpp/test_graph_listener.cpp index bd66c686d7..24c370e65d 100644 --- a/rclcpp/test/rclcpp/test_graph_listener.cpp +++ b/rclcpp/test/rclcpp/test_graph_listener.cpp @@ -87,7 +87,7 @@ TEST_F(TestGraphListener, error_construct_graph_listener) { auto graph_listener_error = std::make_shared(get_global_default_context()); graph_listener_error.reset(); - }, std::runtime_error("failed to create interrupt guard condition: error not set")); + }, std::runtime_error("failed to create guard condition: error not set")); } // Required for mocking_utils below @@ -169,7 +169,7 @@ TEST_F(TestGraphListener, error_run_graph_listener_mock_wait_set_add_guard_condi "lib:rclcpp", rcl_wait_set_add_guard_condition, RCL_RET_ERROR); RCLCPP_EXPECT_THROW_EQ( graph_listener_test->run_protected(), - std::runtime_error("failed to add interrupt guard condition to wait set: error not set")); + std::runtime_error("failed to add guard condition to wait set: error not set")); } TEST_F(TestGraphListener, error_run_graph_listener_mock_wait_error) { @@ -292,9 +292,7 @@ TEST_F(TestGraphListener, test_graph_listener_shutdown_guard_fini_error_throw) { auto mock_wait_set_fini = mocking_utils::patch_and_return( "lib:rclcpp", rcl_guard_condition_fini, RCL_RET_ERROR); - RCLCPP_EXPECT_THROW_EQ( - graph_listener_test->shutdown(), - std::runtime_error("failed to finalize interrupt guard condition: error not set")); + EXPECT_NO_THROW(graph_listener_test->shutdown()); graph_listener_test->mock_cleanup_wait_set(); } diff --git a/rclcpp/test/rclcpp/test_init.cpp b/rclcpp/test/rclcpp/test_init.cpp deleted file mode 100644 index bee35f3992..0000000000 --- a/rclcpp/test/rclcpp/test_init.cpp +++ /dev/null @@ -1,66 +0,0 @@ -// Copyright 2018 Open Source Robotics Foundation, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include - -#include "rclcpp/exceptions.hpp" -#include "rclcpp/utilities.hpp" - -#if !defined(_WIN32) -# pragma GCC diagnostic push -# pragma GCC diagnostic ignored "-Wdeprecated-declarations" -#else // !defined(_WIN32) -# pragma warning(push) -# pragma warning(disable: 4996) -#endif - -TEST(TestInit, is_initialized) { - EXPECT_FALSE(rclcpp::is_initialized()); - - rclcpp::init(0, nullptr); - - EXPECT_TRUE(rclcpp::is_initialized()); - - rclcpp::shutdown(); - - EXPECT_FALSE(rclcpp::is_initialized()); -} - -TEST(TestInit, initialize_with_ros_args) { - EXPECT_FALSE(rclcpp::is_initialized()); - - rclcpp::init(0, nullptr); - - EXPECT_TRUE(rclcpp::is_initialized()); - - rclcpp::shutdown(); - - EXPECT_FALSE(rclcpp::is_initialized()); -} - -TEST(TestInit, initialize_with_unknown_ros_args) { - EXPECT_FALSE(rclcpp::is_initialized()); - - const char * const argv[] = {"--ros-args", "unknown"}; - const int argc = static_cast(sizeof(argv) / sizeof(const char *)); - EXPECT_THROW({rclcpp::init(argc, argv);}, rclcpp::exceptions::UnknownROSArgsError); - - EXPECT_FALSE(rclcpp::is_initialized()); -} - -#if !defined(_WIN32) -# pragma GCC diagnostic pop -#else // !defined(_WIN32) -# pragma warning(pop) -#endif diff --git a/rclcpp/test/rclcpp/test_intra_process_manager.cpp b/rclcpp/test/rclcpp/test_intra_process_manager.cpp index b71fb1d061..00350edeb0 100644 --- a/rclcpp/test/rclcpp/test_intra_process_manager.cpp +++ b/rclcpp/test/rclcpp/test_intra_process_manager.cpp @@ -136,7 +136,10 @@ namespace buffers { namespace mock { -template +template< + typename MessageT, + typename Alloc = std::allocator, + typename MessageDeleter = std::default_delete> class IntraProcessBuffer { public: @@ -216,13 +219,16 @@ class SubscriptionIntraProcessBase const char * topic_name; }; -template -class SubscriptionIntraProcess : public SubscriptionIntraProcessBase +template< + typename MessageT, + typename Alloc = std::allocator, + typename Deleter = std::default_delete> +class SubscriptionIntraProcessBuffer : public SubscriptionIntraProcessBase { public: - RCLCPP_SMART_PTR_DEFINITIONS(SubscriptionIntraProcess) + RCLCPP_SMART_PTR_DEFINITIONS(SubscriptionIntraProcessBuffer) - SubscriptionIntraProcess() + SubscriptionIntraProcessBuffer() : take_shared_method(false) { buffer = std::make_unique>(); @@ -259,6 +265,25 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBase typename rclcpp::experimental::buffers::mock::IntraProcessBuffer::UniquePtr buffer; }; +template< + typename MessageT, + typename Alloc = std::allocator, + typename Deleter = std::default_delete> +class SubscriptionIntraProcess : public SubscriptionIntraProcessBuffer< + MessageT, + Alloc, + Deleter +> +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(SubscriptionIntraProcess) + + SubscriptionIntraProcess() + : SubscriptionIntraProcessBuffer() + { + } +}; + } // namespace mock } // namespace experimental } // namespace rclcpp @@ -267,12 +292,14 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBase #define RCLCPP__PUBLISHER_HPP_ #define RCLCPP__PUBLISHER_BASE_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 IntraProcessBuffer mock::IntraProcessBuffer #define SubscriptionIntraProcessBase mock::SubscriptionIntraProcessBase +#define SubscriptionIntraProcessBuffer mock::SubscriptionIntraProcessBuffer #define SubscriptionIntraProcess mock::SubscriptionIntraProcess #include "../src/rclcpp/intra_process_manager.cpp" #undef Publisher @@ -304,7 +331,7 @@ void Publisher::publish(MessageUniquePtr msg) ipm->template do_intra_process_publish( intra_process_publisher_id_, std::move(msg), - message_allocator_); + *message_allocator_); } } // namespace mock diff --git a/rclcpp/test/rclcpp/test_intra_process_manager_with_allocators.cpp b/rclcpp/test/rclcpp/test_intra_process_manager_with_allocators.cpp new file mode 100644 index 0000000000..6d192ca86b --- /dev/null +++ b/rclcpp/test/rclcpp/test_intra_process_manager_with_allocators.cpp @@ -0,0 +1,303 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include +#include + +#include "test_msgs/msg/empty.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/allocator/allocator_common.hpp" +#include "rclcpp/strategies/allocator_memory_strategy.hpp" + +// For demonstration purposes only, not necessary for allocator_traits +static uint32_t num_allocs = 0; +static uint32_t num_deallocs = 0; +// A very simple custom allocator. Counts calls to allocate and deallocate. +template +struct MyAllocator +{ +public: + using value_type = T; + using size_type = std::size_t; + using pointer = T *; + using const_pointer = const T *; + using difference_type = typename std::pointer_traits::difference_type; + + MyAllocator() noexcept + { + } + + ~MyAllocator() noexcept {} + + template + MyAllocator(const MyAllocator &) noexcept + { + } + + T * allocate(size_t size, const void * = 0) + { + if (size == 0) { + return nullptr; + } + num_allocs++; + return static_cast(std::malloc(size * sizeof(T))); + } + + void deallocate(T * ptr, size_t size) + { + (void)size; + if (!ptr) { + return; + } + num_deallocs++; + std::free(ptr); + } + + template + struct rebind + { + typedef MyAllocator other; + }; +}; + +// Explicit specialization for void +template<> +struct MyAllocator +{ +public: + using value_type = void; + using pointer = void *; + using const_pointer = const void *; + + MyAllocator() noexcept + { + } + + ~MyAllocator() noexcept {} + + template + MyAllocator(const MyAllocator &) noexcept + { + } + + template + struct rebind + { + typedef MyAllocator other; + }; +}; + +template +constexpr bool operator==( + const MyAllocator &, + const MyAllocator &) noexcept +{ + return true; +} + +template +constexpr bool operator!=( + const MyAllocator &, + const MyAllocator &) noexcept +{ + return false; +} + +template< + typename PublishedMessageAllocatorT, + typename PublisherAllocatorT, + typename SubscribedMessageAllocatorT, + typename SubscriptionAllocatorT, + typename MessageMemoryStrategyAllocatorT, + typename MemoryStrategyAllocatorT, + typename ExpectedExceptionT +> +void +do_custom_allocator_test( + PublishedMessageAllocatorT published_message_allocator, + PublisherAllocatorT publisher_allocator, + SubscribedMessageAllocatorT /* subscribed_message_allocator */, // intentionally unused + SubscriptionAllocatorT subscription_allocator, + MessageMemoryStrategyAllocatorT message_memory_strategy, + MemoryStrategyAllocatorT memory_strategy_allocator) +{ + using PublishedMessageAllocTraits = + rclcpp::allocator::AllocRebind; + using PublishedMessageAlloc = typename PublishedMessageAllocTraits::allocator_type; + using PublishedMessageDeleter = + rclcpp::allocator::Deleter; + + using SubscribedMessageAllocTraits = + rclcpp::allocator::AllocRebind; + using SubscribedMessageAlloc = typename SubscribedMessageAllocTraits::allocator_type; + using SubscribedMessageDeleter = + rclcpp::allocator::Deleter; + + // init and node + auto context = std::make_shared(); + context->init(0, nullptr); + auto node = std::make_shared( + "custom_allocator_test", + rclcpp::NodeOptions().context(context).use_intra_process_comms(true)); + + // publisher + auto shared_publisher_allocator = std::make_shared(publisher_allocator); + rclcpp::PublisherOptionsWithAllocator publisher_options; + publisher_options.allocator = shared_publisher_allocator; + auto publisher = + node->create_publisher("custom_allocator_test", 10, publisher_options); + + // callback for subscription + uint32_t counter = 0; + std::promise> received_message; + auto received_message_future = received_message.get_future(); + auto callback = + [&counter, &received_message]( + std::unique_ptr msg) + { + ++counter; + received_message.set_value(std::move(msg)); + }; + + // subscription + auto shared_subscription_allocator = + std::make_shared(subscription_allocator); + rclcpp::SubscriptionOptionsWithAllocator subscription_options; + subscription_options.allocator = shared_subscription_allocator; + auto shared_message_strategy_allocator = + std::make_shared(message_memory_strategy); + auto msg_mem_strat = std::make_shared< + rclcpp::message_memory_strategy::MessageMemoryStrategy< + test_msgs::msg::Empty, + MessageMemoryStrategyAllocatorT + > + >(shared_message_strategy_allocator); + using CallbackMessageT = + typename rclcpp::subscription_traits::has_message_type::type; + auto subscriber = node->create_subscription< + test_msgs::msg::Empty, + decltype(callback), + SubscriptionAllocatorT, + rclcpp::Subscription, + rclcpp::message_memory_strategy::MessageMemoryStrategy< + CallbackMessageT, + MessageMemoryStrategyAllocatorT + > + >( + "custom_allocator_test", + 10, + std::forward(callback), + subscription_options, + msg_mem_strat); + + // executor memory strategy + using rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy; + auto shared_memory_strategy_allocator = std::make_shared( + memory_strategy_allocator); + std::shared_ptr memory_strategy = + std::make_shared>( + shared_memory_strategy_allocator); + + // executor + rclcpp::ExecutorOptions options; + options.memory_strategy = memory_strategy; + options.context = context; + rclcpp::executors::SingleThreadedExecutor executor(options); + + executor.add_node(node); + + // rebind message_allocator to ensure correct type + PublishedMessageDeleter message_deleter; + PublishedMessageAlloc rebound_message_allocator = published_message_allocator; + rclcpp::allocator::set_allocator_for_deleter(&message_deleter, &rebound_message_allocator); + + // allocate a message + auto ptr = PublishedMessageAllocTraits::allocate(rebound_message_allocator, 1); + PublishedMessageAllocTraits::construct(rebound_message_allocator, ptr); + std::unique_ptr msg(ptr, message_deleter); + + // publisher and receive + if constexpr (std::is_same_v) { + // no exception expected + EXPECT_NO_THROW( + { + publisher->publish(std::move(msg)); + executor.spin_until_future_complete(received_message_future, std::chrono::seconds(10)); + }); + EXPECT_EQ(ptr, received_message_future.get().get()); + EXPECT_EQ(1u, counter); + } else { + // exception expected + EXPECT_THROW( + { + publisher->publish(std::move(msg)); + executor.spin_until_future_complete(received_message_future, std::chrono::seconds(10)); + }, ExpectedExceptionT); + } +} + +/* + This tests the case where a custom allocator is used correctly, i.e. the same + custom allocator on both sides. + */ +TEST(TestIntraProcessManagerWithAllocators, custom_allocator) { + using PublishedMessageAllocatorT = std::allocator; + using PublisherAllocatorT = std::allocator; + using SubscribedMessageAllocatorT = std::allocator; + using SubscriptionAllocatorT = std::allocator; + using MessageMemoryStrategyAllocatorT = std::allocator; + using MemoryStrategyAllocatorT = std::allocator; + auto allocator = std::allocator(); + do_custom_allocator_test< + PublishedMessageAllocatorT, + PublisherAllocatorT, + SubscribedMessageAllocatorT, + SubscriptionAllocatorT, + MessageMemoryStrategyAllocatorT, + MemoryStrategyAllocatorT, + void // no exception expected + >(allocator, allocator, allocator, allocator, allocator, allocator); +} + +/* + This tests the case where a custom allocator is used incorrectly, i.e. different + custom allocators on both sides. + */ +TEST(TestIntraProcessManagerWithAllocators, custom_allocator_wrong) { + // explicitly use a different allocator here to provoke a failure + using PublishedMessageAllocatorT = std::allocator; + using PublisherAllocatorT = std::allocator; + using SubscribedMessageAllocatorT = MyAllocator; + using SubscriptionAllocatorT = MyAllocator; + using MessageMemoryStrategyAllocatorT = MyAllocator; + using MemoryStrategyAllocatorT = std::allocator; + auto allocator = std::allocator(); + auto my_allocator = MyAllocator(); + do_custom_allocator_test< + PublishedMessageAllocatorT, + PublisherAllocatorT, + SubscribedMessageAllocatorT, + SubscriptionAllocatorT, + MessageMemoryStrategyAllocatorT, + MemoryStrategyAllocatorT, + std::runtime_error // expected exception + >(allocator, allocator, my_allocator, my_allocator, my_allocator, allocator); +} diff --git a/rclcpp/test/rclcpp/test_loaned_message.cpp b/rclcpp/test/rclcpp/test_loaned_message.cpp index c82ff5fc7c..01284a27c1 100644 --- a/rclcpp/test/rclcpp/test_loaned_message.cpp +++ b/rclcpp/test/rclcpp/test_loaned_message.cpp @@ -43,7 +43,42 @@ TEST_F(TestLoanedMessage, initialize) { auto node = std::make_shared("loaned_message_test_node"); auto pub = node->create_publisher("loaned_message_test_topic", 1); - auto loaned_msg = rclcpp::LoanedMessage(pub.get(), pub->get_allocator()); +// suppress deprecated function warning +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif + + auto pub_allocator = pub->get_allocator(); + +// remove warning suppression +#if !defined(_WIN32) +# pragma GCC diagnostic pop +#else // !defined(_WIN32) +# pragma warning(pop) +#endif + +// suppress deprecated function warning +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif + + auto loaned_msg = rclcpp::LoanedMessage(pub.get(), pub_allocator); + +// remove warning suppression +#if !defined(_WIN32) +# pragma GCC diagnostic pop +#else // !defined(_WIN32) +# pragma warning(pop) +#endif + ASSERT_TRUE(loaned_msg.is_valid()); loaned_msg.get().float32_value = 42.0f; ASSERT_EQ(42.0f, loaned_msg.get().float32_value); diff --git a/rclcpp/test/rclcpp/test_memory_strategy.cpp b/rclcpp/test/rclcpp/test_memory_strategy.cpp index 93fa0c8d5e..574bc1aee5 100644 --- a/rclcpp/test/rclcpp/test_memory_strategy.cpp +++ b/rclcpp/test/rclcpp/test_memory_strategy.cpp @@ -98,7 +98,7 @@ TEST_F(TestMemoryStrategy, get_subscription_by_handle) { { auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - auto subscription_callback = [](const test_msgs::msg::Empty::SharedPtr) {}; + auto subscription_callback = [](test_msgs::msg::Empty::ConstSharedPtr) {}; const rclcpp::QoS qos(10); { @@ -344,7 +344,7 @@ TEST_F(TestMemoryStrategy, get_group_by_subscription) { callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - auto subscription_callback = [](const test_msgs::msg::Empty::SharedPtr) {}; + auto subscription_callback = [](test_msgs::msg::Empty::ConstSharedPtr) {}; const rclcpp::QoS qos(10); rclcpp::SubscriptionOptions subscription_options; diff --git a/rclcpp/test/rclcpp/test_node.cpp b/rclcpp/test/rclcpp/test_node.cpp index 6297c4ebbe..8d2e8a8410 100644 --- a/rclcpp/test/rclcpp/test_node.cpp +++ b/rclcpp/test/rclcpp/test_node.cpp @@ -14,6 +14,8 @@ #include +#include +#include #include #include #include @@ -333,9 +335,14 @@ TEST_F(TestNode, declare_parameter_with_no_initial_values) { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.dynamic_typing = true; // no default, no initial + const std::string parameter_name = "parameter"_unq; rclcpp::ParameterValue value = node->declare_parameter( - "parameter"_unq, rclcpp::ParameterValue{}, descriptor); + parameter_name, rclcpp::ParameterValue{}, descriptor); EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_NOT_SET); + // Does not throw if unset before access + EXPECT_EQ( + rclcpp::PARAMETER_NOT_SET, + node->get_parameter(parameter_name).get_parameter_value().get_type()); } { // int default, no initial @@ -2525,6 +2532,34 @@ void expect_qos_profile_eq( EXPECT_EQ(qos1.liveliness_lease_duration.nsec, qos2.liveliness_lease_duration.nsec); } +namespace +{ + +constexpr std::chrono::nanoseconds DEFAULT_EVENT_TIMEOUT = std::chrono::seconds(3); + +constexpr std::chrono::nanoseconds DEFAULT_EVENT_SLEEP_PERIOD = std::chrono::milliseconds(100); + +bool wait_for_event( + std::shared_ptr node, + std::function predicate, + std::chrono::nanoseconds timeout = DEFAULT_EVENT_TIMEOUT, + std::chrono::nanoseconds sleep_period = DEFAULT_EVENT_SLEEP_PERIOD) +{ + auto start = std::chrono::steady_clock::now(); + std::chrono::nanoseconds time_slept(0); + + bool predicate_result; + while (!(predicate_result = predicate()) && time_slept < timeout) { + rclcpp::Event::SharedPtr graph_event = node->get_graph_event(); + node->wait_for_graph_change(graph_event, sleep_period); + time_slept = std::chrono::duration_cast( + std::chrono::steady_clock::now() - start); + } + return predicate_result; +} + +} // namespace + // test that calling get_publishers_info_by_topic and get_subscriptions_info_by_topic TEST_F(TestNode, get_publishers_subscriptions_info_by_topic) { auto node = std::make_shared("my_node", "/ns"); @@ -2556,6 +2591,10 @@ TEST_F(TestNode, get_publishers_subscriptions_info_by_topic) { }; rclcpp::QoS qos = rclcpp::QoS(qos_initialization, rmw_qos_profile_default); auto publisher = node->create_publisher(topic_name, qos); + // Wait for the underlying RMW implementation to catch up with graph changes + auto topic_is_published = + [&]() {return node->get_publishers_info_by_topic(fq_topic_name).size() > 0u;}; + ASSERT_TRUE(wait_for_event(node, topic_is_published)); // List should have one item auto publisher_list = node->get_publishers_info_by_topic(fq_topic_name); ASSERT_EQ(publisher_list.size(), (size_t)1); @@ -2591,12 +2630,15 @@ TEST_F(TestNode, get_publishers_subscriptions_info_by_topic) { false }; rclcpp::QoS qos2 = rclcpp::QoS(qos_initialization2, rmw_qos_profile_default2); - auto callback = [](const test_msgs::msg::BasicTypes::SharedPtr msg) { + auto callback = [](test_msgs::msg::BasicTypes::ConstSharedPtr msg) { (void)msg; }; auto subscriber = node->create_subscription(topic_name, qos2, callback); - + // Wait for the underlying RMW implementation to catch up with graph changes + auto topic_is_subscribed = + [&]() {return node->get_subscriptions_info_by_topic(fq_topic_name).size() > 0u;}; + ASSERT_TRUE(wait_for_event(node, topic_is_subscribed)); // Both lists should have one item publisher_list = node->get_publishers_info_by_topic(fq_topic_name); auto subscription_list = node->get_subscriptions_info_by_topic(fq_topic_name); @@ -2761,9 +2803,20 @@ TEST_F(TestNode, static_and_dynamic_typing) { EXPECT_EQ("hello!", param); } { + auto param = node->declare_parameter("integer_override_not_given", rclcpp::PARAMETER_INTEGER); + EXPECT_EQ(rclcpp::PARAMETER_NOT_SET, param.get_type()); + // Throws if not set before access EXPECT_THROW( - node->declare_parameter("integer_override_not_given", rclcpp::PARAMETER_INTEGER), - rclcpp::exceptions::NoParameterOverrideProvided); + node->get_parameter("integer_override_not_given"), + rclcpp::exceptions::ParameterUninitializedException); + } + { + auto param = node->declare_parameter("integer_set_after_declare", rclcpp::PARAMETER_INTEGER); + EXPECT_EQ(rclcpp::PARAMETER_NOT_SET, param.get_type()); + auto result = node->set_parameter(rclcpp::Parameter{"integer_set_after_declare", 44}); + ASSERT_TRUE(result.successful) << result.reason; + auto get_param = node->get_parameter("integer_set_after_declare"); + EXPECT_EQ(44, get_param.as_int()); } { EXPECT_THROW( diff --git a/rclcpp/test/rclcpp/test_parameter_client.cpp b/rclcpp/test/rclcpp/test_parameter_client.cpp index ba7cc99c29..4cd9e671be 100644 --- a/rclcpp/test/rclcpp/test_parameter_client.cpp +++ b/rclcpp/test/rclcpp/test_parameter_client.cpp @@ -33,7 +33,7 @@ using namespace std::chrono_literals; class TestParameterClient : public ::testing::Test { public: - void OnMessage(const rcl_interfaces::msg::ParameterEvent::SharedPtr event) + void OnMessage(rcl_interfaces::msg::ParameterEvent::ConstSharedPtr event) { (void)event; } @@ -218,6 +218,30 @@ TEST_F(TestParameterClient, async_parameter_get_parameter_types) { ASSERT_TRUE(callback_called); } +/* + Coverage for async get_parameter_types with allow_undeclared_ enabled + */ +TEST_F(TestParameterClient, async_parameter_get_parameter_types_allow_undeclared) { + auto asynchronous_client = + std::make_shared(node_with_option); + bool callback_called = false; + auto callback = [&callback_called](std::shared_future> result) + { + if (result.valid() && result.get().size() == 1 && + result.get()[0] == rclcpp::PARAMETER_NOT_SET) + { + callback_called = true; + } + }; + std::vector names{"foo"}; + std::shared_future> future = + asynchronous_client->get_parameter_types(names, callback); + auto return_code = rclcpp::spin_until_future_complete( + node_with_option, future, std::chrono::milliseconds(100)); + ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); + ASSERT_TRUE(callback_called); +} + /* Coverage for async get_parameters */ @@ -226,7 +250,8 @@ TEST_F(TestParameterClient, async_parameter_get_parameters) { bool callback_called = false; auto callback = [&callback_called](std::shared_future> result) { - if (result.valid() && result.get().size() == 1 && result.get()[0].get_name() == "foo") { + // We expect the result to be empty since we tried to get a parameter that didn't exist. + if (result.valid() && result.get().size() == 0) { callback_called = true; } }; @@ -239,6 +264,28 @@ TEST_F(TestParameterClient, async_parameter_get_parameters) { ASSERT_TRUE(callback_called); } +/* + Coverage for async get_parameters with allow_undeclared_ enabled + */ +TEST_F(TestParameterClient, async_parameter_get_parameters_allow_undeclared) { + auto asynchronous_client = + std::make_shared(node_with_option); + bool callback_called = false; + auto callback = [&callback_called](std::shared_future> result) + { + if (result.valid() && result.get().size() == 1 && result.get()[0].get_name() == "foo") { + callback_called = true; + } + }; + std::vector names{"foo"}; + std::shared_future> future = asynchronous_client->get_parameters( + names, callback); + auto return_code = rclcpp::spin_until_future_complete( + node_with_option, future, std::chrono::milliseconds(100)); + ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); + ASSERT_TRUE(callback_called); +} + /* Coverage for async set_parameters_atomically */ @@ -292,12 +339,197 @@ TEST_F(TestParameterClient, async_parameter_list_parameters) { */ TEST_F(TestParameterClient, sync_parameter_get_parameter_types) { node->declare_parameter("foo", 4); + node->declare_parameter("bar", "this is bar"); auto synchronous_client = std::make_shared(node); - std::vector names{"foo"}; - std::vector parameter_types = - synchronous_client->get_parameter_types(names, 10s); - ASSERT_EQ(1u, parameter_types.size()); - ASSERT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, parameter_types[0]); + + { + std::vector names{"none"}; + std::vector parameter_types = + synchronous_client->get_parameter_types(names, 10s); + ASSERT_EQ(0u, parameter_types.size()); + } + + { + std::vector names{"none", "foo", "bar"}; + std::vector parameter_types = + synchronous_client->get_parameter_types(names, 10s); + ASSERT_EQ(0u, parameter_types.size()); + } + + { + std::vector names{"foo"}; + std::vector parameter_types = + synchronous_client->get_parameter_types(names, 10s); + ASSERT_EQ(1u, parameter_types.size()); + ASSERT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, parameter_types[0]); + } + + { + std::vector names{"bar"}; + std::vector parameter_types = + synchronous_client->get_parameter_types(names, 10s); + ASSERT_EQ(1u, parameter_types.size()); + ASSERT_EQ(rclcpp::ParameterType::PARAMETER_STRING, parameter_types[0]); + } + + { + std::vector names{"foo", "bar"}; + std::vector parameter_types = + synchronous_client->get_parameter_types(names, 10s); + ASSERT_EQ(2u, parameter_types.size()); + ASSERT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, parameter_types[0]); + ASSERT_EQ(rclcpp::ParameterType::PARAMETER_STRING, parameter_types[1]); + } +} + +/* + Coverage for sync get_parameter_types with allow_undeclared_ enabled + */ +TEST_F(TestParameterClient, sync_parameter_get_parameter_types_allow_undeclared) { + node_with_option->declare_parameter("foo", 4); + node_with_option->declare_parameter("bar", "this is bar"); + auto synchronous_client = std::make_shared(node_with_option); + + { + std::vector names{"none"}; + std::vector parameter_types = + synchronous_client->get_parameter_types(names, 10s); + ASSERT_EQ(1u, parameter_types.size()); + ASSERT_EQ(rclcpp::ParameterType::PARAMETER_NOT_SET, parameter_types[0]); + } + + { + std::vector names{"none", "foo", "bar"}; + std::vector parameter_types = + synchronous_client->get_parameter_types(names, 10s); + ASSERT_EQ(3u, parameter_types.size()); + ASSERT_EQ(rclcpp::ParameterType::PARAMETER_NOT_SET, parameter_types[0]); + ASSERT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, parameter_types[1]); + ASSERT_EQ(rclcpp::ParameterType::PARAMETER_STRING, parameter_types[2]); + } + + { + std::vector names{"foo"}; + std::vector parameter_types = + synchronous_client->get_parameter_types(names, 10s); + ASSERT_EQ(1u, parameter_types.size()); + ASSERT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, parameter_types[0]); + } + + { + std::vector names{"bar"}; + std::vector parameter_types = + synchronous_client->get_parameter_types(names, 10s); + ASSERT_EQ(1u, parameter_types.size()); + ASSERT_EQ(rclcpp::ParameterType::PARAMETER_STRING, parameter_types[0]); + } + + { + std::vector names{"foo", "bar"}; + std::vector parameter_types = + synchronous_client->get_parameter_types(names, 10s); + ASSERT_EQ(2u, parameter_types.size()); + ASSERT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, parameter_types[0]); + ASSERT_EQ(rclcpp::ParameterType::PARAMETER_STRING, parameter_types[1]); + } +} + +/* + Coverage for sync get_parameters + */ +TEST_F(TestParameterClient, sync_parameter_get_parameters) { + node->declare_parameter("foo", 4); + node->declare_parameter("bar", "this is bar"); + auto synchronous_client = std::make_shared(node); + + { + std::vector names{"none"}; + std::vector parameters = synchronous_client->get_parameters(names, 10s); + ASSERT_EQ(0u, parameters.size()); + } + + { + std::vector names{"none", "foo", "bar"}; + std::vector parameters = synchronous_client->get_parameters(names, 10s); + ASSERT_EQ(0u, parameters.size()); + } + + { + std::vector names{"foo"}; + std::vector parameters = synchronous_client->get_parameters(names, 10s); + ASSERT_EQ(1u, parameters.size()); + ASSERT_EQ("foo", parameters[0].get_name()); + ASSERT_EQ(4u, parameters[0].as_int()); + } + + { + std::vector names{"bar"}; + std::vector parameters = synchronous_client->get_parameters(names, 10s); + ASSERT_EQ(1u, parameters.size()); + ASSERT_EQ("bar", parameters[0].get_name()); + ASSERT_EQ("this is bar", parameters[0].as_string()); + } + + { + std::vector names{"foo", "bar"}; + std::vector parameters = synchronous_client->get_parameters(names, 10s); + ASSERT_EQ(2u, parameters.size()); + ASSERT_EQ("foo", parameters[0].get_name()); + ASSERT_EQ(4u, parameters[0].as_int()); + ASSERT_EQ("bar", parameters[1].get_name()); + ASSERT_EQ("this is bar", parameters[1].as_string()); + } +} + +/* + Coverage for sync get_parameters with allow_undeclared_ enabled + */ +TEST_F(TestParameterClient, sync_parameter_get_parameters_allow_undeclared) { + node_with_option->declare_parameter("foo", 4); + node_with_option->declare_parameter("bar", "this is bar"); + auto synchronous_client = std::make_shared(node_with_option); + + { + std::vector names{"none"}; + std::vector parameters = synchronous_client->get_parameters(names, 10s); + ASSERT_EQ(1u, parameters.size()); + } + + { + std::vector names{"none", "foo", "bar"}; + std::vector parameters = synchronous_client->get_parameters(names, 10s); + ASSERT_EQ(3u, parameters.size()); + ASSERT_EQ("foo", parameters[1].get_name()); + ASSERT_EQ(4u, parameters[1].as_int()); + ASSERT_EQ("bar", parameters[2].get_name()); + ASSERT_EQ("this is bar", parameters[2].as_string()); + } + + { + std::vector names{"foo"}; + std::vector parameters = synchronous_client->get_parameters(names, 10s); + ASSERT_EQ(1u, parameters.size()); + ASSERT_EQ("foo", parameters[0].get_name()); + ASSERT_EQ(4u, parameters[0].as_int()); + } + + { + std::vector names{"bar"}; + std::vector parameters = synchronous_client->get_parameters(names, 10s); + ASSERT_EQ(1u, parameters.size()); + ASSERT_EQ("bar", parameters[0].get_name()); + ASSERT_EQ("this is bar", parameters[0].as_string()); + } + + { + std::vector names{"foo", "bar"}; + std::vector parameters = synchronous_client->get_parameters(names, 10s); + ASSERT_EQ(2u, parameters.size()); + ASSERT_EQ("foo", parameters[0].get_name()); + ASSERT_EQ(4u, parameters[0].as_int()); + ASSERT_EQ("bar", parameters[1].get_name()); + ASSERT_EQ("this is bar", parameters[1].as_string()); + } } /* @@ -629,3 +861,90 @@ TEST_F(TestParameterClient, sync_parameter_describe_parameters_allow_undeclared) ASSERT_FALSE(parameter_descs[1].read_only); } } + +/* + Coverage for async delete_parameters + */ +TEST_F(TestParameterClient, async_parameter_delete_parameters) { + auto asynchronous_client = + std::make_shared(node_with_option); + // set parameter + auto set_future = asynchronous_client->set_parameters({rclcpp::Parameter("foo", 4)}); + rclcpp::spin_until_future_complete( + node_with_option, set_future, std::chrono::milliseconds(100)); + ASSERT_EQ(set_future.get()[0].successful, true); + // delete one parameter + auto delete_future = asynchronous_client->delete_parameters({"foo"}); + rclcpp::spin_until_future_complete( + node_with_option, delete_future, std::chrono::milliseconds(100)); + ASSERT_EQ(delete_future.get()[0].successful, true); + // check that deleted parameter isn't set + auto get_future2 = asynchronous_client->get_parameters({"foo"}); + rclcpp::spin_until_future_complete( + node_with_option, get_future2, std::chrono::milliseconds(100)); + ASSERT_EQ( + get_future2.get()[0].get_type(), + rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET); +} +/* + Coverage for sync delete_parameters + */ +TEST_F(TestParameterClient, sync_parameter_delete_parameters) { + auto synchronous_client = + std::make_shared(node_with_option); + // set parameter + auto set_result = synchronous_client->set_parameters({rclcpp::Parameter("foo", 4)}); + // delete one parameter + auto delete_result = synchronous_client->delete_parameters({"foo"}); + // check that deleted parameter isn't set + auto get_result = synchronous_client->get_parameters({"foo"}); + ASSERT_EQ( + get_result[0].get_type(), + rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET); +} + +/* + Coverage for async load_parameters + */ +TEST_F(TestParameterClient, async_parameter_load_parameters) { + auto load_node = std::make_shared( + "load_node", + "namespace", + rclcpp::NodeOptions().allow_undeclared_parameters(true)); + auto asynchronous_client = + std::make_shared(load_node, "/namespace/load_node"); + // load parameters + rcpputils::fs::path test_resources_path{TEST_RESOURCES_DIRECTORY}; + const std::string parameters_filepath = ( + test_resources_path / "test_node" / "load_parameters.yaml").string(); + auto load_future = asynchronous_client->load_parameters(parameters_filepath); + auto result_code = rclcpp::spin_until_future_complete( + load_node, load_future, std::chrono::milliseconds(100)); + ASSERT_EQ(result_code, rclcpp::FutureReturnCode::SUCCESS); + ASSERT_EQ(load_future.get()[0].successful, true); + // list parameters + auto list_parameters = asynchronous_client->list_parameters({}, 3); + rclcpp::spin_until_future_complete( + load_node, list_parameters, std::chrono::milliseconds(100)); + ASSERT_EQ(list_parameters.get().names.size(), static_cast(5)); +} +/* + Coverage for sync load_parameters + */ +TEST_F(TestParameterClient, sync_parameter_load_parameters) { + auto load_node = std::make_shared( + "load_node", + "namespace", + rclcpp::NodeOptions().allow_undeclared_parameters(true)); + auto synchronous_client = + std::make_shared(load_node); + // load parameters + rcpputils::fs::path test_resources_path{TEST_RESOURCES_DIRECTORY}; + const std::string parameters_filepath = ( + test_resources_path / "test_node" / "load_parameters.yaml").string(); + auto load_future = synchronous_client->load_parameters(parameters_filepath); + ASSERT_EQ(load_future[0].successful, true); + // list parameters + auto list_parameters = synchronous_client->list_parameters({}, 3); + ASSERT_EQ(list_parameters.names.size(), static_cast(5)); +} diff --git a/rclcpp/test/rclcpp/test_parameter_event_handler.cpp b/rclcpp/test/rclcpp/test_parameter_event_handler.cpp index 761148f06f..097b0ee391 100644 --- a/rclcpp/test/rclcpp/test_parameter_event_handler.cpp +++ b/rclcpp/test/rclcpp/test_parameter_event_handler.cpp @@ -27,9 +27,9 @@ class TestParameterEventHandler : public rclcpp::ParameterEventHandler : ParameterEventHandler(node) {} - void test_event(const rcl_interfaces::msg::ParameterEvent::SharedPtr event) + void test_event(rcl_interfaces::msg::ParameterEvent::ConstSharedPtr event) { - event_callback(event); + event_callback(*event); } size_t num_event_callbacks() @@ -149,8 +149,8 @@ TEST_F(TestNode, RegisterParameterCallback) TEST_F(TestNode, SameParameterDifferentNode) { - int64_t int_param_node1; - int64_t int_param_node2; + int64_t int_param_node1{0}; + int64_t int_param_node2{0}; auto cb1 = [&int_param_node1](const rclcpp::Parameter & p) { int_param_node1 = p.get_value(); @@ -264,20 +264,20 @@ TEST_F(TestNode, EventCallback) double product; auto cb = [&int_param, &double_param, &product, &received, - this](const rcl_interfaces::msg::ParameterEvent::SharedPtr & event) + this](const rcl_interfaces::msg::ParameterEvent & event) { auto node_name = node->get_fully_qualified_name(); - if (event->node == node_name) { + if (event.node == node_name) { received = true; } rclcpp::Parameter p; - if (ParameterEventHandler::get_parameter_from_event(*event, p, "my_int", node_name)) { + if (ParameterEventHandler::get_parameter_from_event(event, p, "my_int", node_name)) { int_param = p.get_value(); } try { - p = ParameterEventHandler::get_parameter_from_event(*event, "my_double", node_name); + p = ParameterEventHandler::get_parameter_from_event(event, "my_double", node_name); double_param = p.get_value(); } catch (...) { } @@ -286,12 +286,12 @@ TEST_F(TestNode, EventCallback) }; auto cb2 = - [&bool_param, this](const rcl_interfaces::msg::ParameterEvent::SharedPtr & event) + [&bool_param, this](const rcl_interfaces::msg::ParameterEvent & event) { rclcpp::Parameter p; - if (event->node == diff_ns_name) { + if (event.node == diff_ns_name) { if (ParameterEventHandler::get_parameter_from_event( - *event, p, "my_bool", diff_ns_name)) + event, p, "my_bool", diff_ns_name)) { bool_param = p.get_value(); } @@ -405,13 +405,13 @@ TEST_F(TestNode, LastInFirstCallForParameterEventCallbacks) // The callbacks will log the current time for comparison purposes. Add a bit of a stall // to ensure that the time noted in the back-to-back calls isn't the same auto cb1 = - [this, &time_1](const rcl_interfaces::msg::ParameterEvent::SharedPtr &) + [this, &time_1](const rcl_interfaces::msg::ParameterEvent &) { time_1 = node->now(); std::this_thread::sleep_for(std::chrono::milliseconds(10)); }; auto cb2 = - [this, &time_2](const rcl_interfaces::msg::ParameterEvent::SharedPtr &) + [this, &time_2](const rcl_interfaces::msg::ParameterEvent &) { time_2 = node->now(); std::this_thread::sleep_for(std::chrono::milliseconds(10)); diff --git a/rclcpp/test/rclcpp/test_publisher.cpp b/rclcpp/test/rclcpp/test_publisher.cpp index 7d2c9d14c1..a6b8fbc5f4 100644 --- a/rclcpp/test/rclcpp/test_publisher.cpp +++ b/rclcpp/test/rclcpp/test_publisher.cpp @@ -501,8 +501,39 @@ TEST_F(TestPublisher, run_event_handlers) { initialize(); auto publisher = node->create_publisher("topic", 10); - for (const auto & handler : publisher->get_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(); handler->execute(data); } } + +TEST_F(TestPublisher, get_network_flow_endpoints_errors) { + initialize(); + const rclcpp::QoS publisher_qos(1); + auto publisher = node->create_publisher("topic", publisher_qos); + + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_publisher_get_network_flow_endpoints, RCL_RET_ERROR); + auto mock_network_flow_endpoint_array_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_network_flow_endpoint_array_fini, RCL_RET_ERROR); + EXPECT_THROW( + publisher->get_network_flow_endpoints(), + rclcpp::exceptions::RCLError); + } + { + auto mock_network_flow_endpoint_array_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_network_flow_endpoint_array_fini, RCL_RET_ERROR); + EXPECT_THROW( + publisher->get_network_flow_endpoints(), + rclcpp::exceptions::RCLError); + } + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_publisher_get_network_flow_endpoints, RCL_RET_OK); + auto mock_network_flow_endpoint_array_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_network_flow_endpoint_array_fini, RCL_RET_OK); + EXPECT_NO_THROW(publisher->get_network_flow_endpoints()); + } +} diff --git a/rclcpp/test/rclcpp/test_publisher_subscription_count_api.cpp b/rclcpp/test/rclcpp/test_publisher_subscription_count_api.cpp index 9e009acb95..eac46c1d8d 100644 --- a/rclcpp/test/rclcpp/test_publisher_subscription_count_api.cpp +++ b/rclcpp/test/rclcpp/test_publisher_subscription_count_api.cpp @@ -122,7 +122,7 @@ class TestPublisherSubscriptionCount : public ::testing::Test } protected: - static void OnMessage(const test_msgs::msg::Empty::SharedPtr msg) + static void OnMessage(test_msgs::msg::Empty::ConstSharedPtr msg) { (void)msg; } diff --git a/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp b/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp new file mode 100644 index 0000000000..2f5d540568 --- /dev/null +++ b/rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp @@ -0,0 +1,335 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/loaned_message.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "../mocking_utils/patch.hpp" +#include "../utils/rclcpp_gtest_macros.hpp" + +#include "test_msgs/msg/empty.hpp" +#include "rclcpp/msg/string.hpp" + + +#ifdef RMW_IMPLEMENTATION +# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX +# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) +#else +# define CLASSNAME(NAME, SUFFIX) NAME +#endif + + +using namespace std::chrono_literals; + +static const int g_max_loops = 200; +static const std::chrono::milliseconds g_sleep_per_loop(10); + + +class TestPublisher : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } + } + +protected: + void initialize(const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions()) + { + node = std::make_shared("my_node", "/ns", node_options); + } + + void TearDown() + { + node.reset(); + } + + rclcpp::Node::SharedPtr node; +}; + +class CLASSNAME (test_intra_process_within_one_node, RMW_IMPLEMENTATION) : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } +}; + +namespace rclcpp +{ + +template<> +struct TypeAdapter +{ + using is_specialized = std::true_type; + using custom_type = std::string; + using ros_message_type = rclcpp::msg::String; + + static void + convert_to_ros_message( + const custom_type & source, + ros_message_type & destination) + { + destination.data = source; + } + + static void + convert_to_custom( + const ros_message_type & source, + custom_type & destination) + { + destination = source.data; + } +}; + +// Throws in conversion +template<> +struct TypeAdapter +{ + using is_specialized = std::true_type; + using custom_type = int; + using ros_message_type = rclcpp::msg::String; + + static void + convert_to_ros_message( + const custom_type & source, + ros_message_type & destination) + { + (void) source; + (void) destination; + throw std::runtime_error("This should happen"); + } + + static void + convert_to_custom( + const ros_message_type & source, + custom_type & destination) + { + (void) source; + (void) destination; + } +}; + +} // namespace rclcpp + +/* + * Testing publisher creation signatures with a type adapter. + */ +TEST_F(TestPublisher, various_creation_signatures) { + for (auto is_intra_process : {true, false}) { + rclcpp::NodeOptions options; + options.use_intra_process_comms(is_intra_process); + initialize(options); + { + using StringTypeAdapter = rclcpp::TypeAdapter; + auto publisher = node->create_publisher("topic", 42); + (void)publisher; + } + { + using StringTypeAdapter = rclcpp::adapt_type::as; + auto publisher = node->create_publisher("topic", 42); + (void)publisher; + } + } +} + +/* + * Testing that conversion errors are passed up. + */ +TEST_F(TestPublisher, conversion_exception_is_passed_up) { + using BadStringTypeAdapter = rclcpp::TypeAdapter; + for (auto is_intra_process : {true, false}) { + rclcpp::NodeOptions options; + options.use_intra_process_comms(is_intra_process); + initialize(options); + auto pub = node->create_publisher("topic_name", 1); + EXPECT_THROW(pub->publish(1), std::runtime_error); + } +} + +/* + * Testing that publisher sends type adapted types and ROS message types with intra proccess communications. + */ +TEST_F( + CLASSNAME(test_intra_process_within_one_node, RMW_IMPLEMENTATION), + check_type_adapted_message_is_sent_and_received_intra_process) { + using StringTypeAdapter = rclcpp::TypeAdapter; + const std::string message_data = "Message Data"; + const std::string topic_name = "topic_name"; + bool is_received; + + auto callback = + [message_data, &is_received]( + const rclcpp::msg::String::SharedPtr msg, + const rclcpp::MessageInfo & message_info + ) -> void + { + is_received = true; + ASSERT_STREQ(message_data.c_str(), msg->data.c_str()); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + + auto node = rclcpp::Node::make_shared( + "test_intra_process", + rclcpp::NodeOptions().use_intra_process_comms(true)); + auto pub = node->create_publisher(topic_name, 10); + auto sub = node->create_subscription(topic_name, 1, callback); + + auto wait_for_message_to_be_received = [&is_received, &node]() { + rclcpp::executors::SingleThreadedExecutor executor; + int i = 0; + executor.add_node(node); + executor.spin_once(std::chrono::milliseconds(0)); + while (!is_received && i < g_max_loops) { + printf("spin_node_once() - callback (1) expected - try %d/%d\n", ++i, g_max_loops); + executor.spin_once(g_sleep_per_loop); + } + }; + { + { // std::string passed by reference + is_received = false; + pub->publish(message_data); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(); + ASSERT_TRUE(is_received); + } + { // unique pointer to std::string + is_received = false; + auto pu_message = std::make_unique(message_data); + pub->publish(std::move(pu_message)); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(); + ASSERT_TRUE(is_received); + } + { // ROS message passed by reference + is_received = false; + rclcpp::msg::String msg; + msg.data = message_data; + pub->publish(msg); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(); + ASSERT_TRUE(is_received); + } + { // unique ptr to ROS message + is_received = false; + auto pu_msg = std::make_unique(); + pu_msg->data = message_data; + pub->publish(std::move(pu_msg)); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(); + ASSERT_TRUE(is_received); + } + /* TODO(audrow) Enable once loaned messages are supported for intra process communication + { // loaned ROS message + // is_received = false; + // std::allocator allocator; + // rclcpp::LoanedMessage loaned_msg(*pub, allocator); + // loaned_msg.get().data = message_data; + // pub->publish(std::move(loaned_msg)); + // ASSERT_FALSE(is_received); + // wait_for_message_to_be_received(); + // ASSERT_TRUE(is_received); + } + */ + } +} + +/* + * Testing that publisher sends type adapted types and ROS message types with inter proccess communications. + */ +TEST_F(TestPublisher, check_type_adapted_message_is_sent_and_received) { + using StringTypeAdapter = rclcpp::TypeAdapter; + + initialize(); + + const std::string message_data = "Message Data"; + const std::string topic_name = "topic_name"; + + auto do_nothing = [](std::shared_ptr) {FAIL();}; + auto pub = node->create_publisher(topic_name, 1); + auto sub = node->create_subscription(topic_name, 1, do_nothing); + + auto assert_no_message_was_received_yet = [sub]() { + rclcpp::msg::String msg; + rclcpp::MessageInfo msg_info; + EXPECT_FALSE(sub->take(msg, msg_info)); + }; + auto assert_message_was_received = [sub, message_data]() { + rclcpp::msg::String msg; + rclcpp::MessageInfo msg_info; + bool message_received = false; + auto start = std::chrono::steady_clock::now(); + do { + message_received = sub->take(msg, msg_info); + std::this_thread::sleep_for(100ms); + } while (!message_received && std::chrono::steady_clock::now() - start < 10s); + EXPECT_TRUE(message_received); + ASSERT_STREQ(message_data.c_str(), msg.data.c_str()); + }; + + { // std::string passed by reference + assert_no_message_was_received_yet(); + pub->publish(message_data); + assert_message_was_received(); + } + { // unique pointer to std::string + assert_no_message_was_received_yet(); + auto pu_message = std::make_unique(message_data); + pub->publish(std::move(pu_message)); + assert_message_was_received(); + } + { // ROS message passed by reference + assert_no_message_was_received_yet(); + rclcpp::msg::String msg; + msg.data = message_data; + pub->publish(msg); + assert_message_was_received(); + } + { // unique ptr to ROS message + assert_no_message_was_received_yet(); + auto pu_msg = std::make_unique(); + pu_msg->data = message_data; + pub->publish(std::move(pu_msg)); + assert_message_was_received(); + } + { // loaned ROS message + assert_no_message_was_received_yet(); + std::allocator allocator; + rclcpp::LoanedMessage loaned_msg(*pub, allocator); + loaned_msg.get().data = message_data; + pub->publish(std::move(loaned_msg)); + rclcpp::PublisherOptionsWithAllocator> options; + assert_message_was_received(); + } +} diff --git a/rclcpp/test/rclcpp/test_qos.cpp b/rclcpp/test/rclcpp/test_qos.cpp index 6786750ed4..47d1d10b07 100644 --- a/rclcpp/test/rclcpp/test_qos.cpp +++ b/rclcpp/test/rclcpp/test_qos.cpp @@ -14,6 +14,8 @@ #include +#include + #include "rclcpp/qos.hpp" #include "rmw/types.h" @@ -208,3 +210,43 @@ TEST(TestQoS, policy_name_from_kind) { "LIFESPAN_QOS_POLICY", rclcpp::qos_policy_name_from_kind(RMW_QOS_POLICY_LIFESPAN)); } + +TEST(TestQoS, qos_check_compatible) +{ + // Compatible + { + rclcpp::QoS qos = rclcpp::QoS(1) + .reliable() + .durability_volatile() + .deadline(rclcpp::Duration(1, 0u)) + .lifespan(rclcpp::Duration(1, 0u)) + .liveliness(rclcpp::LivelinessPolicy::Automatic) + .liveliness_lease_duration(rclcpp::Duration(1, 0u)); + rclcpp::QoSCheckCompatibleResult ret = rclcpp::qos_check_compatible(qos, qos); + EXPECT_EQ(ret.compatibility, rclcpp::QoSCompatibility::Ok); + EXPECT_EQ(ret.reason, std::string("")); + } + + // Note, the following incompatible tests assume we are using a DDS middleware, + // and may not be valid for other RMWs. + // TODO(jacobperron): programmatically check if current RMW is one of the officially + // supported DDS middlewares before running the following tests + + // Incompatible + { + rclcpp::QoS pub_qos = rclcpp::QoS(1).best_effort(); + rclcpp::QoS sub_qos = rclcpp::QoS(1).reliable(); + rclcpp::QoSCheckCompatibleResult ret = rclcpp::qos_check_compatible(pub_qos, sub_qos); + EXPECT_EQ(ret.compatibility, rclcpp::QoSCompatibility::Error); + EXPECT_FALSE(ret.reason.empty()); + } + + // Warn of possible incompatibility + { + rclcpp::SystemDefaultsQoS pub_qos; + rclcpp::QoS sub_qos = rclcpp::QoS(1).reliable(); + rclcpp::QoSCheckCompatibleResult ret = rclcpp::qos_check_compatible(pub_qos, sub_qos); + EXPECT_EQ(ret.compatibility, rclcpp::QoSCompatibility::Warning); + EXPECT_FALSE(ret.reason.empty()); + } +} diff --git a/rclcpp/test/rclcpp/test_qos_event.cpp b/rclcpp/test/rclcpp/test_qos_event.cpp index 6c94aaff97..292011a09c 100644 --- a/rclcpp/test/rclcpp/test_qos_event.cpp +++ b/rclcpp/test/rclcpp/test_qos_event.cpp @@ -27,6 +27,8 @@ #include "../mocking_utils/patch.hpp" +using namespace std::chrono_literals; + class TestQosEvent : public ::testing::Test { protected: @@ -42,7 +44,7 @@ class TestQosEvent : public ::testing::Test node = std::make_shared("test_qos_event", "/ns"); - message_callback = [node = node.get()](const test_msgs::msg::Empty::SharedPtr /*msg*/) { + message_callback = [node = node.get()](test_msgs::msg::Empty::ConstSharedPtr /*msg*/) { RCLCPP_INFO(node->get_logger(), "Message received"); }; } @@ -55,7 +57,7 @@ class TestQosEvent : public ::testing::Test static constexpr char topic_name[] = "test_topic"; rclcpp::Node::SharedPtr node; bool is_fastrtps; - std::function message_callback; + std::function message_callback; }; constexpr char TestQosEvent::topic_name[]; @@ -325,3 +327,95 @@ TEST_F(TestQosEvent, add_to_wait_set) { EXPECT_THROW(handler.add_to_wait_set(&wait_set), rclcpp::exceptions::RCLError); } } + +TEST_F(TestQosEvent, test_on_new_event_callback) +{ + rclcpp::QoS qos_profile_publisher(10); + qos_profile_publisher.durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); + auto publisher = node->create_publisher( + topic_name, qos_profile_publisher); + + std::atomic c1 {0}; + auto increase_c1_cb = [&c1](size_t count_events) {c1 += count_events;}; + publisher->set_on_new_qos_event_callback(increase_c1_cb, RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS); + + rclcpp::QoS qos_profile_subscription(10); + qos_profile_subscription.durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + auto subscription = node->create_subscription( + topic_name, qos_profile_subscription, message_callback); + + auto start = std::chrono::steady_clock::now(); + do { + std::this_thread::sleep_for(100ms); + } while (c1 == 0 && std::chrono::steady_clock::now() - start < 10s); + + EXPECT_EQ(c1, 1u); + + std::atomic c2 {0}; + auto increase_c2_cb = [&c2](size_t count_events) {c2 += count_events;}; + subscription->set_on_new_qos_event_callback( + increase_c2_cb, + RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS); + + start = std::chrono::steady_clock::now(); + do { + std::this_thread::sleep_for(100ms); + } while (c2 == 0 && std::chrono::steady_clock::now() - start < 10s); + + EXPECT_EQ(c1, 1u); + EXPECT_EQ(c2, 1u); + + auto publisher2 = node->create_publisher( + topic_name, qos_profile_publisher); + + start = std::chrono::steady_clock::now(); + do { + std::this_thread::sleep_for(100ms); + } while (c2 == 1u && std::chrono::steady_clock::now() - start < 10s); + + EXPECT_EQ(c1, 1u); + EXPECT_EQ(c2, 2u); + + publisher->clear_on_new_qos_event_callback(RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS); + + auto subscription2 = node->create_subscription( + topic_name, qos_profile_subscription, message_callback); + + publisher->set_on_new_qos_event_callback(increase_c1_cb, RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS); + + start = std::chrono::steady_clock::now(); + do { + std::this_thread::sleep_for(100ms); + } while (c1 == 1 && std::chrono::steady_clock::now() - start < 10s); + + EXPECT_EQ(c1, 2u); + EXPECT_EQ(c2, 2u); +} + +TEST_F(TestQosEvent, test_invalid_on_new_event_callback) +{ + auto pub = node->create_publisher(topic_name, 10); + auto sub = node->create_subscription(topic_name, 10, message_callback); + auto dummy_cb = [](size_t count_events) {(void)count_events;}; + std::function invalid_cb; + + EXPECT_NO_THROW( + pub->set_on_new_qos_event_callback(dummy_cb, RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS)); + + EXPECT_NO_THROW( + pub->clear_on_new_qos_event_callback(RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS)); + + EXPECT_NO_THROW( + sub->set_on_new_qos_event_callback(dummy_cb, RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS)); + + EXPECT_NO_THROW( + sub->clear_on_new_qos_event_callback(RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS)); + + EXPECT_THROW( + pub->set_on_new_qos_event_callback(invalid_cb, RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS), + std::invalid_argument); + + EXPECT_THROW( + sub->set_on_new_qos_event_callback(invalid_cb, RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS), + std::invalid_argument); +} diff --git a/rclcpp/test/rclcpp/test_qos_parameters.cpp b/rclcpp/test/rclcpp/test_qos_parameters.cpp index c37b9a825c..97d524d176 100644 --- a/rclcpp/test/rclcpp/test_qos_parameters.cpp +++ b/rclcpp/test/rclcpp/test_qos_parameters.cpp @@ -234,7 +234,8 @@ TEST(TestQosParameters, declare_no_parameters_interface) { std::map qos_params; EXPECT_FALSE( - node->get_node_parameters_interface()->get_parameters_by_prefix("qos_overrides", qos_params)); + node->get_node_parameters_interface()->get_parameters_by_prefix( + "qos_overrides./my/fully/qualified/topic_name", qos_params)); rclcpp::shutdown(); } diff --git a/rclcpp/test/rclcpp/test_serialized_message_allocator.cpp b/rclcpp/test/rclcpp/test_serialized_message_allocator.cpp index 968ad0ac2d..78ab77a28f 100644 --- a/rclcpp/test/rclcpp/test_serialized_message_allocator.cpp +++ b/rclcpp/test/rclcpp/test_serialized_message_allocator.cpp @@ -58,7 +58,7 @@ TEST(TestSerializedMessageAllocator, borrow_from_subscription) { std::shared_ptr sub = node->create_subscription( "~/dummy_topic", 10, - [](std::shared_ptr test_msg) {(void) test_msg;}); + [](std::shared_ptr test_msg) {(void) test_msg;}); auto msg0 = sub->create_serialized_message(); EXPECT_EQ(0u, msg0->capacity()); diff --git a/rclcpp/test/rclcpp/test_service.cpp b/rclcpp/test/rclcpp/test_service.cpp index 16ab31cf1b..110e913da4 100644 --- a/rclcpp/test/rclcpp/test_service.cpp +++ b/rclcpp/test/rclcpp/test_service.cpp @@ -27,6 +27,8 @@ #include "test_msgs/srv/empty.hpp" #include "test_msgs/srv/empty.h" +using namespace std::chrono_literals; + class TestService : public ::testing::Test { protected: @@ -235,3 +237,72 @@ TEST_F(TestService, send_response) { rclcpp::exceptions::RCLError); } } + +/* + Testing on_new_request callbacks. + */ +TEST_F(TestService, on_new_request_callback) { + auto server_callback = + [](const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {FAIL();}; + auto server = node->create_service("~/test_service", server_callback); + + std::atomic c1 {0}; + auto increase_c1_cb = [&c1](size_t count_msgs) {c1 += count_msgs;}; + server->set_on_new_request_callback(increase_c1_cb); + + auto client = node->create_client("~/test_service"); + { + auto request = std::make_shared(); + client->async_send_request(request); + } + + auto start = std::chrono::steady_clock::now(); + do { + std::this_thread::sleep_for(100ms); + } while (c1 == 0 && std::chrono::steady_clock::now() - start < 10s); + + EXPECT_EQ(c1.load(), 1u); + + std::atomic c2 {0}; + auto increase_c2_cb = [&c2](size_t count_msgs) {c2 += count_msgs;}; + server->set_on_new_request_callback(increase_c2_cb); + + { + auto request = std::make_shared(); + client->async_send_request(request); + } + + start = std::chrono::steady_clock::now(); + do { + std::this_thread::sleep_for(100ms); + } while (c2 == 0 && std::chrono::steady_clock::now() - start < 10s); + + EXPECT_EQ(c1.load(), 1u); + EXPECT_EQ(c2.load(), 1u); + + server->clear_on_new_request_callback(); + + { + auto request = std::make_shared(); + client->async_send_request(request); + client->async_send_request(request); + client->async_send_request(request); + } + + std::atomic c3 {0}; + auto increase_c3_cb = [&c3](size_t count_msgs) {c3 += count_msgs;}; + server->set_on_new_request_callback(increase_c3_cb); + + start = std::chrono::steady_clock::now(); + do { + std::this_thread::sleep_for(100ms); + } while (c3 == 0 && std::chrono::steady_clock::now() - start < 10s); + + EXPECT_EQ(c1.load(), 1u); + EXPECT_EQ(c2.load(), 1u); + EXPECT_EQ(c3.load(), 3u); + + std::function invalid_cb = nullptr; + EXPECT_THROW(server->set_on_new_request_callback(invalid_cb), std::invalid_argument); +} diff --git a/rclcpp/test/rclcpp/test_subscription.cpp b/rclcpp/test/rclcpp/test_subscription.cpp index 9b190fea18..fdea25dbd5 100644 --- a/rclcpp/test/rclcpp/test_subscription.cpp +++ b/rclcpp/test/rclcpp/test_subscription.cpp @@ -34,7 +34,7 @@ using namespace std::chrono_literals; class TestSubscription : public ::testing::Test { public: - void OnMessage(const test_msgs::msg::Empty::SharedPtr msg) + void OnMessage(test_msgs::msg::Empty::ConstSharedPtr msg) { (void)msg; } @@ -80,7 +80,7 @@ class TestSubscriptionInvalidIntraprocessQos class TestSubscriptionSub : public ::testing::Test { public: - void OnMessage(const test_msgs::msg::Empty::SharedPtr msg) + void OnMessage(test_msgs::msg::Empty::ConstSharedPtr msg) { (void)msg; } @@ -113,7 +113,7 @@ class SubscriptionClassNodeInheritance : public rclcpp::Node { } - void OnMessage(const test_msgs::msg::Empty::SharedPtr msg) + void OnMessage(test_msgs::msg::Empty::ConstSharedPtr msg) { (void)msg; } @@ -130,7 +130,7 @@ class SubscriptionClassNodeInheritance : public rclcpp::Node class SubscriptionClass { public: - void OnMessage(const test_msgs::msg::Empty::SharedPtr msg) + void OnMessage(test_msgs::msg::Empty::ConstSharedPtr msg) { (void)msg; } @@ -150,7 +150,7 @@ class SubscriptionClass TEST_F(TestSubscription, construction_and_destruction) { initialize(); using test_msgs::msg::Empty; - auto callback = [](const Empty::SharedPtr msg) { + auto callback = [](Empty::ConstSharedPtr msg) { (void)msg; }; { @@ -162,7 +162,7 @@ TEST_F(TestSubscription, construction_and_destruction) { // get_subscription_handle() const rclcpp::SubscriptionBase * const_sub = sub.get(); EXPECT_NE(nullptr, const_sub->get_subscription_handle()); - EXPECT_FALSE(sub->use_take_shared_method()); + EXPECT_TRUE(sub->use_take_shared_method()); EXPECT_NE(nullptr, sub->get_message_type_support_handle().typesupport_identifier); EXPECT_NE(nullptr, sub->get_message_type_support_handle().data); @@ -182,7 +182,7 @@ TEST_F(TestSubscription, construction_and_destruction) { */ TEST_F(TestSubscriptionSub, construction_and_destruction) { using test_msgs::msg::Empty; - auto callback = [](const Empty::SharedPtr msg) { + auto callback = [](Empty::ConstSharedPtr msg) { (void)msg; }; { @@ -216,7 +216,7 @@ TEST_F(TestSubscriptionSub, construction_and_destruction) { TEST_F(TestSubscription, various_creation_signatures) { initialize(); using test_msgs::msg::Empty; - auto cb = [](test_msgs::msg::Empty::SharedPtr) {}; + auto cb = [](test_msgs::msg::Empty::ConstSharedPtr) {}; { auto sub = node->create_subscription("topic", 1, cb); (void)sub; @@ -440,6 +440,146 @@ TEST_F(TestSubscription, handle_loaned_message) { EXPECT_NO_THROW(sub->handle_loaned_message(&msg, message_info)); } +/* + Testing on_new_message callbacks. + */ +TEST_F(TestSubscription, on_new_message_callback) { + initialize(rclcpp::NodeOptions().use_intra_process_comms(false)); + using test_msgs::msg::Empty; + + auto do_nothing = [](std::shared_ptr) {FAIL();}; + auto sub = node->create_subscription("~/test_take", 10, do_nothing); + + std::atomic c1 {0}; + auto increase_c1_cb = [&c1](size_t count_msgs) {c1 += count_msgs;}; + sub->set_on_new_message_callback(increase_c1_cb); + + auto pub = node->create_publisher("~/test_take", 1); + { + test_msgs::msg::Empty msg; + pub->publish(msg); + } + + auto start = std::chrono::steady_clock::now(); + do { + std::this_thread::sleep_for(100ms); + } while (c1 == 0 && std::chrono::steady_clock::now() - start < 10s); + + EXPECT_EQ(c1.load(), 1u); + + std::atomic c2 {0}; + auto increase_c2_cb = [&c2](size_t count_msgs) {c2 += count_msgs;}; + sub->set_on_new_message_callback(increase_c2_cb); + + { + test_msgs::msg::Empty msg; + pub->publish(msg); + } + + start = std::chrono::steady_clock::now(); + do { + std::this_thread::sleep_for(100ms); + } while (c2 == 0 && std::chrono::steady_clock::now() - start < 10s); + + EXPECT_EQ(c1.load(), 1u); + EXPECT_EQ(c2.load(), 1u); + + sub->clear_on_new_message_callback(); + + { + test_msgs::msg::Empty msg; + pub->publish(msg); + pub->publish(msg); + pub->publish(msg); + } + + std::atomic c3 {0}; + auto increase_c3_cb = [&c3](size_t count_msgs) {c3 += count_msgs;}; + sub->set_on_new_message_callback(increase_c3_cb); + + start = std::chrono::steady_clock::now(); + do { + std::this_thread::sleep_for(100ms); + } while (c3 == 0 && std::chrono::steady_clock::now() - start < 10s); + + EXPECT_EQ(c1.load(), 1u); + EXPECT_EQ(c2.load(), 1u); + EXPECT_EQ(c3.load(), 3u); + + std::function invalid_cb = nullptr; + EXPECT_THROW(sub->set_on_new_message_callback(invalid_cb), std::invalid_argument); +} + +/* + Testing on_new_intra_process_message callbacks. + */ +TEST_F(TestSubscription, on_new_intra_process_message_callback) { + initialize(rclcpp::NodeOptions().use_intra_process_comms(true)); + using test_msgs::msg::Empty; + + auto do_nothing = [](std::shared_ptr) {FAIL();}; + auto sub = node->create_subscription("~/test_take", 10, do_nothing); + + std::atomic c1 {0}; + auto increase_c1_cb = [&c1](size_t count_msgs) {c1 += count_msgs;}; + sub->set_on_new_intra_process_message_callback(increase_c1_cb); + + auto pub = node->create_publisher("~/test_take", 1); + { + test_msgs::msg::Empty msg; + pub->publish(msg); + } + + auto start = std::chrono::steady_clock::now(); + do { + std::this_thread::sleep_for(100ms); + } while (c1 == 0 && std::chrono::steady_clock::now() - start < 10s); + + EXPECT_EQ(c1.load(), 1u); + + std::atomic c2 {0}; + auto increase_c2_cb = [&c2](size_t count_msgs) {c2 += count_msgs;}; + sub->set_on_new_intra_process_message_callback(increase_c2_cb); + + { + test_msgs::msg::Empty msg; + pub->publish(msg); + } + + start = std::chrono::steady_clock::now(); + do { + std::this_thread::sleep_for(100ms); + } while (c2 == 0 && std::chrono::steady_clock::now() - start < 10s); + + EXPECT_EQ(c1.load(), 1u); + EXPECT_EQ(c2.load(), 1u); + + sub->clear_on_new_intra_process_message_callback(); + + { + test_msgs::msg::Empty msg; + pub->publish(msg); + pub->publish(msg); + pub->publish(msg); + } + + std::atomic c3 {0}; + auto increase_c3_cb = [&c3](size_t count_msgs) {c3 += count_msgs;}; + sub->set_on_new_intra_process_message_callback(increase_c3_cb); + + start = std::chrono::steady_clock::now(); + do { + std::this_thread::sleep_for(100ms); + } while (c3 == 0 && std::chrono::steady_clock::now() - start < 10s); + + EXPECT_EQ(c1.load(), 1u); + EXPECT_EQ(c2.load(), 1u); + EXPECT_EQ(c3.load(), 3u); + + std::function invalid_cb = nullptr; + EXPECT_THROW(sub->set_on_new_intra_process_message_callback(invalid_cb), std::invalid_argument); +} + /* Testing subscription with intraprocess enabled and invalid QoS */ @@ -506,3 +646,37 @@ 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::SharedPtr 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_publisher_count_api.cpp b/rclcpp/test/rclcpp/test_subscription_publisher_count_api.cpp index 1e71e28c5e..92297e0e04 100644 --- a/rclcpp/test/rclcpp/test_subscription_publisher_count_api.cpp +++ b/rclcpp/test/rclcpp/test_subscription_publisher_count_api.cpp @@ -108,7 +108,7 @@ class TestSubscriptionPublisherCount : public ::testing::Test } protected: - static void OnMessage(const test_msgs::msg::Empty::SharedPtr msg) + static void OnMessage(test_msgs::msg::Empty::ConstSharedPtr msg) { (void)msg; } diff --git a/rclcpp/test/rclcpp/test_subscription_with_type_adapter.cpp b/rclcpp/test/rclcpp/test_subscription_with_type_adapter.cpp new file mode 100644 index 0000000000..54c11bf1b1 --- /dev/null +++ b/rclcpp/test/rclcpp/test_subscription_with_type_adapter.cpp @@ -0,0 +1,563 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/loaned_message.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "../mocking_utils/patch.hpp" +#include "../utils/rclcpp_gtest_macros.hpp" + +#include "test_msgs/msg/empty.hpp" +#include "rclcpp/msg/string.hpp" + + +#ifdef RMW_IMPLEMENTATION +# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX +# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) +#else +# define CLASSNAME(NAME, SUFFIX) NAME +#endif + + +using namespace std::chrono_literals; + +static const int g_max_loops = 200; +static const std::chrono::milliseconds g_sleep_per_loop(10); + + +class TestSubscription : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } + } + +protected: + void initialize(const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions()) + { + node = std::make_shared("my_node", "/ns", node_options); + } + + void TearDown() + { + node.reset(); + } + + rclcpp::Node::SharedPtr node; +}; + +class CLASSNAME (test_intra_process_within_one_node, RMW_IMPLEMENTATION) : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } +}; + +namespace rclcpp +{ + +template<> +struct TypeAdapter +{ + using is_specialized = std::true_type; + using custom_type = std::string; + using ros_message_type = rclcpp::msg::String; + + static void + convert_to_ros_message( + const custom_type & source, + ros_message_type & destination) + { + destination.data = source; + } + + static void + convert_to_custom( + const ros_message_type & source, + custom_type & destination) + { + destination = source.data; + } +}; + +} // namespace rclcpp + +void wait_for_message_to_be_received( + bool & is_received, + const std::shared_ptr & node) +{ + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + executor.spin_once(std::chrono::milliseconds(0)); + int i = 0; + while (!is_received && i < g_max_loops) { + printf("spin_node_once() - callback (1) expected - try %d/%d\n", ++i, g_max_loops); + executor.spin_once(g_sleep_per_loop); + } +} +/* + * Testing publisher creation signatures with a type adapter. + */ +TEST_F(TestSubscription, various_creation_signatures) { + initialize(); + { + using StringTypeAdapter = rclcpp::TypeAdapter; + auto sub = + node->create_subscription("topic", 42, [](const std::string &) {}); + (void)sub; + } + { + using StringTypeAdapter = rclcpp::adapt_type::as; + auto sub = + node->create_subscription("topic", 42, [](const std::string &) {}); + (void)sub; + } +} + +/* + * Testing that subscriber receives type adapted types and ROS message types with intra proccess communications. + */ +TEST_F( + CLASSNAME(test_intra_process_within_one_node, RMW_IMPLEMENTATION), + check_type_adapted_messages_are_received_by_intra_process_subscription) { + using StringTypeAdapter = rclcpp::TypeAdapter; + const std::string message_data = "Message Data"; + const std::string topic_name = "topic_name"; + + auto node = rclcpp::Node::make_shared( + "test_intra_process", + rclcpp::NodeOptions().use_intra_process_comms(true)); + auto pub = node->create_publisher(topic_name, 1); + + rclcpp::msg::String msg; + msg.data = message_data; + + { + { // const std::string & + bool is_received = false; + auto callback = + [message_data, &is_received]( + const std::string & msg) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), msg.c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + pub->publish(msg); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // const std::string & with message info + bool is_received = false; + auto callback = + [message_data, &is_received]( + const std::string & msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), msg.c_str()); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + pub->publish(msg); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // std::shared_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + pub->publish(msg); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // std::shared_ptr with message info + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + pub->publish(msg); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // std::unique_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::unique_ptr msg) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + pub->publish(msg); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // std::unique_ptr with message info + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::unique_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + pub->publish(msg); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // const rclcpp::msg::String & + bool is_received = false; + auto callback = + [message_data, &is_received]( + const rclcpp::msg::String & msg) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), msg.data.c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + pub->publish(msg); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // const rclcpp::msg::String & with message info + bool is_received = false; + auto callback = + [message_data, &is_received]( + const rclcpp::msg::String & msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), msg.data.c_str()); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + pub->publish(msg); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // std::shared_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), msg->data.c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + pub->publish(msg); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // std::shared_ptr with message info + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), msg->data.c_str()); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + pub->publish(msg); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // std::unique_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::unique_ptr msg) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), msg->data.c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + pub->publish(msg); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // std::unique_ptr with message info + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::unique_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), msg->data.c_str()); + ASSERT_TRUE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + pub->publish(msg); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + } +} + +/* + * Testing that subscriber receives type adapted types and ROS message types with inter proccess communications. + */ +TEST_F( + CLASSNAME(test_intra_process_within_one_node, RMW_IMPLEMENTATION), + check_type_adapted_messages_are_received_by_inter_process_subscription) { + using StringTypeAdapter = rclcpp::TypeAdapter; + const std::string message_data = "Message Data"; + const std::string topic_name = "topic_name"; + + auto node = rclcpp::Node::make_shared( + "test_intra_process", + rclcpp::NodeOptions().use_intra_process_comms(false)); + auto pub = node->create_publisher(topic_name, 1); + + rclcpp::msg::String msg; + msg.data = message_data; + + { + { // const std::string & + bool is_received = false; + auto callback = + [message_data, &is_received](const std::string & msg) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), msg.c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + pub->publish(msg); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // const std::string & with message info + bool is_received = false; + auto callback = + [message_data, &is_received]( + const std::string & msg, const rclcpp::MessageInfo & message_info) -> void + { + is_received = true; + ASSERT_STREQ(message_data.c_str(), msg.c_str()); + ASSERT_FALSE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + pub->publish(msg); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // std::shared_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + pub->publish(msg); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // std::shared_ptr with message info + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); + ASSERT_FALSE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + pub->publish(msg); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // std::unique_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::unique_ptr msg) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + pub->publish(msg); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // std::unique_ptr with message info + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::unique_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), (*msg).c_str()); + ASSERT_FALSE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + pub->publish(msg); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // const rclcpp::msg::String & + bool is_received = false; + auto callback = + [message_data, &is_received]( + const rclcpp::msg::String & msg) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), msg.data.c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + pub->publish(msg); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // const rclcpp::msg::String & with message info + bool is_received = false; + auto callback = + [message_data, &is_received]( + const rclcpp::msg::String & msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), msg.data.c_str()); + ASSERT_FALSE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + pub->publish(msg); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // std::shared_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), msg->data.c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + pub->publish(msg); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // std::shared_ptr with message info + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::shared_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), msg->data.c_str()); + ASSERT_FALSE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + pub->publish(msg); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + + { // std::unique_ptr + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::unique_ptr msg) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), msg->data.c_str()); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + pub->publish(msg); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + { // std::unique_ptr with message info + bool is_received = false; + auto callback = + [message_data, &is_received]( + std::unique_ptr msg, + const rclcpp::MessageInfo & message_info) -> void { + is_received = true; + ASSERT_STREQ(message_data.c_str(), msg->data.c_str()); + ASSERT_FALSE(message_info.get_rmw_message_info().from_intra_process); + }; + auto sub = node->create_subscription(topic_name, 1, callback); + pub->publish(msg); + ASSERT_FALSE(is_received); + wait_for_message_to_be_received(is_received, node); + ASSERT_TRUE(is_received); + } + } +} diff --git a/rclcpp/test/rclcpp/test_time_source.cpp b/rclcpp/test/rclcpp/test_time_source.cpp index 8901f6b2d4..3b64e9e8b1 100644 --- a/rclcpp/test/rclcpp/test_time_source.cpp +++ b/rclcpp/test/rclcpp/test_time_source.cpp @@ -516,3 +516,229 @@ TEST_F(TestTimeSource, no_pre_jump_callback) { EXPECT_EQ(1, cbo.last_postcallback_id_); EXPECT_EQ(1, cbo.post_callback_calls_); } + +// A TimeSource-inheriting class +// that allows access to TimeSource protected attributes +// use_clock_thread_ and clock_executor_thread_ +class ClockThreadTestingTimeSource : public rclcpp::TimeSource +{ +public: + ClockThreadTestingTimeSource() + : rclcpp::TimeSource() + { + } + + bool GetUseClockThreadOption() + { + return this->use_clock_thread_; + } + + bool IsClockThreadJoinable() + { + return this->clock_executor_thread_.joinable(); + } +}; + +TEST_F(TestTimeSource, check_use_clock_thread_value) { + // Create three nodes, with use_clock_thread option + // respectively set to default, true, and false + + auto default_node_ = std::make_shared( + "default_option_node"); + + auto clock_thread_node_ = std::make_shared( + "clock_thread_node", + rclcpp::NodeOptions().use_clock_thread(true)); + + auto no_clock_thread_node_ = std::make_shared( + "no_clock_thread_node", + rclcpp::NodeOptions().use_clock_thread(false)); + + // Test value of use_clock_thread_ TimeSource attribute + // when the different nodes are attached + + ClockThreadTestingTimeSource ts; + + ts.attachNode(default_node_); + ASSERT_TRUE(ts.GetUseClockThreadOption()); + ts.detachNode(); + + ts.attachNode(clock_thread_node_); + ASSERT_TRUE(ts.GetUseClockThreadOption()); + ts.detachNode(); + + ts.attachNode(no_clock_thread_node_); + ASSERT_FALSE(ts.GetUseClockThreadOption()); + ts.detachNode(); +} + +TEST_F(TestTimeSource, check_clock_thread_status) { + // Test if TimeSource clock-dedicated thread is running + // according to the use_sim_time parameter + // and to the options of the attached node + ClockThreadTestingTimeSource ts; + + // Tests for default options node + auto default_node_ = std::make_shared( + "default_option_node"); + + default_node_->set_parameter(rclcpp::Parameter("use_sim_time", true)); + ts.attachNode(default_node_); + ASSERT_TRUE(ts.IsClockThreadJoinable()); + ts.detachNode(); + + default_node_->set_parameter(rclcpp::Parameter("use_sim_time", false)); + ts.attachNode(default_node_); + ASSERT_FALSE(ts.IsClockThreadJoinable()); + ts.detachNode(); + + // Tests for node with use_clock_thread option forced to false + auto no_clock_thread_node_ = std::make_shared( + "no_clock_thread_node", + rclcpp::NodeOptions().use_clock_thread(false)); + + no_clock_thread_node_->set_parameter(rclcpp::Parameter("use_sim_time", true)); + ts.attachNode(no_clock_thread_node_); + ASSERT_FALSE(ts.IsClockThreadJoinable()); + ts.detachNode(); + + no_clock_thread_node_->set_parameter(rclcpp::Parameter("use_sim_time", false)); + ts.attachNode(no_clock_thread_node_); + ASSERT_FALSE(ts.IsClockThreadJoinable()); + ts.detachNode(); +} + +// A Node-inheriting class +// that regularly publishes a incremented Clock msg on topic `/clock' +class SimClockPublisherNode : public rclcpp::Node +{ +public: + SimClockPublisherNode() + : rclcpp::Node("sim_clock_publisher_node") + { + // Create a clock publisher + clock_pub_ = this->create_publisher( + "/clock", + rclcpp::QoS(1) + ); + + // Create a 1ms timer + pub_timer_ = this->create_wall_timer( + std::chrono::milliseconds(1), + std::bind( + &SimClockPublisherNode::timer_callback, + this) + ); + + // Init clock msg to zero + clock_msg_.clock.sec = 0; + clock_msg_.clock.nanosec = 0; + } + + ~SimClockPublisherNode() + { + // Cleanly stop executor and thread + node_executor.cancel(); + node_thread_.join(); + } + + void SpinNode() + { + // Spin node in its own dedicated thread + node_thread_ = std::thread( + [this]() { + node_executor.add_node(this->get_node_base_interface()); + node_executor.spin(); + }); + } + +private: + void timer_callback() + { + // Increment clock msg and publish it + clock_msg_.clock.nanosec += 1000000; + clock_pub_->publish(clock_msg_); + } + + rclcpp::Publisher::SharedPtr clock_pub_; + rclcpp::TimerBase::SharedPtr pub_timer_; + rosgraph_msgs::msg::Clock clock_msg_; + std::thread node_thread_; + rclcpp::executors::SingleThreadedExecutor node_executor; +}; + +// A Node-inheriting class +// that check its clock time within a timer callback +class ClockThreadTestingNode : public rclcpp::Node +{ +public: + ClockThreadTestingNode() + : rclcpp::Node("clock_thread_testing_node") + { + // Set use_sim_time parameter to true to subscribe to `/clock` topic + this->set_parameter(rclcpp::Parameter("use_sim_time", true)); + + // Create a 100ms timer + timer_ = this->create_wall_timer( + std::chrono::milliseconds(100), + std::bind( + &ClockThreadTestingNode::timer_callback, + this) + ); + } + + bool GetIsCallbackFrozen() + { + return is_callback_frozen_; + } + +private: + void timer_callback() + { + rclcpp::Time start_time = this->now(); + bool is_time_out = false; + + // While loop condition tests + // if the node's clock time is incremented + while (rclcpp::ok() && + !is_time_out) + { + rclcpp::sleep_for(std::chrono::milliseconds(100)); + rclcpp::Time time_now = this->now(); + rclcpp::Duration time_spent = time_now - start_time; + is_time_out = time_spent.seconds() > 1.0; + } + + // If out of while loop, set variable to false + // and cancel timer to avoid to enter the callback again + is_callback_frozen_ = false; + timer_->cancel(); + } + + rclcpp::TimerBase::SharedPtr timer_; + bool is_callback_frozen_ = true; +}; + +TEST_F(TestTimeSource, check_sim_time_updated_in_callback_if_use_clock_thread) { + // Test if clock time of a node with + // parameter use_sim_time = true and option use_clock_thread = true + // is updated while node is not spinning + // (in a timer callback) + + // Create a "sim time" publisher and spin it + SimClockPublisherNode pub_node; + pub_node.SpinNode(); + + // Spin node for 2 seconds + ClockThreadTestingNode clock_thread_testing_node; + auto steady_clock = rclcpp::Clock(RCL_STEADY_TIME); + auto start_time = steady_clock.now(); + while (rclcpp::ok() && + (steady_clock.now() - start_time).seconds() < 2.0) + { + rclcpp::spin_some(clock_thread_testing_node.get_node_base_interface()); + } + + // Node should have get out of timer callback + ASSERT_FALSE(clock_thread_testing_node.GetIsCallbackFrozen()); +} diff --git a/rclcpp/test/rclcpp/test_typesupport_helpers.cpp b/rclcpp/test/rclcpp/test_typesupport_helpers.cpp new file mode 100644 index 0000000000..8cdcfc19c0 --- /dev/null +++ b/rclcpp/test/rclcpp/test_typesupport_helpers.cpp @@ -0,0 +1,77 @@ +// Copyright 2018, Bosch Software Innovations GmbH. +// Copyright 2021, Apex.AI Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include + +#include "rcpputils/shared_library.hpp" + +#include "rclcpp/typesupport_helpers.hpp" + +using namespace ::testing; // NOLINT + +TEST(TypesupportHelpersTest, throws_exception_if_filetype_has_no_type) { + EXPECT_ANY_THROW( + rclcpp::get_typesupport_library("just_a_package_name", "rosidl_typesupport_cpp")); +} + +TEST(TypesupportHelpersTest, throws_exception_if_filetype_has_slash_at_the_start_only) { + EXPECT_ANY_THROW( + rclcpp::get_typesupport_library("/name_with_slash_at_start", "rosidl_typesupport_cpp")); +} + +TEST(TypesupportHelpersTest, throws_exception_if_filetype_has_slash_at_the_end_only) { + EXPECT_ANY_THROW( + rclcpp::get_typesupport_library("name_with_slash_at_end/", "rosidl_typesupport_cpp")); +} + +TEST(TypesupportHelpersTest, throws_exception_if_library_cannot_be_found) { + EXPECT_THROW( + rclcpp::get_typesupport_library("invalid/message", "rosidl_typesupport_cpp"), + std::runtime_error); +} + +TEST(TypesupportHelpersTest, returns_c_type_info_for_valid_legacy_library) { + try { + auto library = rclcpp::get_typesupport_library( + "test_msgs/BasicTypes", "rosidl_typesupport_cpp"); + auto string_typesupport = rclcpp::get_typesupport_handle( + "test_msgs/BasicTypes", "rosidl_typesupport_cpp", *library); + + EXPECT_THAT( + std::string(string_typesupport->typesupport_identifier), + ContainsRegex("rosidl_typesupport")); + } catch (const std::exception & e) { + FAIL() << e.what(); + } +} + +TEST(TypesupportHelpersTest, returns_c_type_info_for_valid_library) { + try { + auto library = rclcpp::get_typesupport_library( + "test_msgs/msg/BasicTypes", "rosidl_typesupport_cpp"); + auto string_typesupport = rclcpp::get_typesupport_handle( + "test_msgs/msg/BasicTypes", "rosidl_typesupport_cpp", *library); + + EXPECT_THAT( + std::string(string_typesupport->typesupport_identifier), + ContainsRegex("rosidl_typesupport")); + } catch (const std::runtime_error & e) { + FAIL() << e.what(); + } +} diff --git a/rclcpp/test/rclcpp/test_wait_set.cpp b/rclcpp/test/rclcpp/test_wait_set.cpp index bbfc9383c8..bf7ee26ed0 100644 --- a/rclcpp/test/rclcpp/test_wait_set.cpp +++ b/rclcpp/test/rclcpp/test_wait_set.cpp @@ -221,7 +221,7 @@ TEST_F(TestWaitSet, add_guard_condition_to_two_different_wait_set) { wait_set2.add_guard_condition(guard_condition); }, std::runtime_error); - auto do_nothing = [](const std::shared_ptr) {}; + auto do_nothing = [](std::shared_ptr) {}; auto sub = node->create_subscription("~/test", 1, do_nothing); wait_set1.add_subscription(sub); ASSERT_THROW( @@ -257,7 +257,7 @@ TEST_F(TestWaitSet, add_guard_condition_to_two_different_wait_set) { rclcpp::PublisherOptions po; po.event_callbacks.deadline_callback = [](rclcpp::QOSDeadlineOfferedInfo &) {}; auto pub = node->create_publisher("~/test", 1, po); - auto qos_event = pub->get_event_handlers()[0]; + auto qos_event = pub->get_event_handlers().begin()->second; wait_set1.add_waitable(qos_event, pub); ASSERT_THROW( { @@ -281,7 +281,7 @@ TEST_F(TestWaitSet, add_remove_wait) { rclcpp::SubscriptionOptions subscription_options; subscription_options.event_callbacks.deadline_callback = [](auto) {}; subscription_options.event_callbacks.liveliness_callback = [](auto) {}; - auto do_nothing = [](const std::shared_ptr) {}; + auto do_nothing = [](std::shared_ptr) {}; auto sub = node->create_subscription( "~/test", 1, do_nothing, subscription_options); @@ -301,7 +301,7 @@ TEST_F(TestWaitSet, add_remove_wait) { [](rclcpp::QOSDeadlineOfferedInfo &) {}; auto pub = node->create_publisher( "~/test", 1, publisher_options); - auto qos_event = pub->get_event_handlers()[0]; + auto qos_event = pub->get_event_handlers().begin()->second; // Subscription mask is required here for coverage. wait_set.add_subscription(sub, {true, true, true}); diff --git a/rclcpp/test/rclcpp/wait_set_policies/test_dynamic_storage.cpp b/rclcpp/test/rclcpp/wait_set_policies/test_dynamic_storage.cpp index 2282a92bb1..7a15e2cf49 100644 --- a/rclcpp/test/rclcpp/wait_set_policies/test_dynamic_storage.cpp +++ b/rclcpp/test/rclcpp/wait_set_policies/test_dynamic_storage.cpp @@ -74,7 +74,7 @@ TEST_F(TestDynamicStorage, default_construct_destruct) { TEST_F(TestDynamicStorage, iterables_construct_destruct) { auto subscription = node->create_subscription( - "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {}); + "topic", 10, [](test_msgs::msg::Empty::ConstSharedPtr) {}); auto timer = node->create_wall_timer(std::chrono::seconds(100), []() {}); auto guard_condition = std::make_shared(); auto service = @@ -110,7 +110,7 @@ TEST_F(TestDynamicStorage, add_remove_dynamically) { options.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; auto subscription = node->create_subscription( - "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {}, options); + "topic", 10, [](test_msgs::msg::Empty::ConstSharedPtr) {}, options); rclcpp::SubscriptionWaitSetMask mask{true, true, true}; wait_set.add_subscription(subscription, mask); @@ -203,7 +203,7 @@ TEST_F(TestDynamicStorage, add_remove_out_of_scope) { { auto subscription = node->create_subscription( - "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {}); + "topic", 10, [](test_msgs::msg::Empty::ConstSharedPtr) {}); wait_set.add_subscription(subscription); // This is short, so if it's not cleaned up, it will trigger wait and it won't timeout @@ -238,7 +238,7 @@ TEST_F(TestDynamicStorage, wait_subscription) { auto publisher = node->create_publisher("topic", 10); auto subscription = node->create_subscription( - "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {}); + "topic", 10, [](test_msgs::msg::Empty::ConstSharedPtr) {}); wait_set.add_subscription(subscription); { 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 8e406276f1..1e4d5f805e 100644 --- a/rclcpp/test/rclcpp/wait_set_policies/test_static_storage.cpp +++ b/rclcpp/test/rclcpp/wait_set_policies/test_static_storage.cpp @@ -67,7 +67,7 @@ class TestWaitable : public rclcpp::Waitable TEST_F(TestStaticStorage, iterables_construct_destruct) { auto subscription = node->create_subscription( - "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {}); + "topic", 10, [](test_msgs::msg::Empty::ConstSharedPtr) {}); // This is long, so it can stick around and be removed auto timer = node->create_wall_timer(std::chrono::seconds(100), []() {}); auto guard_condition = std::make_shared(); @@ -132,7 +132,7 @@ TEST_F(TestStaticStorage, fixed_storage_needs_pruning) { TEST_F(TestStaticStorage, wait_subscription) { auto publisher = node->create_publisher("topic", 10); auto subscription = node->create_subscription( - "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {}); + "topic", 10, [](test_msgs::msg::Empty::ConstSharedPtr) {}); rclcpp::SubscriptionWaitSetMask mask{true, true, true}; rclcpp::StaticWaitSet<1, 0, 0, 0, 0, 0> wait_set({{{subscription, mask}}}); 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 130d281e13..ba0f4d16e8 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 @@ -79,7 +79,7 @@ TEST_F(TestStoragePolicyCommon, rcl_wait_set_resize_error) { rclcpp::WaitSet wait_set; auto subscription = node->create_subscription( - "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {}); + "topic", 10, [](test_msgs::msg::Empty::ConstSharedPtr) {}); rclcpp::SubscriptionWaitSetMask mask{true, true, true}; auto mock = mocking_utils::patch_and_return( @@ -103,7 +103,7 @@ TEST_F(TestStoragePolicyCommon, rcl_wait_set_clear_error) { TEST_F(TestStoragePolicyCommon, rcl_wait_set_add_subscription_error) { rclcpp::WaitSet wait_set; auto subscription = node->create_subscription( - "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {}); + "topic", 10, [](test_msgs::msg::Empty::ConstSharedPtr) {}); rclcpp::SubscriptionWaitSetMask mask{true, true, true}; auto mock = mocking_utils::patch_and_return( 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 8f4870f156..99d07f1a19 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 @@ -75,7 +75,7 @@ TEST_F(TestThreadSafeStorage, default_construct_destruct) { TEST_F(TestThreadSafeStorage, iterables_construct_destruct) { auto subscription = node->create_subscription( - "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {}); + "topic", 10, [](test_msgs::msg::Empty::ConstSharedPtr) {}); // This is long, so it can stick around auto timer = node->create_wall_timer(std::chrono::seconds(100), []() {}); auto guard_condition = std::make_shared(); @@ -113,7 +113,7 @@ TEST_F(TestThreadSafeStorage, add_remove_dynamically) { options.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; auto subscription = node->create_subscription( - "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {}, options); + "topic", 10, [](test_msgs::msg::Empty::ConstSharedPtr) {}, options); rclcpp::SubscriptionWaitSetMask mask{true, true, true}; wait_set.add_subscription(subscription, mask); @@ -207,7 +207,7 @@ TEST_F(TestThreadSafeStorage, add_remove_out_of_scope) { { auto subscription = node->create_subscription( - "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {}); + "topic", 10, [](test_msgs::msg::Empty::ConstSharedPtr) {}); wait_set.add_subscription(subscription); // This is short, so if it's not cleaned up, it will trigger wait @@ -242,7 +242,7 @@ TEST_F(TestThreadSafeStorage, wait_subscription) { auto publisher = node->create_publisher("topic", 10); auto subscription = node->create_subscription( - "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {}); + "topic", 10, [](test_msgs::msg::Empty::ConstSharedPtr) {}); wait_set.add_subscription(subscription); { diff --git a/rclcpp/test/resources/test_node/load_parameters.yaml b/rclcpp/test/resources/test_node/load_parameters.yaml new file mode 100644 index 0000000000..0171032221 --- /dev/null +++ b/rclcpp/test/resources/test_node/load_parameters.yaml @@ -0,0 +1,18 @@ +/**: + ros__parameters: + bar: 5 + foo: 3.5 + +/*: + load_node: + ros__parameters: + bar_foo: "ok" + +namespace: + load_node: + ros__parameters: + foo_bar: true + +bar: + ros__parameters: + fatal: 10 diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index f3a2cc18c2..2297e85055 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,46 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +11.0.0 (2021-05-18) +------------------- +* Bump the benchmark timeout for benchmark_action_client (`#1671 `_) +* Contributors: Scott K Logan + +10.0.0 (2021-05-11) +------------------- +* Returns CancelResponse::REJECT while goal handle failed to transit to CANCELING state (`#1641 `_) +* Fix action server deadlock issue that caused by other mutexes locked in CancelCallback (`#1635 `_) +* Contributors: Kaven Yau + +9.0.2 (2021-04-14) +------------------ + +9.0.1 (2021-04-12) +------------------ + +9.0.0 (2021-04-06) +------------------ +* updating quality declaration links (re: `ros2/docs.ros2.org#52 `_) (`#1615 `_) +* Contributors: shonigmann + +8.2.0 (2021-03-31) +------------------ + +8.1.0 (2021-03-25) +------------------ + +8.0.0 (2021-03-23) +------------------ + +7.0.1 (2021-03-22) +------------------ + +7.0.0 (2021-03-18) +------------------ +* Add support for rmw_connextdds (`#1574 `_) +* node_handle must be destroyed after client_handle to prevent memory leak (`#1562 `_) +* Contributors: Andrea Sorbini, Tomoya Fujita + 6.3.1 (2021-02-08) ------------------ * Finalize rcl_handle to prevent leak (`#1528 `_) (`#1529 `_) diff --git a/rclcpp_action/QUALITY_DECLARATION.md b/rclcpp_action/QUALITY_DECLARATION.md index 47c9b5ec2f..7512ad3312 100644 --- a/rclcpp_action/QUALITY_DECLARATION.md +++ b/rclcpp_action/QUALITY_DECLARATION.md @@ -4,13 +4,13 @@ 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://index.ros.org/doc/ros2/Contributing/Developer-Guide/#package-quality-categories) of the ROS2 developer guide. +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. ## Version Policy [1] ### Version Scheme [1.i] -`rclcpp_action` uses `semver` according to the recommendation for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#versioning). +`rclcpp_action` uses `semver` according to the recommendation for ROS Core packages in the [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#versioning). ### Version Stability [1.ii] @@ -37,11 +37,11 @@ All installed headers are in the `include` directory of the package, headers in ## Change Control Process [2] -`rclcpp_action` follows the recommended guidelines for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#change-control-process). +`rclcpp_action` follows the recommended guidelines for ROS Core packages in the [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#change-control-process). ### Change Requests [2.i] -All changes will occur through a pull request, check [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#change-control-process) for additional information. +All changes will occur through a pull request, check [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#change-control-process) for additional information. ### Contributor Origin [2.ii] @@ -49,7 +49,7 @@ This package uses DCO as its confirmation of contributor origin policy. More inf ### Peer Review Policy [2.iii] -All pull requests will be peer-reviewed, check [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#change-control-process) for additional information. +All pull requests will be peer-reviewed, check [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#change-control-process) for additional information. ### Continuous Integration [2.iv] @@ -109,7 +109,7 @@ The tests aim to cover both typical usage and corner cases, but are quantified b ### Coverage [4.iii] -`rclcpp_action` follows the recommendations for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#code-coverage), and opts to use line coverage instead of branch coverage. +`rclcpp_action` follows the recommendations for ROS Core packages in the [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#code-coverage), and opts to use line coverage instead of branch coverage. This includes: @@ -119,11 +119,11 @@ This includes: Changes are required to make a best effort to keep or increase coverage before being accepted, but decreases are allowed if properly justified and accepted by reviewers. -Current coverage statistics can be viewed [here](https://ci.ros2.org/job/nightly_linux_coverage/lastCompletedBuild/cobertura/src_ros2_rclcpp_rclcpp_action_src/). A description of how coverage statistics are calculated is summarized in this page ["ROS 2 Onboarding Guide"](https://index.ros.org/doc/ros2/Contributing/ROS-2-On-boarding-Guide/#note-on-coverage-runs). +Current coverage statistics can be viewed [here](https://ci.ros2.org/job/nightly_linux_coverage/lastCompletedBuild/cobertura/src_ros2_rclcpp_rclcpp_action_src/). A description of how coverage statistics are calculated is summarized in this page ["ROS 2 Onboarding Guide"](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#note-on-coverage-runs). ### Performance [4.iv] -`rclcpp_action` follows the recommendations for performance testing of C/C++ code in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#performance), and opts to do performance analysis on each release rather than each change. +`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). @@ -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://index.ros.org/doc/ros2/Contributing/Developer-Guide/#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 ROS2 standard linters and static analysis tools for a C++ package as described in the [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#linters-and-static-analysis). Passing implies there are no linter/static errors when testing against CI of supported platforms. Currently nightly test results can be seen here: * [linux-aarch64_release](https://ci.ros2.org/view/nightly/job/nightly_linux-aarch64_release/lastBuild/testReport/rclcpp_action/) diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index b3871060cd..f9488fc1d5 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -356,15 +356,26 @@ class Server : public ServerBase, public std::enable_shared_from_this lock(goal_handles_mutex_); + std::shared_ptr> goal_handle; + { + std::lock_guard lock(goal_handles_mutex_); + auto element = goal_handles_.find(uuid); + if (element != goal_handles_.end()) { + goal_handle = element->second.lock(); + } + } + CancelResponse resp = CancelResponse::REJECT; - auto element = goal_handles_.find(uuid); - if (element != goal_handles_.end()) { - std::shared_ptr> goal_handle = element->second.lock(); - if (goal_handle) { - resp = handle_cancel_(goal_handle); - if (CancelResponse::ACCEPT == resp) { + if (goal_handle) { + resp = handle_cancel_(goal_handle); + if (CancelResponse::ACCEPT == resp) { + try { goal_handle->_cancel_goal(); + } catch (const rclcpp::exceptions::RCLError & ex) { + RCLCPP_DEBUG( + rclcpp::get_logger("rclcpp_action"), + "Failed to cancel goal in call_handle_cancel_callback: %s", ex.what()); + return CancelResponse::REJECT; } } } diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index e258069f76..e753114b22 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 6.3.1 + 11.0.0 Adds action APIs for C++. Ivan Paunovic Mabel Zhang diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index 6915de7865..509e152384 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -500,9 +500,13 @@ ServerBase::execute_result_request_received(std::shared_ptr & data) result_response = create_result_response(action_msgs::msg::GoalStatus::STATUS_UNKNOWN); } else { // Goal exists, check if a result is already available + std::lock_guard lock(pimpl_->unordered_map_mutex_); auto iter = pimpl_->goal_results_.find(uuid); if (iter != pimpl_->goal_results_.end()) { result_response = iter->second; + } else { + // Store the request so it can be responded to later + pimpl_->result_requests_[uuid].push_back(request_header); } } @@ -514,10 +518,6 @@ ServerBase::execute_result_request_received(std::shared_ptr & data) if (RCL_RET_OK != rcl_ret) { rclcpp::exceptions::throw_from_rcl_error(rcl_ret); } - } else { - // Store the request so it can be responded to later - std::lock_guard lock(pimpl_->unordered_map_mutex_); - pimpl_->result_requests_[uuid].push_back(request_header); } data.reset(); } @@ -627,19 +627,30 @@ ServerBase::publish_result(const GoalUUID & uuid, std::shared_ptr result_m } { - std::lock_guard lock(pimpl_->unordered_map_mutex_); + /** + * NOTE: There is a potential deadlock issue if both unordered_map_mutex_ and + * action_server_reentrant_mutex_ locked in other block scopes. Unless using + * std::scoped_lock, locking order must be consistent with the current. + * + * Current locking order: + * + * 1. unordered_map_mutex_ + * 2. action_server_reentrant_mutex_ + * + */ + std::lock_guard unordered_map_lock(pimpl_->unordered_map_mutex_); pimpl_->goal_results_[uuid] = result_msg; - } - // if there are clients who already asked for the result, send it to them - auto iter = pimpl_->result_requests_.find(uuid); - if (iter != pimpl_->result_requests_.end()) { - for (auto & request_header : iter->second) { + // if there are clients who already asked for the result, send it to them + auto iter = pimpl_->result_requests_.find(uuid); + if (iter != pimpl_->result_requests_.end()) { std::lock_guard lock(pimpl_->action_server_reentrant_mutex_); - rcl_ret_t ret = rcl_action_send_result_response( - pimpl_->action_server_.get(), &request_header, result_msg.get()); - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); + 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) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } } } } diff --git a/rclcpp_action/test/benchmark/CMakeLists.txt b/rclcpp_action/test/benchmark/CMakeLists.txt index b9aabf2375..dc87c553af 100644 --- a/rclcpp_action/test/benchmark/CMakeLists.txt +++ b/rclcpp_action/test/benchmark/CMakeLists.txt @@ -7,7 +7,7 @@ find_package(performance_test_fixture REQUIRED) add_performance_test( benchmark_action_client benchmark_action_client.cpp - TIMEOUT 120) + TIMEOUT 240) if(TARGET benchmark_action_client) target_link_libraries(benchmark_action_client ${PROJECT_NAME}) ament_target_dependencies(benchmark_action_client rclcpp test_msgs) diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index 7ff9838658..1a0b074ba3 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -1306,3 +1306,15 @@ TEST_F(TestDeadlockServer, deadlock_while_canceled) send_goal_request(node_, uuid2_); // deadlock here t.join(); } + +TEST_F(TestDeadlockServer, deadlock_while_succeed_and_canceled) +{ + send_goal_request(node_, uuid1_); + std::thread t(&TestDeadlockServer::GoalSucceeded, this); + rclcpp::sleep_for(std::chrono::milliseconds(50)); + auto response_ptr = send_cancel_request(node_, uuid1_); + + // current goal handle is not cancelable, so it returns ERROR_REJECTED + EXPECT_EQ(CancelResponse::ERROR_REJECTED, response_ptr->return_code); + t.join(); +} diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 4848e0b1bc..814e51ac84 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,38 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +11.0.0 (2021-05-18) +------------------- + +10.0.0 (2021-05-11) +------------------- + +9.0.2 (2021-04-14) +------------------ + +9.0.1 (2021-04-12) +------------------ + +9.0.0 (2021-04-06) +------------------ +* updating quality declaration links (re: `ros2/docs.ros2.org#52 `_) (`#1615 `_) +* Contributors: shonigmann + +8.2.0 (2021-03-31) +------------------ + +8.1.0 (2021-03-25) +------------------ + +8.0.0 (2021-03-23) +------------------ + +7.0.1 (2021-03-22) +------------------ + +7.0.0 (2021-03-18) +------------------ + 6.3.1 (2021-02-08) ------------------ diff --git a/rclcpp_components/QUALITY_DECLARATION.md b/rclcpp_components/QUALITY_DECLARATION.md index b46f1f9fde..5b8efa934d 100644 --- a/rclcpp_components/QUALITY_DECLARATION.md +++ b/rclcpp_components/QUALITY_DECLARATION.md @@ -4,13 +4,13 @@ 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://index.ros.org/doc/ros2/Contributing/Developer-Guide/#package-quality-categories) of the ROS2 developer guide. +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. ## Version Policy [1] ### Version Scheme [1.i] -`rclcpp_components` uses `semver` according to the recommendation for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#versioning). +`rclcpp_components` uses `semver` according to the recommendation for ROS Core packages in the [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#versioning). ### Version Stability [1.ii] @@ -37,11 +37,11 @@ All installed headers are in the `include` directory of the package, headers in ## Change Control Process [2] -`rclcpp_components` follows the recommended guidelines for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#change-control-process). +`rclcpp_components` follows the recommended guidelines for ROS Core packages in the [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#change-control-process). ### Change Requests [2.i] -All changes will occur through a pull request, check [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#change-control-process) for additional information. +All changes will occur through a pull request, check [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#change-control-process) for additional information. ### Contributor Origin [2.ii] @@ -49,7 +49,7 @@ This package uses DCO as its confirmation of contributor origin policy. More inf ### Peer Review Policy [2.iii] -All pull requests will be peer-reviewed, check [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#change-control-process) for additional information. +All pull requests will be peer-reviewed, check [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#change-control-process) for additional information. ### Continuous Integration [2.iv] @@ -109,7 +109,7 @@ The tests aim to cover both typical usage and corner cases, but are quantified b ### Coverage [4.iii] -`rclcpp_components` follows the recommendations for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#code-coverage), and opts to use line coverage instead of branch coverage. +`rclcpp_components` follows the recommendations for ROS Core packages in the [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#code-coverage), and opts to use line coverage instead of branch coverage. This includes: @@ -119,11 +119,11 @@ This includes: Changes are required to make a best effort to keep or increase coverage before being accepted, but decreases are allowed if properly justified and accepted by reviewers. -Current coverage statistics can be viewed [here](https://ci.ros2.org/job/nightly_linux_coverage/lastCompletedBuild/cobertura/src_ros2_rclcpp_rclcpp_components_src/). A description of how coverage statistics are calculated is summarized in this page ["ROS 2 Onboarding Guide"](https://index.ros.org/doc/ros2/Contributing/ROS-2-On-boarding-Guide/#note-on-coverage-runs). +Current coverage statistics can be viewed [here](https://ci.ros2.org/job/nightly_linux_coverage/lastCompletedBuild/cobertura/src_ros2_rclcpp_rclcpp_components_src/). A description of how coverage statistics are calculated is summarized in this page ["ROS 2 Onboarding Guide"](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#note-on-coverage-runs). ### Performance [4.iv] -`rclcpp_components` follows the recommendations for performance testing of C/C++ code in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#performance), and opts to do performance analysis on each release rather than each change. +`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). @@ -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://index.ros.org/doc/ros2/Contributing/Developer-Guide/#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 ROS2 standard linters and static analysis tools for a C++ package as described in the [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#linters-and-static-analysis). Passing implies there are no linter/static errors when testing against CI of supported platforms. Currently nightly test results can be seen here: * [linux-aarch64_release](https://ci.ros2.org/view/nightly/job/nightly_linux-aarch64_release/lastBuild/testReport/rclcpp_components/) diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index 4f35fcd2b3..51b80d3f4c 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 6.3.1 + 11.0.0 Package containing tools for dynamically loadable components Ivan Paunovic Mabel Zhang diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 4dbf85da44..6cd8855377 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,54 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +11.0.0 (2021-05-18) +------------------- +* Fix destruction order in lifecycle benchmark (`#1675 `_) +* Contributors: Scott K Logan + +10.0.0 (2021-05-11) +------------------- +* [rclcpp] Type Adaptation feature (`#1557 `_) +* Contributors: Audrow Nash, William Woodall + +9.0.2 (2021-04-14) +------------------ + +9.0.1 (2021-04-12) +------------------ + +9.0.0 (2021-04-06) +------------------ +* Add generic publisher and generic subscription for serialized messages (`#1452 `_) +* updating quality declaration links (re: `ros2/docs.ros2.org#52 `_) (`#1615 `_) +* Contributors: Nikolai Morin, shonigmann + +8.2.0 (2021-03-31) +------------------ +* Fix flaky lifecycle node tests (`#1606 `_) +* Clock subscription callback group spins in its own thread (`#1556 `_) +* Delete debug messages (`#1602 `_) +* add automatically_add_executor_with_node option (`#1594 `_) +* Contributors: BriceRenaudeau, Ivan Santiago Paunovic, Jacob Perron, anaelle-sw + +8.1.0 (2021-03-25) +------------------ + +8.0.0 (2021-03-23) +------------------ +* make rcl_lifecyle_com_interface optional in lifecycle nodes (`#1507 `_) +* Contributors: Karsten Knese + +7.0.1 (2021-03-22) +------------------ + +7.0.0 (2021-03-18) +------------------ +* Add support for rmw_connextdds (`#1574 `_) +* Fix SEGV caused by order of destruction of Node sub-interfaces (`#1469 `_) +* Enforce static parameter types (`#1522 `_) +* Contributors: Andrea Sorbini, Colin MacKenzie, Ivan Santiago Paunovic + 6.3.1 (2021-02-08) ------------------ diff --git a/rclcpp_lifecycle/QUALITY_DECLARATION.md b/rclcpp_lifecycle/QUALITY_DECLARATION.md index c0c60fd8a9..5a37ad395b 100644 --- a/rclcpp_lifecycle/QUALITY_DECLARATION.md +++ b/rclcpp_lifecycle/QUALITY_DECLARATION.md @@ -4,13 +4,13 @@ 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://index.ros.org/doc/ros2/Contributing/Developer-Guide/#package-quality-categories) of the ROS2 developer guide. +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. ## Version Policy [1] ### Version Scheme [1.i] -`rclcpp_lifecycle` uses `semver` according to the recommendation for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#versioning). +`rclcpp_lifecycle` uses `semver` according to the recommendation for ROS Core packages in the [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#versioning). ### Version Stability [1.ii] @@ -37,11 +37,11 @@ All installed headers are in the `include` directory of the package, headers in ## Change Control Process [2] -`rclcpp_lifecycle` follows the recommended guidelines for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#change-control-process). +`rclcpp_lifecycle` follows the recommended guidelines for ROS Core packages in the [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#change-control-process). ### Change Requests [2.i] -All changes will occur through a pull request, check [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#change-control-process) for additional information. +All changes will occur through a pull request, check [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#change-control-process) for additional information. ### Contributor Origin [2.ii] @@ -49,7 +49,7 @@ This package uses DCO as its confirmation of contributor origin policy. More inf ### Peer Review Policy [2.iii] -All pull requests will be peer-reviewed, check [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#change-control-process) for additional information. +All pull requests will be peer-reviewed, check [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#change-control-process) for additional information. ### Continuous Integration [2.iv] @@ -109,7 +109,7 @@ The tests aim to cover both typical usage and corner cases, but are quantified b ### Coverage [4.iii] -`rclcpp_lifecycle` follows the recommendations for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#code-coverage), and opts to use line coverage instead of branch coverage. +`rclcpp_lifecycle` follows the recommendations for ROS Core packages in the [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#code-coverage), and opts to use line coverage instead of branch coverage. This includes: @@ -119,11 +119,11 @@ This includes: Changes are required to make a best effort to keep or increase coverage before being accepted, but decreases are allowed if properly justified and accepted by reviewers. -Current coverage statistics can be viewed [here](https://ci.ros2.org/job/nightly_linux_coverage/lastCompletedBuild/cobertura/src_ros2_rclcpp_rclcpp_lifecycle_src/). A description of how coverage statistics are calculated is summarized in this page ["ROS 2 Onboarding Guide"](https://index.ros.org/doc/ros2/Contributing/ROS-2-On-boarding-Guide/#note-on-coverage-runs). +Current coverage statistics can be viewed [here](https://ci.ros2.org/job/nightly_linux_coverage/lastCompletedBuild/cobertura/src_ros2_rclcpp_rclcpp_lifecycle_src/). A description of how coverage statistics are calculated is summarized in this page ["ROS 2 Onboarding Guide"](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#note-on-coverage-runs). ### Performance [4.iv] -`rclcpp_lifecycle` follows the recommendations for performance testing of C/C++ code in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#performance), and opts to do performance analysis on each release rather than each change. +`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). @@ -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://index.ros.org/doc/ros2/Contributing/Developer-Guide/#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 ROS2 standard linters and static analysis tools for a C++ package as described in the [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#linters-and-static-analysis). Passing implies there are no linter/static errors when testing against CI of supported platforms. Currently nightly test results can be seen here: * [linux-aarch64_release](https://ci.ros2.org/view/nightly/job/nightly_linux-aarch64_release/lastBuild/testReport/rclcpp_lifecycle/) diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 8289187cce..1c3379712e 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -57,6 +57,8 @@ #include "rclcpp/clock.hpp" #include "rclcpp/context.hpp" #include "rclcpp/event.hpp" +#include "rclcpp/generic_publisher.hpp" +#include "rclcpp/generic_subscription.hpp" #include "rclcpp/logger.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/message_memory_strategy.hpp" @@ -127,23 +129,27 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, /** * \param[in] node_name Name of the node. * \param[in] options Additional options to control creation of the node. + * \param[in] enable_communication_interface Deciding whether the communication interface of the underlying rcl_lifecycle_node shall be enabled. */ RCLCPP_LIFECYCLE_PUBLIC explicit LifecycleNode( const std::string & node_name, - const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + const rclcpp::NodeOptions & options = rclcpp::NodeOptions(), + bool enable_communication_interface = true); /// Create a node based on the node name and a rclcpp::Context. /** * \param[in] node_name Name of the node. * \param[in] namespace_ Namespace of the node. * \param[in] options Additional options to control creation of the node. + * \param[in] enable_communication_interface Deciding whether the communication interface of the underlying rcl_lifecycle_node shall be enabled. */ RCLCPP_LIFECYCLE_PUBLIC LifecycleNode( const std::string & node_name, const std::string & namespace_, - const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + const rclcpp::NodeOptions & options = rclcpp::NodeOptions(), + bool enable_communication_interface = true); RCLCPP_LIFECYCLE_PUBLIC virtual ~LifecycleNode(); @@ -175,11 +181,16 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, /// Create and return a callback group. /** * \param[in] group_type callback group type to create by this method. + * \param[in] automatically_add_to_executor_with_node A boolean that + * determines whether a callback group is automatically added to an executor + * with the node with which it is associated. * \return a callback group */ RCLCPP_LIFECYCLE_PUBLIC rclcpp::CallbackGroup::SharedPtr - create_callback_group(rclcpp::CallbackGroupType group_type); + create_callback_group( + rclcpp::CallbackGroupType group_type, + bool automatically_add_to_executor_with_node = true); /// Return the list of callback groups in the node. /** @@ -219,13 +230,8 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, typename MessageT, typename CallbackT, typename AllocatorT = std::allocator, - typename CallbackMessageT = - typename rclcpp::subscription_traits::has_message_type::type, typename SubscriptionT = rclcpp::Subscription, - typename MessageMemoryStrategyT = rclcpp::message_memory_strategy::MessageMemoryStrategy< - CallbackMessageT, - AllocatorT - >> + typename MessageMemoryStrategyT = typename SubscriptionT::MessageMemoryStrategyType> std::shared_ptr create_subscription( const std::string & topic_name, @@ -274,6 +280,35 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default, rclcpp::CallbackGroup::SharedPtr group = nullptr); + /// Create and return a GenericPublisher. + /** + * \sa rclcpp::Node::create_generic_publisher + */ + template> + std::shared_ptr create_generic_publisher( + const std::string & topic_name, + const std::string & topic_type, + const rclcpp::QoS & qos, + const rclcpp::PublisherOptionsWithAllocator & options = ( + rclcpp::PublisherOptionsWithAllocator() + ) + ); + + /// Create and return a GenericSubscription. + /** + * \sa rclcpp::Node::create_generic_subscription + */ + template> + std::shared_ptr create_generic_subscription( + const std::string & topic_name, + const std::string & topic_type, + const rclcpp::QoS & qos, + std::function)> callback, + const rclcpp::SubscriptionOptionsWithAllocator & options = ( + rclcpp::SubscriptionOptionsWithAllocator() + ) + ); + /// Declare and initialize a parameter, return the effective value. /** * \sa rclcpp::Node::declare_parameter diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp index 5582d8aca6..a7f1c686bc 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp @@ -22,12 +22,14 @@ #include #include "rclcpp/contexts/default_context.hpp" -#include "rclcpp/event.hpp" -#include "rclcpp/experimental/intra_process_manager.hpp" -#include "rclcpp/parameter.hpp" +#include "rclcpp/create_generic_publisher.hpp" +#include "rclcpp/create_generic_subscription.hpp" #include "rclcpp/create_publisher.hpp" #include "rclcpp/create_service.hpp" #include "rclcpp/create_subscription.hpp" +#include "rclcpp/event.hpp" +#include "rclcpp/experimental/intra_process_manager.hpp" +#include "rclcpp/parameter.hpp" #include "rclcpp/subscription_options.hpp" #include "rclcpp/type_support_decl.hpp" @@ -59,7 +61,6 @@ template< typename MessageT, typename CallbackT, typename AllocatorT, - typename CallbackMessageT, typename SubscriptionT, typename MessageMemoryStrategyT> std::shared_ptr @@ -130,6 +131,46 @@ LifecycleNode::create_service( service_name, std::forward(callback), qos_profile, group); } +template +std::shared_ptr +LifecycleNode::create_generic_publisher( + const std::string & topic_name, + const std::string & topic_type, + const rclcpp::QoS & qos, + const rclcpp::PublisherOptionsWithAllocator & options) +{ + return rclcpp::create_generic_publisher( + node_topics_, + // TODO(karsten1987): LifecycleNode is currently not supporting subnamespaces + // see https://github.com/ros2/rclcpp/issues/1614 + topic_name, + topic_type, + qos, + options + ); +} + +template +std::shared_ptr +LifecycleNode::create_generic_subscription( + const std::string & topic_name, + const std::string & topic_type, + const rclcpp::QoS & qos, + std::function)> callback, + const rclcpp::SubscriptionOptionsWithAllocator & options) +{ + return rclcpp::create_generic_subscription( + node_topics_, + // TODO(karsten1987): LifecycleNode is currently not supporting subnamespaces + // see https://github.com/ros2/rclcpp/issues/1614 + topic_name, + topic_type, + qos, + std::move(callback), + options + ); +} + template auto LifecycleNode::declare_parameter( diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index a9d0fc0965..60095131e5 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 6.3.1 + 11.0.0 Package containing a prototype for lifecycle implementation Ivan Paunovic Mabel Zhang diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index e7408a35de..2370e43b67 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -47,17 +47,20 @@ namespace rclcpp_lifecycle LifecycleNode::LifecycleNode( const std::string & node_name, - const rclcpp::NodeOptions & options) + const rclcpp::NodeOptions & options, + bool enable_communication_interface) : LifecycleNode( node_name, "", - options) + options, + enable_communication_interface) {} LifecycleNode::LifecycleNode( const std::string & node_name, const std::string & namespace_, - const rclcpp::NodeOptions & options) + const rclcpp::NodeOptions & options, + bool enable_communication_interface) : node_base_(new rclcpp::node_interfaces::NodeBase( node_name, namespace_, @@ -99,13 +102,14 @@ LifecycleNode::LifecycleNode( node_logging_, node_clock_, node_parameters_, - options.clock_qos() + options.clock_qos(), + options.use_clock_thread() )), node_waitables_(new rclcpp::node_interfaces::NodeWaitables(node_base_.get())), node_options_(options), impl_(new LifecycleNodeInterfaceImpl(node_base_, node_services_)) { - impl_->init(); + impl_->init(enable_communication_interface); register_on_configure( std::bind( @@ -161,9 +165,10 @@ LifecycleNode::get_logger() const rclcpp::CallbackGroup::SharedPtr LifecycleNode::create_callback_group( - rclcpp::CallbackGroupType group_type) + rclcpp::CallbackGroupType group_type, + bool automatically_add_to_executor_with_node) { - return node_base_->create_callback_group(group_type); + return node_base_->create_callback_group(group_type, automatically_add_to_executor_with_node); } const rclcpp::ParameterValue & diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp index 231990708f..9f70d2c259 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp @@ -64,9 +64,7 @@ class LifecycleNode::LifecycleNodeInterfaceImpl ~LifecycleNodeInterfaceImpl() { rcl_node_t * node_handle = node_base_interface_->get_rcl_node_handle(); - const rcl_node_options_t * node_options = rcl_node_get_options(node_handle); - auto ret = rcl_lifecycle_state_machine_fini( - &state_machine_, node_handle, &node_options->allocator); + auto ret = rcl_lifecycle_state_machine_fini(&state_machine_, node_handle); if (ret != RCL_RET_OK) { RCUTILS_LOG_FATAL_NAMED( "rclcpp_lifecycle", @@ -75,12 +73,16 @@ class LifecycleNode::LifecycleNodeInterfaceImpl } void - init() + init(bool enable_communication_interface = true) { rcl_node_t * node_handle = node_base_interface_->get_rcl_node_handle(); const rcl_node_options_t * node_options = rcl_node_get_options(node_base_interface_->get_rcl_node_handle()); state_machine_ = rcl_lifecycle_get_zero_initialized_state_machine(); + auto state_machine_options = rcl_lifecycle_get_default_state_machine_options(); + state_machine_options.enable_com_interface = enable_communication_interface; + state_machine_options.allocator = node_options->allocator; + // The call to initialize the state machine takes // currently five different typesupports for all publishers/services // created within the RCL_LIFECYCLE structure. @@ -96,94 +98,95 @@ class LifecycleNode::LifecycleNodeInterfaceImpl rosidl_typesupport_cpp::get_service_type_support_handle(), rosidl_typesupport_cpp::get_service_type_support_handle(), rosidl_typesupport_cpp::get_service_type_support_handle(), - true, - &node_options->allocator); + &state_machine_options); if (ret != RCL_RET_OK) { throw std::runtime_error( std::string("Couldn't initialize state machine for node ") + node_base_interface_->get_name()); } - { // change_state - auto cb = std::bind( - &LifecycleNodeInterfaceImpl::on_change_state, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); - rclcpp::AnyServiceCallback any_cb; - any_cb.set(std::move(cb)); - - srv_change_state_ = std::make_shared>( - node_base_interface_->get_shared_rcl_node_handle(), - &state_machine_.com_interface.srv_change_state, - any_cb); - node_services_interface_->add_service( - std::dynamic_pointer_cast(srv_change_state_), - nullptr); - } + if (enable_communication_interface) { + { // change_state + auto cb = std::bind( + &LifecycleNodeInterfaceImpl::on_change_state, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); + rclcpp::AnyServiceCallback any_cb; + any_cb.set(std::move(cb)); + + srv_change_state_ = std::make_shared>( + node_base_interface_->get_shared_rcl_node_handle(), + &state_machine_.com_interface.srv_change_state, + any_cb); + node_services_interface_->add_service( + std::dynamic_pointer_cast(srv_change_state_), + nullptr); + } - { // get_state - auto cb = std::bind( - &LifecycleNodeInterfaceImpl::on_get_state, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); - rclcpp::AnyServiceCallback any_cb; - any_cb.set(std::move(cb)); - - srv_get_state_ = std::make_shared>( - node_base_interface_->get_shared_rcl_node_handle(), - &state_machine_.com_interface.srv_get_state, - any_cb); - node_services_interface_->add_service( - std::dynamic_pointer_cast(srv_get_state_), - nullptr); - } + { // get_state + auto cb = std::bind( + &LifecycleNodeInterfaceImpl::on_get_state, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); + rclcpp::AnyServiceCallback any_cb; + any_cb.set(std::move(cb)); + + srv_get_state_ = std::make_shared>( + node_base_interface_->get_shared_rcl_node_handle(), + &state_machine_.com_interface.srv_get_state, + any_cb); + node_services_interface_->add_service( + std::dynamic_pointer_cast(srv_get_state_), + nullptr); + } - { // get_available_states - auto cb = std::bind( - &LifecycleNodeInterfaceImpl::on_get_available_states, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); - rclcpp::AnyServiceCallback any_cb; - any_cb.set(std::move(cb)); - - srv_get_available_states_ = std::make_shared>( - node_base_interface_->get_shared_rcl_node_handle(), - &state_machine_.com_interface.srv_get_available_states, - any_cb); - node_services_interface_->add_service( - std::dynamic_pointer_cast(srv_get_available_states_), - nullptr); - } + { // get_available_states + auto cb = std::bind( + &LifecycleNodeInterfaceImpl::on_get_available_states, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); + rclcpp::AnyServiceCallback any_cb; + any_cb.set(std::move(cb)); + + srv_get_available_states_ = std::make_shared>( + node_base_interface_->get_shared_rcl_node_handle(), + &state_machine_.com_interface.srv_get_available_states, + any_cb); + node_services_interface_->add_service( + std::dynamic_pointer_cast(srv_get_available_states_), + nullptr); + } - { // get_available_transitions - auto cb = std::bind( - &LifecycleNodeInterfaceImpl::on_get_available_transitions, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); - rclcpp::AnyServiceCallback any_cb; - any_cb.set(std::move(cb)); - - srv_get_available_transitions_ = - std::make_shared>( - node_base_interface_->get_shared_rcl_node_handle(), - &state_machine_.com_interface.srv_get_available_transitions, - any_cb); - node_services_interface_->add_service( - std::dynamic_pointer_cast(srv_get_available_transitions_), - nullptr); - } + { // get_available_transitions + auto cb = std::bind( + &LifecycleNodeInterfaceImpl::on_get_available_transitions, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); + rclcpp::AnyServiceCallback any_cb; + any_cb.set(std::move(cb)); + + srv_get_available_transitions_ = + std::make_shared>( + node_base_interface_->get_shared_rcl_node_handle(), + &state_machine_.com_interface.srv_get_available_transitions, + any_cb); + node_services_interface_->add_service( + std::dynamic_pointer_cast(srv_get_available_transitions_), + nullptr); + } - { // get_transition_graph - auto cb = std::bind( - &LifecycleNodeInterfaceImpl::on_get_transition_graph, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); - rclcpp::AnyServiceCallback any_cb; - any_cb.set(std::move(cb)); - - srv_get_transition_graph_ = - std::make_shared>( - node_base_interface_->get_shared_rcl_node_handle(), - &state_machine_.com_interface.srv_get_transition_graph, - any_cb); - node_services_interface_->add_service( - std::dynamic_pointer_cast(srv_get_transition_graph_), - nullptr); + { // get_transition_graph + auto cb = std::bind( + &LifecycleNodeInterfaceImpl::on_get_transition_graph, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); + rclcpp::AnyServiceCallback any_cb; + any_cb.set(std::move(cb)); + + srv_get_transition_graph_ = + std::make_shared>( + node_base_interface_->get_shared_rcl_node_handle(), + &state_machine_.com_interface.srv_get_transition_graph, + any_cb); + node_services_interface_->add_service( + std::dynamic_pointer_cast(srv_get_transition_graph_), + nullptr); + } } } diff --git a/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_client.cpp b/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_client.cpp index e65f0cf26d..27b01df51a 100644 --- a/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_client.cpp +++ b/rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_client.cpp @@ -212,8 +212,9 @@ class BenchmarkLifecycleClient : public performance_test_fixture::PerformanceTes performance_test_fixture::PerformanceTest::TearDown(state); executor->cancel(); spinner_.join(); - lifecycle_node.reset(); + executor.reset(); lifecycle_client.reset(); + lifecycle_node.reset(); rclcpp::shutdown(); } diff --git a/rclcpp_lifecycle/test/mocking_utils/patch.hpp b/rclcpp_lifecycle/test/mocking_utils/patch.hpp index 7551bebf0d..a6249da3af 100644 --- a/rclcpp_lifecycle/test/mocking_utils/patch.hpp +++ b/rclcpp_lifecycle/test/mocking_utils/patch.hpp @@ -234,7 +234,27 @@ struct PatchTraits mock_type, ReturnT, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5); }; -/// Traits specialization for void(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7) +/// Traits specialization for void(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5) +/// free functions. +/** + * Necessary for Mimick macros to adjust accordingly when the return + * type is `void`. + * + * \tparam ID Numerical identifier of the patch. Ought to be unique. + * \tparam ArgTx Argument types. + */ +template +struct PatchTraits +{ + mmk_mock_define( + mock_type, void, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5); +}; + +/// Traits specialization for +/// ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7, ArgT8) /// free functions. /** * Necessary for Mimick macros to adjust accordingly when the return @@ -248,15 +268,15 @@ template + typename ArgT8> struct PatchTraits + ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7, ArgT8)> { mmk_mock_define( - mock_type, ReturnT, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7, ArgT8, ArgT9); + mock_type, ReturnT, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7, ArgT8); }; -/// Traits specialization for void(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5) +/// Traits specialization for void(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7) /// free functions. /** * Necessary for Mimick macros to adjust accordingly when the return @@ -265,14 +285,17 @@ struct PatchTraits -struct PatchTraits + typename ArgT4, typename ArgT5, + typename ArgT6, typename ArgT7, + typename ArgT8, typename ArgT9> +struct PatchTraits { mmk_mock_define( - mock_type, void, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5); + mock_type, ReturnT, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7, ArgT8, ArgT9); }; /// Generic trampoline to wrap generalized callables in plain functions. diff --git a/rclcpp_lifecycle/test/test_lifecycle_node.cpp b/rclcpp_lifecycle/test/test_lifecycle_node.cpp index a52fc5ca7f..7f39b3e7cc 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_node.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_node.cpp @@ -34,6 +34,88 @@ using lifecycle_msgs::msg::State; using lifecycle_msgs::msg::Transition; +static const std::chrono::nanoseconds DEFAULT_EVENT_TIMEOUT = std::chrono::seconds(3); +static const std::chrono::nanoseconds DEFAULT_EVENT_SLEEP_PERIOD = std::chrono::milliseconds(100); + +static +bool wait_for_event( + std::shared_ptr node, + std::function predicate, + std::chrono::nanoseconds timeout, + std::chrono::nanoseconds sleep_period) +{ + auto start = std::chrono::steady_clock::now(); + std::chrono::microseconds time_slept(0); + + bool predicate_result; + while (!(predicate_result = predicate()) && + time_slept < std::chrono::duration_cast(timeout)) + { + rclcpp::Event::SharedPtr graph_event = node->get_graph_event(); + node->wait_for_graph_change(graph_event, sleep_period); + time_slept = std::chrono::duration_cast( + std::chrono::steady_clock::now() - start); + } + return predicate_result; +} + +static +bool wait_for_topic( + std::shared_ptr node, + const std::string & topic, + std::chrono::nanoseconds timeout = DEFAULT_EVENT_TIMEOUT, + std::chrono::nanoseconds sleep_period = DEFAULT_EVENT_SLEEP_PERIOD) +{ + return wait_for_event( + node, + [node, topic]() + { + auto topic_names_and_types = node->get_topic_names_and_types(); + return topic_names_and_types.end() != topic_names_and_types.find(topic); + }, + timeout, + sleep_period); +} + +static +bool wait_for_service( + std::shared_ptr node, + const std::string & service, + std::chrono::nanoseconds timeout = DEFAULT_EVENT_TIMEOUT, + std::chrono::nanoseconds sleep_period = DEFAULT_EVENT_SLEEP_PERIOD) +{ + return wait_for_event( + node, + [node, service]() + { + auto service_names_and_types = node->get_service_names_and_types(); + return service_names_and_types.end() != service_names_and_types.find(service); + }, + timeout, + sleep_period); +} + +static +bool wait_for_service_by_node( + std::shared_ptr node, + const std::string & node_name, + const std::string & service, + std::chrono::nanoseconds timeout = DEFAULT_EVENT_TIMEOUT, + std::chrono::nanoseconds sleep_period = DEFAULT_EVENT_SLEEP_PERIOD) +{ + return wait_for_event( + node, + [node, node_name, service]() + { + auto service_names_and_types_by_node = node->get_service_names_and_types_by_node( + node_name, ""); + return service_names_and_types_by_node.end() != service_names_and_types_by_node.find( + service); + }, + timeout, + sleep_period); +} + class TestDefaultStateMachine : public ::testing::Test { protected: @@ -152,7 +234,7 @@ TEST_F(TestDefaultStateMachine, empty_initializer) { TEST_F(TestDefaultStateMachine, empty_initializer_rcl_errors) { { - auto patch = mocking_utils::patch_and_return( + auto patch = mocking_utils::inject_on_return( "lib:rclcpp_lifecycle", rcl_lifecycle_state_machine_init, RCL_RET_ERROR); EXPECT_THROW( std::make_shared("testnode").reset(), @@ -559,10 +641,8 @@ TEST_F(TestDefaultStateMachine, test_graph_topics) { ASSERT_NE(names.end(), std::find(names.begin(), names.end(), std::string("/testnode"))); // Other topics may exist for an rclcpp::Node, but just checking the lifecycle one exists + ASSERT_TRUE(wait_for_topic(test_node, "/testnode/transition_event")); auto topic_names_and_types = test_node->get_topic_names_and_types(); - ASSERT_NE( - topic_names_and_types.end(), - topic_names_and_types.find(std::string("/testnode/transition_event"))); EXPECT_STREQ( topic_names_and_types["/testnode/transition_event"][0].c_str(), "lifecycle_msgs/msg/TransitionEvent"); @@ -580,39 +660,26 @@ TEST_F(TestDefaultStateMachine, test_graph_topics) { TEST_F(TestDefaultStateMachine, test_graph_services) { auto test_node = std::make_shared("testnode"); - auto service_names_and_types = test_node->get_service_names_and_types(); // These are specific to lifecycle nodes, other services are provided by rclcpp::Node - ASSERT_NE( - service_names_and_types.end(), - service_names_and_types.find(std::string("/testnode/change_state"))); + ASSERT_TRUE(wait_for_service(test_node, "/testnode/change_state")); + ASSERT_TRUE(wait_for_service(test_node, "/testnode/get_available_states")); + ASSERT_TRUE(wait_for_service(test_node, "/testnode/get_available_transitions")); + ASSERT_TRUE(wait_for_service(test_node, "/testnode/get_state")); + ASSERT_TRUE(wait_for_service(test_node, "/testnode/get_transition_graph")); + + auto service_names_and_types = test_node->get_service_names_and_types(); EXPECT_STREQ( service_names_and_types["/testnode/change_state"][0].c_str(), "lifecycle_msgs/srv/ChangeState"); - - ASSERT_NE( - service_names_and_types.end(), - service_names_and_types.find(std::string("/testnode/get_available_states"))); EXPECT_STREQ( service_names_and_types["/testnode/get_available_states"][0].c_str(), "lifecycle_msgs/srv/GetAvailableStates"); - - ASSERT_NE( - service_names_and_types.end(), - service_names_and_types.find(std::string("/testnode/get_available_transitions"))); EXPECT_STREQ( service_names_and_types["/testnode/get_available_transitions"][0].c_str(), "lifecycle_msgs/srv/GetAvailableTransitions"); - - ASSERT_NE( - service_names_and_types.end(), - service_names_and_types.find(std::string("/testnode/get_state"))); EXPECT_STREQ( service_names_and_types["/testnode/get_state"][0].c_str(), "lifecycle_msgs/srv/GetState"); - - ASSERT_NE( - service_names_and_types.end(), - service_names_and_types.find(std::string("/testnode/get_transition_graph"))); EXPECT_STREQ( service_names_and_types["/testnode/get_transition_graph"][0].c_str(), "lifecycle_msgs/srv/GetAvailableTransitions"); @@ -621,40 +688,28 @@ TEST_F(TestDefaultStateMachine, test_graph_services) { TEST_F(TestDefaultStateMachine, test_graph_services_by_node) { auto test_node = std::make_shared("testnode"); + // These are specific to lifecycle nodes, other services are provided by rclcpp::Node + ASSERT_TRUE(wait_for_service_by_node(test_node, "testnode", "/testnode/change_state")); + ASSERT_TRUE(wait_for_service_by_node(test_node, "testnode", "/testnode/get_available_states")); + ASSERT_TRUE( + wait_for_service_by_node(test_node, "testnode", "/testnode/get_available_transitions")); + ASSERT_TRUE(wait_for_service_by_node(test_node, "testnode", "/testnode/get_state")); + ASSERT_TRUE(wait_for_service_by_node(test_node, "testnode", "/testnode/get_transition_graph")); + auto service_names_and_types_by_node = test_node->get_service_names_and_types_by_node("testnode", ""); - // These are specific to lifecycle nodes, other services are provided by rclcpp::Node - ASSERT_NE( - service_names_and_types_by_node.end(), - service_names_and_types_by_node.find(std::string("/testnode/change_state"))); EXPECT_STREQ( service_names_and_types_by_node["/testnode/change_state"][0].c_str(), "lifecycle_msgs/srv/ChangeState"); - - ASSERT_NE( - service_names_and_types_by_node.end(), - service_names_and_types_by_node.find(std::string("/testnode/get_available_states"))); EXPECT_STREQ( service_names_and_types_by_node["/testnode/get_available_states"][0].c_str(), "lifecycle_msgs/srv/GetAvailableStates"); - - ASSERT_NE( - service_names_and_types_by_node.end(), - service_names_and_types_by_node.find(std::string("/testnode/get_available_transitions"))); EXPECT_STREQ( service_names_and_types_by_node["/testnode/get_available_transitions"][0].c_str(), "lifecycle_msgs/srv/GetAvailableTransitions"); - - ASSERT_NE( - service_names_and_types_by_node.end(), - service_names_and_types_by_node.find(std::string("/testnode/get_state"))); EXPECT_STREQ( service_names_and_types_by_node["/testnode/get_state"][0].c_str(), "lifecycle_msgs/srv/GetState"); - - ASSERT_NE( - service_names_and_types_by_node.end(), - service_names_and_types_by_node.find(std::string("/testnode/get_transition_graph"))); EXPECT_STREQ( service_names_and_types_by_node["/testnode/get_transition_graph"][0].c_str(), "lifecycle_msgs/srv/GetAvailableTransitions"); @@ -666,7 +721,7 @@ TEST_F(TestDefaultStateMachine, test_callback_groups) { EXPECT_EQ(groups.size(), 1u); auto group = test_node->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive); + rclcpp::CallbackGroupType::MutuallyExclusive, true); EXPECT_NE(nullptr, group); groups = test_node->get_callback_groups();