From 33dae5d679751b603205008fcb31755986bcee1c Mon Sep 17 00:00:00 2001 From: Audrow Nash Date: Tue, 28 Jun 2022 09:20:48 -0500 Subject: [PATCH 01/53] Mirror rolling to master --- .github/workflows/mirror-rolling-to-master.yaml | 13 +++++++++++++ 1 file changed, 13 insertions(+) create mode 100644 .github/workflows/mirror-rolling-to-master.yaml diff --git a/.github/workflows/mirror-rolling-to-master.yaml b/.github/workflows/mirror-rolling-to-master.yaml new file mode 100644 index 0000000000..2885eb4a4f --- /dev/null +++ b/.github/workflows/mirror-rolling-to-master.yaml @@ -0,0 +1,13 @@ +name: Mirror rolling to master + +on: + push: + branches: [ rolling ] + +jobs: + mirror-to-master: + runs-on: ubuntu-latest + steps: + - uses: zofrex/mirror-branch@v1 + with: + target-branch: master From 6dd3a0377bbacd07fa6ed3c9e70c6de70931b45f Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Thu, 7 Jul 2022 00:41:16 +0800 Subject: [PATCH 02/53] use regex for wildcard matching (#1839) * use regex for wildcard matching Co-authored-by: Aaron Lipinski Signed-off-by: Chen Lihui * use map to process the content of parameter file by order Signed-off-by: Chen Lihui * add more test cases Signed-off-by: Chen Lihui * try to not decrease the performance and make the param win last Signed-off-by: Chen Lihui * update node name Signed-off-by: Chen Lihui * update document comment Signed-off-by: Chen Lihui * add more test for parameter_map_from Signed-off-by: Chen Lihui Co-authored-by: Aaron Lipinski --- rclcpp/include/rclcpp/parameter_map.hpp | 4 +- .../detail/resolve_parameter_overrides.cpp | 17 +-- rclcpp/src/rclcpp/parameter_map.cpp | 16 ++- .../node_interfaces/test_node_parameters.cpp | 132 ++++++++++++++++++ rclcpp/test/rclcpp/test_parameter_map.cpp | 130 +++++++++++++++++ .../complicated_wildcards.yaml | 5 + .../test_node_parameters/params_by_order.yaml | 16 +++ .../test_node_parameters/wildcards.yaml | 57 ++++++++ 8 files changed, 364 insertions(+), 13 deletions(-) create mode 100644 rclcpp/test/resources/test_node_parameters/complicated_wildcards.yaml create mode 100644 rclcpp/test/resources/test_node_parameters/params_by_order.yaml create mode 100644 rclcpp/test/resources/test_node_parameters/wildcards.yaml diff --git a/rclcpp/include/rclcpp/parameter_map.hpp b/rclcpp/include/rclcpp/parameter_map.hpp index 9cfcdaaacf..303eac4a95 100644 --- a/rclcpp/include/rclcpp/parameter_map.hpp +++ b/rclcpp/include/rclcpp/parameter_map.hpp @@ -35,11 +35,13 @@ using ParameterMap = std::unordered_map>; /// Convert parameters from rcl_yaml_param_parser into C++ class instances. /// \param[in] c_params C structures containing parameters for multiple nodes. +/// \param[in] node_fqn a Fully Qualified Name of node, default value is nullptr. +/// If it's not nullptr, return the relative node parameters belonging to this node_fqn. /// \returns a map where the keys are fully qualified node names and values a list of parameters. /// \throws InvalidParametersException if the `rcl_params_t` is inconsistent or invalid. RCLCPP_PUBLIC ParameterMap -parameter_map_from(const rcl_params_t * const c_params); +parameter_map_from(const rcl_params_t * const c_params, const char * node_fqn = nullptr); /// Convert parameter value from rcl_yaml_param_parser into a C++ class instance. /// \param[in] c_value C structure containing a value of a parameter. diff --git a/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.cpp b/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.cpp index a62121b37f..3959e64882 100644 --- a/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.cpp +++ b/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.cpp @@ -51,18 +51,13 @@ rclcpp::detail::resolve_parameter_overrides( [params]() { rcl_yaml_node_struct_fini(params); }); - rclcpp::ParameterMap initial_map = rclcpp::parameter_map_from(params); + rclcpp::ParameterMap initial_map = rclcpp::parameter_map_from(params, node_fqn.c_str()); - // Enforce wildcard matching precedence - // TODO(cottsay) implement further wildcard matching - const std::array node_matching_names{"/**", node_fqn}; - for (const auto & node_name : node_matching_names) { - if (initial_map.count(node_name) > 0) { - // Combine parameter yaml files, overwriting values in older ones - for (const rclcpp::Parameter & param : initial_map.at(node_name)) { - result[param.get_name()] = - rclcpp::ParameterValue(param.get_value_message()); - } + if (initial_map.count(node_fqn) > 0) { + // Combine parameter yaml files, overwriting values in older ones + for (const rclcpp::Parameter & param : initial_map.at(node_fqn)) { + result[param.get_name()] = + rclcpp::ParameterValue(param.get_value_message()); } } } diff --git a/rclcpp/src/rclcpp/parameter_map.cpp b/rclcpp/src/rclcpp/parameter_map.cpp index e5e3da019c..8f7bc0655c 100644 --- a/rclcpp/src/rclcpp/parameter_map.cpp +++ b/rclcpp/src/rclcpp/parameter_map.cpp @@ -13,8 +13,10 @@ // limitations under the License. #include +#include #include +#include "rcpputils/find_and_replace.hpp" #include "rclcpp/parameter_map.hpp" using rclcpp::exceptions::InvalidParametersException; @@ -23,7 +25,7 @@ using rclcpp::ParameterMap; using rclcpp::ParameterValue; ParameterMap -rclcpp::parameter_map_from(const rcl_params_t * const c_params) +rclcpp::parameter_map_from(const rcl_params_t * const c_params, const char * node_fqn) { if (NULL == c_params) { throw InvalidParametersException("parameters struct is NULL"); @@ -49,6 +51,17 @@ rclcpp::parameter_map_from(const rcl_params_t * const c_params) node_name = c_node_name; } + if (node_fqn) { + // Update the regular expression ["/*" -> "(/\\w+)" and "/**" -> "(/\\w+)*"] + std::string regex = rcpputils::find_and_replace(node_name, "/*", "(/\\w+)"); + if (!std::regex_match(node_fqn, std::regex(regex))) { + // No need to parse the items because the user just care about node_fqn + continue; + } + + node_name = node_fqn; + } + const rcl_node_params_t * const c_params_node = &(c_params->params[n]); std::vector & params_node = parameters[node_name]; @@ -65,6 +78,7 @@ rclcpp::parameter_map_from(const rcl_params_t * const c_params) params_node.emplace_back(c_param_name, parameter_value_from(c_param_value)); } } + return parameters; } diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp index 31b755b4a7..97e3a3188d 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp @@ -31,6 +31,8 @@ #include "../../mocking_utils/patch.hpp" #include "../../utils/rclcpp_gtest_macros.hpp" +#include "rcpputils/filesystem_helper.hpp" + class TestNodeParameters : public ::testing::Test { public: @@ -47,6 +49,7 @@ class TestNodeParameters : public ::testing::Test dynamic_cast( node->get_node_parameters_interface().get()); ASSERT_NE(nullptr, node_parameters); + test_resources_path /= "test_node_parameters"; } void TearDown() @@ -57,6 +60,8 @@ class TestNodeParameters : public ::testing::Test protected: std::shared_ptr node; rclcpp::node_interfaces::NodeParameters * node_parameters; + + rcpputils::fs::path test_resources_path{TEST_RESOURCES_DIRECTORY}; }; TEST_F(TestNodeParameters, construct_destruct_rcl_errors) { @@ -199,3 +204,130 @@ TEST_F(TestNodeParameters, add_remove_parameters_callback) { node_parameters->remove_on_set_parameters_callback(handle.get()), std::runtime_error("Callback doesn't exist")); } + +TEST_F(TestNodeParameters, wildcard_with_namespace) +{ + rclcpp::NodeOptions opts; + opts.arguments( + { + "--ros-args", + "--params-file", (test_resources_path / "wildcards.yaml").string() + }); + + std::shared_ptr node = std::make_shared("node2", "ns", opts); + + auto * node_parameters = + dynamic_cast( + node->get_node_parameters_interface().get()); + ASSERT_NE(nullptr, node_parameters); + + const auto & parameter_overrides = node_parameters->get_parameter_overrides(); + EXPECT_EQ(7u, parameter_overrides.size()); + EXPECT_EQ(parameter_overrides.at("full_wild").get(), "full_wild"); + EXPECT_EQ(parameter_overrides.at("namespace_wild").get(), "namespace_wild"); + EXPECT_EQ( + parameter_overrides.at("namespace_wild_another").get(), + "namespace_wild_another"); + EXPECT_EQ( + parameter_overrides.at("namespace_wild_one_star").get(), + "namespace_wild_one_star"); + EXPECT_EQ(parameter_overrides.at("node_wild_in_ns").get(), "node_wild_in_ns"); + EXPECT_EQ( + parameter_overrides.at("node_wild_in_ns_another").get(), + "node_wild_in_ns_another"); + EXPECT_EQ(parameter_overrides.at("explicit_in_ns").get(), "explicit_in_ns"); + EXPECT_EQ(parameter_overrides.count("should_not_appear"), 0u); +} + +TEST_F(TestNodeParameters, wildcard_no_namespace) +{ + rclcpp::NodeOptions opts; + opts.arguments( + { + "--ros-args", + "--params-file", (test_resources_path / "wildcards.yaml").string() + }); + + std::shared_ptr node = std::make_shared("node2", opts); + + auto * node_parameters = + dynamic_cast( + node->get_node_parameters_interface().get()); + ASSERT_NE(nullptr, node_parameters); + + const auto & parameter_overrides = node_parameters->get_parameter_overrides(); + EXPECT_EQ(5u, parameter_overrides.size()); + EXPECT_EQ(parameter_overrides.at("full_wild").get(), "full_wild"); + EXPECT_EQ(parameter_overrides.at("namespace_wild").get(), "namespace_wild"); + EXPECT_EQ( + parameter_overrides.at("namespace_wild_another").get(), + "namespace_wild_another"); + EXPECT_EQ(parameter_overrides.at("node_wild_no_ns").get(), "node_wild_no_ns"); + EXPECT_EQ(parameter_overrides.at("explicit_no_ns").get(), "explicit_no_ns"); + EXPECT_EQ(parameter_overrides.count("should_not_appear"), 0u); + // "/*" match exactly one token, not expect to get `namespace_wild_one_star` + EXPECT_EQ(parameter_overrides.count("namespace_wild_one_star"), 0u); +} + +TEST_F(TestNodeParameters, params_by_order) +{ + rclcpp::NodeOptions opts; + opts.arguments( + { + "--ros-args", + "--params-file", (test_resources_path / "params_by_order.yaml").string() + }); + + std::shared_ptr node = std::make_shared("node2", "ns", opts); + + auto * node_parameters = + dynamic_cast( + node->get_node_parameters_interface().get()); + ASSERT_NE(nullptr, node_parameters); + + const auto & parameter_overrides = node_parameters->get_parameter_overrides(); + EXPECT_EQ(3u, parameter_overrides.size()); + EXPECT_EQ(parameter_overrides.at("a_value").get(), "last_one_win"); + EXPECT_EQ(parameter_overrides.at("foo").get(), "foo"); + EXPECT_EQ(parameter_overrides.at("bar").get(), "bar"); +} + +TEST_F(TestNodeParameters, complicated_wildcards) +{ + rclcpp::NodeOptions opts; + opts.arguments( + { + "--ros-args", + "--params-file", (test_resources_path / "complicated_wildcards.yaml").string() + }); + + { + // regex matched: /**/foo/*/bar + std::shared_ptr node = + std::make_shared("node2", "/a/b/c/foo/d/bar", opts); + + auto * node_parameters = + dynamic_cast( + node->get_node_parameters_interface().get()); + ASSERT_NE(nullptr, node_parameters); + + const auto & parameter_overrides = node_parameters->get_parameter_overrides(); + EXPECT_EQ(2u, parameter_overrides.size()); + EXPECT_EQ(parameter_overrides.at("foo").get(), "foo"); + EXPECT_EQ(parameter_overrides.at("bar").get(), "bar"); + } + + { + // regex not matched: /**/foo/*/bar + std::shared_ptr node = + std::make_shared("node2", "/a/b/c/foo/bar", opts); + + auto * node_parameters = + dynamic_cast( + node->get_node_parameters_interface().get()); + ASSERT_NE(nullptr, node_parameters); + + const auto & parameter_overrides = node_parameters->get_parameter_overrides(); + EXPECT_EQ(0u, parameter_overrides.size()); + } +} diff --git a/rclcpp/test/rclcpp/test_parameter_map.cpp b/rclcpp/test/rclcpp/test_parameter_map.cpp index 3f54e4c879..0158b7c7e7 100644 --- a/rclcpp/test/rclcpp/test_parameter_map.cpp +++ b/rclcpp/test/rclcpp/test_parameter_map.cpp @@ -19,6 +19,7 @@ #include #include +#include #include #include "rclcpp/parameter_map.hpp" @@ -353,3 +354,132 @@ TEST(Test_parameter_map_from, string_array_param_value) c_params->params[0].parameter_values[0].string_array_value = NULL; rcl_yaml_node_struct_fini(c_params); } + +TEST(Test_parameter_map_from, one_node_one_param_by_node_fqn) +{ + rcl_params_t * c_params = make_params({"foo"}); + make_node_params(c_params, 0, {"string_param"}); + + std::string hello_world = "hello world"; + char * c_hello_world = new char[hello_world.length() + 1]; + std::snprintf(c_hello_world, hello_world.size() + 1, "%s", hello_world.c_str()); + c_params->params[0].parameter_values[0].string_value = c_hello_world; + + rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params, "/foo"); + const std::vector & params = map.at("/foo"); + EXPECT_STREQ("string_param", params.at(0).get_name().c_str()); + EXPECT_STREQ(hello_world.c_str(), params.at(0).get_value().c_str()); + + c_params->params[0].parameter_values[0].string_value = NULL; + delete[] c_hello_world; + rcl_yaml_node_struct_fini(c_params); +} + +TEST(Test_parameter_map_from, multi_nodes_same_param_name_by_node_fqn) +{ + std::vector node_names_keys = { + "/**", // index: 0 + "/*", // index: 1 + "/**/node", // index: 2 + "/*/node", // index: 3 + "/ns/node" // index: 4 + }; + + rcl_params_t * c_params = make_params(node_names_keys); + + std::vector param_values; + for (size_t i = 0; i < node_names_keys.size(); ++i) { + make_node_params(c_params, i, {"string_param"}); + std::string hello_world = "hello world" + std::to_string(i); + char * c_hello_world = new char[hello_world.length() + 1]; + std::snprintf(c_hello_world, hello_world.size() + 1, "%s", hello_world.c_str()); + c_params->params[i].parameter_values[0].string_value = c_hello_world; + param_values.push_back(c_hello_world); + } + + std::unordered_map> node_fqn_expected = { + {"/ns/foo/another_node", {0}}, + {"/another", {0, 1}}, + {"/node", {0, 1, 2}}, + {"/another_ns/node", {0, 2, 3}}, + {"/ns/node", {0, 2, 3, 4}}, + }; + + for (auto & kv : node_fqn_expected) { + rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params, kv.first.c_str()); + const std::vector & params = map.at(kv.first); + + EXPECT_EQ(kv.second.size(), params.size()); + for (size_t i = 0; i < params.size(); ++i) { + std::string param_value = "hello world" + std::to_string(kv.second[i]); + EXPECT_STREQ("string_param", params.at(i).get_name().c_str()); + EXPECT_STREQ(param_value.c_str(), params.at(i).get_value().c_str()); + } + } + + for (size_t i = 0; i < node_names_keys.size(); ++i) { + c_params->params[i].parameter_values[0].string_value = NULL; + } + for (auto c_hello_world : param_values) { + delete[] c_hello_world; + } + rcl_yaml_node_struct_fini(c_params); +} + +TEST(Test_parameter_map_from, multi_nodes_diff_param_name_by_node_fqn) +{ + std::vector node_names_keys = { + "/**", // index: 0 + "/*", // index: 1 + "/**/node", // index: 2 + "/*/node", // index: 3 + "/ns/**", // index: 4 + "/ns/*", // index: 5 + "/ns/**/node", // index: 6 + "/ns/*/node", // index: 7 + "/ns/**/a/*/node", // index: 8 + "/ns/node" // index: 9 + }; + + rcl_params_t * c_params = make_params(node_names_keys); + + for (size_t i = 0; i < node_names_keys.size(); ++i) { + std::string param_name = "string_param" + std::to_string(i); + make_node_params(c_params, i, {param_name}); + } + + std::string hello_world = "hello world"; + char * c_hello_world = new char[hello_world.length() + 1]; + std::snprintf(c_hello_world, hello_world.size() + 1, "%s", hello_world.c_str()); + + for (size_t i = 0; i < node_names_keys.size(); ++i) { + c_params->params[i].parameter_values[0].string_value = c_hello_world; + } + + std::unordered_map> node_fqn_expected = { + {"/ns/node", {0, 2, 3, 4, 5, 6, 9}}, + {"/node", {0, 1, 2}}, + {"/ns/foo/node", {0, 2, 4, 6, 7}}, + {"/ns/foo/a/node", {0, 2, 4, 6}}, + {"/ns/foo/a/bar/node", {0, 2, 4, 6, 8}}, + {"/ns/a/bar/node", {0, 2, 4, 6, 8}}, + {"/ns/foo/zoo/a/bar/node", {0, 2, 4, 6, 8}}, + }; + + for (auto & kv : node_fqn_expected) { + rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params, kv.first.c_str()); + const std::vector & params = map.at(kv.first); + EXPECT_EQ(kv.second.size(), params.size()); + for (size_t i = 0; i < params.size(); ++i) { + std::string param_name = "string_param" + std::to_string(kv.second[i]); + EXPECT_STREQ(param_name.c_str(), params.at(i).get_name().c_str()); + EXPECT_STREQ(hello_world.c_str(), params.at(i).get_value().c_str()); + } + } + + for (size_t i = 0; i < node_names_keys.size(); ++i) { + c_params->params[i].parameter_values[0].string_value = NULL; + } + delete[] c_hello_world; + rcl_yaml_node_struct_fini(c_params); +} diff --git a/rclcpp/test/resources/test_node_parameters/complicated_wildcards.yaml b/rclcpp/test/resources/test_node_parameters/complicated_wildcards.yaml new file mode 100644 index 0000000000..53da409135 --- /dev/null +++ b/rclcpp/test/resources/test_node_parameters/complicated_wildcards.yaml @@ -0,0 +1,5 @@ +/**/foo/*/bar: + node2: + ros__parameters: + foo: "foo" + bar: "bar" diff --git a/rclcpp/test/resources/test_node_parameters/params_by_order.yaml b/rclcpp/test/resources/test_node_parameters/params_by_order.yaml new file mode 100644 index 0000000000..680d96beaf --- /dev/null +++ b/rclcpp/test/resources/test_node_parameters/params_by_order.yaml @@ -0,0 +1,16 @@ +/**: + node2: + ros__parameters: + a_value: "first" + foo: "foo" + +/ns: + node2: + ros__parameters: + a_value: "second" + bar: "bar" + +/*: + node2: + ros__parameters: + a_value: "last_one_win" diff --git a/rclcpp/test/resources/test_node_parameters/wildcards.yaml b/rclcpp/test/resources/test_node_parameters/wildcards.yaml new file mode 100644 index 0000000000..b89b0d8cd0 --- /dev/null +++ b/rclcpp/test/resources/test_node_parameters/wildcards.yaml @@ -0,0 +1,57 @@ +/**: + ros__parameters: + full_wild: "full_wild" + +/**: + node2: + ros__parameters: + namespace_wild: "namespace_wild" + +/**/node2: + ros__parameters: + namespace_wild_another: "namespace_wild_another" + +/*: + node2: + ros__parameters: + namespace_wild_one_star: "namespace_wild_one_star" + +ns: + "*": + ros__parameters: + node_wild_in_ns: "node_wild_in_ns" + +/ns/*: + ros__parameters: + node_wild_in_ns_another: "node_wild_in_ns_another" + +ns: + node2: + ros__parameters: + explicit_in_ns: "explicit_in_ns" + +"*": + ros__parameters: + node_wild_no_ns: "node_wild_no_ns" + +node2: + ros__parameters: + explicit_no_ns: "explicit_no_ns" + +ns: + nodeX: + ros__parameters: + should_not_appear: "incorrect_node_name" + +/**/nodeX: + ros__parameters: + should_not_appear: "incorrect_node_name" + +nsX: + node2: + ros__parameters: + should_not_appear: "incorrect_namespace" + +/nsX/*: + ros__parameters: + should_not_appear: "incorrect_namespace" From 37b589dc85a2311289d80238bd13055eda25f5e6 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 7 Jul 2022 12:07:12 -0400 Subject: [PATCH 03/53] Fix the documentation for rclcpp::ok to be accurate. (#1965) That is, it returns false if shutdown has been called, and true in all other circumstances. Signed-off-by: Chris Lalancette --- rclcpp/include/rclcpp/utilities.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/utilities.hpp b/rclcpp/include/rclcpp/utilities.hpp index 5274cba33a..5b4ea86a6d 100644 --- a/rclcpp/include/rclcpp/utilities.hpp +++ b/rclcpp/include/rclcpp/utilities.hpp @@ -169,7 +169,7 @@ remove_ros_arguments(int argc, char const * const * argv); * the context initialized by rclcpp::init(). * * \param[in] context Optional check for shutdown of this Context. - * \return true if shutdown has been called, false otherwise + * \return false if shutdown has been called, true otherwise */ RCLCPP_PUBLIC bool From f6056beaa09ec2c19a6cc3b74405e2b83948f161 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Mon, 11 Jul 2022 10:09:44 -0700 Subject: [PATCH 04/53] Make create_client accept rclcpp::QoS (#1964) * Make create_client accept rclcpp::QoS Signed-off-by: Shane Loretz * Deprecate passing rmw_qos_profile_t to create_client Signed-off-by: Shane Loretz --- rclcpp/include/rclcpp/create_client.hpp | 30 ++++ rclcpp/include/rclcpp/node.hpp | 18 ++- rclcpp/include/rclcpp/node_impl.hpp | 16 ++ .../test_allocator_memory_strategy.cpp | 4 +- rclcpp/test/rclcpp/test_client.cpp | 81 +++++++--- rclcpp/test/rclcpp/test_memory_strategy.cpp | 4 +- rclcpp/test/rclcpp/test_service.cpp | 8 +- rclcpp_lifecycle/CMakeLists.txt | 8 + .../rclcpp_lifecycle/lifecycle_node.hpp | 18 ++- .../rclcpp_lifecycle/lifecycle_node_impl.hpp | 29 ++-- rclcpp_lifecycle/test/test_client.cpp | 143 ++++++++++++++++++ 11 files changed, 316 insertions(+), 43 deletions(-) create mode 100644 rclcpp_lifecycle/test/test_client.cpp diff --git a/rclcpp/include/rclcpp/create_client.hpp b/rclcpp/include/rclcpp/create_client.hpp index 1a960b8d8f..00e6ff3c0e 100644 --- a/rclcpp/include/rclcpp/create_client.hpp +++ b/rclcpp/include/rclcpp/create_client.hpp @@ -20,10 +20,40 @@ #include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/node_interfaces/node_services_interface.hpp" +#include "rclcpp/qos.hpp" #include "rmw/rmw.h" namespace rclcpp { +/// Create a service client with a given type. +/** + * \param[in] node_base NodeBaseInterface implementation of the node on which + * to create the client. + * \param[in] node_graph NodeGraphInterface implementation of the node on which + * to create the client. + * \param[in] node_services NodeServicesInterface implementation of the node on + * which to create the client. + * \param[in] service_name The name on which the service is accessible. + * \param[in] qos Quality of service profile for client. + * \param[in] group Callback group to handle the reply to service calls. + * \return Shared pointer to the created client. + */ +template +typename rclcpp::Client::SharedPtr +create_client( + std::shared_ptr node_base, + std::shared_ptr node_graph, + std::shared_ptr node_services, + const std::string & service_name, + const rclcpp::QoS & qos = rclcpp::ServicesQoS(), + rclcpp::CallbackGroup::SharedPtr group = nullptr) +{ + return create_client( + node_base, node_graph, node_services, + service_name, + qos.get_rmw_qos_profile(), + group); +} /// Create a service client with a given type. /// \internal diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 65b8797716..f9ebd4e3ee 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -246,12 +246,28 @@ class Node : public std::enable_shared_from_this * \param[in] qos_profile rmw_qos_profile_t Quality of service profile for client. * \param[in] group Callback group to call the service. * \return Shared pointer to the created client. + * \deprecated use rclcpp::QoS instead of rmw_qos_profile_t */ template + [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] typename rclcpp::Client::SharedPtr create_client( const std::string & service_name, - const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default, + const rmw_qos_profile_t & qos_profile, + rclcpp::CallbackGroup::SharedPtr group = nullptr); + + /// Create and return a Client. + /** + * \param[in] service_name The name on which the service is accessible. + * \param[in] qos Quality of service profile for client. + * \param[in] group Callback group to handle the reply to service calls. + * \return Shared pointer to the created client. + */ + template + typename rclcpp::Client::SharedPtr + create_client( + const std::string & service_name, + const rclcpp::QoS & qos = rclcpp::ServicesQoS(), rclcpp::CallbackGroup::SharedPtr group = nullptr); /// Create and return a Service. diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 086c2bb17c..b957bd2439 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -120,6 +120,22 @@ Node::create_wall_timer( this->node_timers_.get()); } +template +typename Client::SharedPtr +Node::create_client( + const std::string & service_name, + const rclcpp::QoS & qos, + rclcpp::CallbackGroup::SharedPtr group) +{ + return rclcpp::create_client( + node_base_, + node_graph_, + node_services_, + extend_name_with_sub_namespace(service_name, this->get_sub_namespace()), + qos, + group); +} + template typename Client::SharedPtr Node::create_client( diff --git a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp index e1b916e9c0..cd28949f3d 100644 --- a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp +++ b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp @@ -177,7 +177,7 @@ class TestAllocatorMemoryStrategy : public ::testing::Test clients_.push_back( node_with_client->create_client( - "service", rmw_qos_profile_services_default, callback_group)); + "service", rclcpp::ServicesQoS(), callback_group)); return node_with_client; } @@ -831,7 +831,7 @@ TEST_F(TestAllocatorMemoryStrategy, get_next_client_out_of_scope) { node->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive); auto client = node->create_client( - "service", rmw_qos_profile_services_default, callback_group); + "service", rclcpp::ServicesQoS(), callback_group); weak_groups_to_nodes.insert( std::paircreate_client("service"); } + { + // 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 client = node->create_client( + "service", rmw_qos_profile_services_default); + + // remove warning suppression + #if !defined(_WIN32) + # pragma GCC diagnostic pop + #else // !defined(_WIN32) + # pragma warning(pop) + #endif + } + { + auto client = node->create_client( + "service", rclcpp::ServicesQoS()); + } { ASSERT_THROW( @@ -123,6 +147,27 @@ TEST_F(TestClient, construction_with_free_function) { nullptr); }, rclcpp::exceptions::InvalidServiceNameError); } + { + auto client = rclcpp::create_client( + node->get_node_base_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface(), + "service", + rclcpp::ServicesQoS(), + nullptr); + } + { + ASSERT_THROW( + { + auto client = rclcpp::create_client( + node->get_node_base_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface(), + "invalid_?service", + rclcpp::ServicesQoS(), + nullptr); + }, rclcpp::exceptions::InvalidServiceNameError); + } } TEST_F(TestClient, construct_with_rcl_error) { @@ -351,15 +396,15 @@ 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"); - rmw_qos_profile_t client_qos = rmw_qos_profile_services_default; - client_qos.depth = 3; + rclcpp::ServicesQoS client_qos; + client_qos.keep_last(3); auto client = client_node->create_client("test_service", client_qos); std::atomic server_requests_count {0}; auto server_callback = [&server_requests_count]( const test_msgs::srv::Empty::Request::SharedPtr, test_msgs::srv::Empty::Response::SharedPtr) {server_requests_count++;}; auto server = server_node->create_service( - "test_service", server_callback, client_qos); + "test_service", server_callback, client_qos.get_rmw_qos_profile()); auto request = std::make_shared(); std::atomic c1 {0}; @@ -455,32 +500,30 @@ TEST_F(TestClient, rcl_client_response_subscription_get_actual_qos_error) { } TEST_F(TestClient, client_qos) { - rmw_qos_profile_t qos_profile = rmw_qos_profile_services_default; - qos_profile.liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC; - uint64_t duration = 1; - qos_profile.deadline = {duration, duration}; - qos_profile.lifespan = {duration, duration}; - qos_profile.liveliness_lease_duration = {duration, duration}; + rclcpp::ServicesQoS qos_profile; + qos_profile.liveliness(rclcpp::LivelinessPolicy::Automatic); + rclcpp::Duration duration(std::chrono::nanoseconds(1)); + qos_profile.deadline(duration); + qos_profile.lifespan(duration); + qos_profile.liveliness_lease_duration(duration); auto client = node->create_client("client", qos_profile); - auto init_qos = - rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile), qos_profile); auto rp_qos = client->get_request_publisher_actual_qos(); auto rs_qos = client->get_response_subscription_actual_qos(); - EXPECT_EQ(init_qos, rp_qos); + EXPECT_EQ(qos_profile, rp_qos); // Lifespan has no meaning for subscription/readers - rs_qos.lifespan(qos_profile.lifespan); - EXPECT_EQ(init_qos, rs_qos); + rs_qos.lifespan(qos_profile.lifespan()); + EXPECT_EQ(qos_profile, rs_qos); } TEST_F(TestClient, client_qos_depth) { using namespace std::literals::chrono_literals; - rmw_qos_profile_t client_qos_profile = rmw_qos_profile_default; - client_qos_profile.depth = 2; + rclcpp::ServicesQoS client_qos_profile; + client_qos_profile.keep_last(2); auto client = node->create_client("test_qos_depth", client_qos_profile); @@ -522,10 +565,10 @@ TEST_F(TestClient, client_qos_depth) { std::this_thread::sleep_for(2ms); } - EXPECT_GT(server_cb_count_, client_qos_profile.depth); + EXPECT_GT(server_cb_count_, client_qos_profile.depth()); start = std::chrono::steady_clock::now(); - while ((client_cb_count_ < client_qos_profile.depth) && + while ((client_cb_count_ < client_qos_profile.depth()) && (std::chrono::steady_clock::now() - start) < 1s) { rclcpp::spin_some(node); @@ -535,5 +578,5 @@ TEST_F(TestClient, client_qos_depth) { // so more client callbacks might be called than expected. rclcpp::spin_some(node); - EXPECT_EQ(client_cb_count_, client_qos_profile.depth); + EXPECT_EQ(client_cb_count_, client_qos_profile.depth()); } diff --git a/rclcpp/test/rclcpp/test_memory_strategy.cpp b/rclcpp/test/rclcpp/test_memory_strategy.cpp index b6fff57a92..732e4475fc 100644 --- a/rclcpp/test/rclcpp/test_memory_strategy.cpp +++ b/rclcpp/test/rclcpp/test_memory_strategy.cpp @@ -197,7 +197,7 @@ TEST_F(TestMemoryStrategy, get_client_by_handle) { node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); { auto client = node->create_client( - "service", rmw_qos_profile_services_default, callback_group); + "service", rclcpp::ServicesQoS(), callback_group); client_handle = client->get_client_handle(); weak_groups_to_nodes.insert( @@ -435,7 +435,7 @@ TEST_F(TestMemoryStrategy, get_group_by_client) { node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); client = node->create_client( - "service", rmw_qos_profile_services_default, callback_group); + "service", rclcpp::ServicesQoS(), callback_group); weak_groups_to_nodes.insert( std::pair( diff --git a/rclcpp/test/rclcpp/test_service.cpp b/rclcpp/test/rclcpp/test_service.cpp index 1dbb8ca9e4..14cead0864 100644 --- a/rclcpp/test/rclcpp/test_service.cpp +++ b/rclcpp/test/rclcpp/test_service.cpp @@ -246,10 +246,10 @@ TEST_F(TestService, on_new_request_callback) { auto server_callback = [](const test_msgs::srv::Empty::Request::SharedPtr, test_msgs::srv::Empty::Response::SharedPtr) {FAIL();}; - rmw_qos_profile_t service_qos = rmw_qos_profile_services_default; - service_qos.depth = 3; + rclcpp::ServicesQoS service_qos; + service_qos.keep_last(3); auto server = node->create_service( - "~/test_service", server_callback, service_qos); + "~/test_service", server_callback, service_qos.get_rmw_qos_profile()); std::atomic c1 {0}; auto increase_c1_cb = [&c1](size_t count_msgs) {c1 += count_msgs;}; @@ -378,7 +378,7 @@ TEST_F(TestService, server_qos_depth) { auto server = server_node->create_service( "test_qos_depth", std::move(server_callback), server_qos_profile); - rmw_qos_profile_t client_qos_profile = rmw_qos_profile_default; + rclcpp::QoS client_qos_profile(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); auto client = node->create_client("test_qos_depth", client_qos_profile); ::testing::AssertionResult request_result = ::testing::AssertionSuccess(); diff --git a/rclcpp_lifecycle/CMakeLists.txt b/rclcpp_lifecycle/CMakeLists.txt index a823d44a68..8a210e09cc 100644 --- a/rclcpp_lifecycle/CMakeLists.txt +++ b/rclcpp_lifecycle/CMakeLists.txt @@ -109,6 +109,14 @@ if(BUILD_TESTING) ) target_link_libraries(test_lifecycle_service_client ${PROJECT_NAME} mimick) endif() + ament_add_gtest(test_client test/test_client.cpp TIMEOUT 120) + if(TARGET test_client) + target_link_libraries(test_client + ${PROJECT_NAME} + mimick + ${rcl_interfaces_TARGETS} + rclcpp::rclcpp) + endif() ament_add_gtest(test_state_machine_info test/test_state_machine_info.cpp) if(TARGET test_state_machine_info) ament_target_dependencies(test_state_machine_info diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 6048d0d691..d0f26c4a80 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -258,12 +258,28 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, /// Create and return a Client. /** * \sa rclcpp::Node::create_client + * \deprecated use rclcpp::QoS instead of rmw_qos_profile_t */ template + [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] typename rclcpp::Client::SharedPtr create_client( const std::string & service_name, - const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default, + const rmw_qos_profile_t & qos_profile, + rclcpp::CallbackGroup::SharedPtr group = nullptr); + + /// Create and return a Client. + /** + * \param[in] service_name The name on which the service is accessible. + * \param[in] qos Quality of service profile for client. + * \param[in] group Callback group to handle the reply to service calls. + * \return Shared pointer to the created client. + */ + template + typename rclcpp::Client::SharedPtr + create_client( + const std::string & service_name, + const rclcpp::QoS & qos = rclcpp::ServicesQoS(), rclcpp::CallbackGroup::SharedPtr group = nullptr); /// Create and return a Service. diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp index 22fd7f9c08..1e8fc5f662 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp @@ -22,6 +22,7 @@ #include #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" @@ -103,21 +104,21 @@ LifecycleNode::create_client( const rmw_qos_profile_t & qos_profile, rclcpp::CallbackGroup::SharedPtr group) { - rcl_client_options_t options = rcl_client_get_default_options(); - options.qos = qos_profile; - - using rclcpp::Client; - using rclcpp::ClientBase; - - auto cli = Client::make_shared( - node_base_.get(), - node_graph_, - service_name, - options); + return rclcpp::create_client( + node_base_, node_graph_, node_services_, + service_name, qos_profile, group); +} - auto cli_base_ptr = std::dynamic_pointer_cast(cli); - node_services_->add_client(cli_base_ptr, group); - return cli; +template +typename rclcpp::Client::SharedPtr +LifecycleNode::create_client( + const std::string & service_name, + const rclcpp::QoS & qos, + rclcpp::CallbackGroup::SharedPtr group) +{ + return rclcpp::create_client( + node_base_, node_graph_, node_services_, + service_name, qos, group); } template diff --git a/rclcpp_lifecycle/test/test_client.cpp b/rclcpp_lifecycle/test/test_client.cpp new file mode 100644 index 0000000000..89e1f42fa6 --- /dev/null +++ b/rclcpp_lifecycle/test/test_client.cpp @@ -0,0 +1,143 @@ +// Copyright 2022 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "rcl_interfaces/srv/list_parameters.hpp" + + +class TestClient : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node_ = std::make_shared("my_lifecycle_node", "/ns"); + } + + void TearDown() + { + node_.reset(); + } + + rclcpp_lifecycle::LifecycleNode::SharedPtr node_; +}; + +/* + Testing client construction and destruction. + */ +TEST_F(TestClient, construction_and_destruction) { + using rcl_interfaces::srv::ListParameters; + { + auto client = node_->create_client("service"); + EXPECT_TRUE(client); + } + { + // 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 client = node_->create_client( + "service", rmw_qos_profile_services_default); + EXPECT_TRUE(client); + + // remove warning suppression + #if !defined(_WIN32) + # pragma GCC diagnostic pop + #else // !defined(_WIN32) + # pragma warning(pop) + #endif + } + { + auto client = node_->create_client( + "service", rclcpp::ServicesQoS()); + EXPECT_TRUE(client); + } + + { + ASSERT_THROW( + { + auto client = node_->create_client("invalid_service?"); + }, rclcpp::exceptions::InvalidServiceNameError); + } +} + +TEST_F(TestClient, construction_with_free_function) { + { + auto client = rclcpp::create_client( + node_->get_node_base_interface(), + node_->get_node_graph_interface(), + node_->get_node_services_interface(), + "service", + rmw_qos_profile_services_default, + nullptr); + EXPECT_TRUE(client); + } + { + ASSERT_THROW( + { + auto client = rclcpp::create_client( + node_->get_node_base_interface(), + node_->get_node_graph_interface(), + node_->get_node_services_interface(), + "invalid_?service", + rmw_qos_profile_services_default, + nullptr); + }, rclcpp::exceptions::InvalidServiceNameError); + } + { + auto client = rclcpp::create_client( + node_->get_node_base_interface(), + node_->get_node_graph_interface(), + node_->get_node_services_interface(), + "service", + rclcpp::ServicesQoS(), + nullptr); + EXPECT_TRUE(client); + } + { + ASSERT_THROW( + { + auto client = rclcpp::create_client( + node_->get_node_base_interface(), + node_->get_node_graph_interface(), + node_->get_node_services_interface(), + "invalid_?service", + rclcpp::ServicesQoS(), + nullptr); + }, rclcpp::exceptions::InvalidServiceNameError); + } +} From 64e4c72791811e25ee7780fc84b07dc70f36a4c9 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Mon, 11 Jul 2022 17:41:40 -0700 Subject: [PATCH 05/53] Make create_service accept rclcpp::QoS (#1969) Signed-off-by: Shane Loretz --- rclcpp/include/rclcpp/create_service.hpp | 26 +++++ rclcpp/include/rclcpp/node.hpp | 20 +++- rclcpp/include/rclcpp/node_impl.hpp | 17 +++ .../test_allocator_memory_strategy.cpp | 4 +- rclcpp/test/rclcpp/test_client.cpp | 6 +- rclcpp/test/rclcpp/test_memory_strategy.cpp | 4 +- rclcpp/test/rclcpp/test_service.cpp | 32 +++--- rclcpp_lifecycle/CMakeLists.txt | 8 ++ .../rclcpp_lifecycle/lifecycle_node.hpp | 16 ++- .../rclcpp_lifecycle/lifecycle_node_impl.hpp | 13 +++ rclcpp_lifecycle/test/test_service.cpp | 100 ++++++++++++++++++ 11 files changed, 219 insertions(+), 27 deletions(-) create mode 100644 rclcpp_lifecycle/test/test_service.cpp diff --git a/rclcpp/include/rclcpp/create_service.hpp b/rclcpp/include/rclcpp/create_service.hpp index 9aaa02a1ed..42c253a526 100644 --- a/rclcpp/include/rclcpp/create_service.hpp +++ b/rclcpp/include/rclcpp/create_service.hpp @@ -26,6 +26,32 @@ namespace rclcpp { +/// Create a service with a given type. +/** + * \param[in] node_base NodeBaseInterface implementation of the node on which + * to create the service. + * \param[in] node_services NodeServicesInterface implementation of the node on + * which to create the service. + * \param[in] service_name The name on which the service is accessible. + * \param[in] callback The callback to call when the service gets a request. + * \param[in] qos Quality of service profile for the service. + * \param[in] group Callback group to handle the reply to service calls. + * \return Shared pointer to the created service. + */ +template +typename rclcpp::Service::SharedPtr +create_service( + std::shared_ptr node_base, + std::shared_ptr node_services, + const std::string & service_name, + CallbackT && callback, + const rclcpp::QoS & qos, + rclcpp::CallbackGroup::SharedPtr group) +{ + return create_service( + node_base, node_services, service_name, + std::forward(callback), qos.get_rmw_qos_profile(), group); +} /// Create a service with a given type. /// \internal diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index f9ebd4e3ee..54a4b922ab 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -277,13 +277,31 @@ class Node : public std::enable_shared_from_this * \param[in] qos_profile rmw_qos_profile_t Quality of service profile for client. * \param[in] group Callback group to call the service. * \return Shared pointer to the created service. + * \deprecated use rclcpp::QoS instead of rmw_qos_profile_t + */ + template + [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] + typename rclcpp::Service::SharedPtr + create_service( + const std::string & service_name, + CallbackT && callback, + const rmw_qos_profile_t & qos_profile, + rclcpp::CallbackGroup::SharedPtr group = nullptr); + + /// Create and return a Service. + /** + * \param[in] service_name The topic to service on. + * \param[in] callback User-defined callback function. + * \param[in] qos Quality of service profile for the service. + * \param[in] group Callback group to call the service. + * \return Shared pointer to the created service. */ template typename rclcpp::Service::SharedPtr create_service( const std::string & service_name, CallbackT && callback, - const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default, + const rclcpp::QoS & qos = rclcpp::ServicesQoS(), rclcpp::CallbackGroup::SharedPtr group = nullptr); /// Create and return a GenericPublisher. diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index b957bd2439..5b427b5b25 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -152,6 +152,23 @@ Node::create_client( group); } +template +typename rclcpp::Service::SharedPtr +Node::create_service( + const std::string & service_name, + CallbackT && callback, + const rclcpp::QoS & qos, + rclcpp::CallbackGroup::SharedPtr group) +{ + return rclcpp::create_service( + node_base_, + node_services_, + extend_name_with_sub_namespace(service_name, this->get_sub_namespace()), + std::forward(callback), + qos, + group); +} + template typename rclcpp::Service::SharedPtr Node::create_service( diff --git a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp index cd28949f3d..1779d5d88e 100644 --- a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp +++ b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp @@ -162,7 +162,7 @@ class TestAllocatorMemoryStrategy : public ::testing::Test services_.push_back( node_with_service->create_service( - "service", std::move(service_callback), rmw_qos_profile_services_default, callback_group)); + "service", std::move(service_callback), rclcpp::ServicesQoS(), callback_group)); return node_with_service; } @@ -793,7 +793,7 @@ TEST_F(TestAllocatorMemoryStrategy, get_next_service_out_of_scope) { [](const test_msgs::srv::Empty::Request::SharedPtr, test_msgs::srv::Empty::Response::SharedPtr) {}; auto service = node->create_service( - "service", std::move(service_callback), rmw_qos_profile_services_default, callback_group); + "service", std::move(service_callback), rclcpp::ServicesQoS(), callback_group); node->for_each_callback_group( [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) diff --git a/rclcpp/test/rclcpp/test_client.cpp b/rclcpp/test/rclcpp/test_client.cpp index 01b7d1c85c..a9d81d9176 100644 --- a/rclcpp/test/rclcpp/test_client.cpp +++ b/rclcpp/test/rclcpp/test_client.cpp @@ -404,7 +404,7 @@ TEST_F(TestClient, on_new_response_callback) { const test_msgs::srv::Empty::Request::SharedPtr, test_msgs::srv::Empty::Response::SharedPtr) {server_requests_count++;}; auto server = server_node->create_service( - "test_service", server_callback, client_qos.get_rmw_qos_profile()); + "test_service", server_callback, client_qos); auto request = std::make_shared(); std::atomic c1 {0}; @@ -534,10 +534,10 @@ TEST_F(TestClient, client_qos_depth) { auto server_node = std::make_shared("server_node", "/ns"); - rmw_qos_profile_t server_qos_profile = rmw_qos_profile_default; + rclcpp::QoS server_qos(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); auto server = server_node->create_service( - "test_qos_depth", std::move(server_callback), server_qos_profile); + "test_qos_depth", std::move(server_callback), server_qos); auto request = std::make_shared(); ::testing::AssertionResult request_result = ::testing::AssertionSuccess(); diff --git a/rclcpp/test/rclcpp/test_memory_strategy.cpp b/rclcpp/test/rclcpp/test_memory_strategy.cpp index 732e4475fc..e96729a2a1 100644 --- a/rclcpp/test/rclcpp/test_memory_strategy.cpp +++ b/rclcpp/test/rclcpp/test_memory_strategy.cpp @@ -155,7 +155,7 @@ TEST_F(TestMemoryStrategy, get_service_by_handle) { { auto service = node->create_service( "service", std::move(service_callback), - rmw_qos_profile_services_default, callback_group); + rclcpp::ServicesQoS(), callback_group); service_handle = service->get_service_handle(); @@ -396,7 +396,7 @@ TEST_F(TestMemoryStrategy, get_group_by_service) { service = node->create_service( "service", std::move(service_callback), - rmw_qos_profile_services_default, callback_group); + rclcpp::ServicesQoS(), callback_group); weak_groups_to_nodes.insert( std::pair( diff --git a/rclcpp/test/rclcpp/test_service.cpp b/rclcpp/test/rclcpp/test_service.cpp index 14cead0864..a3c361cfde 100644 --- a/rclcpp/test/rclcpp/test_service.cpp +++ b/rclcpp/test/rclcpp/test_service.cpp @@ -249,7 +249,7 @@ TEST_F(TestService, on_new_request_callback) { rclcpp::ServicesQoS service_qos; service_qos.keep_last(3); auto server = node->create_service( - "~/test_service", server_callback, service_qos.get_rmw_qos_profile()); + "~/test_service", server_callback, service_qos); std::atomic c1 {0}; auto increase_c1_cb = [&c1](size_t count_msgs) {c1 += count_msgs;}; @@ -338,28 +338,25 @@ TEST_F(TestService, rcl_service_request_subscription_get_actual_qos_error) { TEST_F(TestService, server_qos) { - rmw_qos_profile_t qos_profile = rmw_qos_profile_services_default; - qos_profile.liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC; - uint64_t duration = 1; - qos_profile.deadline = {duration, duration}; - qos_profile.lifespan = {duration, duration}; - qos_profile.liveliness_lease_duration = {duration, duration}; + rclcpp::ServicesQoS qos_profile; + qos_profile.liveliness(rclcpp::LivelinessPolicy::Automatic); + rclcpp::Duration duration(std::chrono::nanoseconds(1)); + qos_profile.deadline(duration); + qos_profile.lifespan(duration); + qos_profile.liveliness_lease_duration(duration); auto callback = [](const test_msgs::srv::Empty::Request::SharedPtr, test_msgs::srv::Empty::Response::SharedPtr) {}; auto server = node->create_service( - "service", callback, - qos_profile); - auto init_qos = - rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile), qos_profile); + "service", callback, qos_profile); auto rs_qos = server->get_request_subscription_actual_qos(); auto rp_qos = server->get_response_publisher_actual_qos(); - EXPECT_EQ(init_qos, rp_qos); + EXPECT_EQ(qos_profile, rp_qos); // Lifespan has no meaning for subscription/readers - rs_qos.lifespan(qos_profile.lifespan); - EXPECT_EQ(init_qos, rs_qos); + rs_qos.lifespan(qos_profile.lifespan()); + EXPECT_EQ(qos_profile, rs_qos); } TEST_F(TestService, server_qos_depth) { @@ -372,8 +369,7 @@ TEST_F(TestService, server_qos_depth) { auto server_node = std::make_shared("server_node", "/ns"); - rmw_qos_profile_t server_qos_profile = rmw_qos_profile_default; - server_qos_profile.depth = 2; + rclcpp::QoS server_qos_profile(2); auto server = server_node->create_service( "test_qos_depth", std::move(server_callback), server_qos_profile); @@ -398,7 +394,7 @@ TEST_F(TestService, server_qos_depth) { } auto start = std::chrono::steady_clock::now(); - while ((server_cb_count_ < server_qos_profile.depth) && + while ((server_cb_count_ < server_qos_profile.depth()) && (std::chrono::steady_clock::now() - start) < 1s) { rclcpp::spin_some(server_node); @@ -409,5 +405,5 @@ TEST_F(TestService, server_qos_depth) { // so more server responses might be processed than expected. rclcpp::spin_some(server_node); - EXPECT_EQ(server_cb_count_, server_qos_profile.depth); + EXPECT_EQ(server_cb_count_, server_qos_profile.depth()); } diff --git a/rclcpp_lifecycle/CMakeLists.txt b/rclcpp_lifecycle/CMakeLists.txt index 8a210e09cc..7a5b4cf2f0 100644 --- a/rclcpp_lifecycle/CMakeLists.txt +++ b/rclcpp_lifecycle/CMakeLists.txt @@ -117,6 +117,14 @@ if(BUILD_TESTING) ${rcl_interfaces_TARGETS} rclcpp::rclcpp) endif() + ament_add_gtest(test_service test/test_service.cpp TIMEOUT 120) + if(TARGET test_service) + target_link_libraries(test_service + ${PROJECT_NAME} + mimick + ${test_msgs_TARGETS} + rclcpp::rclcpp) + endif() ament_add_gtest(test_state_machine_info test/test_state_machine_info.cpp) if(TARGET test_state_machine_info) ament_target_dependencies(test_state_machine_info diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index d0f26c4a80..2978a5ecaa 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -285,13 +285,27 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, /// Create and return a Service. /** * \sa rclcpp::Node::create_service + * \deprecated use rclcpp::QoS instead of rmw_qos_profile_t */ template + [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] typename rclcpp::Service::SharedPtr create_service( const std::string & service_name, CallbackT && callback, - const rmw_qos_profile_t & qos_profile = rmw_qos_profile_services_default, + const rmw_qos_profile_t & qos_profile, + rclcpp::CallbackGroup::SharedPtr group = nullptr); + + /// Create and return a Service. + /** + * \sa rclcpp::Node::create_service + */ + template + typename rclcpp::Service::SharedPtr + create_service( + const std::string & service_name, + CallbackT && callback, + const rclcpp::QoS & qos = rclcpp::ServicesQoS(), rclcpp::CallbackGroup::SharedPtr group = nullptr); /// Create and return a GenericPublisher. diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp index 1e8fc5f662..88d981e051 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp @@ -134,6 +134,19 @@ LifecycleNode::create_service( service_name, std::forward(callback), qos_profile, group); } +template +typename rclcpp::Service::SharedPtr +LifecycleNode::create_service( + const std::string & service_name, + CallbackT && callback, + const rclcpp::QoS & qos, + rclcpp::CallbackGroup::SharedPtr group) +{ + return rclcpp::create_service( + node_base_, node_services_, + service_name, std::forward(callback), qos, group); +} + template std::shared_ptr LifecycleNode::create_generic_publisher( diff --git a/rclcpp_lifecycle/test/test_service.cpp b/rclcpp_lifecycle/test/test_service.cpp new file mode 100644 index 0000000000..4084b0da0c --- /dev/null +++ b/rclcpp_lifecycle/test/test_service.cpp @@ -0,0 +1,100 @@ +// Copyright 2022 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "test_msgs/srv/empty.hpp" + +using namespace std::chrono_literals; + +class TestService : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node = std::make_shared("my_lifecycle_node", "/ns"); + } + + void TearDown() + { + node.reset(); + } + + rclcpp_lifecycle::LifecycleNode::SharedPtr node; +}; + +/* + Testing service construction and destruction. + */ +TEST_F(TestService, construction_and_destruction) { + using test_msgs::srv::Empty; + auto callback = + [](const Empty::Request::SharedPtr, Empty::Response::SharedPtr) { + }; + { + auto service = node->create_service("service", callback); + EXPECT_NE(nullptr, service->get_service_handle()); + const rclcpp::ServiceBase * const_service_base = service.get(); + EXPECT_NE(nullptr, const_service_base->get_service_handle()); + } + { + // 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 service = node->create_service( + "service", callback, rmw_qos_profile_services_default); + + // remove warning suppression + #if !defined(_WIN32) + # pragma GCC diagnostic pop + #else // !defined(_WIN32) + # pragma warning(pop) + #endif + + EXPECT_NE(nullptr, service->get_service_handle()); + const rclcpp::ServiceBase * const_service_base = service.get(); + EXPECT_NE(nullptr, const_service_base->get_service_handle()); + } + + { + ASSERT_THROW( + { + auto service = node->create_service("invalid_service?", callback); + }, rclcpp::exceptions::InvalidServiceNameError); + } +} From b953bdddf8de213b4a051ecf2d668dad65ff9f89 Mon Sep 17 00:00:00 2001 From: Deepanshu Bansal <36300643+deepanshubansal01@users.noreply.github.com> Date: Thu, 14 Jul 2022 18:22:48 -0400 Subject: [PATCH 06/53] Support pre-set and post-set parameter callbacks in addition to on-set-parameter-callback. (#1947) * Support add_pre_set_parameter and add_post_set_parameter callbacks in addition to add_on_set_parameter_callback in Node API Signed-off-by: deepanshu --- rclcpp/doc/param_callback_design.png | Bin 0 -> 167646 bytes .../doc/proposed_node_parameter_callbacks.md | 29 +++ rclcpp/include/rclcpp/node.hpp | 241 +++++++++++++++++- .../node_interfaces/node_parameters.hpp | 33 ++- .../node_parameters_interface.hpp | 71 +++++- rclcpp/src/rclcpp/node.cpp | 32 ++- .../node_interfaces/node_parameters.cpp | 213 +++++++++++++--- .../node_interfaces/test_node_parameters.cpp | 120 ++++++++- rclcpp/test/rclcpp/test_node.cpp | 228 +++++++++++++++++ .../rclcpp_lifecycle/lifecycle_node.hpp | 56 +++- rclcpp_lifecycle/src/lifecycle_node.cpp | 28 +- 11 files changed, 991 insertions(+), 60 deletions(-) create mode 100644 rclcpp/doc/param_callback_design.png create mode 100644 rclcpp/doc/proposed_node_parameter_callbacks.md diff --git a/rclcpp/doc/param_callback_design.png b/rclcpp/doc/param_callback_design.png new file mode 100644 index 0000000000000000000000000000000000000000..485b5b9532a75c7c0a0a73ca0492efc77b732314 GIT binary patch literal 167646 zcma%iXIPU<*Df6qX`+IHR8f&ms7kMa0HFth^b!K3Kqw(dvw?*sAc_bU5S1pq7ey3B zstPC~D7`Da=gb3pf8YC_^XKfjwh1YdnKf%xx$m`Zn41~09XxT6hK7dC2(D*IL&KOs zLqqrS026qn!Ro6sb3d%FCwMUQboV7dU(xjrAmG3&26D3U(z4J1FIu^J zxdveWV+v^Ho`J5I{Z*p_VESI}!O8(vUPka>lo`g-({6vY!JYvmECIj&F*#K*9rz~n z<-ky)=l-i6p21jm@PqPVFnu7ne92Dw6L%?Fu)o5BCUu%y1w2Rm@m=;ho;!t>*JKna7G69J}9D{x2c7N zy%{k;6>Ei1@(XlTG*t>F;X`#1axkJVQqIQ+siJ~`wiAFy8zcQyY!Ox#cwfA>8^O&C zWkuHUhNIoKAruw#_I(jJkU{9i>D$iPxo zMN!oVhE&qW_=FnRQsivN_GC{vtgQu3-`&@T;P0;+gt1j1$SGnKFxI-BHa3PfSabIf zHx)NMUp;Rnd0%fl9Q?e2VIbB-S>H}Uo@7Mu_CU&ML#t5m@WdG*!P`V*4{sEB#Xufm zgqD{j7{TSRHWqM_8%oF1T-yMlPqHA86%awF0Am<}9O&h$Yi?yga+AXl{1q+nWQ?kO zfC&x~V2FaY1;*Z5SJ}WHM$tF&KpO`^2d88LM;POXp0+rgg;%JptDFhh-!~Kot;|oG zVnu+KV5>^;!IE(LFiWB(n8_HUgK_iq@RcQ4tJ((H+klspv=Isvte=@LLEqNQ3Qy5- z$68}BK6-XIZ?q4=#tUz)g0u-yG!C?oBkIZqxa;W?;lV@-0;c2>Y8I><8ldQ|Y#SPY z3bry+4D|B!Q-B$Tg!sZk0^Q{0@jA+OcpEGCU<-u+D_v+oNDoDG@S{ed;7%e9zzvtz zAsMKe%0ddP=nH+q$Ir?bE=zDF>X{=E7P4d=UzE4E57yY)#G7Ih>Zan2B`A<6Xj>Ii zPhI;!?OF!%#>k6oWr{NPSGM&vHBkyw^7n;>g2@$CLyZ-{to}eF3=F(vamL7C zMOPCGCEHNjP&nEQDX&kqRJ2rkn89siJ&2yVa!5rTe;gd)p=WB2$GPK? z3VvjHJ!2I;g0E4iz9kB)kFs<(K>1rLDF$Ql_9#qdc(Mt1rhSdxK- zj}94Q73vY{X66pV7$RW`2v;+hmm@W z7y=pMi?cPsm;@k@R!AKstg4QUzO{9b5>^LNE)!WVJsUjA*VG)GsT^ES$=ps3E~|jI z^nfcU__+mA(4izU&~`8#$<#t6NRJX^LJ76i4R%N2gVBlzNX>B=e{EG1T2~kELv|zS zDSPSKDPl4BV7#%ikB5&b%-jmD6O1s0tGbg-+;Mj9R$g#j52RtR4c5aR;f=Hhve5C< z^>%}VZj8_kRFWfLv9gM;p(ItZA_@ny^3_FH;qZaxCjfdI0fe-7U{QbNF zP0V5Xs%RY_WmS|4v~62LAe?9IV z;dzz4%CHNqRl+LkfcB& z3?u7lZDtNxAQhYi4m_ac@L;`u7&5e%AcTs$ygtSN++aCnJjN@~Gms+Zi#Jf!(RTGx zR!~9tfYrm`2z?z>lz}H%#}0hS8?S@ZM(a>KOyF{I_O4{KGDcY&W~-y1r>lyV2fq+N zP;j@S*!kP27-EPfa!9^ z+(SM6^u7FjC;>QSBX3Jnd0$1>V3lCBt4*+>o1UyP_^7>`0txte16$zM4T60=l!N@0 zK`f%91X+F+JrF8@$A6=Rz0d*t{ZH5h(`VcCB+}3b(HQAzTa%q;A2QzKZ&|IB;5?-z zboJ`6z^SU*+S<03U%%&}|0w<;Av-z;1nzFLByyf=y>_Y7VXgP)&&n7sO48% zR&i!0dDn;g!8NdeMRd@C7Sk?RJi+K2maeG~>Al;v z-F=oQP8IoQJc)XnebhS~?dO!Z{qEvM5a0R0$;pVF&9RPiwv0C%6U=sos5@C!)bH#G zHXV1I<(ltqpK6T%No=JarLVb@HHvvCJBw#NXp}wbyX?eDk(vy8lvPGI zqxKX`7cfV|b72iD4SN+?1o!excIcm^A3-6(jNcnpk=Z&p74G0QHC zX_M;DcXpd+cDVQ6hdvXpT;#KZ@^ebvKBY-JF+FqmTg>d!((f@*rTZf-D(Fc`FZZi+ zN`H+a?l_CiTCO^n6rIxiP*f^C88m#1^fs zNl1ZR{i$p^d==z)3qE$7O4+Zj>$>|-V!=;`+wA}JNnRW0v^{0u_-6RDcWzO}y)cq% ze;2Mg#xE?y`AvD<=dV7dzfT^9Gt0NkLW&ajD2Kkl0C#)LdGwzV=u$|(MBWnkd&Oc-OOw!Ofzs=;ypZh?C#uzlqTQHq+tmW|PXEnKkGwSnUGlH)gvHL5WlxjDeeshzGT}|gCYe<|7aWqc!Hv5L zdD_Fqo*!IVahjno;3(7R+SJ;AvaZ=zl$9y#lOS+* z_+|h3C$Nvu^|WtNJ!7T|9=_E?$2%6m%y{Xf>7=nIV++=LS3LyVD}_=clfc-hmgXijA<%PjLbNB>rM06yEze5 zk^QZHbeU0pJE{yFQ&Y~Sju9}oi9oI=%99fJRtBxk!Z>zywr$^V?g?vbySpGK$1`xK zA)J(;ozJcitbSh6uU|HIxBR3)UWQns8+R5Ec+fIS^|o^k`$y{0Qp7*H5B3^KU7&^v ze=+{$vclI(xwsS~7(uKLTbnQ2;#euV7Y+w1`Bm_3;Tfu>-$T(2qllCB)UVqX^V{8x zk<^#I#k{*fs=9TyuMfhHg|vSEbBuHrvcW5;eKieQzHGgV+?AU`{Dypk2aAVZi}-%=PeniJX6*iZ4qAvaO{{cRDR zjkaO89EU4MGtKA1dz4m#9EpGW)sm*&^R%*N6Old5hF^Zs*h6`0V*$+UT zFFUe2-;h}zE!e6#aSsmTC!dNY{k+~>(_P%Fn=fi`n^;d-nJ#Ox-;rPfO9qAq=tpv- zIQzCoW64l|Fcybgb%@_V_eI(k-a!Gi`NttI_A&1n$Dkd|U>b`G6_x9DK(ZjS zo%7dW2kG8bhC|S}Nly>S@XS`~&PO1)^X8Ljc?`unh2`pbkrjBA{73K6NGb{#;98?k>yv{s_Vi^MpCQs>lT%mHF_i z|DF}hEq?8e?W2vS{dGb>+qCKK9tfVbkE&#u+|?YkkkGW z_ryl7{IMiP7ke_He?_6;gfnf}KVz$db+&+yea7Guy2xJ1uXhWW|Hc3H!h7C&@#;-t zaL4wY^RyEGG4eWC^W6lk?FT;Z8>O2?91IzGZCxdPz4wopH)I+2ka+T2T@(!V0P8PX~oQ)Puz z|B5U5aC*MifzY1Qr#u*#T>JZ`#E$_&<6Okf3QBq9i&d>>nq%)t@^CahUUl4i2VbzY zzP;2rcpKI+IkU92{2966b3|`S3x=&(J|WLLFDo zkCM%_hKQ=;C#!5;>J|JojnD<2BY*R7ZDtGA;(g6-={z6F%IA=P8hKfd+cagy8|;ig zT`PWfA`ir+vlCg7!PCwT#SZUv7u#%6AByf>1eZq!uC-hJKXS@>GW(SD?fvlUD}$J< z@NuuK@L=UO$x`c+Rozbww;F?HdZj`HT?!0E8+TS8@I`L_eOiX8FD%d8OjDNxfn>FU zPDKA4hvxS1=IY!)jq|&T>+C0|tDLel!heuD)hB2^%gwEUoriR4QtKSt;}m~r?*2_M zdF?SMLk#J23<>Jg*vfwpHebE7mLE{uTiU+>?A(m@#^y}XLSfDH%a;+}S>dbxeNJHo z)Gd{f+ati_O^un}ce%{+16MgXe)nR4(dY0>8)In#r>rzKCq>O(>YwY`83E!~xl3(RN39}Sn9$VCcm1etEwEA#ip<(FqhX&1*8 z1Fy;GV#B7gv<6K7`@xW*aDnv0s;uO&wZRZ|6|wWj8|#;Q5VcDk7dqR|&*T?0r7W|@ zoD9AvDw1}fA0bo!rJ$_GSfhj|v7PFf^M$;#(l^*`C^+X7Hm69vDnBI}{LPc666nK5 zB=^Y-Nml8qM8KY!7;?ggCD`p7oI*o^DxkItxK1qx?O4D zlV;N2e)oh8jpP$o&W??SgnY-I>C5Yj4A`cl1`ptr*NP%{+EymJoBt{|yjNVZ^pULm z5ioxrlj)C;Do+X9oXQ=DYO{E1P+d^*{bR1wH`kbxmjYFXH~utptd%e>C*!Zcr@x|{ z2iKko469MMmujudR9j9Gn`rS}kF1{w-3mnN{=8uw1lNC7V`te)`I|?w zs*A?ZeUCdv=oz)eR?!BIZ2HFu%RHgiV&4TCvpo^uaU#2}eMV|-T#2?pIes;46AT?q zimzzcS#=PUYJAPaw;p^ZgQ!D0cqru5@I~<>1spfq9g4>=j?@;rA&EDt?z07#kG?ur z@ft)_nzVaE-8H3*uD!8koN+I*dBvb-2OVTQI7n=9TrN0B`>3Ux5`~kQmllmG!GYD@ zlCEi`_jq#CFrI%(Y3LG(L!9=mm5JQ7EwF!ewNWnL9u1c^R>@aU%(Yssi-(pX*AES2 zPN;D3DSguQ)tb5MfS-DNCMqpOo#!GkI;M{KWME>6W6 zGo`vN;IjP**0~(Xe0g*iD}Le)dRq|gl|nPWD?)72<%!`{G|A1 zmCy~F9*hHq`!k!M#t*}DDEbaHfq1p5VgawYXJBzE=-!`P4U>b^^DW;{p~I1pF&fumiWz9aX(*NLGs zJ+Dqh!#3%J&@^+vs6@uS52H6AA-AHM2wXJo;F-^P)rK$Pm| z$k9@WaTK%zmn;05NCq`;Nsi&Jl3?3qjnyFF*Cvk*kU`*-6;<``XbTUl=8CZV$31sj z@IC48E3d!|zOo;<<%k41Z3lfvBXzg7b|N!O?F%FNXHcJ`#`n;n@U@jkcf38HToI@3 z&kSGvwi`ktV@__0qSZKex2|P7j+$#t-=Wt`r+njJ#Po~YOO7HDwDsUvpHun7vNI2e zE5_a%6T38vyK#sKf7pVR_>lbj#?FH7^+VY3Zvw>hut%feJXN0rPPTqTaAUKM9OuK8 zB}M)n@+)m0A1GkTpuMhVZKab<5U|s?t$XBO_xIh>+NZE0a>bRQ-U^59K`V& zJ(OqyFvuNOT!)o$^vgH2&Me*^I_{t)KXULk(3>@I}{AvY~735}G0sXJY z@`&!umYQF$@J9+(KydLt5zbf~0sg|GeSrQH?TtEF&Ocl4_z2`b{50~iN4=|7zx!UW z#O9ZLJg0GvLfu&l!DRSfCsgF*wCK7TD-LhJW)@sO>vS+NC77o0mP|ucOv>ZzXjo`s zwNg~d7fxEI!#Gy+oJNpC;2Arj2LJq4ZwrZT%Vv8_UN`biQ zeEpgeRYz08PVC9O@^degVwKnv>jZ{Pj>xZwUre*+t=s16x&(ssisSfsu5Z`UWa*yM zc4~!e>YO|DPPDCqp2%dDeDE6F|4C1D8x4kCk~geL(_;LWx$Cu~wtuFKbf}BAe2X$I z+tI)T2-D^X*iAS?;pDrznguDb~3c{a`<9N+bniOLTEz>?-E zm#=C4E7>5Wph<{2KYC)!f8D1Aisei3vHNlD(Scz^#MlQJ8cdJWRsIjzjxHT4>%&*V zPx-#$ctX(xKKxTh(#y*VtfTA^3-xR0rvWca;ypz|u5`8* zUnJ)&dz$XYI$AneOX&SvbX?Sq3B47TW7mLCx4cMGe`A}Skr=l4;S35$7Tt51#;H?v zf}bPqbJGThNtU^WwGFZl3AZqW!5Rb)Zag>Ivfv<|e#r7d;q1kb!JxjC0EU$0w)?GE zr%MN-&SV9aB>ueW?L91aEtuwGXw!WPzi}6VT^K&b72v|n#iqt=eod!_OXQc)ge&7; zkft%~bE43zHV0%Xg<)%Cz(9bGtD~5WB;h zUdn*v2qZ*3jXz%4Fke3YyLeN0BQ0S278Iq&Lby&Io6O-?(}-D0dv>I~3>(8HUAj@I zmc~z-Cm%WyHyLSUUYxpfjprj5i^)gE9@w8Z8cr__h5O`&?^#N{Q7vn+<4Qf0&2WIE z&Rz;H(l(->NW6K4dgje}N17s<1)7ZLZ=;#dM#Iy*z2=#l%mQV_j{m9~iN8=IQB)_t zmJ_Y##I4Wv=k%sBiwTzX5Boi_A-V1zJ%P_iElJgkg!9v9vud){n?ix7B&hQb*VI)d z)_tg}OE?g;%YF!x5qtjh!H3)JN1|ZN4B4k1t8M>j-W`j)%_c5S^F>dKK1KH`467XL za=PH)h`vG)&6cZ)O2(-)n0z(xJ`PWNrVvg^JI9vRK$?r{yo7x?ls}_6Q{%|hLH+uf zmkyCv7e&tDe)aJ_0WzllC?#^(_MgBE64^c~5t2YaJAWq{T%2&LDM6S&90As8`^+r4BnLVE4}H%XIwXIhUHOEZu1MLz9Gt&QOkVn)t9a zoz6KyJpKFFtLpDQdF6CYtK_;b>{Z8~5N{mtQm5%+cyfA4Hs*luZbX|@<@y14OJDOd ztSS@XX%VC>EqhBHS%b>K;0FWbkvR20#=<+NpP<<>^D8&bWd5Pa)M*$2vhQdP8CBRE~pmJF6fyjC!fz|5$;KA zrgN%kEf*iUv=oRn#KRD@6-9{NX?tz*X~O2Dw@6(wP@s!X>G|3D!)A*G?$OT@ z8aBKl$=WHTvtLBx4$`ETiHGKh&RvOUxtU+_(@=diZ`ac|UZh`Q?63n!TpM|}D)CZM zQcpt>Sf?BQk3F_}SJ&Bd1E-w_y8pjpV^+TS1kK$lT}z+a{Hi62bVcdU(8_kjGu?tK zMg7-NixP$rP45>PsXq;8?)(%=i4mZ*>Y5)EB7>+uB>TIN8}D!ph@QiUp}--K&mwBB zr=py`<$f00p?h&O>Y-i{l<)}{Tdki|F-=hry3^WU+Ht<^+6-6tp|^~e=~6+Y?U~g> z*N?al@`i9tn!8OGU6i<*(wrzSVzOt78XID=X+7vlUTA{6Xnldc7$_+6u z9!dI_^klIjWD6k~i&MWtKy+Erk4H&pd1)LGQnGm6%C|w2mBjZ)Ig0Y+ zGHq#VM&sv5ONTC|l{eu;-XKZoId$u19w4U-l z_rALQk`_a$0f{w57Jg*Lflxn2d)A^Y`-jjNBjX>0hP#NfE){;wAM)3#eyqCI3z|4$*bBdw_mKR}}66yJmqX*ZJ5 z@qooJrGV{FtCSKS$T3Ady>Ri?#je|YBlHJy36r*ldYlli;;@6ArNA4GFBe%4s|8MW zb$|Rj(Ca<(jZ<7qFQ++q`zG#)*nYaMzuN*KUVF~gc^{sVP~t$3VZyUZxcHRRSVvGjxGk-ry6sas~(o4p02+l_u1D6ulrJ0Rpb+Ix8tVr^%%>|Lt zZ!rMca;%&%01%0$-&WqT($ExzP`&?!&M~LD_qd~>5zhz z(X25rE)+$^k5L&ixZU#&DU5Fm_$C3sFWh(iBEajBO-0Txn6b8+(o{(>F8HRA62h6_2`}!k`@whD9WE>sX740 z5oT!!L`AMj>>+~}HnrMn03lSs+XS&P6qvp zq*g)yJ3DB)Q;eq|#Jvw7?)qTlZ0y_u6S{}XfUE`@vtpdhkF)i~U1VGY_qJ}AN}r$q zZ)6=C+V(QH{YhjmTg*{wyBQYm17Ijktb?1dzo9dA(};)R*7Gx z?$6~1?(wPtYqU=d6ILS^dJ7y)zG^)T^=|@U{Pq*a-G3 zw{3a?3yS~dnHIFOwN^xFmRlw4QL=7l@30@qnE6>BRv2fhQM>oW4GFJ;4>N;`8NfA6VB#(VO9pzF~pJJ9+#k zKFByY769xkdG}~?>3+lw!O7r;1~Klq5BB9E1(hKv0XVhl;-=8auE$TeR;=P*uhLoN z96DR^7{WgL`Eb5ra3^rDOZTFj#@|g&K=0xp96r4zLXa{kyavoc6@+-2!rB#Vl=p(7q*^l8{qDyBh|E_M0r8(CK>t3%CyKDER z0Hir|9dy5OK`tf(oc0UK!S!C>%-`L;Rj&R4sl~=(w;~p-zMoLFEd~zX6&A=7^f^Y= zF^)^n*lcCvm2HBMAGSeaHeX+K68r7^)nm0zrTqlRH6?GCHS}y6&*TA~eCSm#0QuiH zrd02KM$ni4;^h2zA66#ZbG`Mv_PfGdmdXqk<<%;H76aHG%18>HD*JijRTwfxa-2TX zZfKwQw-#WJ*p3-bh|ko_pBV(IM}@}Ctw#C2NU)K;;Y5e#Jo2CkSll9k3q6OHFgd(8AXeT1WxV}(6RP~}-wqF48W5Zvryy(r{SeTR` z?VQ=WVHqRIsg_SIReIx!k6l9l!M@IA%sO_7B2MZ#s1SMN0gqc-(I>W;C=#xVH|$aY z#2TzP%Uu9mnr1;(c|`A~{mg_xVOHBk>c8lD7>jqag%Nh4xcAtoBRB7ss;E zi9XurPs_mFJM(&16{vS%)VtNc_IU_f<2cDquZKMOD+-Y*ut>Z2qQUBMMek5sOTV|4 zz)1|E(P)FUxdG+MzT%VD%Kw9q1IYD1)a*u@u_MIpc?f<@(}RG1*f(arcmYGy~XhH2l0T) zdwSNAp+a1vbL39>g|#1~L4cKx86*hIjXw-=T5IOetODFf8?OATEgk}C09^KPrmx6( z{*)EY=~4Sqr{>V0|NE2pJ4UDd8@Cs4ZH+%5{D2Tp>8jz&FLC4TFqCt6dF|hKe1qIb zwO`}+F#D{LoGS7iA}D55HsdnsJM&TtRT{p|>oaer-sxUH_Mj3n3M>Cs zBm|goUq5{)?@9uo??Gw9qaJP=edPls2?58_ZB^SEkMJdUCH-TOCd%0Dc5G%T4_hK8 z(tV}-@=!7mdlS~p*s5bIoKV3?H%(%vf{upXM;E6_<;oP7%J|7IanC($&$;NCYQ zsU^ymG1YfaB7gX2#1@XatQ{tr5&dAPT{)|Ed$Fzl4&w&xW561;rAH+jZYQXY^D4W& zJrL5NI#Je(q{nQo&6ajjh={kVfYJE+Y_5OA3s=;;&JJ8L#5O@Zfxx=5NX~0E1iYl=VOE7g<}`LAE7^!nSm=l_lZ(-IAp&NyL;ha`th-pabf$9ByW)^?l>U+&h3 zluk@3V?jBE<~VrW_uQs70!&oHcopPpK>&Bi;XPtn+h13@eZ{mlS+Wi zY&6yb97zM$oMb34_4-`+7!^LGMfG|ZWE&~fqsHla%MVa!%7Wo+N6wVfizA|{Kt5j6 zIIhp*IQm&DdyLD&xK>X@2GBPeL2n`Irq>>G(vShRBS5J8yB)Ny26$fq7nnio^}6@= zG%Df473;rm)qXgW^&<|}p)wCPx1^d5IN69u8QbByX7&pz8n@NwV5>ix6+WAc3<#Gq zZ^8xxKUagwjxR$T@+{Z*y8w4}C*dN8#fKjf%m=Z+e$e?tW6Fp#Fvi&naITJ2cfW_Zo`PfjF+pnI z9b{Nl=#N9((PwK;?L*cMZa2h!0APIf*B=ONb7p7mzXXiv4{&2=sH?4_3G1=;!>5PD zoMDfSPF8*Mda&_@2UMovj`DNfSBgq5qrIBrC?wdGd|+zqu0x^M)usBipEH1ySLb+t zW0LU@zPRPsE?SSXQ)$4jAxn5uaVCQ1=V-nYKy||#oQ3|FU6}iFkSq6|+rdgepY2hm zYe5j%3}Fz+4O*A+TCHwSZZp*EesE_ZeKh~{G*6ZB=o;X6CTXREV%Ye4PyEUdOAxZM zbP;Og&{%JJ2pCTdQx>eg#_qa12MKv$P9f6t9WD7_6~lMwl2(!Vsobm%>)!m7qx_QQ ziK42N$2o2uRY`6>J~;KQG&hY!>tr~2EETnlWIxh%O*}WFN08~0e4Au}yY5@X;nY~a zp66YfmUXg9fON6o8{U2|7`ZOTG!FTuqmPtWRY-!%wEb~r0nZN~j5*wz&LcusCsR8s z&oS8D43T0p!coVYOcfM~>8)WjihB_4+NNPmng_PX(bnM`D8z}xvr{uF-w>{^AofoF z5blny`1skqnt?~h`AclFK)K)LoO>rh#m_%QK2Ty`_$U(Chn^FUSG05DS{sin@j04C z7!8{YMVCh*MJ$Y(Iiq0R@3u9ElPP)}+)~ENGK{iSBn(@r%>-AU{(bw^A)lhzN#u%Ek3yB+~#b?i^NrwoOCL^+=KS1yG`4lGCvi z*~*;QNlTXlsFv*D#1Y4k!Lj!-x(rVJX=NPjqP#`2K+{U&ikQjlGd#n)0MeY~pwYPs zTNEt`1N7!v_e@Hl2T^g@L_P4BH;2mU%bD;%p`CW+q1i2Fq5-wL**sPRb20i4Kkp+} zfJx>sPaJ8|SmUv5y}Dq(qdiN}j+isOk!;JmBJ=rbSwrVBMBYws4-_a3GI0o~rE}-h zz4;(?&G6#K{P*Se@e)8EOUJjNI2{_Z$t*~T#L#Ml6hF(~wy)zC?A z_35#^O6SL@)ZtRVMj2i(vSnPp6)we&NlI5jvQDr$KOc;c0@#BJsF+ z+gj8R%^Ihw2|D3n$j@(cppv7YPw3!1o})~|CXI>o)ycGfXsj)ju13)28u95+7&V|O z8a@@`qXb(f0p|P9#-dF&_(tCFc1N6raArb`22Iut+0&PKoZdX95n2X7;V0|f(aqz} z5vP7?sH zve+A?Bk$%wVZ+LcL!gG{L{_FqnRdqy5A}##b;MQD`OEQ>c3Ps?Q?0N0L^5tTiKWtu z0UX_54p}7z)Hhp*bXtug`uO2=3hX;)E$A^s0Jp7V-K=>#__wVddtyMjh z&xTJT#pmLPOmLa9E*${{?$_=8L?Nx4H#vXtzhhGAAYYH2%})`eakRYqNO$rJt4!VS zLP6#M%sg||r>YohiS6u%K3p9jjj2gv8Da2dRw3D$OI-duT?FV%YUxH}-O)$A%yG_w z0Du6DdslGYNRn=nt~Q-*_zg$M?IrJyNue0>OS0WpbRm3lQKo%i&-dlLVDlVNPT5QC z#yJMIyMf7Q8#QFIT>PzMd}19}J$6@vqR3(rU0SEYv0l|<|UB$OB+aGV^1-Q&ubrilc+?FK_b>tz-R-|RZng-vu;E^ z{_!5fFB2p_`hX9lPvxzq52Y{hG<6`BY38$^A81lPMm+7v6fP8yes+kH(JYX1LWKYl zAps%o75L3$$>C4xT%PT(>8c;xQ2OaxR6otsp?8CO@KgO~0gDb0G+6p= z6S)t^_r$qqhKfNYs%NN}rN~fskQ$HlF?$vQgcVli$ewfW>0Y%qttoF@mMnzuWFE6_ znY{=C?H;~ud1W)e#0efgOKMKKTh+^q7JTcJW;ZY+xm#40tsV7Pvgw|Q_?ySakiCs=nYkHg*Pus;-#x(g% z)CB7bHn$`r!f;|~QRn-ITf}EW+1ZaGN7uyE>2^-%7=oyMN@hxcCU=>o%Sbc=& z>Ryf4s>g@6Unc6qY8z=rkKY7^vbflBEuRwwE$M!Oyw6tmczZn-yaDCJZ008ap9mr961-o5&N-?(dYw> z%PFed)H#}(G9IUO!w1WrAURqKin$nI@mPz*} z6&gB>Mx3sBaIp(Qi8*<{an;Aq()@0XPdgCM_(jx{MOJ{9^~-nz&B|?NM);>jENf`u z`xDrzjn-r)`Urs%JF4#}Ct{oYg*KziNDF#nXKq1do!%(_u4qZRF&$)o2*Bw|Q+ZGA6Y zSX;kKYf`YQ0bpeNtbbrss7iSCaH6g><}p5%RBGbAUq&qPtP`kpUu@`WpNXEvfaN{x z6ZDmY(bj$?*v;%EAN%w}B`IF!g*^OLK)Yh!dWxW-K&Z!q_$SPC9dpbdY%kmi4>a3) zo_bJhb^U*FTX(*z<@Ep^P8>^Qycl5So)Hi@!=SmVo%%IlzYmIMZ9fxc%wWu*&gq-8 zN_vA1mEPM+d@>T-^R)}x2-Xj@4?hL9?`SN2f@x^3KGME&sn93DHd*jO-q|m=Zs99MbAO;ZkfR}I0UXDj4i48If@2`q@?^bh zGBufcYpySU&VEObk6mf4jwU~nx@)sW*E@rrvERbrWP-t20NgKOn_jA&kUr$L1z3i~ zPH433>_LeWxBuR0mKbl{uLEYal!EVd;7Gx5Z3u%Zz|WT()-P*7RvlA51n@*G)+hYY zN;DMsmwq>#ok0&+Q5AsQ!R>DB^=KpmGEAEe82uE>r$WSD(UUF5K0#(DOr*SGuc<&Y zuL1Ny>_{{WCxhyE_ko=9davRkdBGPZzIgz)^DVFJUjYQg9w7s|st|_s{!qIZM0eGD z{f*pnCyW`OcVL3k2B=+GKX)pa;eJ>C8eeVZXf9illHLZF4%91@3c<{U4{Q6*bNKAH z4J|n&M(ry_Yh!DpLSv-(znD72Te&#K+Rb1AqKXh*Hr_<-^?8BPXsEay!qb6Hp_%iS zEI_AP*0Bf70M13uY>hMD*(>aa>T&i`B#=BzKC<}7*=f=~o@P}AZ5L3--+{w>Bv-np zt~GQOphlXFAt#2x+K|p%n;R(v2wuDZlw^>R=i;!^HxF)VsQ`5i0(O#-8sF--=4%Tb zD)IB6Wby?CmzU*tjEbpuMyS7=0=45IJdS}DEh@q?yAhzsxmJC(zqzfkp1C1Gef3`B zI|y5By)u-G%$nR<7ujjEl)kk+P1`>=Hzb;%_FKAs*)jx_;?`zGY<@6R=+U`*GnMe?5AQw>USZ~7KCc2N`W8^%F6TjRI-2roZpzp z3W2KUF8}~z@I`Lscj2auZroWzTMmf`EDSPSi%6n zZ3k(Y`4h!;H!CNf+^PXsOkKMQn}AwB4%F++=a+T_+;;EY9gL*z;6cE7j5yyXld$Hw z7zc30W*jI=e8{dsuLeLHgSEHZNU}P>NB;hRYS<$+r=LIlago$+!+kY4J6x8toW1Ac zyr6tN0QLqUkcNZ&EGvRSNCS|k|Fxf&R=;6DLR3KlE30#$Oy!VPUOwM>##i~V1>O1m zUkID7Llv*jCxVOJ>*<`i%C%L~yZWWkjn5sraM6al=}t?8kqPL!(RDl|=nA}hdwu~h z7!i4d2>Lgm)a@o7GyoBH+`d!Gc~oRTzXJY6&fNg0`0<5SY#168MDM86KGE3zqYKqV zR|5yy1xlU$*qA%OzOIjy0?dntX?Ajl(6}>R@go3H2~gYGIHm8chcX(s<}UzjV5u6U z)a7j6MM{laEbX+k-FLsv7L?LWTv(|4jH~)}{b)(?W*mAY*4QADU3D}u*f2K4cKCLe z&QPqIGd3qx?Z^;-eg{Ea<7AfkJ$%j7G}G+YhOWt#ugrp%h9Ocwd26<`GWap5INvg+ z3Pf)E?=hNyvav3&^m()efEyB6Wf6g1u(ccm@HiP{re5ngEe!kZOv<#l@T{>Sp;!ljuy1ZMEHuJrEp3;vE+i4BNw~Fg8_Csci1<*Gi%GmxGfmPh;J` zKz%(?do5WYW(SS7Yp3UazB+oJ2lSO~NxhF~+k>lS(mu-F4L|aEtMF{&ujZIqQ1<;T z92Bw@qKcdf=->m)LLLB(F1+yOeg$ogp3&W-(Y*6msIEKH&9P6uA?woO4W#fQQ0lPO zhPQ_i)(;Oz&3&znAEw-rEP>CcaD4+s9_DMqS2)t6zJi{t5rf;jzW^+p@p5JYDrqVJ z)WpMcZ#JN6b0DsSu$h9&bEA&d9eQa{C2V{OZ|!W!E#|1Xhnh8qi3W*k$D3ZagX=Ok z_5hV1AnhJr4z#ouh;px+q`8+8u? z&5Ak3Da%>D*Tc}DpJeb5?9;HB+Fv~?Rqt32KFWMl=(K(dC`X@qt?+22iJoJ5N+foE z`@QC_MK^~)f?+&%U4b~P70K3M$RL@}`MC&`lzo^3@Me4?{ovXFL9?$EgOm~7)OU`$ z|5)ZWTR3fcOoNMH_UXsr!|Jm4-l#yh`n)rBGfnV}2O`je_sl%AR!c~7Qtw4~0CB7& ztWp8lhX3;1LO^nHT{C=Q6I8`lrm;;K{tTTssp$o^MLh6%`GVg9RI2X;N)Wwn_g*BK zMxGRTCBDs`50q-+$EjqK-`Y}1-A(Aiy*@!h!SF9_Q5WA`tOc>jZbX;3kaN`nr|U;r zx*cBCfNVmt?&0qa#Mg}V7rVu@Mtt%L#)CnXtnGcTIGM$<3w;+!aSMHOl|%DTjdGEp za8Fjs;V9dASKx8hXp$l<;?)-6MM%)8a9L17=r<^7m|CGi93cdBSF%3wQ@vCLKy&q~ zcDjyFSNcV3n#27CCk2v&$H2p)3*9yQKLc;M5Id`HsH)QMa)s@Z zvvcU!QJLiYzpng7Y1&4HDoc^eY#W09Z@CELP0jz-0%#1ozYGKlJ>b3qrs zlO}MT{Ri*M7EUd6MP@2!;ep|LCAekpb-!gsm~ve}7_m&Z@pPb7wqJTl87axIqc2mf z>0#y!;g*qq6m8b)L-dk%6&NhDw zD1?ICm2kNWceZQpZoiTnZb=l@RrA;ex}&~MbCvEJyt|2_pr?|IIWEMoPCsfMC3h+6 z{E;RO&ZEPuhv;r{)mOebC_#SCeUKBIS5UFYd6xeky&P>t6z{=5yoov~=Mn)ctg*Nw zrcWot{Rg7HLJyp9I)A`KbTOsXv5D^*RkTF*2X|6Ejanfqzq=*}XhE>>eqi`D`gdK| z9|%?Iquop_IzdjGWtOL765G;#2uQs4zX5t$oRNqDW-VSjmi|*VjMsL%dfQ|gXMt77 z=xv~RKly*ydh58Ty0-m$0vNhex(P_Squr3IBP5hO;C zR8m?bWN7KGncv#reLv6peBR4HuaAm*?R~Db&f_?~Ck3tirXcR-MZAVSmf`6`CE$_? zsAm&C>~Rk zxiumii8K^^97Q+jSYL|6lQ6xIwpw3K>&-&G;|*SKAKUOlSUQJ_^ru!Pk)!Qjt_QJv zt;|EG_Q_kPl}5!@6AlDk>xm4kO;?w{R0(Pxl}Trt(?L54-|Qyd*Q#@&CiyJs{^y;m z3K=%Rj)KvXnf3*B`p@M1pAtj+Fpq52*C(H*z^uqo2<(++lv~FnQK;k`ajE&8xaVI$puQ?G!h4G-8M{0GW`L9Gm{7mDroSW(EsSe8r_Y zqIH`(EDSa?0=3r%E3yqfcWpk7)MVA6HpcwLI#7QrhQp-oFsZl3=fneFUSnePg(aP!9TI%Kb#(6z}I-~dkzT;F#qj+0$2BQZ)Tofscpz(mF{n;B@b;i znSsv5R))k7l}>HxDH&#STewZ=lS2jB2`pv>6qi&c-_ZN3^38Eb@sT{!{Kbr+t+|M& zuK&7e^ov+4RNsmH{S!s;vC*`F=v$Y0RKfpv5#A8TT?gTUpNGZ zYx#k?+s5Gm<}F#Ks_8-+BnhZ&y+UE27Ov8Jvu7q&{gQS2RA~4T=CIG134bn4{D9#M zVkhJAV-L9g>RFt=Nj(b&X?h=Hdm#Xzac z7CM@?#qeuVK%wrifVV!^LU#Q5fRGcR?P0&GA|c)P@QexHs}e-i0WV3CjGL z5$$At6kpyvgAzRsfz7!;VS7qJ=F_N|zP^4ktR)?ac(`F&GE_ALu#D1|HCHgbuv_FP zILx2g)A5U?UWC;t1&_!C{|E z!Q*|z&C!LF`Zqhs846uzI6Ae35g&9vXu@6ALXbUvup8-gJy}q(cV^_Z zWu7ArAu^yd*8hmg{P>eNvR(D*`z6rw4tN@SYO?9^E>F6`Q(ylGUAMx^-|R}c;R+1K z2I?ZM%#OqgxGyjrjPXc4?0wh^fh6`sS~h)djJ1Yzx?F%>9Zr0c_%mlSCd`(H$>{X~ z{4^eFzoK)|tr4Gsu46A{##eFk8ER<;mS6T<2R@bM4EcFokyrYkNtWQ$@H?Z1B1-{I zJZF)myi%SBw@XA~Js@qsrw<={g<7RCG900#W-hGTzeghi56=)-dVScy@gC+Uy2Lr2 zj>2kZ-h{uB0;*wDu2rr?hFBUJw%VovsUmb&a5C1+NgPb*q-^ z$jS@~G$Ao5E5;~NHgek-tt8Lb2kTtDM-%~#Ibp3c!IIKfUQrEd>O@iac2$%Jdy-k@ zsG5Avz7xI0Y2u%bVziasf5mB}+l3q@QD9)|^y+aKnRdm=&G0;Ve}Df%w%n(16_ zmTgvczR9(gw#`$wR8|`?+n$9E)Srt3QblumG>%rsY3g!LSl7-VFQEL=6f`H75wwgX`OTR1|1(6hLl9o%|xq zV+~mz>Ygte%skgIWpD*M77hN)yD>ZkGH{1p$5mZ*n2Z?rM`8t(<6DwWm{I2Cpmyd7 zj(k#1I4zyiC89H;^wCaF!rX{05usHi#S+@m^mnK<43V?lEGB$LtiiD0Mg_Vcj&+&b zAemx1U7d0vT3L(MCW>@o#`VYcWPh(99nu9cG>IX-))NK9?nh!vmcrM#t8umcZULrG^7lWivxr}#X{0)9AJ~lQi8s6FvIfm;_hMD)JuSXalMtTSU>ihk^!~zrS(-2Aob^z)gfD@ z$Ws(=@boGMaQ9vzvkB>3ZoqJF&@A8m z_^+D9c|Q%$edI(=djJoF|wn#p!i zugLL=Y?H72+h+B@DPW`+`BHm`u_E2{VeOL$ACI5d#FPr{+?a5l>^>1h%5x>+&+yVJ zbY`9wZxfv;N7%>}ux{K=Bw@5P-SYGDz1XVSDfUgCf~&Yu!OIB6XKWEb&%%l2N-DLp zzWb%GqY!$)*X29lM_J03(j)3_#s9K9@N9!3$3>WUqdrHOsq_}Y!9;<_kH^0_5yTyS zckW-Io8b5x-RkGrhDd>b)|N#E6<`x%IkjUL6vNZt%fk8K)sF)QKRZg7KPcGE3H+Jn7rT%N@;W@LHXI zRAs((4d`h4lG>nwXu>U9f4yBB|1M!X@=z)M@YqCh12jyJ!w+J9M=e(DdYWY2VeKENM;+9%Fulgqm!YZrEP!c4?M zR1Tw!}ZeAp@B)YnZ9=4CNfxTTpS%v>b>6aVipZ_iLA+)LtS zJ5v$rV>*xM$v6hS;IJ1+IG*ruH>)#ZR6fXr9SY3K9`QUy zh?0JmrzUlzV555)ak#6@!-45i=pBcV1RE@}+3ZJGatW_* z{70uyx&3RJYKGq4 zkh<$H-GoNs4~Id|DJ7zqBqiE|Gkw*O<-{(=I(^QOexS(Q2G3ZEcnH`tR}w*sSxno(_$7&YSG zEiLP^+NHIyt;QINU4h#RW8(4r2ggq z&bKrHX^IUb83TwKy5rF4#ksf{pgAyE1p-@2(!<8P5TOK@_Hx*GcpWLukb(Na*PE#F zS-TKX2+Hdzx+pRcegq5lyyPF?X>4dzgYovnV)b~a=eh5}9k6^-dw90GPb#Oc1e$A5VHsQX*fIBe*&+wv35^tWb2L)){3FbcD3$3oUo4BQ z_$b2?a<&!u<9nU%)vtDISY3>000IxY5|pmZhzRojz&dEK9X!_xK^y90B{uo37(Svx zc$G6-p5rc$+?k`;(X|N^p$@oJGl$yIx5RKh9_?woXv+noV3WQAv%sDI@`@p<50D1l zp~<-Cna)otV{FqOc&|-@4B;Fv$GoW{J84H3=m33P)P%h{R3GQBW}KrOB5FXSM3L=W zB*P~9mtp&G{+;6ru*nM^Z4IB-=rysz(vge;9NMr#QP?#NNgvJ|Yk%2Jg(S~FH~`_kZld#-)}q@1m)Li8|4CuG+1%Nkuz<5UuV~K2ywMwYTr9YmSeF&? zO7&AyZ4dL{L~0ZTnx9)SK(JD!`c=nzOd~IK%^c1C3)gg7UdnEv+uSxfY2DWk>69Xt zdHnV@*f)dLGN^3NczvvuKKsgJtmXyhnSpY>LN8oc0ACu^LgOBr^e8d*=ML2_XIan7 zVrXLtIQQQ-)Zord);G1bC9LK$r$SCpLXQfFYyW2XRd_v4etI`w*BS@|{8d5E-2l3)k$N~#?JR>n1l5- zR7(DU`apf3UXEk;?xWXhm$#$WrYC~^SF%&v#5$zw{s|^b@Z>4uq^}v~!mA+U#8V-q zoA)7ov&dP}e9lV(CI%eiq^yjnoMY!l$L;!Kd#7_3gxybntHevp7DC%ppdnK_5V`$6s=M)yew$G%&|MxlHPGsW1#S)&CD zje8(e2d*=aKy}qn75ZFiN^{<3PIka~@F1bQQ;3&MgmbEEwU%3Cqq`?UCul?(wi4WZ>_<1RXI{p`n0 z02cFa7{{Ed`SyawjsFn0vpRX_NzLCE)oopn@v{cp>tGj7A2%WDVkK`&;3Of z|FZ0dNVf05bEI?nXKlsiKYW){JsQ4`2hR6p(AC-GVKo1N!nB8uw>f`b>6>~CJ4=}P za;QvY81(VbCW1@V@|GBZn!t?_w~ZeUD&#ehfQ<@_PVaCB?tZ;|rirmYI)R{%w6m4% ze)DOad>`eRLlsD3%Na=Wz9Zn-Ipi)itqNHSd=Bb}FAkSEh#AZhb^~|YR)ET|Et@^N z0^5S6@4a2qcy`#?wYsvZ?6nsF2nvrFt|(*>9#rrh^y4K?Vi}`>1dcjU5T^L28=F$k zrd-eFDCH>if-^!U7fU0u_e`jJB}=^{P%_~c)8ESDfKJVgggbjInf{OgHW0t(z)hzD z+$#iSw}iAwqyjZJk63Z^%J$-5msZEBp4Jr&NrHTjHUns-!JI73pa@=(_+G`1+U+eV zn18b<<_LCxJfu-qBu%$gAes;^ep^Dx-7M-48_#Bez1Wx}!22~0j36o8T6|A#L9I`{ zfTp@(|KtV}5NpT%uTnr#cI|&?eC|S&i{9S~bJzNJ&o#K7oXvoZOg-{A7BuREk?E%M zJ_JXTeHtmbDdEVdG>yA38TFtr_-T^Wp**u9(^XcO0D5XCl3)U7kJ}#ZpON2Z=kZ*} z1dWGMvi;$&?ul8|ThKRg_1#%s+HwRri#6`e&_-hT@4%`nqY(PQDd62!9Ed{=njJsD z{=k39B^}i3|06!8F%wzR$`YPn|0kL06!_10zyaLpr(ua*Kq;I>BM^IJ_A`zC@J$LB zZTgQD9OaC#n>4Hu_t|2Wt%KWc`H+tKbhl#fn-EeT>az{JD0RgI4b)z`^V}GuN??8- zc81P9p0`n{=RJ_P8-Hx8>83<|{O=R=*pBU(Fv@4J|LZ&A#+aM{iN)BWy&xNxdXP2v zr}zVPIvg1eYX_&)T6S5-NEXrhP#R3cXsyN<+Uw+>7k^a(<0!V4j|Z>~p#v~pxZX_~ z;Xh+5fOVq2xcipIr@F^ZLza6pJ`E@!RfI&6-nO0|e1VDCJ9O#zT|^Fa11ho@==$%E z1uZOa_FIIRPmhW-Btby{5GtOBVt#VBt&2yhHj6lp#Gt^J3#t?Q`BvkzJ(iX=1|F8c z^-8sXeQ#FzsaG<-tIhRVo+3^X=Sz;mZsYEMk27~-&i;D8;oj7!(;D`HG(86}2hTKQ z+-l4?ycd@(6zXkV0>Ps0v`Gc53>+8{?CUKF)78|2lqk!8KVMex#mkOOLZ_kqfXs92 zx%F}IqE(HkuX=XnS&Gd4FmV1SRj4HHL?3O%1EB4D?%Y)}!bMXJ88vwNS`5uRiYVko zM=;niS|dC;Ilc+dI4ERKYNfhU+ZXdONR;7HH`9RA3 zt%~wLZ0wo{Si0MW*&wVod9R@#@N4%k25CY>(!KaU$MJSby8?o_41^Qol0-HD@)kQm zSv|mS%veaqE%2`uyq>%cVshvqxt>;K!Jkj@6| zT*oUtQwgrM9iMOMFEV|Qs>AO)6P&<*M`WW(K=K&{H<2YYqH?#K#YOHVzUh;`vtP7L ziBECf_lQO=wMiaQ($}>@_Zh9G8+jwI7v*nNS06svh7>*3HrF4YZ02nS{Td2>0W6%3 z@J7I$53As{T>^N#lIC_zkItsTmZsM6Y98zf8_3o82qLB4Mobu{hy`Q;!gfa3b{N3a zYVSsJ@7SIo2yO#-+B8e5n(m0~2iFETT~znix|gfbcjIg-$h5(Ie+;_hr|$JG*Z`M; zXoI3wXo2uU|Lb(;;}C$k!kGmD)<8S?AD0HQn*htY`n-*ow8Vi(iyGtPjhm>LMZZ7- zup1uELK_jcyC^b!H&P6Uhq+#=e%n7>j5c(RQ-AfoGv5<7TA(df4FqnusXAgPhSaLE za+4n@5fTIU=Z`|m6--rM57+zRGC@>A=hQg#7q^3S)k%tPgUIRadjOy`YJUjSf|Iz- z{cq=oKIR^CNengq5h|0KK<0H)*V)d2YKT?tUM!G^w?ni<{Ix6iz@f9kjPP+j z7C2}u>QB_$|NdwY^Y++pMKX8XHc`I!=3l&UkEskgv}qy0DN#wx4G&E9oKmd(6{PBV zF=u`n$W%ee8z8`r6V%!SV(d@9I%a`_4RWAtEE3Q3ljY@M;NhCi4#;*LG@iDVtqaNJ zUggdf`!frw>%a&d6mNWKE!WT~hi2SBb@2ViH_@Y+)gN^3qscPIY zEmSc#?o!ot((eC+O+1+=e!M&i(jckn$kF#y%NfBzr7x) zH`xj>5iJ9LfWCM8|7_Ud0|2hcQF>C;lRO$Th&^_0wBN7bhp?}78v_YBP$;9Fs584| zVr-YT!$5#HGEw)g^|H>D8OsYAS|>n*pNHq}{&0nKVs%Q$Fu!vh7uI67_SLKjLjLvE zpV19I-jr)>SuQTV+n)KEzmD3!E|D$lBYRfQzIrLO*qGuV7Vj#<4xuBa~(O z@zuqhqO$LXM+a#{<@Nb}B*Qu-URSelIzbu3TN{_b1N;w}fC2&X^76->bkHpa zSd1LDLz~2iYdLbC-W=-2L)enfkQ1db_9?Svs{}|lCBATlILjvv`>MYt2bEVEXh3?F zYG)y@Xg^nt*}|T#68KgQHrXvM;ENJ$VnE88C&+R!z7XWBp@#ml04N`fS1D$0hclgv zqWO;(z!);G-HNv?W;`~pIp+|k9cP>lO@D*~JP!DP#IbuI{No{2e^`p@fa*r5u)h$9 ztK8TZJPH8nW;cr*tqxdhrzxI5P+T!+r|A z@`XJImpcF|IpHGQ03G(@Oev6L6=GucfZ*!^Wxb*J=ctLk#58bz{T=vt?3kRlQQUX< zS0cuJXEq8yqM>8mPLV%Q^Eqfz!H>0;6x9q8{VfT$-1EktMtsVKbpCCdAfcsG(0=Bv zXyTa;UaMo*QDXOPh~a_Dh3xH$F<>@9hihjQc%wBZG$M0TecM%6vB(EGiFe%rbnC`s zO4fkSSId8q_$2iw#}M%%{zD4IHDKG)OY`#s=sP#CX8y^IwfV@e(CCEIK~QmBB;D(u zqUlrb;NF_k*BSorj|4U?si1Io@8dY}NrZmqe`M=RY(^g_N|r!&xMi*<>o>SDEvp0L z-pgb_*YXdXsksf6oN_gh6T19jUMJaKD8h@LX!$89R!|;I@LU`y$Sg}jv#S%DQ!4>b zA8Z8tLI1-0U}xm8KBEIkQV7X@mW5;1x7Ie@$yX@%{aHai6)M709oQ-#JY&1-mRDH) zF&WtM{IhSGg{31~Ro1>SJZNGv+bmYG==Hr`P`FuC&{Yi*d81);{G(veCJD5QcW#?1 zKK44`ygGfH%!W8kuO-$)zR7f{T-_j)-o2bx*LRghJ!;u*pdy$65t*!PgNR2V>Jmr? zgxnyQ+@C4RMSmTuJ6zXaiD}!Z0SVlyck$NMmp5)Usn6az+HMuR=y&sXy<+pQ_6|n(g=G*xgcHO56OG zeb)!nk2ZfYiAv^gn`V!J*2>?2O+Xr34%mbusZ6DLyR)pA&tZojZ`!OgsD~Ap0q5HG zuemK76J&KodHJj5UzM)QA4hzH0j=x1UW1!Q$#n%H~JOTTkS=r-&_ZF*Z*9wrhFp~y@yhU1dJ7^ydLd{#LMxL{xbbt?`XTLTO6!R^Z}?%J$QF*EY{Aj?9fi9^yyi z0gsPi4uFc5n`x;!IQ{*?W~|yo8D!9`T&`a9g-PJ~@Fv%aMy6=;7vs+^@!wCWn29JX zCOG!(OMRyyQ*ii!FeQGYN$TiH9D2Kdc4+Ga&V4;_iqK6-@n$Hn5q!eJVEZx|t(U#s z2$gSk#x3IL!<;{fRM}^sMxK5YH;mZGy4NzHh2s^f)C1&*b&|Y9a_;qQeTeLqIOz3 zT}n*5MgTs|FarAyBW4#$ia|y{XoK-}`!-Ikx@klcy+G``c?3Mk$2jWaGdXlCvf2I& zEI|j0!W;EMMfw8Ez&_kRQw>PJ*(+B|VQIj1yM5!%m3FCCCs2w!f!FvUyW$csDlUTr z>OJIG!;r_W=A_%x5@0CVvkJ15^Fg$Tf<3luFwK))6FtW*O4g9jT?AOJEsLNf+A-sG z58|c)GQ5V2E|k&@;ze4!{vG^2jgPJ^;z+ zy@TxI!ydZ*7GMr~0MlauyK{bte|_ZmYx%LcqMMhS`|?;6Har^u1!I<4VFIS34#4}< z-E__BifHrZ_(`xi z3TDFXw^HEuJlN^gm-4ZQxyN7ht){R7ouO`eMB!^Q6W}JXRb=wV6mYOgE95=_CpkXv zuIr@_QD<_tDBp7>ip&^++ep_v*OEnEA84M$$-n?68>6=WnNc7=!yl)uhj`seMsA>= z)74tKUHTWIEIfjk!l*2bSg1$XC~V78WfP>vn0^`-cfL9lidRGC-UlB)%Sz-#^fw!-?~V3qFxjFPbk9IK&m zj1#;(?A=t3xdKHLM5x%GX*e%nlC+_@(Z&ru-~&%YD*KmmCmq zM@vu>Ifk-S8pHB&%ddIs%MaOhAT0QI?(}L9srH>n$pxAbR>`gw_?CAAGaTkiKJBZ! zvt?!RQjtDd2ZlFgYo2Og5k5ekY6n+%i>u&0k$*>GXd3y6*uo!tT4RMztlfC9pDkLT zQ%1gzRC#>o`YE&?0_PtF_1sOq zZ@Pxlp38^wH~IT4Cl+~^$C6-{lD^Ac(+$Ey2Oo%f%z7|{yaySF6>DxklK>lF1&5=_ z`O@**`%wE~Ybg29;~1)@E3q-&`&U(N|A zS?f37D6^jU@io+39PK9oa${fR5zDd4)Z_2o;gAI%1QO^;qy z;LGwoKw{K|ZyRF?@T*-P-A({4F9vY?w<6bma4Q-fQuyQmkk3aQB*q zlt`Xd-+a=?d27@u?L0d8u;ePtt-|kJpnLvv4Wa#%xxe`)&UnEW>^I+~fF4zWlFGfl z_ol6NSOmpxF5*c_NyfdiId6SIj7d3+VQ)@7{u{hEMXw|A%O-Tx1DS3R4`m~8Q9 zCOb8k4C-PeOPr%wUYht%f+{$IaXpF}gH0re!<7)%!$fi-nL~8PU^l|f2LMXBWspbd zgVvT)Fu&>tpWj@NgcB74&)_nkpx$4ak*FN~<56c4Dc_TxA^AZJla!$X%4hh>EK_E< zUXkt1%9z_hEXi=B)~$R+Tb}-JU{D5bq3QwP*A$N8fCmN_E5EJH!BBGPgOFkKYex^0 zP~5yBy#kWHO7dwik}pD3`%83D65@y81#%3_ocXoD8$|=3$H8|I_~YYmu_Hddq;d%# zJ5JAZu;|6nT8U30c#6C#&i@8-;ZSb%hr3z+mDUp}2EmOxuGRBGY0JVx^mJWs)|$BV zs7w~EqkXZr^gpvua~6Zt6>)YR_EFejp{aOa@Je%`PIBAYom8Gv>_=oDOiw+$xfqar zFgQG&i$BsqoEmqzTnxpG?r~ojM9+U|TZDWPLoHlJ&}vyQNkUxt59lV22yg)HmkF|3 z>a{#ctY7~vyszFj5_tkr>|X)N!L6d!XGqL>uF=)jK-GPhrL?WUyncQ&>3ND{o$8LR z{YD7Gl{tRNc{HpOmqdKmxEHI4qoiIvZ6QyU^3!XPZ5)OGT{oY!c7DdfwFvZKBO%7o zW#an#WRiP&ZhGWlw6{)KcV1-2dTMx;_C?DK3ce$;jd2`>3Fzry2|eR5l`2V}_uxWk zCJ!i4<<>$sz$bJiCu@SwNs&%5nZvK}`%sKWQb`x0)5(Wk(O^F={dl7j(6lS1>XoN` ze)+3uBo(od;WA*?jdJY?26^lTy8FY^I4lKd#Cms5{bhk&+LYtj2IcZ2>teco#Y|9C8oZAz33feJyt8Qg~9XLxgha>uJX54)=F(_3`{ zw)ba?X_q*gs4bh?ZJ~)2-jaV?SYAWZaeB6hBlPxvx@QvRAEm%pt>YVAmVs@g;L`R3 z1*m`Z7({~)bUW-H7%nPhiW5+{Nnh%s zL%^h8LK00N88>>W?Ymc7j}?{UH$-Ozj%d{&JU|x(D|YxRflWN1JgbfFy$}A+{KGu6 z!0b@cs6I`+o}Xd3tK9FlqzG1LKC2kJ4ppna@;-)8yu(mcYmzvm=10`|SS8~bpQ%`b z023*+onDcUrb_^ zG4WCXHcw91sTm(RZQ<~C`e$!9!T0c@fx3OqF_rJ51HM4VCg}oUF9&&DNn7AI)enxuwo2{RS?^ClWaHLq@zB62s~8R*=H z-xs{s*y-NJ-+Yvq4CaB1Q0mAXAy3gWL8HHv`MxxM?|??^e3d7jpky?#jt~14#joQ# zoKvLFWtZs}biXwj-;ey}+$M@Mx)Pz9o*2(o2yoYdHP0KUr6lr&x>lcjIr(@PU%TZT zWv=U^xvS{ky*gA>fNO8Kf3ImI*O0fnJi=P1#XGIzn_qw@)_qAG!tyZlv0hljGHgBO zoaNUP;vz4%E_0aAG`GStc)J;|j-s{Hj_^bre3(g7E`(tyV|2v_I>!2f9x?m&yI^iEc65YqNt+Ql7s2B_+PLUCCfD-QRA~%7H z1w_$Rk8`zA@^Xt&pTy@O`t(ciOaCFEHXQorv+fNlrrCPZJHi;Ifwn1Ot#p3VDUs4Uwco zy>x8`RPfh+YisW7KW_BX#ihw;P>ToB@| z-U3&s)u>L5`xs>>=H5YK6EK_Q7t`PcbSv3q%?MpB1h({nePgXmq2`R9B0TB2<>d&~ zo;L0ByRTLEo(7f8%Q9rg!emK~J`9jdxPzqQhEbiWsNCnB<0VSxqQ3Ikfk)l8IB8jW z;FXkJv$@PNW4aqypbe?22)(tSL(K2&sPq7gI!civuutX+1c`e3s%%*}`xEaU1=wAz zRCgl1XLjluarr=X{5=#6NW^DPwUOZ>q%hyR$?`(Nsi{%J^`NcieD6Q=lwu8b@eYuM+@(D3R+*)LE+GJP64z#DefrFO)1swIe2$u$?QJNr#Y6?-1ebm>S!a>PzeVa zGt(ozPjs81VYb#6Te|GowTn6d770J#Z7~I=f6b%S=hA~!c7;vU64x`{AAWqTY#aGd zN7APnd8q9Qdr{FmQXNR*`%?(T08QhHctmn;BeN%x=G1Us^qDE5KEv+{i-F!HoT1_> zg4h_=)&l^%^n~)8{0*tW@wV{;Yfb8I-3W!kM^bYXPUm|A1W*bnJry?E%_-2f+uUiS zU@8gX!}tBM$q#H*s(%Z*NNQ4QfhBM1SHkf99`y@0ii=8lDc^5!3#|AXEZWpA*7H4* z1pE;@yQBulIHsx?dz;!CG}X(~+ka;*U`;lX$u?yh!XW@RdxqJ7SHg9|!f!X&_2;R? z&CGCtffKt~_?!*QLE8kW<;*0un)Q~Cy{YpPZx=GhRCO+Ft)PYbJ-S03U0z=9ZL8m> zvRW0UUICduFAx+cr!FLLgv-UC+cv(OnKp}1FSZ`E>^fVwV6(9zwRn@jRO0cjPvkT2 zAU{JKBEMx4np6nIiT*zz^}3AM|G4%6gIG1<8a=Nrjvtd@SpxBu;vd&%H<22*X=`6C z&3w6s?lqv@(%)2|i!=o7A&Ifk!`AagSPYV7H}4+pS3P%4Fq%Qcveklz#O=#ahZ{-68;dNCHZ7j#7Gbl)T0-#}`(f@l#ulN01U~%XHem=_B~|!!ZUi9VW3Cz;<>{m)Hxax@fHK!e%xsRW$3z@cw%JcG^B||(^(=Ukw#KN)(lX39U2HiUd|%w2*Lw zcofuhf9=BKs3t)LqQ_M9qpGx{dd0M)^rI4F%(7(kq+caIHzL3PDPDDW7)te*;3GCJ zQ=ghz_qg+I;jglv2Onmux;Q^r-3(Yrxq998t)G&Y>Q>RI;LEBH#o|VwW$8m%_2#BQ5+Sj=P}6ldeRpgvFyDY5Oej5PMfjms82$-gzU*r*YFU` zI*XuhL=WpR(bIl1o_YS&qOn3z@THrE6f^GOkYWanHMTPta*MqgX0Owu+|_zKS~qS7 zUErd?I5~QK%o-h6{f}tuLx>-dtX=AF3S^obHAgks_^R0qG^y3#dAwAM4lOMRaldCp z-(3tUTBWXetWmr44m|09BxywSPr2()`T_@kos8-Q!+Kp0W=+p=v`>z1P?bNnUL81@ zSPgh5W2x!@#=7JNh@7IK9zmzc6?XjV`VLJy4hN3^8FZbO9d0P9D5|WG2zi0I+WiKs zA6hzoK_dq8sEW?j)YeXJ?dSDvE&L%^4!hj|W}jPNhy?}UfBI@Q?X*`(u5XOFE`Zs0 zCy&d|4_~|V=~n}In2bQVPtFPE1&_`KJUSaO{MZ*Y$2S)w`!?jaK=UTF{%FY+wD2B~YOe=?E5a?%(xef~FFrp6;1`YO zDI^$8=KH}IH4boxs127~wS~g7XT$;ba}vyKqk$AJmPE1i?7UOHEsZndV4FP~z5<3# zF&7h041>5k?HfRAEe7nq5OBtlwa#X$-3O=J29O%34?J4535BLP@*W%jG80O9VvX(UZ6M}n8CjqJDYJz94-Z73c<#*V zs~D>29)R#X9hA;oT|pDR{9J(!+~$DSoT)zK)<`9`h8_sbbpFF2S-1OV6}0_BK~PsT zk-L2?L5C2oj^8Z?4XdZ1Bp89I#qfy38vJQ5)g|@z0^vozdeZ@PL1(ZJS%i%L?rM+0 zs}6Dox;voxQBcmsVi$dlp!D*$G8gbj0W;I|v6)E-416y9XJ$GDnVIa~1WB~Cc#O73}iyRzaQ{p21wXlXdx%k z?l7<&$$qK_zsKu|1=M%{3?!HaUqk~ zZact`TDleh-d!3718L2I?b+6+q2q8jMK{~OrS%&GFyimi6yY^V0n^dq1OJVHR%-^B z(-atfB>!m6;q;ZMNfbxxWIT96<8Qz@G5Ct8dg0W4DKFKC!@l~Qt6l|%`lr8aE_^T} zSl$HLc$#T9$?nH1di31=WTs|tU_-moC&uCmrg18s41HQ1v06A8`c$;{OlvW3yQs+W zwVMwZ2i6Ek8Cl}uqyO;&utQ{%hOZm@ZKg-#0ENH+?*Qa}wbcTu(it}HrWnW!Vh8Up2WSIU)0d?I3Q7-;hfbyvYE6&5di z%xiWugE@Yx1)~D_tSNX%F%UFZsk--L0IUlfTMtd9-uU{1ISm=4er4+-3+c1fqiOfU zzOY9C(y;*76`1*RH*27)E8W}%iYE=wh)ukK6wv@G)Tyt|WzkFHKyqQpiU%@!lQftm z6L}N>K#k<_U_7cuJ#1|6{n1}JOV07TM_>{vcv&X_d4X&*!?@&NLIWCiQIfV<|33R~ znuEVpqLkH^6vqth^daCfG+Rwa@*XIMUI7bYlk3X6dChRi<9AzxnhKED*M2cw*r((W zMx^P^f@)YOsNVNBz#F#{s(#2DxB=k4Z`=;*mkSsBk%k1z9+nlaT2T3J)9~-b#pvJL ztEk3{+0`RDiS5#Sj|;zWs99$*WSymWljA4O`Pm8YhU5T4{Ei1BRGj7Z8I)2TcB5U= z*(c9b*l5bj9m8ArX>UYH#lF|b6x(=;huJ+J5F&TXd@e)|CQBL-MP|!Qh|Q^NM#jRm zsqyoMX!7iNo}{Rv5N^Pd4%#F)g<%0f0!>tV!B`atc>#ZQwgaEb3h=!NvjB+n*u8ee zo{7F+Ko4jDH}CO>RLHK)6%3L(Y$DNFil{XYH3nVU1nZOJ| zis*KT`mkR*0SHhw2Jc{>@q_jF{xu&Q43jMmIKY!T^@IQv(X4AWtHxOCG9bcGLogy2 zB=&-=O4RGoF3@db-3ApqVL-qN0GoQvD|Na0Ao1@uQ6*Chs($uP6@Yy|1*5?vt=0X| zuaKq4ob`H83*???@-@KSwIgCk?)r_H7AF_Nwr&yPpWz;Tmcj_OyF0?p^I2 zxYWzFCEq2#gaC8E=^R_l$EEdI$sDJ7Ze>jxmK-LC)J{p?eTQ}f%o``9I5CS+b& z1YQ2{qw?+DXK^(-NY!pJy7riWW*wGx1Q1}xz z_w&X6gMb83?i$Al_?Mdoj7+}8&3UVimm zk@r$OJ;IIeS|y8zKu zbmAqc3s!Pa@GPL!W@f+(0TIG0BuM*G^hY?1gc9;6W^oYKK(>eKH>zet zAA6E(!k1Wh3}Y2AFVBHVsYwacf54XJqX1}Ld=rAcet-q=a!}|kgy2On~QD!LzNE}@O;0;^^PIn(~(23wTC%%oChwrq$CH3Md zc#OO$ARnX5{utN!nHPz>*mq9wKzaW>;WNv^Phne``$%PpQ0X+}UBr{JsaS4@5iNDS z4MZ`?0v|t*B#H@!CQm;#)LzLTGKU1k%;hx0&sk9EWu6>DhcEb$;T~|RPHB?&U?I#J z%58E5YYrgZ;c9NI@a*Zgqb*pyP1Z+?=G`IWcIzbZcIPe1qX`~zA2*3*7OM+sJ-mMt#uaUg$9oL-!&Keg z0tN8Q`M?n(W`++B=vHC3>hAQ+`Hs>o^*qMNPFeLmUPQW2G#zFB1ZOdqkiIwVDE0F% z7?krb5r3L<;7@-R(PWdMHDoqs!!#W8`1gFlU_N?y(eIZ&7#1tG{m~lihW)Kz+yVcr zQVNeZmAIEGn~NMtvhQfkzv*6iQXl?Bb#8MO*WWwpy{=}hE%>hI~)#iC~1U zrBo=DQ)}-b)_I5k{{awE~c$76m+3wi0w1m$+ zM;qlSz{n$SoTt5Nl6HQ9wD&6QQz{jmI*cJkXx~_XhR;|O_6VlBWc;_x$6_YSxoiiu zTIO&B%Zs8Z#nCp9cSreY2vg$d-?f>`iVd$xS2;}Ibq7V`G!MN9#gB_DL7T(l@X%Kj z#cTm(NRs{U=Ie@oUm$d+)j%nkvZpCS(t7CLCKh=muIN9>HHTDc{R(?@Uf`@7vvM2r z7n}6cN-wy~V`aCz&3Qu4&ee3dmJ^qs^MRU^bU37zM3*#AAN9lClwr8>2FEp6o@gI( zWF4#Gscw^x)^OTP>q?S9)&*5^Jb_IFeb=T#Cc(Q*Kcihz9qkKSXiy$VET z!Y8-CybIv1PISDl9NWOsGYC4*eCNGK><)FmHdd0qC~OJH)Y*J{23H;HKVCqyd)9@l zwrS?Mch^i8x1zg!A3IhKr2^)Aw8pyHPs}Q0Z|ooR0bA;T*-h1WGhp^F8RY(j8~nLt zlciXecnJLSJQXFECu??+t5Z2rIk@O`p^MIm$Np#SC+1B{?pAgAGf_$|-rrpm!X^Zg zB3ds6cP=HMmllt-g+|N>Y|K2IJ$c#zi2c?|1NE?H#NrMcQZenYU2`|~T7(^ZO1l$z>$x<^0L#&p zx@B>qcM`o=n4HAYr6_b;gbX&pS%z!)a1>^s%vz#G8`o~$Q}@okb-Z$(+jv;J&gwKx zUg#LoT#x0+5xnbp(rhx3_OMAex9u6bge{cDZ5O7r#&P%?|6t5o>!96V5Kq`(wMdcZ z(~1C1?wka?pkKOn?cV_T^)BrY4+R8IPTd8MnAB&Dg3TnVW#i9>W-uW)*4`*Rb)K|g z^}t8axRj^%F&*oTeqs@A>8 zzwGOJ7gTL%*!phaFH5k1)`Cq*3wTq^?jD&5Xu@X+F)z%bYPRf)&kkLrcx6g;rat}8 z&N4Igaqyw2=iC|^J@}+N&mVw1H`qZp%YpuVFk1A>But56UgsKrPw`SQ4f6bjB%N|z zMT-x}A9sEeg&=8V)R)hgHbLaNJE#0< zl^ZXsVZ63f6Jd_#FTP?grU{-;a)y5s)i}Ex_eodp>);054I^W3$aQ01>zdp=CCs$^ z)?(Y=S$GSkF|&)^e(F?fKY3J@HZhX3pqqO_bP4_!6Y{$)I2Z{#0M~EUt3+;StilS| z!(cVT96wqy9A7vTco=Y$(x?!t3<7Ko=%ZB(tInSupuCeuU*hi08)2%o4@Md#B=+SA z%J*ST1u#gqpgkbRIiz3;vwfUouV{mL)QW(fLee!Jx)r2r@%PDlo6KA6zom9A{Tiaf zC|ijakj~!L8(p1L$A?CtM(x(t_| zJ?#SzjV3s0C@%+$@#YHH9V679)p>l5Chi1>T$){WzE35$`0Ca?EBO&IjF4@%CR6#$a(X!5*&ZLn>oRY@#x45m$qjW#cdMe5!CtmKKX3z zc;6Oo6}K9xaj$m$(}{Ebg%=k8kvPC?ueClFycpvNf?!|~HI~|lg7sFE>+hfl(-|8W z7Z1(u9Z^zPn}(=+1AS9czgW6(BvY9EQ(0OVS0*`p>H>`bKfT<6Mgt!Kr7PH;7z@*o zU!ZNG*yVF|F@~R_BfAtLEXrL9aNw?3MR!VuC`y5MZm!xgce#s6e(;}XZ#4|Gz%og* z4u!nJ-0+`%ix4QQ>1utk){5pbBGZ`J&`ykY9Z@s%)23j!bFVu!rA+&3bTr?Ao3h$V ze?0;$+fF4C)l+@W+MGKIN1U(J;eF^MDNjJQW*wZQw_@{%nq?FS({vUOJ z9u{-|{*S}6wC`!vzLBKG(!Ns)A(2R0kfbOP?M>2x_C+C;N=i~_(LRb)v=Ob^_q6X* zQ@`^$@&3HOpYQKJ?)%^Sn7^*;I<98sHP7WdALnDa^Zi~@tZv$OeX$Oy5v+Wehs*=s zy2YPjSAD&-|yzeZzhx&$zQp#~eq35i5!-{& z5IZKNtuXsJGVzxpVWr`m@Wh<NvG9)nLZqkOMLc*OkQv$x-tf&B3_CZafN@;-h& zr&$%TE7-W|_?c>NMPc$2OQkk)o8QXoFuif<@1ksPnK@~0pSw!?wdq{yNrF?4RAG>c zk!Va%jfR}k!1cn<8G-6sZ5gx`S688R;#vJzIrPpy=%V(vh{h55e#4ys$O|&X%sc*m z`ZJqHXZu8H67z5l`dWpi<9J%y5nQ6q?P`L-u$V^Pj%zFq%KNIhlW$`)TDcqJ?}&Y# zu=-^DB>deyKTdwuF`SLth=j`rr8gH(eCu-Ay%6+fn27zYd+hWyyY?*>l@Xq$JJ7;? zu{V3`sES!^Vq~FFW)kzKp_RJrv#6$0U>9ZHtiABMyN3<;Tl-h4f3JJ$P8i&~sE!S0 z2Znu;9Z?I^yDZ#Rb`FAof^`q1c&x1Ec~qR1$-aHZyG7czy-G;tod=lZjYemdKQ4vx zcB!b#{a5+rb$`{aMPrn0l}pqD;7(!oNZ$I~vfmAWV|J^qTKVr3@NdZ(WehbZkE`#D zv{X3&Nts$vkR59JH$w2UYGW|=&f*fYkP&Me zxZrYsUN3-iowkJLky&Ypdb&QRBzuAG^>)`*+qriVVKOJ)Vt)T2WBXrYvJZiG&e@~7 zyHAvf!zokq-Yodrq8R==A-BsIkhb`)|Ld1y_gdorp5VCq4Lmwl~NBhBz1PY$|JY zSo+0*;PY%}4U3}xR7%u^8r-QE9u+4&k<*=>6|2LGL{?Huk95;Z#i7u$ZkC?1niGyo&swM(O2QW! zGD!MQ6IKq8?f0!7_STD>89Q=*UT%<-t)|8U0OW_i1+c|=CdEASl8Hm!pE)uHOpnP@ z>X^p6Wto)F9hY}uw;hx*+lgns+ha`lxWary`=aP<3CG^&VlT%M^}h3A?irMa%jnvF zSLzWV27Azko?yk;$ltrgLXab}H9a6`Yjb;v9>q^n=g?F26=9ULcNuZ8k%0%N*ILy= zDipHgPuD+;7h}I4psT3sj}ycm;y%>*t(@(KK@slFkpUU@Zry!9lzP}$Z>0#E$7Yw| zJ$T}I+6CtBoJf;#U#e~dFg`poNYTWI}&#O;VNDo_Z&N* zH=A@qTg1zXi7>l}(b0SwyUk9C;L`lo-q}Z8ppoHXRqKuOB64b9%IW%=vXVcyPOV>XN= z{ZRX{Bw9UxKc-7ZPpLINp|h55RiizlZ9GsR%BbphtnrQB-6(HrH{7?k`VPUKKWUQr z+f>Q;+?ykKHhojVP-@!8fa+4FV8tt8vQAE)N|S6c7FK)ukLPM8ekDDGcu1`Z=Q-iq zXFkT=(AL?>+Fo&N$B4wW_66lst6kIA^jgGCSn`i|V<$b;#JZtjCtb*i-%Wjej_rp7 zAG^cD16Y=`$`XgLw^knc33i^HTt2ATSu6yDwbgFtae0cwcH+!No4dH2HB7A=RkAM* zq#WMbXhMLqiz4=cfduH%v$YH$-1D4@mIRED3`FwIyLZfuSr(TWA8OvteupSYJl2F? zRiAr0s7!OpM*PuSyNxr}nSG+hba0M6GL;W=9UEw1hxvm|#xWk!vi8ppx;$u;p>80D zmEW$)QB9kaJ3~#trn$14Fc2W12&0>u@)N%c_bNzPPm!?GY97V{Tg8PmNc&GoIng)j z3u9&gf#SEE8orb4dS9LT!WRF>zcH7!Sij-iXWYahv12O@Q4fDHSa||pW0dpIA$R;6 zz4@2t8Qq+;N-*}YnvN!^%eM|-B?=mbYz}EW6v%^>)VAKVoxpwNz9+BGz*pYAm)i_3 zkKtgVqizt^`NLBqv7}YofraH=zcmC`J&k!s2)=_D1iK}s0 zc}6>rebB@ub}b8>lB!T}`?Py7N0DyszER~IR_z^4Z#?T+7+$mL&VHA~N9ty689gF9 z?7*p2#Cp$m*T{hr)}4(yR@uI$nSl-l;`@a$1qAy6hV;+%IpWgQk<6m{pWOKbxo9th1@?YH{EI9W}{y=HUH5wZZN+}Hb<*xkGtcxryNf_ z+ZcWByFqq*>2G?6 z!|YDzIhqq8IuzIk0(X^)dq?Vp4K`{=9|Acv?}FoO!yqL*JY2h2NdO~>k!woxToAr; zL65)djq1oVTF7hfK0S0K+rn)OJALS!&asg{2YBwAhAUbBWDF8;W8d1)-> zVF8?+zdXtTf$}zEyP{*XL9KJOyq98~0h#0L>n~n0)3QeQkN2eNiqwW(`KjvATC%>+ zg%V}bHQkWvBr3O?opeMd@6=1WGscfv{Uq;)PlT-7o3UImoTvt1Ovkd`7l7JCMS%s{ z(-uBYV5m(Yow;oo4+i*|=MEem(o5Z&le**QK{Xvh3u_I;OjMVz9j7%IcLMbNvuCTi zNEqkN0nU2-Z-g+WeHY1M8nu-upItAED8FE6Fuea2^X%<&GZ@?Nxzk&n-$scA&1>ns zm?V}xn+qOdtrPb8xM70yt0)cS`n&Co_c~0J+jC-`8o->SK|%zSQ+*4x8RK>bPSvyi z)blpEu58#x+~S-)t8ksjc_KpHFe^RvmxrT=%T~)NavBh`e|{+k>a_IkhG{gybW&PK7eeVI|ZvFEl(?JSvpiSXK&GUU}-!lXs7nBw$tN(p_ z+XJc8H%Wm}$fkNCZmHDUoMalZZc^A=-OaA;w?pmwwO~Glov(8$7_1>9Yn)1_gcMnn zFrUpwKJt&?@YdS9--&gj_Mt!n%r6|yK<+~E(Naz7Iw#@<=YY>x63btiI;WGDR6+j!OK#EBh-frIS&<3I}ypttmbmX#6w zDhxeQQh5z|vQ@FHYxOA5C>A5kWV;q6wCWsfhLmRaAdTfrUe8^|gN?}Dzd#H56V#4a zrd&%|vs5WPEK|Od5;1;tlD;}~q|5zDW37r19Q8l$Cebrz*POgpN?FGgyAjfA>`rj> zAiO7|%A3GV^|>dYSbV|~0Dsm8b+{<=63(K~b=+-Wse(~X*L4=GM?TXs$wE%*?pTKtGZro98h(_Y#ymrH z`|#^sYWhR3Q)ubSPD`0h$K1?? zT1rx+ZJFu#U!NF5oP|$#k;QrLZyAGq@R4RSohu^>+^2k-5Z-t4+B9L8vnKu-w$an1 z%}H-W(c?;i^9uYk8%u7pgf?Lt+M8=|}ml1Z8*kxVCq$5bXY0BQEyu~$py zg3;2;bSA%y=G*4`KB2GOgpj4o2|wW6*pV@J0}Etcxe0g<@|;3UqLwb>mduCb(+ej z{YF+VLlh}poZC^A!RVTk7Y}cw`jj%qd%V-Ua;sfz!+h7{&2?2@Be^r%+J66!dujAp zcJg-#AJ%{;!@<#X_Y|?MxXS=m>^NFgQiPlN%sqSA0yACw4Tcb3*k#K>b~zKX;HW+15J7%KF#)mvZ28m18=ePb(3PGkfBxjQzfW^0tW4gAvWU5ze%BF1^zrXoKx>dK)(q}%XYQ%4 zv3MrfzbWjq2kOqQQ@Y4W-t|ECZ=gSGe+_<9S06 zmp6T3+PX)=5CmY)zLPl*sV$TY!d;GD+3V}swG<1Ho6e2t^jV#{=qTHceWpE5)jrN@ z-0~wJC0;lZ>>J8*tYP8oP6PY)rfkdd(8+11H=qU9z=?K2YWr{bF9J93L5BEFG*6$g z?FlD@Ysu@NcaU2yj~-8J(ArQJx`2I*ggPTfE<^1(qNUu}=YJeb40l)0g$BY+{hRAYYK4GJ=Dgn+lG_;RVx-@fslEEi49@!y}N(vz*HxUhqLqBdWj!Syq^O z$KTdWp39Fdvas+bK8FKR4VMPgD#>+qNR#wOB&vhEBLn=FfX9_#QhrUd!QD#~%c_f?(1N&MEJPOQ(Q7#Nvuw4XpCqrE-mKcNUH z)E?ev9lcj~6Q%n=_raB{Gv;n)6azMokl_{@653J^(NiP9lsr(?b&dEH+y}%6+o@?q z7)10W_VqlbWp7tAYR_Eia(@?g^-ram5L7yz9=4ut>B6@|j&`VBe8@b9lYM%gXQ45Z zman^OYxgUbA;f_!%kh5m@60aSa*`z-ks9y#J}MH5jD?fkcs!;6C_*dL8Q{-1o>u_8 zzYFCi-Y@oKXSw8B3K`Jl?s2l2g3J`JdyU%CU0}8puw2VSDHSMlt^uXn4dnuCZK6#O z6T;?-jeD4AG`PCtfX{xm$mG1_`LXS0f6k=w)x zE-nRBPImp9y_O1MpEEyIbbvChil)bW;Afm#kT=a$FCf;j-MCul|R3#L04uj!7J{{>0(4e z7-yVCp=~qhGK8nPsQ>ruq>g{OYg_>=(2i-^-IIRwYx}5ccA6HRd55Q(sf3F-(%R;S>JxWaqanv_$Tw))XDzO)}f_ybD#m? zxtV!UO<<&zu~8jdTlA<8m%f?Hmjs8X!*G`ErFSI<6eP5i|L35T?&*BwAKTLFCU|Ac zeJqjN(`?%AV@n5PS+h4#Ho+uScuTr7xYh~q9W)vuH^OS}piho?g^#OJ+YsO44tH0W z_BD_VhL+E1{ycTk|IqfwsRx=-wSemPBM*R+Xz9fTr&H84%rf+U(@Mbr^c4pRL>f)^zna%IoMYq8b)>tJHRd z{(f+p+UaEDsReiJqm_l;D7EGw*VBovkdJ%ilAi+!ua<7W zRnmlZicsXxM0(NbDYcR(t2NK-vWuh6U4D>W6pmQAH@rk9~7?JeKT3d2q$u)u&(h3>4Ji zw3GIUOao^zbc(niA4rqn+ko=dJhAkLen)uC-0?0LanMV5S!gl|aa^zM8Xp>;QOn^i z?a^3u2in?2*-P`EEJi{`kDPYg)!xOT+?WWcW6q$<%O%aRzQj^AxQ66G@SHqaaBt=@ zqnC!w@y#7;3z?Q+gaaN^7R&ZJuc$M)Qx`15)4HnNJED9ts$ju_r=!qK!h0SAWI@5M ziRbfupncVQ`vX-*VQl0gpN9Lbd*3}DF|UB;w0TDCLJW}fewO=eEVHE3XwN>ex=UZQ zqrAn@bv#67eGv5cVXO@oU%N{FW|#^VmYWsmP}AZ!Ph;s#xxOjwA+TvE7+grb(^;}m^=L*x$78S$9=yrk4fnqV5D)#|ZT0WrUw89MqJGG| z3$5Mu>BWgaV`#qEoF+YpZH#H}7YOmhT68^B*l4Hu9`$=lqmf}N61}liWZNCNo;C}D z-8xPD&~J-HxID0GZo@ZAIIdljOTMx4_>M)YM;y?8`)yrkl)8LhFRp{>O;xvD=2DGc&WWd?B2UGO{H*r_wQ5{o!V(%bDU(m1t~lzMEaFs zXK$3l2y@fo`FvPnJH^ds`JS&FoqKAUc(P@#U4OBN=BZ3%n^=t9!$he^|9dNZS!Ry7g9C^XMZO-rGANnbe0vnmLuoj? zYrT)+EXsSK#7A%lL|kg`$;MlF966u;nrwFJ{o0Z4V^Yy(*(DP(Kg(}-M37#h-)zBB z%k7X9VFbw(@JG9mFP&ZGa$3}D zbc3Z``mVp@x`qj4kur?@5*|rgv*;RRFkUX7dQ6j_+Wuef7=~SsY@A z=Y0>Gv^f@&jn4m~?V14)cQ*5~&)H451X4%o_coe#q4wbIm5TJs71N>mWA1#GA=&V4 z;QMsfTbogyGO_S?(wE|TsbX(-%J~k!P(F@Ri%n~~UO!{pZMfiXV~95R3Fs-NDOGHX z=q4A8qw!kcd3Q?aLe%=t4ejS}5m4S>&L@SA1R9~vLMrTe?C<&2Jy|e!9Y^jf5=Mb| z|9|Z!SWmqZaDFE64y`PXX=z)1)RTY-mPlu?hBtreDSo3zhhjvBwRF96`TjElh$2? zgtEcNdzPq2YB33bxL^0pZs9*D__&|5ce?EC#me0dtvQJHEPtswJir4B^lwmAjBP7l zC$9#cNIB@z8C_Li(SMFQ=R4{6czLsTWrxN2UvitZ7yt**&YH{XyK}vl-6SD^tr_D! z3Lu{`@x3Eq9u>9r?x(ZyH?y*iK%qY=HGQ88y(zyp$nGaY^6w-_srk?; z+xti$9liq%_y5n!(#ZaQmSqfW5jsX1?8_;20J?!U`1Km03mmHd>}D0+?&#VvM}mGk z12ku5LS7BLq_fpi=CQkKLTZ`x=9u%T)1 z6&;i7VlkWZWWaY}(ekb1=ZBkZ*8ia_J_bqu z$5FLC*O?>yZzDb(^S>MMGFn^_t_ZC}_=Ps3gjL98Z+EWNg;6jy8}dVv^utW z885J}Rx}omg^g*ur7^&q-C1%p3^L3&rQ2RW_8S+YS^K~ z<8TlCXW4aGgx5k}`eO5V4rBh3V_jL%`1}BpHStYSm$WSWnpus~)0P}t=;{f%{U zCD>7_-jg6pzm!8=Vy*kp^}=}MIf$x$v{938cHKWtBkgz1=I;DhOu^sXS*_ovMYTo+ z?+tT-(ZsczmeUQed}jGX!5=do9Zk~XiPZ66W%;D{)%WtXyi#~`X?x+ZL}bETEM@zj zy}CttTzYDdl4DH)0{#d0JWetdGC=1%<(89OJU3{%bFO_SY~Hl|BaR+66Q)uMx!$sj z%>&@_h8{xcyY=h`cNZ2*O*YDL9Q5I9z4d z3}mhp7%5w54J^n})_+VTocLmSUi+?sv*)Tf7A~WwSyrD|Ke3j~ofvb!*hj(J&k`Xd z;c!xcfW_E23cI&13d ze>XNYI{klJRS;u1bD%Mi-A7ZOq(|sEqV{o=5q280zJ)l|E$Ukc!iH~pmc9OglkAh; z5_Aa8wk!hfdz}1(^g`~nWeF)BkXq zbvftw{uq81tnG{DD@r@&+TGd~d^fK_l4I3roAQap1OI!`?}8IYU|Y|#kwqB3UD;V!dGV1H$Ps z@@yH!z^~ptu=rocon4a9PSYI%*}{>O-1GnIc-r)#yaomWq@>Og6ngK%(E2~ah)ZoR z>E!hYZalmhs>rSKdR?sypuk{si~oFG_8*~#k1`9oKATs!^;9g0|cb(69c&;Hwrb;5&dsqh;v zDCV@RHLb7s{qK!9koa{4Uz2SyJKkOH1BTP5z8tml99Jk?DQ<$~|H&8P6 zvid9^Xa&(%CAjHrMe?>BD4heEQuG5Mxq4S8NI84Ke~}(c{SK~mzbT&N^B+5O)|#{g zfUQ5s{w0B*poXN5Q~P#=OqvqUVy*S|8vIv&dLr#||<@_ae{_rejxJA?vaNZ2!0tV1^-I$+Y(#%#!MvU=4? zVR%y9v2^{G!`rZ35V$1cyaEq~0n`=8koA^ohRJd8U0&PJZRJ=|Xxne+lR>tLCLk$r z754`GhjRRW`#07KEf`T%sRsz~ve%A~B4vfgTK`j0p#?LJA(NkKaGaFw`+jSP}!!=z)siU}< z((m}ej2AUJsOEW01;YZ^7RP7pKPO&Ymx22TIA#D!i}sky^)O8f0$S7xx#yKI4&4fR z)Q1#ITE~yZAIYV{Z(jg^IR<3lsXUSUqXOi&Jm6_vPZtQziyd^&e2KI|u2x8;+uvQ& zVmLn=ytkegNf4bt<@?!`TbUx$gYU`&-ip`t@6Ew%trHvX2yydqUsoQTyMDiA=+3N0 z)NTBkqnab&PrN4{#zyRC@K2`-hlNnP5x`FaC43#K&tERwyeQOkZt=w5W&`BVabN-# zXRNlz(E~>_$jwW+YH@Y;v0Z=FlIygOD9774eLS`}wIFF1M44HGq>m1UW|xDF@M2nE zfY}SG$58o|SD9}fl%G?9xv99iH>9Ri9G1@F6lMZlm}Hd?nk88SiaoP83p0#p0*k#6 z7d-i)9;9Df9h$S%;^Go7wNONQ@Vl~^Dt_^Mr&(-$AP#^vSF@3nv_6iX@)m_rf9%z@ zBY$xj^zC+Fq5Bdj`CBv@{qLeFkZ@DOcMjA}T)i?jpweMN<}ijQu})GKGw~t8%rzD7{Tp#7)hm+A$i^mR}VI$%xK8M<1ZOf1(>1m z56`3}2;8!vs@K(&gN7ehFY^b#Zv&kd(mf7a8cDC>wMJxC{?6`?vGA1!q);DGp+)@* zLNi}dCkpM1%)`}{4-WaiJ&FeRRPperAvj`G`!FNE9tJ*_?^Wv&R{(_$QAx})2T2W5 zY=;#-s+nU-?*bXE>be$u_&`S44@uj45cfj=N-1o&+c#)n|G68E&w%BoKN^wXY)R$t ztr<}HYHs8{Ozo&K(Op~j11$`);|kgF-(wx#u9z9kciumR+NAJ4vhgs)p&1VaIaPER zTr)Zk=Rb7%ZhvU%)z_vTNWU40HuhVqV^yElMw9A`sw`ruGB0SPWUB`~0c9{7i+{iM3rZXNrttsfLOC|P_|l+faxiF%!|-^3L# zehFgb8nE^PZKR9q&|}*Rlf&S@xcv{z_h+7Q@&nYhiCR@sp+n1Q<}(j`Xg34ofG%|X zLvKJ1m=^mZA@A#gF83MHyU0!Y8`p2>MV#RJ352zv^#{;+c6aH(_ddcs3vwCcCj|&H z;K{NaEBBru`5{vQOUkev$)+#G!ykfzspc>Km&SXn#MeV%=uL|E0C^AMus z9I@G1-2BH>4T*j5E^PN#O?01Fz#%6OBe=M$NhfAPpB5zidO0|6WfFBc2#g-6x@XtA(RtS)5ks@luX7iCX0M6xD6s!*`)-hk?Ze!XwkTct#m`YyFdAUF zI=6r3is#oA1vqGbkP3?yZo+beCM-B}N#b7En6%`1>K!XwYK+lt> zJq@zVe~r_wp6{O|?GL1GVqV#@9A;MwU16e)3@;Awvs_btqIK#$0!(g}8eiSFB)h;; z&F=JOX_+C&-t*zw(?#LIMJ|d$;cs8Q3Bh#TC!T{ipw3Uop%kiduzV+7oICYKb3H{F(2%Z?B5NCe7m5_OIxI4We?T`QC~86Hql;J|hHBir2d@ zC|vhM7WF6b2U{dwnEj{B6`E2_{Z-(m^F z*9D)9?D1=;e@K#h&5HVmgg+^K|3&P%)Fug&+E{n71wcFsp3t{!P6rjr+MV{<%0BVw z(Aqdn0k(zqxucY)QZ=6)70{yS@$FcZu|*+H&>NGoEjEK$HgA+E-Ki|-;tT!v79Mr_ z#|1zmoxDm}sKP1T#8@t5+FdWfGzS!r`7J2V^AG9p<^e`7YqB4zk z`LS*adon+I71lymIj?za$)|MxK05%@OF{4B5qC=d_oAKm#|zyAM6@7#@O~ zNckr1j&qzqHbDdc3Zf~**UFI23j>%Qg~<>7+q+*zpBLUlaF(u9z5FF-R-W3W1ek!D zPwpPp%j0R2mYbL0@Bm|BGfgizP5@~wdXtPozl>gwDUdNr^8!j@$tKFFej76kMnGCf z&faUeK{9oPTdy)T>KGr4Y=U!u#i7sT3iVGVoU=U0uS^~i-TYAz1eKtQ^sVj@q7{J1clk^26Unfh z4#c7y$Zh80JDIX#01LPf?n`7rgFGXe7r@R!<#q97QB#^Me3t2gaN=bjNN#^ ztR!#b__7x`V>3e~U_8>2ZMmh;b?U(Q$*kk66{Ot%pafBN29oKjIt#M$Kn%I7$M~o$ z$LbJbO>G)v65g}qre$)bx~`$(SRgSG(|q2!W?LovBEl?SP%#b0%E;$?59$Hf*&v*! z$@)_eyLFvH3llsbkJotR4v@}Nf;BRCsQPTR{O?;o{L9w{<$A%-neNVS7t=WSl`3Hl zD=69u*Pk%tDz4=q;{`PQtgx}#YxOu}-!y$V(+8SMi09P{KT-&}5NsL5f{^)NzYD=m z&*;@};^ReMn`#DmR{1sH^;|f$#C^_z-Lmd*nUC(hM3CV3f^bU<8dvtd^j^Xbu|#nY^B4Fm(MBx@cG(svdu&F!yQ2^Fyx?sn<}R=Y42$P#94 z1-v9e-|3b=^PmP&B80DNmA|!VD_fL+@U~6%qStOyPa^?%(C} zFVHOxBSPPWYO9CmADGH^xjXOJERwQz$s-2dri}8*J#Ndk!=h$Sf^6PUJFSD6(AFQfoP<+(Da`;0%P<{7XSu!vKfu9o64=G* z861^R$cC8=kLnye@v_2mo-R z_~Dr#Wl26^r$l(Tdqd`1ReRCXCE=W^ZO-$Df>U z_`%s#quan$ut30Bfu-$VWs&8OgD^!sC~M5#78EU09w9S?3$3W$F#M%OkaW5U-T64? zDkRD>l&{n|Ry07uMBH89_*%>;baqm}|4wcr?6tfvd7+6RB5A;mdV3R#d~`0=*&f3O zp-`F^;Fa-qzAe#!@7(!*OdT5P-CN@9w;b|<8vb;G@kMwpAx(9i9w$Widp!5&W`It> zbeTNmBRqKMNdQ<(?p23hXSbH3ic8J4qvtqh*CLnr(#!w6`+9rd)5LCrN+NG~AC(&V zyW%5~rL^pQU=FD#AX#-8ajgyd!$Yj}-e|9OuP+wtz8kY1>A~-|CE}PFo3@hOv14GW z8P`65P!@G7Ol@Rh+&qJBB~&!>6~>ozYB`81F4e#5HX!UzV6uE;-IYzJh~|4vqv<-$ z6?O2q64Op&Q>o7jXLsNKN%6$z)Qh5C3@XL3XD-~2dxhKTl&#ysQ-gQAxkKBFMdrhH z=j3OW4SpB(F+lB`TKFn05d;oYgx;TJEG=SPxrG1lX{jTn>nV;8MeIiIf7S zQ|(XA0re}-&sQaQ4ufM$yY4>=LQiCj(E;hDdFPm z-RxsDy_=M(&nGj$N+EF|kZP0OPMI$j2^&-8|MJ-_^AT>)Ey4+kE#@~BPVvHUA27d2 zVd7S8fXBY*^%%~J8nL?St1=}c6zum7qk)Rb4MJoHVQ&)KG+mIGbJ&QHw#{+EUJ-se zWpL}#-=Iy$UBNwvl&lC@zs`C(+u7sUK(J>Y;o>4GAs!_pHmD!GGe=Msk$1-J!e{z^ zTnnMQ#1PKa7H)wZp<9+@-M4a0syb|5FjWh)2RG{~B3O>k@y&TClYi!3+HXB&JTvwg zK}oFrP~{AW(LR@9hX~nf6S*g;TFqnk?La(wfg=MN-JWxwcUbmhy;d`s&{*AK$5|5BQ5C* zFXl+oP0YS2qWQq54c}aKR}QD(b*n@`k{z3)AlgfG(a%v>e!Bp}fbU-U#8qTMtM+Rt z3uimdnva0z&vN{m|RRe1m}J?nkjXM-ekc`MU5|NkT!?P8%vFSklKKeXJ=+e!UsgG z^28k`(Fpa>?!!I8(HsiXx%Zk2v$H3h?r0AuaSPo+U8!r>(<@ANf{j`|D2__#B%e{x zWkouB)?RTEM>W;*z~Ef;H<`=JZ}2iaQfU!Ewa?Xw3{+4-mft?Rhas$BXpxvOoQ=>lH2G% zS22Hp^m>0gwQ~lRm5eJXUsv{f2n00>-(t`h4&RnJtQ(awOuUf%rI3YGyKIE1H;>(M zW%kp_Mh&6c23Ps2>gI98<27UcFBC^*x%^w|?JQTnzG;_R;qstDKt;*sRG`r6&q>yF zR6dt5#BPeaoI~~D@_ENtc8YP^e_#ZHqCmS7g<9LSpaTU}iDI?pn4K`uC|Uph8Hoz# zM`thJZ;VJk%OE9&IY4{|CdA_VMV)_d`TPNv{vB*R@pSUj`$ZBAkE@M^LTLwZ^)q?i za!v<_kpUSmmQh*adz8U3qT>lW9;eHtG;36~xXy}Dsu5*Ze2c} zjrLfitY5mRScS?dG18;%Lz!0o`Ph$`lFP)cjkM-N)SOh|?UnRcU&8fERU2D{wdI^1 zt=8dei)G~><@TFb(XTvrZ1VAqzxJi4*{Rcr&> z$7i(l)!6X!60F|~VXKn+rtghdiKR8UBztW=GAZbKT%rK`r;n>?gCIf6)2SsN+i=4u zvUDQ>XG_q?zhEW!fuT`PC{?u(u0@=k$pLB9`@`IWsJ|#8lfi_&VN60(TDI^kNCJ!Vm_Xy1>QW}XTWfjMOpmh560vS zsk5aym8U!On@sU*di{aSqVF&3&7IR2E>O*y0{#%PIxU!%U0b;#==_1gE&?}OKW0!tlZpu$_TU>r3@&?XZnS->lEVypN}#kdxt)fJvbTkJ_`{V5Gfa zaMEhU+@pvE|0k4pcXEI4eYa}}=M>u~@g#bl`;jqz2mPNHn011a?mOck?z|nQ_F)@Q zuL$#W1P>M}WvU$yULW-Cisn1JN_{=5_%WW!+2!Pi?M-mwQ1H5xNB9sO#4Cf!t) zrkA>NxNIriI$WfTu#y%>&6B#HN9GkLKxABxaT@q+J4dojY)EUy2z_5V7%@aVN_er* zL^`%_uaEe5?hoJ8*ymFZiQ?jK@e-Sp_sd=tiw2XV_4@)t>xE%|$OVzcewyvADXb%O zWHGIM?z#_HlZx~>qtd9hvb!8(akvyKqgV#tWjR#6=51YxPVT$5bJ9`@)eG0iz2W!Q zF$trr@ACa(UMGl4iY2Ioi!7;u389e~bl$6nhpUiiFunY+VPn*X@Wa_!0?%KWFRWPFQbX%V8TU$lI) zaHQcEP=3l5u;Hv$hPU>d__nhD)Q$`>gKY#u4dUYm zU#C#DzF>0vAr4&b_Q)skwc2OC-yQ(_DftFJ5#QgPT3#NuU(KC;!19e`(sHJ&);AGrabEr@43&cLOuMRgXjr{ZWo`hCi}_^zou2@ zIAM58uJBG)IM3twFS@~7s1M6tFy;v#yW9CeyBwnUG=o95J0q?;61jfY{j@V?fze&EfMUM_BXGt{x)kdr>!b>!?&KWt7Z)gBh+t3Dvv0%M1`H~!I5(Wk}Y-;;F|xqYWXoYvvut5}8jraa|JD!-SMr;JuDCymdESdF!$ZYk?w6GRl3z|hd#CsS4hZ0RaXdLh zq_8FLW>Z8nS`y)iGX~7jrgCWSRv_biQnl(`n6tZO%$jj(bI&nf+teZqUDS`QyoVo9 zJFm9m$J6!8&N@#-YTmtaO6TQA*2q=u%T)uCG zUmqc5zp&EJ{jD(thMrZ>w)CD8k17+w$vM`5xx6ci;P)oCr2HK zqvhC>FTggtDcQiQj*rd&h14{4lq^4FGi?1 zJ+5T#kz2x*P#Z@b#FygDIsEJKDu?Xf0_OL{9l=@-!e?(FHj!De)RRb{@XT+HpT3#JjM zi_z@sKUCdllR-FF#VL zRq)#ae3(MZBGAeifQDF5C2W+%&27{HIQLf+{h+l?z29#K3SM!o2(d8+FGj< z6TjjvCPCGGnntf`@k;0k7)&SuJ^H-xv{4yisWK%YiLA|P%k`Y>^_aOOw96OlPR{RG zHx)@}-V_oIGte|=ia=>Vk$j>f0CLFVA%`4@H)|O~-Ok`oZzBG0`$LM7MmD0p!2{!B zJ3a6`^ShSd4g+9=jS3HG=C`5IFL+JR$1lXZbY{Ga<`1gNVE0!7)Rb7n-3Bo;UIFr& zThJK~qTNhXi=0D-0Eik0Oz)k6!0NW~O$1p^BAVQo{@O6P(F4qt@_(|r(=5XmCfPT3 z2DW$yZq93FxTX)_D6k9gYNOT_9(yWma3h5#qHv?}+ESs%$uNquK?#Xc`qnP~zjEKY zR#B|83RJiI{L_m{9Wr`q!@%(z8bt=xn@9>D`na7PBfF{b7K*w@VO z2l}!A=bnT)C`SdjDDjLe0k+=~5L)*m!Ry1M_5`wihMv@$7pABc;$iAr9E*zsHA3lU zQ6^}GBlx9G!?og%crgOt=qxk(wRKO~S@CHY)>@q1_-z@XyWay8Hnb%kZlo zXE*$Tbo53l<`B|`UMdID`419!|AScwg%*n*Yd7X}T}ePoD2R?jCJPStIP{~e)xZ~z z%wE~_ctX_}0D3lgbC;hc6a?I7xl-7jS{c|&WNY$53oA^(?au%pt+wzn)3nPyW8N{l zbn!t>Ay<#*_d}BY&`rpdKz(ct{{Usw?TIk&nTMA5ex5!j1EsbsVtufbul}B*ieFu@ zcT02z{Qt9G0(hU_gM3a=@N5!e0yJq2;4D^z`p$he`KXi&_-@%Waz?MHK70A`wr@bY zn)&F8q&a zqfgQ5gn;GV+FYPQi~0L=J>PnJU?jn%*nTAiv4%osJpV4LIbKIL3)+|>i&xY~iUr=0 zB$KwI*G4ivEJ48YZ*7j41}{5!zj}MLc%Q`Eiu~D``FabQiG=ch#n(X7VO0k+1|F3C za>=SH#i#QJ0tH=6-r0JNmu&PnT|%zx8!HVy6_D@9Gq?;E z>U!ZDXmDe=w{D+o&o`apw+*#OBPn!QD$Bhhm~bzElRhVcB7MlUF1b%o$+48PpV549 z9eHF92-J`Q9j19w4u*_Pp9kPcA91THMFR!4gsKe1?RW#N3K|r(LdT>Uz}S&uiOjK- z0ePERR*3|Y7@WZ&UQUCFff{2CI7hN^pfzi#{&Z|Cdi5yu*s z-sxNHpLj5Be-KyV{Afe^WrIts<>4O0m=M|n)(@o8M%X;NG#{Be*G$7G>^11g z+qaK}1`$tLr4`TQdGLX9y5!}lmW9?Sh->0L8B~Jw_wVE&gPZyM{yP&3RLUNhGZx;u zj*cF`L048}5}JL!L(K;jue;yz(gpqI zjwvx`Q5{L*twB?r+poM~kLVNIcRByoBo1Sf+jiICZQp!-7-VNP2S3}$n5DS?8)wh^ z36g!wpcXsMa{B1T1H#@7@jf&mWJO*GoO;Cza>BjArG5N}Nf`Qk`K#snmTPeau4>Bj zNDgALnnj^NTjh26F0oLsfo3h4xc{!n`6h4nX_YVz?c_eC2LhR*!+Pu4@s?3*RlCrY zSKVC*!+Z)|-SqbQ1au_EQz&i#ah)iB@`$S!WHvNDPn1gTr&tg~ue2{iEG94Vv8Q7L z;Iq#ORCR%t>$9;iUdoQ)P*?+licjgk5RC=-DC{;S!8neIs81XRU{QdEnU52WmIS&y zRPMGasvN92u?BpTPXaH@V}m|FKM+_|ts$lYo1kGDMhp7W2l;g;oS7M{Vx#ep6Ht0mV1rapNU;DOGKCR58xFfUWdw;#nDNaX7 zFB1mam7CEi3ze^fvb%0FYwCCP0UV^T$hchBGPg1RQp^9`^}=ljg(-uDQKq275`%OT zCt}l&CW$y>aQO`Jz7G#yhv3!uG+^v$JEQhNjFh`dvy@Db>d9rFmVW|ha%Q<6Qm znv%XKzs~)BcqVfdjTp^^CxL6$3&T`;I7YqVZd7fpga8iqRr6a~BaN`e_=9q`+t8o* zpz9m?tliiwcuN6Q*|qA%A^T@!X!YdPFEd%PcM-ekon zl2#zyoF>^%s+y~3gNSFnCwd^@Q~5P8+MYS5P)+Zppdrri>`Gs6WuU&Gn5k^}UfEU# z=jU3w!5kB$OiS|4U({>PIoh|By#_73H&(m#d+JM_dweIwIQ~4m`}>SAFD>aUIGa!= z&g~5T<0*lyv|f9So-G{+xl|dqrOwf-V7Cme-aKrcuT@ecsv_DH$HRq!qnR~8t+w~ zW`%9G1WOld$Yp*&3OawI6#37r2YbswW=T^(lgh1P1JmVI#2x(-Ty3?+tQnfSEqL2v zL+j-)k>d)u`d|g`9-vJI&*GGoewZ{&s|1*$9M-{~2H$?M4{S1RYu2pM4u+ ze4T}gh_0=J!&VNKC4=*qwjry62B-@VHAeQ#>PsObN|DSZi;84mbSNf3Q3kL*;+auo zLDIjltC<8d(-l{Uq&uQOp~bIV55F$E92jjyvq%*sjrk@PMs@w`8WhjAf`< zu_Hv5VUJ;j!QsM?DkR$TFf(TR;@3)yZ#6|-{~2Se9l=GI0)yxdsfw-P@1`pl^^*=_ zb1~G|ECR>>L)Lr8Q`yJ=<7eQAtdhM_83`p@IHHJ95+S3IJwmqQNTo6&E1415r%1{= zr)BTG&nbKF?Kt1}<-YIF=llEq?*6#zzTJ=ex~}(ljpulgSjpLLXh*cD<%uabm>C`E zh0_7Rac8x7;lF4m2bJMp&i&wga|ui^^hUH2xL}-qE-^E}SB49>)~?QUMPwZuQm^IV-f8B-|fB%k2BS4dSlICQXjkdasav*c4Z`c zSq^*AS^)q^wfJ{a)4hPYz;ViOCyDZq!wQ||1KR{)o{z~4);le*oD}xJ;|+p?CIyh# zq{o-xBK4ts*Nov`QvhB0uiZ7#Pz(4S(a-4%p#u!d(R-QaCF;5Q>4(8Q74fk*9sG12 z>b)&Cp)SWml6kuXNfG9-L?7@7wAP}s7W-8IGyj!qG9NJ~#TcR5&Stp^rqd|Ro=;c9 zi}b4s^wn}Gql7%=e<_nLE*}qE3u%K-H7K|CKXiX`S0Uw-y0{me-&!TF${*E_SHj(| z@CYe0LC+4 z?TYmqb}!F|hDwuyPm!EA>E<8si12(l&lZ8)wL9Fkl1mgt#kjq9P(tkaUF7}$vvYq- zpjX9&*+GU698>MR{&P&V-&s&4fWFV6nqzjgZMAK5M+6c94V`tTv0#SN!jRJs@CXa4 z6!?vC%$dg>+WMaIiSX7}st3MQBdnRHUkFk$vW#d-^T%Xgdf!Tic7m{5xLu}x!^BZA zcL};nish_{C=dP(bO5obdO*)B{Y5=S72CfA~LnddCS*}AX-geG+(Jm2wHe} zJ@&aL!P|%iIJj<1bF=iK-RMYc1f94!5SjUGqxhI3hjOJ$ERn4=Te+Gx63e_Ij?~NK zWa7Ti0h0(F`ix=%GQnAmT@_@7+bu9U@>PjpZvQTA%JVqkOdbx#C*ub()c&=;u!H1= zT4V}5dz#}syGZMsDSH3VS60mo^0E3{0@p8?Puj&^l>J>!?;H`s9yH-;^p!1oq&8K= zMnFn7iUE;0!;em$^Wd#g7Pa~4&zjfSMR%PUG=V#PdJwxp-8}M7m8_e&H(s`d&UbWBxZ`;lPPX*nA zmKHlI^*cp3s{#nLc=hanq0_M~$Sv536Kz^ZB;f)V~{@G zgs}J{>W?%WS8OEEP5`fuLQ6A|?b{ZQWN3}^_m%AlB93v;)C>B@=FWm#Gb{9>0DbuM z_}TylbWNB|RkxW-&e$TD6TM$(P)j{nkRvjtBQqYF*L=YP%>ZkKN~Im&kEscP?h$YY zDb>cdvFrf5y%}KzXt^})OAzi3H(-h-gW`n#R_ zcNQIOo;7zU3rGpPoP&8eaC0(~Q>blgnVuU6Wg|SwF2AMq0&Nb}7sEmmh>oV=@+)vV z>On32q~PZs6Y$t+-i_xkg)R?u>Woj+DQDu*-lH+c!{k|O`7*A>DLiH6hi!w2tYV1n zv{pBQ)jHLAI{7n=Z|wg*D1jGy+_-PuShkY`+q`Tjf3tj85}(B_Bsc%WsS3>7>)?0) z`Xk8w&QsnCdRXfWr(TLQmo+2qx$o6FE%SR%!?ZKS*kaQ9L^Hz{&~*kcNm=dp1Y2H2 zU#*L#-wMG?4M}vKuz8wxVRAC;ZRD9v9abq9kLdeh5)G2sYP$ z(jVK(Vb;8RdGM5C+MZPExP?6PPqrN)7`4gY^7dPFW7pTE!+=Ib!+-x`y1seae7O?F z6ky#ft<e8f?o#fYYu0 zhWuyM-;1j}M@rJ&cwujby~xrGrvC+|rcoX)IIC%HYpaN|W3|v1ns`&(!rDxJ3ALvN zR$s|BXKB3@$hdrkRs4ns2v6I>t6U%TUPkY$cf1_GObHiE39%yFv^u#=6im66N>~E! z;8Lh#1CRLxxzT(x7BA_fKJW_SUKmo_OGyw2`~Cgf4=5I_0Hc-@80tK6BR%f)0PRD9 z(u`#LMQ6H<#y`|7InyY`D>kY@b?AvoOqf%Ebi@Mj(_W@WbyvR~?t;)bE9Vm9SC zsCHmrVHnjiUS(;OWs5k2qNN%S{8V0D+=h3Z6@nFU6Q zBNHDKIkd`<3i+Z0SZKDjF`(1hZSGWxg&gbf7?=5)xA{kc^kFRDrdh zRMlyTPGsrG2uwR*d8+eS8Y{hPR?06+6rYFpBuXc%%7-?p=B@F61M>Q!+#U ztu8q}kn;TRnbkzvG3S$9Npmb0!WraZb@f9oDwKQ@4bRzt*>PKA#lbx+^pYD(27UZW z?8R>czw=h7%taBcvI#kJOzde?xHG%Ir#s(k%*PIEko*|B^U7;BPO%NzM2~?b1g@^71_|Xyfaa{3=xE$X< z*{464)Clx=struQ-s4Zb&fU3yqNVBsl|#YlZPN9()SX6mW@!Mz`uH;qTCmeLTD^zQ z7xfvo8Dj*ybJfC7knx)2J(d@NkCFp_YRocloEP}vq|#>h`>#WMs(X#a;+FpWB=VCmbtRK@j zx=AHJ6ojnVZdCDbqDl!d($GYd!z&}iDrgAKc8pVjPcAxKj)pt1`o7_N`iUxmMb~wP z?Rrg-hI}mBlloh=FLTWe=-u1c$aHUn6=fum2d^&4vkH*uJAO`c23_XgNir0l=E5d+ z3u&Zvs%$tUdeK8eydt!-Z=_=~=6Gt>spLUF>Cc5!dZvm~1}DsEQasH`=T`ro58Ip( z+h{*i8~b>~W)C}(k%ji@`U#pO%8fh<@FLM^X$SS^h1n|1rAf}uO@&)@yswAzacK<= zAgK+8n6rc5v`3u^yn6KPAaWb!}bM|4H!AUhYV0GEIFJah*H;nwK0|hJ3-Y{+)n| zPozw*{cbyY{zSEK_0i2n@|dQ=FnX%!Fv$Po>j3n%{ht4CW;eJj2*=gDFaFhG4BWxna{2zs~L)Y zJUV^lz!-Ln5LK^~!6)#_kaYxZmmzxd^@mh%)9WUmzSD63qSb)$_}yzbMTVGt0+nD! z;8paM3RtHYYzd|T^B9D6!iZXr;8+AtC%?zo>sK7-ui6L+prZA5;g!OJ!gw*i1bCIk zg=6F+&U|3x%=~eyH=mDaJdQO8iQ{-*MS44cfpue@u+npyS+^s0*ggIto@*%q?wV)+ zD)%YJl2^g0Z02D*0H_>zNnhrLQrc8i`CytEXNj@q8PvIx}(vMT^!a za7j(6z0xQg^KU1k6-U4JrsBu8%TI>LQ;HhNUGUZ^{}3= z&%QotKK~P324Nu~ex!&fcLpy8g?GWtn)T0rau#2EqOeQb()-ejl#IKW@qT`JOtHiAQQV$~;CdTg!Oaj&R4L##6az zqeYJ1K3z8Yq4&9klS$iG7&)+ns#~yM>q%33?&+UL?9a9PG5)FNX0WPF;{-&oe!(yC z8&4zX`~L7PCvMv_9xR)!$AqE-1vj;oo$KaL?Af04ZO_4a;w4=Fk$d49X4SX_U7j1b zc>xS$gn2@@z;dNUjd9rvXtd55o`c2>+Tg9z$(cSp{SR;0pl(5kLmR-r{xOS$ zY$dX&{G0w9hBln~CUD0C7O9)!cU~^a|Dy^99-tQ_A)n;ij{i2#u*KM7NOuFEgCp{< zf|#;}&5<3$Eibe2BL4Mn_bsQoS!{bNBj*!ooeeIX0|P#J_e=$!zL*sz$~)X@yxTVy z*0`G($PalB+yeG0Ke-xSxe1(?W9B8Y_qP3~o(a9GP~{PI{V>Q5Mmwo59$NgWnL=?D zG9%U3GSo(G0oQ0tlNGXlZV zIe?f2!R`NdNL?5fj0T1fNJ!EKkp8lphk*w)Lo6V39=4BXkrGY7i+pP(s4V!>``0KM zM8c<=9@)ru4JY*&=dapcUy}sxd<&NKXY^`ZR+s4IDf-EEko${$8uL)$dsz*)fb|zo z7J$iNN6da>2xPQ+5!eC54J|`H%zf!O#UD1*6jUX^gK5D?%|A1IwfBLS#MzN($ayZt z-(vX-0H9Ww&+!f0{s!Brx`bSSxC4D7iy-h)o9{O0|JP&;+6VN`-p;q=A#-A5_RxRk z7NrZ_)%~j5rv1n&Z%3EDQ%FeAO27&rnWrROfImZ9noVb9 zg$7>1ryeL-9oAy$9`cF9(Q{%U|Jg`nfB`t;z!v7vJ{PFI4+vVoLumsVPWtE*aCot= zvW09F9LDA14AkeQN(hk7I)> zt8|-gi-$YVJ`k-uK~njR_!)z zIn*I+RC&VwZUK-Xw?KvwU#=f8s8I$`V7VkPsR8B;Q_PTGPa&%i$4p0(V;wQVm>$+d0V zok~&IO7dLveb8IcXYM)qcA#`kDPVJ>Vli+^$#M(8QNf{_iN)pK_#O@XLt)>t83RQN zhFw5U#?NBbv-lsCe`|3u4OF=wtFx8mjpy1+mUQ?wS*}IF!CM*Ji;L^EsqKnrooD== zww%_J8Mf`$oz|VOi}J?Rw&xjcelUC`NA6s2a%`6jKPt~f8iq~zA^pNT+ZfzO`&E7C zt@($7cDvGDv7T}!t$PRG)%kbZ7l!~t`z0a;z=o>cIRBo43MR~Q=m>->Exzrmdf)@5 zU_XL}Hdb&ClW0Z&J~HnLz&2e$^SQ~|%yFMlfBQI!U68(iXKck1c*zkUgzhCz%Hd}i zft~dt7?ir3@pJukV%7V-27SzH#?w`~pV?BAg9rC-vEF%4>4Qzt$jcG@j!t$KdpTcE zDO+T29%6A=Pi|?K+m+d+rhbcY-Th|B-@RCFg!poWh6j8FRCD2pg97oy?6DSYd=IUB z@U%MrtSflNSIV}0SMbmvQ(F?BNBzk;Rbn|_jKv+yaW1Na@Uf9dSFfz^j}lj#zG5I+ z#a2>VU`{24ykyCvkE09|@#+lPPYJOZ_BavCd#(>v;OM zhEMgJIS!iTDoYCMoSNM#NvtlcoKjvIFyH>c&L^g$ejo~?ywDH4?*q6RUcf6r*9V|7 zKJ57I-<7X|_s2%=h4ISDkN)I8SY8B!p65r`bL7q_D$#CFYpM>8%vo&Bwt~+u>ASw$v74MJ}!S}b5(Fr_P= zS(aHu>D`QmCFlLlX}Rn8!>O3ELTJwY0?%1HnuKAQ= z-wfZ%b4{=EstIvTM615BiQSGyN@0w8`8MWlQwRdcD(_f>Y$utP2^wqczUdGk4 zw-e`yxL-%7ZelMgHo9VJFQ#>nm%~)&CHxw6is=YDz=>db6TlUVefWb%kXwQHk=)-X z!o7X3ohHi~&-xMoOERwZGLw#8!n6g9cBT5y=FME(-G*dDyNj5uN+L-fC`SCt_ZBOC zAOVZd7&lQKPQm%LICaYVd}}hV*c;udnS-wzVA5_=TFhRR@3c%VC3L#Q99SY#xJh@N z1$0iBIN%LJM0j!Dcv<{!k*dKxBSniL;uk!_GBz@FQ~{pABmT(jf1xuXW1BN}K)GfaaV#4D3iNw`#VkUz~%zH?CQYA7NCQ zk8WhE7ueVXaxr4{Qqa;I-$t(H8Il7S;5}EfS%!NAwxMNZ!0;H~h>qoMC)GB2(FZ@r zw~R%3v415{cTTkd>BS-dR2)&xnf125w+H={L+3?O2mOYay4*~5n|qMG_LXZ@Z?DwX$<(x{1kQCHzV#E z4Kf9~sqX7NfJ9-76@AsNT*up-RJXq~DWdqNABVa5{^a)7$E?QQBGz4+yfC7vvKq6P zBUkP4T4|v6yV>R#c;$d{pnUX_4Blxv-d07Tmek5nq12BmSiC*SiqN!wiT&$Q4(fTXDM%}8D#lz%EHs}i zpbSaEW&i>(xftmdXuVZvr!2OxbVb4=X8pn*^$k=fqI(4^Dc(!%bz73t{J!~gw#M@| zxP;7@>a-e@DHALh+-X|qj}Kv)B{m(GIZ?0=EBAw!ukg^0?`2t(2Fb})O~OuLuak<% zR!Xp1Mz%nQ-o1}u%I6lc4yGSaLJDjI?C)RCD6oU2EHts#ECY_HM8I%Ll*;b3;n+6V zVmdyh$s_0BO>RMRW?dI;yNKY@SsL9a_Bn&)#WKlj(n}-co9tzu8A$oggEQ&!`kj0s z?dZ#(-(s6gZVrskc&bRK66+|qeBG#+*xxlq>v^cXoq8a1R^_s_$Nl2zlck)g^>kLe zOS9Gt9qkvTvKll-r|BrSJdilpTwO@t$^aYQ<#f&OBX7nw<@n3MoHol!%b9()8HxEWip6PlP?rl)AZd5NFnT^wZ zU33&N%C?*^xHd?B5KnY~+YRJN8Nk)iL$F1gT|PjSIuaQ7?i{x~*ZG@8cd)5#i1W7h zt|pvX=JGufN7dLcY?y4c`C(_me9z@{e;ItB1 z?)1gpCJ1J5ZckO(-6KHVyt`ZISKnF< zbbd8z4on&6-McRBVm%A#(M9ic3h6VBxPRFqbZ3;Rhc#JjmK=G*NZ`bB!bC=Y+D%U1 zur{D452I2RsL!t(l~>$u+>YG&?MpD+-v+`^6;oOO~~uvHrqXRstW zjUl{Sx-c6TpRX`yuvhbP$$y*~lDl+TjK_SKZkdO3wInw(Oagm<@5vWGPc)*P0#cmz z1)$#!H4A%ybwr^VkwRq;g5`2<;MjE-k0csh#KoKUtQ)`pM3 zo^-6ob}TZZIgwa_U5y<7btJH@%ji#YoSdC_sx8wWc9J|Sna46`pnYWynF7M)#&MFf z#3#mpP}yljE_J)obU#Qy4EQ-KCZFW^J~Wu?b5FRy$Qp05u#9{q&#`=l`>q3Hz!%q% zinw^3%~68UixUGR^fv63vKc?@BAO39f1WVP!R*V z{al2ZDv5X!q43EK8xSU@;Pz)NT6B*uX|k7VY-<15-U=jPT1hAyQbz~c2y88I4`|Sv zOqJ}lurFn0l_AH2_(pamk%52cE#GKAjgC3ceP8v9-hG&_{52Q1htC?*v+SV5dOTwe#!e6FZnVe4;bseidR%OOC-N0x@cMkYSG^?lN`w zv6ALLYF0a}URmtGqy)vn8OFb~-7HoQwkBQ6J`Y>koVo_8v8&b{N(U0*CSL;U^YO@i zUN(k?y06Nvo8<#UaOfD^`J!tL>YR!c3}f|vX+w+9;nvS8z1wm%5tJ18lVO;+vD=N3 zWqUGYIqq~>P>Q`nyL>yx>sAEdD`eC|xL;QrYiFVrV1&wz#%uJ=>nm`EP{eOY3ogh<|nBkoal8A%FF`M1OGSr<+wlt^|wJmSqow}yO@80Uyq!!luVU3wIkG*z;eV_3Te~$;+c)>mF zm`}}G>havPt~Pkgv9nUsJta}cM0UR3)K4A#&a{qD)-XbxbKA6*)|4zK-^tq^6eQzm zB$%UY|m?T}d?i^@E z4jPjKrFTRorUuCgfARI_*FDZl7KcyHR_+!%7BT|4CS&ESWb!e>=`0guU?cDdBhWYP zTTXuT|3ok-d!xFMr=MLD3$x1*7SPWTS=LryD7zctFj>6+1DfK|@hHd|xexmTe+K_% zdmZs9q{Wd}Jsm81S0wHoz27=ac)AXbSV| z3Y^s~>IUrsd$%2tD1ie!We@+tRqA6rGwh(DByzBEI$!#68Iu3haqeXl+Bi(Xf=i^p zBu7!8_nn$3i6_9xb@H9pPZ0vAxJ^l8#O7lqfdc<3m@dJG-#zNlpCUVzwWPe$nyr%L zqjsNTTq1+xaMQ@vanze7zfDFymc6m;5V+~&KUF}7D~R{&RPTq3*CNWIn%reA!`N<0 zsZZiNIZJKi$8MLkE_@56_COo0Xjh%<2tTuMbJ==1!Hfzb;AQI6pY~^hw=&+#Y_5}M z`shZCTl?sm?eTYQyju z(%uLHc|F|k;I37+DIQj6Pq_TZGsJB0>o37{R4csP=4z7-)$Cb*IF@P@p%juY6&Iop zvo>A<)lr)dlV82M!PQ*FrbbU5Fo!Mg7b=63nNgma1c<>FqDZsqyI|g#&Dsy8ftV60 z_+7t?63n+O+Y-;vp=pGi(3;s2-NKtZSWW>}tYb0yE=h@WLsrOL2Br&>Vm9QamJg^= z>_)p`okwA%*giMRTJnolU|wM6zw_1<&jo9{81Hp{3u5ZUE5I``AgR^~3R&tE2sIJ6 zE^}SrZ67L)m%ZWti+(Y#`h3o@vy-7|XZBIFRP$mNPR_AsEL;iYy1~`bqe#Ow7%y*k^2hhs8`q%T7wH}Z z6QiIijgL(7W7llYV&6@F7!PG%es|QETsHdxKCJN#UYIdOjRd;(Kt!>Mh130%HON&6~&+xzRBLrIB#%Q(SC^U; zN|J4x5*E!I@POh{iqAUdKT{LGnfxdi-fBTJZR@FyxdUrO&xn*tMdD@W_IJI`W$QTL zbsaDkfw?@de!l<8c|k1QReL@8*^->hf!V(;JY%0YS(y-V=+QDxQIUnd^``slP2jGgk$=^OO}&~fyK^TZaDC%;+62!)OUX^04by)gsubZD zK1h6*Nb@$prk|974~(q_=fZMh6ON0b{@3EX@P?rD*jwO;koMO>TM zNY<~Ht76&#_MpY;KBc{JdjTV}DQDT>(E|EU#0PV|MN0a*`45X^Piog)6fXNTxSt?o z6^SWD#YIwNbMD0w2e$LUGD?cHuj_P4X76Tr<0tu>bHYH4L&Y;|o&RTMa54AaM>kVi zkDVw~Spu#Isg4VdF2Q}R2aH8FXBKdYs`-*dX=Q^Ah*H1hTiOBb zE;lYzi6Jh26D;C3$kMa|CCFI{&1*hybf6b?vJGYjb=bFe(4}?qn^)vrZ_9&;2@9x| zTmgDI#F>DD4M?RKE}4EI2U9a;X<;}CT4wO5^8Wkb>!k-fi3-1#Fi{B0L# zF!`G}U5Wo~yuMPRD7n0L#_2-MoXm1b&afX}V#p(v*m@<}tRG=#r81WpTrDoeh zWbD37ryOkIKVWK*(d0He@+e9_B>zCD;3Deo*b=-R;Y~j3wV{l1_jK=TD?tJ4r@jrV zkN6F^wUu;x@0~HOZ&ZrhKeu3oV*%K~UQTP8WnKYa#wcVvAgFi8sih)kRhG73!yRYQ zn<`p3CbTllUUi5`dcAaMC1Af0moDk`j{XtknYcTj+dJ;#(&$#0zu-1RGzbQWfdTMG z_tdE~6I=VAlZns&ly~G1GB6XP!^xU!*Fl(p4f1qdZTkYDWCzuv%5h52>7;-qxG-Q- zJ;tH85|$uF{vv3ZJ2ld(tTkWkY9rhuj?ugGSfY)rvfCDc1m{S11Wu*?Nc~26QTeE1 zNCrIm%aatvBkFL{&jHvKRC@?l5q#&9AYnIE#*p-o;H zB)<(N?(adu}(&vjd8Hy1D8K=m2=SJ^h-*g|(@ z1$*M8vW3etzS*1BhX>f0k#y9HiVyO?nwbMFXDwn#L=7?t-duxZK^h(=5G ze(>;X`@C7H+c$HNU&RaZk6r4{0jF1Qh&b?faB%& z;F54Lhj z6E>krp3xrxV~wJ!}># zQy>(4FMRv6lPAFd1-38S$Ck-{X;5wG()lJLPA4|{L1kb;2!)GuX%K!_kHW+utTYfl z7Kp0D6yd4yKR8RvpJWs#efUClJpHmhbRi~|=Ap)c#t&Zs8n~}s zw2g^jG{rKKaIp5kt?YetDSrz_daonfgO~CC>QBy$&ytTCdBNmUf@k0Ykj3O}dSzLb z;?+V7@YSC!yV`VBaWwxv3cdTZ&I!W>jP4(qr{!F%7A`fCyg2Cc72+2s*~2xPU`^Rp zw@RU;QT8^0Qy+pBbVyPH^T2NWn(7|o{wnp+HS+`^?DZM!HJ4kc`LOBIyaqQYr7&YS zSc4EUXjr>Wn8L&8u}T~S)--UNtgRh3GJ^~!XLGdhIhL}$=d%Mpa*by}m(D-u73;at z+?x|^hg^k%>SIL9Nqa`MJeAxLoI-Qy%sZ%}>~YU1tH)a3f~AyGP{V@|_s6U$ z(n^}wo!%|mA@{vsBY3M9{Bi2pxazWF{+?1k08j^C3yUQut;YudaQ&bPiPEg83%ymD zZsGG<+|)}FLF(l8#+;k{AWP@p7Wh=8-(eUN;?FYwEyORTqoN09Pc9nccW?U;;3zL3 zXTkoH^--r`+jxjkwg$?6#tUYnYY|U=pcq2`JPay9PR(unC=_ zegGns4goJhniF1Q_}~3S28y^7JBR66@-cEN5WFk`EuYJ4KlJA%M@`5be8vgbg6UZ{ z{*CMOyOo(tUTguF!Tr*`(CqzpNE?6Ex~*) z`DQ<^hD-Jp2DY<2gk4eH&P+yPy{&(pWaYh(UzCF^zv(1WrU^O`6farTOz<=S3s>f| zr}MLZCvtmO_nq|9PFU!Lj&?BA42;5Cb}em@<=(p?d7(pSqiYJtVKse#cNf0VH@i)I zp>I;IN*ttL9oZdjkGht+(ht_}1oiAE4(z`k1VPcW4_tl0vsG5t9axC*4gGF-as{v! zc&7wR&br!w^Y8+xHhxhP@$jAPQ?D*q>g=OUGG!4d}p zRUIzLWXznjRNd`(Rgvm5`b`Pg^T+b71_9##DE6UHBzr1gMyw8vmp3f=vRaF8qd*Qg z3B+3jTXmr#7h13mG+CSxJ@?N$B2F}k^2_vfKtJjjLwt+kY!b!G^nE&fS_XFu(ba-W z&#W2~ogXAUK|e4asvdST5E{x6!Ru0rRCdJU%R%xI*uA13-{Q%!xt313G;>zw@61_C zuiFbjV!XHow^Z_mV8g>AFIC4ar@f&~c4{tC%F1_Bd9~ak5%3&z*`jehh&w%0XA^Fz zI2LC$xWy8C1o1KiB~L5w5^w`>QK( z#-5Sz{uG{#+aL7rk}MtQfzD(GT6af$qM#E~&~wmPxBf;_vE}3dKt)#;4H7H5``MYH zb4_&2wzO2#=`*W@PPjia9VNf@q3hun!k$3ffNC78V&dLjdrH+2WScLA2! z2P^AA(Z;WOg6hd**ZA+z$|Fj7UW_sgaIWgfPAKVL1{!KBu5`PBk9*b z)x4dCPZ0}V^k~%7RY(up|FQ#pMyOxKMQI_bE=%3AmE+0SY1}ZQj7Ycy+YP+}?{mm) zD9faP>&FFZZLdM&J?`o4;qeKsSPYO`+HOGwMx!r{0+@?X@pi3X;YQhx_=uX*o>KB- zOP%$CJUQ)+o=ellp1f;sEWtMHGd}F2Er2cACI1!ry&K;?*y6KYk_g5P4O>J&Oi!*+ zqj5Glp)nZ!fK(!xEhP~&qaz$Wupi&>#_+85=$hHgy3^WTFsuE_Q0GRE!9@8Av2(la z15fcz=3d-r#$F&CT+)ZNY>TS7(}6>HFc_BpPuI0wRQF_Cjy?Bbn9ODj(#^Q9u!Eaa z1SRS}cc@I5-)YR7gPZy3I=mY>lmtdn++a~B1nAadv-DF94wS&1dc56Z{!^Vdh;#RS z2AV9sR5sg6#9}Ayp{v1I8{fGCw%nT2X3;?T@4<2dr8}K&MmyO|wyG3}nZXFe?(gZD zz+`$uZTq`oJ?f>cI90%7N!SGc4RlKlfbxfoH!`rc!sp!dEqd0|i9DC?45HO3HyPZ0 z$q)OzcR|iEz?+|A`4%JEgDTr|dAT~YPVAg;`L^vAV|;e6JL!FTeB*YjPfb@4r~~z_ z8iAeX&Z0+R<#5KmI}*D+7}pu!H&S)M5daFGv$oK?_UZUg_694}~JK^Yh*I!g=6W#h`@*@Z5j zSc+q@b@xI3&}Pi5!1tBp1vk}rq{(42^OR!KUH$h2PHR9wu6jmSQ_u_AVC5paP$VCL zkVvnd&fNqjOmRSdWq$Q~w&ViePQ4~$XGQJ4&ARW;JqWO01nd>Ge=A@$RV?0P3b?at zBHsT-=X&P)x02_@AmjXycnYG#EJ?VB&_|3X=`IigS$1*y^PNEjBW^cWE4w!mTHV0K z5-dKht~-efT5!6SRt(5L-XJ&Ysa#843Y@F#9W4h{`^0JbTo2lK6_$cE53{P_M@VL;68i=k?89>|{4gFkv0q=)sorHQk?+XfmkwV{{ zp|aNqK)TqfZg5is8r|NuZ=3sU?{eFBkl^`@G|{>&#r}+_Kx5f^19b z@3T;WsUnNk**VZA4kyQdr@7-k;yHcgXj!_M;;7>o{${dsH53EiTedtpx_~QN0Ih}* zpy!^t4VO;`=kJv|x*1Z2TiFoVgS>^q$|{!3|eC2CO)zoWYaHbJG*G6C5&>>4=6EzH>q z?dV7PevP*}+dxZHhqExnbahb(8NE*}>8pk~j7wh7R282Y9ZN;q%8IMJdz%n#EC3`D zDYa@B=dNn5AvfQBP!M?q`?@S8I+mrY7YdPp98@eaJZd@ZP7gNG1_Sb8jLKN4+}H06 zvYEmrTi07SFq&iUF09`?#Y8THj*zHRn~HxgH*vdmee0yk@;qQj9E?D*>CB9(?QK~4 zq?X(QFjAOF@}+mtELt*Y^Eb+rG$pUR3Z92d-KD3X_%s4+Q4SWtc93LLSCYwwWD?PPPOsf(T~=Cu-BZg zb{P3S4T`a@k)2kbMTjYw%^r5;;9(feptvJ0vtY+kk^7=uj zlT&+=-_NtX;I#e%wj_s7jySj$PUaPMZ*ovgu*QIKKzC@Des z#G{az3sHFg$`ZvH36c-&Ie{ra%`)JNcTD)*)9~nCox07sk71jq`H?N20y386Qf#oP zhk=AIpZLF{)QdZrRW*~2l8aiVs2Ley9h;u%dv_JvxlM^9OPDRH$xLBpPSrsqdJSrT z>oI5kQ6Q;c>ig{VBk+K(c{=*mS}7j#ozmOmmc7Mb8<-u5Na3{sE5D_6@Ez1;Z`ME8 zY!2&keCIw`Ket9*j4WC%A`_6)&(i}fzzL=fU^!i3i`20DFfV!#MWScu7ibgqbcSJ- z66N5hh=haEq|NWdez_8xn%b>Pvh5sacunaLA`7mYs_FyuR#!I0^=HMQOv<2I024$|_7`=Vk+Cg*>8Z z60r)&=rM*4$NPgpOK$FW43a5qpYROs2b9-~Nk%G0jSNl0wZ%dQLvb_Zy)Zi!UgER1 zQ=6?$y<0(qPTvO!n7@{arbn-9h70$;3`%|~k$tdz15}>(@a=Tpg9+~;AEM6KqKEPVyvw@`Re)^U9_myn&0zGNf`<>_B2^+odIq5Io#WAJ;&*SE0_p(yJ9=(Chc50(`_Dqnd(=;*bd04aFsnpu05%s@;uV53@p|< zcWqS=FGDndcp}UIE&lV)4Tej=TS4$E`$wc~8px`ItT^4DT&<{nxGpqgZ{JMNVW~t-mfx=~yR~QpXdNBS&LaGX9udqQj0c|8?Qy zMOZ{SKN(JF)mJ634s^ooymAxphLNs0m9TqQxr+MIzaM{B&8U@#C7XDD-F~ilNsxR! ztl_Vhz0eKBQPWq?*w&Rz>phSj2(ggdx5x|*VVI@>uOAZ`)$b`_k3rujU4*TGG}MAC zwECeu&Bv@;whe>y*mqM1FqpuUCs1)Ywt-i?mKMfpb<9L`hXdQ{3r|(?dajP#3{T_y zjoQ4+zbmHtYkrNwtU7kGqHTCOxC&09a&^CNc2tL%@8qd(s-$U1m(?W2Fs6WS%`kp0^>uPETofv3)|OXN|?Sg1;S9B z_D)51Gd*8geOjq3S|zqcnC`;fapvLf?utW=3k*jg*U(jpoBh2!>hWxc4;DFPK~!cX0{ksfifZwLL%>m~biD{;;-6dA99UjJCg`H+&TseNF!2X$8IuOQ2{3QvTLe0-?{^U{No^&c?{cDwVISbh zk2Su_onHn0CBurpXX_JOa^V_OPbY;7L`vRM@6@t$m?n~LTd7K4<+wHm?=kn&%=-G^ z6p3n-P9yaaLb>_Xr+hX1!}YfB#TmqQ)3s_EMpPu+6okM8`f>P*z1UAF9>g5AN5fev zD*H_}E7#yazVk@f!z&gK{G`KF8DWS*=5bh57QDhH7Oui!==DWas(qD4{&;^= z20fO^Ucf9MH~wtyAi4MDk2!ur@y6wrP>cfUZ%*h@3zAq3@&SBexr~j@WCp>QD>POc zNzafwwHZoN|8uPju!<|jZy+;wlZvyprvFyKgF?dAqnj6C&*Ajz=8W=F1i$npTP-BV z|Dox;H4$2w3N(_xn?|yBN+64p?^-qsRwG)}iH4UO0xCKJcqFpQ3M{ z9~jRi@31+k;$mDddf)`Dx0O!Hq=+>KN*b6>m(qYBoR`Akd>g1toROa?$J60PtVphh zJf?{#LvVWFzsAEkKlO&j`55>j5C%Ly^$iWp&9cN z@8aFVo&M$-St8v`PLE@@+?yt=QwjL{MlJ>aty8voGIx$zjT>}#IlNZ8Kf(`!VX$W> zXYp>;k_=wYZbK=1V^-T;Pav}iu_K|U$fA|M^_d_#jh{B z^Un5n#fc1Ls!`!nJt?f*-$qHqSa?V2+s9A4TDJ;f2{!&}`mTy9W!cjxLiB&Pxb*l5 zOiJeYOI|}Q9ptI6)5j*O1Y;t~kN_5+i+fBlWKZ7GZ(H}bRzZqup{*cs)`8-s2kU7| z9=uEP7$iCK9Ipf=#GLTCqzFKNb55rTexyd4SobE0$xqUn@208_dR7kUtM3-)Jqg2; z(3Zxzc6`=y<7}8B2%g9MIWtPL4e8`kcKltq6EULaVT*#O|@qC zZ~nGu-@tc-$-fG5m-f~Fdk0;hZ*mVtc?hG&|625g@9$jdoX+@&z2iAaGi@K1`VN9b z*cnYr4hT@bUKJ%Pe#kNfip&=&Dk<`PCzt<*Pl+M3lc8(y1a=#o$2?^jWtT`@k=#Zv z3X$2*P0pu{&*7yM?R!&8-vlU26_VO|ph*lW0;~>7smgm!#PIR)@D454FkY zVSJhqUuyGTz+U^G+n(RC+Ur`%@R5fJfzo{_Bn7~hzdrb^=zNxB@h5a&Jycjq@v(fs% zv*+ChiqExKz~Azuyi0{_MKFqxmlycNwS8{`L5YN`4z*)PR9;KWCapSWtVdUcVBAR{ zxTh;IESD}miF0mIyJyzUV+SI0#8{S||T51|h@m;zcEVB9Z zp|I1oLpW)gBLq>^d9inQ$G*ovt){u|St!;NJ@;7m=Xdf_PWCjvoc_qxmbaNv?9zGg zw`n3@NF|>Yl6?y$4F)$e5P&JTWwtANOYQ0l?B(ncLs4PwV zJK7@^OulcutaVU57|wsTVIu6omBu3fYI6_EiR;o7P$qRB!!c6WI%XDtG*;#EIn%lx zFI2XUO?vtFbT9ZJ-VKm5}eV(nPJVG&3)XRt|y-Dx+{YM)BZ;L{6=K??uG#!MKL^g4UoR zSQRqKb@3G@bd>dRrK3TAK7EhbQJq%)VuGn=$@H7iHU~w@+#WOO{=*=A_i&&?!b@)_rX-)c*Gj=)K zBx)vxm9KkZ%aCT+)Nx3QzOt`WhNytiD;ax&I^>YH%nG{W5awef@sXw3jr(5k{Z^ z!taomuo*hQrByky94Ygu&PH=u2%`uW40pXgll-My`ibLXvc-J(_vfzju}_30UhA+d%yQ01S|X99mxO+6kvuBHVo?^2VBC%6J3&0$6mM7} ze(_TABDRV}j!AvpT%Rq?K%5;=}RYf1U#gF8s zI~ppE7-$y~Yq$q=>~MOMI}l`LcaoXplxL6T{7xCWTswYIM~!*7Xzh4&ZI21882l?7 zWsY{&oR)B_%Lb#?Vs3OLMw#cl@J_!`@LLh*2@PcFx&Af2z}s$n(j-w`7OmNS6do=* zQPFUCqH^G4EAISp&CLnf0D*!k+_milfsSc9ThB-6in@@9lV3jZ#xTj^s3>fvwDP7p zpFWf5bIOm@jP-nO7t`RJWp|3{F#H)tLP&m4P{3p3EfR0sUa&Vdqqzuc-D$Gk&6C?% z?lkZ<%4jXAS-+#el$fDTn!y56xHAP*c!VKJ(;`hx6@=fZYVGH)v>l%{)$&}0LRDo zP^#(M-b}CN_Xm6DBD;=y#iXBi-~V0rdaU`NEJBT!O7jC&f@!SObSG9ZD4b*}EhbH_b{X zGrV5SF#@_4fQJl>f5(dz9hxAyG5{SziL{C_rJLjEHjloL;ha{tC!Ab zhRJH#kX}dwuZIh+AZPApD(cweTQU?z;6#Y@X$yna3M#i%-}v@4n0|K6y9#@}75yi? za>AoBc{P{k?3W~;4RS;NUdz|^wmTj3(3E+m*wt`*i3`f`=J52uA2e$STx{C?&jYu; zK>oTMP3Ti=SaO2Qx?JIU4gac;(G!a0Z>;#vXS~<}&(q09^nM-OW3)GGE#!Aov}(<` z&E(&vxg&?N*(P~3?cx1p^kY?<_RD@=4I zYZZSBmj5#tN0P>a_{d&a+hp-n+h&4>6ot6+?li*5BxSoqQ9cD!-3woU+{)-~$*>;2(oF0S(agVfCArYsr{;$8PT%LcvVjlyL~ElhV? za@Vg!mY3lCh$t}WZ|-a@W^Nef3U?Aq?`_9T9oMMQLmjlxyT@waa=aq6oe2tBcd(57xM!p&F}{cgm`vwa7z1$?9va)`h?X!Qefu#n`3A;(O>G zd)ikystqxlZ{o(xR#9x)1q6ve=?JXeZn4rL$jg9Z~ zpuUfOF~8N;qYwS76bSP_nYrd2vKs6~iiAv5rl=U|(H^8txY+JCX|Ynw>-Vfk9P=vR z{?O;pQlCY1!=TGeE_H9WV(cF1eP}K)&MFzdJ&o8%#&3eemUkH;Me$Pe;-Ej^-VZ4@ zQrU~kxHZ&|XK#esm$NcO1|rvx(PmqyCZG+6!O@_#02ylR&#vBb=BVc+Dr#U4i=z#0 z@#QSr2XZN&;9Tl%>{Z7xmAss`| zc>=rfyNQqA>z0p(w%}1eJ#;2l?feJrguhlpIIzkqXdOj`YMYs~z>paic}L7%uF*a{ zPX6qvJ%Nlg96F4871JUr*r!cwoIJE5{X>#faqeW!? zxC{hhWR$+4g3+;hi1`k><;X$(U7hegZ2K56<-QrKBK30 zuhtPdAAFqBV8-^jAXEZToe~0bpnS2 zcbCX7*AP7)hVmB3IHV1E`@L~Do?qmcT>w|wVr-vsHME7_tZAY{IZs}x-Hm@qP*!Q% z@TI7@w_4dwqeP~$^0GMBE%tz`b*)*6&on@Km0E?Yu{U?IrOzK?Z3N-QqhvrFUca}i z6Qb{oMn!r{OC(sy*9(Yr9GY+oODgqATIOjffAL{jyc5uGIe}wyV)#H8%Pdi(MXjL# zLb~LSrm+_{fk~jL!)bp(7s_|+6L5#kh9o-)95lFh2g!#W26ydg+;mGXSktBR4NmQ* zX=kQe6=~8xk~8_4n|nD|w^F|vz;b;tU&fcepIWW7f)}xg&8#-xj}MsZNo8Ovi``5b z;MO<;riKs1qds#5LtV7PhJvT)28lFsopW+g{@hW{6MK-~@{0cxx30z>0B?tgF83Y! zh=m(#Z5oW{t>zUy=y~ACM3NS(D2G9?3iks&CTv;Aupkt>#m1Jr`P8DTN(B47nsa9oc*z;Yc8N};r{k+K zPY17hf$qY*7CR9GmPTjFuHpN6G0Pqj(P)iwjWK+4bvW#jU4}-CR{{@M{nvP-6ntCK1m4+`;o037%}}5>~krw z`-yGn>UBQpPQsp6u*%?M_MFDh)Xm0-Dgd?n6sIMuW|+}TmHYeOjL^elFP%rQq2d=` zRFm>ViI?zA_cuMCeaT1$V0f7Kv!@4notGkqyd?{B`cFanpwB=B{B5BernE?pTUaEM z;qGx?zjtUf4~Vp5)_;Ga-n(oZks;r0>}A!*4Ss#aMA*o}mu!3YW<-x#TCd>k>`s9* z{Ue4~+Mt@ys8#m5i^u@-4|E7>B~`&LdYp`nmP7MVS8+m+bbTt zk0l8`I~n5m*jk`opugXSF{pw(yzumeD5NPcv-X-~Y=3G%;&dl5YkK7yG7> zTrH9*uy-JftnMp$cWF$G;DCQyDob?~eX7-s>@c9)O-Z}P14nsF--OOcQm_*sjlGv8 zI=m>j=f}s6D9?yva2M1+iG^QhFL=7ZGYR?u)^|&oBlPjMqs^|fO8@J~XDt}m840vCqvOid!2ZB#<{dO) z^;h$3!sp93*}aP!X;LxIf9>yU%}71_cZj;U35(r}ThDU|3R|=Uw#&7L$p392HQ_O! zF~X$i8IuI-lfrTF$&<&Mu5(9i-S!@0PqWt);ws4Uc}vw-Sz^9-Ig4-(bk}#!|C>HD zp21U^0jfed5h7hslw2wpOIFwO;TjCW`6eGv z!%bqMMv8cly-8xF-Brt_%v$-S};8E&6Bh(JQxXof7K!?EX8VC?5r7gbtUscO@H&9GId#JO8_)*3mtCDI%ac-o0I>`#J~qxRZdzn$SHWVL(}8+~s&nt2 zcm+FH(&QZxO6)(L zdi>t*$$0B&$Y#w4CruOdpbyf(PjdC0puz9b7vd1@2<|h!1Z&sqVrC2X`Np(s>hGRC zJ$j~iVB32!q&4KWxA1((Qy5Hf5kUPI=dZJl6s0)FE6{b%+xX7jK57(DBwyrE;B|7o zk;ymG9z5?AduPZr>`dZ)0TWz!pYMscOxqUuVrE{5#{ekE(e#U<6dmUCyvZ|#dodoB zM?wA>Vd-y|$oH=);!eu)r)`Zn59&Fu{QpcOHO@i$Wi|; zAxJ~*z}@2ew}&z5%w-}^9@St|@)q<>0!6%$iLGhOF!q@Zxl_79H#Wf5UN5o&9IxIf zluNM!=iJ}#b2nbITz=-iE(2U7+^t&3N=KIYimxk3;N{Nic)Bz842Ym6OJGf+&Uh6w zfl45Ag>~X#W@cR3rbR{->qJu{Qux=InbK5|ZL0_N5iP!^cRPCS#-i764%X`lo@utC zG9zF^(1-Vf$iSI$EepH26$<_vErCCh^b~e7{L?afCCqbm&Mg!oS|~Jn4A0 zEX?WIlEU^4lOGrslb^_5Zu4prnF7bGG}Xqn-D+|L=lw3-?;PnT9R}_rKf$+JUa(Y^ z4qQRrwb%1WxGTyY!?N3AwZ8U<$K9&%!sRN(geJQ9Bw*ZTR$a<}&SYzU2yrGL0-ug= z{h7LmA)Wqk&wP$)Ym8N6jm!H^`BeLMJXG?dSI+mF0 z{WAVaTr_0e-ehgadexRsmyx!A-MlDZSk;F0@5}hUm=CKbQ`D(d5#k@5EDR3YR0!Fw zhx@l3PQx#!#|c2pAW5-=m?IL&OmRI&TdjnelMH=qDC3YDT0A1DxY{N>KRkL0930Hn zoZlo$cbSJv>z8LJeNXt)WVW7N6JEtiJbb@{4LXFoRXcF)%RB-2=yC2cg4x%FmZJdX z7`t55bL+FOSJx{%J{4ISx>!s^=-3FzR=-OTn{MF38RGm!uCjOFrZW^|R&|4^o?d#A4^HYLArUD=Me97NTV zz_%++txK~OtokTK{KBW~Lv8+CPHavc1;~fl0(D#B&uhsXXPq8%W%^m}@6?!eb2sva zlN`!+ckiV(xEILC-*p6Pu7mHVfwft9Vd`Owv;29TRE_xBMb@6C9>911W3MuG(Lj#8 zx<;QENjE!vy{cdK<98hbOjD0{1EvZQ*=|@{i$}YUq>WnC*?m)RG|TH!H_2A3L&bCr zddA<#k^Xa}Bm72i1Pc`7g+CWRRU>-4%yit)MtKrmrCr8k+#(5C$3^2 z4V^zbn6+HXCV~rwYWEAypW!_Zd%I=9F&wUaLzMw)|7G?Cr7vUx%IKNahJc-cE{T7t zncnTuW0Itay!#^OKrm3}%VW9sYGOpttL*ITopIQW%vB%manKsA@bFS;*_jrwvl78H z;1$Af`Eu|NTox0gp_L`QYW=wRe}rX|Zw$ZM=le9)#FMM>T3fi>lvK*;;8RIF?CbrL z8i7auBG;w60NW>I(uV~Wq^1Lq8@p0=PtZGp5u=yNx}LFqgUK;~#N~j~fYDrTupamf zDPlkjr#7Ao{XQE7e&3T>x~5PGpPYVQq8jb4RgLLC(nMt5R>JI%i{>!DyWw~4{{p@L z2L$99BB!pLm+xNc6NbD)e}LA&1tQlr8@_w}Zx^f)ZUM;xSWH={65Yl7&&fdpp7*0h z)~3l35}bBV&t?y7ap>IVGXPx~+K_+nz89$WIpw0Xs7R;Ux-(x+kK-(e;2VJK5CLSj z{X9cDA`@0cez^#QA)SNcDp^qSm>{hypH3<}b<2+G09g4}S z8ZvtIbc6u0K`Sy#fSxIxS(p%nH~g;iZ4rPEqOHX)%niQ|uQk}S9Ha+PH0}{+C2L}7L?11+mZ}9I_xoy7}7cI9e!EPP@-%QHPN=pWE>?9lz zaGFyf{sj%iftX-`E zJozVv_(Ry!n+M!&TR(x&sD~u>AD{+=ZSOw&@D+%s&aVPK#Up3OrRv_h{@(Dg7*Gw+K22V)aBT}nvU&j>)6V!g{!lMK8*&ds zh=KOZ|1O^^85yAe>12s{*uQ{;F^FgY^*aW{m|p>*)cU&@n!=Ct&ww;W!f<$)+CM&V zzi8nDG^|jbpP*of8F`BE{J`(FZ@wPr_VQByf5cKP^ty&@OIVt=a&X$e+J~-OB!+K%iOSwI}gw zi#Z{~AWPXYLkQkS`?3Lp+duQqGHXK1NNxk!cTd|lvuk1hlmD!v;^*fB6?J2JjDc1a^WH0Wgv?R2Yt@^&ggj|CVPk8zV~1 zQwy4)T5(peo9N8fTKEYrWwV7G>**%?Pyb4#1P$nH;kwITQ4+WCPCCWr4Ct$;^ zj&GSLSQx&z*mPDmkUu1*!AIBGRf7PO%8w|*OiOMV@~a*T12D-2oSg^5J_o-p|C!Vl z0rgo+y7B5tJ4|gSg3kA!Yc>1ZrqjHkXYklx<%LrtCCM_~ZNHij^Xz@sb&DlnDg`+t zS62n>?{>|Uj-I~}SoDf*0s83p5>ubLe=wr}%Te<`qA$9s`G003U=5T2;LeK&G?#IQ zMnJ37za9Z!A6!E>XVCq)5PuT>PlVivPM9I?3%~{egMXmJoc8dhY!>Y2uleS$5fKm$ zlh1U`fDiRp&*}-0CYf?}ns{tPktj^AWU;UIli z#1eK`k{zlt5b!bK(T9-3_qOw&)pDAMvnJ2HxaV!AYakGj3;;B}F$&M!0hZ-~FVR4I zybQl={yJVJ-hdPy8v-zpr21w6E?o~8Qe1nh#cnmH3pjS}M016Q0iU=9BnY?tR@{gk z5B373g|~W%nn=>DMfD=)+I}Rhli3ht9BTOK1#$D}uXnW*{#IA#AtoexLZh!*z~&Bng;6!f7$Wc`cdDeEW}lpnO7)bwrZ z*_#1(HX*t&oYt7b-XU>`F;ZYay&)!~$+vXZ2}=TcmpXbh);HHK2whjpH{xZcdtc`< z0|O=F6ucFt_;BB^Q^dyG>%TEvwutR%-#mbO%zh&(menVT-d0CKWohz_rLmw9Y%}0Y z2~4P%S-6VN998t)Mugn^udsp&0b>sbV*i`OC}lF!pqZk?n}y0vCXf0uJ3uttww4gQG+bJx-g$wj)O>56eW*9K!3^_ND6zYpNk1 zC_9xc2SyyiW7m#cs_zkpsBumiv6F=vzPh#V<|y9H9;b+zm?35?p<24v2W0q4%UbZXSC*r~N} zwGB+^gg(D%ANn;dUW@#@=Nne}-n{7UbP2&?fiM^&qp*#$z-`%r4O*}Lz zz1*xH(2@(FNtda2p{i>l4&(oVOif0w_>ks1y>m~cJK z>L}66iqf4pDdMns+r7gaMsFD2B`p2C#_OdBALf#{oawAI93Q-lqIhn2Il`F#tWAMa za3f8eq(z7EU~D(?BHc(y_}thed5R3TJgK`qHV;GMy}CIAHpN3``UQTx>UT*A9xwfp z61u`G>{g&M_Q)ajNkoaP`z-Uz# zcdu&BJ{Mp08Epmiv9$x50uARqo#eSWsYYy?L5tRQo#*D2i#9l>)LH#(#uaIrOwJmC zbxc_SsLrNU-dJtH5Z1agqDHLBaEB?vUL5Yy=P@Etd>5y!^!;);$tEtWw|}Xlwf35t zr!CH0rT1rPQW!@4DC=S`k)T1%c|r>Ks?V>6?MN5hW{cyP$LY9U;tbekpiZQ_8Z#~{ zQ!2gFM3>6yADvXy*r@1QSH+qaiB|6m4^IKwqi{9O2smLa%Lfk8)7{wC_8>)Kczk^^ zsT+H)2MIySZ+Vq-{n!@3%F3qyO#vqk?MvDlJC|q6uP*YmQ?L%mF40{F1ANts)1-Rc zvVC5kP5Hw9;>Akl&}(cbF|g-7c2n9r3q+UbUtnm4@BKQ-@m1lD~LT^1>6rP16olt&Rd6 z^!U;lu$k|`)w@|#n|e;cC)+{T5P@*Ety3L)Nq_3gMUlo;C_vEM3(mHU47k3!ql^|t zuGNEO_Fp^Z8&!QoOHs5kb8U>V6)vy2K%G0iL!V1$Da7&GH_lxcEvp;X!TE!m1TKTP z)U&q%uEMKUf$;a_yrCxSS&8mRe#41xerN~mdtu-DcWl|Q+=U-#@gMtq9eq&68gprp zATGDYg8w973;2W$yopD!%pKd=1#k8SnuXVsmwDPlYq`G$>`2_mauUV0$U~Vy)H|%| zpn{@w9>x_ad9sa2H5827)c?j>0+_zmhY4c2E3=lqOdnBzrrL9IV5OcO@H%T@zG6*} z{k-T7Xd;e!cNuO0Y*CXugjMu4sSSyX;&sq9qjwEdCg21R#Em5Oc&Z~dCkFMPkrXv` zLR24&;!z}%poc3vUIpOyAV#}pkTT{PCOWtoWHa>V&0pUN(&p8&+aIWLm{e^Gqoz&q zhGeo&`9FcJJZp7VgN)5!M}g6()@|ZU(XCdZUg(o=Z^^iR!l14H7eO!w$vItP$S*NC$V- zNj?Al%@c1y_rR;3pmWrK1R#)Q+*XE}T7*=J$;kv}{8tqQd7Ywr3EGZ`ly)B>W7T#p z3P6^@#CiqU%2{@()ynVol;=^cuP=|?QKhr3s5i#c>U`6U_$G6UgU9T z9T{IhAZbgHK|~+MK#AM7(2vBD^JYoRXT0byBuvP&oJswOqCkd3PJ3*85Ha_m+$QHph1YdBA&dv@4f z_?MAw7vcKp+%3+c+~kslftVqIGnA`*m!_d(eaepq;oaGP))gPpRG_LL_!3)K?kZv6 z$ww-v^t7w|j|hf&NA#fb(QUgc<7EmB(=7W?A|5$j^QuNn$J=#Ugvnh#V67;3N0`9e>Dl?#9UABM-xz(tD6!3$FghupAZ&z%+ zug0akWpyC_btP5^N1M0UBPDU)UPdZ_){-@bXy{6y-oNVr14Ni=?|tn13BV38tedF zdaQQBr?f2B%AmM2<6U(NeJ9s;U8#J_;s)Ga7oV6?9M^k7x2wD`f633AK`g#`@5@j; z*Ib|(q08c-(pWO%(-<%Jp*rVt--+rJOFwz4xT=r@@E9Cf2Sk><=EdcYi)3@G&+g@u z)O{d<1{3c3YP)Y{GSpV|7DNqFDasWUG>opZV|m3xPaBis^IZ?z4Ed{1Yl2FP^Z+qL zN}XUB**sC8w#x1NftJXwdm59{g!Y=+9q~Shw9%m?VYlgZd!t@+YVTNTv$4d2Pop zISC4o3+ajfH8s-NFT_hL9>t?w1Oi{RSrSV4457Xcx zn!q_4?wXs3s0ic1yKus)3~E3{#NH(wd=1s-U3K92=~JPz7=P5QNq)N<)}%_P>z4^v zl~6KuVnNA;3kr!0cCVNRk&YCuK>~T) z)4#IOvx329YP(3XCP7Uj5hmcb zqa#Nn`Fl}J+)9|tur`Vn?IL3q%B7MViZDnl*%0|NAJr1i3`AWc((qg_cyxc}3doz( zKrMZrT6tMAR$Qvy6T)QE@ zzfD@XO*}fWC?e_xwp(EY#$Ckn=6!LD+Ci??7nd#y35rn@CN7QO*HDcRtNSaEc3fJ3Q2s({%^W|>%1E0#wS#4}t)QQ1W zTU=;s3xda&%tS;YbNMpV!#^#*TjdO|xTy+fMpb__-(-ID_z}VINK9hGfp*o^-5ata z0|i=Ea~P{E?K5Og+bVHYar*Ca3}2bw%?&EzePoof561PjoWj4vYr#Glm&63V`Z=*w zA7&jbE{ndvU;hP>q$G6^oPHWJzM1@uQa!0=i7k}w=lDe+$&bDkINFBe-Q*ra|I( zzqbNXldQ zxYxLZ2>r}U7`gAON1c>w2O5lBy4-xMDQ<>Z@uu%+!6NN9d=yrl=|G<%H()#R+bvzb zj=>MKE@d=Xj0`?Z{6I`&>Op3ua1t@0MS7Y;>X(8-DM+nEm8anm%5^%d$CAx7J}q+D zQ4(%O!*YE^#dH<2(gd9m&kR)Onj(8XhmUr+k*#AE{zKH2kbccr5?bh$p9zUQAT-J{ zzbG_3LJY-ZRM(}*c#9IW;~1J1wSeX8qE>U8yTA*u1rH@|a)k7Q$zIcK#i%ZBdjCay zg9XKlO5f>CiDISDJAcvw_-|UkQD&GKgLx%-8G5guq-1(+ zvB?#7`->rH<3F`G<=!?CQw$Gp4cdZD$CDGzOTL22R<11e3A5bmtsHz%O@01ILx-XR z54lx_CF2;mTy|wRR0Y+SC;$W(3B@1>pzS)KJxO2%isVI4FiQVPICUKiK(n^g@6OoB_Vu zs|iW~-Fq&fhZ_2m?eS-y#ft;gfj+t;PcuDe1TviQlux5AIYWgL-0>9W({kAC6Z)gU zSN(LV!Z_z2iRZn_DdGRWj?uA(g*R6AHTh&@K z55az_DcjsU-7532-`V}rq zH?L0R!jWtfd{eO#oSP);yR>X^54utji#Y_#7@ng}TBVI8nZ2*Fb@9go5o!qwyG%J< zg{wl|v3FuePFuQGjCVf5Jlu9w2;}`3xAQf_FFeowN>Myt>knS%`N+AT8cL-QqB_Py z?OuFuJL{;v+;s1;_yO~RJYm8bsh4z`R7cI5^E7jgoBFGX1LyKl(}?F@TRV_h$OY%K z=|x=C(hxSyr3o81FD*hK6EiT8BTEF+zf5ZMk`{saFhQj0U!E8CX_0$uMwFeKeKp(} zBXJwp{4&s2U8*__j^REpZ=Gd6zuP>=Q+)kdnykSLwgX9Hm}xIbgkaZTKkKbYQ7@Dh z%_o@IPU<60pTyJNf~zN8a>YC^uTwg=EC<_O%4qrOuo&r~Og^`}52=yd;rhb@*~mcP zX-C4xFP{H!l%kQ#DGBVcA>}MBXg)tyT!v79(9)*6YeM*-u4XLtb(p=^iDSG^AF0kr zV$#Kl@=?+Vc?o4n4^_-hp0-MIknev3;9q>+?mdixp`*@<+xeLn_%1ePP4bH9QYbF@ z&g2EMh(CwU5k>(oomAHw0xXqkNLLI)G%`1h{X~Db zbGJJRr4Dr)A4$-vrV{=m^G@WE9-!4X|&ZOjpf$*bEigWGl5uQlOvnO1U zWD$?m8QokYV0(+|8eVW&t{)FJ7z05~4MN7L*>RNL82DczMj13HoAG$EXPi&HF4k}1 z11oKuHzmFav?p!qQge@RQPt@v4y+i~N+3wm`Z5k%R5V~|ylna_O48r=)pqOD@ZVd= z+91-J7C#}f%o^16-3@B+qQbSrrw0{_Cd2e@x^O}pIhDY&_RTKxj@}^#l(sPR?CMOX zRJ4}h5sfjpeA66f6^tpa5Ri%fJ0@5ycBcH`P6YxMtHbh(*FV2H=^KYG-JYh=3D1#4 z_Q#%SuH#e0c<)r7nsJ(fzwDm_lKA1ru?nXuxL>cvLqX&R{A#?sxu_YLBrt(WRhAZ> zxS?Eik#{f$=-3>t7%pX>bO22Nb@h^7`Fu$zT*mPITw@4sd0~8*16Z^R>eYR99KE0; z5~0CXuQUpG>N9`GMJaF4JM5H>U-v^)?50M%Bi!$maF_hHY*$+tM3upKx0c%d@h8N+ zL7AKD2qhuoTZ@;9?nCE$izFDvMd<53i*b+%su9FEVjSA$(Y?i}l-ug#!tA%@ppGv* z4p5p~JI(SHc!OoN#D6?9=M z+!{+W%E;IBT-``u|E}R-$N#nSl1{Bmt4fnOgb%JgY9@RM*4Vm>>=(i@`(`tE+7OVt z7SkV2R5czTr8!tBXi_s$7m$fz_EFz2H`FC;o(S43r*s5Id_U5ZzGh>uFhX0if3|sT zf%|Fnu^e;wm6Rp)!!Fcs!Zau;Y^c7|GO*i5$ILQAktPh&ZNI!^r)~R@UNPxh#^T}% zJa!&7vH7u~L(JiPSRU<^7_$rK8miwOiTi+8ZvpSnoFr>)pQsWBg`#+wR||Ec5%7)( zJy>Yx{>fAX?R3h;zP`upZWvBffoh#bWiY$9zqVs50o0vsuiiDYZ;EI0+UBMUhBCb!qW-aSHDgGho8=WDZDdfjyDGDO;12;+}RigdVWf$Ph^`Kq*rG2_@g+S6j;Uk z9(B3y>O$OqVK+&+>Fxx5f}rtwpBQS$ES^EER&N;^A8)?14s zm9$$mai;biZzO$Q?=xHcVf6-8(LgpfNmBCJl5H|xfpDEv&(&w1&UR0=Y+AoHs|~pd`^t@8`w*n0-wDWHYxF13q+Qxg2dQ+ zm~A!m62Y0TkSxdLRU*ER^67V7BJ`lxw9$n9I*QPr^-KGyifoIw3U(iRKutGAD)?dT zJpxIsi3)3yPf3oVIE-YiUus2GM{+}5Lz&eNIgm$IR=HXVS4zT%*)UHYL2r}#yF_=T zwv8^(v0Z~{*gbY6`NbK6`OGV!!IT3HvddL(5-^d{gRdM2(;~hz@XR_2LHkMHAKl>~ z%MT%^$l~(axf#&I?9YjSaylM)y=`cQiYmvb^?;p&<6EDTNojl;JyfdXP0zOr=5v&u zGJriv{_r{Z%cRiRXu*|5M}-lsQtJ=cT*NH9&DB`y=zaIl4KJbV&2+((PkIDZ89Y~# z53N%cshXWdV}d6IUR_~J7-#?`?r*{{m2!)RLY~r}k}K=k&Jib*V)eojLBrX+J}iNC zLFb~1Ur_GFDW4(O7|a=^G-uxxcH$}ovIea~g{Ny=A_Y{g#nA2s1Q$L^{Ydhn5sNYL z@$vhqGGf(#%zrX=309HDlX)L~zZk8b>?>a>(}dx#JoT}gVJ9te_qD z{3?CwJPS|>@D2zC+{A4;i;w+5>k%yO606U}-GWu6Zx{4m1NEJIN@yzxl;UnE`xjyN zGnLa64+vzf$ijAeRI7NFP1W@apeAR`6lmbHoP)zQrXW1Uh4 zJHeL7cAaoL&iOV$>TtP(Z0ug@A4Jj!p!L&4N@rp`j;56v{mJ8D~d|OL32rUEW%eSa&%)eA}Ht zu*zdY!$u^2Mq#=@g)!1&GeO>!=MA>?ICRm2;YT!lXX8mEQjl*($KBhhiIH#+`V_2S z`zmQ`!M<;8Dx7QfJ4*w6^6K*Mrh0qmk}HazU0W~ORO!`V+_BtB*p4dt)k0^C*Qcabjm~IAuq4}E5h$_MTs2+m6$h!4=(Tjcy)1i z^aYlZyD=Ix(4L*Juw|Rww)1Rj=g7(zubIWh656obG(mist` zgiD30mcfR%I^oiZtf)I6uAG9%8x?O%1LQF?2r1eqjvOrJJAy&NyoiROd+3luXI}UV zqYX5fIa;O9GSR*n#Hgs!MH#)^LYYPa`ctD%Ga?*2EozyW($TAovHw zt3z2!fl1}u>bQ^j6P`*|;Cw<=v}|pj$P%5g>(?Njs7g=UoD(j9`%d3Cu)|gqnZ5yX zZwGm6cb$dGe1e0Np&pKI$ugm8674o7bUV~`5mC_>k>vNShEsPz15FB(;my0Zvrqva9h$P`nyY>ZOK(7WXfZ~LWs zEw)j`bNoiX>(+4hwBzuFkEG(Au@uo!omvp`D{!|gEFt)HWWW$2ca$HD`z2f?BKiM#`s%nQzxVxT8{J5W zq#z|N(lsR&6~Q0`27-W!f`CYl0ZOM*(kUP+DM)UBbcdvLm(+;O!G6zte}3QB{_*uv zx92(MKKHrLbzS$liW!FzRD2mA+-g)#WVFz{`W{A3<~i!yRoG0JuV3^rsn7{yAy{2k?eY*Y(anY`PMjpxai!{@~I3*#&ysbw=~ z7jn)jP{uOs6DrD7yj5@}c8pFuVXwO5+;GgWlqbKU(eXWd&Wv*|%aUCN+Ua;3j&)7{ z?SqCOdU@jh=Hw#Q?)Ur9FPo+xc1D*<6Q)P@{lZqE=YhZOlxLCR8#Ng*>Ts4*pnJ=? z@<#x(Q1rsis{{=v$1TgVKOshyVf`yt*pD1X9vu08(O_J0w+AUnWlX_QTA%xxO@BM& zf{~U=IoDq1nNfSDH2lssWsNim5`OG2^6NyB#}Iu3@61i4-=38~-Sw}3I%ZiUu?lHHRxCy1^6!cQ5cEs`2MmPtt!G%_wD4!ENXw&fDQkYK5B=jifu zOVX2)uVM_@Et*RO6{}@dD%C`5+gBfey1cbt10{m6HXjo!I+UiDE314E0R{G3nx^v3b}>=2iH zuq!*m6pr>LLVvv3mcgq-`{(6dvcDJh9uau)C}L~1`*X1|E~9dhmws&r+?ZY3kPHsD zOn0qG?lrD6SAl7K(>ne*qh)Hi)+u9YAYwibATRgULu?c4tIBhUkCP2fbOofG56_ zE>(&krXIE9>-LEEVjSm`dX^@&rIi zh>n&o9t{bp>iE9s+~#uL2M*AY(Q0r%RMl`WV&Hbx0*zNfGqTV)S+jV)_VRLNry@gl zm=U{nwpk_oNXaV zjl0^dWX?C7*F*_MIAY@F$Bm8_gg46mGK>dYqmrOftSUqO&M5gAW+Ct?Pe>VrX4{nlpog<^KTv)6r2mTl1nmn?b`*1a($W56MH$d0 zQv$pLo>_b`-poO(rN0thXLL*M)qD=H8&zlAG=G`&jodb|7{7CLxM0#gcEdsO!$*t>%z3$_o!oGSMv=GOI&nlav3 z!DpeZf?S2{(+qGe8V>9yoz>;l){}mLqk-3-488aUZ=$LOrm*r=F|F?*SK2hI;P#Cb zMt-}T8Ud1jB+VuKsm0;2+3eL=>G00m58EVM!$PH1&3lvv+5v|S3U}#+Yh2Q_PAlil z%k^3V5?@1{1u1W`!!q?s_IfII2T=XD1wyGA~mwve>(p86st+a{Tt}SB2ip zS-z=@OE@sm%AsI6Mt5e*(oF~x`1^czUK+CS(OGAgj7xcZWQzbL>@}H4UN*z&B(c~; zowj6TM~t&x|5H|=BNFH-&Gqx-jEu`pu!tUI7X?{+MqV3dF5*)rT}y#rdDKkJAi$qYaxbW2v3o4{w(pBk9j9}{dW3s4 zP0%k7fY*C^bhPGC2R}qwjaf&G;koWyRi``Lff7HdnWBU>F}u?MY6o zg(!+@QwsJw`NB2Ng72`G-B#Vb>hEwEoPHn62OKZvK?Sn5WKBJAKwqxq z0z{vRKfnei(GwHqAlZIl(LS5Lu)9u)SO2fJ^&`9M3Yiwn-OD1Z?dO1G9qG0>N9CzQ zg^(@*36~!y1BQ{p{4i_5Hs-tOvf~vvhc&snHZ@xe6^e->4hOl~ZhD|bF%9@Ws$AWK zd|AwTa`2mZuYLBsX}3=p3FoTys4sU1pa*Q}#QM}3B;1T5 zkQ1v#?Y^*_eKicX?M0Q3tB~k&FFf6E6mmqiJ)l6_N@l6G1^Fn(@>%~gJJ=`9r|NMBV(%h+~B7_p`Ofk z7*LlmPL?}iFL?h>UCd-l*Nr`CjgkGmhZnUG)tZz-ZuvSz7VS|k?!Tzi!-Veunk1NbAzhvmWbte!dWo)3}~F5}U$wa$*{%R;caGq}M{ngU~5x%R1& zZIaLen~}6B*>BE^RBDf1kzRY|ehL3kSvT|jP)M)1!2EdS0w=rSxLOW2wmYXr#{(i{ zre9`sOp(mZE>{9ncgXTnDuC3jDj%wMTDEPb`y2sg>To@U4oLp4?7F4`jIUL}>tH{?(oT?Nt|!o7Jap0T@Fe^@ z{U+m#&7r6C**6^tbI*_FB znkdL-+24mwx!iNT*vpG^mR~GOQ*{{)mW`Z|QM|x3HpZch8BBNew(gyX9+P6;(~54d z7cEo9#mUoRL;hd32YPoIxpK$t45A+IbPe^c_36^iRy;Lmnw@ucq9&icF!Q9Z-1TL_ z%r14<*0P_1;V7E$7vMAt*_Gf*8s)8L@Sn6N_nT84r5r#3B917{hxeUWF8K)bZj>od z>OV;j$!I@o5%dU4PgiN)>8CKaeqQ#9EbA_%6ABgK%UaRiH*WuB9vD>|rgVBat}b%& zIdaTyV!w9Ed$=u#_%7zLa`ngOzD}w&J?1R_LQ|T6yYDNI&(kuj`|xu^HnVxv( z3%|!vlstn^up*jPT#B1gqxW``Ly%G9ZXXbKyhQFywv1vC=R>St1wLv2?L+Y`=V!H^ zQ=8GJzX;K{14ho=#80FXhz{{Zv`R7D%C2;@>Gvbw<_0dTz)+a$nU;PAgK#W!7wZ6Y z0Ok_-P6Y$da4&~3w{QkAiL$UtSwm>GRFB?$2rh43GaHdnEj;4c>0>`@8+a@V&>oQI z-ckji5=W5p9S)HfJ9+6T03g)n4f`S{C&8na8fMWOL!Fg!2D<}rKSmk-R;mJwJ!E03 z$4Y(@X$M&rRp{@+ta=nGk6pGq&QT1ma4-Q52~=unw`L480fmo(X{>G;+kO)cj+7~I z5%zIFj6!#SqL1-FOBIM=Z!j(4#po(1iUHJa0Da^iZH6lj6FY!!=-rM_uL|4O&fgMX za;Xn{Xb%fKJ7)2bel^YK%lsRbGtFim_;){V2S_l7YN(#Ya^msc#im)Z4(3`!{@=tV2hGNYd#q86U2VumX`d|3i1$cPG>3VcUc>rY z&@JZMZ)dFC0t0QMt*$-vV&J`QAIG8kSWg{GwX?ue6)RN!;TU@KM4B_37oXe;SSw~F zE|>I1^I({G$vW(eP-=2XN-GnhQcKKPbPmbnN^lsU6ron!ei94%WqSBZT%CW8y5=ru zOJ*9BDJeu=%k-$p)CJGAd@mKjK8y|7TcWjvw+}OVUeu&$gmsAQcd_FKI45Oel<*R_ zN?`+m_n-qG8)57@)rAHlSN9n`dyl!P?FV)L0tBbarTo|Q%9P&5#6DVq#I~H&IKG^) zc*5M?OreqOFA+HS%7d;(9JfqFm5kqmtxoXgWsWOs3DZ72v#;P+l{Uk{ON>HpGdcE+ z54`e?=_6Z$+-+T9346)>a#Rz`t-y|%IpLfrGa#0?`zp^v{01Ze<+Udn4jPh%;@emB z1_b(p#?^djPB^Bi3TYX0+G81SGBbJ~2I!&r`LK{!Ze^6_I3YM#+p?S~n71e5#g%-r zTdDa(k^1SFK9RB!FCQ~XP2!>e8XC(qXO zq4x*gnECvy+iNPlNAYs+PB*Um6MxlcTYz*&aIWQe)U=kr>9;uoAx96Ok25sz2{w#G zYCEJ@zaUIozC!Bfr#Zv2L~Bc0J{gTIv8y6; zVw(NJd^$TVGt76b#e|Bo(UobMGlUcZz86YAfc_RvZpcJ6}+on zGp(w7CZ+{Lnr#e3LCP;5b`5ULWb-|@agy10PI?TzTOp+_4ds1ArqD>O6a@J-acqP3 zN*Yx8tFfsXa6OTWXR0sFboQ&E#|3u2kY2yQ-S9g$S%`H$>uNns#mCa%H$9##h?%H# z;q@<9J<5$c`yO$6Tito)NE`iF`a9_6nok@{AVd#KV9aQviMgnxfO})ltFAY#Q`~BP zCD&dXI{)m~a}VOPy{x3Z4a94jnrXmc9ep(Be~6`})nvZ|Yyz%Wr0JKjIz=W$siSfU2Y@B&rW)f~k1)5pO?n2+A+<^5n67*w`94RE1i>Q_SGtFxpl$wN0usB8h$1UVhi3niAT~1v4Ireid~cfy^C(H5tkd80!03dgF11&>An9!HXx=%PH zzqFL4u|xlBAfbY}pF+-!){((FlCEs;mPTScs~ZE)j@*D)(0-kSU9Yi|oX2;uwO=Wn5SFy)!#6reN@8$LZk! zi;CS6uPManL63EW!X{t7qiH_Z*zhdzx5r!B`y*JT{lS0|noAO6wZ)(e0A@eTJr z;`1J&{vfW_s;7C4SX}%D!f8VpK}y`Q*h_D zGQ%EXXIYbx@2V{_pQv(h%P?uyo~P)NH;eUoCK?oFmJFi)>{?!O)U44It9>^HZMkS*P z(gGnrF9|43@xr~%yf6>oDj6cRY@i#9>|eDPyXsFv%H!gJU9em2B%c=xkdc20LZZUQ zo4v8X+VUTY72&^58Hoe$aF;jub|^f_D&NjJ(&C}#z*?cuY2$s)TjhB>UO+TUgaFge zY*0nj0)8lOZQl#i74AWQfJ9)%!;tKB$+<3cM#_*vpUf_=1jq(j2ZBQ%iBEV1k2z`* zHAe3PW2m-W1-K|6WK>;cWbFrFpE$LdXbV>;!_X^+iY!liEo4>%Fq|n(T8X2Q;^={3 zR$eS*fOUdwBt%pBb$r8LGTz`%0}+MJcNQ+j^t>?PO3h>G%(x=_l{U*iF&O(n)B$dG z)~`!tOMX}6HNP4qz`%H*DDo{VdBwTgkD)hgi6WMwe&r3^HfQ%KBhzGf1EU}06qw-j zIr7_OJL7kakeH-?x?w&$uQ~i6v6exd#t-b~F%VB=NvT@9N23Zumm75m(~4YP%Qb!& z`Qm%M`^#Ye<-N@jv@6mFbReuCpas;WFf}N}zZY7(+EYvPKGUY%jyAN9sLo(@Rc;-> zgEgSO?}@A7#2P3^+hYwG3>-cl>>CIkIzb-AH;PTc1nwyfr6K0Vh6`HF1^u2u_A;?< z?4=ttRB(+v;^#qWUiCi-v-Z+TKt?F^(nkD!4ph>^T9}dn@sy8T8~vv{)kNb$*{n`_HwM?6KbQXyvO`ks%E@Pw`$wFDYwUf;w{El0BXL%M#v9l5sNVqnnV^Xm#MD@?+%qh!ZWuYN zr%S>&%p8Fe8eaGBon8nL^SSj@drOpm9L{m=)O&4)A%~==EkpC?h8sKlhy14zS>A^! z)fq{|&lWDRuq1=JlTtBCN8|WsTBrwO%EJu@o6AWfha>12Xa5?}m`VhYwj?C{j%MBV z%X$7AeeT9jODFE&IXj&XmXee&(le|-|Dh?C%Fwat@ZnLq!wnW&+(yKY2Pn7W?HVj% z2i!tOA=_8b5AgobG;*aRmGFC)Vy)W0Tat~EhG{YkDHcE@ot}CP9lz+PORQ%AZFcY? zAvaMk{gNt6B**iJwP^U^q@=TY3G$n3lg$X^>cwNx_8*YK(C8)O?^n+9lB8z5t?*t` z29}1WA5LFXoi0CyyYe6J%xccaUgO>0YvU?mnY$G)nMCbs-0jNvD1XFv2b`wmAm8yN zh{%q&LS}u4RNmZq4PQ?+vvR_Ix89tJYNl7%#`CiXhv~)aia{gnW~1iFxBW-YTj!R= zoz4Yy10)*1zD`tOuMh zSsL6K8bWWu@fenVAf*%z(C2LFSIc^1*GjQN1|dR87OoOZm%h&&R*9TSoc?}^H#-CW zX}^`~j60W3W~NDYi4&x&54*;`m;PFKZFTX26Tl$NsKVcaiHc&-IWO$0s3>$^@`Ojjmsl z=xh2oOlZNOYQo;&Pm(?{0cNbNIoK~ccerzJ(r+&_hSn9xa^98J23!ZIgY}bQ{~_tS7;)#K%`l=jFYyNPwST~`%eX?0dDxx1@uYDeD!e%s7 z+tW^VAG%>1?VtgA4tX_v_j6UhbUJi^jUXh=6d;T@<(+-}jF4dK&(tn<*idg82b$Nv zZDoHXAyK=QV*@rBtF`O<_Qevf3=Fh!)X2SMIi{-O_uYi54?ZN#Te#mWFqqAea&MwV!u*LF~WE#|1wU8ulx_CQ^xaeHO-(Y!&D)QNUBr$rL%9? z2=HTIY4j`t_4D#6`qQTo%#9^|?g)Rl&5(vK*e!08;k80dvSK!U)yk%7e*ikRM)FH4 zPB$L(sqUR{6NzB#|DL~p28OiF`y0a4pv5oZJZRU|0%oEM`XWuUYK#M^{%4@CBMo@5 zfUPg%oMe}0NyAb2Hp$$6zFQg{xl;qCjTW%OmWkAj`VRCZlF)El`REY*%LL)~MPq+@ zAX}>ibW>y7WHRgVDM)$t*)+!2ozD+z)GkLd{N>g>C7Fn}dwcZCk409WO;(I#BXE0z zZLJ&#?xrR1OXLZ=EsU~(-r$fiPC29f+UrJ5ZKIMYV;tFFvr`wBK$o0*V?m`kh+~d{ zr~&C^BPVITt3#S4S}Fe*eKTj4uYWzT0k1I#!+*Eh>q{+|NtU&R;$x|l;o;!B)~*z* zZ_Bsr?GG8A7Wc{z_5WIPh|xe9<`Zi*`HGf*7b8Ba8GkIe^JXat&VXXqIGG0|eUV^m zzaCog!CY7pvS_t;Dg$rg#G#FLb!R^H7Czm53i${&n)~ZbD}rtM+Nh6==L6r&)F0*{ zy7mIrMDX&q)5+$@Vk|L*Mk)8aQLtRUn%lVNVpI!glE?8w_I0;pb!3+4+`9YlZH}7v z#98pK=3})Os3RoG_1!N)td!ZIz%8%S!?DFXGoOpUU4V>%x9Ce;w%lhS8N|^kPP^YL zSPgsYx!Xk9?tMlw!Xpy|b6e^U)gObpT&<0Re83#oqRasr>Uo5Qr}G$dn_}j^kD80G z=;g%FO8v0*Tuv})1{?YL(x>NJLfYS`9gf_Sbw0P4O|0m-Vh}lF@n-t&(hJB{-w7z0%hC1Z5(a=7@nP<||!jJQXwe6~+sVmxO4VqlDKpCp){o$4ezcP-i!jZk6C2#68a1V$Z(V+C6-IMVA{7b@4^?L`cX36LhXq5OyP}>nZvi_ZfCoAmrOh`37w!n6KNtBXA4gBRT#hi%{n~zgx3MS*KZYTf!!u z&gR4r_mj|XL0h%j;fi|O)$})iJdc98UQ%&6m0l~@V-C3O&QBpC&a|58k2Lk&Yqhmf zrI}M=9>s>dsfvK-DUgw10#}McFlm!rHdjXDUX^lIZIyQi-&U9)3DAQbrw+#uni-MS za$PiBR-_9jext4@dU=4&=@BBz(-AFd95D^Vl11;da5qy8E zm);xb@;p5p7qu0Xd30-a>cGUx>&cdCzgvU?7t*QLtVsT$!kYd^Qp>wwW9Ensn9ZEk z8k#?)vMHRCuG=c#1uu9J37pYs?UpXulo#L|*`~2JA$F~c50et@lgMi)dskE$?wN`&(v=}MgCnO zbOp z5Nvu8%E3>LVaxjQ5K?#s_RX?`mzrZ-elqL1qZ(2C51-uz`Vp?3Ld>NrUKplHz~Upg z11&CUQ@%g4ZJ}4B11_;n=1R>mGPCB@@cU8teQI!mkWt>MT~2)TnCvP*$Ipbh>UT*X z#sQLkf~aEhqW@vr&+9R6!-H}6@C9Z8c-{ekf>>m;LYPpC+V8*{a<ZNS;BAfVrka{fpjs{Cv9kM1OsT-{9{kY4rVQ* z99ly+LL(yjBiTKe(>^ryF)(Z(QKlV#=>6h1=o4B-Z6NvcdfM?oNtkeo=u3ukY(2u~ zRg&)U=20$?k?++1{ZqGipl$i3e!uG#OXTfq_>gEy3Hu)Hv{&Up4*j6(JXK}x=2KXM z!PJqOLf}0awV1PtQ2C64rS`7i3$<|$h26Tm*3f8WE|;|-o0i~r4<4EcX)sQYmj=yL zBJS4{-q%{Gb~;P?#OGJ9ncSH@rT~kR#rH9;goS)W0&O%QG-W;0Rp5Qu-bEKORfuPc zuJ}=YF}<4fsd0BciD12e1J6kt-YK(vK-=*~L@@xXIo%ll&ENXP*UhW0U`v zq761_bRKv}Dh?)srgoZw5j$ADN1Ljl4orYs8vP(+g z^fK2gxm5jFb5e<*ho`P}s}#-BU&UEpIb8^JZY9&ffeCn9KI6Tq!=J43@Pv<_p4k^w z!%7x{jMISezrD(72K)%64^_ByJ(Wt;Rg0xb@4km%|F?=hmH9zEzU`}xJ7}WDCnEo2 z-*B-{R3o7E>a>bLYQ^crNltjmtDDxv%;O{Bqf=I@z0NnYauTrJ(TnpTpa+0-gL~v} z9mNSvM9gLgXRXxP9aIwaHX3#3k1l<%G6v>c%* z&{t9iYd6GC1^w-or%Xdob?`zYo>S=Wl}nV~OuM7(OX6QEjmgkqwVz_U)fm9=i~djCfShfv zivxhJU_L%sjN!OUMjH6BJNw;W&+Mif<;po3;_|xlrBsLM#cNM!|bW(ERtaym{{PJ8R3#I4#+x`&h zu(vRYxpA)cdWbJ17RPJN0VgjFD1_&1Dh1z$r14+w&v}E37OnH{cdMVwd9Q5TdjMw9 zNA0&>|(zF*Eiw_Ov3;thExP5EQyQ&!Z=J zn){9NZg+H0Jc|>Y+3c2f2^&772NrxG^pTVK^&a_tU2c?ID`phjvDG%9!zPD#Jv>7W z5|SpiCXkbSqx;%3L2;m08?i{28{t&HW}?twT3P|2^z0Twr2*H*%y^MG-@@jEHzr(# zVNSWlGho>=Zgqy+`k{GtvPkuF>OjXfUn8y;8`JU95y(OmXA#eHpfphK9=TUk`DSpp z%}O{41!u1jvp$Ex)mDhK*?8}I>7f#~{YT4zg3{?<_YKSBwbgAmcZxNO$sx|Wj= zyjA0mBV49mkG1;lm<3T3T9nX7>e`89)d z^!fmv;JUj?9JC_vMH})#MB|M;du&O;a0%OKv1?OY(n$t%?J?IlD>>7GNC7|f-7@QQ z$aX7^gb|OXsf;D9qul8&J#lQN$&A{eY5U>_=oL3b>e{#NIctnVw!5A9EOi~?ul?0u z=)U;9-2inVd>GOSdBd_Fbj$7;~5{AURhW)OuOv*C0U~$pcvH)?k3JkpyjxV z)T$ip<|LKCN9X#zw&a_O*&)0%-4YeI+I50PGt~S4T#5Kwc^7xDA zVjW)iF2xCpfMpz0SXZ~MKy&iaBfq6My#*r>By^eoQKNIiVE)E@{zJQ&qk5W{BV#BV zh)Ikw_eBSF>p+}3dlMzK39k5-A)2^VUcb-`=vATpfL>|9W?#@jT3P6EO2TmGgg?V;dS8K$|qD9TsMo;B8HFv$QoD>SVMdKDO|tmnCaC14x9n(BNU z4_}{rYETi1|Pfm8+n>(YsP5mn^LRLlFfz|4rAy_xTqmFKq8E?OI;s7(M%Xfg$7rc^W+FC?t z!xB4o?=bU5B7V%QU0v3^EOTF&r5@b(KB(G zY!0kINLA+)jsDLc9UpK0lp7qxIdD>9)cP~r`jlV!0wWWZJiuLUu8^17!7O|FmjwCM z=jfTo9|DCyd6ueCFe?Tde~{{ZE$=Kr&T;#?<}lYGtC660M@vC(EF%}Cn5aI-&y@>V zj#XLD2N*5#LT0XOgppAs>mp2TS|WNud!>zgi3lXFFsTFPK%Nf29Pw>(+)3w%j%k*q(5EER3o+032llY1f3hM{R1@c`31+|7P=FGecnn~ z=zbqakN(ixXr5ovEqxRee;2rwCb9J9kB#+WlBn(|l<<>*BcbsQ`twPbfx~*z0w?pS z%J92;;Xqf1`gvHn;Q&R!rap`k)3fv2K>(kyC_ZS2?fy3V77^9UShSoAmdQ_1a_}zo z*1R!{1bAFUr_AaS zETs`%n_f85vFNFoJb;Ag0h$N-%z^9rA^z9yWc=c=^1P^9>{6K^fXA(y(~6ZnEK%4Qj_=v@i-wh zTcWLP483~JcV+79&zSENm}c*c3jofJ7WM)Efb#X#eTE{en>>C3d+lp$it;E(lrQC8 zz!mQ6KS645i=xx6l2u-r>q0&jhh#&(o`j_@<{l3Es}iC>SkHvfYvTs`elAE+-rexF z?3L!yuDbEoxt9{9ES^hoCFsfhY0cF1k)6Y;F}BkY^xkGs@d?_a()l~ZHVLF3EV*!M z0aiAw7*3f$-z)$W7<6YpYt(vDlT!gt{0B&19smi5r%15Iuu@SBLMkxa(QuK?%y*xA zOYR+K+r5-7qB1T(5thgQ_|3`gybZXGMtCJd?%*;aAf0S_f4l_TJ2f*KWCl>Y8?HBV z3!^S-1^;v?`LPi0*|kvOfBKy4=?$FCyF!OF=Wea`V{MpQPT&V&)XYPdu1?MVBDQWl z`eYJ7*{=Uwd}$Nm+;F(?t|W~BI2G7sws$%V_8tJkYK)d!<-Z7~UcA=n_xOO0G|%jq z-)ul{1#YLvVB`cumd@WhH;B$zAc4;*P5zbZVcH-@MYPkrh4wrl$LSZfxBi3VN#{I# zS0MKUKc3U4{VDI3F#3RnKZQ}mWZVLuCR0 zAeHCWku*Yf07`YO3}w$-ROw&^nB!4!vmmi|BJYw?b{e)pP(myq+#cIsf$D@x<-Gxx z{BX-j`NXp+0)4ePt=rI3CS0V^cY`N&&tvL+m%onv-%gxnJI=8ViW!e}@ZYZ^i!EHR z4=^?C*5NxZ;pZE@+0QR>Vq`SZE|g~yUKYeI&32=Y@mxilhpeZ!Y7L;OC#4^4HF4>6 z_m#*)wo0>Y`B3(Oswri0^Z9Ou*$By5v(pbm5nQ1N&o*8VQInPzUfdP!ts1EQP*{m9^i(e&&o~QI03#%0h2i z!;@!XBB|y47m8F)QZorL`i_5cA$J$;nEVeu_y{W5JQI>fcujpc6)j-YMiU2Pe3X^q z^lBA7`S?H1Do?k~{w6|qDwu6GU%0J+nDMsznBJSySJJaSn@^EKd@G7xgjb}56h{2% ze>V%vNn$8DJ<|;~YHej*j(-eEoy7PgwM!KXF9@0Y&MnmcQTgFYRV%$Z&OhW{f3grU z4_@RO7O`vIT5Nc4Y?$ETkDR(q{N5cXNIH9hxe{c$_647D5c8km$XzJh7tdfc=qXB< zXsqS9nufO42Mt5$i*df&@`r<63i8^!VwZ^r&w)nG>tx}5(JPtf%D-q#zp^S`Y= z`9aDK1!tXE*8)c3UVUBC1dr z=idxzHV64S*}fHh;yIu(J$-(Vpx}(G%VoTTUS81x4{6}VWZsAW7&Qk-U#Iiz;bq+t zxaU8zQ@mh~D;ojETQ_ACwx3oTq$%cGdfF63ud^V-%Z(_lJ%l{L zWH}Ht36w`wP%;UiU$6sY@f1``f2fgA_6shK7`eFvkmb0biF%v&rvrJCf!^lj)JgV8 zoS(7xUzfI)^=<0k3aNaubUoN`3pRTmZHH^i*?tiOJ6i|gT*`oY_r1{%r-f}u&sD+K z>b)f^)kegt%POBOPrx0|5LE$yZ_w(kiWCp2xe%f(ba(T$Q;IBFtbH)@taHvIGqV9) z;{~{cmftTKD*iuHN~UzojZxO*_`b!S6bBQ4UWHzU8Xu?{jhO8+!-q8TDxtnBW>U{m z?cU!|(a`>~uc(R3k~^WkOAmB9L1XB>%BaBnIH_7Y;rVI zvulm$QK=)aWc%L>LDK}u>!U-ftpsVnR{~$fD_|02vfm=X4wEkhX9;c=I+`RR&IDh= zjrjf^`E{TlTo&MKX<#O3Z#is2T%iw1Ocb*PMaNa^Va#oqdXWQMlqiRa|J?xeA2n4k zsA8TXC&7CHMffmxko(-4VO3R(gFgs;h8lT3$u_~93v#|E(V3_(M+XI&$Z-GRhYf)< zfMWqYav$PSomts33p9=jYvOjloG!i5K-E~pQpR@g9L;+nR|^8#2O=F#KA!IS!*;VN z(GE>#gjqt2_rUqN^Qams0*Cd3R2F>7>29>*Yjs@zGJzW$g&-(MIWN{9O+L3RSG>S+ z8>0LE{q*-Q51c=JwEz4({?O0}&dNj{F1iaMX`@27z-m9Yq)_Xs-m}xBXCyY7)Pz&) z-@0`fpO71|Hs-S)LKHbwJR*4ElwX4+KN7qdym_+(&AXEF{F}^bn%ih))7OoEwBW&i zNc8q^O}IJ67&9zU+Op-GY;gduwHc#3#`m?9uLw6o0yp+A;tDsP)j>T# zdXJ=)cu95l!?n#G87tVw+6=mH;OC`RP^!69!}blN5c)l$)43Q@e3^rrq!L;6!aSG0 zHzxkvpIm8^%r)+CfB&i3+4CyyBL98E<;*Iyzj;1U1Y-u~lvNcK9X*XY#?o$^xE6ts z{8JwG9jN$g^>q&LleCY|?;;N&C7xc2u6mD8Urw1o36B}d%~k}SZSqURvmVd?x}hAO zKjCc*?)r4(|0nE_z$hETDXP;T=T5cJx=Z>D2xTPmzdzo_*CZoAP?ZW)Tnst!w~DVm z+5Yzv(6r$auHyHz_M6$|c%9P^$fE0L5U8OB|6LllV|xsNIq^UcRAH1Tl4<|%+dvW0(d2Z3 zyZZO+<0_FL;h<42x|F2Mdt)EK@4CeII`R>dH9sb6QZRuun6xfu&X=y46p=5XJN)B_ z0>oC)tV1JZKYl#hup!DVSSoyXv*(gkQ(TMjooiv>19EF9`?{WdD(NP(@`nFX7(t_3w?0$|U- zk=Og~d=&x53QK`2Z0%>cC3Ee(y0?4#C2!o!BSUZ$*LwHkGSx|C%?qqeCILXy$`bp8;X(QHNkdwJ45>is%l4M>2R!@YR$ZQm_I(bEcsKb>A zO%vkl-F36`f8w9oWU^1}+Is-X^*vD7 zaTnaI%^R$rQ~ottj_XouuY#S{<2ArQM2~K0=8wb@Eq(@ADJK!C9wHB%kNZ-qGgeFv zrnm^hmrpezebBVmPp!AF8W`LB(&5_ zzzrYli~kq?Smdh*`%_NpmxzsNe8Tt#T3``Q`o59(FtID=ms#Kadwo{O=tmB4a2PxF zwk`R=AIN1U32ZKsmO@29#%F9MX^oJgWh|`S2$Qn=oj0cBU;0RdV?6_QRbq=apN4$> z-E0ZIC2NyeKhLkg+q4Rj=DXqjglENGFQw3b znGT$AJZTw9V7&0Pw_+#1c~3p(_%EQrs>IVefSjEQ+;=o^OQlT|TtA6#*f=e8k&07J6$2m_;sWj}Z?O3feg0c#pd74UnHf3oQRNBN zjnnI(aqN-d8DeqM?Z2U=b%|!(y=?B>;9=A;v-jy6$f3U+AalI>=9*MUe~Ac~1ZVBX zAC#o;?St~ENro&SXa%4!!{8E3tf(~!yIRTqXE*~`A1F=HOSX+xCM}OTvQH69TBwSx zN0Kk*e$|1+>FL%B>DTIqhGz!Oy9nch22c+PCYS$0>;o*y@3wWr;%!g~z-mQmCSo~V zy4#ukxnk-Q_xpE0oyub5lw4YQRM1uWDJtzolqp$W2s%Uet=>L+ykQr9o zdElpi`cu&}@pcn%EjobH9f#c82pTzD;)@^F0%1aSZb98hsvrvb@VM;n;5N*Su4Fj{ zw;^_g3A+!Zx+3_7Ncz=tE8w&I)8zaBMSd-$voM3k$(###hzyZ#JQ3&t@ibYF|KC#yi9&kHPL)i{3|Q3=Ul+a@7m38Tv8;${b;r#%7q^<(zBe$|DV!=%HNnOt(?rJd^UULc zbiX{P7vnfx;BnsY25SL7654;4|GS0v6btb|i~m2FAMMJ0s8RGFSWz0H%zIzgO`*Ey z5;m$z zUj;k-Kut_AiOF^X!$Fp(U9$T3xxX^omBYc=S|PzrL6q%Ot3rvU0umq(GY^Ud89+M+ z%kgK$UzZRf(c$P69`9ExM@(_^Z2c_QBOPw&uC*fN{>K&1tY@$^!3SlL8bu4!0rk$Xlw-> zm6RlkbC8C$oQSqidz$`N&4-|*C_IS*3C0t%eihXdsSCfKBv}ej-6rv1g7DinqZ~5* z6k;C-bv%c^>;i|884dFlcEy91JoJW|r{ES9t9K#RJc$AiICMJiB+P!3nsws%n>hK$ zhqUl!tG%__ax?!|9(m|lc(Z*4RoCULuFiKmNOB$0<`(PAdVuoVd{y|5i|;I1s#ff9 zPM`%{>n&kk4La!GU>MsB-&Uk2iODRq06Mp)v&IIJY$Rn#vT=|K$T-@jc?e`e0y<~Y zdgpJ2M;`w_&fYs5>-YZ~zb==(N4CO6glrjQkH|=<6tc^x5DhCX*(-#MtVA27BqfCG z>{+t+$d++&-Om^G`F!vDK92kN9lyh0@Ao0RUe|e^&&TsIp68&y3hGdN`(-eKQJk26 zZ$kD;U;4XDK%E0>GRe|gh|6zd7?=&qum$2?fy$HwHm{15Dd#QIvje^>-^t~fZ2VWg z%iTH48EIPYe@MW;Nkv{B{e_LiM(}6pR2&;;n&2k^E9MA zjV&+s%QW5fBWxsQx%)usH%Hydvf8V8vV~0OXKaO|UCTBFPI6#`W597|6Dj(Eyr68$ z`Qy+%Dk7sQO@Vlg5|Ik8?u!o5PBMt$?$#U1mNr`JMGKGb%pI50yk}n6jD!o`XJ{yW zMeasgF>s^y;ECO&Nu>-vhj+Q7Lj>6VxAn()D@v{^Uq40=QonQU{`w4!E3~Y5lg(%I z3z_HlQQIC;%)#|AKwpbsOLpJG|2eZ-Lte_B1?CCf0Y}}e!IszO`43b(+kax5M4p1b z&y#}tGFX11Dwd#&VK;H6=2ljmIQ5&-O<-^tTu?vbj7J0Ml*!@6n|uWGl->4z<2m)ptni)?{) zsQ*}IL%H~*HUcoawdd)kyyv}Upc)wO<75x_tTqvO%Ya2BIS=+~RoVb9q9-^o^#cqH z_luq~fqrA(53;|sES09ytz3P6y{H}cvv{vf84b6)h!T|^P&Lm^Gz3rDBACsB!DuUf z?{iK8k9x^@-Du?gLkl3qYgl1OZJ*b8C@BTma~LoSZVK!Ho2>~F%)P6n~+X!z|)TB zj!a2r$&?#>4;Y?5O>|^b_^*92`PF8u$z70k)$i5Y^8fZZ|6PcqMMsEUOW!CBF(fr0 z&11mTBXS&^9+?}4oF{)sq?U;>?LnCSW3fGt<~Z+?jExs6V`K6Z!MNJmD`)IJ^4- z-*JBX*?%4!F@+D^N+?}J>bx_lRX8Bp#63v({_&3#)~!_E{l~V%d03J1lgNwQ`xr*p z{@d~Ai8pS(g$hmfuPr)FE8|TQc_8CJ2I|!O;s@{0o>v9?DmbEqUoAqIu{}(gL1z5( zoU6~}`GS31*+C|J?r{0{ZjH-pky~)02O}psr^8rXoZUy&$CgXNIIvE4JU4nWLP9q) zNHD6<0})g)i+6WcA9yQ;GHAk7V7Y?T*OVJPMue=iL(Z&QZ})7|Y%-2#*bw8isX0JQ zE6wut>_K8Cefkbp8ILNR-2i-Gr+3v)%g?=X3!WxDOgjzJKL5|=_{Cg|`LZWOwoUx$>hJ z;8t$u5ej(`GFydU>|VQ&`hT@$+nm_(#8cB=N@ zV|OmkYxCWfFz-1*>x$Ue5a#0a@48$Ta#x=hvtA^!Yd&AVnleuP4mQzR$U(;fWzYln zdwJ6JpIXX$#K3P(+zM1nR$1s1+O3lF~c*7-= z8C3E5Ql~Cl%`2(|mnf4!s$;erSnfmM!Z|?%WDSG&n=VFa{Q}YfR3Ucc{7*A47XiM6 zz)&i!j$@+WzZSU;D(RXUq+d#Rul zdI$(3{p~BDYkbQl1z#^{0*aLizW%K;s>V3`ZOnrgU}935PICUcZ0=&*ETQ45gw2k9 z{u$6#2>hKYdyZ+qU=$_>$Ic-D8>{LIw$LCjL4=$$y>x62MDe%H@D1#;zixI&*w@9n z)$xV~!7EU| zyWkGe;$a`+;Ms=nr9nXbTnj`w4hBqtf%4lQ1P%v#CBk!kxM9Zaba$+161W4E=QS!i zO2D_tly^1#cfXLxYg2l{gJ)2mJVsh|f*U$zzhmX!TOt3!3~RdDe_T#+e^s2ot-u+6 z2z?o4mmTpe**0RYRYAPP_u7y|OO)fu!G*WAhlb%lC=gUkj57s4@ge|9E$h_foj-%2 zkkL`p2h_%J;Hg-7)n~-?OPrQu=KR8~{gmLhby`T=^wPF9Cjcyzdkx*qFtkV!s%!8& zxDNraV;j~dpfYMaGO$_K&CL|{!C8~1_eT{1TW8rqrBpe{8a-sbfIpM~V+dyaM$HrWwE z05dN}X_i41<2$1t5e?M9+rUdYPi`Qs1tcaUeBonT^2yE(zaWsxQQ`F@S~kMz_#1Ad z{CNV3v(=?*0A#`V@Y3zRjq(`fts}1s8-#}e2Hod57T4)3kZer^mxU5%)a^FF*Tdp+ zOMf7%0V%UmBJrM;A6E;arw7aZFTL1=)@zhd^6|0dsY@|;?m}!pZHM#GjOFo2zi<8f zY&1d@&I{=(dq4C=xb9(ig>)X@kD2P5o_Xat`5iiYCk5rH`c-r2VJeaS z>O;SKU-KNL9a;;ZLuWE?%_Z1#V<=Zods1?OO;vwl=nzcsKs8G^HoP zwAX8I|Dc;rnt^S(n~6=Mn&^VjBu^S_x>9i2R&NUz|8dxR^}lra;itbfasGmy@=Kr; z_$-$B9uWF7cBPGK5$GnrHTA?UNZP30d*&e6PP8@`VDDOEZoJ5H8>BFX> zv-_GpRGuVHB6WeQN*NB2CXjYEpOlYh@@`T3GZ*GYdPcx3^+B}T(@SFbN09Xz-HNW8 zKx0UQ5WI+rn2a=b?(A-RFu0yz#hD2F*(2{?qt}6B+1}hs7gD*scsG#00TQ799OWo!;aX?ADuU9?jv?BBueKwtfkj@ zVillkR=$~V7lAdB3ULC4zZui4smT-!`2kikY$5deZcp+;C7!3DNvVBG#KbPR{b>#gwiefogbHZ^zQrXC@QB-FdqxZoL#SU+xpx z@cnT_mKrm8ljl7DmPUG+z=eOM5h+BwR`&<@#nyPg-(`NJZW9m6z_W2g5p1PP%jNG{ zluw+LGPynAHR;xM&h*<&PGBYUh_ld7p|6`xX5pigR{=OHsYIrb=?{0L1ta}B5J>_j z^${mLk_@_mX|}jfw;ldq7anjlH*n0?64pEw8Pwn(g+j==fMi?^w@S9R`Z5yW5Fn9v z@Khz@4OryJf*0Ou-%3nQLGOHwCt;6U1yBgE(0>PHM?OP?%|G+ggBbbvP%mKC=03{2 z`6YWmr%_GJag*$aM8on2{400Ac!a5=FPOjc^dZ7pY00xTU7ev=L)sQppnHTl(2v9) z->VEgFk3)kVsU$4PRFd_x^)mXdtOlf`07XQmQO3Oh)6ZdR{NZHyEEZ1a&YyCZT*!Z zC&ykK*h;PNu6+a<{3E5EH@Uh$Q5;(9LEs)R-U`)v ztR)8uEl(k3prP6I-R9Yy4v;?R@>I;e>^}d8bUY5G3x>XwzA?6#=#j9Opyb)X?u>Mi zL2dYIewwa`3UOsa>&OYP`;9Atq*yP~7o@weZV&JAKbd$|^!hk@`#%L1;SOUp&NSm9 z{j#n2-1$RGxXPi|v*JVSOif6p6j>n6E2_?rq&Dq_cYv^+3Ok?P3U6Nja7g%-Rj0~y zk}$H_2j#(7$R%xEf%_y;Vv5em*M9cV=)w-Ck9X-!mYoLix^Mp<$vH|48SU#57J6W( zS=$P}OvwS271Z`b`wq44%1_G`bCMxJ0_;uaFT)VG#j_&l`zq^SQR+>cWz98KB%6H; zoH}WN0{?^ULTcu%KQl!(VuY8qvy;Gjd)=lV!d85nbM1j#|WXtWZlJy64{GS?g82vdGXVP|EO7(jz07H z7q4>^NgbioE;t26p2;0bU1w zlINP*jJFwm;vQ9uZZ0~gI_)q$Y36PE7tkBlc<-d$1|DFKW3MOd3D*A@dM+WJ!1K*0 z&<*ys5m3wj`!-ruqR$dhl|y(}k*g}mrULcO@7IMy{KRDQwxuhqi2qyKQy2Ru?NPal z)_nm@QAwd`d^Hy75lB@__f|u0r=x7_NKxSaYEVCiQs0Z@z4nzcZ|+jrH2w0mTvs#+ z-u1FkEN3-4-? zhVYKy&YrBbGV@;&{qH_q1UaJRrwY=?yznpNUYn~$y5y5FhkmK?Lyx_$ZsLzkPrBog zo)xhKfjo$A@uJBjK2P*BEMRpn;kDNohsRX=lP@w2O?Ev)V%s^HK7mL&KW>s7`6xQQ zdwyD8+<~5N_gq}?s|dpK!aDr&g4e#!{gI~Zs!6ja{uP{UdtLHuosUB3ML5_?5eL;7 zOW4nP-%SVB0XU;i0gmfIqTxl6`14*lQZ0FiP`r1>o%c&L*4Xv9sA{lfxV85VL|zeI z_MVCLm9bL(+qs>mowV)e^Aic~@eNM;5$pZ415ddaNQ9W^k;h;nbV-&KmIqfa&F~f= zrmOuvndWTH!x*!93b*bp=n?Ug-iM+VF~ekt7;0(wFZhoIeK_kJr8v?LdLbTIn3$k~ ze(*h3PdqaM4zQ8(b+T$`ATsRGq(s!j@_NdtHe@E7rmPljYVu%6sDfYtX9ghLMI@vS zUCVwOhfqyORvAH4qb-P<05?}N+)xL?2)|X67eF%zkYAk3AHQS+CMGJ@OE-;PL>q5D z8^@pC+aI3zq`x%-#2zI05`9Dwaj_>g48ME(5}bfvlA2Qo-B)6|kn zv(?4)zvEd(j^pWaci$hsS4B97@z839BA2OCA3QD-#V##?hbvlX0O2?h(;^bs8;Fbt z#i;Dc>jTksE8y$zMI|Y^#SKK^_;Yu)O*8)}2Cv>J3H?%x~OsE3gXv9K7aPcb<)lquT3v7F~e4&Y^K{{aKG#Z@@ zV23M_xNpFf288mJg@6s%4&Rp}nN7IMx0v1yp}(tyo~1Efd361QL_L3eb$nIS-8L;& z8U&q=5r9eLLx}FTF)z?J41IpZJsD{BSGuGOehjXOag&0by!}$%ceyq7?cyqBkZA>y zje0-iNIK}Vd3Jy8+?gJal^x_=n|al_hRizg-#bC?9K1M{$XBm@Y`JZY+ySSiTC9D@ zi=TE1WJOve4$LzT+KM@co8NS9QY9pVSkc-IFoJ=dl!oH~K=_Yu*1FAW7f|^1L4U%V zYDT@FW8D|3NH%`K+Z#p_%*hZsKOvCaePJCaQgZoj5zDea-6A5~a{X=^h-P)-)7;&H z;sF4IoaZfWeEV492D(m=-_91a?$F}W%;tY~@tk?_%wpC+gC~cT# zX3j_gxU>}cto`zonKC|e%d$qwMn@MiH23Lr9v#vzM=bt}KxdQK zzq=N+g6MT9UmBe<_)A9?kf0!3&Fa_o6^H6*-GDI%w~}yUAiS_l0CHzbL2~nV-p7%p zZIUezAb5Q6$?e(144HB|xKk ze2DtvU7!+fDwO|vTfT;xL~`C!lhD893k)gRR)?|u+JWnY^M!V1DXsssrrh_MP3#i) z@*G0ll#R$=GK?npx^9OMbl>52*0k@ z{nTxtYoV*<-PBKTRJQByb-t9y-Q`tq^j85-#0GNm!-%D9M=)m5 ze|IIbw+EROG6x|q8zh-;$NzF0>55@^$tgtU9Fc}RlIuq#!VH=P+m)2d4>$acC1ZCn zLq3S~ip24I5jD;%yvNc<6c3^8sFsyE#C93RM!Hc3eeelmAJi&imlDXl-x9E6+#!~@ zC#%!29-~@fAKE@kzfGB=`q&@2AO6Cswb*mROfS}z+Xz(#`trBux3CfKR@BQq$=;uU z%~1uH+n=Qijak`-kr>s=u@5TSv*;O+1b?l=7u*V$M-zh|95*^o>IE|un@3^fK+lrj z2I-mPRpBUtxc{c!h0BTOMhNVJFC_CJ;Qo<9J>Su|OmVRS^EMAw?JpX+eu+}hZsKZk z&eb=#8U;x&{DkAi)(0OXcFj2rj`k&txRjsccLB16AhEhC$YW2|nVxsdv}i|2t_>jd zMhW3;;=+2OMQ77Qw)E=*35(8gzBh|3MPFGh-<@~bGTgZuqi3?G5VAV*6j=Rn)wpI6me=3=5a(W1C=Gsg}^PwYvycnP}ADpo6B zhVzg{CqIgbRo63?lh_5pPWC=4z57r_z%UH|Fuwq01v^9??Zv7p)b<^fshMJa5tJvp z%qhE!Lm>Uz>Wf!CMYUjr;b0j*Hc5hF=3zmVve2il>D7Iw{@BcxJP5p>((*km5g+s< zkgzA7E@W9i*MFk}W=wjIPm*Tv$oKxDvJF@^@t)WoQSOy6!LEhIk6!@e~>3>BRnDCCU6_q=ojTYP?>Mk&;<`z zITjz4Uu7;BSnM8E+B*S|L|-GK@9()LsC7#%PZPT9Iz-X4dCjsw9#J z#(_sJ|3F>i!*-zTaW>g<=$QEtCDn7PautTzftx!p-X_}m`iDj8Ex3N8U;)J(-b5@x z;>Pcs{M;I|jIu|+30JfP0Sp_aO?-qzf_ae^m!y0hTro>`aAqP!HSS+YSgW6R`PM)E zlYAxBsS*k?Ypq(jkf!M*-y8HZblbGbkYtuzT$d&jCe|eE-WNE7?3Ay)bHyY29$#{k z&vT6gvG-XL%>V{eT1|Ired$RI0uDvZL)b99!a3tm4j=l0;5YKMsXK(-ozYU3?^HXe zn}=ktU5L7wZyK>NL+8Jt;mbXw5wTJ3FEK=&K(=(8ccZGhqjWU9k>Apf;Nhc!yyn|I4bdm^GxPjdn$$W6s8*W zfZDiYKd;&7qXw>>Pu{J%9Tx#sQ@4~U--(FID`cfw35t(cigfRc7Z3D9B;x7)q-I5- ztz(&fdr}!$pXWNNEgj`rN7h!DW0-aN11gJXgwaNKOsE1|A>!qIN?h$_m>JIF z!$E9}=!4{Slw=7#y=ux$#vHLXCB$By%`3-Y9 zl9!2E73t*7RbF6p6qc%)Y7@vb1u$YnSuE&g)H<#&r&FRLB?0S8mrO0u@;PkdSj8qV z^5Hz&kOSzWa1(HTsAFHzEc7%vn@MRIC(Xw|FjrOx%QY*41^*}R%RqbJZUhQM>*ud|zdH%$X3t)v9e zoO`^katbe3*0S`JDpI0ZJV|3SX<8G{n+Xw(ocx2W=*#XLHpp&0wWb!N9K89hMsmUY z+FLC_<97kLv@YSM>mNMV>_5+zD}F(o>P$O7S`bsoJ%#IYU{(f#Bqh2?K-;AbH1o6+yL*4Ja{$pZ?j^A|DH!-Rf-%Q{jY}BgQ-W0EReaSf~H$1l!m2-GG**30bjJ>&q z%1QV<5S;RNkikQb`83OQ{3`{Bjp}f3P`w9g>V2A32>6X+xm?oKd%N8#6)uhyq$sB& zVWd~y1(a!s;4Ire(7&beq!w5Id`)r--7j^1B3Q>(R{S)?Snj{Pc${_I?L!1=FsE!N zyAfc>@sswLGv>UxqzuhDFxOE{mp*%!o$L`QOukDu*zOJId&Jqi+;G}u5LpI8V z4=oxrbBX56s`JMcn;Wm+K|5^e@=H#O9XV8y;&{bars7VkBFT`xrE`0Ns$9G1nZl%s zakK*_AzWtAKHlIsdlyWDJd?Rza!pc29QSR^cOt!s_0|fBb+Y3v%DA$oMhF)$5l)lH zdkpi%t!gqerBmzZzgIj=CBGuav#{JK&b?ycpvU4SkW2bPtoH&+Lg0@K%58w|6`6d; zAx@zj58hgtwk1skb0MO%`e6fF_W=WY<+e88<%N08u_ji_FB5q^zqiB<^^lqY4oVjT zB$Wk*P!f;S{Zx!#K;B7fla@!tNJ7Ju_M8%HdvVolVllXP5EnM|+ecc35vqnysyuc0d?&+gg}};Ba&1t z0V8jcl=k7FSl}lMa-C|?q;E!HV~06XI}ebi5_x0J$8#rsF@`1F^skd4+h(!*I7Uc(9R;a2`Q2DWM}_#|2X#d6D`~+*9?8vh~nr60%MI8}`LsP9aTb9U$8_Gprn-1&+?;jfDB z14L#WWKX0EPYsx(EL9iS9nvIEn#IedoIQxku#nYlws5ClI&3cUaIO@+`oJ}*v<(+n z8Z&j-kMRa|tKO>gW;FV0_zzME{+G#$&A95QXoB=@)jKsO2QH(GE3R%;Z+Md7_;^Iu z?kjIiv61*R2tB)Lu~-Z!pmwV@=*|*XTP>;%X^>?OkCTmJe4pOMEgV%sRZYtoeOV3g zGvBqYFq$%#5pNRz#Jb4L@sq?SCxj=DSmDdG2q!7tzA8Wg2BICa)n$}rK?OV(s*qlx zr8%X6eohzm9ghsNSgKT7KKYJ8_4g7YBHsI&^9t4bG}bT30o@gz71BKxEijUDV@2sE z&t)ow)bOwj5a0Q@m7xocYt9k1Q4V|KFj^_L^n}ogyU`}FR+13+_9zS=IcU^%(I4YS z66J8ZkR+ApQ-^kqzAkO#dEhmiug)75!AXLT0Vh3n3Q08}NTk zX0J&u8!+()n`)+(J&oP>=2DyOdvjl+Un4_;5u><>YU%xz5q+<-2!*=RtNmx(YHfff zuMp?)#ZhmR>Uy1{hCtEa*!6<0UFIP<>p#GYbWymGPF)E^f*6a-9y8u~4`a^$AHk)Z z*haKGV3D3MuA<1Q2S_Lr9QtDhuoSaY|0{oV{doaRFDPuxu}^58tj?YsOXC{XEQ>$H zXBFWt-}CdfL7rUcJha8%yW>IHDu4MOyRmWIKLh^zz=DImdu9n*Cj}*bIL)K4sTLsu zQcs8oVz<9qpZnJU@*(3b$W~J8sv8w{1vaGDeweD6uW;H$t;SAf&PZ1_K`s=KUJpC8QLw<-h zr4Dan*0D-5V5yfPf$}|ub!~4M-_TmUx&t59J7Z@VPMxI&uZ!FNEc>%HFUN}eyq~}J zGa}?zy=)avBx;6%fX;xkY<=FzBz-7|9%%kai4y*i~W` z`k}O8R8(S==orA<|-nWDFPaD=lQWWteUG^y$c2d&$wA z<%0;p1aoBUGEO5CO`!bUP8Hm$A`pOr4Mrv-(3W3X8$^;aB&-(n2>L=s-UQRg_VRdH zn}2R!0B?K5`_EA7D#3izbt5zd`N)4H`V}2`giOrM%i{iImw4_x1!B!)13r27M9XQ! zF8Yjo@}AIaJ#|kVQ4urIM8d`!Cv6mK}s(Usn)-L@;Hs%Ouwj;b$#=TaZHvf9yei!pLy6dGDOPM421F_<*8 zVgbxqa{~%VOCi}YJG>H#Gl##*+?%ROFtPO5ZHOD*M9dPo53C%$!3P}zy^Vs4D$JF< zX5^|5Igs>?3E-osVVdVHgru(mxAi&yCEkin|J4`X>P0-x7Q^*6yWAx2d1jg@9t^ru z9sJ*gt3ucD)ia-4L0Xx=^$1B;odf7Ohp=y3YqLFStBQnMtZty!=T<$?6Jx)BeCD|> znz!aXq*ALT(Lsnw%LW+&3c<+J$qpG_skHJyzIOz)(4Lq(!(kmthj4p~wJN)k*T8cl z>0I1>T!Bs=KHpj!B;^9c+x!JpACWbxxc6RujWN$G2`(RpL*Br=#hz*pZ{Z;eeuwBG z<4c=1nun_f$++B9iKL~`d5GTetmiQ zxwGVz4`o}XE{~5uMIpjFZxbV@6S#M54lE*x63_Blo70HpV!j=TPer3RM#^QhuY7^? z3G`oWX57j@DtWPn(=`YrgF+4corlC(X(SuUgI4zdxWI^mB0H{@NwFmFWP?H~f;@?Dr^UoM8 z-P!~{8Z}Hbbnsm7bGS;>42$qmG9;J1&jxvjDqOwk7wN{4J%~*FL&cT@APo`y(3|}e zl00yw8;Kxm?}g)#vcOt(idg#PSNa_-?svBVO@thL)u*+SIOn;AA`sWx5i0Q9+_?b! zQ!R(zmvtBL7kzX>jAys6RiL?;Bx%Des091ru`5jsm6&&7>|Tv9s%NSMT=H^UH(S*h zs@L21ToN8%qhuT0h|+2WvZn+qba9C&5k$)>G#)CVf}~Ms!{6TQ#F4$&87r5i>XQ`j zDv?P9thOLVxWx3q-xeG?V|+&W$b?Z(Sh#yMXtE_5H4GE<5J7|%FsL@Mu>aQdv*Zr} zqqCYc$bn}A12-hMzPrnN%w;^csR)G$lJfNDD0@}MW~>NXJ&3bUb7&3mG42gw9Ww{) zho#r~iCFs)c@1Oak>j0p$=H=%s2EIEHHw9ZF+0lA7raw;oMZm7su=5}{&pbt^sNsz zZBMBzyqC;_d4)Fc$_nDuZh)}iyck)+l)_{U5E(}?+Q4)}A1kG$TXQWVd1 z(%sM0(4GywF@wG!ltzvZ?&NA@8H&ZsDw%VONY&|X z071tkSRQmuDa6JV(B6;Tku;2`@FYm(WPpe>< z+H<1W2)H)8nE>!$X?jw<@A2VsK3lG0dBZYK02 z-q!*Zcz385PD~`*jj*AKrd39t#u^i4B}#%oC$^-t8maz?xSjir5piC!xpBi{88Wo8 z96v5@SkauJL{^pY*^G~->4)RDpV#CaT!zbOnuK9#kqNM0P!-Z z+KWrwM0}#K7sc1tV3mX!eQKg$!1rx>fx*!B6Sb~#Wy-=7=c>wBtj36E_{{+zp`YN@ z45!qNXTs3q;|+A9E$ztexPLrSMy3f}TD6fyrx5vg6S3J&`oZX%2wxbos8R#TdtdZU z9G|1(r^~BTZ3jy$g{a)0BO{=1c*K-t0B?1}&asBwff|uR5;l?46e63hC(@{F#42bw zsMXF{EW7;3a*IHx<97Q&^}t?w%dj^0a9VF7rgQ;ruM3}A3T82T-iqotG&=dK>6oD4 zYaSomVAKlC3Xap55=$b_G} zNC}GsJ=4bmNys8^vP2da#2bccAHvQb zzBed($_?!ZQoIq+|5HQ~L>C^xkfxEVs1q9~N+Z;Hlo8ByG)JX}-8Tn*ozKyhB=!o} z`678DwK(E6*eh7F0*O4Sc??#Cd8oC&O^d%^LJgt15eMhuqe1-B(RMXeX6%XZev;no z#e;D^vI1Bibe*U85Yw!esz`-6<#mMlV>m#TwTVnMAH+OYFLSPVdrc*a!CLPFcPBzk zVv_iX8kWjSeNp5XQX)qpTcatu>q_8*V;CzeLn*hh;%?7Ij$aDdMOC6a4@^+dNBT({ zE}|KlSJS<0@Av88^80kIW;2GLe`ZB$X$gbD8E`q7m#1ykg#aojI3|}cFPHpEHs-C>| zJMJ9i&B7Ax2}A|&g~zV&o2p7f_Zs)oXBkY8S+mfm8Srkfm{pi)E)YF5L0P<@dfuYA z#Dt6QA8BGLZJwad67sqyB&%{witUZlU5tc-n2>-usuuSQ*MoY~YTuMB$-sXF6FFr* zuoG#8$2JhH28Ct`UVqD8O-f9n9PH3n`uI&fhMZ=jiIh@`64k_&Mb@UWc+{J@FWcUW zr1jBe&~7W)K>aM%ll9Yz;v@Sh%H0}mqSD}kLXY~7E1Qc>tmzFWd9${W7$y3TnV@2p zryncRLLA zs+st}PK4Iay>0~y5Y-DN>>$Z(1tgw&Bo>)8us`Z%FCd9n13lNyA$^_I6P$FGKwB8@ zGV0SsR5m#|rj$6j(DBOFBeJwF>rbdbp%(pdpZs*ib!-UwSe=~i0gdWPh*sVmqi^erLy*-25` zq1=99%A6jwAIZ;Hh(vUv9B^&}2E1lm3fYEz3H+L3(tmK#UKQ*Eb{2@2Pz2RsPgB88 z&%M|?MNLm(?QEwihRQ_;3Betjw=0Z(Nogc|G%Nd#eyMrAnjOqLv{Ca&_p&(eFl*DN2)^=_c89X$A2L`pHST$e}++-&*j z1?BKsl^3KjS*ObvHNhe1vbU z0EXn#WB)ms;3a9eJY(fy5#7a48m8IbQ#@3+eVct>nM3E3z(Ha>aV|>nC0AfN#gQ%M zS1$ikdAo?QynJ8nN$FhZTqxq;69lgnB%_d-_#am@SHb5valPR6?p2vw(h32iP)%bZ z36wk9mhCwiJ<*V()8Vk={0$x0pHr@PfI~E+%MyaT*z;mS?SPl+&vR!9{zQ+78FeXx z1{igxv_4%hs<_wtR)pmZ70VeTwYR)S!j1?>e36@rxM1XMWUZ$2d^k>vk&&7=!QYdx z#_qT61*YGO&kK)ACcoSnywiPrwoK|ybJLymfp?Q9w&ySunz}TU)eMI6^a_+}6yZAW zrO4AzA=faePvUCu9nn5+atLWDR?5H){8>=J#S6A3na6@4;`d8si#&uDyB0n zF}&9FS8rfE+=M1yCfcl3BXW|K-{1)*G5T;){^5>K{3*`jwKxrQhK$4}A7iqlyGosk z1WyrBp~-u3`hNO|STFi^&2$#3WwH`|m6VQ=ekRGrc+|P|mubd<&rNHzh&{WMgOA+E zpvUPHt+Qcv4u2ssR9RG-;UPQE#k=6B{AO*(*os=L-1$(+9fl>@1y>b|ty zv8OQC@S)!bdG?FyQa1NPQ?4CS|2akyspR8RPDW)%?`3SOm$i$nm_201=JCcVTu?tR z|MAEV><`WSH>SNLb5g+=Iye5i6o!uvOcplys|n~xSKDw&eZ63iAMug#2Hi02`QA&$ z_hV<%b#C8`Dfz1W#_~YN)n4B>r&Zr4c_?Y%ncl{T^53N?3>3X~E_IclDR0`OZbGZK z**&oBJ=<2swe^_kDE_5E3@h+F^;R6sm4+LM2PYm{YOrE2ANGHsQg4-BvlimtH5T$D z>M4?twe**|1AezWlggBBwfky1st@SDP=%u@&I(kvQiV}wU*SFRItsGGG8~FhbZQq8 zT{|`8Ih`&Er17$aPXygurf&|Rld@H~9o~AFL`~9wo*^gpUTjrA#<&AZ9Ig|j`b=D_ zvTsYh0Zr*)9rW&d2egxNv6&_M@*3G{_ZVm>>K%iR+|l7{S4+?nvxt>z=cU)8pa?Ei z+wvlJ*wS1*RxLI7n9y{rH-W-|ToHTbR25O%Np`VeYCo}HrR-^=9XZ}cW*r+RBDkmi(GIueC0%x50H_9rW|NxChci$0E}dQOjdxT&9s z*&wGR)a}C!wFDmu2U^qE6YV!%!+^xZAZdGv9KoPlui7KrYVNB!KD-gqdsvtA;JfEb zBg5xKDpQU)tupmFM6DilKauDrdr>H+iZ(K0bL_3%)R8}Vl_l`QlT-A5Ush<3c62Fn zi4(x6548KAp>t1JmF^|E#!7a)H`n&>^H$(Uj)b_f#U9P|le=MEc?kYtldh3H(Y8$C_#FpJR?$x$Yz_qr76b^4Sm zTBr&-yF;8M*M!ukBFCTRD$P=j$>i$D|5Q}VXXw%qVrm}JE3?4XM8*`wn{eI>-(GJ2 zXj>l;S_FhZcI59Cn_2zjOw(cOR}r$%Y@-_7m-qTqdK?bH*&AO@1&m zcgwl|ejpIQRaK|=irir$(`RI6dIMEGLpSqM5dbTp|&N30$vSNU3=>$m>xZ|XjE)IvI~RM=dFGuZ6(saq0bxPXx* zE7+~TcRaSi%Dikky=xI~84Q#7f5XR`$^{>(sfA8rq3v@;Pp*6Yx8drQRnskzoxF$W z?W^KbuJN4rrBc7(GJ7&6@&&gQ3)f3vw`zaoWfip&*5r?Sq8Z01s`Dw@)|*mCce#7n zUDjAP_QXSS#q6u1!by(u?QF58?6wTcsoKAI^?8$CpUv&WwFX-729vhA(`#xT^)_*C zm8~XU59~zIzdMk9lDF}?{%Z8DX>R8mSstf3E%%%@o;yNDdGzP1c+;-;(;trw`9^-N z&LYgJ@!6$Ly&4Uxqt3B19NnyXBF6k_C3GvAmgdVjQm1PSiAA7;W2(m5pG~PFW&Lrl zdR)utq0f)ScV*P9tnqSsYiPB|(i>J=>f3qkk0r#i`NMw%lk+pi-{gojA7c)E{>*CZ z;zY0`JwC^Q_ZUUoLXI%%PIi5qO_*YsOSVd!WW0fn+j5h7=>ixYZv055Yxk4Tj-7ZP z=xa7yf$L<~^n1X>sm|s8NaymKQB>fEWQ}Znrx*g$F|%BUCnuO~EED|RaW?A&)t zjLqHJ*p;||^FbarexLHF>VLv5`{#0HUyYWftBy`&M$wHV3J0Att4j$cr^7e%g~k&& z#wxDRp0fG6b1KmTtZ?4~DfB+utOQ;&|G_nD4yzA)y*@KqUxPyq&--`bI0$!o%6qAM}cwZGkYi2cqnG(UXR z?;et5ntOh>Ecd$FvyVw#c1@Z+@yaB9+WwwqRg`N{oyCG8VL>M+8cVvBcF)D{W|B`* zW`3JxYZ&Id7~1lxw`xxiNX6Jw-9CnmA#`1Z)D*w}gL0R;#4&M${GiN5X<|j%FQ%66 zJuCU#1}^dq4?3te#xvwxeFiomtn=Bj;7@@`CA%#B@3-&RNF{c7Z-mx>jM zjny1dKNK6eVd%eQ!mqirr$L7olaSMl4v#1P=-6$ zHmcdcGCr_X$u>N_j7qOZ(0TxVNr@ug(lYwdoCq6yRLtAQ ztn}G+N3VOqMpcW6^SQ}#EsVOjg3~x%RaW|sb(MUw&&={!jL}rxp%Ssp*uoi4dU`$DWByO-v!Je%soQC7)6x$ESDx6KcLd3Wl-CpF2+l&T}NCR0rO zg5tNTPpgi!JGcqYFx*u;AFFgTTGCd6@(j0~vOm;^_PLpX zq1ZZ;rZ16G>Hxd<;$5e$Po zzUhwND)s*LSXg898;s@K$i3bloVh4++%baZkT4xqCd3xIopwcTZ%uoDEt4r3W@`?q zd*xa(3tC~vie)V<7 z()zfKpCxCjEz8ON(MGNbYPw@Elk#aKWl&&8g`pXn_D=hXH!F)MoBNbja(6y!K9|1b z-Ns9XaBL^;>iVBJY+QOdoJxzuht ztY~-!|MB><0_PQj&bU~V&PNO2&HD6)|Hw<7Q%Gx^)mO+3nUK4l&Mt%xa}#XHAos)8 zX~{P}=HSOzVlqDu=m-Vf;hGujmGMj+eDB5NS82$3zuV=(s>T-Yj^k++00cE=K{aQt z*=KUsY6iU{pA>Fv(^b*ws%Y%9TzhFs6Wd%UUMOzqV`d8Pw=B?N!8eN7U(4@qdwFbO z*nv~?5w&G9(*%R5+1Xj~@^fz$$rp&n^0ig!>~eN#G8n|0E5)ymis6H}$vxLZ?V)whhL^5<@uJ4_>a9Vfq`!$`7H5R|I3j%+hGJkh<{q8E~ zNx$5&Y^>x?+*ocH&k7IjSDNskzxDO-n596AghEXuL-Uxtz@%hJTz!1gVSG&nekuE; zrE0^Ej^C$*7VxsugmAa~ScQVoq4YDZJ|z%{v1`e?sV;#ex6|t)=k!ubp}_Wpm(?pj ztB+i~N?I-;$2HC{U12xH<y-_=Ud{r3gOA%H_){T<>V$Q3`8qHi&0^Cbu{P9$k)Qte{?7xRH?; znVo>t>m=t)$hK@Zo~^vNZn!R9d$@!r<$qXMDvsHfWT!8;)TmHJI%R{OBLwW#CY~mw_KjZ<0 zv*y2VHU1oMn_F2PZ>)l-OeJvYiV`=#UV46L{CvEC0poDmo6L0>`#0b=QG9=3bpVME z#QRII;4uY?lRzdoo>bXu&IFIf8i!pufGy@-`GCms4ZP1iz9zaqr99Y|B=eaU-Z5>z z+0S47TW!c8@eXU3IZ2nhv`6%rb`_mJ=IT53)*4Kj2qMN1WR476va} zv72JjHsPI?T>R(0&n{lG^~#OPfj9&M#N$ux^84rgdHf}t`|kp|s`l!w(D8t7ikmLl z%|9&*jyeu30T9KHNi^F0z8k7g{A=+;58N|VPZLV+z|d@$?)H?RsgMcb_hImJk!#A` zu}hz3-X1A>Rx(Qgk-%TAdIf2^@LTNEcHuP%tgfxC#mB$Q_hdeLM`=sb<>;F0;+*FV z7Qbmy!{wxt@~|)_i%j7R->u(`eqSZHEx4WZGas4Gq9V)XG1*MxG0{Zv&Y{Jngn1D@ zioIMMatxTMnB{S6Dj>$FVNjjv_WqDKc)m}(rj{p!Hjzhle$kdOB=~5rldg-Wc29lG zj4Tj(SaIL3(!uMfO1lyYd%Pn=gOgT6bzR~6A#xQJznz`a30JiKc>#8ozWIKeMFhbZ z#bq5&xPWuSVaXdpQ-PO!$zH#xbyzYhaz2MoHxvsSMGWe@`a2_9+;eyQJ{aTsc4ciU zgDM6$Ev^@Jh4?PZeSY1{arO0{Y2MQyP5#DEeCE7aiTmYVm|=*y70FuiWPW5D?+Q|F zxIf=_?WGaFUvEa2>F~?_&dhDQI@?eBXW^of>4H^OVvfUUtYQ|uuzW)mZ#0#fvQ=biMmj+_Bwix&T=#@lduttDW^6KfBx$g+I2bbJk@RdT1-Xb z)AB`rx5zOumBBlMcWow`QZV)s{vu2@v2C#*-k&pV%3r!$=fL-RzC-7Fjnj3GrR)5j zaa^IQzxb?IB1USHO_sgMK| z-sKf`XWn3^$Ot*|E&ZVmZTf$iBnX9id`t6Y)~0NWQin0taIk?%GDvU!kRI7RGnG|Z z90zk-hymw^e!p;-bV9&ZmJ|```D;Wi!=x?(Co&L_jiK~fU*U%Im_+7nHLjl|^^_Ou z?@&12>wN!8-(7udxwt-FP@A!nynXq#=lB3PN4N*p-Fq6lRQMKC1o_rCkH!;jA5m30 zK|I7%m|pTe(yNea#0P>FVL9@w{-Mu9ZWce_WQCfwINL4XsWLBKRNlGko-?n9pm+xc zQY^J5nUc*F^^*s^0|2RBn9w^g^;(d3VtTE?g+nA}^*t8A@2R$b-iy%uI+CZ)pn`#r-B zWXyG3PI^bx%RDQ-ws7h51X|Q_6v|!y@gAk{y~RSaoKD+Pru^5V!QrM*Q*p!&m1;Hb z-|?sXzcsGyV)`O2i8ig;4|Q=U;NlG6oNm4ZXh}v13c%l!u^GDkBjOhe`J2*D$f-B9 z+*VtR`^?(joUP0I`$|(X1BGJUmjl?AtF@P*wESRDzb!$uS?akof_6_0WEe}1RQy#Z zsVT#{+&FoX^ZEgm#-N)Fx8?`m`>*_X)xTy>uqXWJD|b26?>YYW(EeXkop&JBegFRp znK_P~?Hntk%&ct3x>Qn$!jV-}MwD#HI`%n6va=dWA}TVnk8C9)naAFn<2ZirkFNXv z-oLBAuA+|5=RIEI`Fy;?R6DrolTE2%SW;w4*fsZM+{q1cEXwdK7 zgD&4e6J_0I=FIl!$=y)wM-#GA0CCf>*#-3LaNd)TqhTUNVeG|Kg!ePawZl6QEkBlV z|H3>Ba|d><`}k&XHlkpUubyjgMyp8!k++vw^^*WEmPGrs0_u^HBB6NLxR}=oY@zi- z>b57)Q9c-{&qYws1p!Ys;stGNPO0DlHqcP>rdv*dKyUn|P2Et>d)D*Y#IJYz3O~4y z*YB=de_ed+d*Z=uLTZ3cJ}yuzR6i|?p?2h3{6jw8MmDB9f&~@$tDV6oD=F9R_qV6Y z+b_?RXwmxgZ$RNqb&kNJh%$$yZx#DuPkYb7;hZ=x%E(LByF($bR2)rR*_xl)5 zQl%|m*>01GevsjeOeT9jx2FN5WS7!bQPa`xbAb>AXYy6`YH3Vde=*>(k+y0`ts2pq zt3x5fTUFZyfA8F**Z0+%j(+#ZGkem`URQQw-AKxdv?4(Up@8@rAhWo(&z|`oguB{%$`Pu#x;IEJgB9mn9{d)HSIF!pXlp!wJ z*3%$*qw-sIPYthe**4`7vDfuwHt0$gLDE&sxoUs%@BQDqc)57uj)?%aPWM zD4Vl?2k(?Le}57hargy*?dwY&+mM4O*M7(%f!3F~n5JMv_Kc0>GNz#~3Lh`0saV*G z3TTau{#@-k84>$|^7JAlXMj9KnwY+FXuW=!L~nc!KIYlp#d4ZI&osAI9#B=SCxEI; zyW8Jup!14_O1rh}aJ3!QQyOq8Kwy4VDdJi0treROvKSZ61@fY2$hfOq;C81h+-4Ei^Mvzgj1h7?-vKmpU(Ac1$f zPiLZeOViN2G3;Uq5(@TRzk>OZAael+Bork5jo88O$DZ`&Tpivb!0`xtHDW#=O8tsi zeQ*cHi7B}H;#S5MCq#x%i2!pXR+k`SRbtnq1-lo0Aqo6B^v25qLk%_T?WN4;phClW>&fv(+g; zh44IMO%U0}u#LC3xd2D`w9Mn>;Zr>gD`4r^4*-?OWRvl?!YEKwU4R0~$jinb6Z6<> zq5a7J&^G!#(CdBxL!`s_yao_M2n{ilEH$UGimC6vN1U39Y2y|@E`Edx1Pkm3VBfL+ z-zS06CzA{dKP#yS5f{qS(0XSUub$|hdFJQlhbDoKq+q0+$RASjfVty>FhW>9zI?+& z(?jmFBvJw8bd)g{lc#Hd4th)4MO}%+52QEVr@VlSQ_oT_&ig()=cn)I1u6i9Y@=OT zI^msU1vHi$NXQu|{u`VB&&Sa%(+Z8IM#_5Djuj6J*cGv2kL?R?e^C>mh~8xglLX4^z)-*b8FoC zH6o<&s86f+ZrkEW1>;AzDGb~H-sj(g=4SsFF_NSrFM(Br?Px -kkd245}o%!yZI#=b{`nuFjYf^rf2NXkj{kiO+Eou;l9LmG#||0bVMdU{9q zmhjRB$ft#RJizejP@j$IFt92mK`z0MbK++_$OGjqloKI|66)?O{IU_s0vO$S=-f6` zvu8CDxn^GFWwkE02gV&8O1}y!NaBtR{pex!lKC!Z0s8R07~axCgFkLN185w=L36Nb zXfyPE9^UfR3rhFTFO@1<+DyUJw4MDb1(|1U0T1gW5I36)00E*TW5l#lM;1K@=IH8v zN`flJu;1^t$WZ|EV?SF5$LNUJwN9RoQT>#z1)E?)4P zQhBWYZ>ff3JmbKWw|K&vz5X{E>VFK?i-Gs4#LHZqW&HOBi2DG_SH~$pLar9NG0_y= zP>_P=j+};q?F23gM-cbFWxu^rIR<#r*g&U)ZlW!%_Y#<{sLI*uAm*V?*UL!yS(`&#hOSbD+KzYs6C$p?8PXfW15Ur{Gu|sI z*F0WV#pUqU+~(Ez&;E!Lt=|LE(j!^eg-i&0*L)99J`w<}RRJlupK`l6bac0V1)v{j zfk9kzQg1#d1g3g=x?<-$K$j}`fLpkjXW_c*0sFWW_9}B=gP5Vis>5pY)4C1r?!N_| zxd>Ry!&_w!ir|D#UqOzaonF!fRVIY$YsJJiMi0q5tAN8Dt_xEU<^`n|Tc9cwcs>6& zF6hA9Ec^VeG{n&yy7d*B_0)J2l{9nXyfwz2P!8#H8*AnZOloU>|EM@8TE&?dt6xH@x zO)hn+%$M_w_(C5fxu|9Reb8(uP|!`RAfe}ubix(YvD#`|Tz4lh2ip zkKvXfp`|Sd7-3!h)A9|^EM2*Tmh>&D_^8^m0lIp%?3MZ%|C#fnZ23-r<4j{vVMYR9c&s?f~O z?BLDEAO;R8TEC@ob8qm#1t3$vr|m%HNhl4^yC^!k0pJT9S-Xv5FqYCDr|f9HYlmf; z0&&AL7Ox^c)=YgVeq1#|J~wsPBXCr#KA%?ccfX;-IsyO;dfvv5rYg&oF3+PuGUBe? zv)Hdt#rWukogvchySgdAuO$W-q}`T_rvSVFrrl z0MHT)c&fn5rfJ?UC(8O+Nej*n~wNC zyxWtj2Oy7X>h;pGhX2O1T>p({jfKHku$d6}Vf^{|$F9%i?_7GW?&oD(xAVQa&k`zM ztkyG7Iy8G`?+-($o0HCd3h!%CHiR4Eh@F-|cZxhy#Q` zl)=Nkb1o*($xrI@xTOjdg`AZo<6nS07x~};AdZCQ#isW6xh}BZkErIby6pS!%6^@aNj{m+=m~R&WD-!Pq2>5XYXCO5N|4)u`mydaEIn- zG(bl_8b~1{IICy%#-U!QXUTXtPdFfaD^>wYi9je%{%VWHc8OuQG`dRp*KbhznVj4P z^wI?_>t8L>0dvOVJfpk%g60e0TR1*}i_^Vbwj~tuqF#6|gMdWqG*vvd>zD#%zNaQrGO5YPC=$P^CDw#eW_Yq)n;5*#@kxf>Bgd(Q=8sxS3 zme;?mc7rv&yW{1^X7(b*s_S>`ixR}AiGX$Uz#A+hR>wj!=c!`rfUUlxYV`{@s8Bcn z=Gh`U@pE9Df?~JTjg+3N4l`hD2)v#YNh#tt0f2>-z@4uRpw7+u3(Pq1cDs)%@t#g0 zn%M`-1a{m*%ga0~)W~)FsclfZ*WpI(z$xW5{GjCnAmD~)lL%r$6PkyMcQhG*#J)C{ zQMgavdW|>bd>fRe6h)8);q*V5@ZJCs9nRS+UvvMedFLy z86Q+0ut|y@oIT z0Dw|SaS}wWsakSzi`KM(m^DTwzE{2g8%zUOovd`?#4jM2m2vI`iwxLe67Wf%YqpBU z8EkfSNar2IP^a_xQ9_~O`NXdyTkj3sFiCu@~0u z*XfzX8Yxe^eo=8ym^5!*8zuZ3a3>m0_4z}XC5Z77Le$k5L>?b-wU$vsoNYC>j+nWl zxj%T5O9kRwJlhPG)fQychRPopcJGwm79joNecPH^t{qSlt#+_KdKkw%^)s(1RsK%% zY*6kb&RnRr3y3;bo0V#0m*cOj2^kcRGvQ+VIoBdG+?0NmUNGx;BsrgVcLZx-Xk)za zblDo*0H)V*Ctv}mM||di9MQzm{Yi+_2uSHn(TjojM}5_n9d15S>25VXKl|4$z6Ufe zdticLo&%dXOyGMs2X#gS(^Vcii|vH_E&C2|-X{R@G7Fm6n{r)$>)wHj$X5JeHP1qN zzsBdcugGRyndb4nCu%duHs}2WLC!wioc_(HB^{)gr|(1ENjl9lrN07jFdrK9Oj5wihCzv1p?1%5|DoO|XxH+s>E7 z!<<(2Ji4VNvP$GT4x@|lh+^*KaVazPud}uOGY;?6*Lon0aVK^R-NRJ^Q`n6!AZtJ} zw~k`CBifPcSQGq7TGTvz{A8*TVx5a3mUhOxdfu$$BJIUoqe$4nP=5`_0#_vMan%go zPPMx>RZK1{d;7R}e}^%RP3B&X^?8g?90nye8T@*hjHegY43ek#O}zSy>=kIlY?Ave zQg!A$M}bu%E0UkfcH~>0ln`FFX(&Gk!nwtufdispNJeGygCnQ`FhD4zq>mmv{womD zOBoc#h3X6&Wy4lvKEqd0>Oyx!O;lEOYw83jvR=Vv{g@9ScgTbduOzCL`NdD^wxo`8 zTeG#T*OHqG)0A=_Bc4an)%i3OWVZ&XTbyE+}-42 ze@FkD#}3jog92YC;Jh|0`59=`^BYgc10~kstbt~fgZ##oBzB_S&vU4+>P_93mcLe$ zbB%9NUZ1`F%6D*y9Q!GylVgo}T z`}qm)#UeyHV4(0!Ls7BS3ns!9bM3WhE3<~t%{2;b!~K(R$c&{L_>+9$CwRXGo)I`^ zGt>fEcuI%CBc5XI4z3J-6DxuJqOYt8h)a~Xp`!Vh@J>QU_`~w`Ig=pOs+EA1=-OTr z_u)SnIN6pHgNwahg@CLKy7+t+wIM09;(27 zVgb{+yg2soHd=6991k;1ssP%XW}GvRG{C9^$fy}f+nw;>^ly`+cw-`)0Iz4a`6Ti9 zV=LDixMAC|IoR}+;&WZ(^O7K5cMB9SE@kr@>C@-)`!sM-IyguKKC2A*6nQ-Wcxd<@ zh);BBpP=DrDj5OLDp{S8e{Q_I|2|@J)boq2=aYB8hv9P!6-E*`C~woZvlO3S2P>WU z>=cj|A`4XnLxyi_V2DajGgagQCdrRBEYEQaS%Sun8Y*|d;-rlRP6(Nb$czjWOO9fP z33vN#rcPK(d-hL`DAo>K$kG?IhEe&I5J0h@+U}70(d$i6Pa59(Sv!AYZ9I*Dj~%a{dkD_&AD9|QCpSfL+%q?K{?d-X1@AHYgH%w)kMUFCkEDgYwA%|`z>j@XxSzCie6QzXRtXQG{N=U5*h{k8EMT7L zFlY6I^OH-C`FEv?7PDIu_}ty47nyy^qg6ia07BpMY3lQR^qfqcMbrLYwo@Nb_=RY& z`nUo%$+GXT&3;?X$Hk@_yJq+B-VM@yhmv^~5s(-wmB)=@hR)cEI1daXj+W7TZ`*q*ri`4B2s!MSP$FQHy{QD@Q!}kT@h;F!`e7TB zT zrGy;oE7T0|spYmF)JD5+OadhAo@ONfmPugMjF>fp9X~zG9)tqlH)(jj&kC=-HLHpyGSE&4s7Du=FCQHC)zYWdYTB8uVh|3iRc5Z%VuaIglZCwoYJYhF^aja^x z!@AgE(2 zh|u&QWa)%{R42QrU}N3SvII6MaG_9%0;!ylT;uI|-zqMKhpePI67ld62|=TuJSPyamn5$Eoz)E`q(n6#u_VuEeS56f}?Krsk=aPg<6 z^vFjaiFvsQswJ71)vEOs&&4vxIsN%>bF>OsDy(WA_mQs0hWS>#V6}TzZ(;b>_8|E6 z)M~UY9HxucZokR~QepJF_zqz9F4wmP;_frfs3VQuOUyMXv|sjD{sUZ)>b1!#Wro^+ zJg1(`pzGYekZsQSZiznu!u1!TVHdBWK(2lAxC)gA?UAMg{eaeEa>4~Dy9OWpw=du` z<)+L7Lp$j$-!f!1jIdjaYk<(VKhFbiD2?Goys9AzeQjrWMnj3^Gr^?>*q><(tuT&w z2ikX#g`Q;=E}k@DOc|+M^cK7vPieG_0XL)jAK;or>a~Ral8*uc_HFadk3GV|qop z6bL*APm`)3sQ<_)l$T&bQieZN4KP_5cHQ#v1bS(wo4NX}bN8rRz7GE`Gx&$LN@1kp zKUx6Vg^7S!u^rjo^!;udBbdSfom+hWbZ%7mz@-&hMRSYpU-p&R1YqucLEQ{FALmGs7NaA3v@l`B^rD_6}+0 z8VT#RDi)O?yZW>SNrnba36YC{x+}FX?Vj{_`(vLQf?lt7>pPJz@i5_!9akl-UBezc zST5|nzj_Fz6|#NwrSbf@uY(WYC+?kzKe0EZRZny!UKjn=!#+I?Qv4{>ZR{5|+Pkm| z`d48y>;H7x3OVcnQ|759(I$H(k6#PT#2hl?)}ZQ*ps>+_1~+B$XtybOMf%Y?Xrxo9 z6hCbpGmFk#HEz&5Dtd+!&ivbKty(Z$_D!h0O@Q94p$_`pBVhbIIhSt^tCX$9 zD!|98W*JHoowRc;MoH63OmNkF(I*Z80-pE+zwU|-;Vex3U9eS)S5hQejnU|AbQ0%?DcOGLQQu(*q)eDgsXRJFx3dzCbiS@&b78s{AtAsdB(?wod zX572U%RqB>_TB@jBBCe6!P_&K*@bx+k$n~!BSfm+m1Yk6ZRP#&U0Rg)>5cSKwEJVP z} z$C7*ikwt%8(h6@+J10p#+1egGD9C}}LO9*&%;36ej-{qZNEz-TjDi;>}&jQ7#s)4y%*5LMbWerI*gH$u?!{hb>JJfl~r_^goJt*BZB%01q%wMT9@p9tlh7*h@l<_n!>RBnF8SC80#Y@p3Pf_22Ei{GV6fC)eFdtqpW_{73mhrmM1` zW2D7YfpaGOg4v^%k3pC9v|7oGLDsK@E?AbUW=$2di>{me_!V}Ig!k)Y5b;qBNpbc4 z-Dcf2Dd7I1!B|@5p&=qS3gg!@J_qCwKmBanK)38-d?+BwJeof&n60>SSD2;Km!KYd z84~w>eKPZ&W*u@mxmE!7{+{vU*ZH-*HpMkl@B1Z*zgvZpQIsqPv~}2!5Az}iOA8A; zY5vQH$nG5ni3Z|tPt^rf=CEA4Hb%Utm$QNfO%KJ!iuv~Ye8$Ivy+ohdlqfA@% zNZ)tPOgQ6mmlT^T)_A(M3)IY;_Qbn*JoOp_Dq`-CzFVyRJW(-VBwwE>mgKdrc4c>s zKr?4=c=d|8eqm6>3K+bxtw#HRY;w)+z8jNhI6u4F~}zTPiE^WYDC?w0*&)sa0c=~0)aBMh&^*m}YD86Xk_{AB5@Jy>$HubDHi zSx-i07}Mfw@HO5`&da{&{ozO;PW1`(+{C}YzaS{G0i-?kQMfyAv%}kK*Y=-&CB$DKdS)FtYhpOBD zk>ikc|1RAfw6`yqvPiYA(x2=_yf?b_EAcSpwh7R8QwComfgTziESq;)A>zg^T`h6x zJ#w~i6{nm0sT-&R@6gVEd;D4odODf224Xz0E4-^;9$OEcIO<97ZG1Og_C;Ft60U)z z3LkUKcJpboq^=&5cO0GU^8{bR^l!%9^rwD;P#bmo+JajekT5McH01DhHjHYukqeF# z6+k_?KPglo00}R4LFQM?+sS@a2z&^j=m=^(vjZaQ0&p<^2{CA9wb8bYwT}5PIxYJ` zGKIe(oXZDD9PJP?-)DTZGTfxV1LlbX$-bX0Xh*}kK=t`1F|#*bp@I{Vr*xAq?kho! z(XNHsBRYLFr!)*Ip5Lp$;O1?X_kDkFOw71QKZd&HKv7BpKjmLV0x5_VFZhbID`5ltJi@%8(n<#uY++}vLK|Wr4!5Yw(F;xff}uYAMY?#b~Q$_ zHh}3|e9+vj)p4dEWuVScq&$Sid!zeE8sM}Z2hn-pE)<~6j)L%==jFw84glHstW}Ad z(b%bI{bF%=Y6fuG)w}tn??NMS8-NUndj#aN5n>u^k=mgSfjS8)j`y_JATX$IA<;Ec~D;IHWFmSOt}kfbyYV_H2{1_g1f^bf3l{ zNQ+}=?m>-*T=e1c<5~vb4d4B?0s&7Nu4WT|u71oukWyRm=~@_oF}@1=^@>5dN=9u) zrHd<6NpbI<&Hl|J!3!W`Kf51Mb+8Xgsr*?7i+2ooEX=C_dTeGX`=}_Px%bMxPIIa8 z&|Yo#Rzu$g-Ntms!kwJAk`VTvq{MJJaqCMzsIJ2~Cq!2k_=<_<>XP-+jF9g8{n>jClNk>i>$A*rXCAX-kCSl#z=72AKTFvvsr>P)=6Co6)Eu_7%qoONL7B z>9~PLu~z=MonY@l;5xO}tlHW772==@@K8uPC!Ut04hCp7Fm-{Szu`CyFC40}LmiM+ zpeR3W1x_cZ!9nNd`y*YoIm{2@q8tI*CoO=Sfwv#%eR-jnd;lF3boi|r;u0FNtY@N! z1=!bQWFQiod3jLCSP-bxvqHKD)M7T&AXm|!4g~)j!7h0n^>kN6+NTw(^8V`z1c!M9 zVelhM8&dqTEozsr8A)xzUSCf7qA=LF4jqlt3Yz>^krEHcWMf z0Hh zeA9$Umnf)WIuC}!bJBgQyrLw4y#`Qix4lU3G#g5!9o62Q zDYjx?U~e%GGc5@M6&RbOk7jSL1FI-9E!v~psV8?e>6MzuF!=s)F#nbTl&pJRh%OLm zR9fqrpeXbc=q0%W-AowNm{{9U@th8Yjxbzf=K06VFJ6DyU$FRH*kjq~aXgq{t5khDfvcO)M-EO2uU$dmnm zA?GcyyY~z0n1-e##+w`_!AJ_7wemh+kf8phMpaD zSL8+MC9DU(?r z8hQ2ACBE6|qYim$m%pgKFssppd_i2>&k+AG3#8p3y|9df3_(eFR}-|-p7-Weoz=Sb zEKhBYVFzjVY?BI7af~rPjD{6g_CuI4Lj~QtKg@+nqm^qx*_RQT!4mU+7&H?nYQYnC zYr@>1MSX{cl4t;ZLbD0|t~yWv#GAf>0CmD@i1*M~hL*!0{2TOhH%Ni9VN|AH+ES$B z#m&5fg&P;3=7_+Q0YBy;392Ff0)Nr!r_a@As>T`JFe6J5utYguKXPF|P*c{M`Ty1{RBf8=yJdLbirb1L{67@9m&!7oSl1Agmddl7CDKMvpNJ zo}9a|2Pq7)KvOW{W2dU*OB(TKD?q6*&R*=TC#=~ci>!p!STh@&TIv(tT!XTR#Z#SQ zo&cX^kXi`_L+9})YCL6Ag{XTj$0+S{R`7x*{O+ehq^hRQxN*&LN5JunJ-@3~T_Huj ziQmwV+$;}i$^4D+NTw^!mlx^aKTNN2{U#UK>gwKGH5_oDs&f2H$M=E}Fz&dUfZ1rzQ&Gv~atON2R z0eSeS7j>X=T*iMbuw~r$C@#&>g-+TVgkNHvD5}*@g`uiM{;*6<{AyCduV! zmx+Y3nAV^USe$8Esbo*kF|5)1(9lsCoqXdpm2BQ@^$|OW^~buAm=a=Xe?rQMaUc|l z6o$>Q3N8tz%`$|-=9s)yIG788U83;ME$w`rRqeY8X_UDekdFx(_KG~t*mJFllj>9` z_nx`UPG!Fy1=qf5T>jKcI01W?BlDji!Q+TsK^KIzCTQ|Ct;p9BQmHi!rJ5-bCv#|- zX$S9(irq~t;jA?g*wZ+XAaEt?wN{LHO_-PkO-WsiDIsJ!m*AQTUlE1L=9zl>6$*U5M3d7TI;&1f&tx`46Xztb>OO1>9 zq0~FC^{&?|SzXJ9Nriy*9@qro&iU_kpFyr;=$>ahOC7c;z$?|Qv7uGTmi24w7BxlY zQ-7R24`1-NPopJb1M15sydVK>Rkyie;7}vA_O5Hi)U)KBko+l`$bIcydzwOGTwtOq zCPZ*mBtI*#mV;?tSJ@t{unycikd04w?A4k`#^ur%rLEHdkETk#Mi<#oyRG zbAYlK?fb8eQMG^c^ElsfIA!ng1UeS_($0Ti2Vof^60AB=_-?tJI{PWCF&qn+E_Te0 zbPt+@rmjo595Zcz_i_}RG`${kJM_s-0NkTSH#SvAT7^1%q(vd6ZMI8On(&5(9JyyJ zFvPG^(ku_L9`FD-=)k09By6xvygS8wcL$05Ij^m+Z8bQ#-*iKCi?Wf=K|-22F9?2F zZOBC74LkBFB0CZlx+#)>~*Ly zi(EjMQQ^AU89lFs(QIMW@%O+qw>)u2wZ*UPBF+@=g0`(25@=0%1VXc!Z53y3wIagU zr+IthD5)^l@r{KkqkXNO%#YvF)X^=6^W>{B4-$mLC;~u@rzCs(Je9;@#Qku!GADmx z+whex{BWVqr)$z)ntrL7wr-PpUUe)_lWROoXq}rcQOTebs9sgayuYAaJ8xD@k3a44 zr0O@ZZJaSe<<_0BPtPS2)nSjZ7a!8xe<#Rui$b>kOQcT3DKQgO-z&fDSGCdKx1WsG zh2tG*U4?9VJg?+wRq0@LhH~TK9nT7>rDka=zaJu6&v@|gjbU2@Y3{)s&Q=f-o=>Jf zJdfhB3{|F*JDfp}nLW}Z(vRTf3G$5(%|>wyhMtSO-X12otrY@0%^Eoyyz?&-MS}(| z5t{Mqxl1zpSHf&6vO3(>=uVLJvrD4K@Vq`t0n5HcM5W=i!XPu27fG1D*J@;Qrkr%h zU*fZ`WOZ1~eimA2Tq`IriX6i8AK@sD0|#6&E6qh>x~MdivgW##b~&#jA&+nVePv8{ z=&g_sB^ZrjzT!m9+zoHF3_~rvgOH)%IL1)aHwB~9E1-q+&~EQeky~RJr?RDaK+|d{ zEa7kp7X?IP0!XSVY`g&pQ7ahxJ(Qs*6nh7it#i5vTuV5A1WQ6TsKvwG;a|5w?=84Y zwEkHO3E>j81FC88grC96VwHXLxJoPu#lgA-j)y~4mU~m=8v%HABwt-L?M;okvW$?S z`IMQC8aL)5{dL`>-~u2KFetk#n^J_!Lfv68vkarzIl+WC#%m&@->!aG(Lsh3{OPZ` zXOY$}Q_TC+LOtCxJ#Qj|$5ty;g1IjL^oNXJMefd@dp^-#ZHV1Gg__slW>GnLqZZPT zkvcOH&R0sohNenR=P7eE>(g_)muP4U+C1#hD+xeH84{me zCw>K^9$IK!jR)N(2Uw-eN<6-#Q+6q8V3hLvYNv4E~fZ}8uGbTOdLjei2 zHlPsdDCEJ9wA&kR_s*R6I8*)WY!&ansx)USH`FNt`g7G%E2u0z(V7QGRVrQsfDvVh zyz6(UO!`i0)8Z(TTQ+@oM{^kwI)Frg(+C&aGP{&v_`%BTGQ}7W7rk`i=Qdp z=<#2}^J2bT52X%eGA)2>!|$QOb-c9UYhTovn~LbAe?N5GPN0fW!5v_ zj~~x>swhji(@?~|9GSrr1Ne|WD8 z@-(Rt95w5SsC+3JW`W&rG++ixw{#sOG9`{Nk?mijE(#` z^A*ozk1~%s!DV}aR~X^b51-limzG;t>B~&}9^dOH#OpG{ zehOCM_-dm0r26xM@{NNXB985R_hg^eBUy-Hb&)@p!qSQe1%+oIF?Nf(FQWeE`H|tR zP@D4PwTWTL-2!4z<^Y%?@*Z39ugJJxl0N-!ZHytDx8}NKV$0##h=->&N`d`J7pf$` z;9NA0W28~>MwAE&Nv#)I=UZe3d`g+nb{#bCbc(07Gn}?dl5jp|$f?yRD}5+2(!bux zaaH6+E!+t8sP_&n8lM5ppuZ&PpkUgFHRe#c@%eiqrb4<3zu2!_Skin4?qTl<_g|w}$ z&y6B)i61*95I?sOKpZf_g&ghcIx2FLhv%|mb`N_l4#x-+P^KOK zKwX-e?Zf>I`8E=!e^xMAoD_$4r)>&wrl}}9{C%K2ASj?R>cBv-sW`!^<18V)yxD1jQbax&9d|($3VB##0#UK9|X{A zti7~(aa-jg`bZmV1w<8M$bCT+0??rt9<9?L0KF}4>DLM4gUfo#8#?FlxACI*8j1b- zyfxIy%}`O(3~T^4{Jyu*-Z7tlvaVsuJ#vEb+4g!ierzOm3lPMf`XvOV`2TsdwVT~h z!H2TA8zIr#MKqw2p#p0;G=m#a*}-Z+sjJg%sa^~bsH;=K6P^UUAaJfaf|gERg# z*;TxXye-oIa4+y@xzE#>@YOH};c~`Nk-LNR5hu%-O1enBV&vu@$MYW@J1;YK-pu2! z=|H~Iha`@&4idZ?_JF}BK5p%)X71S1twxUvoY`_?U)7F(Lold**A6eB*4~No8EEsg z?$ca^nc`9RLK4}hBLm^K7Y|Pfn5es(NTM#X-f(f2!T8=*!}IJFEj@-&3u06RgP)5P zQQ1p!Amdd08%6{<&35cZxCC}WoWdL5&!3M_8K&_+v!e9o>NnhLccMA38#Cv__cK$J@OpOMtnaZLWRtdtmNU-RsAywe=v*PH8*bnB5Dlsi`4P zM6(s13^xaZ#Pg(qDTsSBmu9Q@BkMooO4sbFJS6tsdI^NuaGwQhhl`@LtF#O4hy9xM z>$Af7pOU*J<4+8Q3bC`}*PDcb;I~k`y3gRaf2bU--;@RoM>v^cl##nbe^Q2PP%IEE zSeDp}am5H=ggu0GnYF%l{YMM%P1}L_qJ`*Lp9>inlObCYN?wOshpI;tFNTapWB^DV z^9R%&?%)=CBB8_4943cC*OL2Bheq5KZ*Ef?lVUH^AykRmjh;zu*K@`2y ziea9*`}_%lId!Ux8ji_l8sA4nUj0_Q8{n}xlmXXkXCS}hXD&vCSPsF+u8+7R13vKn zc2vWNzEnrn)y3~$AGftYntR#opKX-27*gjFn6r47)lo6kqUe9W^Gxlzo;vJ)yJo1W za5noD9qf8Y3CnnEoer~7L#Bq-<)@rqHjES`9mFcN*ghTr8}AafGnkT|))8hQ*e@Ov z|M2YWaVp2YgV%R9s7hMZEaKdI@fM-7TY|wV*R;DTA_~69yz^cYnhf9llw!oe6f**< z9tKa7G;(v)01k#9lx7T8bmSZ=E$mB(S^%2rX6i;6#_2Z*@ZQpPbhoZ2e9_U=5gi-IaEWU zSh|x(bm`e4qs6l>y6;nr&->lon&XU+ETfclrSWQT>4GY2M8u&Fe)E&Hbo3}3U=>~+ z-tp@HH7Zw$%!v4&&i08S(d+3oQWQtb-DA~Xyurx$iw#f*%XQ$~oZFF?-&|DT<`-eg zMjc6-$I5}N-ifuY_mu^m@huFpl-j+8*Ic|?W!vq4!%Xk-_3T@3=?_TmM2tK}IM?JiyP*gE=6YAJ z0TD%{#z+u#U9IsZ%f{EHKn;p#gT@k6Di{5;57z>E>Dg+T;!yYH#IP@)G4SZH@FF%a z3FUSh20<9%Fd5e{nk5!T#;h{)u!sOol94q9Gt)DFP8CYyuVVI38WsI+d3GAZ5KJmk zr#n@c2RPiDzSJ@3a6CQbkw{6E`x}vu*mlmQ3`4qxpO=0~rG&if+|paB#mloi%Ux?( z^8kMhY(P_tORu904ZIFzqbsxC`>k-FeI}JOmoF4tC3=%@f?bZ21?L}^b=a=lfmr)DyPOC6T(MTGPH|R<#<%2lg%lm2YzKl8VwHxGz%OMw-AGw%#ojMC?TOufUOK)kmHQAd)K8u(Q-xR$V z^=tGHF;>@gH@cfoLZ!a=W_lG^Zx1-*Nsk&1C5r_V?FxRre!hQ6^qR`;-jh+FYY6eG zmyB!{Z?73l8z*Ps?q!s7=Kiu)Yv@frxhM_ocJ1X)lA9qvd9X1t zkq;7$xjfhsVo$rd2Y(A^lbSC5^vP%Md4RxrbawP;#ID$SipuytJp5BmzO~`5op3>i z0ImYw3ZFIe*5XfWt^=xK);4b~kKd0GWW4;zXWQ09;}XAT`z?n6wrr1USkKUfa|(z@ zkX|xj55SPOApDtpTiIl)C5{J_k7>1Ka^1#YuJ9yY4~sNf%*DWnb|E6a2X`+glKIBu zW6T|l-3LVB2+_PIyF(r~i5l|IH0%OxHRTN7`8R8VRLCSJbBxjX(=sgly(tQk*Sk59 zZgq4jQu@leFXT8kb1W_doiSHs9>H6dJYfu#*wq@=apif_WlQUPvpsHu?U*V>&p)F$ zfz3qUeB(DCcYe~aqG|C}Ixpm14Q(Vh6JOh3*^=Rh$#;8V+`!=96EMWLPLE7at%j9n zJiIpq3s0TDrRnM2q}H9qCz`63vO@C-zFXFkbDRcY7C=Ljn+9ahr)As*RH9$&U+UJ} zq^VFkZzRDv&B8l-wc_XCRapE7U0$7F%Nv@7i?|jI0PSyZe!x{BCy z=CD|zAPopzZ*-~WKnddpy%p4}y?R*4SoB~07tDBvj{|B0z1kAG*mcy^2PQMY=a{p? zl}Q+LLzcmrU+MRYGn0-eGz3NivQ#hMMAdVT@CE@Gtzi8bkIH0%A0UD#p|?WV5U4y) z_shxDdGwmFw;y_Vuiwh^nXs*z=wJRr3Fr%7B}w|6fq%9Sqw?5+TYHdQyO*&=pW$ZO z_>lwCS3E-Sh_3rG-Z z@+=&(9kV?b3O-kt*d2PzOWV5=zvYP^#+Ur{PFFND^d4cqj=&a5v0nhD$%IC(+|Qpm zaQ(27f7~kcIjM|bH-+r=0o~Kr7s^dZmzE#({m%NuR3vayv(WJt@Oh4dAuKU_pS!@I zsOSHLs`X5HfZ}~i83*e3uWD==k8-@yfrx5qnfidmy4Ppq`wu|`VX-M{mDxR*^R2?D z?b@M3-~P7Yt>$!c99tkVgF*Fdh*(I9C3E#s>zH#x52Jy_dWxv|QLJ<{vu;TnH2Nn5 z0ANqXIqbvJ)iGID4{p{It;Y3*|3DVhe#wmmNb(HmaGCrayK8n@9>R%VpOdBtYGSW^ zD&UUB%eLrEi{z14xs9eZ%-SjRSH)kW7jT`1+W)evy`z7PwyXh(0hf+gVp=a+ z84b7|V3=|j4L`5_uYESe5c?OG1jK5ITX;$H5no6-1lyZ*XWIXMVlJp#o2Ds6`IFy6 z8^}D32y$d|JnSV%C-nVGxo^CoCSSl6Xf}i-Pse7mKQhq z0x1j(6}nR8TZ#-O`D(AtE6FXO%=N33;`W64M!)>T4RtW-X9O1lx zZ!^!-;C&*Ub(xdt!Sx+G zX(;jICqWieLp9yp)u(k(Z(%yFNCLuK`w)Q*eeQ^#mogmzDLf>J(E(Lpni8c-no7oR zXdDjc<0UwVL6jQEPa2bFzL^IhTRg)q;u=zJJAw0u6){LWX#yyT&|^Az9MYW>oAXXX zO1A}e2;GoWKH_()%wy{N^SdFOcDN0$UM`9>gQl+q2Gw%4N9>^8f;|O}WWCcBIY=h^0-z#r!;Xpyypb|qaNm49*IR&d8iyKkLOu26z&_j3 z61#<8yOreGRt)qg9~m1AZa2S0}3&3sAmZy z{toiMJBb&0yWje8tZA43rF6xjUJvi77v|IbN7bKflHcBrGx;Dv#28uO-@vHU_||Du zFg02ah@jlS&-=)(*w-=O@5E?r|3j)2-d-HE1>7-# zq~%xiOT!D#KC1(I2H(H`Ty!V^FXNE?*H4D z>?smiD=HyND2%d3QQRdZls#EO$vQ|>2t~4vo1{e)N@Oc#Um}q$WHd4%>=!w{icOqYV5NB#796c-8^jB58Z1tPo=pR{6pTq?{ulL z~}}Ewj}abL=6xX2j1I5in9>TLcgp&R2$1FFss*9IXFRXje#@YqFQ<3I? zYx(%Q0S$a0Xw{W%B6bQ(3DQr9H7g5mE0V zA)H>7m8t5MPqht&rS%8WA;#j2hy`cR0fEft{C#B4Tx<79dt)AiLqgaD&4VZ_{$+Pl z8*P`bxfOa!EZb`m0chL*vxBa1ib{&X*1%a+{g^ffBg=Gc2*Wt1LHmKlU!G0j)^la+ z)Eq5qU+Aiv@H0*QzEIVfQf3lS^Q`r(@?5q5pu6Q@-izCcfCO|GUuxL&%J|49vd2$B z9Xjjw6i1*;UYjokwLRf9PI6H?8;hzN2SDGfA(D1 zyW7Jy+OL7uBER+0TCFHh=)(JE8S~#Et`UsEByV?@nQ{j#q5 zKh+gQcy@db=%@|A|LHERpHGNu>7K|c0cc72f!Co5bsGz?HHBgMREmKGJm`Z1a_AxWuOPox z$6I)hO+I4%1!oNf_pj44JCzWe7Z9vvNNKcN`>1#Rc-*@G*x=>nh9--5{2hJ3h*h{z z`v+xgxhP3wt0#bVLIc|YVEcR0TQBj5+{q**4~nZ~G!J<1J;B&{Q0oeW^Eyd!Kmt~) zs7qrcFBQlVRbc*HY?bfJ&-7jON-zi>X_kR zOA*^wG+4IoI#i1ES&LY=xh0?0{{UGud+(TN!bRnFwjh|VbS%7-^cm2D^~%t58hL9_ zc&8@>(a=CX+|$t>&Xmma{UuLd$9&FmU5l`|+h6IGYv#tN_= zI{112sfG$^Uu60-!1QgyM*lYm4!`W$ zTJA0pIv=IHVGH(Ud)J$}AMC;^K6TL^h*&6Ai}Mme{r!L7-@`%)MS=Z@cTr_?S5sR< z`>OBW`vux6$Fx_~-u#oh23muADGzYy?3qPrBM`jIggsFTY}uLl|;QnY~P`` zS_)qGMy@Dp>s3|*#X#m^4STHSwVnBPPetcL83wSL5f49d|8A4GYe%G0tV_SN;I5J{ zcyzhn%X_PX19#&Ga7)}H{MIntQ&RS|Tk!n2yDyrsS>tv1v0Wr^Nh0tlci=~#Ce{+Q zr3>-iF4LF5QAkvpm`2b?)->Fl_VFG3UTGoKchs9plITREa%(CllCWm7m$WP}zjXA= zAsW~1%T5Y&$AQi*+|ZGaSlB*Vb{|6>J4g5T^giaIZ^Ezl6({Pb`#mq|hCMlNu>MD=p#=`bu&$*byBq z-;KwF^vkUd{hsV>)t(S$bi`aN5%dXD$kVU9DboJJhURGh8;85cRMR)K!2^g zkD(IS9g_$sc!C%=%?3dq6h#iDANf0j%Z+?grXkv|_Lj@{Awb4pV?(ZMSmM5qMSxL* z;Yb#Zdkkb%F~nr6+xmQ88APP%!j&_HS&?-bO>GR9wC(Q?WM#hh_4R@ixOW9M@?#Mz z>GQ97T?WWg5&(9$nf6(`uo0qCebxFe`MI=yU*9#X1UyC`fLt{K3JdihEL71IGf2q* zI4xxSnA`&lJGG7DUl@%zuxvDw6=m@HIqeNRnH(*gVBf;5(ZdXsR=c>=bqxEeFo2`s z7vGc8&QU80fPv~3)?@(k0hkc)kYzL$;$DcT(yyI_zi8YIVb2o%vH_G@CyNUPOV%BO zv7jtY<)$F9|H@dk^67=$G=|BHlM9$!K#8#n?K|#rO>5W__0AM!Zzg&PiMDh5Z)S;Q ztbdqdt;>@EfL}i@Gij!pJd!%EhNovBd-05@omMvsQChJ;SuT~1!@>~?BTVIp50{inH(|?ZH zRSxeVn3o*_KBE+j`AK;#j6DvRjP`O;hApEf%wl(Ib6!PO>xG^GO3$;g^YmNzSILXb1CIf9Im6b}59ciywAA{| z_9f>$r@erMVYAL9*w_FdTDUiZ&&f`-5N;58dPPtY>J#L5LK(3_7gEp2y|SK{3AQtl z)!APbsC>~}bJ7?NUS`UFA6OM8si0XPQMgS?Tw|`A>6;ms= zzN-VE9>0d5603#}MpZe#uu*R^-Y0j} z?Hq@~{nDy&F^`T%YI}lpSkD%y4IrW*jN5g;O-2bI3Lp|Ss>oFJ7mOD?#DCiSQ=0O` zk{Z_u(ttnYsOO+m6t`myf>oS29LcxxPYo~gaSY@k>&8RGas=FroIo2>J0H9cP|P8j zg-N;v+D$xZy`F4!Pa2Oj+*xKyjh}4Mv}m8a(&#C&7c9aJ@(lE~@ilR$y0DH3axxISC0Xn#Ov=F*a0!ct-Dx-U`fEke>DhOEW3JVUZ6!|Lnm4I$3t#Z=XE`ZSxh zm3-^E?Uw~Vm*UM4YDa^$=DhQH7bxl}lh;)?2b3BS3m5_iRA455Aatw;iAV)Q_OMVF&U!2y9(qS zYlmeJxATupeSB4OQ2W=yaC3@ANWhBxEl-&J69!(X4GgnB-6aMX`XQt~0kSkXecsmsyjs$!L8OGkhf0Hf%aPMl zD{ANyY|Al#%RIv$36(Bg$B(`18-Y!%v>Af@%R;bvY4tZ0r1!~fWn@V+(RO_TmR?iD zeyfAp1HxO$WP zl!;n=Dw#or4%2@S&#rYsk$;^3FWNG4e>9FyMbUeNgN|HFh13H!PwR2+Ux4axk)HWh zsN+HAmSsm)pRwUl;;6r5yp)@4cMO8%@h0{N5EfxW^RZu&JX_5;>R&tLQn3bI>3gSU zx(pIQACp$rhoAA5$`3dF{V-744DeVdLglb-%Ff`1q!=;F2B4N zAalV!cibdzxvp>D)U&Kq`4gzy&qn!%N)S-(_zAGhocQ{=?v-oG{}QJ)@t!hh)@A1HDB zwsS#`nsu8ehDvxnN!=82%2#d_BWOSJg0+%MVi&T5qenQtA78O7 zJycntRa2Wc<$q^-z@^-;yL&a^eG<$iw%)_+CP7apkO}{>^j~)X`=kjbT*^rZnz{>r_RUZFe zjN6(bKcx1%Oi%W5u+w?hd}rzQmRscoK0%hhpj!WVUwXn;IPv)mm3;0AXxw^JwKra! zs-2hz+|8-{1|JD=(158;P?Ujllg6*E<52rT#AY1@d`8aaaO_&2GiVtF-(lhovitt* zhPsL8_$=aNPR+!Wc=zx_J*1+6~naSTVtHoazUJjNpoX%St`WnKm`4pVwLW`n;?T3c0;fG=(?IE}~2olmZsPkdaJkSNoeP3_*bEUwM)zmQd*+ zKid8gFrp$H?r@}k2V4`quNh2umwn|n<3s1n6vp->MB_UT)w16tG|Bh|4$j{1Nf3Zp zMJdo(nF5&WAw(KvK@i9#V9o3TxAfkB(z62p4vX6MKf4#tCOWT<=h07;e4^uJ?i4UU zGb)~IIHceEmAK2LQV3O#LKgQU(I33Wbd5*10AX0GH-Jfk0Zru#zr{k)@(v6n?=2jZ z%X$%&=(sG2Kza&*0V;zbQoi>f((y}ZQ`TwT=}@iW--+U?&1WB^{_Qf6$krP(F@9BJurDUl*jMLXZ?Q?Zu6b+J5VsS^y}U-2Q_)7IJgf>me$GG68qo?T;^s4K z?!YL!vdOL9J7vNN%J>crEk2IP=^8Vlw8{0a+b?(fEbjV1iqzugSy^4}BYaFNLDbLl zP*Jw{vV^Fu93ly^lFnjWY`%)ug|c_8=^q`>eLvH5il9uG`(B1<8|d6j3PiImbJp{t zj)SPO{Lh0Pv>)QBj2!L@kf5Yjc&8fR|4@DCIaEoOw^`V8N)IwDJlqX(FkLSbu=ge` zM%pe*3Xi#VT!LCbR?}F~6t9Z!(?*5GPFD+R2wm3{V6LBTXATKr)Y0@mqtmT2-ruCV z=LfK6OP{$vuPx86qiF{axHcp2muIAr(9rC~?v<9QQj*hTr+FNNaZdx85ngG@-~ zQVc>dnsnc%f{zO2M?H(woyboUl9kwp6+AA@K>84c7vpi^X;puE2>ap2!^_n>be=5F z8<(_g&y-e&@H+O<8d!6w@b~Umb{7ZLzU0y zq*Q=E@dMCI3Q9!KIKY3nel^^g1E5qjxnTUx=u8p@b2tgw;znVGydDle>Ex$? zXW{9oy|+3l-qHAiDYhLX9@O@qIIg($;e`eaX>D``pCvG8gj$348y?5nE}-9Q+~ z@>iMyLT~u$=W`6|?e*&V`T@qyPb{thl3};4&(tUS^wqxN44MYQ)I%gG>xX12^ z+nL_=hS~NJs|sj_IbR2RhQP3!9YV8KthUw4fW%(>fq1T?k98}C4liPr~?My-k>tz0^djUqL%L_}r|mf(qRF)kbw9f$()!#W8l=PxH& z3e(&ur{X?P+WHJ7y1#_sw2YJo65Rp|@S8)Qr^fd&B|jFoEl(4tOXi$n*-g9c;afC^ zCVI;Q+?D8?0)Spb-C&tQ4Fkay$q!`S#Yq+nr~IWGJMI6e*#U3{iLYK|7VMASB9yZ_ z`YJ1vAJF@&2kkgSq-jClFD~EDGV8jPI$uDPt+F>Zl`ow^JJQY`05Ko2E#j z@XYmr(DD)_nPkGsy#1#A+{_O~a52=F%>3YSAL#>;2OU2wbWrHx6d~{!j6|3`dV+I; zN5W|OUBGi{6;Lf{*YJjVifO`qYtXx->#CYp!1Sb6rdE>wNd>$;t~#0GwLCD_{z{>V zz(ULt6=bfYPciJ{jc)^7z5$mad%_T#DzPsdWAM?))MHN@$B{ZB1E;JtmbR9LhHCx@wnCILz4V-~R?ES*_Y}XqDUZFFWRCiZtvbDz ziARdLGtJCb|M6A%^Ls9G-(q;P+x`htrH-`}&gSHx<_oq_Y1e}n-;2+e!0~^xJO6E! zC)}rFOO}=1rTaf%!>l=1XV*{DQk>#p&9=@rlfF#()q{DINL4VqBkHbT$O>xfE~f91 zZ7ZB(YPPJe?FAyq3FAV`YeVgyN{##^l2y!dX@iC&4aVLZnk(~khsI0#NfRC&)T2|x zVSq&Q5`v?&T(3;w4hrUO=v*7IQ8Jr&^mf^&i`LmDGto-_TVf2Qt#*s7)K2Y&!r6pd znadh9rHO~YtrW~{y=_Bpt3q7r`_64>An!d{w5Zp2x?9m~SY0tjsotNS|A`7|c?id5 zV@bV$5M|A5+>53*!G(c6R@4o#EB%%qA1xEjT5E<^8V8B`Jth{P8pG7){58op`$u`Y zl(`XR9&f4*{X``F`EKil6yMR-Z~-1%d|SxM3xO@U9zDb7I=Q7&9i$3aM@v?mFnFux z%#`t5%(#kIya?DqccpauPT8&$`2sML{nVNv^p8Ie?XGrZlaOs(jJ+` z`lFK>ax^ORCbZUuNt6^dJAlcm&C>>KYkuk{IzetW2d#9(r#z(J?qhpcLHmZPobqkx zu|dh60!QX-FUoR1XTcj?D_&rBX&D!E&P7x)*G{elXy|AsQ@4GiSfOTBNM7)rU8@uq z<&BfaphoUaZ=>~)v- zuKe^xmG5o%uK9;rXLl_>l9Zlm^mh?3>C{i$HRJt_`Fy=DmGtbswnm@m2MrDrB$|4; zOv9D+8_(j;8*ndIXgGuZ>|*~Mv=L;$P1jd)CfHg_j;8w(mO9$+H#qPsjItK`+AV9J zO}T0AeW>_k;L_H2LrvpzYy^>2dODnUrWfaxw_48CMUF3AeUInCPE%oK;e9SuL`V5K7?MYnhgLor@|tzXGZP%5@`?pU|w8dblm z&#;mNPeIf!%5Bugs>U>5IbK?l9}uUgx77)fV&8RFx64*K01Rx3#w0%&nj~Mh+opyY z$Vc@G#((78PyI5Mo2MW0OuSekWl>bRs!jF%{tyoY!eC^76TERgP9ri_hX{EnZe zUW5YLFQXlT%Xgd!FP~TM9U4D{@Ujxuy^0o;GtdPL;!IPXK#dU*JmNs{LrGNjZ?`Rb z)JMl6W_A~jCGXyHxO%pxy?JEKsc`pBQhO(fF~RwBwA!c#WVp6w7 zi$o#j(hb}9FXR%y}}&sbe=ZlpbDFqFr;Mq%soZLbf{hrg&= z{lcmLw4iEPIAr_zQ`X`}nLl(7%j}V?%&qG){Oo@Ssa`cdUY=D<7BhRJr73ISX0vEO z3^-I-o%UE}9_Nq0!EKL;h+2J?Kr_YDb7HS+SMh@dzsuRTOJJp+F^`k%rd~5}=lPH% z>8BuvqwJ=z%}(&(vYZ`ca&DOPZ(Lp(h%wtLCR)b3APl zWK7u?GQDRfn@*l9_M12>%Y9!y3B|F9ktnvz8`ZYhx-ktxv*hhqF)3$k4>8%T^X`JY z7@l{LNjoh_#%h~bmg)#q@wG`vo3+{9K(FA$fI#I_+J0Y1LKwHr%FB7;{RGS-R=z}4 zQm|9=`TIeJc{AXrrP!k%Is}XC7gbjv>V+G=zN}+h>vscDpmzj@liTtl#&Be@fu#gNLijQ|5Pxc&gLH)f)W{-e`^ZxAkp#~hT=Lim;*b{k7`(UVQwuE8spq z*WNMMUZ_nW*bNyeW74d=B_kTfgMZD7eR9bd22r_D^`rkmkBU9{ILl4MJWWp?kM~4$}oU%Kcpu zqwHxFVGBcRal6=?^$9DA<{AX)cD>c_#M}~3-b&1N+YcoB+0w?>?-$Yu8m+TU)*AyR|6b3k{w06#papuTW$ zimQ~_b}ui_j#&FWlF+sneNC{;9U4>_DZb0M)v5l5iiboP=e0$iSDbC(~AEOC8$K zTIeymwey)Wf$Gk({30dufC^9Q$GvVXccgi2;ZaKM5Y(qTv`4~kxYWL99F8_#ii9h< z1X4b9@PZ*CxL-xBryl`Xct zyWld`@4BFJL?XUh=S+rE13YzHsd3LWRsPjNp@?P-N&@z=cimO*$QQaKfB2~fSK|;c|pHrIed+7vb`x4`tj#GaM zZ1fzL_O1kF%`$4j_t7K4j8!;)d`u59t&TbvextZE@#u*ur=GI;I(WTaUg7G}f9#k} z8ZF`cv>r`p>*2Dl2BoxlZB#GF`D(t9!$#^9Kb`-}v|Y)WA%V0@R&%_0$!1N4$hbR) zs__GdgAJrnJIv@X#kuQSm5|$-+_~0F(JL>^&XqE+Hoi~B4hyVQ{N%k%P$LajV93{@ zt?xR_9%hu*!Pp%4^#nwH|Nb88s;2Am?YQZ*#HA*bSBpDyDJ-6sq4A|E8KyEWNMv$6 z%H%jQ#5LY9grA)+9rQnVfIW=2@n$3ZxxX6u&o3IJN{njtUC6y8OVs$Y_ZcEO`LsiF zviTEP&gQU0z}Lh0_WP&C7CpzU%=bB!4|#fVD1;YL%J!B9-fp@s(vHK&qnE0S19g>c z;r_1rrB=N)d)P|Zfv-PBTqu8Q@-bIPW7R?hdJzSPH%|;LVriSRVntr!R;*}4CF@wD zz~MLEx=@g~dfWQC389&dR>4{PQbBIqAq&lh?}S@RllQkDcnPHO_eUJ(RGvi5+P^P; zv9ZnxdCM!$;Q6G1@4*o}l1PJuZrHsX88j7N>GXWZsnA-pVw^JLDK99fMqu>VIA zwJ<;b=d1Dvq7S2&8Yv+Muq**A*dMdA1mnY1MRvm4Y{L(LljXw-8){`{KyO1fAeIHo zHf?-=J8q$sIpLMr>dGmcicf9P`FpdGatZeRVOx%h(LWd_z&iDu|hu!V1XdSV(2DV&oEiSv+P|Cc}*s%L*y5mw@Th7W09Pc3V z=yEE&xK#WZvQ6he+LW+es#rFJ9)02$!&dSJ?shtI72?hlZ$-E7U|`m9l1(M#_pts7 zz%!-3j03e1{VmBG39Q+z&|O!+m#BdimNR|fT~B&Lrmf}_*B3QY!Xv82FoWF0(%iOk zT|%>y(wYIuft()jiz%P>Odccr{zlNvQ2;v}B4*h*26RvFDcN6tu(Mk}?>OsC1-iKWA^D*8RFbQpvlQ<5IXzo|*@E>H{dj%q_8A zl>$@Wc6a6C$j$cdp0;)G%dz<#X2m4g(Y+PeokH;F*-=OxtlBAg3TofBN@8GrSG+l% z&nqk~eSUr}!In>&{8dC8%$q>+u;m)miIr39M}NHfJ}Ljx{PwwyL%^wiwSP$7U}1Rn zbcv_y*%IpluyioYhE#`RWC!^+?>_m54GS2rf5hyW9EGR8qrp^Uql4&fhgFUoJ5r%v z=bY(bTKwtig0&8$X9vuK`fV?Qi)ABNjDZhMs4g+!UZ%1I^Xf1$ZsC+|B1 z#p(0hDK}rD0wAtbqes|zWR9ve+!`N1_R6zqLp5+u8};DfNwDrr4I1&!Zu^`LtIfZY zI-lN!t)IWL#)l^!lt%F{!Aevu#(eIr|ICV8;~im=sUVicU$=}LURMRba4bv*6l>{H%H;YyBE)7X13I#aeUc-Em@ZP?n0i#g#O0LQ{2bS zV%c~Tf29=f+w92D$*w-0?{<`+W-iBnUH)o1_=j`?<4M|QREGX9O8+unaertI++t;J z+C`(fg;S$>#8FYS2I<4-qUPn38Qi+wxOJ$?@<%y}p0?)-2G2XifVW~KzM3_I)62V4~_t~p_E$ae{D`n~eoP2)5>>wZJHz^Q5r?q=W0 zz_Nv1`ttM6kUfj^6KtIVp}GDi)YR1M5xz0$V^9-)qAm91)#aIi=vm(7Tg_Rm6HcIR?4zMJ5q=_=-FAb@OCY*i6Ab zTa~^)htc)hL+oAd{Q>v@UbLk{8);MmB!`*b{wS7BH_!inUE_RKH;H2KNMdl$eclub P_;*4>Pd)RvdBFbxXl)NL literal 0 HcmV?d00001 diff --git a/rclcpp/doc/proposed_node_parameter_callbacks.md b/rclcpp/doc/proposed_node_parameter_callbacks.md new file mode 100644 index 0000000000..cd39f352db --- /dev/null +++ b/rclcpp/doc/proposed_node_parameter_callbacks.md @@ -0,0 +1,29 @@ +# Proposed node parameters callback Design + +## Introduction: + +The original requirement came in **gazebo_ros_pkgs** for setting individual wheel slip parameters based on global wheel slip value [link to original issue](https://github.com/ros-simulation/gazebo_ros_pkgs/pull/1365). + +The main requirement is to set one or more parameters after another parameter is set successfully. + +Additionally, it would be nice if users could be notified locally (via a callback) when parameters have been set successfully (i.e. post validation). + +Related discussion can be found in [#609](https://github.com/ros2/rclcpp/issues/609) [#1789](https://github.com/ros2/rclcpp/pull/1789) + +With the current parameters API, the `add_on_set_parameters_callback` is intended for validation of parameter values before they are set, it should **not** cause any side-effects. + +There is also the `ParameterEventHandler` that publishes changes to node parameters on `/parameter_events` topic for external nodes to see. Though the node could subscribe to the `/parameter_events` topic to be notified of changes to its own parameters, it is less than ideal since there is a delay caused by waiting for an executor to process the callback. + +We propose adding a `PostSetParametersCallbackHandle` for successful parameter set similar to `OnSetParametersCallbackHandle` for parameter validation. Also, we propose adding a `PreSetParametersCallbackHandle` useful for modifying list of parameters being set. + +The validation callback is often abused to trigger side effects in the code, for instance updating class attributes even before a parameter has been set successfully. Instead of relying on the `/parameter_events` topic to be notified of parameter changes, users can register a callback with a new API, `add_post_set_parameters_callback`. + +It is possible to use the proposed `add_post_set_parameters_callback` for setting additional parameters, but this might result in infinite recursion and does not allow those additional parameters to be set atomically with the original parameter(s) changed. +To workaround these issues, we propose adding a "pre set" callback type that can be registered with `add_pre_set_parameters_callback`, which will be triggered before the validation callbacks and can be used to modify the parameter list. + +![Desgin API](https://github.com/ros2/rclcpp/blob/deepanshu/local-param-changed-callback-support/rclcpp/doc/param_callback_design.png?raw=true) + +## Alternatives + +* Users could call `set_parameter` while processing a message from the `/parameter_events` topic, however, there is extra overhead in having to create subscription (as noted earlier). +* Users could call `set_parameter` inside the "on set" parameters callback, however it is not well-defined how side-effects should handle cases where parameter validation fails. \ No newline at end of file diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 54a4b922ab..bd5d39f6bb 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -370,11 +370,21 @@ class Node : public std::enable_shared_from_this * * If `ignore_override` is `true`, the parameter override will be ignored. * - * This method, if successful, will result in any callback registered with - * add_on_set_parameters_callback to be called. + * This method will result in any callback registered with + * `add_on_set_parameters_callback` and `add_post_set_parameters_callback` + * to be called for the parameter being set. + * + * If a callback was registered previously with `add_on_set_parameters_callback`, + * it will be called prior to setting the parameter for the node. * If that callback prevents the initial value for the parameter from being * set then rclcpp::exceptions::InvalidParameterValueException is thrown. * + * If a callback was registered previously with `add_post_set_parameters_callback`, + * it will be called after setting the parameter successfully for the node. + * + * This method will _not_ result in any callbacks registered with + * `add_pre_set_parameters_callback` to be called. + * * The returned reference will remain valid until the parameter is * undeclared. * @@ -491,11 +501,22 @@ class Node : public std::enable_shared_from_this * If `ignore_overrides` is `true`, all the overrides of the parameters declared * by the function call will be ignored. * + * This method will result in any callback registered with + * `add_on_set_parameters_callback` and `add_post_set_parameters_callback` + * to be called once for each parameter. + * * This method, if successful, will result in any callback registered with - * add_on_set_parameters_callback to be called, once for each parameter. + * `add_on_set_parameters_callback` to be called, once for each parameter. * If that callback prevents the initial value for any parameter from being * set then rclcpp::exceptions::InvalidParameterValueException is thrown. * + * If a callback was registered previously with `add_post_set_parameters_callback`, + * it will be called after setting the parameters successfully for the node, + * once for each parameter. + * + * This method will _not_ result in any callbacks registered with + * `add_pre_set_parameters_callback` to be called. + * * \param[in] namespace_ The namespace in which to declare the parameters. * \param[in] parameters The parameters to set in the given namespace. * \param[in] ignore_overrides When `true`, the parameters overrides are ignored. @@ -533,8 +554,9 @@ class Node : public std::enable_shared_from_this /// Undeclare a previously declared parameter. /** - * This method will not cause a callback registered with - * add_on_set_parameters_callback to be called. + * This method will _not_ cause a callback registered with any of the + * `add_pre_set_parameters_callback`, `add_on_set_parameters_callback` and + * `add_post_set_parameters_callback` to be called. * * \param[in] name The name of the parameter to be undeclared. * \throws rclcpp::exceptions::ParameterNotDeclaredException if the parameter @@ -568,11 +590,24 @@ class Node : public std::enable_shared_from_this * Parameter overrides are ignored by set_parameter. * * This method will result in any callback registered with - * add_on_set_parameters_callback to be called. + * `add_pre_set_parameters_callback`, add_on_set_parameters_callback` and + * `add_post_set_parameters_callback` to be called once for the parameter + * being set. + * + * This method will result in any callback registered with + * `add_on_set_parameters_callback` to be called. * If the callback prevents the parameter from being set, then it will be * reflected in the SetParametersResult that is returned, but no exception * will be thrown. * + * If a callback was registered previously with `add_pre_set_parameters_callback`, + * it will be called once prior to the validation of the parameter for the node. + * If this callback makes modified parameter list empty, then it will be reflected + * in the returned result; no exceptions will be raised in this case. + * + * If a callback was registered previously with `add_post_set_parameters_callback`, + * it will be called once after setting the parameter successfully for the node. + * * If the value type of the parameter is rclcpp::PARAMETER_NOT_SET, and the * existing parameter type is something else, then the parameter will be * implicitly undeclared. @@ -609,11 +644,25 @@ class Node : public std::enable_shared_from_this * corresponding SetParametersResult in the vector returned by this function. * * This method will result in any callback registered with - * add_on_set_parameters_callback to be called, once for each parameter. + * `add_pre_set_parameters_callback`, `add_on_set_parameters_callback` and + * `add_post_set_parameters_callback` to be called once for each parameter. + + * If a callback was registered previously with `add_pre_set_parameters_callback`, + * it will be called prior to the validation of parameters for the node, + * once for each parameter. + * If this callback makes modified parameter list empty, then it will be reflected + * in the returned result; no exceptions will be raised in this case. + * + * This method will result in any callback registered with + * `add_on_set_parameters_callback` to be called, once for each parameter. * If the callback prevents the parameter from being set, then, as mentioned * before, it will be reflected in the corresponding SetParametersResult * that is returned, but no exception will be thrown. * + * If a callback was registered previously with `add_post_set_parameters_callback`, + * it will be called after setting the parameters successfully for the node, + * once for each parameter. + * * Like set_parameter() this method will implicitly undeclare parameters * with the type rclcpp::PARAMETER_NOT_SET. * @@ -640,11 +689,25 @@ class Node : public std::enable_shared_from_this * If the exception is thrown then none of the parameters will have been set. * * This method will result in any callback registered with - * add_on_set_parameters_callback to be called, just one time. + * `add_pre_set_parameters_callback`, `add_on_set_parameters_callback` and + * `add_post_set_parameters_callback` to be called only 'once' for all parameters. + * + * If a callback was registered previously with `add_pre_set_parameters_callback`, + * it will be called prior to the validation of node parameters, just one time + * for all parameters. + * If this callback makes modified parameter list empty, then it will be reflected + * in the returned result; no exceptions will be raised in this case. + * + * This method will result in any callback registered with + * 'add_on_set_parameters_callback' to be called, just one time. * If the callback prevents the parameters from being set, then it will be * reflected in the SetParametersResult which is returned, but no exception * will be thrown. * + * If a callback was registered previously with `add_post_set_parameters_callback`, + * it will be called after setting the node parameters successfully, just one time + * for all parameters. + * * If you pass multiple rclcpp::Parameter instances with the same name, then * only the last one in the vector (forward iteration) will be set. * @@ -893,12 +956,82 @@ class Node : public std::enable_shared_from_this rcl_interfaces::msg::ListParametersResult list_parameters(const std::vector & prefixes, uint64_t depth) const; + using PreSetParametersCallbackHandle = + rclcpp::node_interfaces::PreSetParametersCallbackHandle; + using PreSetParametersCallbackType = + rclcpp::node_interfaces::NodeParametersInterface::PreSetParametersCallbackType; + using OnSetParametersCallbackHandle = rclcpp::node_interfaces::OnSetParametersCallbackHandle; - using OnParametersSetCallbackType = - rclcpp::node_interfaces::NodeParametersInterface::OnParametersSetCallbackType; + using OnSetParametersCallbackType = + rclcpp::node_interfaces::NodeParametersInterface::OnSetParametersCallbackType; + using OnParametersSetCallbackType [[deprecated("use OnSetParametersCallbackType instead")]] = + OnSetParametersCallbackType; + + using PostSetParametersCallbackHandle = + rclcpp::node_interfaces::PostSetParametersCallbackHandle; + using PostSetParametersCallbackType = + rclcpp::node_interfaces::NodeParametersInterface::PostSetParametersCallbackType; - /// Add a callback for when parameters are being set. + /// Add a callback that gets triggered before parameters are validated. + /** + * This callback can be used to modify the original list of parameters being + * set by the user. + * + * The modified list of parameters is then forwarded to the "on set parameter" + * callback for validation. + * + * The callback is called whenever any of the `set_parameter*` methods are called + * or when a set parameter service request is received. + * + * The callback takes a reference to the vector of parameters to be set. + * + * The vector of parameters may be modified by the callback. + * + * One of the use case of "pre set callback" can be updating additional parameters + * conditioned on changes to a parameter. + * + * For an example callback: + * + *```cpp + * void + * preSetParameterCallback(std::vector & parameters) + * { + * for (auto & param : parameters) { + * if (param.get_name() == "param1") { + * parameters.push_back(rclcpp::Parameter("param2", 4.0)); + * } + * } + * } + * ``` + * The above callback appends 'param2' to the list of parameters to be set if + * 'param1' is being set by the user. + * + * All parameters in the vector will be set atomically. + * + * Note that the callback is only called while setting parameters with `set_parameter`, + * `set_parameters`, `set_parameters_atomically`, or externally with a parameters service. + * + * The callback is not called when parameters are declared with `declare_parameter` + * or `declare_parameters`. + * + * The callback is not called when parameters are undeclared with `undeclare_parameter`. + * + * An empty modified parameter list from the callback will result in "set_parameter*" + * returning an unsuccessful result. + * + * The `remove_pre_set_parameters_callback` can be used to deregister the callback. + * + * \param callback The callback to register. + * \returns A shared pointer. The callback is valid as long as the smart pointer is alive. + * \throws std::bad_alloc if the allocation of the PreSetParametersCallbackHandle fails. + */ + RCLCPP_PUBLIC + RCUTILS_WARN_UNUSED + PreSetParametersCallbackHandle::SharedPtr + add_pre_set_parameters_callback(PreSetParametersCallbackType callback); + + /// Add a callback to validate parameters before they are set. /** * The callback signature is designed to allow handling of any of the above * `set_parameter*` or `declare_parameter*` methods, and so it takes a const @@ -937,6 +1070,8 @@ class Node : public std::enable_shared_from_this * this callback, so when checking a new value against the existing one, you * must account for the case where the parameter is not yet set. * + * The callback is not called when parameters are undeclared with `undeclare_parameter`. + * * Some constraints like read_only are enforced before the callback is called. * * The callback may introspect other already set parameters (by calling any @@ -965,7 +1100,77 @@ class Node : public std::enable_shared_from_this RCLCPP_PUBLIC RCUTILS_WARN_UNUSED OnSetParametersCallbackHandle::SharedPtr - add_on_set_parameters_callback(OnParametersSetCallbackType callback); + add_on_set_parameters_callback(OnSetParametersCallbackType callback); + + /// Add a callback that gets triggered after parameters are set successfully. + /** + * The callback is called when any of the `set_parameter*` or `declare_parameter*` + * methods are successful. + * + * The callback takes a reference to a const vector of parameters that have been + * set successfully. + * + * The post callback can be valuable as a place to cause side-effects based on + * parameter changes. + * For instance updating internally tracked class attributes once parameters + * have been changed successfully. + * + * For an example callback: + * + * ```cpp + * void + * postSetParameterCallback(const std::vector & parameters) + * { + * for(const auto & param:parameters) { + * // the internal class member can be changed after + * // successful change to param1 or param2 + * if(param.get_name() == "param1") { + * internal_tracked_class_parameter_1_ = param.get_value(); + * } + * else if(param.get_name() == "param2") { + * internal_tracked_class_parameter_2_ = param.get_value(); + * } + * } + * } + * ``` + * + * The above callback takes a const reference to list of parameters that have been + * set successfully and as a result of this updates the internally tracked class attributes + * `internal_tracked_class_parameter_1_` and `internal_tracked_class_parameter_2_` + * respectively. + * + * This callback should not modify parameters. + * + * The callback is called when parameters are declared with `declare_parameter` + * or `declare_parameters`. See `declare_parameter` or `declare_parameters` above. + * + * The callback is not called when parameters are undeclared with `undeclare_parameter`. + * + * If you want to make changes to parameters based on changes to another, use + * `add_pre_set_parameters_callback`. + * + * The `remove_post_set_parameters_callback` can be used to deregister the callback. + * + * \param callback The callback to register. + * \returns A shared pointer. The callback is valid as long as the smart pointer is alive. + * \throws std::bad_alloc if the allocation of the OnSetParametersCallbackHandle fails. + */ + RCLCPP_PUBLIC + RCUTILS_WARN_UNUSED + PostSetParametersCallbackHandle::SharedPtr + add_post_set_parameters_callback(PostSetParametersCallbackType callback); + + /// Remove a callback registered with `add_pre_set_parameters_callback`. + /** + * Delete a handler returned by `add_pre_set_parameters_callback`. + * + * \param handler The callback handler to remove. + * \throws std::runtime_error if the handler was not created with `add_pre_set_parameters_callback`, + * or if it has been removed before. + */ + RCLCPP_PUBLIC + void + remove_pre_set_parameters_callback(const PreSetParametersCallbackHandle * const handler); /// Remove a callback registered with `add_on_set_parameters_callback`. /** @@ -994,6 +1199,18 @@ class Node : public std::enable_shared_from_this void remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const handler); + /// Remove a callback registered with `add_post_set_parameters_callback`. + /** + * Delete a handler returned by `add_post_set_parameters_callback`. + * + * \param handler The callback handler to remove. + * \throws std::runtime_error if the handler was not created with `add_post_set_parameters_callback`, + * or if it has been removed before. + */ + RCLCPP_PUBLIC + void + remove_post_set_parameters_callback(const PostSetParametersCallbackHandle * const handler); + /// Get the fully-qualified names of all available nodes. /** * The fully-qualified name includes the local namespace and name of the node. diff --git a/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp b/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp index 7cc130f256..eda68451a9 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp @@ -181,20 +181,43 @@ class NodeParameters : public NodeParametersInterface rcl_interfaces::msg::ListParametersResult list_parameters(const std::vector & prefixes, uint64_t depth) const override; + RCLCPP_PUBLIC + RCUTILS_WARN_UNUSED + PreSetParametersCallbackHandle::SharedPtr + add_pre_set_parameters_callback(PreSetParametersCallbackType callback) override; + RCLCPP_PUBLIC RCUTILS_WARN_UNUSED OnSetParametersCallbackHandle::SharedPtr - add_on_set_parameters_callback(OnParametersSetCallbackType callback) override; + add_on_set_parameters_callback(OnSetParametersCallbackType callback) override; + + RCLCPP_PUBLIC + RCUTILS_WARN_UNUSED + PostSetParametersCallbackHandle::SharedPtr + add_post_set_parameters_callback(PostSetParametersCallbackType callback) override; RCLCPP_PUBLIC void remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const handler) override; + RCLCPP_PUBLIC + void + remove_post_set_parameters_callback(const PostSetParametersCallbackHandle * const handler) + override; + + RCLCPP_PUBLIC + void + remove_pre_set_parameters_callback(const PreSetParametersCallbackHandle * const handler) override; + RCLCPP_PUBLIC const std::map & get_parameter_overrides() const override; - using CallbacksContainerType = std::list; + using PreSetCallbacksHandleContainer = std::list; + using OnSetCallbacksHandleContainer = std::list; + using PostSetCallbacksHandleContainer = std::list; + using CallbacksContainerType [[deprecated("use OnSetCallbacksHandleContainer instead")]] = + OnSetCallbacksHandleContainer; protected: RCLCPP_PUBLIC @@ -211,7 +234,11 @@ class NodeParameters : public NodeParametersInterface // declare_parameter, etc). In those cases, this will be set to false. bool parameter_modification_enabled_{true}; - CallbacksContainerType on_parameters_set_callback_container_; + PreSetCallbacksHandleContainer pre_set_parameters_callback_container_; + + OnSetCallbacksHandleContainer on_set_parameters_callback_container_; + + PostSetCallbacksHandleContainer post_set_parameters_callback_container_; std::map parameters_; diff --git a/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp index 743c1b8d4f..104e00327a 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp @@ -33,16 +33,38 @@ namespace rclcpp namespace node_interfaces { +struct PreSetParametersCallbackHandle +{ + RCLCPP_SMART_PTR_DEFINITIONS(PreSetParametersCallbackHandle) + + using PreSetParametersCallbackType = + std::function &)>; + + PreSetParametersCallbackType callback; +}; + struct OnSetParametersCallbackHandle { RCLCPP_SMART_PTR_DEFINITIONS(OnSetParametersCallbackHandle) - using OnParametersSetCallbackType = + using OnSetParametersCallbackType = std::function< rcl_interfaces::msg::SetParametersResult( const std::vector &)>; + using OnParametersSetCallbackType [[deprecated("use OnSetParametersCallbackType instead")]] = + OnSetParametersCallbackType; + + OnSetParametersCallbackType callback; +}; + +struct PostSetParametersCallbackHandle +{ + RCLCPP_SMART_PTR_DEFINITIONS(PostSetParametersCallbackHandle) + + using PostSetParametersCallbackType = + std::function &)>; - OnParametersSetCallbackType callback; + PostSetParametersCallbackType callback; }; /// Pure virtual interface class for the NodeParameters part of the Node API. @@ -185,16 +207,46 @@ class NodeParametersInterface rcl_interfaces::msg::ListParametersResult list_parameters(const std::vector & prefixes, uint64_t depth) const = 0; - using OnParametersSetCallbackType = OnSetParametersCallbackHandle::OnParametersSetCallbackType; + using OnSetParametersCallbackType = OnSetParametersCallbackHandle::OnSetParametersCallbackType; + using PostSetParametersCallbackType = + PostSetParametersCallbackHandle::PostSetParametersCallbackType; + using PreSetParametersCallbackType = PreSetParametersCallbackHandle::PreSetParametersCallbackType; + + /// Add a callback that gets triggered before parameters are validated. + /** + * \sa rclcpp::Node::add_pre_set_parameters_callback + */ + RCLCPP_PUBLIC + virtual + PreSetParametersCallbackHandle::SharedPtr + add_pre_set_parameters_callback(PreSetParametersCallbackType callback) = 0; - /// Add a callback for when parameters are being set. + /// Add a callback to validate parameters before they are set. /** * \sa rclcpp::Node::add_on_set_parameters_callback */ RCLCPP_PUBLIC virtual OnSetParametersCallbackHandle::SharedPtr - add_on_set_parameters_callback(OnParametersSetCallbackType callback) = 0; + add_on_set_parameters_callback(OnSetParametersCallbackType callback) = 0; + + /// Add a callback that gets triggered after parameters are set successfully. + /** + * \sa rclcpp::Node::add_post_set_parameters_callback + */ + RCLCPP_PUBLIC + virtual + PostSetParametersCallbackHandle::SharedPtr + add_post_set_parameters_callback(PostSetParametersCallbackType callback) = 0; + + /// Remove a callback registered with `add_pre_set_parameters_callback`. + /** + * \sa rclcpp::Node::remove_pre_set_parameters_callback + */ + RCLCPP_PUBLIC + virtual + void + remove_pre_set_parameters_callback(const PreSetParametersCallbackHandle * const handler) = 0; /// Remove a callback registered with `add_on_set_parameters_callback`. /** @@ -205,6 +257,15 @@ class NodeParametersInterface void remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const handler) = 0; + /// Remove a callback registered with `add_post_set_parameters_callback`. + /** + * \sa rclcpp::Node::remove_post_set_parameters_callback + */ + RCLCPP_PUBLIC + virtual + void + remove_post_set_parameters_callback(const PostSetParametersCallbackHandle * const handler) = 0; + /// Return the initial parameter values used by the NodeParameters to override default values. RCLCPP_PUBLIC virtual diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index 8107458f43..7dba3e51ad 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -353,7 +353,7 @@ Node::has_parameter(const std::string & name) const rcl_interfaces::msg::SetParametersResult Node::set_parameter(const rclcpp::Parameter & parameter) { - return this->set_parameters_atomically({parameter}); + return node_parameters_->set_parameters_atomically({parameter}); } std::vector @@ -418,16 +418,40 @@ Node::list_parameters(const std::vector & prefixes, uint64_t depth) return node_parameters_->list_parameters(prefixes, depth); } +rclcpp::Node::PreSetParametersCallbackHandle::SharedPtr +Node::add_pre_set_parameters_callback(PreSetParametersCallbackType callback) +{ + return node_parameters_->add_pre_set_parameters_callback(callback); +} + rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr -Node::add_on_set_parameters_callback(OnParametersSetCallbackType callback) +Node::add_on_set_parameters_callback(OnSetParametersCallbackType callback) { return node_parameters_->add_on_set_parameters_callback(callback); } +rclcpp::Node::PostSetParametersCallbackHandle::SharedPtr +Node::add_post_set_parameters_callback(PostSetParametersCallbackType callback) +{ + return node_parameters_->add_post_set_parameters_callback(callback); +} + +void +Node::remove_pre_set_parameters_callback(const PreSetParametersCallbackHandle * const handler) +{ + node_parameters_->remove_pre_set_parameters_callback(handler); +} + +void +Node::remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const handler) +{ + node_parameters_->remove_on_set_parameters_callback(handler); +} + void -Node::remove_on_set_parameters_callback(const OnSetParametersCallbackHandle * const callback) +Node::remove_post_set_parameters_callback(const PostSetParametersCallbackHandle * const handler) { - return node_parameters_->remove_on_set_parameters_callback(callback); + node_parameters_->remove_post_set_parameters_callback(handler); } std::vector diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index 0f366792b7..c17eb887b7 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -305,18 +305,54 @@ __check_parameters( return result; } -using OnParametersSetCallbackType = - rclcpp::node_interfaces::NodeParametersInterface::OnParametersSetCallbackType; -using CallbacksContainerType = - rclcpp::node_interfaces::NodeParameters::CallbacksContainerType; +using PreSetParametersCallbackType = + rclcpp::node_interfaces::NodeParametersInterface::PreSetParametersCallbackType; +using PreSetParametersCallbackHandle = + rclcpp::node_interfaces::PreSetParametersCallbackHandle; +using PreSetCallbacksHandleContainer = + rclcpp::node_interfaces::NodeParameters::PreSetCallbacksHandleContainer; + +using OnSetParametersCallbackType = + rclcpp::node_interfaces::NodeParametersInterface::OnSetParametersCallbackType; using OnSetParametersCallbackHandle = rclcpp::node_interfaces::OnSetParametersCallbackHandle; +using OnSetCallbacksHandleContainer = + rclcpp::node_interfaces::NodeParameters::OnSetCallbacksHandleContainer; + +using PostSetParametersCallbackType = + rclcpp::node_interfaces::NodeParametersInterface::PostSetParametersCallbackType; +using PostSetParametersCallbackHandle = + rclcpp::node_interfaces::PostSetParametersCallbackHandle; +using PostSetCallbacksHandleContainer = + rclcpp::node_interfaces::NodeParameters::PostSetCallbacksHandleContainer; + +RCLCPP_LOCAL +void +__call_pre_set_parameters_callbacks( + std::vector & parameters, + PreSetCallbacksHandleContainer & callback_container) +{ + if (callback_container.empty()) { + return; + } + + auto it = callback_container.begin(); + while (it != callback_container.end()) { + auto shared_handle = it->lock(); + if (nullptr != shared_handle) { + shared_handle->callback(parameters); + it++; + } else { + it = callback_container.erase(it); + } + } +} RCLCPP_LOCAL rcl_interfaces::msg::SetParametersResult -__call_on_parameters_set_callbacks( +__call_on_set_parameters_callbacks( const std::vector & parameters, - CallbacksContainerType & callback_container) + OnSetCallbacksHandleContainer & callback_container) { rcl_interfaces::msg::SetParametersResult result; result.successful = true; @@ -336,12 +372,34 @@ __call_on_parameters_set_callbacks( return result; } +RCLCPP_LOCAL +void __call_post_set_parameters_callbacks( + const std::vector & parameters, + PostSetCallbacksHandleContainer & callback_container) +{ + if (callback_container.empty()) { + return; + } + + auto it = callback_container.begin(); + while (it != callback_container.end()) { + auto shared_handle = it->lock(); + if (nullptr != shared_handle) { + shared_handle->callback(parameters); + it++; + } else { + it = callback_container.erase(it); + } + } +} + RCLCPP_LOCAL rcl_interfaces::msg::SetParametersResult __set_parameters_atomically_common( const std::vector & parameters, std::map & parameter_infos, - CallbacksContainerType & callback_container, + OnSetCallbacksHandleContainer & on_set_callback_container, + PostSetCallbacksHandleContainer & post_set_callback_container, bool allow_undeclared = false) { // Check if the value being set complies with the descriptor. @@ -352,7 +410,7 @@ __set_parameters_atomically_common( } // Call the user callbacks to see if the new value(s) are allowed. result = - __call_on_parameters_set_callbacks(parameters, callback_container); + __call_on_set_parameters_callbacks(parameters, on_set_callback_container); if (!result.successful) { return result; } @@ -364,6 +422,8 @@ __set_parameters_atomically_common( parameter_infos[name].descriptor.type = parameters[i].get_type(); parameter_infos[name].value = parameters[i].get_parameter_value(); } + // Call the user post set parameter callback + __call_post_set_parameters_callbacks(parameters, post_set_callback_container); } // Either way, return the result. @@ -378,7 +438,8 @@ __declare_parameter_common( const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor, std::map & parameters_out, const std::map & overrides, - CallbacksContainerType & callback_container, + OnSetCallbacksHandleContainer & on_set_callback_container, + PostSetCallbacksHandleContainer & post_set_callback_container, rcl_interfaces::msg::ParameterEvent * parameter_event_out, bool ignore_override = false) { @@ -414,7 +475,9 @@ __declare_parameter_common( auto result = __set_parameters_atomically_common( parameter_wrappers, parameter_infos, - callback_container); + on_set_callback_container, + post_set_callback_container + ); if (!result.successful) { return result; @@ -441,7 +504,8 @@ declare_parameter_helper( bool ignore_override, std::map & parameters, const std::map & overrides, - CallbacksContainerType & callback_container, + OnSetCallbacksHandleContainer & on_set_callback_container, + PostSetCallbacksHandleContainer & post_set_callback_container, rclcpp::Publisher * events_publisher, const std::string & combined_name, rclcpp::node_interfaces::NodeClockInterface & node_clock) @@ -477,7 +541,8 @@ declare_parameter_helper( parameter_descriptor, parameters, overrides, - callback_container, + on_set_callback_container, + post_set_callback_container, ¶meter_event, ignore_override); @@ -524,7 +589,8 @@ NodeParameters::declare_parameter( ignore_override, parameters_, parameter_overrides_, - on_parameters_set_callback_container_, + on_set_parameters_callback_container_, + post_set_parameters_callback_container_, events_publisher_.get(), combined_name_, *node_clock_); @@ -559,7 +625,8 @@ NodeParameters::declare_parameter( ignore_override, parameters_, parameter_overrides_, - on_parameters_set_callback_container_, + on_set_parameters_callback_container_, + post_set_parameters_callback_container_, events_publisher_.get(), combined_name_, *node_clock_); @@ -633,12 +700,27 @@ NodeParameters::set_parameters_atomically(const std::vector & rcl_interfaces::msg::SetParametersResult result; + // call any user registered pre set parameter callbacks + // this callback can make changes to the original parameters list + // also check if the changed parameter list is empty or not, if empty return + std::vector parameters_after_pre_set_callback(parameters); + __call_pre_set_parameters_callbacks( + parameters_after_pre_set_callback, + pre_set_parameters_callback_container_); + + if (parameters_after_pre_set_callback.empty()) { + result.successful = false; + result.reason = "parameter list cannot be empty, this might be due to " + "pre_set_parameters_callback modifying the original parameters list."; + return result; + } + // Check if any of the parameters are read-only, or if any parameters are not // declared. // If not declared, keep track of them in order to declare them later, when // undeclared parameters are allowed, and if they're not allowed, fail. std::vector parameters_to_be_declared; - for (const auto & parameter : parameters) { + for (const auto & parameter : parameters_after_pre_set_callback) { const std::string & name = parameter.get_name(); // Check to make sure the parameter name is valid. @@ -678,7 +760,8 @@ NodeParameters::set_parameters_atomically(const std::vector & std::map staged_parameter_changes; rcl_interfaces::msg::ParameterEvent parameter_event_msg; parameter_event_msg.node = combined_name_; - CallbacksContainerType empty_callback_container; + OnSetCallbacksHandleContainer empty_on_set_callback_container; + PostSetCallbacksHandleContainer empty_post_set_callback_container; // Implicit declare uses dynamic type descriptor. rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -693,7 +776,8 @@ NodeParameters::set_parameters_atomically(const std::vector & staged_parameter_changes, parameter_overrides_, // Only call callbacks once below - empty_callback_container, // callback_container is explicitly empty + empty_on_set_callback_container, // callback_container is explicitly empty + empty_post_set_callback_container, // callback_container is explicitly empty ¶meter_event_msg, true); if (!result.successful) { @@ -706,12 +790,14 @@ NodeParameters::set_parameters_atomically(const std::vector & // If there were implicitly declared parameters, then we may need to copy the input parameters // and then assign the value that was selected after the declare (could be affected by the // initial parameter values). - const std::vector * parameters_to_be_set = ¶meters; + const std::vector * parameters_to_be_set = ¶meters_after_pre_set_callback; std::vector parameters_copy; if (0 != staged_parameter_changes.size()) { // If there were any implicitly declared parameters. bool any_initial_values_used = false; for (const auto & staged_parameter_change : staged_parameter_changes) { - auto it = __find_parameter_by_name(parameters, staged_parameter_change.first); + auto it = __find_parameter_by_name( + parameters_after_pre_set_callback, + staged_parameter_change.first); if (it->get_parameter_value() != staged_parameter_change.second.value) { // In this case, the value of the staged parameter differs from the // input from the user, and therefore we need to update things before setting. @@ -721,7 +807,7 @@ NodeParameters::set_parameters_atomically(const std::vector & } } if (any_initial_values_used) { - parameters_copy = parameters; + parameters_copy = parameters_after_pre_set_callback; for (const auto & staged_parameter_change : staged_parameter_changes) { auto it = __find_parameter_by_name(parameters_copy, staged_parameter_change.first); *it = Parameter(staged_parameter_change.first, staged_parameter_change.second.value); @@ -754,8 +840,9 @@ NodeParameters::set_parameters_atomically(const std::vector & // they are actually set on the official parameter storage parameters_, // These callbacks are called once. When a callback returns an unsuccessful result, - // the remaining aren't called. - on_parameters_set_callback_container_, + // the remaining aren't called + on_set_parameters_callback_container_, + post_set_parameters_callback_container_, allow_undeclared_); // allow undeclared // If not successful, then stop here. @@ -811,7 +898,6 @@ NodeParameters::set_parameters_atomically(const std::vector & parameter_event_msg.stamp = node_clock_->get_clock()->now(); events_publisher_->publish(parameter_event_msg); } - return result; } @@ -997,6 +1083,26 @@ NodeParameters::list_parameters(const std::vector & prefixes, uint6 return result; } +void +NodeParameters::remove_pre_set_parameters_callback( + const PreSetParametersCallbackHandle * const handle) +{ + std::lock_guard lock(mutex_); + ParameterMutationRecursionGuard guard(parameter_modification_enabled_); + + auto it = std::find_if( + pre_set_parameters_callback_container_.begin(), + pre_set_parameters_callback_container_.end(), + [handle](const auto & weak_handle) { + return handle == weak_handle.lock().get(); + }); + if (it != pre_set_parameters_callback_container_.end()) { + pre_set_parameters_callback_container_.erase(it); + } else { + throw std::runtime_error("Pre set parameter callback doesn't exist"); + } +} + void NodeParameters::remove_on_set_parameters_callback( const OnSetParametersCallbackHandle * const handle) @@ -1005,20 +1111,53 @@ NodeParameters::remove_on_set_parameters_callback( ParameterMutationRecursionGuard guard(parameter_modification_enabled_); auto it = std::find_if( - on_parameters_set_callback_container_.begin(), - on_parameters_set_callback_container_.end(), + on_set_parameters_callback_container_.begin(), + on_set_parameters_callback_container_.end(), + [handle](const auto & weak_handle) { + return handle == weak_handle.lock().get(); + }); + if (it != on_set_parameters_callback_container_.end()) { + on_set_parameters_callback_container_.erase(it); + } else { + throw std::runtime_error("On set parameter callback doesn't exist"); + } +} + +void +NodeParameters::remove_post_set_parameters_callback( + const PostSetParametersCallbackHandle * const handle) +{ + std::lock_guard lock(mutex_); + ParameterMutationRecursionGuard guard(parameter_modification_enabled_); + + auto it = std::find_if( + post_set_parameters_callback_container_.begin(), + post_set_parameters_callback_container_.end(), [handle](const auto & weak_handle) { return handle == weak_handle.lock().get(); }); - if (it != on_parameters_set_callback_container_.end()) { - on_parameters_set_callback_container_.erase(it); + if (it != post_set_parameters_callback_container_.end()) { + post_set_parameters_callback_container_.erase(it); } else { - throw std::runtime_error("Callback doesn't exist"); + throw std::runtime_error("Post set parameter callback doesn't exist"); } } +PreSetParametersCallbackHandle::SharedPtr +NodeParameters::add_pre_set_parameters_callback(PreSetParametersCallbackType callback) +{ + std::lock_guard lock(mutex_); + ParameterMutationRecursionGuard guard(parameter_modification_enabled_); + + auto handle = std::make_shared(); + handle->callback = callback; + // the last callback registered is executed first. + pre_set_parameters_callback_container_.emplace_front(handle); + return handle; +} + OnSetParametersCallbackHandle::SharedPtr -NodeParameters::add_on_set_parameters_callback(OnParametersSetCallbackType callback) +NodeParameters::add_on_set_parameters_callback(OnSetParametersCallbackType callback) { std::lock_guard lock(mutex_); ParameterMutationRecursionGuard guard(parameter_modification_enabled_); @@ -1026,7 +1165,21 @@ NodeParameters::add_on_set_parameters_callback(OnParametersSetCallbackType callb auto handle = std::make_shared(); handle->callback = callback; // the last callback registered is executed first. - on_parameters_set_callback_container_.emplace_front(handle); + on_set_parameters_callback_container_.emplace_front(handle); + return handle; +} + +PostSetParametersCallbackHandle::SharedPtr +NodeParameters::add_post_set_parameters_callback( + PostSetParametersCallbackType callback) +{ + std::lock_guard lock(mutex_); + ParameterMutationRecursionGuard guard(parameter_modification_enabled_); + + auto handle = std::make_shared(); + handle->callback = callback; + // the last callback registered is executed first. + post_set_parameters_callback_container_.emplace_front(handle); return handle; } diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp index 97e3a3188d..58bf010767 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp @@ -175,7 +175,7 @@ TEST_F(TestNodeParameters, set_parameters) { EXPECT_TRUE(result[0].successful); } -TEST_F(TestNodeParameters, add_remove_parameters_callback) { +TEST_F(TestNodeParameters, add_remove_on_set_parameters_callback) { rcl_interfaces::msg::ParameterDescriptor bool_descriptor; bool_descriptor.name = "bool_parameter"; bool_descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; @@ -202,7 +202,123 @@ TEST_F(TestNodeParameters, add_remove_parameters_callback) { RCLCPP_EXPECT_THROW_EQ( node_parameters->remove_on_set_parameters_callback(handle.get()), - std::runtime_error("Callback doesn't exist")); + std::runtime_error("On set parameter callback doesn't exist")); +} + +TEST_F(TestNodeParameters, add_remove_pre_set_parameters_callback) { + // `add_pre_set_parameters_callback` used to modify parameters list. + auto modify_parameter_list_callback = [](std::vector & parameters) { + for (const auto & param : parameters) { + if (param.get_name() == "param1") { + parameters.emplace_back("param2", 2.0); + } + } + }; + + // `add_pre_set_parameters_callback` used to make the parameters list empty. + auto empty_parameter_list_callback = [](std::vector & parameters) { + parameters = {}; + }; + + auto handle1 = + node_parameters->add_pre_set_parameters_callback(modify_parameter_list_callback); + + double default_value = 0.0; + node_parameters->declare_parameter( + "param1", rclcpp::ParameterValue(default_value)); + node_parameters->declare_parameter( + "param2", rclcpp::ParameterValue(default_value)); + + // verify that `declare_parameter` does not call any of the callbacks registered with + // `add_pre_set_parameters_callback` + EXPECT_TRUE(node_parameters->has_parameter("param1")); + EXPECT_EQ(node_parameters->get_parameter("param1").get_value(), default_value); + EXPECT_TRUE(node_parameters->has_parameter("param2")); + EXPECT_EQ(node_parameters->get_parameter("param2").get_value(), default_value); + + // verify that the `param2` was set successfully conditioned on setting of + // `param1` + const std::vector parameters_to_be_set = { + rclcpp::Parameter("param1", 1.0)}; + auto result = node_parameters->set_parameters(parameters_to_be_set); + // we expect the result size to be same as the original "parameters_to_be_set" + // since the pre set parameter callback will set the modified param list atomically. + ASSERT_EQ(1u, result.size()); + EXPECT_TRUE(result[0].successful); + EXPECT_TRUE(node_parameters->has_parameter("param1")); + EXPECT_EQ(node_parameters->get_parameter("param1").get_value(), 1.0); + EXPECT_TRUE(node_parameters->has_parameter("param2")); + EXPECT_EQ(node_parameters->get_parameter("param2").get_value(), 2.0); + EXPECT_NO_THROW(node_parameters->remove_pre_set_parameters_callback(handle1.get())); + RCLCPP_EXPECT_THROW_EQ( + node_parameters->remove_pre_set_parameters_callback(handle1.get()), + std::runtime_error("Pre set parameter callback doesn't exist")); + + // verify that the result should be unsuccessful if the pre set callback makes + // parameter list empty + auto handle2 = + node_parameters->add_pre_set_parameters_callback(empty_parameter_list_callback); + auto results = node_parameters->set_parameters(parameters_to_be_set); + + std::string reason = "parameter list cannot be empty, this might be due to " + "pre_set_parameters_callback modifying the original parameters list."; + EXPECT_FALSE(results[0].successful); + EXPECT_EQ(results[0].reason, reason); + EXPECT_NO_THROW(node_parameters->remove_pre_set_parameters_callback(handle2.get())); + RCLCPP_EXPECT_THROW_EQ( + node_parameters->remove_pre_set_parameters_callback(handle2.get()), + std::runtime_error("Pre set parameter callback doesn't exist")); +} + +TEST_F(TestNodeParameters, add_remove_post_set_parameters_callback) { + rcl_interfaces::msg::ParameterDescriptor param1_descriptor; + param1_descriptor.name = "double_parameter1"; + param1_descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + param1_descriptor.read_only = false; + + rcl_interfaces::msg::ParameterDescriptor param2_descriptor; + param2_descriptor.name = "double_parameter2"; + param2_descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + param2_descriptor.read_only = false; + + double variable_tracking_param1_internally = node_parameters->declare_parameter( + "param1", rclcpp::ParameterValue(0.0), param1_descriptor, false).get(); + double variable_tracking_param2_internally = node_parameters->declare_parameter( + "param2", rclcpp::ParameterValue(0.0), param2_descriptor, false).get(); + + EXPECT_EQ(variable_tracking_param1_internally, 0.0); + EXPECT_EQ(variable_tracking_param2_internally, 0.0); + + const std::vector parameters_to_be_set = { + rclcpp::Parameter("param1", 1.0), + rclcpp::Parameter("param2", 2.0)}; + + // register a callback for successful set parameter and change the internally tracked variables. + auto callback = [&](const std::vector & parameters) { + for (const auto & param : parameters) { + if (param.get_name() == "param1") { + variable_tracking_param1_internally = param.get_value(); + } else if (param.get_name() == "param2") { + variable_tracking_param2_internally = param.get_value(); + } + } + }; + + auto handle = node_parameters->add_post_set_parameters_callback(callback); + auto result = node_parameters->set_parameters(parameters_to_be_set); + ASSERT_EQ(2u, result.size()); + EXPECT_TRUE(result[0].successful); + EXPECT_TRUE(result[1].successful); + EXPECT_TRUE(node_parameters->has_parameter("param1")); + EXPECT_TRUE(node_parameters->has_parameter("param2")); + EXPECT_EQ(variable_tracking_param1_internally, 1.0); + EXPECT_EQ(variable_tracking_param2_internally, 2.0); + + EXPECT_NO_THROW(node_parameters->remove_post_set_parameters_callback(handle.get())); + + RCLCPP_EXPECT_THROW_EQ( + node_parameters->remove_post_set_parameters_callback(handle.get()), + std::runtime_error("Post set parameter callback doesn't exist")); } TEST_F(TestNodeParameters, wildcard_with_namespace) diff --git a/rclcpp/test/rclcpp/test_node.cpp b/rclcpp/test/rclcpp/test_node.cpp index 58af342561..c40811a4e4 100644 --- a/rclcpp/test/rclcpp/test_node.cpp +++ b/rclcpp/test/rclcpp/test_node.cpp @@ -1444,6 +1444,36 @@ TEST_F(TestNode, set_parameter_undeclared_parameters_not_allowed) { EXPECT_EQ(value.get_type(), rclcpp::PARAMETER_STRING); EXPECT_EQ(value.get_value(), "asd"); } + { + // adding a parameter in "pre set parameter" callback, when that + // parameter has not been declared before will throw + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + auto default_value = 0; // default value of name1 param + + // declare name1 parameter only + node->declare_parameter(name1, default_value); + + // add undeclared parameter with name2 to modified list of parameters + auto pre_set_parameters = + [&](std::vector & parameters) { + for (const auto & param : parameters) { + if (param.get_name() == name1) { + parameters.emplace_back(rclcpp::Parameter(name2, 2)); + } + } + }; + + auto handler = node->add_pre_set_parameters_callback(pre_set_parameters); + EXPECT_THROW( + node->set_parameter(rclcpp::Parameter(name1, 4)), + rclcpp::exceptions::ParameterNotDeclaredException); + EXPECT_TRUE(node->has_parameter(name1)); + EXPECT_EQ(node->get_parameter(name1).get_value(), default_value); + EXPECT_FALSE(node->has_parameter(name2)); + RCPPUTILS_SCOPE_EXIT( + {node->remove_pre_set_parameters_callback(handler.get());}); // always reset + } } TEST_F(TestNode, set_parameter_undeclared_parameters_allowed) { @@ -1481,6 +1511,36 @@ TEST_F(TestNode, set_parameter_undeclared_parameters_allowed) { EXPECT_TRUE(node->has_parameter(name)); EXPECT_EQ(node->get_parameter(name).get_value(), 43); } + { + // adding a parameter in "pre set parameter" callback, when that + // parameter has not been declared will not throw if undeclared + // parameters are allowed + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + + // declare name1 parameter only + node->declare_parameter(name1, 0); + + // add undeclared parameter with name2 to modified list of parameters + auto pre_set_parameters = + [&](std::vector & parameters) { + for (const auto & param : parameters) { + if (param.get_name() == name1) { + parameters.emplace_back(rclcpp::Parameter(name2, 2)); + } + } + }; + + auto handler = node->add_pre_set_parameters_callback(pre_set_parameters); + auto result = node->set_parameter(rclcpp::Parameter(name1, 1)); + EXPECT_TRUE(result.successful); + EXPECT_TRUE(node->has_parameter(name1)); + EXPECT_TRUE(node->has_parameter(name2)); + EXPECT_EQ(node->get_parameter(name1).get_value(), 1); + EXPECT_EQ(node->get_parameter(name2).get_value(), 2); + RCPPUTILS_SCOPE_EXIT( + {node->remove_pre_set_parameters_callback(handler.get());}); // always reset + } } TEST_F(TestNode, set_parameters_undeclared_parameters_not_allowed) { @@ -1625,6 +1685,51 @@ TEST_F(TestNode, set_parameters_undeclared_parameters_not_allowed) { EXPECT_FALSE(node->has_parameter(name)); } + { + // adding a parameter in "pre set parameter" callback when that + // parameter has not been declared before will throw. However, when + // multiple params are being set using "set_parameters", the params + // which are not conditioned on each other in "pre set callback" will + // still be set successfully. This is the desired behaviour since + // "set_parameters" sets params non atomically. + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + auto name3 = "parameter"_unq; + auto default_value = 0; + + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rclcpp::PARAMETER_INTEGER; + + // declare name1 and name2 parameter only + node->declare_parameter(name1, default_value, descriptor); + node->declare_parameter(name2, default_value, descriptor); + + // add undeclared parameter with name3 to modified list of parameters + // conditioned of name2 param + auto pre_set_parameters = + [&](std::vector & parameters) { + for (const auto & param : parameters) { + if (param.get_name() == name2) { + parameters.emplace_back(rclcpp::Parameter(name3, 3)); + } + } + }; + + auto handler = node->add_pre_set_parameters_callback(pre_set_parameters); + EXPECT_THROW( + node->set_parameters({rclcpp::Parameter(name1, 1), rclcpp::Parameter(name2, 2)}), + rclcpp::exceptions::ParameterNotDeclaredException); + EXPECT_TRUE(node->has_parameter(name1)); + EXPECT_TRUE(node->has_parameter(name2)); + EXPECT_FALSE(node->has_parameter(name3)); + + // we still expect the value of name1 param to be set successfully, since + // the setting of name2 param is only conditioned on setting of name3 param + EXPECT_EQ(node->get_parameter(name1).get_value(), 1); + EXPECT_EQ(node->get_parameter(name2).get_value(), default_value); + RCPPUTILS_SCOPE_EXIT( + {node->remove_pre_set_parameters_callback(handler.get());}); // always reset + } } // test set_parameters with undeclared allowed @@ -1673,6 +1778,48 @@ TEST_F(TestNode, set_parameters_undeclared_parameters_allowed) { EXPECT_EQ(node->get_parameter(name1).get_value(), 42); EXPECT_EQ(node->get_parameter(name2).get_value(), "test"); } + { + // adding a parameter in "pre set parameter" callback when that + // parameter has not been declared before will not throw when + // undeclared parameters are allowed. + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + auto name3 = "parameter"_unq; + auto default_value = 0; + + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rclcpp::PARAMETER_INTEGER; + + // declare name1 and name2 parameter only + node->declare_parameter(name1, default_value, descriptor); + node->declare_parameter(name2, default_value, descriptor); + + // add undeclared parameter with name3 to modified list of parameters + // conditioned of name2 param + auto pre_set_parameters = + [&](std::vector & parameters) { + for (const auto & param : parameters) { + if (param.get_name() == name2) { + parameters.emplace_back(rclcpp::Parameter(name3, 3)); + } + } + }; + + auto handler = node->add_pre_set_parameters_callback(pre_set_parameters); + auto results = node->set_parameters({rclcpp::Parameter(name1, 1), rclcpp::Parameter(name2, 2)}); + EXPECT_EQ(2u, results.size()); + EXPECT_TRUE(results[0].successful); + EXPECT_TRUE(results[1].successful); + EXPECT_TRUE(node->has_parameter(name1)); + EXPECT_TRUE(node->has_parameter(name2)); + EXPECT_TRUE(node->has_parameter(name3)); + EXPECT_EQ(node->get_parameter(name1).get_value(), 1); + EXPECT_EQ(node->get_parameter(name2).get_value(), 2); + EXPECT_EQ(node->get_parameter(name3).get_value(), 3); + + RCPPUTILS_SCOPE_EXIT( + {node->remove_pre_set_parameters_callback(handler.get());}); // always reset + } } TEST_F(TestNode, set_parameters_atomically_undeclared_parameters_not_allowed) { @@ -1815,6 +1962,48 @@ TEST_F(TestNode, set_parameters_atomically_undeclared_parameters_not_allowed) { EXPECT_FALSE(node->has_parameter(name)); } + { + // adding a parameter in "pre set parameter" callback when that + // parameter has not been declared before will throw and since + // multiple params are being set using "set_parameters_atomically", + // a failure in set of one param will result in all params being + // set unsuccessfully. + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + auto name3 = "parameter"_unq; + auto default_value = 0; + + // declare name1 and name2 parameter only + node->declare_parameter(name1, default_value); + node->declare_parameter(name2, default_value); + + // add undeclared parameter with name3 to modified list of parameters + // conditioned of name2 param + auto pre_set_parameters = + [&](std::vector & parameters) { + for (const auto & param : parameters) { + if (param.get_name() == name2) { + parameters.emplace_back(rclcpp::Parameter(name3, 3)); + } + } + }; + + auto handler = node->add_pre_set_parameters_callback(pre_set_parameters); + EXPECT_THROW( + node->set_parameters_atomically( + {rclcpp::Parameter(name1, 1), + rclcpp::Parameter(name2, 2)}), + rclcpp::exceptions::ParameterNotDeclaredException); + EXPECT_TRUE(node->has_parameter(name1)); + EXPECT_TRUE(node->has_parameter(name2)); + EXPECT_FALSE(node->has_parameter(name3)); + + // the values of all the params is still default. + EXPECT_EQ(node->get_parameter(name1).get_value(), default_value); + EXPECT_EQ(node->get_parameter(name2).get_value(), default_value); + RCPPUTILS_SCOPE_EXIT( + {node->remove_pre_set_parameters_callback(handler.get());}); // always reset + } } // test set_parameters with undeclared allowed @@ -1903,6 +2092,45 @@ TEST_F(TestNode, set_parameters_atomically_undeclared_parameters_allowed) { EXPECT_FALSE(node->has_parameter(name2)); // important! name2 remains undeclared EXPECT_EQ(node->get_parameter(name3).get_value(), "test"); } + { + // adding a parameter in "pre set parameter" callback when that + // parameter has not been declared before will not throw when + // undeclared parameters are allowed. + auto name1 = "parameter"_unq; + auto name2 = "parameter"_unq; + auto name3 = "parameter"_unq; + auto default_value = 0; + + // declare name1 and name2 parameter only + node->declare_parameter(name1, default_value); + node->declare_parameter(name2, default_value); + + // add undeclared parameter with name3 to modified list of parameters + // conditioned of name2 param + auto pre_set_parameters = + [&](std::vector & parameters) { + for (const auto & param : parameters) { + if (param.get_name() == name2) { + parameters.emplace_back(rclcpp::Parameter(name3, 3)); + } + } + }; + + auto handler = node->add_pre_set_parameters_callback(pre_set_parameters); + auto result = node->set_parameters_atomically( + {rclcpp::Parameter(name1, 1), + rclcpp::Parameter(name2, 2)}); + EXPECT_TRUE(result.successful); + EXPECT_TRUE(node->has_parameter(name1)); + EXPECT_TRUE(node->has_parameter(name2)); + EXPECT_TRUE(node->has_parameter(name3)); + EXPECT_EQ(node->get_parameter(name1).get_value(), 1); + EXPECT_EQ(node->get_parameter(name2).get_value(), 2); + EXPECT_EQ(node->get_parameter(name3).get_value(), 3); + + RCPPUTILS_SCOPE_EXIT( + {node->remove_pre_set_parameters_callback(handler.get());}); // always reset + } } // test get_parameter with undeclared not allowed diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 2978a5ecaa..84c62b9e6a 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -538,10 +538,32 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, rcl_interfaces::msg::ListParametersResult list_parameters(const std::vector & prefixes, uint64_t depth) const; + using PreSetParametersCallbackHandle = + rclcpp::node_interfaces::PreSetParametersCallbackHandle; + using PreSetParametersCallbackType = + rclcpp::node_interfaces::NodeParametersInterface::PreSetParametersCallbackType; + using OnSetParametersCallbackHandle = rclcpp::node_interfaces::OnSetParametersCallbackHandle; - using OnParametersSetCallbackType = - rclcpp::node_interfaces::NodeParametersInterface::OnParametersSetCallbackType; + using OnSetParametersCallbackType = + rclcpp::node_interfaces::NodeParametersInterface::OnSetParametersCallbackType; + using OnParametersSetCallbackType [[deprecated("use OnSetParametersCallbackType instead")]] = + OnSetParametersCallbackType; + + using PostSetParametersCallbackHandle = + rclcpp::node_interfaces::PostSetParametersCallbackHandle; + using PostSetParametersCallbackType = + rclcpp::node_interfaces::NodeParametersInterface::PostSetParametersCallbackType; + + /// Add a callback that gets triggered before parameters are validated. + /** + * \sa rclcpp::Node::add_pre_set_parameters_callback + */ + RCLCPP_LIFECYCLE_PUBLIC + RCUTILS_WARN_UNUSED + rclcpp_lifecycle::LifecycleNode::PreSetParametersCallbackHandle::SharedPtr + add_pre_set_parameters_callback( + rclcpp_lifecycle::LifecycleNode::PreSetParametersCallbackType callback); /// Add a callback for when parameters are being set. /** @@ -551,7 +573,26 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, RCUTILS_WARN_UNUSED rclcpp_lifecycle::LifecycleNode::OnSetParametersCallbackHandle::SharedPtr add_on_set_parameters_callback( - rclcpp_lifecycle::LifecycleNode::OnParametersSetCallbackType callback); + rclcpp_lifecycle::LifecycleNode::OnSetParametersCallbackType callback); + + /// Add a callback that gets triggered after parameters are set successfully. + /** + * \sa rclcpp::Node::add_post_set_parameters_callback + */ + RCLCPP_LIFECYCLE_PUBLIC + RCUTILS_WARN_UNUSED + rclcpp_lifecycle::LifecycleNode::PostSetParametersCallbackHandle::SharedPtr + add_post_set_parameters_callback( + rclcpp_lifecycle::LifecycleNode::PostSetParametersCallbackType callback); + + /// Remove a callback registered with `add_pre_set_parameters_callback`. + /** + * \sa rclcpp::Node::remove_pre_set_parameters_callback + */ + RCLCPP_LIFECYCLE_PUBLIC + void + remove_pre_set_parameters_callback( + const rclcpp_lifecycle::LifecycleNode::PreSetParametersCallbackHandle * const handler); /// Remove a callback registered with `add_on_set_parameters_callback`. /** @@ -562,6 +603,15 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, remove_on_set_parameters_callback( const rclcpp_lifecycle::LifecycleNode::OnSetParametersCallbackHandle * const handler); + /// Remove a callback registered with `add_post_set_parameters_callback`. + /** + * \sa rclcpp::Node::remove_post_set_parameters_callback + */ + RCLCPP_LIFECYCLE_PUBLIC + void + remove_post_set_parameters_callback( + const rclcpp_lifecycle::LifecycleNode::PostSetParametersCallbackHandle * const handler); + /// Return a vector of existing node names (string). /** * \sa rclcpp::Node::get_node_names diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 06378b594b..0851c596ec 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -283,12 +283,31 @@ LifecycleNode::list_parameters( return node_parameters_->list_parameters(prefixes, depth); } +rclcpp::Node::PreSetParametersCallbackHandle::SharedPtr +LifecycleNode::add_pre_set_parameters_callback(PreSetParametersCallbackType callback) +{ + return node_parameters_->add_pre_set_parameters_callback(callback); +} + rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr -LifecycleNode::add_on_set_parameters_callback(OnParametersSetCallbackType callback) +LifecycleNode::add_on_set_parameters_callback(OnSetParametersCallbackType callback) { return node_parameters_->add_on_set_parameters_callback(callback); } +rclcpp::Node::PostSetParametersCallbackHandle::SharedPtr +LifecycleNode::add_post_set_parameters_callback(PostSetParametersCallbackType callback) +{ + return node_parameters_->add_post_set_parameters_callback(callback); +} + +void +LifecycleNode::remove_pre_set_parameters_callback( + const PreSetParametersCallbackHandle * const callback) +{ + node_parameters_->remove_pre_set_parameters_callback(callback); +} + void LifecycleNode::remove_on_set_parameters_callback( const OnSetParametersCallbackHandle * const callback) @@ -296,6 +315,13 @@ LifecycleNode::remove_on_set_parameters_callback( node_parameters_->remove_on_set_parameters_callback(callback); } +void +LifecycleNode::remove_post_set_parameters_callback( + const PostSetParametersCallbackHandle * const callback) +{ + node_parameters_->remove_post_set_parameters_callback(callback); +} + std::vector LifecycleNode::get_node_names() const { From 3d69031d2a614cf02dc10ea5db153471ea32b1f2 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Thu, 18 Aug 2022 12:30:51 +0800 Subject: [PATCH 07/53] fix memory leak (#1994) Signed-off-by: Chen Lihui --- rclcpp/src/rclcpp/parameter_map.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/rclcpp/src/rclcpp/parameter_map.cpp b/rclcpp/src/rclcpp/parameter_map.cpp index 8f7bc0655c..cb458e7db3 100644 --- a/rclcpp/src/rclcpp/parameter_map.cpp +++ b/rclcpp/src/rclcpp/parameter_map.cpp @@ -17,6 +17,7 @@ #include #include "rcpputils/find_and_replace.hpp" +#include "rcpputils/scope_exit.hpp" #include "rclcpp/parameter_map.hpp" using rclcpp::exceptions::InvalidParametersException; @@ -146,9 +147,11 @@ 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); + RCPPUTILS_SCOPE_EXIT(rcl_yaml_node_struct_fini(rcl_parameters); ); const char * path = yaml_filename.c_str(); if (!rcl_parse_yaml_file(path, rcl_parameters)) { rclcpp::exceptions::throw_from_rcl_error(RCL_RET_ERROR); } + return rclcpp::parameter_map_from(rcl_parameters); } From dab9f5e0a331b9694c4b5aceb8263d49aa052ecb Mon Sep 17 00:00:00 2001 From: Brian Date: Mon, 29 Aug 2022 12:14:09 -0700 Subject: [PATCH 08/53] [docs] add note about callback lifetime for {on, post}_set_parameter_callback (#1981) [docs] add note about {on, post, pre}_set_parameter_callback lifetime Signed-off-by: Brian Chen --- rclcpp/include/rclcpp/node.hpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index bd5d39f6bb..35162de03b 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -991,6 +991,9 @@ class Node : public std::enable_shared_from_this * One of the use case of "pre set callback" can be updating additional parameters * conditioned on changes to a parameter. * + * Users should retain a copy of the returned shared pointer, as the callback + * is valid only as long as the smart pointer is alive. + * * For an example callback: * *```cpp @@ -1039,6 +1042,9 @@ class Node : public std::enable_shared_from_this * rcl_interfaces::msg::SetParametersResult to indicate whether or not the * parameter should be set or not, and if not why. * + * Users should retain a copy of the returned shared pointer, as the callback + * is valid only as long as the smart pointer is alive. + * * For an example callback: * * ```cpp @@ -1107,6 +1113,9 @@ class Node : public std::enable_shared_from_this * The callback is called when any of the `set_parameter*` or `declare_parameter*` * methods are successful. * + * Users should retain a copy of the returned shared pointer, as the callback + * is valid only as long as the smart pointer is alive. + * * The callback takes a reference to a const vector of parameters that have been * set successfully. * From 6167a575b3eb85c90240e20736f4e5823ac4c681 Mon Sep 17 00:00:00 2001 From: Andrew Symington Date: Tue, 30 Aug 2022 09:10:11 -0400 Subject: [PATCH 09/53] Add a `create_timer` method to `Node` and `LifecycleNode` classes (#1975) Signed-off-by: Andrew Symington --- rclcpp/include/rclcpp/create_timer.hpp | 155 +++++++++++++----- rclcpp/include/rclcpp/node.hpp | 15 +- rclcpp/include/rclcpp/node_impl.hpp | 16 ++ rclcpp/test/rclcpp/test_create_timer.cpp | 62 ++++++- rclcpp/test/rclcpp/test_time_source.cpp | 69 ++++---- rclcpp/test/rclcpp/test_timer.cpp | 105 +++++++++--- .../rclcpp_lifecycle/lifecycle_node.hpp | 15 +- .../rclcpp_lifecycle/lifecycle_node_impl.hpp | 27 ++- .../test/test_lifecycle_publisher.cpp | 49 +++++- 9 files changed, 394 insertions(+), 119 deletions(-) diff --git a/rclcpp/include/rclcpp/create_timer.hpp b/rclcpp/include/rclcpp/create_timer.hpp index 345f43fcbb..5225f0fde0 100644 --- a/rclcpp/include/rclcpp/create_timer.hpp +++ b/rclcpp/include/rclcpp/create_timer.hpp @@ -23,12 +23,63 @@ #include "rclcpp/duration.hpp" #include "rclcpp/node_interfaces/get_node_base_interface.hpp" +#include "rclcpp/node_interfaces/get_node_clock_interface.hpp" #include "rclcpp/node_interfaces/get_node_timers_interface.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_clock_interface.hpp" #include "rclcpp/node_interfaces/node_timers_interface.hpp" namespace rclcpp { +namespace detail +{ +/// Perform a safe cast to a timer period in nanoseconds +/** + * + * \tparam DurationRepT + * \tparam DurationT + * \param period period to execute callback. This duration must be 0 <= period < nanoseconds::max() + * \return period, expressed as chrono::duration::nanoseconds + * \throws std::invalid_argument if period is negative or too large + */ +template +std::chrono::nanoseconds +safe_cast_to_period_in_ns(std::chrono::duration period) +{ + if (period < std::chrono::duration::zero()) { + throw std::invalid_argument{"timer period cannot be negative"}; + } + + // Casting to a double representation might lose precision and allow the check below to succeed + // but the actual cast to nanoseconds fail. Using 1 DurationT worth of nanoseconds less than max. + constexpr auto maximum_safe_cast_ns = + std::chrono::nanoseconds::max() - std::chrono::duration(1); + + // If period is greater than nanoseconds::max(), the duration_cast to nanoseconds will overflow + // a signed integer, which is undefined behavior. Checking whether any std::chrono::duration is + // greater than nanoseconds::max() is a difficult general problem. This is a more conservative + // version of Howard Hinnant's (the guy>) response here: + // https://stackoverflow.com/a/44637334/2089061 + // However, this doesn't solve the issue for all possible duration types of period. + // Follow-up issue: https://github.com/ros2/rclcpp/issues/1177 + constexpr auto ns_max_as_double = + std::chrono::duration_cast>( + maximum_safe_cast_ns); + if (period > ns_max_as_double) { + throw std::invalid_argument{ + "timer period must be less than std::chrono::nanoseconds::max()"}; + } + + const auto period_ns = std::chrono::duration_cast(period); + if (period_ns < std::chrono::nanoseconds::zero()) { + throw std::runtime_error{ + "Casting timer period to nanoseconds resulted in integer overflow."}; + } + + return period_ns; +} +} // namespace detail + /// Create a timer with a given clock /// \internal template @@ -41,14 +92,13 @@ create_timer( CallbackT && callback, rclcpp::CallbackGroup::SharedPtr group = nullptr) { - auto timer = rclcpp::GenericTimer::make_shared( + return create_timer( + node_base.get(), + node_timers.get(), clock, period.to_chrono(), std::forward(callback), - node_base->get_context()); - - node_timers->add_timer(timer, group); - return timer; + group); } /// Create a timer with a given clock @@ -62,82 +112,99 @@ create_timer( rclcpp::CallbackGroup::SharedPtr group = nullptr) { return create_timer( - rclcpp::node_interfaces::get_node_base_interface(node), - rclcpp::node_interfaces::get_node_timers_interface(node), clock, - period, + period.to_chrono(), std::forward(callback), - group); + group, + rclcpp::node_interfaces::get_node_base_interface(node).get(), + rclcpp::node_interfaces::get_node_timers_interface(node).get()); } -/// Convenience method to create a timer with node resources. +/// Convenience method to create a general timer with node resources. /** * * \tparam DurationRepT * \tparam DurationT * \tparam CallbackT + * \param clock clock to be used * \param period period to execute callback. This duration must be 0 <= period < nanoseconds::max() * \param callback callback to execute via the timer period - * \param group - * \param node_base - * \param node_timers - * \return - * \throws std::invalid argument if either node_base or node_timers - * are null, or period is negative or too large + * \param group callback group + * \param node_base node base interface + * \param node_timers node timer interface + * \return shared pointer to a generic timer + * \throws std::invalid_argument if either clock, node_base or node_timers + * are nullptr, or period is negative or too large */ template -typename rclcpp::WallTimer::SharedPtr -create_wall_timer( +typename rclcpp::GenericTimer::SharedPtr +create_timer( + rclcpp::Clock::SharedPtr clock, std::chrono::duration period, CallbackT callback, rclcpp::CallbackGroup::SharedPtr group, node_interfaces::NodeBaseInterface * node_base, node_interfaces::NodeTimersInterface * node_timers) { + if (clock == nullptr) { + throw std::invalid_argument{"clock cannot be null"}; + } if (node_base == nullptr) { throw std::invalid_argument{"input node_base cannot be null"}; } - if (node_timers == nullptr) { throw std::invalid_argument{"input node_timers cannot be null"}; } - if (period < std::chrono::duration::zero()) { - throw std::invalid_argument{"timer period cannot be negative"}; - } + const std::chrono::nanoseconds period_ns = detail::safe_cast_to_period_in_ns(period); - // Casting to a double representation might lose precision and allow the check below to succeed - // but the actual cast to nanoseconds fail. Using 1 DurationT worth of nanoseconds less than max. - constexpr auto maximum_safe_cast_ns = - std::chrono::nanoseconds::max() - std::chrono::duration(1); + // Add a new generic timer. + auto timer = rclcpp::GenericTimer::make_shared( + std::move(clock), period_ns, std::move(callback), node_base->get_context()); + node_timers->add_timer(timer, group); + return timer; +} - // If period is greater than nanoseconds::max(), the duration_cast to nanoseconds will overflow - // a signed integer, which is undefined behavior. Checking whether any std::chrono::duration is - // greater than nanoseconds::max() is a difficult general problem. This is a more conservative - // version of Howard Hinnant's (the guy>) response here: - // https://stackoverflow.com/a/44637334/2089061 - // However, this doesn't solve the issue for all possible duration types of period. - // Follow-up issue: https://github.com/ros2/rclcpp/issues/1177 - constexpr auto ns_max_as_double = - std::chrono::duration_cast>( - maximum_safe_cast_ns); - if (period > ns_max_as_double) { - throw std::invalid_argument{ - "timer period must be less than std::chrono::nanoseconds::max()"}; +/// Convenience method to create a wall timer with node resources. +/** + * + * \tparam DurationRepT + * \tparam DurationT + * \tparam CallbackT + * \param period period to execute callback. This duration must be 0 <= period < nanoseconds::max() + * \param callback callback to execute via the timer period + * \param group callback group + * \param node_base node base interface + * \param node_timers node timer interface + * \return shared pointer to a wall timer + * \throws std::invalid_argument if either node_base or node_timers + * are null, or period is negative or too large + */ +template +typename rclcpp::WallTimer::SharedPtr +create_wall_timer( + std::chrono::duration period, + CallbackT callback, + rclcpp::CallbackGroup::SharedPtr group, + node_interfaces::NodeBaseInterface * node_base, + node_interfaces::NodeTimersInterface * node_timers) +{ + if (node_base == nullptr) { + throw std::invalid_argument{"input node_base cannot be null"}; } - const auto period_ns = std::chrono::duration_cast(period); - if (period_ns < std::chrono::nanoseconds::zero()) { - throw std::runtime_error{ - "Casting timer period to nanoseconds resulted in integer overflow."}; + if (node_timers == nullptr) { + throw std::invalid_argument{"input node_timers cannot be null"}; } + const std::chrono::nanoseconds period_ns = detail::safe_cast_to_period_in_ns(period); + + // Add a new wall timer. auto timer = rclcpp::WallTimer::make_shared( period_ns, std::move(callback), node_base->get_context()); node_timers->add_timer(timer, group); return timer; } - } // namespace rclcpp #endif // RCLCPP__CREATE_TIMER_HPP_ diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 35162de03b..e514137b51 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -227,7 +227,7 @@ class Node : public std::enable_shared_from_this ) ); - /// Create a timer. + /// Create a wall timer that uses the wall clock to drive the callback. /** * \param[in] period Time interval between triggers of the callback. * \param[in] callback User-defined callback function. @@ -240,6 +240,19 @@ class Node : public std::enable_shared_from_this CallbackT callback, rclcpp::CallbackGroup::SharedPtr group = nullptr); + /// Create a timer that uses the node clock to drive the callback. + /** + * \param[in] period Time interval between triggers of the callback. + * \param[in] callback User-defined callback function. + * \param[in] group Callback group to execute this timer's callback in. + */ + template + typename rclcpp::GenericTimer::SharedPtr + create_timer( + std::chrono::duration period, + CallbackT callback, + rclcpp::CallbackGroup::SharedPtr group = nullptr); + /// Create and return a Client. /** * \param[in] service_name The topic to service on. diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 5b427b5b25..d6b090d4d1 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -120,6 +120,22 @@ Node::create_wall_timer( this->node_timers_.get()); } +template +typename rclcpp::GenericTimer::SharedPtr +Node::create_timer( + std::chrono::duration period, + CallbackT callback, + rclcpp::CallbackGroup::SharedPtr group) +{ + return rclcpp::create_timer( + this->get_clock(), + period, + std::move(callback), + group, + this->node_base_.get(), + this->node_timers_.get()); +} + template typename Client::SharedPtr Node::create_client( diff --git a/rclcpp/test/rclcpp/test_create_timer.cpp b/rclcpp/test/rclcpp/test_create_timer.cpp index 13c3564544..f253af4838 100644 --- a/rclcpp/test/rclcpp/test_create_timer.cpp +++ b/rclcpp/test/rclcpp/test_create_timer.cpp @@ -62,7 +62,7 @@ TEST(TestCreateTimer, call_with_node_wrapper_compiles) rclcpp::shutdown(); } -TEST(TestCreateTimer, call_wall_timer_with_bad_arguments) +TEST(TestCreateWallTimer, call_wall_timer_with_bad_arguments) { rclcpp::init(0, nullptr); NodeWrapper node("test_create_wall_timers_with_bad_arguments"); @@ -117,6 +117,66 @@ TEST(TestCreateTimer, call_wall_timer_with_bad_arguments) rclcpp::shutdown(); } +TEST(TestCreateTimer, call_timer_with_bad_arguments) +{ + rclcpp::init(0, nullptr); + NodeWrapper node("test_create_timers_with_bad_arguments"); + auto callback = []() {}; + rclcpp::CallbackGroup::SharedPtr group = nullptr; + auto node_interface = + rclcpp::node_interfaces::get_node_base_interface(node).get(); + auto timers_interface = + rclcpp::node_interfaces::get_node_timers_interface(node).get(); + + auto clock = node.get_node_clock_interface()->get_clock(); + + // Negative period + EXPECT_THROW( + rclcpp::create_timer( + clock, -1ms, callback, group, node_interface, timers_interface), + std::invalid_argument); + + // Very negative period + constexpr auto nanoseconds_min = std::chrono::nanoseconds::min(); + EXPECT_THROW( + rclcpp::create_timer( + clock, nanoseconds_min, callback, group, node_interface, timers_interface), + std::invalid_argument); + + // Period must be less than nanoseconds::max() + constexpr auto nanoseconds_max = std::chrono::nanoseconds::min(); + EXPECT_THROW( + rclcpp::create_timer( + clock, nanoseconds_max, callback, group, node_interface, timers_interface), + std::invalid_argument); + + EXPECT_NO_THROW( + rclcpp::create_timer( + clock, nanoseconds_max - 1us, callback, group, node_interface, timers_interface)); + + EXPECT_NO_THROW( + rclcpp::create_timer(clock, 0ms, callback, group, node_interface, timers_interface)); + + // Period must be less than nanoseconds::max() + constexpr auto hours_max = std::chrono::hours::max(); + EXPECT_THROW( + rclcpp::create_timer( + clock, hours_max, callback, group, node_interface, timers_interface), + std::invalid_argument); + + // node_interface is null + EXPECT_THROW( + rclcpp::create_timer(clock, 1ms, callback, group, nullptr, timers_interface), + std::invalid_argument); + + // timers_interface is null + EXPECT_THROW( + rclcpp::create_timer(clock, 1ms, callback, group, node_interface, nullptr), + std::invalid_argument); + + rclcpp::shutdown(); +} + static void test_timer_callback(void) {} TEST(TestCreateTimer, timer_function_pointer) diff --git a/rclcpp/test/rclcpp/test_time_source.cpp b/rclcpp/test/rclcpp/test_time_source.cpp index 2bf89a5b09..a5aad9a498 100644 --- a/rclcpp/test/rclcpp/test_time_source.cpp +++ b/rclcpp/test/rclcpp/test_time_source.cpp @@ -81,7 +81,7 @@ void spin_until_time( executor.spin_once(10ms); - if (clock->now().nanoseconds() == end_time.count()) { + if (clock->now().nanoseconds() >= end_time.count()) { return; } } @@ -108,7 +108,7 @@ void spin_until_ros_time_updated( executor.add_node(node); auto start = std::chrono::system_clock::now(); - while (std::chrono::system_clock::now() < (start + 1s)) { + while (std::chrono::system_clock::now() < (start + 2s)) { if (!rclcpp::ok()) { break; // Break for ctrl-c } @@ -630,7 +630,8 @@ class SimClockPublisherNode : public rclcpp::Node { public: SimClockPublisherNode() - : rclcpp::Node("sim_clock_publisher_node") + : rclcpp::Node("sim_clock_publisher_node"), + pub_time_(0, 0) { // Create a clock publisher clock_pub_ = this->create_publisher( @@ -645,10 +646,6 @@ class SimClockPublisherNode : public rclcpp::Node &SimClockPublisherNode::timer_callback, this) ); - - // Init clock msg to zero - clock_msg_.clock.sec = 0; - clock_msg_.clock.nanosec = 0; } ~SimClockPublisherNode() @@ -671,13 +668,15 @@ class SimClockPublisherNode : public rclcpp::Node private: void timer_callback() { - // Increment clock msg and publish it - clock_msg_.clock.nanosec += 1000000; + // Increment the time, update the clock msg and publish it + pub_time_ += rclcpp::Duration(0, 1000000); + clock_msg_.clock = pub_time_; clock_pub_->publish(clock_msg_); } rclcpp::Publisher::SharedPtr clock_pub_; rclcpp::TimerBase::SharedPtr pub_timer_; + rclcpp::Time pub_time_; rosgraph_msgs::msg::Clock clock_msg_; std::thread node_thread_; rclcpp::executors::SingleThreadedExecutor node_executor; @@ -695,7 +694,7 @@ class ClockThreadTestingNode : public rclcpp::Node this->set_parameter(rclcpp::Parameter("use_sim_time", true)); // Create a 100ms timer - timer_ = this->create_wall_timer( + timer_ = this->create_timer( std::chrono::milliseconds(100), std::bind( &ClockThreadTestingNode::timer_callback, @@ -735,29 +734,33 @@ class ClockThreadTestingNode : public rclcpp::Node bool is_callback_frozen_ = true; }; -TEST_F(TestTimeSource, check_sim_time_updated_in_callback_if_use_clock_thread) { - // Test if clock time of a node with - // parameter use_sim_time = true and option use_clock_thread = true - // is updated while node is not spinning - // (in a timer callback) - - // Create a "sim time" publisher and spin it - 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()); -} +// TODO(ivanpauno): This test was using a wall timer, when it was supposed to use sim time. +// It was also using `use_clock_tread = false`, when it was supposed to be `true`. +// Fixing the test to work as originally intended makes it super flaky. +// Disabling it until the test is fixed. +// TEST_F(TestTimeSource, check_sim_time_updated_in_callback_if_use_clock_thread) { +// // Test if clock time of a node with +// // parameter use_sim_time = true and option use_clock_thread = true +// // is updated while node is not spinning +// // (in a timer callback) + +// // Create a "sim time" publisher and spin it +// SimClockPublisherNode pub_node; +// pub_node.SpinNode(); + +// // Spin node for 2 seconds +// ClockThreadTestingNode clock_thread_testing_node; +// auto steady_clock = rclcpp::Clock(RCL_STEADY_TIME); +// auto start_time = steady_clock.now(); +// while (rclcpp::ok() && +// (steady_clock.now() - start_time).seconds() < 2.0) +// { +// rclcpp::spin_some(clock_thread_testing_node.get_node_base_interface()); +// } + +// // Node should have get out of timer callback +// ASSERT_FALSE(clock_thread_testing_node.GetIsCallbackFrozen()); +// } TEST_F(TestTimeSource, clock_sleep_until_with_ros_time_basic) { SimClockPublisherNode pub_node; diff --git a/rclcpp/test/rclcpp/test_timer.cpp b/rclcpp/test/rclcpp/test_timer.cpp index 7a6599dfe4..59a1218519 100644 --- a/rclcpp/test/rclcpp/test_timer.cpp +++ b/rclcpp/test/rclcpp/test_timer.cpp @@ -30,8 +30,15 @@ using namespace std::chrono_literals; +/// We want to test everything for both the wall and generic timer. +enum class TimerType +{ + WALL_TIMER, + GENERIC_TIMER, +}; + /// Timer testing bring up and teardown -class TestTimer : public ::testing::Test +class TestTimer : public ::testing::TestWithParam { protected: void SetUp() override @@ -44,10 +51,7 @@ class TestTimer : public ::testing::Test test_node = std::make_shared("test_timer_node"); - timer = test_node->create_wall_timer( - 100ms, - [this]() -> void - { + auto timer_callback = [this]() -> void { this->has_timer_run.store(true); if (this->cancel_timer.load()) { @@ -55,10 +59,20 @@ class TestTimer : public ::testing::Test } // prevent any tests running timer from blocking this->executor->cancel(); - } - ); - EXPECT_TRUE(timer->is_steady()); - + }; + + // Store the timer type for use in TEST_P declarations. + timer_type = GetParam(); + switch (timer_type) { + case TimerType::WALL_TIMER: + timer = test_node->create_wall_timer(100ms, timer_callback); + EXPECT_TRUE(timer->is_steady()); + break; + case TimerType::GENERIC_TIMER: + timer = test_node->create_timer(100ms, timer_callback); + EXPECT_FALSE(timer->is_steady()); + break; + } executor->add_node(test_node); // don't start spinning, let the test dictate when } @@ -72,6 +86,7 @@ class TestTimer : public ::testing::Test } // set to true if the timer callback executed, false otherwise + TimerType timer_type; std::atomic has_timer_run; // flag used to cancel the timer in the timer callback. If true cancel the timer, otherwise // cancel the executor (preventing any tests from blocking) @@ -91,7 +106,7 @@ void test_initial_conditions( } /// Simple test -TEST_F(TestTimer, test_simple_cancel) +TEST_P(TestTimer, test_simple_cancel) { // expect clean state, don't run otherwise test_initial_conditions(timer, has_timer_run); @@ -104,7 +119,7 @@ TEST_F(TestTimer, test_simple_cancel) } /// Test state when using reset -TEST_F(TestTimer, test_is_canceled_reset) +TEST_P(TestTimer, test_is_canceled_reset) { // expect clean state, don't run otherwise test_initial_conditions(timer, has_timer_run); @@ -129,7 +144,7 @@ TEST_F(TestTimer, test_is_canceled_reset) } /// Run and check state, cancel the executor -TEST_F(TestTimer, test_run_cancel_executor) +TEST_P(TestTimer, test_run_cancel_executor) { // expect clean state, don't run otherwise test_initial_conditions(timer, has_timer_run); @@ -146,7 +161,7 @@ TEST_F(TestTimer, test_run_cancel_executor) } /// Run and check state, cancel the timer -TEST_F(TestTimer, test_run_cancel_timer) +TEST_P(TestTimer, test_run_cancel_timer) { // expect clean state, don't run otherwise test_initial_conditions(timer, has_timer_run); @@ -159,7 +174,7 @@ TEST_F(TestTimer, test_run_cancel_timer) EXPECT_TRUE(timer->is_canceled()); } -TEST_F(TestTimer, test_bad_arguments) { +TEST_P(TestTimer, test_bad_arguments) { auto node_base = rclcpp::node_interfaces::get_node_base_interface(test_node); auto context = node_base->get_context(); @@ -198,13 +213,19 @@ TEST_F(TestTimer, test_bad_arguments) { rclcpp::exceptions::RCLError); } -TEST_F(TestTimer, callback_with_timer) { +TEST_P(TestTimer, callback_with_timer) { rclcpp::TimerBase * timer_ptr = nullptr; - timer = test_node->create_wall_timer( - std::chrono::milliseconds(1), - [&timer_ptr](rclcpp::TimerBase & timer) { + auto timer_callback = [&timer_ptr](rclcpp::TimerBase & timer) { timer_ptr = &timer; - }); + }; + switch (timer_type) { + case TimerType::WALL_TIMER: + timer = test_node->create_wall_timer(1ms, timer_callback); + break; + case TimerType::GENERIC_TIMER: + timer = test_node->create_timer(1ms, timer_callback); + break; + } auto start = std::chrono::steady_clock::now(); while (nullptr == timer_ptr && (std::chrono::steady_clock::now() - start) < std::chrono::milliseconds(100)) @@ -216,13 +237,19 @@ TEST_F(TestTimer, callback_with_timer) { EXPECT_FALSE(timer_ptr->is_ready()); } -TEST_F(TestTimer, callback_with_period_zero) { +TEST_P(TestTimer, callback_with_period_zero) { rclcpp::TimerBase * timer_ptr = nullptr; - timer = test_node->create_wall_timer( - std::chrono::milliseconds(0), - [&timer_ptr](rclcpp::TimerBase & timer) { + auto timer_callback = [&timer_ptr](rclcpp::TimerBase & timer) { timer_ptr = &timer; - }); + }; + switch (timer_type) { + case TimerType::WALL_TIMER: + timer = test_node->create_wall_timer(0ms, timer_callback); + break; + case TimerType::GENERIC_TIMER: + timer = test_node->create_timer(0ms, timer_callback); + break; + } auto start = std::chrono::steady_clock::now(); while (nullptr == timer_ptr && (std::chrono::steady_clock::now() - start) < std::chrono::milliseconds(100)) @@ -235,7 +262,7 @@ TEST_F(TestTimer, callback_with_period_zero) { } /// Test internal failures using mocks -TEST_F(TestTimer, test_failures_with_exceptions) +TEST_P(TestTimer, test_failures_with_exceptions) { // expect clean state, don't run otherwise test_initial_conditions(timer, has_timer_run); @@ -245,8 +272,16 @@ TEST_F(TestTimer, test_failures_with_exceptions) auto mock = mocking_utils::inject_on_return("lib:rclcpp", rcl_timer_fini, RCL_RET_ERROR); EXPECT_NO_THROW( { - timer_to_test_destructor = - test_node->create_wall_timer(std::chrono::milliseconds(0), [](void) {}); + switch (timer_type) { + case TimerType::WALL_TIMER: + timer_to_test_destructor = + test_node->create_wall_timer(std::chrono::milliseconds(0), [](void) {}); + break; + case TimerType::GENERIC_TIMER: + timer_to_test_destructor = + test_node->create_timer(std::chrono::milliseconds(0), [](void) {}); + break; + } timer_to_test_destructor.reset(); }); } @@ -283,3 +318,19 @@ TEST_F(TestTimer, test_failures_with_exceptions) std::runtime_error("Timer could not get time until next call: error not set")); } } + +INSTANTIATE_TEST_SUITE_P( + PerTimerType, TestTimer, + ::testing::Values(TimerType::WALL_TIMER, TimerType::GENERIC_TIMER), + [](const ::testing::TestParamInfo & info) -> std::string { + switch (info.param) { + case TimerType::WALL_TIMER: + return std::string("wall_timer"); + case TimerType::GENERIC_TIMER: + return std::string("generic_timer"); + default: + break; + } + return std::string("unknown"); + } +); diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 84c62b9e6a..5e06876018 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -242,7 +242,7 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, ) ); - /// Create a timer. + /// Create a timer that uses the wall clock to drive the callback. /** * \param[in] period Time interval between triggers of the callback. * \param[in] callback User-defined callback function. @@ -255,6 +255,19 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, CallbackT callback, rclcpp::CallbackGroup::SharedPtr group = nullptr); + /// Create a timer that uses the node clock to drive the callback. + /** + * \param[in] period Time interval between triggers of the callback. + * \param[in] callback User-defined callback function. + * \param[in] group Callback group to execute this timer's callback in. + */ + template + typename rclcpp::GenericTimer::SharedPtr + create_timer( + std::chrono::duration period, + CallbackT callback, + rclcpp::CallbackGroup::SharedPtr group = nullptr); + /// Create and return a Client. /** * \sa rclcpp::Node::create_client diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp index 88d981e051..e2a5696b92 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp @@ -90,11 +90,28 @@ LifecycleNode::create_wall_timer( CallbackT callback, rclcpp::CallbackGroup::SharedPtr group) { - auto timer = rclcpp::WallTimer::make_shared( - std::chrono::duration_cast(period), - std::move(callback), this->node_base_->get_context()); - node_timers_->add_timer(timer, group); - return timer; + return rclcpp::create_wall_timer( + period, + std::move(callback), + group, + this->node_base_.get(), + this->node_timers_.get()); +} + +template +typename rclcpp::GenericTimer::SharedPtr +LifecycleNode::create_timer( + std::chrono::duration period, + CallbackT callback, + rclcpp::CallbackGroup::SharedPtr group) +{ + return rclcpp::create_timer( + this->get_clock(), + period, + std::move(callback), + group, + this->node_base_.get(), + this->node_timers_.get()); } template diff --git a/rclcpp_lifecycle/test/test_lifecycle_publisher.cpp b/rclcpp_lifecycle/test/test_lifecycle_publisher.cpp index 2a5d08f728..d10544ef1f 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_publisher.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_publisher.cpp @@ -42,10 +42,17 @@ class TestDefaultStateMachine : public ::testing::Test } }; +/// We want to test everything for both the wall and generic timer. +enum class TimerType +{ + WALL_TIMER, + GENERIC_TIMER, +}; + class EmptyLifecycleNode : public rclcpp_lifecycle::LifecycleNode { public: - explicit EmptyLifecycleNode(const std::string & node_name) + explicit EmptyLifecycleNode(const std::string & node_name, const TimerType & timer_type) : rclcpp_lifecycle::LifecycleNode(node_name) { rclcpp::PublisherOptionsWithAllocator> options; @@ -55,8 +62,20 @@ class EmptyLifecycleNode : public rclcpp_lifecycle::LifecycleNode add_managed_entity(publisher_); // For coverage this is being added here - auto timer = create_wall_timer(std::chrono::seconds(1), []() {}); - add_timer_handle(timer); + switch (timer_type) { + case TimerType::WALL_TIMER: + { + auto timer = create_wall_timer(std::chrono::seconds(1), []() {}); + add_timer_handle(timer); + break; + } + case TimerType::GENERIC_TIMER: + { + auto timer = create_timer(std::chrono::seconds(1), []() {}); + add_timer_handle(timer); + break; + } + } } std::shared_ptr> publisher() @@ -68,13 +87,13 @@ class EmptyLifecycleNode : public rclcpp_lifecycle::LifecycleNode std::shared_ptr> publisher_; }; -class TestLifecyclePublisher : public ::testing::Test +class TestLifecyclePublisher : public ::testing::TestWithParam { public: void SetUp() { rclcpp::init(0, nullptr); - node_ = std::make_shared("node"); + node_ = std::make_shared("node", GetParam()); } void TearDown() @@ -86,7 +105,7 @@ class TestLifecyclePublisher : public ::testing::Test std::shared_ptr node_; }; -TEST_F(TestLifecyclePublisher, publish_managed_by_node) { +TEST_P(TestLifecyclePublisher, publish_managed_by_node) { // transition via LifecycleNode auto success = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; auto reset_key = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; @@ -125,7 +144,7 @@ TEST_F(TestLifecyclePublisher, publish_managed_by_node) { } } -TEST_F(TestLifecyclePublisher, publish) { +TEST_P(TestLifecyclePublisher, publish) { // transition via LifecyclePublisher node_->publisher()->on_deactivate(); EXPECT_FALSE(node_->publisher()->is_activated()); @@ -148,3 +167,19 @@ TEST_F(TestLifecyclePublisher, publish) { EXPECT_NO_THROW(node_->publisher()->publish(std::move(msg_ptr))); } } + +INSTANTIATE_TEST_SUITE_P( + PerTimerType, TestLifecyclePublisher, + ::testing::Values(TimerType::WALL_TIMER, TimerType::GENERIC_TIMER), + [](const ::testing::TestParamInfo & info) -> std::string { + switch (info.param) { + case TimerType::WALL_TIMER: + return std::string("wall_timer"); + case TimerType::GENERIC_TIMER: + return std::string("generic_timer"); + default: + break; + } + return std::string("unknown"); + } +); From 11b5f8db2191f7921364f88cd20c04a80526121a Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 30 Aug 2022 13:42:14 -0700 Subject: [PATCH 10/53] Revert "Add a `create_timer` method to `Node` and `LifecycleNode` classes (#1975)" (#2009) This reverts commit 6167a575b3eb85c90240e20736f4e5823ac4c681. Signed-off-by: Shane Loretz Signed-off-by: Shane Loretz --- rclcpp/include/rclcpp/create_timer.hpp | 155 +++++------------- rclcpp/include/rclcpp/node.hpp | 15 +- rclcpp/include/rclcpp/node_impl.hpp | 16 -- rclcpp/test/rclcpp/test_create_timer.cpp | 62 +------ rclcpp/test/rclcpp/test_time_source.cpp | 69 ++++---- rclcpp/test/rclcpp/test_timer.cpp | 105 +++--------- .../rclcpp_lifecycle/lifecycle_node.hpp | 15 +- .../rclcpp_lifecycle/lifecycle_node_impl.hpp | 27 +-- .../test/test_lifecycle_publisher.cpp | 49 +----- 9 files changed, 119 insertions(+), 394 deletions(-) diff --git a/rclcpp/include/rclcpp/create_timer.hpp b/rclcpp/include/rclcpp/create_timer.hpp index 5225f0fde0..345f43fcbb 100644 --- a/rclcpp/include/rclcpp/create_timer.hpp +++ b/rclcpp/include/rclcpp/create_timer.hpp @@ -23,63 +23,12 @@ #include "rclcpp/duration.hpp" #include "rclcpp/node_interfaces/get_node_base_interface.hpp" -#include "rclcpp/node_interfaces/get_node_clock_interface.hpp" #include "rclcpp/node_interfaces/get_node_timers_interface.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" -#include "rclcpp/node_interfaces/node_clock_interface.hpp" #include "rclcpp/node_interfaces/node_timers_interface.hpp" namespace rclcpp { -namespace detail -{ -/// Perform a safe cast to a timer period in nanoseconds -/** - * - * \tparam DurationRepT - * \tparam DurationT - * \param period period to execute callback. This duration must be 0 <= period < nanoseconds::max() - * \return period, expressed as chrono::duration::nanoseconds - * \throws std::invalid_argument if period is negative or too large - */ -template -std::chrono::nanoseconds -safe_cast_to_period_in_ns(std::chrono::duration period) -{ - if (period < std::chrono::duration::zero()) { - throw std::invalid_argument{"timer period cannot be negative"}; - } - - // Casting to a double representation might lose precision and allow the check below to succeed - // but the actual cast to nanoseconds fail. Using 1 DurationT worth of nanoseconds less than max. - constexpr auto maximum_safe_cast_ns = - std::chrono::nanoseconds::max() - std::chrono::duration(1); - - // If period is greater than nanoseconds::max(), the duration_cast to nanoseconds will overflow - // a signed integer, which is undefined behavior. Checking whether any std::chrono::duration is - // greater than nanoseconds::max() is a difficult general problem. This is a more conservative - // version of Howard Hinnant's (the guy>) response here: - // https://stackoverflow.com/a/44637334/2089061 - // However, this doesn't solve the issue for all possible duration types of period. - // Follow-up issue: https://github.com/ros2/rclcpp/issues/1177 - constexpr auto ns_max_as_double = - std::chrono::duration_cast>( - maximum_safe_cast_ns); - if (period > ns_max_as_double) { - throw std::invalid_argument{ - "timer period must be less than std::chrono::nanoseconds::max()"}; - } - - const auto period_ns = std::chrono::duration_cast(period); - if (period_ns < std::chrono::nanoseconds::zero()) { - throw std::runtime_error{ - "Casting timer period to nanoseconds resulted in integer overflow."}; - } - - return period_ns; -} -} // namespace detail - /// Create a timer with a given clock /// \internal template @@ -92,13 +41,14 @@ create_timer( CallbackT && callback, rclcpp::CallbackGroup::SharedPtr group = nullptr) { - return create_timer( - node_base.get(), - node_timers.get(), + auto timer = rclcpp::GenericTimer::make_shared( clock, period.to_chrono(), std::forward(callback), - group); + node_base->get_context()); + + node_timers->add_timer(timer, group); + return timer; } /// Create a timer with a given clock @@ -112,99 +62,82 @@ create_timer( rclcpp::CallbackGroup::SharedPtr group = nullptr) { return create_timer( + rclcpp::node_interfaces::get_node_base_interface(node), + rclcpp::node_interfaces::get_node_timers_interface(node), clock, - period.to_chrono(), + period, std::forward(callback), - group, - rclcpp::node_interfaces::get_node_base_interface(node).get(), - rclcpp::node_interfaces::get_node_timers_interface(node).get()); + group); } -/// Convenience method to create a general timer with node resources. +/// Convenience method to create a timer with node resources. /** * * \tparam DurationRepT * \tparam DurationT * \tparam CallbackT - * \param clock clock to be used * \param period period to execute callback. This duration must be 0 <= period < nanoseconds::max() * \param callback callback to execute via the timer period - * \param group callback group - * \param node_base node base interface - * \param node_timers node timer interface - * \return shared pointer to a generic timer - * \throws std::invalid_argument if either clock, node_base or node_timers - * are nullptr, or period is negative or too large + * \param group + * \param node_base + * \param node_timers + * \return + * \throws std::invalid argument if either node_base or node_timers + * are null, or period is negative or too large */ template -typename rclcpp::GenericTimer::SharedPtr -create_timer( - rclcpp::Clock::SharedPtr clock, +typename rclcpp::WallTimer::SharedPtr +create_wall_timer( std::chrono::duration period, CallbackT callback, rclcpp::CallbackGroup::SharedPtr group, node_interfaces::NodeBaseInterface * node_base, node_interfaces::NodeTimersInterface * node_timers) { - if (clock == nullptr) { - throw std::invalid_argument{"clock cannot be null"}; - } if (node_base == nullptr) { throw std::invalid_argument{"input node_base cannot be null"}; } + if (node_timers == nullptr) { throw std::invalid_argument{"input node_timers cannot be null"}; } - const std::chrono::nanoseconds period_ns = detail::safe_cast_to_period_in_ns(period); + if (period < std::chrono::duration::zero()) { + throw std::invalid_argument{"timer period cannot be negative"}; + } - // Add a new generic timer. - auto timer = rclcpp::GenericTimer::make_shared( - std::move(clock), period_ns, std::move(callback), node_base->get_context()); - node_timers->add_timer(timer, group); - return timer; -} + // Casting to a double representation might lose precision and allow the check below to succeed + // but the actual cast to nanoseconds fail. Using 1 DurationT worth of nanoseconds less than max. + constexpr auto maximum_safe_cast_ns = + std::chrono::nanoseconds::max() - std::chrono::duration(1); -/// Convenience method to create a wall timer with node resources. -/** - * - * \tparam DurationRepT - * \tparam DurationT - * \tparam CallbackT - * \param period period to execute callback. This duration must be 0 <= period < nanoseconds::max() - * \param callback callback to execute via the timer period - * \param group callback group - * \param node_base node base interface - * \param node_timers node timer interface - * \return shared pointer to a wall timer - * \throws std::invalid_argument if either node_base or node_timers - * are null, or period is negative or too large - */ -template -typename rclcpp::WallTimer::SharedPtr -create_wall_timer( - std::chrono::duration period, - CallbackT callback, - rclcpp::CallbackGroup::SharedPtr group, - node_interfaces::NodeBaseInterface * node_base, - node_interfaces::NodeTimersInterface * node_timers) -{ - if (node_base == nullptr) { - throw std::invalid_argument{"input node_base cannot be null"}; + // If period is greater than nanoseconds::max(), the duration_cast to nanoseconds will overflow + // a signed integer, which is undefined behavior. Checking whether any std::chrono::duration is + // greater than nanoseconds::max() is a difficult general problem. This is a more conservative + // version of Howard Hinnant's (the guy>) response here: + // https://stackoverflow.com/a/44637334/2089061 + // However, this doesn't solve the issue for all possible duration types of period. + // Follow-up issue: https://github.com/ros2/rclcpp/issues/1177 + constexpr auto ns_max_as_double = + std::chrono::duration_cast>( + maximum_safe_cast_ns); + if (period > ns_max_as_double) { + throw std::invalid_argument{ + "timer period must be less than std::chrono::nanoseconds::max()"}; } - if (node_timers == nullptr) { - throw std::invalid_argument{"input node_timers cannot be null"}; + const auto period_ns = std::chrono::duration_cast(period); + if (period_ns < std::chrono::nanoseconds::zero()) { + throw std::runtime_error{ + "Casting timer period to nanoseconds resulted in integer overflow."}; } - const std::chrono::nanoseconds period_ns = detail::safe_cast_to_period_in_ns(period); - - // Add a new wall timer. auto timer = rclcpp::WallTimer::make_shared( period_ns, std::move(callback), node_base->get_context()); node_timers->add_timer(timer, group); return timer; } + } // namespace rclcpp #endif // RCLCPP__CREATE_TIMER_HPP_ diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index e514137b51..35162de03b 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -227,7 +227,7 @@ class Node : public std::enable_shared_from_this ) ); - /// Create a wall timer that uses the wall clock to drive the callback. + /// Create a timer. /** * \param[in] period Time interval between triggers of the callback. * \param[in] callback User-defined callback function. @@ -240,19 +240,6 @@ class Node : public std::enable_shared_from_this CallbackT callback, rclcpp::CallbackGroup::SharedPtr group = nullptr); - /// Create a timer that uses the node clock to drive the callback. - /** - * \param[in] period Time interval between triggers of the callback. - * \param[in] callback User-defined callback function. - * \param[in] group Callback group to execute this timer's callback in. - */ - template - typename rclcpp::GenericTimer::SharedPtr - create_timer( - std::chrono::duration period, - CallbackT callback, - rclcpp::CallbackGroup::SharedPtr group = nullptr); - /// Create and return a Client. /** * \param[in] service_name The topic to service on. diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index d6b090d4d1..5b427b5b25 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -120,22 +120,6 @@ Node::create_wall_timer( this->node_timers_.get()); } -template -typename rclcpp::GenericTimer::SharedPtr -Node::create_timer( - std::chrono::duration period, - CallbackT callback, - rclcpp::CallbackGroup::SharedPtr group) -{ - return rclcpp::create_timer( - this->get_clock(), - period, - std::move(callback), - group, - this->node_base_.get(), - this->node_timers_.get()); -} - template typename Client::SharedPtr Node::create_client( diff --git a/rclcpp/test/rclcpp/test_create_timer.cpp b/rclcpp/test/rclcpp/test_create_timer.cpp index f253af4838..13c3564544 100644 --- a/rclcpp/test/rclcpp/test_create_timer.cpp +++ b/rclcpp/test/rclcpp/test_create_timer.cpp @@ -62,7 +62,7 @@ TEST(TestCreateTimer, call_with_node_wrapper_compiles) rclcpp::shutdown(); } -TEST(TestCreateWallTimer, call_wall_timer_with_bad_arguments) +TEST(TestCreateTimer, call_wall_timer_with_bad_arguments) { rclcpp::init(0, nullptr); NodeWrapper node("test_create_wall_timers_with_bad_arguments"); @@ -117,66 +117,6 @@ TEST(TestCreateWallTimer, call_wall_timer_with_bad_arguments) rclcpp::shutdown(); } -TEST(TestCreateTimer, call_timer_with_bad_arguments) -{ - rclcpp::init(0, nullptr); - NodeWrapper node("test_create_timers_with_bad_arguments"); - auto callback = []() {}; - rclcpp::CallbackGroup::SharedPtr group = nullptr; - auto node_interface = - rclcpp::node_interfaces::get_node_base_interface(node).get(); - auto timers_interface = - rclcpp::node_interfaces::get_node_timers_interface(node).get(); - - auto clock = node.get_node_clock_interface()->get_clock(); - - // Negative period - EXPECT_THROW( - rclcpp::create_timer( - clock, -1ms, callback, group, node_interface, timers_interface), - std::invalid_argument); - - // Very negative period - constexpr auto nanoseconds_min = std::chrono::nanoseconds::min(); - EXPECT_THROW( - rclcpp::create_timer( - clock, nanoseconds_min, callback, group, node_interface, timers_interface), - std::invalid_argument); - - // Period must be less than nanoseconds::max() - constexpr auto nanoseconds_max = std::chrono::nanoseconds::min(); - EXPECT_THROW( - rclcpp::create_timer( - clock, nanoseconds_max, callback, group, node_interface, timers_interface), - std::invalid_argument); - - EXPECT_NO_THROW( - rclcpp::create_timer( - clock, nanoseconds_max - 1us, callback, group, node_interface, timers_interface)); - - EXPECT_NO_THROW( - rclcpp::create_timer(clock, 0ms, callback, group, node_interface, timers_interface)); - - // Period must be less than nanoseconds::max() - constexpr auto hours_max = std::chrono::hours::max(); - EXPECT_THROW( - rclcpp::create_timer( - clock, hours_max, callback, group, node_interface, timers_interface), - std::invalid_argument); - - // node_interface is null - EXPECT_THROW( - rclcpp::create_timer(clock, 1ms, callback, group, nullptr, timers_interface), - std::invalid_argument); - - // timers_interface is null - EXPECT_THROW( - rclcpp::create_timer(clock, 1ms, callback, group, node_interface, nullptr), - std::invalid_argument); - - rclcpp::shutdown(); -} - static void test_timer_callback(void) {} TEST(TestCreateTimer, timer_function_pointer) diff --git a/rclcpp/test/rclcpp/test_time_source.cpp b/rclcpp/test/rclcpp/test_time_source.cpp index a5aad9a498..2bf89a5b09 100644 --- a/rclcpp/test/rclcpp/test_time_source.cpp +++ b/rclcpp/test/rclcpp/test_time_source.cpp @@ -81,7 +81,7 @@ void spin_until_time( executor.spin_once(10ms); - if (clock->now().nanoseconds() >= end_time.count()) { + if (clock->now().nanoseconds() == end_time.count()) { return; } } @@ -108,7 +108,7 @@ void spin_until_ros_time_updated( executor.add_node(node); auto start = std::chrono::system_clock::now(); - while (std::chrono::system_clock::now() < (start + 2s)) { + while (std::chrono::system_clock::now() < (start + 1s)) { if (!rclcpp::ok()) { break; // Break for ctrl-c } @@ -630,8 +630,7 @@ class SimClockPublisherNode : public rclcpp::Node { public: SimClockPublisherNode() - : rclcpp::Node("sim_clock_publisher_node"), - pub_time_(0, 0) + : rclcpp::Node("sim_clock_publisher_node") { // Create a clock publisher clock_pub_ = this->create_publisher( @@ -646,6 +645,10 @@ class SimClockPublisherNode : public rclcpp::Node &SimClockPublisherNode::timer_callback, this) ); + + // Init clock msg to zero + clock_msg_.clock.sec = 0; + clock_msg_.clock.nanosec = 0; } ~SimClockPublisherNode() @@ -668,15 +671,13 @@ class SimClockPublisherNode : public rclcpp::Node private: void timer_callback() { - // Increment the time, update the clock msg and publish it - pub_time_ += rclcpp::Duration(0, 1000000); - clock_msg_.clock = pub_time_; + // 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_; - rclcpp::Time pub_time_; rosgraph_msgs::msg::Clock clock_msg_; std::thread node_thread_; rclcpp::executors::SingleThreadedExecutor node_executor; @@ -694,7 +695,7 @@ class ClockThreadTestingNode : public rclcpp::Node this->set_parameter(rclcpp::Parameter("use_sim_time", true)); // Create a 100ms timer - timer_ = this->create_timer( + timer_ = this->create_wall_timer( std::chrono::milliseconds(100), std::bind( &ClockThreadTestingNode::timer_callback, @@ -734,33 +735,29 @@ class ClockThreadTestingNode : public rclcpp::Node bool is_callback_frozen_ = true; }; -// TODO(ivanpauno): This test was using a wall timer, when it was supposed to use sim time. -// It was also using `use_clock_tread = false`, when it was supposed to be `true`. -// Fixing the test to work as originally intended makes it super flaky. -// Disabling it until the test is fixed. -// TEST_F(TestTimeSource, check_sim_time_updated_in_callback_if_use_clock_thread) { -// // Test if clock time of a node with -// // parameter use_sim_time = true and option use_clock_thread = true -// // is updated while node is not spinning -// // (in a timer callback) - -// // Create a "sim time" publisher and spin it -// SimClockPublisherNode pub_node; -// pub_node.SpinNode(); - -// // Spin node for 2 seconds -// ClockThreadTestingNode clock_thread_testing_node; -// auto steady_clock = rclcpp::Clock(RCL_STEADY_TIME); -// auto start_time = steady_clock.now(); -// while (rclcpp::ok() && -// (steady_clock.now() - start_time).seconds() < 2.0) -// { -// rclcpp::spin_some(clock_thread_testing_node.get_node_base_interface()); -// } - -// // Node should have get out of timer callback -// ASSERT_FALSE(clock_thread_testing_node.GetIsCallbackFrozen()); -// } +TEST_F(TestTimeSource, check_sim_time_updated_in_callback_if_use_clock_thread) { + // Test if clock time of a node with + // parameter use_sim_time = true and option use_clock_thread = true + // is updated while node is not spinning + // (in a timer callback) + + // Create a "sim time" publisher and spin it + SimClockPublisherNode pub_node; + pub_node.SpinNode(); + + // Spin node for 2 seconds + ClockThreadTestingNode clock_thread_testing_node; + auto steady_clock = rclcpp::Clock(RCL_STEADY_TIME); + auto start_time = steady_clock.now(); + while (rclcpp::ok() && + (steady_clock.now() - start_time).seconds() < 2.0) + { + rclcpp::spin_some(clock_thread_testing_node.get_node_base_interface()); + } + + // Node should have get out of timer callback + ASSERT_FALSE(clock_thread_testing_node.GetIsCallbackFrozen()); +} TEST_F(TestTimeSource, clock_sleep_until_with_ros_time_basic) { SimClockPublisherNode pub_node; diff --git a/rclcpp/test/rclcpp/test_timer.cpp b/rclcpp/test/rclcpp/test_timer.cpp index 59a1218519..7a6599dfe4 100644 --- a/rclcpp/test/rclcpp/test_timer.cpp +++ b/rclcpp/test/rclcpp/test_timer.cpp @@ -30,15 +30,8 @@ using namespace std::chrono_literals; -/// We want to test everything for both the wall and generic timer. -enum class TimerType -{ - WALL_TIMER, - GENERIC_TIMER, -}; - /// Timer testing bring up and teardown -class TestTimer : public ::testing::TestWithParam +class TestTimer : public ::testing::Test { protected: void SetUp() override @@ -51,7 +44,10 @@ class TestTimer : public ::testing::TestWithParam test_node = std::make_shared("test_timer_node"); - auto timer_callback = [this]() -> void { + timer = test_node->create_wall_timer( + 100ms, + [this]() -> void + { this->has_timer_run.store(true); if (this->cancel_timer.load()) { @@ -59,20 +55,10 @@ class TestTimer : public ::testing::TestWithParam } // prevent any tests running timer from blocking this->executor->cancel(); - }; - - // Store the timer type for use in TEST_P declarations. - timer_type = GetParam(); - switch (timer_type) { - case TimerType::WALL_TIMER: - timer = test_node->create_wall_timer(100ms, timer_callback); - EXPECT_TRUE(timer->is_steady()); - break; - case TimerType::GENERIC_TIMER: - timer = test_node->create_timer(100ms, timer_callback); - EXPECT_FALSE(timer->is_steady()); - break; - } + } + ); + EXPECT_TRUE(timer->is_steady()); + executor->add_node(test_node); // don't start spinning, let the test dictate when } @@ -86,7 +72,6 @@ class TestTimer : public ::testing::TestWithParam } // set to true if the timer callback executed, false otherwise - TimerType timer_type; std::atomic has_timer_run; // flag used to cancel the timer in the timer callback. If true cancel the timer, otherwise // cancel the executor (preventing any tests from blocking) @@ -106,7 +91,7 @@ void test_initial_conditions( } /// Simple test -TEST_P(TestTimer, test_simple_cancel) +TEST_F(TestTimer, test_simple_cancel) { // expect clean state, don't run otherwise test_initial_conditions(timer, has_timer_run); @@ -119,7 +104,7 @@ TEST_P(TestTimer, test_simple_cancel) } /// Test state when using reset -TEST_P(TestTimer, test_is_canceled_reset) +TEST_F(TestTimer, test_is_canceled_reset) { // expect clean state, don't run otherwise test_initial_conditions(timer, has_timer_run); @@ -144,7 +129,7 @@ TEST_P(TestTimer, test_is_canceled_reset) } /// Run and check state, cancel the executor -TEST_P(TestTimer, test_run_cancel_executor) +TEST_F(TestTimer, test_run_cancel_executor) { // expect clean state, don't run otherwise test_initial_conditions(timer, has_timer_run); @@ -161,7 +146,7 @@ TEST_P(TestTimer, test_run_cancel_executor) } /// Run and check state, cancel the timer -TEST_P(TestTimer, test_run_cancel_timer) +TEST_F(TestTimer, test_run_cancel_timer) { // expect clean state, don't run otherwise test_initial_conditions(timer, has_timer_run); @@ -174,7 +159,7 @@ TEST_P(TestTimer, test_run_cancel_timer) EXPECT_TRUE(timer->is_canceled()); } -TEST_P(TestTimer, test_bad_arguments) { +TEST_F(TestTimer, test_bad_arguments) { auto node_base = rclcpp::node_interfaces::get_node_base_interface(test_node); auto context = node_base->get_context(); @@ -213,19 +198,13 @@ TEST_P(TestTimer, test_bad_arguments) { rclcpp::exceptions::RCLError); } -TEST_P(TestTimer, callback_with_timer) { +TEST_F(TestTimer, callback_with_timer) { rclcpp::TimerBase * timer_ptr = nullptr; - auto timer_callback = [&timer_ptr](rclcpp::TimerBase & timer) { + timer = test_node->create_wall_timer( + std::chrono::milliseconds(1), + [&timer_ptr](rclcpp::TimerBase & timer) { timer_ptr = &timer; - }; - switch (timer_type) { - case TimerType::WALL_TIMER: - timer = test_node->create_wall_timer(1ms, timer_callback); - break; - case TimerType::GENERIC_TIMER: - timer = test_node->create_timer(1ms, timer_callback); - break; - } + }); auto start = std::chrono::steady_clock::now(); while (nullptr == timer_ptr && (std::chrono::steady_clock::now() - start) < std::chrono::milliseconds(100)) @@ -237,19 +216,13 @@ TEST_P(TestTimer, callback_with_timer) { EXPECT_FALSE(timer_ptr->is_ready()); } -TEST_P(TestTimer, callback_with_period_zero) { +TEST_F(TestTimer, callback_with_period_zero) { rclcpp::TimerBase * timer_ptr = nullptr; - auto timer_callback = [&timer_ptr](rclcpp::TimerBase & timer) { + timer = test_node->create_wall_timer( + std::chrono::milliseconds(0), + [&timer_ptr](rclcpp::TimerBase & timer) { timer_ptr = &timer; - }; - switch (timer_type) { - case TimerType::WALL_TIMER: - timer = test_node->create_wall_timer(0ms, timer_callback); - break; - case TimerType::GENERIC_TIMER: - timer = test_node->create_timer(0ms, timer_callback); - break; - } + }); auto start = std::chrono::steady_clock::now(); while (nullptr == timer_ptr && (std::chrono::steady_clock::now() - start) < std::chrono::milliseconds(100)) @@ -262,7 +235,7 @@ TEST_P(TestTimer, callback_with_period_zero) { } /// Test internal failures using mocks -TEST_P(TestTimer, test_failures_with_exceptions) +TEST_F(TestTimer, test_failures_with_exceptions) { // expect clean state, don't run otherwise test_initial_conditions(timer, has_timer_run); @@ -272,16 +245,8 @@ TEST_P(TestTimer, test_failures_with_exceptions) auto mock = mocking_utils::inject_on_return("lib:rclcpp", rcl_timer_fini, RCL_RET_ERROR); EXPECT_NO_THROW( { - switch (timer_type) { - case TimerType::WALL_TIMER: - timer_to_test_destructor = - test_node->create_wall_timer(std::chrono::milliseconds(0), [](void) {}); - break; - case TimerType::GENERIC_TIMER: - timer_to_test_destructor = - test_node->create_timer(std::chrono::milliseconds(0), [](void) {}); - break; - } + timer_to_test_destructor = + test_node->create_wall_timer(std::chrono::milliseconds(0), [](void) {}); timer_to_test_destructor.reset(); }); } @@ -318,19 +283,3 @@ TEST_P(TestTimer, test_failures_with_exceptions) std::runtime_error("Timer could not get time until next call: error not set")); } } - -INSTANTIATE_TEST_SUITE_P( - PerTimerType, TestTimer, - ::testing::Values(TimerType::WALL_TIMER, TimerType::GENERIC_TIMER), - [](const ::testing::TestParamInfo & info) -> std::string { - switch (info.param) { - case TimerType::WALL_TIMER: - return std::string("wall_timer"); - case TimerType::GENERIC_TIMER: - return std::string("generic_timer"); - default: - break; - } - return std::string("unknown"); - } -); diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 5e06876018..84c62b9e6a 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -242,7 +242,7 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, ) ); - /// Create a timer that uses the wall clock to drive the callback. + /// Create a timer. /** * \param[in] period Time interval between triggers of the callback. * \param[in] callback User-defined callback function. @@ -255,19 +255,6 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, CallbackT callback, rclcpp::CallbackGroup::SharedPtr group = nullptr); - /// Create a timer that uses the node clock to drive the callback. - /** - * \param[in] period Time interval between triggers of the callback. - * \param[in] callback User-defined callback function. - * \param[in] group Callback group to execute this timer's callback in. - */ - template - typename rclcpp::GenericTimer::SharedPtr - create_timer( - std::chrono::duration period, - CallbackT callback, - rclcpp::CallbackGroup::SharedPtr group = nullptr); - /// Create and return a Client. /** * \sa rclcpp::Node::create_client diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp index e2a5696b92..88d981e051 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp @@ -90,28 +90,11 @@ LifecycleNode::create_wall_timer( CallbackT callback, rclcpp::CallbackGroup::SharedPtr group) { - return rclcpp::create_wall_timer( - period, - std::move(callback), - group, - this->node_base_.get(), - this->node_timers_.get()); -} - -template -typename rclcpp::GenericTimer::SharedPtr -LifecycleNode::create_timer( - std::chrono::duration period, - CallbackT callback, - rclcpp::CallbackGroup::SharedPtr group) -{ - return rclcpp::create_timer( - this->get_clock(), - period, - std::move(callback), - group, - this->node_base_.get(), - this->node_timers_.get()); + auto timer = rclcpp::WallTimer::make_shared( + std::chrono::duration_cast(period), + std::move(callback), this->node_base_->get_context()); + node_timers_->add_timer(timer, group); + return timer; } template diff --git a/rclcpp_lifecycle/test/test_lifecycle_publisher.cpp b/rclcpp_lifecycle/test/test_lifecycle_publisher.cpp index d10544ef1f..2a5d08f728 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_publisher.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_publisher.cpp @@ -42,17 +42,10 @@ class TestDefaultStateMachine : public ::testing::Test } }; -/// We want to test everything for both the wall and generic timer. -enum class TimerType -{ - WALL_TIMER, - GENERIC_TIMER, -}; - class EmptyLifecycleNode : public rclcpp_lifecycle::LifecycleNode { public: - explicit EmptyLifecycleNode(const std::string & node_name, const TimerType & timer_type) + explicit EmptyLifecycleNode(const std::string & node_name) : rclcpp_lifecycle::LifecycleNode(node_name) { rclcpp::PublisherOptionsWithAllocator> options; @@ -62,20 +55,8 @@ class EmptyLifecycleNode : public rclcpp_lifecycle::LifecycleNode add_managed_entity(publisher_); // For coverage this is being added here - switch (timer_type) { - case TimerType::WALL_TIMER: - { - auto timer = create_wall_timer(std::chrono::seconds(1), []() {}); - add_timer_handle(timer); - break; - } - case TimerType::GENERIC_TIMER: - { - auto timer = create_timer(std::chrono::seconds(1), []() {}); - add_timer_handle(timer); - break; - } - } + auto timer = create_wall_timer(std::chrono::seconds(1), []() {}); + add_timer_handle(timer); } std::shared_ptr> publisher() @@ -87,13 +68,13 @@ class EmptyLifecycleNode : public rclcpp_lifecycle::LifecycleNode std::shared_ptr> publisher_; }; -class TestLifecyclePublisher : public ::testing::TestWithParam +class TestLifecyclePublisher : public ::testing::Test { public: void SetUp() { rclcpp::init(0, nullptr); - node_ = std::make_shared("node", GetParam()); + node_ = std::make_shared("node"); } void TearDown() @@ -105,7 +86,7 @@ class TestLifecyclePublisher : public ::testing::TestWithParam std::shared_ptr node_; }; -TEST_P(TestLifecyclePublisher, publish_managed_by_node) { +TEST_F(TestLifecyclePublisher, publish_managed_by_node) { // transition via LifecycleNode auto success = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; auto reset_key = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; @@ -144,7 +125,7 @@ TEST_P(TestLifecyclePublisher, publish_managed_by_node) { } } -TEST_P(TestLifecyclePublisher, publish) { +TEST_F(TestLifecyclePublisher, publish) { // transition via LifecyclePublisher node_->publisher()->on_deactivate(); EXPECT_FALSE(node_->publisher()->is_activated()); @@ -167,19 +148,3 @@ TEST_P(TestLifecyclePublisher, publish) { EXPECT_NO_THROW(node_->publisher()->publish(std::move(msg_ptr))); } } - -INSTANTIATE_TEST_SUITE_P( - PerTimerType, TestLifecyclePublisher, - ::testing::Values(TimerType::WALL_TIMER, TimerType::GENERIC_TIMER), - [](const ::testing::TestParamInfo & info) -> std::string { - switch (info.param) { - case TimerType::WALL_TIMER: - return std::string("wall_timer"); - case TimerType::GENERIC_TIMER: - return std::string("generic_timer"); - default: - break; - } - return std::string("unknown"); - } -); From 93222cc2cde3e05f89673a4a8450575300c9c33a Mon Sep 17 00:00:00 2001 From: schrodinbug Date: Wed, 31 Aug 2022 08:40:11 -0400 Subject: [PATCH 11/53] force compiler warning if callback handles not captured (#2000) clarify documentation to show that not capturing returned handles will result in callbacks immediately being unregistered Signed-off-by: Jason Beach --- .../rclcpp/parameter_event_handler.hpp | 23 +++++++++++++++---- 1 file changed, 19 insertions(+), 4 deletions(-) diff --git a/rclcpp/include/rclcpp/parameter_event_handler.hpp b/rclcpp/include/rclcpp/parameter_event_handler.hpp index 52a0233966..93b8177fb2 100644 --- a/rclcpp/include/rclcpp/parameter_event_handler.hpp +++ b/rclcpp/include/rclcpp/parameter_event_handler.hpp @@ -86,6 +86,9 @@ struct ParameterEventCallbackHandle * the ROS node supplied in the ParameterEventHandler constructor. * The callback, a lambda function in this case, simply prints out the value of the parameter. * + * Note: the object returned from add_parameter_callback must be captured or the callback will + * be immediately unregistered. + * * You may also monitor for changes to parameters in other nodes by supplying the node * name to add_parameter_callback: * @@ -103,8 +106,8 @@ struct ParameterEventCallbackHandle * In this case, the callback will be invoked whenever "some_remote_param_name" changes * on remote node "some_remote_node_name". * - * To remove a parameter callback, call remove_parameter_callback, passing the handle returned - * from add_parameter_callback: + * To remove a parameter callback, reset the callback handle smart pointer or call + * remove_parameter_callback, passing the handle returned from add_parameter_callback: * * param_handler->remove_parameter_callback(handle2); * @@ -152,9 +155,12 @@ struct ParameterEventCallbackHandle * For both parameter callbacks and parameter event callbacks, when multiple callbacks are added, * the callbacks are invoked last-in, first-called order (LIFO). * - * To remove a parameter event callback, use: + * Note: the callback handle returned from add_parameter_event_callback must be captured or + * the callback will immediately be unregistered. + * + * To remove a parameter event callback, reset the callback smart pointer or use: * - * param_handler->remove_event_parameter_callback(handle); + * param_handler->remove_event_parameter_callback(handle3); */ class ParameterEventHandler { @@ -189,10 +195,14 @@ class ParameterEventHandler /** * This function may be called multiple times to set multiple parameter event callbacks. * + * Note: if the returned callback handle smart pointer is not captured, the callback is + * immediatedly unregistered. A compiler warning should be generated to warn of this. + * * \param[in] callback Function callback to be invoked on parameter updates. * \returns A handle used to refer to the callback. */ RCLCPP_PUBLIC + RCUTILS_WARN_UNUSED ParameterEventCallbackHandle::SharedPtr add_parameter_event_callback( ParameterEventCallbackType callback); @@ -212,12 +222,17 @@ class ParameterEventHandler /** * If a node_name is not provided, defaults to the current node. * + * Note: if the returned callback handle smart pointer is not captured, the callback + * is immediately unregistered. A compiler warning should be generated to warn + * of this. + * * \param[in] parameter_name Name of parameter to monitor. * \param[in] callback Function callback to be invoked upon parameter update. * \param[in] node_name Name of node which hosts the parameter. * \returns A handle used to refer to the callback. */ RCLCPP_PUBLIC + RCUTILS_WARN_UNUSED ParameterCallbackHandle::SharedPtr add_parameter_callback( const std::string & parameter_name, From df994e435df6cbdb19ab7ada839f403367fb769a Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 31 Aug 2022 10:21:13 -0300 Subject: [PATCH 12/53] Revert "Revert "Add a create_timer method to Node and `LifecycleNode` classes (#1975)" (#2009) (#2010) Signed-off-by: Ivan Santiago Paunovic --- rclcpp/include/rclcpp/create_timer.hpp | 155 +++++++++++++----- rclcpp/include/rclcpp/node.hpp | 15 +- rclcpp/include/rclcpp/node_impl.hpp | 16 ++ rclcpp/test/rclcpp/test_create_timer.cpp | 62 ++++++- rclcpp/test/rclcpp/test_time_source.cpp | 69 ++++---- rclcpp/test/rclcpp/test_timer.cpp | 105 +++++++++--- .../rclcpp_lifecycle/lifecycle_node.hpp | 15 +- .../rclcpp_lifecycle/lifecycle_node_impl.hpp | 27 ++- .../test/test_lifecycle_publisher.cpp | 49 +++++- 9 files changed, 394 insertions(+), 119 deletions(-) diff --git a/rclcpp/include/rclcpp/create_timer.hpp b/rclcpp/include/rclcpp/create_timer.hpp index 345f43fcbb..d371466f0d 100644 --- a/rclcpp/include/rclcpp/create_timer.hpp +++ b/rclcpp/include/rclcpp/create_timer.hpp @@ -23,12 +23,63 @@ #include "rclcpp/duration.hpp" #include "rclcpp/node_interfaces/get_node_base_interface.hpp" +#include "rclcpp/node_interfaces/get_node_clock_interface.hpp" #include "rclcpp/node_interfaces/get_node_timers_interface.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_clock_interface.hpp" #include "rclcpp/node_interfaces/node_timers_interface.hpp" namespace rclcpp { +namespace detail +{ +/// Perform a safe cast to a timer period in nanoseconds +/** + * + * \tparam DurationRepT + * \tparam DurationT + * \param period period to execute callback. This duration must be 0 <= period < nanoseconds::max() + * \return period, expressed as chrono::duration::nanoseconds + * \throws std::invalid_argument if period is negative or too large + */ +template +std::chrono::nanoseconds +safe_cast_to_period_in_ns(std::chrono::duration period) +{ + if (period < std::chrono::duration::zero()) { + throw std::invalid_argument{"timer period cannot be negative"}; + } + + // Casting to a double representation might lose precision and allow the check below to succeed + // but the actual cast to nanoseconds fail. Using 1 DurationT worth of nanoseconds less than max. + constexpr auto maximum_safe_cast_ns = + std::chrono::nanoseconds::max() - std::chrono::duration(1); + + // If period is greater than nanoseconds::max(), the duration_cast to nanoseconds will overflow + // a signed integer, which is undefined behavior. Checking whether any std::chrono::duration is + // greater than nanoseconds::max() is a difficult general problem. This is a more conservative + // version of Howard Hinnant's (the guy>) response here: + // https://stackoverflow.com/a/44637334/2089061 + // However, this doesn't solve the issue for all possible duration types of period. + // Follow-up issue: https://github.com/ros2/rclcpp/issues/1177 + constexpr auto ns_max_as_double = + std::chrono::duration_cast>( + maximum_safe_cast_ns); + if (period > ns_max_as_double) { + throw std::invalid_argument{ + "timer period must be less than std::chrono::nanoseconds::max()"}; + } + + const auto period_ns = std::chrono::duration_cast(period); + if (period_ns < std::chrono::nanoseconds::zero()) { + throw std::runtime_error{ + "Casting timer period to nanoseconds resulted in integer overflow."}; + } + + return period_ns; +} +} // namespace detail + /// Create a timer with a given clock /// \internal template @@ -41,14 +92,13 @@ create_timer( CallbackT && callback, rclcpp::CallbackGroup::SharedPtr group = nullptr) { - auto timer = rclcpp::GenericTimer::make_shared( + return create_timer( clock, period.to_chrono(), std::forward(callback), - node_base->get_context()); - - node_timers->add_timer(timer, group); - return timer; + group, + node_base.get(), + node_timers.get()); } /// Create a timer with a given clock @@ -62,82 +112,99 @@ create_timer( rclcpp::CallbackGroup::SharedPtr group = nullptr) { return create_timer( - rclcpp::node_interfaces::get_node_base_interface(node), - rclcpp::node_interfaces::get_node_timers_interface(node), clock, - period, + period.to_chrono(), std::forward(callback), - group); + group, + rclcpp::node_interfaces::get_node_base_interface(node).get(), + rclcpp::node_interfaces::get_node_timers_interface(node).get()); } -/// Convenience method to create a timer with node resources. +/// Convenience method to create a general timer with node resources. /** * * \tparam DurationRepT * \tparam DurationT * \tparam CallbackT + * \param clock clock to be used * \param period period to execute callback. This duration must be 0 <= period < nanoseconds::max() * \param callback callback to execute via the timer period - * \param group - * \param node_base - * \param node_timers - * \return - * \throws std::invalid argument if either node_base or node_timers - * are null, or period is negative or too large + * \param group callback group + * \param node_base node base interface + * \param node_timers node timer interface + * \return shared pointer to a generic timer + * \throws std::invalid_argument if either clock, node_base or node_timers + * are nullptr, or period is negative or too large */ template -typename rclcpp::WallTimer::SharedPtr -create_wall_timer( +typename rclcpp::GenericTimer::SharedPtr +create_timer( + rclcpp::Clock::SharedPtr clock, std::chrono::duration period, CallbackT callback, rclcpp::CallbackGroup::SharedPtr group, node_interfaces::NodeBaseInterface * node_base, node_interfaces::NodeTimersInterface * node_timers) { + if (clock == nullptr) { + throw std::invalid_argument{"clock cannot be null"}; + } if (node_base == nullptr) { throw std::invalid_argument{"input node_base cannot be null"}; } - if (node_timers == nullptr) { throw std::invalid_argument{"input node_timers cannot be null"}; } - if (period < std::chrono::duration::zero()) { - throw std::invalid_argument{"timer period cannot be negative"}; - } + const std::chrono::nanoseconds period_ns = detail::safe_cast_to_period_in_ns(period); - // Casting to a double representation might lose precision and allow the check below to succeed - // but the actual cast to nanoseconds fail. Using 1 DurationT worth of nanoseconds less than max. - constexpr auto maximum_safe_cast_ns = - std::chrono::nanoseconds::max() - std::chrono::duration(1); + // Add a new generic timer. + auto timer = rclcpp::GenericTimer::make_shared( + std::move(clock), period_ns, std::move(callback), node_base->get_context()); + node_timers->add_timer(timer, group); + return timer; +} - // If period is greater than nanoseconds::max(), the duration_cast to nanoseconds will overflow - // a signed integer, which is undefined behavior. Checking whether any std::chrono::duration is - // greater than nanoseconds::max() is a difficult general problem. This is a more conservative - // version of Howard Hinnant's (the guy>) response here: - // https://stackoverflow.com/a/44637334/2089061 - // However, this doesn't solve the issue for all possible duration types of period. - // Follow-up issue: https://github.com/ros2/rclcpp/issues/1177 - constexpr auto ns_max_as_double = - std::chrono::duration_cast>( - maximum_safe_cast_ns); - if (period > ns_max_as_double) { - throw std::invalid_argument{ - "timer period must be less than std::chrono::nanoseconds::max()"}; +/// Convenience method to create a wall timer with node resources. +/** + * + * \tparam DurationRepT + * \tparam DurationT + * \tparam CallbackT + * \param period period to execute callback. This duration must be 0 <= period < nanoseconds::max() + * \param callback callback to execute via the timer period + * \param group callback group + * \param node_base node base interface + * \param node_timers node timer interface + * \return shared pointer to a wall timer + * \throws std::invalid_argument if either node_base or node_timers + * are null, or period is negative or too large + */ +template +typename rclcpp::WallTimer::SharedPtr +create_wall_timer( + std::chrono::duration period, + CallbackT callback, + rclcpp::CallbackGroup::SharedPtr group, + node_interfaces::NodeBaseInterface * node_base, + node_interfaces::NodeTimersInterface * node_timers) +{ + if (node_base == nullptr) { + throw std::invalid_argument{"input node_base cannot be null"}; } - const auto period_ns = std::chrono::duration_cast(period); - if (period_ns < std::chrono::nanoseconds::zero()) { - throw std::runtime_error{ - "Casting timer period to nanoseconds resulted in integer overflow."}; + if (node_timers == nullptr) { + throw std::invalid_argument{"input node_timers cannot be null"}; } + const std::chrono::nanoseconds period_ns = detail::safe_cast_to_period_in_ns(period); + + // Add a new wall timer. auto timer = rclcpp::WallTimer::make_shared( period_ns, std::move(callback), node_base->get_context()); node_timers->add_timer(timer, group); return timer; } - } // namespace rclcpp #endif // RCLCPP__CREATE_TIMER_HPP_ diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 35162de03b..e514137b51 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -227,7 +227,7 @@ class Node : public std::enable_shared_from_this ) ); - /// Create a timer. + /// Create a wall timer that uses the wall clock to drive the callback. /** * \param[in] period Time interval between triggers of the callback. * \param[in] callback User-defined callback function. @@ -240,6 +240,19 @@ class Node : public std::enable_shared_from_this CallbackT callback, rclcpp::CallbackGroup::SharedPtr group = nullptr); + /// Create a timer that uses the node clock to drive the callback. + /** + * \param[in] period Time interval between triggers of the callback. + * \param[in] callback User-defined callback function. + * \param[in] group Callback group to execute this timer's callback in. + */ + template + typename rclcpp::GenericTimer::SharedPtr + create_timer( + std::chrono::duration period, + CallbackT callback, + rclcpp::CallbackGroup::SharedPtr group = nullptr); + /// Create and return a Client. /** * \param[in] service_name The topic to service on. diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 5b427b5b25..d6b090d4d1 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -120,6 +120,22 @@ Node::create_wall_timer( this->node_timers_.get()); } +template +typename rclcpp::GenericTimer::SharedPtr +Node::create_timer( + std::chrono::duration period, + CallbackT callback, + rclcpp::CallbackGroup::SharedPtr group) +{ + return rclcpp::create_timer( + this->get_clock(), + period, + std::move(callback), + group, + this->node_base_.get(), + this->node_timers_.get()); +} + template typename Client::SharedPtr Node::create_client( diff --git a/rclcpp/test/rclcpp/test_create_timer.cpp b/rclcpp/test/rclcpp/test_create_timer.cpp index 13c3564544..f253af4838 100644 --- a/rclcpp/test/rclcpp/test_create_timer.cpp +++ b/rclcpp/test/rclcpp/test_create_timer.cpp @@ -62,7 +62,7 @@ TEST(TestCreateTimer, call_with_node_wrapper_compiles) rclcpp::shutdown(); } -TEST(TestCreateTimer, call_wall_timer_with_bad_arguments) +TEST(TestCreateWallTimer, call_wall_timer_with_bad_arguments) { rclcpp::init(0, nullptr); NodeWrapper node("test_create_wall_timers_with_bad_arguments"); @@ -117,6 +117,66 @@ TEST(TestCreateTimer, call_wall_timer_with_bad_arguments) rclcpp::shutdown(); } +TEST(TestCreateTimer, call_timer_with_bad_arguments) +{ + rclcpp::init(0, nullptr); + NodeWrapper node("test_create_timers_with_bad_arguments"); + auto callback = []() {}; + rclcpp::CallbackGroup::SharedPtr group = nullptr; + auto node_interface = + rclcpp::node_interfaces::get_node_base_interface(node).get(); + auto timers_interface = + rclcpp::node_interfaces::get_node_timers_interface(node).get(); + + auto clock = node.get_node_clock_interface()->get_clock(); + + // Negative period + EXPECT_THROW( + rclcpp::create_timer( + clock, -1ms, callback, group, node_interface, timers_interface), + std::invalid_argument); + + // Very negative period + constexpr auto nanoseconds_min = std::chrono::nanoseconds::min(); + EXPECT_THROW( + rclcpp::create_timer( + clock, nanoseconds_min, callback, group, node_interface, timers_interface), + std::invalid_argument); + + // Period must be less than nanoseconds::max() + constexpr auto nanoseconds_max = std::chrono::nanoseconds::min(); + EXPECT_THROW( + rclcpp::create_timer( + clock, nanoseconds_max, callback, group, node_interface, timers_interface), + std::invalid_argument); + + EXPECT_NO_THROW( + rclcpp::create_timer( + clock, nanoseconds_max - 1us, callback, group, node_interface, timers_interface)); + + EXPECT_NO_THROW( + rclcpp::create_timer(clock, 0ms, callback, group, node_interface, timers_interface)); + + // Period must be less than nanoseconds::max() + constexpr auto hours_max = std::chrono::hours::max(); + EXPECT_THROW( + rclcpp::create_timer( + clock, hours_max, callback, group, node_interface, timers_interface), + std::invalid_argument); + + // node_interface is null + EXPECT_THROW( + rclcpp::create_timer(clock, 1ms, callback, group, nullptr, timers_interface), + std::invalid_argument); + + // timers_interface is null + EXPECT_THROW( + rclcpp::create_timer(clock, 1ms, callback, group, node_interface, nullptr), + std::invalid_argument); + + rclcpp::shutdown(); +} + static void test_timer_callback(void) {} TEST(TestCreateTimer, timer_function_pointer) diff --git a/rclcpp/test/rclcpp/test_time_source.cpp b/rclcpp/test/rclcpp/test_time_source.cpp index 2bf89a5b09..a5aad9a498 100644 --- a/rclcpp/test/rclcpp/test_time_source.cpp +++ b/rclcpp/test/rclcpp/test_time_source.cpp @@ -81,7 +81,7 @@ void spin_until_time( executor.spin_once(10ms); - if (clock->now().nanoseconds() == end_time.count()) { + if (clock->now().nanoseconds() >= end_time.count()) { return; } } @@ -108,7 +108,7 @@ void spin_until_ros_time_updated( executor.add_node(node); auto start = std::chrono::system_clock::now(); - while (std::chrono::system_clock::now() < (start + 1s)) { + while (std::chrono::system_clock::now() < (start + 2s)) { if (!rclcpp::ok()) { break; // Break for ctrl-c } @@ -630,7 +630,8 @@ class SimClockPublisherNode : public rclcpp::Node { public: SimClockPublisherNode() - : rclcpp::Node("sim_clock_publisher_node") + : rclcpp::Node("sim_clock_publisher_node"), + pub_time_(0, 0) { // Create a clock publisher clock_pub_ = this->create_publisher( @@ -645,10 +646,6 @@ class SimClockPublisherNode : public rclcpp::Node &SimClockPublisherNode::timer_callback, this) ); - - // Init clock msg to zero - clock_msg_.clock.sec = 0; - clock_msg_.clock.nanosec = 0; } ~SimClockPublisherNode() @@ -671,13 +668,15 @@ class SimClockPublisherNode : public rclcpp::Node private: void timer_callback() { - // Increment clock msg and publish it - clock_msg_.clock.nanosec += 1000000; + // Increment the time, update the clock msg and publish it + pub_time_ += rclcpp::Duration(0, 1000000); + clock_msg_.clock = pub_time_; clock_pub_->publish(clock_msg_); } rclcpp::Publisher::SharedPtr clock_pub_; rclcpp::TimerBase::SharedPtr pub_timer_; + rclcpp::Time pub_time_; rosgraph_msgs::msg::Clock clock_msg_; std::thread node_thread_; rclcpp::executors::SingleThreadedExecutor node_executor; @@ -695,7 +694,7 @@ class ClockThreadTestingNode : public rclcpp::Node this->set_parameter(rclcpp::Parameter("use_sim_time", true)); // Create a 100ms timer - timer_ = this->create_wall_timer( + timer_ = this->create_timer( std::chrono::milliseconds(100), std::bind( &ClockThreadTestingNode::timer_callback, @@ -735,29 +734,33 @@ class ClockThreadTestingNode : public rclcpp::Node bool is_callback_frozen_ = true; }; -TEST_F(TestTimeSource, check_sim_time_updated_in_callback_if_use_clock_thread) { - // Test if clock time of a node with - // parameter use_sim_time = true and option use_clock_thread = true - // is updated while node is not spinning - // (in a timer callback) - - // Create a "sim time" publisher and spin it - 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()); -} +// TODO(ivanpauno): This test was using a wall timer, when it was supposed to use sim time. +// It was also using `use_clock_tread = false`, when it was supposed to be `true`. +// Fixing the test to work as originally intended makes it super flaky. +// Disabling it until the test is fixed. +// TEST_F(TestTimeSource, check_sim_time_updated_in_callback_if_use_clock_thread) { +// // Test if clock time of a node with +// // parameter use_sim_time = true and option use_clock_thread = true +// // is updated while node is not spinning +// // (in a timer callback) + +// // Create a "sim time" publisher and spin it +// SimClockPublisherNode pub_node; +// pub_node.SpinNode(); + +// // Spin node for 2 seconds +// ClockThreadTestingNode clock_thread_testing_node; +// auto steady_clock = rclcpp::Clock(RCL_STEADY_TIME); +// auto start_time = steady_clock.now(); +// while (rclcpp::ok() && +// (steady_clock.now() - start_time).seconds() < 2.0) +// { +// rclcpp::spin_some(clock_thread_testing_node.get_node_base_interface()); +// } + +// // Node should have get out of timer callback +// ASSERT_FALSE(clock_thread_testing_node.GetIsCallbackFrozen()); +// } TEST_F(TestTimeSource, clock_sleep_until_with_ros_time_basic) { SimClockPublisherNode pub_node; diff --git a/rclcpp/test/rclcpp/test_timer.cpp b/rclcpp/test/rclcpp/test_timer.cpp index 7a6599dfe4..59a1218519 100644 --- a/rclcpp/test/rclcpp/test_timer.cpp +++ b/rclcpp/test/rclcpp/test_timer.cpp @@ -30,8 +30,15 @@ using namespace std::chrono_literals; +/// We want to test everything for both the wall and generic timer. +enum class TimerType +{ + WALL_TIMER, + GENERIC_TIMER, +}; + /// Timer testing bring up and teardown -class TestTimer : public ::testing::Test +class TestTimer : public ::testing::TestWithParam { protected: void SetUp() override @@ -44,10 +51,7 @@ class TestTimer : public ::testing::Test test_node = std::make_shared("test_timer_node"); - timer = test_node->create_wall_timer( - 100ms, - [this]() -> void - { + auto timer_callback = [this]() -> void { this->has_timer_run.store(true); if (this->cancel_timer.load()) { @@ -55,10 +59,20 @@ class TestTimer : public ::testing::Test } // prevent any tests running timer from blocking this->executor->cancel(); - } - ); - EXPECT_TRUE(timer->is_steady()); - + }; + + // Store the timer type for use in TEST_P declarations. + timer_type = GetParam(); + switch (timer_type) { + case TimerType::WALL_TIMER: + timer = test_node->create_wall_timer(100ms, timer_callback); + EXPECT_TRUE(timer->is_steady()); + break; + case TimerType::GENERIC_TIMER: + timer = test_node->create_timer(100ms, timer_callback); + EXPECT_FALSE(timer->is_steady()); + break; + } executor->add_node(test_node); // don't start spinning, let the test dictate when } @@ -72,6 +86,7 @@ class TestTimer : public ::testing::Test } // set to true if the timer callback executed, false otherwise + TimerType timer_type; std::atomic has_timer_run; // flag used to cancel the timer in the timer callback. If true cancel the timer, otherwise // cancel the executor (preventing any tests from blocking) @@ -91,7 +106,7 @@ void test_initial_conditions( } /// Simple test -TEST_F(TestTimer, test_simple_cancel) +TEST_P(TestTimer, test_simple_cancel) { // expect clean state, don't run otherwise test_initial_conditions(timer, has_timer_run); @@ -104,7 +119,7 @@ TEST_F(TestTimer, test_simple_cancel) } /// Test state when using reset -TEST_F(TestTimer, test_is_canceled_reset) +TEST_P(TestTimer, test_is_canceled_reset) { // expect clean state, don't run otherwise test_initial_conditions(timer, has_timer_run); @@ -129,7 +144,7 @@ TEST_F(TestTimer, test_is_canceled_reset) } /// Run and check state, cancel the executor -TEST_F(TestTimer, test_run_cancel_executor) +TEST_P(TestTimer, test_run_cancel_executor) { // expect clean state, don't run otherwise test_initial_conditions(timer, has_timer_run); @@ -146,7 +161,7 @@ TEST_F(TestTimer, test_run_cancel_executor) } /// Run and check state, cancel the timer -TEST_F(TestTimer, test_run_cancel_timer) +TEST_P(TestTimer, test_run_cancel_timer) { // expect clean state, don't run otherwise test_initial_conditions(timer, has_timer_run); @@ -159,7 +174,7 @@ TEST_F(TestTimer, test_run_cancel_timer) EXPECT_TRUE(timer->is_canceled()); } -TEST_F(TestTimer, test_bad_arguments) { +TEST_P(TestTimer, test_bad_arguments) { auto node_base = rclcpp::node_interfaces::get_node_base_interface(test_node); auto context = node_base->get_context(); @@ -198,13 +213,19 @@ TEST_F(TestTimer, test_bad_arguments) { rclcpp::exceptions::RCLError); } -TEST_F(TestTimer, callback_with_timer) { +TEST_P(TestTimer, callback_with_timer) { rclcpp::TimerBase * timer_ptr = nullptr; - timer = test_node->create_wall_timer( - std::chrono::milliseconds(1), - [&timer_ptr](rclcpp::TimerBase & timer) { + auto timer_callback = [&timer_ptr](rclcpp::TimerBase & timer) { timer_ptr = &timer; - }); + }; + switch (timer_type) { + case TimerType::WALL_TIMER: + timer = test_node->create_wall_timer(1ms, timer_callback); + break; + case TimerType::GENERIC_TIMER: + timer = test_node->create_timer(1ms, timer_callback); + break; + } auto start = std::chrono::steady_clock::now(); while (nullptr == timer_ptr && (std::chrono::steady_clock::now() - start) < std::chrono::milliseconds(100)) @@ -216,13 +237,19 @@ TEST_F(TestTimer, callback_with_timer) { EXPECT_FALSE(timer_ptr->is_ready()); } -TEST_F(TestTimer, callback_with_period_zero) { +TEST_P(TestTimer, callback_with_period_zero) { rclcpp::TimerBase * timer_ptr = nullptr; - timer = test_node->create_wall_timer( - std::chrono::milliseconds(0), - [&timer_ptr](rclcpp::TimerBase & timer) { + auto timer_callback = [&timer_ptr](rclcpp::TimerBase & timer) { timer_ptr = &timer; - }); + }; + switch (timer_type) { + case TimerType::WALL_TIMER: + timer = test_node->create_wall_timer(0ms, timer_callback); + break; + case TimerType::GENERIC_TIMER: + timer = test_node->create_timer(0ms, timer_callback); + break; + } auto start = std::chrono::steady_clock::now(); while (nullptr == timer_ptr && (std::chrono::steady_clock::now() - start) < std::chrono::milliseconds(100)) @@ -235,7 +262,7 @@ TEST_F(TestTimer, callback_with_period_zero) { } /// Test internal failures using mocks -TEST_F(TestTimer, test_failures_with_exceptions) +TEST_P(TestTimer, test_failures_with_exceptions) { // expect clean state, don't run otherwise test_initial_conditions(timer, has_timer_run); @@ -245,8 +272,16 @@ TEST_F(TestTimer, test_failures_with_exceptions) auto mock = mocking_utils::inject_on_return("lib:rclcpp", rcl_timer_fini, RCL_RET_ERROR); EXPECT_NO_THROW( { - timer_to_test_destructor = - test_node->create_wall_timer(std::chrono::milliseconds(0), [](void) {}); + switch (timer_type) { + case TimerType::WALL_TIMER: + timer_to_test_destructor = + test_node->create_wall_timer(std::chrono::milliseconds(0), [](void) {}); + break; + case TimerType::GENERIC_TIMER: + timer_to_test_destructor = + test_node->create_timer(std::chrono::milliseconds(0), [](void) {}); + break; + } timer_to_test_destructor.reset(); }); } @@ -283,3 +318,19 @@ TEST_F(TestTimer, test_failures_with_exceptions) std::runtime_error("Timer could not get time until next call: error not set")); } } + +INSTANTIATE_TEST_SUITE_P( + PerTimerType, TestTimer, + ::testing::Values(TimerType::WALL_TIMER, TimerType::GENERIC_TIMER), + [](const ::testing::TestParamInfo & info) -> std::string { + switch (info.param) { + case TimerType::WALL_TIMER: + return std::string("wall_timer"); + case TimerType::GENERIC_TIMER: + return std::string("generic_timer"); + default: + break; + } + return std::string("unknown"); + } +); diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 84c62b9e6a..5e06876018 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -242,7 +242,7 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, ) ); - /// Create a timer. + /// Create a timer that uses the wall clock to drive the callback. /** * \param[in] period Time interval between triggers of the callback. * \param[in] callback User-defined callback function. @@ -255,6 +255,19 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, CallbackT callback, rclcpp::CallbackGroup::SharedPtr group = nullptr); + /// Create a timer that uses the node clock to drive the callback. + /** + * \param[in] period Time interval between triggers of the callback. + * \param[in] callback User-defined callback function. + * \param[in] group Callback group to execute this timer's callback in. + */ + template + typename rclcpp::GenericTimer::SharedPtr + create_timer( + std::chrono::duration period, + CallbackT callback, + rclcpp::CallbackGroup::SharedPtr group = nullptr); + /// Create and return a Client. /** * \sa rclcpp::Node::create_client diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp index 88d981e051..e2a5696b92 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp @@ -90,11 +90,28 @@ LifecycleNode::create_wall_timer( CallbackT callback, rclcpp::CallbackGroup::SharedPtr group) { - auto timer = rclcpp::WallTimer::make_shared( - std::chrono::duration_cast(period), - std::move(callback), this->node_base_->get_context()); - node_timers_->add_timer(timer, group); - return timer; + return rclcpp::create_wall_timer( + period, + std::move(callback), + group, + this->node_base_.get(), + this->node_timers_.get()); +} + +template +typename rclcpp::GenericTimer::SharedPtr +LifecycleNode::create_timer( + std::chrono::duration period, + CallbackT callback, + rclcpp::CallbackGroup::SharedPtr group) +{ + return rclcpp::create_timer( + this->get_clock(), + period, + std::move(callback), + group, + this->node_base_.get(), + this->node_timers_.get()); } template diff --git a/rclcpp_lifecycle/test/test_lifecycle_publisher.cpp b/rclcpp_lifecycle/test/test_lifecycle_publisher.cpp index 2a5d08f728..d10544ef1f 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_publisher.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_publisher.cpp @@ -42,10 +42,17 @@ class TestDefaultStateMachine : public ::testing::Test } }; +/// We want to test everything for both the wall and generic timer. +enum class TimerType +{ + WALL_TIMER, + GENERIC_TIMER, +}; + class EmptyLifecycleNode : public rclcpp_lifecycle::LifecycleNode { public: - explicit EmptyLifecycleNode(const std::string & node_name) + explicit EmptyLifecycleNode(const std::string & node_name, const TimerType & timer_type) : rclcpp_lifecycle::LifecycleNode(node_name) { rclcpp::PublisherOptionsWithAllocator> options; @@ -55,8 +62,20 @@ class EmptyLifecycleNode : public rclcpp_lifecycle::LifecycleNode add_managed_entity(publisher_); // For coverage this is being added here - auto timer = create_wall_timer(std::chrono::seconds(1), []() {}); - add_timer_handle(timer); + switch (timer_type) { + case TimerType::WALL_TIMER: + { + auto timer = create_wall_timer(std::chrono::seconds(1), []() {}); + add_timer_handle(timer); + break; + } + case TimerType::GENERIC_TIMER: + { + auto timer = create_timer(std::chrono::seconds(1), []() {}); + add_timer_handle(timer); + break; + } + } } std::shared_ptr> publisher() @@ -68,13 +87,13 @@ class EmptyLifecycleNode : public rclcpp_lifecycle::LifecycleNode std::shared_ptr> publisher_; }; -class TestLifecyclePublisher : public ::testing::Test +class TestLifecyclePublisher : public ::testing::TestWithParam { public: void SetUp() { rclcpp::init(0, nullptr); - node_ = std::make_shared("node"); + node_ = std::make_shared("node", GetParam()); } void TearDown() @@ -86,7 +105,7 @@ class TestLifecyclePublisher : public ::testing::Test std::shared_ptr node_; }; -TEST_F(TestLifecyclePublisher, publish_managed_by_node) { +TEST_P(TestLifecyclePublisher, publish_managed_by_node) { // transition via LifecycleNode auto success = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; auto reset_key = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; @@ -125,7 +144,7 @@ TEST_F(TestLifecyclePublisher, publish_managed_by_node) { } } -TEST_F(TestLifecyclePublisher, publish) { +TEST_P(TestLifecyclePublisher, publish) { // transition via LifecyclePublisher node_->publisher()->on_deactivate(); EXPECT_FALSE(node_->publisher()->is_activated()); @@ -148,3 +167,19 @@ TEST_F(TestLifecyclePublisher, publish) { EXPECT_NO_THROW(node_->publisher()->publish(std::move(msg_ptr))); } } + +INSTANTIATE_TEST_SUITE_P( + PerTimerType, TestLifecyclePublisher, + ::testing::Values(TimerType::WALL_TIMER, TimerType::GENERIC_TIMER), + [](const ::testing::TestParamInfo & info) -> std::string { + switch (info.param) { + case TimerType::WALL_TIMER: + return std::string("wall_timer"); + case TimerType::GENERIC_TIMER: + return std::string("generic_timer"); + default: + break; + } + return std::string("unknown"); + } +); From ea8daa37845e6137cba07a18eb653d97d87e6174 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Fri, 2 Sep 2022 11:58:56 -0600 Subject: [PATCH 13/53] operator+= and operator-= for Duration (#1988) * operator+= and operator-= for Duration Signed-off-by: Tyler Weaver --- rclcpp/include/rclcpp/duration.hpp | 7 +++++++ rclcpp/src/rclcpp/duration.cpp | 21 +++++++++++++++++++++ rclcpp/test/rclcpp/test_duration.cpp | 14 ++++++++++++++ 3 files changed, 42 insertions(+) diff --git a/rclcpp/include/rclcpp/duration.hpp b/rclcpp/include/rclcpp/duration.hpp index 28b3718660..537e2a9bf6 100644 --- a/rclcpp/include/rclcpp/duration.hpp +++ b/rclcpp/include/rclcpp/duration.hpp @@ -92,9 +92,13 @@ class RCLCPP_PUBLIC Duration Duration operator+(const rclcpp::Duration & rhs) const; + Duration & operator+=(const rclcpp::Duration & rhs); + Duration operator-(const rclcpp::Duration & rhs) const; + Duration & operator-=(const rclcpp::Duration & rhs); + /// Get the maximum representable value. /** * \return the maximum representable value @@ -106,6 +110,9 @@ class RCLCPP_PUBLIC Duration Duration operator*(double scale) const; + Duration & + operator*=(double scale); + /// Get duration in nanosecods /** * \return the duration in nanoseconds as a rcl_duration_value_t. diff --git a/rclcpp/src/rclcpp/duration.cpp b/rclcpp/src/rclcpp/duration.cpp index 2966cc4899..2eac7f6b58 100644 --- a/rclcpp/src/rclcpp/duration.cpp +++ b/rclcpp/src/rclcpp/duration.cpp @@ -161,6 +161,13 @@ Duration::operator+(const rclcpp::Duration & rhs) const rcl_duration_.nanoseconds + rhs.rcl_duration_.nanoseconds); } +Duration & +Duration::operator+=(const rclcpp::Duration & rhs) +{ + *this = *this + rhs; + return *this; +} + void bounds_check_duration_difference(int64_t lhsns, int64_t rhsns, uint64_t max) { @@ -190,6 +197,13 @@ Duration::operator-(const rclcpp::Duration & rhs) const rcl_duration_.nanoseconds - rhs.rcl_duration_.nanoseconds); } +Duration & +Duration::operator-=(const rclcpp::Duration & rhs) +{ + *this = *this - rhs; + return *this; +} + void bounds_check_duration_scale(int64_t dns, double scale, uint64_t max) { @@ -222,6 +236,13 @@ Duration::operator*(double scale) const static_cast(rcl_duration_.nanoseconds) * scale_ld)); } +Duration & +Duration::operator*=(double scale) +{ + *this = *this * scale; + return *this; +} + rcl_duration_value_t Duration::nanoseconds() const { diff --git a/rclcpp/test/rclcpp/test_duration.cpp b/rclcpp/test/rclcpp/test_duration.cpp index 319ce06601..d5908c7d4b 100644 --- a/rclcpp/test/rclcpp/test_duration.cpp +++ b/rclcpp/test/rclcpp/test_duration.cpp @@ -52,9 +52,23 @@ TEST_F(TestDuration, operators) { EXPECT_EQ(sub.nanoseconds(), young.nanoseconds() - old.nanoseconds()); EXPECT_EQ(sub, young - old); + rclcpp::Duration addequal = old; + addequal += young; + EXPECT_EQ(addequal.nanoseconds(), old.nanoseconds() + young.nanoseconds()); + EXPECT_EQ(addequal, old + young); + + rclcpp::Duration subequal = young; + subequal -= old; + EXPECT_EQ(subequal.nanoseconds(), young.nanoseconds() - old.nanoseconds()); + EXPECT_EQ(subequal, young - old); + rclcpp::Duration scale = old * 3; EXPECT_EQ(scale.nanoseconds(), old.nanoseconds() * 3); + rclcpp::Duration scaleequal = old; + scaleequal *= 3; + EXPECT_EQ(scaleequal.nanoseconds(), old.nanoseconds() * 3); + rclcpp::Duration time = rclcpp::Duration(0, 0); rclcpp::Duration copy_constructor_duration(time); rclcpp::Duration assignment_op_duration = rclcpp::Duration(1, 0); From 92d4f3e347c8d12f25ee23b2dba5ae9179c14853 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Fri, 9 Sep 2022 03:55:41 +0800 Subject: [PATCH 14/53] support regex match for parameter client (#1992) * support regex match for parameter client Signed-off-by: Chen Lihui * address review that change behavior of a public method Signed-off-by: Chen Lihui * remove an unnecessary comment Signed-off-by: Chen Lihui * update gmock compilation setting Signed-off-by: Chen Lihui Signed-off-by: Chen Lihui --- rclcpp/include/rclcpp/parameter_client.hpp | 3 + rclcpp/include/rclcpp/parameter_map.hpp | 11 +- rclcpp/src/rclcpp/parameter_client.cpp | 28 ++-- rclcpp/src/rclcpp/parameter_map.cpp | 31 +++- rclcpp/test/rclcpp/CMakeLists.txt | 2 +- rclcpp/test/rclcpp/test_parameter_client.cpp | 137 +++++++++++++++++- .../load_complicated_parameters.yaml | 25 ++++ .../test_node/no_valid_parameters.yaml | 4 + 8 files changed, 217 insertions(+), 24 deletions(-) create mode 100644 rclcpp/test/resources/test_node/load_complicated_parameters.yaml create mode 100644 rclcpp/test/resources/test_node/no_valid_parameters.yaml diff --git a/rclcpp/include/rclcpp/parameter_client.hpp b/rclcpp/include/rclcpp/parameter_client.hpp index 5e509bbf10..1bbe4c2ebc 100644 --- a/rclcpp/include/rclcpp/parameter_client.hpp +++ b/rclcpp/include/rclcpp/parameter_client.hpp @@ -185,6 +185,9 @@ class AsyncParametersClient /** * This function filters the parameters to be set based on the node name. * + * If two duplicate keys exist in node names belongs to one FQN, there is no guarantee + * which one could be set. + * * \param parameter_map named parameters to be loaded * \return the future of the set_parameter service used to load the parameters * \throw InvalidParametersException if there is no parameter to set diff --git a/rclcpp/include/rclcpp/parameter_map.hpp b/rclcpp/include/rclcpp/parameter_map.hpp index 303eac4a95..17e2128a7b 100644 --- a/rclcpp/include/rclcpp/parameter_map.hpp +++ b/rclcpp/include/rclcpp/parameter_map.hpp @@ -53,11 +53,20 @@ parameter_value_from(const rcl_variant_t * const c_value); /// Get the ParameterMap from a yaml file. /// \param[in] yaml_filename full name of the yaml file. +/// \param[in] node_fqn a Fully Qualified Name of node, default value is nullptr. /// \returns an instance of a parameter map /// \throws from rcl error of rcl_parse_yaml_file() RCLCPP_PUBLIC ParameterMap -parameter_map_from_yaml_file(const std::string & yaml_filename); +parameter_map_from_yaml_file(const std::string & yaml_filename, const char * node_fqn = nullptr); + +/// Get the Parameters from ParameterMap. +/// \param[in] parameter_map a parameter map. +/// \param[in] node_fqn a Fully Qualified Name of node, default value is nullptr. +/// \returns a list of a parameter +RCLCPP_PUBLIC +std::vector +parameters_from_map(const ParameterMap & parameter_map, const char * node_fqn = nullptr); } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/parameter_client.cpp b/rclcpp/src/rclcpp/parameter_client.cpp index 38ced0e1a5..64e415e82f 100644 --- a/rclcpp/src/rclcpp/parameter_client.cpp +++ b/rclcpp/src/rclcpp/parameter_client.cpp @@ -290,28 +290,24 @@ std::shared_future> AsyncParametersClient::load_parameters( const std::string & yaml_filename) { - rclcpp::ParameterMap parameter_map = rclcpp::parameter_map_from_yaml_file(yaml_filename); - return this->load_parameters(parameter_map); + rclcpp::ParameterMap parameter_map = + rclcpp::parameter_map_from_yaml_file(yaml_filename, remote_node_name_.c_str()); + + auto iter = parameter_map.find(remote_node_name_); + if (iter == parameter_map.end() || iter->second.size() == 0) { + throw rclcpp::exceptions::InvalidParametersException("No valid parameter"); + } + auto future_result = set_parameters(iter->second); + + return future_result; } std::shared_future> AsyncParametersClient::load_parameters( const rclcpp::ParameterMap & parameter_map) { - std::vector parameters; - std::string remote_name = remote_node_name_.substr(remote_node_name_.substr(1).find("/") + 2); - for (const auto & params : parameter_map) { - std::string node_full_name = params.first; - std::string node_name = node_full_name.substr(node_full_name.find("/*/") + 3); - if (node_full_name == remote_node_name_ || - node_full_name == "/**" || - (node_name == remote_name)) - { - for (const auto & param : params.second) { - parameters.push_back(param); - } - } - } + std::vector parameters = + rclcpp::parameters_from_map(parameter_map, remote_node_name_.c_str()); if (parameters.size() == 0) { throw rclcpp::exceptions::InvalidParametersException("No valid parameter"); diff --git a/rclcpp/src/rclcpp/parameter_map.cpp b/rclcpp/src/rclcpp/parameter_map.cpp index cb458e7db3..5ed67daae6 100644 --- a/rclcpp/src/rclcpp/parameter_map.cpp +++ b/rclcpp/src/rclcpp/parameter_map.cpp @@ -25,6 +25,13 @@ using rclcpp::exceptions::InvalidParameterValueException; using rclcpp::ParameterMap; using rclcpp::ParameterValue; +static bool is_node_name_matched(const std::string & node_name, const char * node_fqn) +{ + // Update the regular expression ["/*" -> "(/\\w+)" and "/**" -> "(/\\w+)*"] + std::string regex = rcpputils::find_and_replace(node_name, "/*", "(/\\w+)"); + return std::regex_match(node_fqn, std::regex(regex)); +} + ParameterMap rclcpp::parameter_map_from(const rcl_params_t * const c_params, const char * node_fqn) { @@ -53,9 +60,7 @@ rclcpp::parameter_map_from(const rcl_params_t * const c_params, const char * nod } if (node_fqn) { - // Update the regular expression ["/*" -> "(/\\w+)" and "/**" -> "(/\\w+)*"] - std::string regex = rcpputils::find_and_replace(node_name, "/*", "(/\\w+)"); - if (!std::regex_match(node_fqn, std::regex(regex))) { + if (!is_node_name_matched(node_name, node_fqn)) { // No need to parse the items because the user just care about node_fqn continue; } @@ -143,7 +148,7 @@ rclcpp::parameter_value_from(const rcl_variant_t * const c_param_value) } ParameterMap -rclcpp::parameter_map_from_yaml_file(const std::string & yaml_filename) +rclcpp::parameter_map_from_yaml_file(const std::string & yaml_filename, const char * node_fqn) { rcutils_allocator_t allocator = rcutils_get_default_allocator(); rcl_params_t * rcl_parameters = rcl_yaml_node_struct_init(allocator); @@ -153,5 +158,21 @@ rclcpp::parameter_map_from_yaml_file(const std::string & yaml_filename) rclcpp::exceptions::throw_from_rcl_error(RCL_RET_ERROR); } - return rclcpp::parameter_map_from(rcl_parameters); + return rclcpp::parameter_map_from(rcl_parameters, node_fqn); +} + +std::vector +rclcpp::parameters_from_map(const ParameterMap & parameter_map, const char * node_fqn) +{ + std::vector parameters; + std::string node_name_old; + for (auto & [node_name, node_parameters] : parameter_map) { + if (node_fqn && !is_node_name_matched(node_name, node_fqn)) { + // No need to parse the items because the user just care about node_fqn + continue; + } + parameters.insert(parameters.end(), node_parameters.begin(), node_parameters.end()); + } + + return parameters; } diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index 6f915feef5..d4e497f4bf 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -299,7 +299,7 @@ if(TARGET test_init_options) ament_target_dependencies(test_init_options "rcl") target_link_libraries(test_init_options ${PROJECT_NAME} mimick) endif() -ament_add_gtest(test_parameter_client test_parameter_client.cpp) +ament_add_gmock(test_parameter_client test_parameter_client.cpp) if(TARGET test_parameter_client) ament_target_dependencies(test_parameter_client "rcl_interfaces" diff --git a/rclcpp/test/rclcpp/test_parameter_client.cpp b/rclcpp/test/rclcpp/test_parameter_client.cpp index 4cd9e671be..64ef2d903b 100644 --- a/rclcpp/test/rclcpp/test_parameter_client.cpp +++ b/rclcpp/test/rclcpp/test_parameter_client.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include @@ -948,3 +948,138 @@ TEST_F(TestParameterClient, sync_parameter_load_parameters) { auto list_parameters = synchronous_client->list_parameters({}, 3); ASSERT_EQ(list_parameters.names.size(), static_cast(5)); } + +/* + Coverage for async load_parameters with complicated regex expression + */ +TEST_F(TestParameterClient, async_parameter_load_parameters_complicated_regex) { + 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_complicated_parameters.yaml").string(); + auto load_future = asynchronous_client->load_parameters(parameters_filepath); + auto result_code = rclcpp::spin_until_future_complete( + load_node, load_future, std::chrono::milliseconds(100)); + ASSERT_EQ(result_code, rclcpp::FutureReturnCode::SUCCESS); + ASSERT_EQ(load_future.get()[0].successful, true); + // list parameters + auto list_parameters = asynchronous_client->list_parameters({}, 3); + rclcpp::spin_until_future_complete( + load_node, list_parameters, std::chrono::milliseconds(100)); + ASSERT_EQ(list_parameters.get().names.size(), static_cast(6)); + // to check the parameter "a_value" + std::string param_name = "a_value"; + auto param = load_node->get_parameter(param_name); + ASSERT_EQ(param.get_value(), "last_one_win"); +} + +/* + Coverage for async load_parameters to load file without valid parameters + */ +TEST_F(TestParameterClient, async_parameter_load_no_valid_parameter) { + auto load_node = std::make_shared( + "load_node", + "namespace", + rclcpp::NodeOptions().allow_undeclared_parameters(true)); + auto asynchronous_client = + std::make_shared(load_node, "/namespace/load_node"); + // load parameters + rcpputils::fs::path test_resources_path{TEST_RESOURCES_DIRECTORY}; + const std::string parameters_filepath = ( + test_resources_path / "test_node" / "no_valid_parameters.yaml").string(); + EXPECT_THROW( + asynchronous_client->load_parameters(parameters_filepath), + rclcpp::exceptions::InvalidParametersException); +} + +/* + Coverage for async load_parameters from maps with complicated regex expression + */ +TEST_F(TestParameterClient, async_parameter_load_parameters_from_map) { + auto load_node = std::make_shared( + "load_node", + "namespace", + rclcpp::NodeOptions().allow_undeclared_parameters(true)); + auto asynchronous_client = + std::make_shared(load_node, "/namespace/load_node"); + // load parameters + rclcpp::ParameterMap parameter_map = { + {"/**", + { + {"bar", 5}, + {"foo", 3.5}, + {"a_value", "first"} + } + }, + {"/*/load_node", + { + {"bar_foo", "ok"}, + {"a_value", "second"} + } + }, + {"/namespace/load_node", + { + {"foo_bar", true}, + {"a_value", "third"} + } + }, + {"/bar", + { + {"fatal", 10} + } + }, + {"/**/namespace/*", + { + {"a_value", "not_win"} + } + } + }; + + auto load_future = asynchronous_client->load_parameters(parameter_map); + auto result_code = rclcpp::spin_until_future_complete( + load_node, load_future, std::chrono::milliseconds(100)); + ASSERT_EQ(result_code, rclcpp::FutureReturnCode::SUCCESS); + ASSERT_EQ(load_future.get()[0].successful, true); + // list parameters + auto list_parameters = asynchronous_client->list_parameters({}, 3); + rclcpp::spin_until_future_complete( + load_node, list_parameters, std::chrono::milliseconds(100)); + ASSERT_EQ(list_parameters.get().names.size(), static_cast(6)); + // to check the parameter "a_value" + std::string param_name = "a_value"; + auto param = load_node->get_parameter(param_name); + // rclcpp::ParameterMap is an unordered map, no guarantee which value will be set for `a_value`. + EXPECT_THAT( + (std::array{"first", "second", "third", "not_win"}), + testing::Contains(param.get_value())); +} + +/* + Coverage for async load_parameters from maps without valid parameters + */ +TEST_F(TestParameterClient, async_parameter_load_from_map_no_valid_parameter) { + auto load_node = std::make_shared( + "load_node", + "namespace", + rclcpp::NodeOptions().allow_undeclared_parameters(true)); + auto asynchronous_client = + std::make_shared(load_node, "/namespace/load_node"); + // load parameters + rclcpp::ParameterMap parameter_map = { + {"/no/valid/parameters/node", + { + {"bar", 5}, + {"bar", 3.5} + } + } + }; + EXPECT_THROW( + asynchronous_client->load_parameters(parameter_map), + rclcpp::exceptions::InvalidParametersException); +} diff --git a/rclcpp/test/resources/test_node/load_complicated_parameters.yaml b/rclcpp/test/resources/test_node/load_complicated_parameters.yaml new file mode 100644 index 0000000000..7722f636c2 --- /dev/null +++ b/rclcpp/test/resources/test_node/load_complicated_parameters.yaml @@ -0,0 +1,25 @@ +/**: + ros__parameters: + bar: 5 + foo: 3.5 + a_value: "first" + +/*: + load_node: + ros__parameters: + bar_foo: "ok" + a_value: "second" + +namespace: + load_node: + ros__parameters: + foo_bar: true + a_value: "third" + +bar: + ros__parameters: + fatal: 10 + +/**/namespace/*: + ros__parameters: + a_value: "last_one_win" diff --git a/rclcpp/test/resources/test_node/no_valid_parameters.yaml b/rclcpp/test/resources/test_node/no_valid_parameters.yaml new file mode 100644 index 0000000000..a75356cd77 --- /dev/null +++ b/rclcpp/test/resources/test_node/no_valid_parameters.yaml @@ -0,0 +1,4 @@ +/no/valid/parameters/node: + ros__parameters: + bar: 5 + foo: 3.5 From d0e1e837b512569a6dc58b5579bcb87c5a5bf2bc Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Sat, 10 Sep 2022 06:06:33 +0800 Subject: [PATCH 15/53] Make ParameterService and Sync/AsyncParameterClient accept rclcpp::QoS (#1978) * Make ParameterService and Sync/AsyncParameterClient accept rclcpp::QoS Signed-off-by: Barry Xu * Avoid deprecated function using another deprecated function Signed-off-by: Barry Xu Signed-off-by: Barry Xu --- rclcpp/include/rclcpp/parameter_client.hpp | 196 ++++++++++++++++++-- rclcpp/include/rclcpp/parameter_service.hpp | 19 +- rclcpp/src/rclcpp/parameter_client.cpp | 4 +- rclcpp/src/rclcpp/parameter_service.cpp | 2 +- 4 files changed, 197 insertions(+), 24 deletions(-) diff --git a/rclcpp/include/rclcpp/parameter_client.hpp b/rclcpp/include/rclcpp/parameter_client.hpp index 1bbe4c2ebc..7dff6e9ad6 100644 --- a/rclcpp/include/rclcpp/parameter_client.hpp +++ b/rclcpp/include/rclcpp/parameter_client.hpp @@ -39,6 +39,7 @@ #include "rclcpp/node.hpp" #include "rclcpp/parameter.hpp" #include "rclcpp/parameter_map.hpp" +#include "rclcpp/qos.hpp" #include "rclcpp/type_support_decl.hpp" #include "rclcpp/visibility_control.hpp" #include "rmw/rmw.h" @@ -51,6 +52,37 @@ class AsyncParametersClient public: RCLCPP_SMART_PTR_DEFINITIONS(AsyncParametersClient) + /// Create an async parameters client. + /** + * \param[in] node_base_interface The node base interface of the corresponding node. + * \param[in] node_topics_interface Node topic base interface. + * \param[in] node_graph_interface The node graph interface of the corresponding node. + * \param[in] node_services_interface Node service interface. + * \param[in] remote_node_name Name of the remote node + * \param[in] qos_profile The rmw qos profile to use to subscribe + * \param[in] group (optional) The async parameter client will be added to this callback group. + * \deprecated use rclcpp::QoS instead of rmw_qos_profile_t + */ + [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] + RCLCPP_PUBLIC + AsyncParametersClient( + const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, + const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface, + const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface, + const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface, + const std::string & remote_node_name, + const rmw_qos_profile_t & qos_profile, + rclcpp::CallbackGroup::SharedPtr group = nullptr) + : AsyncParametersClient( + node_base_interface, + node_topics_interface, + node_graph_interface, + node_services_interface, + remote_node_name, + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile)), + group) + {} + /// Create an async parameters client. /** * \param[in] node_base_interface The node base interface of the corresponding node. @@ -58,7 +90,7 @@ class AsyncParametersClient * \param[in] node_graph_interface The node graph interface of the corresponding node. * \param[in] node_services_interface Node service interface. * \param[in] remote_node_name (optional) name of the remote node - * \param[in] qos_profile (optional) The rmw qos profile to use to subscribe + * \param[in] qos_profile (optional) The qos profile to use to subscribe * \param[in] group (optional) The async parameter client will be added to this callback group. */ RCLCPP_PUBLIC @@ -68,21 +100,45 @@ class AsyncParametersClient const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface, const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface, const std::string & remote_node_name = "", - const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters, + const rclcpp::QoS & qos_profile = rclcpp::ParametersQoS(), rclcpp::CallbackGroup::SharedPtr group = nullptr); /// Constructor /** - * \param[in] node The async paramters client will be added to this node. + * \param[in] node The async parameters client will be added to this node. + * \param[in] remote_node_name name of the remote node + * \param[in] qos_profile The rmw qos profile to use to subscribe + * \param[in] group (optional) The async parameter client will be added to this callback group. + * \deprecated use rclcpp::QoS instead of rmw_qos_profile_t + */ + template + [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] + AsyncParametersClient( + const std::shared_ptr node, + const std::string & remote_node_name, + const rmw_qos_profile_t & qos_profile, + rclcpp::CallbackGroup::SharedPtr group = nullptr) + : AsyncParametersClient( + node->get_node_base_interface(), + node->get_node_topics_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface(), + remote_node_name, + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile)), + group) + {} + + /** + * \param[in] node The async parameters client will be added to this node. * \param[in] remote_node_name (optional) name of the remote node - * \param[in] qos_profile (optional) The rmw qos profile to use to subscribe + * \param[in] qos_profile (optional) The qos profile to use to subscribe * \param[in] group (optional) The async parameter client will be added to this callback group. */ template explicit AsyncParametersClient( const std::shared_ptr node, const std::string & remote_node_name = "", - const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters, + const rclcpp::QoS & qos_profile = rclcpp::ParametersQoS(), rclcpp::CallbackGroup::SharedPtr group = nullptr) : AsyncParametersClient( node->get_node_base_interface(), @@ -96,16 +152,40 @@ class AsyncParametersClient /// Constructor /** - * \param[in] node The async paramters client will be added to this node. + * \param[in] node The async parameters client will be added to this node. + * \param[in] remote_node_name Name of the remote node + * \param[in] qos_profile The rmw qos profile to use to subscribe + * \param[in] group (optional) The async parameter client will be added to this callback group. + * \deprecated use rclcpp::QoS instead of rmw_qos_profile_t + */ + template + [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] + AsyncParametersClient( + NodeT * node, + const std::string & remote_node_name, + const rmw_qos_profile_t & qos_profile, + rclcpp::CallbackGroup::SharedPtr group = nullptr) + : AsyncParametersClient( + node->get_node_base_interface(), + node->get_node_topics_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface(), + remote_node_name, + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile)), + group) + {} + + /** + * \param[in] node The async parameters client will be added to this node. * \param[in] remote_node_name (optional) name of the remote node - * \param[in] qos_profile (optional) The rmw qos profile to use to subscribe + * \param[in] qos_profile (optional) The qos profile to use to subscribe * \param[in] group (optional) The async parameter client will be added to this callback group. */ template explicit AsyncParametersClient( NodeT * node, const std::string & remote_node_name = "", - const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters, + const rclcpp::QoS & qos_profile = rclcpp::ParametersQoS(), rclcpp::CallbackGroup::SharedPtr group = nullptr) : AsyncParametersClient( node->get_node_base_interface(), @@ -211,9 +291,7 @@ class AsyncParametersClient typename rclcpp::Subscription::SharedPtr on_parameter_event( CallbackT && callback, - const rclcpp::QoS & qos = ( - rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_parameter_events)) - ), + const rclcpp::QoS & qos = rclcpp::ParameterEventsQoS(), const rclcpp::SubscriptionOptionsWithAllocator & options = ( rclcpp::SubscriptionOptionsWithAllocator() )) @@ -238,9 +316,7 @@ class AsyncParametersClient on_parameter_event( NodeT && node, CallbackT && callback, - const rclcpp::QoS & qos = ( - rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_parameter_events)) - ), + const rclcpp::QoS & qos = rclcpp::ParameterEventsQoS(), const rclcpp::SubscriptionOptionsWithAllocator & options = ( rclcpp::SubscriptionOptionsWithAllocator() )) @@ -307,11 +383,24 @@ class SyncParametersClient public: RCLCPP_SMART_PTR_DEFINITIONS(SyncParametersClient) + template + [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] + SyncParametersClient( + std::shared_ptr node, + const std::string & remote_node_name, + const rmw_qos_profile_t & qos_profile) + : SyncParametersClient( + std::make_shared(), + node, + remote_node_name, + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile))) + {} + template explicit SyncParametersClient( std::shared_ptr node, const std::string & remote_node_name = "", - const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters) + const rclcpp::QoS & qos_profile = rclcpp::ParametersQoS()) : SyncParametersClient( std::make_shared(), node, @@ -319,12 +408,29 @@ class SyncParametersClient qos_profile) {} + template + [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] + SyncParametersClient( + rclcpp::Executor::SharedPtr executor, + std::shared_ptr node, + const std::string & remote_node_name, + const rmw_qos_profile_t & qos_profile) + : SyncParametersClient( + executor, + node->get_node_base_interface(), + node->get_node_topics_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface(), + remote_node_name, + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile))) + {} + template SyncParametersClient( rclcpp::Executor::SharedPtr executor, std::shared_ptr node, const std::string & remote_node_name = "", - const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters) + const rclcpp::QoS & qos_profile = rclcpp::ParametersQoS()) : SyncParametersClient( executor, node->get_node_base_interface(), @@ -335,11 +441,24 @@ class SyncParametersClient qos_profile) {} + template + [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] + SyncParametersClient( + NodeT * node, + const std::string & remote_node_name, + const rmw_qos_profile_t & qos_profile) + : SyncParametersClient( + std::make_shared(), + node, + remote_node_name, + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile))) + {} + template explicit SyncParametersClient( NodeT * node, const std::string & remote_node_name = "", - const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters) + const rclcpp::QoS & qos_profile = rclcpp::ParametersQoS()) : SyncParametersClient( std::make_shared(), node, @@ -347,12 +466,29 @@ class SyncParametersClient qos_profile) {} + template + [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] + SyncParametersClient( + rclcpp::Executor::SharedPtr executor, + NodeT * node, + const std::string & remote_node_name, + const rmw_qos_profile_t & qos_profile) + : SyncParametersClient( + executor, + node->get_node_base_interface(), + node->get_node_topics_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface(), + remote_node_name, + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile))) + {} + template SyncParametersClient( rclcpp::Executor::SharedPtr executor, NodeT * node, const std::string & remote_node_name = "", - const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters) + const rclcpp::QoS & qos_profile = rclcpp::ParametersQoS()) : SyncParametersClient( executor, node->get_node_base_interface(), @@ -363,6 +499,28 @@ class SyncParametersClient qos_profile) {} + [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] + RCLCPP_PUBLIC + SyncParametersClient( + rclcpp::Executor::SharedPtr executor, + const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface, + const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface, + const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface, + const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface, + const std::string & remote_node_name, + const rmw_qos_profile_t & qos_profile) + : executor_(executor), node_base_interface_(node_base_interface) + { + async_parameters_client_ = + std::make_shared( + node_base_interface, + node_topics_interface, + node_graph_interface, + node_services_interface, + remote_node_name, + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile))); + } + RCLCPP_PUBLIC SyncParametersClient( rclcpp::Executor::SharedPtr executor, @@ -371,7 +529,7 @@ class SyncParametersClient const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface, const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface, const std::string & remote_node_name = "", - const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters) + const rclcpp::QoS & qos_profile = rclcpp::ParametersQoS()) : executor_(executor), node_base_interface_(node_base_interface) { async_parameters_client_ = diff --git a/rclcpp/include/rclcpp/parameter_service.hpp b/rclcpp/include/rclcpp/parameter_service.hpp index 143a5223c8..a952939aca 100644 --- a/rclcpp/include/rclcpp/parameter_service.hpp +++ b/rclcpp/include/rclcpp/parameter_service.hpp @@ -28,6 +28,7 @@ #include "rclcpp/macros.hpp" #include "rclcpp/node.hpp" #include "rclcpp/parameter.hpp" +#include "rclcpp/qos.hpp" #include "rclcpp/visibility_control.hpp" #include "rmw/rmw.h" @@ -39,12 +40,26 @@ class ParameterService public: RCLCPP_SMART_PTR_DEFINITIONS(ParameterService) + [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] RCLCPP_PUBLIC - explicit ParameterService( + ParameterService( const std::shared_ptr node_base, const std::shared_ptr node_services, rclcpp::node_interfaces::NodeParametersInterface * node_params, - const rmw_qos_profile_t & qos_profile = rmw_qos_profile_parameters); + const rmw_qos_profile_t & qos_profile) + : ParameterService( + node_base, + node_services, + node_params, + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile))) + {} + + RCLCPP_PUBLIC + ParameterService( + const std::shared_ptr node_base, + const std::shared_ptr node_services, + rclcpp::node_interfaces::NodeParametersInterface * node_params, + const rclcpp::QoS & qos_profile = rclcpp::ParametersQoS()); private: rclcpp::Service::SharedPtr get_parameters_service_; diff --git a/rclcpp/src/rclcpp/parameter_client.cpp b/rclcpp/src/rclcpp/parameter_client.cpp index 64e415e82f..b60585ef00 100644 --- a/rclcpp/src/rclcpp/parameter_client.cpp +++ b/rclcpp/src/rclcpp/parameter_client.cpp @@ -35,7 +35,7 @@ AsyncParametersClient::AsyncParametersClient( const rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface, const rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface, const std::string & remote_node_name, - const rmw_qos_profile_t & qos_profile, + const rclcpp::QoS & qos_profile, rclcpp::CallbackGroup::SharedPtr group) : node_topics_interface_(node_topics_interface) { @@ -46,7 +46,7 @@ AsyncParametersClient::AsyncParametersClient( } rcl_client_options_t options = rcl_client_get_default_options(); - options.qos = qos_profile; + options.qos = qos_profile.get_rmw_qos_profile(); using rclcpp::Client; using rclcpp::ClientBase; diff --git a/rclcpp/src/rclcpp/parameter_service.cpp b/rclcpp/src/rclcpp/parameter_service.cpp index 5c30917499..501ac399f8 100644 --- a/rclcpp/src/rclcpp/parameter_service.cpp +++ b/rclcpp/src/rclcpp/parameter_service.cpp @@ -29,7 +29,7 @@ ParameterService::ParameterService( const std::shared_ptr node_base, const std::shared_ptr node_services, rclcpp::node_interfaces::NodeParametersInterface * node_params, - const rmw_qos_profile_t & qos_profile) + const rclcpp::QoS & qos_profile) { const std::string node_name = node_base->get_name(); From 4744fb6f500cbb3d7a0619f4ef6a7bdf34370e34 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 13 Sep 2022 20:28:54 +0000 Subject: [PATCH 16/53] Changelog. Signed-off-by: Chris Lalancette --- rclcpp/CHANGELOG.rst | 29 +++++++++++++++++++++++++++++ rclcpp_action/CHANGELOG.rst | 6 ++++++ rclcpp_components/CHANGELOG.rst | 6 ++++++ rclcpp_lifecycle/CHANGELOG.rst | 10 ++++++++++ 4 files changed, 51 insertions(+) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 76ba0e5c4c..ac76f47319 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,35 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Make ParameterService and Sync/AsyncParameterClient accept rclcpp::QoS (`#1978 `_) +* support regex match for parameter client (`#1992 `_) +* operator+= and operator-= for Duration (`#1988 `_) +* Revert "Revert "Add a create_timer method to Node and `LifecycleNode` classes (`#1975 `_)" (`#2009 `_) (`#2010 `_) +* force compiler warning if callback handles not captured (`#2000 `_) +* Revert "Add a `create_timer` method to `Node` and `LifecycleNode` classes (`#1975 `_)" (`#2009 `_) +* Add a `create_timer` method to `Node` and `LifecycleNode` classes (`#1975 `_) +* [docs] add note about callback lifetime for {on, post}_set_parameter_callback (`#1981 `_) +* fix memory leak (`#1994 `_) +* Support pre-set and post-set parameter callbacks in addition to on-set-parameter-callback. (`#1947 `_) +* Make create_service accept rclcpp::QoS (`#1969 `_) +* Make create_client accept rclcpp::QoS (`#1964 `_) +* Fix the documentation for rclcpp::ok to be accurate. (`#1965 `_) +* use regex for wildcard matching (`#1839 `_) +* Revert "Introduce executors new spin_for method, replace spin_until_future_complete with spin_until_complete. (`#1821 `_) (`#1874 `_)" (`#1956 `_) +* Introduce executors new spin_for method, replace spin_until_future_complete with spin_until_complete. (`#1821 `_) (`#1874 `_) +* test adjustment for LoanedMessage. (`#1951 `_) +* fix virtual dispatch issues identified by clang-tidy (`#1816 `_) +* Remove unused on_parameters_set_callback\_ (`#1945 `_) +* Fix subscription.is_serialized() for callbacks with message info (`#1950 `_) +* wait for subscriptions on another thread. (`#1940 `_) +* Fix documentation of `RCLCPP\_[INFO,WARN,...]` (`#1943 `_) +* Always trigger guard condition waitset (`#1923 `_) +* Add statistics for handle_loaned_message (`#1927 `_) +* Drop wrong template specialization (`#1926 `_) +* Contributors: Alberto Soragna, Andrew Symington, Barry Xu, Brian, Chen Lihui, Chris Lalancette, Daniel Reuter, Deepanshu Bansal, Hubert Liberacki, Ivan Santiago Paunovic, Jochen Sprickerhof, Nikolai Morin, Shane Loretz, Tomoya Fujita, Tyler Weaver, William Woodall, schrodinbug + 16.2.0 (2022-05-03) ------------------- * Update get_parameter_from_event to follow the function description (`#1922 `_) diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 9838922b49..f519631256 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,12 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Revert "Introduce executors new spin_for method, replace spin_until_future_complete with spin_until_complete. (`#1821 `_) (`#1874 `_)" (`#1956 `_) +* Introduce executors new spin_for method, replace spin_until_future_complete with spin_until_complete. (`#1821 `_) (`#1874 `_) +* Contributors: Hubert Liberacki, William Woodall + 16.2.0 (2022-05-03) ------------------- diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 805592a07f..48724d03fc 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Revert "Introduce executors new spin_for method, replace spin_until_future_complete with spin_until_complete. (`#1821 `_) (`#1874 `_)" (`#1956 `_) +* Introduce executors new spin_for method, replace spin_until_future_complete with spin_until_complete. (`#1821 `_) (`#1874 `_) +* Contributors: Hubert Liberacki, William Woodall + 16.2.0 (2022-05-03) ------------------- diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 420252b66a..94901f6eb7 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,16 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Revert "Revert "Add a create_timer method to Node and `LifecycleNode` classes (`#1975 `_)" (`#2009 `_) (`#2010 `_) +* Revert "Add a `create_timer` method to `Node` and `LifecycleNode` classes (`#1975 `_)" (`#2009 `_) +* Add a `create_timer` method to `Node` and `LifecycleNode` classes (`#1975 `_) +* Support pre-set and post-set parameter callbacks in addition to on-set-parameter-callback. (`#1947 `_) +* Make create_service accept rclcpp::QoS (`#1969 `_) +* Make create_client accept rclcpp::QoS (`#1964 `_) +* Contributors: Andrew Symington, Deepanshu Bansal, Ivan Santiago Paunovic, Shane Loretz + 16.2.0 (2022-05-03) ------------------- From 7bc05da5d1e5cf3209af264e2c270ceecd4c9d18 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 13 Sep 2022 20:29:01 +0000 Subject: [PATCH 17/53] 17.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 ac76f47319..08914deb08 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +17.0.0 (2022-09-13) +------------------- * Make ParameterService and Sync/AsyncParameterClient accept rclcpp::QoS (`#1978 `_) * support regex match for parameter client (`#1992 `_) * operator+= and operator-= for Duration (`#1988 `_) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 6de7d25d07..d05253db94 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 16.2.0 + 17.0.0 The ROS client library in C++. Ivan Paunovic Jacob Perron diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index f519631256..ef4104bcac 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +17.0.0 (2022-09-13) +------------------- * Revert "Introduce executors new spin_for method, replace spin_until_future_complete with spin_until_complete. (`#1821 `_) (`#1874 `_)" (`#1956 `_) * Introduce executors new spin_for method, replace spin_until_future_complete with spin_until_complete. (`#1821 `_) (`#1874 `_) * Contributors: Hubert Liberacki, William Woodall diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index dd6e61338c..f9cbaeb721 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 16.2.0 + 17.0.0 Adds action APIs for C++. Ivan Paunovic Jacob Perron diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 48724d03fc..c496eb406d 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +17.0.0 (2022-09-13) +------------------- * Revert "Introduce executors new spin_for method, replace spin_until_future_complete with spin_until_complete. (`#1821 `_) (`#1874 `_)" (`#1956 `_) * Introduce executors new spin_for method, replace spin_until_future_complete with spin_until_complete. (`#1821 `_) (`#1874 `_) * Contributors: Hubert Liberacki, William Woodall diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index 18275306ff..2032fc84ee 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 16.2.0 + 17.0.0 Package containing tools for dynamically loadable components Ivan Paunovic Jacob Perron diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 94901f6eb7..8b88b682b1 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +17.0.0 (2022-09-13) +------------------- * Revert "Revert "Add a create_timer method to Node and `LifecycleNode` classes (`#1975 `_)" (`#2009 `_) (`#2010 `_) * Revert "Add a `create_timer` method to `Node` and `LifecycleNode` classes (`#1975 `_)" (`#2009 `_) * Add a `create_timer` method to `Node` and `LifecycleNode` classes (`#1975 `_) diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 3ff98f3069..85280c5e36 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 16.2.0 + 17.0.0 Package containing a prototype for lifecycle implementation Ivan Paunovic Jacob Perron From 978439191fdb3924af900a506dd35b68f8da725c Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Thu, 15 Sep 2022 20:17:28 +0800 Subject: [PATCH 18/53] fix mismatched issue if using zero_allocate (#1995) * fix mismatched issue if uzing zero_allocated Signed-off-by: Chen Lihui --- .../rclcpp/allocator/allocator_common.hpp | 18 +++++++ .../allocator/test_allocator_common.cpp | 47 +++++++++++++++++++ 2 files changed, 65 insertions(+) diff --git a/rclcpp/include/rclcpp/allocator/allocator_common.hpp b/rclcpp/include/rclcpp/allocator/allocator_common.hpp index d117376517..12b2f383b6 100644 --- a/rclcpp/include/rclcpp/allocator/allocator_common.hpp +++ b/rclcpp/include/rclcpp/allocator/allocator_common.hpp @@ -15,6 +15,7 @@ #ifndef RCLCPP__ALLOCATOR__ALLOCATOR_COMMON_HPP_ #define RCLCPP__ALLOCATOR__ALLOCATOR_COMMON_HPP_ +#include #include #include "rcl/allocator.h" @@ -39,6 +40,22 @@ void * retyped_allocate(size_t size, void * untyped_allocator) return std::allocator_traits::allocate(*typed_allocator, size); } +template +void * retyped_zero_allocate(size_t number_of_elem, size_t size_of_elem, void * untyped_allocator) +{ + auto typed_allocator = static_cast(untyped_allocator); + if (!typed_allocator) { + throw std::runtime_error("Received incorrect allocator type"); + } + size_t size = number_of_elem * size_of_elem; + void * allocated_memory = + std::allocator_traits::allocate(*typed_allocator, size); + if (allocated_memory) { + std::memset(allocated_memory, 0, size); + } + return allocated_memory; +} + template void retyped_deallocate(void * untyped_pointer, void * untyped_allocator) { @@ -73,6 +90,7 @@ rcl_allocator_t get_rcl_allocator(Alloc & allocator) rcl_allocator_t rcl_allocator = rcl_get_default_allocator(); #ifndef _WIN32 rcl_allocator.allocate = &retyped_allocate; + rcl_allocator.zero_allocate = &retyped_zero_allocate; rcl_allocator.deallocate = &retyped_deallocate; rcl_allocator.reallocate = &retyped_reallocate; rcl_allocator.state = &allocator; diff --git a/rclcpp/test/rclcpp/allocator/test_allocator_common.cpp b/rclcpp/test/rclcpp/allocator/test_allocator_common.cpp index 341846a3eb..4619b7665d 100644 --- a/rclcpp/test/rclcpp/allocator/test_allocator_common.cpp +++ b/rclcpp/test/rclcpp/allocator/test_allocator_common.cpp @@ -51,6 +51,53 @@ TEST(TestAllocatorCommon, retyped_allocate) { EXPECT_NO_THROW(code2()); } +TEST(TestAllocatorCommon, retyped_zero_allocate_basic) { + std::allocator allocator; + void * untyped_allocator = &allocator; + void * allocated_mem = + rclcpp::allocator::retyped_zero_allocate>(20u, 1u, untyped_allocator); + ASSERT_TRUE(nullptr != allocated_mem); + + auto code = [&untyped_allocator, allocated_mem]() { + rclcpp::allocator::retyped_deallocate>( + allocated_mem, untyped_allocator); + }; + EXPECT_NO_THROW(code()); +} + +TEST(TestAllocatorCommon, retyped_zero_allocate) { + std::allocator allocator; + void * untyped_allocator = &allocator; + void * allocated_mem = + rclcpp::allocator::retyped_zero_allocate>(20u, 1u, untyped_allocator); + // The more natural check here is ASSERT_NE(nullptr, ptr), but clang static + // analysis throws a false-positive memory leak warning. Use ASSERT_TRUE instead. + ASSERT_TRUE(nullptr != allocated_mem); + + auto code = [&untyped_allocator, allocated_mem]() { + rclcpp::allocator::retyped_deallocate>( + allocated_mem, untyped_allocator); + }; + EXPECT_NO_THROW(code()); + + allocated_mem = allocator.allocate(1); + // The more natural check here is ASSERT_NE(nullptr, ptr), but clang static + // analysis throws a false-positive memory leak warning. Use ASSERT_TRUE instead. + ASSERT_TRUE(nullptr != allocated_mem); + void * reallocated_mem = + rclcpp::allocator::retyped_reallocate>( + allocated_mem, 2u, untyped_allocator); + // The more natural check here is ASSERT_NE(nullptr, ptr), but clang static + // analysis throws a false-positive memory leak warning. Use ASSERT_TRUE instead. + ASSERT_TRUE(nullptr != reallocated_mem); + + auto code2 = [&untyped_allocator, reallocated_mem]() { + rclcpp::allocator::retyped_deallocate>( + reallocated_mem, untyped_allocator); + }; + EXPECT_NO_THROW(code2()); +} + TEST(TestAllocatorCommon, get_rcl_allocator) { std::allocator allocator; auto rcl_allocator = rclcpp::allocator::get_rcl_allocator(allocator); From 6a8c61c0269187c9505632647f6a9f5698b523e8 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Tue, 20 Sep 2022 01:08:25 +0800 Subject: [PATCH 19/53] use unique ptr and remove unuseful container (#2013) Signed-off-by: Chen Lihui Signed-off-by: Chen Lihui --- rclcpp_components/src/node_main.cpp.in | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/rclcpp_components/src/node_main.cpp.in b/rclcpp_components/src/node_main.cpp.in index 0ca5eb8c61..71754d1f82 100644 --- a/rclcpp_components/src/node_main.cpp.in +++ b/rclcpp_components/src/node_main.cpp.in @@ -30,14 +30,13 @@ int main(int argc, char * argv[]) rclcpp::executors::@executor@ exec; rclcpp::NodeOptions options; options.arguments(args); - std::vector loaders; std::vector node_wrappers; std::string library_name = "@library_name@"; std::string class_name = "rclcpp_components::NodeFactoryTemplate<@component@>"; RCLCPP_DEBUG(logger, "Load library %s", library_name.c_str()); - auto loader = new class_loader::ClassLoader(library_name); + auto loader = std::make_unique(library_name); auto classes = loader->getAvailableClasses(); for (const auto & clazz : classes) { std::string name = clazz.c_str(); @@ -59,8 +58,6 @@ int main(int argc, char * argv[]) exec.add_node(node); } } - loaders.push_back(loader); - exec.spin(); From 145933b03793af008a54c9203185a23ec2fde9b0 Mon Sep 17 00:00:00 2001 From: mauropasse Date: Fri, 23 Sep 2022 00:30:59 +0100 Subject: [PATCH 20/53] Do not clear entities callbacks on destruction (#2002) * Do not clear entities callbacks on destruction Removing these clearings since they were not necessary, since the objects are being destroyed anyway. Signed-off-by: Mauro Passerino * Fix CI Signed-off-by: Mauro Passerino * Restore clear_on_ready_callback on ~QOSEventHandlerBase Needed since QOSEventHandlerBase does not own the pub/sub listeners. So the QOSEventHandler can be destroyed while the corresponding listeners are still alive, so we need to clear these callbacks. Signed-off-by: Mauro Passerino * Add coment on clearing callback for QoS event Signed-off-by: Mauro Passerino Signed-off-by: Mauro Passerino Co-authored-by: Mauro Passerino --- rclcpp/include/rclcpp/client.hpp | 2 +- .../experimental/subscription_intra_process_base.hpp | 2 +- rclcpp/include/rclcpp/service.hpp | 2 +- rclcpp/src/rclcpp/client.cpp | 7 ------- rclcpp/src/rclcpp/publisher_base.cpp | 5 ----- rclcpp/src/rclcpp/qos_event.cpp | 5 +++++ rclcpp/src/rclcpp/service.cpp | 4 ---- rclcpp/src/rclcpp/subscription_base.cpp | 7 ------- rclcpp/src/rclcpp/subscription_intra_process_base.cpp | 5 ----- rclcpp_action/src/client.cpp | 1 - rclcpp_action/src/server.cpp | 1 - 11 files changed, 8 insertions(+), 33 deletions(-) diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index e88fa8a949..04e0ce9a33 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -130,7 +130,7 @@ class ClientBase rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph); RCLCPP_PUBLIC - virtual ~ClientBase(); + virtual ~ClientBase() = default; /// Take the next response for this client as a type erased pointer. /** diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp index 037d1aaa2a..6583e74ae7 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp @@ -52,7 +52,7 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable {} RCLCPP_PUBLIC - virtual ~SubscriptionIntraProcessBase(); + virtual ~SubscriptionIntraProcessBase() = default; RCLCPP_PUBLIC size_t diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 3f500eaa09..65b457ffda 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -55,7 +55,7 @@ class ServiceBase explicit ServiceBase(std::shared_ptr node_handle); RCLCPP_PUBLIC - virtual ~ServiceBase(); + virtual ~ServiceBase() = default; /// Return the name of the service. /** \return The name of the service. */ diff --git a/rclcpp/src/rclcpp/client.cpp b/rclcpp/src/rclcpp/client.cpp index 17cc68e153..4adc6d4a96 100644 --- a/rclcpp/src/rclcpp/client.cpp +++ b/rclcpp/src/rclcpp/client.cpp @@ -65,13 +65,6 @@ ClientBase::ClientBase( }); } -ClientBase::~ClientBase() -{ - clear_on_new_response_callback(); - // Make sure the client handle is destructed as early as possible and before the node handle - client_handle_.reset(); -} - bool ClientBase::take_type_erased_response(void * response_out, rmw_request_id_t & request_header_out) { diff --git a/rclcpp/src/rclcpp/publisher_base.cpp b/rclcpp/src/rclcpp/publisher_base.cpp index 723d56e0d5..9517af6d3c 100644 --- a/rclcpp/src/rclcpp/publisher_base.cpp +++ b/rclcpp/src/rclcpp/publisher_base.cpp @@ -102,11 +102,6 @@ PublisherBase::PublisherBase( PublisherBase::~PublisherBase() { - for (const auto & pair : event_handlers_) { - rcl_publisher_event_type_t event_type = pair.first; - clear_on_new_qos_event_callback(event_type); - } - // must fini the events before fini-ing the publisher event_handlers_.clear(); diff --git a/rclcpp/src/rclcpp/qos_event.cpp b/rclcpp/src/rclcpp/qos_event.cpp index 8bfd5b3304..91df5bb346 100644 --- a/rclcpp/src/rclcpp/qos_event.cpp +++ b/rclcpp/src/rclcpp/qos_event.cpp @@ -35,6 +35,11 @@ UnsupportedEventTypeException::UnsupportedEventTypeException( QOSEventHandlerBase::~QOSEventHandlerBase() { + // Since the rmw event listener holds a reference to + // this callback, we need to clear it on destruction of this class. + // This clearing is not needed for other rclcpp entities like pub/subs, since + // they do own the underlying rmw entities, which are destroyed + // on their rclcpp destructors, thus no risk of dangling pointers. if (on_new_event_callback_) { clear_on_ready_callback(); } diff --git a/rclcpp/src/rclcpp/service.cpp b/rclcpp/src/rclcpp/service.cpp index 7bc580ce68..ea1a83a2a6 100644 --- a/rclcpp/src/rclcpp/service.cpp +++ b/rclcpp/src/rclcpp/service.cpp @@ -32,10 +32,6 @@ ServiceBase::ServiceBase(std::shared_ptr node_handle) node_logger_(rclcpp::get_node_logger(node_handle_.get())) {} -ServiceBase::~ServiceBase() -{ - clear_on_new_request_callback(); -} bool ServiceBase::take_type_erased_request(void * request_out, rmw_request_id_t & request_id_out) diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index 300f465a41..871321fbe8 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -87,13 +87,6 @@ SubscriptionBase::SubscriptionBase( SubscriptionBase::~SubscriptionBase() { - clear_on_new_message_callback(); - - for (const auto & pair : event_handlers_) { - rcl_subscription_event_type_t event_type = pair.first; - clear_on_new_qos_event_callback(event_type); - } - if (!use_intra_process_) { return; } diff --git a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp index 21ccb433ff..b13123b65b 100644 --- a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp +++ b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp @@ -17,11 +17,6 @@ using rclcpp::experimental::SubscriptionIntraProcessBase; -SubscriptionIntraProcessBase::~SubscriptionIntraProcessBase() -{ - clear_on_ready_callback(); -} - void SubscriptionIntraProcessBase::add_to_wait_set(rcl_wait_set_t * wait_set) { diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index f9144ecf49..2266b7cee6 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -136,7 +136,6 @@ ClientBase::ClientBase( ClientBase::~ClientBase() { - clear_on_ready_callback(); } bool diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index b0afb9aa50..42c4074495 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -132,7 +132,6 @@ ServerBase::ServerBase( ServerBase::~ServerBase() { - clear_on_ready_callback(); } size_t From 95837d34f1a717780bb629eaa5d889ee4d222fef Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Thu, 29 Sep 2022 07:54:41 -0400 Subject: [PATCH 21/53] Make sure to include-what-you-use in the node_interfaces. (#2018) Signed-off-by: Chris Lalancette --- rclcpp/include/rclcpp/node_interfaces/node_base.hpp | 2 +- rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp | 4 ++-- rclcpp/include/rclcpp/node_interfaces/node_graph.hpp | 2 ++ rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp | 3 ++- .../rclcpp/node_interfaces/node_parameters_interface.hpp | 2 +- .../include/rclcpp/node_interfaces/node_topics_interface.hpp | 2 -- 6 files changed, 8 insertions(+), 7 deletions(-) diff --git a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp index 90011a26e0..a6c84e4aa0 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp @@ -15,7 +15,7 @@ #ifndef RCLCPP__NODE_INTERFACES__NODE_BASE_HPP_ #define RCLCPP__NODE_INTERFACES__NODE_BASE_HPP_ -#include +#include #include #include #include diff --git a/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp index b0f8fbd65e..2e94b80d9c 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp @@ -15,10 +15,10 @@ #ifndef RCLCPP__NODE_INTERFACES__NODE_BASE_INTERFACE_HPP_ #define RCLCPP__NODE_INTERFACES__NODE_BASE_INTERFACE_HPP_ +#include +#include #include -#include #include -#include #include "rcl/node.h" diff --git a/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp b/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp index 794038d386..d167515784 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_graph.hpp @@ -15,7 +15,9 @@ #ifndef RCLCPP__NODE_INTERFACES__NODE_GRAPH_HPP_ #define RCLCPP__NODE_INTERFACES__NODE_GRAPH_HPP_ +#include #include +#include #include #include #include diff --git a/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp b/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp index eda68451a9..ffbb400c11 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_parameters.hpp @@ -15,9 +15,10 @@ #ifndef RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_HPP_ #define RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_HPP_ +#include #include #include -#include +#include #include #include diff --git a/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp index 104e00327a..3156668a73 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp @@ -15,8 +15,8 @@ #ifndef RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_INTERFACE_HPP_ #define RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_INTERFACE_HPP_ +#include #include -#include #include #include diff --git a/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp index ca69e86e73..7aa40a3d5e 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp @@ -15,8 +15,6 @@ #ifndef RCLCPP__NODE_INTERFACES__NODE_TOPICS_INTERFACE_HPP_ #define RCLCPP__NODE_INTERFACES__NODE_TOPICS_INTERFACE_HPP_ -#include -#include #include #include "rcl/publisher.h" From dec766228dae4e050d37c8d5b8185383fdec76ab Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 4 Oct 2022 14:04:12 -0400 Subject: [PATCH 22/53] Cleanup the rclcpp_lifecycle dependencies. (#2021) That is, make sure they are all listed in package.xml, found in the CMakeLists.txt, and properly included where they are used. Signed-off-by: Chris Lalancette --- rclcpp_lifecycle/CMakeLists.txt | 97 +++++++------------ .../rclcpp_lifecycle/lifecycle_node.hpp | 8 +- .../rclcpp_lifecycle/lifecycle_node_impl.hpp | 16 ++- .../rclcpp_lifecycle/lifecycle_publisher.hpp | 3 + .../lifecycle_node_interface.hpp | 2 - .../include/rclcpp_lifecycle/state.hpp | 1 + .../include/rclcpp_lifecycle/transition.hpp | 1 + rclcpp_lifecycle/package.xml | 11 ++- rclcpp_lifecycle/src/lifecycle_node.cpp | 11 ++- .../src/lifecycle_node_interface_impl.hpp | 5 +- .../lifecycle_node_interface.cpp | 2 +- rclcpp_lifecycle/src/state.cpp | 1 + rclcpp_lifecycle/src/transition.cpp | 1 + rclcpp_lifecycle/test/test_client.cpp | 1 + .../test/test_lifecycle_service_client.cpp | 2 + rclcpp_lifecycle/test/test_service.cpp | 2 + .../test/test_transition_wrapper.cpp | 4 +- 17 files changed, 87 insertions(+), 81 deletions(-) diff --git a/rclcpp_lifecycle/CMakeLists.txt b/rclcpp_lifecycle/CMakeLists.txt index 7a5b4cf2f0..70eea6f808 100644 --- a/rclcpp_lifecycle/CMakeLists.txt +++ b/rclcpp_lifecycle/CMakeLists.txt @@ -11,10 +11,13 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() find_package(ament_cmake_ros REQUIRED) +find_package(lifecycle_msgs REQUIRED) +find_package(rcl REQUIRED) find_package(rclcpp REQUIRED) +find_package(rcl_interfaces REQUIRED) find_package(rcl_lifecycle REQUIRED) +find_package(rcutils REQUIRED) find_package(rosidl_typesupport_cpp REQUIRED) -find_package(lifecycle_msgs REQUIRED) ### CPP High level library add_library(rclcpp_lifecycle @@ -28,12 +31,14 @@ target_include_directories(${PROJECT_NAME} PUBLIC "$" "$") -# specific order: dependents before dependencies -ament_target_dependencies(rclcpp_lifecycle - "rclcpp" - "rcl_lifecycle" - "lifecycle_msgs" - "rosidl_typesupport_cpp" +target_link_libraries(${PROJECT_NAME} + ${lifecycle_msgs_TARGETS} + rcl::rcl + rclcpp::rclcpp + ${rcl_interfaces_TARGETS} + rcl_lifecycle::rcl_lifecycle + rcutils::rcutils + rosidl_typesupport_cpp::rosidl_typesupport_cpp ) # Causes the visibility macros to use dllexport rather than dllimport, @@ -46,6 +51,9 @@ install(TARGETS LIBRARY DESTINATION lib RUNTIME DESTINATION bin) +install(DIRECTORY include/ + DESTINATION include/${PROJECT_NAME}) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # Give cppcheck hints about macro definitions coming from outside this package @@ -58,15 +66,13 @@ if(BUILD_TESTING) benchmark_lifecycle_client test/benchmark/benchmark_lifecycle_client.cpp) if(TARGET benchmark_lifecycle_client) - target_link_libraries(benchmark_lifecycle_client ${PROJECT_NAME}) - ament_target_dependencies(benchmark_lifecycle_client rclcpp) + target_link_libraries(benchmark_lifecycle_client ${PROJECT_NAME} rclcpp::rclcpp) endif() add_performance_test( benchmark_lifecycle_node test/benchmark/benchmark_lifecycle_node.cpp) if(TARGET benchmark_lifecycle_node) - target_link_libraries(benchmark_lifecycle_node ${PROJECT_NAME}) - ament_target_dependencies(benchmark_lifecycle_node rclcpp) + target_link_libraries(benchmark_lifecycle_node ${PROJECT_NAME} rclcpp::rclcpp) endif() add_performance_test( benchmark_state @@ -83,31 +89,21 @@ if(BUILD_TESTING) ament_add_gtest(test_lifecycle_node test/test_lifecycle_node.cpp TIMEOUT 120) if(TARGET test_lifecycle_node) - ament_target_dependencies(test_lifecycle_node - "rcl_lifecycle" - "rclcpp" - "rcutils" - ) - target_link_libraries(test_lifecycle_node ${PROJECT_NAME} mimick) + target_link_libraries(test_lifecycle_node ${PROJECT_NAME} mimick rcl_lifecycle::rcl_lifecycle rclcpp::rclcpp rcutils::rcutils) endif() ament_add_gtest(test_lifecycle_publisher test/test_lifecycle_publisher.cpp) if(TARGET test_lifecycle_publisher) - ament_target_dependencies(test_lifecycle_publisher - "rcl_lifecycle" - "rclcpp" - "test_msgs" - ) - target_link_libraries(test_lifecycle_publisher ${PROJECT_NAME}) + target_link_libraries(test_lifecycle_publisher ${PROJECT_NAME} rcl_lifecycle::rcl_lifecycle rclcpp::rclcpp ${test_msgs_TARGETS}) endif() ament_add_gtest(test_lifecycle_service_client test/test_lifecycle_service_client.cpp TIMEOUT 120) if(TARGET test_lifecycle_service_client) - ament_target_dependencies(test_lifecycle_service_client - "rcl_lifecycle" - "rclcpp" - "rcpputils" - "rcutils" - ) - target_link_libraries(test_lifecycle_service_client ${PROJECT_NAME} mimick) + target_link_libraries(test_lifecycle_service_client + ${PROJECT_NAME} + mimick + rcl_lifecycle::rcl_lifecycle + rclcpp::rclcpp + rcpputils::rcpputils + rcutils::rcutils) endif() ament_add_gtest(test_client test/test_client.cpp TIMEOUT 120) if(TARGET test_client) @@ -127,44 +123,23 @@ if(BUILD_TESTING) endif() ament_add_gtest(test_state_machine_info test/test_state_machine_info.cpp) if(TARGET test_state_machine_info) - ament_target_dependencies(test_state_machine_info - "rcl_lifecycle" - "rclcpp" - ) - target_link_libraries(test_state_machine_info ${PROJECT_NAME}) + target_link_libraries(test_state_machine_info ${PROJECT_NAME} rcl_lifecycle::rcl_lifecycle rclcpp::rclcpp) endif() ament_add_gtest(test_register_custom_callbacks test/test_register_custom_callbacks.cpp) if(TARGET test_register_custom_callbacks) - ament_target_dependencies(test_register_custom_callbacks - "rcl_lifecycle" - "rclcpp" - ) - target_link_libraries(test_register_custom_callbacks ${PROJECT_NAME}) + target_link_libraries(test_register_custom_callbacks ${PROJECT_NAME} rcl_lifecycle::rcl_lifecycle rclcpp::rclcpp) endif() ament_add_gtest(test_callback_exceptions test/test_callback_exceptions.cpp) if(TARGET test_callback_exceptions) - ament_target_dependencies(test_callback_exceptions - "rcl_lifecycle" - "rclcpp" - ) - target_link_libraries(test_callback_exceptions ${PROJECT_NAME}) + target_link_libraries(test_callback_exceptions ${PROJECT_NAME} rcl_lifecycle::rcl_lifecycle rclcpp::rclcpp) endif() ament_add_gtest(test_state_wrapper test/test_state_wrapper.cpp) if(TARGET test_state_wrapper) - ament_target_dependencies(test_state_wrapper - "rcl_lifecycle" - "rclcpp" - ) - target_link_libraries(test_state_wrapper ${PROJECT_NAME}) + target_link_libraries(test_state_wrapper ${PROJECT_NAME} rcl_lifecycle::rcl_lifecycle rclcpp::rclcpp) endif() ament_add_gtest(test_transition_wrapper test/test_transition_wrapper.cpp) if(TARGET test_transition_wrapper) - ament_target_dependencies(test_transition_wrapper - "rcl_lifecycle" - "rclcpp" - "rcutils" - ) - target_link_libraries(test_transition_wrapper ${PROJECT_NAME} mimick) + target_link_libraries(test_transition_wrapper ${PROJECT_NAME} mimick rcl_lifecycle::rcl_lifecycle rclcpp::rclcpp rcutils::rcutils) target_compile_definitions(test_transition_wrapper PUBLIC RCUTILS_ENABLE_FAULT_INJECTION ) @@ -178,11 +153,7 @@ ament_export_libraries(${PROJECT_NAME}) # Export modern CMake targets ament_export_targets(${PROJECT_NAME}) -# specific order: dependents before dependencies -ament_export_dependencies(rclcpp) -ament_export_dependencies(rcl_lifecycle) -ament_export_dependencies(lifecycle_msgs) -ament_package() +# Export dependencies +ament_export_dependencies(lifecycle_msgs rcl rclcpp rcl_interfaces rcl_lifecycle rcutils rosidl_typesupport_cpp) -install(DIRECTORY include/ - DESTINATION include/${PROJECT_NAME}) +ament_package() diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 5e06876018..914cc4a2d3 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -36,6 +36,8 @@ #ifndef RCLCPP_LIFECYCLE__LIFECYCLE_NODE_HPP_ #define RCLCPP_LIFECYCLE__LIFECYCLE_NODE_HPP_ +#include +#include #include #include #include @@ -49,13 +51,11 @@ #include "rcl_interfaces/msg/list_parameters_result.hpp" #include "rcl_interfaces/msg/parameter_descriptor.hpp" -#include "rcl_interfaces/msg/parameter_event.hpp" #include "rcl_interfaces/msg/set_parameters_result.hpp" #include "rclcpp/callback_group.hpp" #include "rclcpp/client.hpp" #include "rclcpp/clock.hpp" -#include "rclcpp/context.hpp" #include "rclcpp/event.hpp" #include "rclcpp/generic_publisher.hpp" #include "rclcpp/generic_subscription.hpp" @@ -83,6 +83,8 @@ #include "rclcpp/time.hpp" #include "rclcpp/timer.hpp" +#include "rmw/types.h" + #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/lifecycle_publisher.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -137,7 +139,7 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, const rclcpp::NodeOptions & options = rclcpp::NodeOptions(), bool enable_communication_interface = true); - /// Create a node based on the node name and a rclcpp::Context. + /// Create a node based on the node name /** * \param[in] node_name Name of the node. * \param[in] namespace_ Namespace of the node. diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp index e2a5696b92..5ffc8a5f1b 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp @@ -15,29 +15,35 @@ #ifndef RCLCPP_LIFECYCLE__LIFECYCLE_NODE_IMPL_HPP_ #define RCLCPP_LIFECYCLE__LIFECYCLE_NODE_IMPL_HPP_ +#include +#include +#include #include #include #include #include #include -#include "rclcpp/contexts/default_context.hpp" +#include "rcl_interfaces/msg/parameter_descriptor.hpp" + +#include "rclcpp/callback_group.hpp" #include "rclcpp/create_client.hpp" #include "rclcpp/create_generic_publisher.hpp" #include "rclcpp/create_generic_subscription.hpp" #include "rclcpp/create_publisher.hpp" #include "rclcpp/create_service.hpp" #include "rclcpp/create_subscription.hpp" -#include "rclcpp/event.hpp" -#include "rclcpp/experimental/intra_process_manager.hpp" #include "rclcpp/parameter.hpp" +#include "rclcpp/publisher_options.hpp" +#include "rclcpp/qos.hpp" #include "rclcpp/subscription_options.hpp" #include "rclcpp/type_support_decl.hpp" -#include "lifecycle_publisher.hpp" -#include "rclcpp_lifecycle/visibility_control.h" +#include "rmw/types.h" #include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "rclcpp_lifecycle/lifecycle_publisher.hpp" +#include "rclcpp_lifecycle/visibility_control.h" namespace rclcpp_lifecycle { diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp index 8df08dcb41..fa7c5b0b5e 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp @@ -20,7 +20,10 @@ #include #include "rclcpp/logging.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/publisher.hpp" +#include "rclcpp/publisher_options.hpp" +#include "rclcpp/qos.hpp" #include "rclcpp_lifecycle/managed_entity.hpp" diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp index 9f2459e296..90f98ec8bc 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp @@ -17,8 +17,6 @@ #include "lifecycle_msgs/msg/transition.hpp" -#include "rcl_lifecycle/rcl_lifecycle.h" - #include "rclcpp_lifecycle/state.hpp" #include "rclcpp_lifecycle/visibility_control.h" diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp index a0ac997ff3..86750f1fed 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp @@ -18,6 +18,7 @@ #include #include "rcl_lifecycle/data_types.h" + #include "rclcpp_lifecycle/visibility_control.h" #include "rcutils/allocator.h" diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp index 874be69aa6..4093579e04 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/transition.hpp @@ -18,6 +18,7 @@ #include #include "rcl_lifecycle/data_types.h" + #include "rclcpp_lifecycle/state.hpp" #include "rclcpp_lifecycle/visibility_control.h" diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 85280c5e36..cfd97eafd6 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -13,24 +13,29 @@ ament_cmake_ros lifecycle_msgs + rcl rclcpp + rcl_interfaces rcl_lifecycle + rcutils + rmw rosidl_typesupport_cpp lifecycle_msgs + rcl rclcpp + rcl_interfaces rcl_lifecycle + rcutils + rmw rosidl_typesupport_cpp - rmw - ament_cmake_gtest ament_lint_auto ament_lint_common mimick_vendor performance_test_fixture rcpputils - rcutils test_msgs diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 0851c596ec..6765b8037e 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -14,15 +14,22 @@ #include "rclcpp_lifecycle/lifecycle_node.hpp" -#include +#include +#include #include #include -#include +#include +#include #include +#include #include "lifecycle_msgs/msg/state.hpp" #include "lifecycle_msgs/msg/transition.hpp" +#include "rcl_interfaces/msg/list_parameters_result.hpp" +#include "rcl_interfaces/msg/parameter_descriptor.hpp" +#include "rcl_interfaces/msg/set_parameters_result.hpp" + #include "rclcpp/exceptions.hpp" #include "rclcpp/graph_listener.hpp" #include "rclcpp/logger.hpp" diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp index 51de8eab07..4b9a3ff5fd 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp @@ -20,9 +20,10 @@ #include #include #include +#include #include -#include #include +#include #include "lifecycle_msgs/msg/transition_description.hpp" #include "lifecycle_msgs/msg/transition_event.h" // for getting the c-typesupport @@ -42,6 +43,8 @@ #include "rcutils/logging_macros.h" +#include "rmw/types.h" + namespace rclcpp_lifecycle { diff --git a/rclcpp_lifecycle/src/node_interfaces/lifecycle_node_interface.cpp b/rclcpp_lifecycle/src/node_interfaces/lifecycle_node_interface.cpp index 1c8a5a646a..06b889be05 100644 --- a/rclcpp_lifecycle/src/node_interfaces/lifecycle_node_interface.cpp +++ b/rclcpp_lifecycle/src/node_interfaces/lifecycle_node_interface.cpp @@ -14,7 +14,7 @@ #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" -#include "rcl_lifecycle/rcl_lifecycle.h" +#include "rclcpp_lifecycle/state.hpp" namespace rclcpp_lifecycle { diff --git a/rclcpp_lifecycle/src/state.cpp b/rclcpp_lifecycle/src/state.cpp index f7aca2688e..959b40ed2a 100644 --- a/rclcpp_lifecycle/src/state.cpp +++ b/rclcpp_lifecycle/src/state.cpp @@ -14,6 +14,7 @@ #include "rclcpp_lifecycle/state.hpp" +#include #include #include "lifecycle_msgs/msg/state.hpp" diff --git a/rclcpp_lifecycle/src/transition.cpp b/rclcpp_lifecycle/src/transition.cpp index 50f0b33735..360c1cfb13 100644 --- a/rclcpp_lifecycle/src/transition.cpp +++ b/rclcpp_lifecycle/src/transition.cpp @@ -14,6 +14,7 @@ #include "rclcpp_lifecycle/transition.hpp" +#include #include #include "lifecycle_msgs/msg/transition.hpp" diff --git a/rclcpp_lifecycle/test/test_client.cpp b/rclcpp_lifecycle/test/test_client.cpp index 89e1f42fa6..7ded8e346e 100644 --- a/rclcpp_lifecycle/test/test_client.cpp +++ b/rclcpp_lifecycle/test/test_client.cpp @@ -24,6 +24,7 @@ #include "rcl_interfaces/srv/list_parameters.hpp" +#include "rmw/qos_profiles.h" class TestClient : public ::testing::Test { diff --git a/rclcpp_lifecycle/test/test_lifecycle_service_client.cpp b/rclcpp_lifecycle/test/test_lifecycle_service_client.cpp index 6dd2c0ec17..3698c807f6 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_service_client.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_service_client.cpp @@ -33,6 +33,8 @@ #include "lifecycle_msgs/srv/get_available_transitions.hpp" #include "lifecycle_msgs/srv/get_state.hpp" +#include "rcl_lifecycle/rcl_lifecycle.h" + #include "rclcpp/node_interfaces/node_graph.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" diff --git a/rclcpp_lifecycle/test/test_service.cpp b/rclcpp_lifecycle/test/test_service.cpp index 4084b0da0c..b3108a9042 100644 --- a/rclcpp_lifecycle/test/test_service.cpp +++ b/rclcpp_lifecycle/test/test_service.cpp @@ -22,6 +22,8 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "rmw/qos_profiles.h" + #include "test_msgs/srv/empty.hpp" using namespace std::chrono_literals; diff --git a/rclcpp_lifecycle/test/test_transition_wrapper.cpp b/rclcpp_lifecycle/test/test_transition_wrapper.cpp index 4f72d90bb6..39791d5da2 100644 --- a/rclcpp_lifecycle/test/test_transition_wrapper.cpp +++ b/rclcpp_lifecycle/test/test_transition_wrapper.cpp @@ -18,10 +18,12 @@ #include #include -#include "rcutils/testing/fault_injection.h" +#include "rcl_lifecycle/rcl_lifecycle.h" #include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "rcutils/testing/fault_injection.h" + #include "./mocking_utils/patch.hpp" class TestTransitionWrapper : public ::testing::Test From 3aca271ef5d25c1b8c12e67ecf85166856a88000 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crist=C3=B3bal=20Arroyo?= <69475004+Crola1702@users.noreply.github.com> Date: Mon, 10 Oct 2022 06:17:24 -0700 Subject: [PATCH 23/53] Set cpplint test timeout to 3 minutes (#2022) Signed-off-by: Crola1702 --- rclcpp/CMakeLists.txt | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 9572f422a8..3abd999f3d 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -259,4 +259,8 @@ if(TEST cppcheck) set_tests_properties(cppcheck PROPERTIES TIMEOUT 500) endif() +if(TEST cpplint) + set_tests_properties(cpplint PROPERTIES TIMEOUT 180) +endif() + ament_generate_version_header(${PROJECT_NAME}) From b9b1468d15c7ddc697c079e6934d54f183294280 Mon Sep 17 00:00:00 2001 From: uupks Date: Thu, 13 Oct 2022 20:30:02 +0800 Subject: [PATCH 24/53] check thread whether joinable before join (#2019) Signed-off-by: uupks --- rclcpp/src/rclcpp/signal_handler.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/rclcpp/src/rclcpp/signal_handler.cpp b/rclcpp/src/rclcpp/signal_handler.cpp index 35ceb56781..7085c63bdf 100644 --- a/rclcpp/src/rclcpp/signal_handler.cpp +++ b/rclcpp/src/rclcpp/signal_handler.cpp @@ -191,7 +191,9 @@ SignalHandler::uninstall() signal_handlers_options_ = SignalHandlerOptions::None; RCLCPP_DEBUG(get_logger(), "SignalHandler::uninstall(): notifying deferred signal handler"); notify_signal_handler(); - signal_handler_thread_.join(); + if (signal_handler_thread_.joinable()) { + signal_handler_thread_.join(); + } teardown_wait_for_signal(); } catch (...) { installed_.exchange(true); From 28890bf126ff16adc1fb4763a7f9750d7397790d Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 24 Oct 2022 08:43:17 -0400 Subject: [PATCH 25/53] Cleanup the lifecycle implementation (#2027) * Split lifecycle_node_interface_impl into header and implementation. There is no reason it should all be in the header file. No functional change. * Mark LifecycleNodeInterfaceImpl as final. * Update documentation about return codes. * Mark a bunch of LifecycleNodeInterfaceImpl methods as const. * Make most of LifecycleNodeInterfaceImpl private. * Mark some LifecycleNode methods as const. * Disable copies on LifecycleNodeInterfaceImpl. Signed-off-by: Chris Lalancette --- rclcpp_lifecycle/CMakeLists.txt | 1 + .../rclcpp_lifecycle/lifecycle_node.hpp | 6 +- .../lifecycle_node_interface.hpp | 10 +- rclcpp_lifecycle/src/lifecycle_node.cpp | 6 +- .../src/lifecycle_node_interface_impl.cpp | 540 ++++++++++++++++++ .../src/lifecycle_node_interface_impl.hpp | 514 ++--------------- 6 files changed, 612 insertions(+), 465 deletions(-) create mode 100644 rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp diff --git a/rclcpp_lifecycle/CMakeLists.txt b/rclcpp_lifecycle/CMakeLists.txt index 70eea6f808..b3461a7333 100644 --- a/rclcpp_lifecycle/CMakeLists.txt +++ b/rclcpp_lifecycle/CMakeLists.txt @@ -22,6 +22,7 @@ find_package(rosidl_typesupport_cpp REQUIRED) ### CPP High level library add_library(rclcpp_lifecycle src/lifecycle_node.cpp + src/lifecycle_node_interface_impl.cpp src/managed_entity.cpp src/node_interfaces/lifecycle_node_interface.cpp src/state.cpp diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 914cc4a2d3..80ec76a769 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -847,7 +847,7 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, */ RCLCPP_LIFECYCLE_PUBLIC std::vector - get_available_states(); + get_available_states() const; /// Return a list with the current available transitions. /** @@ -855,7 +855,7 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, */ RCLCPP_LIFECYCLE_PUBLIC std::vector - get_available_transitions(); + get_available_transitions() const; /// Return a list with all the transitions. /** @@ -863,7 +863,7 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, */ RCLCPP_LIFECYCLE_PUBLIC std::vector - get_transition_graph(); + get_transition_graph() const; /// Trigger the specified transition. /* diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp index 90f98ec8bc..e653ad412d 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp @@ -61,7 +61,7 @@ class LifecycleNodeInterface /// Callback function for cleanup transition /* - * \return true by default + * \return SUCCESS by default */ RCLCPP_LIFECYCLE_PUBLIC virtual CallbackReturn @@ -69,7 +69,7 @@ class LifecycleNodeInterface /// Callback function for shutdown transition /* - * \return true by default + * \return SUCCESS by default */ RCLCPP_LIFECYCLE_PUBLIC virtual CallbackReturn @@ -77,7 +77,7 @@ class LifecycleNodeInterface /// Callback function for activate transition /* - * \return true by default + * \return SUCCESS by default */ RCLCPP_LIFECYCLE_PUBLIC virtual CallbackReturn @@ -85,7 +85,7 @@ class LifecycleNodeInterface /// Callback function for deactivate transition /* - * \return true by default + * \return SUCCESS by default */ RCLCPP_LIFECYCLE_PUBLIC virtual CallbackReturn @@ -93,7 +93,7 @@ class LifecycleNodeInterface /// Callback function for errorneous transition /* - * \return false by default + * \return SUCCESS by default */ RCLCPP_LIFECYCLE_PUBLIC virtual CallbackReturn diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 6765b8037e..f2db5f3950 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -541,19 +541,19 @@ LifecycleNode::get_current_state() } std::vector -LifecycleNode::get_available_states() +LifecycleNode::get_available_states() const { return impl_->get_available_states(); } std::vector -LifecycleNode::get_available_transitions() +LifecycleNode::get_available_transitions() const { return impl_->get_available_transitions(); } std::vector -LifecycleNode::get_transition_graph() +LifecycleNode::get_transition_graph() const { return impl_->get_transition_graph(); } diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp new file mode 100644 index 0000000000..f176cbf535 --- /dev/null +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp @@ -0,0 +1,540 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include + +#include "lifecycle_msgs/msg/transition_description.hpp" +#include "lifecycle_msgs/msg/transition_event.h" // for getting the c-typesupport +#include "lifecycle_msgs/msg/transition_event.hpp" +#include "lifecycle_msgs/srv/change_state.hpp" +#include "lifecycle_msgs/srv/get_state.hpp" +#include "lifecycle_msgs/srv/get_available_states.hpp" +#include "lifecycle_msgs/srv/get_available_transitions.hpp" + +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_services_interface.hpp" + +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +#include "rcl/error_handling.h" +#include "rcl/node.h" + +#include "rcl_lifecycle/rcl_lifecycle.h" +#include "rcl_lifecycle/transition_map.h" + +#include "rcutils/logging_macros.h" + +#include "rmw/types.h" + +#include "lifecycle_node_interface_impl.hpp" + +namespace rclcpp_lifecycle +{ + +LifecycleNode::LifecycleNodeInterfaceImpl::LifecycleNodeInterfaceImpl( + std::shared_ptr node_base_interface, + std::shared_ptr node_services_interface) +: node_base_interface_(node_base_interface), + node_services_interface_(node_services_interface) +{ +} + +LifecycleNode::LifecycleNodeInterfaceImpl::~LifecycleNodeInterfaceImpl() +{ + rcl_node_t * node_handle = node_base_interface_->get_rcl_node_handle(); + auto ret = rcl_lifecycle_state_machine_fini(&state_machine_, node_handle); + if (ret != RCL_RET_OK) { + RCUTILS_LOG_FATAL_NAMED( + "rclcpp_lifecycle", + "failed to destroy rcl_state_machine"); + } +} + +void +LifecycleNode::LifecycleNodeInterfaceImpl::init(bool enable_communication_interface) +{ + rcl_node_t * node_handle = node_base_interface_->get_rcl_node_handle(); + const rcl_node_options_t * node_options = + rcl_node_get_options(node_base_interface_->get_rcl_node_handle()); + state_machine_ = rcl_lifecycle_get_zero_initialized_state_machine(); + auto state_machine_options = rcl_lifecycle_get_default_state_machine_options(); + state_machine_options.enable_com_interface = enable_communication_interface; + state_machine_options.allocator = node_options->allocator; + + // The call to initialize the state machine takes + // currently five different typesupports for all publishers/services + // created within the RCL_LIFECYCLE structure. + // The publisher takes a C-Typesupport since the publishing (i.e. creating + // the message) is done fully in RCL. + // Services are handled in C++, so that it needs a C++ typesupport structure. + rcl_ret_t ret = rcl_lifecycle_state_machine_init( + &state_machine_, + node_handle, + ROSIDL_GET_MSG_TYPE_SUPPORT(lifecycle_msgs, msg, TransitionEvent), + rosidl_typesupport_cpp::get_service_type_support_handle(), + rosidl_typesupport_cpp::get_service_type_support_handle(), + rosidl_typesupport_cpp::get_service_type_support_handle(), + rosidl_typesupport_cpp::get_service_type_support_handle(), + rosidl_typesupport_cpp::get_service_type_support_handle(), + &state_machine_options); + if (ret != RCL_RET_OK) { + throw std::runtime_error( + std::string("Couldn't initialize state machine for node ") + + node_base_interface_->get_name()); + } + + if (enable_communication_interface) { + { // change_state + auto cb = std::bind( + &LifecycleNode::LifecycleNodeInterfaceImpl::on_change_state, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); + rclcpp::AnyServiceCallback any_cb; + any_cb.set(std::move(cb)); + + srv_change_state_ = std::make_shared>( + node_base_interface_->get_shared_rcl_node_handle(), + &state_machine_.com_interface.srv_change_state, + any_cb); + node_services_interface_->add_service( + std::dynamic_pointer_cast(srv_change_state_), + nullptr); + } + + { // get_state + auto cb = std::bind( + &LifecycleNode::LifecycleNodeInterfaceImpl::on_get_state, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); + rclcpp::AnyServiceCallback any_cb; + any_cb.set(std::move(cb)); + + srv_get_state_ = std::make_shared>( + node_base_interface_->get_shared_rcl_node_handle(), + &state_machine_.com_interface.srv_get_state, + any_cb); + node_services_interface_->add_service( + std::dynamic_pointer_cast(srv_get_state_), + nullptr); + } + + { // get_available_states + auto cb = std::bind( + &LifecycleNode::LifecycleNodeInterfaceImpl::on_get_available_states, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); + rclcpp::AnyServiceCallback any_cb; + any_cb.set(std::move(cb)); + + srv_get_available_states_ = std::make_shared>( + node_base_interface_->get_shared_rcl_node_handle(), + &state_machine_.com_interface.srv_get_available_states, + any_cb); + node_services_interface_->add_service( + std::dynamic_pointer_cast(srv_get_available_states_), + nullptr); + } + + { // get_available_transitions + auto cb = std::bind( + &LifecycleNode::LifecycleNodeInterfaceImpl::on_get_available_transitions, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); + rclcpp::AnyServiceCallback any_cb; + any_cb.set(std::move(cb)); + + srv_get_available_transitions_ = + std::make_shared>( + node_base_interface_->get_shared_rcl_node_handle(), + &state_machine_.com_interface.srv_get_available_transitions, + any_cb); + node_services_interface_->add_service( + std::dynamic_pointer_cast(srv_get_available_transitions_), + nullptr); + } + + { // get_transition_graph + auto cb = std::bind( + &LifecycleNode::LifecycleNodeInterfaceImpl::on_get_transition_graph, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); + rclcpp::AnyServiceCallback any_cb; + any_cb.set(std::move(cb)); + + srv_get_transition_graph_ = + std::make_shared>( + node_base_interface_->get_shared_rcl_node_handle(), + &state_machine_.com_interface.srv_get_transition_graph, + any_cb); + node_services_interface_->add_service( + std::dynamic_pointer_cast(srv_get_transition_graph_), + nullptr); + } + } +} + +bool +LifecycleNode::LifecycleNodeInterfaceImpl::register_callback( + std::uint8_t lifecycle_transition, + std::function & cb) +{ + cb_map_[lifecycle_transition] = cb; + return true; +} + +void +LifecycleNode::LifecycleNodeInterfaceImpl::on_change_state( + const std::shared_ptr header, + const std::shared_ptr req, + std::shared_ptr resp) +{ + (void)header; + if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { + throw std::runtime_error( + "Can't get state. State machine is not initialized."); + } + + auto transition_id = req->transition.id; + // if there's a label attached to the request, + // we check the transition attached to this label. + // we further can't compare the id of the looked up transition + // because ros2 service call defaults all intergers to zero. + // that means if we call ros2 service call ... {transition: {label: shutdown}} + // the id of the request is 0 (zero) whereas the id from the lookup up transition + // can be different. + // the result of this is that the label takes presedence of the id. + if (req->transition.label.size() != 0) { + auto rcl_transition = rcl_lifecycle_get_transition_by_label( + state_machine_.current_state, req->transition.label.c_str()); + if (rcl_transition == nullptr) { + resp->success = false; + return; + } + transition_id = static_cast(rcl_transition->id); + } + + node_interfaces::LifecycleNodeInterface::CallbackReturn cb_return_code; + auto ret = change_state(transition_id, cb_return_code); + (void) ret; + // TODO(karsten1987): Lifecycle msgs have to be extended to keep both returns + // 1. return is the actual transition + // 2. return is whether an error occurred or not + resp->success = + (cb_return_code == node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); +} + +void +LifecycleNode::LifecycleNodeInterfaceImpl::on_get_state( + const std::shared_ptr header, + const std::shared_ptr req, + std::shared_ptr resp) const +{ + (void)header; + (void)req; + if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { + throw std::runtime_error( + "Can't get state. State machine is not initialized."); + } + resp->current_state.id = static_cast(state_machine_.current_state->id); + resp->current_state.label = state_machine_.current_state->label; +} + +void +LifecycleNode::LifecycleNodeInterfaceImpl::on_get_available_states( + const std::shared_ptr header, + const std::shared_ptr req, + std::shared_ptr resp) const +{ + (void)header; + (void)req; + if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { + throw std::runtime_error( + "Can't get available states. State machine is not initialized."); + } + + resp->available_states.resize(state_machine_.transition_map.states_size); + for (unsigned int i = 0; i < state_machine_.transition_map.states_size; ++i) { + resp->available_states[i].id = + static_cast(state_machine_.transition_map.states[i].id); + resp->available_states[i].label = + static_cast(state_machine_.transition_map.states[i].label); + } +} + +void +LifecycleNode::LifecycleNodeInterfaceImpl::on_get_available_transitions( + const std::shared_ptr header, + const std::shared_ptr req, + std::shared_ptr resp) const +{ + (void)header; + (void)req; + if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { + throw std::runtime_error( + "Can't get available transitions. State machine is not initialized."); + } + + resp->available_transitions.resize(state_machine_.current_state->valid_transition_size); + for (unsigned int i = 0; i < state_machine_.current_state->valid_transition_size; ++i) { + lifecycle_msgs::msg::TransitionDescription & trans_desc = resp->available_transitions[i]; + + auto rcl_transition = state_machine_.current_state->valid_transitions[i]; + trans_desc.transition.id = static_cast(rcl_transition.id); + trans_desc.transition.label = rcl_transition.label; + trans_desc.start_state.id = static_cast(rcl_transition.start->id); + trans_desc.start_state.label = rcl_transition.start->label; + trans_desc.goal_state.id = static_cast(rcl_transition.goal->id); + trans_desc.goal_state.label = rcl_transition.goal->label; + } +} + +void +LifecycleNode::LifecycleNodeInterfaceImpl::on_get_transition_graph( + const std::shared_ptr header, + const std::shared_ptr req, + std::shared_ptr resp) const +{ + (void)header; + (void)req; + if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { + throw std::runtime_error( + "Can't get available transitions. State machine is not initialized."); + } + + resp->available_transitions.resize(state_machine_.transition_map.transitions_size); + for (unsigned int i = 0; i < state_machine_.transition_map.transitions_size; ++i) { + lifecycle_msgs::msg::TransitionDescription & trans_desc = resp->available_transitions[i]; + + auto rcl_transition = state_machine_.transition_map.transitions[i]; + trans_desc.transition.id = static_cast(rcl_transition.id); + trans_desc.transition.label = rcl_transition.label; + trans_desc.start_state.id = static_cast(rcl_transition.start->id); + trans_desc.start_state.label = rcl_transition.start->label; + trans_desc.goal_state.id = static_cast(rcl_transition.goal->id); + trans_desc.goal_state.label = rcl_transition.goal->label; + } +} + +const State & +LifecycleNode::LifecycleNodeInterfaceImpl::get_current_state() +{ + current_state_ = State(state_machine_.current_state); + return current_state_; +} + +std::vector +LifecycleNode::LifecycleNodeInterfaceImpl::get_available_states() const +{ + std::vector states; + states.reserve(state_machine_.transition_map.states_size); + + for (unsigned int i = 0; i < state_machine_.transition_map.states_size; ++i) { + states.emplace_back(&state_machine_.transition_map.states[i]); + } + return states; +} + +std::vector +LifecycleNode::LifecycleNodeInterfaceImpl::get_available_transitions() const +{ + std::vector transitions; + transitions.reserve(state_machine_.current_state->valid_transition_size); + + for (unsigned int i = 0; i < state_machine_.current_state->valid_transition_size; ++i) { + transitions.emplace_back(&state_machine_.current_state->valid_transitions[i]); + } + return transitions; +} + +std::vector +LifecycleNode::LifecycleNodeInterfaceImpl::get_transition_graph() const +{ + std::vector transitions; + transitions.reserve(state_machine_.transition_map.transitions_size); + + for (unsigned int i = 0; i < state_machine_.transition_map.transitions_size; ++i) { + transitions.emplace_back(&state_machine_.transition_map.transitions[i]); + } + return transitions; +} + +rcl_ret_t +LifecycleNode::LifecycleNodeInterfaceImpl::change_state( + std::uint8_t transition_id, + node_interfaces::LifecycleNodeInterface::CallbackReturn & cb_return_code) +{ + if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { + RCUTILS_LOG_ERROR( + "Unable to change state for state machine for %s: %s", + node_base_interface_->get_name(), rcl_get_error_string().str); + return RCL_RET_ERROR; + } + + constexpr bool publish_update = true; + // keep the initial state to pass to a transition callback + State initial_state(state_machine_.current_state); + + if ( + rcl_lifecycle_trigger_transition_by_id( + &state_machine_, transition_id, publish_update) != RCL_RET_OK) + { + RCUTILS_LOG_ERROR( + "Unable to start transition %u from current state %s: %s", + transition_id, state_machine_.current_state->label, rcl_get_error_string().str); + rcutils_reset_error(); + return RCL_RET_ERROR; + } + + auto get_label_for_return_code = + [](node_interfaces::LifecycleNodeInterface::CallbackReturn cb_return_code) -> const char *{ + auto cb_id = static_cast(cb_return_code); + if (cb_id == lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS) { + return rcl_lifecycle_transition_success_label; + } else if (cb_id == lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_FAILURE) { + return rcl_lifecycle_transition_failure_label; + } + return rcl_lifecycle_transition_error_label; + }; + + cb_return_code = execute_callback(state_machine_.current_state->id, initial_state); + auto transition_label = get_label_for_return_code(cb_return_code); + + if ( + rcl_lifecycle_trigger_transition_by_label( + &state_machine_, transition_label, publish_update) != RCL_RET_OK) + { + RCUTILS_LOG_ERROR( + "Failed to finish transition %u. Current state is now: %s (%s)", + transition_id, state_machine_.current_state->label, rcl_get_error_string().str); + rcutils_reset_error(); + return RCL_RET_ERROR; + } + + // error handling ?! + // TODO(karsten1987): iterate over possible ret value + if (cb_return_code == node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR) { + RCUTILS_LOG_WARN("Error occurred while doing error handling."); + + auto error_cb_code = execute_callback(state_machine_.current_state->id, initial_state); + auto error_cb_label = get_label_for_return_code(error_cb_code); + if ( + rcl_lifecycle_trigger_transition_by_label( + &state_machine_, error_cb_label, publish_update) != RCL_RET_OK) + { + RCUTILS_LOG_ERROR("Failed to call cleanup on error state: %s", rcl_get_error_string().str); + rcutils_reset_error(); + return RCL_RET_ERROR; + } + } + // This true holds in both cases where the actual callback + // was successful or not, since at this point we have a valid transistion + // to either a new primary state or error state + return RCL_RET_OK; +} + +node_interfaces::LifecycleNodeInterface::CallbackReturn +LifecycleNode::LifecycleNodeInterfaceImpl::execute_callback( + unsigned int cb_id, const State & previous_state) const +{ + // in case no callback was attached, we forward directly + auto cb_success = node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + + auto it = cb_map_.find(static_cast(cb_id)); + if (it != cb_map_.end()) { + auto callback = it->second; + try { + cb_success = callback(State(previous_state)); + } catch (const std::exception & e) { + RCUTILS_LOG_ERROR("Caught exception in callback for transition %d", it->first); + RCUTILS_LOG_ERROR("Original error: %s", e.what()); + cb_success = node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + } + } + return cb_success; +} + +const State & LifecycleNode::LifecycleNodeInterfaceImpl::trigger_transition( + const char * transition_label) +{ + node_interfaces::LifecycleNodeInterface::CallbackReturn error; + return trigger_transition(transition_label, error); +} + +const State & LifecycleNode::LifecycleNodeInterfaceImpl::trigger_transition( + const char * transition_label, + node_interfaces::LifecycleNodeInterface::CallbackReturn & cb_return_code) +{ + auto transition = + rcl_lifecycle_get_transition_by_label(state_machine_.current_state, transition_label); + if (transition) { + change_state(static_cast(transition->id), cb_return_code); + } + return get_current_state(); +} + +const State & +LifecycleNode::LifecycleNodeInterfaceImpl::trigger_transition(uint8_t transition_id) +{ + node_interfaces::LifecycleNodeInterface::CallbackReturn error; + change_state(transition_id, error); + (void) error; + return get_current_state(); +} + +const State & +LifecycleNode::LifecycleNodeInterfaceImpl::trigger_transition( + uint8_t transition_id, + node_interfaces::LifecycleNodeInterface::CallbackReturn & cb_return_code) +{ + change_state(transition_id, cb_return_code); + return get_current_state(); +} + +void +LifecycleNode::LifecycleNodeInterfaceImpl::add_managed_entity( + std::weak_ptr managed_entity) +{ + weak_managed_entities_.push_back(managed_entity); +} + +void +LifecycleNode::LifecycleNodeInterfaceImpl::add_timer_handle( + std::shared_ptr timer) +{ + weak_timers_.push_back(timer); +} + +void +LifecycleNode::LifecycleNodeInterfaceImpl::on_activate() const +{ + for (const auto & weak_entity : weak_managed_entities_) { + auto entity = weak_entity.lock(); + if (entity) { + entity->on_activate(); + } + } +} + +void +LifecycleNode::LifecycleNodeInterfaceImpl::on_deactivate() const +{ + for (const auto & weak_entity : weak_managed_entities_) { + auto entity = weak_entity.lock(); + if (entity) { + entity->on_deactivate(); + } + } +} + +} // namespace rclcpp_lifecycle diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp index 4b9a3ff5fd..d973278005 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp @@ -20,35 +20,28 @@ #include #include #include -#include -#include -#include #include -#include "lifecycle_msgs/msg/transition_description.hpp" -#include "lifecycle_msgs/msg/transition_event.h" // for getting the c-typesupport #include "lifecycle_msgs/msg/transition_event.hpp" #include "lifecycle_msgs/srv/change_state.hpp" #include "lifecycle_msgs/srv/get_state.hpp" #include "lifecycle_msgs/srv/get_available_states.hpp" #include "lifecycle_msgs/srv/get_available_transitions.hpp" -#include "rcl/error_handling.h" - #include "rcl_lifecycle/rcl_lifecycle.h" -#include "rcl_lifecycle/transition_map.h" +#include "rclcpp/macros.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/node_interfaces/node_services_interface.hpp" -#include "rcutils/logging_macros.h" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rmw/types.h" namespace rclcpp_lifecycle { -class LifecycleNode::LifecycleNodeInterfaceImpl +class LifecycleNode::LifecycleNodeInterfaceImpl final { using ChangeStateSrv = lifecycle_msgs::srv::ChangeState; using GetStateSrv = lifecycle_msgs::srv::GetState; @@ -59,489 +52,102 @@ class LifecycleNode::LifecycleNodeInterfaceImpl public: LifecycleNodeInterfaceImpl( std::shared_ptr node_base_interface, - std::shared_ptr node_services_interface) - : node_base_interface_(node_base_interface), - node_services_interface_(node_services_interface) - {} - - ~LifecycleNodeInterfaceImpl() - { - rcl_node_t * node_handle = node_base_interface_->get_rcl_node_handle(); - auto ret = rcl_lifecycle_state_machine_fini(&state_machine_, node_handle); - if (ret != RCL_RET_OK) { - RCUTILS_LOG_FATAL_NAMED( - "rclcpp_lifecycle", - "failed to destroy rcl_state_machine"); - } - } + std::shared_ptr node_services_interface); + + ~LifecycleNodeInterfaceImpl(); void - init(bool enable_communication_interface = true) - { - rcl_node_t * node_handle = node_base_interface_->get_rcl_node_handle(); - const rcl_node_options_t * node_options = - rcl_node_get_options(node_base_interface_->get_rcl_node_handle()); - state_machine_ = rcl_lifecycle_get_zero_initialized_state_machine(); - auto state_machine_options = rcl_lifecycle_get_default_state_machine_options(); - state_machine_options.enable_com_interface = enable_communication_interface; - state_machine_options.allocator = node_options->allocator; - - // The call to initialize the state machine takes - // currently five different typesupports for all publishers/services - // created within the RCL_LIFECYCLE structure. - // The publisher takes a C-Typesupport since the publishing (i.e. creating - // the message) is done fully in RCL. - // Services are handled in C++, so that it needs a C++ typesupport structure. - rcl_ret_t ret = rcl_lifecycle_state_machine_init( - &state_machine_, - node_handle, - ROSIDL_GET_MSG_TYPE_SUPPORT(lifecycle_msgs, msg, TransitionEvent), - rosidl_typesupport_cpp::get_service_type_support_handle(), - rosidl_typesupport_cpp::get_service_type_support_handle(), - rosidl_typesupport_cpp::get_service_type_support_handle(), - rosidl_typesupport_cpp::get_service_type_support_handle(), - rosidl_typesupport_cpp::get_service_type_support_handle(), - &state_machine_options); - if (ret != RCL_RET_OK) { - throw std::runtime_error( - std::string("Couldn't initialize state machine for node ") + - node_base_interface_->get_name()); - } - - if (enable_communication_interface) { - { // change_state - auto cb = std::bind( - &LifecycleNodeInterfaceImpl::on_change_state, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); - rclcpp::AnyServiceCallback any_cb; - any_cb.set(std::move(cb)); - - srv_change_state_ = std::make_shared>( - node_base_interface_->get_shared_rcl_node_handle(), - &state_machine_.com_interface.srv_change_state, - any_cb); - node_services_interface_->add_service( - std::dynamic_pointer_cast(srv_change_state_), - nullptr); - } - - { // get_state - auto cb = std::bind( - &LifecycleNodeInterfaceImpl::on_get_state, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); - rclcpp::AnyServiceCallback any_cb; - any_cb.set(std::move(cb)); - - srv_get_state_ = std::make_shared>( - node_base_interface_->get_shared_rcl_node_handle(), - &state_machine_.com_interface.srv_get_state, - any_cb); - node_services_interface_->add_service( - std::dynamic_pointer_cast(srv_get_state_), - nullptr); - } - - { // get_available_states - auto cb = std::bind( - &LifecycleNodeInterfaceImpl::on_get_available_states, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); - rclcpp::AnyServiceCallback any_cb; - any_cb.set(std::move(cb)); - - srv_get_available_states_ = std::make_shared>( - node_base_interface_->get_shared_rcl_node_handle(), - &state_machine_.com_interface.srv_get_available_states, - any_cb); - node_services_interface_->add_service( - std::dynamic_pointer_cast(srv_get_available_states_), - nullptr); - } - - { // get_available_transitions - auto cb = std::bind( - &LifecycleNodeInterfaceImpl::on_get_available_transitions, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); - rclcpp::AnyServiceCallback any_cb; - any_cb.set(std::move(cb)); - - srv_get_available_transitions_ = - std::make_shared>( - node_base_interface_->get_shared_rcl_node_handle(), - &state_machine_.com_interface.srv_get_available_transitions, - any_cb); - node_services_interface_->add_service( - std::dynamic_pointer_cast(srv_get_available_transitions_), - nullptr); - } - - { // get_transition_graph - auto cb = std::bind( - &LifecycleNodeInterfaceImpl::on_get_transition_graph, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); - rclcpp::AnyServiceCallback any_cb; - any_cb.set(std::move(cb)); - - srv_get_transition_graph_ = - std::make_shared>( - node_base_interface_->get_shared_rcl_node_handle(), - &state_machine_.com_interface.srv_get_transition_graph, - any_cb); - node_services_interface_->add_service( - std::dynamic_pointer_cast(srv_get_transition_graph_), - nullptr); - } - } - } + init(bool enable_communication_interface = true); bool register_callback( std::uint8_t lifecycle_transition, - std::function & cb) - { - cb_map_[lifecycle_transition] = cb; - return true; - } + std::function & cb); + + const State & + get_current_state(); + + std::vector + get_available_states() const; + + std::vector + get_available_transitions() const; + + std::vector + get_transition_graph() const; + + const State & + trigger_transition(uint8_t transition_id); + + const State & + trigger_transition( + uint8_t transition_id, + node_interfaces::LifecycleNodeInterface::CallbackReturn & cb_return_code); + + const State & trigger_transition(const char * transition_label); + + const State & trigger_transition( + const char * transition_label, + node_interfaces::LifecycleNodeInterface::CallbackReturn & cb_return_code); + + void + on_activate() const; + + void + on_deactivate() const; + + void + add_managed_entity(std::weak_ptr managed_entity); + + void + add_timer_handle(std::shared_ptr timer); + +private: + RCLCPP_DISABLE_COPY(LifecycleNodeInterfaceImpl) void on_change_state( const std::shared_ptr header, const std::shared_ptr req, - std::shared_ptr resp) - { - (void)header; - if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { - throw std::runtime_error( - "Can't get state. State machine is not initialized."); - } - - auto transition_id = req->transition.id; - // if there's a label attached to the request, - // we check the transition attached to this label. - // we further can't compare the id of the looked up transition - // because ros2 service call defaults all intergers to zero. - // that means if we call ros2 service call ... {transition: {label: shutdown}} - // the id of the request is 0 (zero) whereas the id from the lookup up transition - // can be different. - // the result of this is that the label takes presedence of the id. - if (req->transition.label.size() != 0) { - auto rcl_transition = rcl_lifecycle_get_transition_by_label( - state_machine_.current_state, req->transition.label.c_str()); - if (rcl_transition == nullptr) { - resp->success = false; - return; - } - transition_id = static_cast(rcl_transition->id); - } - - node_interfaces::LifecycleNodeInterface::CallbackReturn cb_return_code; - auto ret = change_state(transition_id, cb_return_code); - (void) ret; - // TODO(karsten1987): Lifecycle msgs have to be extended to keep both returns - // 1. return is the actual transition - // 2. return is whether an error occurred or not - resp->success = - (cb_return_code == node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); - } + std::shared_ptr resp); void on_get_state( const std::shared_ptr header, const std::shared_ptr req, - std::shared_ptr resp) - { - (void)header; - (void)req; - if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { - throw std::runtime_error( - "Can't get state. State machine is not initialized."); - } - resp->current_state.id = static_cast(state_machine_.current_state->id); - resp->current_state.label = state_machine_.current_state->label; - } + std::shared_ptr resp) const; void on_get_available_states( const std::shared_ptr header, const std::shared_ptr req, - std::shared_ptr resp) - { - (void)header; - (void)req; - if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { - throw std::runtime_error( - "Can't get available states. State machine is not initialized."); - } - - resp->available_states.resize(state_machine_.transition_map.states_size); - for (unsigned int i = 0; i < state_machine_.transition_map.states_size; ++i) { - resp->available_states[i].id = - static_cast(state_machine_.transition_map.states[i].id); - resp->available_states[i].label = - static_cast(state_machine_.transition_map.states[i].label); - } - } + std::shared_ptr resp) const; void on_get_available_transitions( const std::shared_ptr header, const std::shared_ptr req, - std::shared_ptr resp) - { - (void)header; - (void)req; - if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { - throw std::runtime_error( - "Can't get available transitions. State machine is not initialized."); - } - - resp->available_transitions.resize(state_machine_.current_state->valid_transition_size); - for (unsigned int i = 0; i < state_machine_.current_state->valid_transition_size; ++i) { - lifecycle_msgs::msg::TransitionDescription & trans_desc = resp->available_transitions[i]; - - auto rcl_transition = state_machine_.current_state->valid_transitions[i]; - trans_desc.transition.id = static_cast(rcl_transition.id); - trans_desc.transition.label = rcl_transition.label; - trans_desc.start_state.id = static_cast(rcl_transition.start->id); - trans_desc.start_state.label = rcl_transition.start->label; - trans_desc.goal_state.id = static_cast(rcl_transition.goal->id); - trans_desc.goal_state.label = rcl_transition.goal->label; - } - } + std::shared_ptr resp) const; void on_get_transition_graph( const std::shared_ptr header, const std::shared_ptr req, - std::shared_ptr resp) - { - (void)header; - (void)req; - if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { - throw std::runtime_error( - "Can't get available transitions. State machine is not initialized."); - } - - resp->available_transitions.resize(state_machine_.transition_map.transitions_size); - for (unsigned int i = 0; i < state_machine_.transition_map.transitions_size; ++i) { - lifecycle_msgs::msg::TransitionDescription & trans_desc = resp->available_transitions[i]; - - auto rcl_transition = state_machine_.transition_map.transitions[i]; - trans_desc.transition.id = static_cast(rcl_transition.id); - trans_desc.transition.label = rcl_transition.label; - trans_desc.start_state.id = static_cast(rcl_transition.start->id); - trans_desc.start_state.label = rcl_transition.start->label; - trans_desc.goal_state.id = static_cast(rcl_transition.goal->id); - trans_desc.goal_state.label = rcl_transition.goal->label; - } - } - - const State & - get_current_state() - { - current_state_ = State(state_machine_.current_state); - return current_state_; - } - - std::vector - get_available_states() - { - std::vector states; - states.reserve(state_machine_.transition_map.states_size); - - for (unsigned int i = 0; i < state_machine_.transition_map.states_size; ++i) { - states.emplace_back(&state_machine_.transition_map.states[i]); - } - return states; - } - - std::vector - get_available_transitions() - { - std::vector transitions; - transitions.reserve(state_machine_.current_state->valid_transition_size); - - for (unsigned int i = 0; i < state_machine_.current_state->valid_transition_size; ++i) { - transitions.emplace_back(&state_machine_.current_state->valid_transitions[i]); - } - return transitions; - } - - std::vector - get_transition_graph() - { - std::vector transitions; - transitions.reserve(state_machine_.transition_map.transitions_size); - - for (unsigned int i = 0; i < state_machine_.transition_map.transitions_size; ++i) { - transitions.emplace_back(&state_machine_.transition_map.transitions[i]); - } - return transitions; - } + std::shared_ptr resp) const; rcl_ret_t - change_state(std::uint8_t transition_id, LifecycleNodeInterface::CallbackReturn & cb_return_code) - { - if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { - RCUTILS_LOG_ERROR( - "Unable to change state for state machine for %s: %s", - node_base_interface_->get_name(), rcl_get_error_string().str); - return RCL_RET_ERROR; - } - - constexpr bool publish_update = true; - // keep the initial state to pass to a transition callback - State initial_state(state_machine_.current_state); - - if ( - rcl_lifecycle_trigger_transition_by_id( - &state_machine_, transition_id, publish_update) != RCL_RET_OK) - { - RCUTILS_LOG_ERROR( - "Unable to start transition %u from current state %s: %s", - transition_id, state_machine_.current_state->label, rcl_get_error_string().str); - rcutils_reset_error(); - return RCL_RET_ERROR; - } - - auto get_label_for_return_code = - [](node_interfaces::LifecycleNodeInterface::CallbackReturn cb_return_code) -> const char *{ - auto cb_id = static_cast(cb_return_code); - if (cb_id == lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS) { - return rcl_lifecycle_transition_success_label; - } else if (cb_id == lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_FAILURE) { - return rcl_lifecycle_transition_failure_label; - } - return rcl_lifecycle_transition_error_label; - }; - - cb_return_code = execute_callback(state_machine_.current_state->id, initial_state); - auto transition_label = get_label_for_return_code(cb_return_code); - - if ( - rcl_lifecycle_trigger_transition_by_label( - &state_machine_, transition_label, publish_update) != RCL_RET_OK) - { - RCUTILS_LOG_ERROR( - "Failed to finish transition %u. Current state is now: %s (%s)", - transition_id, state_machine_.current_state->label, rcl_get_error_string().str); - rcutils_reset_error(); - return RCL_RET_ERROR; - } - - // error handling ?! - // TODO(karsten1987): iterate over possible ret value - if (cb_return_code == node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR) { - RCUTILS_LOG_WARN("Error occurred while doing error handling."); - - auto error_cb_code = execute_callback(state_machine_.current_state->id, initial_state); - auto error_cb_label = get_label_for_return_code(error_cb_code); - if ( - rcl_lifecycle_trigger_transition_by_label( - &state_machine_, error_cb_label, publish_update) != RCL_RET_OK) - { - RCUTILS_LOG_ERROR("Failed to call cleanup on error state: %s", rcl_get_error_string().str); - rcutils_reset_error(); - return RCL_RET_ERROR; - } - } - // This true holds in both cases where the actual callback - // was successful or not, since at this point we have a valid transistion - // to either a new primary state or error state - return RCL_RET_OK; - } - - LifecycleNodeInterface::CallbackReturn - execute_callback(unsigned int cb_id, const State & previous_state) - { - // in case no callback was attached, we forward directly - auto cb_success = node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - - auto it = cb_map_.find(static_cast(cb_id)); - if (it != cb_map_.end()) { - auto callback = it->second; - try { - cb_success = callback(State(previous_state)); - } catch (const std::exception & e) { - RCUTILS_LOG_ERROR("Caught exception in callback for transition %d", it->first); - RCUTILS_LOG_ERROR("Original error: %s", e.what()); - cb_success = node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; - } - } - return cb_success; - } - - const State & trigger_transition(const char * transition_label) - { - LifecycleNodeInterface::CallbackReturn error; - return trigger_transition(transition_label, error); - } + change_state( + std::uint8_t transition_id, + node_interfaces::LifecycleNodeInterface::CallbackReturn & cb_return_code); - const State & trigger_transition( - const char * transition_label, LifecycleNodeInterface::CallbackReturn & cb_return_code) - { - auto transition = - rcl_lifecycle_get_transition_by_label(state_machine_.current_state, transition_label); - if (transition) { - change_state(static_cast(transition->id), cb_return_code); - } - return get_current_state(); - } - - const State & - trigger_transition(uint8_t transition_id) - { - LifecycleNodeInterface::CallbackReturn error; - change_state(transition_id, error); - (void) error; - return get_current_state(); - } - - const State & - trigger_transition(uint8_t transition_id, LifecycleNodeInterface::CallbackReturn & cb_return_code) - { - change_state(transition_id, cb_return_code); - return get_current_state(); - } - - void - add_managed_entity(std::weak_ptr managed_entity) - { - weak_managed_entities_.push_back(managed_entity); - } - - void - add_timer_handle(std::shared_ptr timer) - { - weak_timers_.push_back(timer); - } - - void - on_activate() - { - for (const auto & weak_entity : weak_managed_entities_) { - auto entity = weak_entity.lock(); - if (entity) { - entity->on_activate(); - } - } - } - - void - on_deactivate() - { - for (const auto & weak_entity : weak_managed_entities_) { - auto entity = weak_entity.lock(); - if (entity) { - entity->on_deactivate(); - } - } - } + node_interfaces::LifecycleNodeInterface::CallbackReturn + execute_callback(unsigned int cb_id, const State & previous_state) const; rcl_lifecycle_state_machine_t state_machine_; State current_state_; std::map< std::uint8_t, - std::function> cb_map_; + std::function> cb_map_; using NodeBasePtr = std::shared_ptr; using NodeServicesPtr = std::shared_ptr; From 2d32d03ba3fc6780818705a8ab2ca0512b64e1c4 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 24 Oct 2022 17:14:12 -0400 Subject: [PATCH 26/53] Make lifecycle impl get_current_state() const. (#2031) We should only need to update the current state when it changes, so we do that in the change_state method (which is not const). Then we can just return the current_state_ object in get_current_state, and then mark it as const. Signed-off-by: Chris Lalancette --- .../include/rclcpp_lifecycle/lifecycle_node.hpp | 2 +- rclcpp_lifecycle/src/lifecycle_node.cpp | 2 +- .../src/lifecycle_node_interface_impl.cpp | 15 +++++++++++++-- .../src/lifecycle_node_interface_impl.hpp | 2 +- 4 files changed, 16 insertions(+), 5 deletions(-) diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 80ec76a769..50d4717ec3 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -839,7 +839,7 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, */ RCLCPP_LIFECYCLE_PUBLIC const State & - get_current_state(); + get_current_state() const; /// Return a list with the available states. /** diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index f2db5f3950..773febda95 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -535,7 +535,7 @@ LifecycleNode::register_on_error( } const State & -LifecycleNode::get_current_state() +LifecycleNode::get_current_state() const { return impl_->get_current_state(); } diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp index f176cbf535..30c0ba3c0e 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp @@ -182,6 +182,8 @@ LifecycleNode::LifecycleNodeInterfaceImpl::init(bool enable_communication_interf nullptr); } } + + current_state_ = State(state_machine_.current_state); } bool @@ -327,9 +329,8 @@ LifecycleNode::LifecycleNodeInterfaceImpl::on_get_transition_graph( } const State & -LifecycleNode::LifecycleNodeInterfaceImpl::get_current_state() +LifecycleNode::LifecycleNodeInterfaceImpl::get_current_state() const { - current_state_ = State(state_machine_.current_state); return current_state_; } @@ -396,6 +397,9 @@ LifecycleNode::LifecycleNodeInterfaceImpl::change_state( return RCL_RET_ERROR; } + // Update the internal current_state_ + current_state_ = State(state_machine_.current_state); + auto get_label_for_return_code = [](node_interfaces::LifecycleNodeInterface::CallbackReturn cb_return_code) -> const char *{ auto cb_id = static_cast(cb_return_code); @@ -421,6 +425,9 @@ LifecycleNode::LifecycleNodeInterfaceImpl::change_state( return RCL_RET_ERROR; } + // Update the internal current_state_ + current_state_ = State(state_machine_.current_state); + // error handling ?! // TODO(karsten1987): iterate over possible ret value if (cb_return_code == node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR) { @@ -437,6 +444,10 @@ LifecycleNode::LifecycleNodeInterfaceImpl::change_state( return RCL_RET_ERROR; } } + + // Update the internal current_state_ + current_state_ = State(state_machine_.current_state); + // This true holds in both cases where the actual callback // was successful or not, since at this point we have a valid transistion // to either a new primary state or error state diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp index d973278005..9777bbf3a8 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp @@ -65,7 +65,7 @@ class LifecycleNode::LifecycleNodeInterfaceImpl final std::function & cb); const State & - get_current_state(); + get_current_state() const; std::vector get_available_states() const; From 85c0af4fa06264a6b92a519197774c0e10b9606e Mon Sep 17 00:00:00 2001 From: Alexis Paques Date: Tue, 25 Oct 2022 19:33:14 +0200 Subject: [PATCH 27/53] Set the minimum number of threads of the Multithreaded executor to 2 (#2030) * Set the minimum number of threads of the Multithreaded executor to 2 Signed-off-by: Alexis Paques --- .../rclcpp/executors/multi_threaded_executor.cpp | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp index bb477690be..507d47f913 100644 --- a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp @@ -21,6 +21,7 @@ #include "rcpputils/scope_exit.hpp" +#include "rclcpp/logging.hpp" #include "rclcpp/utilities.hpp" using rclcpp::executors::MultiThreadedExecutor; @@ -34,9 +35,15 @@ MultiThreadedExecutor::MultiThreadedExecutor( yield_before_execute_(yield_before_execute), next_exec_timeout_(next_exec_timeout) { - number_of_threads_ = number_of_threads ? number_of_threads : std::thread::hardware_concurrency(); - if (number_of_threads_ == 0) { - number_of_threads_ = 1; + number_of_threads_ = number_of_threads > 0 ? + number_of_threads : + std::max(std::thread::hardware_concurrency(), 2U); + + if (number_of_threads_ == 1) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "MultiThreadedExecutor is used with a single thread.\n" + "Use the SingleThreadedExecutor instead."); } } From d119157948720f5888d47898e1c59b56fe1f86c5 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Thu, 27 Oct 2022 05:18:21 +0800 Subject: [PATCH 28/53] Fix bug that a callback not reached (#1640) * Add a test case a subscriber on a new executor with a callback group triggered to receive a message Signed-off-by: Chen Lihui * fix flaky and not to use spin_some Signed-off-by: Chen Lihui * update comment Signed-off-by: Chen Lihui * update for not using anti-pattern source code Signed-off-by: Chen Lihui * add a notify guard condition for callback group Co-authored-by: William Woodall Signed-off-by: Chen Lihui * Notify guard condition of Node not to be used in Executor it is only for the waitset of GraphListener Signed-off-by: Chen Lihui * put code in the try catch Signed-off-by: Chen Lihui * defer to create guard condition Signed-off-by: Chen Lihui * use context directly for the create function Signed-off-by: Chen Lihui * cpplint Signed-off-by: Chen Lihui * fix that some case might call add_node after shutdown Signed-off-by: Chen Lihui * nitpick and fix some potential bug Signed-off-by: Chen Lihui * add sanity check as some case might not create notify guard condition after shutdown Signed-off-by: Chen Lihui * Cleanup includes. Signed-off-by: Chris Lalancette * remove destroy method make a callback group can only create one guard condition Signed-off-by: Chen Lihui * remove limitation that guard condition can not be re-created in callback group Signed-off-by: Chen Lihui Signed-off-by: Chen Lihui Signed-off-by: Chris Lalancette Co-authored-by: William Woodall Co-authored-by: Chris Lalancette --- rclcpp/include/rclcpp/callback_group.hpp | 22 ++++++- rclcpp/include/rclcpp/executor.hpp | 12 ++-- rclcpp/src/rclcpp/callback_group.cpp | 45 ++++++++++++- rclcpp/src/rclcpp/executor.cpp | 58 +++++++++-------- .../rclcpp/node_interfaces/node_services.cpp | 12 ++-- .../rclcpp/node_interfaces/node_timers.cpp | 5 +- .../rclcpp/node_interfaces/node_topics.cpp | 2 + .../rclcpp/node_interfaces/node_waitables.cpp | 6 +- rclcpp/test/rclcpp/CMakeLists.txt | 21 +++--- .../test_add_callback_groups_to_executor.cpp | 64 +++++++++++++++++++ 10 files changed, 196 insertions(+), 51 deletions(-) diff --git a/rclcpp/include/rclcpp/callback_group.hpp b/rclcpp/include/rclcpp/callback_group.hpp index 94bceced81..7d03edf343 100644 --- a/rclcpp/include/rclcpp/callback_group.hpp +++ b/rclcpp/include/rclcpp/callback_group.hpp @@ -16,11 +16,14 @@ #define RCLCPP__CALLBACK_GROUP_HPP_ #include +#include +#include #include -#include #include #include "rclcpp/client.hpp" +#include "rclcpp/context.hpp" +#include "rclcpp/guard_condition.hpp" #include "rclcpp/publisher_base.hpp" #include "rclcpp/service.hpp" #include "rclcpp/subscription_base.hpp" @@ -95,6 +98,10 @@ class CallbackGroup CallbackGroupType group_type, bool automatically_add_to_executor_with_node = true); + /// Default destructor. + RCLCPP_PUBLIC + ~CallbackGroup(); + template rclcpp::SubscriptionBase::SharedPtr find_subscription_ptrs_if(Function func) const @@ -171,6 +178,16 @@ class CallbackGroup bool automatically_add_to_executor_with_node() const; + /// Defer creating the notify guard condition and return it. + RCLCPP_PUBLIC + rclcpp::GuardCondition::SharedPtr + get_notify_guard_condition(const rclcpp::Context::SharedPtr context_ptr); + + /// Trigger the notify guard condition. + RCLCPP_PUBLIC + void + trigger_notify_guard_condition(); + protected: RCLCPP_DISABLE_COPY(CallbackGroup) @@ -213,6 +230,9 @@ class CallbackGroup std::vector waitable_ptrs_; std::atomic_bool can_be_taken_from_; const bool automatically_add_to_executor_with_node_; + // defer the creation of the guard condition + std::shared_ptr notify_guard_condition_ = nullptr; + std::recursive_mutex notify_guard_condition_mutex_; private: template diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index ed2ddc4a0a..65d0a930cb 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -560,14 +560,14 @@ class Executor virtual void spin_once_impl(std::chrono::nanoseconds timeout); - typedef std::map> - WeakNodesToGuardConditionsMap; + std::owner_less> + WeakCallbackGroupsToGuardConditionsMap; - /// maps nodes to guard conditions - WeakNodesToGuardConditionsMap - weak_nodes_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + /// maps callback groups to guard conditions + WeakCallbackGroupsToGuardConditionsMap + weak_groups_to_guard_conditions_ RCPPUTILS_TSA_GUARDED_BY(mutex_); /// maps callback groups associated to nodes WeakCallbackGroupsToNodesMap diff --git a/rclcpp/src/rclcpp/callback_group.cpp b/rclcpp/src/rclcpp/callback_group.cpp index 4b11156cf9..734c781a69 100644 --- a/rclcpp/src/rclcpp/callback_group.cpp +++ b/rclcpp/src/rclcpp/callback_group.cpp @@ -12,9 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "rclcpp/callback_group.hpp" +#include +#include +#include +#include +#include +#include -#include +#include "rclcpp/callback_group.hpp" +#include "rclcpp/client.hpp" +#include "rclcpp/service.hpp" +#include "rclcpp/subscription_base.hpp" +#include "rclcpp/timer.hpp" +#include "rclcpp/waitable.hpp" using rclcpp::CallbackGroup; using rclcpp::CallbackGroupType; @@ -27,6 +37,10 @@ CallbackGroup::CallbackGroup( automatically_add_to_executor_with_node_(automatically_add_to_executor_with_node) {} +CallbackGroup::~CallbackGroup() +{ + trigger_notify_guard_condition(); +} std::atomic_bool & CallbackGroup::can_be_taken_from() @@ -97,6 +111,33 @@ CallbackGroup::automatically_add_to_executor_with_node() const return automatically_add_to_executor_with_node_; } +rclcpp::GuardCondition::SharedPtr +CallbackGroup::get_notify_guard_condition(const rclcpp::Context::SharedPtr context_ptr) +{ + std::lock_guard lock(notify_guard_condition_mutex_); + if (notify_guard_condition_ && context_ptr != notify_guard_condition_->get_context()) { + if (associated_with_executor_) { + trigger_notify_guard_condition(); + } + notify_guard_condition_ = nullptr; + } + + if (!notify_guard_condition_) { + notify_guard_condition_ = std::make_shared(context_ptr); + } + + return notify_guard_condition_; +} + +void +CallbackGroup::trigger_notify_guard_condition() +{ + std::lock_guard lock(notify_guard_condition_mutex_); + if (notify_guard_condition_) { + notify_guard_condition_->trigger(); + } +} + void CallbackGroup::add_subscription( const rclcpp::SubscriptionBase::SharedPtr subscription_ptr) diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 73b7b8d80d..401beb0a73 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -106,11 +106,11 @@ Executor::~Executor() weak_groups_associated_with_executor_to_nodes_.clear(); weak_groups_to_nodes_associated_with_executor_.clear(); weak_groups_to_nodes_.clear(); - for (const auto & pair : weak_nodes_to_guard_conditions_) { + for (const auto & pair : weak_groups_to_guard_conditions_) { auto guard_condition = pair.second; memory_strategy_->remove_guard_condition(guard_condition); } - weak_nodes_to_guard_conditions_.clear(); + weak_groups_to_guard_conditions_.clear(); // Finalize the wait set. if (rcl_wait_set_fini(&wait_set_) != RCL_RET_OK) { @@ -204,8 +204,7 @@ Executor::add_callback_group_to_map( if (has_executor.exchange(true)) { throw std::runtime_error("Callback group has already been added to an executor."); } - bool is_new_node = !has_node(node_ptr, weak_groups_to_nodes_associated_with_executor_) && - !has_node(node_ptr, weak_groups_associated_with_executor_to_nodes_); + rclcpp::CallbackGroup::WeakPtr weak_group_ptr = group_ptr; auto insert_info = weak_groups_to_nodes.insert(std::make_pair(weak_group_ptr, node_ptr)); @@ -215,21 +214,24 @@ 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) { - const auto & gc = node_ptr->get_notify_guard_condition(); - weak_nodes_to_guard_conditions_[node_ptr] = &gc; - if (notify) { - // Interrupt waiting to handle new node - try { - interrupt_guard_condition_.trigger(); - } catch (const rclcpp::exceptions::RCLError & ex) { - throw std::runtime_error( - std::string( - "Failed to trigger guard condition on callback group add: ") + ex.what()); - } + + if (node_ptr->get_context()->is_valid()) { + auto callback_group_guard_condition = + group_ptr->get_notify_guard_condition(node_ptr->get_context()); + weak_groups_to_guard_conditions_[weak_group_ptr] = callback_group_guard_condition.get(); + // Add the callback_group's notify condition to the guard condition handles + memory_strategy_->add_guard_condition(*callback_group_guard_condition); + } + + if (notify) { + // Interrupt waiting to handle new node + try { + interrupt_guard_condition_.trigger(); + } catch (const rclcpp::exceptions::RCLError & ex) { + throw std::runtime_error( + std::string( + "Failed to trigger guard condition on callback group add: ") + ex.what()); } - // Add the node's notify condition to the guard condition handles - memory_strategy_->add_guard_condition(gc); } } @@ -300,7 +302,12 @@ 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_)) { - weak_nodes_to_guard_conditions_.erase(node_ptr); + auto iter = weak_groups_to_guard_conditions_.find(weak_group_ptr); + if (iter != weak_groups_to_guard_conditions_.end()) { + memory_strategy_->remove_guard_condition(iter->second); + } + weak_groups_to_guard_conditions_.erase(weak_group_ptr); + if (notify) { try { interrupt_guard_condition_.trigger(); @@ -310,7 +317,6 @@ Executor::remove_callback_group_from_map( "Failed to trigger guard condition on callback group remove: ") + ex.what()); } } - memory_strategy_->remove_guard_condition(&node_ptr->get_notify_guard_condition()); } } @@ -700,12 +706,6 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) auto weak_node_ptr = pair.second; if (weak_group_ptr.expired() || weak_node_ptr.expired()) { invalid_group_ptrs.push_back(weak_group_ptr); - auto node_guard_pair = weak_nodes_to_guard_conditions_.find(weak_node_ptr); - if (node_guard_pair != weak_nodes_to_guard_conditions_.end()) { - auto guard_condition = node_guard_pair->second; - weak_nodes_to_guard_conditions_.erase(weak_node_ptr); - memory_strategy_->remove_guard_condition(guard_condition); - } } } std::for_each( @@ -721,6 +721,12 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) { weak_groups_associated_with_executor_to_nodes_.erase(group_ptr); } + auto callback_guard_pair = weak_groups_to_guard_conditions_.find(group_ptr); + if (callback_guard_pair != weak_groups_to_guard_conditions_.end()) { + auto guard_condition = callback_guard_pair->second; + weak_groups_to_guard_conditions_.erase(group_ptr); + memory_strategy_->remove_guard_condition(guard_condition); + } weak_groups_to_nodes_.erase(group_ptr); }); } diff --git a/rclcpp/src/rclcpp/node_interfaces/node_services.cpp b/rclcpp/src/rclcpp/node_interfaces/node_services.cpp index 14ab1c82c4..2f1afd3224 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_services.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_services.cpp @@ -35,15 +35,17 @@ NodeServices::add_service( // TODO(jacquelinekay): use custom exception throw std::runtime_error("Cannot create service, group not in node."); } - group->add_service(service_base_ptr); } else { - node_base_->get_default_callback_group()->add_service(service_base_ptr); + group = node_base_->get_default_callback_group(); } + group->add_service(service_base_ptr); + // Notify the executor that a new service was created using the parent Node. auto & node_gc = node_base_->get_notify_guard_condition(); try { node_gc.trigger(); + group->trigger_notify_guard_condition(); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( std::string("failed to notify wait set on service creation: ") + ex.what()); @@ -60,15 +62,17 @@ NodeServices::add_client( // TODO(jacquelinekay): use custom exception throw std::runtime_error("Cannot create client, group not in node."); } - group->add_client(client_base_ptr); } else { - node_base_->get_default_callback_group()->add_client(client_base_ptr); + group = node_base_->get_default_callback_group(); } + group->add_client(client_base_ptr); + // Notify the executor that a new client was created using the parent Node. auto & node_gc = node_base_->get_notify_guard_condition(); try { node_gc.trigger(); + group->trigger_notify_guard_condition(); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( std::string("failed to notify wait set on client creation: ") + ex.what()); diff --git a/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp b/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp index b463e8a0e7..d2e821a9e6 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_timers.cpp @@ -37,14 +37,15 @@ NodeTimers::add_timer( // TODO(jacquelinekay): use custom exception throw std::runtime_error("Cannot create timer, group not in node."); } - callback_group->add_timer(timer); } else { - node_base_->get_default_callback_group()->add_timer(timer); + callback_group = node_base_->get_default_callback_group(); } + callback_group->add_timer(timer); auto & node_gc = node_base_->get_notify_guard_condition(); try { node_gc.trigger(); + callback_group->trigger_notify_guard_condition(); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( std::string("failed to notify wait set on timer creation: ") + ex.what()); diff --git a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp index 159409528d..167a35f35d 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp @@ -73,6 +73,7 @@ NodeTopics::add_publisher( auto & node_gc = node_base_->get_notify_guard_condition(); try { node_gc.trigger(); + callback_group->trigger_notify_guard_condition(); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( std::string("failed to notify wait set on publisher creation: ") + ex.what()); @@ -121,6 +122,7 @@ NodeTopics::add_subscription( auto & node_gc = node_base_->get_notify_guard_condition(); try { node_gc.trigger(); + callback_group->trigger_notify_guard_condition(); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( std::string("failed to notify wait set on subscription creation: ") + ex.what()); diff --git a/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp b/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp index 6f243f6025..1d1fe2ce59 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_waitables.cpp @@ -35,15 +35,17 @@ NodeWaitables::add_waitable( // TODO(jacobperron): use custom exception throw std::runtime_error("Cannot create waitable, group not in node."); } - group->add_waitable(waitable_ptr); } else { - node_base_->get_default_callback_group()->add_waitable(waitable_ptr); + group = node_base_->get_default_callback_group(); } + group->add_waitable(waitable_ptr); + // Notify the executor that a new waitable was created using the parent Node. auto & node_gc = node_base_->get_notify_guard_condition(); try { node_gc.trigger(); + group->trigger_notify_guard_condition(); } catch (const rclcpp::exceptions::RCLError & ex) { throw std::runtime_error( std::string("failed to notify wait set on waitable creation: ") + ex.what()); diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index d4e497f4bf..75aa9ac63b 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -100,15 +100,20 @@ if(TARGET test_create_subscription) "test_msgs" ) endif() -ament_add_gtest(test_add_callback_groups_to_executor - test_add_callback_groups_to_executor.cpp - TIMEOUT 120) -if(TARGET test_add_callback_groups_to_executor) - target_link_libraries(test_add_callback_groups_to_executor ${PROJECT_NAME}) - ament_target_dependencies(test_add_callback_groups_to_executor - "test_msgs" +function(test_add_callback_groups_to_executor_for_rmw_implementation) + set(rmw_implementation_env_var RMW_IMPLEMENTATION=${rmw_implementation}) + ament_add_gmock(test_add_callback_groups_to_executor${target_suffix} test_add_callback_groups_to_executor.cpp + ENV ${rmw_implementation_env_var} + TIMEOUT 120 ) -endif() + if(TARGET test_add_callback_groups_to_executor${target_suffix}) + target_link_libraries(test_add_callback_groups_to_executor${target_suffix} ${PROJECT_NAME}) + ament_target_dependencies(test_add_callback_groups_to_executor${target_suffix} + "test_msgs" + ) + endif() +endfunction() +call_for_each_rmw_implementation(test_add_callback_groups_to_executor_for_rmw_implementation) ament_add_gtest(test_expand_topic_or_service_name test_expand_topic_or_service_name.cpp) if(TARGET test_expand_topic_or_service_name) ament_target_dependencies(test_expand_topic_or_service_name diff --git a/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp b/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp index fa636b7157..07ca1e87d8 100644 --- a/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp +++ b/rclcpp/test/rclcpp/test_add_callback_groups_to_executor.cpp @@ -276,6 +276,70 @@ TYPED_TEST(TestAddCallbackGroupsToExecutor, one_node_many_callback_groups_many_e ASSERT_EQ(timer_executor.get_all_callback_groups().size(), 2u); } +/* + * Test callback groups from one node to many executors. + * A subscriber on a new executor with a callback group not received a message + * because the executor can't be triggered while a subscriber created, see + * https://github.com/ros2/rclcpp/issues/1611 +*/ +TYPED_TEST(TestAddCallbackGroupsToExecutor, subscriber_triggered_to_receive_message) +{ + auto node = std::make_shared("my_node", "/ns"); + + // create a thread running an executor with a new callback group for a coming subscriber + rclcpp::CallbackGroup::SharedPtr cb_grp = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, false); + rclcpp::executors::SingleThreadedExecutor cb_grp_executor; + + std::promise received_message_promise; + auto received_message_future = received_message_promise.get_future(); + rclcpp::FutureReturnCode return_code = rclcpp::FutureReturnCode::TIMEOUT; + std::thread cb_grp_thread = std::thread( + [&cb_grp, &node, &cb_grp_executor, &received_message_future, &return_code]() { + cb_grp_executor.add_callback_group(cb_grp, node->get_node_base_interface()); + return_code = cb_grp_executor.spin_until_future_complete(received_message_future, 10s); + }); + + // expect the subscriber to receive a message + auto sub_callback = [&received_message_promise](test_msgs::msg::Empty::ConstSharedPtr) { + received_message_promise.set_value(true); + }; + + rclcpp::Subscription::SharedPtr subscription; + rclcpp::Publisher::SharedPtr publisher; + // to create a timer with a callback run on another executor + rclcpp::TimerBase::SharedPtr timer = nullptr; + std::promise timer_promise; + auto timer_callback = + [&subscription, &publisher, &timer, &cb_grp, &node, &sub_callback, &timer_promise]() { + if (timer) { + timer.reset(); + } + + // create a subscription using the `cb_grp` callback group + rclcpp::QoS qos = rclcpp::QoS(1).reliable(); + auto options = rclcpp::SubscriptionOptions(); + options.callback_group = cb_grp; + subscription = + node->create_subscription("topic_name", qos, sub_callback, options); + // create a publisher to send data + publisher = + node->create_publisher("topic_name", qos); + publisher->publish(test_msgs::msg::Empty()); + timer_promise.set_value(); + }; + + rclcpp::executors::SingleThreadedExecutor timer_executor; + timer = node->create_wall_timer(100ms, timer_callback); + timer_executor.add_node(node); + auto future = timer_promise.get_future(); + timer_executor.spin_until_future_complete(future); + cb_grp_thread.join(); + + ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); + EXPECT_TRUE(received_message_future.get()); +} + /* * Test removing callback group from executor that its not associated with. */ From e5d13a2478154da5fcbc6b9f2ea1d880902647cf Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Fri, 28 Oct 2022 09:15:14 -0700 Subject: [PATCH 29/53] Bugfix 20210810 get current state (#1756) * protect state_machine_ with mutex lock. protect state_handle_ with mutex lock. reconsider mutex lock scope. remove mutex lock from constructors. lock just once during initialization of LifecycleNodeInterfaceImpl. Signed-off-by: Tomoya Fujita * Move updating of current_state to right after initialization. This is slightly more correct in the case that registering one of the services fails. Signed-off-by: Chris Lalancette Signed-off-by: Tomoya Fujita Signed-off-by: Chris Lalancette Co-authored-by: Chris Lalancette --- .../include/rclcpp_lifecycle/state.hpp | 2 + .../src/lifecycle_node_interface_impl.cpp | 139 +++++++++++------- .../src/lifecycle_node_interface_impl.hpp | 1 + rclcpp_lifecycle/src/state.cpp | 6 + 4 files changed, 95 insertions(+), 53 deletions(-) diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp index 86750f1fed..62e1c51fb3 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/state.hpp @@ -15,6 +15,7 @@ #ifndef RCLCPP_LIFECYCLE__STATE_HPP_ #define RCLCPP_LIFECYCLE__STATE_HPP_ +#include #include #include "rcl_lifecycle/data_types.h" @@ -92,6 +93,7 @@ class State bool owns_rcl_state_handle_; + mutable std::recursive_mutex state_handle_mutex_; rcl_lifecycle_state_t * state_handle_; }; diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp index 30c0ba3c0e..5c5f7797e1 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp @@ -14,6 +14,7 @@ #include #include +#include #include #include #include @@ -58,7 +59,11 @@ LifecycleNode::LifecycleNodeInterfaceImpl::LifecycleNodeInterfaceImpl( LifecycleNode::LifecycleNodeInterfaceImpl::~LifecycleNodeInterfaceImpl() { rcl_node_t * node_handle = node_base_interface_->get_rcl_node_handle(); - auto ret = rcl_lifecycle_state_machine_fini(&state_machine_, node_handle); + rcl_ret_t ret; + { + std::lock_guard lock(state_machine_mutex_); + ret = rcl_lifecycle_state_machine_fini(&state_machine_, node_handle); + } if (ret != RCL_RET_OK) { RCUTILS_LOG_FATAL_NAMED( "rclcpp_lifecycle", @@ -72,7 +77,6 @@ LifecycleNode::LifecycleNodeInterfaceImpl::init(bool enable_communication_interf 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; @@ -83,6 +87,8 @@ LifecycleNode::LifecycleNodeInterfaceImpl::init(bool enable_communication_interf // The publisher takes a C-Typesupport since the publishing (i.e. creating // the message) is done fully in RCL. // Services are handled in C++, so that it needs a C++ typesupport structure. + std::lock_guard lock(state_machine_mutex_); + state_machine_ = rcl_lifecycle_get_zero_initialized_state_machine(); rcl_ret_t ret = rcl_lifecycle_state_machine_init( &state_machine_, node_handle, @@ -99,6 +105,8 @@ LifecycleNode::LifecycleNodeInterfaceImpl::init(bool enable_communication_interf node_base_interface_->get_name()); } + current_state_ = State(state_machine_.current_state); + if (enable_communication_interface) { { // change_state auto cb = std::bind( @@ -182,8 +190,6 @@ LifecycleNode::LifecycleNodeInterfaceImpl::init(bool enable_communication_interf nullptr); } } - - current_state_ = State(state_machine_.current_state); } bool @@ -202,28 +208,31 @@ LifecycleNode::LifecycleNodeInterfaceImpl::on_change_state( std::shared_ptr resp) { (void)header; - if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { - throw std::runtime_error( - "Can't get state. State machine is not initialized."); - } + std::uint8_t transition_id; + { + std::lock_guard lock(state_machine_mutex_); + if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { + throw std::runtime_error("Can't get state. State machine is not initialized."); + } - auto transition_id = req->transition.id; - // if there's a label attached to the request, - // we check the transition attached to this label. - // we further can't compare the id of the looked up transition - // because ros2 service call defaults all intergers to zero. - // that means if we call ros2 service call ... {transition: {label: shutdown}} - // the id of the request is 0 (zero) whereas the id from the lookup up transition - // can be different. - // the result of this is that the label takes presedence of the id. - if (req->transition.label.size() != 0) { - auto rcl_transition = rcl_lifecycle_get_transition_by_label( - state_machine_.current_state, req->transition.label.c_str()); - if (rcl_transition == nullptr) { - resp->success = false; - return; + transition_id = req->transition.id; + // if there's a label attached to the request, + // we check the transition attached to this label. + // we further can't compare the id of the looked up transition + // because ros2 service call defaults all intergers to zero. + // that means if we call ros2 service call ... {transition: {label: shutdown}} + // the id of the request is 0 (zero) whereas the id from the lookup up transition + // can be different. + // the result of this is that the label takes presedence of the id. + if (req->transition.label.size() != 0) { + auto rcl_transition = rcl_lifecycle_get_transition_by_label( + state_machine_.current_state, req->transition.label.c_str()); + if (rcl_transition == nullptr) { + resp->success = false; + return; + } + transition_id = static_cast(rcl_transition->id); } - transition_id = static_cast(rcl_transition->id); } node_interfaces::LifecycleNodeInterface::CallbackReturn cb_return_code; @@ -244,6 +253,7 @@ LifecycleNode::LifecycleNodeInterfaceImpl::on_get_state( { (void)header; (void)req; + std::lock_guard lock(state_machine_mutex_); if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { throw std::runtime_error( "Can't get state. State machine is not initialized."); @@ -260,6 +270,7 @@ LifecycleNode::LifecycleNodeInterfaceImpl::on_get_available_states( { (void)header; (void)req; + std::lock_guard lock(state_machine_mutex_); if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { throw std::runtime_error( "Can't get available states. State machine is not initialized."); @@ -282,6 +293,7 @@ LifecycleNode::LifecycleNodeInterfaceImpl::on_get_available_transitions( { (void)header; (void)req; + std::lock_guard lock(state_machine_mutex_); if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { throw std::runtime_error( "Can't get available transitions. State machine is not initialized."); @@ -309,6 +321,7 @@ LifecycleNode::LifecycleNodeInterfaceImpl::on_get_transition_graph( { (void)header; (void)req; + std::lock_guard lock(state_machine_mutex_); if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { throw std::runtime_error( "Can't get available transitions. State machine is not initialized."); @@ -338,6 +351,7 @@ std::vector LifecycleNode::LifecycleNodeInterfaceImpl::get_available_states() const { std::vector states; + std::lock_guard lock(state_machine_mutex_); states.reserve(state_machine_.transition_map.states_size); for (unsigned int i = 0; i < state_machine_.transition_map.states_size; ++i) { @@ -350,6 +364,7 @@ std::vector LifecycleNode::LifecycleNodeInterfaceImpl::get_available_transitions() const { std::vector transitions; + std::lock_guard lock(state_machine_mutex_); transitions.reserve(state_machine_.current_state->valid_transition_size); for (unsigned int i = 0; i < state_machine_.current_state->valid_transition_size; ++i) { @@ -362,6 +377,7 @@ std::vector LifecycleNode::LifecycleNodeInterfaceImpl::get_transition_graph() const { std::vector transitions; + std::lock_guard lock(state_machine_mutex_); transitions.reserve(state_machine_.transition_map.transitions_size); for (unsigned int i = 0; i < state_machine_.transition_map.transitions_size; ++i) { @@ -375,26 +391,33 @@ LifecycleNode::LifecycleNodeInterfaceImpl::change_state( std::uint8_t transition_id, node_interfaces::LifecycleNodeInterface::CallbackReturn & cb_return_code) { - if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { - RCUTILS_LOG_ERROR( - "Unable to change state for state machine for %s: %s", - node_base_interface_->get_name(), rcl_get_error_string().str); - return RCL_RET_ERROR; - } - constexpr bool publish_update = true; - // keep the initial state to pass to a transition callback - State initial_state(state_machine_.current_state); + State initial_state; + unsigned int current_state_id; - if ( - rcl_lifecycle_trigger_transition_by_id( - &state_machine_, transition_id, publish_update) != RCL_RET_OK) { - RCUTILS_LOG_ERROR( - "Unable to start transition %u from current state %s: %s", - transition_id, state_machine_.current_state->label, rcl_get_error_string().str); - rcutils_reset_error(); - return RCL_RET_ERROR; + std::lock_guard lock(state_machine_mutex_); + if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { + RCUTILS_LOG_ERROR( + "Unable to change state for state machine for %s: %s", + node_base_interface_->get_name(), rcl_get_error_string().str); + return RCL_RET_ERROR; + } + + // keep the initial state to pass to a transition callback + initial_state = State(state_machine_.current_state); + + if ( + rcl_lifecycle_trigger_transition_by_id( + &state_machine_, transition_id, publish_update) != RCL_RET_OK) + { + RCUTILS_LOG_ERROR( + "Unable to start transition %u from current state %s: %s", + transition_id, state_machine_.current_state->label, rcl_get_error_string().str); + rcutils_reset_error(); + return RCL_RET_ERROR; + } + current_state_id = state_machine_.current_state->id; } // Update the internal current_state_ @@ -411,18 +434,22 @@ LifecycleNode::LifecycleNodeInterfaceImpl::change_state( return rcl_lifecycle_transition_error_label; }; - cb_return_code = execute_callback(state_machine_.current_state->id, initial_state); + cb_return_code = execute_callback(current_state_id, initial_state); auto transition_label = get_label_for_return_code(cb_return_code); - if ( - rcl_lifecycle_trigger_transition_by_label( - &state_machine_, transition_label, publish_update) != RCL_RET_OK) { - RCUTILS_LOG_ERROR( - "Failed to finish transition %u. Current state is now: %s (%s)", - transition_id, state_machine_.current_state->label, rcl_get_error_string().str); - rcutils_reset_error(); - return RCL_RET_ERROR; + std::lock_guard lock(state_machine_mutex_); + if ( + rcl_lifecycle_trigger_transition_by_label( + &state_machine_, transition_label, publish_update) != RCL_RET_OK) + { + RCUTILS_LOG_ERROR( + "Failed to finish transition %u. Current state is now: %s (%s)", + transition_id, state_machine_.current_state->label, rcl_get_error_string().str); + rcutils_reset_error(); + return RCL_RET_ERROR; + } + current_state_id = state_machine_.current_state->id; } // Update the internal current_state_ @@ -433,8 +460,9 @@ LifecycleNode::LifecycleNodeInterfaceImpl::change_state( if (cb_return_code == node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR) { RCUTILS_LOG_WARN("Error occurred while doing error handling."); - auto error_cb_code = execute_callback(state_machine_.current_state->id, initial_state); + auto error_cb_code = execute_callback(current_state_id, initial_state); auto error_cb_label = get_label_for_return_code(error_cb_code); + std::lock_guard lock(state_machine_mutex_); if ( rcl_lifecycle_trigger_transition_by_label( &state_machine_, error_cb_label, publish_update) != RCL_RET_OK) @@ -486,8 +514,13 @@ const State & LifecycleNode::LifecycleNodeInterfaceImpl::trigger_transition( const char * transition_label, node_interfaces::LifecycleNodeInterface::CallbackReturn & cb_return_code) { - auto transition = - rcl_lifecycle_get_transition_by_label(state_machine_.current_state, transition_label); + const rcl_lifecycle_transition_t * transition; + { + std::lock_guard lock(state_machine_mutex_); + + transition = + rcl_lifecycle_get_transition_by_label(state_machine_.current_state, transition_label); + } if (transition) { change_state(static_cast(transition->id), cb_return_code); } diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp index 9777bbf3a8..d09f44831c 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp @@ -143,6 +143,7 @@ class LifecycleNode::LifecycleNodeInterfaceImpl final node_interfaces::LifecycleNodeInterface::CallbackReturn execute_callback(unsigned int cb_id, const State & previous_state) const; + mutable std::recursive_mutex state_machine_mutex_; rcl_lifecycle_state_machine_t state_machine_; State current_state_; std::map< diff --git a/rclcpp_lifecycle/src/state.cpp b/rclcpp_lifecycle/src/state.cpp index 959b40ed2a..e69078dfba 100644 --- a/rclcpp_lifecycle/src/state.cpp +++ b/rclcpp_lifecycle/src/state.cpp @@ -71,6 +71,7 @@ State::State( if (!rcl_lifecycle_state_handle) { throw std::runtime_error("rcl_lifecycle_state_handle is null"); } + state_handle_ = const_cast(rcl_lifecycle_state_handle); } @@ -94,6 +95,8 @@ State::operator=(const State & rhs) return *this; } + // hold the lock until state_handle_ is reconstructed + std::lock_guard lock(state_handle_mutex_); // reset all currently used resources reset(); @@ -129,6 +132,7 @@ State::operator=(const State & rhs) uint8_t State::id() const { + std::lock_guard lock(state_handle_mutex_); if (!state_handle_) { throw std::runtime_error("Error in state! Internal state_handle is NULL."); } @@ -138,6 +142,7 @@ State::id() const std::string State::label() const { + std::lock_guard lock(state_handle_mutex_); if (!state_handle_) { throw std::runtime_error("Error in state! Internal state_handle is NULL."); } @@ -147,6 +152,7 @@ State::label() const void State::reset() noexcept { + std::lock_guard lock(state_handle_mutex_); if (!owns_rcl_state_handle_) { state_handle_ = nullptr; } From 6f22443513e14eac41f5bb830a566bf820a8e078 Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Fri, 28 Oct 2022 09:17:23 -0700 Subject: [PATCH 30/53] MultiThreadExecutor number of threads is at least 2+ in default. (#2032) Signed-off-by: Tomoya Fujita --- rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp index c1de8051f2..119013ebfb 100644 --- a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp @@ -47,7 +47,7 @@ class MultiThreadedExecutor : public rclcpp::Executor * * \param options common options for all executors * \param number_of_threads number of threads to have in the thread pool, - * the default 0 will use the number of cpu cores found instead + * the default 0 will use the number of cpu cores found (minimum of 2) * \param yield_before_execute if true std::this_thread::yield() is called * \param timeout maximum time to wait */ From edbfe1404b24d0bc85ff88e8ff1f006670788e46 Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Fri, 28 Oct 2022 09:17:47 -0700 Subject: [PATCH 31/53] LifecycleNode on_configure doc fix. (#2034) Signed-off-by: Tomoya Fujita --- .../node_interfaces/lifecycle_node_interface.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp index e653ad412d..ed919ca556 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp @@ -53,7 +53,7 @@ class LifecycleNodeInterface /// Callback function for configure transition /* - * \return true by default + * \return SUCCESS by default */ RCLCPP_LIFECYCLE_PUBLIC virtual CallbackReturn From 586932ebf8f844a8484f91bc618f3e9fdd72dbc1 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 2 Nov 2022 18:06:12 +0000 Subject: [PATCH 32/53] Changelog. Signed-off-by: Chris Lalancette --- rclcpp/CHANGELOG.rst | 12 ++++++++++++ rclcpp_action/CHANGELOG.rst | 5 +++++ rclcpp_components/CHANGELOG.rst | 5 +++++ rclcpp_lifecycle/CHANGELOG.rst | 9 +++++++++ 4 files changed, 31 insertions(+) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 08914deb08..fc620249fe 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,18 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* MultiThreadExecutor number of threads is at least 2+ in default. (`#2032 `_) +* Fix bug that a callback not reached (`#1640 `_) +* Set the minimum number of threads of the Multithreaded executor to 2 (`#2030 `_) +* check thread whether joinable before join (`#2019 `_) +* Set cpplint test timeout to 3 minutes (`#2022 `_) +* Make sure to include-what-you-use in the node_interfaces. (`#2018 `_) +* Do not clear entities callbacks on destruction (`#2002 `_) +* fix mismatched issue if using zero_allocate (`#1995 `_) +* Contributors: Alexis Paques, Chen Lihui, Chris Lalancette, Cristóbal Arroyo, Tomoya Fujita, mauropasse, uupks + 17.0.0 (2022-09-13) ------------------- * Make ParameterService and Sync/AsyncParameterClient accept rclcpp::QoS (`#1978 `_) diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index ef4104bcac..0ab833bc3a 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,11 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Do not clear entities callbacks on destruction (`#2002 `_) +* Contributors: mauropasse + 17.0.0 (2022-09-13) ------------------- * Revert "Introduce executors new spin_for method, replace spin_until_future_complete with spin_until_complete. (`#1821 `_) (`#1874 `_)" (`#1956 `_) diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index c496eb406d..6fa85fbcba 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* use unique ptr and remove unuseful container (`#2013 `_) +* Contributors: Chen Lihui + 17.0.0 (2022-09-13) ------------------- * Revert "Introduce executors new spin_for method, replace spin_until_future_complete with spin_until_complete. (`#1821 `_) (`#1874 `_)" (`#1956 `_) diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 8b88b682b1..f062dd56a9 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,15 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* LifecycleNode on_configure doc fix. (`#2034 `_) +* Bugfix 20210810 get current state (`#1756 `_) +* Make lifecycle impl get_current_state() const. (`#2031 `_) +* Cleanup the lifecycle implementation (`#2027 `_) +* Cleanup the rclcpp_lifecycle dependencies. (`#2021 `_) +* Contributors: Chris Lalancette, Tomoya Fujita + 17.0.0 (2022-09-13) ------------------- * Revert "Revert "Add a create_timer method to Node and `LifecycleNode` classes (`#1975 `_)" (`#2009 `_) (`#2010 `_) From 7c6785176a878cf16eb98995b732f405b9e776ed Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 2 Nov 2022 18:06:21 +0000 Subject: [PATCH 33/53] 17.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 fc620249fe..15a1aede88 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +17.1.0 (2022-11-02) +------------------- * MultiThreadExecutor number of threads is at least 2+ in default. (`#2032 `_) * Fix bug that a callback not reached (`#1640 `_) * Set the minimum number of threads of the Multithreaded executor to 2 (`#2030 `_) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index d05253db94..f113613fd3 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 17.0.0 + 17.1.0 The ROS client library in C++. Ivan Paunovic Jacob Perron diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index 0ab833bc3a..a063bf32f2 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +17.1.0 (2022-11-02) +------------------- * Do not clear entities callbacks on destruction (`#2002 `_) * Contributors: mauropasse diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index f9cbaeb721..758eb0db4e 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 17.0.0 + 17.1.0 Adds action APIs for C++. Ivan Paunovic Jacob Perron diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index 6fa85fbcba..bc08ed28cd 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +17.1.0 (2022-11-02) +------------------- * use unique ptr and remove unuseful container (`#2013 `_) * Contributors: Chen Lihui diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index 2032fc84ee..3557608bc1 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 17.0.0 + 17.1.0 Package containing tools for dynamically loadable components Ivan Paunovic Jacob Perron diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index f062dd56a9..03d02cb2c2 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,8 +3,8 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +17.1.0 (2022-11-02) +------------------- * LifecycleNode on_configure doc fix. (`#2034 `_) * Bugfix 20210810 get current state (`#1756 `_) * Make lifecycle impl get_current_state() const. (`#2031 `_) diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index cfd97eafd6..ab94c9fffb 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 17.0.0 + 17.1.0 Package containing a prototype for lifecycle implementation Ivan Paunovic Jacob Perron From 82f1fbff0b29fb13bf93b54fc0fdc996c800f6bc Mon Sep 17 00:00:00 2001 From: Audrow Nash Date: Tue, 8 Nov 2022 07:21:43 -0600 Subject: [PATCH 34/53] [rolling] Update maintainers (#2043) * Update maintainers to Ivan Paunovic, Michel Hidalgo, and William Woodall Signed-off-by: Audrow Nash --- CODEOWNERS | 2 ++ rclcpp/package.xml | 6 +++++- rclcpp_action/package.xml | 6 +++++- rclcpp_components/package.xml | 6 +++++- rclcpp_lifecycle/package.xml | 6 +++++- 5 files changed, 22 insertions(+), 4 deletions(-) create mode 100644 CODEOWNERS diff --git a/CODEOWNERS b/CODEOWNERS new file mode 100644 index 0000000000..7ee4b9af4d --- /dev/null +++ b/CODEOWNERS @@ -0,0 +1,2 @@ +# This file was generated by https://github.com/audrow/update-ros2-repos +* @ivanpauno @hidmic @wjwwood diff --git a/rclcpp/package.xml b/rclcpp/package.xml index f113613fd3..6775f4a46d 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -4,11 +4,15 @@ rclcpp 17.1.0 The ROS client library in C++. + Ivan Paunovic - Jacob Perron + Michel Hidalgo William Woodall + Apache License 2.0 + Dirk Thomas + Jacob Perron ament_cmake_ros ament_cmake_gen_version_h diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index 758eb0db4e..38e7fdcba1 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -4,11 +4,15 @@ rclcpp_action 17.1.0 Adds action APIs for C++. + Ivan Paunovic - Jacob Perron + Michel Hidalgo William Woodall + Apache License 2.0 + Dirk Thomas + Jacob Perron ament_cmake_ros diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index 3557608bc1..cfb88b9e3f 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -4,10 +4,14 @@ rclcpp_components 17.1.0 Package containing tools for dynamically loadable components + Ivan Paunovic - Jacob Perron + Michel Hidalgo William Woodall + Apache License 2.0 + + Jacob Perron Michael Carroll ament_cmake_ros diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index ab94c9fffb..8fa958a0bc 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -4,10 +4,14 @@ rclcpp_lifecycle 17.1.0 Package containing a prototype for lifecycle implementation + Ivan Paunovic - Jacob Perron + Michel Hidalgo William Woodall + Apache License 2.0 + + Jacob Perron Karsten Knese ament_cmake_ros From 91bc312190e61c8915b0152e5c927c987fc22d6c Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Wed, 9 Nov 2022 23:50:01 +0800 Subject: [PATCH 35/53] fix a case that not throw ParameterUninitializedException (#2036) * fix a case that not throw ParameterUninitializedException Signed-off-by: Chen Lihui * catch ParameterUninitializedException exception while calling get_parameters in service callback Signed-off-by: Chen Lihui * update doc Signed-off-by: Chen Lihui * add one more test Signed-off-by: Chen Lihui * explicitly use this to call a method inside the class itself Co-authored-by: Ivan Santiago Paunovic Signed-off-by: Chen Lihui Signed-off-by: Chen Lihui Co-authored-by: Ivan Santiago Paunovic --- rclcpp/include/rclcpp/node.hpp | 16 ++++++--- .../node_interfaces/node_parameters.cpp | 34 +++++++------------ rclcpp/src/rclcpp/parameter_service.cpp | 2 ++ rclcpp/test/rclcpp/test_node.cpp | 34 +++++++++++++++++++ rclcpp/test/rclcpp/test_parameter_client.cpp | 18 ++++++++++ 5 files changed, 77 insertions(+), 27 deletions(-) diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index e514137b51..7ecb67e9b1 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -740,17 +740,21 @@ class Node : public std::enable_shared_from_this /** * If the parameter has not been declared, then this method may throw the * rclcpp::exceptions::ParameterNotDeclaredException exception. + * If the parameter has not been initialized, then this method may throw the + * rclcpp::exceptions::ParameterUninitializedException exception. * * If undeclared parameters are allowed, see the node option * rclcpp::NodeOptions::allow_undeclared_parameters, then this method will - * not throw an exception, and instead return a default initialized - * rclcpp::Parameter, which has a type of + * not throw the rclcpp::exceptions::ParameterNotDeclaredException exception, + * and instead return a default initialized rclcpp::Parameter, which has a type of * rclcpp::ParameterType::PARAMETER_NOT_SET. * * \param[in] name The name of the parameter to get. * \return The requested parameter inside of a rclcpp parameter object. * \throws rclcpp::exceptions::ParameterNotDeclaredException if the parameter * has not been declared and undeclared parameters are not allowed. + * \throws rclcpp::exceptions::ParameterUninitializedException if the parameter + * has not been initialized. */ RCLCPP_PUBLIC rclcpp::Parameter @@ -834,12 +838,12 @@ class Node : public std::enable_shared_from_this /// Return the parameters by the given parameter names. /** - * Like get_parameters(), this method may throw the + * Like get_parameter(const std::string &), this method may throw the * rclcpp::exceptions::ParameterNotDeclaredException exception if the * requested parameter has not been declared and undeclared parameters are - * not allowed. + * not allowed, and may throw the rclcpp::exceptions::ParameterUninitializedException exception. * - * Also like get_parameters(), if undeclared parameters are allowed and the + * Also like get_parameter(const std::string &), if undeclared parameters are allowed and the * parameter has not been declared, then the corresponding rclcpp::Parameter * will be default initialized and therefore have the type * rclcpp::ParameterType::PARAMETER_NOT_SET. @@ -849,6 +853,8 @@ class Node : public std::enable_shared_from_this * \throws rclcpp::exceptions::ParameterNotDeclaredException if any of the * parameters have not been declared and undeclared parameters are not * allowed. + * \throws rclcpp::exceptions::ParameterUninitializedException if any of the + * parameters have not been initialized. */ RCLCPP_PUBLIC std::vector diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index c17eb887b7..9dafcba381 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -904,22 +904,12 @@ NodeParameters::set_parameters_atomically(const std::vector & std::vector NodeParameters::get_parameters(const std::vector & names) const { - std::lock_guard lock(mutex_); std::vector results; results.reserve(names.size()); + std::lock_guard lock(mutex_); for (auto & name : names) { - auto found_parameter = parameters_.find(name); - if (found_parameter != parameters_.cend()) { - // found - results.emplace_back(name, found_parameter->second.value); - } else if (this->allow_undeclared_) { - // not found, but undeclared allowed - results.emplace_back(name, rclcpp::ParameterValue()); - } else { - // not found, and undeclared are not allowed - throw rclcpp::exceptions::ParameterNotDeclaredException(name); - } + results.emplace_back(this->get_parameter(name)); } return results; } @@ -930,18 +920,18 @@ NodeParameters::get_parameter(const std::string & name) const std::lock_guard lock(mutex_); auto param_iter = parameters_.find(name); - if ( - parameters_.end() != param_iter && - (param_iter->second.value.get_type() != rclcpp::ParameterType::PARAMETER_NOT_SET || - param_iter->second.descriptor.dynamic_typing)) - { - return rclcpp::Parameter{name, param_iter->second.value}; + if (parameters_.end() != param_iter) { + if ( + param_iter->second.value.get_type() != rclcpp::ParameterType::PARAMETER_NOT_SET || + param_iter->second.descriptor.dynamic_typing) + { + return rclcpp::Parameter{name, param_iter->second.value}; + } + throw rclcpp::exceptions::ParameterUninitializedException(name); } else if (this->allow_undeclared_) { - return rclcpp::Parameter{}; - } else if (parameters_.end() == param_iter) { - throw rclcpp::exceptions::ParameterNotDeclaredException(name); + return rclcpp::Parameter{name}; } else { - throw rclcpp::exceptions::ParameterUninitializedException(name); + throw rclcpp::exceptions::ParameterNotDeclaredException(name); } } diff --git a/rclcpp/src/rclcpp/parameter_service.cpp b/rclcpp/src/rclcpp/parameter_service.cpp index 501ac399f8..0923798339 100644 --- a/rclcpp/src/rclcpp/parameter_service.cpp +++ b/rclcpp/src/rclcpp/parameter_service.cpp @@ -48,6 +48,8 @@ ParameterService::ParameterService( } } catch (const rclcpp::exceptions::ParameterNotDeclaredException & ex) { RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Failed to get parameters: %s", ex.what()); + } catch (const rclcpp::exceptions::ParameterUninitializedException & ex) { + RCLCPP_DEBUG(rclcpp::get_logger("rclcpp"), "Failed to get parameters: %s", ex.what()); } }, qos_profile, nullptr); diff --git a/rclcpp/test/rclcpp/test_node.cpp b/rclcpp/test/rclcpp/test_node.cpp index c40811a4e4..19c87b3684 100644 --- a/rclcpp/test/rclcpp/test_node.cpp +++ b/rclcpp/test/rclcpp/test_node.cpp @@ -3346,6 +3346,9 @@ TEST_F(TestNode, static_and_dynamic_typing) { EXPECT_THROW( node->get_parameter("integer_override_not_given"), rclcpp::exceptions::ParameterUninitializedException); + EXPECT_THROW( + node->get_parameters({"integer_override_not_given"}), + rclcpp::exceptions::ParameterUninitializedException); } { auto param = node->declare_parameter("integer_set_after_declare", rclcpp::PARAMETER_INTEGER); @@ -3367,3 +3370,34 @@ TEST_F(TestNode, static_and_dynamic_typing) { rclcpp::exceptions::InvalidParameterTypeException); } } + +TEST_F(TestNode, parameter_uninitialized_exception_even_if_allow_undeclared) { + rclcpp::NodeOptions no; + no.allow_undeclared_parameters(true); + auto node = std::make_shared("node", "ns", no); + { + const std::string param_name = "integer_override_not_given"; + auto param_value = node->declare_parameter(param_name, rclcpp::PARAMETER_INTEGER); + EXPECT_EQ(rclcpp::PARAMETER_NOT_SET, param_value.get_type()); + // Throws if not set before access + EXPECT_THROW( + node->get_parameter(param_name), + rclcpp::exceptions::ParameterUninitializedException); + EXPECT_THROW( + node->get_parameters({param_name}), + rclcpp::exceptions::ParameterUninitializedException); + } +} + +TEST_F(TestNode, get_parameter_with_node_allow_undeclared) { + rclcpp::NodeOptions no; + no.allow_undeclared_parameters(true); + auto node = std::make_shared("node", "ns", no); + { + const std::string param_name = "allow_undeclared_param"; + auto param = node->get_parameter(param_name); + EXPECT_EQ(param_name, param.get_name()); + EXPECT_EQ(rclcpp::PARAMETER_NOT_SET, param.get_type()); + EXPECT_EQ(rclcpp::ParameterValue{}, param.get_parameter_value()); + } +} diff --git a/rclcpp/test/rclcpp/test_parameter_client.cpp b/rclcpp/test/rclcpp/test_parameter_client.cpp index 64ef2d903b..2ce414d327 100644 --- a/rclcpp/test/rclcpp/test_parameter_client.cpp +++ b/rclcpp/test/rclcpp/test_parameter_client.cpp @@ -440,6 +440,7 @@ TEST_F(TestParameterClient, sync_parameter_get_parameter_types_allow_undeclared) TEST_F(TestParameterClient, sync_parameter_get_parameters) { node->declare_parameter("foo", 4); node->declare_parameter("bar", "this is bar"); + node->declare_parameter("baz", rclcpp::PARAMETER_INTEGER); auto synchronous_client = std::make_shared(node); { @@ -448,6 +449,14 @@ TEST_F(TestParameterClient, sync_parameter_get_parameters) { ASSERT_EQ(0u, parameters.size()); } + { + // not throw ParameterUninitializedException while getting parameter from service + // even if the parameter is not initialized in the node + std::vector names{"baz"}; + std::vector parameters = synchronous_client->get_parameters(names, 10s); + ASSERT_EQ(0u, parameters.size()); + } + { std::vector names{"none", "foo", "bar"}; std::vector parameters = synchronous_client->get_parameters(names, 10s); @@ -487,6 +496,7 @@ TEST_F(TestParameterClient, sync_parameter_get_parameters) { TEST_F(TestParameterClient, sync_parameter_get_parameters_allow_undeclared) { node_with_option->declare_parameter("foo", 4); node_with_option->declare_parameter("bar", "this is bar"); + node_with_option->declare_parameter("baz", rclcpp::PARAMETER_INTEGER); auto synchronous_client = std::make_shared(node_with_option); { @@ -495,6 +505,14 @@ TEST_F(TestParameterClient, sync_parameter_get_parameters_allow_undeclared) { ASSERT_EQ(1u, parameters.size()); } + { + // not throw ParameterUninitializedException while getting parameter from service + // even if the parameter is not initialized in the node + std::vector names{"baz"}; + std::vector parameters = synchronous_client->get_parameters(names, 10s); + ASSERT_EQ(0u, parameters.size()); + } + { std::vector names{"none", "foo", "bar"}; std::vector parameters = synchronous_client->get_parameters(names, 10s); From e5d20474da86f2835e2704d2e3bd78ada5a37d86 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 23 Nov 2022 08:44:34 -0800 Subject: [PATCH 36/53] Mark rclcpp::Clock::now() as const (#2050) Signed-off-by: Shane Loretz Signed-off-by: Shane Loretz --- rclcpp/include/rclcpp/clock.hpp | 2 +- rclcpp/src/rclcpp/clock.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/rclcpp/include/rclcpp/clock.hpp b/rclcpp/include/rclcpp/clock.hpp index 42d3e01e7e..1e08e1d8fd 100644 --- a/rclcpp/include/rclcpp/clock.hpp +++ b/rclcpp/include/rclcpp/clock.hpp @@ -77,7 +77,7 @@ class Clock */ RCLCPP_PUBLIC Time - now(); + now() const; /** * Sleep until a specified Time, according to clock type. diff --git a/rclcpp/src/rclcpp/clock.cpp b/rclcpp/src/rclcpp/clock.cpp index 66c8db70e1..b34f328690 100644 --- a/rclcpp/src/rclcpp/clock.cpp +++ b/rclcpp/src/rclcpp/clock.cpp @@ -67,7 +67,7 @@ Clock::Clock(rcl_clock_type_t clock_type) Clock::~Clock() {} Time -Clock::now() +Clock::now() const { Time now(0, 0, impl_->rcl_clock_.type); From a00ef22d6d3a5b3ce67c60459642665e2fdcd914 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 30 Nov 2022 08:31:53 -0500 Subject: [PATCH 37/53] Add in a warning for a KeepLast depth of 0. (#2048) * Add in a warning for a KeepLast depth of 0. It really doesn't make much sense to have a KeepLast depth of 0; no data could possibly be stored. Indeed, the underlying DDS implementations don't actually support this. It currently "works" because a KeepLast depth of 0 is assumed to be system default, so the RMW layer typically chooses "1" instead. But this isn't something we should be encouraging users to do, so add a warning. Signed-off-by: Chris Lalancette --- rclcpp/src/rclcpp/qos.cpp | 29 +++++++++++++++++++++- rclcpp/test/rclcpp/test_qos_parameters.cpp | 6 +++++ 2 files changed, 34 insertions(+), 1 deletion(-) diff --git a/rclcpp/src/rclcpp/qos.cpp b/rclcpp/src/rclcpp/qos.cpp index 70b9976db6..2d0277bfb5 100644 --- a/rclcpp/src/rclcpp/qos.cpp +++ b/rclcpp/src/rclcpp/qos.cpp @@ -16,6 +16,8 @@ #include +#include "rclcpp/logging.hpp" + #include "rmw/error_handling.h" #include "rmw/types.h" #include "rmw/qos_profiles.h" @@ -67,13 +69,30 @@ KeepAll::KeepAll() KeepLast::KeepLast(size_t depth) : QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_LAST, depth) -{} +{ + if (depth == 0) { + RCLCPP_WARN_ONCE( + rclcpp::get_logger( + "rclcpp"), + "A zero depth with KEEP_LAST doesn't make sense; no data could be stored." + "This will be interpreted as SYSTEM_DEFAULT"); + } +} QoS::QoS( const QoSInitialization & qos_initialization, const rmw_qos_profile_t & initial_profile) : rmw_qos_profile_(initial_profile) { + if (qos_initialization.history_policy == RMW_QOS_POLICY_HISTORY_KEEP_LAST && + qos_initialization.depth == 0) + { + RCLCPP_WARN_ONCE( + rclcpp::get_logger( + "rclcpp"), + "A zero depth with KEEP_LAST doesn't make sense; no data could be stored." + "This will be interpreted as SYSTEM_DEFAULT"); + } rmw_qos_profile_.history = qos_initialization.history_policy; rmw_qos_profile_.depth = qos_initialization.depth; } @@ -111,6 +130,14 @@ QoS::history(HistoryPolicy history) QoS & QoS::keep_last(size_t depth) { + if (depth == 0) { + RCLCPP_WARN_ONCE( + rclcpp::get_logger( + "rclcpp"), + "A zero depth with KEEP_LAST doesn't make sense; no data could be stored." + "This will be interpreted as SYSTEM_DEFAULT"); + } + rmw_qos_profile_.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; rmw_qos_profile_.depth = depth; return *this; diff --git a/rclcpp/test/rclcpp/test_qos_parameters.cpp b/rclcpp/test/rclcpp/test_qos_parameters.cpp index 97d524d176..be9a12cb5e 100644 --- a/rclcpp/test/rclcpp/test_qos_parameters.cpp +++ b/rclcpp/test/rclcpp/test_qos_parameters.cpp @@ -255,3 +255,9 @@ TEST(TestQosParameters, internal_functions_failure_modes) { nullptr, rclcpp::QosPolicyKind::Reliability), std::invalid_argument); } + +TEST(TestQosParameters, keep_last_zero) { + rclcpp::KeepLast keep_last(0); + + EXPECT_EQ(keep_last.depth, RMW_QOS_POLICY_DEPTH_SYSTEM_DEFAULT); +} From 66b19448b0520b15a9e6c28483863b2a4351c2f6 Mon Sep 17 00:00:00 2001 From: Lei Liu <64953129+llapx@users.noreply.github.com> Date: Fri, 2 Dec 2022 10:17:43 +0800 Subject: [PATCH 38/53] Fix SharedFuture from async_send_request never becomes valid (#2044) Signed-off-by: Lei Liu --- rclcpp/include/rclcpp/client.hpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index 04e0ce9a33..b10e3e1593 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -792,16 +792,14 @@ class Client : public ClientBase async_send_request_impl(const Request & request, CallbackInfoVariant value) { int64_t sequence_number; + std::lock_guard lock(pending_requests_mutex_); rcl_ret_t ret = rcl_send_request(get_client_handle().get(), &request, &sequence_number); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send request"); } - { - std::lock_guard lock(pending_requests_mutex_); - pending_requests_.try_emplace( - sequence_number, - std::make_pair(std::chrono::system_clock::now(), std::move(value))); - } + pending_requests_.try_emplace( + sequence_number, + std::make_pair(std::chrono::system_clock::now(), std::move(value))); return sequence_number; } From 338eed0c060ab58608fc6e44c6936bde2c4e061e Mon Sep 17 00:00:00 2001 From: methylDragon Date: Tue, 6 Dec 2022 11:19:40 -0800 Subject: [PATCH 39/53] Remove templating on to_rcl_subscription_options (#2056) --- rclcpp/include/rclcpp/generic_subscription.hpp | 2 +- rclcpp/include/rclcpp/subscription.hpp | 2 +- rclcpp/include/rclcpp/subscription_options.hpp | 1 - rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp | 4 ++-- 4 files changed, 4 insertions(+), 5 deletions(-) diff --git a/rclcpp/include/rclcpp/generic_subscription.hpp b/rclcpp/include/rclcpp/generic_subscription.hpp index 673712eedb..16c8fe6fbf 100644 --- a/rclcpp/include/rclcpp/generic_subscription.hpp +++ b/rclcpp/include/rclcpp/generic_subscription.hpp @@ -81,7 +81,7 @@ class GenericSubscription : public rclcpp::SubscriptionBase node_base, *rclcpp::get_typesupport_handle(topic_type, "rosidl_typesupport_cpp", *ts_lib), topic_name, - options.template to_rcl_subscription_options(qos), + options.to_rcl_subscription_options(qos), true), callback_(callback), ts_lib_(ts_lib) diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 11bf9c6e43..74e6b5c05d 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -140,7 +140,7 @@ class Subscription : public SubscriptionBase node_base, type_support_handle, topic_name, - options.template to_rcl_subscription_options(qos), + options.to_rcl_subscription_options(qos), callback.is_serialized_message_callback()), any_callback_(callback), options_(options), diff --git a/rclcpp/include/rclcpp/subscription_options.hpp b/rclcpp/include/rclcpp/subscription_options.hpp index 2b819da399..c5c3e21eb1 100644 --- a/rclcpp/include/rclcpp/subscription_options.hpp +++ b/rclcpp/include/rclcpp/subscription_options.hpp @@ -110,7 +110,6 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase * \param qos QoS profile for subcription. * \return rcl_subscription_options_t structure based on the rclcpp::QoS */ - template rcl_subscription_options_t to_rcl_subscription_options(const rclcpp::QoS & qos) const { diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp index a99633bec6..264577a739 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp @@ -44,8 +44,8 @@ const rcl_publisher_options_t PublisherOptions() const rcl_subscription_options_t SubscriptionOptions() { - return rclcpp::SubscriptionOptionsWithAllocator>().template - to_rcl_subscription_options(rclcpp::QoS(10)); + return rclcpp::SubscriptionOptionsWithAllocator>() + .to_rcl_subscription_options(rclcpp::QoS(10)); } } // namespace From 1ac37b692c4cce54f0ffeaad1f4fe3d5688322bd Mon Sep 17 00:00:00 2001 From: andrei Date: Tue, 13 Dec 2022 22:31:14 +0900 Subject: [PATCH 40/53] fix nullptr dereference in prune_requests_older_than (#2008) * fix nullptr dereference in prune_requests_older_than Signed-off-by: akela1101 * add tests for prune_requests_older_than Signed-off-by: akela1101 * Update rclcpp/test/rclcpp/test_client.cpp Co-authored-by: Chen Lihui Signed-off-by: akela1101 Signed-off-by: akela1101 Co-authored-by: Chen Lihui --- rclcpp/include/rclcpp/client.hpp | 4 +++- rclcpp/test/rclcpp/test_client.cpp | 21 +++++++++++++++++++++ 2 files changed, 24 insertions(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index b10e3e1593..d84ee089b5 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -769,7 +769,9 @@ class Client : public ClientBase auto old_size = pending_requests_.size(); for (auto it = pending_requests_.begin(), last = pending_requests_.end(); it != last; ) { if (it->second.first < time_point) { - pruned_requests->push_back(it->first); + if (pruned_requests) { + pruned_requests->push_back(it->first); + } it = pending_requests_.erase(it); } else { ++it; diff --git a/rclcpp/test/rclcpp/test_client.cpp b/rclcpp/test/rclcpp/test_client.cpp index a9d81d9176..9070e1caa9 100644 --- a/rclcpp/test/rclcpp/test_client.cpp +++ b/rclcpp/test/rclcpp/test_client.cpp @@ -327,6 +327,27 @@ TEST_F(TestClientWithServer, test_client_remove_pending_request) { EXPECT_TRUE(client->remove_pending_request(future)); } +TEST_F(TestClientWithServer, prune_requests_older_than_no_pruned) { + auto client = node->create_client(service_name); + auto request = std::make_shared(); + auto future = client->async_send_request(request); + auto time = std::chrono::system_clock::now() + 1s; + + EXPECT_EQ(1u, client->prune_requests_older_than(time)); +} + +TEST_F(TestClientWithServer, prune_requests_older_than_with_pruned) { + auto client = node->create_client(service_name); + auto request = std::make_shared(); + auto future = client->async_send_request(request); + auto time = std::chrono::system_clock::now() + 1s; + + std::vector pruned_requests; + EXPECT_EQ(1u, client->prune_requests_older_than(time, &pruned_requests)); + ASSERT_EQ(1u, pruned_requests.size()); + EXPECT_EQ(future.request_id, pruned_requests[0]); +} + TEST_F(TestClientWithServer, async_send_request_rcl_send_request_error) { // Checking rcl_send_request in rclcpp::Client::async_send_request() auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_send_request, RCL_RET_ERROR); From 432bf21f0261bab209b37ccfe6da550f02751a22 Mon Sep 17 00:00:00 2001 From: Jeffery Hsu Date: Tue, 13 Dec 2022 11:11:09 -0600 Subject: [PATCH 41/53] Add clock type to node_options (#1982) * add clock type to node_options and change node/lifecycle_node constructor accordingly Signed-off-by: Jeffery Hsu * Modify TimeSource::NodeState class to work with different clock types. Add test cases. Signed-off-by: Jeffery Hsu * Change on_parameter_event to output RCLCPP_ERROR and check ros_time_active_ in ClocksState::attachClock() Signed-off-by: Jeffery Hsu * Remove a redundant include Signed-off-by: Jeffery Hsu * Disallow setting use_sim_time to true if a clock_type can't support it. Following the reasoning in c54a6f1cd2b5bfbe530c362ad7e8fa22f753e325, on_set_parameters doesn't try to enforce use_sim_time to be of boolean type explicitly. Signed-off-by: Jeffery Hsu * minior style change Signed-off-by: Jeffery Hsu * remove reason string for successful result Signed-off-by: Jeffery Hsu Signed-off-by: Jeffery Hsu --- .../rclcpp/node_interfaces/node_clock.hpp | 6 +- rclcpp/include/rclcpp/node_options.hpp | 17 ++++ rclcpp/src/rclcpp/node.cpp | 3 +- .../src/rclcpp/node_interfaces/node_clock.cpp | 9 +- rclcpp/src/rclcpp/node_options.cpp | 14 +++ rclcpp/src/rclcpp/time_source.cpp | 92 +++++++++++++++---- rclcpp/test/rclcpp/test_node.cpp | 68 ++++++++++++++ rclcpp/test/rclcpp/test_node_options.cpp | 9 ++ rclcpp/test/rclcpp/test_time_source.cpp | 29 ++++++ rclcpp_lifecycle/src/lifecycle_node.cpp | 3 +- 10 files changed, 222 insertions(+), 28 deletions(-) diff --git a/rclcpp/include/rclcpp/node_interfaces/node_clock.hpp b/rclcpp/include/rclcpp/node_interfaces/node_clock.hpp index fd9a34a907..fb2e0670a7 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_clock.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_clock.hpp @@ -15,6 +15,7 @@ #ifndef RCLCPP__NODE_INTERFACES__NODE_CLOCK_HPP_ #define RCLCPP__NODE_INTERFACES__NODE_CLOCK_HPP_ +#include "rcl/time.h" #include "rclcpp/clock.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" @@ -42,7 +43,8 @@ class NodeClock : public NodeClockInterface rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics, rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services, - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging); + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + rcl_clock_type_t clock_type); RCLCPP_PUBLIC virtual @@ -67,7 +69,7 @@ class NodeClock : public NodeClockInterface rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_; rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_; - rclcpp::Clock::SharedPtr ros_clock_; + rclcpp::Clock::SharedPtr clock_; }; } // namespace node_interfaces diff --git a/rclcpp/include/rclcpp/node_options.hpp b/rclcpp/include/rclcpp/node_options.hpp index d9735357dd..47a97fd3c4 100644 --- a/rclcpp/include/rclcpp/node_options.hpp +++ b/rclcpp/include/rclcpp/node_options.hpp @@ -19,6 +19,7 @@ #include #include +#include "rcl/time.h" #include "rcl/node_options.h" #include "rclcpp/context.hpp" #include "rclcpp/contexts/default_context.hpp" @@ -46,6 +47,7 @@ class NodeOptions * - enable_topic_statistics = false * - start_parameter_services = true * - start_parameter_event_publisher = true + * - clock_type = RCL_ROS_TIME * - clock_qos = rclcpp::ClockQoS() * - use_clock_thread = true * - rosout_qos = rclcpp::RosoutQoS() @@ -246,6 +248,19 @@ class NodeOptions NodeOptions & start_parameter_event_publisher(bool start_parameter_event_publisher); + /// Return a reference to the clock type. + RCLCPP_PUBLIC + const rcl_clock_type_t & + clock_type() const; + + /// Set the clock type. + /** + * The clock type to be used by the node. + */ + RCLCPP_PUBLIC + NodeOptions & + clock_type(const rcl_clock_type_t & clock_type); + /// Return a reference to the clock QoS. RCLCPP_PUBLIC const rclcpp::QoS & @@ -400,6 +415,8 @@ class NodeOptions bool start_parameter_event_publisher_ {true}; + rcl_clock_type_t clock_type_ {RCL_ROS_TIME}; + rclcpp::QoS clock_qos_ = rclcpp::ClockQoS(); bool use_clock_thread_ {true}; diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index 7dba3e51ad..c1fbdb1f98 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -176,7 +176,8 @@ Node::Node( node_topics_, node_graph_, node_services_, - node_logging_ + node_logging_, + options.clock_type() )), node_parameters_(new rclcpp::node_interfaces::NodeParameters( node_base_, diff --git a/rclcpp/src/rclcpp/node_interfaces/node_clock.cpp b/rclcpp/src/rclcpp/node_interfaces/node_clock.cpp index 734d7bdb37..a37c65b71b 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_clock.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_clock.cpp @@ -24,13 +24,14 @@ NodeClock::NodeClock( rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics, rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services, - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging) + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + rcl_clock_type_t clock_type) : node_base_(node_base), node_topics_(node_topics), node_graph_(node_graph), node_services_(node_services), node_logging_(node_logging), - ros_clock_(std::make_shared(RCL_ROS_TIME)) + clock_(std::make_shared(clock_type)) {} NodeClock::~NodeClock() @@ -39,11 +40,11 @@ NodeClock::~NodeClock() rclcpp::Clock::SharedPtr NodeClock::get_clock() { - return ros_clock_; + return clock_; } rclcpp::Clock::ConstSharedPtr NodeClock::get_clock() const { - return ros_clock_; + return clock_; } diff --git a/rclcpp/src/rclcpp/node_options.cpp b/rclcpp/src/rclcpp/node_options.cpp index 06feffd1e6..b90a4b27e7 100644 --- a/rclcpp/src/rclcpp/node_options.cpp +++ b/rclcpp/src/rclcpp/node_options.cpp @@ -77,6 +77,7 @@ NodeOptions::operator=(const NodeOptions & other) this->enable_topic_statistics_ = other.enable_topic_statistics_; this->start_parameter_services_ = other.start_parameter_services_; this->start_parameter_event_publisher_ = other.start_parameter_event_publisher_; + this->clock_type_ = other.clock_type_; this->clock_qos_ = other.clock_qos_; this->use_clock_thread_ = other.use_clock_thread_; this->parameter_event_qos_ = other.parameter_event_qos_; @@ -260,6 +261,19 @@ NodeOptions::start_parameter_event_publisher(bool start_parameter_event_publishe return *this; } +const rcl_clock_type_t & +NodeOptions::clock_type() const +{ + return this->clock_type_; +} + +NodeOptions & +NodeOptions::clock_type(const rcl_clock_type_t & clock_type) +{ + this->clock_type_ = clock_type; + return *this; +} + const rclcpp::QoS & NodeOptions::clock_qos() const { diff --git a/rclcpp/src/rclcpp/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp index 42a9208251..bf1f9b3daa 100644 --- a/rclcpp/src/rclcpp/time_source.cpp +++ b/rclcpp/src/rclcpp/time_source.cpp @@ -91,10 +91,13 @@ class ClocksState final // Attach a clock void attachClock(rclcpp::Clock::SharedPtr clock) { - if (clock->get_clock_type() != RCL_ROS_TIME) { - throw std::invalid_argument("Cannot attach clock to a time source that's not a ROS clock"); + { + std::lock_guard clock_guard(clock->get_clock_mutex()); + if (clock->get_clock_type() != RCL_ROS_TIME && ros_time_active_) { + throw std::invalid_argument( + "ros_time_active_ can't be true while clock is not of RCL_ROS_TIME type"); + } } - std::lock_guard guard(clock_list_lock_); associated_clocks_.push_back(clock); // Set the clock to zero unless there's a recently received message @@ -125,27 +128,32 @@ class ClocksState final { std::lock_guard clock_guard(clock->get_clock_mutex()); - // Do change - if (!set_ros_time_enabled && clock->ros_time_is_active()) { - auto ret = rcl_disable_ros_time_override(clock->get_clock_handle()); - if (ret != RCL_RET_OK) { - rclcpp::exceptions::throw_from_rcl_error( - ret, "Failed to disable ros_time_override_status"); + if (clock->get_clock_type() == RCL_ROS_TIME) { + // Do change + if (!set_ros_time_enabled && clock->ros_time_is_active()) { + auto ret = rcl_disable_ros_time_override(clock->get_clock_handle()); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "Failed to disable ros_time_override_status"); + } + } else if (set_ros_time_enabled && !clock->ros_time_is_active()) { + auto ret = rcl_enable_ros_time_override(clock->get_clock_handle()); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "Failed to enable ros_time_override_status"); + } } - } else if (set_ros_time_enabled && !clock->ros_time_is_active()) { - auto ret = rcl_enable_ros_time_override(clock->get_clock_handle()); + + auto ret = rcl_set_ros_time_override( + clock->get_clock_handle(), + rclcpp::Time(*msg).nanoseconds()); if (ret != RCL_RET_OK) { rclcpp::exceptions::throw_from_rcl_error( - ret, "Failed to enable ros_time_override_status"); + ret, "Failed to set ros_time_override_status"); } - } - - auto ret = rcl_set_ros_time_override( - clock->get_clock_handle(), - rclcpp::Time(*msg).nanoseconds()); - if (ret != RCL_RET_OK) { - rclcpp::exceptions::throw_from_rcl_error( - ret, "Failed to set ros_time_override_status"); + } else if (set_ros_time_enabled) { + throw std::invalid_argument( + "set_ros_time_enabled can't be true while clock is not of RCL_ROS_TIME type"); } } @@ -166,6 +174,18 @@ class ClocksState final last_msg_set_ = msg; } + bool are_all_clocks_rcl_ros_time() + { + std::lock_guard guard(clock_list_lock_); + for (auto & clock : associated_clocks_) { + std::lock_guard clock_guard(clock->get_clock_mutex()); + if (clock->get_clock_type() != RCL_ROS_TIME) { + return false; + } + } + return true; + } + private: // Store (and update on node attach) logger for logging. Logger logger_; @@ -266,6 +286,10 @@ class TimeSource::NodeState final throw std::invalid_argument("Invalid type for parameter 'use_sim_time', should be 'bool'"); } + on_set_parameters_callback_ = node_parameters_->add_on_set_parameters_callback( + std::bind(&TimeSource::NodeState::on_set_parameters, this, std::placeholders::_1)); + + // TODO(tfoote) use parameters interface not subscribe to events via topic ticketed #609 parameter_subscription_ = rclcpp::AsyncParametersClient::on_parameter_event( node_topics_, @@ -285,6 +309,10 @@ class TimeSource::NodeState final // can't possibly call any of the callbacks as we are cleaning up. destroy_clock_sub(); clocks_state_.disable_ros_time(); + if (on_set_parameters_callback_) { + node_parameters_->remove_on_set_parameters_callback(on_set_parameters_callback_.get()); + } + on_set_parameters_callback_.reset(); parameter_subscription_.reset(); node_base_.reset(); node_topics_.reset(); @@ -419,10 +447,34 @@ class TimeSource::NodeState final clock_subscription_.reset(); } + // On set Parameters callback handle + node_interfaces::OnSetParametersCallbackHandle::SharedPtr on_set_parameters_callback_{nullptr}; + // Parameter Event subscription using ParamSubscriptionT = rclcpp::Subscription; std::shared_ptr parameter_subscription_; + // Callback for parameter settings + rcl_interfaces::msg::SetParametersResult on_set_parameters( + const std::vector & parameters) + { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + for (const auto & param : parameters) { + if (param.get_name() == "use_sim_time" && param.get_type() == rclcpp::PARAMETER_BOOL) { + if (param.as_bool() && !(clocks_state_.are_all_clocks_rcl_ros_time())) { + result.successful = false; + result.reason = + "use_sim_time parameter can't be true while clocks are not all of RCL_ROS_TIME type"; + RCLCPP_ERROR( + logger_, + "use_sim_time parameter can't be true while clocks are not all of RCL_ROS_TIME type"); + } + } + } + return result; + } + // Callback for parameter updates void on_parameter_event(std::shared_ptr event) { diff --git a/rclcpp/test/rclcpp/test_node.cpp b/rclcpp/test/rclcpp/test_node.cpp index 19c87b3684..512c2c1384 100644 --- a/rclcpp/test/rclcpp/test_node.cpp +++ b/rclcpp/test/rclcpp/test_node.cpp @@ -95,6 +95,74 @@ TEST_F(TestNode, construction_and_destruction) { (void)node; }, rclcpp::exceptions::InvalidNamespaceError); } + + { + rclcpp::NodeOptions options; + ASSERT_NO_THROW( + { + const auto node = std::make_shared("my_node", "/ns", options); + EXPECT_EQ(RCL_ROS_TIME, node->get_clock()->get_clock_type()); + }); + } + + { + rclcpp::NodeOptions options; + options.parameter_overrides( + { + {"use_sim_time", true}, + }); + ASSERT_NO_THROW( + { + const auto node = std::make_shared("my_node", "/ns", options); + EXPECT_EQ(RCL_ROS_TIME, node->get_clock()->get_clock_type()); + }); + } + + { + rclcpp::NodeOptions options; + options.clock_type(RCL_SYSTEM_TIME); + ASSERT_NO_THROW( + { + const auto node = std::make_shared("my_node", "/ns", options); + EXPECT_EQ(RCL_SYSTEM_TIME, node->get_clock()->get_clock_type()); + }); + } + + { + rclcpp::NodeOptions options; + options.parameter_overrides( + { + {"use_sim_time", true}, + }); + options.clock_type(RCL_SYSTEM_TIME); + ASSERT_THROW( + const auto node = std::make_shared( + "my_node", "/ns", + options), std::invalid_argument); + } + + { + rclcpp::NodeOptions options; + options.clock_type(RCL_STEADY_TIME); + ASSERT_NO_THROW( + { + const auto node = std::make_shared("my_node", "/ns", options); + EXPECT_EQ(RCL_STEADY_TIME, node->get_clock()->get_clock_type()); + }); + } + + { + rclcpp::NodeOptions options; + options.parameter_overrides( + { + {"use_sim_time", true}, + }); + options.clock_type(RCL_STEADY_TIME); + ASSERT_THROW( + const auto node = std::make_shared( + "my_node", "/ns", + options), std::invalid_argument); + } } /* diff --git a/rclcpp/test/rclcpp/test_node_options.cpp b/rclcpp/test/rclcpp/test_node_options.cpp index 3fd0a0d52b..2468923229 100644 --- a/rclcpp/test/rclcpp/test_node_options.cpp +++ b/rclcpp/test/rclcpp/test_node_options.cpp @@ -316,3 +316,12 @@ TEST(TestNodeOptions, set_get_allocator) { // Check invalid allocator EXPECT_THROW(options.get_rcl_node_options(), std::bad_alloc); } + +TEST(TestNodeOptions, clock_type) { + rclcpp::NodeOptions options; + EXPECT_EQ(RCL_ROS_TIME, options.clock_type()); + options.clock_type(RCL_SYSTEM_TIME); + EXPECT_EQ(RCL_SYSTEM_TIME, options.clock_type()); + options.clock_type(RCL_STEADY_TIME); + EXPECT_EQ(RCL_STEADY_TIME, options.clock_type()); +} diff --git a/rclcpp/test/rclcpp/test_time_source.cpp b/rclcpp/test/rclcpp/test_time_source.cpp index a5aad9a498..22aefcda3b 100644 --- a/rclcpp/test/rclcpp/test_time_source.cpp +++ b/rclcpp/test/rclcpp/test_time_source.cpp @@ -267,6 +267,35 @@ TEST(TimeSource, invalid_sim_time_parameter_override) rclcpp::shutdown(); } +TEST(TimeSource, valid_clock_type_for_sim_time) +{ + rclcpp::init(0, nullptr); + + rclcpp::NodeOptions options; + auto node = std::make_shared("my_node", options); + EXPECT_TRUE( + node->set_parameter( + rclcpp::Parameter( + "use_sim_time", rclcpp::ParameterValue( + true))).successful); + rclcpp::shutdown(); +} + +TEST(TimeSource, invalid_clock_type_for_sim_time) +{ + rclcpp::init(0, nullptr); + + rclcpp::NodeOptions options; + options.clock_type(RCL_STEADY_TIME); + auto node = std::make_shared("my_node", options); + EXPECT_FALSE( + node->set_parameter( + rclcpp::Parameter( + "use_sim_time", rclcpp::ParameterValue( + true))).successful); + rclcpp::shutdown(); +} + TEST_F(TestTimeSource, clock) { rclcpp::TimeSource ts(node); auto ros_clock = std::make_shared(RCL_ROS_TIME); diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 773febda95..723c7bc8c1 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -85,7 +85,8 @@ LifecycleNode::LifecycleNode( node_topics_, node_graph_, node_services_, - node_logging_ + node_logging_, + options.clock_type() )), node_parameters_(new rclcpp::node_interfaces::NodeParameters( node_base_, From 86335dd4acd91d5dd973c4e4e97014e5e8a916bc Mon Sep 17 00:00:00 2001 From: Mateusz Szczygielski <112629916+msz-rai@users.noreply.github.com> Date: Tue, 13 Dec 2022 19:40:38 +0100 Subject: [PATCH 42/53] Fix logging macros to build with msvc and cpp20 (#2063) Signed-off-by: Mateusz Szczygielski Signed-off-by: Mateusz Szczygielski --- rclcpp/resource/logging.hpp.em | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/resource/logging.hpp.em b/rclcpp/resource/logging.hpp.em index 63581ceb54..01087e5dd0 100644 --- a/rclcpp/resource/logging.hpp.em +++ b/rclcpp/resource/logging.hpp.em @@ -124,7 +124,7 @@ def get_rclcpp_suffix_from_features(features): ) \ do { \ static_assert( \ - ::std::is_same::type>::type, \ + ::std::is_same>, \ typename ::rclcpp::Logger>::value, \ "First argument to logging macros must be an rclcpp::Logger"); \ @[ if 'throttle' in feature_combination]@ \ From a20a295a3bd78c1b27217de477a6ad17e0ba0df8 Mon Sep 17 00:00:00 2001 From: mauropasse Date: Fri, 16 Dec 2022 09:44:59 +0000 Subject: [PATCH 43/53] Set explicitly callback type (#2059) * Set explicitly callback type Signed-off-by: Mauro Passerino * Revert "Set explicitly callback type" This reverts commit dfb4c54adb0580d976199738a8a6dcdb261a87b0. Signed-off-by: Mauro Passerino * Add type info to cpp_callback_trampoline Signed-off-by: Mauro Passerino Signed-off-by: Mauro Passerino Co-authored-by: Mauro Passerino --- rclcpp/include/rclcpp/client.hpp | 5 +++-- rclcpp/include/rclcpp/detail/cpp_callback_trampoline.hpp | 4 +++- rclcpp/include/rclcpp/qos_event.hpp | 5 +++-- rclcpp/include/rclcpp/service.hpp | 5 +++-- rclcpp/include/rclcpp/subscription_base.hpp | 5 +++-- rclcpp_action/src/client.cpp | 4 ++-- rclcpp_action/src/server.cpp | 4 ++-- 7 files changed, 19 insertions(+), 13 deletions(-) diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index d84ee089b5..c3a2210a71 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -312,7 +312,7 @@ class ClientBase // 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, + rclcpp::detail::cpp_callback_trampoline, static_cast(&new_callback)); // Store the std::function to keep it in scope, also overwrites the existing one. @@ -320,7 +320,8 @@ class ClientBase // Set it again, now using the permanent storage. set_on_new_response_callback( - rclcpp::detail::cpp_callback_trampoline, + rclcpp::detail::cpp_callback_trampoline< + decltype(on_new_response_callback_), const void *, size_t>, static_cast(&on_new_response_callback_)); } diff --git a/rclcpp/include/rclcpp/detail/cpp_callback_trampoline.hpp b/rclcpp/include/rclcpp/detail/cpp_callback_trampoline.hpp index 4f46dfc939..e9c07b71e0 100644 --- a/rclcpp/include/rclcpp/detail/cpp_callback_trampoline.hpp +++ b/rclcpp/include/rclcpp/detail/cpp_callback_trampoline.hpp @@ -41,6 +41,7 @@ namespace detail * so no exceptions should be thrown at this point, and doing so results in * undefined behavior. * + * \tparam UserDataRealT Declared type of the passed function * \tparam UserDataT Deduced type based on what is passed for user data, * usually this type is either `void *` or `const void *`. * \tparam Args the arguments being passed to the callback @@ -50,6 +51,7 @@ namespace detail * \returns whatever the callback returns, if anything */ template< + typename UserDataRealT, typename UserDataT, typename ... Args, typename ReturnT = void @@ -57,7 +59,7 @@ template< ReturnT cpp_callback_trampoline(UserDataT user_data, Args ... args) noexcept { - auto & actual_callback = *reinterpret_cast *>(user_data); + auto & actual_callback = *static_cast(user_data); return actual_callback(args ...); } diff --git a/rclcpp/include/rclcpp/qos_event.hpp b/rclcpp/include/rclcpp/qos_event.hpp index f258c5d5ba..b48854c4bd 100644 --- a/rclcpp/include/rclcpp/qos_event.hpp +++ b/rclcpp/include/rclcpp/qos_event.hpp @@ -185,7 +185,7 @@ class QOSEventHandlerBase : public Waitable // 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, + rclcpp::detail::cpp_callback_trampoline, static_cast(&new_callback)); // Store the std::function to keep it in scope, also overwrites the existing one. @@ -193,7 +193,8 @@ class QOSEventHandlerBase : public Waitable // Set it again, now using the permanent storage. set_on_new_event_callback( - rclcpp::detail::cpp_callback_trampoline, + rclcpp::detail::cpp_callback_trampoline< + decltype(on_new_event_callback_), const void *, size_t>, static_cast(&on_new_event_callback_)); } diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 65b457ffda..9e258a0223 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -222,7 +222,7 @@ class ServiceBase // 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, + rclcpp::detail::cpp_callback_trampoline, static_cast(&new_callback)); // Store the std::function to keep it in scope, also overwrites the existing one. @@ -230,7 +230,8 @@ class ServiceBase // Set it again, now using the permanent storage. set_on_new_request_callback( - rclcpp::detail::cpp_callback_trampoline, + rclcpp::detail::cpp_callback_trampoline< + decltype(on_new_request_callback_), const void *, size_t>, static_cast(&on_new_request_callback_)); } diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index f21f27e864..09fabe79e5 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -348,7 +348,7 @@ class SubscriptionBase : public std::enable_shared_from_this // 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, + rclcpp::detail::cpp_callback_trampoline, static_cast(&new_callback)); // Store the std::function to keep it in scope, also overwrites the existing one. @@ -356,7 +356,8 @@ class SubscriptionBase : public std::enable_shared_from_this // Set it again, now using the permanent storage. set_on_new_message_callback( - rclcpp::detail::cpp_callback_trampoline, + rclcpp::detail::cpp_callback_trampoline< + decltype(on_new_message_callback_), const void *, size_t>, static_cast(&on_new_message_callback_)); } diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index 2266b7cee6..e687ab3400 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -433,7 +433,7 @@ ClientBase::set_callback_to_entity( // been replaced but the middleware hasn't been told about the new one yet. set_on_ready_callback( entity_type, - rclcpp::detail::cpp_callback_trampoline, + rclcpp::detail::cpp_callback_trampoline, static_cast(&new_callback)); std::lock_guard lock(listener_mutex_); @@ -453,7 +453,7 @@ ClientBase::set_callback_to_entity( auto & cb = it->second; set_on_ready_callback( entity_type, - rclcpp::detail::cpp_callback_trampoline, + rclcpp::detail::cpp_callback_trampolinesecond), const void *, size_t>, static_cast(&cb)); } diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index 42c4074495..c0fa96a88e 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -745,7 +745,7 @@ ServerBase::set_callback_to_entity( // been replaced but the middleware hasn't been told about the new one yet. set_on_ready_callback( entity_type, - rclcpp::detail::cpp_callback_trampoline, + rclcpp::detail::cpp_callback_trampoline, static_cast(&new_callback)); std::lock_guard lock(listener_mutex_); @@ -765,7 +765,7 @@ ServerBase::set_callback_to_entity( auto & cb = it->second; set_on_ready_callback( entity_type, - rclcpp::detail::cpp_callback_trampoline, + rclcpp::detail::cpp_callback_trampolinesecond), const void *, size_t>, static_cast(&cb)); } From c091fe1a4538dbb370a31d0e590bd44ae4194483 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Mon, 19 Dec 2022 18:18:54 -0800 Subject: [PATCH 44/53] Implement validity checks for rclcpp::Clock (#2040) --- rclcpp/include/rclcpp/clock.hpp | 45 ++++++++++++++++++++ rclcpp/src/rclcpp/clock.cpp | 65 +++++++++++++++++++++++++++++ rclcpp/test/rclcpp/test_time.cpp | 70 ++++++++++++++++++++++++++++++++ 3 files changed, 180 insertions(+) diff --git a/rclcpp/include/rclcpp/clock.hpp b/rclcpp/include/rclcpp/clock.hpp index 1e08e1d8fd..702b224d53 100644 --- a/rclcpp/include/rclcpp/clock.hpp +++ b/rclcpp/include/rclcpp/clock.hpp @@ -137,6 +137,51 @@ class Clock Duration rel_time, Context::SharedPtr context = contexts::get_global_default_context()); + /** + * Check if the clock is started. + * + * A started clock is a clock that reflects non-zero time. + * Typically a clock will be unstarted if it is using RCL_ROS_TIME with ROS time and + * nothing has been published on the clock topic yet. + * + * \return true if clock is started + * \throws std::runtime_error if the clock is not rcl_clock_valid + */ + RCLCPP_PUBLIC + bool + started(); + + /** + * Wait until clock to start. + * + * \rclcpp::Clock::started + * \param context the context to wait in + * \return true if clock was already started or became started + * \throws std::runtime_error if the context is invalid or clock is not rcl_clock_valid + */ + RCLCPP_PUBLIC + bool + wait_until_started(Context::SharedPtr context = contexts::get_global_default_context()); + + /** + * Wait for clock to start, with timeout. + * + * The timeout is waited in steady time. + * + * \rclcpp::Clock::started + * \param timeout the maximum time to wait for. + * \param context the context to wait in. + * \param wait_tick_ns the time to wait between each iteration of the wait loop (in nanoseconds). + * \return true if clock was or became valid + * \throws std::runtime_error if the context is invalid or clock is not rcl_clock_valid + */ + RCLCPP_PUBLIC + bool + wait_until_started( + const rclcpp::Duration & timeout, + Context::SharedPtr context = contexts::get_global_default_context(), + const rclcpp::Duration & wait_tick_ns = rclcpp::Duration(0, static_cast(1e7))); + /** * Returns the clock of the type `RCL_ROS_TIME` is active. * diff --git a/rclcpp/src/rclcpp/clock.cpp b/rclcpp/src/rclcpp/clock.cpp index b34f328690..f46649a77d 100644 --- a/rclcpp/src/rclcpp/clock.cpp +++ b/rclcpp/src/rclcpp/clock.cpp @@ -182,6 +182,71 @@ Clock::sleep_for(Duration rel_time, Context::SharedPtr context) return sleep_until(now() + rel_time, context); } +bool +Clock::started() +{ + if (!rcl_clock_valid(get_clock_handle())) { + throw std::runtime_error("clock is not rcl_clock_valid"); + } + return rcl_clock_time_started(get_clock_handle()); +} + +bool +Clock::wait_until_started(Context::SharedPtr context) +{ + if (!context || !context->is_valid()) { + throw std::runtime_error("context cannot be slept with because it's invalid"); + } + if (!rcl_clock_valid(get_clock_handle())) { + throw std::runtime_error("clock cannot be waited on as it is not rcl_clock_valid"); + } + + if (started()) { + return true; + } else { + // Wait until the first non-zero time + return sleep_until(rclcpp::Time(0, 1, get_clock_type()), context); + } +} + +bool +Clock::wait_until_started( + const Duration & timeout, + Context::SharedPtr context, + const Duration & wait_tick_ns) +{ + if (!context || !context->is_valid()) { + throw std::runtime_error("context cannot be slept with because it's invalid"); + } + if (!rcl_clock_valid(get_clock_handle())) { + throw std::runtime_error("clock cannot be waited on as it is not rcl_clock_valid"); + } + + Clock timeout_clock = Clock(RCL_STEADY_TIME); + Time start = timeout_clock.now(); + + // Check if the clock has started every wait_tick_ns nanoseconds + // Context check checks for rclcpp::shutdown() + while (!started() && context->is_valid()) { + if (timeout < wait_tick_ns) { + timeout_clock.sleep_for(timeout); + } else { + Duration time_left = start + timeout - timeout_clock.now(); + if (time_left > wait_tick_ns) { + timeout_clock.sleep_for(Duration(wait_tick_ns)); + } else { + timeout_clock.sleep_for(time_left); + } + } + + if (timeout_clock.now() - start > timeout) { + return started(); + } + } + return started(); +} + + bool Clock::ros_time_is_active() { diff --git a/rclcpp/test/rclcpp/test_time.cpp b/rclcpp/test/rclcpp/test_time.cpp index 2f188b2d7c..f3969d3886 100644 --- a/rclcpp/test/rclcpp/test_time.cpp +++ b/rclcpp/test/rclcpp/test_time.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include "rcl/error_handling.h" #include "rcl/time.h" @@ -848,3 +849,72 @@ TEST_F(TestClockSleep, sleep_for_basic_ros) { sleep_thread.join(); EXPECT_TRUE(sleep_succeeded); } + +class TestClockStarted : public ::testing::Test +{ +protected: + void SetUp() + { + rclcpp::init(0, nullptr); + } + + void TearDown() + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestClockStarted, started) { + // rclcpp::Clock ros_clock(RCL_ROS_TIME); + // auto ros_clock_handle = ros_clock.get_clock_handle(); + // + // // At this point, the ROS clock is reading system time since the ROS time override isn't on + // // So we expect it to be started (it's extremely unlikely that system time is at epoch start) + // EXPECT_TRUE(ros_clock.started()); + // EXPECT_TRUE(ros_clock.wait_until_started()); + // EXPECT_TRUE(ros_clock.wait_until_started(rclcpp::Duration(0, static_cast(1e7)))); + // EXPECT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(ros_clock_handle)); + // EXPECT_TRUE(ros_clock.ros_time_is_active()); + // EXPECT_FALSE(ros_clock.started()); + // EXPECT_EQ(RCL_RET_OK, rcl_set_ros_time_override(ros_clock_handle, 1)); + // EXPECT_TRUE(ros_clock.started()); + // + // rclcpp::Clock system_clock(RCL_SYSTEM_TIME); + // EXPECT_TRUE(system_clock.started()); + // EXPECT_TRUE(system_clock.wait_until_started()); + // EXPECT_TRUE(system_clock.wait_until_started(rclcpp::Duration(0, static_cast(1e7)))); + // + // rclcpp::Clock steady_clock(RCL_STEADY_TIME); + // EXPECT_TRUE(steady_clock.started()); + // EXPECT_TRUE(steady_clock.wait_until_started()); + // EXPECT_TRUE(steady_clock.wait_until_started(rclcpp::Duration(0, static_cast(1e7)))); + // + // rclcpp::Clock uninit_clock(RCL_CLOCK_UNINITIALIZED); + // RCLCPP_EXPECT_THROW_EQ( + // uninit_clock.started(), std::runtime_error("clock is not rcl_clock_valid")); + // RCLCPP_EXPECT_THROW_EQ( + // uninit_clock.wait_until_started(rclcpp::Duration(0, static_cast(1e7))), + // std::runtime_error("clock cannot be waited on as it is not rcl_clock_valid")); +} + +TEST_F(TestClockStarted, started_timeout) { + rclcpp::Clock ros_clock(RCL_ROS_TIME); + auto ros_clock_handle = ros_clock.get_clock_handle(); + + EXPECT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(ros_clock_handle)); + EXPECT_TRUE(ros_clock.ros_time_is_active()); + + EXPECT_EQ(RCL_RET_OK, rcl_set_ros_time_override(ros_clock_handle, 0)); + + EXPECT_FALSE(ros_clock.started()); + EXPECT_FALSE(ros_clock.wait_until_started(rclcpp::Duration(0, static_cast(1e7)))); + + std::thread t([]() { + std::this_thread::sleep_for(std::chrono::seconds(1)); + rclcpp::shutdown(); + }); + + // Test rclcpp shutdown escape hatch (otherwise this waits indefinitely) + EXPECT_FALSE(ros_clock.wait_until_started()); + t.join(); +} From 9c1c9896a39047faadec0e784b692c5292d3c1dd Mon Sep 17 00:00:00 2001 From: methylDragon Date: Mon, 19 Dec 2022 18:21:40 -0800 Subject: [PATCH 45/53] Move event callback binding to PublisherBase and SubscriptionBase (#2066) --- rclcpp/include/rclcpp/generic_publisher.hpp | 36 +++------------- .../include/rclcpp/generic_subscription.hpp | 38 ++-------------- rclcpp/include/rclcpp/publisher.hpp | 32 ++------------ rclcpp/include/rclcpp/publisher_base.hpp | 11 ++++- rclcpp/include/rclcpp/subscription.hpp | 35 ++------------- rclcpp/include/rclcpp/subscription_base.hpp | 10 +++++ rclcpp/src/rclcpp/publisher_base.cpp | 41 +++++++++++++++++- rclcpp/src/rclcpp/subscription_base.cpp | 43 ++++++++++++++++++- .../node_interfaces/test_node_topics.cpp | 18 ++++---- rclcpp/test/rclcpp/test_publisher.cpp | 9 ++-- rclcpp/test/rclcpp/test_type_support.cpp | 17 +++++--- 11 files changed, 141 insertions(+), 149 deletions(-) diff --git a/rclcpp/include/rclcpp/generic_publisher.hpp b/rclcpp/include/rclcpp/generic_publisher.hpp index e1b46002bc..7cd2d8bc39 100644 --- a/rclcpp/include/rclcpp/generic_publisher.hpp +++ b/rclcpp/include/rclcpp/generic_publisher.hpp @@ -78,38 +78,12 @@ class GenericPublisher : public rclcpp::PublisherBase node_base, topic_name, *rclcpp::get_typesupport_handle(topic_type, "rosidl_typesupport_cpp", *ts_lib), - options.template to_rcl_publisher_options(qos)), + options.template to_rcl_publisher_options(qos), + // NOTE(methylDragon): Passing these args separately is necessary for event binding + options.event_callbacks, + options.use_default_callbacks), 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; diff --git a/rclcpp/include/rclcpp/generic_subscription.hpp b/rclcpp/include/rclcpp/generic_subscription.hpp index 16c8fe6fbf..12a1c79f8f 100644 --- a/rclcpp/include/rclcpp/generic_subscription.hpp +++ b/rclcpp/include/rclcpp/generic_subscription.hpp @@ -82,44 +82,12 @@ class GenericSubscription : public rclcpp::SubscriptionBase *rclcpp::get_typesupport_handle(topic_type, "rosidl_typesupport_cpp", *ts_lib), topic_name, options.to_rcl_subscription_options(qos), + options.event_callbacks, + options.use_default_callbacks, 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; diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 78b5700611..43fa4ef6a2 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -131,40 +131,16 @@ class Publisher : public PublisherBase node_base, topic, rclcpp::get_message_type_support_handle(), - options.template to_rcl_publisher_options(qos)), + options.template to_rcl_publisher_options(qos), + // NOTE(methylDragon): Passing these args separately is necessary for event binding + options.event_callbacks, + options.use_default_callbacks), options_(options), published_type_allocator_(*options.get_allocator()), ros_message_type_allocator_(*options.get_allocator()) { allocator::set_allocator_for_deleter(&published_type_deleter_, &published_type_allocator_); allocator::set_allocator_for_deleter(&ros_message_type_deleter_, &ros_message_type_allocator_); - - if (options_.event_callbacks.deadline_callback) { - this->add_event_handler( - options_.event_callbacks.deadline_callback, - RCL_PUBLISHER_OFFERED_DEADLINE_MISSED); - } - if (options_.event_callbacks.liveliness_callback) { - this->add_event_handler( - options_.event_callbacks.liveliness_callback, - RCL_PUBLISHER_LIVELINESS_LOST); - } - if (options_.event_callbacks.incompatible_qos_callback) { - this->add_event_handler( - options_.event_callbacks.incompatible_qos_callback, - RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS); - } else if (options_.use_default_callbacks) { - // Register default callback when not specified - try { - this->add_event_handler( - [this](QOSOfferedIncompatibleQoSInfo & info) { - this->default_incompatible_qos_callback(info); - }, - RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS); - } catch (UnsupportedEventTypeException & /*exc*/) { - // pass - } - } // Setup continues in the post construction method, post_init_setup(). } diff --git a/rclcpp/include/rclcpp/publisher_base.hpp b/rclcpp/include/rclcpp/publisher_base.hpp index 8416757a53..153d5a6ebe 100644 --- a/rclcpp/include/rclcpp/publisher_base.hpp +++ b/rclcpp/include/rclcpp/publisher_base.hpp @@ -78,11 +78,18 @@ class PublisherBase : public std::enable_shared_from_this rclcpp::node_interfaces::NodeBaseInterface * node_base, const std::string & topic, const rosidl_message_type_support_t & type_support, - const rcl_publisher_options_t & publisher_options); + const rcl_publisher_options_t & publisher_options, + const PublisherEventCallbacks & event_callbacks, + bool use_default_callbacks); RCLCPP_PUBLIC virtual ~PublisherBase(); + /// Add event handlers for passed in event_callbacks. + RCLCPP_PUBLIC + void + bind_event_callbacks(const PublisherEventCallbacks & event_callbacks, bool use_default_callbacks); + /// Get the topic that this publisher publishes on. /** \return The topic name. */ RCLCPP_PUBLIC @@ -348,6 +355,8 @@ class PublisherBase : public std::enable_shared_from_this rmw_gid_t rmw_gid_; const rosidl_message_type_support_t type_support_; + + const PublisherEventCallbacks event_callbacks_; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 74e6b5c05d..d9e84b29f8 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -141,43 +141,14 @@ class Subscription : public SubscriptionBase type_support_handle, topic_name, options.to_rcl_subscription_options(qos), + // NOTE(methylDragon): Passing these args separately is necessary for event binding + options.event_callbacks, + options.use_default_callbacks, callback.is_serialized_message_callback()), any_callback_(callback), options_(options), message_memory_strategy_(message_memory_strategy) { - if (options_.event_callbacks.deadline_callback) { - this->add_event_handler( - options_.event_callbacks.deadline_callback, - RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED); - } - if (options_.event_callbacks.liveliness_callback) { - this->add_event_handler( - options_.event_callbacks.liveliness_callback, - RCL_SUBSCRIPTION_LIVELINESS_CHANGED); - } - if (options_.event_callbacks.incompatible_qos_callback) { - this->add_event_handler( - options_.event_callbacks.incompatible_qos_callback, - RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS); - } else if (options_.use_default_callbacks) { - // Register default callback when not specified - try { - this->add_event_handler( - [this](QOSRequestedIncompatibleQoSInfo & info) { - this->default_incompatible_qos_callback(info); - }, - RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS); - } catch (UnsupportedEventTypeException & /*exc*/) { - // pass - } - } - if (options_.event_callbacks.message_lost_callback) { - this->add_event_handler( - options_.event_callbacks.message_lost_callback, - RCL_SUBSCRIPTION_MESSAGE_LOST); - } - // Setup intra process publishing if requested. if (rclcpp::detail::resolve_use_intra_process(options_, *node_base)) { using rclcpp::detail::resolve_intra_process_buffer_type; diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index 09fabe79e5..b1aeb4eb7d 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -84,12 +84,20 @@ class SubscriptionBase : public std::enable_shared_from_this const rosidl_message_type_support_t & type_support_handle, const std::string & topic_name, const rcl_subscription_options_t & subscription_options, + const SubscriptionEventCallbacks & event_callbacks, + bool use_default_callbacks, bool is_serialized = false); /// Destructor. RCLCPP_PUBLIC virtual ~SubscriptionBase(); + /// Add event handlers for passed in event_callbacks. + RCLCPP_PUBLIC + void + bind_event_callbacks( + const SubscriptionEventCallbacks & event_callbacks, bool use_default_callbacks); + /// Get the topic that this subscription is subscribed on. RCLCPP_PUBLIC const char * @@ -570,6 +578,8 @@ class SubscriptionBase : public std::enable_shared_from_this uint64_t intra_process_subscription_id_; std::shared_ptr subscription_intra_process_; + const SubscriptionEventCallbacks event_callbacks_; + private: RCLCPP_DISABLE_COPY(SubscriptionBase) diff --git a/rclcpp/src/rclcpp/publisher_base.cpp b/rclcpp/src/rclcpp/publisher_base.cpp index 9517af6d3c..ce0540659a 100644 --- a/rclcpp/src/rclcpp/publisher_base.cpp +++ b/rclcpp/src/rclcpp/publisher_base.cpp @@ -45,11 +45,14 @@ PublisherBase::PublisherBase( rclcpp::node_interfaces::NodeBaseInterface * node_base, const std::string & topic, const rosidl_message_type_support_t & type_support, - const rcl_publisher_options_t & publisher_options) + const rcl_publisher_options_t & publisher_options, + const PublisherEventCallbacks & event_callbacks, + bool use_default_callbacks) : rcl_node_handle_(node_base->get_shared_rcl_node_handle()), intra_process_is_enabled_(false), intra_process_publisher_id_(0), - type_support_(type_support) + type_support_(type_support), + event_callbacks_(event_callbacks) { auto custom_deleter = [node_handle = this->rcl_node_handle_](rcl_publisher_t * rcl_pub) { @@ -98,6 +101,8 @@ PublisherBase::PublisherBase( rmw_reset_error(); throw std::runtime_error(msg); } + + bind_event_callbacks(event_callbacks_, use_default_callbacks); } PublisherBase::~PublisherBase() @@ -126,6 +131,38 @@ PublisherBase::get_topic_name() const return rcl_publisher_get_topic_name(publisher_handle_.get()); } +void +PublisherBase::bind_event_callbacks( + const PublisherEventCallbacks & event_callbacks, bool use_default_callbacks) +{ + if (event_callbacks.deadline_callback) { + this->add_event_handler( + event_callbacks.deadline_callback, + RCL_PUBLISHER_OFFERED_DEADLINE_MISSED); + } + if (event_callbacks.liveliness_callback) { + this->add_event_handler( + event_callbacks.liveliness_callback, + RCL_PUBLISHER_LIVELINESS_LOST); + } + if (event_callbacks.incompatible_qos_callback) { + this->add_event_handler( + event_callbacks.incompatible_qos_callback, + RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS); + } else if (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 + } + } +} + size_t PublisherBase::get_queue_size() const { diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index 871321fbe8..0225747b3a 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -39,12 +39,15 @@ SubscriptionBase::SubscriptionBase( const rosidl_message_type_support_t & type_support_handle, const std::string & topic_name, const rcl_subscription_options_t & subscription_options, + const SubscriptionEventCallbacks & event_callbacks, + bool use_default_callbacks, 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), + event_callbacks_(event_callbacks), type_support_(type_support_handle), is_serialized_(is_serialized) { @@ -80,9 +83,10 @@ SubscriptionBase::SubscriptionBase( rcl_node_get_name(rcl_node_handle), rcl_node_get_namespace(rcl_node_handle)); } - rclcpp::exceptions::throw_from_rcl_error(ret, "could not create subscription"); } + + bind_event_callbacks(event_callbacks_, use_default_callbacks); } SubscriptionBase::~SubscriptionBase() @@ -101,6 +105,43 @@ SubscriptionBase::~SubscriptionBase() ipm->remove_subscription(intra_process_subscription_id_); } +void +SubscriptionBase::bind_event_callbacks( + const SubscriptionEventCallbacks & event_callbacks, bool use_default_callbacks) +{ + if (event_callbacks.deadline_callback) { + this->add_event_handler( + event_callbacks.deadline_callback, + RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED); + } + if (event_callbacks.liveliness_callback) { + this->add_event_handler( + event_callbacks.liveliness_callback, + RCL_SUBSCRIPTION_LIVELINESS_CHANGED); + } + if (event_callbacks.incompatible_qos_callback) { + this->add_event_handler( + event_callbacks.incompatible_qos_callback, + RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS); + } else if (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 (event_callbacks.message_lost_callback) { + this->add_event_handler( + event_callbacks.message_lost_callback, + RCL_SUBSCRIPTION_MESSAGE_LOST); + } +} + const char * SubscriptionBase::get_topic_name() const { diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp index 264577a739..ecfc89a6aa 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp @@ -36,16 +36,14 @@ const rosidl_message_type_support_t EmptyTypeSupport() return *rosidl_typesupport_cpp::get_message_type_support_handle(); } -const rcl_publisher_options_t PublisherOptions() +const rclcpp::PublisherOptionsWithAllocator> PublisherOptions() { - return rclcpp::PublisherOptionsWithAllocator>().template - to_rcl_publisher_options(rclcpp::QoS(10)); + return rclcpp::PublisherOptionsWithAllocator>(); } -const rcl_subscription_options_t SubscriptionOptions() +const rclcpp::SubscriptionOptionsWithAllocator> SubscriptionOptions() { - return rclcpp::SubscriptionOptionsWithAllocator>() - .to_rcl_subscription_options(rclcpp::QoS(10)); + return rclcpp::SubscriptionOptionsWithAllocator>(); } } // namespace @@ -55,7 +53,9 @@ class TestPublisher : public rclcpp::PublisherBase public: explicit TestPublisher(rclcpp::Node * node) : rclcpp::PublisherBase( - node->get_node_base_interface().get(), "topic", EmptyTypeSupport(), PublisherOptions()) {} + node->get_node_base_interface().get(), "topic", EmptyTypeSupport(), + PublisherOptions().to_rcl_publisher_options(rclcpp::QoS(10)), + PublisherOptions().event_callbacks, PublisherOptions().use_default_callbacks) {} }; class TestSubscription : public rclcpp::SubscriptionBase @@ -63,7 +63,9 @@ class TestSubscription : public rclcpp::SubscriptionBase public: explicit TestSubscription(rclcpp::Node * node) : rclcpp::SubscriptionBase( - node->get_node_base_interface().get(), EmptyTypeSupport(), "topic", SubscriptionOptions()) {} + node->get_node_base_interface().get(), EmptyTypeSupport(), "topic", + SubscriptionOptions().to_rcl_subscription_options(rclcpp::QoS(10)), + SubscriptionOptions().event_callbacks, SubscriptionOptions().use_default_callbacks) {} std::shared_ptr create_message() override {return nullptr;} std::shared_ptr diff --git a/rclcpp/test/rclcpp/test_publisher.cpp b/rclcpp/test/rclcpp/test_publisher.cpp index 35605230d9..5cffe1d5e1 100644 --- a/rclcpp/test/rclcpp/test_publisher.cpp +++ b/rclcpp/test/rclcpp/test_publisher.cpp @@ -245,10 +245,9 @@ const rosidl_message_type_support_t EmptyTypeSupport() return *rosidl_typesupport_cpp::get_message_type_support_handle(); } -const rcl_publisher_options_t PublisherOptions() +const rclcpp::PublisherOptionsWithAllocator> PublisherOptions() { - return rclcpp::PublisherOptionsWithAllocator>().template - to_rcl_publisher_options(rclcpp::QoS(10)); + return rclcpp::PublisherOptionsWithAllocator>(); } class TestPublisherBase : public rclcpp::PublisherBase @@ -256,7 +255,9 @@ class TestPublisherBase : public rclcpp::PublisherBase public: explicit TestPublisherBase(rclcpp::Node * node) : rclcpp::PublisherBase( - node->get_node_base_interface().get(), "topic", EmptyTypeSupport(), PublisherOptions()) {} + node->get_node_base_interface().get(), "topic", EmptyTypeSupport(), + PublisherOptions().to_rcl_publisher_options(rclcpp::QoS(10)), + PublisherOptions().event_callbacks, PublisherOptions().use_default_callbacks) {} }; /* diff --git a/rclcpp/test/rclcpp/test_type_support.cpp b/rclcpp/test/rclcpp/test_type_support.cpp index 1732031029..682907e2a9 100644 --- a/rclcpp/test/rclcpp/test_type_support.cpp +++ b/rclcpp/test/rclcpp/test_type_support.cpp @@ -58,10 +58,9 @@ class TestTypeSupport : public ::testing::Test rclcpp::Node::SharedPtr node = std::make_shared("my_node", "/ns"); }; -const rcl_publisher_options_t PublisherOptions() +const rclcpp::PublisherOptionsWithAllocator> PublisherOptions() { - return rclcpp::PublisherOptionsWithAllocator>().template - to_rcl_publisher_options(rclcpp::QoS(10)); + return rclcpp::PublisherOptionsWithAllocator>(); } // Auxiliary classes used to test rosidl_message_type_support_t getters @@ -77,7 +76,8 @@ class TestTSParameterEvent : public rclcpp::PublisherBase node->get_node_base_interface().get(), "topicTSParameterEvent", *ts_parameter_event, - PublisherOptions()) {} + PublisherOptions().to_rcl_publisher_options(rclcpp::QoS(10)), + PublisherOptions().event_callbacks, PublisherOptions().use_default_callbacks) {} }; const rosidl_message_type_support_t * ts_set_parameter_result = @@ -91,7 +91,8 @@ class TestTSSetParameterResult : public rclcpp::PublisherBase node->get_node_base_interface().get(), "topicTSSetParameterResult", *ts_set_parameter_result, - PublisherOptions()) {} + PublisherOptions().to_rcl_publisher_options(rclcpp::QoS(10)), + PublisherOptions().event_callbacks, PublisherOptions().use_default_callbacks) {} }; const rosidl_message_type_support_t * ts_parameter_descriptor = @@ -105,7 +106,8 @@ class TestTSParameterDescriptor : public rclcpp::PublisherBase node->get_node_base_interface().get(), "topicTSParameterDescriptor", *ts_parameter_descriptor, - PublisherOptions()) {} + PublisherOptions().to_rcl_publisher_options(rclcpp::QoS(10)), + PublisherOptions().event_callbacks, PublisherOptions().use_default_callbacks) {} }; const rosidl_message_type_support_t * ts_list_parameter_result = @@ -119,7 +121,8 @@ class TestTSListParametersResult : public rclcpp::PublisherBase node->get_node_base_interface().get(), "topicTSListParametersResult", *ts_list_parameter_result, - PublisherOptions()) {} + PublisherOptions().to_rcl_publisher_options(rclcpp::QoS(10)), + PublisherOptions().event_callbacks, PublisherOptions().use_default_callbacks) {} }; /* From 3fb012e2e979475f5044ab0e0f9b91d336db5f46 Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Wed, 21 Dec 2022 10:11:24 +0000 Subject: [PATCH 46/53] do not throw exception if trying to dequeue an empty intra-process buffer (#2061) Signed-off-by: Alberto Soragna Signed-off-by: Alberto Soragna --- .../experimental/buffers/ring_buffer_implementation.hpp | 3 +-- .../rclcpp/experimental/subscription_intra_process.hpp | 8 +++++++- 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp b/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp index c01240b429..245d417d86 100644 --- a/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp +++ b/rclcpp/include/rclcpp/experimental/buffers/ring_buffer_implementation.hpp @@ -86,8 +86,7 @@ class RingBufferImplementation : public BufferImplementationBase std::lock_guard lock(mutex_); if (!has_data_()) { - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Calling dequeue on empty intra-process buffer"); - throw std::runtime_error("Calling dequeue on empty intra-process buffer"); + return BufferT(); } auto request = std::move(ring_buffer_[read_index_]); diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp index 803d940086..91ea91a7c3 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp @@ -109,8 +109,14 @@ class SubscriptionIntraProcess if (any_callback_.use_take_shared_method()) { shared_msg = this->buffer_->consume_shared(); + if (!shared_msg) { + return nullptr; + } } else { unique_msg = this->buffer_->consume_unique(); + if (!unique_msg) { + return nullptr; + } } return std::static_pointer_cast( std::make_shared>( @@ -138,7 +144,7 @@ class SubscriptionIntraProcess execute_impl(std::shared_ptr & data) { if (!data) { - throw std::runtime_error("'data' is empty"); + return; } rmw_message_info_t msg_info; From c5491a4e58a6d4ee41ee52b2feaeb67182da8a4a Mon Sep 17 00:00:00 2001 From: jrutgeer <74099146+jrutgeer@users.noreply.github.com> Date: Fri, 23 Dec 2022 14:27:42 +0100 Subject: [PATCH 47/53] API url info fix (#2071) * Fixed API url info. Signed-off-by: jrutgeer * Fix api url info in README Signed-off-by: jrutgeer Signed-off-by: jrutgeer --- README.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 0226a75f0a..f4fe5837aa 100644 --- a/README.md +++ b/README.md @@ -8,7 +8,8 @@ rclcpp provides the standard C++ API for interacting with ROS 2. `#include "rclcpp/rclcpp.hpp"` allows use of the most common elements of the ROS 2 system. -Visit the [rclcpp API documentation](http://docs.ros2.org/latest/api/rclcpp/) for a complete list of its main components. +The link to the latest API documentation can be found on the rclcpp package info page, at the [ROS Index](https://index.ros.org/p/rclcpp/). + ### Examples From a73e0bd23b17c9dd20395586bf9872fa189c4292 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Wed, 28 Dec 2022 17:59:02 -0800 Subject: [PATCH 48/53] Implement Unified Node Interface (NodeInterfaces class) (#2041) Co-authored-by: William Woodall Co-authored-by: methylDragon --- .../rclcpp/detail/template_contains.hpp | 47 ++++ .../include/rclcpp/detail/template_unique.hpp | 49 ++++ .../detail/node_interfaces_helpers.hpp | 204 +++++++++++++++ .../node_interfaces/node_base_interface.hpp | 3 + .../node_interfaces/node_clock_interface.hpp | 3 + .../node_interfaces/node_graph_interface.hpp | 3 + .../node_interfaces/node_interfaces.hpp | 165 ++++++++++++ .../node_logging_interface.hpp | 3 + .../node_parameters_interface.hpp | 3 + .../node_services_interface.hpp | 3 + .../node_time_source_interface.hpp | 3 + .../node_interfaces/node_timers_interface.hpp | 3 + .../node_interfaces/node_topics_interface.hpp | 3 + .../node_waitables_interface.hpp | 3 + rclcpp/test/rclcpp/CMakeLists.txt | 10 + .../detail/test_template_utils.cpp | 56 ++++ .../node_interfaces/test_node_interfaces.cpp | 245 ++++++++++++++++++ .../lifecycle_node_interface.hpp | 5 + 18 files changed, 811 insertions(+) create mode 100644 rclcpp/include/rclcpp/detail/template_contains.hpp create mode 100644 rclcpp/include/rclcpp/detail/template_unique.hpp create mode 100644 rclcpp/include/rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp create mode 100644 rclcpp/include/rclcpp/node_interfaces/node_interfaces.hpp create mode 100644 rclcpp/test/rclcpp/node_interfaces/detail/test_template_utils.cpp create mode 100644 rclcpp/test/rclcpp/node_interfaces/test_node_interfaces.cpp diff --git a/rclcpp/include/rclcpp/detail/template_contains.hpp b/rclcpp/include/rclcpp/detail/template_contains.hpp new file mode 100644 index 0000000000..b60a75f36d --- /dev/null +++ b/rclcpp/include/rclcpp/detail/template_contains.hpp @@ -0,0 +1,47 @@ +// Copyright 2022 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__DETAIL__TEMPLATE_CONTAINS_HPP_ +#define RCLCPP__DETAIL__TEMPLATE_CONTAINS_HPP_ + +#include + +namespace rclcpp +{ +namespace detail +{ + +/// Template meta-function that checks if a given T is contained in the list Us. +template +struct template_contains; + +template +inline constexpr bool template_contains_v = template_contains::value; + +template +struct template_contains +{ + enum { value = (std::is_same_v|| template_contains_v)}; +}; + +template +struct template_contains +{ + enum { value = false }; +}; + +} // namespace detail +} // namespace rclcpp + +#endif // RCLCPP__DETAIL__TEMPLATE_CONTAINS_HPP_ diff --git a/rclcpp/include/rclcpp/detail/template_unique.hpp b/rclcpp/include/rclcpp/detail/template_unique.hpp new file mode 100644 index 0000000000..4986102d78 --- /dev/null +++ b/rclcpp/include/rclcpp/detail/template_unique.hpp @@ -0,0 +1,49 @@ +// Copyright 2022 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__DETAIL__TEMPLATE_UNIQUE_HPP_ +#define RCLCPP__DETAIL__TEMPLATE_UNIQUE_HPP_ + +#include + +#include "rclcpp/detail/template_contains.hpp" + +namespace rclcpp +{ +namespace detail +{ + +/// Template meta-function that checks if a given list Ts contains unique types. +template +struct template_unique; + +template +inline constexpr bool template_unique_v = template_unique::value; + +template +struct template_unique +{ + enum { value = !template_contains_v&& template_unique_v}; +}; + +template +struct template_unique +{ + enum { value = true }; +}; + +} // namespace detail +} // namespace rclcpp + +#endif // RCLCPP__DETAIL__TEMPLATE_UNIQUE_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp b/rclcpp/include/rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp new file mode 100644 index 0000000000..8f7b452f5f --- /dev/null +++ b/rclcpp/include/rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp @@ -0,0 +1,204 @@ +// Copyright 2022 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__NODE_INTERFACES__DETAIL__NODE_INTERFACES_HELPERS_HPP_ +#define RCLCPP__NODE_INTERFACES__DETAIL__NODE_INTERFACES_HELPERS_HPP_ + +#include +#include +#include +#include + +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ +namespace node_interfaces +{ +namespace detail +{ + +// Support and Helper template classes for the NodeInterfaces class. + +template +std::tuple...> +init_tuple(NodeT & n); + +/// Stores the interfaces in a tuple, provides constructors, and getters. +template +struct NodeInterfacesStorage +{ + template + NodeInterfacesStorage(NodeT & node) // NOLINT(runtime/explicit) + : interfaces_(init_tuple(node)) + {} + + explicit NodeInterfacesStorage(std::shared_ptr... args) + : interfaces_(args ...) + {} + + /// Individual Node Interface non-const getter. + template + std::shared_ptr + get() + { + static_assert( + (std::is_same_v|| ...), + "NodeInterfaces class does not contain given NodeInterfaceT"); + return std::get>(interfaces_); + } + + /// Individual Node Interface const getter. + template + std::shared_ptr + get() const + { + static_assert( + (std::is_same_v|| ...), + "NodeInterfaces class does not contain given NodeInterfaceT"); + return std::get>(interfaces_); + } + +protected: + std::tuple...> interfaces_; +}; + +/// Prototype of NodeInterfacesSupports. +/** + * Should read NodeInterfacesSupports<..., T, ...> as "NodeInterfaces supports T", and + * if NodeInterfacesSupport is specialized for T, the is_supported should be + * set to std::true_type, but by default it is std::false_type, which will + * lead to a compiler error when trying to use T with NodeInterfaces. + */ +template +struct NodeInterfacesSupports; + +/// Prototype of NodeInterfacesSupportCheck template meta-function. +/** + * This meta-function checks that all the types given are supported, + * throwing a more human-readable error if an unsupported type is used. + */ +template +struct NodeInterfacesSupportCheck; + +/// Iterating specialization that ensures classes are supported and inherited. +template +struct NodeInterfacesSupportCheck + : public NodeInterfacesSupportCheck +{ + static_assert( + NodeInterfacesSupports::is_supported::value, + "given NodeInterfaceT is not supported by rclcpp::node_interfaces::NodeInterfaces"); +}; + +/// Terminating case when there are no more "RemainingInterfaceTs". +template +struct NodeInterfacesSupportCheck +{}; + +/// Default specialization, needs to be specialized for each supported interface. +template +struct NodeInterfacesSupports +{ + // Specializations need to set this to std::true_type in addition to other interfaces. + using is_supported = std::false_type; +}; + +/// Terminating specialization of NodeInterfacesSupports. +template +struct NodeInterfacesSupports + : public StorageClassT +{ + /// Perfect forwarding constructor to get arguments down to StorageClassT. + template + explicit NodeInterfacesSupports(ArgsT && ... args) + : StorageClassT(std::forward(args) ...) + {} +}; + +// Helper functions to initialize the tuple in NodeInterfaces. + +template +void +init_element(TupleT & t, NodeT & n) +{ + std::get>(t) = + NodeInterfacesSupports::get_from_node_like(n); +} + +template +std::tuple...> +init_tuple(NodeT & n) +{ + using StorageClassT = NodeInterfacesStorage; + std::tuple...> t; + (init_element(t, n), ...); + return t; +} + +/// Macro for creating specializations with less boilerplate. +/** + * You can use this macro to add support for your interface class if: + * + * - The standard getter is get_node_{NodeInterfaceName}_interface(), and + * - the getter returns a non-const shared_ptr<{NodeInterfaceType}> + * + * Examples of using this can be seen in the standard node interface headers + * in rclcpp, e.g. rclcpp/node_interfaces/node_base_interface.hpp has: + * + * RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeBaseInterface, base) + * + * If your interface has a non-standard getter, or you want to instrument it or + * something like that, then you'll need to create your own specialization of + * the NodeInterfacesSupports struct without this macro. + */ +#define RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(NodeInterfaceType, NodeInterfaceName) \ + namespace rclcpp::node_interfaces::detail { \ + template \ + struct NodeInterfacesSupports< \ + StorageClassT, \ + NodeInterfaceType, \ + RemainingInterfaceTs ...> \ + : public NodeInterfacesSupports \ + { \ + using is_supported = std::true_type; \ + \ + template \ + static \ + std::shared_ptr \ + get_from_node_like(NodeT & node_like) \ + { \ + return node_like.get_node_ ## NodeInterfaceName ## _interface(); \ + } \ + \ + /* Perfect forwarding constructor to get arguments down to StorageClassT (eventually). */ \ + template \ + explicit NodeInterfacesSupports(ArgsT && ... args) \ + : NodeInterfacesSupports( \ + std::forward(args) ...) \ + {} \ + \ + std::shared_ptr \ + get_node_ ## NodeInterfaceName ## _interface() \ + { \ + return StorageClassT::template get(); \ + } \ + }; \ + } // namespace rclcpp::node_interfaces::detail + +} // namespace detail +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__DETAIL__NODE_INTERFACES_HELPERS_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp index 2e94b80d9c..fd4f64b22b 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp @@ -26,6 +26,7 @@ #include "rclcpp/context.hpp" #include "rclcpp/guard_condition.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp" #include "rclcpp/visibility_control.hpp" namespace rclcpp @@ -177,4 +178,6 @@ class NodeBaseInterface } // namespace node_interfaces } // namespace rclcpp +RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeBaseInterface, base) + #endif // RCLCPP__NODE_INTERFACES__NODE_BASE_INTERFACE_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_clock_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_clock_interface.hpp index 8965a371d8..744eef4ce6 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_clock_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_clock_interface.hpp @@ -17,6 +17,7 @@ #include "rclcpp/clock.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp" #include "rclcpp/visibility_control.hpp" namespace rclcpp @@ -50,4 +51,6 @@ class NodeClockInterface } // namespace node_interfaces } // namespace rclcpp +RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeClockInterface, clock) + #endif // RCLCPP__NODE_INTERFACES__NODE_CLOCK_INTERFACE_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp index 3958f5b2d9..fefda0da69 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_graph_interface.hpp @@ -29,6 +29,7 @@ #include "rclcpp/event.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/visibility_control.hpp" @@ -382,4 +383,6 @@ class NodeGraphInterface } // namespace node_interfaces } // namespace rclcpp +RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeGraphInterface, graph) + #endif // RCLCPP__NODE_INTERFACES__NODE_GRAPH_INTERFACE_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_interfaces.hpp b/rclcpp/include/rclcpp/node_interfaces/node_interfaces.hpp new file mode 100644 index 0000000000..2f40380c75 --- /dev/null +++ b/rclcpp/include/rclcpp/node_interfaces/node_interfaces.hpp @@ -0,0 +1,165 @@ +// Copyright 2022 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__NODE_INTERFACES__NODE_INTERFACES_HPP_ +#define RCLCPP__NODE_INTERFACES__NODE_INTERFACES_HPP_ + +#include + +#include "rclcpp/detail/template_unique.hpp" +#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp" + +#define ALL_RCLCPP_NODE_INTERFACES \ + rclcpp::node_interfaces::NodeBaseInterface, \ + rclcpp::node_interfaces::NodeClockInterface, \ + rclcpp::node_interfaces::NodeGraphInterface, \ + rclcpp::node_interfaces::NodeLoggingInterface, \ + rclcpp::node_interfaces::NodeParametersInterface, \ + rclcpp::node_interfaces::NodeServicesInterface, \ + rclcpp::node_interfaces::NodeTimeSourceInterface, \ + rclcpp::node_interfaces::NodeTimersInterface, \ + rclcpp::node_interfaces::NodeTopicsInterface, \ + rclcpp::node_interfaces::NodeWaitablesInterface + + +namespace rclcpp +{ +namespace node_interfaces +{ + + +/// A helper class for aggregating node interfaces. +template +class NodeInterfaces + : public detail::NodeInterfacesSupportCheck< + detail::NodeInterfacesStorage, + InterfaceTs ... + >, + public detail::NodeInterfacesSupports< + detail::NodeInterfacesStorage, + InterfaceTs ... + > +{ + static_assert( + 0 != sizeof ...(InterfaceTs), + "must provide at least one interface as a template argument"); + static_assert( + rclcpp::detail::template_unique_v, + "must provide unique template parameters"); + + using NodeInterfacesSupportsT = detail::NodeInterfacesSupports< + detail::NodeInterfacesStorage, + InterfaceTs ... + >; + +public: + /// Create a new NodeInterfaces object using the given node-like object's interfaces. + /** + * Specify which interfaces you need by passing them as template parameters. + * + * This allows you to aggregate interfaces from different sources together to pass as a single + * aggregate object to any functions that take node interfaces or node-likes, without needing to + * templatize that function. + * + * You may also use this constructor to create a NodeInterfaces that contains a subset of + * another NodeInterfaces' interfaces. + * + * Finally, this class supports implicit conversion from node-like objects, allowing you to + * directly pass a node-like to a function that takes a NodeInterfaces object. + * + * Usage examples: + * ```cpp + * // Suppose we have some function: + * void fn(NodeInterfaces interfaces); + * + * // Then we can, explicitly: + * rclcpp::Node node("some_node"); + * auto ni = NodeInterfaces(node); + * fn(ni); + * + * // But also: + * fn(node); + * + * // Subsetting a NodeInterfaces object also works! + * auto ni_base = NodeInterfaces(ni); + * + * // Or aggregate them (you could aggregate interfaces from disparate node-likes) + * auto ni_aggregated = NodeInterfaces( + * node->get_node_base_interface(), + * node->get_node_clock_interface() + * ) + * + * // And then to access the interfaces: + * // Get with get<> + * auto base = ni.get(); + * + * // Or the appropriate getter + * auto clock = ni.get_clock_interface(); + * ``` + * + * You may use any of the standard node interfaces that come with rclcpp: + * - rclcpp::node_interfaces::NodeBaseInterface + * - rclcpp::node_interfaces::NodeClockInterface + * - rclcpp::node_interfaces::NodeGraphInterface + * - rclcpp::node_interfaces::NodeLoggingInterface + * - rclcpp::node_interfaces::NodeParametersInterface + * - rclcpp::node_interfaces::NodeServicesInterface + * - rclcpp::node_interfaces::NodeTimeSourceInterface + * - rclcpp::node_interfaces::NodeTimersInterface + * - rclcpp::node_interfaces::NodeTopicsInterface + * - rclcpp::node_interfaces::NodeWaitablesInterface + * + * Or you use custom interfaces as long as you make a template specialization + * of the rclcpp::node_interfaces::detail::NodeInterfacesSupport struct using + * the RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT macro. + * + * Usage example: + * ```RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeBaseInterface, base)``` + * + * If you choose not to use the helper macro, then you can specialize the + * template yourself, but you must: + * + * - Provide a template specialization of the get_from_node_like method that gets the interface + * from any node-like that stores the interface, using the node-like's getter + * - Designate the is_supported type as std::true_type using a using directive + * - Provide any number of getter methods to be used to obtain the interface with the + * NodeInterface object, noting that the getters of the storage class will apply to all + * supported interfaces. + * - The getter method names should not clash in name with any other interface getter + * specializations if those other interfaces are meant to be aggregated in the same + * NodeInterfaces object. + * + * \param[in] node Node-like object from which to get the node interfaces + */ + template + NodeInterfaces(NodeT & node) // NOLINT(runtime/explicit) + : NodeInterfacesSupportsT(node) + {} + + /// NodeT::SharedPtr Constructor + template + NodeInterfaces(std::shared_ptr node) // NOLINT(runtime/explicit) + : NodeInterfaces(node ? *node : throw std::invalid_argument("given node pointer is nullptr")) + {} + + explicit NodeInterfaces(std::shared_ptr... args) + : NodeInterfacesSupportsT(args ...) + {} +}; + + +} // namespace node_interfaces +} // namespace rclcpp + +#endif // RCLCPP__NODE_INTERFACES__NODE_INTERFACES_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_logging_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_logging_interface.hpp index c2add566b9..870a81a53b 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_logging_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_logging_interface.hpp @@ -19,6 +19,7 @@ #include "rclcpp/logger.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp" #include "rclcpp/visibility_control.hpp" namespace rclcpp @@ -58,4 +59,6 @@ class NodeLoggingInterface } // namespace node_interfaces } // namespace rclcpp +RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeLoggingInterface, logging) + #endif // RCLCPP__NODE_INTERFACES__NODE_LOGGING_INTERFACE_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp index 3156668a73..301c1e3214 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_parameters_interface.hpp @@ -25,6 +25,7 @@ #include "rcl_interfaces/msg/set_parameters_result.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp" #include "rclcpp/parameter.hpp" #include "rclcpp/visibility_control.hpp" @@ -276,4 +277,6 @@ class NodeParametersInterface } // namespace node_interfaces } // namespace rclcpp +RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeParametersInterface, parameters) + #endif // RCLCPP__NODE_INTERFACES__NODE_PARAMETERS_INTERFACE_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp index ab6cdab42e..ff58ef36b6 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_services_interface.hpp @@ -20,6 +20,7 @@ #include "rclcpp/callback_group.hpp" #include "rclcpp/client.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp" #include "rclcpp/service.hpp" #include "rclcpp/visibility_control.hpp" @@ -62,4 +63,6 @@ class NodeServicesInterface } // namespace node_interfaces } // namespace rclcpp +RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeServicesInterface, services) + #endif // RCLCPP__NODE_INTERFACES__NODE_SERVICES_INTERFACE_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_time_source_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_time_source_interface.hpp index 5b7065193e..3783e5d83a 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_time_source_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_time_source_interface.hpp @@ -16,6 +16,7 @@ #define RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_INTERFACE_HPP_ #include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp" #include "rclcpp/visibility_control.hpp" namespace rclcpp @@ -37,4 +38,6 @@ class NodeTimeSourceInterface } // namespace node_interfaces } // namespace rclcpp +RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeTimeSourceInterface, time_source) + #endif // RCLCPP__NODE_INTERFACES__NODE_TIME_SOURCE_INTERFACE_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_timers_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_timers_interface.hpp index 4573d3d02b..2f1aaef51e 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_timers_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_timers_interface.hpp @@ -17,6 +17,7 @@ #include "rclcpp/callback_group.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp" #include "rclcpp/timer.hpp" #include "rclcpp/visibility_control.hpp" @@ -47,4 +48,6 @@ class NodeTimersInterface } // namespace node_interfaces } // namespace rclcpp +RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeTimersInterface, timers) + #endif // RCLCPP__NODE_INTERFACES__NODE_TIMERS_INTERFACE_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp index 7aa40a3d5e..aa5530f8dd 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_topics_interface.hpp @@ -22,6 +22,7 @@ #include "rclcpp/callback_group.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/node_interfaces/node_timers_interface.hpp" #include "rclcpp/publisher.hpp" @@ -95,4 +96,6 @@ class NodeTopicsInterface } // namespace node_interfaces } // namespace rclcpp +RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeTopicsInterface, topics) + #endif // RCLCPP__NODE_INTERFACES__NODE_TOPICS_INTERFACE_HPP_ diff --git a/rclcpp/include/rclcpp/node_interfaces/node_waitables_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_waitables_interface.hpp index 0faae1da62..fd0029a89b 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_waitables_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_waitables_interface.hpp @@ -17,6 +17,7 @@ #include "rclcpp/callback_group.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp" #include "rclcpp/visibility_control.hpp" #include "rclcpp/waitable.hpp" @@ -54,4 +55,6 @@ class NodeWaitablesInterface } // namespace node_interfaces } // namespace rclcpp +RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT(rclcpp::node_interfaces::NodeWaitablesInterface, waitables) + #endif // RCLCPP__NODE_INTERFACES__NODE_WAITABLES_INTERFACE_HPP_ diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index 75aa9ac63b..6dffb543e9 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -234,6 +234,11 @@ if(TARGET test_node_interfaces__node_graph) "test_msgs") target_link_libraries(test_node_interfaces__node_graph ${PROJECT_NAME} mimick) endif() +ament_add_gtest(test_node_interfaces__node_interfaces + node_interfaces/test_node_interfaces.cpp) +if(TARGET test_node_interfaces__node_interfaces) + target_link_libraries(test_node_interfaces__node_interfaces ${PROJECT_NAME} mimick) +endif() ament_add_gtest(test_node_interfaces__node_parameters node_interfaces/test_node_parameters.cpp) if(TARGET test_node_interfaces__node_parameters) @@ -262,6 +267,11 @@ ament_add_gtest(test_node_interfaces__node_waitables if(TARGET test_node_interfaces__node_waitables) target_link_libraries(test_node_interfaces__node_waitables ${PROJECT_NAME} mimick) endif() +ament_add_gtest(test_node_interfaces__test_template_utils # Compile time test + node_interfaces/detail/test_template_utils.cpp) +if(TARGET test_node_interfaces__test_template_utils) + target_link_libraries(test_node_interfaces__test_template_utils ${PROJECT_NAME}) +endif() # TODO(wjwwood): reenable these build failure tests when I can get Jenkins to ignore their output # rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ref_rclcpp_node diff --git a/rclcpp/test/rclcpp/node_interfaces/detail/test_template_utils.cpp b/rclcpp/test/rclcpp/node_interfaces/detail/test_template_utils.cpp new file mode 100644 index 0000000000..9ae715ebce --- /dev/null +++ b/rclcpp/test/rclcpp/node_interfaces/detail/test_template_utils.cpp @@ -0,0 +1,56 @@ +// Copyright 2022 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "rclcpp/detail/template_contains.hpp" +#include "rclcpp/detail/template_unique.hpp" + + +TEST(NoOpTests, test_node_interfaces_template_utils) { +} // This is just to let gtest work + +namespace rclcpp +{ +namespace detail +{ + +// This tests template logic at compile time +namespace +{ + +struct test_template_unique +{ + static_assert(template_unique_v, "failed"); + static_assert(template_unique_v, "failed"); + static_assert(!template_unique_v, "failed"); + static_assert(!template_unique_v, "failed"); + static_assert(!template_unique_v, "failed"); +}; + + +struct test_template_contains +{ + static_assert(template_contains_v, "failed"); + static_assert(template_contains_v, "failed"); + static_assert(template_contains_v, "failed"); + static_assert(!template_contains_v, "failed"); + static_assert(!template_contains_v, "failed"); + static_assert(!template_contains_v, "failed"); +}; + +} // namespace + +} // namespace detail +} // namespace rclcpp diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_interfaces.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_interfaces.cpp new file mode 100644 index 0000000000..96596dea37 --- /dev/null +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_interfaces.cpp @@ -0,0 +1,245 @@ +// Copyright 2022 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "rclcpp/node.hpp" +#include "rclcpp/node_interfaces/node_interfaces.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_graph_interface.hpp" + +class TestNodeInterfaces : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } +}; + +/* + Testing NodeInterfaces construction from nodes. + */ +TEST_F(TestNodeInterfaces, node_interfaces_nominal) { + auto node = std::make_shared("my_node"); + + // Create a NodeInterfaces for base and graph using a rclcpp::Node. + { + using rclcpp::node_interfaces::NodeInterfaces; + using rclcpp::node_interfaces::NodeBaseInterface; + using rclcpp::node_interfaces::NodeGraphInterface; + auto node_interfaces = NodeInterfaces(node); + } + + // Implicit conversion of rclcpp::Node into function that uses NodeInterfaces of base. + { + using rclcpp::node_interfaces::NodeInterfaces; + using rclcpp::node_interfaces::NodeBaseInterface; + auto some_func = [](NodeInterfaces ni) { + auto base_interface = ni.get(); + }; + + some_func(node); + } + + // Implicit narrowing of NodeInterfaces into a new interface NodeInterfaces with fewer interfaces. + { + using rclcpp::node_interfaces::NodeInterfaces; + using rclcpp::node_interfaces::NodeBaseInterface; + using rclcpp::node_interfaces::NodeGraphInterface; + auto some_func = [](NodeInterfaces ni_with_one) { + auto base_interface = ni_with_one.get(); + }; + + NodeInterfaces ni_with_two(node); + + some_func(ni_with_two); + } + + // Create a NodeInterfaces via aggregation of interfaces in constructor. + { + using rclcpp::node_interfaces::NodeInterfaces; + using rclcpp::node_interfaces::NodeBaseInterface; + using rclcpp::node_interfaces::NodeGraphInterface; + auto loose_node_base = node->get_node_base_interface(); + auto loose_node_graph = node->get_node_graph_interface(); + auto ni = NodeInterfaces( + loose_node_base, + loose_node_graph); + } +} + +/* + Test construction with all standard rclcpp::node_interfaces::Node*Interfaces. + */ +TEST_F(TestNodeInterfaces, node_interfaces_standard_interfaces) { + auto node = std::make_shared("my_node", "/ns"); + + auto ni = rclcpp::node_interfaces::NodeInterfaces< + rclcpp::node_interfaces::NodeBaseInterface, + rclcpp::node_interfaces::NodeClockInterface, + rclcpp::node_interfaces::NodeGraphInterface, + rclcpp::node_interfaces::NodeLoggingInterface, + rclcpp::node_interfaces::NodeTimersInterface, + rclcpp::node_interfaces::NodeTopicsInterface, + rclcpp::node_interfaces::NodeServicesInterface, + rclcpp::node_interfaces::NodeWaitablesInterface, + rclcpp::node_interfaces::NodeParametersInterface, + rclcpp::node_interfaces::NodeTimeSourceInterface + >(node); +} + +/* + Testing getters. + */ +TEST_F(TestNodeInterfaces, ni_init) { + auto node = std::make_shared("my_node", "/ns"); + + using rclcpp::node_interfaces::NodeInterfaces; + using rclcpp::node_interfaces::NodeBaseInterface; + using rclcpp::node_interfaces::NodeClockInterface; + using rclcpp::node_interfaces::NodeGraphInterface; + using rclcpp::node_interfaces::NodeLoggingInterface; + using rclcpp::node_interfaces::NodeTimersInterface; + using rclcpp::node_interfaces::NodeTopicsInterface; + using rclcpp::node_interfaces::NodeServicesInterface; + using rclcpp::node_interfaces::NodeWaitablesInterface; + using rclcpp::node_interfaces::NodeParametersInterface; + using rclcpp::node_interfaces::NodeTimeSourceInterface; + + auto ni = NodeInterfaces< + NodeBaseInterface, + NodeClockInterface, + NodeGraphInterface, + NodeLoggingInterface, + NodeTimersInterface, + NodeTopicsInterface, + NodeServicesInterface, + NodeWaitablesInterface, + NodeParametersInterface, + NodeTimeSourceInterface + >(node); + + { + auto base = ni.get(); + base = ni.get_node_base_interface(); + EXPECT_STREQ(base->get_name(), "my_node"); // Test for functionality + } + { + auto clock = ni.get(); + clock = ni.get_node_clock_interface(); + clock->get_clock(); + } + { + auto graph = ni.get(); + graph = ni.get_node_graph_interface(); + } + { + auto logging = ni.get(); + logging = ni.get_node_logging_interface(); + } + { + auto timers = ni.get(); + timers = ni.get_node_timers_interface(); + } + { + auto topics = ni.get(); + topics = ni.get_node_topics_interface(); + } + { + auto services = ni.get(); + services = ni.get_node_services_interface(); + } + { + auto waitables = ni.get(); + waitables = ni.get_node_waitables_interface(); + } + { + auto parameters = ni.get(); + parameters = ni.get_node_parameters_interface(); + } + { + auto time_source = ni.get(); + time_source = ni.get_node_time_source_interface(); + } +} + +/* + Testing macro'ed getters. + */ +TEST_F(TestNodeInterfaces, ni_all_init) { + auto node = std::make_shared("my_node", "/ns"); + + using rclcpp::node_interfaces::NodeInterfaces; + using rclcpp::node_interfaces::NodeBaseInterface; + using rclcpp::node_interfaces::NodeClockInterface; + using rclcpp::node_interfaces::NodeGraphInterface; + using rclcpp::node_interfaces::NodeLoggingInterface; + using rclcpp::node_interfaces::NodeTimersInterface; + using rclcpp::node_interfaces::NodeTopicsInterface; + using rclcpp::node_interfaces::NodeServicesInterface; + using rclcpp::node_interfaces::NodeWaitablesInterface; + using rclcpp::node_interfaces::NodeParametersInterface; + using rclcpp::node_interfaces::NodeTimeSourceInterface; + + auto ni = rclcpp::node_interfaces::NodeInterfaces(node); + + { + auto base = ni.get(); + base = ni.get_node_base_interface(); + EXPECT_STREQ(base->get_name(), "my_node"); // Test for functionality + } + { + auto clock = ni.get(); + clock = ni.get_node_clock_interface(); + clock->get_clock(); + } + { + auto graph = ni.get(); + graph = ni.get_node_graph_interface(); + } + { + auto logging = ni.get(); + logging = ni.get_node_logging_interface(); + } + { + auto timers = ni.get(); + timers = ni.get_node_timers_interface(); + } + { + auto topics = ni.get(); + topics = ni.get_node_topics_interface(); + } + { + auto services = ni.get(); + services = ni.get_node_services_interface(); + } + { + auto waitables = ni.get(); + waitables = ni.get_node_waitables_interface(); + } + { + auto parameters = ni.get(); + parameters = ni.get_node_parameters_interface(); + } + { + auto time_source = ni.get(); + time_source = ni.get_node_time_source_interface(); + } +} diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp index ed919ca556..272c4def27 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp @@ -19,6 +19,7 @@ #include "rclcpp_lifecycle/state.hpp" #include "rclcpp_lifecycle/visibility_control.h" +#include "rclcpp/node_interfaces/detail/node_interfaces_helpers.hpp" namespace rclcpp_lifecycle { @@ -106,4 +107,8 @@ class LifecycleNodeInterface } // namespace node_interfaces } // namespace rclcpp_lifecycle + +RCLCPP_NODE_INTERFACE_HELPERS_SUPPORT( + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface, lifecycle_node) + #endif // RCLCPP_LIFECYCLE__NODE_INTERFACES__LIFECYCLE_NODE_INTERFACE_HPP_ From c63f9eae0f16504512829917d1c53d515a592485 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Thu, 29 Dec 2022 10:30:32 -0800 Subject: [PATCH 49/53] 18.0.0 Signed-off-by: Shane Loretz --- rclcpp/CHANGELOG.rst | 18 ++++++++++++++++++ rclcpp/package.xml | 2 +- rclcpp_action/CHANGELOG.rst | 6 ++++++ rclcpp_action/package.xml | 2 +- rclcpp_components/CHANGELOG.rst | 5 +++++ rclcpp_components/package.xml | 2 +- rclcpp_lifecycle/CHANGELOG.rst | 7 +++++++ rclcpp_lifecycle/package.xml | 2 +- 8 files changed, 40 insertions(+), 4 deletions(-) diff --git a/rclcpp/CHANGELOG.rst b/rclcpp/CHANGELOG.rst index 15a1aede88..0c8201217c 100644 --- a/rclcpp/CHANGELOG.rst +++ b/rclcpp/CHANGELOG.rst @@ -2,6 +2,24 @@ Changelog for package rclcpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +18.0.0 (2022-12-29) +------------------- +* Implement Unified Node Interface (NodeInterfaces class) (`#2041 `_) +* Do not throw exception if trying to dequeue an empty intra-process buffer (`#2061 `_) +* Move event callback binding to PublisherBase and SubscriptionBase (`#2066 `_) +* Implement validity checks for rclcpp::Clock (`#2040 `_) +* Explicitly set callback type (`#2059 `_) +* Fix logging macros to build with msvc and cpp20 (`#2063 `_) +* Add clock type to node_options (`#1982 `_) +* Fix nullptr dereference in prune_requests_older_than (`#2008 `_) +* Remove templating on to_rcl_subscription_options (`#2056 `_) +* Fix SharedFuture from async_send_request never becoming valid (`#2044 `_) +* Add in a warning for a KeepLast depth of 0. (`#2048 `_) +* Mark rclcpp::Clock::now() as const (`#2050 `_) +* Fix a case that did not throw ParameterUninitializedException (`#2036 `_) +* Update maintainers (`#2043 `_) +* Contributors: Alberto Soragna, Audrow Nash, Chen Lihui, Chris Lalancette, Jeffery Hsu, Lei Liu, Mateusz Szczygielski, Shane Loretz, andrei, mauropasse, methylDragon + 17.1.0 (2022-11-02) ------------------- * MultiThreadExecutor number of threads is at least 2+ in default. (`#2032 `_) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 6775f4a46d..a18cd7ae1d 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -2,7 +2,7 @@ rclcpp - 17.1.0 + 18.0.0 The ROS client library in C++. Ivan Paunovic diff --git a/rclcpp_action/CHANGELOG.rst b/rclcpp_action/CHANGELOG.rst index a063bf32f2..2a08ebe63f 100644 --- a/rclcpp_action/CHANGELOG.rst +++ b/rclcpp_action/CHANGELOG.rst @@ -3,6 +3,12 @@ Changelog for package rclcpp_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +18.0.0 (2022-12-29) +------------------- +* Explicitly set callback type (`#2059 `_) +* Update maintainers (`#2043 `_) +* Contributors: Audrow Nash, mauropasse + 17.1.0 (2022-11-02) ------------------- * Do not clear entities callbacks on destruction (`#2002 `_) diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index 38e7fdcba1..375e978be1 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -2,7 +2,7 @@ rclcpp_action - 17.1.0 + 18.0.0 Adds action APIs for C++. Ivan Paunovic diff --git a/rclcpp_components/CHANGELOG.rst b/rclcpp_components/CHANGELOG.rst index bc08ed28cd..820be8b817 100644 --- a/rclcpp_components/CHANGELOG.rst +++ b/rclcpp_components/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package rclcpp_components ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +18.0.0 (2022-12-29) +------------------- +* Update maintainers (`#2043 `_) +* Contributors: Audrow Nash + 17.1.0 (2022-11-02) ------------------- * use unique ptr and remove unuseful container (`#2013 `_) diff --git a/rclcpp_components/package.xml b/rclcpp_components/package.xml index cfb88b9e3f..de1b88e242 100644 --- a/rclcpp_components/package.xml +++ b/rclcpp_components/package.xml @@ -2,7 +2,7 @@ rclcpp_components - 17.1.0 + 18.0.0 Package containing tools for dynamically loadable components Ivan Paunovic diff --git a/rclcpp_lifecycle/CHANGELOG.rst b/rclcpp_lifecycle/CHANGELOG.rst index 03d02cb2c2..0285505488 100644 --- a/rclcpp_lifecycle/CHANGELOG.rst +++ b/rclcpp_lifecycle/CHANGELOG.rst @@ -3,6 +3,13 @@ Changelog for package rclcpp_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +18.0.0 (2022-12-29) +------------------- +* Implement Unified Node Interface (NodeInterfaces class) (`#2041 `_) +* Add clock type to node_options (`#1982 `_) +* Update maintainers (`#2043 `_) +* Contributors: Audrow Nash, Jeffery Hsu, methylDragon + 17.1.0 (2022-11-02) ------------------- * LifecycleNode on_configure doc fix. (`#2034 `_) diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 8fa958a0bc..092105238b 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -2,7 +2,7 @@ rclcpp_lifecycle - 17.1.0 + 18.0.0 Package containing a prototype for lifecycle implementation Ivan Paunovic From 1bbb03302a0eb36955e7ecc19189b9f6bbadcd6f Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 4 Jan 2023 08:49:57 -0500 Subject: [PATCH 50/53] Add in a fix for older compilers. (#2075) * Add in a fix for older compilers. The addition of the NodeInterfaces class made it stop compiling with older compilers (such as gcc 9.4.0 on Ubuntu 20.04). The error has to do with calling the copy constructor on rclcpp::Node, which is deleted. Work around this by removing the NodeInterfaces shared_ptr constructor, which isn't technically needed. Signed-off-by: Chris Lalancette --- .../rclcpp/node_interfaces/node_interfaces.hpp | 6 ------ .../rclcpp/node_interfaces/test_node_interfaces.cpp | 12 ++++++------ 2 files changed, 6 insertions(+), 12 deletions(-) diff --git a/rclcpp/include/rclcpp/node_interfaces/node_interfaces.hpp b/rclcpp/include/rclcpp/node_interfaces/node_interfaces.hpp index 2f40380c75..97959145b3 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_interfaces.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_interfaces.hpp @@ -147,12 +147,6 @@ class NodeInterfaces : NodeInterfacesSupportsT(node) {} - /// NodeT::SharedPtr Constructor - template - NodeInterfaces(std::shared_ptr node) // NOLINT(runtime/explicit) - : NodeInterfaces(node ? *node : throw std::invalid_argument("given node pointer is nullptr")) - {} - explicit NodeInterfaces(std::shared_ptr... args) : NodeInterfacesSupportsT(args ...) {} diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_interfaces.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_interfaces.cpp index 96596dea37..ebb91c4770 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_interfaces.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_interfaces.cpp @@ -44,7 +44,7 @@ TEST_F(TestNodeInterfaces, node_interfaces_nominal) { using rclcpp::node_interfaces::NodeInterfaces; using rclcpp::node_interfaces::NodeBaseInterface; using rclcpp::node_interfaces::NodeGraphInterface; - auto node_interfaces = NodeInterfaces(node); + auto node_interfaces = NodeInterfaces(*node); } // Implicit conversion of rclcpp::Node into function that uses NodeInterfaces of base. @@ -55,7 +55,7 @@ TEST_F(TestNodeInterfaces, node_interfaces_nominal) { auto base_interface = ni.get(); }; - some_func(node); + some_func(*node); } // Implicit narrowing of NodeInterfaces into a new interface NodeInterfaces with fewer interfaces. @@ -67,7 +67,7 @@ TEST_F(TestNodeInterfaces, node_interfaces_nominal) { auto base_interface = ni_with_one.get(); }; - NodeInterfaces ni_with_two(node); + NodeInterfaces ni_with_two(*node); some_func(ni_with_two); } @@ -102,7 +102,7 @@ TEST_F(TestNodeInterfaces, node_interfaces_standard_interfaces) { rclcpp::node_interfaces::NodeWaitablesInterface, rclcpp::node_interfaces::NodeParametersInterface, rclcpp::node_interfaces::NodeTimeSourceInterface - >(node); + >(*node); } /* @@ -134,7 +134,7 @@ TEST_F(TestNodeInterfaces, ni_init) { NodeWaitablesInterface, NodeParametersInterface, NodeTimeSourceInterface - >(node); + >(*node); { auto base = ni.get(); @@ -198,7 +198,7 @@ TEST_F(TestNodeInterfaces, ni_all_init) { using rclcpp::node_interfaces::NodeParametersInterface; using rclcpp::node_interfaces::NodeTimeSourceInterface; - auto ni = rclcpp::node_interfaces::NodeInterfaces(node); + auto ni = rclcpp::node_interfaces::NodeInterfaces(*node); { auto base = ni.get(); From 3db2ece14545ade3ab9cec0146be57c1a3ae83f8 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Fri, 13 Jan 2023 16:47:13 -0500 Subject: [PATCH 51/53] Fix the keep_last warning when using system defaults. (#2082) If the user creates SystemDefaultsQoS setting, they are explicitly asking for SystemDefaults. In that case, we should *not* warn, and just let the underlying RMW choose what it wants. Implement that here by passing a boolean parameter through that decides when we should print out the warning, and then skip printing that warning when SystemDefaultsQoS is created. Signed-off-by: Chris Lalancette Signed-off-by: Chris Lalancette --- rclcpp/include/rclcpp/qos.hpp | 6 ++++-- rclcpp/src/rclcpp/qos.cpp | 37 +++++++++++++++-------------------- 2 files changed, 20 insertions(+), 23 deletions(-) diff --git a/rclcpp/include/rclcpp/qos.hpp b/rclcpp/include/rclcpp/qos.hpp index 4946d37889..2ad49487c5 100644 --- a/rclcpp/include/rclcpp/qos.hpp +++ b/rclcpp/include/rclcpp/qos.hpp @@ -80,7 +80,9 @@ struct RCLCPP_PUBLIC QoSInitialization size_t depth; /// Constructor which takes both a history policy and a depth (even if it would be unused). - QoSInitialization(rmw_qos_history_policy_t history_policy_arg, size_t depth_arg); + QoSInitialization( + rmw_qos_history_policy_t history_policy_arg, size_t depth_arg, + bool print_depth_warning = true); /// Create a QoSInitialization from an existing rmw_qos_profile_t, using its history and depth. static @@ -97,7 +99,7 @@ struct RCLCPP_PUBLIC KeepAll : public rclcpp::QoSInitialization /// Use to initialize the QoS with the keep_last history setting and the given depth. struct RCLCPP_PUBLIC KeepLast : public rclcpp::QoSInitialization { - explicit KeepLast(size_t depth); + explicit KeepLast(size_t depth, bool print_depth_warning = true); }; /// Encapsulation of Quality of Service settings. diff --git a/rclcpp/src/rclcpp/qos.cpp b/rclcpp/src/rclcpp/qos.cpp index 2d0277bfb5..2453149aa4 100644 --- a/rclcpp/src/rclcpp/qos.cpp +++ b/rclcpp/src/rclcpp/qos.cpp @@ -45,9 +45,19 @@ std::string qos_policy_name_from_kind(rmw_qos_policy_kind_t policy_kind) } } -QoSInitialization::QoSInitialization(rmw_qos_history_policy_t history_policy_arg, size_t depth_arg) +QoSInitialization::QoSInitialization( + rmw_qos_history_policy_t history_policy_arg, size_t depth_arg, + bool print_depth_warning) : history_policy(history_policy_arg), depth(depth_arg) -{} +{ + if (history_policy == RMW_QOS_POLICY_HISTORY_KEEP_LAST && depth == 0 && print_depth_warning) { + RCLCPP_WARN_ONCE( + rclcpp::get_logger( + "rclcpp"), + "A zero depth with KEEP_LAST doesn't make sense; no data could be stored. " + "This will be interpreted as SYSTEM_DEFAULT"); + } +} QoSInitialization QoSInitialization::from_rmw(const rmw_qos_profile_t & rmw_qos) @@ -55,8 +65,9 @@ QoSInitialization::from_rmw(const rmw_qos_profile_t & rmw_qos) switch (rmw_qos.history) { case RMW_QOS_POLICY_HISTORY_KEEP_ALL: return KeepAll(); - case RMW_QOS_POLICY_HISTORY_KEEP_LAST: case RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT: + return KeepLast(rmw_qos.depth, false); + case RMW_QOS_POLICY_HISTORY_KEEP_LAST: case RMW_QOS_POLICY_HISTORY_UNKNOWN: default: return KeepLast(rmw_qos.depth); @@ -67,16 +78,9 @@ KeepAll::KeepAll() : QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_ALL, 0) {} -KeepLast::KeepLast(size_t depth) -: QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_LAST, depth) +KeepLast::KeepLast(size_t depth, bool print_depth_warning) +: QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_LAST, depth, print_depth_warning) { - if (depth == 0) { - RCLCPP_WARN_ONCE( - rclcpp::get_logger( - "rclcpp"), - "A zero depth with KEEP_LAST doesn't make sense; no data could be stored." - "This will be interpreted as SYSTEM_DEFAULT"); - } } QoS::QoS( @@ -84,15 +88,6 @@ QoS::QoS( const rmw_qos_profile_t & initial_profile) : rmw_qos_profile_(initial_profile) { - if (qos_initialization.history_policy == RMW_QOS_POLICY_HISTORY_KEEP_LAST && - qos_initialization.depth == 0) - { - RCLCPP_WARN_ONCE( - rclcpp::get_logger( - "rclcpp"), - "A zero depth with KEEP_LAST doesn't make sense; no data could be stored." - "This will be interpreted as SYSTEM_DEFAULT"); - } rmw_qos_profile_.history = qos_initialization.history_policy; rmw_qos_profile_.depth = qos_initialization.depth; } From 37adc03c11171f67c213ee72f7ce43fdcbda7b9e Mon Sep 17 00:00:00 2001 From: Alexander Hans Date: Tue, 17 Jan 2023 20:50:44 +0100 Subject: [PATCH 52/53] Fix -Wmaybe-uninitialized warning (#2081) * Fix -Wmaybe-uninitialized warning gcc 12 warned about `callback_list_ptr` potentially being uninitialized when compiling in release mode (i.e., with `-O2`). Since `shutdown_type` is a compile-time parameter, we fix the warning by enforcing the decision at compile time. Signed-off-by: Alexander Hans --- rclcpp/include/rclcpp/context.hpp | 8 ++- rclcpp/src/rclcpp/context.cpp | 111 +++++++++++++----------------- 2 files changed, 54 insertions(+), 65 deletions(-) diff --git a/rclcpp/include/rclcpp/context.hpp b/rclcpp/include/rclcpp/context.hpp index 3add3f6d46..2917ce3363 100644 --- a/rclcpp/include/rclcpp/context.hpp +++ b/rclcpp/include/rclcpp/context.hpp @@ -398,20 +398,22 @@ class Context : public std::enable_shared_from_this using ShutdownCallback = ShutdownCallbackHandle::ShutdownCallbackType; + template RCLCPP_LOCAL ShutdownCallbackHandle add_shutdown_callback( - ShutdownType shutdown_type, ShutdownCallback callback); + template RCLCPP_LOCAL bool remove_shutdown_callback( - ShutdownType shutdown_type, const ShutdownCallbackHandle & callback_handle); + template + RCLCPP_LOCAL std::vector - get_shutdown_callback(ShutdownType shutdown_type) const; + get_shutdown_callback() const; }; /// Return a copy of the list of context shared pointers. diff --git a/rclcpp/src/rclcpp/context.cpp b/rclcpp/src/rclcpp/context.cpp index 1599172e2e..971c0ee73a 100644 --- a/rclcpp/src/rclcpp/context.cpp +++ b/rclcpp/src/rclcpp/context.cpp @@ -365,49 +365,45 @@ Context::on_shutdown(OnShutdownCallback callback) rclcpp::OnShutdownCallbackHandle Context::add_on_shutdown_callback(OnShutdownCallback callback) { - return add_shutdown_callback(ShutdownType::on_shutdown, callback); + return add_shutdown_callback(callback); } bool Context::remove_on_shutdown_callback(const OnShutdownCallbackHandle & callback_handle) { - return remove_shutdown_callback(ShutdownType::on_shutdown, callback_handle); + return remove_shutdown_callback(callback_handle); } rclcpp::PreShutdownCallbackHandle Context::add_pre_shutdown_callback(PreShutdownCallback callback) { - return add_shutdown_callback(ShutdownType::pre_shutdown, callback); + return add_shutdown_callback(callback); } bool Context::remove_pre_shutdown_callback( const PreShutdownCallbackHandle & callback_handle) { - return remove_shutdown_callback(ShutdownType::pre_shutdown, callback_handle); + return remove_shutdown_callback(callback_handle); } +template rclcpp::ShutdownCallbackHandle Context::add_shutdown_callback( - ShutdownType shutdown_type, ShutdownCallback callback) { auto callback_shared_ptr = std::make_shared(callback); - switch (shutdown_type) { - case ShutdownType::pre_shutdown: - { - std::lock_guard lock(pre_shutdown_callbacks_mutex_); - pre_shutdown_callbacks_.emplace(callback_shared_ptr); - } - break; - case ShutdownType::on_shutdown: - { - std::lock_guard lock(on_shutdown_callbacks_mutex_); - on_shutdown_callbacks_.emplace(callback_shared_ptr); - } - break; + static_assert( + shutdown_type == ShutdownType::pre_shutdown || shutdown_type == ShutdownType::on_shutdown); + + if constexpr (shutdown_type == ShutdownType::pre_shutdown) { + std::lock_guard lock(pre_shutdown_callbacks_mutex_); + pre_shutdown_callbacks_.emplace(callback_shared_ptr); + } else { + std::lock_guard lock(on_shutdown_callbacks_mutex_); + on_shutdown_callbacks_.emplace(callback_shared_ptr); } ShutdownCallbackHandle callback_handle; @@ -415,73 +411,64 @@ Context::add_shutdown_callback( return callback_handle; } +template bool Context::remove_shutdown_callback( - ShutdownType shutdown_type, const ShutdownCallbackHandle & callback_handle) { - std::mutex * mutex_ptr = nullptr; - std::unordered_set< - std::shared_ptr> * callback_list_ptr; - - switch (shutdown_type) { - case ShutdownType::pre_shutdown: - mutex_ptr = &pre_shutdown_callbacks_mutex_; - callback_list_ptr = &pre_shutdown_callbacks_; - break; - case ShutdownType::on_shutdown: - mutex_ptr = &on_shutdown_callbacks_mutex_; - callback_list_ptr = &on_shutdown_callbacks_; - break; - } - - std::lock_guard lock(*mutex_ptr); - auto callback_shared_ptr = callback_handle.callback.lock(); + const auto callback_shared_ptr = callback_handle.callback.lock(); if (callback_shared_ptr == nullptr) { return false; } - return callback_list_ptr->erase(callback_shared_ptr) == 1; + + const auto remove_callback = [this, &callback_shared_ptr](auto & mutex, auto & callback_set) { + const std::lock_guard lock(mutex); + return callback_set.erase(callback_shared_ptr) == 1; + }; + + static_assert( + shutdown_type == ShutdownType::pre_shutdown || shutdown_type == ShutdownType::on_shutdown); + + if constexpr (shutdown_type == ShutdownType::pre_shutdown) { + return remove_callback(pre_shutdown_callbacks_mutex_, pre_shutdown_callbacks_); + } else { + return remove_callback(on_shutdown_callbacks_mutex_, on_shutdown_callbacks_); + } } std::vector Context::get_on_shutdown_callbacks() const { - return get_shutdown_callback(ShutdownType::on_shutdown); + return get_shutdown_callback(); } std::vector Context::get_pre_shutdown_callbacks() const { - return get_shutdown_callback(ShutdownType::pre_shutdown); + return get_shutdown_callback(); } +template std::vector -Context::get_shutdown_callback(ShutdownType shutdown_type) const +Context::get_shutdown_callback() const { - std::mutex * mutex_ptr = nullptr; - const std::unordered_set< - std::shared_ptr> * callback_list_ptr; - - switch (shutdown_type) { - case ShutdownType::pre_shutdown: - mutex_ptr = &pre_shutdown_callbacks_mutex_; - callback_list_ptr = &pre_shutdown_callbacks_; - break; - case ShutdownType::on_shutdown: - mutex_ptr = &on_shutdown_callbacks_mutex_; - callback_list_ptr = &on_shutdown_callbacks_; - break; - } + const auto get_callback_vector = [this](auto & mutex, auto & callback_set) { + const std::lock_guard lock(mutex); + std::vector callbacks; + for (auto & callback : callback_set) { + callbacks.push_back(*callback); + } + return callbacks; + }; - std::vector callbacks; - { - std::lock_guard lock(*mutex_ptr); - for (auto & iter : *callback_list_ptr) { - callbacks.emplace_back(*iter); - } - } + static_assert( + shutdown_type == ShutdownType::pre_shutdown || shutdown_type == ShutdownType::on_shutdown); - return callbacks; + if constexpr (shutdown_type == ShutdownType::pre_shutdown) { + return get_callback_vector(pre_shutdown_callbacks_mutex_, pre_shutdown_callbacks_); + } else { + return get_callback_vector(on_shutdown_callbacks_mutex_, on_shutdown_callbacks_); + } } std::shared_ptr From ab71df3ce1d8993e8424e7d0934b2e205284c682 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Mon, 23 Jan 2023 09:46:50 -0600 Subject: [PATCH 53/53] Improve component_manager_isolated shutdown (#2085) This eliminates a potential hang when the isolated container is being shutdown via the rclcpp SignalHandler. Signed-off-by: Michael Carroll Co-authored-by: Chris Lalancette --- .../component_manager_isolated.hpp | 48 +++++++++++++------ 1 file changed, 34 insertions(+), 14 deletions(-) diff --git a/rclcpp_components/include/rclcpp_components/component_manager_isolated.hpp b/rclcpp_components/include/rclcpp_components/component_manager_isolated.hpp index ea77532312..e2061e4da2 100644 --- a/rclcpp_components/include/rclcpp_components/component_manager_isolated.hpp +++ b/rclcpp_components/include/rclcpp_components/component_manager_isolated.hpp @@ -38,6 +38,16 @@ class ComponentManagerIsolated : public rclcpp_components::ComponentManager { std::shared_ptr executor; std::thread thread; + std::atomic_bool thread_initialized; + + /// Constructor for the wrapper. + /// This is necessary as atomic variables don't have copy/move operators + /// implemented so this structure is not copyable/movable by default + explicit DedicatedExecutorWrapper(std::shared_ptr exec) + : executor(exec), + thread_initialized(false) + { + } }; public: @@ -59,15 +69,19 @@ class ComponentManagerIsolated : public rclcpp_components::ComponentManager void add_node_to_executor(uint64_t node_id) override { - DedicatedExecutorWrapper executor_wrapper; auto exec = std::make_shared(); exec->add_node(node_wrappers_[node_id].get_node_base_interface()); - executor_wrapper.executor = exec; - executor_wrapper.thread = std::thread( - [exec]() { + + // Emplace rather than std::move since move operations are deleted for atomics + auto result = dedicated_executor_wrappers_.emplace(std::make_pair(node_id, exec)); + DedicatedExecutorWrapper & wrapper = result.first->second; + wrapper.executor = exec; + auto & thread_initialized = wrapper.thread_initialized; + wrapper.thread = std::thread( + [exec, &thread_initialized]() { + thread_initialized = true; exec->spin(); }); - dedicated_executor_wrappers_[node_id] = std::move(executor_wrapper); } /// Remove component node from executor model, it's invoked in on_unload_node() /** @@ -90,15 +104,21 @@ class ComponentManagerIsolated : public rclcpp_components::ComponentManager */ void cancel_executor(DedicatedExecutorWrapper & executor_wrapper) { - // We can't immediately call the cancel() API on an executor because if it is not - // already spinning, this operation will have no effect. - // We rely on the assumption that this class creates executors and then immediately makes them - // spin in a separate thread, i.e. the time gap between when the executor is created and when - // it starts to spin is small (although it's not negligible). - - while (!executor_wrapper.executor->is_spinning()) { - // This is an arbitrarily small delay to avoid busy looping - rclcpp::sleep_for(std::chrono::milliseconds(1)); + // Verify that the executor thread has begun spinning. + // If it has not, then wait until the thread starts to ensure + // that cancel() will fully stop the execution + // + // This prevents a previous race condition that occurs between the + // creation of the executor spin thread and cancelling an executor + + if (!executor_wrapper.thread_initialized) { + auto context = this->get_node_base_interface()->get_context(); + + // Guarantee that either the executor is spinning or we are shutting down. + while (!executor_wrapper.executor->is_spinning() && rclcpp::ok(context)) { + // This is an arbitrarily small delay to avoid busy looping + rclcpp::sleep_for(std::chrono::milliseconds(1)); + } } // After the while loop we are sure that the executor is now spinning, so