From 35c89c8afcac89a43872f0bf5b763ff324c82b05 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 12 Mar 2021 13:07:12 -0300 Subject: [PATCH 01/93] Enable qos parameter overrides for the /parameter_events topic (#1532) Signed-off-by: Ivan Santiago Paunovic --- rclcpp/CMakeLists.txt | 1 + .../detail/resolve_parameter_overrides.cpp | 77 +++++++++++++++++++ .../detail/resolve_parameter_overrides.hpp | 44 +++++++++++ rclcpp/src/rclcpp/node.cpp | 64 ++++++++++++++- .../node_interfaces/node_parameters.cpp | 46 ++--------- rclcpp/test/rclcpp/test_qos_parameters.cpp | 3 +- 6 files changed, 194 insertions(+), 41 deletions(-) create mode 100644 rclcpp/src/rclcpp/detail/resolve_parameter_overrides.cpp create mode 100644 rclcpp/src/rclcpp/detail/resolve_parameter_overrides.hpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 448113bb93..79d67325f4 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -40,6 +40,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 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/node.cpp b/rclcpp/src/rclcpp/node.cpp index abe7b8fc4b..0d45f64148 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() @@ -146,6 +194,20 @@ Node::Node( 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_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index 36abc8e163..c6aa2af6e2 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. 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(); } From 736550cace37200355ca7ccfde4196f78036d8db Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Wed, 17 Mar 2021 17:00:18 -0700 Subject: [PATCH 02/93] Fix doc typo in CallbackGroup constructor (#1582) Signed-off-by: Jacob Perron --- rclcpp/include/rclcpp/callback_group.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rclcpp/include/rclcpp/callback_group.hpp b/rclcpp/include/rclcpp/callback_group.hpp index e9de8dc996..fe52d287bb 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. */ From e33105057cf64477d598ff79e9a2a226abd73f38 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 18 Mar 2021 11:13:48 -0300 Subject: [PATCH 03/93] Document design decisions that were made for statically typed parameters (#1568) Signed-off-by: Ivan Santiago Paunovic --- .../notes_on_statically_typed_parameters.md | 141 ++++++++++++++++++ 1 file changed, 141 insertions(+) create mode 100644 rclcpp/doc/notes_on_statically_typed_parameters.md 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)); +``` From 7bfda87d32bab7e9caa843b2ededa18e427fd00c Mon Sep 17 00:00:00 2001 From: Scott K Logan Date: Thu, 18 Mar 2021 14:02:21 -0700 Subject: [PATCH 04/93] 7.0.0 Signed-off-by: Scott K Logan --- rclcpp/CHANGELOG.rst | 21 +++++++++++++++++++++ rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 6 ++++++ rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 3 +++ rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 7 +++++++ rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 41 insertions(+), 4 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 118c6504be..4253e34176 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,27 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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/package.xml b/rclcpp/package.xml index d876bfb377..f7bdd96a1a 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 6.3.1 + 7.0.0 The ROS client library in C++. Ivan Paunovic Mabel Zhang diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index f3a2cc18c2..2624495d7c 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,12 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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/package.xml b/rclcpp_action/package.xml index e258069f76..bb30b98ace 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 6.3.1 + 7.0.0 Adds action APIs for C++. Ivan Paunovic Mabel Zhang diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 4848e0b1bc..7104038a94 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +7.0.0 (2021-03-18) +------------------ + 6.3.1 (2021-02-08) ------------------ diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index 4f35fcd2b3..d9ed30118d 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 6.3.1 + 7.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..c0f71257cd 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,13 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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/package.xml b/rclcpp_lifecycle/package.xml index a9d0fc0965..cab34319aa 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 6.3.1 + 7.0.0 Package containing a prototype for lifecycle implementation Ivan Paunovic Mabel Zhang From f5afe380d5f2af5b84b2e64ec54d9afe22ceda99 Mon Sep 17 00:00:00 2001 From: suab321321 <52123334+suab321321@users.noreply.github.com> Date: Sat, 20 Mar 2021 04:23:01 +0530 Subject: [PATCH 05/93] Made 'Context::shutdown_reason' function a const function (#1578) Signed-off-by: Abhinav Singh --- rclcpp/include/rclcpp/context.hpp | 4 ++-- rclcpp/src/rclcpp/context.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/rclcpp/include/rclcpp/context.hpp b/rclcpp/include/rclcpp/context.hpp index c480444f8f..9140f3f117 100644 --- a/rclcpp/include/rclcpp/context.hpp +++ b/rclcpp/include/rclcpp/context.hpp @@ -150,7 +150,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. /** @@ -286,7 +286,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_; diff --git a/rclcpp/src/rclcpp/context.cpp b/rclcpp/src/rclcpp/context.cpp index 9e00f7a1eb..90ea3db908 100644 --- a/rclcpp/src/rclcpp/context.cpp +++ b/rclcpp/src/rclcpp/context.cpp @@ -291,7 +291,7 @@ Context::get_domain_id() const } std::string -Context::shutdown_reason() +Context::shutdown_reason() const { std::lock_guard lock(init_mutex_); return shutdown_reason_; From 75051616f1eaaea52805b921f5beca01db22a399 Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Mon, 22 Mar 2021 22:54:27 +0900 Subject: [PATCH 06/93] get_parameters_service_ should return empty if allow_undeclared_ is false (#1514) Signed-off-by: Tomoya.Fujita --- rclcpp/src/rclcpp/parameter_service.cpp | 21 +- rclcpp/test/rclcpp/test_parameter_client.cpp | 244 ++++++++++++++++++- 2 files changed, 249 insertions(+), 16 deletions(-) diff --git a/rclcpp/src/rclcpp/parameter_service.cpp b/rclcpp/src/rclcpp/parameter_service.cpp index 0cbb9667e4..a766696f64 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/test/rclcpp/test_parameter_client.cpp b/rclcpp/test/rclcpp/test_parameter_client.cpp index ba7cc99c29..c7f8ad6581 100644 --- a/rclcpp/test/rclcpp/test_parameter_client.cpp +++ b/rclcpp/test/rclcpp/test_parameter_client.cpp @@ -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()); + } } /* From cae3836ca0c435d00d338d138056f020d5a6286d Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Mon, 22 Mar 2021 14:20:03 +0000 Subject: [PATCH 07/93] Changelogs Signed-off-by: Ivan Santiago Paunovic --- rclcpp/CHANGELOG.rst | 6 ++++++ rclcpp_action/CHANGELOG.rst | 3 +++ rclcpp_components/CHANGELOG.rst | 3 +++ rclcpp_lifecycle/CHANGELOG.rst | 3 +++ 4 files changed, 15 insertions(+) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 4253e34176..d28c04643f 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 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 `_) diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 2624495d7c..4a697015b9 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 7.0.0 (2021-03-18) ------------------ * Add support for rmw_connextdds (`#1574 `_) diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 7104038a94..6c08b6f195 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 7.0.0 (2021-03-18) ------------------ diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index c0f71257cd..eac4735ce2 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 7.0.0 (2021-03-18) ------------------ * Add support for rmw_connextdds (`#1574 `_) From 59ad83ab5a1a6a21d5ef8f9f8ac84fb973dad398 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Mon, 22 Mar 2021 14:20:25 +0000 Subject: [PATCH 08/93] 7.0.1 --- rclcpp/CHANGELOG.rst | 4 ++-- rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 4 ++-- rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 4 ++-- rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 4 ++-- rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index d28c04643f..c00f2a155c 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +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 diff --git a/rclcpp/package.xml b/rclcpp/package.xml index f7bdd96a1a..e19a3639e9 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 7.0.0 + 7.0.1 The ROS client library in C++. Ivan Paunovic Mabel Zhang diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 4a697015b9..e15fd2dbf7 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +7.0.1 (2021-03-22) +------------------ 7.0.0 (2021-03-18) ------------------ diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index bb30b98ace..40aa995f9d 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 7.0.0 + 7.0.1 Adds action APIs for C++. Ivan Paunovic Mabel Zhang diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 6c08b6f195..5a7e97e2e1 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +7.0.1 (2021-03-22) +------------------ 7.0.0 (2021-03-18) ------------------ diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index d9ed30118d..a7b9eb3f62 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 7.0.0 + 7.0.1 Package containing tools for dynamically loadable components Ivan Paunovic Mabel Zhang diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index eac4735ce2..c7529df6c7 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +7.0.1 (2021-03-22) +------------------ 7.0.0 (2021-03-18) ------------------ diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index cab34319aa..bbfaf5f203 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 7.0.0 + 7.0.1 Package containing a prototype for lifecycle implementation Ivan Paunovic Mabel Zhang From 182ad3860e80b78a573a51e3221c8adb3d550ea7 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Mon, 22 Mar 2021 15:29:38 -0700 Subject: [PATCH 09/93] Guard against integer overflow in duration conversion (#1584) Guard against overflow when converting from rclcpp::Duration to builtin_interfaces::msg::Duration, which is a unsigned to signed conversion. Use non-std int types for consistency Handle large negative values Signed-off-by: Jacob Perron --- rclcpp/src/rclcpp/duration.cpp | 33 +++++++++++++++++++++------- rclcpp/test/rclcpp/test_duration.cpp | 29 ++++++++++++++++++++++++ 2 files changed, 54 insertions(+), 8 deletions(-) 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/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) { From 052596c971c1767155bdcbce573bd52a445c9ea2 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 23 Mar 2021 11:26:35 -0700 Subject: [PATCH 10/93] make rcl_lifecyle_com_interface optional in lifecycle nodes (#1507) * make rcl_com_interface optional Signed-off-by: Karsten Knese * Update rclcpp_lifecycle/test/mocking_utils/patch.hpp Co-authored-by: Tomoya Fujita * line break Signed-off-by: Karsten Knese * use state_machine_options Signed-off-by: Karsten Knese Co-authored-by: Tomoya Fujita --- .../rclcpp_lifecycle/lifecycle_node.hpp | 8 +- rclcpp_lifecycle/src/lifecycle_node.cpp | 11 +- .../src/lifecycle_node_interface_impl.hpp | 171 +++++++++--------- rclcpp_lifecycle/test/mocking_utils/patch.hpp | 41 ++++- rclcpp_lifecycle/test/test_lifecycle_node.cpp | 2 +- 5 files changed, 135 insertions(+), 98 deletions(-) diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 8289187cce..f8184fff5a 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -127,23 +127,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(); diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index e7408a35de..62f20205dd 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_, @@ -105,7 +108,7 @@ LifecycleNode::LifecycleNode( node_options_(options), impl_(new LifecycleNodeInterfaceImpl(node_base_, node_services_)) { - impl_->init(); + impl_->init(enable_communication_interface); register_on_configure( std::bind( diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp index 231990708f..bd29ae7c8a 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp @@ -63,10 +63,11 @@ class LifecycleNode::LifecycleNodeInterfaceImpl ~LifecycleNodeInterfaceImpl() { + fprintf(stderr, "oh yeah destructuro\n"); 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); + fprintf(stderr, "oh yeah end destructuro\n"); + auto ret = rcl_lifecycle_state_machine_fini(&state_machine_, node_handle); + fprintf(stderr, "oh yeah end destructuro222\n"); if (ret != RCL_RET_OK) { RCUTILS_LOG_FATAL_NAMED( "rclcpp_lifecycle", @@ -75,12 +76,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 +101,96 @@ 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) { + fprintf(stderr, "oh yeah %s\n", node_base_interface_->get_name()); 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/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..71ad659235 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_node.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_node.cpp @@ -152,7 +152,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(), From 63fdf82ebf7c86b7f7039569f784cd76d2f2baa9 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 23 Mar 2021 21:16:14 +0000 Subject: [PATCH 11/93] Changelog. Signed-off-by: Chris Lalancette --- rclcpp/CHANGELOG.rst | 5 +++++ rclcpp_action/CHANGELOG.rst | 3 +++ rclcpp_components/CHANGELOG.rst | 3 +++ rclcpp_lifecycle/CHANGELOG.rst | 5 +++++ 4 files changed, 16 insertions(+) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index c00f2a155c..c8059e3ec0 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 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 `_) diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index e15fd2dbf7..7216096e14 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 7.0.1 (2021-03-22) ------------------ diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 5a7e97e2e1..2b5d140bf2 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 7.0.1 (2021-03-22) ------------------ diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index c7529df6c7..6965b509aa 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,11 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* make rcl_lifecyle_com_interface optional in lifecycle nodes (`#1507 `_) +* Contributors: Karsten Knese + 7.0.1 (2021-03-22) ------------------ From 763c56481f2a8686e26caa97e0b9e8983601a6f8 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 23 Mar 2021 21:16:22 +0000 Subject: [PATCH 12/93] 8.0.0 --- rclcpp/CHANGELOG.rst | 4 ++-- rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 4 ++-- rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 4 ++-- rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 4 ++-- rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index c8059e3ec0..9b8a18b156 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +8.0.0 (2021-03-23) +------------------ * Guard against integer overflow in duration conversion (`#1584 `_) * Contributors: Jacob Perron diff --git a/rclcpp/package.xml b/rclcpp/package.xml index e19a3639e9..8eea932cc6 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 7.0.1 + 8.0.0 The ROS client library in C++. Ivan Paunovic Mabel Zhang diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 7216096e14..233142bfbb 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +8.0.0 (2021-03-23) +------------------ 7.0.1 (2021-03-22) ------------------ diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index 40aa995f9d..369359499d 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 7.0.1 + 8.0.0 Adds action APIs for C++. Ivan Paunovic Mabel Zhang diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 2b5d140bf2..03aa20a2fd 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +8.0.0 (2021-03-23) +------------------ 7.0.1 (2021-03-22) ------------------ diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index a7b9eb3f62..e9d3fb2d55 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 7.0.1 + 8.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 6965b509aa..f5967f85ae 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +8.0.0 (2021-03-23) +------------------ * make rcl_lifecyle_com_interface optional in lifecycle nodes (`#1507 `_) * Contributors: Karsten Knese diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index bbfaf5f203..90c6a32d86 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 7.0.1 + 8.0.0 Package containing a prototype for lifecycle implementation Ivan Paunovic Mabel Zhang From 3fa511a8a03682eb5e876c0def204b79bd3491c4 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 23 Mar 2021 16:31:46 -0700 Subject: [PATCH 13/93] use const auto & to iterate over parameters (#1593) Clang is complaining about the looping variable being referenced as `const val` but should rather be `const ref`. ``` /Users/karsten/workspace/ros2/ros2_master/src/ros2/rclcpp/rclcpp/src/rclcpp/parameter_service.cpp:46:25: error: loop variable 'param' of type 'const rclcpp::Parameter' creates a copy from type 'const rclcpp::Parameter' [-Werror,-Wrange-loop-construct] for (const auto param : parameters) { ^ /Users/karsten/workspace/ros2/ros2_master/src/ros2/rclcpp/rclcpp/src/rclcpp/parameter_service.cpp:46:14: note: use reference type 'const rclcpp::Parameter &' to prevent copying for (const auto param : parameters) { ^~~~~~~~~~~~~~~~~~ & 1 error generated. ``` Signed-off-by: Karsten Knese --- rclcpp/src/rclcpp/parameter_service.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/src/rclcpp/parameter_service.cpp b/rclcpp/src/rclcpp/parameter_service.cpp index a766696f64..5c30917499 100644 --- a/rclcpp/src/rclcpp/parameter_service.cpp +++ b/rclcpp/src/rclcpp/parameter_service.cpp @@ -43,7 +43,7 @@ ParameterService::ParameterService( { try { auto parameters = node_params->get_parameters(request->names); - for (const auto param : parameters) { + for (const auto & param : parameters) { response->values.push_back(param.get_value_message()); } } catch (const rclcpp::exceptions::ParameterNotDeclaredException & ex) { From 0088e35052270eca4ccbb0f0c2325f4030c39baf Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Wed, 24 Mar 2021 08:37:29 -0700 Subject: [PATCH 14/93] Document misuse of parameters callback (#1590) * Document misuse of parameters callback Related to https://github.com/ros2/rclcpp/issues/1587 Signed-off-by: Jacob Perron * Remove bad example Signed-off-by: Jacob Perron --- rclcpp/include/rclcpp/node.hpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 5ff9546ec3..78eb1db93b 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -840,6 +840,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 From dbb4c354d6ba2b9614d351521782f851a58a4609 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Thu, 25 Mar 2021 08:21:47 -0700 Subject: [PATCH 15/93] Add API for checking QoS profile compatibility (#1554) * Add API for checking QoS profile compatibility Depends on https://github.com/ros2/rmw/pull/299 Signed-off-by: Jacob Perron * Refactor as free function Returns a struct containing the compatibility enum value and string for the reason. Updated tests to reflect behavior changes upstream. Signed-off-by: Jacob Perron --- .../include/rclcpp/exceptions/exceptions.hpp | 7 +++ rclcpp/include/rclcpp/qos.hpp | 63 +++++++++++++++++++ rclcpp/src/rclcpp/qos.cpp | 41 ++++++++++++ rclcpp/test/rclcpp/test_qos.cpp | 42 +++++++++++++ 4 files changed, 153 insertions(+) diff --git a/rclcpp/include/rclcpp/exceptions/exceptions.hpp b/rclcpp/include/rclcpp/exceptions/exceptions.hpp index a0c711290c..27c695c0b6 100644 --- a/rclcpp/include/rclcpp/exceptions/exceptions.hpp +++ b/rclcpp/include/rclcpp/exceptions/exceptions.hpp @@ -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/qos.hpp b/rclcpp/include/rclcpp/qos.hpp index edc3a58bbe..4160360756 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 { @@ -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/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/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()); + } +} From 98a62f6a18b234e20148fac9b9eec93573addad7 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 25 Mar 2021 14:49:12 -0400 Subject: [PATCH 16/93] Remove rmw_connext_cpp references. (#1595) * Remove rmw_connext_cpp references. Signed-off-by: Chris Lalancette Co-authored-by: Ivan Santiago Paunovic --- .../executors/test_static_executor_entities_collector.cpp | 2 +- .../rclcpp/strategies/test_allocator_memory_strategy.cpp | 5 +---- 2 files changed, 2 insertions(+), 5 deletions(-) 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..5b20c6b201 100644 --- a/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp +++ b/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp @@ -267,7 +267,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); diff --git a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp index b401d6d046..b0951c9218 100644 --- a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp +++ b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp @@ -511,10 +511,7 @@ 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") { // For cyclonedds, a subscription will also add an event and waitable expected_sizes.size_of_events += 1; expected_sizes.size_of_waitables += 1; From 06a4a560177bfc1d3a61e6c932bc1fcc82b1b7eb Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 25 Mar 2021 21:06:26 +0000 Subject: [PATCH 17/93] Changelog. Signed-off-by: Chris Lalancette --- rclcpp/CHANGELOG.rst | 8 ++++++++ rclcpp_action/CHANGELOG.rst | 3 +++ rclcpp_components/CHANGELOG.rst | 3 +++ rclcpp_lifecycle/CHANGELOG.rst | 3 +++ 4 files changed, 17 insertions(+) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 9b8a18b156..d60a998de4 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 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 `_) diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 233142bfbb..bf61a242dc 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 8.0.0 (2021-03-23) ------------------ diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 03aa20a2fd..dc38b67fb7 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 8.0.0 (2021-03-23) ------------------ diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index f5967f85ae..431f62da5e 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 8.0.0 (2021-03-23) ------------------ * make rcl_lifecyle_com_interface optional in lifecycle nodes (`#1507 `_) From a7ec7d9243d83533540603e6d4369650319dfe7a Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 25 Mar 2021 21:06:58 +0000 Subject: [PATCH 18/93] 8.1.0 --- rclcpp/CHANGELOG.rst | 4 ++-- rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 4 ++-- rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 4 ++-- rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 4 ++-- rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index d60a998de4..9709bbdf82 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +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 `_) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 8eea932cc6..87cca58ba5 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 8.0.0 + 8.1.0 The ROS client library in C++. Ivan Paunovic Mabel Zhang diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index bf61a242dc..6fe45a13db 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +8.1.0 (2021-03-25) +------------------ 8.0.0 (2021-03-23) ------------------ diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index 369359499d..bb14c105db 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 8.0.0 + 8.1.0 Adds action APIs for C++. Ivan Paunovic Mabel Zhang diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index dc38b67fb7..7e7def6ca5 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +8.1.0 (2021-03-25) +------------------ 8.0.0 (2021-03-23) ------------------ diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index e9d3fb2d55..dc0ce2b2af 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 8.0.0 + 8.1.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 431f62da5e..264166d7b9 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +8.1.0 (2021-03-25) +------------------ 8.0.0 (2021-03-23) ------------------ diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 90c6a32d86..57aa09c23e 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 8.0.0 + 8.1.0 Package containing a prototype for lifecycle implementation Ivan Paunovic Mabel Zhang From 92ece94ca323e4e39d45ac5b043b200ec4d97b04 Mon Sep 17 00:00:00 2001 From: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Date: Thu, 25 Mar 2021 22:10:59 +0100 Subject: [PATCH 19/93] add automatically_add_executor_with_node option (#1594) * add automatically_add_executor_with_node option Signed-off-by: Brice * Fix typo Signed-off-by: Brice * add option usage in test Signed-off-by: Brice * Document parameter Signed-off-by: Brice --- .../include/rclcpp_lifecycle/lifecycle_node.hpp | 7 ++++++- rclcpp_lifecycle/src/lifecycle_node.cpp | 5 +++-- rclcpp_lifecycle/test/test_lifecycle_node.cpp | 2 +- 3 files changed, 10 insertions(+), 4 deletions(-) diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index f8184fff5a..451c09e383 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -179,11 +179,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. /** diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 62f20205dd..ee09e66974 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -164,9 +164,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/test/test_lifecycle_node.cpp b/rclcpp_lifecycle/test/test_lifecycle_node.cpp index 71ad659235..44ccb79390 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_node.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_node.cpp @@ -666,7 +666,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(); From 63e79223f98867ba00c059eb612438ca9461a2f7 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Mon, 29 Mar 2021 10:58:51 -0300 Subject: [PATCH 20/93] Delete debug messages (#1602) Signed-off-by: Ivan Santiago Paunovic --- rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp index bd29ae7c8a..9f70d2c259 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp @@ -63,11 +63,8 @@ class LifecycleNode::LifecycleNodeInterfaceImpl ~LifecycleNodeInterfaceImpl() { - fprintf(stderr, "oh yeah destructuro\n"); rcl_node_t * node_handle = node_base_interface_->get_rcl_node_handle(); - fprintf(stderr, "oh yeah end destructuro\n"); auto ret = rcl_lifecycle_state_machine_fini(&state_machine_, node_handle); - fprintf(stderr, "oh yeah end destructuro222\n"); if (ret != RCL_RET_OK) { RCUTILS_LOG_FATAL_NAMED( "rclcpp_lifecycle", @@ -103,7 +100,6 @@ class LifecycleNode::LifecycleNodeInterfaceImpl rosidl_typesupport_cpp::get_service_type_support_handle(), &state_machine_options); if (ret != RCL_RET_OK) { - fprintf(stderr, "oh yeah %s\n", node_base_interface_->get_name()); throw std::runtime_error( std::string("Couldn't initialize state machine for node ") + node_base_interface_->get_name()); From bc8c71b63f60f5df4796a42b3d9c246c9e6f2b09 Mon Sep 17 00:00:00 2001 From: anaelle-sw <63144493+anaelle-sw@users.noreply.github.com> Date: Tue, 30 Mar 2021 00:45:19 +0200 Subject: [PATCH 21/93] Clock subscription callback group spins in its own thread (#1556) * Clock subscription callback group spins in its own thread Signed-off-by: anaelle-sw * fix line length Signed-off-by: anaelle-sw * fix code style divergence Signed-off-by: anaelle-sw * fix code style divergence Signed-off-by: anaelle-sw * Initialize only once clock_callbackgroup Signed-off-by: anaelle-sw * Check if thread joinable before start a new thread Signed-off-by: anaelle-sw * Cancel executor and join thread in destroy_clock_sub Signed-off-by: anaelle-sw * Check if clock_thread_ is joinable before cancel executor and join Signed-off-by: anaelle-sw * Add use_clock_thread as an option Signed-off-by: anaelle-sw * Fix code style divergence Signed-off-by: anaelle-sw * Fix code style divergence Signed-off-by: anaelle-sw * Fix code style divergence Signed-off-by: anaelle-sw * Add TimeSource tests Signed-off-by: anaelle-sw * update use_clock_thread value in function attachNode Signed-off-by: anaelle * join clock thread in function detachNode Signed-off-by: anaelle * TimeSource tests: fixes + comments + more tested cases Signed-off-by: anaelle * clean destroy_clock_sub() Signed-off-by: anaelle * flag to ensure clock_executor is cancelled Signed-off-by: anaelle * Always re-init clock_callback_group when creating a new clock sub Signed-off-by: anaelle * spin_until_future_complete() to cleanly cancel executor Signed-off-by: anaelle * fix tests warnings Signed-off-by: anaelle * fix test error: cancel clock executor Signed-off-by: anaelle * clean comments Signed-off-by: anaelle * fix precision loss Signed-off-by: anaelle --- .../node_interfaces/node_time_source.hpp | 3 +- rclcpp/include/rclcpp/node_options.hpp | 17 ++ rclcpp/include/rclcpp/time_source.hpp | 18 +- rclcpp/src/rclcpp/node.cpp | 3 +- .../node_interfaces/node_time_source.cpp | 5 +- rclcpp/src/rclcpp/node_options.cpp | 14 ++ rclcpp/src/rclcpp/time_source.cpp | 42 +++- rclcpp/test/rclcpp/test_time_source.cpp | 226 ++++++++++++++++++ rclcpp_lifecycle/src/lifecycle_node.cpp | 3 +- 9 files changed, 319 insertions(+), 12 deletions(-) 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/time_source.hpp b/rclcpp/include/rclcpp/time_source.hpp index 3f569b49f9..bad683b4d3 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,6 +151,9 @@ 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 clock_executor_; + std::promise cancel_clock_executor_promise_; // The clock callback itself void clock_cb(const rosgraph_msgs::msg::Clock::SharedPtr msg); diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index 0d45f64148..117702760e 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -187,7 +187,8 @@ 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), 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_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/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp index c8d8270859..9f8642825c 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(); @@ -242,6 +250,24 @@ 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_; + if (!clock_executor_thread_.joinable()) { + clock_executor_thread_ = std::thread( + [this]() { + cancel_clock_executor_promise_ = std::promise{}; + 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,6 +281,12 @@ 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(); } 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_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index ee09e66974..2370e43b67 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -102,7 +102,8 @@ 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), From 3ab65715933fda37be06f14d3d6522161a1f1928 Mon Sep 17 00:00:00 2001 From: Christophe Bedard Date: Mon, 29 Mar 2021 23:55:44 -0400 Subject: [PATCH 22/93] Namespace tracetools C++ functions (#1603) Signed-off-by: Christophe Bedard --- rclcpp/include/rclcpp/any_service_callback.hpp | 4 ++-- rclcpp/include/rclcpp/any_subscription_callback.hpp | 8 ++++---- rclcpp/include/rclcpp/timer.hpp | 2 +- 3 files changed, 7 insertions(+), 7 deletions(-) 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 5a708435bd..9a31ecff53 100644 --- a/rclcpp/include/rclcpp/any_subscription_callback.hpp +++ b/rclcpp/include/rclcpp/any_subscription_callback.hpp @@ -243,22 +243,22 @@ class AnySubscriptionCallback TRACEPOINT( rclcpp_callback_register, static_cast(this), - get_symbol(shared_ptr_callback_)); + tracetools::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_)); + tracetools::get_symbol(shared_ptr_with_info_callback_)); } else if (unique_ptr_callback_) { TRACEPOINT( rclcpp_callback_register, static_cast(this), - get_symbol(unique_ptr_callback_)); + tracetools::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_)); + tracetools::get_symbol(unique_ptr_with_info_callback_)); } #endif // TRACETOOLS_DISABLED } 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. From 2562f715abe2a6878e3c44a23e33e6b9d2d633bd Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 30 Mar 2021 08:58:32 -0400 Subject: [PATCH 23/93] Revert "Namespace tracetools C++ functions (#1603)" (#1607) This reverts commit 3ab65715933fda37be06f14d3d6522161a1f1928. Signed-off-by: Chris Lalancette --- rclcpp/include/rclcpp/any_service_callback.hpp | 4 ++-- rclcpp/include/rclcpp/any_subscription_callback.hpp | 8 ++++---- rclcpp/include/rclcpp/timer.hpp | 2 +- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/rclcpp/include/rclcpp/any_service_callback.hpp b/rclcpp/include/rclcpp/any_service_callback.hpp index 78c3ed0052..86d420860d 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), - tracetools::get_symbol(shared_ptr_callback_)); + get_symbol(shared_ptr_callback_)); } else if (shared_ptr_with_request_header_callback_) { TRACEPOINT( rclcpp_callback_register, static_cast(this), - tracetools::get_symbol(shared_ptr_with_request_header_callback_)); + 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 9a31ecff53..5a708435bd 100644 --- a/rclcpp/include/rclcpp/any_subscription_callback.hpp +++ b/rclcpp/include/rclcpp/any_subscription_callback.hpp @@ -243,22 +243,22 @@ class AnySubscriptionCallback TRACEPOINT( rclcpp_callback_register, static_cast(this), - tracetools::get_symbol(shared_ptr_callback_)); + get_symbol(shared_ptr_callback_)); } else if (shared_ptr_with_info_callback_) { TRACEPOINT( rclcpp_callback_register, static_cast(this), - tracetools::get_symbol(shared_ptr_with_info_callback_)); + get_symbol(shared_ptr_with_info_callback_)); } else if (unique_ptr_callback_) { TRACEPOINT( rclcpp_callback_register, static_cast(this), - tracetools::get_symbol(unique_ptr_callback_)); + get_symbol(unique_ptr_callback_)); } else if (unique_ptr_with_info_callback_) { TRACEPOINT( rclcpp_callback_register, static_cast(this), - tracetools::get_symbol(unique_ptr_with_info_callback_)); + get_symbol(unique_ptr_with_info_callback_)); } #endif // TRACETOOLS_DISABLED } diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp index cc8e5c6ccd..a2662f4abe 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_), - tracetools::get_symbol(callback_)); + get_symbol(callback_)); } /// Default destructor. From f986ca3edce8447740e51f21cb7114cba3e2666f Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Tue, 30 Mar 2021 14:42:49 -0700 Subject: [PATCH 24/93] Fix flaky lifecycle node tests (#1606) Apparently, the topics and services that LifecycleNode provides are not available immediately after creating a node. Fix flaky tests by accounting for some delay between the LifecycleNode constructor and queries about its topics and services. Signed-off-by: Jacob Perron --- rclcpp_lifecycle/test/test_lifecycle_node.cpp | 141 ++++++++++++------ 1 file changed, 98 insertions(+), 43 deletions(-) diff --git a/rclcpp_lifecycle/test/test_lifecycle_node.cpp b/rclcpp_lifecycle/test/test_lifecycle_node.cpp index 44ccb79390..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: @@ -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"); From aa3a65d3ffe51e9600dc346a8b99840365f8e07f Mon Sep 17 00:00:00 2001 From: Christophe Bedard Date: Wed, 31 Mar 2021 00:51:57 -0400 Subject: [PATCH 25/93] Namespace tracetools C++ functions (#1608) Signed-off-by: Christophe Bedard --- rclcpp/include/rclcpp/any_service_callback.hpp | 4 ++-- rclcpp/include/rclcpp/any_subscription_callback.hpp | 8 ++++---- rclcpp/include/rclcpp/timer.hpp | 2 +- 3 files changed, 7 insertions(+), 7 deletions(-) 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 5a708435bd..9a31ecff53 100644 --- a/rclcpp/include/rclcpp/any_subscription_callback.hpp +++ b/rclcpp/include/rclcpp/any_subscription_callback.hpp @@ -243,22 +243,22 @@ class AnySubscriptionCallback TRACEPOINT( rclcpp_callback_register, static_cast(this), - get_symbol(shared_ptr_callback_)); + tracetools::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_)); + tracetools::get_symbol(shared_ptr_with_info_callback_)); } else if (unique_ptr_callback_) { TRACEPOINT( rclcpp_callback_register, static_cast(this), - get_symbol(unique_ptr_callback_)); + tracetools::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_)); + tracetools::get_symbol(unique_ptr_with_info_callback_)); } #endif // TRACETOOLS_DISABLED } 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. From 51a4b2155e9ec30b2a6669a4deeabdcaa5953bc7 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 31 Mar 2021 10:58:37 -0300 Subject: [PATCH 26/93] Initialize integers in test_parameter_event_handler.cpp to avoid undefined behavior (#1609) Signed-off-by: Ivan Santiago Paunovic --- rclcpp/test/rclcpp/test_parameter_event_handler.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rclcpp/test/rclcpp/test_parameter_event_handler.cpp b/rclcpp/test/rclcpp/test_parameter_event_handler.cpp index 761148f06f..868419249c 100644 --- a/rclcpp/test/rclcpp/test_parameter_event_handler.cpp +++ b/rclcpp/test/rclcpp/test_parameter_event_handler.cpp @@ -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(); From 474720511e8dd42ad5a41c266adb57abfc7031ac Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 31 Mar 2021 14:57:16 +0000 Subject: [PATCH 27/93] Changelog. Signed-off-by: Chris Lalancette --- rclcpp/CHANGELOG.rst | 9 +++++++++ rclcpp_action/CHANGELOG.rst | 3 +++ rclcpp_components/CHANGELOG.rst | 3 +++ rclcpp_lifecycle/CHANGELOG.rst | 8 ++++++++ 4 files changed, 23 insertions(+) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 9709bbdf82..c88b8db807 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 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 `_) diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 6fe45a13db..d67b7f64ec 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 8.1.0 (2021-03-25) ------------------ diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 7e7def6ca5..3af330f0a3 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 8.1.0 (2021-03-25) ------------------ diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 264166d7b9..e4c0bbf941 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,14 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 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) ------------------ From e7e3504fcfae77ea59830243f5d18e441429916e Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 31 Mar 2021 14:57:27 +0000 Subject: [PATCH 28/93] 8.2.0 --- rclcpp/CHANGELOG.rst | 4 ++-- rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 4 ++-- rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 4 ++-- rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 4 ++-- rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index c88b8db807..dd2465792c 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +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 `_) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 87cca58ba5..bb0977e7b0 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 8.1.0 + 8.2.0 The ROS client library in C++. Ivan Paunovic Mabel Zhang diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index d67b7f64ec..9d408a3273 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +8.2.0 (2021-03-31) +------------------ 8.1.0 (2021-03-25) ------------------ diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index bb14c105db..9301e79452 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 8.1.0 + 8.2.0 Adds action APIs for C++. Ivan Paunovic Mabel Zhang diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 3af330f0a3..d1755ec8bf 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +8.2.0 (2021-03-31) +------------------ 8.1.0 (2021-03-25) ------------------ diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index dc0ce2b2af..6bb3e35bd0 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 8.1.0 + 8.2.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 e4c0bbf941..24df585075 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +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 `_) diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 57aa09c23e..5eefe81ce5 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 8.1.0 + 8.2.0 Package containing a prototype for lifecycle implementation Ivan Paunovic Mabel Zhang From f19633f7998e630390b004dfc9e8c45632304543 Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Thu, 11 Mar 2021 16:12:38 +0000 Subject: [PATCH 29/93] Add RMW listener APIs Signed-off-by: Alberto Soragna --- rclcpp/include/rclcpp/client.hpp | 6 ++++++ .../subscription_intra_process_base.hpp | 6 ++++++ rclcpp/include/rclcpp/qos_event.hpp | 6 ++++++ rclcpp/include/rclcpp/service.hpp | 6 ++++++ rclcpp/include/rclcpp/subscription_base.hpp | 6 ++++++ rclcpp/include/rclcpp/waitable.hpp | 7 +++++++ rclcpp/src/rclcpp/client.cpp | 15 +++++++++++++++ rclcpp/src/rclcpp/qos_event.cpp | 16 ++++++++++++++++ rclcpp/src/rclcpp/service.cpp | 15 +++++++++++++++ rclcpp/src/rclcpp/subscription_base.cpp | 15 +++++++++++++++ .../rclcpp/subscription_intra_process_base.cpp | 16 ++++++++++++++++ rclcpp/src/rclcpp/waitable.cpp | 14 ++++++++++++++ 12 files changed, 128 insertions(+) diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index bd2d326012..3da956e583 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -150,6 +150,12 @@ class ClientBase bool exchange_in_use_by_wait_set_state(bool in_use_state); + RCLCPP_PUBLIC + void + set_listener_callback( + rmw_listener_callback_t callback, + const void * user_data) const; + protected: RCLCPP_DISABLE_COPY(ClientBase) diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp index c41317c705..f40f997762 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp @@ -74,6 +74,12 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable rmw_qos_profile_t get_actual_qos() const; + RCLCPP_PUBLIC + void + set_listener_callback( + rmw_listener_callback_t callback, + const void * user_data) const override; + protected: std::recursive_mutex reentrant_mutex_; rcl_guard_condition_t gc_; diff --git a/rclcpp/include/rclcpp/qos_event.hpp b/rclcpp/include/rclcpp/qos_event.hpp index b3bf61cea5..b3841cf2f5 100644 --- a/rclcpp/include/rclcpp/qos_event.hpp +++ b/rclcpp/include/rclcpp/qos_event.hpp @@ -102,6 +102,12 @@ class QOSEventHandlerBase : public Waitable bool is_ready(rcl_wait_set_t * wait_set) override; + RCLCPP_PUBLIC + void + set_listener_callback( + rmw_listener_callback_t callback, + const void * user_data) const override; + protected: rcl_event_t event_handle_; size_t wait_set_event_index_; diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index d36a18d4dc..283a5058e0 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -121,6 +121,12 @@ class ServiceBase bool exchange_in_use_by_wait_set_state(bool in_use_state); + RCLCPP_PUBLIC + void + set_listener_callback( + rmw_listener_callback_t callback, + const void * user_data) const; + protected: RCLCPP_DISABLE_COPY(ServiceBase) diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index 28b92ffb13..d7b7e9abaf 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -263,6 +263,12 @@ 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); + RCLCPP_PUBLIC + void + set_listener_callback( + rmw_listener_callback_t callback, + const void * user_data) const; + protected: template void diff --git a/rclcpp/include/rclcpp/waitable.hpp b/rclcpp/include/rclcpp/waitable.hpp index 85deabda85..9ae79c3508 100644 --- a/rclcpp/include/rclcpp/waitable.hpp +++ b/rclcpp/include/rclcpp/waitable.hpp @@ -203,6 +203,13 @@ class Waitable bool exchange_in_use_by_wait_set_state(bool in_use_state); + RCLCPP_PUBLIC + virtual + void + set_listener_callback( + rmw_listener_callback_t callback, + const void * user_data) const; + private: std::atomic in_use_by_wait_set_{false}; }; // class Waitable diff --git a/rclcpp/src/rclcpp/client.cpp b/rclcpp/src/rclcpp/client.cpp index 6a1d3934d4..c74dd59c8d 100644 --- a/rclcpp/src/rclcpp/client.cpp +++ b/rclcpp/src/rclcpp/client.cpp @@ -198,3 +198,18 @@ ClientBase::exchange_in_use_by_wait_set_state(bool in_use_state) { return in_use_by_wait_set_.exchange(in_use_state); } + +void +ClientBase::set_listener_callback( + rmw_listener_callback_t callback, + const void * user_data) const +{ + rcl_ret_t ret = rcl_client_set_listener_callback( + client_handle_.get(), + callback, + user_data); + + if (RCL_RET_OK != ret) { + throw std::runtime_error("Couldn't set listener callback to client"); + } +} diff --git a/rclcpp/src/rclcpp/qos_event.cpp b/rclcpp/src/rclcpp/qos_event.cpp index 8af3918c48..b646a52cd8 100644 --- a/rclcpp/src/rclcpp/qos_event.cpp +++ b/rclcpp/src/rclcpp/qos_event.cpp @@ -68,4 +68,20 @@ QOSEventHandlerBase::is_ready(rcl_wait_set_t * wait_set) return wait_set->events[wait_set_event_index_] == &event_handle_; } +void +QOSEventHandlerBase::set_listener_callback( + rmw_listener_callback_t callback, + const void * user_data) const +{ + rcl_ret_t ret = rcl_event_set_listener_callback( + &event_handle_, + callback, + user_data, + true /* Use previous events */); + + if (RCL_RET_OK != ret) { + throw std::runtime_error("Couldn't set listener callback to QOSEventHandlerBase"); + } +} + } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/service.cpp b/rclcpp/src/rclcpp/service.cpp index 827062bdd3..e279668bdf 100644 --- a/rclcpp/src/rclcpp/service.cpp +++ b/rclcpp/src/rclcpp/service.cpp @@ -84,3 +84,18 @@ ServiceBase::exchange_in_use_by_wait_set_state(bool in_use_state) { return in_use_by_wait_set_.exchange(in_use_state); } + +void +ServiceBase::set_listener_callback( + rmw_listener_callback_t callback, + const void * user_data) const +{ + rcl_ret_t ret = rcl_service_set_listener_callback( + service_handle_.get(), + callback, + user_data); + + if (RCL_RET_OK != ret) { + throw std::runtime_error("Couldn't set listener callback to service"); + } +} diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index 824d0f2f85..065de5cbbd 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -288,3 +288,18 @@ SubscriptionBase::exchange_in_use_by_wait_set_state( } throw std::runtime_error("given pointer_to_subscription_part does not match any part"); } + +void +SubscriptionBase::set_listener_callback( + rmw_listener_callback_t callback, + const void * user_data) const +{ + rcl_ret_t ret = rcl_subscription_set_listener_callback( + subscription_handle_.get(), + callback, + user_data); + + if (RCL_RET_OK != ret) { + throw std::runtime_error("Couldn't set listener callback to subscription"); + } +} diff --git a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp index 3b951f34de..65e3cbbe83 100644 --- a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp +++ b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp @@ -36,3 +36,19 @@ 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, + true /*Use previous events*/); + + if (RCL_RET_OK != ret) { + throw std::runtime_error("Couldn't set guard condition listener callback"); + } +} diff --git a/rclcpp/src/rclcpp/waitable.cpp b/rclcpp/src/rclcpp/waitable.cpp index b76c7215e0..09164f2e35 100644 --- a/rclcpp/src/rclcpp/waitable.cpp +++ b/rclcpp/src/rclcpp/waitable.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include + #include "rclcpp/waitable.hpp" using rclcpp::Waitable; @@ -57,3 +59,15 @@ Waitable::exchange_in_use_by_wait_set_state(bool in_use_state) { return in_use_by_wait_set_.exchange(in_use_state); } + +void +Waitable::set_listener_callback( + rmw_listener_callback_t callback, + const void * user_data) const +{ + (void)callback; + (void)user_data; + + throw std::runtime_error( + "Custom waitables should override set_listener_callback() if they want to use RMW listeners"); +} From b94349c4c87949fded7b0e358155d30846ac96c4 Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Tue, 16 Mar 2021 11:20:28 +0000 Subject: [PATCH 30/93] split long log into two lines Signed-off-by: Alberto Soragna --- rclcpp/src/rclcpp/waitable.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/rclcpp/src/rclcpp/waitable.cpp b/rclcpp/src/rclcpp/waitable.cpp index 09164f2e35..ed2398bc0d 100644 --- a/rclcpp/src/rclcpp/waitable.cpp +++ b/rclcpp/src/rclcpp/waitable.cpp @@ -69,5 +69,6 @@ Waitable::set_listener_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_listener_callback() " + "if they want to use RMW listeners"); } From f125d576b5f07110e3fc13e7309b230b5ddcd506 Mon Sep 17 00:00:00 2001 From: Mauro Passerino Date: Wed, 24 Mar 2021 16:53:21 -0300 Subject: [PATCH 31/93] Remove use_previous_event Signed-off-by: Mauro Passerino --- rclcpp/src/rclcpp/qos_event.cpp | 3 +-- rclcpp/src/rclcpp/subscription_intra_process_base.cpp | 3 +-- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/rclcpp/src/rclcpp/qos_event.cpp b/rclcpp/src/rclcpp/qos_event.cpp index b646a52cd8..0cd1968154 100644 --- a/rclcpp/src/rclcpp/qos_event.cpp +++ b/rclcpp/src/rclcpp/qos_event.cpp @@ -76,8 +76,7 @@ QOSEventHandlerBase::set_listener_callback( rcl_ret_t ret = rcl_event_set_listener_callback( &event_handle_, callback, - user_data, - true /* Use previous events */); + user_data); if (RCL_RET_OK != ret) { throw std::runtime_error("Couldn't set listener callback to QOSEventHandlerBase"); diff --git a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp index 65e3cbbe83..8a2aa7d521 100644 --- a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp +++ b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp @@ -45,8 +45,7 @@ SubscriptionIntraProcessBase::set_listener_callback( rcl_ret_t ret = rcl_guard_condition_set_listener_callback( &gc_, callback, - user_data, - true /*Use previous events*/); + user_data); if (RCL_RET_OK != ret) { throw std::runtime_error("Couldn't set guard condition listener callback"); From e187411e179e6e5fea131ae8861e68b2ed1278ba Mon Sep 17 00:00:00 2001 From: Mauro Passerino Date: Thu, 1 Apr 2021 08:24:57 -0300 Subject: [PATCH 32/93] Do not set gc listener callback Signed-off-by: Mauro Passerino --- rclcpp/src/rclcpp/subscription_intra_process_base.cpp | 10 ++-------- 1 file changed, 2 insertions(+), 8 deletions(-) diff --git a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp index 8a2aa7d521..8e2054962a 100644 --- a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp +++ b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp @@ -42,12 +42,6 @@ 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"); - } + (void)callback; + (void)user_data; } From bb09b5c2f0549f0c8623b6d44c66187131d26a86 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Thu, 1 Apr 2021 20:58:12 -0700 Subject: [PATCH 33/93] significant refactor to make callbacks type safe Signed-off-by: William Woodall --- rclcpp/include/rclcpp/client.hpp | 83 ++++++++++++++++-- .../rclcpp/detail/cpp_callback_trampoline.hpp | 67 ++++++++++++++ .../subscription_intra_process_base.hpp | 24 ++--- rclcpp/include/rclcpp/qos_event.hpp | 77 ++++++++++++++-- rclcpp/include/rclcpp/service.hpp | 87 +++++++++++++++++-- rclcpp/include/rclcpp/subscription_base.hpp | 74 +++++++++++++++- rclcpp/include/rclcpp/waitable.hpp | 7 -- rclcpp/src/rclcpp/client.cpp | 12 +-- rclcpp/src/rclcpp/context.cpp | 2 - rclcpp/src/rclcpp/qos_event.cpp | 11 +-- rclcpp/src/rclcpp/service.cpp | 12 +-- rclcpp/src/rclcpp/subscription_base.cpp | 12 +-- .../subscription_intra_process_base.cpp | 9 -- rclcpp/src/rclcpp/waitable.cpp | 13 --- 14 files changed, 394 insertions(+), 96 deletions(-) create mode 100644 rclcpp/include/rclcpp/detail/cpp_callback_trampoline.hpp diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index 3da956e583..2ca256ab6e 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -28,18 +28,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 +151,68 @@ 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. + * + * 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_listener_callback( - rmw_listener_callback_t callback, - const void * user_data) const; + set_on_new_response_callback(std::function callback) + { + 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"); + } + }; + + // 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_)); + } protected: RCLCPP_DISABLE_COPY(ClientBase) @@ -171,13 +229,20 @@ 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::function on_new_response_callback_; }; template @@ -303,8 +368,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/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/experimental/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp index f40f997762..4cc8f74a6d 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp @@ -47,23 +47,23 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable 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,12 +74,6 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable rmw_qos_profile_t get_actual_qos() const; - RCLCPP_PUBLIC - void - set_listener_callback( - rmw_listener_callback_t callback, - const void * user_data) const override; - protected: std::recursive_mutex reentrant_mutex_; rcl_guard_condition_t gc_; diff --git a/rclcpp/include/rclcpp/qos_event.hpp b/rclcpp/include/rclcpp/qos_event.hpp index b3841cf2f5..2eaff577b7 100644 --- a/rclcpp/include/rclcpp/qos_event.hpp +++ b/rclcpp/include/rclcpp/qos_event.hpp @@ -21,12 +21,15 @@ #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 @@ -102,15 +105,78 @@ 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. + * + * 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 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_listener_callback( - rmw_listener_callback_t callback, - const void * user_data) const override; + set_on_new_event_callback(std::function callback) + { + auto new_callback = + [callback, this](size_t number_of_events) { + try { + callback(number_of_events); + } 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 new event' callback: " << + exception.what()); + } catch (...) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("rclcpp"), + "rclcpp::QOSEventHandlerBase@" << this << + " caught unhandled exception in user-provided callback " << + "for the 'on new event' callback"); + } + }; + + // Set it temporarily to the new callback, while we replace the old one. + // This two-step setting, prevents a gap where the old std::function has + // been replaced but the middleware hasn't been told about the new one yet. + set_on_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_)); + } 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::function on_new_event_callback_; }; template @@ -123,9 +189,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/service.hpp b/rclcpp/include/rclcpp/service.hpp index 283a5058e0..bb02834f31 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -25,16 +25,20 @@ #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 +125,68 @@ 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. + * + * 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_listener_callback( - rmw_listener_callback_t callback, - const void * user_data) const; + set_on_new_request_callback(std::function callback) + { + 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"); + } + }; + + // 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_)); + } protected: RCLCPP_DISABLE_COPY(ServiceBase) @@ -138,12 +199,20 @@ 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::function on_new_request_callback_; }; template diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index d7b7e9abaf..11e7ba4ddc 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -24,9 +24,11 @@ #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" @@ -263,11 +265,68 @@ 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); - RCLCPP_PUBLIC + /// Set a callback to be called when each new message is received. + /** + * The callback receives a size_t which is the number of messages received + * since the last time this callback was called. + * Normally this is 1, but can be > 1 if messages were received before any + * callback was set. + * + * Since this callback is called from the middleware, you should aim to make + * it fast and not blocking. + * If you need to do a lot of work or wait for some other event, you should + * spin it off to another thread, otherwise you risk blocking the middleware. + * + * Calling it again will clear any previously set callback. + * + * This function is thread-safe. + * + * If you want more information available in the callback, like the subscription + * or other information, you may use a lambda with captures or std::bind. + * + * \sa rmw_subscription_set_on_new_message_callback + * \sa rcl_subscription_set_on_new_message_callback + * + * \param[in] callback functor to be called when a new message is received + */ void - set_listener_callback( - rmw_listener_callback_t callback, - const void * user_data) const; + set_on_new_message_callback(std::function callback) + { + 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"); + } + }; + + // 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_)); + } protected: template @@ -293,11 +352,16 @@ 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_; @@ -315,6 +379,8 @@ 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::function on_new_message_callback_; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/waitable.hpp b/rclcpp/include/rclcpp/waitable.hpp index 9ae79c3508..85deabda85 100644 --- a/rclcpp/include/rclcpp/waitable.hpp +++ b/rclcpp/include/rclcpp/waitable.hpp @@ -203,13 +203,6 @@ class Waitable bool exchange_in_use_by_wait_set_state(bool in_use_state); - RCLCPP_PUBLIC - virtual - void - set_listener_callback( - rmw_listener_callback_t callback, - const void * user_data) const; - private: std::atomic in_use_by_wait_set_{false}; }; // class Waitable 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 90ea3db908..db4a485ec0 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; 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..7b8d58a505 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -39,6 +39,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), @@ -290,16 +291,17 @@ 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"); } } diff --git a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp index 8e2054962a..3b951f34de 100644 --- a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp +++ b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp @@ -36,12 +36,3 @@ SubscriptionIntraProcessBase::get_actual_qos() const { return qos_profile_; } - -void -SubscriptionIntraProcessBase::set_listener_callback( - rmw_listener_callback_t callback, - const void * user_data) const -{ - (void)callback; - (void)user_data; -} diff --git a/rclcpp/src/rclcpp/waitable.cpp b/rclcpp/src/rclcpp/waitable.cpp index ed2398bc0d..ef3a50a8d9 100644 --- a/rclcpp/src/rclcpp/waitable.cpp +++ b/rclcpp/src/rclcpp/waitable.cpp @@ -59,16 +59,3 @@ Waitable::exchange_in_use_by_wait_set_state(bool in_use_state) { return in_use_by_wait_set_.exchange(in_use_state); } - -void -Waitable::set_listener_callback( - rmw_listener_callback_t callback, - const void * user_data) const -{ - (void)callback; - (void)user_data; - - throw std::runtime_error( - "Custom waitables should override set_listener_callback() " - "if they want to use RMW listeners"); -} From 70dfa2e778aba0196d871f907d34ff07b80198d3 Mon Sep 17 00:00:00 2001 From: shonigmann Date: Thu, 1 Apr 2021 23:39:08 -0700 Subject: [PATCH 34/93] updating quality declaration links (re: ros2/docs.ros2.org#52) (#1615) Signed-off-by: Simon Honigmann Co-authored-by: Simon Honigmann --- rclcpp/QUALITY_DECLARATION.md | 18 +++++++++--------- rclcpp_action/QUALITY_DECLARATION.md | 18 +++++++++--------- rclcpp_components/QUALITY_DECLARATION.md | 18 +++++++++--------- rclcpp_lifecycle/QUALITY_DECLARATION.md | 18 +++++++++--------- 4 files changed, 36 insertions(+), 36 deletions(-) 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_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_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_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/) From cd3fd53c8c20fa2fcc2942c238d83c51c8230eba Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Sat, 3 Apr 2021 07:59:34 +0900 Subject: [PATCH 35/93] use context from `node_base_` for clock executor. (#1617) Signed-off-by: Tomoya Fujita --- rclcpp/include/rclcpp/time_source.hpp | 2 +- rclcpp/src/rclcpp/time_source.cpp | 12 ++++++++---- 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/rclcpp/include/rclcpp/time_source.hpp b/rclcpp/include/rclcpp/time_source.hpp index bad683b4d3..6f5280572a 100644 --- a/rclcpp/include/rclcpp/time_source.hpp +++ b/rclcpp/include/rclcpp/time_source.hpp @@ -152,7 +152,7 @@ class TimeSource std::shared_ptr clock_subscription_{nullptr}; std::mutex clock_sub_lock_; rclcpp::CallbackGroup::SharedPtr clock_callback_group_; - rclcpp::executors::SingleThreadedExecutor clock_executor_; + rclcpp::executors::SingleThreadedExecutor::SharedPtr clock_executor_; std::promise cancel_clock_executor_promise_; // The clock callback itself diff --git a/rclcpp/src/rclcpp/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp index 9f8642825c..24f48b7c7c 100644 --- a/rclcpp/src/rclcpp/time_source.cpp +++ b/rclcpp/src/rclcpp/time_source.cpp @@ -256,13 +256,17 @@ void TimeSource::create_clock_sub() 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()) { clock_executor_thread_ = std::thread( [this]() { cancel_clock_executor_promise_ = std::promise{}; 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_executor_->add_callback_group(clock_callback_group_, node_base_); + clock_executor_->spin_until_future_complete(future); } ); } @@ -283,9 +287,9 @@ 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_->cancel(); clock_executor_thread_.join(); - clock_executor_.remove_callback_group(clock_callback_group_); + clock_executor_->remove_callback_group(clock_callback_group_); } clock_subscription_.reset(); } From 95adde2a19426c23d6ff6e13fab21d62e5e2ae69 Mon Sep 17 00:00:00 2001 From: Nikolai Morin Date: Sat, 3 Apr 2021 02:01:47 +0200 Subject: [PATCH 36/93] Add generic publisher and generic subscription for serialized messages (#1452) * Copying files from rosbag2 The generic_* files are from rosbag2_transport typesupport_helpers incl. test is from rosbag2_cpp memory_management.hpp is from rosbag2_test_common test_pubsub.cpp was renamed from test_rosbag2_node.cpp from rosbag2_transport Signed-off-by: Nikolai Morin * Rebrand into rclcpp_generic Add package.xml, CMakeLists.txt, Doxyfile, README.md and CHANGELOG.rst Rename namespaces Make GenericPublisher and GenericSubscription self-contained by storing shared library New create() methods that return shared pointers Add docstrings Include only what is needed Make linters & tests pass Signed-off-by: Nikolai Morin * Review feedback * Delete CHANGELOG.rst * Enable cppcheck * Remove all references to rosbag2/ros2bag Signed-off-by: Nikolai Morin * Move rclpp_generic into rclcpp Signed-off-by: Nikolai Morin * Rename namespace rclcpp_generic to rclcpp::generic Signed-off-by: Nikolai Morin * Free 'create' functions instead of static functions in class Signed-off-by: Nikolai Morin * Remove 'generic' subdirectory and namespace hierarchy Signed-off-by: Nikolai Morin * Order includes according to style guide Signed-off-by: Nikolai Morin * Remove extra README.md Signed-off-by: Nikolai Morin * Also add brief to class docs Signed-off-by: Nikolai Morin * Make ament_index_cpp a build_depend Signed-off-by: Nikolai Morin * Add to rclcpp.hpp Signed-off-by: Nikolai Morin * Remove memory_management, use rclcpp::SerializedMessage in GenericPublisher::publish Signed-off-by: Nikolai Morin * Clean up the typesupport_helpers Signed-off-by: Nikolai Morin * Use make_shared, add UnimplementedError Signed-off-by: Nikolai Morin * Add more comments, make member variable private, remove unnecessary include Signed-off-by: Nikolai Morin * Apply suggestions from code review Co-authored-by: William Woodall Signed-off-by: Nikolai Morin * Rename test Signed-off-by: Nikolai Morin * Update copyright and remove ament_target_dependencies for test Signed-off-by: Nikolai Morin * Accept PublisherOptions and SubscriptionOptions Signed-off-by: Nikolai Morin * Remove target_include_directories Signed-off-by: Nikolai Morin * Add explanatory comment to SubscriptionBase Signed-off-by: Nikolai Morin * Use kSolibPrefix and kSolibExtension from rcpputils Signed-off-by: Nikolai Morin * Fix downstream build failure by making ament_index_cpp a build_export_depend Signed-off-by: Nikolai Morin * Use path_for_library(), fix documentation nitpicks Signed-off-by: Nikolai Morin * Improve error handling in get_typesupport_handle Signed-off-by: Nikolai Morin * Accept SubscriptionOptions in GenericSubscription Signed-off-by: Nikolai Morin * Make use of PublisherOptions in GenericPublisher Signed-off-by: Nikolai Morin * Document typesupport_helpers Signed-off-by: Nikolai Morin * Improve documentation Signed-off-by: Nikolai Morin * Use std::function instead of function pointer Co-authored-by: William Woodall Signed-off-by: Nikolai Morin * Minimize vertical whitespace Signed-off-by: Nikolai Morin * Add TODO for callback with message info Signed-off-by: Nikolai Morin * Link issue in TODO Signed-off-by: Nikolai Morin * Add missing include for functional Signed-off-by: nnmm * Fix compilation Signed-off-by: Jacob Perron * Fix lint Signed-off-by: Jacob Perron * Address review comments (#1) * fix redefinition of default template arguments Signed-off-by: Karsten Knese * address review comments Signed-off-by: Karsten Knese * rename test executable Signed-off-by: Karsten Knese * add functionality to lifecycle nodes Signed-off-by: Karsten Knese * Refactor typesupport helpers * Make extract_type_identifier function private * Remove unused extract_type_and_package function * Update unit tests Signed-off-by: Jacob Perron * Remove note about ament from classes This comment only applies to the free functions. Signed-off-by: Jacob Perron * Fix formatting Co-authored-by: Karsten Knese * Fix warning Possible loss of data from double to rcutils_duration_value_t Signed-off-by: Jacob Perron * Add missing visibility macros Signed-off-by: Jacob Perron Co-authored-by: William Woodall Co-authored-by: Jacob Perron Co-authored-by: Karsten Knese --- rclcpp/CMakeLists.txt | 12 +- .../rclcpp/create_generic_publisher.hpp | 69 ++++++ .../rclcpp/create_generic_subscription.hpp | 79 +++++++ rclcpp/include/rclcpp/generic_publisher.hpp | 127 ++++++++++++ .../include/rclcpp/generic_subscription.hpp | 161 ++++++++++++++ rclcpp/include/rclcpp/node.hpp | 52 +++++ rclcpp/include/rclcpp/node_impl.hpp | 41 +++- rclcpp/include/rclcpp/rclcpp.hpp | 10 + rclcpp/include/rclcpp/subscription_base.hpp | 7 +- rclcpp/include/rclcpp/typesupport_helpers.hpp | 57 +++++ rclcpp/package.xml | 2 + rclcpp/src/rclcpp/generic_publisher.cpp | 34 +++ rclcpp/src/rclcpp/generic_subscription.cpp | 67 ++++++ rclcpp/src/rclcpp/typesupport_helpers.cpp | 136 ++++++++++++ rclcpp/test/rclcpp/CMakeLists.txt | 19 ++ rclcpp/test/rclcpp/test_generic_pubsub.cpp | 196 ++++++++++++++++++ .../test/rclcpp/test_typesupport_helpers.cpp | 77 +++++++ .../rclcpp_lifecycle/lifecycle_node.hpp | 31 +++ .../rclcpp_lifecycle/lifecycle_node_impl.hpp | 48 ++++- 19 files changed, 1216 insertions(+), 9 deletions(-) create mode 100644 rclcpp/include/rclcpp/create_generic_publisher.hpp create mode 100644 rclcpp/include/rclcpp/create_generic_subscription.hpp create mode 100644 rclcpp/include/rclcpp/generic_publisher.hpp create mode 100644 rclcpp/include/rclcpp/generic_subscription.hpp create mode 100644 rclcpp/include/rclcpp/typesupport_helpers.hpp create mode 100644 rclcpp/src/rclcpp/generic_publisher.cpp create mode 100644 rclcpp/src/rclcpp/generic_subscription.cpp create mode 100644 rclcpp/src/rclcpp/typesupport_helpers.cpp create mode 100644 rclcpp/test/rclcpp/test_generic_pubsub.cpp create mode 100644 rclcpp/test/rclcpp/test_typesupport_helpers.cpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 79d67325f4..d28ce2be97 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) @@ -51,12 +52,14 @@ 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/multi_threaded_executor.cpp src/rclcpp/executors/single_threaded_executor.cpp src/rclcpp/executors/static_executor_entities_collector.cpp src/rclcpp/executors/static_single_threaded_executor.cpp + src/rclcpp/expand_topic_or_service_name.cpp src/rclcpp/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 @@ -67,7 +70,6 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/memory_strategy.cpp src/rclcpp/message_info.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 @@ -78,13 +80,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 @@ -99,6 +102,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 @@ -179,6 +183,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" @@ -209,6 +214,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) 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/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..d9f40d868b --- /dev/null +++ b/rclcpp/include/rclcpp/generic_subscription.hpp @@ -0,0 +1,161 @@ +// 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; + + /// 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/node.hpp b/rclcpp/include/rclcpp/node.hpp index 78eb1db93b..b2156a0068 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" @@ -266,6 +269,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. diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 8ed078113f..48e2430541 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" @@ -152,6 +154,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/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/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index 28b92ffb13..c9a3ec7b7c 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -61,8 +61,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 +80,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(); 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/package.xml b/rclcpp/package.xml index bb0977e7b0..c7709693d7 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -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/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..d72d21a0f9 --- /dev/null +++ b/rclcpp/src/rclcpp/generic_subscription.cpp @@ -0,0 +1,67 @@ +// 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 & message, const rclcpp::MessageInfo & message_info) +{ + (void) message_info; + auto typed_message = std::static_pointer_cast(message); + callback_(typed_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/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/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index d26c3ed07d..f6244b5815 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -368,6 +368,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 +488,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 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_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_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 451c09e383..d81750ba08 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" @@ -283,6 +285,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..77598fdcb7 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" @@ -130,6 +132,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( From 1037822a63330495dcf0de5e8f20544375a5f116 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 2 Apr 2021 17:30:29 -0700 Subject: [PATCH 37/93] refactor AnySubscriptionCallback and add/deprecate callback signatures (#1598) * refactor AnySubscriptionCallback to... use std::variant and make the dispatch functions constexpr, avoiding runtime dispatching. Also, deprecate the std::function)> signature, as it is unsafe to share one shared_ptr when multiple subscriptions take it, because they could mutate MessageT while others are using it. So you'd have to make a copy for each subscription, which is no different than the std::unique_ptr signature or the user making their own copy in a shared_ptr from the const MessageT & signature or the std::shared_ptr signature. Added a std::function &)> signature to go along side the existing std::function)> signature. Removed redundant 'const' before pass-by-value signatures, e.g. std::function)> became std::function)>. This will not affect API or any users using the old style. Signed-off-by: William Woodall * fix use of std::bind, free functions, etc. using new function_traits::as_std_function<> Signed-off-by: William Woodall * fix use of const MessageT & callbacks by fixing subscriptin_traits Signed-off-by: William Woodall * fix deprecation warnings Signed-off-by: William Woodall * use target_compile_features to require c++17 for downstream users of rclcpp Signed-off-by: William Woodall * uncrustify Signed-off-by: William Woodall * cpplint Signed-off-by: William Woodall * use target_compile_features(... cxx_std_17) Signed-off-by: William Woodall * Keep both std::shared_ptr and const std::shared_ptr & signatures. The const std::shared_ptr & signature is being kept because it can be more flexible and efficient than std::shared_ptr, but costs realtively little to support. The std::shared_ptr signature is being kept because we want to avoid deprecating it and causing disruption, and because it is convenient to write, and in most cases will not materially impact the performance. Signed-off-by: William Woodall * defer deprecation of the shared_ptr sub callbacks Signed-off-by: William Woodall * fix unused variable warning Signed-off-by: William Woodall * small fixups to AnySubscriptionCallback Signed-off-by: William Woodall * add check for unset AnySubscriptionCallback in dispatch methods Signed-off-by: William Woodall * update dispatch methods to handle all scenarios Signed-off-by: William Woodall * updated tests for AnySubscriptionCallback, include full parameterized i/o matrix Signed-off-by: William Woodall * fixup test with changed assumption Signed-off-by: William Woodall * remove use of std::unary_function, which was removed in c++17 Signed-off-by: William Woodall * silence c++17 warnings on windows for now Signed-off-by: William Woodall --- rclcpp/CMakeLists.txt | 13 +- .../rclcpp/any_subscription_callback.hpp | 492 +++++++++++------- .../subscription_callback_type_helper.hpp | 164 ++++++ rclcpp/include/rclcpp/function_traits.hpp | 26 + .../rclcpp/parameter_event_handler.hpp | 12 +- .../rclcpp/parameter_events_filter.hpp | 6 +- .../include/rclcpp/subscription_factory.hpp | 2 +- rclcpp/include/rclcpp/subscription_traits.hpp | 2 +- rclcpp/include/rclcpp/time_source.hpp | 6 +- .../node_interfaces/node_parameters.cpp | 20 +- rclcpp/src/rclcpp/parameter_event_handler.cpp | 30 +- rclcpp/src/rclcpp/parameter_events_filter.cpp | 3 +- rclcpp/src/rclcpp/time_source.cpp | 5 +- rclcpp/test/benchmark/benchmark_executor.cpp | 2 +- .../test/rclcpp/executors/test_executors.cpp | 2 +- ...est_static_executor_entities_collector.cpp | 4 +- .../node_interfaces/test_node_graph.cpp | 2 +- .../test_allocator_memory_strategy.cpp | 4 +- .../test_add_callback_groups_to_executor.cpp | 8 +- .../rclcpp/test_any_subscription_callback.cpp | 418 ++++++++++----- .../test/rclcpp/test_create_subscription.cpp | 8 +- rclcpp/test/rclcpp/test_memory_strategy.cpp | 4 +- rclcpp/test/rclcpp/test_node.cpp | 2 +- rclcpp/test/rclcpp/test_parameter_client.cpp | 2 +- .../rclcpp/test_parameter_event_handler.cpp | 22 +- .../test_publisher_subscription_count_api.cpp | 2 +- rclcpp/test/rclcpp/test_qos_event.cpp | 4 +- .../test_serialized_message_allocator.cpp | 2 +- rclcpp/test/rclcpp/test_subscription.cpp | 16 +- .../test_subscription_publisher_count_api.cpp | 2 +- rclcpp/test/rclcpp/test_wait_set.cpp | 4 +- .../test_dynamic_storage.cpp | 8 +- .../wait_set_policies/test_static_storage.cpp | 4 +- .../test_storage_policy_common.cpp | 4 +- .../test_thread_safe_synchronization.cpp | 8 +- 35 files changed, 884 insertions(+), 429 deletions(-) create mode 100644 rclcpp/include/rclcpp/detail/subscription_callback_type_helper.hpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index d28ce2be97..ea1e516fc0 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -21,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 @@ -174,8 +175,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 "$" "$" diff --git a/rclcpp/include/rclcpp/any_subscription_callback.hpp b/rclcpp/include/rclcpp/any_subscription_callback.hpp index 9a31ecff53..61b5e2e82e 100644 --- a/rclcpp/include/rclcpp/any_subscription_callback.hpp +++ b/rclcpp/include/rclcpp/any_subscription_callback.hpp @@ -15,256 +15,394 @@ #ifndef RCLCPP__ANY_SUBSCRIPTION_CALLBACK_HPP_ #define RCLCPP__ANY_SUBSCRIPTION_CALLBACK_HPP_ -#include - #include #include #include #include #include +#include // NOLINT[build/include_order] + +#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" + +template +inline constexpr bool always_false_v = false; namespace rclcpp { -template -class AnySubscriptionCallback +namespace detail { - using MessageAllocTraits = allocator::AllocRebind; + +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 SharedPtrCallback = std::function)>; - using SharedPtrWithInfoCallback = - std::function, const rclcpp::MessageInfo &)>; - using ConstSharedPtrCallback = std::function)>; - using ConstSharedPtrWithInfoCallback = - std::function, const rclcpp::MessageInfo &)>; - using UniquePtrCallback = std::function; +template +struct AnySubscriptionCallbackHelper +{ + using MessageDeleter = typename MessageDeleterHelper::MessageDeleter; + + using ConstRefCallback = + std::function; + using ConstRefWithInfoCallback = + std::function; + + using UniquePtrCallback = + std::function)>; using UniquePtrWithInfoCallback = - std::function; + std::function, const rclcpp::MessageInfo &)>; + + using SharedConstPtrCallback = + std::function)>; + using SharedConstPtrWithInfoCallback = + std::function, const rclcpp::MessageInfo &)>; + + using ConstRefSharedConstPtrCallback = + std::function &)>; + using ConstRefSharedConstPtrWithInfoCallback = + std::function &, const rclcpp::MessageInfo &)>; - 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_; + // Deprecated signatures: + using SharedPtrCallback = + std::function)>; + using SharedPtrWithInfoCallback = + std::function, const rclcpp::MessageInfo &)>; + + using variant_type = std::variant< + ConstRefCallback, + ConstRefWithInfoCallback, + UniquePtrCallback, + UniquePtrWithInfoCallback, + SharedConstPtrCallback, + SharedConstPtrWithInfoCallback, + ConstRefSharedConstPtrCallback, + ConstRefSharedConstPtrWithInfoCallback, + SharedPtrCallback, + SharedPtrWithInfoCallback + >; +}; + +} // namespace detail + +template< + typename MessageT, + typename AllocatorT = std::allocator +> +class AnySubscriptionCallback +{ +private: + using HelperT = typename rclcpp::detail::AnySubscriptionCallbackHelper; + using MessageDeleterHelper = rclcpp::detail::MessageDeleterHelper; + + using MessageAllocTraits = typename MessageDeleterHelper::MessageAllocTraits; + using MessageAlloc = typename MessageDeleterHelper::MessageAlloc; + using MessageDeleter = typename MessageDeleterHelper::MessageDeleter; + + // See AnySubscriptionCallbackHelper for the types of these. + using ConstRefCallback = typename HelperT::ConstRefCallback; + using ConstRefWithInfoCallback = typename HelperT::ConstRefWithInfoCallback; + + using UniquePtrCallback = typename HelperT::UniquePtrCallback; + using UniquePtrWithInfoCallback = typename HelperT::UniquePtrWithInfoCallback; + + using SharedConstPtrCallback = typename HelperT::SharedConstPtrCallback; + using SharedConstPtrWithInfoCallback = typename HelperT::SharedConstPtrWithInfoCallback; + + using ConstRefSharedConstPtrCallback = + typename HelperT::ConstRefSharedConstPtrCallback; + using ConstRefSharedConstPtrWithInfoCallback = + typename HelperT::ConstRefSharedConstPtrWithInfoCallback; + + using SharedPtrCallback = typename HelperT::SharedPtrCallback; + using SharedPtrWithInfoCallback = typename HelperT::SharedPtrWithInfoCallback; 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] + { + message_allocator_ = allocator; + allocator::set_allocator_for_deleter(&message_deleter_, &message_allocator_); + } + + [[deprecated("use AnySubscriptionCallback(const AllocatorT & allocator) instead")]] + explicit + AnySubscriptionCallback(std::shared_ptr allocator) // NOLINT[runtime/explicit] { 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()); + message_allocator_ = *allocator; + allocator::set_allocator_for_deleter(&message_deleter_, &message_allocator_); } AnySubscriptionCallback(const AnySubscriptionCallback &) = default; - template< - typename CallbackT, - typename std::enable_if< - rclcpp::function_traits::same_arguments< - CallbackT, - SharedPtrCallback - >::value - >::type * = nullptr - > - void set(CallbackT callback) + /// 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) { - shared_ptr_callback_ = 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; - template< - typename CallbackT, - typename std::enable_if< + // Determine if the given CallbackT is a deprecated signature or not. + constexpr auto is_deprecated = rclcpp::function_traits::same_arguments< - CallbackT, - SharedPtrWithInfoCallback + typename scbth::callback_type, + std::function)> >::value - >::type * = nullptr - > - void set(CallbackT callback) - { - shared_ptr_with_info_callback_ = callback; - } - - template< - typename CallbackT, - typename std::enable_if< + || rclcpp::function_traits::same_arguments< - CallbackT, - ConstSharedPtrCallback - >::value - >::type * = nullptr - > - void set(CallbackT callback) - { - const_shared_ptr_callback_ = callback; + 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; } - template< - typename CallbackT, - typename std::enable_if< - rclcpp::function_traits::same_arguments< - CallbackT, - ConstSharedPtrWithInfoCallback - >::value - >::type * = nullptr - > - void set(CallbackT callback) + /// Function for shared_ptr to non-const MessageT, which is deprecated. + // TODO(wjwwood): enable this deprecation after Galactic + // [[deprecated( + // "use 'void (std::shared_ptr)' instead" + // )]] + void + set_deprecated(std::function)> callback) + // set(CallbackT callback) { - const_shared_ptr_with_info_callback_ = callback; + callback_variant_ = callback; } - template< - typename CallbackT, - typename std::enable_if< - rclcpp::function_traits::same_arguments< - CallbackT, - UniquePtrCallback - >::value - >::type * = nullptr - > - void set(CallbackT callback) + /// Function for shared_ptr to non-const MessageT with MessageInfo, which is deprecated. + // 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) { - unique_ptr_callback_ = callback; + callback_variant_ = callback; } - template< - typename CallbackT, - typename std::enable_if< - rclcpp::function_traits::same_arguments< - CallbackT, - UniquePtrWithInfoCallback - >::value - >::type * = nullptr - > - void set(CallbackT callback) + std::unique_ptr + create_unique_ptr_from_shared_ptr_message(const std::shared_ptr & message) { - unique_ptr_with_info_callback_ = callback; + auto ptr = MessageAllocTraits::allocate(message_allocator_, 1); + MessageAllocTraits::construct(message_allocator_, ptr, *message); + return std::unique_ptr(ptr, message_deleter_); } - void dispatch( - std::shared_ptr message, const rclcpp::MessageInfo & message_info) + 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; + + if constexpr (std::is_same_v) { + callback(*message); + } else if constexpr (std::is_same_v) { + callback(*message, message_info); + } else if constexpr (std::is_same_v) { + callback(create_unique_ptr_from_shared_ptr_message(message)); + } else if constexpr (std::is_same_v) { + callback(create_unique_ptr_from_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); + } else { + 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) + void + dispatch_intra_process( + std::shared_ptr message, + const rclcpp::MessageInfo & message_info) { TRACEPOINT(callback_start, static_cast(this), true); - 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"); + // 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; + + if constexpr (std::is_same_v) { + 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_unique_ptr_from_shared_ptr_message(message)); + } else if constexpr ( // NOLINT[readability/braces] + std::is_same_v|| + std::is_same_v) + { + callback(create_unique_ptr_from_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); + } else { + 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 (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"); + // 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](auto && callback) { + using T = std::decay_t; + + if constexpr (std::is_same_v) { + 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|| + 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|| + std::is_same_v|| + std::is_same_v) + { + callback(std::move(message), message_info); + } else { + 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 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_); } - void register_callback_for_tracing() + void + register_callback_for_tracing() { #ifndef TRACETOOLS_DISABLED - if (shared_ptr_callback_) { - TRACEPOINT( - rclcpp_callback_register, - static_cast(this), - tracetools::get_symbol(shared_ptr_callback_)); - } else if (shared_ptr_with_info_callback_) { - TRACEPOINT( - rclcpp_callback_register, - static_cast(this), - tracetools::get_symbol(shared_ptr_with_info_callback_)); - } else if (unique_ptr_callback_) { - TRACEPOINT( - rclcpp_callback_register, - static_cast(this), - tracetools::get_symbol(unique_ptr_callback_)); - } else if (unique_ptr_with_info_callback_) { - TRACEPOINT( - rclcpp_callback_register, - static_cast(this), - tracetools::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_; + // 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_; + + MessageAlloc message_allocator_; MessageDeleter message_deleter_; }; 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/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/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/subscription_factory.hpp b/rclcpp/include/rclcpp/subscription_factory.hpp index 694be60a74..7708f27df0 100644 --- a/rclcpp/include/rclcpp/subscription_factory.hpp +++ b/rclcpp/include/rclcpp/subscription_factory.hpp @@ -93,7 +93,7 @@ create_subscription_factory( auto allocator = options.get_allocator(); using rclcpp::AnySubscriptionCallback; - AnySubscriptionCallback any_subscription_callback(allocator); + AnySubscriptionCallback any_subscription_callback(*allocator); any_subscription_callback.set(std::forward(callback)); SubscriptionFactory factory { 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 6f5280572a..b4eb2aff24 100644 --- a/rclcpp/include/rclcpp/time_source.hpp +++ b/rclcpp/include/rclcpp/time_source.hpp @@ -156,7 +156,7 @@ class TimeSource 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(); @@ -170,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}; @@ -191,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/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index c6aa2af6e2..16aa9c4903 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -954,22 +954,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) @@ -980,7 +964,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/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/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp index 24f48b7c7c..595520da4e 100644 --- a/rclcpp/src/rclcpp/time_source.cpp +++ b/rclcpp/src/rclcpp/time_source.cpp @@ -216,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(); @@ -294,7 +294,8 @@ void TimeSource::destroy_clock_sub() 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/test/benchmark/benchmark_executor.cpp b/rclcpp/test/benchmark/benchmark_executor.cpp index 27e6d2ff5b..c1fb3cf52c 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))); diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index ad34273f68..970cd3bdb2 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -62,7 +62,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)); 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 5b20c6b201..fba322f9e2 100644 --- a/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp +++ b/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp @@ -255,7 +255,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 = @@ -455,7 +455,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 = diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp index 08717164e2..6690b88bf6 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 = diff --git a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp index b0951c9218..0b4286dcf2 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); @@ -778,7 +778,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 25bd2aba03..fa636b7157 100644 --- a/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp +++ b/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp @@ -96,7 +96,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; @@ -139,7 +139,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; @@ -222,7 +222,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; @@ -256,7 +256,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..b8d7fd9ba3 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); } 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,285 @@ 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); +class InstanceContextImpl +{ +public: + InstanceContextImpl() = default; + virtual ~InstanceContextImpl() = default; - // 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); -} + explicit InstanceContextImpl(rclcpp::AnySubscriptionCallback asc) + : any_subscription_callback_(asc) + {} -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++; - }; + virtual + rclcpp::AnySubscriptionCallback + get_any_subscription_callback_to_test() const + { + return any_subscription_callback_; + } - any_subscription_callback_.set(shared_ptr_w_info_callback); +protected: + rclcpp::AnySubscriptionCallback any_subscription_callback_; +}; - EXPECT_NO_THROW(any_subscription_callback_.dispatch(msg_shared_ptr_, message_info_)); - EXPECT_EQ(callback_count, 1); +class InstanceContext +{ +public: + InstanceContext(const std::string & name, std::shared_ptr impl) + : name(name), impl_(impl) + {} - // 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); + InstanceContext( + const std::string & name, + rclcpp::AnySubscriptionCallback asc) + : name(name), impl_(std::make_shared(asc)) + {} - // 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); -} + InstanceContext(const InstanceContext & other) + : InstanceContext(other.name, other.impl_) {} -TEST_F(TestAnySubscriptionCallback, set_dispatch_const_shared_ptr) { - int callback_count = 0; - auto const_shared_ptr_callback = [&callback_count]( - std::shared_ptr) { - callback_count++; - }; + rclcpp::AnySubscriptionCallback + get_any_subscription_callback_to_test() const + { + return impl_->get_any_subscription_callback_to_test(); + } - any_subscription_callback_.set(const_shared_ptr_callback); + std::string name; - // Ok to promote shared_ptr to ConstSharedPtr - EXPECT_NO_THROW(any_subscription_callback_.dispatch(msg_shared_ptr_, message_info_)); - EXPECT_EQ(callback_count, 1); +protected: + std::shared_ptr impl_; +}; - EXPECT_NO_THROW( - any_subscription_callback_.dispatch_intra_process(msg_const_shared_ptr_, message_info_)); - EXPECT_EQ(callback_count, 2); +class DispatchTests + : public TestAnySubscriptionCallback, + public ::testing::WithParamInterface +{}; - // 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(const ::testing::TestParamInfo & info) +{ + return info.param.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++; - }; - - any_subscription_callback_.set( - std::move(const_shared_ptr_callback)); - - // Ok to promote shared_ptr to ConstSharedPtr - EXPECT_NO_THROW(any_subscription_callback_.dispatch(msg_shared_ptr_, message_info_)); - EXPECT_EQ(callback_count, 1); +// Testing dispatch with shared_ptr as input +TEST_P(DispatchTests, test_inter_shared_dispatch) { + auto any_subscription_callback_to_test = GetParam().get_any_subscription_callback_to_test(); + any_subscription_callback_to_test.dispatch(msg_shared_ptr_, message_info_); +} - EXPECT_NO_THROW( - any_subscription_callback_.dispatch_intra_process(msg_const_shared_ptr_, message_info_)); - EXPECT_EQ(callback_count, 2); +// Testing dispatch with shared_ptr as input +TEST_P(DispatchTests, test_intra_shared_dispatch) { + auto any_subscription_callback_to_test = GetParam().get_any_subscription_callback_to_test(); + any_subscription_callback_to_test.dispatch_intra_process(msg_shared_ptr_, message_info_); +} - // 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); +// Testing dispatch with unique_ptr as input +TEST_P(DispatchTests, test_intra_unique_dispatch) { + auto any_subscription_callback_to_test = GetParam().get_any_subscription_callback_to_test(); + any_subscription_callback_to_test.dispatch_intra_process(get_unique_ptr_msg(), message_info_); } -TEST_F(TestAnySubscriptionCallback, set_dispatch_unique_ptr) { - int callback_count = 0; - auto unique_ptr_callback = [&callback_count]( - std::unique_ptr) { - callback_count++; - }; +// 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)}; - any_subscription_callback_.set(unique_ptr_callback); +public: + using InstanceContextImpl::InstanceContextImpl; + virtual ~BindContextImpl() = default; - // Message is copied into unique_ptr - EXPECT_NO_THROW(any_subscription_callback_.dispatch(msg_shared_ptr_, message_info_)); - EXPECT_EQ(callback_count, 1); + void on_message(CallbackArgs ...) const {} - EXPECT_THROW( - any_subscription_callback_.dispatch_intra_process(msg_const_shared_ptr_, message_info_), - std::runtime_error); - EXPECT_EQ(callback_count, 1); + 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) + ); + } + } +}; - // 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); -} +template +class BindContext : public InstanceContext +{ +public: + explicit BindContext(const std::string & name) + : InstanceContext(name, std::make_shared>()) + {} +}; -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++; - }; +// +// 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 +); - any_subscription_callback_.set(unique_ptr_callback); +// +// 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, const rclcpp::MessageInfo &>( + "bind_method_with_info") + ), + format_parameter +); - // 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 `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 +); - EXPECT_THROW( - any_subscription_callback_.dispatch_intra_process(msg_const_shared_ptr_, message_info_), - std::runtime_error); - EXPECT_EQ(callback_count, 1); +// +// 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 +); - // 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_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 +); 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_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..e1fb4b8ced 100644 --- a/rclcpp/test/rclcpp/test_node.cpp +++ b/rclcpp/test/rclcpp/test_node.cpp @@ -2591,7 +2591,7 @@ 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 = diff --git a/rclcpp/test/rclcpp/test_parameter_client.cpp b/rclcpp/test/rclcpp/test_parameter_client.cpp index c7f8ad6581..04240b7af9 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; } diff --git a/rclcpp/test/rclcpp/test_parameter_event_handler.cpp b/rclcpp/test/rclcpp/test_parameter_event_handler.cpp index 868419249c..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() @@ -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_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_qos_event.cpp b/rclcpp/test/rclcpp/test_qos_event.cpp index 6c94aaff97..960ffdcb4c 100644 --- a/rclcpp/test/rclcpp/test_qos_event.cpp +++ b/rclcpp/test/rclcpp/test_qos_event.cpp @@ -42,7 +42,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 +55,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[]; 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_subscription.cpp b/rclcpp/test/rclcpp/test_subscription.cpp index 9b190fea18..0e798cabef 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; 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_wait_set.cpp b/rclcpp/test/rclcpp/test_wait_set.cpp index bbfc9383c8..f9d97a5759 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( @@ -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); 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); { From 7226725d2fccd25620cb204ff57fd6db2a225545 Mon Sep 17 00:00:00 2001 From: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Date: Sun, 4 Apr 2021 08:22:01 +0200 Subject: [PATCH 38/93] Create load_parameters and delete_parameters methods (#1596) * Create delete_parameters method Signed-off-by: Brice * cleaner way to delete Signed-off-by: Brice * add delete_parameters TEST Signed-off-by: Brice * fix uncrustify Signed-off-by: Brice * create load_parameter method Signed-off-by: Brice * create load_parameters TEST Signed-off-by: Brice * add comments Signed-off-by: Brice * fix exceptions Signed-off-by: Brice * fix const auto Signed-off-by: Brice * fix param node check Signed-off-by: Brice * Change TEST to use remote_node_name_ Signed-off-by: Brice * change comment style Signed-off-by: Brice * parameter_map_from_yaml_file Signed-off-by: Brice * Fix comments Signed-off-by: Brice * fix sign-compare warning Signed-off-by: Brice * use c style Signed-off-by: Brice --- rclcpp/include/rclcpp/parameter_client.hpp | 91 +++++++++++++++++++ rclcpp/include/rclcpp/parameter_map.hpp | 9 ++ rclcpp/src/rclcpp/parameter_client.cpp | 87 +++++++++++++++++- rclcpp/src/rclcpp/parameter_map.cpp | 12 +++ rclcpp/test/rclcpp/test_parameter_client.cpp | 87 ++++++++++++++++++ .../resources/test_node/load_parameters.yaml | 18 ++++ 6 files changed, 303 insertions(+), 1 deletion(-) create mode 100644 rclcpp/test/resources/test_node/load_parameters.yaml 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_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/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_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/test/rclcpp/test_parameter_client.cpp b/rclcpp/test/rclcpp/test_parameter_client.cpp index 04240b7af9..4cd9e671be 100644 --- a/rclcpp/test/rclcpp/test_parameter_client.cpp +++ b/rclcpp/test/rclcpp/test_parameter_client.cpp @@ -861,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/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 From 7b94f288e5fb933627804b2907c197438f2401f0 Mon Sep 17 00:00:00 2001 From: Christophe Bedard Date: Sun, 4 Apr 2021 18:19:34 -0400 Subject: [PATCH 39/93] Add publishing instrumentation (#1600) Signed-off-by: Christophe Bedard --- rclcpp/include/rclcpp/publisher.hpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 14b2f333c1..f3fbc34faa 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -40,6 +40,8 @@ #include "rclcpp/type_support_decl.hpp" #include "rclcpp/visibility_control.hpp" +#include "tracetools/tracetools.h" + namespace rclcpp { @@ -279,6 +281,10 @@ class Publisher : public PublisherBase void do_inter_process_publish(const MessageT & 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) { From 06a4ee01d41e6870000ce2011acd157d3e85c5ce Mon Sep 17 00:00:00 2001 From: mauropasse Date: Mon, 5 Apr 2021 02:53:55 +0100 Subject: [PATCH 40/93] Add spin_some support to the StaticSingleThreadedExecutor (#1338) * spin_some/spin_all/spin_once support: static executor Signed-off-by: Mauro * Use spin_once instead of spin_once_impl Signed-off-by: Mauro Passerino * Fix memory leak Signed-off-by: Mauro Passerino * revert spinSome change Signed-off-by: Mauro Passerino * Override spin_once_impl only This way the StaticSingleThreadedExecutor uses spin_once and spin_until_future_complete APIs from the base executor class, but uses its own overrided version of spin_once_impl. Signed-off-by: Mauro Passerino Co-authored-by: Mauro --- rclcpp/include/rclcpp/executor.hpp | 2 +- .../static_executor_entities_collector.hpp | 7 + .../static_single_threaded_executor.hpp | 143 ++++++------------ .../static_executor_entities_collector.cpp | 4 + .../static_single_threaded_executor.cpp | 109 ++++++++++++- .../test/rclcpp/executors/test_executors.cpp | 108 ++++++------- .../test_static_single_threaded_executor.cpp | 10 -- 7 files changed, 218 insertions(+), 165 deletions(-) diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 946aa18851..9321f679fc 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -544,7 +544,7 @@ class Executor RCLCPP_DISABLE_COPY(Executor) RCLCPP_PUBLIC - void + virtual void spin_once_impl(std::chrono::nanoseconds timeout); typedef std::map #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/src/rclcpp/executors/static_executor_entities_collector.cpp b/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp index a0f55d3d5b..fada15e5aa 100644 --- a/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp +++ b/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp @@ -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 diff --git a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp index 4b5afc308f..5402d32460 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, @@ -138,14 +216,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 +238,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 +250,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 +262,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 +275,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/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index 970cd3bdb2..1cf90d9901 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -349,49 +349,6 @@ 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: @@ -422,10 +379,12 @@ class TestWaitable : public rclcpp::Waitable 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); - if (RCL_RET_OK != ret) { - return false; - } - ret = rcl_trigger_guard_condition(&gc_); + return RCL_RET_OK == ret; + } + + bool trigger() + { + rcl_ret_t ret = rcl_trigger_guard_condition(&gc_); return RCL_RET_OK == ret; } @@ -447,7 +406,7 @@ class TestWaitable : public rclcpp::Waitable { (void) data; count_++; - std::this_thread::sleep_for(1ms); + std::this_thread::sleep_for(3ms); } size_t @@ -464,7 +423,7 @@ class TestWaitable : public rclcpp::Waitable rcl_guard_condition_t gc_; }; -TYPED_TEST(TestExecutorsStable, spinAll) { +TYPED_TEST(TestExecutors, spinAll) { using ExecutorType = TypeParam; ExecutorType executor; auto waitable_interfaces = this->node->get_node_waitables_interface(); @@ -488,6 +447,7 @@ TYPED_TEST(TestExecutorsStable, spinAll) { !spin_exited && (std::chrono::steady_clock::now() - start < 1s)) { + my_waitable->trigger(); this->publisher->publish(test_msgs::msg::Empty()); std::this_thread::sleep_for(1ms); } @@ -504,7 +464,7 @@ TYPED_TEST(TestExecutorsStable, spinAll) { spinner.join(); } -TYPED_TEST(TestExecutorsStable, spinSome) { +TYPED_TEST(TestExecutors, spinSome) { using ExecutorType = TypeParam; ExecutorType executor; auto waitable_interfaces = this->node->get_node_waitables_interface(); @@ -529,6 +489,7 @@ TYPED_TEST(TestExecutorsStable, spinSome) { !spin_exited && (std::chrono::steady_clock::now() - start < 1s)) { + my_waitable->trigger(); this->publisher->publish(test_msgs::msg::Empty()); std::this_thread::sleep_for(1ms); } @@ -543,7 +504,7 @@ TYPED_TEST(TestExecutorsStable, spinSome) { } // Check spin_node_until_future_complete with node base pointer -TYPED_TEST(TestExecutorsStable, testSpinNodeUntilFutureCompleteNodeBasePtr) { +TYPED_TEST(TestExecutors, testSpinNodeUntilFutureCompleteNodeBasePtr) { using ExecutorType = TypeParam; ExecutorType executor; @@ -558,7 +519,7 @@ TYPED_TEST(TestExecutorsStable, testSpinNodeUntilFutureCompleteNodeBasePtr) { } // Check spin_node_until_future_complete with node pointer -TYPED_TEST(TestExecutorsStable, testSpinNodeUntilFutureCompleteNodePtr) { +TYPED_TEST(TestExecutors, testSpinNodeUntilFutureCompleteNodePtr) { using ExecutorType = TypeParam; ExecutorType executor; @@ -572,6 +533,49 @@ TYPED_TEST(TestExecutorsStable, testSpinNodeUntilFutureCompleteNodePtr) { EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); } +// Check spin_until_future_complete can be properly interrupted. +TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteInterrupted) { + using ExecutorType = TypeParam; + ExecutorType executor; + executor.add_node(this->node); + + bool spin_exited = false; + + // This needs to block longer than it takes to get to the shutdown call below and for + // spin_until_future_complete to return + std::future future = std::async( + std::launch::async, + [&spin_exited]() { + auto start = std::chrono::steady_clock::now(); + while (!spin_exited && (std::chrono::steady_clock::now() - start) < 1s) { + std::this_thread::sleep_for(1ms); + } + }); + + // Long timeout + std::thread spinner([&spin_exited, &executor, &future]() { + auto ret = executor.spin_until_future_complete(future, 1s); + EXPECT_EQ(rclcpp::FutureReturnCode::INTERRUPTED, ret); + spin_exited = true; + }); + + // Do some minimal work + this->publisher->publish(test_msgs::msg::Empty()); + std::this_thread::sleep_for(1ms); + + // Force interruption + rclcpp::shutdown(); + + // Give it time to exit + auto start = std::chrono::steady_clock::now(); + while (!spin_exited && (std::chrono::steady_clock::now() - start) < 1s) { + std::this_thread::sleep_for(1ms); + } + + EXPECT_TRUE(spin_exited); + spinner.join(); +} + // Check spin_until_future_complete with node base pointer (instantiates its own executor) TEST(TestExecutors, testSpinUntilFutureCompleteNodeBasePtr) { rclcpp::init(0, nullptr); 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"); From 6af478d0bade3bd9c309f3e0e6424ac0ef2369e4 Mon Sep 17 00:00:00 2001 From: Ananya Muddukrishna Date: Mon, 5 Apr 2021 19:16:46 +0200 Subject: [PATCH 41/93] Unique network flows (#1496) * Add option to enable unique network flow - Option enabled for publishers and subscriptions - TODO: Discuss error handling if not supported Signed-off-by: Ananya Muddukrishna * Get network flows of publishers and subscriptions Signed-off-by: Ananya Muddukrishna * Use new unique network flow option Signed-off-by: Ananya Muddukrishna * Rename files for clarity Signed-off-by: Ananya Muddukrishna * Rename API for clarity and add DSCP option Signed-off-by: Ananya Muddukrishna * Convert integer to string prior to streaming Signed-off-by: Ananya Muddukrishna * Uncrustify Signed-off-by: Ananya Muddukrishna * Fix linkage error thrown by MSVC compiler Signed-off-by: Ananya Muddukrishna * Use updated rmw interface Signed-off-by: Ananya Muddukrishna * Improve readability Signed-off-by: Ananya Muddukrishna * Forward declare friend functions Signed-off-by: Ananya Muddukrishna Co-authored-by: Ananya Muddukrishna --- rclcpp/CMakeLists.txt | 1 + .../include/rclcpp/network_flow_endpoint.hpp | 115 ++++++++++++++++++ rclcpp/include/rclcpp/publisher_base.hpp | 10 ++ rclcpp/include/rclcpp/publisher_options.hpp | 7 ++ rclcpp/include/rclcpp/subscription_base.hpp | 10 ++ .../include/rclcpp/subscription_options.hpp | 7 ++ rclcpp/src/rclcpp/network_flow_endpoint.cpp | 84 +++++++++++++ rclcpp/src/rclcpp/publisher_base.cpp | 37 ++++++ rclcpp/src/rclcpp/subscription_base.cpp | 37 ++++++ rclcpp/test/rclcpp/test_publisher.cpp | 30 +++++ rclcpp/test/rclcpp/test_subscription.cpp | 34 ++++++ 11 files changed, 372 insertions(+) create mode 100644 rclcpp/include/rclcpp/network_flow_endpoint.hpp create mode 100644 rclcpp/src/rclcpp/network_flow_endpoint.cpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index ea1e516fc0..891b4549aa 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -70,6 +70,7 @@ 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_interfaces/node_base.cpp src/rclcpp/node_interfaces/node_clock.cpp 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/publisher_base.hpp b/rclcpp/include/rclcpp/publisher_base.hpp index 48ebfea860..c67a0439e0 100644 --- a/rclcpp/include/rclcpp/publisher_base.hpp +++ b/rclcpp/include/rclcpp/publisher_base.hpp @@ -28,6 +28,7 @@ #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" @@ -193,6 +194,15 @@ class PublisherBase : public std::enable_shared_from_this uint64_t intra_process_publisher_id, IntraProcessManagerSharedPtr ipm); + /// Get network flow endpoints + /** + * Describes network flow endpoints that this publisher is sending messages out on + * \return vector of NetworkFlowEndpoint + */ + RCLCPP_PUBLIC + std::vector + get_network_flow_endpoints() const; + protected: template void diff --git a/rclcpp/include/rclcpp/publisher_options.hpp b/rclcpp/include/rclcpp/publisher_options.hpp index 2e12c12bcb..8388f85e47 100644 --- a/rclcpp/include/rclcpp/publisher_options.hpp +++ b/rclcpp/include/rclcpp/publisher_options.hpp @@ -45,6 +45,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; @@ -80,6 +85,8 @@ struct PublisherOptionsWithAllocator : public PublisherOptionsBase auto message_alloc = std::make_shared(*this->get_allocator().get()); result.allocator = rclcpp::allocator::get_rcl_allocator(*message_alloc); 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()) { diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index c9a3ec7b7c..b0642bb69d 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -31,6 +31,7 @@ #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" @@ -266,6 +267,15 @@ 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); + /// 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 void diff --git a/rclcpp/include/rclcpp/subscription_options.hpp b/rclcpp/include/rclcpp/subscription_options.hpp index ade1bb8e9f..d9d4085d85 100644 --- a/rclcpp/include/rclcpp/subscription_options.hpp +++ b/rclcpp/include/rclcpp/subscription_options.hpp @@ -45,6 +45,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; @@ -108,6 +113,8 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase result.allocator = allocator::get_rcl_allocator(*message_alloc); 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()) { 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/publisher_base.cpp b/rclcpp/src/rclcpp/publisher_base.cpp index aadee307b3..17d72594d5 100644 --- a/rclcpp/src/rclcpp/publisher_base.cpp +++ b/rclcpp/src/rclcpp/publisher_base.cpp @@ -34,6 +34,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" @@ -268,3 +269,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/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index 824d0f2f85..ba76e5f9bc 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -288,3 +288,40 @@ SubscriptionBase::exchange_in_use_by_wait_set_state( } throw std::runtime_error("given pointer_to_subscription_part does not match any part"); } + +std::vector SubscriptionBase::get_network_flow_endpoints() const +{ + 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/test/rclcpp/test_publisher.cpp b/rclcpp/test/rclcpp/test_publisher.cpp index 7d2c9d14c1..bec8bbb17a 100644 --- a/rclcpp/test/rclcpp/test_publisher.cpp +++ b/rclcpp/test/rclcpp/test_publisher.cpp @@ -506,3 +506,33 @@ TEST_F(TestPublisher, run_event_handlers) { 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_subscription.cpp b/rclcpp/test/rclcpp/test_subscription.cpp index 0e798cabef..9cf9acdcf8 100644 --- a/rclcpp/test/rclcpp/test_subscription.cpp +++ b/rclcpp/test/rclcpp/test_subscription.cpp @@ -506,3 +506,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 = [](const 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()); + } +} From 091a8bcf86ca2697df8b94f485d62fc5687cc19c Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 5 Apr 2021 14:07:58 -0400 Subject: [PATCH 42/93] Change index.ros.org -> docs.ros.org. (#1620) Signed-off-by: Chris Lalancette --- README.md | 4 ++-- rclcpp/include/rclcpp/qos.hpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) 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/include/rclcpp/qos.hpp b/rclcpp/include/rclcpp/qos.hpp index 4160360756..99d5acbd33 100644 --- a/rclcpp/include/rclcpp/qos.hpp +++ b/rclcpp/include/rclcpp/qos.hpp @@ -103,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 From 61fcc766f84848c5effabc999b639d684b63814a Mon Sep 17 00:00:00 2001 From: William Woodall Date: Mon, 5 Apr 2021 22:32:20 -0700 Subject: [PATCH 43/93] remove deprecated code which was deprecated in foxy and should be removed in galactic (#1622) Signed-off-by: William Woodall --- rclcpp/include/rclcpp/any_executable.hpp | 6 -- rclcpp/include/rclcpp/callback_group.hpp | 7 -- .../rclcpp/contexts/default_context.hpp | 16 ----- rclcpp/include/rclcpp/executor.hpp | 6 -- rclcpp/include/rclcpp/executor_options.hpp | 14 ---- rclcpp/include/rclcpp/service.hpp | 9 --- rclcpp/include/rclcpp/utilities.hpp | 15 ----- rclcpp/test/rclcpp/CMakeLists.txt | 8 --- rclcpp/test/rclcpp/test_init.cpp | 66 ------------------- 9 files changed, 147 deletions(-) delete mode 100644 rclcpp/test/rclcpp/test_init.cpp 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/callback_group.hpp b/rclcpp/include/rclcpp/callback_group.hpp index fe52d287bb..efa8352206 100644 --- a/rclcpp/include/rclcpp/callback_group.hpp +++ b/rclcpp/include/rclcpp/callback_group.hpp @@ -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/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/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 9321f679fc..bc4e167a1e 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -573,12 +573,6 @@ class Executor weak_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_); }; -namespace executor -{ - -using Executor [[deprecated("use rclcpp::Executor instead")]] = rclcpp::Executor; - -} // 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/service.hpp b/rclcpp/include/rclcpp/service.hpp index d36a18d4dc..079be09b28 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -349,15 +349,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/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/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index f6244b5815..010a911889 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -558,14 +558,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/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 From f9f90e0226f59f00b1b4bf8e8cbda7f163d7a399 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 6 Apr 2021 13:58:50 +0000 Subject: [PATCH 44/93] Changelog. Signed-off-by: Chris Lalancette --- rclcpp/CHANGELOG.rst | 14 ++++++++++++++ rclcpp_action/CHANGELOG.rst | 5 +++++ rclcpp_components/CHANGELOG.rst | 5 +++++ rclcpp_lifecycle/CHANGELOG.rst | 6 ++++++ 4 files changed, 30 insertions(+) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index dd2465792c..ee816f335d 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,20 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 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 `_) diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 9d408a3273..c5c5de985d 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,11 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* updating quality declaration links (re: `ros2/docs.ros2.org#52 `_) (`#1615 `_) +* Contributors: shonigmann + 8.2.0 (2021-03-31) ------------------ diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index d1755ec8bf..e74e0c5f32 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* updating quality declaration links (re: `ros2/docs.ros2.org#52 `_) (`#1615 `_) +* Contributors: shonigmann + 8.2.0 (2021-03-31) ------------------ diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 24df585075..e060770145 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,12 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 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 `_) From 98fc7fecc25b2fecc089d1117cee6803eda34836 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 6 Apr 2021 13:59:00 +0000 Subject: [PATCH 45/93] 9.0.0 --- rclcpp/CHANGELOG.rst | 4 ++-- rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 4 ++-- rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 4 ++-- rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 4 ++-- rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index ee816f335d..9dcc518d2f 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +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 `_) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index c7709693d7..4cdd2625fd 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 8.2.0 + 9.0.0 The ROS client library in C++. Ivan Paunovic Mabel Zhang diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index c5c5de985d..c0669da05d 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +9.0.0 (2021-04-06) +------------------ * updating quality declaration links (re: `ros2/docs.ros2.org#52 `_) (`#1615 `_) * Contributors: shonigmann diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index 9301e79452..08246eaf35 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 8.2.0 + 9.0.0 Adds action APIs for C++. Ivan Paunovic Mabel Zhang diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index e74e0c5f32..a14a28e34b 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +9.0.0 (2021-04-06) +------------------ * updating quality declaration links (re: `ros2/docs.ros2.org#52 `_) (`#1615 `_) * Contributors: shonigmann diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index 6bb3e35bd0..a1ba2ec449 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 8.2.0 + 9.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 e060770145..6507f0ea0f 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +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 diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 5eefe81ce5..54f784ab22 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 8.2.0 + 9.0.0 Package containing a prototype for lifecycle implementation Ivan Paunovic Mabel Zhang From 41fedb7beb123dee01b9e54e3579270203cd9404 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Wed, 7 Apr 2021 12:33:21 -0300 Subject: [PATCH 46/93] Fix race condition in TimeSource clock thread setup (#1623) Signed-off-by: Michel Hidalgo --- rclcpp/src/rclcpp/time_source.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/src/rclcpp/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp index 595520da4e..140b5531d2 100644 --- a/rclcpp/src/rclcpp/time_source.cpp +++ b/rclcpp/src/rclcpp/time_source.cpp @@ -261,9 +261,9 @@ void TimeSource::create_clock_sub() clock_executor_ = std::make_shared(exec_options); if (!clock_executor_thread_.joinable()) { + cancel_clock_executor_promise_ = std::promise{}; clock_executor_thread_ = std::thread( [this]() { - cancel_clock_executor_promise_ = std::promise{}; 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); From 53cc25d33a96dabbb271214a15c467a875d5a695 Mon Sep 17 00:00:00 2001 From: Mauro Passerino Date: Thu, 1 Apr 2021 08:35:50 -0300 Subject: [PATCH 47/93] Use rclcpp::guard_condition Signed-off-by: Mauro Passerino --- rclcpp/include/rclcpp/executor.hpp | 4 +- .../static_executor_entities_collector.hpp | 4 +- .../subscription_intra_process.hpp | 27 ++-------- .../subscription_intra_process_base.hpp | 9 ++-- rclcpp/include/rclcpp/graph_listener.hpp | 3 +- rclcpp/include/rclcpp/guard_condition.hpp | 13 ++++- rclcpp/include/rclcpp/memory_strategy.hpp | 6 ++- .../rclcpp/node_interfaces/node_base.hpp | 12 ++++- .../node_interfaces/node_base_interface.hpp | 20 ++++++- .../strategies/allocator_memory_strategy.hpp | 14 ++--- rclcpp/src/rclcpp/executor.cpp | 54 +++++-------------- .../static_executor_entities_collector.cpp | 14 +++-- .../static_single_threaded_executor.cpp | 16 ++---- rclcpp/src/rclcpp/graph_listener.cpp | 32 +++-------- rclcpp/src/rclcpp/guard_condition.cpp | 18 +++++-- .../src/rclcpp/node_interfaces/node_base.cpp | 48 ++++++++--------- .../src/rclcpp/node_interfaces/node_graph.cpp | 8 +-- .../rclcpp/node_interfaces/node_services.cpp | 20 +------ .../rclcpp/node_interfaces/node_timers.cpp | 8 ++- .../rclcpp/node_interfaces/node_topics.cpp | 18 +------ .../rclcpp/node_interfaces/node_waitables.cpp | 10 +--- .../subscription_intra_process_base.cpp | 5 +- rclcpp/test/benchmark/benchmark_executor.cpp | 3 +- .../test/rclcpp/executors/test_executors.cpp | 35 +++--------- ...est_static_executor_entities_collector.cpp | 42 ++++++--------- .../node_interfaces/test_node_services.cpp | 4 +- .../node_interfaces/test_node_timers.cpp | 2 +- .../node_interfaces/test_node_topics.cpp | 4 +- .../node_interfaces/test_node_waitables.cpp | 2 +- .../test_allocator_memory_strategy.cpp | 25 ++++----- rclcpp/test/rclcpp/test_executor.cpp | 8 +-- rclcpp/test/rclcpp/test_graph_listener.cpp | 8 ++- 32 files changed, 184 insertions(+), 312 deletions(-) diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 9321f679fc..ae2deeafe7 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -524,7 +524,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 +548,7 @@ class Executor spin_once_impl(std::chrono::nanoseconds timeout); typedef std::map> WeakNodesToGuardConditionsMap; diff --git a/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp b/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp index 51f80b32c9..8fc9cd0f17 100644 --- a/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp +++ b/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp @@ -65,7 +65,7 @@ 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 @@ -326,7 +326,7 @@ class StaticExecutorEntitiesCollector final WeakCallbackGroupsToNodesMap weak_groups_to_nodes_associated_with_executor_; typedef std::map> WeakNodesToGuardConditionsMap; WeakNodesToGuardConditionsMap weak_nodes_to_guard_conditions_; diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp index 46e019daee..f9939927c8 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp @@ -67,7 +67,7 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBase const std::string & topic_name, rmw_qos_profile_t qos_profile, rclcpp::IntraProcessBufferType buffer_type) - : SubscriptionIntraProcessBase(topic_name, qos_profile), + : SubscriptionIntraProcessBase(context, topic_name, qos_profile), any_callback_(callback) { if (!std::is_same::value) { @@ -80,18 +80,6 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBase 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,15 +92,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); - } - } + ~SubscriptionIntraProcess() = default; bool is_ready(rcl_wait_set_t * wait_set) @@ -168,8 +148,7 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBase void trigger_guard_condition() { - rcl_ret_t ret = rcl_trigger_guard_condition(&gc_); - (void)ret; + gc_.trigger(); } template diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp index f40f997762..e215411bdf 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp @@ -25,6 +25,7 @@ #include "rcl/error_handling.h" +#include "rclcpp/guard_condition.hpp" #include "rclcpp/type_support_decl.hpp" #include "rclcpp/waitable.hpp" @@ -39,8 +40,10 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable RCLCPP_SMART_PTR_ALIASES_ONLY(SubscriptionIntraProcessBase) 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; @@ -82,7 +85,7 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable protected: std::recursive_mutex reentrant_mutex_; - rcl_guard_condition_t gc_; + rclcpp::GuardCondition gc_; private: virtual void 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..71c7fa16a6 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,6 +91,15 @@ 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; + protected: rclcpp::Context::SharedPtr context_; rcl_guard_condition_t rcl_guard_condition_; 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/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/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/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index d33df58ee2..bd289ba878 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -43,19 +43,13 @@ 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( [weak_gc = std::weak_ptr{shutdown_guard_condition_}]() { auto strong_gc = weak_gc.lock(); @@ -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 @@ -227,16 +209,13 @@ Executor::add_callback_group_to_map( 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(); + weak_nodes_to_guard_conditions_[node_weak_ptr] = node_ptr->get_notify_rclcpp_guard_condition(); if (notify) { // Interrupt waiting to handle new node - rcl_ret_t ret = rcl_trigger_guard_condition(&interrupt_guard_condition_); - if (ret != RCL_RET_OK) { - throw_from_rcl_error(ret, "Failed to trigger guard condition on callback group add"); - } + 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_ptr->get_notify_rclcpp_guard_condition()); } } @@ -306,12 +285,9 @@ Executor::remove_callback_group_from_map( rclcpp::node_interfaces::NodeBaseInterface::WeakPtr node_weak_ptr(node_ptr); weak_nodes_to_guard_conditions_.erase(node_weak_ptr); if (notify) { - rcl_ret_t ret = rcl_trigger_guard_condition(&interrupt_guard_condition_); - if (ret != RCL_RET_OK) { - throw_from_rcl_error(ret, "Failed to trigger guard condition on callback group remove"); - } + 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 +458,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 +496,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 diff --git a/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp b/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp index a0f55d3d5b..ab2f8b6f4d 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(); @@ -263,10 +263,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 +320,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 +426,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..9f7b095308 100644 --- a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp @@ -60,9 +60,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 +71,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 +88,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 +102,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(); } } 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..a5f039fe41 100644 --- a/rclcpp/src/rclcpp/guard_condition.cpp +++ b/rclcpp/src/rclcpp/guard_condition.cpp @@ -20,19 +20,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 +46,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()); } } } @@ -77,4 +78,13 @@ 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"); + } +} } // namespace rclcpp 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_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_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..6b6f6cd917 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp @@ -60,14 +60,7 @@ NodeTopics::add_publisher( } // Notify the executor that a new publisher was created using the parent Node. - { - auto notify_guard_condition_lock = node_base_->acquire_notify_guard_condition_lock(); - if (rcl_trigger_guard_condition(node_base_->get_notify_guard_condition()) != RCL_RET_OK) { - throw std::runtime_error( - std::string("Failed to notify wait set on publisher creation: ") + - rmw_get_error_string().str); - } - } + node_base_->trigger_notify_guard_condition(); } rclcpp::SubscriptionBase::SharedPtr @@ -108,14 +101,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/subscription_intra_process_base.cpp b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp index 8a2aa7d521..e4e1c67b88 100644 --- a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp +++ b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp @@ -21,8 +21,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 * diff --git a/rclcpp/test/benchmark/benchmark_executor.cpp b/rclcpp/test/benchmark/benchmark_executor.cpp index 27e6d2ff5b..f3b68b5291 100644 --- a/rclcpp/test/benchmark/benchmark_executor.cpp +++ b/rclcpp/test/benchmark/benchmark_executor.cpp @@ -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/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index 7cecdff81a..0a13c5a013 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" @@ -401,40 +402,18 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteInterrupted) { 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 @@ -482,7 +461,7 @@ class TestWaitable : public rclcpp::Waitable private: size_t count_ = 0; - rcl_guard_condition_t gc_; + rclcpp::GuardCondition gc_; }; TYPED_TEST(TestExecutorsStable, spinAll) { 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..a8e755affb 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()); @@ -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()); { @@ -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/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..fcba94494e 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp @@ -127,7 +127,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 +155,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..ad0920b1fc 100644 --- a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp +++ b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp @@ -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)); @@ -586,24 +586,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) { 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_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(); } From 560e0cd5470560d635fd5be64bf842ffe586558c Mon Sep 17 00:00:00 2001 From: Mauro Passerino Date: Fri, 9 Apr 2021 11:59:24 -0300 Subject: [PATCH 48/93] EventsExecutor: Use rclcpp::GuardCondition Signed-off-by: Mauro Passerino --- .../rclcpp/executors/events_executor.hpp | 2 -- .../events_executor_entities_collector.hpp | 6 +++--- .../events_executor_notify_waitable.hpp | 13 +++--------- .../src/rclcpp/executors/events_executor.cpp | 2 +- .../events_executor_entities_collector.cpp | 10 +++++----- .../rclcpp/executors/test_events_executor.cpp | 6 ------ ...est_events_executor_entities_collector.cpp | 20 ------------------- 7 files changed, 12 insertions(+), 47 deletions(-) diff --git a/rclcpp/include/rclcpp/executors/events_executor.hpp b/rclcpp/include/rclcpp/executors/events_executor.hpp index 44de455301..8da8be55cb 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 diff --git a/rclcpp/include/rclcpp/executors/events_executor_entities_collector.hpp b/rclcpp/include/rclcpp/executors/events_executor_entities_collector.hpp index 2781026492..a4042fee8a 100644 --- a/rclcpp/include/rclcpp/executors/events_executor_entities_collector.hpp +++ b/rclcpp/include/rclcpp/executors/events_executor_entities_collector.hpp @@ -209,10 +209,10 @@ 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(const rclcpp::GuardCondition * guard_condition); void - unset_guard_condition_callback(const rcl_guard_condition_t * guard_condition); + unset_guard_condition_callback(const rclcpp::GuardCondition * guard_condition); void remove_callback_data(void * entity_id, ExecutorEventType type); @@ -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_; diff --git a/rclcpp/include/rclcpp/executors/events_executor_notify_waitable.hpp b/rclcpp/include/rclcpp/executors/events_executor_notify_waitable.hpp index 2a19e2d9af..fe42d5a768 100644 --- a/rclcpp/include/rclcpp/executors/events_executor_notify_waitable.hpp +++ b/rclcpp/include/rclcpp/executors/events_executor_notify_waitable.hpp @@ -53,7 +53,7 @@ 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); } @@ -65,14 +65,7 @@ class EventsExecutorNotifyWaitable final : public EventWaitable const void * user_data) const 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"); - } + // gc->set_listener_callback(); } } @@ -85,7 +78,7 @@ class EventsExecutorNotifyWaitable final : public EventWaitable } private: - std::list notify_guard_conditions_; + std::list notify_guard_conditions_; }; } // namespace executors diff --git a/rclcpp/src/rclcpp/executors/events_executor.cpp b/rclcpp/src/rclcpp/executors/events_executor.cpp index 0ccda41211..0d0fceed23 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_); diff --git a/rclcpp/src/rclcpp/executors/events_executor_entities_collector.cpp b/rclcpp/src/rclcpp/executors/events_executor_entities_collector.cpp index 1122be96df..4ed996caef 100644 --- a/rclcpp/src/rclcpp/executors/events_executor_entities_collector.cpp +++ b/rclcpp/src/rclcpp/executors/events_executor_entities_collector.cpp @@ -141,10 +141,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 @@ -375,7 +375,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,7 +485,7 @@ EventsExecutorEntitiesCollector::get_automatically_added_callback_groups_from_no void EventsExecutorEntitiesCollector::set_guard_condition_callback( - const rcl_guard_condition_t * guard_condition) + const rclcpp::GuardCondition * guard_condition) { rcl_ret_t ret = rcl_guard_condition_set_listener_callback( guard_condition, @@ -499,7 +499,7 @@ EventsExecutorEntitiesCollector::set_guard_condition_callback( void EventsExecutorEntitiesCollector::unset_guard_condition_callback( - const rcl_guard_condition_t * guard_condition) + const rclcpp::GuardCondition * guard_condition) { rcl_ret_t ret = rcl_guard_condition_set_listener_callback( guard_condition, nullptr, nullptr); 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); - } -} From 56405fc4aea2f24ce8784deddbadbfa1b5a98600 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Thu, 1 Apr 2021 20:58:12 -0700 Subject: [PATCH 49/93] significant refactor to make callbacks type safe Signed-off-by: William Woodall --- rclcpp/include/rclcpp/client.hpp | 83 ++++++++++++++++-- .../rclcpp/detail/cpp_callback_trampoline.hpp | 67 ++++++++++++++ .../subscription_intra_process_base.hpp | 24 ++--- rclcpp/include/rclcpp/qos_event.hpp | 77 ++++++++++++++-- rclcpp/include/rclcpp/service.hpp | 87 +++++++++++++++++-- rclcpp/include/rclcpp/subscription_base.hpp | 74 +++++++++++++++- rclcpp/include/rclcpp/waitable.hpp | 7 -- rclcpp/src/rclcpp/client.cpp | 12 +-- rclcpp/src/rclcpp/context.cpp | 2 - rclcpp/src/rclcpp/qos_event.cpp | 11 +-- rclcpp/src/rclcpp/service.cpp | 12 +-- rclcpp/src/rclcpp/subscription_base.cpp | 12 +-- .../subscription_intra_process_base.cpp | 10 +-- 13 files changed, 396 insertions(+), 82 deletions(-) create mode 100644 rclcpp/include/rclcpp/detail/cpp_callback_trampoline.hpp diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index 3da956e583..2ca256ab6e 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -28,18 +28,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 +151,68 @@ 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. + * + * 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_listener_callback( - rmw_listener_callback_t callback, - const void * user_data) const; + set_on_new_response_callback(std::function callback) + { + 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"); + } + }; + + // 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_)); + } protected: RCLCPP_DISABLE_COPY(ClientBase) @@ -171,13 +229,20 @@ 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::function on_new_response_callback_; }; template @@ -303,8 +368,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/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/experimental/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp index e215411bdf..e6c53f9ec3 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp @@ -50,23 +50,23 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable 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 @@ -77,12 +77,6 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable rmw_qos_profile_t get_actual_qos() const; - RCLCPP_PUBLIC - void - set_listener_callback( - rmw_listener_callback_t callback, - const void * user_data) const override; - protected: std::recursive_mutex reentrant_mutex_; rclcpp::GuardCondition gc_; diff --git a/rclcpp/include/rclcpp/qos_event.hpp b/rclcpp/include/rclcpp/qos_event.hpp index b3841cf2f5..2eaff577b7 100644 --- a/rclcpp/include/rclcpp/qos_event.hpp +++ b/rclcpp/include/rclcpp/qos_event.hpp @@ -21,12 +21,15 @@ #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 @@ -102,15 +105,78 @@ 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. + * + * 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 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_listener_callback( - rmw_listener_callback_t callback, - const void * user_data) const override; + set_on_new_event_callback(std::function callback) + { + auto new_callback = + [callback, this](size_t number_of_events) { + try { + callback(number_of_events); + } 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 new event' callback: " << + exception.what()); + } catch (...) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("rclcpp"), + "rclcpp::QOSEventHandlerBase@" << this << + " caught unhandled exception in user-provided callback " << + "for the 'on new event' callback"); + } + }; + + // Set it temporarily to the new callback, while we replace the old one. + // This two-step setting, prevents a gap where the old std::function has + // been replaced but the middleware hasn't been told about the new one yet. + set_on_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_)); + } 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::function on_new_event_callback_; }; template @@ -123,9 +189,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/service.hpp b/rclcpp/include/rclcpp/service.hpp index 283a5058e0..bb02834f31 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -25,16 +25,20 @@ #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 +125,68 @@ 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. + * + * 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_listener_callback( - rmw_listener_callback_t callback, - const void * user_data) const; + set_on_new_request_callback(std::function callback) + { + 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"); + } + }; + + // 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_)); + } protected: RCLCPP_DISABLE_COPY(ServiceBase) @@ -138,12 +199,20 @@ 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::function on_new_request_callback_; }; template diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index d7b7e9abaf..11e7ba4ddc 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -24,9 +24,11 @@ #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" @@ -263,11 +265,68 @@ 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); - RCLCPP_PUBLIC + /// Set a callback to be called when each new message is received. + /** + * The callback receives a size_t which is the number of messages received + * since the last time this callback was called. + * Normally this is 1, but can be > 1 if messages were received before any + * callback was set. + * + * Since this callback is called from the middleware, you should aim to make + * it fast and not blocking. + * If you need to do a lot of work or wait for some other event, you should + * spin it off to another thread, otherwise you risk blocking the middleware. + * + * Calling it again will clear any previously set callback. + * + * This function is thread-safe. + * + * If you want more information available in the callback, like the subscription + * or other information, you may use a lambda with captures or std::bind. + * + * \sa rmw_subscription_set_on_new_message_callback + * \sa rcl_subscription_set_on_new_message_callback + * + * \param[in] callback functor to be called when a new message is received + */ void - set_listener_callback( - rmw_listener_callback_t callback, - const void * user_data) const; + set_on_new_message_callback(std::function callback) + { + 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"); + } + }; + + // 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_)); + } protected: template @@ -293,11 +352,16 @@ 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_; @@ -315,6 +379,8 @@ 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::function on_new_message_callback_; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/waitable.hpp b/rclcpp/include/rclcpp/waitable.hpp index 9ae79c3508..85deabda85 100644 --- a/rclcpp/include/rclcpp/waitable.hpp +++ b/rclcpp/include/rclcpp/waitable.hpp @@ -203,13 +203,6 @@ class Waitable bool exchange_in_use_by_wait_set_state(bool in_use_state); - RCLCPP_PUBLIC - virtual - void - set_listener_callback( - rmw_listener_callback_t callback, - const void * user_data) const; - private: std::atomic in_use_by_wait_set_{false}; }; // class Waitable 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..e3ae7fa255 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; 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..7b8d58a505 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -39,6 +39,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), @@ -290,16 +291,17 @@ 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"); } } diff --git a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp index e4e1c67b88..859bca9d6d 100644 --- a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp +++ b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp @@ -43,12 +43,6 @@ 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"); - } + (void)callback; + (void)user_data; } From 6ef29c18fd5fb168931bf5d9577240439da68d71 Mon Sep 17 00:00:00 2001 From: Mauro Passerino Date: Thu, 1 Apr 2021 15:23:58 -0300 Subject: [PATCH 50/93] Remove const on set_listener_callback Signed-off-by: Mauro Passerino --- .../rclcpp/executors/events_executor_notify_waitable.hpp | 2 +- .../experimental/subscription_intra_process_base.hpp | 6 ++++++ rclcpp/include/rclcpp/qos_event.hpp | 6 ++++++ rclcpp/include/rclcpp/waitable.hpp | 7 +++++++ rclcpp/src/rclcpp/qos_event.cpp | 9 +++++++++ rclcpp/src/rclcpp/subscription_intra_process_base.cpp | 2 +- rclcpp/src/rclcpp/waitable.cpp | 2 +- rclcpp/test/rclcpp/executors/test_executors.cpp | 2 +- 8 files changed, 32 insertions(+), 4 deletions(-) diff --git a/rclcpp/include/rclcpp/executors/events_executor_notify_waitable.hpp b/rclcpp/include/rclcpp/executors/events_executor_notify_waitable.hpp index fe42d5a768..33b44ddae2 100644 --- a/rclcpp/include/rclcpp/executors/events_executor_notify_waitable.hpp +++ b/rclcpp/include/rclcpp/executors/events_executor_notify_waitable.hpp @@ -62,7 +62,7 @@ class EventsExecutorNotifyWaitable final : public EventWaitable void set_listener_callback( rmw_listener_callback_t callback, - const void * user_data) const override + const void * user_data) override { for (auto gc : notify_guard_conditions_) { // gc->set_listener_callback(); diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp index e6c53f9ec3..c7dd9458c8 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp @@ -77,6 +77,12 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable rmw_qos_profile_t get_actual_qos() const; + RCLCPP_PUBLIC + void + set_listener_callback( + rmw_listener_callback_t callback, + const void * user_data) override; + protected: std::recursive_mutex reentrant_mutex_; rclcpp::GuardCondition gc_; diff --git a/rclcpp/include/rclcpp/qos_event.hpp b/rclcpp/include/rclcpp/qos_event.hpp index 2eaff577b7..d941cdc1ac 100644 --- a/rclcpp/include/rclcpp/qos_event.hpp +++ b/rclcpp/include/rclcpp/qos_event.hpp @@ -169,6 +169,12 @@ class QOSEventHandlerBase : public Waitable static_cast(&on_new_event_callback_)); } + RCLCPP_PUBLIC + void + set_listener_callback( + rmw_listener_callback_t callback, + const void * user_data) override; + protected: RCLCPP_PUBLIC void diff --git a/rclcpp/include/rclcpp/waitable.hpp b/rclcpp/include/rclcpp/waitable.hpp index 85deabda85..d089b3a988 100644 --- a/rclcpp/include/rclcpp/waitable.hpp +++ b/rclcpp/include/rclcpp/waitable.hpp @@ -203,6 +203,13 @@ class Waitable bool exchange_in_use_by_wait_set_state(bool in_use_state); + RCLCPP_PUBLIC + virtual + void + set_listener_callback( + rmw_listener_callback_t callback, + const void * user_data); + private: std::atomic in_use_by_wait_set_{false}; }; // class Waitable diff --git a/rclcpp/src/rclcpp/qos_event.cpp b/rclcpp/src/rclcpp/qos_event.cpp index 3862cf68c5..eb4032383c 100644 --- a/rclcpp/src/rclcpp/qos_event.cpp +++ b/rclcpp/src/rclcpp/qos_event.cpp @@ -84,4 +84,13 @@ QOSEventHandlerBase::set_on_new_event_callback( } } +void +QOSEventHandlerBase::set_listener_callback( + rmw_listener_callback_t callback, + const void * user_data) +{ + set_on_new_event_callback(callback, user_data); +} + + } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp index 859bca9d6d..93fe9f104e 100644 --- a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp +++ b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp @@ -41,7 +41,7 @@ SubscriptionIntraProcessBase::get_actual_qos() const void SubscriptionIntraProcessBase::set_listener_callback( rmw_listener_callback_t callback, - const void * user_data) const + const void * user_data) { (void)callback; (void)user_data; diff --git a/rclcpp/src/rclcpp/waitable.cpp b/rclcpp/src/rclcpp/waitable.cpp index 2e0410d00e..f6b457e501 100644 --- a/rclcpp/src/rclcpp/waitable.cpp +++ b/rclcpp/src/rclcpp/waitable.cpp @@ -63,7 +63,7 @@ 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 + const void * user_data) { (void)callback; (void)user_data; diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index 0a13c5a013..d816049f0a 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -449,7 +449,7 @@ class TestWaitable : public rclcpp::Waitable void set_listener_callback( rmw_listener_callback_t callback, - const void * user_data) const override + const void * user_data) override { rcl_ret_t ret = rcl_guard_condition_set_listener_callback( &gc_, callback, user_data); From a9c6521466ef248b400b78505f32966057857f57 Mon Sep 17 00:00:00 2001 From: Andrea Sorbini Date: Fri, 9 Apr 2021 14:01:20 -0700 Subject: [PATCH 51/93] Restore test exception for Connext (#1625) Signed-off-by: Andrea Sorbini --- .../rclcpp/strategies/test_allocator_memory_strategy.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp index 0b4286dcf2..f57171ec8e 100644 --- a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp +++ b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp @@ -511,8 +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(); - if (implementation_identifier == "rmw_cyclonedds_cpp") { - // For cyclonedds, a subscription will also add an event and waitable + if (implementation_identifier == "rmw_cyclonedds_cpp" || + implementation_identifier == "rmw_connextdds") + { + // For cyclonedds and connext, a subscription will also add an event and waitable expected_sizes.size_of_events += 1; expected_sizes.size_of_waitables += 1; } From 1412e9437cf87310447877ebde72bbb69c7a6d91 Mon Sep 17 00:00:00 2001 From: Mauro Passerino Date: Fri, 9 Apr 2021 18:05:43 -0300 Subject: [PATCH 52/93] EventsExecutor to use std::function Signed-off-by: Mauro Passerino --- .../rclcpp/executors/events_executor.hpp | 25 ---- .../events_executor_entities_collector.hpp | 18 +-- .../executors/events_executor_event_types.hpp | 39 ------ rclcpp/include/rclcpp/waitable.hpp | 1 + .../events_executor_entities_collector.cpp | 111 ++++-------------- rclcpp/src/rclcpp/qos_event.cpp | 6 +- .../subscription_intra_process_base.cpp | 5 +- rclcpp/src/rclcpp/waitable.cpp | 5 +- .../test/rclcpp/executors/test_executors.cpp | 10 +- 9 files changed, 34 insertions(+), 186 deletions(-) diff --git a/rclcpp/include/rclcpp/executors/events_executor.hpp b/rclcpp/include/rclcpp/executors/events_executor.hpp index 8da8be55cb..ac136a241c 100644 --- a/rclcpp/include/rclcpp/executors/events_executor.hpp +++ b/rclcpp/include/rclcpp/executors/events_executor.hpp @@ -209,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 a4042fee8a..64d0d7e564 100644 --- a/rclcpp/include/rclcpp/executors/events_executor_entities_collector.hpp +++ b/rclcpp/include/rclcpp/executors/events_executor_entities_collector.hpp @@ -214,11 +214,8 @@ class EventsExecutorEntitiesCollector final void unset_guard_condition_callback(const rclcpp::GuardCondition * guard_condition); - void - remove_callback_data(void * entity_id, ExecutorEventType type); - - const EventsExecutorCallbackData * - get_callback_data(void * entity_id, ExecutorEventType type); + std::function + create_entity_callback(void * entity_id, ExecutorEventType type); /// Return true if the node belongs to the collector /** @@ -269,17 +266,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..30b2c83175 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, @@ -38,42 +35,6 @@ struct ExecutorEvent 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/waitable.hpp b/rclcpp/include/rclcpp/waitable.hpp index d089b3a988..a8c22371eb 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" diff --git a/rclcpp/src/rclcpp/executors/events_executor_entities_collector.cpp b/rclcpp/src/rclcpp/executors/events_executor_entities_collector.cpp index 4ed996caef..f94ec82907 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( @@ -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(); @@ -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_listener_callback( + // create_entity_callback(waitable.get(), WAITABLE_EVENT)); } 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->set_on_new_message_callback(nullptr); 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->set_on_new_request_callback(nullptr); 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->set_on_new_response_callback(nullptr); 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->set_listener_callback(nullptr); weak_waitables_map_.erase(waitable.get()); - remove_callback_data(waitable.get(), WAITABLE_EVENT); } return false; }); @@ -487,28 +478,13 @@ void EventsExecutorEntitiesCollector::set_guard_condition_callback( const 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)); - - if (ret != RCL_RET_OK) { - throw std::runtime_error("Couldn't set guard condition event callback"); - } + // create_entity_callback(this, WAITABLE_EVENT); } void EventsExecutorEntitiesCollector::unset_guard_condition_callback( const 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); } rclcpp::SubscriptionBase::SharedPtr @@ -592,56 +568,21 @@ 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_listener_callback([] (size_t, int){}); } -const EventsExecutorCallbackData * -EventsExecutorEntitiesCollector::get_callback_data( +std::function +EventsExecutorEntitiesCollector::create_entity_callback( void * 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; -} - -void -EventsExecutorEntitiesCollector::remove_callback_data( - void * 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. - // 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, entity_id, event_type](size_t num_events) { + ExecutorEvent event = {entity_id, 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(); + }; } diff --git a/rclcpp/src/rclcpp/qos_event.cpp b/rclcpp/src/rclcpp/qos_event.cpp index eb4032383c..7e0a65d5cb 100644 --- a/rclcpp/src/rclcpp/qos_event.cpp +++ b/rclcpp/src/rclcpp/qos_event.cpp @@ -85,11 +85,9 @@ QOSEventHandlerBase::set_on_new_event_callback( } void -QOSEventHandlerBase::set_listener_callback( - rmw_listener_callback_t callback, - const void * user_data) +QOSEventHandlerBase::set_listener_callback(std::function callback) { - set_on_new_event_callback(callback, user_data); + // set_on_new_event_callback(callback); } diff --git a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp index 93fe9f104e..8e30d06350 100644 --- a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp +++ b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp @@ -39,10 +39,7 @@ SubscriptionIntraProcessBase::get_actual_qos() const } void -SubscriptionIntraProcessBase::set_listener_callback( - rmw_listener_callback_t callback, - const void * user_data) +SubscriptionIntraProcessBase::set_listener_callback(std::function callback) { (void)callback; - (void)user_data; } diff --git a/rclcpp/src/rclcpp/waitable.cpp b/rclcpp/src/rclcpp/waitable.cpp index f6b457e501..3e6efe166d 100644 --- a/rclcpp/src/rclcpp/waitable.cpp +++ b/rclcpp/src/rclcpp/waitable.cpp @@ -61,12 +61,9 @@ 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) +Waitable::set_listener_callback(std::function callback) { (void)callback; - (void)user_data; throw std::runtime_error( "Custom waitables should override set_listener_callback() " diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index d816049f0a..cb4935d802 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -447,16 +447,8 @@ class TestWaitable : public rclcpp::Waitable } void - set_listener_callback( - rmw_listener_callback_t callback, - const void * user_data) override + set_listener_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: From 32aa3db79b78552c7dbe0d74b1049bf536c99367 Mon Sep 17 00:00:00 2001 From: Mauro Passerino Date: Fri, 9 Apr 2021 18:17:10 -0300 Subject: [PATCH 53/93] GuardCondition to have callback Signed-off-by: Mauro Passerino --- .../events_executor_entities_collector.hpp | 5 +++- .../executors/events_executor_event_types.hpp | 3 ++- .../events_executor_notify_waitable.hpp | 11 ++++----- .../subscription_intra_process_base.hpp | 4 +--- rclcpp/include/rclcpp/qos_event.hpp | 4 +--- rclcpp/include/rclcpp/waitable.hpp | 4 +--- .../src/rclcpp/executors/events_executor.cpp | 8 +++---- .../events_executor_entities_collector.cpp | 23 +++++++++++++++---- .../rclcpp/executors/test_events_queue.cpp | 2 +- 9 files changed, 38 insertions(+), 26 deletions(-) diff --git a/rclcpp/include/rclcpp/executors/events_executor_entities_collector.hpp b/rclcpp/include/rclcpp/executors/events_executor_entities_collector.hpp index 64d0d7e564..b42232e9f2 100644 --- a/rclcpp/include/rclcpp/executors/events_executor_entities_collector.hpp +++ b/rclcpp/include/rclcpp/executors/events_executor_entities_collector.hpp @@ -215,7 +215,10 @@ class EventsExecutorEntitiesCollector final unset_guard_condition_callback(const rclcpp::GuardCondition * guard_condition); std::function - create_entity_callback(void * entity_id, ExecutorEventType type); + create_entity_callback(void * exec_entity_id, ExecutorEventType type); + + std::function + create_waitable_callback(void * waitable_id); /// Return true if the node belongs to the collector /** diff --git a/rclcpp/include/rclcpp/executors/events_executor_event_types.hpp b/rclcpp/include/rclcpp/executors/events_executor_event_types.hpp index 30b2c83175..c2b9a76c74 100644 --- a/rclcpp/include/rclcpp/executors/events_executor_event_types.hpp +++ b/rclcpp/include/rclcpp/executors/events_executor_event_types.hpp @@ -30,7 +30,8 @@ enum ExecutorEventType struct ExecutorEvent { - const void * entity_id; + const void * exec_entity_id; + int gen_entity_id; ExecutorEventType type; size_t num_events; }; diff --git a/rclcpp/include/rclcpp/executors/events_executor_notify_waitable.hpp b/rclcpp/include/rclcpp/executors/events_executor_notify_waitable.hpp index 33b44ddae2..69af6b84d3 100644 --- a/rclcpp/include/rclcpp/executors/events_executor_notify_waitable.hpp +++ b/rclcpp/include/rclcpp/executors/events_executor_notify_waitable.hpp @@ -60,13 +60,12 @@ class EventsExecutorNotifyWaitable final : public EventWaitable RCLCPP_PUBLIC void - set_listener_callback( - rmw_listener_callback_t callback, - const void * user_data) override + set_listener_callback(std::function callback) override { - for (auto gc : notify_guard_conditions_) { - // gc->set_listener_callback(); - } + (void)callback; + // for (auto gc : notify_guard_conditions_) { + // gc->set_listener_callback(); + // } } RCLCPP_PUBLIC diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp index c7dd9458c8..6c22e16cda 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp @@ -79,9 +79,7 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable RCLCPP_PUBLIC void - set_listener_callback( - rmw_listener_callback_t callback, - const void * user_data) override; + set_listener_callback(std::function callback) override; protected: std::recursive_mutex reentrant_mutex_; diff --git a/rclcpp/include/rclcpp/qos_event.hpp b/rclcpp/include/rclcpp/qos_event.hpp index d941cdc1ac..d870a428f9 100644 --- a/rclcpp/include/rclcpp/qos_event.hpp +++ b/rclcpp/include/rclcpp/qos_event.hpp @@ -171,9 +171,7 @@ class QOSEventHandlerBase : public Waitable RCLCPP_PUBLIC void - set_listener_callback( - rmw_listener_callback_t callback, - const void * user_data) override; + set_listener_callback(std::function callback) override; protected: RCLCPP_PUBLIC diff --git a/rclcpp/include/rclcpp/waitable.hpp b/rclcpp/include/rclcpp/waitable.hpp index a8c22371eb..a2d20070db 100644 --- a/rclcpp/include/rclcpp/waitable.hpp +++ b/rclcpp/include/rclcpp/waitable.hpp @@ -207,9 +207,7 @@ class Waitable RCLCPP_PUBLIC virtual void - set_listener_callback( - rmw_listener_callback_t callback, - const void * user_data); + set_listener_callback(std::function callback); private: std::atomic in_use_by_wait_set_{false}; diff --git a/rclcpp/src/rclcpp/executors/events_executor.cpp b/rclcpp/src/rclcpp/executors/events_executor.cpp index 0d0fceed23..1d609913b5 100644 --- a/rclcpp/src/rclcpp/executors/events_executor.cpp +++ b/rclcpp/src/rclcpp/executors/events_executor.cpp @@ -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 f94ec82907..7e112dd601 100644 --- a/rclcpp/src/rclcpp/executors/events_executor_entities_collector.cpp +++ b/rclcpp/src/rclcpp/executors/events_executor_entities_collector.cpp @@ -478,7 +478,7 @@ void EventsExecutorEntitiesCollector::set_guard_condition_callback( const rclcpp::GuardCondition * guard_condition) { - // create_entity_callback(this, WAITABLE_EVENT); + create_waitable_callback(this); } void @@ -573,10 +573,25 @@ EventsExecutorEntitiesCollector::add_waitable(rclcpp::Waitable::SharedPtr waitab std::function EventsExecutorEntitiesCollector::create_entity_callback( - void * entity_id, ExecutorEventType event_type) + void * exec_entity_id, ExecutorEventType event_type) { - return [this, entity_id, event_type](size_t num_events) { - ExecutorEvent event = {entity_id, event_type, num_events}; + 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(); + }; +} + +std::function +EventsExecutorEntitiesCollector::create_waitable_callback(void * exec_entity_id) +{ + 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_); diff --git a/rclcpp/test/rclcpp/executors/test_events_queue.cpp b/rclcpp/test/rclcpp/executors/test_events_queue.cpp index 9d600e61f7..7ea0d657c3 100644 --- a/rclcpp/test/rclcpp/executors/test_events_queue.cpp +++ b/rclcpp/test/rclcpp/executors/test_events_queue.cpp @@ -60,6 +60,6 @@ TEST(TestEventsQueue, SimpleQueueTest) 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); } From 7cadae3154b1719e6dca9c0a30062cfd143ed2b3 Mon Sep 17 00:00:00 2001 From: Mauro Passerino Date: Mon, 12 Apr 2021 11:58:45 -0300 Subject: [PATCH 54/93] IPC Subscription to have callback Signed-off-by: Mauro Passerino --- .../events_executor_entities_collector.hpp | 6 ++--- .../subscription_intra_process.hpp | 7 +++++- .../subscription_intra_process_base.hpp | 2 ++ rclcpp/include/rclcpp/guard_condition.hpp | 6 +++++ .../events_executor_entities_collector.cpp | 13 +++++----- rclcpp/src/rclcpp/guard_condition.cpp | 24 ++++++++++++++++--- .../subscription_intra_process_base.cpp | 9 ++++++- 7 files changed, 53 insertions(+), 14 deletions(-) diff --git a/rclcpp/include/rclcpp/executors/events_executor_entities_collector.hpp b/rclcpp/include/rclcpp/executors/events_executor_entities_collector.hpp index b42232e9f2..6e66d32f1d 100644 --- a/rclcpp/include/rclcpp/executors/events_executor_entities_collector.hpp +++ b/rclcpp/include/rclcpp/executors/events_executor_entities_collector.hpp @@ -209,10 +209,10 @@ class EventsExecutorEntitiesCollector final unset_callback_group_entities_callbacks(rclcpp::CallbackGroup::SharedPtr group); void - set_guard_condition_callback(const rclcpp::GuardCondition * guard_condition); + set_guard_condition_callback(rclcpp::GuardCondition * guard_condition); void - unset_guard_condition_callback(const rclcpp::GuardCondition * guard_condition); + unset_guard_condition_callback(rclcpp::GuardCondition * guard_condition); std::function create_entity_callback(void * exec_entity_id, ExecutorEventType type); @@ -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_; diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp index f9939927c8..18f2f06f9d 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp @@ -148,7 +148,12 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBase void trigger_guard_condition() { - gc_.trigger(); + if (on_new_message_callback_) { + on_new_message_callback_(1); + } else { + gc_.trigger(); + unread_count_++; + } } template diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp index 6c22e16cda..f4eb501f61 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp @@ -84,6 +84,8 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable protected: std::recursive_mutex reentrant_mutex_; rclcpp::GuardCondition gc_; + std::function on_new_message_callback_{nullptr}; + size_t unread_count_{0}; private: virtual void diff --git a/rclcpp/include/rclcpp/guard_condition.hpp b/rclcpp/include/rclcpp/guard_condition.hpp index 71c7fa16a6..f7b69c47df 100644 --- a/rclcpp/include/rclcpp/guard_condition.hpp +++ b/rclcpp/include/rclcpp/guard_condition.hpp @@ -100,10 +100,16 @@ class GuardCondition 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/src/rclcpp/executors/events_executor_entities_collector.cpp b/rclcpp/src/rclcpp/executors/events_executor_entities_collector.cpp index 7e112dd601..0616f5f43a 100644 --- a/rclcpp/src/rclcpp/executors/events_executor_entities_collector.cpp +++ b/rclcpp/src/rclcpp/executors/events_executor_entities_collector.cpp @@ -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); } } @@ -267,8 +267,8 @@ EventsExecutorEntitiesCollector::set_callback_group_entities_callbacks( if (waitable) { weak_waitables_map_.emplace(waitable.get(), waitable); - // waitable->set_listener_callback( - // create_entity_callback(waitable.get(), WAITABLE_EVENT)); + waitable->set_listener_callback( + create_waitable_callback(waitable.get())); } return false; }); @@ -476,15 +476,16 @@ EventsExecutorEntitiesCollector::get_automatically_added_callback_groups_from_no void EventsExecutorEntitiesCollector::set_guard_condition_callback( - const rclcpp::GuardCondition * guard_condition) + rclcpp::GuardCondition * guard_condition) { - create_waitable_callback(this); + guard_condition->set_on_trigger_callback(create_waitable_callback(this)); } void EventsExecutorEntitiesCollector::unset_guard_condition_callback( - const rclcpp::GuardCondition * guard_condition) + rclcpp::GuardCondition * guard_condition) { + guard_condition->set_on_trigger_callback(nullptr); } rclcpp::SubscriptionBase::SharedPtr diff --git a/rclcpp/src/rclcpp/guard_condition.cpp b/rclcpp/src/rclcpp/guard_condition.cpp index a5f039fe41..8e34b2539b 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" @@ -66,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_++; } } @@ -87,4 +94,15 @@ GuardCondition::add_to_wait_set(rcl_wait_set_t * wait_set) const ret, "failed to add guard condition to wait set"); } } + +void +GuardCondition::set_on_trigger_callback(std::function callback) +{ + on_trigger_callback_ = std::bind(callback, std::placeholders::_1, -1); + + if (unread_count_) { + on_trigger_callback_(unread_count_); + unread_count_ = 0; + } +} } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp index 8e30d06350..2d9bdeb829 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; @@ -41,5 +43,10 @@ SubscriptionIntraProcessBase::get_actual_qos() const void SubscriptionIntraProcessBase::set_listener_callback(std::function callback) { - (void)callback; + on_new_message_callback_ = std::bind(callback, std::placeholders::_1, -1); + + if (unread_count_ && on_new_message_callback_) { + on_new_message_callback_(unread_count_); + unread_count_ = 0; + } } From 41c8f8a53e84668a20ada06c7b57d19966fe3503 Mon Sep 17 00:00:00 2001 From: Mauro Passerino Date: Mon, 12 Apr 2021 12:40:49 -0300 Subject: [PATCH 55/93] EntitiesCollector GCs to push single event Signed-off-by: Mauro Passerino --- rclcpp/include/rclcpp/guard_condition.hpp | 2 +- .../events_executor_entities_collector.cpp | 19 +++++++++++++++++-- rclcpp/src/rclcpp/guard_condition.cpp | 15 ++++++++++----- .../subscription_intra_process_base.cpp | 18 +++++++++++++----- 4 files changed, 41 insertions(+), 13 deletions(-) diff --git a/rclcpp/include/rclcpp/guard_condition.hpp b/rclcpp/include/rclcpp/guard_condition.hpp index f7b69c47df..5ad2d5a757 100644 --- a/rclcpp/include/rclcpp/guard_condition.hpp +++ b/rclcpp/include/rclcpp/guard_condition.hpp @@ -102,7 +102,7 @@ class GuardCondition RCLCPP_PUBLIC void - set_on_trigger_callback(std::function callback); + set_on_trigger_callback(std::function callback); protected: rclcpp::Context::SharedPtr context_; diff --git a/rclcpp/src/rclcpp/executors/events_executor_entities_collector.cpp b/rclcpp/src/rclcpp/executors/events_executor_entities_collector.cpp index 0616f5f43a..35709a42f4 100644 --- a/rclcpp/src/rclcpp/executors/events_executor_entities_collector.cpp +++ b/rclcpp/src/rclcpp/executors/events_executor_entities_collector.cpp @@ -478,7 +478,21 @@ void EventsExecutorEntitiesCollector::set_guard_condition_callback( rclcpp::GuardCondition * guard_condition) { - guard_condition->set_on_trigger_callback(create_waitable_callback(this)); + 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(); + }; + + guard_condition->set_on_trigger_callback(gc_callback); } void @@ -569,7 +583,8 @@ EventsExecutorEntitiesCollector::add_waitable(rclcpp::Waitable::SharedPtr waitab { weak_waitables_map_.emplace(waitable.get(), waitable); - waitable->set_listener_callback([] (size_t, int){}); + waitable->set_listener_callback( + create_waitable_callback(waitable.get())); } std::function diff --git a/rclcpp/src/rclcpp/guard_condition.cpp b/rclcpp/src/rclcpp/guard_condition.cpp index 8e34b2539b..3d44638ea7 100644 --- a/rclcpp/src/rclcpp/guard_condition.cpp +++ b/rclcpp/src/rclcpp/guard_condition.cpp @@ -96,13 +96,18 @@ GuardCondition::add_to_wait_set(rcl_wait_set_t * wait_set) const } void -GuardCondition::set_on_trigger_callback(std::function callback) +GuardCondition::set_on_trigger_callback(std::function callback) { - on_trigger_callback_ = std::bind(callback, std::placeholders::_1, -1); + if (callback) { + on_trigger_callback_ = callback; - if (unread_count_) { - on_trigger_callback_(unread_count_); - unread_count_ = 0; + if (unread_count_) { + callback(unread_count_); + unread_count_ = 0; + } + return; } + + on_trigger_callback_ = nullptr; } } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp index 2d9bdeb829..bdc5ebb01a 100644 --- a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp +++ b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp @@ -43,10 +43,18 @@ SubscriptionIntraProcessBase::get_actual_qos() const void SubscriptionIntraProcessBase::set_listener_callback(std::function callback) { - on_new_message_callback_ = std::bind(callback, std::placeholders::_1, -1); - - if (unread_count_ && on_new_message_callback_) { - on_new_message_callback_(unread_count_); - unread_count_ = 0; + if (callback) { + on_new_message_callback_ = std::bind(callback, std::placeholders::_1, -1); + if (unread_count_) { + if (unread_count_ < qos_profile_.depth) { + on_new_message_callback_(unread_count_); + } else { + on_new_message_callback_(qos_profile_.depth); + } + unread_count_ = 0; + } + return; } + + on_new_message_callback_ = nullptr; } From 98ab933a73988969583dde08dfbc2b3f014deda8 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 12 Apr 2021 18:05:17 +0000 Subject: [PATCH 56/93] Changelog. Signed-off-by: Chris Lalancette --- rclcpp/CHANGELOG.rst | 6 ++++++ rclcpp_action/CHANGELOG.rst | 3 +++ rclcpp_components/CHANGELOG.rst | 3 +++ rclcpp_lifecycle/CHANGELOG.rst | 3 +++ 4 files changed, 15 insertions(+) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 9dcc518d2f..7558ecf71c 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 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 `_) diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index c0669da05d..17e288d98a 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 9.0.0 (2021-04-06) ------------------ * updating quality declaration links (re: `ros2/docs.ros2.org#52 `_) (`#1615 `_) diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index a14a28e34b..c1a3164f6f 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 9.0.0 (2021-04-06) ------------------ * updating quality declaration links (re: `ros2/docs.ros2.org#52 `_) (`#1615 `_) diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 6507f0ea0f..a96b95451d 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 9.0.0 (2021-04-06) ------------------ * Add generic publisher and generic subscription for serialized messages (`#1452 `_) From a62287bf8d1eab0f146394ff0efdd4da44d4ef34 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 12 Apr 2021 18:05:27 +0000 Subject: [PATCH 57/93] 9.0.1 --- rclcpp/CHANGELOG.rst | 4 ++-- rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 4 ++-- rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 4 ++-- rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 4 ++-- rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 7558ecf71c..82a534b7a6 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +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 diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 4cdd2625fd..5cc73b34fc 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 9.0.0 + 9.0.1 The ROS client library in C++. Ivan Paunovic Mabel Zhang diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 17e288d98a..1c29015fcc 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +9.0.1 (2021-04-12) +------------------ 9.0.0 (2021-04-06) ------------------ diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index 08246eaf35..db8dd81d0a 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 9.0.0 + 9.0.1 Adds action APIs for C++. Ivan Paunovic Mabel Zhang diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index c1a3164f6f..6d77b9ff3f 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +9.0.1 (2021-04-12) +------------------ 9.0.0 (2021-04-06) ------------------ diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index a1ba2ec449..64d850552a 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 9.0.0 + 9.0.1 Package containing tools for dynamically loadable components Ivan Paunovic Mabel Zhang diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index a96b95451d..f25c38f5c6 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +9.0.1 (2021-04-12) +------------------ 9.0.0 (2021-04-06) ------------------ diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 54f784ab22..855577cc98 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 9.0.0 + 9.0.1 Package containing a prototype for lifecycle implementation Ivan Paunovic Mabel Zhang From 893679e44fef04095356edceaaf4e50810c15aef Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Tue, 13 Apr 2021 09:32:06 -0300 Subject: [PATCH 58/93] Check first parameter type and range before calling the user validation callbacks (#1627) Signed-off-by: Ivan Santiago Paunovic --- rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index 16aa9c4903..1f9e32acbc 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -299,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; } From 085f161230769b57a0e4a711bb45a09c94b69b69 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Tue, 13 Apr 2021 11:26:22 -0300 Subject: [PATCH 59/93] Do not test the value of the history policy when testing the get_publishers/subscriptions_info_by_topic() methods (#1626) Signed-off-by: Ivan Santiago Paunovic --- .../node_interfaces/test_node_graph.cpp | 22 ++----------------- 1 file changed, 2 insertions(+), 20 deletions(-) diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp index 6690b88bf6..6a44d7abf7 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp @@ -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(); From c4a68b4199b35100ce580c519e3260ce5c297a5d Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Tue, 13 Apr 2021 17:36:15 -0300 Subject: [PATCH 60/93] Use a different implementation of mutex two priorities (#1628) Signed-off-by: Ivan Santiago Paunovic --- .../rclcpp/detail/mutex_two_priorities.hpp | 11 ++--- .../rclcpp/detail/mutex_two_priorities.cpp | 43 ++++++++++++++++--- 2 files changed, 42 insertions(+), 12 deletions(-) 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/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 From 1c2bd84725f009ab75dc63ddbd5040580bac4ed1 Mon Sep 17 00:00:00 2001 From: Miguel Company Date: Wed, 14 Apr 2021 02:53:02 +0200 Subject: [PATCH 61/93] Avoid returning loan when none was obtained. (#1629) Signed-off-by: Miguel Company --- rclcpp/src/rclcpp/executor.cpp | 20 +++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index d33df58ee2..630db50b9b 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -610,16 +610,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. From 14aba06922d4f19f82dd38836724bb8ff5b7c6ac Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 14 Apr 2021 20:05:13 +0000 Subject: [PATCH 62/93] Changelog. Signed-off-by: Chris Lalancette --- rclcpp/CHANGELOG.rst | 8 ++++++++ rclcpp_action/CHANGELOG.rst | 3 +++ rclcpp_components/CHANGELOG.rst | 3 +++ rclcpp_lifecycle/CHANGELOG.rst | 3 +++ 4 files changed, 17 insertions(+) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 82a534b7a6..dcc5c368d8 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 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 `_) diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 1c29015fcc..a6de3e4cc9 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 9.0.1 (2021-04-12) ------------------ diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 6d77b9ff3f..551e93784b 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 9.0.1 (2021-04-12) ------------------ diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index f25c38f5c6..5744bd5fc2 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 9.0.1 (2021-04-12) ------------------ From ec70642c553cce307e099af7802ae7cbe509bf1b Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 14 Apr 2021 20:05:19 +0000 Subject: [PATCH 63/93] 9.0.2 --- rclcpp/CHANGELOG.rst | 4 ++-- rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 4 ++-- rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 4 ++-- rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 4 ++-- rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index dcc5c368d8..e3ceb06d10 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +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 `_) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 5cc73b34fc..568915ef72 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 9.0.1 + 9.0.2 The ROS client library in C++. Ivan Paunovic Mabel Zhang diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index a6de3e4cc9..e41da9c05d 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +9.0.2 (2021-04-14) +------------------ 9.0.1 (2021-04-12) ------------------ diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index db8dd81d0a..50877fc9f3 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 9.0.1 + 9.0.2 Adds action APIs for C++. Ivan Paunovic Mabel Zhang diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 551e93784b..bf48f99d54 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +9.0.2 (2021-04-14) +------------------ 9.0.1 (2021-04-12) ------------------ diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index 64d850552a..87cbd4865c 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 9.0.1 + 9.0.2 Package containing tools for dynamically loadable components Ivan Paunovic Mabel Zhang diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 5744bd5fc2..5141bf06a2 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +9.0.2 (2021-04-14) +------------------ 9.0.1 (2021-04-12) ------------------ diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 855577cc98..bfe74af56f 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 9.0.1 + 9.0.2 Package containing a prototype for lifecycle implementation Ivan Paunovic Mabel Zhang From b8df9347a10bff383fdc8890c5b1bfdf16430d8f Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Thu, 15 Apr 2021 10:47:13 -0700 Subject: [PATCH 64/93] Clarify node parameters docs (#1631) Set and initialize seem redundant. Update the doc brief to match the equivalent in node.hpp. Signed-off-by: Jacob Perron --- .../rclcpp/node_interfaces/node_parameters_interface.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 */ From 1cfa92ca1dc4a0d74866914a60c375b29d7692d8 Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Mon, 19 Apr 2021 12:19:19 +0100 Subject: [PATCH 65/93] Add on_ready callback to waitables, add clear_* functions, add unit-tests Signed-off-by: Alberto Soragna --- rclcpp/include/rclcpp/client.hpp | 21 +++- .../subscription_intra_process.hpp | 13 +++ .../subscription_intra_process_base.hpp | 97 ++++++++++++++++++- rclcpp/include/rclcpp/qos_event.hpp | 54 +++++++++-- rclcpp/include/rclcpp/service.hpp | 22 ++++- rclcpp/include/rclcpp/subscription_base.hpp | 18 +++- rclcpp/include/rclcpp/waitable.hpp | 40 ++++++++ rclcpp/src/rclcpp/waitable.cpp | 18 ++++ rclcpp/test/rclcpp/test_client.cpp | 91 +++++++++++++++++ rclcpp/test/rclcpp/test_service.cpp | 71 ++++++++++++++ rclcpp/test/rclcpp/test_subscription.cpp | 70 +++++++++++++ 11 files changed, 501 insertions(+), 14 deletions(-) diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index 2ca256ab6e..20769e7213 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -165,19 +165,29 @@ class ClientBase * * 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 rclcpp::ClientBase::clear_on_new_response_callback * \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 */ + RCLCPP_PUBLIC 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 { @@ -214,6 +224,15 @@ class ClientBase static_cast(&on_new_response_callback_)); } + /// Unset the callback registered for new responses, if any. + RCLCPP_PUBLIC + void + clear_on_new_response_callback() + { + set_on_new_response_callback(nullptr, nullptr); + on_new_response_callback_ = nullptr; + } + protected: RCLCPP_DISABLE_COPY(ClientBase) @@ -242,7 +261,7 @@ class ClientBase std::atomic in_use_by_wait_set_{false}; - std::function on_new_response_callback_; + std::function on_new_response_callback_{nullptr}; }; template diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp index 46e019daee..ae0fa25f2f 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp @@ -148,6 +148,7 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBase provide_intra_process_message(ConstMessageSharedPtr message) { buffer_->add_shared(std::move(message)); + invoke_callback(); trigger_guard_condition(); } @@ -155,6 +156,7 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBase provide_intra_process_message(MessageUniquePtr message) { buffer_->add_unique(std::move(message)); + invoke_callback(); trigger_guard_condition(); } @@ -165,6 +167,17 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBase } private: + void + invoke_callback() + { + std::lock_guard lock(reentrant_mutex_); + if (on_new_message_callback_) { + on_new_message_callback_(1); + } else { + unread_count_++; + } + } + void trigger_guard_condition() { diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp index 4cc8f74a6d..1f9b54c890 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp @@ -15,16 +15,16 @@ #ifndef RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_ #define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_ -#include - -#include +#include #include #include #include #include #include "rcl/error_handling.h" +#include "rmw/impl/cpp/demangle.hpp" +#include "rclcpp/logging.hpp" #include "rclcpp/type_support_decl.hpp" #include "rclcpp/waitable.hpp" @@ -38,6 +38,11 @@ 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) @@ -74,8 +79,94 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable rmw_qos_profile_t get_actual_qos() const; + /// Set a callback to be called when each new event instance occurs. + /** + * 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. + * + * 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 + * bounded 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 client + * or other information, you may use a lambda with captures or std::bind. + * + * 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::SubscriptionIntraProcessBase::clear_on_ready_callback + * + * \param[in] callback functor to be called when a new message is received. + */ + RCLCPP_PUBLIC + 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) { + // 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. + RCLCPP_PUBLIC + void + clear_on_ready_callback() override + { + std::lock_guard lock(reentrant_mutex_); + on_new_message_callback_ = nullptr; + } + protected: std::recursive_mutex reentrant_mutex_; + std::function on_new_message_callback_ {nullptr}; + size_t unread_count_{0}; rcl_guard_condition_t gc_; private: diff --git a/rclcpp/include/rclcpp/qos_event.hpp b/rclcpp/include/rclcpp/qos_event.hpp index 2eaff577b7..2f29d40ec4 100644 --- a/rclcpp/include/rclcpp/qos_event.hpp +++ b/rclcpp/include/rclcpp/qos_event.hpp @@ -87,6 +87,11 @@ class UnsupportedEventTypeException : public exceptions::RCLErrorBase, public st class QOSEventHandlerBase : public Waitable { public: + enum class EntityType + { + Event, + }; + RCLCPP_PUBLIC virtual ~QOSEventHandlerBase(); @@ -107,11 +112,21 @@ class QOSEventHandlerBase : public Waitable /// 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 + * 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 events occurred before any + * Normally this is 1, but can be > 1 if responses 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 + * bounded 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 @@ -119,37 +134,51 @@ class QOSEventHandlerBase : public Waitable * * 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 + * 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. * + * 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::QOSEventHandlerBase::clear_on_ready_callback * \sa rmw_event_set_callback * \sa rcl_event_set_callback * * \param[in] callback functor to be called when a new event occurs */ + RCLCPP_PUBLIC void - set_on_new_event_callback(std::function callback) + 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); + 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 new event' callback: " << + " 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 new event' callback"); + "for the 'on ready' callback"); } }; @@ -169,6 +198,15 @@ class QOSEventHandlerBase : public Waitable static_cast(&on_new_event_callback_)); } + /// Unset the callback registered for new events, if any. + RCLCPP_PUBLIC + void + clear_on_ready_callback() override + { + set_on_new_event_callback(nullptr, nullptr); + on_new_event_callback_ = nullptr; + } + protected: RCLCPP_PUBLIC void @@ -176,7 +214,7 @@ class QOSEventHandlerBase : public Waitable rcl_event_t event_handle_; size_t wait_set_event_index_; - std::function on_new_event_callback_; + std::function on_new_event_callback_{nullptr}; }; template diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index bb02834f31..8ad53f0c9a 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -139,19 +139,30 @@ class ServiceBase * * 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 rclcpp::ServiceBase::clear_on_new_request_callback * \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 */ + RCLCPP_PUBLIC 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 { @@ -188,6 +199,15 @@ class ServiceBase static_cast(&on_new_request_callback_)); } + /// Unset the callback registered for new requests, if any. + RCLCPP_PUBLIC + void + clear_on_new_request_callback() + { + set_on_new_request_callback(nullptr, nullptr); + on_new_request_callback_ = nullptr; + } + protected: RCLCPP_DISABLE_COPY(ServiceBase) @@ -212,7 +232,7 @@ class ServiceBase std::atomic in_use_by_wait_set_{false}; - std::function on_new_request_callback_; + std::function on_new_request_callback_{nullptr}; }; template diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index 11e7ba4ddc..19e74cc731 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -289,9 +289,16 @@ class SubscriptionBase : public std::enable_shared_from_this * * \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 { @@ -328,6 +335,15 @@ class SubscriptionBase : public std::enable_shared_from_this static_cast(&on_new_message_callback_)); } + /// Unset the callback registered for new messages, if any. + RCLCPP_PUBLIC + void + clear_on_new_message_callback() + { + set_on_new_message_callback(nullptr, nullptr); + on_new_message_callback_ = nullptr; + } + protected: template void @@ -380,7 +396,7 @@ class SubscriptionBase : public std::enable_shared_from_this std::unordered_map> qos_events_in_use_by_wait_set_; - std::function on_new_message_callback_; + std::function on_new_message_callback_{nullptr}; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/waitable.hpp b/rclcpp/include/rclcpp/waitable.hpp index 85deabda85..207cab5018 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,6 +204,45 @@ class Waitable bool exchange_in_use_by_wait_set_state(bool in_use_state); + /// Set a callback to be called whenever the waitable becomes ready. + /** + * The callback receives a size_t which is the number of 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. + * + * 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 + * bounded to the function. + * This implies that the provided callback can use the identifier to behave + * differently depending on which entity triggered the waitable to become ready. + * + * Note: this function must be overridden with a proper implementation + * by the custom classes who inherit from rclcpp::Waitable if they want to use it. + * + * \sa rclcpp::Waitable::clear_on_ready_callback + * + * \param[in] callback functor to be called when the waitable becomes ready + */ + RCLCPP_PUBLIC + virtual + void + set_on_ready_callback(std::function callback); + + /// Unset any callback registered via set_on_ready_callback. + /** + * Note: this function must be overridden with a proper implementation + * by the custom classes who inherit from rclcpp::Waitable if they want to use it. + */ + RCLCPP_PUBLIC + virtual + void + clear_on_ready_callback(); + private: std::atomic in_use_by_wait_set_{false}; }; // class Waitable diff --git a/rclcpp/src/rclcpp/waitable.cpp b/rclcpp/src/rclcpp/waitable.cpp index ef3a50a8d9..a42968dc95 100644 --- a/rclcpp/src/rclcpp/waitable.cpp +++ b/rclcpp/src/rclcpp/waitable.cpp @@ -59,3 +59,21 @@ Waitable::exchange_in_use_by_wait_set_state(bool in_use_state) { return in_use_by_wait_set_.exchange(in_use_state); } + +void +Waitable::set_on_ready_callback(std::function callback) +{ + (void)callback; + + throw std::runtime_error( + "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/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_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..761e3bc269 100644 --- a/rclcpp/test/rclcpp/test_subscription.cpp +++ b/rclcpp/test/rclcpp/test_subscription.cpp @@ -440,6 +440,76 @@ 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(); + using test_msgs::msg::Empty; + + auto do_nothing = [](std::shared_ptr) {FAIL();}; + auto sub = node->create_subscription("~/test_take", 1, 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 subscription with intraprocess enabled and invalid QoS */ From bff6916e8f3f4d0cb56ebe62a5958bfa5a586977 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 21 Apr 2021 11:58:05 -0700 Subject: [PATCH 66/93] Increase cppcheck timeout to 500s (#1634) Signed-off-by: Shane Loretz --- rclcpp/CMakeLists.txt | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 891b4549aa..95dc5311d1 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -247,3 +247,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() From e151cc203d4b82debb100883afe2b214161eda85 Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Fri, 23 Apr 2021 11:04:16 +0100 Subject: [PATCH 67/93] add mutex to set and clear listener APIs Signed-off-by: Alberto Soragna --- rclcpp/include/rclcpp/client.hpp | 7 +++++-- .../experimental/subscription_intra_process_base.hpp | 1 - rclcpp/include/rclcpp/qos_event.hpp | 5 +++++ rclcpp/include/rclcpp/service.hpp | 5 +++++ rclcpp/include/rclcpp/subscription_base.hpp | 7 +++++-- rclcpp/include/rclcpp/waitable.hpp | 2 -- 6 files changed, 20 insertions(+), 7 deletions(-) diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index 20769e7213..6ac59ebd65 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -178,7 +179,6 @@ class ClientBase * * \param[in] callback functor to be called when a new response is received */ - RCLCPP_PUBLIC void set_on_new_response_callback(std::function callback) { @@ -208,6 +208,8 @@ class ClientBase } }; + 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. @@ -225,10 +227,10 @@ class ClientBase } /// Unset the callback registered for new responses, if any. - RCLCPP_PUBLIC void clear_on_new_response_callback() { + std::lock_guard lock(reentrant_mutex_); set_on_new_response_callback(nullptr, nullptr); on_new_response_callback_ = nullptr; } @@ -261,6 +263,7 @@ class ClientBase std::atomic in_use_by_wait_set_{false}; + std::recursive_mutex reentrant_mutex_; std::function on_new_response_callback_{nullptr}; }; diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp index 1f9b54c890..1e0736fd16 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp @@ -112,7 +112,6 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable * * \param[in] callback functor to be called when a new message is received. */ - RCLCPP_PUBLIC void set_on_ready_callback(std::function callback) override { diff --git a/rclcpp/include/rclcpp/qos_event.hpp b/rclcpp/include/rclcpp/qos_event.hpp index 2f29d40ec4..4b2c49a742 100644 --- a/rclcpp/include/rclcpp/qos_event.hpp +++ b/rclcpp/include/rclcpp/qos_event.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include @@ -182,6 +183,8 @@ class QOSEventHandlerBase : public Waitable } }; + 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. @@ -203,6 +206,7 @@ class QOSEventHandlerBase : public Waitable void clear_on_ready_callback() override { + std::lock_guard lock(reentrant_mutex_); set_on_new_event_callback(nullptr, nullptr); on_new_event_callback_ = nullptr; } @@ -214,6 +218,7 @@ class QOSEventHandlerBase : public Waitable rcl_event_t event_handle_; size_t wait_set_event_index_; + std::recursive_mutex reentrant_mutex_; std::function on_new_event_callback_{nullptr}; }; diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 8ad53f0c9a..52e4bdb069 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -183,6 +184,8 @@ class ServiceBase } }; + 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. @@ -204,6 +207,7 @@ class ServiceBase void clear_on_new_request_callback() { + std::lock_guard lock(reentrant_mutex_); set_on_new_request_callback(nullptr, nullptr); on_new_request_callback_ = nullptr; } @@ -232,6 +236,7 @@ class ServiceBase std::atomic in_use_by_wait_set_{false}; + std::recursive_mutex reentrant_mutex_; std::function on_new_request_callback_{nullptr}; }; diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index 19e74cc731..2d0021e143 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 @@ -289,7 +290,6 @@ class SubscriptionBase : public std::enable_shared_from_this * * \param[in] callback functor to be called when a new message is received */ - RCLCPP_PUBLIC void set_on_new_message_callback(std::function callback) { @@ -319,6 +319,8 @@ class SubscriptionBase : public std::enable_shared_from_this } }; + 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. @@ -336,10 +338,10 @@ class SubscriptionBase : public std::enable_shared_from_this } /// 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; } @@ -396,6 +398,7 @@ class SubscriptionBase : public std::enable_shared_from_this std::unordered_map> qos_events_in_use_by_wait_set_; + std::recursive_mutex reentrant_mutex_; std::function on_new_message_callback_{nullptr}; }; diff --git a/rclcpp/include/rclcpp/waitable.hpp b/rclcpp/include/rclcpp/waitable.hpp index 207cab5018..8e2f2c7226 100644 --- a/rclcpp/include/rclcpp/waitable.hpp +++ b/rclcpp/include/rclcpp/waitable.hpp @@ -228,7 +228,6 @@ class Waitable * * \param[in] callback functor to be called when the waitable becomes ready */ - RCLCPP_PUBLIC virtual void set_on_ready_callback(std::function callback); @@ -238,7 +237,6 @@ class Waitable * Note: this function must be overridden with a proper implementation * by the custom classes who inherit from rclcpp::Waitable if they want to use it. */ - RCLCPP_PUBLIC virtual void clear_on_ready_callback(); From dd0cd981f4167bbdb3d86a30303ef52457a235b9 Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Fri, 23 Apr 2021 11:30:52 +0100 Subject: [PATCH 68/93] allow to set ipc sub callback from regular subscription Signed-off-by: Alberto Soragna --- rclcpp/include/rclcpp/subscription.hpp | 5 -- rclcpp/include/rclcpp/subscription_base.hpp | 50 ++++++++++++++ rclcpp/test/rclcpp/test_subscription.cpp | 72 ++++++++++++++++++++- 3 files changed, 121 insertions(+), 6 deletions(-) diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index fe98dc5796..a6c71c3c37 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -343,11 +343,6 @@ class Subscription : public SubscriptionBase message_memory_strategy_; /// Component which computes and publishes topic statistics for this subscriber SubscriptionTopicStatisticsSharedPtr subscription_topic_statistics_{nullptr}; - using SubscriptionIntraProcessT = rclcpp::experimental::SubscriptionIntraProcess< - CallbackMessageT, - AllocatorT, - typename MessageUniquePtr::deleter_type>; - std::shared_ptr subscription_intra_process_; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index 2d0021e143..9c7c5f6992 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -346,6 +346,55 @@ class SubscriptionBase : public std::enable_shared_from_this on_new_message_callback_ = nullptr; } + /// Set a callback to be called when each new intra-process message is received. + /** + * The callback receives a size_t which is the number of messages received + * since the last time this callback was called. + * Normally this is 1, but can be > 1 if messages were received before any + * callback was set. + * + * Calling it again will clear any previously set callback. + * + * This function is thread-safe. + * + * If you want more information available in the callback, like the subscription + * or other information, you may use a lambda with captures or std::bind. + * + * \sa rclcpp::SubscriptionIntraProcessBase::set_on_ready_callback + * + * \param[in] callback functor to be called when a new message is received + */ + void + set_on_new_intra_process_message_callback(std::function callback) + { + if (!use_intra_process_) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Calling set_on_new_intra_process_message_callback for subscription with IPC disabled"); + return; + } + + // The on_ready_callback signature has an extra `int` argument used to disambiguate between + // possible different entities within a generic waitable. + // We hide that detail to users of this method. + std::function new_callback = std::bind(callback, std::placeholders::_1); + subscription_intra_process_->set_on_ready_callback(new_callback); + } + + /// Unset the callback registered for new intra-process messages, if any. + void + clear_on_new_intra_process_message_callback() + { + if (!use_intra_process_) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Calling clear_on_new_intra_process_message_callback for subscription with IPC disabled"); + return; + } + + subscription_intra_process_->clear_on_ready_callback(); + } + protected: template void @@ -386,6 +435,7 @@ class SubscriptionBase : public std::enable_shared_from_this bool use_intra_process_; IntraProcessManagerWeakPtr weak_ipm_; uint64_t intra_process_subscription_id_; + std::shared_ptr subscription_intra_process_; private: RCLCPP_DISABLE_COPY(SubscriptionBase) diff --git a/rclcpp/test/rclcpp/test_subscription.cpp b/rclcpp/test/rclcpp/test_subscription.cpp index 761e3bc269..f91ea7921b 100644 --- a/rclcpp/test/rclcpp/test_subscription.cpp +++ b/rclcpp/test/rclcpp/test_subscription.cpp @@ -444,7 +444,7 @@ TEST_F(TestSubscription, handle_loaned_message) { Testing on_new_message callbacks. */ TEST_F(TestSubscription, on_new_message_callback) { - initialize(); + initialize(rclcpp::NodeOptions().use_intra_process_comms(false)); using test_msgs::msg::Empty; auto do_nothing = [](std::shared_ptr) {FAIL();}; @@ -510,6 +510,76 @@ TEST_F(TestSubscription, on_new_message_callback) { 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", 1, 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 */ From d2dab492f1d9e997ab910b19155c3c7b3c26f9a1 Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Fri, 23 Apr 2021 13:36:55 +0100 Subject: [PATCH 69/93] fix minor failures Signed-off-by: Alberto Soragna --- rclcpp/include/rclcpp/subscription.hpp | 4 ++++ rclcpp/include/rclcpp/subscription_base.hpp | 10 ++++++++-- rclcpp/test/rclcpp/test_subscription.cpp | 4 ++-- 3 files changed, 14 insertions(+), 4 deletions(-) diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index a6c71c3c37..cd75aba228 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -171,6 +171,10 @@ class Subscription : public SubscriptionBase // First create a SubscriptionIntraProcess which will be given to the intra-process manager. auto context = node_base->get_context(); + using SubscriptionIntraProcessT = rclcpp::experimental::SubscriptionIntraProcess< + CallbackMessageT, + AllocatorT, + typename MessageUniquePtr::deleter_type>; subscription_intra_process_ = std::make_shared( callback, options.get_allocator(), diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index 9c7c5f6992..db16989cf9 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -374,7 +374,13 @@ class SubscriptionBase : public std::enable_shared_from_this return; } - // The on_ready_callback signature has an extra `int` argument used to disambiguate between + 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); @@ -435,7 +441,7 @@ class SubscriptionBase : public std::enable_shared_from_this bool use_intra_process_; IntraProcessManagerWeakPtr weak_ipm_; uint64_t intra_process_subscription_id_; - std::shared_ptr subscription_intra_process_; + std::shared_ptr subscription_intra_process_; private: RCLCPP_DISABLE_COPY(SubscriptionBase) diff --git a/rclcpp/test/rclcpp/test_subscription.cpp b/rclcpp/test/rclcpp/test_subscription.cpp index f91ea7921b..ddd95931de 100644 --- a/rclcpp/test/rclcpp/test_subscription.cpp +++ b/rclcpp/test/rclcpp/test_subscription.cpp @@ -448,7 +448,7 @@ TEST_F(TestSubscription, on_new_message_callback) { using test_msgs::msg::Empty; auto do_nothing = [](std::shared_ptr) {FAIL();}; - auto sub = node->create_subscription("~/test_take", 1, do_nothing); + auto sub = node->create_subscription("~/test_take", 10, do_nothing); std::atomic c1 {0}; auto increase_c1_cb = [&c1](size_t count_msgs) {c1 += count_msgs;}; @@ -518,7 +518,7 @@ TEST_F(TestSubscription, on_new_intra_process_message_callback) { using test_msgs::msg::Empty; auto do_nothing = [](std::shared_ptr) {FAIL();}; - auto sub = node->create_subscription("~/test_take", 1, do_nothing); + auto sub = node->create_subscription("~/test_take", 10, do_nothing); std::atomic c1 {0}; auto increase_c1_cb = [&c1](size_t count_msgs) {c1 += count_msgs;}; From 4ad66d1aa1cefd0348ef17464bcc08b8cb6a428f Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Fri, 23 Apr 2021 14:48:14 +0100 Subject: [PATCH 70/93] fix typos and errors in comments Signed-off-by: Alberto Soragna --- rclcpp/include/rclcpp/client.hpp | 1 - .../subscription_intra_process_base.hpp | 15 +++++---------- rclcpp/include/rclcpp/qos_event.hpp | 12 ++++-------- rclcpp/include/rclcpp/service.hpp | 3 --- rclcpp/include/rclcpp/waitable.hpp | 2 +- 5 files changed, 10 insertions(+), 23 deletions(-) diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index 6ac59ebd65..cbf132c523 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -173,7 +173,6 @@ class ClientBase * 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 rclcpp::ClientBase::clear_on_new_response_callback * \sa rmw_client_set_on_new_response_callback * \sa rcl_client_set_on_new_response_callback * diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp index 1e0736fd16..8c93a9524b 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp @@ -79,11 +79,11 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable rmw_qos_profile_t get_actual_qos() const; - /// Set a callback to be called when each new event instance occurs. + /// Set a callback to be called when each new message arrives. /** - * The callback receives a size_t which is the number of responses 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 responses were received before any + * 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. @@ -92,7 +92,7 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable * 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 - * bounded to the function. + * 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. * @@ -102,14 +102,9 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable * * This function is thread-safe. * - * If you want more information available in the callback, like the client + * 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. * - * 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::SubscriptionIntraProcessBase::clear_on_ready_callback - * * \param[in] callback functor to be called when a new message is received. */ void diff --git a/rclcpp/include/rclcpp/qos_event.hpp b/rclcpp/include/rclcpp/qos_event.hpp index 4b2c49a742..fbe686629a 100644 --- a/rclcpp/include/rclcpp/qos_event.hpp +++ b/rclcpp/include/rclcpp/qos_event.hpp @@ -113,9 +113,9 @@ class QOSEventHandlerBase : public Waitable /// Set a callback to be called when each new event instance occurs. /** - * The callback receives a size_t which is the number of responses received + * 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 responses were received before any + * Normally this is 1, but can be > 1 if events occurred before any * callback was set. * * The callback also receives an int identifier argument. @@ -124,7 +124,7 @@ class QOSEventHandlerBase : public Waitable * 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 - * bounded to the function. + * 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. * @@ -139,13 +139,9 @@ class QOSEventHandlerBase : public Waitable * * This function is thread-safe. * - * If you want more information available in the callback, like the client + * 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. * - * 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::QOSEventHandlerBase::clear_on_ready_callback * \sa rmw_event_set_callback * \sa rcl_event_set_callback * diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 52e4bdb069..fedf03240f 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -148,13 +148,11 @@ class ServiceBase * 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 rclcpp::ServiceBase::clear_on_new_request_callback * \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 */ - RCLCPP_PUBLIC void set_on_new_request_callback(std::function callback) { @@ -203,7 +201,6 @@ class ServiceBase } /// Unset the callback registered for new requests, if any. - RCLCPP_PUBLIC void clear_on_new_request_callback() { diff --git a/rclcpp/include/rclcpp/waitable.hpp b/rclcpp/include/rclcpp/waitable.hpp index 8e2f2c7226..b8105b7ba4 100644 --- a/rclcpp/include/rclcpp/waitable.hpp +++ b/rclcpp/include/rclcpp/waitable.hpp @@ -217,7 +217,7 @@ class Waitable * 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 - * bounded to the function. + * 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. * From 01c2111280c8e55754894bd692b0541e6160802f Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Fri, 23 Apr 2021 15:18:26 +0100 Subject: [PATCH 71/93] fix comments Signed-off-by: Alberto Soragna --- .../rclcpp/experimental/subscription_intra_process_base.hpp | 1 - rclcpp/include/rclcpp/qos_event.hpp | 2 -- rclcpp/include/rclcpp/waitable.hpp | 4 ++-- 3 files changed, 2 insertions(+), 5 deletions(-) diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp index 8c93a9524b..99270c732c 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp @@ -149,7 +149,6 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable } /// Unset the callback registered for new messages, if any. - RCLCPP_PUBLIC void clear_on_ready_callback() override { diff --git a/rclcpp/include/rclcpp/qos_event.hpp b/rclcpp/include/rclcpp/qos_event.hpp index fbe686629a..078305037c 100644 --- a/rclcpp/include/rclcpp/qos_event.hpp +++ b/rclcpp/include/rclcpp/qos_event.hpp @@ -147,7 +147,6 @@ class QOSEventHandlerBase : public Waitable * * \param[in] callback functor to be called when a new event occurs */ - RCLCPP_PUBLIC void set_on_ready_callback(std::function callback) override { @@ -198,7 +197,6 @@ class QOSEventHandlerBase : public Waitable } /// Unset the callback registered for new events, if any. - RCLCPP_PUBLIC void clear_on_ready_callback() override { diff --git a/rclcpp/include/rclcpp/waitable.hpp b/rclcpp/include/rclcpp/waitable.hpp index b8105b7ba4..8318f84266 100644 --- a/rclcpp/include/rclcpp/waitable.hpp +++ b/rclcpp/include/rclcpp/waitable.hpp @@ -206,9 +206,9 @@ class Waitable /// Set a callback to be called whenever the waitable becomes ready. /** - * The callback receives a size_t which is the number of responses received + * 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 responses were received before any + * 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. From 476238ba29cede1b7907c09fd3a1b92cb176ea54 Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Fri, 23 Apr 2021 19:01:51 +0100 Subject: [PATCH 72/93] expose qos listener APIs from pub and sub; add unit-tests Signed-off-by: Alberto Soragna --- rclcpp/include/rclcpp/publisher_base.hpp | 77 ++++++++++++++++++- rclcpp/include/rclcpp/subscription_base.hpp | 75 +++++++++++++++++- rclcpp/include/rclcpp/wait_set_template.hpp | 6 +- .../rclcpp/node_interfaces/node_topics.cpp | 6 +- rclcpp/src/rclcpp/publisher_base.cpp | 4 +- rclcpp/src/rclcpp/subscription_base.cpp | 7 +- rclcpp/test/rclcpp/test_publisher.cpp | 3 +- rclcpp/test/rclcpp/test_qos_event.cpp | 66 ++++++++++++++++ rclcpp/test/rclcpp/test_wait_set.cpp | 4 +- 9 files changed, 230 insertions(+), 18 deletions(-) diff --git a/rclcpp/include/rclcpp/publisher_base.hpp b/rclcpp/include/rclcpp/publisher_base.hpp index 48ebfea860..ed6df45466 100644 --- a/rclcpp/include/rclcpp/publisher_base.hpp +++ b/rclcpp/include/rclcpp/publisher_base.hpp @@ -23,6 +23,8 @@ #include #include #include +#include +#include #include #include "rcl/publisher.h" @@ -109,9 +111,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 +196,71 @@ 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 + */ + 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. + void + clear_on_new_qos_event_callback(rcl_publisher_event_type_t event_type) + { + if (event_handlers_.count(event_type) == 0) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Calling clear_on_new_qos_event_callback for non registered event_type"); + return; + } + + event_handlers_[event_type]->clear_on_ready_callback(); + } + protected: template void @@ -206,7 +274,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 +284,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/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index db16989cf9..6af76769fe 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -98,9 +98,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. @@ -401,6 +402,71 @@ class SubscriptionBase : public std::enable_shared_from_this 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 + */ + 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. + void + clear_on_new_qos_event_callback(rcl_subscription_event_type_t event_type) + { + if (event_handlers_.count(event_type) == 0) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Calling clear_on_new_qos_event_callback for non registered event_type"); + return; + } + + event_handlers_[event_type]->clear_on_ready_callback(); + } + protected: template void @@ -415,7 +481,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 @@ -436,7 +502,8 @@ class SubscriptionBase : public std::enable_shared_from_this 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_; 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/src/rclcpp/node_interfaces/node_topics.cpp b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp index c57fbcee5d..f6047930e1 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp @@ -55,7 +55,8 @@ 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); } @@ -97,7 +98,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); } diff --git a/rclcpp/src/rclcpp/publisher_base.cpp b/rclcpp/src/rclcpp/publisher_base.cpp index aadee307b3..0f43f7e36c 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" @@ -153,7 +154,8 @@ PublisherBase::get_publisher_handle() const return publisher_handle_; } -const std::vector> & +const +std::unordered_map> & PublisherBase::get_event_handlers() const { return event_handlers_; diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index 7b8d58a505..1827facaca 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" @@ -116,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_; @@ -282,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); } diff --git a/rclcpp/test/rclcpp/test_publisher.cpp b/rclcpp/test/rclcpp/test_publisher.cpp index 7d2c9d14c1..bf80adfc90 100644 --- a/rclcpp/test/rclcpp/test_publisher.cpp +++ b/rclcpp/test/rclcpp/test_publisher.cpp @@ -501,7 +501,8 @@ 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); } diff --git a/rclcpp/test/rclcpp/test_qos_event.cpp b/rclcpp/test/rclcpp/test_qos_event.cpp index 6c94aaff97..9622b81eab 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: @@ -325,3 +327,67 @@ 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); +} diff --git a/rclcpp/test/rclcpp/test_wait_set.cpp b/rclcpp/test/rclcpp/test_wait_set.cpp index bbfc9383c8..6c5ba9373f 100644 --- a/rclcpp/test/rclcpp/test_wait_set.cpp +++ b/rclcpp/test/rclcpp/test_wait_set.cpp @@ -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( { @@ -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}); From 79dba0a5d2be7ca65a1f025c8d2f97a8b9fc03e5 Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Fri, 23 Apr 2021 19:15:27 +0100 Subject: [PATCH 73/93] add qos event unit-test for invalid callbacks Signed-off-by: Alberto Soragna --- rclcpp/test/rclcpp/test_qos_event.cpp | 28 +++++++++++++++++++++++++++ 1 file changed, 28 insertions(+) diff --git a/rclcpp/test/rclcpp/test_qos_event.cpp b/rclcpp/test/rclcpp/test_qos_event.cpp index 9622b81eab..fdc334aa40 100644 --- a/rclcpp/test/rclcpp/test_qos_event.cpp +++ b/rclcpp/test/rclcpp/test_qos_event.cpp @@ -391,3 +391,31 @@ TEST_F(TestQosEvent, test_on_new_event_callback) 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); +} From fba080cf34256243705e45b1eadc1c49057b5896 Mon Sep 17 00:00:00 2001 From: Kaven Yau Date: Sun, 25 Apr 2021 15:32:30 +0800 Subject: [PATCH 74/93] Fix action server deadlock issue that caused by other mutexes locked in CancelCallback (#1635) * Fix deadlock issue that caused by other mutexes locked in CancelCallback Signed-off-by: Kaven Yau * Add unit test for rclcpp action server deadlock Signed-off-by: Kaven Yau * Update rclcpp_action/test/test_server.cpp Co-authored-by: William Woodall Co-authored-by: Kaven Yau Co-authored-by: Jacob Perron Co-authored-by: William Woodall --- .../include/rclcpp_action/server.hpp | 22 +++++++++++-------- rclcpp_action/test/test_server.cpp | 15 ++++++++++++- 2 files changed, 27 insertions(+), 10 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index b3871060cd..adc38c7b96 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -356,16 +356,20 @@ 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) { - goal_handle->_cancel_goal(); - } + if (goal_handle) { + resp = handle_cancel_(goal_handle); + if (CancelResponse::ACCEPT == resp) { + goal_handle->_cancel_goal(); } } return resp; diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index 7ff9838658..af94d67cbf 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -1234,10 +1234,14 @@ class TestDeadlockServer : public TestServer this->TryLockFor(lock, std::chrono::milliseconds(1000)); return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }, - [this](std::shared_ptr) { + [this](std::shared_ptr handle) { // instead of making a deadlock, check if it can acquire the lock in a second std::unique_lock lock(server_mutex_, std::defer_lock); this->TryLockFor(lock, std::chrono::milliseconds(1000)); + // TODO(KavenYau): this check may become obsolete with https://github.com/ros2/rclcpp/issues/1599 + if (!handle->is_active()) { + return rclcpp_action::CancelResponse::REJECT; + } return rclcpp_action::CancelResponse::ACCEPT; }, [this](std::shared_ptr handle) { @@ -1306,3 +1310,12 @@ 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)); + send_cancel_request(node_, uuid1_); + t.join(); +} From e4e56ef985c90d5fc92a21adedebd98b96cdb08f Mon Sep 17 00:00:00 2001 From: Mauro Passerino Date: Wed, 28 Apr 2021 14:36:15 -0300 Subject: [PATCH 75/93] Use QoS depth to limit callbacks count Signed-off-by: Mauro Passerino --- .../experimental/subscription_intra_process_base.hpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp index 99270c732c..702eff2fa8 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp @@ -142,8 +142,12 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable on_new_message_callback_ = new_callback; if (unread_count_ > 0) { - // Use qos profile depth as upper bound for unread_count_ - on_new_message_callback_(std::min(unread_count_, qos_profile_.depth)); + 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; } } From 79c2dd8e8b3e73366c59474b91271a2fd57954bc Mon Sep 17 00:00:00 2001 From: William Woodall Date: Thu, 29 Apr 2021 10:56:16 -0700 Subject: [PATCH 76/93] use dynamic_pointer_cast to detect allocator mismatch in intra process manager (#1643) * use dynamic_pointer_cast to detect allocator mismatch in intra process manager Signed-off-by: William Woodall * add test case for mismatched allocators Signed-off-by: William Woodall * forward template arguments to avoid mismatched types in intra process manager Signed-off-by: William Woodall * style fixes Signed-off-by: William Woodall * refactor to test message address and count, more DRY Signed-off-by: William Woodall * update copyright Signed-off-by: William Woodall * fix typo Signed-off-by: William Woodall Co-authored-by: Michel Hidalgo Co-authored-by: Michel Hidalgo --- .../experimental/intra_process_manager.hpp | 36 ++- rclcpp/test/rclcpp/CMakeLists.txt | 7 + .../rclcpp/test_intra_process_manager.cpp | 5 +- ..._intra_process_manager_with_allocators.cpp | 276 ++++++++++++++++++ 4 files changed, 314 insertions(+), 10 deletions(-) create mode 100644 rclcpp/test/rclcpp/test_intra_process_manager_with_allocators.cpp diff --git a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp index 9d4b62654c..f3990cb76b 100644 --- a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp +++ b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp @@ -202,7 +202,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) { @@ -227,7 +228,7 @@ class IntraProcessManager // for the buffers that do not require ownership auto shared_msg = std::allocate_shared(*allocator, *message); - this->template add_shared_msg_to_buffers( + this->template add_shared_msg_to_buffers( shared_msg, sub_ids.take_shared_subscriptions); this->template add_owned_msg_to_buffers( std::move(message), sub_ids.take_ownership_subscriptions, allocator); @@ -263,7 +264,7 @@ class IntraProcessManager // If there are no owning, just convert to shared. std::shared_ptr shared_msg = std::move(message); if (!sub_ids.take_shared_subscriptions.empty()) { - this->template add_shared_msg_to_buffers( + this->template add_shared_msg_to_buffers( shared_msg, sub_ids.take_shared_subscriptions); } return shared_msg; @@ -273,7 +274,7 @@ class IntraProcessManager auto shared_msg = std::allocate_shared(*allocator, *message); if (!sub_ids.take_shared_subscriptions.empty()) { - this->template add_shared_msg_to_buffers( + this->template add_shared_msg_to_buffers( shared_msg, sub_ids.take_shared_subscriptions); } @@ -350,7 +351,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 +367,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::SubscriptionIntraProcess >(subscription_base); + if (nullptr == subscription) { + throw std::runtime_error( + "failed to dynamic cast SubscriptionIntraProcessBase to " + "SubscriptionIntraProcess, which " + "can happen when the publisher and subscription use different " + "allocator types, which is not supported"); + } subscription->provide_intra_process_message(message); } else { @@ -394,9 +405,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::SubscriptionIntraProcess >(subscription_base); + if (nullptr == subscription) { + throw std::runtime_error( + "failed to dynamic cast SubscriptionIntraProcessBase to " + "SubscriptionIntraProcess, 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 diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index 010a911889..f5206c1d05 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -143,6 +143,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 diff --git a/rclcpp/test/rclcpp/test_intra_process_manager.cpp b/rclcpp/test/rclcpp/test_intra_process_manager.cpp index b71fb1d061..ec7ccc7e5a 100644 --- a/rclcpp/test/rclcpp/test_intra_process_manager.cpp +++ b/rclcpp/test/rclcpp/test_intra_process_manager.cpp @@ -216,7 +216,10 @@ class SubscriptionIntraProcessBase const char * topic_name; }; -template +template< + typename MessageT, + typename Alloc = std::allocator, + typename Deleter = std::default_delete> class SubscriptionIntraProcess : public SubscriptionIntraProcessBase { public: 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..f8cb96a421 --- /dev/null +++ b/rclcpp/test/rclcpp/test_intra_process_manager_with_allocators.cpp @@ -0,0 +1,276 @@ +// 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 "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; + }; +}; + +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, + CallbackMessageT, + 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); +} From 6806cdf825d9fcdbb4bfe2c6e2a74c4cb8bc5e11 Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Sat, 1 May 2021 01:53:19 +0800 Subject: [PATCH 77/93] Use OnShutdown callback handle instead of OnShutdown callback (#1639) 1. Add remove_on_shutdown_callback() in rclcpp::Context Signed-off-by: Barry Xu 2. Add add_on_shutdown_callback(), which returns a handle that can be removed by remove_on_shutdown_callback(). Signed-off-by: Barry Xu --- rclcpp/include/rclcpp/context.hpp | 60 ++++++++++++++++++++++++------ rclcpp/include/rclcpp/executor.hpp | 4 ++ rclcpp/src/rclcpp/context.cpp | 50 ++++++++++++++++++++----- rclcpp/src/rclcpp/executor.cpp | 10 ++++- 4 files changed, 102 insertions(+), 22 deletions(-) diff --git a/rclcpp/include/rclcpp/context.hpp b/rclcpp/include/rclcpp/context.hpp index 9140f3f117..ae6d3d0a99 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. @@ -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 @@ -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/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index bc4e167a1e..13ed108348 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" @@ -571,6 +572,9 @@ class Executor /// nodes that are associated with the executor std::list weak_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + + /// shutdown callback handle registered to Context + rclcpp::OnShutdownCallbackHandle shutdown_callback_handle_; }; } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/context.cpp b/rclcpp/src/rclcpp/context.cpp index 90ea3db908..580689d694 100644 --- a/rclcpp/src/rclcpp/context.cpp +++ b/rclcpp/src/rclcpp/context.cpp @@ -315,9 +315,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 +348,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) { - return on_shutdown_callbacks_; + 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; } -std::vector & -Context::get_on_shutdown_callbacks() +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() const +{ + 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/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 630db50b9b..20876668a4 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -56,7 +56,7 @@ Executor::Executor(const rclcpp::ExecutorOptions & options) 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) { @@ -138,6 +138,14 @@ Executor::~Executor() } // Remove and release the sigint guard condition memory_strategy_->remove_guard_condition(&shutdown_guard_condition_->get_rcl_guard_condition()); + + // Remove shutdown callback handle registered to Context + if (!context_->remove_on_shutdown_callback(shutdown_callback_handle_)) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "failed to remove registered on_shutdown callback"); + rcl_reset_error(); + } } std::vector From d051b8aa20fa263b1480945aebd1a8e5df315b67 Mon Sep 17 00:00:00 2001 From: Kaven Yau Date: Sat, 1 May 2021 04:56:38 +0800 Subject: [PATCH 78/93] Returns CancelResponse::REJECT while goal handle failed to transit to CANCELING state (#1641) Signed-off-by: Kaven Yau --- rclcpp_action/include/rclcpp_action/server.hpp | 9 ++++++++- rclcpp_action/test/test_server.cpp | 11 +++++------ 2 files changed, 13 insertions(+), 7 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index adc38c7b96..f9488fc1d5 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -369,7 +369,14 @@ class Server : public ServerBase, public std::enable_shared_from_this_cancel_goal(); + try { + goal_handle->_cancel_goal(); + } catch (const rclcpp::exceptions::RCLError & ex) { + RCLCPP_DEBUG( + rclcpp::get_logger("rclcpp_action"), + "Failed to cancel goal in call_handle_cancel_callback: %s", ex.what()); + return CancelResponse::REJECT; + } } } return resp; diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index af94d67cbf..1a0b074ba3 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -1234,14 +1234,10 @@ class TestDeadlockServer : public TestServer this->TryLockFor(lock, std::chrono::milliseconds(1000)); return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }, - [this](std::shared_ptr handle) { + [this](std::shared_ptr) { // instead of making a deadlock, check if it can acquire the lock in a second std::unique_lock lock(server_mutex_, std::defer_lock); this->TryLockFor(lock, std::chrono::milliseconds(1000)); - // TODO(KavenYau): this check may become obsolete with https://github.com/ros2/rclcpp/issues/1599 - if (!handle->is_active()) { - return rclcpp_action::CancelResponse::REJECT; - } return rclcpp_action::CancelResponse::ACCEPT; }, [this](std::shared_ptr handle) { @@ -1316,6 +1312,9 @@ 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)); - send_cancel_request(node_, uuid1_); + 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(); } From c15eb1eaac2e05b507164a557cda5ff11d49c813 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Mon, 3 May 2021 15:46:25 -0300 Subject: [PATCH 79/93] Fix get_publishers_subscriptions_info_by_topic test in test_node.cpp (#1648) * Wait on graph guard condition for graph changes to propagate Signed-off-by: Michel Hidalgo --- rclcpp/test/rclcpp/test_node.cpp | 39 +++++++++++++++++++++++++++++++- 1 file changed, 38 insertions(+), 1 deletion(-) diff --git a/rclcpp/test/rclcpp/test_node.cpp b/rclcpp/test/rclcpp/test_node.cpp index e1fb4b8ced..dff05a0667 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 @@ -2525,6 +2527,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 +2586,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); @@ -2596,7 +2630,10 @@ TEST_F(TestNode, get_publishers_subscriptions_info_by_topic) { }; 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); From 1fc2d58799a6d4530522be5c236c70be53455e60 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Tue, 4 May 2021 19:45:15 -0300 Subject: [PATCH 80/93] Keep custom allocator in publisher and subscription options alive. (#1647) * Keep custom allocator in publisher and subscription options alive. Also, enforce an allocator bound to void is used to avoid surprises. Signed-off-by: Michel Hidalgo * Avoid sizeof(void) in MyAllocator implementation. Signed-off-by: Michel Hidalgo * Address peer review comment Signed-off-by: Ivan Santiago Paunovic * Use a lazely initialized private field when 'allocator' is not initialized Signed-off-by: Ivan Santiago Paunovic Co-authored-by: Ivan Santiago Paunovic --- rclcpp/include/rclcpp/publisher_options.hpp | 20 +++++++++++++----- .../include/rclcpp/subscription_options.hpp | 21 +++++++++++++------ ..._intra_process_manager_with_allocators.cpp | 6 +++++- 3 files changed, 35 insertions(+), 12 deletions(-) diff --git a/rclcpp/include/rclcpp/publisher_options.hpp b/rclcpp/include/rclcpp/publisher_options.hpp index 8388f85e47..efaa4b48c8 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" @@ -64,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; @@ -80,10 +85,7 @@ 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 = rclcpp::allocator::get_rcl_allocator(*this->get_allocator()); result.qos = qos.get_rmw_qos_profile(); result.rmw_publisher_options.require_unique_network_flow_endpoints = this->require_unique_network_flow_endpoints; @@ -102,10 +104,18 @@ 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: + // 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_; }; using PublisherOptions = PublisherOptionsWithAllocator>; diff --git a/rclcpp/include/rclcpp/subscription_options.hpp b/rclcpp/include/rclcpp/subscription_options.hpp index d9d4085d85..373850e00b 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" @@ -86,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; @@ -107,10 +112,7 @@ 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 = allocator::get_rcl_allocator(*this->get_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 = @@ -124,15 +126,22 @@ 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: + // 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_; }; using SubscriptionOptions = SubscriptionOptionsWithAllocator>; diff --git a/rclcpp/test/rclcpp/test_intra_process_manager_with_allocators.cpp b/rclcpp/test/rclcpp/test_intra_process_manager_with_allocators.cpp index f8cb96a421..8443706126 100644 --- a/rclcpp/test/rclcpp/test_intra_process_manager_with_allocators.cpp +++ b/rclcpp/test/rclcpp/test_intra_process_manager_with_allocators.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include "test_msgs/msg/empty.hpp" @@ -57,7 +58,10 @@ struct MyAllocator return nullptr; } num_allocs++; - return static_cast(std::malloc(size * sizeof(T))); + // Use sizeof(char) in place for sizeof(void) + constexpr size_t value_size = sizeof( + typename std::conditional::value, T, char>::type); + return static_cast(std::malloc(size * value_size)); } void deallocate(T * ptr, size_t size) From 0659d829cec5a39c6ad861d597b4aa9aee224a25 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Wed, 5 May 2021 17:17:42 -0300 Subject: [PATCH 81/93] Do not attempt to use void allocators for memory allocation. (#1657) Keep a rebound allocator for byte-sized memory blocks around for publisher and subscription options. Follow-up after 1fc2d58799a6d4530522be5c236c70be53455e60 Signed-off-by: Michel Hidalgo --- rclcpp/include/rclcpp/publisher_options.hpp | 19 ++++++++++- .../include/rclcpp/subscription_options.hpp | 19 ++++++++++- ..._intra_process_manager_with_allocators.cpp | 34 ++++++++++++++++--- 3 files changed, 65 insertions(+), 7 deletions(-) diff --git a/rclcpp/include/rclcpp/publisher_options.hpp b/rclcpp/include/rclcpp/publisher_options.hpp index efaa4b48c8..48250307e9 100644 --- a/rclcpp/include/rclcpp/publisher_options.hpp +++ b/rclcpp/include/rclcpp/publisher_options.hpp @@ -85,7 +85,7 @@ struct PublisherOptionsWithAllocator : public PublisherOptionsBase to_rcl_publisher_options(const rclcpp::QoS & qos) const { rcl_publisher_options_t result = rcl_publisher_get_default_options(); - result.allocator = rclcpp::allocator::get_rcl_allocator(*this->get_allocator()); + 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; @@ -113,9 +113,26 @@ struct PublisherOptionsWithAllocator : public PublisherOptionsBase } 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/subscription_options.hpp b/rclcpp/include/rclcpp/subscription_options.hpp index 373850e00b..d6d2d4c60d 100644 --- a/rclcpp/include/rclcpp/subscription_options.hpp +++ b/rclcpp/include/rclcpp/subscription_options.hpp @@ -112,7 +112,7 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase to_rcl_subscription_options(const rclcpp::QoS & qos) const { rcl_subscription_options_t result = rcl_subscription_get_default_options(); - result.allocator = allocator::get_rcl_allocator(*this->get_allocator()); + 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 = @@ -139,9 +139,26 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase } 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/test/rclcpp/test_intra_process_manager_with_allocators.cpp b/rclcpp/test/rclcpp/test_intra_process_manager_with_allocators.cpp index 8443706126..9bef3c2c0f 100644 --- a/rclcpp/test/rclcpp/test_intra_process_manager_with_allocators.cpp +++ b/rclcpp/test/rclcpp/test_intra_process_manager_with_allocators.cpp @@ -31,7 +31,7 @@ static uint32_t num_allocs = 0; static uint32_t num_deallocs = 0; // A very simple custom allocator. Counts calls to allocate and deallocate. -template +template struct MyAllocator { public: @@ -58,10 +58,7 @@ struct MyAllocator return nullptr; } num_allocs++; - // Use sizeof(char) in place for sizeof(void) - constexpr size_t value_size = sizeof( - typename std::conditional::value, T, char>::type); - return static_cast(std::malloc(size * value_size)); + return static_cast(std::malloc(size * sizeof(T))); } void deallocate(T * ptr, size_t size) @@ -81,6 +78,33 @@ struct MyAllocator }; }; +// 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 &, From 2e4c97ac5616fb024ff6fc6544742be3ba77c6b1 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Thu, 6 May 2021 21:33:06 -0700 Subject: [PATCH 82/93] [rclcpp] Type Adaptation feature (#1557) * initial version of type_adaptor.hpp Signed-off-by: William Woodall * initial version of rclcpp::get_message_type_support_handle() Signed-off-by: William Woodall * initial version of rclcpp::is_ros_compatible_type check Signed-off-by: William Woodall * fixup include statement order in publisher.hpp Signed-off-by: William Woodall * use new rclcpp::get_message_type_support_handle() and check in Publisher Signed-off-by: William Woodall * update adaptor->adapter, update TypeAdapter to use two arguments, add implicit default Signed-off-by: William Woodall * move away from shared_ptr to just allocator, like the STL Signed-off-by: William Woodall * fixes to TypeAdapter and adding new publish function signatures Signed-off-by: William Woodall * bugfixes Signed-off-by: William Woodall * more bugfixes Signed-off-by: William Woodall * Add nullptr check Signed-off-by: Audrow Nash * Remove public from struct inheritance Signed-off-by: Audrow Nash * Add tests for publisher with type adapter Signed-off-by: Audrow Nash * Update packages to C++17 Signed-off-by: Audrow Nash * Revert "Update packages to C++17" This reverts commit 4585605223639bbd9d18053e5ef39725638512b4. Signed-off-by: William Woodall * Begin updating AnySubscriptionCallback to use the TypeAdapter Signed-off-by: Audrow Nash * Use type adapter's custom type Signed-off-by: Audrow Nash * Correct which AnySubscriptionCallbackHelper is selected Signed-off-by: Audrow Nash * Setup dispatch function to work with adapted types Signed-off-by: Audrow Nash * Improve template logic on dispatch methods Signed-off-by: Audrow Nash * implement TypeAdapter for Subscription Signed-off-by: William Woodall * Add intraprocess tests with all supported message types Signed-off-by: Audrow Nash * Add intra process tests Signed-off-by: Audrow Nash * Add tests for subscription with type adapter Signed-off-by: Audrow Nash * Fix null allocator test Signed-off-by: Audrow Nash * Handle serialized message correctly Signed-off-by: Audrow Nash * Fix generic subscription Signed-off-by: Audrow Nash * Fix trailing space Signed-off-by: Audrow Nash * fix some issues found while testing type_adapter in demos Signed-off-by: William Woodall * add more tests, WIP Signed-off-by: William Woodall * Improve pub/sub tests Signed-off-by: Audrow Nash * Apply uncrustify formatting Signed-off-by: Audrow Nash * finish new tests for any subscription callback with type adapter Signed-off-by: William Woodall * fix adapt_type<...>::as<...> syntax Signed-off-by: William Woodall * fix explicit template instantiation of create_subscription() in new test Signed-off-by: William Woodall * cpplint fix Signed-off-by: William Woodall * Fix bug by aligning allocator types on both sides of ipm Signed-off-by: Audrow Nash * Fix intra process manager tests Signed-off-by: Audrow Nash Co-authored-by: Audrow Nash --- .../rclcpp/any_subscription_callback.hpp | 730 +++++++++++++++--- rclcpp/include/rclcpp/create_subscription.hpp | 33 +- .../experimental/intra_process_manager.hpp | 23 +- .../subscription_intra_process.hpp | 111 +-- .../subscription_intra_process_buffer.hpp | 142 ++++ .../include/rclcpp/generic_subscription.hpp | 7 + .../get_message_type_support_handle.hpp | 90 +++ .../include/rclcpp/is_ros_compatible_type.hpp | 35 + rclcpp/include/rclcpp/loaned_message.hpp | 3 + rclcpp/include/rclcpp/node.hpp | 9 +- rclcpp/include/rclcpp/node_impl.hpp | 1 - rclcpp/include/rclcpp/publisher.hpp | 257 +++++- rclcpp/include/rclcpp/serialized_message.hpp | 2 +- rclcpp/include/rclcpp/subscription.hpp | 105 ++- rclcpp/include/rclcpp/subscription_base.hpp | 7 + .../include/rclcpp/subscription_factory.hpp | 20 +- rclcpp/include/rclcpp/type_adapter.hpp | 197 +++++ rclcpp/src/rclcpp/executor.cpp | 3 +- rclcpp/src/rclcpp/generic_subscription.cpp | 16 +- rclcpp/test/msg/String.msg | 1 + rclcpp/test/rclcpp/CMakeLists.txt | 44 +- .../node_interfaces/test_node_topics.cpp | 2 + .../rclcpp/test_any_subscription_callback.cpp | 342 ++++++-- .../rclcpp/test_intra_process_manager.cpp | 34 +- ..._intra_process_manager_with_allocators.cpp | 1 - rclcpp/test/rclcpp/test_loaned_message.cpp | 37 +- .../test_publisher_with_type_adapter.cpp | 335 ++++++++ rclcpp/test/rclcpp/test_subscription.cpp | 2 +- .../test_subscription_with_type_adapter.cpp | 563 ++++++++++++++ .../rclcpp_lifecycle/lifecycle_node.hpp | 7 +- .../rclcpp_lifecycle/lifecycle_node_impl.hpp | 1 - 31 files changed, 2792 insertions(+), 368 deletions(-) create mode 100644 rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp create mode 100644 rclcpp/include/rclcpp/get_message_type_support_handle.hpp create mode 100644 rclcpp/include/rclcpp/is_ros_compatible_type.hpp create mode 100644 rclcpp/include/rclcpp/type_adapter.hpp create mode 100644 rclcpp/test/msg/String.msg create mode 100644 rclcpp/test/rclcpp/test_publisher_with_type_adapter.cpp create mode 100644 rclcpp/test/rclcpp/test_subscription_with_type_adapter.cpp diff --git a/rclcpp/include/rclcpp/any_subscription_callback.hpp b/rclcpp/include/rclcpp/any_subscription_callback.hpp index 61b5e2e82e..bb45d6a07d 100644 --- a/rclcpp/include/rclcpp/any_subscription_callback.hpp +++ b/rclcpp/include/rclcpp/any_subscription_callback.hpp @@ -22,6 +22,7 @@ #include #include // NOLINT[build/include_order] +#include "rosidl_runtime_cpp/traits.hpp" #include "tracetools/tracetools.h" #include "tracetools/utils.hpp" @@ -29,6 +30,9 @@ #include "rclcpp/detail/subscription_callback_type_helper.hpp" #include "rclcpp/function_traits.hpp" #include "rclcpp/message_info.hpp" +#include "rclcpp/serialized_message.hpp" +#include "rclcpp/type_adapter.hpp" + template inline constexpr bool always_false_v = false; @@ -42,53 +46,189 @@ namespace detail template struct MessageDeleterHelper { - using MessageAllocTraits = allocator::AllocRebind; - using MessageAlloc = typename MessageAllocTraits::allocator_type; - using MessageDeleter = allocator::Deleter; + using AllocTraits = allocator::AllocRebind; + using Alloc = typename AllocTraits::allocator_type; + using Deleter = allocator::Deleter; }; +/// Struct which contains all possible callback signatures, with or without a TypeAdapter.s template -struct AnySubscriptionCallbackHelper +struct AnySubscriptionCallbackPossibleTypes { - using MessageDeleter = typename MessageDeleterHelper::MessageDeleter; + /// 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; + std::function; + using ConstRefROSMessageCallback = + std::function; using ConstRefWithInfoCallback = - std::function; + std::function; + using ConstRefWithInfoROSMessageCallback = + std::function; + using ConstRefSerializedMessageCallback = + std::function; + using ConstRefSerializedMessageWithInfoCallback = + std::function; using UniquePtrCallback = - std::function)>; + std::function)>; + using UniquePtrROSMessageCallback = + std::function)>; using UniquePtrWithInfoCallback = - std::function, const rclcpp::MessageInfo &)>; + 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)>; + std::function)>; + using SharedConstPtrROSMessageCallback = + std::function)>; using SharedConstPtrWithInfoCallback = - std::function, const rclcpp::MessageInfo &)>; + 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 &)>; + std::function &)>; + using ConstRefSharedConstPtrROSMessageCallback = + std::function &)>; using ConstRefSharedConstPtrWithInfoCallback = - std::function &, const rclcpp::MessageInfo &)>; + 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)>; + std::function)>; + using SharedPtrROSMessageCallback = + std::function)>; using SharedPtrWithInfoCallback = - std::function, const rclcpp::MessageInfo &)>; + 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< - ConstRefCallback, - ConstRefWithInfoCallback, - UniquePtrCallback, - UniquePtrWithInfoCallback, - SharedConstPtrCallback, - SharedConstPtrWithInfoCallback, - ConstRefSharedConstPtrCallback, - ConstRefSharedConstPtrWithInfoCallback, - SharedPtrCallback, - SharedPtrWithInfoCallback + 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 >; }; @@ -101,49 +241,124 @@ template< 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 MessageDeleterHelper = rclcpp::detail::MessageDeleterHelper; - using MessageAllocTraits = typename MessageDeleterHelper::MessageAllocTraits; - using MessageAlloc = typename MessageDeleterHelper::MessageAlloc; - using MessageDeleter = typename MessageDeleterHelper::MessageDeleter; + using SubscribedTypeDeleterHelper = + rclcpp::detail::MessageDeleterHelper; + using SubscribedTypeAllocatorTraits = typename SubscribedTypeDeleterHelper::AllocTraits; + using SubscribedTypeAllocator = typename SubscribedTypeDeleterHelper::Alloc; + using SubscribedTypeDeleter = typename SubscribedTypeDeleterHelper::Deleter; - // See AnySubscriptionCallbackHelper for the types of these. - using ConstRefCallback = typename HelperT::ConstRefCallback; - using ConstRefWithInfoCallback = typename HelperT::ConstRefWithInfoCallback; + using ROSMessageTypeDeleterHelper = + rclcpp::detail::MessageDeleterHelper; + using ROSMessageTypeAllocatorTraits = typename ROSMessageTypeDeleterHelper::AllocTraits; + using ROSMessageTypeAllocator = typename ROSMessageTypeDeleterHelper::Alloc; + using ROSMessageTypeDeleter = typename ROSMessageTypeDeleterHelper::Deleter; - using UniquePtrCallback = typename HelperT::UniquePtrCallback; - using UniquePtrWithInfoCallback = typename HelperT::UniquePtrWithInfoCallback; + using SerializedMessageDeleterHelper = + rclcpp::detail::MessageDeleterHelper; + using SerializedMessageAllocatorTraits = typename SerializedMessageDeleterHelper::AllocTraits; + using SerializedMessageAllocator = typename SerializedMessageDeleterHelper::Alloc; + using SerializedMessageDeleter = typename SerializedMessageDeleterHelper::Deleter; - using SharedConstPtrCallback = typename HelperT::SharedConstPtrCallback; - using SharedConstPtrWithInfoCallback = typename HelperT::SharedConstPtrWithInfoCallback; + // 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 = + 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 HelperT::ConstRefSharedConstPtrCallback; + typename CallbackTypes::ConstRefSharedConstPtrCallback; + using ConstRefSharedConstPtrROSMessageCallback = + typename CallbackTypes::ConstRefSharedConstPtrROSMessageCallback; using ConstRefSharedConstPtrWithInfoCallback = - typename HelperT::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); + } + } - using SharedPtrCallback = typename HelperT::SharedPtrCallback; - using SharedPtrWithInfoCallback = typename HelperT::SharedPtrWithInfoCallback; + const T * pointer; + }; public: explicit AnySubscriptionCallback(const AllocatorT & allocator = AllocatorT()) // NOLINT[runtime/explicit] + : subscribed_type_allocator_(allocator), + ros_message_type_allocator_(allocator) { - message_allocator_ = allocator; - allocator::set_allocator_for_deleter(&message_deleter_, &message_allocator_); + 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] - { - if (allocator == nullptr) { - throw std::runtime_error("invalid allocator"); - } - message_allocator_ = *allocator; - allocator::set_allocator_for_deleter(&message_deleter_, &message_allocator_); - } + : AnySubscriptionCallback(*NotNull(allocator.get(), "invalid allocator").pointer) + {} AnySubscriptionCallback(const AnySubscriptionCallback &) = default; @@ -190,40 +405,79 @@ class AnySubscriptionCallback } /// 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) - // set(CallbackT callback) + set_deprecated(std::function)> callback) { callback_variant_ = callback; } /// Function for shared_ptr to non-const MessageT with MessageInfo, which is deprecated. + template // TODO(wjwwood): enable this deprecation after Galactic // [[deprecated( // "use 'void (std::shared_ptr, const rclcpp::MessageInfo &)' instead" // )]] void - set_deprecated( - std::function, const rclcpp::MessageInfo &)> callback) + set_deprecated(std::function, const rclcpp::MessageInfo &)> callback) { callback_variant_ = callback; } - std::unique_ptr - create_unique_ptr_from_shared_ptr_message(const std::shared_ptr & message) + std::unique_ptr + create_ros_unique_ptr_from_ros_shared_ptr_message( + const std::shared_ptr & message) { - auto ptr = MessageAllocTraits::allocate(message_allocator_, 1); - MessageAllocTraits::construct(message_allocator_, ptr, *message); - return std::unique_ptr(ptr, message_deleter_); + auto ptr = ROSMessageTypeAllocatorTraits::allocate(ros_message_type_allocator_, 1); + ROSMessageTypeAllocatorTraits::construct(ros_message_type_allocator_, ptr, *message); + return std::unique_ptr(ptr, ros_message_type_deleter_); } + std::unique_ptr + create_serialized_message_unique_ptr_from_shared_ptr( + const std::shared_ptr & serialized_message) + { + auto ptr = SerializedMessageAllocatorTraits::allocate(serialized_message_allocator_, 1); + SerializedMessageAllocatorTraits::construct( + serialized_message_allocator_, ptr, *serialized_message); + return std::unique_ptr< + rclcpp::SerializedMessage, + SerializedMessageDeleter + >(ptr, serialized_message_deleter_); + } + + std::unique_ptr + create_custom_unique_ptr_from_custom_shared_ptr_message( + const std::shared_ptr & message) + { + auto ptr = SubscribedTypeAllocatorTraits::allocate(subscribed_type_allocator_, 1); + SubscribedTypeAllocatorTraits::construct(subscribed_type_allocator_, ptr, *message); + return std::unique_ptr(ptr, subscribed_type_deleter_); + } + + std::unique_ptr + convert_ros_message_to_custom_type_unique_ptr(const ROSMessageType & msg) + { + if constexpr (rclcpp::TypeAdapter::is_specialized::value) { + auto ptr = SubscribedTypeAllocatorTraits::allocate(subscribed_type_allocator_, 1); + SubscribedTypeAllocatorTraits::construct(subscribed_type_allocator_, ptr); + rclcpp::TypeAdapter::convert_to_custom(msg, *ptr); + return std::unique_ptr(ptr, subscribed_type_deleter_); + } else { + throw std::runtime_error( + "convert_ros_message_to_custom_type_unique_ptr " + "unexpectedly called without TypeAdapter"); + } + } + + // Dispatch when input is a ros message and the output could be anything. void dispatch( - std::shared_ptr message, + std::shared_ptr message, const rclcpp::MessageInfo & message_info) { TRACEPOINT(callback_start, static_cast(this), false); @@ -238,28 +492,162 @@ class AnySubscriptionCallback std::visit( [&message, &message_info, this](auto && callback) { using T = std::decay_t; - - if constexpr (std::is_same_v) { + 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) { + } else if constexpr (std::is_same_v) { callback(*message, message_info); - } else if constexpr (std::is_same_v) { - callback(create_unique_ptr_from_shared_ptr_message(message)); - } else if constexpr (std::is_same_v) { - callback(create_unique_ptr_from_shared_ptr_message(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) + 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)); + } + + // 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), false); + // Check if the variant is "unset", throw if it is. + if (callback_variant_.index() == 0) { + if (std::get<0>(callback_variant_) == nullptr) { + // This can happen if it is default initialized, or if it is assigned nullptr. + throw std::runtime_error("dispatch called on an unset AnySubscriptionCallback"); + } + } + // Dispatch. + std::visit( + [&serialized_message, &message_info, this](auto && callback) { + using T = std::decay_t; + + // condition to catch SerializedMessage types + if constexpr (std::is_same_v) { + callback(*serialized_message); + } else if constexpr (std::is_same_v) { + callback(*serialized_message, message_info); + } else if constexpr (std::is_same_v) { + callback(create_serialized_message_unique_ptr_from_shared_ptr(serialized_message)); + } else if constexpr (std::is_same_v) { + callback( + create_serialized_message_unique_ptr_from_shared_ptr(serialized_message), + message_info); + } else if constexpr ( // NOLINT[readability/braces] + std::is_same_v|| + std::is_same_v|| + std::is_same_v) + { + callback(create_serialized_message_unique_ptr_from_shared_ptr(serialized_message)); + } else if constexpr ( // NOLINT[readability/braces] + std::is_same_v|| + std::is_same_v|| + std::is_same_v) + { + callback( + create_serialized_message_unique_ptr_from_shared_ptr(serialized_message), + message_info); + } + // conditions for output anything else + else if constexpr ( // NOLINT[whitespace/newline] + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| std::is_same_v|| - std::is_same_v) + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v|| + std::is_same_v) { - callback(message, message_info); - } else { + 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_); @@ -268,7 +656,7 @@ class AnySubscriptionCallback void dispatch_intra_process( - std::shared_ptr message, + std::shared_ptr message, const rclcpp::MessageInfo & message_info) { TRACEPOINT(callback_start, static_cast(this), true); @@ -283,32 +671,89 @@ class AnySubscriptionCallback std::visit( [&message, &message_info, this](auto && callback) { using T = std::decay_t; - - if constexpr (std::is_same_v) { + 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) { + } 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) + std::is_same_v|| + std::is_same_v) { - callback(create_unique_ptr_from_shared_ptr_message(message)); + 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) + std::is_same_v|| + std::is_same_v) { - callback(create_unique_ptr_from_shared_ptr_message(message), message_info); + 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|| + std::is_same_v) { callback(message); } else if constexpr ( // NOLINT[readability/braces] - std::is_same_v|| - std::is_same_v) + std::is_same_v|| + std::is_same_v) { callback(message, message_info); - } else { + } + // 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_); @@ -317,7 +762,7 @@ class AnySubscriptionCallback void dispatch_intra_process( - std::unique_ptr message, + std::unique_ptr message, const rclcpp::MessageInfo & message_info) { TRACEPOINT(callback_start, static_cast(this), true); @@ -330,28 +775,91 @@ class AnySubscriptionCallback } // Dispatch. std::visit( - [&message, &message_info](auto && callback) { + [&message, &message_info, this](auto && callback) { using T = std::decay_t; - - if constexpr (std::is_same_v) { + 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) { + } 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|| - std::is_same_v|| - std::is_same_v) + 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|| - std::is_same_v|| - std::is_same_v) + std::is_same_v|| + std::is_same_v) { callback(std::move(message), message_info); - } else { + } 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_); @@ -369,6 +877,18 @@ class AnySubscriptionCallback std::holds_alternative(callback_variant_); } + constexpr + bool + is_serialized_message_callback() const + { + return + std::holds_alternative(callback_variant_) || + std::holds_alternative(callback_variant_) || + std::holds_alternative(callback_variant_) || + std::holds_alternative(callback_variant_) || + std::holds_alternative(callback_variant_); + } + void register_callback_for_tracing() { @@ -402,8 +922,12 @@ class AnySubscriptionCallback // For now, compose the variant into this class as a private attribute. typename HelperT::variant_type callback_variant_; - MessageAlloc message_allocator_; - MessageDeleter message_deleter_; + 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/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/experimental/intra_process_manager.hpp b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp index f3990cb76b..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; @@ -226,7 +227,7 @@ 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( shared_msg, sub_ids.take_shared_subscriptions); @@ -243,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; @@ -271,7 +272,7 @@ class IntraProcessManager } 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( @@ -368,12 +369,12 @@ class IntraProcessManager auto subscription_base = subscription_it->second.subscription.lock(); if (subscription_base) { auto subscription = std::dynamic_pointer_cast< - rclcpp::experimental::SubscriptionIntraProcess + rclcpp::experimental::SubscriptionIntraProcessBuffer >(subscription_base); if (nullptr == subscription) { throw std::runtime_error( "failed to dynamic cast SubscriptionIntraProcessBase to " - "SubscriptionIntraProcess, which " + "SubscriptionIntraProcessBuffer, which " "can happen when the publisher and subscription use different " "allocator types, which is not supported"); } @@ -393,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; @@ -406,12 +407,12 @@ class IntraProcessManager auto subscription_base = subscription_it->second.subscription.lock(); if (subscription_base) { auto subscription = std::dynamic_pointer_cast< - rclcpp::experimental::SubscriptionIntraProcess + rclcpp::experimental::SubscriptionIntraProcessBuffer >(subscription_base); if (nullptr == subscription) { throw std::runtime_error( "failed to dynamic cast SubscriptionIntraProcessBase to " - "SubscriptionIntraProcess, which " + "SubscriptionIntraProcessBuffer, which " "can happen when the publisher and subscription use different " "allocator types, which is not supported"); } @@ -423,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_buffer.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp new file mode 100644 index 0000000000..b58c2853ca --- /dev/null +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp @@ -0,0 +1,142 @@ +// 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(topic_name, qos_profile) + { + // 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( + "SubscriptionIntraProcessBuffer init error initializing guard condition"); + } + } + + virtual ~SubscriptionIntraProcessBuffer() + { + 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(); + } + + 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(); + } + +protected: + void + trigger_guard_condition() + { + rcl_ret_t ret = rcl_trigger_guard_condition(&gc_); + (void)ret; + } + + BufferUniquePtr buffer_; +}; + +} // namespace experimental +} // namespace rclcpp + +#endif // RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BUFFER_HPP_ diff --git a/rclcpp/include/rclcpp/generic_subscription.hpp b/rclcpp/include/rclcpp/generic_subscription.hpp index d9f40d868b..673712eedb 100644 --- a/rclcpp/include/rclcpp/generic_subscription.hpp +++ b/rclcpp/include/rclcpp/generic_subscription.hpp @@ -136,6 +136,13 @@ class GenericSubscription : public rclcpp::SubscriptionBase 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( 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/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/node.hpp b/rclcpp/include/rclcpp/node.hpp index b2156a0068..2e44c6c60b 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -206,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( diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 48e2430541..df6039cf49 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -86,7 +86,6 @@ template< typename MessageT, typename CallbackT, typename AllocatorT, - typename CallbackMessageT, typename SubscriptionT, typename MessageMemoryStrategyT> std::shared_ptr diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index f3fbc34faa..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,16 +24,21 @@ #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" @@ -49,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) @@ -80,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( @@ -170,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); @@ -207,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_) { @@ -218,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) { @@ -245,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"); @@ -271,15 +415,28 @@ 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, @@ -316,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); @@ -336,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) { @@ -347,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) { @@ -365,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. @@ -378,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/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/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 b0642bb69d..7653292b5e 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -183,6 +183,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 diff --git a/rclcpp/include/rclcpp/subscription_factory.hpp b/rclcpp/include/rclcpp/subscription_factory.hpp index 7708f27df0..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/type_adapter.hpp b/rclcpp/include/rclcpp/type_adapter.hpp new file mode 100644 index 0000000000..0b779c8062 --- /dev/null +++ b/rclcpp/include/rclcpp/type_adapter.hpp @@ -0,0 +1,197 @@ +// 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. +/** + * \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/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 20876668a4..ea08204798 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -589,8 +589,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()) { diff --git a/rclcpp/src/rclcpp/generic_subscription.cpp b/rclcpp/src/rclcpp/generic_subscription.cpp index d72d21a0f9..cc50955773 100644 --- a/rclcpp/src/rclcpp/generic_subscription.cpp +++ b/rclcpp/src/rclcpp/generic_subscription.cpp @@ -36,11 +36,19 @@ std::shared_ptr GenericSubscription::create_serialize } void GenericSubscription::handle_message( - std::shared_ptr & message, const rclcpp::MessageInfo & message_info) + std::shared_ptr &, + const rclcpp::MessageInfo &) { - (void) message_info; - auto typed_message = std::static_pointer_cast(message); - callback_(typed_message); + 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( 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 f5206c1d05..610c2d68e0 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 @@ -355,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 @@ -507,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" diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp index 16f1d92b86..bb59c83bb2 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 {} }; diff --git a/rclcpp/test/rclcpp/test_any_subscription_callback.cpp b/rclcpp/test/rclcpp/test_any_subscription_callback.cpp index b8d7fd9ba3..8e44eb71bf 100644 --- a/rclcpp/test/rclcpp/test_any_subscription_callback.cpp +++ b/rclcpp/test/rclcpp/test_any_subscription_callback.cpp @@ -67,7 +67,7 @@ void construct_with_null_allocator() TEST(AnySubscriptionCallback, null_allocator) { EXPECT_THROW( construct_with_null_allocator(), - std::runtime_error); + std::invalid_argument); } TEST_F(TestAnySubscriptionCallback, construct_destruct) { @@ -95,44 +95,69 @@ TEST_F(TestAnySubscriptionCallback, unset_dispatch_throw) { // Parameterized test to test across all callback types and dispatch types. // +// Type adapter to be used in tests. +struct MyEmpty {}; + +template<> +struct rclcpp::TypeAdapter +{ + using is_specialized = std::true_type; + using custom_type = MyEmpty; + using ros_message_type = test_msgs::msg::Empty; + + static + void + convert_to_ros_message(const custom_type &, ros_message_type &) + {} + + static + void + convert_to_custom(const ros_message_type &, custom_type &) + {} +}; + +using MyTA = rclcpp::TypeAdapter; + +template class InstanceContextImpl { public: InstanceContextImpl() = default; virtual ~InstanceContextImpl() = default; - explicit InstanceContextImpl(rclcpp::AnySubscriptionCallback asc) + explicit InstanceContextImpl(rclcpp::AnySubscriptionCallback asc) : any_subscription_callback_(asc) {} virtual - rclcpp::AnySubscriptionCallback + rclcpp::AnySubscriptionCallback get_any_subscription_callback_to_test() const { return any_subscription_callback_; } protected: - rclcpp::AnySubscriptionCallback any_subscription_callback_; + rclcpp::AnySubscriptionCallback any_subscription_callback_; }; +template class InstanceContext { public: - InstanceContext(const std::string & name, std::shared_ptr impl) + InstanceContext(const std::string & name, std::shared_ptr> impl) : name(name), impl_(impl) {} InstanceContext( const std::string & name, - rclcpp::AnySubscriptionCallback asc) - : name(name), impl_(std::make_shared(asc)) + rclcpp::AnySubscriptionCallback asc) + : name(name), impl_(std::make_shared>(asc)) {} InstanceContext(const InstanceContext & other) : InstanceContext(other.name, other.impl_) {} - rclcpp::AnySubscriptionCallback + rclcpp::AnySubscriptionCallback get_any_subscription_callback_to_test() const { return impl_->get_any_subscription_callback_to_test(); @@ -141,12 +166,17 @@ class InstanceContext std::string name; protected: - std::shared_ptr impl_; + std::shared_ptr> impl_; }; class DispatchTests : public TestAnySubscriptionCallback, - public ::testing::WithParamInterface + public ::testing::WithParamInterface> +{}; + +class DispatchTestsWithTA + : public TestAnySubscriptionCallback, + public ::testing::WithParamInterface> {}; auto @@ -155,57 +185,67 @@ format_parameter(const ::testing::TestParamInfo & info return info.param.name; } -// Testing dispatch with shared_ptr as input -TEST_P(DispatchTests, test_inter_shared_dispatch) { - auto any_subscription_callback_to_test = GetParam().get_any_subscription_callback_to_test(); - any_subscription_callback_to_test.dispatch(msg_shared_ptr_, message_info_); +auto +format_parameter_with_ta(const ::testing::TestParamInfo & info) +{ + return info.param.name; } -// Testing dispatch with shared_ptr as input -TEST_P(DispatchTests, test_intra_shared_dispatch) { - auto any_subscription_callback_to_test = GetParam().get_any_subscription_callback_to_test(); - any_subscription_callback_to_test.dispatch_intra_process(msg_shared_ptr_, message_info_); -} +#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_); \ + } -// Testing dispatch with unique_ptr as input -TEST_P(DispatchTests, test_intra_unique_dispatch) { - auto any_subscription_callback_to_test = GetParam().get_any_subscription_callback_to_test(); - any_subscription_callback_to_test.dispatch_intra_process(get_unique_ptr_msg(), message_info_); -} +PARAMETERIZED_TESTS(DispatchTests) +PARAMETERIZED_TESTS(DispatchTestsWithTA) // Generic classes for testing callbacks using std::bind to class methods. -template -class BindContextImpl : public InstanceContextImpl +template +class BindContextImpl : public InstanceContextImpl { static constexpr size_t number_of_callback_args{sizeof...(CallbackArgs)}; public: - using InstanceContextImpl::InstanceContextImpl; + using InstanceContextImpl::InstanceContextImpl; virtual ~BindContextImpl() = default; void on_message(CallbackArgs ...) const {} - rclcpp::AnySubscriptionCallback + rclcpp::AnySubscriptionCallback get_any_subscription_callback_to_test() const override { if constexpr (number_of_callback_args == 1) { - return rclcpp::AnySubscriptionCallback().set( + return rclcpp::AnySubscriptionCallback().set( std::bind(&BindContextImpl::on_message, this, std::placeholders::_1) ); } else { - return rclcpp::AnySubscriptionCallback().set( + return rclcpp::AnySubscriptionCallback().set( std::bind(&BindContextImpl::on_message, this, std::placeholders::_1, std::placeholders::_2) ); } } }; -template -class BindContext : public InstanceContext +template +class BindContext : public InstanceContext { public: explicit BindContext(const std::string & name) - : InstanceContext(name, std::make_shared>()) + : InstanceContext(name, std::make_shared>()) {} }; @@ -232,13 +272,53 @@ INSTANTIATE_TEST_SUITE_P( rclcpp::AnySubscriptionCallback().set( const_ref_w_info_free_func)}, // bind function - BindContext("bind_method"), - BindContext( + BindContext("bind_method"), + BindContext( "bind_method_with_info") ), format_parameter ); +void const_ref_ta_free_func(const MyEmpty &) {} +void const_ref_ta_w_info_free_func(const MyEmpty &, const rclcpp::MessageInfo &) {} + +INSTANTIATE_TEST_SUITE_P( + ConstRefTACallbackTests, + DispatchTestsWithTA, + ::testing::Values( + // lambda + InstanceContext{"lambda_ta", rclcpp::AnySubscriptionCallback().set( + [](const MyEmpty &) {})}, + InstanceContext{"lambda_ta_with_info", + rclcpp::AnySubscriptionCallback().set( + [](const MyEmpty &, const rclcpp::MessageInfo &) {})}, + InstanceContext{"lambda", rclcpp::AnySubscriptionCallback().set( + [](const test_msgs::msg::Empty &) {})}, + InstanceContext{"lambda_with_info", + rclcpp::AnySubscriptionCallback().set( + [](const test_msgs::msg::Empty &, const rclcpp::MessageInfo &) {})}, + // free function + InstanceContext{"free_function_ta", rclcpp::AnySubscriptionCallback().set( + const_ref_ta_free_func)}, + InstanceContext{"free_function_ta_with_info", + rclcpp::AnySubscriptionCallback().set( + const_ref_ta_w_info_free_func)}, + InstanceContext{"free_function", rclcpp::AnySubscriptionCallback().set( + const_ref_free_func)}, + InstanceContext{"free_function_with_info", + rclcpp::AnySubscriptionCallback().set( + const_ref_w_info_free_func)}, + // bind function + BindContext("bind_method_ta"), + BindContext( + "bind_method_ta_with_info"), + BindContext("bind_method"), + BindContext( + "bind_method_with_info") + ), + format_parameter_with_ta +); + // // Versions of `std::unique_ptr` // @@ -264,13 +344,56 @@ INSTANTIATE_TEST_SUITE_P( rclcpp::AnySubscriptionCallback().set( unique_ptr_w_info_free_func)}, // bind function - BindContext>("bind_method"), - BindContext, const rclcpp::MessageInfo &>( - "bind_method_with_info") + BindContext>("bind_method"), + BindContext< + test_msgs::msg::Empty, + std::unique_ptr, + const rclcpp::MessageInfo & + >("bind_method_with_info") ), format_parameter ); +void unique_ptr_ta_free_func(std::unique_ptr) {} +void unique_ptr_ta_w_info_free_func(std::unique_ptr, const rclcpp::MessageInfo &) {} + +INSTANTIATE_TEST_SUITE_P( + UniquePtrCallbackTests, + DispatchTestsWithTA, + ::testing::Values( + // lambda + InstanceContext{"lambda_ta", rclcpp::AnySubscriptionCallback().set( + [](std::unique_ptr) {})}, + InstanceContext{"lambda_ta_with_info", + rclcpp::AnySubscriptionCallback().set( + [](std::unique_ptr, const rclcpp::MessageInfo &) {})}, + InstanceContext{"lambda", rclcpp::AnySubscriptionCallback().set( + [](std::unique_ptr) {})}, + InstanceContext{"lambda_with_info", + rclcpp::AnySubscriptionCallback().set( + [](std::unique_ptr, const rclcpp::MessageInfo &) {})}, + // free function + InstanceContext{"free_function_ta", rclcpp::AnySubscriptionCallback().set( + unique_ptr_ta_free_func)}, + InstanceContext{"free_function_ta_with_info", + rclcpp::AnySubscriptionCallback().set( + unique_ptr_ta_w_info_free_func)}, + InstanceContext{"free_function", rclcpp::AnySubscriptionCallback().set( + unique_ptr_free_func)}, + InstanceContext{"free_function_with_info", + rclcpp::AnySubscriptionCallback().set( + unique_ptr_w_info_free_func)}, + // bind function + BindContext>("bind_method_ta"), + BindContext, const rclcpp::MessageInfo &>( + "bind_method_ta_with_info"), + BindContext>("bind_method"), + BindContext, const rclcpp::MessageInfo &>( + "bind_method_with_info") + ), + format_parameter_with_ta +); + // // Versions of `std::shared_ptr` // @@ -296,13 +419,56 @@ INSTANTIATE_TEST_SUITE_P( rclcpp::AnySubscriptionCallback().set( shared_const_ptr_w_info_free_func)}, // bind function - BindContext>("bind_method"), - BindContext, const rclcpp::MessageInfo &>( + 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 &` // @@ -328,13 +494,58 @@ INSTANTIATE_TEST_SUITE_P( rclcpp::AnySubscriptionCallback().set( const_ref_shared_const_ptr_w_info_free_func)}, // bind function - BindContext &>("bind_method"), - BindContext &, const rclcpp::MessageInfo &>( + 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` // @@ -360,9 +571,52 @@ INSTANTIATE_TEST_SUITE_P( rclcpp::AnySubscriptionCallback().set( shared_ptr_w_info_free_func)}, // bind function - BindContext>("bind_method"), - BindContext, const rclcpp::MessageInfo &>( + 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_intra_process_manager.cpp b/rclcpp/test/rclcpp/test_intra_process_manager.cpp index ec7ccc7e5a..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: @@ -220,12 +223,12 @@ template< typename MessageT, typename Alloc = std::allocator, typename Deleter = std::default_delete> -class SubscriptionIntraProcess : public SubscriptionIntraProcessBase +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>(); @@ -262,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 @@ -270,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 @@ -307,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 index 9bef3c2c0f..6d192ca86b 100644 --- a/rclcpp/test/rclcpp/test_intra_process_manager_with_allocators.cpp +++ b/rclcpp/test/rclcpp/test_intra_process_manager_with_allocators.cpp @@ -196,7 +196,6 @@ do_custom_allocator_test( test_msgs::msg::Empty, decltype(callback), SubscriptionAllocatorT, - CallbackMessageT, rclcpp::Subscription, rclcpp::message_memory_strategy::MessageMemoryStrategy< CallbackMessageT, 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_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_subscription.cpp b/rclcpp/test/rclcpp/test_subscription.cpp index 9cf9acdcf8..2895865c1f 100644 --- a/rclcpp/test/rclcpp/test_subscription.cpp +++ b/rclcpp/test/rclcpp/test_subscription.cpp @@ -510,7 +510,7 @@ INSTANTIATE_TEST_SUITE_P( TEST_F(TestSubscription, get_network_flow_endpoints_errors) { initialize(); const rclcpp::QoS subscription_qos(1); - auto subscription_callback = [](const test_msgs::msg::Empty::SharedPtr msg) { + auto subscription_callback = [](test_msgs::msg::Empty::SharedPtr msg) { (void)msg; }; auto subscription = node->create_subscription( 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_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index d81750ba08..1c3379712e 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -230,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, diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp index 77598fdcb7..a7f1c686bc 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp @@ -61,7 +61,6 @@ template< typename MessageT, typename CallbackT, typename AllocatorT, - typename CallbackMessageT, typename SubscriptionT, typename MessageMemoryStrategyT> std::shared_ptr From 5dad229662e3066b79207143347054d1201c5263 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Mon, 10 May 2021 12:45:34 -0700 Subject: [PATCH 83/93] Fix doc typo (#1663) Signed-off-by: Jacob Perron --- rclcpp/include/rclcpp/context.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/context.hpp b/rclcpp/include/rclcpp/context.hpp index ae6d3d0a99..5d71ffe06c 100644 --- a/rclcpp/include/rclcpp/context.hpp +++ b/rclcpp/include/rclcpp/context.hpp @@ -92,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. From a3383c309ac11c39b35b0a9f2cd0a508ce865663 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 11 May 2021 15:12:40 +0000 Subject: [PATCH 84/93] Changelog. Signed-off-by: Chris Lalancette --- rclcpp/CHANGELOG.rst | 13 +++++++++++++ rclcpp_action/CHANGELOG.rst | 6 ++++++ rclcpp_components/CHANGELOG.rst | 3 +++ rclcpp_lifecycle/CHANGELOG.rst | 5 +++++ 4 files changed, 27 insertions(+) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index e3ceb06d10..6ebfbf0e79 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,19 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 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 `_) diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index e41da9c05d..88f2217304 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,12 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 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) ------------------ diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index bf48f99d54..e0522fb634 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 9.0.2 (2021-04-14) ------------------ diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 5141bf06a2..ea94896221 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,11 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* [rclcpp] Type Adaptation feature (`#1557 `_) +* Contributors: Audrow Nash, William Woodall + 9.0.2 (2021-04-14) ------------------ From 72443ff295fcae98d24ff90a37828c4c282d7449 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 11 May 2021 15:12:49 +0000 Subject: [PATCH 85/93] 10.0.0 --- rclcpp/CHANGELOG.rst | 4 ++-- rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 4 ++-- rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 4 ++-- rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 4 ++-- rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 6ebfbf0e79..e1425d26b5 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +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 `_) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 568915ef72..c8d23cbf18 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 9.0.2 + 10.0.0 The ROS client library in C++. Ivan Paunovic Mabel Zhang diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 88f2217304..be9b99f5b8 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +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 diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index 50877fc9f3..a797580a96 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 9.0.2 + 10.0.0 Adds action APIs for C++. Ivan Paunovic Mabel Zhang diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index e0522fb634..bc5dad8950 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +10.0.0 (2021-05-11) +------------------- 9.0.2 (2021-04-14) ------------------ diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index 87cbd4865c..029ee028c2 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 9.0.2 + 10.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 ea94896221..c438cdb3ff 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +10.0.0 (2021-05-11) +------------------- * [rclcpp] Type Adaptation feature (`#1557 `_) * Contributors: Audrow Nash, William Woodall diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index bfe74af56f..456ebc6264 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 9.0.2 + 10.0.0 Package containing a prototype for lifecycle implementation Ivan Paunovic Mabel Zhang From d488535f366f9f59d3f72f6e15d1b5258c7d63c6 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 13 May 2021 12:13:07 -0300 Subject: [PATCH 86/93] [service] Don't use a weak_ptr to avoid leaking (#1668) Signed-off-by: Ivan Santiago Paunovic --- rclcpp/include/rclcpp/service.hpp | 23 +++++++---------------- 1 file changed, 7 insertions(+), 16 deletions(-) diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 079be09b28..ae442964e1 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -177,25 +177,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; }); From f245b4cc8107c2702c7edd1486e6025427ade57d Mon Sep 17 00:00:00 2001 From: Scott K Logan Date: Thu, 13 May 2021 12:10:19 -0700 Subject: [PATCH 87/93] Bump the benchmark timeout for benchmark_action_client (#1671) Pausing and resuming the measurement inside the timing loop can cause the initial run duration calculation to underestimate how long the benchmark is taking to run, which results in the recorded run taking a lot longer than it should. This is a known issue in libbenchmark. This test is affected by that behavior, and typically takes a bit longer than the others. The easiest thing to do right now is to just bump the timeout. My tests show that 180 seconds is typically sufficient for this test, so 240 should be a safe point to conclude that the test is malfunctioning. Signed-off-by: Scott K Logan --- rclcpp_action/test/benchmark/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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) From 18fcd9138b9cbc46c1ae75fe783f2028755abfab Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 14 May 2021 18:33:10 -0700 Subject: [PATCH 88/93] fix syntax issue with gcc (#1674) * fix syntax issue with gcc Signed-off-by: William Woodall * uncrustify Signed-off-by: William Woodall --- rclcpp/include/rclcpp/type_adapter.hpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/rclcpp/include/rclcpp/type_adapter.hpp b/rclcpp/include/rclcpp/type_adapter.hpp index 0b779c8062..d6834ccc22 100644 --- a/rclcpp/include/rclcpp/type_adapter.hpp +++ b/rclcpp/include/rclcpp/type_adapter.hpp @@ -179,18 +179,22 @@ struct TypeAdapter::is_speciali /// 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 \ + 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 From 56f68f9c44f26a4bd07d45caf39aad5d9945939c Mon Sep 17 00:00:00 2001 From: Scott K Logan Date: Fri, 14 May 2021 21:28:55 -0700 Subject: [PATCH 89/93] Fix destruction order in lifecycle benchmark (#1675) Signed-off-by: Scott K Logan --- rclcpp_lifecycle/test/benchmark/benchmark_lifecycle_client.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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(); } From 07b6ea0ff4f2e5c29dab9b068814576343122f56 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Tue, 18 May 2021 17:55:48 -0700 Subject: [PATCH 90/93] Declare parameters uninitialized (#1673) * Declare parameters uninitialized Fixes #1649 Allow declaring parameters without an initial value or override. This was possible prior to Galactic, but was made impossible since we started enforcing the types of parameters in Galactic. Signed-off-by: Jacob Perron * Remove assertion Signed-off-by: Jacob Perron * Throw NoParameterOverrideProvided exception if accessed before initialized Signed-off-by: Jacob Perron * Add test getting static parameter after it is set Signed-off-by: Jacob Perron * Do not throw on access of uninitialized dynamically typed parameter Signed-off-by: Jacob Perron * Rename exception type Signed-off-by: Jacob Perron * Remove unused exception type Signed-off-by: Jacob Perron * Uncrustify Signed-off-by: Jacob Perron --- .../include/rclcpp/exceptions/exceptions.hpp | 8 ++-- .../node_interfaces/node_parameters.cpp | 40 +++++++++++++------ rclcpp/test/rclcpp/test_node.cpp | 22 ++++++++-- 3 files changed, 50 insertions(+), 20 deletions(-) diff --git a/rclcpp/include/rclcpp/exceptions/exceptions.hpp b/rclcpp/include/rclcpp/exceptions/exceptions.hpp index 27c695c0b6..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") {} }; diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index 1f9e32acbc..3d91943bc5 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -349,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. @@ -413,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, @@ -806,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); } } diff --git a/rclcpp/test/rclcpp/test_node.cpp b/rclcpp/test/rclcpp/test_node.cpp index dff05a0667..8d2e8a8410 100644 --- a/rclcpp/test/rclcpp/test_node.cpp +++ b/rclcpp/test/rclcpp/test_node.cpp @@ -335,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 @@ -2798,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( From 0251f70fe8a952fae242e6dcdc5b11bc4c11dc76 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Tue, 18 May 2021 18:00:56 -0700 Subject: [PATCH 91/93] 11.0.0 --- rclcpp/CHANGELOG.rst | 7 +++++++ rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 5 +++++ rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 3 +++ rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 5 +++++ rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 24 insertions(+), 4 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index e1425d26b5..0676ae673c 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,13 @@ 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 `_) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index c8d23cbf18..28067938e5 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 10.0.0 + 11.0.0 The ROS client library in C++. Ivan Paunovic Mabel Zhang diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index be9b99f5b8..2297e85055 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,11 @@ 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 `_) diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index a797580a96..e753114b22 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 10.0.0 + 11.0.0 Adds action APIs for C++. Ivan Paunovic Mabel Zhang diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index bc5dad8950..814e51ac84 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +11.0.0 (2021-05-18) +------------------- + 10.0.0 (2021-05-11) ------------------- diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index 029ee028c2..51b80d3f4c 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 10.0.0 + 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 c438cdb3ff..6cd8855377 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,11 @@ 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 `_) diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 456ebc6264..60095131e5 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 10.0.0 + 11.0.0 Package containing a prototype for lifecycle implementation Ivan Paunovic Mabel Zhang From 1c617515403db16b8cbabd834073eaca0f9a452d Mon Sep 17 00:00:00 2001 From: Kaven Yau Date: Fri, 21 May 2021 04:15:59 +0800 Subject: [PATCH 92/93] Fix occasionally missing goal result caused by race condition (#1677) * Fix occasionally missing goal result caused by race condition Signed-off-by: Kaven Yau * Take action_server_reentrant_mutex_ out of the sending result loop Signed-off-by: Kaven Yau * add note for explaining the current locking order in server.cpp Signed-off-by: Kaven Yau --- rclcpp_action/src/server.cpp | 39 +++++++++++++++++++++++------------- 1 file changed, 25 insertions(+), 14 deletions(-) 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); + } } } } From 394da52796c37ef371e71620a153e9961b07b44b Mon Sep 17 00:00:00 2001 From: Mauro Passerino Date: Wed, 26 May 2021 17:55:43 +0100 Subject: [PATCH 93/93] Some fixes Signed-off-by: Mauro Passerino --- .../events_executor_notify_waitable.hpp | 2 +- .../subscription_intra_process_buffer.hpp | 29 ++--------------- rclcpp/src/rclcpp/executor.cpp | 32 +++++-------------- .../events_executor_entities_collector.cpp | 12 +++---- rclcpp/src/rclcpp/qos_event.cpp | 7 ---- .../subscription_intra_process_base.cpp | 19 ----------- .../rclcpp/executors/test_events_queue.cpp | 4 +-- .../test/rclcpp/executors/test_executors.cpp | 2 +- 8 files changed, 21 insertions(+), 86 deletions(-) diff --git a/rclcpp/include/rclcpp/executors/events_executor_notify_waitable.hpp b/rclcpp/include/rclcpp/executors/events_executor_notify_waitable.hpp index 69af6b84d3..d15d9eba28 100644 --- a/rclcpp/include/rclcpp/executors/events_executor_notify_waitable.hpp +++ b/rclcpp/include/rclcpp/executors/events_executor_notify_waitable.hpp @@ -60,7 +60,7 @@ class EventsExecutorNotifyWaitable final : public EventWaitable RCLCPP_PUBLIC void - set_listener_callback(std::function callback) override + set_on_ready_callback(std::function callback) override { (void)callback; // for (auto gc : notify_guard_conditions_) { diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp index c2af461b97..546e5b81db 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp @@ -66,36 +66,13 @@ class SubscriptionIntraProcessBuffer : public SubscriptionIntraProcessBase const std::string & topic_name, rmw_qos_profile_t qos_profile, rclcpp::IntraProcessBufferType buffer_type) - : SubscriptionIntraProcessBase(topic_name, qos_profile) + : SubscriptionIntraProcessBase(context, topic_name, qos_profile) { // 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( - "SubscriptionIntraProcessBuffer init error initializing guard condition"); - } - } - - virtual ~SubscriptionIntraProcessBuffer() - { - 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 @@ -135,6 +112,7 @@ class SubscriptionIntraProcessBuffer : public SubscriptionIntraProcessBase if (on_new_message_callback_) { on_new_message_callback_(1); } else { + gc_.trigger(); unread_count_++; } } @@ -142,8 +120,7 @@ class SubscriptionIntraProcessBuffer : public SubscriptionIntraProcessBase void trigger_guard_condition() { - rcl_ret_t ret = rcl_trigger_guard_condition(&gc_); - (void)ret; + gc_.trigger(); } BufferUniquePtr buffer_; diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index a079af804a..5465a76d01 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -76,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"); } } @@ -214,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); } } @@ -291,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()); } } @@ -470,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 diff --git a/rclcpp/src/rclcpp/executors/events_executor_entities_collector.cpp b/rclcpp/src/rclcpp/executors/events_executor_entities_collector.cpp index 35709a42f4..1d46b9d187 100644 --- a/rclcpp/src/rclcpp/executors/events_executor_entities_collector.cpp +++ b/rclcpp/src/rclcpp/executors/events_executor_entities_collector.cpp @@ -267,7 +267,7 @@ EventsExecutorEntitiesCollector::set_callback_group_entities_callbacks( if (waitable) { weak_waitables_map_.emplace(waitable.get(), waitable); - waitable->set_listener_callback( + waitable->set_on_ready_callback( create_waitable_callback(waitable.get())); } return false; @@ -291,7 +291,7 @@ EventsExecutorEntitiesCollector::unset_callback_group_entities_callbacks( group->find_subscription_ptrs_if( [this](const rclcpp::SubscriptionBase::SharedPtr & subscription) { if (subscription) { - subscription->set_on_new_message_callback(nullptr); + subscription->clear_on_new_message_callback(); weak_subscriptions_map_.erase(subscription.get()); } return false; @@ -299,7 +299,7 @@ EventsExecutorEntitiesCollector::unset_callback_group_entities_callbacks( group->find_service_ptrs_if( [this](const rclcpp::ServiceBase::SharedPtr & service) { if (service) { - service->set_on_new_request_callback(nullptr); + service->clear_on_new_request_callback(); weak_services_map_.erase(service.get()); } return false; @@ -307,7 +307,7 @@ EventsExecutorEntitiesCollector::unset_callback_group_entities_callbacks( group->find_client_ptrs_if( [this](const rclcpp::ClientBase::SharedPtr & client) { if (client) { - client->set_on_new_response_callback(nullptr); + client->clear_on_new_response_callback(); weak_clients_map_.erase(client.get()); } return false; @@ -315,7 +315,7 @@ EventsExecutorEntitiesCollector::unset_callback_group_entities_callbacks( group->find_waitable_ptrs_if( [this](const rclcpp::Waitable::SharedPtr & waitable) { if (waitable) { - waitable->set_listener_callback(nullptr); + waitable->clear_on_ready_callback(); weak_waitables_map_.erase(waitable.get()); } return false; @@ -583,7 +583,7 @@ EventsExecutorEntitiesCollector::add_waitable(rclcpp::Waitable::SharedPtr waitab { weak_waitables_map_.emplace(waitable.get(), waitable); - waitable->set_listener_callback( + waitable->set_on_ready_callback( create_waitable_callback(waitable.get())); } diff --git a/rclcpp/src/rclcpp/qos_event.cpp b/rclcpp/src/rclcpp/qos_event.cpp index 7e0a65d5cb..3862cf68c5 100644 --- a/rclcpp/src/rclcpp/qos_event.cpp +++ b/rclcpp/src/rclcpp/qos_event.cpp @@ -84,11 +84,4 @@ QOSEventHandlerBase::set_on_new_event_callback( } } -void -QOSEventHandlerBase::set_listener_callback(std::function callback) -{ - // set_on_new_event_callback(callback); -} - - } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp index bdc5ebb01a..d1fd1c700e 100644 --- a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp +++ b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp @@ -39,22 +39,3 @@ SubscriptionIntraProcessBase::get_actual_qos() const { return qos_profile_; } - -void -SubscriptionIntraProcessBase::set_listener_callback(std::function callback) -{ - if (callback) { - on_new_message_callback_ = std::bind(callback, std::placeholders::_1, -1); - if (unread_count_) { - if (unread_count_ < qos_profile_.depth) { - on_new_message_callback_(unread_count_); - } else { - on_new_message_callback_(qos_profile_.depth); - } - unread_count_ = 0; - } - return; - } - - on_new_message_callback_ = nullptr; -} diff --git a/rclcpp/test/rclcpp/executors/test_events_queue.cpp b/rclcpp/test/rclcpp/executors/test_events_queue.cpp index 7ea0d657c3..a07681698e 100644 --- a/rclcpp/test/rclcpp/executors/test_events_queue.cpp +++ b/rclcpp/test/rclcpp/executors/test_events_queue.cpp @@ -52,8 +52,8 @@ 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); diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index 74e14eb38c..3a386e3a9f 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -404,7 +404,7 @@ class TestWaitable : public rclcpp::Waitable } void - set_listener_callback(std::function callback) override + set_on_ready_callback(std::function callback) override { }