From 5111f980163104820d1284c06eded88f12b3aa61 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 12 Feb 2024 13:27:00 -0800 Subject: [PATCH 01/36] New MPPI Cost Critic (Contrib: Brice Renaudeau) (#4090) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Share code Signed-off-by: Brice * Update inflation_cost_critic.hpp - copyright - ifndef Signed-off-by: Brice * fix lint cpp - extra space Signed-off-by: Brice * Fix Smac Planner confined collision checker (#4055) * Update collision_checker.cpp Signed-off-by: Steve Macenski * Fix tests Signed-off-by: Steve Macenski * Update test_a_star.cpp Signed-off-by: Steve Macenski --------- Signed-off-by: Steve Macenski Signed-off-by: Brice * Prevent analytic expansions from shortcutting Smac Planner feasible paths (#3962) * a potential solution to smac shortcutting * costmap reoslution * some fixes * completed prototype * some fixes for collision detection and performance * completing shortcutting fix * updating tests * adding readme --------- Signed-off-by: Steve Macenski Signed-off-by: Brice * change pointer free order in amcl to avoid use-after-free bug mentioned in #4068 (#4070) Signed-off-by: GoesM Co-authored-by: GoesM Signed-off-by: Brice * [Smac Planner] Massive Improvement of Behavior for SE2 Footprint Checking (ie non-circular robots) In Confined Settings (#4067) * prototype to test SE2 footprint H improvements * some fixes * fixed * invert logic * Working final prototype to be tested * complete unit test conversions * Update inflation_layer.hpp Signed-off-by: Steve Macenski --------- Signed-off-by: Steve Macenski Signed-off-by: Brice * Adding new Smac paper to readme Signed-off-by: Steve Macenski Signed-off-by: Brice * Update README.md Signed-off-by: Steve Macenski Signed-off-by: Brice * [behavior_tree] don't repeat yourself in "blackboard->set" (#4074) * don't repeat yourself: templates in tests Signed-off-by: Davide Faconti * misse change Signed-off-by: Davide Faconti --------- Signed-off-by: Davide Faconti Signed-off-by: Brice * Allow path end pose deviation revive (#4065) * Support stitching paths in compute path to poses * Update nav2_planner/src/planner_server.cpp Co-authored-by: Steve Macenski * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg Co-authored-by: pepisg Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Steve Macenski Signed-off-by: Brice * Updated code to use getInflationLayer() method (#4076) * updated code to use getInflationLayer method Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> * Fix linting Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> --------- Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Brice * 1594 twist stamped publisher (#4077) * Add TwistStamped to controller_server via TwistPublisher util * Add a new util class for publishing either Twist or TwistStamped * Add a new parameter for selecting to stamp the twist data * Consume TwistPublisher in nav2_controller Signed-off-by: Ryan Friedman * Fix small issues * Unused variable * Incorrect doxygen Signed-off-by: Ryan Friedman * Remove stored node and assert Signed-off-by: Ryan Friedman * Add tests for node * Facing timeout even though it does the same thing as velocity smoother test Signed-off-by: Ryan Friedman * Add missing spin call to solve timeout Signed-off-by: Ryan Friedman * Fix copyright (me instead of intel) Signed-off-by: Ryan Friedman * Add full test coverage with subscriber Signed-off-by: Ryan Friedman * Remove unused rclcpp fixture * Can't use it due to needing to join the pub thread after rclcpp shuts down Signed-off-by: Ryan Friedman * Use TwistStamped in nav2_behaviors Signed-off-by: Ryan Friedman * Use TwistStamped in collision monitor node Signed-off-by: Ryan Friedman * Add TwistStamped readme updates to velocity smoother Signed-off-by: Ryan Friedman * Add TwistSubscriber implementation Signed-off-by: Ryan Friedman * Fix syntax errors Signed-off-by: Ryan Friedman * Use TwistSubscriber in test_velocity_smoother Signed-off-by: Ryan Friedman * Use TwistSubscriber in assisted_teleop Signed-off-by: Ryan Friedman * Use TwistSubscriber in collision monitor node Signed-off-by: Ryan Friedman * Use TwistSubscriber in velocity smoother Signed-off-by: Ryan Friedman * Remove unused code Signed-off-by: Ryan Friedman * add timestamp and frame_id to TwistStamped message * Add missing utility include Signed-off-by: Ryan Friedman * Document TwistPublisher and TwistSubscriber usage Signed-off-by: Ryan Friedman * Use pass-by-reference * Instead of std::move(std::unique_ptr) Signed-off-by: Ryan Friedman * Finish twist subscriber tests Signed-off-by: Ryan Friedman * Add other constructor and docs Signed-off-by: Ryan Friedman * Fix linter issues Signed-off-by: Ryan Friedman * Manually fix paren alignment Signed-off-by: Ryan Friedman * Remove GSoC reference Signed-off-by: Ryan Friedman * Document twist bool param in README Signed-off-by: Ryan Friedman * Handle twistPublisher in collision monitor * Implement behavior in the stamped callback * Unstamped callback calls the stamped callback * Switch to unique pointer for publisher Signed-off-by: Ryan Friedman * Convert to using TwistStamped interally * Use incoming twistStamped timestamp if available * Convert all internal representations to use TwistStamped Signed-off-by: Ryan Friedman * Remove nav2_util usage instructions Signed-off-by: Ryan Friedman * Remove unused Twist only subscriber Signed-off-by: Ryan Friedman * More linter fixes Signed-off-by: Ryan Friedman * Prefer working with unique_ptr for cmd_vel * This makes it easier to switch to std::move instead of dereference on publish Signed-off-by: Ryan Friedman * Completing twist stamped migration * shared to unique ptr Signed-off-by: Steve Macenski * twist add stamps and properly propogated * nav2_util: fix for compiling with clang - Resolve error: moving a temporary object prevents copy elision [-Werror,-Wpessimizing-move] Signed-off-by: Rhys Mainwaring --------- Signed-off-by: Ryan Friedman Signed-off-by: Steve Macenski Signed-off-by: Rhys Mainwaring Co-authored-by: pedro-fuoco Co-authored-by: Steve Macenski Co-authored-by: Rhys Mainwaring Signed-off-by: Brice * Change costmap_queue to shared library (#4072) Signed-off-by: cybaol Signed-off-by: Brice * fix include of hpp Signed-off-by: Brice Renaudeau * inflation cost optmiizations and cleanu * rename, add defaults, and docs * smoke test addition * lintg * normalize weight * update readme * increment cache * Update cost_critic.hpp Signed-off-by: Steve Macenski * Update cost_critic.cpp Signed-off-by: Steve Macenski --------- Signed-off-by: Brice Signed-off-by: Steve Macenski Signed-off-by: GoesM Signed-off-by: Davide Faconti Signed-off-by: gg Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Ryan Friedman Signed-off-by: Rhys Mainwaring Signed-off-by: cybaol Signed-off-by: Brice Renaudeau Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: GoesM Co-authored-by: Davide Faconti Co-authored-by: Joshua Wallace Co-authored-by: pepisg Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: jncfa <20467009+jncfa@users.noreply.github.com> Co-authored-by: Ryan Co-authored-by: pedro-fuoco Co-authored-by: Rhys Mainwaring Co-authored-by: Kino --- nav2_mppi_controller/CMakeLists.txt | 1 + nav2_mppi_controller/README.md | 26 +++ nav2_mppi_controller/critics.xml | 4 + .../critics/cost_critic.hpp | 98 +++++++++ .../src/critics/cost_critic.cpp | 200 ++++++++++++++++++ nav2_mppi_controller/test/critics_tests.cpp | 1 + .../test/optimizer_smoke_test.cpp | 2 +- 7 files changed, 331 insertions(+), 1 deletion(-) create mode 100644 nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp create mode 100644 nav2_mppi_controller/src/critics/cost_critic.cpp diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt index 1501e4321a1..d2bb6d9c9da 100644 --- a/nav2_mppi_controller/CMakeLists.txt +++ b/nav2_mppi_controller/CMakeLists.txt @@ -79,6 +79,7 @@ add_library(mppi_controller SHARED add_library(mppi_critics SHARED src/critics/obstacles_critic.cpp + src/critics/cost_critic.cpp src/critics/goal_critic.cpp src/critics/goal_angle_critic.cpp src/critics/path_align_critic.cpp diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index fd7ad802142..b6cfc0e452e 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -98,6 +98,9 @@ This process is then repeated a number of times and returns a converged solution #### Obstacles Critic + +Uses estimated distances from obstacles using cost and inflation parameters to avoid obstacles + | Parameter | Type | Definition | | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | | consider_footprint | bool | Default: False. Whether to use point cost (if robot is circular or low compute power) or compute SE2 footprint cost. | @@ -109,6 +112,20 @@ This process is then repeated a number of times and returns a converged solution | near_goal_distance | double | Default 0.5. Distance near goal to stop applying preferential obstacle term to allow robot to smoothly converge to goal pose in close proximity to obstacles. | inflation_layer_name | string | Default "". Name of the inflation layer. If empty, it uses the last inflation layer in the costmap. If you have multiple inflation layers, you may want to specify the name of the layer to use. | +#### Cost Critic + +Uses inflated costmap cost directly to avoid obstacles + + | Parameter | Type | Definition | + | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | + | consider_footprint | bool | Default: False. Whether to use point cost (if robot is circular or low compute power) or compute SE2 footprint cost. | + | cost_weight | double | Default 3.81. Wight to apply to critic to avoid obstacles. | + | cost_power | int | Default 1. Power order to apply to term. | + | collision_cost | double | Default 1000000.0. Cost to apply to a true collision in a trajectory. | + | critical_cost | double | Default 300.0. Cost to apply to a pose with any point in in inflated space to prefer distance from obstacles. | + | near_goal_distance | double | Default 0.5. Distance near goal to stop applying preferential obstacle term to allow robot to smoothly converge to goal pose in close proximity to obstacles. + | inflation_layer_name | string | Default "". Name of the inflation layer. If empty, it uses the last inflation layer in the costmap. If you have multiple inflation layers, you may want to specify the name of the layer to use. | + #### Path Align Critic | Parameter | Type | Definition | | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | @@ -213,6 +230,15 @@ controller_server: collision_cost: 10000.0 collision_margin_distance: 0.1 near_goal_distance: 0.5 + # Option to replace Obstacles and use Cost instead + # CostCritic: + # enabled: true + # cost_power: 1 + # cost_weight: 3.81 + # critical_cost: 300.0 + # consider_footprint: true + # collision_cost: 1000000.0 + # near_goal_distance: 1.0 PathAlignCritic: enabled: true cost_power: 1 diff --git a/nav2_mppi_controller/critics.xml b/nav2_mppi_controller/critics.xml index 790568e7967..d578d23a9ec 100644 --- a/nav2_mppi_controller/critics.xml +++ b/nav2_mppi_controller/critics.xml @@ -5,6 +5,10 @@ mppi critic for obstacle avoidance + + mppi critic for obstacle avoidance using costmap score + + mppi critic for driving towards the goal diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp new file mode 100644 index 00000000000..77df20adcb5 --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp @@ -0,0 +1,98 @@ +// Copyright (c) 2023 Robocc Brice Renaudeau +// +// 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 NAV2_MPPI_CONTROLLER__CRITICS__COST_CRITIC_HPP_ +#define NAV2_MPPI_CONTROLLER__CRITICS__COST_CRITIC_HPP_ + +#include +#include + +#include "nav2_costmap_2d/footprint_collision_checker.hpp" +#include "nav2_costmap_2d/inflation_layer.hpp" + +#include "nav2_mppi_controller/critic_function.hpp" +#include "nav2_mppi_controller/models/state.hpp" +#include "nav2_mppi_controller/tools/utils.hpp" + +namespace mppi::critics +{ + +/** + * @class mppi::critics::CostCritic + * @brief Critic objective function for avoiding obstacles using costmap's inflated cost + */ +class CostCritic : public CriticFunction +{ +public: + /** + * @brief Initialize critic + */ + void initialize() override; + + /** + * @brief Evaluate cost related to obstacle avoidance + * + * @param costs [out] add obstacle cost values to this tensor + */ + void score(CriticData & data) override; + +protected: + /** + * @brief Checks if cost represents a collision + * @param cost Point cost at pose center + * @param x X of pose + * @param y Y of pose + * @param theta theta of pose + * @return bool if in collision + */ + bool inCollision(float cost, float x, float y, float theta); + + /** + * @brief cost at a robot pose + * @param x X of pose + * @param y Y of pose + * @return Collision information at pose + */ + float costAtPose(float x, float y); + + /** + * @brief Find the min cost of the inflation decay function for which the robot MAY be + * in collision in any orientation + * @param costmap Costmap2DROS to get minimum inscribed cost (e.g. 128 in inflation layer documentation) + * @return double circumscribed cost, any higher than this and need to do full footprint collision checking + * since some element of the robot could be in collision + */ + float findCircumscribedCost(std::shared_ptr costmap); + +protected: + nav2_costmap_2d::FootprintCollisionChecker + collision_checker_{nullptr}; + float possibly_inscribed_cost_; + + bool consider_footprint_{true}; + float circumscribed_radius_{0}; + float circumscribed_cost_{0}; + float collision_cost_{0}; + float critical_cost_{0}; + float weight_{0}; + + float near_goal_distance_; + std::string inflation_layer_name_; + + unsigned int power_{0}; +}; + +} // namespace mppi::critics + +#endif // NAV2_MPPI_CONTROLLER__CRITICS__COST_CRITIC_HPP_ diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp new file mode 100644 index 00000000000..137e9433760 --- /dev/null +++ b/nav2_mppi_controller/src/critics/cost_critic.cpp @@ -0,0 +1,200 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include "nav2_mppi_controller/critics/cost_critic.hpp" + +namespace mppi::critics +{ + +void CostCritic::initialize() +{ + auto getParam = parameters_handler_->getParamGetter(name_); + getParam(consider_footprint_, "consider_footprint", false); + getParam(power_, "cost_power", 1); + getParam(weight_, "cost_weight", 3.81); + getParam(critical_cost_, "critical_cost", 300.0); + getParam(collision_cost_, "collision_cost", 1000000.0); + getParam(near_goal_distance_, "near_goal_distance", 0.5); + getParam(inflation_layer_name_, "inflation_layer_name", std::string("")); + + // Normalized by cost value to put in same regime as other weights + weight_ /= 254.0f; + + collision_checker_.setCostmap(costmap_); + possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_); + + if (possibly_inscribed_cost_ < 1.0f) { + RCLCPP_ERROR( + logger_, + "Inflation layer either not found or inflation is not set sufficiently for " + "optimized non-circular collision checking capabilities. It is HIGHLY recommended to set" + " the inflation radius to be at MINIMUM half of the robot's largest cross-section. See " + "github.com/ros-planning/navigation2/tree/main/nav2_smac_planner#potential-fields" + " for full instructions. This will substantially impact run-time performance."); + } + + RCLCPP_INFO( + logger_, + "InflationCostCritic instantiated with %d power and %f / %f weights. " + "Critic will collision check based on %s cost.", + power_, critical_cost_, weight_, consider_footprint_ ? + "footprint" : "circular"); +} + +float CostCritic::findCircumscribedCost( + std::shared_ptr costmap) +{ + double result = -1.0; + const double circum_radius = costmap->getLayeredCostmap()->getCircumscribedRadius(); + if (static_cast(circum_radius) == circumscribed_radius_) { + // early return if footprint size is unchanged + return circumscribed_cost_; + } + + // check if the costmap has an inflation layer + const auto inflation_layer = nav2_costmap_2d::InflationLayer::getInflationLayer( + costmap, + inflation_layer_name_); + if (inflation_layer != nullptr) { + const double resolution = costmap->getCostmap()->getResolution(); + result = inflation_layer->computeCost(circum_radius / resolution); + } else { + RCLCPP_WARN( + logger_, + "No inflation layer found in costmap configuration. " + "If this is an SE2-collision checking plugin, it cannot use costmap potential " + "field to speed up collision checking by only checking the full footprint " + "when robot is within possibly-inscribed radius of an obstacle. This may " + "significantly slow down planning times and not avoid anything but absolute collisions!"); + } + + circumscribed_radius_ = static_cast(circum_radius); + circumscribed_cost_ = static_cast(result); + + return circumscribed_cost_; +} + +void CostCritic::score(CriticData & data) +{ + using xt::evaluation_strategy::immediate; + if (!enabled_) { + return; + } + + if (consider_footprint_) { + // footprint may have changed since initialization if user has dynamic footprints + possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_); + } + + // If near the goal, don't apply the preferential term since the goal is near obstacles + bool near_goal = false; + if (utils::withinPositionGoalTolerance(near_goal_distance_, data.state.pose.pose, data.path)) { + near_goal = true; + } + + auto && repulsive_cost = xt::xtensor::from_shape({data.costs.shape(0)}); + repulsive_cost.fill(0.0); + + const size_t traj_len = data.trajectories.x.shape(1); + bool all_trajectories_collide = true; + for (size_t i = 0; i < data.trajectories.x.shape(0); ++i) { + bool trajectory_collide = false; + const auto & traj = data.trajectories; + float pose_cost; + + for (size_t j = 0; j < traj_len; j++) { + // The costAtPose doesn't use orientation + // The footprintCostAtPose will always return "INSCRIBED" if footprint is over it + // So the center point has more information than the footprint + pose_cost = costAtPose(traj.x(i, j), traj.y(i, j)); + if (pose_cost < 1.0f) {continue;} // In free space + + if (inCollision(pose_cost, traj.x(i, j), traj.y(i, j), traj.yaws(i, j))) { + trajectory_collide = true; + break; + } + + // Let near-collision trajectory points be punished severely + // Note that we collision check based on the footprint actual, + // but score based on the center-point cost regardless + using namespace nav2_costmap_2d; // NOLINT + if (pose_cost >= INSCRIBED_INFLATED_OBSTACLE) { + repulsive_cost[i] += critical_cost_; + } else if (!near_goal) { // Generally prefer trajectories further from obstacles + repulsive_cost[i] += pose_cost; + } + } + + if (!trajectory_collide) { + all_trajectories_collide = false; + } else { + repulsive_cost[i] = collision_cost_; + } + } + + data.costs += xt::pow((weight_ * repulsive_cost / traj_len), power_); + data.fail_flag = all_trajectories_collide; +} + +/** + * @brief Checks if cost represents a collision + * @param cost Costmap cost + * @return bool if in collision + */ +bool CostCritic::inCollision(float cost, float x, float y, float theta) +{ + bool is_tracking_unknown = + costmap_ros_->getLayeredCostmap()->isTrackingUnknown(); + + // If consider_footprint_ check footprint scort for collision + if (consider_footprint_ && + (cost >= possibly_inscribed_cost_ || possibly_inscribed_cost_ < 1.0f)) + { + cost = static_cast(collision_checker_.footprintCostAtPose( + x, y, theta, costmap_ros_->getRobotFootprint())); + } + + switch (static_cast(cost)) { + using namespace nav2_costmap_2d; // NOLINT + case (LETHAL_OBSTACLE): + return true; + case (INSCRIBED_INFLATED_OBSTACLE): + return consider_footprint_ ? false : true; + case (NO_INFORMATION): + return is_tracking_unknown ? false : true; + } + + return false; +} + +float CostCritic::costAtPose(float x, float y) +{ + using namespace nav2_costmap_2d; // NOLINT + unsigned int x_i, y_i; + if (!collision_checker_.worldToMap(x, y, x_i, y_i)) { + return nav2_costmap_2d::NO_INFORMATION; + } + + return collision_checker_.pointCost(x_i, y_i); +} + +} // namespace mppi::critics + +#include + +PLUGINLIB_EXPORT_CLASS( + mppi::critics::CostCritic, + mppi::critics::CriticFunction) diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index 08f50945fd5..fee853cda43 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -24,6 +24,7 @@ #include "nav2_mppi_controller/critics/goal_angle_critic.hpp" #include "nav2_mppi_controller/critics/goal_critic.hpp" #include "nav2_mppi_controller/critics/obstacles_critic.hpp" +#include "nav2_mppi_controller/critics/cost_critic.hpp" #include "nav2_mppi_controller/critics/path_align_critic.hpp" #include "nav2_mppi_controller/critics/path_align_legacy_critic.hpp" #include "nav2_mppi_controller/critics/path_angle_critic.hpp" diff --git a/nav2_mppi_controller/test/optimizer_smoke_test.cpp b/nav2_mppi_controller/test/optimizer_smoke_test.cpp index d694496395d..9a77f6d8b06 100644 --- a/nav2_mppi_controller/test/optimizer_smoke_test.cpp +++ b/nav2_mppi_controller/test/optimizer_smoke_test.cpp @@ -103,7 +103,7 @@ INSTANTIATE_TEST_SUITE_P( std::make_tuple( "DiffDrive", std::vector( - {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"}, + {{"GoalCritic"}, {"GoalAngleCritic"}, {"CostCritic"}, {"PathAngleCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}}), true), std::make_tuple( From ab9b1010d7a126c0c407cd09228d97068646ee4c Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Mon, 12 Feb 2024 22:28:42 +0100 Subject: [PATCH 02/36] [behavior_tree] support both ports and blackboard (#4060) * check result of blackboard->get and use halTree Signed-off-by: Davide Faconti * remove unused header Signed-off-by: Davide Faconti * BT: add port inputs when missing and use getInputPortOrBlackboard Signed-off-by: Davide Faconti * add description Signed-off-by: Davide Faconti * change return value of getInputPortOrBlackboard Signed-off-by: Davide Faconti * updated tree XML Signed-off-by: Davide Faconti --------- Signed-off-by: Davide Faconti --- .../behavior_tree_engine.hpp | 1 - .../include/nav2_behavior_tree/bt_utils.hpp | 46 +++++++++++++++++-- .../globally_updated_goal_condition.hpp | 7 ++- .../condition/goal_updated_condition.hpp | 7 ++- .../initial_pose_received_condition.hpp | 19 +++++++- .../decorator/goal_updated_controller.hpp | 7 ++- .../plugins/decorator/speed_controller.hpp | 4 ++ nav2_behavior_tree/nav2_tree_nodes.xml | 15 +++++- .../action/remove_passed_goals_action.cpp | 2 +- .../condition/distance_traveled_condition.cpp | 4 +- .../globally_updated_goal_condition.cpp | 9 ++-- .../condition/goal_reached_condition.cpp | 2 +- .../condition/goal_updated_condition.cpp | 9 ++-- .../initial_pose_received_condition.cpp | 16 +++++-- .../plugins/decorator/distance_controller.cpp | 4 +- .../decorator/goal_updated_controller.cpp | 10 ++-- .../plugins/decorator/speed_controller.cpp | 9 ++-- .../src/behavior_tree_engine.cpp | 2 +- ...test_compute_path_through_poses_action.cpp | 12 ++--- .../test_compute_path_to_pose_action.cpp | 12 ++--- .../action/test_controller_selector_node.cpp | 8 ++-- .../action/test_follow_path_action.cpp | 2 +- .../test_goal_checker_selector_node.cpp | 8 ++-- .../test_navigate_through_poses_action.cpp | 2 +- .../action/test_navigate_to_pose_action.cpp | 2 +- .../action/test_planner_selector_node.cpp | 8 ++-- .../test_remove_passed_goals_action.cpp | 2 +- .../action/test_smooth_path_action.cpp | 2 +- .../action/test_smoother_selector_node.cpp | 8 ++-- .../action/test_truncate_path_action.cpp | 2 +- .../test_truncate_path_local_action.cpp | 18 ++++---- .../condition/test_initial_pose_received.cpp | 30 +++--------- .../decorator/test_goal_updater_node.cpp | 6 +-- nav2_behavior_tree/test/test_bt_utils.cpp | 4 +- 34 files changed, 186 insertions(+), 113 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp index daf609d067c..9a5b03ca673 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp @@ -23,7 +23,6 @@ #include "behaviortree_cpp_v3/behavior_tree.h" #include "behaviortree_cpp_v3/bt_factory.h" #include "behaviortree_cpp_v3/xml_parsing.h" -#include "behaviortree_cpp_v3/loggers/bt_zmq_publisher.h" #include "rclcpp/rclcpp.hpp" diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp index c0425d1711f..7e8ea23ec73 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp @@ -132,20 +132,27 @@ inline std::set convertFromString(StringView key) } /** - * @brief Return parameter value from behavior tree node or ros2 parameter file + * @brief Return parameter value from behavior tree node or ros2 parameter file. * @param node rclcpp::Node::SharedPtr * @param param_name std::string * @param behavior_tree_node T2 * @return */ -template +template T1 deconflictPortAndParamFrame( rclcpp::Node::SharedPtr node, std::string param_name, - T2 * behavior_tree_node) + const T2 * behavior_tree_node) { T1 param_value; - if (!behavior_tree_node->getInput(param_name, param_value)) { + bool param_from_input = behavior_tree_node->getInput(param_name, param_value); + + if constexpr (std::is_same_v) { + // not valid if port doesn't exist or it is an empty string + param_from_input &= !param_value.empty(); + } + + if (!param_from_input) { RCLCPP_DEBUG( node->get_logger(), "Parameter '%s' not provided by behavior tree xml file, " @@ -162,6 +169,37 @@ T1 deconflictPortAndParamFrame( } } +/** + * @brief Try reading an import port first, and if that doesn't work + * fallback to reading directly the blackboard. + * The blackboard must be passed explitly because config() is private in BT.CPP 4.X + * + * @param bt_node node + * @param blackboard the blackboard ovtained with node->config().blackboard + * @param param_name std::string + * @param behavior_tree_node the node + * @return + */ +template inline +bool getInputPortOrBlackboard( + const BT::TreeNode & bt_node, + const BT::Blackboard & blackboard, + const std::string & param_name, + T & value) +{ + if (bt_node.getInput(param_name, value)) { + return true; + } + if (blackboard.get(param_name, value)) { + return true; + } + return false; +} + +// Macro to remove boiler plate when using getInputPortOrBlackboard +#define getInputOrBlackboard(name, value) \ + getInputPortOrBlackboard(*this, *(this->config().blackboard), name, value); + } // namespace BT #endif // NAV2_BEHAVIOR_TREE__BT_UTILS_HPP_ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp index 64f77f1473d..7fe4a654044 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp @@ -57,7 +57,12 @@ class GloballyUpdatedGoalCondition : public BT::ConditionNode */ static BT::PortsList providedPorts() { - return {}; + return { + BT::InputPort>( + "goals", "Vector of navigation goals"), + BT::InputPort( + "goal", "Navigation goal"), + }; } private: diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp index 1e8c9712c16..d7191c9749d 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp @@ -54,7 +54,12 @@ class GoalUpdatedCondition : public BT::ConditionNode */ static BT::PortsList providedPorts() { - return {}; + return { + BT::InputPort>( + "goals", "Vector of navigation goals"), + BT::InputPort( + "goal", "Navigation goal"), + }; } private: diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp index 2bb7d995b80..ee5bb025c9a 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp @@ -15,6 +15,7 @@ #ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__INITIAL_POSE_RECEIVED_CONDITION_HPP_ #define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__INITIAL_POSE_RECEIVED_CONDITION_HPP_ +#include #include "behaviortree_cpp_v3/behavior_tree.h" namespace nav2_behavior_tree @@ -23,7 +24,21 @@ namespace nav2_behavior_tree * @brief A BT::ConditionNode that returns SUCCESS if initial pose * has been received and FAILURE otherwise */ -BT::NodeStatus initialPoseReceived(BT::TreeNode & tree_node); -} +class InitialPoseReceived : public BT::ConditionNode +{ +public: + InitialPoseReceived( + const std::string & name, + const BT::NodeConfiguration & config); + + static BT::PortsList providedPorts() + { + return {BT::InputPort("initial_pose_received")}; + } + + BT::NodeStatus tick() override; +}; + +} // namespace nav2_behavior_tree #endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__INITIAL_POSE_RECEIVED_CONDITION_HPP_ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp index 4ac0ab44eeb..9c59c877f67 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp @@ -48,7 +48,12 @@ class GoalUpdatedController : public BT::DecoratorNode */ static BT::PortsList providedPorts() { - return {}; + return { + BT::InputPort>( + "goals", "Vector of navigation goals"), + BT::InputPort( + "goal", "Navigation goal"), + }; } private: diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp index 8150eb31098..caf30982a60 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp @@ -57,6 +57,10 @@ class SpeedController : public BT::DecoratorNode BT::InputPort("max_rate", 1.0, "Maximum rate"), BT::InputPort("min_speed", 0.0, "Minimum speed"), BT::InputPort("max_speed", 0.5, "Maximum speed"), + BT::InputPort>( + "goals", "Vector of navigation goals"), + BT::InputPort( + "goal", "Navigation goal"), }; } diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 3e23aafbd0a..51d0166f9b9 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -224,9 +224,15 @@ Parent frame for transform - + + Vector of navigation goals + Navigation goal + - + + Vector of navigation goals + Navigation goal + Min battery % or voltage before triggering @@ -255,6 +261,7 @@ + SUCCESS if initial_pose_received true @@ -313,6 +320,8 @@ Maximum rate Minimum speed Maximum speed + Vector of navigation goals + Navigation goal @@ -322,6 +331,8 @@ + Vector of navigation goals + Navigation goal diff --git a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp index bf8b95c84f6..fd255f78c54 100644 --- a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp +++ b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp @@ -41,7 +41,7 @@ void RemovePassedGoals::initialize() auto node = config().blackboard->get("node"); node->get_parameter("transform_tolerance", transform_tolerance_); - robot_base_frame_ = BT::deconflictPortAndParamFrame( + robot_base_frame_ = BT::deconflictPortAndParamFrame( node, "robot_base_frame", this); } diff --git a/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp b/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp index 64394ee3782..167caec938c 100644 --- a/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp @@ -43,9 +43,9 @@ void DistanceTraveledCondition::initialize() tf_ = config().blackboard->get>("tf_buffer"); node_->get_parameter("transform_tolerance", transform_tolerance_); - global_frame_ = BT::deconflictPortAndParamFrame( + global_frame_ = BT::deconflictPortAndParamFrame( node_, "global_frame", this); - robot_base_frame_ = BT::deconflictPortAndParamFrame( + robot_base_frame_ = BT::deconflictPortAndParamFrame( node_, "robot_base_frame", this); initialized_ = true; } diff --git a/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp b/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp index fee838c9379..61c45696f57 100644 --- a/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp @@ -16,6 +16,7 @@ #include #include "nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp" +#include "nav2_behavior_tree/bt_utils.hpp" namespace nav2_behavior_tree { @@ -33,15 +34,15 @@ BT::NodeStatus GloballyUpdatedGoalCondition::tick() { if (first_time) { first_time = false; - config().blackboard->get>("goals", goals_); - config().blackboard->get("goal", goal_); + BT::getInputOrBlackboard("goals", goals_); + BT::getInputOrBlackboard("goal", goal_); return BT::NodeStatus::SUCCESS; } std::vector current_goals; - config().blackboard->get>("goals", current_goals); + BT::getInputOrBlackboard("goals", current_goals); geometry_msgs::msg::PoseStamped current_goal; - config().blackboard->get("goal", current_goal); + BT::getInputOrBlackboard("goal", current_goal); if (goal_ != current_goal || goals_ != current_goals) { goal_ = current_goal; diff --git a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp index 2697680415b..8609ab55902 100644 --- a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp @@ -33,7 +33,7 @@ GoalReachedCondition::GoalReachedCondition( { auto node = config().blackboard->get("node"); - robot_base_frame_ = BT::deconflictPortAndParamFrame( + robot_base_frame_ = BT::deconflictPortAndParamFrame( node, "robot_base_frame", this); } diff --git a/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp b/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp index 34930bb582e..f2f112c348e 100644 --- a/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp @@ -15,6 +15,7 @@ #include #include #include "nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp" +#include "nav2_behavior_tree/bt_utils.hpp" namespace nav2_behavior_tree { @@ -28,15 +29,15 @@ GoalUpdatedCondition::GoalUpdatedCondition( BT::NodeStatus GoalUpdatedCondition::tick() { if (status() == BT::NodeStatus::IDLE) { - config().blackboard->get>("goals", goals_); - config().blackboard->get("goal", goal_); + BT::getInputOrBlackboard("goals", goals_); + BT::getInputOrBlackboard("goal", goal_); return BT::NodeStatus::FAILURE; } std::vector current_goals; - config().blackboard->get>("goals", current_goals); geometry_msgs::msg::PoseStamped current_goal; - config().blackboard->get("goal", current_goal); + BT::getInputOrBlackboard("goals", current_goals); + BT::getInputOrBlackboard("goal", current_goal); if (goal_ != current_goal || goals_ != current_goals) { goal_ = current_goal; diff --git a/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.cpp b/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.cpp index 41796834c63..0a74ce68c95 100644 --- a/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.cpp @@ -13,13 +13,21 @@ // limitations under the License. #include "nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp" +#include "nav2_behavior_tree/bt_utils.hpp" namespace nav2_behavior_tree { +InitialPoseReceived::InitialPoseReceived( + const std::string & name, + const BT::NodeConfiguration & config) +: BT::ConditionNode(name, config) +{ +} -BT::NodeStatus initialPoseReceived(BT::TreeNode & tree_node) +BT::NodeStatus InitialPoseReceived::tick() { - auto initPoseReceived = tree_node.config().blackboard->get("initial_pose_received"); + bool initPoseReceived = false; + BT::getInputOrBlackboard("initial_pose_received", initPoseReceived); return initPoseReceived ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE; } @@ -28,7 +36,5 @@ BT::NodeStatus initialPoseReceived(BT::TreeNode & tree_node) #include "behaviortree_cpp_v3/bt_factory.h" BT_REGISTER_NODES(factory) { - factory.registerSimpleCondition( - "InitialPoseReceived", - std::bind(&nav2_behavior_tree::initialPoseReceived, std::placeholders::_1)); + factory.registerNodeType("InitialPoseReceived"); } diff --git a/nav2_behavior_tree/plugins/decorator/distance_controller.cpp b/nav2_behavior_tree/plugins/decorator/distance_controller.cpp index b59fba77b45..3cef0ea978c 100644 --- a/nav2_behavior_tree/plugins/decorator/distance_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/distance_controller.cpp @@ -43,9 +43,9 @@ DistanceController::DistanceController( tf_ = config().blackboard->get>("tf_buffer"); node_->get_parameter("transform_tolerance", transform_tolerance_); - global_frame_ = BT::deconflictPortAndParamFrame( + global_frame_ = BT::deconflictPortAndParamFrame( node_, "global_frame", this); - robot_base_frame_ = BT::deconflictPortAndParamFrame( + robot_base_frame_ = BT::deconflictPortAndParamFrame( node_, "robot_base_frame", this); } diff --git a/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp b/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp index f8a2d8cefc2..049bc61f7ec 100644 --- a/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp @@ -19,7 +19,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "behaviortree_cpp_v3/decorator_node.h" #include "nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp" - +#include "nav2_behavior_tree/bt_utils.hpp" namespace nav2_behavior_tree { @@ -37,8 +37,8 @@ BT::NodeStatus GoalUpdatedController::tick() // Reset since we're starting a new iteration of // the goal updated controller (moving from IDLE to RUNNING) - config().blackboard->get>("goals", goals_); - config().blackboard->get("goal", goal_); + BT::getInputOrBlackboard("goals", goals_); + BT::getInputOrBlackboard("goal", goal_); goal_was_updated_ = true; } @@ -46,9 +46,9 @@ BT::NodeStatus GoalUpdatedController::tick() setStatus(BT::NodeStatus::RUNNING); std::vector current_goals; - config().blackboard->get>("goals", current_goals); + BT::getInputOrBlackboard("goals", current_goals); geometry_msgs::msg::PoseStamped current_goal; - config().blackboard->get("goal", current_goal); + BT::getInputOrBlackboard("goal", current_goal); if (goal_ != current_goal || goals_ != current_goals) { goal_ = current_goal; diff --git a/nav2_behavior_tree/plugins/decorator/speed_controller.cpp b/nav2_behavior_tree/plugins/decorator/speed_controller.cpp index f47dfed38a8..8d160aa76e1 100644 --- a/nav2_behavior_tree/plugins/decorator/speed_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/speed_controller.cpp @@ -19,6 +19,7 @@ #include "nav2_util/geometry_utils.hpp" #include "nav2_behavior_tree/plugins/decorator/speed_controller.hpp" +#include "nav2_behavior_tree/bt_utils.hpp" namespace nav2_behavior_tree { @@ -61,17 +62,17 @@ inline BT::NodeStatus SpeedController::tick() if (status() == BT::NodeStatus::IDLE) { // Reset since we're starting a new iteration of // the speed controller (moving from IDLE to RUNNING) - config().blackboard->get>("goals", goals_); - config().blackboard->get("goal", goal_); + BT::getInputOrBlackboard("goals", goals_); + BT::getInputOrBlackboard("goal", goal_); period_ = 1.0 / max_rate_; start_ = node_->now(); first_tick_ = true; } std::vector current_goals; - config().blackboard->get>("goals", current_goals); + BT::getInputOrBlackboard("goals", current_goals); geometry_msgs::msg::PoseStamped current_goal; - config().blackboard->get("goal", current_goal); + BT::getInputOrBlackboard("goal", current_goal); if (goal_ != current_goal || goals_ != current_goals) { // Reset state and set period to max since we have a new goal diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp index 429368ea96a..33100d76bb3 100644 --- a/nav2_behavior_tree/src/behavior_tree_engine.cpp +++ b/nav2_behavior_tree/src/behavior_tree_engine.cpp @@ -48,7 +48,7 @@ BehaviorTreeEngine::run( try { while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) { if (cancelRequested()) { - tree->rootNode()->halt(); + tree->haltTree(); return BtStatus::CANCELED; } diff --git a/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp b/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp index 001c3de67f9..20c4461acf5 100644 --- a/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp @@ -156,13 +156,13 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick) // check if returned path is correct nav_msgs::msg::Path path; - config_->blackboard->get("path", path); + EXPECT_TRUE(config_->blackboard->get("path", path)); EXPECT_EQ(path.poses.size(), 2u); EXPECT_EQ(path.poses[0].pose.position.x, 0.0); EXPECT_EQ(path.poses[1].pose.position.x, 1.0); // halt node so another goal can be sent - tree_->rootNode()->halt(); + tree_->haltTree(); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); // set new goal @@ -176,7 +176,7 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick) EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_EQ(action_server_->getCurrentGoal()->goals[0].pose.position.x, -2.5); - config_->blackboard->get("path", path); + EXPECT_TRUE(config_->blackboard->get("path", path)); EXPECT_EQ(path.poses.size(), 2u); EXPECT_EQ(path.poses[0].pose.position.x, 0.0); EXPECT_EQ(path.poses[1].pose.position.x, -2.5); @@ -222,13 +222,13 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick_use_start) // check if returned path is correct nav_msgs::msg::Path path; - config_->blackboard->get("path", path); + EXPECT_TRUE(config_->blackboard->get("path", path)); EXPECT_EQ(path.poses.size(), 2u); EXPECT_EQ(path.poses[0].pose.position.x, 2.0); EXPECT_EQ(path.poses[1].pose.position.x, 1.0); // halt node so another goal can be sent - tree_->rootNode()->halt(); + tree_->haltTree(); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); // set new goal and new start @@ -244,7 +244,7 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick_use_start) EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_EQ(action_server_->getCurrentGoal()->goals[0].pose.position.x, -2.5); - config_->blackboard->get("path", path); + EXPECT_TRUE(config_->blackboard->get("path", path)); EXPECT_EQ(path.poses.size(), 2u); EXPECT_EQ(path.poses[0].pose.position.x, -1.5); EXPECT_EQ(path.poses[1].pose.position.x, -2.5); diff --git a/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp b/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp index f1da97cb25c..6065f4543d2 100644 --- a/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp @@ -153,13 +153,13 @@ TEST_F(ComputePathToPoseActionTestFixture, test_tick) // check if returned path is correct nav_msgs::msg::Path path; - config_->blackboard->get("path", path); + EXPECT_TRUE(config_->blackboard->get("path", path)); EXPECT_EQ(path.poses.size(), 2u); EXPECT_EQ(path.poses[0].pose.position.x, 0.0); EXPECT_EQ(path.poses[1].pose.position.x, 1.0); // halt node so another goal can be sent - tree_->rootNode()->halt(); + tree_->haltTree(); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); // set new goal @@ -173,7 +173,7 @@ TEST_F(ComputePathToPoseActionTestFixture, test_tick) EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_EQ(action_server_->getCurrentGoal()->goal.pose.position.x, -2.5); - config_->blackboard->get("path", path); + EXPECT_TRUE(config_->blackboard->get("path", path)); EXPECT_EQ(path.poses.size(), 2u); EXPECT_EQ(path.poses[0].pose.position.x, 0.0); EXPECT_EQ(path.poses[1].pose.position.x, -2.5); @@ -219,13 +219,13 @@ TEST_F(ComputePathToPoseActionTestFixture, test_tick_use_start) // check if returned path is correct nav_msgs::msg::Path path; - config_->blackboard->get("path", path); + EXPECT_TRUE(config_->blackboard->get("path", path)); EXPECT_EQ(path.poses.size(), 2u); EXPECT_EQ(path.poses[0].pose.position.x, 2.0); EXPECT_EQ(path.poses[1].pose.position.x, 1.0); // halt node so another goal can be sent - tree_->rootNode()->halt(); + tree_->haltTree(); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); // set new goal and new start @@ -241,7 +241,7 @@ TEST_F(ComputePathToPoseActionTestFixture, test_tick_use_start) EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_EQ(action_server_->getCurrentGoal()->goal.pose.position.x, -2.5); - config_->blackboard->get("path", path); + EXPECT_TRUE(config_->blackboard->get("path", path)); EXPECT_EQ(path.poses.size(), 2u); EXPECT_EQ(path.poses[0].pose.position.x, -1.5); EXPECT_EQ(path.poses[1].pose.position.x, -2.5); diff --git a/nav2_behavior_tree/test/plugins/action/test_controller_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_controller_selector_node.cpp index 8c98db78b30..3218877c847 100644 --- a/nav2_behavior_tree/test/plugins/action/test_controller_selector_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_controller_selector_node.cpp @@ -95,7 +95,7 @@ TEST_F(ControllerSelectorTestFixture, test_custom_topic) // check default value std::string selected_controller_result; - config_->blackboard->get("selected_controller", selected_controller_result); + EXPECT_TRUE(config_->blackboard->get("selected_controller", selected_controller_result)); EXPECT_EQ(selected_controller_result, "DWB"); @@ -119,7 +119,7 @@ TEST_F(ControllerSelectorTestFixture, test_custom_topic) } // check controller updated - config_->blackboard->get("selected_controller", selected_controller_result); + EXPECT_TRUE(config_->blackboard->get("selected_controller", selected_controller_result)); EXPECT_EQ("DWC", selected_controller_result); } @@ -143,7 +143,7 @@ TEST_F(ControllerSelectorTestFixture, test_default_topic) // check default value std::string selected_controller_result; - config_->blackboard->get("selected_controller", selected_controller_result); + EXPECT_TRUE(config_->blackboard->get("selected_controller", selected_controller_result)); EXPECT_EQ(selected_controller_result, "GridBased"); @@ -167,7 +167,7 @@ TEST_F(ControllerSelectorTestFixture, test_default_topic) } // check controller updated - config_->blackboard->get("selected_controller", selected_controller_result); + EXPECT_TRUE(config_->blackboard->get("selected_controller", selected_controller_result)); EXPECT_EQ("RRT", selected_controller_result); } diff --git a/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp b/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp index fea7a2b2b96..47766d8e9d9 100644 --- a/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp @@ -145,7 +145,7 @@ TEST_F(FollowPathActionTestFixture, test_tick) EXPECT_EQ(action_server_->getCurrentGoal()->controller_id, std::string("FollowPath")); // halt node so another goal can be sent - tree_->rootNode()->halt(); + tree_->haltTree(); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); // set new goal diff --git a/nav2_behavior_tree/test/plugins/action/test_goal_checker_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_goal_checker_selector_node.cpp index 1fd0e286cb3..747686000ed 100644 --- a/nav2_behavior_tree/test/plugins/action/test_goal_checker_selector_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_goal_checker_selector_node.cpp @@ -95,7 +95,7 @@ TEST_F(GoalCheckerSelectorTestFixture, test_custom_topic) // check default value std::string selected_goal_checker_result; - config_->blackboard->get("selected_goal_checker", selected_goal_checker_result); + EXPECT_TRUE(config_->blackboard->get("selected_goal_checker", selected_goal_checker_result)); EXPECT_EQ(selected_goal_checker_result, "SimpleGoalCheck"); @@ -119,7 +119,7 @@ TEST_F(GoalCheckerSelectorTestFixture, test_custom_topic) } // check goal_checker updated - config_->blackboard->get("selected_goal_checker", selected_goal_checker_result); + EXPECT_TRUE(config_->blackboard->get("selected_goal_checker", selected_goal_checker_result)); EXPECT_EQ("AngularGoalChecker", selected_goal_checker_result); } @@ -143,7 +143,7 @@ TEST_F(GoalCheckerSelectorTestFixture, test_default_topic) // check default value std::string selected_goal_checker_result; - config_->blackboard->get("selected_goal_checker", selected_goal_checker_result); + EXPECT_TRUE(config_->blackboard->get("selected_goal_checker", selected_goal_checker_result)); EXPECT_EQ(selected_goal_checker_result, "GridBased"); @@ -167,7 +167,7 @@ TEST_F(GoalCheckerSelectorTestFixture, test_default_topic) } // check goal_checker updated - config_->blackboard->get("selected_goal_checker", selected_goal_checker_result); + EXPECT_TRUE(config_->blackboard->get("selected_goal_checker", selected_goal_checker_result)); EXPECT_EQ("RRT", selected_goal_checker_result); } diff --git a/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp b/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp index 90cdfbbb0ca..73de1ac53f5 100644 --- a/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp @@ -148,7 +148,7 @@ TEST_F(NavigateThroughPosesActionTestFixture, test_tick) EXPECT_EQ(action_server_->getCurrentGoal()->poses, poses); // halt node so another goal can be sent - tree_->rootNode()->halt(); + tree_->haltTree(); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); } diff --git a/nav2_behavior_tree/test/plugins/action/test_navigate_to_pose_action.cpp b/nav2_behavior_tree/test/plugins/action/test_navigate_to_pose_action.cpp index 5b3764befb9..aef672b940c 100644 --- a/nav2_behavior_tree/test/plugins/action/test_navigate_to_pose_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_navigate_to_pose_action.cpp @@ -139,7 +139,7 @@ TEST_F(NavigateToPoseActionTestFixture, test_tick) EXPECT_EQ(action_server_->getCurrentGoal()->pose, pose); // halt node so another goal can be sent - tree_->rootNode()->halt(); + tree_->haltTree(); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); // set new goal diff --git a/nav2_behavior_tree/test/plugins/action/test_planner_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_planner_selector_node.cpp index b8851d81c36..efc59adb6c8 100644 --- a/nav2_behavior_tree/test/plugins/action/test_planner_selector_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_planner_selector_node.cpp @@ -93,7 +93,7 @@ TEST_F(PlannerSelectorTestFixture, test_custom_topic) // check default value std::string selected_planner_result; - config_->blackboard->get("selected_planner", selected_planner_result); + EXPECT_TRUE(config_->blackboard->get("selected_planner", selected_planner_result)); EXPECT_EQ(selected_planner_result, "GridBased"); @@ -117,7 +117,7 @@ TEST_F(PlannerSelectorTestFixture, test_custom_topic) } // check planner updated - config_->blackboard->get("selected_planner", selected_planner_result); + EXPECT_TRUE(config_->blackboard->get("selected_planner", selected_planner_result)); EXPECT_EQ("RRT", selected_planner_result); } @@ -141,7 +141,7 @@ TEST_F(PlannerSelectorTestFixture, test_default_topic) // check default value std::string selected_planner_result; - config_->blackboard->get("selected_planner", selected_planner_result); + EXPECT_TRUE(config_->blackboard->get("selected_planner", selected_planner_result)); EXPECT_EQ(selected_planner_result, "GridBased"); @@ -165,7 +165,7 @@ TEST_F(PlannerSelectorTestFixture, test_default_topic) } // check planner updated - config_->blackboard->get("selected_planner", selected_planner_result); + EXPECT_TRUE(config_->blackboard->get("selected_planner", selected_planner_result)); EXPECT_EQ("RRT", selected_planner_result); } diff --git a/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp b/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp index 7bfda58e548..a8ac368a160 100644 --- a/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp @@ -137,7 +137,7 @@ TEST_F(RemovePassedGoalsTestFixture, test_tick) // check that it removed the point in range std::vector output_poses; - config_->blackboard->get("goals", output_poses); + EXPECT_TRUE(config_->blackboard->get("goals", output_poses)); EXPECT_EQ(output_poses.size(), 2u); EXPECT_EQ(output_poses[0], poses[2]); diff --git a/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp b/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp index 6460a5d9b95..917b696acf8 100644 --- a/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp @@ -138,7 +138,7 @@ TEST_F(SmoothPathActionTestFixture, test_tick) EXPECT_EQ(action_server_->getCurrentGoal()->path, path); // halt node so another goal can be sent - tree_->rootNode()->halt(); + tree_->haltTree(); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); // set new goal diff --git a/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp index de73cc4fe58..c3476615069 100644 --- a/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp @@ -95,7 +95,7 @@ TEST_F(SmootherSelectorTestFixture, test_custom_topic) // check default value std::string selected_smoother_result; - config_->blackboard->get("selected_smoother", selected_smoother_result); + EXPECT_TRUE(config_->blackboard->get("selected_smoother", selected_smoother_result)); EXPECT_EQ(selected_smoother_result, "DWB"); @@ -119,7 +119,7 @@ TEST_F(SmootherSelectorTestFixture, test_custom_topic) } // check smoother updated - config_->blackboard->get("selected_smoother", selected_smoother_result); + EXPECT_TRUE(config_->blackboard->get("selected_smoother", selected_smoother_result)); EXPECT_EQ("DWC", selected_smoother_result); } @@ -143,7 +143,7 @@ TEST_F(SmootherSelectorTestFixture, test_default_topic) // check default value std::string selected_smoother_result; - config_->blackboard->get("selected_smoother", selected_smoother_result); + EXPECT_TRUE(config_->blackboard->get("selected_smoother", selected_smoother_result)); EXPECT_EQ(selected_smoother_result, "GridBased"); @@ -167,7 +167,7 @@ TEST_F(SmootherSelectorTestFixture, test_default_topic) } // check smoother updated - config_->blackboard->get("selected_smoother", selected_smoother_result); + EXPECT_TRUE(config_->blackboard->get("selected_smoother", selected_smoother_result)); EXPECT_EQ("RRT", selected_smoother_result); } diff --git a/nav2_behavior_tree/test/plugins/action/test_truncate_path_action.cpp b/nav2_behavior_tree/test/plugins/action/test_truncate_path_action.cpp index 9ca640d47dd..3610cd41a9e 100644 --- a/nav2_behavior_tree/test/plugins/action/test_truncate_path_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_truncate_path_action.cpp @@ -127,7 +127,7 @@ TEST_F(TruncatePathTestFixture, test_tick) } nav_msgs::msg::Path truncated_path; - config_->blackboard->get("truncated_path", truncated_path); + EXPECT_TRUE(config_->blackboard->get("truncated_path", truncated_path)); EXPECT_NE(path, truncated_path); EXPECT_EQ(truncated_path.poses.size(), 2u); diff --git a/nav2_behavior_tree/test/plugins/action/test_truncate_path_local_action.cpp b/nav2_behavior_tree/test/plugins/action/test_truncate_path_local_action.cpp index 00564d4e8fb..0559d6effe3 100644 --- a/nav2_behavior_tree/test/plugins/action/test_truncate_path_local_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_truncate_path_local_action.cpp @@ -138,7 +138,7 @@ TEST_F(TruncatePathLocalTestFixture, test_tick) } nav_msgs::msg::Path truncated_path; - config_->blackboard->get("truncated_path", truncated_path); + EXPECT_TRUE(config_->blackboard->get("truncated_path", truncated_path)); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_NE(path, truncated_path); @@ -160,7 +160,7 @@ TEST_F(TruncatePathLocalTestFixture, test_tick) { tree_->rootNode()->executeTick(); } - config_->blackboard->get("truncated_path", truncated_path); + EXPECT_TRUE(config_->blackboard->get("truncated_path", truncated_path)); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_NE(path, truncated_path); @@ -182,7 +182,7 @@ TEST_F(TruncatePathLocalTestFixture, test_tick) { tree_->rootNode()->executeTick(); } - config_->blackboard->get("truncated_path", truncated_path); + EXPECT_TRUE(config_->blackboard->get("truncated_path", truncated_path)); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_NE(path, truncated_path); @@ -233,7 +233,7 @@ TEST_F(TruncatePathLocalTestFixture, test_success_on_empty_path) tree_->rootNode()->executeTick(); } nav_msgs::msg::Path truncated_path; - config_->blackboard->get("truncated_path", truncated_path); + EXPECT_TRUE(config_->blackboard->get("truncated_path", truncated_path)); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_EQ(path, truncated_path); @@ -276,7 +276,7 @@ TEST_F(TruncatePathLocalTestFixture, test_failure_on_no_pose) tree_->rootNode()->executeTick(); } nav_msgs::msg::Path truncated_path; - config_->blackboard->get("truncated_path", truncated_path); + EXPECT_TRUE(config_->blackboard->get("truncated_path", truncated_path)); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::FAILURE); SUCCEED(); @@ -317,7 +317,7 @@ TEST_F(TruncatePathLocalTestFixture, test_failure_on_invalid_robot_frame) tree_->rootNode()->executeTick(); } nav_msgs::msg::Path truncated_path; - config_->blackboard->get("truncated_path", truncated_path); + EXPECT_TRUE(config_->blackboard->get("truncated_path", truncated_path)); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::FAILURE); SUCCEED(); @@ -362,7 +362,7 @@ TEST_F(TruncatePathLocalTestFixture, test_path_pruning) { tree_->rootNode()->executeTick(); } - config_->blackboard->get("truncated_path", truncated_path); + EXPECT_TRUE(config_->blackboard->get("truncated_path", truncated_path)); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_NE(path, truncated_path); @@ -395,7 +395,7 @@ TEST_F(TruncatePathLocalTestFixture, test_path_pruning) { tree_->rootNode()->executeTick(); } - config_->blackboard->get("truncated_path", truncated_path); + EXPECT_TRUE(config_->blackboard->get("truncated_path", truncated_path)); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_NE(path, truncated_path); @@ -428,7 +428,7 @@ TEST_F(TruncatePathLocalTestFixture, test_path_pruning) { tree_->rootNode()->executeTick(); } - config_->blackboard->get("truncated_path", truncated_path); + EXPECT_TRUE(config_->blackboard->get("truncated_path", truncated_path)); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_NE(path, truncated_path); diff --git a/nav2_behavior_tree/test/plugins/condition/test_initial_pose_received.cpp b/nav2_behavior_tree/test/plugins/condition/test_initial_pose_received.cpp index 1f9aa41bb84..7b8a67529ed 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_initial_pose_received.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_initial_pose_received.cpp @@ -21,48 +21,30 @@ #include "utils/test_behavior_tree_fixture.hpp" #include "nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp" -class TestNode : public BT::SyncActionNode -{ -public: - TestNode(const std::string & name, const BT::NodeConfiguration & config) - : SyncActionNode(name, config) - {} - - BT::NodeStatus tick() - { - return BT::NodeStatus::SUCCESS; - } - - static BT::PortsList providedPorts() - { - return {}; - } -}; - class InitialPoseReceivedConditionTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture { public: void SetUp() { - test_node_ = std::make_shared("TestNode", *config_); + bt_node_ = std::make_shared("TestNode", *config_); } void TearDown() { - test_node_.reset(); + bt_node_.reset(); } protected: - static std::shared_ptr test_node_; + static std::shared_ptr bt_node_; }; -std::shared_ptr InitialPoseReceivedConditionTestFixture::test_node_ = nullptr; +std::shared_ptr InitialPoseReceivedConditionTestFixture::bt_node_ = nullptr; TEST_F(InitialPoseReceivedConditionTestFixture, test_behavior) { - EXPECT_EQ(nav2_behavior_tree::initialPoseReceived(*test_node_), BT::NodeStatus::FAILURE); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); config_->blackboard->set("initial_pose_received", true); - EXPECT_EQ(nav2_behavior_tree::initialPoseReceived(*test_node_), BT::NodeStatus::SUCCESS); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); } int main(int argc, char ** argv) diff --git a/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp b/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp index 221371a4143..1adde82318b 100644 --- a/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp +++ b/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp @@ -107,7 +107,7 @@ TEST_F(GoalUpdaterTestFixture, test_tick) // tick tree without publishing updated goal and get updated_goal tree_->rootNode()->executeTick(); geometry_msgs::msg::PoseStamped updated_goal; - config_->blackboard->get("updated_goal", updated_goal); + EXPECT_TRUE(config_->blackboard->get("updated_goal", updated_goal)); } TEST_F(GoalUpdaterTestFixture, test_older_goal_update) @@ -141,7 +141,7 @@ TEST_F(GoalUpdaterTestFixture, test_older_goal_update) goal_updater_pub->publish(goal_to_update); tree_->rootNode()->executeTick(); geometry_msgs::msg::PoseStamped updated_goal; - config_->blackboard->get("updated_goal", updated_goal); + EXPECT_TRUE(config_->blackboard->get("updated_goal", updated_goal)); // expect to succeed and not update goal EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); @@ -184,7 +184,7 @@ TEST_F(GoalUpdaterTestFixture, test_get_latest_goal_update) goal_updater_pub->publish(goal_to_update_2); tree_->rootNode()->executeTick(); geometry_msgs::msg::PoseStamped updated_goal; - config_->blackboard->get("updated_goal", updated_goal); + EXPECT_TRUE(config_->blackboard->get("updated_goal", updated_goal)); // expect to succeed EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); diff --git a/nav2_behavior_tree/test/test_bt_utils.cpp b/nav2_behavior_tree/test/test_bt_utils.cpp index 076b9c3b7c4..238dee27dcd 100644 --- a/nav2_behavior_tree/test/test_bt_utils.cpp +++ b/nav2_behavior_tree/test/test_bt_utils.cpp @@ -310,12 +310,12 @@ TEST(deconflictPortAndParamFrameTest, test_correct_syntax) node->declare_parameter("test", 2); node->declare_parameter("test_alternative", 3); - int value = BT::deconflictPortAndParamFrame( + int value = BT::deconflictPortAndParamFrame( node, "test_alternative", tree.rootNode()); EXPECT_EQ(value, 3); - value = BT::deconflictPortAndParamFrame( + value = BT::deconflictPortAndParamFrame( node, "test", tree.rootNode()); EXPECT_EQ(value, 1); From 913f7b6e3756fa325057b64a6603959e00b9b31f Mon Sep 17 00:00:00 2001 From: Ryan Date: Tue, 13 Feb 2024 15:31:47 -0700 Subject: [PATCH 03/36] Use ament_export_targets for all targets (#4112) * Matches new internal ALIAS targets * Use ALIAS targets for all internal linkage * Remove unnecessary calls to ament_target_dependencies in test code * Export includes in proper folders for overlays in colcon Signed-off-by: Ryan Friedman --- nav2_costmap_2d/CMakeLists.txt | 53 ++++++++-------- .../test/integration/CMakeLists.txt | 60 ++++++------------- .../test/regression/CMakeLists.txt | 6 +- nav2_costmap_2d/test/unit/CMakeLists.txt | 29 ++++----- 4 files changed, 64 insertions(+), 84 deletions(-) diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt index df0ddb2dc3a..723f81f0f9b 100644 --- a/nav2_costmap_2d/CMakeLists.txt +++ b/nav2_costmap_2d/CMakeLists.txt @@ -26,17 +26,10 @@ find_package(visualization_msgs REQUIRED) find_package(angles REQUIRED) remove_definitions(-DDISABLE_LIBUSB-1.0) -find_package(Eigen3 REQUIRED) +find_package(Eigen3 3.3 REQUIRED) nav2_package() -include_directories( - include - ${EIGEN3_INCLUDE_DIRS} -) - -add_definitions(${EIGEN3_DEFINITIONS}) - add_library(nav2_costmap_2d_core SHARED src/costmap_2d.cpp src/layer.cpp @@ -51,6 +44,13 @@ add_library(nav2_costmap_2d_core SHARED src/footprint_collision_checker.cpp plugins/costmap_filters/costmap_filter.cpp ) +add_library(${PROJECT_NAME}::nav2_costmap_2d_core ALIAS nav2_costmap_2d_core) + +target_include_directories(nav2_costmap_2d_core + PUBLIC + "$" + "$" +) set(dependencies geometry_msgs @@ -78,6 +78,7 @@ set(dependencies ament_target_dependencies(nav2_costmap_2d_core ${dependencies} ) +target_link_libraries(nav2_costmap_2d_core Eigen3::Eigen) add_library(layers SHARED plugins/inflation_layer.cpp @@ -88,11 +89,12 @@ add_library(layers SHARED plugins/range_sensor_layer.cpp plugins/denoise_layer.cpp ) +add_library(${PROJECT_NAME}::layers ALIAS layers) ament_target_dependencies(layers ${dependencies} ) target_link_libraries(layers - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) add_library(filters SHARED @@ -100,11 +102,14 @@ add_library(filters SHARED plugins/costmap_filters/speed_filter.cpp plugins/costmap_filters/binary_filter.cpp ) +add_library(${PROJECT_NAME}::filters ALIAS filters) + + ament_target_dependencies(filters ${dependencies} ) target_link_libraries(filters - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) add_library(nav2_costmap_2d_client SHARED @@ -112,18 +117,19 @@ add_library(nav2_costmap_2d_client SHARED src/costmap_subscriber.cpp src/costmap_topic_collision_checker.cpp ) +add_library(${PROJECT_NAME}::nav2_costmap_2d_client ALIAS nav2_costmap_2d_client) ament_target_dependencies(nav2_costmap_2d_client ${dependencies} ) target_link_libraries(nav2_costmap_2d_client - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) add_executable(nav2_costmap_2d_markers src/costmap_2d_markers.cpp) target_link_libraries(nav2_costmap_2d_markers - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) ament_target_dependencies(nav2_costmap_2d_markers @@ -132,7 +138,7 @@ ament_target_dependencies(nav2_costmap_2d_markers add_executable(nav2_costmap_2d_cloud src/costmap_2d_cloud.cpp) target_link_libraries(nav2_costmap_2d_cloud - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) add_executable(nav2_costmap_2d src/costmap_2d_node.cpp) @@ -141,26 +147,24 @@ ament_target_dependencies(nav2_costmap_2d ) target_link_libraries(nav2_costmap_2d - nav2_costmap_2d_core - layers - filters + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::layers + ${PROJECT_NAME}::filters ) install(TARGETS - nav2_costmap_2d_core layers filters + nav2_costmap_2d_core nav2_costmap_2d_client + EXPORT export_${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin ) -install(TARGETS - nav2_costmap_2d - nav2_costmap_2d_markers - nav2_costmap_2d_cloud - RUNTIME DESTINATION lib/${PROJECT_NAME} +install(TARGETS nav2_costmap_2d_markers nav2_costmap_2d_cloud nav2_costmap_2d + DESTINATION lib/${PROJECT_NAME} ) install(FILES costmap_plugins.xml @@ -168,7 +172,7 @@ install(FILES costmap_plugins.xml ) install(DIRECTORY include/ - DESTINATION include/ + DESTINATION include/${PROJECT_NAME} ) if(BUILD_TESTING) @@ -183,8 +187,7 @@ if(BUILD_TESTING) pluginlib_export_plugin_description_file(nav2_costmap_2d test/regression/order_layer.xml) endif() -ament_export_include_directories(include) -ament_export_libraries(layers filters nav2_costmap_2d_core nav2_costmap_2d_client) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) ament_export_dependencies(${dependencies}) pluginlib_export_plugin_description_file(nav2_costmap_2d costmap_plugins.xml) ament_package() diff --git a/nav2_costmap_2d/test/integration/CMakeLists.txt b/nav2_costmap_2d/test/integration/CMakeLists.txt index 868b5081a1b..7fbef3d41db 100644 --- a/nav2_costmap_2d/test/integration/CMakeLists.txt +++ b/nav2_costmap_2d/test/integration/CMakeLists.txt @@ -1,90 +1,66 @@ ament_add_gtest_executable(footprint_tests_exec footprint_tests.cpp ) -ament_target_dependencies(footprint_tests_exec - ${dependencies} -) target_link_libraries(footprint_tests_exec - nav2_costmap_2d_core - layers + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::layers ) ament_add_gtest_executable(test_collision_checker_exec test_costmap_topic_collision_checker.cpp ) -ament_target_dependencies(test_collision_checker_exec - ${dependencies} -) target_link_libraries(test_collision_checker_exec - nav2_costmap_2d_core - nav2_costmap_2d_client - layers + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_client + ${PROJECT_NAME}::layers ) ament_add_gtest_executable(inflation_tests_exec inflation_tests.cpp ) -ament_target_dependencies(inflation_tests_exec - ${dependencies} -) target_link_libraries(inflation_tests_exec - nav2_costmap_2d_core - layers + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::layers ) ament_add_gtest_executable(obstacle_tests_exec obstacle_tests.cpp ) -ament_target_dependencies(obstacle_tests_exec - ${dependencies} -) target_link_libraries(obstacle_tests_exec - nav2_costmap_2d_core - layers + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::layers ) ament_add_gtest_executable(range_tests_exec range_tests.cpp ) -ament_target_dependencies(range_tests_exec - ${dependencies} -) target_link_libraries(range_tests_exec - nav2_costmap_2d_core - layers + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::layers ) ament_add_gtest(dyn_params_tests dyn_params_tests.cpp ) -ament_target_dependencies(dyn_params_tests - ${dependencies} -) target_link_libraries(dyn_params_tests - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) ament_add_gtest_executable(test_costmap_publisher_exec test_costmap_2d_publisher.cpp ) -ament_target_dependencies(test_costmap_publisher_exec - ${dependencies} -) target_link_libraries(test_costmap_publisher_exec - nav2_costmap_2d_core - nav2_costmap_2d_client - layers + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_client + ${PROJECT_NAME}::layers ) ament_add_gtest_executable(test_costmap_subscriber_exec test_costmap_subscriber.cpp ) -ament_target_dependencies(test_costmap_subscriber_exec - ${dependencies} -) target_link_libraries(test_costmap_subscriber_exec - nav2_costmap_2d_core - nav2_costmap_2d_client + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_client ) ament_add_test(test_collision_checker @@ -166,6 +142,6 @@ ament_add_test(test_costmap_subscriber_exec # ${dependencies} # ) # target_link_libraries(costmap_tester -# nav2_costmap_2d_core +# ${PROJECT_NAME}::nav2_costmap_2d_core # layers # ) diff --git a/nav2_costmap_2d/test/regression/CMakeLists.txt b/nav2_costmap_2d/test/regression/CMakeLists.txt index 78fbfa6cf79..aafc4d561be 100644 --- a/nav2_costmap_2d/test/regression/CMakeLists.txt +++ b/nav2_costmap_2d/test/regression/CMakeLists.txt @@ -1,7 +1,7 @@ # Bresenham2D corner cases test ament_add_gtest(costmap_bresenham_2d costmap_bresenham_2d.cpp) target_link_libraries(costmap_bresenham_2d - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) # OrderLayer for checking Costmap2D plugins API calling order @@ -11,7 +11,7 @@ ament_target_dependencies(order_layer ${dependencies} ) target_link_libraries(order_layer - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) install(TARGETS order_layer @@ -23,5 +23,5 @@ install(TARGETS # Costmap2D plugins API calling order test ament_add_gtest(plugin_api_order plugin_api_order.cpp) target_link_libraries(plugin_api_order - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) diff --git a/nav2_costmap_2d/test/unit/CMakeLists.txt b/nav2_costmap_2d/test/unit/CMakeLists.txt index e69501277ea..d3c5edd813c 100644 --- a/nav2_costmap_2d/test/unit/CMakeLists.txt +++ b/nav2_costmap_2d/test/unit/CMakeLists.txt @@ -1,57 +1,58 @@ ament_add_gtest(collision_footprint_test footprint_collision_checker_test.cpp) target_link_libraries(collision_footprint_test - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) ament_add_gtest(costmap_convesion_test costmap_conversion_test.cpp) target_link_libraries(costmap_convesion_test - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) ament_add_gtest(declare_parameter_test declare_parameter_test.cpp) target_link_libraries(declare_parameter_test - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) ament_add_gtest(costmap_filter_test costmap_filter_test.cpp) target_link_libraries(costmap_filter_test - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) ament_add_gtest(keepout_filter_test keepout_filter_test.cpp) target_link_libraries(keepout_filter_test - nav2_costmap_2d_core - filters + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::filters ) ament_add_gtest(speed_filter_test speed_filter_test.cpp) target_link_libraries(speed_filter_test - nav2_costmap_2d_core - filters + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::filters ) ament_add_gtest(binary_filter_test binary_filter_test.cpp) target_link_libraries(binary_filter_test - nav2_costmap_2d_core - filters + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::filters ) ament_add_gtest(copy_window_test copy_window_test.cpp) target_link_libraries(copy_window_test - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) ament_add_gtest(costmap_filter_service_test costmap_filter_service_test.cpp) target_link_libraries(costmap_filter_service_test - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) ament_add_gtest(denoise_layer_test denoise_layer_test.cpp image_test.cpp image_processing_test.cpp) target_link_libraries(denoise_layer_test - nav2_costmap_2d_core layers + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::layers ) ament_add_gtest(lifecycle_test lifecycle_test.cpp) target_link_libraries(lifecycle_test - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) \ No newline at end of file From 1e7a0f71ff0be53481cd67c629c372df52785e10 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 15 Feb 2024 10:26:11 -0800 Subject: [PATCH 04/36] adding new Friends of Nav2 Sponsors! --- README.md | 2 +- doc/sponsors_jan_2024.png | Bin 0 -> 184628 bytes 2 files changed, 1 insertion(+), 1 deletion(-) create mode 100644 doc/sponsors_jan_2024.png diff --git a/README.md b/README.md index 3fbb186d458..e8eecf89439 100644 --- a/README.md +++ b/README.md @@ -27,7 +27,7 @@ If you need professional services related to Nav2, please contact Open Navigatio Please thank our amazing sponsors for their generous support of Nav2 on behalf of the community to allow the project to continue to be professionally maintained, developed, and supported for the long-haul! [Open Navigation LLC](https://www.opennav.org/) provides project leadership, maintenance, development, and support services to the Nav2 & ROS community.

- +

### [Dexory](https://www.dexory.com/) develops robotics and AI logistics solutions to drive better business decisions using a digital twin of warehouses to provide inventory insights. diff --git a/doc/sponsors_jan_2024.png b/doc/sponsors_jan_2024.png new file mode 100644 index 0000000000000000000000000000000000000000..601df4909c92e83c7773a2aa47669ddd76cc998b GIT binary patch literal 184628 zcmeFZRa}%`)IClp24#YTbcmGFA>A!4Ez;ec1|lV0LwDC84TI7(baxEh-OT@>-}im5 z|Mx#{{5)LDJm;K!_Fj9fwI|@cyae_`!iOj*DA-bxqDm+z_k2-M?vUL_1AqC!HoAy{ z;)^0BDx~77yES9#Y0ew~>qdUc=T z^$)T%vR8wWF1sI>SbuvZwR_kceYA|5*u1RqE|Fh@RH*lOjumPkSLwl*{P)lI*%j%& z&;LAu|MzIgd@=s-IZqG;`~RMlKBEx+-%}>{lBNMhj;q_?}_aB{Z!Qd zeH48CND%#hpL|vR-xvPxX8gY`{ok!X`TrR-+T>409)4p-`~2wm>ytSPGJ10h3qN+Q z8^w@kH5`GYOg@?+l2qrXgL?2%lj^+6lLEwScB8qud551${aX|=BSd{cp8f%iu+_p2 zO~8O1#b=^Ow&9S37w<^!aMgWwzn}W~X~*xk-0Yj9*Z3yP0^(jJy?V!W-n4rw4XJgw zdgt%F4v)^K@X2?ZC}q;--0ZjIKN>rK zK3h1D*gtP_DU(9%MognXHk(u%_c}t?+L3mK@9|$IP}MZkl^m*j&DDpr@Avm~3@GU; zMn|egMZ{b$7?_JX3a?FZ4YEG)BzXSl*}@Y3MaSuJ)$!SSf(dhaMYMfkwF^Gn>GDrd{g)?({XWFpBpr<3YnB#nmOc&^Na)=) z{f@DP`#yPnVINHJT7!ae)M0aX?P2D#?0VT`8Ym{4N5$SJf0D5 z;{O58@|hkP)E-TxMXnKITC5(cd_+CT9>w4$VJHryW z!GV69G9s;8P?7FTM4eRGRlDu6=JbAGv(UW1qAE?8^yzasOem3rtX*>YuCZamAMbsp zQ)~&Bm-W)_0)NuV9t05RK@T>S%nDb8;HR{Q#~;< z#&@+8m`clmAu!%+Ay{6`wY#>%x|g4;F3Nq;Q&rQgLGEE^c^iWhS~1VaEq9D|(O1lK z^_W*zRo%C@$5bA2m~ExuKk)KG3^#=E_AxmNyqU5qD4y8ZpOm>5>hhk{c*e`OrvAI# z*sOSxs)^H9o&paQa+AMY$3~)T_B(+#@Av8yQig^a5zn8+JhTv9Ou%G( z&70HTWBv48h`LX1cvnTlrg0m|r?F9cF*6(zb(wHXpwIdVdgw^$Lho-?Y=OJI4|86! ze(xwt!L1MOoQ2`hoyQL?JgK}{UW{3hPHNltx!>aMFmc8cc7H`XSdGp{~W@aVcf2&F5*Tk zdxcCRb-&jm(={`BWDF??uJ2E;-9k38int}o2b6OavEy^L*ISCWG zi6aPN|E}dS^u|0Rld=YxrCx-nxEb>@K1E%D7;NXPF_)Ll)puO`=8UyY;p$@CZM=Mx zqN%19z`e^Rh%)o#xRHca6+;s%fo<-$c>@Ueqsp8dTbTtSer|bk5ftU@`Y7o^huDj`%Z%+2&j&PS{$tJ zM~wGT*|4c|Qb(jlQcwH2mwv^>jazD09S@%uz!Y}{)K$BCTeQL@pPjyif0~T};VQL* z3Mts(cRIH-Bglr`FbyhldrY{TAtM2KQZ}cM2}s zR#Y_v!+14ObVqMB8rEGautLhTL^a&9Vo%vu#E-$&&Ww?Eb~EGI;9foA*0Vnh!q^E=LEdOt1S7-xD2Se(>G6NT#3Jb#oDrlzDxj_#0tj;1g8 z9swe6qKmXsRi@i;IVjAcmPk{ z#>E#gcQN~iPRz9tv#-|&Cm5$|+*WHOU?p>{G|!*M8mK8A{<|uSw;}O+@e;R_hJ&b+ zUC3T0ml0aA(OgJ+?)UIb2|_(w=X9@g^Cn}P%7uBE-7I^v{hK4mzAisRD>NqV+E6Sq zYacu5ELoXXmCdcyB&lL|?8Oz8-%Ld)_gm*^pE~0cEKG6P%>6nn)cfcPiqG=0>e_EM z+&le_Y9gnw$oxOxN@qJKIFr^dl0FL$kMFx6mL<_ET}`pzyQc|t8rdi`u6A7h^Q{Go zz!j@s31+y={Gl>bdz;#Y89w5%mgwbgW4QcrANwOZn(6$VJN;2A%{^-*5IjRNljUlw zc9_hm*Oy2omI18@jywIi;sxC3rs)t`*F7##tK4WrOuzPalnG?qwWWnR!vTreAurS2 zLCqWXr^O+)p(t zs4c-i8CHM!&ZAImCUK2#c1(D-1@C$6VHumhBbl!_yC$4iaBOrmA}zfAgf%Iia4h_$ z;gg;+Zr_kts}ZO}Na3AEdcL1!6-QlqP6{s_V~ekHO7AzGg{r6@J`9p3y=wPW9U5P#nK#K=4TAYALtk1d>Jme6D^-t1yykcSN_V(v5sJiq@o?t&Zbb3fAU zTk(Xsy;baV*)Se~TxfFTUbtQ^17j-=3oa^~EgG7sdC*bJ3FNkHT$POF?JfAd8 zk0GQ8dUWxLVw*lW4!)C?rU&IV8Md~BSY%hTu;X;faGr6RfN-5$iZNlnR5|j%h7{S< z#5jITZulWB-Pz=DB!Gj;rZr-#i*@EvyxsJ2wfM~YEUs&@spa`|)l1wt6#`T82|-9u zB2CaVZi|<+kvoR_p^R*A;%=?FZgX5(-HQp8kOhuQoVkup#wRwoD6OCw1U)B+6t7?9 z`F}uV-oo2?=eXEgF!t6p@$w$|-vcETe2N(M-a{PE+u=k^i8xd!4#ry}jXXL$DW-SWGgNR30JE8|9-J zJSG9#mBFRzP)VN~syqF`YIQrcU)yPE zeV^bzuVc~m#8p+)SjSAV9-*14WQceD!aytF)~eWN*^3a z_{myzCay(2lT~n&m%kWL4bX}>6wLJ(%6SO#dZUH##`cCF}T z?H)Y***5h;j{Mo^89!!j_?Y}|`R|}tdy}o_i&z8bt zF)_(VD}`jfmSGa{QhWIKj2NE?`V^6lQD+-MCan$Wn?d>`+&i6ih)vle@Jy?Y4;Pv_ zuKUb)PeXb~98wUH4({K*l^UO*#G-Y*gXBBkYKr*SbjQTfLZvLk+OmIfIskC0Okk?d z?=l2hgjsR#{M=k|tZUv!g5|o1(FqOsM`-dCM@dkc&C=@$w|aFY&9c)y>|=e0@rz9J zOA>bn{OzkpDS-s<1jLJb>{66g87F4slFDe8%|!}-Cb_+T{~<5|^Ub|~$0c46U9YC@ zlo3$Nw6hs{#)pZFhz=#%E)lv#4VtfgY4Y8!{9z^j3ij62%rI3pbqg^od$B-oqy~Tu z*!F2Z17wn|5ll7cbj-*4qcGHk7r!{kPN$~pKh2peSXT)%OWqTA+OzZu zs%Y=!{}C)dzrQ!Ufluxm6R!l8%4KpLoq$T9ckn@jlZ>0)0=7BJ)PPN$AmTuG%zAhL zOO90kW;cWz6{EVt?^@A!pQw`LjiVy#2ZZ2!QDKegEkn#P71<#D_d45QH z%9+3FVJXLvg2>;+yz#aE@WWs(ZBu9X1<@D)(2avjAGl%B7cUX9a5ytX-H)33h-nLs z-FN;a^Bj*J`t|9FmCQPRg2EEMe1D)7WH6{~*l6~%tm@YsbnMdIS)P1$`Qq;VRHM8} zy3HFtuYm(635q%DH)Z1ABvpvcSOP5_h(>kCA*ppo0Z1YS+GD=Vl7l^|4pE(xm=Ezh z76f>6D9-;vj1gVT-CO!vmKN?fQ@Pp%GC(W!@{11kmw#`}7u2J%3p*Vp=@nvHg-VzK zO-|~J$v60VP-ww^+Hl+6*%#JWPf$#xc6cB`#NTK*ND#iyY;fDd-nxm2jZEb#Ow}{V zvwkQZAA5#redT={UN70Eya#$Q$F3rj%i}v{);^QVQ=GSnJ}q_cOHfwU`oHaCV1iZ z54iWJ%vi2@8?(a4h`HS9U$3x|AUPiYT@DnL?GOI_>#r1`W^_yo6F+s-nFXCbiXhem z`Qw3CF0~We>=~8?9cCCq=Y$dG*R*eu|M>ByLpa_Ut^6$%xkl7 za2r{>92u(z_1dKdQEUH@flq(C(0smf<-NT*p7;BY`2PR}i`sVW<_W2l$x{9G^fK*4 zjbAl3lJ+l_Xhk=*KZuD^1B(l?3_z?5XeW(eCB|+Sw6K=igz2%Cq>dX!b83~mjx;Yx zuiBg+S&XpXr?HcA6I%}P=u6UW_#W)?(ITBxYea0s7Ls<^mSjRXM}7ZJT>RM_ScZ8; z#2C03Z(Rn8+ZE>HLPsXL^s6ydU0;Q;DZ~xs3qY}ikU%YvQSSefeO-pfl^Hb@IED(=kMo`o82h8_T zE-bDk$=-4g>35P;;w_R@Y!sur_3^IJ1oq z_9rw-s;xnuNVlb*HKq2S4%`2U+DBQ8L`%yc^s0}en?(B z22@371nZb_jD*{Xnfhg?@X{kehcwMji#%LVQM}36-u;}!(j+daZYQfXjBP z{*wQXs_HusvRLcpx-fz__FZU|F-qhn{y!)3V@u`?2zfhD{tGS`2(G$3B85SR8QAWD ze`ua3|4u&=;QU4QQ+KF3wmyRra2_fmWa{w zPz#AAA7K=DI0gRg`qh4FU8hJysj3XPQGgpmB5)~qixpzw9MU`vi5qx}k8C~ur2KGs zfcP|+H^$JZBWlI&Nfkd`!hI9}FpS_|FX@mu`Fb-Sy9`oiyE-FL0&-eQC_2vH0@}Nl;0+5{NDWx_cR9z!17Tvcw0?m%i@WPVR4zEnS=`3Drf2txuWdbHzQ5ez zD^I_+^;(Q2Wv%kD06t<;XCp@#w2@rwl>Qd&h){Ch6&0rqc8l!ujZGKyAek<@p43hn zqvdA|+Ld|$ITm4Q=B2D-^s(j%j$Wr7H- z@<;D5eMimb(d((OvY2+!%2{l;eQ5(qkP9h{rF@F2G-?yGbTf3+c^ZO}t zQd&X5NYHmg_|5~lkbuyooTbue$G;Cqy8M3svR2LxzNS$(4{7sCR}Vw=na*gP($oH9 zfN21dr#^iU7_pINrqAbd*-T6Re7Y`~?I9}Ly@H~M9q{G_RcLEHsoh<#ag(a2Y@%Lj zky=|7WEyaph=7h&J0Xek$q3Ai$i}cC%|M}T#|vG&ufhFN!_6()H4-K6U#$b51&9gI zXnA#4jP2f8!H@b;-5^YVTkOQNzy-c(HZ5u9H%%z~ME{irKXD;)_41zhk1C1m2j7}Z zm?`f4bPAg$j~e?>ME%F+Q#I>#eEX6_19Ge0GUYZk^21u-8^ac(T-((5e$HM*ME6zX zNon2CdOw6CkjEL* z8G(2nP$eNAe$$us)P-if=FWEDe|I|xl?`+vqN22>@r8!go1MfGIFTmAxi`8yglW3{psO4Os4qg(zA zEq1h}yOKnm@a!5uj%I@nhu{!I`1i2Kiqq{X=mk!eA#0N1ifi%R3i)nOB>Q=-4UwoBP5cs?%$F@G-nms$u%-eb!MlfHAN zXRQ;wdKGT=cemlFza*);dV2M$jtzN7+njVfh^%HD%F@#{>!n!IDl02Xb9`JIUXrETDF!U;@%amy^nF zSieL$H^x%)=-#;^;M^}mp^=tca53+wGEJHESsy%`(4NQlzX4EKZd_up^8j_D9gwm` z{5|evmw#HQtd07?u3(R7!27%Gh-!j|DqZcwjgv1gD5bAoXsxCNLH_pf?Bb6?s)tu z>&`9IOn(PRM}exf(XaR$%`1}D@8Baa|Kxxo30ZBZtH%oo45$4EW5c={F16INH4Wb4JCFVL7gUl8Mh>xZmblK0Mp`n`ypMfq_M&8u zOMFVSXii0231;0b)p??%yB+1EZo`wTPPPa|Y%bB!~>FMb31=KWEtaZz# zAR;|Q=luX+#g5FT7@6v)L=9VBiq!#~K$0V71vE8f4#?%6LQkfiAT*|b02l!F+kR)w z;1jn^fzNwUc7+%{xcF_;9(O;rag@1bf#^~8+i6Yc12%O4A`OT_fp*MN8bfNt5A%?G;(v|jU-u{@z2IWy& zvt=|v5OJ}xRe5^tPxblzNeUfW`~TclAt1HAfs`+-mshczK(DI)0vNSh*cCKoz4s1K zFj+NX*qC;O)2_k6KU$`mA%oDsb8C-ykDBUo$RBW3I@^Lk%6x_wy>kp3^%y6vY@5oR zE5Uhv0Xll`&o)LLb`E#?Uv&b0399!isPcDlexS=u;BMH1pFMn-#Ho&{p zoqxHOum**=c@&L3S3ot%9nJzN|9qrt==$%23p{)dw>_AFNcuzcS;KrpwDKD4=Dq`{ zo~old&;!y@{KX%Dz_pv5&8kW>L^mte{c5ty@5)YFB3aLe0tG74Egw7_TCmW4fOnFB zt|!`skOiC%XfV=HAi4P~fZ}CwOpi1aF=b*Y9%9iIY7+}K?ibdVvo+nK;^SPQ8Gn7u z>9uyLsrI#Ia&j_a@IV=#b&( zgyaC-V)33__u~L_$~oGeH@bgD@%JP_@5FC=WNhvIHbds(S!|Tev|Z)c{DfsgK42ez zPgu>>eW+bIK$pb*FC}KHn5d#ZB??tfdkf{nCA|HHPxQVtUny9Ak8gT4L~by2R`zRy zozq;f3vOL!G)ysBe#TSl`Zo5)QHlLS!#dYcWQ})Wef>epxddn^nx{=v?mg#IP6z>g zspn?~ejy2zkPG<=wc$|?4T8tV0r}o_k}4hx7s@Mmi($(BBz#s2*8u)Tk1Wci>#DLN zPrl+)W^b1rzq(=K0BUgBTEAg zCkQPwGS$H%q0R@MNOGw*cV@lEf*IZ)UV3bR4dh_0^UZgw%N%O}B!#^XYvnAlc}^iO zAVGizl&{8qu3%_EIpW}9) zE~F8;8{Ibmz<@xhL4nNg-^KM35trBaJ=&tcQk-DCcEcZ&g(-8rNc0vE7={-22?rpGa*4BE?2KyWy9mimA^= zM$wD@^4)oPkNqUPAnd>a@!YEuJG(}Ojq!w3-;(_2bbb2qlze&hS=*Q8%SHaIu32Rf zUsB*P1Kkhd$dNg9N!Iw7BdBJ1X53g#jotuBfcSXpOJiaLuss180Bq5gkKrRiVm9kt zJ_ip2TN6a7p(&Bd<+tpvT6|XLN+!?I@Eq*)Nf7g6PO%V4#37qwAWTauTX#pCU4ia| z-A>El6&sDnWIkx8!68znKh1S-mn18!@#eJcxFb4H zR8!;f9^in3#!_$+)>=lrturz+3~IXsUZDlqpp5l~=l<6Kxxb+j z^sj+ywFcrW|7%f)WI(&Yyq3P<`(MbtkR9|MY9e8Wa1r6YPjX7;F`$1F zL)6`2F*n*dRLeCk0CL{ciFQXPeMA!N8u(bG>H%Tf)TwS4^Q*@K?wTq>`s?{}v{4st zD<4UKO{dSXdTZBt)Yn7Z@MkYK6eP}<9xNO~+Fo(TWvsXzk11I3&3$S9gNDcDBjQpL zfW;x%wSbnf0Mbt-qtz5tOKa*4ZsR%X0Msv#Af+xz%mG10|_n4*It zrDJ!WOb1q}Gd8&|cmd`j%}ZECqpbL$uTMFruKOyc-%a8#N+Khw%$Ldl0SA~QAicS0 zY+tWC)XJWyqTGhX-$cl)qGdFb_B82dGE7}fexTOA5y&1OU^qcSNneaO0NFT#MzS(! zWnb0AqwzNs)(0!*#q5ZeB>ybiGw5q4j}%$ zYU|XBQ+UmD#rKRK3@Y#he4aBtQeq=N0+A`sUUm@*1=?$ABD6IKGpLKhHQ9Kl;5i~V zVY~X+>WCJ#$Sb<^hbwqNOC17938UY04hlMZQ zZw0^{fPLoirMdG3GS68iLdzq?5wddIs7Uoi2Pd^miJ+#HwymxwfGsRP7N(!jgg0(` zT?rf6i%PU1Id}MXc?ZRz3$KPxrR9aX2lXug%izerY0d+-fCi6SvM}l0(Lh24lEZJw4;s;`Lg? z#KaqFo9u~=%0jQDpTWYw+cm(kza9E$bof>tOI>OjrR7v)Q^99%rl!XotbFwca5o|5 z?jSMMc{Zn1dI*7%a*?#^qoCI^+Mp^gq;+U>3A{Y7U$0H37%i)|_fCVR8yi{yIuMVS zC!h19yHFtn|JBAhhUaqKRvt_%EBFDVTETx`){x8>FF+JHukcH_`pW=ahoPhe0BY0) zWJ|zZxt@Jysz2PGFmN!RH~c~M#CKGL4Tkk2Ev9D%{tuU<(@*ECMd zHgk5-b%%OCTt+?m{q_VP7x_^agLImW+TggwX`84dC7&UX4J{ki2U}Rmj?a#`lPdk0 z&hJtKY;g`K$m~d zi_f(_1Cc$b?L`0jXPM9W1)r5&GpVAkVukTgfP6j!pSD@D(vHon$k3-fSO0~`Bq{6! zwnm_a9Vh^F-DQ}4!h2hfC>!7BA7rEmyP-6APtid=04e%LG4y&)yOGEh`0V-`@MlIz zl6NGc&96@`Q-yiF`mt{GRUNm-B4S!IPG6zwDB zWcuv1Yjd=0+W3{z`r8fRF_8hC@gF?Vxqymbi?)pVEAfjOt?Jhuz$ON!Q%q5o=ud-# ztDMD;)CKJR5R1k-{#i_GF*+R85g75vmtKxpb4z^}$u%xUm$0G9d0Ss8i_TI^Uy@sE z3ooI!K#u^DJm}6OLE4<083)R(u zE%(pJE?zSoyl2oVw0?1B066#KtOG+TP^02_+40`7z`%K~{wB1+Z>q88HetRY4r03*ggP=C!5;v&o!0~Mq4k^&PowVfNo(5zhol?wz^e1i%bM+Wk zr+mWq;64$5XFRWrKx9n5sb*L*#=1?9&aa?EeEi~bof?8$^HW%yqMDk9Pv@fg;YMH| zegxRGIT{N@3eYYukb;6YG6!?3_%Wglm!7+ddteVT4ayCW&q1Tt>26J#!>~3u6~=oR zGi{QjeKOpqZ@BwK!41Xhc(gn}2IhLdq+C z?^KP541Lk&d!_A0)0k>_SnL2m$avscXO>dCqtge``c$z)WqZFCzL@jltptcPO2f*;-w+}p=I=C?ChH@9J# z=S(d31}t{wz26mYYFEj+b?%jbJ~VKbxpsh95aep094E%AGr$H=1iorRXT@oQ$8@_! z#R7u&Boy=zrwrQleDEKjA@@2`^2)8a6%`TwBi-b=vD%)mf#GZEAQoQa_AwA@Kg z0tOJC0s>p|fSoimyT$3|uGh(D@@XV!Yb&BVnR)BnpqWV4#m8Q?e7|MUrKWOWqtQDR zAw1XKES5gGHZJOL$=FlmCd@J3d??$^)UwaL-d#^!SC_|nQmh3Wk$_y5Mn=%jlAy@{ z!&n!gWM8j%zK>d-u!**3k}g+};h`qZQdU=pJ*6T*uh)HzT?bOAL6BGz+h$Dy)~ z=zglu3h-mbM5`w^T!{kPKxcFCiust%nCDJ*Phm`an+!&z;_g!*4Y94BuptiVxgJjAD(N@qOccx&*?IzX)S#eGLy@aVW z|$)0~m)bL#i2ZQuwxCAmlX&VA_9&I)jxgYYShwqq*;rUy-K%aF^s zNhOa<0Jp5^un*<0(B^83xih)jU@w(9)s(tNU-ovE5n5Q&J;l1 z?T#beVW)89qXI~~58~9lNjA2!i-9pk&b;N>Dxew6KJXLyKKZvfGDHMDh-G~LZ(h}o zOY0kQv{;3sz<=DV&72l65{6eaZCjXIE$b=K55Sr2of^CbWEh*qTu9MXZbkaZp41^J z{{yq+^%Ufj`{rPDLHT9RxXBLC?%Fo1>)l@-w#S;L3JHH|lPw>Co)X6gJE0MsWx^^#!^P+GTyDW+u7fG$r&J=)OUeliLcM_&5A+F0O!!nHr(Sq& zu^zA(G|1Sr%|4{4s(*RO6iFTj^d+u;czmr$h(#iHipK=AF5B>N!dQV~wF6dgz-y4AlcUzZ>G*Vf#}0iMY) z#SIgh33Ks+iZ7M+KIH^$^f<%~1W)i1V)dqb9Iku_LYX_d=sXF}oHutcfmsE?9z5Z$ zadU4(Q*o-xPV3V_EZ$YzxPlOhT!1-=z=2K^Xh?4za_KS(JNe?HM8Ie`7>@#cnpTIk z;|{6RTA|e)3MCdWo;@QKRVN3}fhdsjz4vbA<8jSYJ$a#mg&-jSs|2zB!DGfOYqOtU z76BCuAaXC8-jKzfWo4~~z^lP9P@%^x`NP|ValU{;<)`h&-;%STQ5gMh)|83+t`%rK zcF*r%W(shZLD~iEFGvV3q^&XFVi-&J<29z0CwH! za(u;iBn1z2?4)YInQw%&gW(A4IS~bDJVT5yX(T{vU^VQk9!HHJKvk6n9h_fQv)FFE zqhT=}RBXJS7gJSr5Y1{N+fmMs9^wG}(^|T5scY+>mMGb9$y#OGjug_Dk*4~M@kQr% zGTluf9@h>bSg^S2HZ(TiuDSKPYQ?<&5E-p}wk3x6=@r{)8&zZ7avJCelaO_WISKr@ z*#Sd$^MoXNwm@G?dCU>vPyA0vfVYLF47E-jN(6{j?0$MGp`i7u1~n>nBW!}UPhjyd zi=>~kF)%P6n8O{FIb$r@9d6C(CYjq!%a>lsu=9v$J<8=U;mJE_`sSJ^SwsqeOMefd z16|=hNEq$rjBbw)4NwW!hAA~VF8vJ|pH}6IP-|VP-^6sZsN$$A)(ErS!s02+w z`9Bi~{m{5Q3>pRA&X!d#N%}(KDw-@q#bh8wVic3-7+UKNVDt?g5He7^Bl2@o^<^=7 zqLsAf2#q7DIk^JcKKdBjh5SKg~StA$$h{<4w6x+w^o#LKM-v!Kh^C|tIh#l zZIYk$j2HO-XMH4j91nIxb;jiLdt5$tOd!@4qZ9OG-FBCOlzqWgIRDFQZuur-bx;7; z9VBVM?d9?H9%4#^YnItHPn6CYtoF|5xejARmDU;B0swJNK{jtj)Xj==mu7Rbgi*Jq4%ns*wkK`()(hF9eQ$uB1;H7!D>68`B58f=1VSn2VfGjl!HeyWN;Hn zn#dBK50HLCfyg7w6Qdx@(3SU`*!d+o2=5I*#jj%oarzP(D$E`}Fy7(y@EP$h0%n6- zlFeq&rn{&2blf}JKidzXC+!ga6|Ud81JooMHBL0s|CD&;+?~sB=dB7UjZRt~)pMr2 z7kZ1Ay0@avhDj~3S>Db%QRV?OT^$g-85r2jVJ*--`jyFEHGJ%iZq>1h&_YUmeprLh z&2Qi`ycI;FrgQ<-`PzmCc6L$T5qhxh&{jCHIi7Sgy+_jzAQBK!)ph>_Bx9YA06U-c z3@$XW$}cg$hp+d%>e)q{>&sgC^RfGn1(ju}^07lNK(eV}Z7p}{T+g$rk2tu;=qF~R zb*ioM(z125Ra9Qc0&JDgpWQQ`qwY>Xk%Qr~m+VfcS^@Qm!ZV9RxL$fgNX-X=nEya6 zMBeas^AaW?HbaxV-rQ1`X^B}Eav zzsnABlBy#UA=HK8UFmO!^H=sMGoFI6prIGm@U=s9d;5^&%C+UqQ?|Y_5fnMr!4RJ- zK>XgYnEQ|^d5MotS6^zo>#3c2A$IVPbl~KYnF_J}fNg?F8z!)$yg{nXwl%W(;#Q&e zg5DsRI$h7i<*mbAJy7NW4F7vkgO6$hWx7b;;n;nsue(HX=)7(gby7-VcNypfev|!` z0bo_L)nE@r_Maa22gpbGlfpr1^nm<1h^v0Ke{%KYv)~%(v$uwT+t3qr15EVsK4#X* z{;Z8Vek9kY2v)7~*_)!1*Y3b7n%Hpl?VyBAQdP-*1q>)k$Z7mG#h6jka5=xAyd7b{ z&cHx3?TU_3Bj$Bz93yiV$QT2>WX_nFS>dz&dn3a?fWjA&2=zT+ub?gf4Os0wd&yep zJE~qdy>8{s{hD2w8!)o0 z>4Ql4%_2Afra7!f?FdbrY*!Mo211`B80UpMZ#_`?lxGmQ|v zc9Y^(KMotr3opPJkia_aXTcZo~=mPt-=>$n4yJ z(TF#-RE~`U4!|wHe-~}P@^(|N!NiLeubUFDQAKn;pmArU=rp1PI;dQD6iQ@XLt=)- z&qLIJv2mYACWYTe3w}BCG%Y>CW&dIb%x_G%Crfyj&|P!`KY&q13ECbQD7YODg5UX^ zO2f_Gec6%({88I4h+D&xtCs5-NuLh35{aXN@@r=}9uG-2__X;)O<1MsQTtkr36sFW z^=vf|;q5n*F?$#thSj9nKb~BSpMmr3_%=CN0*p%-!O8R!wp|7KA@;QRC2`e;{3(OO z(D-O|l^M6QLEGN%VCu!vPr_$J;mfV*a7l(FX&*QmE&@c;!En+he}=^7OXrR|IrIH5 zAh8g$y>*B0OG?ac*iS2&D1j;q=9mbc#vbq<`g9_JxmtS8-U^6z5k7a}BNlsGlsP-g z_)%Bk-18_<>{~Vp%~UOn$r7jo$Go3egVo=h#xo3RY_XeG0;cDg_bMQFQS``;+%Aj} z!*?x1Hg87)AnSxA$BOlQa^e_H3TRDi-=`#R7TEC5<1d0sZ$ox3@IvC*^H^qGJKxrv zqM3Vfbv9fVZ^?Cz{PAAJV!wlty*f(@DW|Q;liFeHk};@0PP)guj-~WikTnhPl-HF* zh(*EGm$y!8k5Jv~n`b&^l&70HpKZVhR~APXQn%A0ZtI<`9~P%K_cyt|4xo#sg5lIo z0bsI7H?ICTar3PuSkIaZCFei0U)Xo*G@BWpG>b||jUq7t8648EBcld$Ezl%HGxhbs zUkn?26Y~zotQd&j!1EIwtscmk_dpb&eS8fbODizWcuotZAfH!oZoi`~2*Ql~Y!2o? zZ!)&1h~(XzR5>*pw;ewn;A?oj+2aKTJ)z3Z*f}A7vmtErYtiA)o-MS8tY9)p_7TuA znm*C?I;OF5M=STwZhD0TGq+SF@EcMB68V(akZ$53Bxq{^*l$577UTno(HspRw}Yz; zRG+ExiaQ+Zxv~Q;hi-(L=Q2eY^JM=J;Dm6S0Cm=La1bk?mhU9ljr>sqBrF zSKvn$^fo0r=<+6jhwDvM{m0!jhhAOxtMaAVi=GC@?HJ_)!eBW%Flh^%d|tP@Qlz1n z5p|#4I!UFPk2SS`h`~2FfKYUK8gZ9bd_X8Rbxj23(_Zu7qSmzGzOA%fi^;#@WO+t9 z6rxR<>(ycb0PCcJUyzhkUovpB;*W|80(uRMbO-%BXDB?c@#DJF1o2J15A4^sE`pmf zHg(|X1^Q1%H*@yhz)O^*f0@QuQI(w;A-7fES~u=a6y%tE#5e}*^c1tnc)(AX`uY8b zr$?{dpMAx&1LLU&JX_5QjDymI9;XBJD>O>sDaOCoYC=lPntM$;-qL`jEA!Bd`h72aCGFAk3j`~_TmN%Js$%Sl#dNg9SR0tK-}N$ zK4JGF#{s^a9fSOZMC=HkXUn&)qoK`Digxkfi0-aORu}IZZ$AF@<_MTLCzpHn_7({v1bSBVF$D0mtsr{OeHh38@|)CiF-i7P|;yTUHeaV8zO{l z7<{Zq?xlMkF;e<*v-1$QRcp{5gr%npS+3hLk1^o9I?*FmZoLn0-TnZw45|I&YahOD z?ZTYNw-5xWu)_!=Z2wdn4EM0SOQ&Zn&|bGId|6}$F{~)SrC#FChO#(>kr;;Z`4F({ zn$U@`US5PiDQQ0{M`Rd(MS(Du?bGpO7reS^0CRm)^KLZEDe<#3HMsm*-D^cLmO*MDmNW=F3Z80hRa zq^4)Zlj0m3b~ey|%vcfz8l^|d@Fh|{uN16Es!%CNfrx@vdzLO>vlh3{F$f;Fw%t-8 z@(aYjNMk?g4O!{pchN&GHkg7a=6pyv>|lmd>xH8aCu_3{K>vQtCE>eEPP=q&;r+FZ zyEH;5GU$7^`R?sZN6atcJN=gA_d+kfXFT%`EDeeHSw010);M3(l6}i^rEllYM~}(-$QSz~BY6VRq~i@Bxa0?q;@_G>H^Bhv_ly6 z(*}GcV9INu&KzJZ2mFgxN4Ue{Rr9P!kty$d=Sfd;RdmsDlKCco|EUpdQ`ZffvJ-FM zwtg_KT0ICoJ8KxDlO$b9?kfOzWCAibvlotFjZJ=k9EIci-=KCk%HV_UUdCx~IVT@& z@Kl7{PM&2I;M_W`eaB$NWRJQf(8a{S0vkxJOowhvN7I7w zWNM@YB#6zt|AT*QiGiUcL7;_GB`BDL>z`z8D$pCc3%j$Dt(uu@{_Hll`5yCl z%41DjEV7ZD#_E1*JRs3n-6w?~(r%j*l?4BkGTdZRZiILq4Qq6dKQu??BOSNF^uN*| zl*kJb9hGRdTpb-(b8wxT zQY_Ch&u{uDJG}HfgVxpHPp=%VID%wUv4A;YAs^_IXNC*)SBpBAB}as8S_jDV8p$AF z!N1!Wpuh77E%n|%o{8maJN`GZmcIDi3AOmoZ#@B@pR5~e8Yd3A@KnW$ii(=^RvZ}@ zWiIv2dt%~3hyx04nX>9kPuSEf&TGrUy*-;-uklhfBQ@jZRAomp7%@sXpNe<=ngeDY zTk9F6SQ?0>St&gZfH|%C|D)T5D!gbFtdyD6E>hf^(d*iVnqO;mwo#Vg$_B zsVj1^O6pvgv%_9l9rZ|pC)sZ6SIel-ow)_5lBn(<_gBb8x67{&ouxcv#V=FQpM5!A zKWO}V4Y=9cA1|GPkqtp0J(mzsDeLL)g@4n}-yG~v#eXI zZK037>UR!~hk~Es31>;QmL|U9%36w=p6j_qiu|nGMYribjX|gXqs`|LopN57R|CMw zE4rPPUeFfIqA)D7;1#-{rm0%Iu^E(fiBSDU+-}hV<78xL>_KhWLOc$N3BKjHZPZt_ zz_zn3(C830p?E3Yf9j#s=bzGe$&;}UD_~wg{+J>F?RsnFnm`$_Mu7kU$fZ#pY9*bL z<`ubS;@(v_a;E|y!;h{B1pRzh5I-&| z(Fz8?(s$Y{cj%aC`CWc~H{`9jpzX29O7D;l{khw6*ZBFl?@%t?BrvNn>P<=@l~;Ah zwOtOZK=vf&(z+Y9p2{J0R^GO^DbnWl^5YiP8+IV*7^)o_lfUY^8ngR+$l8dEQh8&| zgic47+pd#z!)V#-6{TvN*I07Z930P4>4;}+YT=+r^iax>`RzWM#y)Jf)jBsORU zL^SQQh|=W0J&EC-a%A@1IC!f9E;(woB7Cn2h(KSk*xY9w_A+t8TB*B4L{p)``2Y%g zzkbkfKwEWm_+h|16<+X3>mxaA(4pLYdr)&)OqoMZZ<^*o@J5eX>u6{EO2KS;&uven z?K7C_doSwHs%l=i>f~pe92L^S4%G(9F7dk-&aIpMxG(m-!Jr~QswtYnNJP|#giy(0 z&64H^TU_2VviBh$NBnj*`x5pt*| z&?^IBcI6}142Fp=00+hg1Nbor|+P{<37115x#DS3N&9A2@kuO#3@a>k%5!wYktC4d@tOW zNO2;jgjJ1H+~1u-I%rKm$kGHjb-2JHM3L)p{8-=bp&Pm_qFuJ`TpaovO<%ppL-LmT zoo%9c4WB5hD?7~%@HW>`g?)}lx}UfKoqiiU4iFCBRHPC@*?1^7`1l+OsWY)35+oV_ zG=dnI2lq(mnZ;6`OSRnr$4g7>%N{JcJyu?aZ6^GwkaD1USB}P(FVty%e5F2r#(4gX z3)VVR4;mL?IK_a@Zuu&AvY!0?x~f$g`n<Z7*6-nRQBh(k$K`>i zmZJ!L9=C=8D@and>lg34eY}==+)~C>&TttK?t7L(&$oN^B=To6kp`Q&0`EfmGw^|XVv0)3x=?a$JXh z-MlFtl~G1n%WeBB`SI`+h^{ll*i`uqWCM3#L_t&$&v@wZ2mKl8VJNljp5a;}#<_H* zC5(T)#;QYeBV}Uf4m1Q!y}G+SMSW`!hR?*O)#oR&WKcIBbVyY;m^Fd zAd6kLEt96BEr^+&llYmpG=<k-r~of~qPNbGZzN zRkTD}oBI_Rm(&OtFzwUv(((y_r&5K!Pz&;|LHm&TtrDye9Y10uPY9I%go6CI)&s17 zdmkRP-<6UJeHmUHB&J{+=;2I^%qD;nYHJFC5BlINYNtkFoKs%)_W+Dmx)3r&L{jLz zCL6|2r~;i3!X}UEct`Tva(-1EgemeX#Z6U!{Z;p;Ekk`uI@%>3h+kK(l~Jq577L4=pt^pgho8nS@%wG7owkj#8#fA``+@%!>_M%%B@|xFuDLn~ zu^>Xz)0+Ow@T~Gn$eQD3eLiIOZSjHZfaBYXh@n?`Hut=ZAS{~VTsPOP_2hRP`s6f` zbB_Jq5Yen|b98?AI;tiYNkB^QPuu7<)UVUMBMM3ceUH&oU_{b6Ij5GHCzNO2$%H+WDeP=cfTWLp=Ps|Pt7h&?+BcnLdVCGFe+lsun z)VW|GIY(9Y7q_rm2ujC^^slB6!hI;#45Za|6*ofo+3!a76L`R+Go{vD;Zw%o-)1L~6PDc<_~nGVI&8Z%nrz{)qi*Yz>{V zIfR`>$Y(|4r}{9*3im{@Hcalr-odU&UC&o7Cl41S6VE?5t)@CM<ggLcMtTKPoA{oc|y zClOSvwm0mb-2dKvrpBGucXF68&=gw;71m7rq&CcEShDYn26>#qSuHSnB~BQeYtI(~ zsBwpC+|P7dEmUAILs(EomAGEB%a2KrT|&i0J<*X&U|i%saD`-9GShgj+WOI`pl=^Z zOu{RtdIcw-p)Es`@L@mofO(7nvn*EfeEqJ zV&W!=-o3iz-giI(w3;gMUaMXd==j(c%*ZZ7g3J~OT^}&bN@~3jzX)b+7%$?s_jMJ2 z3UFSNord^J<26F70rC`=&916!lJwafq)mh0i)b$h{O;0)!A8wRn)q?q*_EqT&7&Cf zza(hbk2|($DC61-G$+_=$fLScJDWmtQ;|Lax*tp z>@Rj#OQcjB<(!8H2dApf(?u44p5Dx)Sm<)cFC1saUwQh0`l<<%CkhMgt!VLA70H=Z zH9Z7Mjib*O(h_0p&^Qw^qoS_v(FrNek(hr{>7mDABek{snMOr~BwbViuXfQ}>aqS_+_sXQq{W+~BMF=c2wW0GLIyjjVu{$5%UfbGVFMP^Gd%jHijw#h6LD8ih`f`9 zbCjtvj6(I8x>j(G$abo!?)s5jt9sd7z)iu;kz;#>Xc$SgxwrG#TvHkNG1NUW?brD| zC(2uIUw5|13ka>6jcH$TaLg4DC=s7tmBFp6q$H@_(~?^@!{p_fpVM2pd0;asFb@bhzdnS|z;5Pxbiyo1f)cJMitB z{o>C*(f_=;?_oMFw@m!7+~L;-7d;DI(imw9UYmvM;!E^0+QK#S@9qL487J4#qWD*m z^otF5=Nt#PRl_{jStn~`2HJn1Aa~s{6uA6eTlATVeqTD{=N|l(8L1~SN~*dpn}>r= zye%L1J|#5eW>|0T%j3=<3Eca4-RA5>43$rllUQ^1o&=2^-F@T~J7(2&2^r&yoG_J$ zNOOkakYuLBDNf8n2CbV$NfmeS9-(Q3|ihp5A)7?}WbEhD_QnCX?@4 z!q8NTEqM-S2F19K(oj(1xY7*5U3ug(Y%V6bgT|dnD(^9U7eAWxLO)Y|^5tbz{?LZ0JkVmRSyE1$U;y3Vk{Yuq zbA~R-P*MY_EdFuh#~$aZCmXe>-Lu9I%Pr3i#a@T0vyo!sC3=^C{M-cj)~5lS56H+niX%SUEEUbQ$@%J3W~SJ@Ov6Kp7y`bcMvc+J_xTYbp( zh}{QMS}>#)kYK5?RK^_l$! zDjQP$)mYrfh$zePn`Zq+*cWV>T(am!Z zEk$S@=$0{TX)7B;)EZybwuy;e^ zQay>E9V#55K~*;@e8_2u7vJ|0Qm@;+l$ZCsz43A3uI2dzEAXflugsA;E%Q};C%O#R zRZk3YT@7Uts+Kv;C7GG^0!@{QJj9r!_FRW#@aB{?Dkxu%9`RTGTIV^fkfvHJCtgCN zp_k;*vFhAgScDi4&42AJScdH%kiGUcx*93z$o@D@M^3WhtNnf=K|!FsdVI*|o{sDn z))&nVb(aoB+u&9#;oS@^RveE<>ec)A^H{28jth*9oF-3GGs&VDFXvA<2z8jhxtD@0 zk8W2r^E2G!qz?I%HFn{4+*?;JIgf1y(GE3|hWp{|cRdz1u}~>Z#|&=dev*G9oq1^9 zSbQw=_{jgA-|bi)?Yi?PCstQc;>gk!kHQA8ERW0fL-c8@RB6xoF)q4tH&XFOu@a}I z<6Vih))c)!6)8Qta}cd&_EEj-P=4Qw-Zw!~%ZnU7`~z(1RtCY>SUJp&Z2B2e;Ar2t zUCzeHTahLdkWC$_`1rFqri4aw_t|1WHiOMkkUK-u&D#+XX&3g^L^MU&`D1N9UbrT2 zUgnr=#zY>W>05n3MjNjtmLO5hDfn9ac*B`*Ve zK2m}sd0f=|RvcRC+YBYF*7hs!b-3(*GNncRvoypy*RMoMig?Q77+a+|eg4So*SW$F ze}ZbsmoueDswAZA`LL}2oCrgkV}qMKn!YmVGOoO`_UM$okt4d;`O6Pvm&BN0p5RXo z%|-~>ui&b9Y~GI@*c5x~H}J_;r(~;AV6v7r@>^T>i*26*gN{BQ$uhYfy2t?Q?GtJ1#hd}j7Uq`T&QzVAhlaYXYgqD}V6I5pAAeA`<9bAeOe!JY z0RwqqyEd`zU@ZT9#Kp>^*FlYi%0^E?VH85HQi7aeT;#%<{88$F9@YNiw{P@vz7tks z_BRqcZ0l~a@@$CYVNzrn(no664j>_wBK`SbVY=`sm(5=Xo@_&=>})t?%Y4UQ=ly># zHe1?0o7rM?QfLS7HE+#Oak}@fwK*++HIsV9DM9Kwe9aEh<@h;L8H@9>)%mp2EjZsZ zx7;VBnZ5Mfq}QnMyVm^cU!3W7=qU&YunKCp%qIy)$NlKn5@;z5_nOOlA;{GQ1XfkY zzIxei#=h?ofq}_FZ(ZvMIdu^MUNl5yq^EcO90@Uxi)FEsYBh;vF44gCyB@J>WR)HAW`$3Z^pEfx)2F<55jh#m0b?u3J8#@JtgR` z3euVoUDt9-9^77`ziM(!M*h)ajcnUSYG-1zAM-WZc;*TZ3_GCinLIrUMlz%C@M0{p z&?YDxPYw$Zx`VWY_wT1c&Oc}fRAuxyx>UF`V7|)rygTZZ(3<6Il;59+VK>3|;*^fi z=3F6#t0hWS_dF2o5#BJSuL$MD9h9R+Sz!jP!y4BhlwM`>$Zx|Ik9Avz61@%`NDQP} zed!M(O)6f)nV+Y@{kF*yT7M8PP%=YdIp($d=|PsJ;q{M(d&n!QY_f0a63cm8Lc|#C zq?&ork!q2DCmLjkd8B-A*L%(Hcztn9A{t_XyVPHreU&T)4ezQ+fgqI#L>88^5B60v zoQg%oi++|_&HC3Wq^6^*E)E^T-wPiZ$O_-7F*Y__c*Y{fWz01Gp~QmTv~PrV#eOHC z)aAOMK>s_%oOdq3H$Qc>q|=LafB!rXSgo$F>OCO)$7 zud_z7XYMfPbRe%_wCno5AjOR2z3}fx!tH;7snbMiLnO2i1vzOP-~7f^6G1c-%?@Q{ zNv!0`q0Xc}tgCm30bnbz`#+r z|G=KOYZ%&dAm`m0_Zax5*U-9@@KdV{uIH(aWuXolznDk{+f!E?40EF4?-k1&zZzya zS*rHy61=@L!#{0*Lsuhl!jWMqPuY@buQwPSFh7~JZ~n@}l_5`~S1WOw&5SE?Z@&H} zxxN3nZpqh;C)aoUFMB8b0gMkqJMf0BcxSO5VVDkdZ-}=!{h;!QIzc^_8CYc|gf@K(1@6aW9F5e&G#4F10wI$nTjzp+`zFk{J+;=F{Q9by@ zoHJRXOuhOFN}N#JH5>bEy7@r;V;+e2e;yBp5Y$_Z=iFMPkI-Li^C zLX!Tod{13YR~Zu{mpfN$2nt7gEDKuzsVUz}aYTt7Um(C`*6{e~iGy`6n95;@uUVfn zoECY~L^%>Ki(zQEz=Md*#zZqLHo7^eazqx`p45Gr>_LN?65-(0O_Q0wjx2)*en8HR zy3Kt(#fnfK-$iQvdJP`y2^^};W8!25x=3cddQuiAmD&~t-fZQ0xnM#-(z)5Vy*(I6 zXP16Qn}+TCbafJ~sizNzUCcNghQTTC!22$x(Qkn!-A_zQ6kXjwGkMC|!8C&q&z?1$PD4ndq;PYPq;FS}G2W z`1lkQq=7%U;T0v>!^TEyGOtcY5(9LxU^4WwecpS&w?*!mf<6+TETF|4S3;9-DUyXkyw3WLjW;uFe28&?6*m8xS=FGuL z_hy9@5|ZejNb?cqlXdEp`|~ew2ZvVqw3$Z@m2bWLz1de|GYA4HJA5#QEt^I}clP34 z8P)3ucB=I#W}11pSNI6y-pVFby?yR|@N zw}cs16M<66%awH>g68b{2J!`+?U0A`v2OAY+3c4_z53v!mT|m9LCPlhb1kG4Y)v=! z`w=_amcNa4K&YuD{5$`y;AbfAt$X}RQJ^1*%MN!*)S|(2HFGzyFQoL> zWFt++tt-)l=er3Kiyb#0B=4XHRCNu)hH}5we)!4$deKC{ZGKB1nRr15-SN6mt27C$ zq#D2cIpG$lch1%^Us9>Q!<)IM)@-Z9`J|O975fSj!ymx?6sYcrnw=YnE#1bRYIW3n zUye^K^bcO)&QhfNe2e7S6sNbBgUS2U*zD;3hzi^G@pJEu1ye(Y#OF9|>AW0FBIb_C zUy4`_J0)@G0!#M~5;D5vDB*7Trsr?}=x|;sf4zGUr}8vSn`>U4F$jLqPV!*fq!Yp`dWKucWWq`H`kwfg`ru$Jxa&;$e?fsYK&cnq_9jz zsjL6Q=9N1vv~=+K)jaPCG-D~!8+O!Euv>d}Bn0{^#d}A-(o$E1jA;uz+=<>O_~G4B zFj>8y91)&ZD}{<$8U>dU>XDTgEVp275;ilDa7^JP?Y9@nrsL0+V$q%3?}UsAZ`Rjb zgz%XR*ey&lg>WeYqF1we%*b3c=X4E8fb$QA!H+|8aBlsAB|!)|?eaf4E$*^1orCBAySEq~VT{D$STl(A7xz)l4EvB#EY{YlN(#sut(}KY`=0Kbnsq^KZ zz%+y@nIWmD3bg;pro`@k?c!-mzR7o&7Z~OQ+{`9Z;esEcs3|fHLRUHEX(FCQuRE{t zx5s=7W~{quC%8#k@k_^@?maV-#lNAC7C+kRhluI5_i!$XZE64e%3E`G%|QrQgq~tA zu{yk6ED+LB3Yg9Asp%3RO* zP0q5N>KF4j*UgbWqWuQ=uSB%(|ERin?9060Zm%27PhIB{-)32C^gBw^rsiU|X5LLj z6s?U3o*L+GZW#FlJ05-a+a#45TF#Wf< zBoUI&6(xtVB?K)7d)Xe~{U{}In48;UkzVr*na#59qfPPJ;6w&41eh*1>xD-C2<~7J zR?@3WJ%solUDq*nZJwV&d6TGRS#yPyj5<9c8BFBC4u%Rs>!x@X6sFDO7!{7U|H{-uI>Xt&F{uHSSG++WkQIHZ*~XsxS$O(PtSC~F60q&coyVR* z-IpdNo_&%ooaP{qmv*oSL(pBqIXeT^GqFk6kk9xb5@uViw@JS1ypt75Ao(n$?t?NQMJs0~wxA%5NYCBH_RM4QN01GNkbAleir`!>wqJL;fcL zW@13Gzo28>o{{AB(L`2;xZnj|0VI6^vV_0@`C3{$GC)+>6=x2u< z)Eng*^mxenr1F$-kJIx_<$-A%5_K;?}i5$`EJplK3<^V|}x`bbM%+N|#rqNiqhAw?VEdyeS zo!5U%I|vy$=v4mPCV6XBgn`MKUou14Y5CV|(t9Oocs6D+5PqSz$iLfw-KzrC8PtWP zWZ~@_eC~sgea{=x9>Zc4DIbv5tJ287nf;uiD)9M4P=NI^ssmI_Z3kIEixjwhg+i<< z>*gV*Bqbh*(WZURt|mG^lm!y!>CHb$2fYiW8uBX^N&U}qb=Kd1NjX(9=hG*2QCZE7 zY=N9-Yb>TPcVf4cMs^k(WhSt)?+W(Ss}}8AHi}dm8pNNr>UTQHFS`P%>mcLHLLpB`8Bm0^7inss$X_9o7BPeAi?xGy|zfA>P zyvlvV?{>3ejbz$(SNTyQO7#vYPR0x0FSy@}B^J6&+-;v*EhdJd!NI`;dJ~dmQ>|R{ z_M%>TcBR8g(nq8HMyQ{F8kPHdoxz6U&{4>WNbXQQ>zv31xI{Po?=H~_<|uoJci(yV ztjl_+GcWuX{`=X?LOF?!1*co5dQ93LPqMtN2A|`ivT4B`6kCkO!S8avmvVEoFY{!n zgEJ{YO+YO7<=nvP_weCC^VJ4YV^_%BNP+(bplrNe(FIJXg=ca40Ob8SyOhe3&*Pnw zic^YEd*zzGC>u_8@|bkA-x|-YYxkkHUc&9g(8TidHVTY)CST#)njw`;%z8G~5c}h3 ze8CLrvN?XGiqMmz?TMb(_*UYl#rqY|^p(dGt-guZX(!|E&hx3eJsm2Q&xf z#T?DCmMg!(GZ%noJ~B%nf=)j)I<)zkfB8t+o zAHn!yWpwu!_Z+U?Sz~1Ph4YvQO2g}fS{+5j7GB3MMWHbXTyzhznF@R#{e2QT#E@mo zuf4Cc>fb>xO}_*Z39tkcX^J@k&0#@f!d>x4o57eXInQT_N{%O?A{w>+i@cAi4aM&yKbkg5$~Tmgy=|9nj!JAAA|cI z74iW&M)AeceCA#MZLuUjIE-KJ|A9kM5d*6?l|9kqj3Q+(Eqt5P9KCh@_coA8X3&v6 zH2AWa;cA+kr*E-k+f7QG{B`Z|Aw|k^gdZH^MP~k{`+|N;KOe+)M-vLS6w>wTa{-8+ z3p}cfuQ2KJ?Z9eh2zKj3Vh3!4B+vZspIjQDIPpaO2VN@&NM~36gN-4Emln$BC<9eQ zT;V@twba7TbQ}?fp&eDo^~RJxd9i553}{L*=|zYgmJ>Lc@-&F`*OyyS!+lx8Nf0RY z8)LT-^jDsRbbanNT*RGe#O(V2e>y9)*um<)Uh5F=)6vWS6}Q$51 z$)gw(MvSAeAu;cx&4>^&H%0f~D^Wn+XEc*e?M7Po=ydfuDZemYMfcc zC_bY*#n}E+Ux&Y6Q}@`nR_wC_PJbSRNMS2qzQ(Ah-?`}#ai?(3f$z)exL2xlU>|6U zKhxhAoO}uG(5>@i4mZsSNqh?D?0@x)?^~hN8pjPZuP>%=pyv3DsXJY+4sh@vj#Df{ z5id|mt$_bHzRa6JB2jd`gkXaXU#hZ^>}i|HyV#od{Hko}!5ssg`6x9Hp1uMFB!0|A zi6$k8F;OnWCa*`C>2Sxp#jE zXr2jtUjAeHkb3_iG?RmyIjtUBj(Mou2^GvqdwN%k2>s2V^{UJAF9W;g+24OYS=fqu{|p7{K@D}~S5mR~P+$JH0upv+ z`)v~D{}4vbBqW}kP|^PT1%yDU(s4I*=+QKDp5KK+*m%mN=mS4ZBu&%N%}B{IvqpvM z1cj<6yYpR<#B!VE-kxog&%ccqz%}2-O6%@VZ|seC))OyNKSn|!%DG-OQM%0Mx_G`* zNB#kO{Oq6hjpZ!>T%?)`e4c>Lb$ z__Tn-x#YFCXs(B=oz-`j!|DRExtI9+8s>24|HliUJjqcO5pBMhWcKEt=9Y6rZ=LK` z!np-B@xNcf5!yaaP{c=J(9MsH^7ds;TVel4pwI37MYAnNU0Oz7wUlkniFaw$5u9N6 z$vrV#-8iFya&HsXgM2$_Z(rw=P--nR2_(i_E2@{G3wa|#B2v4eNnwTlTaA0SBwWNt z3aIXyxLf4@_%i6+^yyu`yVZw(St#~4>BGTzR0K~ZCMf@q#d&F-jgo^2&G*q&__j}0 z>>>P;Y=#h@xT+3a6P=iJT}D23J>?$$j)kr+lcf6(UyJ^D?p!Fz%!qmN?gg1nyfxn! zegGv%j-V_fbX_FqD%Ct4ehFOhDe|z(FZQhj7Hf0C)R-}oe~$-fK#}2EY{u>DnnGY_ z-?**C-K|*~u5DJ?NbwjfgsUfghg%pDtS8T)SQ{<%pB4*kpBvjx%9|PqM=;yK4{D%0 z;}b|#1`SbQ_oB#+t$&Cvi zOOFi;`xF{J)VHi<&S9egg42_*bWX&zQaiyntfW!u+jE565}0s#&?X){P-a zp0;>(?mgSY^Er(h@RME}A)lGL#gh4m7TkXL3?nT{I=Q|!<>PC&TS$8fbhJ|-kbiLU zJl!FDXgT93zG6;c3R=Go#qqzP9sT7z!B!SvLXp)VO;`xgc=AT&X7&$+_G#iiH>JvizlrEIG zPP#<*g-NenZYmB!1*w_ItZ#;zI;m7NxYy?cvnxq3!ASJ4=ga+lxk3~a%|~R>GX0sl23h&_ zPAeJ`rm<0aj!1lno3;0cs7krF^UGgulFfoO1|bg1k{@_4H&Bl9j)N5flKPb(?WlPA zUF&^hoRFgjN9;1m^%%{Zb)j$($*Bsv+HP6hje(g-W*)Jjv5l_@iOXt&?;OhN)&yAh zd$5%;S7dhnpkoTPAuf0ZuWoE!VGm;XLef(i^#V6><+x+Gcu zw9Mk+)2^0TP0x>h-j}C`$@s^@`aYdOQv2bu(_=U{m1DV5cQP%KBd<0S{@!_&^*ujB zM7n?D9C#`jr~JNnrODH9ZtzJlF%K{$^S?OF-OpQrOiBi>2`-G7&nsVi!Tx~jkqNo# zzf1KfgmBM4{lLGl(rk8vk+L~-qTlZ76Jy(c~} zFbKLF@r!J9J3ewJ`>lDn;-@NMTBNogc66&8Wz2Lpq)+4$KjX>WE!|pe<^gS1bRnXS z&l(uBGk=G*VJCm@p<$@qfRa9Nh`UvKGzXFoUL?zU@s(<@Rs0w1t{QeHhie@U`x!VA zyKJ_``YBh~IPAtMW$tBBB0eWnQR1lZ5gx+n9`7A+8e8!cAa_F`)VnAlUC+ee~+O4eN8RUIs` zI}4zF@lSupSJ;1OAS5T7Q1w9x_|bM_1upOv%w?7n zz@cc??PByydqh9E$8Mfrpxc|{DK`}?OWEr)L_**EH zIiC+6Tn|h!O{o#$c3vj2x4{F+@&a0K zq%+qh7#ie|{;m^{bEG{_&O1Qr-9-55K00mPsa_IRq^el{YD>8oFx8mVPqCCcBve=gt26xM(4hxY1Cbq{T7^xiDQnw=bwhO*S&wcL zULfV++$-P&|HIX_VszpB0dH=)bfr0+2yWk-EDgUn|DZN!ch}wVDmcJ`p_GMKu~Oj& zMu_?3inid)%%BzM-e{t%wpHgA%u&QhuJqH)6W7m293_nJ_ zk6v9S{zX!ijhY&|8d-{l^qQZQ;pqHBL$az>pk=g_wXg4WLK*pA%%22rTPX}Sx971w z-&DK>oJ^8zIO(r6q+JB)I(-CIroO>eNN^uC?y=yqyep`FS7S4unj+~1eH55-(`d_-7AUN|aBT-AW-oGj)K*q)orlaUQrpAZhRf3so1 z6cB)F*0;`gV+7Z>jl)1O^Mu8i!?V*W!h>ZyBmIu_P*E<$;^9O1*3o z`bO<>=GG`R z3;`JPy4E$AEMYNrEK>KbKUD%;Z4Vcm_yFoQ-ZvTgJGAcMZEMGeM#_l>Z(Aooorthh z%;Vbdq#%y(p)5EYPW>G=o=M5O3k&C1{aj+PnH;OcUGH5XpQCczu`&$DjF6qq(|=0$ zYmEiE5^-|HnmHWeV#0MfNKwpzhb`OC zsxSkrO;{X>-8!}ED&S6%AbGsj_MEuFoL<#}(r!J(1UE|jxt-O@%NB9*PMZTQ6MYfK zy~U~g1RoCnh=>SVts)?e31t|GhZSB5!W#wL+I(1^!1Mlx2$(_60{0nmzYlG4J;<1& z2Sjd^r#YAe(Sl&cx^yHORYnN;ApGPg#C>aDJyCx6kFD zyl+cd^MH|Dt|AL{9>sOE#XFC#dr=(FgIoD#bNh8<&I1wdvs2l0Onzk4dX&E} zwij%^G4rI$33KM@XQNx7R&@-MFhwqH9}2-@S;DmzOntHbiFdb1VlJ!Nm;6jk$P=y= zRHxLOau;4FczBB6oWB)F_P-Zq)^(v!qp7@$WJbwc|Dtd2Mbw+j38>-GIj1| z+!*qjP?a~I%YrJr?JL;`BOD4xZ91z51s0h0*zfbKv5x+-hOgbn7Q0)G<$%OjFbK-4# zsybrZSHjD|r4CW;)9EXHE7E_8iz%q`M4KrP<{y53+F%fo!J-3BD6P-SsDb#Uf`qKx@!m6?hszLeQ@RPEtZ2Ve$%F2(SUxV%Y->eeXYM?qJ}kM>`DQ`0zM41 zAaLC!(W|+Gy+K}{ib<|AkB6->Xc_1vWV=XM)|v60b8@Y=_%r;gu87uSM~k<}p=Wx1 zrJMcal>?_n^X?RLt}edo^O&f+#=!PcdfhEGTkg;%!|a=W zHhbZ^!*P=U9}z77#~|%p-ku{m;IrgFZ^p^ICzEaJXQjsgRHp))jI5bU#t`VMh9;!tJl-I#nTYQ1XS~&^0^-`=!uD9Nyzlr z?#4DY5x>&Jm{?%?p(C%3qLMC3BGPpFlh5bv@in*~U{-`@saw{3n7q9gY*y9--`BPh z#Q*`<;{ehuXZ{cDL_!o231;ECR z6r4LlY#+V$OkZ%O9wF+P@FB{#TkRM1wjt{wdeu`gcnks8aHO)fl!ghEHmhar+wcM~ z#$=1_901Mnc$^n1Q!P4OI>klvo!RPt6-!em*jGB+OI(qxWVcI}!X=)u4J2a>=gzYk z%b|L3icYW9Lp#P>zbgitynioh{s+Yzim+|)B{E%Fd|2%I2En<8*ye$!sWS098m#JO zXoSigE|;Lu_CZb|CLbSBMmh1$X$E=9l4C7ra*&T-a%ITs|Iq)E8a}-H8Cy(9-}S@3 z=V_f11b7Ahie=CcGn<51z-MIvCd;9M%b)aboVA_-`y}3e?0nUP4`eSNv7&5)j+T>m zURj-gxp^hztFj6^ug>#vM*XEN2Ga z`1k%LKBoQ~(p8HT^Pv8ke1F~qa%$ZRN7~4Ujd5Ui_U@h6i@S)|SD$h%pU)1v);xMY zeiLDb{bsHV%MCvF+IIo|bjNrIPjFHd0*0&VqKC$B`_f}AyKEsLcY%Rkr+%--f7Z|d zeV-!v6hkb|i@M@&Ws0{LAXfLbzu1-bT?aJ+lzD&mua zfZ^$8On77;9!f~Ohs_7{DTrjoF1^pJqR$EX$|80XfM4*-cPtpq`d0D*#pe^BWnOmC z>#4VfCaoRdenT;~rh;yV=o|*Sy@e7muxYrg2=w(>iW+fl9K)R>Jory zys$z{R=2`xzJs5PRu02F8dl5WWIjqL)qQ)_5^69Fhh8Y3oY(8+6>=`6ez*(On5eXR zN634&lAXy)@?9mt%T{_{D#+@a0MTk+VSG@mZxm~GSL*y?{Ud$+;cx!1fc_KIs5-8eO83+|1t-W1;C^PuEufQWi(KR zz|B^U83gvHH8;eIs?P%JNVmy@0?r@MJ$OULe^|U(ak5LGu%&!k4J8Q%pY*_=9{M{Hm2`Rd5`n+^Zaj_pevE$A~A^(A1KRt8a=JPIIu9L>`=54LDaG!N(XR-I<#}1@*t&pUiTTf01V>jPovLhB zIzW$)>@Iox#ojqRGxeAe@=k1IfYBHG%jV(IPdo{a`TP+okJH+z*XEq$p4p$=U3?vS zQl+98N9Y}8Fy#Zg$=ezf=`KEuT>SuGi5oM<+^}0Q)QX(d-zx13PAaFwEZMud%UlMq zsoh%T!_|?@TEm18Kfoy?CAp+|eu{;<>m?{#wF2W?PU0T=_&%~2JGzU7K@cr&BasTN z8~+2%ab#`w?jSWZ#S(X|)f^d!xIo1Y7BSqLNAJhzbpR-ME9SUfka;vUv3EvxfQKiR zbLZ%7VnbU5AAr5_QVj64A6;mCGccjr#yVU%QFAXr<01}Ds(v;Rma&GB>%on;c5?a7 zwW=h&dwvgQz>Nng^yujteixzEiT2v;_TpC2B*m$*hs)X}{EJCpMNz8Ozavcr1P;~}?AB7)Ht_)gll$|dEjrmA>w8w0*LJ^%iT6I1g{ zC$<7f@NrDt?C>n4`UI5V=AomrzrcdS?YB9J!_{5Xrg`FdOdvPZo)MIkU9BFy7@9%! zlk8K>0RuMGlF_MI+7HZ(J%0sv`MT8b9R>!v)Xf+nfeY%n>}JaJpoG$L@N0OY(_)$K z0B;AZp6AjDNy^>7&&4qrYXvkPgp^c3+nMnea`k9dkJ?FnETWDzSnWJ(7HEt&=gxW8rnh@2+2%dJ5U=E?j&FYybHfx`> zeaJ@M`g{Nsg7!qT?(Bd7$z`yi&1POMsoalcs|2%RYtQ62Lp!Th8YGlsw~!9JijDeJ zs_TZu*kT~BfcFmEgY)~>mx%s86?TYdto;qt1d#t?;;U4A*~ZG(rc=78Y4ej00cl`! zK)7Neb6l!aIqeVL4nYr8=|GNmdabH_+J`H3pEjAz9LK&)EZ2hw8Bkjvt6h~&ANsd% zbpG$OnqdU}AYY-@u6o(er4y`WmT&v{{d^D;;Iz8k&`@t2_2rk}%>rI+bHDf2wG6pb z?B1!j+Jz$@*OX`Xh1F})cWn~fFSg8SBZ~xH|IJp&u!emKxFV<9qh9$T_UwT$YYU&~ z`5m|y;3ZF+q_Fucb_LY(kAV2TriEdQ$;Go96y={xI*|_cb0NwGXZKUw* z$CyX)Xx4QK!T8;B?AF2fn{$NTQ|45V$&44i-h~`|*CAJO6#ogd?$Kp9;($PPPaRGN*iQI8bQ*XMXb`o7N6*6pl3G=o!3Sg)>ThNb z2Z2~T^m+rEV1ba={rDqSW+f2}JOG1-)pk+ox(0GkS*m?a#t)<%AdiZ}K{EdiA zeWaC|DZ^mBpv%O0mA4SeL@)K*l2D?9J2MO7@QDNlEO<!sllL;barY$hFUgRz)NJcL?n0OE)^qj&jVBCnFqw@p!hL z@5~{! zbl^5X%Au<(wiV01RTKZZ%_HfyZ{G)BB~;~#od)G zN*8H4;uJ6;(yLNz%1ZOpb+a++JCMo8wceq-Oq7h&bE}N4$oA^|W={K!LOoNAAI7F3 z)6hUc21*U|txft(LJbSpy8X_}12vqpmzv7_EHCd| z(3}-pavqL$U9Dwk3XMt{Yjq^Dr>3I@&*Y068kHcv>~^}xY8QGD#qDO-2oc>R5g`4KF*Biqa15#@6`3`4SR_V!4&d1ye(LBBA&?ze-UV$NJTR zd}S{g7nY2kcE&h9FE@m)mz?<+{4xM3>)##;9PRBdKC)fNbfi~xCO^HiZC_%x%;Wqc z32Kt?llOdU)T#3Nfk&#TBzgnme1=mSWfcu7IS09T#Cl- ze0?HAe03?AoSoNrrc;JNwVJiqjtKuF_qsAM=O#-EhesY-l9+Wo2C7%Y&yPeAu)uSJz-DcTHHeNUR{AEW z$>7xWytIT>6$wN{kyl`f3fm}Pvd?Hk8AG1>>;scw* zPJ;D{u5$PFyU+zrVYokpZZ>Eqn`)tCB|E&A;zeZl%gOmFo6$#Xe-Zm+up7Zsj5eQ9 z|DQlxz}{vdLlqq^)&j8>nioGgbfxk?HqtEHeO{}4pC6P709ap{CZZ*mC)_q!;g>vu zp9~MCyJ6}WHjNT9LtW@I0=CtH(@&vYnr6ywJU1J=qFE z#MJ*|@2%gW;I{W+&_^*rDU}ov6cA9lK@gCZmVP9pLAn_fl@93!rCV~S0hMl~d!(D8 zJKlxoIp=uZKjC|Q_(A8IftfvfuejH}VhaIYhGVkqe%rm@&8cfdyVzwqI_8yMrqz90 zKL>1LiTP5a<+U03`kzExA+7-PbkqGop<8}I+t3%xS#3x)b>szeZYLnO)y49pWqgh&cezoSYK}WwJ|Lega zyzG%Ct(rcY5;-&>^dLNe4zGw=OOQG&kh+ZdvqGEw^gA>A#m{ViRWMQw$4M#^O-2>G z5f8YyoV(5jwF0X*Zj;BRAaG;&3e3js8nRNEYX+Qe=v{pPMLsTEP!8re@F{qyQ>Wg* z;1T4|s^QOb z7>h9i4#Q@P#0e>NJ5uOh(Evb=ipyPx;>xZz_rZhQ^p|wi^3cK1<)TDSR4(1FRhnL! z%X`X}%m*|Hl)4Gfu*Z%tTo<{>$V8<)&0opm={MEX=eRMNdp1i%yFkwUsnxfwzll{J zxf?c}_bm9Jc$}61rLMPfhY{eSaOB3J|0E$+RY{(%uh@qcj|1H3!kzNzPqUJabrEBR z(5uls6Xa7uQt?EU>%>a|8rO8bi-VD7=l=o4at8y=Kyg5)M07l-P3L0DcZlSOh>oAq zCeM3bp7?KS6v|+7)ej&<=459~PkNCP3JrD5MaE*Uwbh+hAv^7`PM5DI@Jy^8>UiCz zb~Yh#0Cz5|4SY;4B3`*o@pTv~Ie>$JAWAKV>|iN#V9;i?_qu*U8-MdWr0D1Qp-KRf zEkkx2bBTIkjc0ja&R;NRApYg&EMc;C1Z9Wi?c)Mt{`p@8+0$J%F7*PCjyr~8GH#76 z0;oPJT$;x$^E!ZihonSe<<>8&XKj^xy~}aJnFnEt${zy4isOLRfE2CM-t=}og8cRq zD|5U?y4Uv3CEtGy2 zgd|SKA|=M2y=}Y9+x<6ynBVhPoFajpYG+U{Uc~Z?&Hbc-U+&E3M4U|EkS`C2xSCpj z7SlOOcigkzKAZ|=+vF}m4qFMVP=rtPR2(<4Y-hKmahio_pI(6K3H7F4L7*>mpKe?+ zb`muZO${U15DY;)JHdl^Axrp`vCT-L{T+~xqru^aVn2rwxErSWRGSBfi$ z4f2(2sYb)fJs`vMEmu*nYbDPX~DBYfrLs6RY6Lq!~!iS&WgF!u5A*eh#>JFBvyt`Z6(l02~KQ zCnYGX0rUdhiIB(iWW=LOQFBlrL#)#W+jG_W++Wgoc>__XG4^Iuv*BO}uVv8PeZ2qu>R{&<%PUNep>fe`NVs4Krx2&?l;j& zZ*^fN|9kmr&@oz6X-f#}fyo|Ldl(pH^LQ84+9*uMFP13akWTo4s~9>#{=@nQ9N9Ke zTLLtOV%uE`(E?LWSP}C_JM3JA%%*aFgH>VzsU2dzkn&LUVEBa(#l62&=)-~kh8pO{ zyLR>^au*K7W!1AsBxy;$eEG8Rt$=}%C+P3>vbz;B5E23PlszHGkjk>^Q;&P2Ymw$b zzR|(EmF?R(D|%)N&sJZ6i)xN?i~T|NXMnZQ^Q6Fb5quoJeeB9U>0fLcsgD@}Du$Om zvtAnvcfQ6n46`hbPa5CPF;A7Dy6K=#;b_$lD6}@LaE>QA%rUX+659S6ShmyAjX_UZEyM{J38Mo(U^@LBGU_u3LpcH#feGHig4hsL`a zbIaTO*@Q4i6bkBJsSCBv_%_~r!cUg|ST+6|(BFOC4n2V{F1zVMsKWzP9`HU#uN5g+ zvd9npUAEXh{uGu#)3Rp&y_J##1$X=8;=#zvY-YVa@th zy1QaNkVN$KLz_=eeI&O#JsT~dWw&|KJ=U8;55^iSARlii^Hq}KoseEKk`b~6?R(KDPsPf#f_@?~s59d1MV1o845Mj> zMzh3>_lEtOOn0DDIhBQ9jq(aMlyzUJ|EW0fTk7LgAc_t0`WP7zVdZk67nTB6s1sT!-E5-hFgK29Q(L40v(D+en zzIG1swZW!kyIu8X{oKI&D`c7ty!C^^E&ZQ&sA6Q{c%Y91p@Ybgqr`^x(m)uy0#SXz zOK;CQgI`DDa>?@EBFTRSrA-bW`Q8m!PA;s}>@~#C9c&p;{ywz_XQu{@Su3;9Pvy2Z z_W3Ma))IXSDY4g+MZPeW%RtD_Nn|X5)RUj57An3%HaFK!iWAB3 z?!&N|0w^!pxsq}Df5ayN?D&D#p+5|ukZhqZo4YY3Bt)%qP2-&U=NB-d(;ru}@r!M? z@f45i50X+2cI8YZ7}x|2Bt;KtnP^+B=kgdH?QB4=_UrzNw*lDU`6H1R*9?(9A zP9zR9ewzNCdMo-qL5a*E@GQW3v|@Pg#Ra2Zw=z+y;84nSw)LcUzelrYdJ+mg!F^C+Gln;J>bUpj&AoJI zK2tJp|IN30k*Q{@;Y!eiV&8L_mDkdJU>+};Tc_;k1-+j2bd6Wr^3lT8QVq72s=94Ckjv+5+} z2dZvgHwqeC>p6j9$%Enw^o15Zvk9p2aKy+H_BcO>63dFyFVXy=yDzMVA8EKe|B|kQ zJlb5?hCBXdM-5!sVqcLLdq7BR(q}LYBUTi9tfR75HlZBB6vsNqH&6z68<@xV0WH%a zA{zuLS|}-cSoazFqW;Uby{BZ6&qvqwn9ff^cHQ5dGMdO|EhL(ae znZtMd+tBnwIuSIXtuQ;N!-y&n#+fl`yS>aD7;v)k(Qo(>AHaLOX>ph(RiGSs z6LOdfGkKn5caut*0ga%dAJqX&HU+~0{W;F3eqFSG`raRBy)JDXE-;8~z(TBP=E~}w zWJr{P_8p&TUv+@g&YefbFdyjtF>5to!%5ei-_3WfyRlSp;baeh((qI`O;pB^3|FgKWK~E4B3fN2p zLId`;U8^Gm^G&xz$~|Z!S2rUmp^>D`k{gbX%xq-b##t{hJ0l~^V#uxZ4D3fP1Oa~r zX4}9T?9Rc5X9@*+=zWU7P#Gg4`zE<`dl94{zv0J(8V0)TUlfkVXA_7XhP6PGS_y`` zzX_T=5lieFgJs9I+U^BH+MPy!CWlBeePOa&y1>-Q@S8^fF9f2l;vte>5T{7#THs%|66sYdOcPL>p;I_w^6n{@fk;FV?WWm?N5JI`!uDNF6jUDM(@J9stEKV?76cog;AgKyUmS#-x z=BA0QT`sua_`^S{Hvz=}13Jup_y+{t(|ynFbU1ff;Bf1W^4KY5W4fSehfF-%`T2e7xkp@Q(bYT$YRPTQ+kGVoLSdU ze<-V_+&3srgtz^AXKaUzEA?Z|?Ist>op2;#eVxDNlz!-n`>Ou=Fk!q0Fg_ zvQT(t99yDM@ERFY)g}{n^5u)}?x|azLF2lzziYM1ccgQcMtJu|j!L!++-3;ao64Te zuEvAPd!Xv#nr#M7FANSoTG;A z$oZTn1k>{+krzvzVaXV%w-5_GprO&JwobQdo$w9KI#MnbD@yP*6m6`Nm|J9}u~?`N zaI1N>sq`djk3N?g_f%0e#b#Tr^3H8O+tGVhy(n}{v&Qij`_d&%op{*pY}Ix$B=WgQ zyxIAPb)a}P$os^pf7qmCI73&QtX~#|G!rCA=9rstJ-d(vaZ^Zy@rF>ga`=GpYPair zRlUaQU;LA^jO4I+#&pV9@o{lv8(g-7Wum@4nMd0)M7tL$&dX(}B}K=@S#PwlGc+dn zUVXMNl~cHS-cT%K$kfHr< zaN{pu4GOWqG4aQWX(BNDo8tMlZ52=ImTpb_Ns;^Xs$G zpYD_X)z$KMBw8Z-BT8xATVADdsJDEn-r$Wj%N^UioBuu2L^mH(T^SL}xml4preQMM z)61&S_@EhSseIVB!n$8hGZ~oLy&}4TWm7g(he%{_g4EWQoyu(UdW9q0hg%Z(=-WgthO7MPl+6 z>tHjlyN^$1$&Q?BF;eSLHd78KryMSWHBgtt7fSya)mdAC7nSL&)#I*MkE%ZO@WV!? z+N5%XgoK=O+w}dk37-5Re{xW&9%^xH_?{0pyd#|CMx>aA13Bi&o3ubD7v<4yfgpUG z8&X~gwGGTXlL$JhC-M~`LRh(M=P-QBy{3{*@w2{HpEfU%$~fR9%0pl0^XJkb9Ylq2 z&%nP$5s{}{m?du4Qz|nOs8;4Wa{TMT<;$a027$j1oytR2Lc+(#N26vYDeW_Il91jh zyz5Q&tA6_-%gPj+)Yo<7zc7g9Y_l#&tDAHPpX-+23v(BT7M|)VbjK%&E6(OPnDC^0 z(UwwnZH`)uDD(KqVmy}89#s;H`V5Z3X2AE)vtyNPq31Lfmai;F#Td5v||A$fNCLci7ehEQXT zESK|SNb=m>@Fmye_a8MbpomdkZ8vv|yUP^>fBv*WZ%3znA-C!+JU*F@HuYINcoSD9 z20NE|>b;+~UCo?6=6_p#^+3G`72?idg+0YKbhbB0ANVy|*5!nqpV? z*;)MZ`6XGHWs^c3s7VF45!GBu`b>N&NiZFMw_y@@DCvLaC4XBt5xAo< z+}Ldci-((gX590^*nu3ga-WFRMp%`k%C^y{$=-hJTUe!(u5yBa1L{@i_pH5$ECpqL z-rWRe%n5-VF0HVR*dw))WG$pm#Vonf6D@^s{6?P5gE5!u2`IN+pZR$%yYXpjZ!Go9 z(d*8=<-1|N&Wwb&K7Xkr~{?Fo%+3B$lj?yF%HD=RvL zF^O|$R1K91%F;jwm5ZJoe-jTvbI zd<&VaKE0_sRj@8p7yLx8qWet0t$-lX>-6hjsg+JZjCR)E^U6Zy%q)sNlalqdJM6}@ z!`YmxK7I-mt{reUi2(sQIj6x6PyFzT^7?45>9~7)!?A7Mr$3jwsA96EiOQ58V;a|@ zZQHkNt@ju)Nx+>xbCbqyKd^Y{==S?pes!SIBeE7Vxn4!!5oj#tS=x`tgMyI@F8mMv@Z&_e3hD`y6^)fen79wI z9a>dHMC1>&N}AwW2>PA_02@k5%cD9-rR+X2t1OUIqq2owi_?(-)#hpoUb2Hs{s!k+k9mDNdYF9f>fAFzcPI*%3vszE{{;R(Vs;lm=TNQrFDw11&c zf4k%B+QS9S39twM!Ar`SpvlGU!(h3(EcZsyr-O*ylc|)H`wNTDR^j5|=>7+nlX`g3 z^Sw)k%181DI!3wl*CG~w2?Tc#u{@WMxLig&J!+OgpQrnzJt0>d$BQqrBiva^LSkjl zE)w1Ri6DZ9oixjIyqfLF7DCF5d&W-VcHhcmkHHTxa~;}cH&1!qTCxa$t?j1K)ulRWzfSnWd8s!uppp%1dE)EMPv6sJm#(ZM*4FO0@y}8J z7L7e@+XI)y*NlTH#?!f6{F2%_rWK?w?bOR(%bg=Hua;9#*uaqPE@iwHCxchoqjtrW z*vPG(`y6vV;&Vn%6bW{!j}+EA>cwIlNqA5#OQji0qv=wKnKh60myTw>MMZIanL853 z$}O%FtDgIo*&E2Ew8pg-kaqa%=hZXBgmY!+52*+*s*`aCZ(kysDjz7h%b3 zw{x)BwO02m!MOu$w^G{n`uTYPDZIxgnNz(QM7-#XqygQgs1JaO!#aovQ&LjiS9{+; z?#>h|=Tqa#)0U!F#t|W*@@XNVia&lNGeHWt(8RDYA+-KZ)fa6)%WQV}dYo~=lnZiA!GDRaum;hROqsqr9%xG^)bA@){7@5c}!~IFE(RQ!d1U%1cK=9o44y zSnAO}Q{Tp1f}HW973FJE0h(;!H#*^TK0U;ZcS>zAeeCjwsSZ&xW|Y&~+$jw>aCV4( zY}^nNfH)?1d~jbN;bcHx{LEZavU<3W>^J<70gV1o*5?eBa`OA(f@h#|?Fg@{OEGsj zGL0%#)=X*9p~-f)tRUS7UtW^Pw#3mi=S zlb(iD%aJ^S^bR$o`R@8Jug<;W>QOL;n%34WH&3OuH4h_|zK2$Fb?n~env(6JJ~(bA z`~zcTE}F_9!6+J-vh`$+Rf8X1sg#Qou&kA>K`>n$#SQYFHNhnRHa0-@N9360Dj z5AY3~X}m#+gI&Dg7lh!qTfGRImQ|Ln%4uF{yWIhy*ibUjA9$IvsM~q)`O1(Qx=@dN zZd?F;Jm@eu%6*kHk;i4_RZ~$$6qkG2!TN6#Bz`q1G$iDF-UTPS9Y!_L?Sx?Z)R~1< z*2U3OjlVuQ-L+U?)R|vX8>~` zTja39%yc;`wv$+~e>7OuHwm<+ib0f`gYRD%P;pv?(rz zm5aYR?$Yb_tF46{6^6ix4CCtHT)SOncuHr5d|3k*wSkFmwyea#iGm|c)3 z@a4GNH+-G^PLFIF3taFLx|$mv{7cv#oZ(ofJH-1_z34EGr%h>fsNy_PFToR@xV9pB zr|_t#Vi&XoSUaLy`5UUnjGZUcvtHuQq~@-0hlYhcibz0zXWfiE;lzw#s>sUbzAXqkkj3&CqB0NO7auXJH*P&YA8j`1~MM!lU10wNr2xR zAC8S(oaOD~w%yuY1Geyx%f;$MMOiPg+^@9inqL^WE4XI$Um#gmPse=7y*I}l*{z-s zrt8z{Y=u$PPA|^@KtoUJ2uRXT>Iq|LKY*{BG!`{>?YuA9<|k&^znBjs@@yZJ+hFJb z)khZ@ZB9(xvb=Z^4Wk(*lIP@qS~WNG@h15v2wWxQd;NJbE4{aBi;lqS)Og-lsv{ z)SDk(`b#ut28rT~A@ChJ@BdmH0~apnxOL7-iz<&v9l`m%eE$W(2qr5wP21fYKl0vW zOiIX8h62GfOF^`f6(Uka3hc_Rg0ZQLW}njY%7kXeC&}jhvYO3o7Ti^U=qHORwxW3; zs6pX$YSuWN+IJ-fH-5IVTF)^s*ql~-dAJ%{H>@{Y>R(ww_J5Sr~UMd&0(y}^KF&9ll8^D^*Vy$p2tO{t3WFWyB;l3pF~z;ILF3Z z2vc7|#Am*F9JEkijl^cX7@+=mu`yLp=G3jbjXl&b_t+v9q5O7`b;{gP2ai~b|88&TvHCI{I*AKN4iI*JTWc|5P)Tz(iAK?l~UxE z<%Ct$ag^%!1$N!K>3VfL5ENbOKw4Q-rR9aL8##}20`oPjKDZFC$z2*qKP)7fPyP+M&7?qNjeN7 z5>%&=jANhH7BHc?cTe5S8iWj+nu49>vsOpKv}Mxhil{YMA{x4eI3#a9xH5{7hXeL3%#l)$4PY@G{`8Fsy(5cz`Lv{^xNa z;HrNfHoe0l`RC!?tN-`Q|F;|eCma8-X-cq3YXl?Q5{Kn+bT^MNH8K`v;{n@-_5AD~ zcoHlhmLuEV;6*o0rvm%qXyjjKM(Z*)co@>_`?7F^8f9| z|5LJIOA8!5g@9|B17_TZ;DNJ?i-v{PdT##XFndp z)E@2tlbo7EXxL49gI~k2Jy@*wO=@9b;gt42e`{%|$S_yEgkB-Vn@!K1l7Jwz=JX^s z;ooRN=pEKUuG`^Gbab>t&kWf^c$W|wDP29iRK|1vd=`WS?xw4w<;qz~pVC^nS@oNO zz3?a{HU9e7>sZ@jSjYYFom#@^r~@Kw(zGh=IW0zmOE3NN`eFcl1eM6gFnVQ462>`c zKVrr_&GLKZH~)FPmlS(TgIdi0p;kpPn|eodf3Aj3jl0(Hy?UzWSOJJ5~Y#Q*G<5bCB-O?X%sn{juFWH`OV`B^g}*jCelJftN$f&=HD z&oc?aIylurIrsJS6dC>e(jCQZ>g?=1QflrO6ckx{;m_ZqdGNq;@ay;P?!xDv?scbd zvl20=#PQfI_N2?DUjOG)u`VUiOioQjGHIC@8>{8Kd}2Hv_w@U_3+(Ld;z1P0zrLS0 zCiv$)J=p&0QdnPKC*`!*T3gFd%2b&6@qy;2&D(PqV7&$w76NVuTfxCM7MlM32awJ* zyvNPGKieLwm?mvyXUERLk*AHSNJ~qLh={1JuFlOhKRr1PqTnYXC1oK0k4wPDwvG7B z$U9mc?X9B`r*7~`;D|<0F1yEvyJxcK`Hy^H5&r+Y%C|XcTH4u$08-eJSV2T}em*AY zsi(`<{7|k2l48td3xh_$n-;xpABO&gTTGhX-rm5VehS1pTeVO%L_=6YGooT*2Zx89 z(X6od?&LhSM&nhY$qjk9xPkWD%c6bx=%j=_yh`WmBwSp)m1Py>F3ubaqve~Of^yu} z)6bthdtc1`dlNR`=4UJ{`sMx2Ia0X9t}h!I8d?;sX0D-8$)r^s%VT~JQ?g{gI%>Dp z^;xem@bK`ElH0ns6Aqja!K7Ab+ZI)ztvyam98qEtJv@+q>Nb$K)Sr*o*rLOG_S>0m zjLkv1`u_d*US5LETMxOo_CQ%DOZk|Xm^vML3knM468J7(xpL*&wJMvrK%>X^?%kuK zli<;wKe_MMRRne5pIvyt#ZHD@@JSb}Nvax55EqVPLLRk`>`6 zm7J-VzJf;l8ZI%Nd~{b9dwo5>o|}P2#@f1&i`sj0xHXcQlF#wRjT>Jr(I)XLsW1XGE?^2e2Bb$PsS$$3DjQl%nS$5Dr1U^y+vUAN}trKP1| z^|q5(SXp9m>2(r|I67$Oot>B%Ws~R_-_3X~etwr7z9&!Qd2EU6f;$rh1raAlaLQ&Y z%k-v3FV)oo$oLOh`m(t#e;Xr@Wj9>x;xatM!PWv;5e=k;7Fe2h7p}U0div>}XiiSf z>y9|5?ZrMwcuR<@1c!y$V#ZN4QW3piV;$Eg@5JIin^IR-ul;zP*J(o^j=+VIiTM2c zj~`*#m0*#>>6JsIV@I!mjfXGT63)PB)RlxdJ%K+b4rQEWqNn%0PlBb?yEKr$yFAoW zp!a~EA9aO@frp#-bmtI5MMV|T`;u&~UZ*?Jqhrs(r+Bm1PbQX=mxm{UPNP(Pn}iWn zWse!6?y%~9`rAA-@rUCj)yHpw+PS*9WrM~i31J)mb(sI-!&Riw&&jUOA|PSiv@ADv z7gh5#7eF?^@ubS=zxew1Q(oT6_E=6hJ$-~8B|bij+kq`Pd3+EVzoDeGNQp_`aH)Bv z>;8t#Tn9hmgip28>O7VJw-xc>@hZ-bWAH8zk^3+rj7(j;y)K*G0F1vY&iRg_P zhP2Ko?)^ECN14;@f%;hFk<(Eb;?0{kuC7(Z^=!;K)ypHu{D=sZA?ukI)r`L_bxx6J z_gjcC+}}1-h$+(*iyRj8=GV7EgI38Nl?xZH5Wcat=CPfBq0;%(uW}bQ5NX_#_RI&r z`xC2Two0x{EQi6S|4SpI`N0A`29=zLmyU7XUgfo(o*%NAwLn)-ffT|1VycB?mb#{#L5xLX!6h7(N=GZCN@}R7VFnAR zvsr0>TN>_^VGuq<0*is~$B!S`UbPE@U9PIC0#yn##vy$5>eYE1+#_)JsmTWI%waC#lJFUCbY`pMj~5wYK8twu=ctVo88Ry>vR=R5zClEva*P;-R6LfyK_w&JJx=ea--%2mVX-NUgxg95m(P#f#lH3zfhA{r#!u#S1t% zQtAYCZj~q$ie;upx-1PK9hi8dP?YBvI%F8O89345rFLqJLjE?bZX^q8ekyMtk?U;4< zwzgI(Gvt4~9&ggmmZOkrQtulMU`;j6Y)D008wZcV-^b^Z?7zHrdCCv$Ezr`7*KR}A zKAmyOfq=ceeY$LX$krGc1;sc3yfaAVzpT%WVmao?YT{hOMcWGUU*MI$s`zI=|; zj{O9?Cr#$+)vK+8>Y$BN4FQ{v%@c#8)TybHFI*+|_L<`xz)HH`-D4=;*t<2<)%9k1 z&}wokGWhlGDnIyGHV%%Tux#bl-jS*#}9IYh6u zt?k()Z#5>aETyM+Iy)cqWhoDIbv;teJ3<_Y(ofhlQhEP=F)jEtGq2>7Ii zh6cb@NQ*H^X=#6df6Wq;lGxZvcq%V1p`gI0yj)a^bmZc7viNvXh(#)0*d(qdoeH|` z>>)R=KX`EK@Zfq*?vi(bt*vdgN?+E?Oa(Ba)yJjM*(%yNxMWFa!!wBxIsv_O?#+-l zXbJn2md-u3xahpUG2@?m{SM20yfKM%R#sMC_an!K{?~0$5Qk8a%i0mrD>b#YwxZFA z*GRAge43h?z;^I*b1$W&bZ&Up9kT|HAu ze4*IbSU^>VX69gRVq#*tCzK>V3x9~=v}Bs~tLDP`MSXa>r6xX5SEv8uJ!MbN90V7# zu^jCk9eHB(=KTC_)YMbMCGAVSnWmOH=;r3sg6zW)tl^?)!_e23X#V+Cx|4(m0p7vE z!I+rng~inE!<~*hxzI z7XQSfCXs&r+}qa|pOTP_jEs=b_HbwA+qZAL4y&Uzh|~3{#>r&RQpowH>dg*<1=6l^ zq>9gGo3ybRDSSycF)`8ovD;gsePrOGF3CM=>eTksn~F?nr(=KXGCud(Q#CTmn7*Fq zbc2*Mww?H%fEJH%pn>4J?hd;09)!5vNP_)mfX?J3PPv}&>mokkeiF+G<9lRMKvB;V zQW`GS7HMnbdPSF4vq1MlQ4uUW4@58@^L{IV6Cyg^S3mwTHCln*Y`_xkyTnlQU@RgtPn098ycLvZ8{Qmop3f?faRl`O)3g^&;*w8#{ZeYQ~Gw z(Hcwg`)&|OQ;JF|C`gWGhYfQ1etAffTLx{!_~>be)s$H!E)M{WMQ`7}Wn*gx&v?b3 zgnbeA=Z=O(p}s_di}`AAW)GYSOX=*~oNA@L`NA9e3<#S5`cyk@&bla;YYtU9VCH`& z}Y1vsjn(6{dN{Kk4GGEt$KO-jfw2eExa`oy+v2mzvn=4?l zthKeZtsuXE03s5XdJw&)eqJ~Xdnm69_zXia zC+)DmG(I9O)d2>CN8eQ+S)0^>0zR+5z$sVC;Dh_DhMK`W-d?4 zrh$Q7*!*yJg+L-^ZB12GB%naRewQv?f{?LEgzOQJFA#Ih!x`%}_-6v%2R@_-9p_6( zrvTV*y&QmJh4Z$Fg`h)w4Ew>sLAO}X*2;*gw2$q3v-*Yx**LBzJ@pS&hTdN$xIZ%O zLPJZNr&BZD)bxCK^p*}GAVt8&Ag~J})sE}Ba2#sp8V&q`b?~AA(?(N4yJm&=Rz_Ac9jHh+7o0334xC)W{ z<;#~LYQD|&CMh#B6T*0|E5@cU3!3ycY3vc=AZ};b#`)kVZ)xr{rPI#bBvzxBjV()F zL_JL}nPSinjPBM@o^zD!C)fNkS({J)f>4ys zI6PyoEJl9sYk?2aLJCj+pF`dU?qiA%f9RSBKd1bZ2Fx+*wCgL7{tduJicl zsKRzZ0UWGZVTUvkqk6GyJkO+!*r^9No17lRaBM%ztfr)sB_P59Fg!z_igLhc z1D|)s?U*w#&TFEaw<~3-3h|OIh=|nME%l>F1yt+YQLw$sh```EEFLB%i=E}6Gyh(? z$Gk2F?ysp@2U2KF_rbf}iWKAFsknHN9h{ln$}qc?{^ss3W~e9~VgZ$`-o``#J^ubO z-8y{pgI&!tHOv&!zkRqx*SGHt{Y=M0b0ovY3pO9F0G8X8lt~oa%Jl?GKPgl(5ftFC zI^I6Q1YAe2sH9|fZSaEEZrZPM4kKg5u&fw)y6kNF8@>limzqeT+3M$2H5|e3nlZ`*8JEL|}3Z zSiO^zLk!-6Dpm5)m82ffqAE}!2$(trI9=!ZvZ1F18@NsY&%VXEqYu@KGhWtfl$u?J zXh4D@27_H3g@z#SH3=;-gNoVS+609Bp@E?vwgy$j#l?qTsJEa&1HdR(lkTCgi*KjO zB?^RSTUgJZ93PHVI&`{ow%K60!6rZ$B^BG6B=q*2OblB&;6&TDYnPI!yc`ya^78Tk zLVtZ#d+uDGx0~zf$@zF5yDYV$*AQ!$6lbMYJH9}FYZ|PB=tdhrh-^myNu@!eK>E(1 znX!Sseok&~vRZ1(NCIb7xAXYk*DdaQ_jG$^+qK69u9J~Hkd3ircpMCPM?WM)>_Anh zQi1F#1`0z{{=`uY+x4g=5x z94kdCNkB=7Sz6L4x1J#rz|+WhLG|E4dgueW*D)V_jKPKZR9Cy>LJ2MrlcwW05A5C5 zvCs~~glS8xV^MV^90l?B&a62zqFf!U3{2<+l z0xJ^}pB6Y&D(zRmA67eLbziu6vD6YR&|`0FX9r>)yPhxy`~CN;SD(BRV(RmXoi<FF8UoQ5N@cz`a+dAD0lu5|70&Zt=gDK^^%cnFhD(f|_n?j8c1%bwpoadawdCCGB@ zmq!<-deo2Ta_0gThmDV|Q>yK{Dgq${1Ax~6K$=1=y%QKW`|U@Jx&Ho&J#+NH0%yD^ z(A?h`9~Z!ZC*n?=sTG012C*?lSBvDTr}bqId@U%5XLGW?dxtGawwwCrXu{Gbjmem~ z{&o$9NM@ZvqxGNE;u}j=!JScqwDk(UfxhlDHjD<9Y49H%2OV>(-}k zF>4b>uFA{HKWYn-&WRd$&?#_~^+mTuM4mA9{q|bx#?2{OTCbTr2St6+lJoPN#tUsA zOKfaxg+aB|&t24qzJH(CTb~-zG~sjFNC93Mj8P}sYV#Sg01^~nS{v=Ue_-Iw_I6)) zcS|4Zu%y(ZX&^kcx*hDtr(NLmhKsTpHA?whcFZ`|y~?^qzrOMe5ASmsu*?HS+XTD< zVEvZYJ5BUu~6D`7{6bK7U{so8xC~thytYV*HW(sguRQr=u53p^) z#EJRe{^Z5U011g{R1Im92G%jGbys%hma)zwuv(0MEZ_tncQ`G73d_kcuY8~DwcxxK;NNZ%JCLb}ba%rsoD&Ps8Urc-(0zhhO6T@+ zZq5>T2ydBG$?z;9dX+~y)$fqVPB7F0kKF?O{RxrlXt!NAlT!)~`x4XGr6tnA!Qre` zo36>=5U?&JB+gR@0j%k=lmfPnjXNs{i=o2p&YGS5{rELOD^{ig%Ee}9Cw5GN5X^oNaR(fw3Ef0S&R$Omnis!jb z@%YU_v2iSiMV0%CdQMxQ|^B3+$=qF--CF0Wd)Cd zFKRPa^jbMEw!i~Rk}3pyVez2WAzAT!q{SZu9-{QZ80&LJ)5#DyDrc)e-s!ojs+^ziU)If4gSFD^2s zZ9+op!RfwXUrV^)f$aQ9wu4vcN|k!C+2X{|;81<)E0M^LvI%(>jIkW1Q&T<1hk40j z{+f>wCjx>szi{xhYuw}e1DOrQ1ik-B81X1CS!35;7HC2OfT(n43~1`fHQxcCT<-O|gK>aiN$ zS*8Xnf9vQhKN85T0vUAL>-xMo+jbMjmltX2Cp8m?q1zYMq*@+BH6|Bi2ZV2zJ<&@L zL@<#H_i3W6Q%82uOG?>!xymwRG4^I?Ch~iNqqx_|xXm23q_U3~HI{e+w@JOt(`>wO z^~8h5%O-+=^>-Z_V8JI{^gP_Hf2HF3?2yA;i;Og8fTeHX=S`_cvE4oWH$Azzxoph_ zw9Up5$NTAArE`bHRMhoY2d~zz$UP!`zkT~TnBZ&Mqa_Bds!JdSIBL-@13y$uX61IbMppJoR7SU{4k zg!Xr46r0LsYl z@M~6!3J6Dh?3D5}x!0|LpU^$LNl9OtDoyaszb=i%VzkVRM<9aEgP3%%%4xICR+3ca zurNg`k_l;PxI5gxwYy=!Q}MDFqEz>_UvKrp$et^~Q!UNZIAnbHS@h-nw;*oP2Dt&(ynS>0fc3g@yb;Ln#FAHYSavVDK#{ zCUtsT#=jO8o^ZPLXDMY`fAA&JK+4J_77c&3dAL0^*Sl37nVk*vjJWy$W&7wT{}Sh$ z$82o)I5Mm@o0NzHiG5b10y3sJ!5YV(U$Bo4O3eC4#~b?Vx}GDQuh+ky)LKx=w#Z1= zY5dWhTwofMHwIc^_v7?E*PEH??nzM7+KYHkGL4NM^`}{frNwR%+ zMRg>lUp((qtrr?)BhJHqYPT?1PoY&w-yucOQrFeh29U2!I>)&{cm!$83nrb$k2sOr zP0VbXTPKSr(?&MTi1)bj6Pj}2^UmK zEdcrImj_>}r8AGHi-{4quN|*RS&2^er#S>A_?oiTNao;DyLo=RPF9zmqk~+P=)cug zeow{W#fvWAnN}D3rPby6ho#+HK|KTgPuN-PO@7_oSzXnt#9bah)OEf3a&eoot4M=$ zy|JN#Nf+zM%Y$N{sUknUT6dI7zD5!4+^&C9uJpNig_xVwf>K`RJ>>3NG$y3mkzGzQ z*ogkCYvD_BauB89S-N+1tP%<814(xP@<&raf)sp?YvW!*L?67oFz6b@XqgoNYLo61 zA0*NNY&AF<>&;miE5?W92X1xOo8kM;b;L7}00D0d1f8Ojqq8#)7gtnNloFU{a9g6j zgie;zF${oM4LjoYzqJZb96v=6o z6c^VKhbUK5D`GKfR=cYGe_WkaR26KrwKuJVh)9=8Nq0+$qBKZ%Nw;)KcZ*7wAl)Dc z2q>kJn{E)0lrCxh<#*1x`G+@NH~`sitTpE|*K50*qL`Na&ptFCR_TR>)yYpw#TJv0 zkdW-#>6+1wIE$(+TZSLu&0r1uSX_*Zj8=pxKcx`9+cQE{?$0T@11HQt_5lNt!;U^i z3j)yw7c9CaX;qb#@pWnWZd+5}M!$i5XJX^`S6TseZ9x6Wla5FtmZ9NQa=zLom(%SN zr|a{-JJaxg4WcIDg(CKt7Y)$>+Odb{DxMkM7;iP)8>Ec>&pfAsV}wuF8x*eXrI1Md2Fox zDK?Q!TAEVoBcZi9(azj3ZVw_7_x&ute1(*ua#sq9Kf;*8jggW+z2z!G>tIn=5VJ8} zo($i-CS2T?I^()N+FTzFxIAO2JGj~NF=tpzTzoya#nXA|>}Y%5U7gZkn;cnTXk?WA z?{aH9J%D0cjZ(l+*7jzd>qg!2{+U%EriiIr%S`#4yboi6g=WW!<4 zc=7Apo-I7Z`p*(A`&?eTZ>2j*ZAg)JQfx*?*lUl|*lXjNWUiny{M*BNq9e(l_ zluOLHMxwVXZbsQBt&4HrWgvSZ^nePfXHZ>fO5<|JHH^#UB3VbXL228m%Tb zel{Zz49dXCRiDAqj^3~0`0}<^49l6q!%S+dAEh&|>-+bx(9ldlClhE5p<#vY@N@#@ z$GD+iGnirc@?6!}*{x?AC7n?u00KOJ_TwHm_bX7G`1$#@&5$PV>2oaL>`&%0l}%!2 zqdGJMFXry>(#i^h4zU1p*)0KIn>KTPD9~Gf}s3!n{_{bCKp9}e)Nar&YcHTi~=YwA=pIbAY>|BT*iLx z&f+qa=Ku3BcLpw{BWPmerKF^$l&8>kQrrgFkIAc^C+>S0zEvpYs2ji`VTT`_9Ocl0 ziJQ$=(XY2b8gC1f85%JkK~7G`uIQAbjY$xT!^JngcrRza`&6bciuH^!NxJ-6Axkw& z?0DC9WnplnZ9Z*Jv}qRdcK&R0c=)a6YS*KShRY2aQp0NeH!zV! zPIFO7uf6}qQu5D~shAQ<(RM2RbS7I{8>|2HXRvH~r^K(x@A@@0v28!SuQd3$Q$nvd zlLc2I%`DCAw=E3~FT!G6GvE$lWccm;_L|+_?Um#0l67OTn{e&B8U2E$#cgXs$hUih zSRpMk=Kkfc$pVXtrDt)=cBZ#YXPccZR%+L`WAuk9O6^{h_@>oC(dh@m$HyE>osp;hfx0(3}X;z|63CS#)pFFOG= z1$%&Zz_=@fLRurj&i?e|1RWjyxj2)67@fajz3U$hIk_HrRUtU9#IG+7X5$y9eBrd$ z(9jrKxKalRgR8qEf^hz*F^y<&1VA|*ASR$vDCJ%QHx&9!=3&i=pCl4E;}wPu;4J3n zID&o+I(gX%W!UG@M{b+RH8nLZ!Mw0OO}Na@*cAKGEWh>i`~eJMh~ci@^FKdzgkKTA zqL7$xaTF}&f=OoMa&skh>730jJx^@IIolX1D7m+jvRJ_*W3 z1r-ZokCxfwmX*=UDk;q)kv~RC>)yR{ZT32joy;4!cDvmCy=h|D`mL${7I0Jl{KukK z%lvjzD{^OFY*x#~n$IKS2;WhQdVAI%A6qRAH*a#uj~ zdVKY+1kf0)`d=nGkd^u6265&31@fscUkod=cD?pJYx!HuS3ie=EN72#+k$bVtN8De z;~&Nbe~+cU{OKOp98-33k5DQX%7~1M)hzi6{+j#SciHv7@Y?=}7bbo`W#w7r-lNg; zo|RzQQA$_kke9^+0=8;cZu0_4(<01T6`ue3|qk! zmkX0q1=a)&EeIOTwGiL zvtvd)kV=b)38gm{)!F#-r`~oFAkGlcipJilZxVX4*2cBwb zCXod?s6ImU5%Roy_ioENXJCIH8z?X+8@!HL?dHCmVq%7(2fnGUu8xdkj>yZ&EwOlM zYhq<)=dd%u;Opk-SY1~aaHw_`-Qf!n8fpe6vfHE{C9{VqjemZ9l&MOTR_>u%=jomo z=j7xR%aisg(*1yjU3*+8>ipkHfD=H$ZzHj{32>J2j$M12}c%;h( zlU6Sk((<^>T{_Cr1oj-xQ#}!>cz-kyEMt*f7YtWmK#L#bBb5ix2s-nH4d=W+pp+cXCV%o>Kz!L#03#n@z+l@Sqx-M*XOKf zb&<={hjDvnfr_LevAsy z0jBq{V0~gZuZnt|+%6w>5v%}QF&^CIko>v1Y>4n zvj?J4wG$2L7ipvbRt7OCsURR#Zrp27+xDjNv2bwU>taq@i~D2hH@ZaR7&?RF043~W zv~Ey7z~pXcupZo9Xa}CGcin*9VeZS#Teqk=jS&1I#beX$+qb8WMex!^g@>P<9kK9}2>YJw-e+Zv zzBK+x;Lpy+mLcvZHg`!AT2)^UjOkUA_h0+aO$uq+Xy@%8RQM7H9m>mQ%batRn|?zU zBqZY?4$rpw`E^B;f%R0l!~+ZE_Iq=2Wn*J|TMSil;756oIG&TV zv;eh>BdjZaF0RqB!4S&d%gY4w7BWnJ*T>t!;X|u9s~qy_w}Xfyk*ogM*}stS$X(>_ zV9o-;edF99sWxR?(Qb~f-gy;KQE5d*4WXg=4Ixjv7dy#X(uUO`pXs?yUEY)F60g++V&6w@ffd%@ zBC)2REjb7uqqI+3ZW#Wg-ceP>wE&{ArRC~k(%HO=!SR9dw{I1O&E37ddYleowVC`f zwDe37m%f>kD&vSXbKVI#r@aKJu#7FLY(Z72+#{QDjT(1$PLugoZyEvD)cjQ8+Mwo7 zeV_)mUi=Md5~Rk(K-*sm!!Kgd_#pM8ZN#M~Ux8|R#IM=|k;<6qv9YpiKx{1gA;aS=K3*`RaPeZ~75ad0q@pZ5oa_sV z6{Bss_2H0g&o_U_LQc-c)Rd4)U;pzMz*ir`@RQ!Y*~ec)oat$3xNV;7WoH{b>@*E# zj1db9je+=d-W2JHfUV0D%ab)&U0<`~RbHcH8qj8B=mXRNt(kGAJVbS~684%%d1sp9 zsD&0+Hs$lSl}0C}U#L(CxY_FHkekQC=fYp+jcSSdPSU&4TTjrB`r05M$LqF#=(z*H zI2R4h3Ng`5U-!rygvnDA7J#5?naNDS{W*n;s}oqj@e(& z`Ld3c=;4Gao13qss+Szqx@LQA_p?J+v9>2F#lpg2?g7F_GS(H@2mhY8Sp9s7)mye?}?DPOGqH#Cm@=qASKm%dHU-cu0&KsL{V|^;sV16 zmSTKLj}NQ|@UC|+A2KoB>;qRi{PYV6BZ9oq3c$R@#l;C$ucN@>R-e;XJnuTCH1`h< zOjQpz=-Jqs-L`btyM*A(3RWSz^Y@g*7cjJ$I=gcK4RUk8gP;6RrQP1r(t3S;oq~)k zEkzr;_9+oTAras}JU?^g4vhSu(9js`>dF^x^s?UAh(vGOUEl@__X;Iot6*Sx8Op@; zqrJUR0Ztc3mwoE>KdUsPckkZ0LryIw{L1BbayXZYI2$+i6$LRh!ClG?n%locy61;& z@)B!U(r-lM8C$+gPrvXsaf<)>n_|4;{560l!fKhdS{ZVgs5`B`m(qrbaQEdsjk0BBE$d?w3g5onuDwgoVfX zLd8x;%kf8ZllwYL1bpy&m`jWgSTBAqec!xYn2DOb;N^3CI9dBs71zaOuc3;Q)D?M8 zXM>J3rA|ZRR-K#M*jT=_G%VaT1IH67EmA#g$B;}>-+VMt`D;IfrPk_&^EFNKUe3E2 zeZBJ94ma<2SJu62H%^c<9WJZKRKH`>G@D2z+Hbx-JK?&|ev83CU!km1<|pnF$so=h z*ei40GpNi~R@QZ9!wfFk(`1kNg)Vklp~onG$kdMe=)Tv?IEP_XgvX-Z|G$85j0x=C zt+gIy1_}_C6#3Z#z%=UWAOUTFMA_ZlJ@Le7RvFa(%KCZ>(vkp$qwQG{SdxFt48ss> zfF*z$iH*-L32QK*!1uW`m{4m^HpkUK9y0#$!m$|4-l>TR&Qiq7%=Gl=h|NI^1Ua=L z^oe1A8KljOj2su*Fi7inIWgOl@4tH5+@{@>$ccu8)rf*g3FS8l3Gd!hQzHou$u@je4pYJh3G&TXG{)+b z%Xc1L`+xoQE4~uB8@?OPl}iiK&5y#W1k*0*C(#DVye$6%mOJbFIEUrHT`VJZiBEhGpGK&N`pE(Gg#+z8CqJ(YHOzxh2gJp|Dn*t#AC7N zYWsD#Z@y`tcz8|fXxkJez9qW5M`j|QmY0y8oII<6mo|-x#BB7U6>Z)vn17wd<2*L} zeIm<%i!en#^n4^QnUo4o!H1rVytXnWf$MIq^uHD8pmVYS5Mmd#8?Q=TI)1OsU zwfYHik#hWY6VG3$g!K4z=gH{pypPr-L0dE5=GqJmcx)0Zmhdc$8) zPR{w&t2Xn&63zJ*U)2{cl&il0aw4c;i(L$6v)1`(UD43c5S*So6pY~GC4c>{kI0sU ze;Wn$#~L~wjbaKnM&>S%O%WC?h0!rF*JmYoB+SH6q({M&A-GO@HKAXDE2m=#CM`ZV zp_~teq{z>oKjU@15xjXqZkwbD&pW8+OOm3ad*8F^S~7{hz91zd`?>oHIWqF-^@SjH zMvcZe!V&``1gzuBUr&&~9JhFIW!uq^8FVh=lHMZTsFE@U29nR72{1940UVqweG1+P ztl80#kx#X*xVX3q^6~^{wl=G4YnxD-E^cgZ@VE8N5~3iVUznjwF%L}MtfRFR;pq$`3Y>Wc<6k}2Wv+s>$_HyT7N-q@ zYHij1>^GB-@00`5=_5V;#1!Y9=+(m{jsk_u4+=wH$8Vc*_UG7L`ab(6lK}^sr;d(> zIm1lf-WEd6MC)<8$;xt6k|z80C*#@1@LfkKeNnfQmf|=2g$ZIL1BPqy-!87vBmg88 zb8CAdh{0o;xTRY^NKV zoT9+H=$a(bBM)~H;+(u~+(UoeYOxq+PqtFOA-nY9gGUoVo#Fp?gXk7jSA#*&2irx9 zKZp<#Hvn6&4rJ{q6^lL&2?+tv&X^Q6Spfe)H5?P!HtV_UkHZ;BSPkhq}pLb6Q)EiTW)!IEL#xs zi4TC*fO>fN;0Eq-g4r-M#GgM-(~EKl>pgkv%+k!v6|9T10*sS$fZtC#U<=pP)C3KF z0s;RRVM+QM$T$xdoYSU;1~AZ~-`@%ShlxXcf{V%flioTf57KO=t`SoL?nXe*q@5l@ zDm1iz=MiF-?D0BJw8dMol9I%Hg(#SZ@ygj5u1(s9Oe}=zBO@|U)Mp_=|aH8pq&3JRvAXj@uZGBe+a zR*+Z5?CYnSf88vr6o*UG_u#N5U)X8}(k_Y_S7*yCX$!tNzDE3TLUvA#+N5t%g-4?M z)D@Ebp`aA@sLXG~`_LWRNaSzuiTmB~X(iebhcm)1JJ;Ls`n8)q-eKUakg@Ztt?8IQ z<0O5CMrP*yi_NGme-A%@=D5BL>-+dg3vJCw#dN;eH%c@g2czI(ed#-N_v=c7_2NTB2+g@pKtJOx@@7k z#z*kVzk%{w0_$aV>vc^*s|IT}2ub~!T!gOZ25+iAi#m%nzsB(oJ$(sM1GDUN<)n@f z9Ey~As$aqxBHsMG;VrSZ^i24uT*gWf1<`j7Xk%v>@W47XI~;3 zq4MD8e{W1@!^BhsViLcqhzrD5;4+xFtA;LQ+Jzh1Dz@?*G*F0twy!9@`}lDgAgwh* zcRYVgGcz-VWX>Gu9wtn@?&mPReEAZ1vnoU7>5kssO<1xMn~o-U**P|l_uONj_%8{_ zbQmYIJCXp!)XtNWm%t<{%ffjd-3uEjIW5f;e|uC;S=q5aT`1Wx5!j4RpFVXme8d*} zcO>{O@{jT%8cpGlCUg1U&u9xjgHD2;S+D9hxmL5+WpC|iC z+@_;mSl>Exj(fu5>x)K2q^_+k^+L5!B_RSGF1TFT5m}t|ZKRx@3N5QcQDT;3Dq^88EpQ?A5``Ssm zwu9eJWO%#TsyH#fL(%Kq++5bJ3vmR}v!OwbghNH;#h99=p`f3nwl>*iJ*N$o=&L76 zx)fd3T8_`pdR)#N8pa$@kdAz_jh=)n)}*fpKYag|ik0@^sS>rgle=5MhG(OSv1Qb2--zGQ<79ycMNVZ*E$Y1 z_C4K40$4K7pG#CFL&+WISq~L z9T_>f>5h((vHrQq&c509;bEKK&$CpZs<_Q)xgi!j@{rQ>^_gTGnMf)*ns72X+SGvf z?IV9TC#N7v2%a4s9zt#NuSCI192p+pLB-UM&&uLtQ&g8B!*BS?t2PSA!sYgs?y;!w z9y1%0P^CjwVuq-g-^fgQy2GcdNi28xuu_HbC2|8#kC6drmX2tln6%77Tm-W(15+A# z{6iZv#9X^4za@MUuLiBlt2a4{g#D}QNI!#}eTwe(e3Q8QvF&i(*ZQ3DxoUd3DYCWf z0yIm@s%Q8#XC9tXL0@s5S9p&zu;@H8-xJ>zP^_s&Z{H8x=QnSnHcg#-SIO2slxx{ zvE%=h$FA>|GOqMz^f)@Ok6{Dd`{Tz~^Kq$veWE!5-IN-nU~@V@J3lZ-$p$4=PgQj| zoRC#?;Q)k4@XRZ;sshP;Lv<9=1qtw4^b8Dc`SCo7R7L_PHg%1>+MMJGz43|U-76Cl zd3SpM{+^zoM`Ug8p@D8SwY4vK@@3NyC+-r$nmA}+3bD`&uN9z zcfUyd$PVBC_38e{iVBY7=f(IQT3V#94JAF&MIcfgAn}!2Vm1=A@_1S$X=?wg_NP%_ za@ zO^u$&2M$`=)1>uC3r|l^ZS98GeP1!Ri_b|(0xxkePeJU^)t(f5A!qRmJ;wGasmyH5 zZLZ?;1IrV*%76TiIxpDq^JnMKoxKrA_Nl9?YAP$kWHNH@mxXozV1AaV8K-g(z<+&% z8qVWOh>oMguOL5a-nul}->b{?Y&spRZv);7Tg_*R>Un6HLOaXr3e)!TyX#WA2H~8%qP2*vp?lpv&_)B$nEG@pH z`n7Aux*apdQaQvqJX69#!U^H+wl&6JRe1Yt*RyASr~4Rbfx^n;r8*yr^GfsO)1fal zu*bTfJa9*`^oUO7w5NRg=a1;V=bvA$yx-ou{@btr5#X0{p)MM$%zN30mORh^76JoPCwM}C-RY( z$8CRp7Y!3vtJ#aseS7-Q>*9o~J?6Hgu&^*pOZcEP^A14{mou5ZzP_!DJl(^GH!@J! zLi<7zrqGeQn^?{F7#YK45FmPa%lRHFDk)jq2#&s5oo$e)aET^;?VDep%`a?%-H zg<(m!*m9EHzx%xX$S%;0Rfjn7fd>?Qng3VxDP;&cJyuY_i+VYs3`d))s_OJieVI2i z3ro-RblQC<(`Q##S2J5n-*F}CRe_<}J38_m+()7g{C0A3!u_jzK7W0c^!9CVMA7$< zWV_E;-2D7q9je8;KkicPA2tPZ!Vgqn6tPeeA^l#U(BH*&L14>Af)Dv zO9&+^4GSq9p#On5vM*$1YcWJ?1~RzD;$%NyVc-1uqnnJBR9j1nEG-#aZ`wm`X$@J~ zDe-&1%+KAZZ1W9l>UU-WO7rrMK9l-RxLc7ag*= z8zYH@5r!ESiWL@|gL!>~j~nXH{KXd-Kq|@%$jsy7#ylraRW{ZZYA}BDCaAL`7%So{ zSwsZ>&09AESUyU<7K$ZEA8h2aIdpZ|wZ#eGV=s5NpEnl`tE~7>c8ws;H_w;udY^9& zf@%M)*vxI}*J3UA10+T2>>MmO1Hk^vR|P|;v9a;e`)>+a+Aq2L%6n^PpKQpuCDmpL zCEB0vjeqR1SL{Dw9_)RtByKhML5W7(?Kuzk-H^utaw}i2F3S}aSxIYiGUuj7OBxLF zjw}(gNF+kk%+6RQ+Y5cZ^}9zWYG7t$mM~twijti7Cvre+r5f}f}m(ROTZH^I$s>Hlne`CqwNR7ShH!uQ43@OsT^cFX(s zMc_()e&}Bs@_9f10j5zt-WT#k4wfE!#aCA3AKz@%-+2D}yX-3s{x{ddt1)Ch2xV1Y z8_(tz;H_6swEEaEvsT0QFk4m-k4%f4=ESBI@RLNAA4;uRs%Vjs}jn_9x_>Vxe|tZSx1w%*5VGAak_9>JzOT@RC{=I zWS4IAls;NiR$mk&30EwS1Jx=Zb&7$QO~L_572O{m50_bp)Q#SOKtE=yE^eDbcN)Zp@->8k{ z-@k8B3QBmh28%2K^bTHGrA$$(pwk1fTWAFU_huItGxOUNQ&LgEBolv`{|@-83r}BP zUt42ANZtb2SF?UK(@X6i%2>!}GxpEi8~UmOARv@2!G3%GTC%d%plq$3W!fO{{`N(Y zaB$C1-U;>6)1!>Cn1aY9IT@KTes1^F6nP9=f2;@MY(UM=^aK|lp9{F`g=doRrd}>w zVD!2@q7dgl+e#CdF#SwaOa1$Je7Tj~@QcfGMQgcXliQz_KELf*zl$}MsTvEzRv$ZY zqJLE^2p>V2V!q_Wlz|)=pP=8W3>P00MLDdmm|yA3%*~hY(7eefIFczjx6qN&jWNUQGYR?-P^}DnO?EN@N&-v#gBQOk^z0@l(+E? zFVpy*+}G7>Ih(MObyXPrk?q41_C{vdCN|5;4`6- z5ie9GchBsOGJ`rzO%Cwvsy~L)n=-cF)$sjI_x4?hvD$A6orK1g?FGVDgZLfwsd#)X zzJ?8Eny1yHL~^P6zn{bP~t*d>vz=4r(ZO9S{xM};8}6}5Nfy~WVT8z$uK_a_hWI>QserB z>|^{M1XQ{Ke}` zF5ic)a^%R?23m6%mT5J_iYXioCjJg=>r%Q_-=2u#NHFKK;NZV}C>c6mH+z_+nOAP( zktUf#Ap{2xpEC9cR?^L}_xT$3lkjV#3OQx*{PJT`XcQ~Xq{f=-voN_><&mC5QE_>b zbF0ZLu}{+i9ywm90^xKOi0S_k8{+Tc=HK`%U&8NbPaG^x6E;)rHxv zHK`Vcwg#Edc5R8eVi2*^{e1rH z92{Y_slT zA@VsZQ^dRfTNkk*%}9J9iHgd=@X!y+hJ7a%Egy3;b9TeVXzdFZ&J*m^gr<3tH{PuN zq3=B})~Z@}9$KPM$d$B*-Ktt6cz*GF&lFs#=eB0%Eq?Q2#1WH&JE%yIf7e1I172T| z?acqXMA?6i5V72kNvZnTza>&>crfBT!i7kEY{kq}*5q36arn9PTXkzSIi0@m;oXnL zjbG=`F6nf&iobTWxJb$#<(P^~htOJK-YU^*Bu~39t)TF`w{$}k>mbIq8lCR-%7A#D zhb=2=c3ehas};>kcS1K|(gV_4OEQrpAuqTY7@WT%<;C{+%U-@f?NUukPszy54!9d5 zhpVwh6)a=e;$`~25Lb-8Yd^*|hl`@s;(p3f%Vmo60ICyXL3<#PjerHddPl)a%gG=! zn~M1lBlV~izjw4fz24i^V_wIns;;Km{OMjPxL{Rr)VifoLC2r7((U@nKuk*0$xDTq z8GGy5J*it+o;D?VO+`huwbrX*#0o0H76glCN?Cq(`c56_ZmHyob+&E~gbGISmo~EU zmeVG9wlzp!4JJ~w)(ZCW@xefSi>RN21x$y#K~dhS$^ay|lkk@N1s%~{cFqQkI=k5YbMgUQL zeb`+TXa>mcFfcH9V9OKYo&))>?sR7;Rso@}qJlyt>iA1So?EaIy1o-;NritTSJcfO zKL6JjCsidSEXSn9Fs1=U0v(1oz<39mAL+(9%EpF<^MB>3V>ma+5p1-yH87ZCH67mh z!y`{w(EE;t+`9Ydaac$Q3lr1NpFJhT#a4gTAoas?=SF+zeU&(jkl<5H&bim|zCrC+ zxRehW#L)MAw*}9lvDurmaCKy4Tq=`0l*BF771-?1(4=^HoGB9eSEx zd}+Mz-{0SF!07-E5O<3h`bLW%bWJ4vSvy~nH$A>>uw0sX^#(^gL0ystT@-g8-{WG9 z;P^TJh%*YWp5FQK=IH21;4t_Evny|C#X0-{wtEuzCY77ZSVvFty#KHsVTsN6rsZQ1 zS^ECxD$^8CA9#@1ezqE8h;(D4Ot&VK>RK?fu|OChq5AN5`b>Asw6ocWRm#278!{`_ zX`==WibX|i5s`ff<3I9PLPDbn5E<2X(ik(cL-8u+h~qMm#cwX8Ua23ljM-XC-N&yW z;{FZgT%}&dOxhwnO1_&BE|LYs`sdE0cr{ou7RE#M{C6c1YhJyQkxt>Anw#2p>El8) zS`U2FV;e4&9Gd9<#VRMyt~8vteI+GtiSf!!F(=G0wpNz-&zL0&a=Axi6<6~k2fOYY zEn#n!_8!eX)L zR7M_$KGYAPFlasJVrKOYN3DBub${|j$I$u#d8@^hyetOt&_VAj_Ola#FR!Bzmijn8 zPQ2#?#Iq+0sz*O-ZusY)Q@(0PU90tr@{y{czhM{m;{H!c&ljiNt_m1s;aE26_EVX1 z$S(6y6Hd}@RgDK%`*uYOTjz^~6Skz{cKql5Ig?UE2ZyXBN*NYB*jWGLY@Zc?KLHaL zL*XK3f-xwxK}8($0-Erc+P~p&1s$1fYhd;L0m^_O=kl1+a?#=q8DMO zd2#&`gn$7vs7iT3&&=EibJs%01g7Ruv%v>5?@J9S_*w{Ym95Rq-XaQ-G(=-cWNXUP z+OIv`-5W5CqSmwl4GD=nWh`*tHo4R!Gft=OND0P9KBxP~0B3y9EJs1YOLoxGwo!k5 zp?nHCB~G8!j(3TWt z`P)7Xr|sFKxc(ZjO-&0UEXS%2$%wkM8N0fenOJD61nKCPiPlJYU8NplVKFjVXx^3b z_KL}y3@0$yjYvDcVL>9GtLsgu$|EQ%sp3=^+;NY9;C{znvYdh4g~)d-^Itqc+V~68 zU|`hDpS2Ce4E`60U}JXn$N-_7J=Z`k2|q#WQtgI5iL1zo7*5 zJ{aY97f0Z&zB=eq;D{qQ>CQIKE~)o2W9Hc%_XMaw$ZtD6DwnWDwvY^@k-5(aIIMHd zD2LziX$llmaYx6-$7J+dKJ+2PD*L~!}?3(zpkUI z;5&GMk^GKF0eKfRA8;uGUqV#W_+IwHER@sxDu}4q?gNu+UlZ_?&>WJQeN+=d(5?`-zc}-3`7)`YGX6 zO3#=L9+CvXw%(^#nc7v>QUM>GSzh;byhfLPaNo)5gEIM?+1u!>RJNxV>Y`$MdST=0`CrgdlsFunmF+$(_d@gQx??zBQ)+e-3Vo%JG+0!uB8iCgr zdJXn+<&6Y3g^{RhjbU+eCQAo>RL(c2JlbNhkk7B+zAT;7GFnCrBFnO4St8^}5sbMw zIsFMN;cSUoZJ7lS)b76=Du$=-GKl{dU)z3rXKGWf@xnD=JSFUN_%6MhG)F#l!~g(?qbT!G?Wm6wOE%d7}CRlzz$ z@61<-WxJnH=r*Zb1>_j#N!a%c;QjA@+&Kc`}rei#FM7)?BBd0 z*zj5Ji%ZeLjeC9gIx^bYdOqQKZ{G$6ekd+Z)rg|0{q!j&BO@p`5$02+m86BiX8mm_ zfPXR$B0F$fKyQtV$m{BcNhm+2bx`)r3JD1g9vzh_Dk`ZqDw5TAYi|XgJyqB>HKuH< zXt+QqBV4O6C&#$6+H8sr@6izI!Tzp?<28-j9GxquYya(D$!W;pDXX`WJdr@SdPuub$xb(hcY~&!w z-;4TPyLOIsr}_{5wi%yXe+%wxr($wA_n&t#lJRG$_FDaaKwWRRlK`NBks{ zRF*-L*MCsPX7V6`r{`wq{_mouB7LZp?~`0gtnqZcd#ChBQwHZ|YP3vxqAs_+bzLE@ zuZhOLV%p<6=j*Y>NvvVmElds{fY<;I1Op3LCm40`j#_&W!@U~{;)g6O zrcqW2X=&3iQ&$kL17)_^ybtsyGFvybqxE4%Vc|0zd&6!}MZiD>x$G;lL{mXw3j$aZ zXj6C2!OMW?s`)wZkergz4uHppSg%mBbDI8*4`vJz&0czGZVsFapY1one;^C>UuB-a= zDeT~ti;LSUH&>TeZjLU2t#-DXy?tzer-0CP6P;d$iD{!eDEz!;H(N5k9V9`(6BPaS z+U5bR!@qe`ABKZMgv?L z+ILCTVhfZqjpD3S+|iKm8>}KU-Q|<)>-g$8JyzB`x9DDHbmV%_g-1L$3279)cqBD; zHqQQmYEp%=`U?$JhAxOWeV=oasl=UY@C}1d0kq9ewmkmcN(;1e74-+O`Lo+maoIb* zL$&OzYn%6bxLE}sV-62L#A2_yop~J`Qh?L00P4p~orv5GtTgCB(VuXhIEfNzUEd&= zqVW#E@V5`-`@41%_cP~pe7Bu9XT;BIU$c|xYrQur?M;6@G@?}cikv-mY`+i*Mq4tU z1)zNXJ@e%eA6jiDy-HQ`4+_!*alfZuLY;($H%a{M2|l*uSpWT|_l7G*k_QbLLnB6; zqv+^))jiXel0M#rt4B%J-tClpOCpGTX#Te~>C$;2(pOFD%w}w@^^ujQBF=)DWh=rZ z3dAKd9+KZbF7^k_v9r?>_B0xtY1+1(Ms3V|7R6C%DdtQCQfxhr-V^&Y@EWrVQ9Q%c zL0j`d`*YTWkNiux3FAFb=`_pdrEvm*Hdo#F;}=am;250yo-Ies`S?LlR5=^pMVi6d z@0fvj^9I=|J`48B6dq8oY|k!iSC+6sHI`SX?%tZk^ZZ|tNd3hNfNs9WmM};IZ9{4I zYpe{tXKXO{nX}Va;iE^7GI{WWemw3)xWEl&XKTx#m`*ya1HqY0A!l=J_qwoY-A)1SuL`|fUTeEeKclqJ5WW`DpetgLK_SoOZM zk59Swd-2s(TS)o0{D(oAyk?Qbsi_^6#>&p%G?O~ey$Nk;2>|jK9wBIG2zmD|CZ;Uo zQ%u>rcYLHiH|DLatq9-Vp0@}V!=V+yYtv>Sgkk{13!kF0vc4ZDj@vVy$(}%@qFt92 z7f)52!Tjq92;P%xiwQaAIa~A#Pfad)5(jMTlMz$yuPwCSVlDp{tm1Vg^AE+bb9#kc zqj2MUELnTtTUQnYp7Ygc>$RKNofT9?&PTk5o;u{mmF#Nvirb5MFU|Lgj%sVi%MIGs zIYzzU;pt4T<3p?XR`K*tAB!e4Gnyv!lG>8PzQ+^TZ$pn75_XIF<$=T(KoI>d_g#;B zwg2;uHGl^o>S>^7VX=Xl6Vwnd-z&C3MpND+T5LxqVo+q3N+HXGebo zV8nUmxl|?0cWC_l*=Qir4u&eXE^i1$lKE4^WXU?a>8U(#%A%?&C%`2^m7HkP=@=Lg zhcp|UvGv;QGwv`E2!OG66Wvxw(Vd*0ek>?3#lKQT;eB+&4yJ28JrBBpjw)8iO@cV< z&kzCYV`7Ag*-{#WAh1!4dhFSNv2aGG;^h}k&C6NX<}&FX`HT_u=9_tcs)(nx-b=Mw zKZ$rK)u6RedFndBDPz1KDL$Jebn{!l&?@_`Z$1fCkVcY!v78L>%SX0VX+%IlL|g8W#>q(k%` z5+_1}f{K~kM}te>q0s~)g$AOjFjx}@JGmTCDR6lL(NOmu;qWQ&9a$w7Jo4Rtt6F)a z`@ibwRs5du9p{RL^}W566t*~^W9>wtmZnBm)3NKJ+kI423-}f> ziIV2)%H_|_#57ssU7wOrRUNqaTxr1DeP>~hxY_7~i2Dkw-g^9;cSkr3^{G)DQ2l#% z9MM>mclgZlE;Z+Up&N4QdF9`!B~(pKM_bx!Y1Vl#k$16Ay)`n;OD-p0FAv<7SS5iZ zMM_}1HOEz7VDN|HCTDToRP_t&k$pTn&>xInv83?hL~O#!mO7&tyB#lwz$?yu@SIt0 zOBxb-ImmVnxp)-PzJwQ$hU3BS_(lbNL8CscaZ^$6TRwlx(R8t-??0YVn1+;#;c*yz z+7RJxpyL+#nLdvAgpezVrKP46RIl*wWwEcR;+g< z41ui3%?%Resw1MHINzEUu+6tEWC2qhf>vOvvr?K~L*7zISlFP^<)%ivvT%#kLJD!_ z;;#T6K0bBu47?v@gyPe*{L5n&>{_6%nKR$q2g+Y7z+mLhL*PQrZaCj9K#*t3Yg8Dp zfoGvGJNQCPZKlc;s|EECtW5D|Fk+~(u5N5_G$uS+=WX&ApXK6@J(}!VZ__z_$3E{7 zq6I$In|%}=qX1Gp*x^u5y&^}PG5T&Z&s%uq;Ck(k6cumW2!gcl`=lhshfI*tvVs#G zq=dDNfc1?H0G~?|pBGzOV`E3gzmS*Ld-B8uc`rfo!H0IUmj#`j&7eELd=XB8Sju&@ z?{^}Yt*tj;v#8atRX~=iC#k~!C9dJD6Jv5OrJeWPU?apC37eefWTO6Y9sk7g*3 z+h2lzYzH5LeB~z~jKU&!Pe(eEAY`u(DJVkQ$Xc+=)Z7Ym>=N z%zL9-luQRs_b4_RvSLP!ADH0ed>jO5f5AJ;}HOF zG8_8SVNl@;rZ2kaC%2;w{v9VyWuCd_tlOfuZVkR*RLI~xm5Q51xl$r_2iB&z*_Wfl zm3P(f-%2l?3>OVmROqBXHIEf1idSRTvi41&W6q9vjD#5(o0d<^xx!*3jlh7*R4lMDUoF6Rl+C3!Clo)vM z4LbPzL(^nW1L-aOSNIVws)Qdupf-N$J7NBjt=~!{&2U-u(aE_w?4d;3xcJMvD^a&3 zNE%Ogznm@K>Vyk3q`T_v__i{eJ@A>nbN8{9Wy|35nIrYF2DX^|9Uj=(j zW?O26;+54kEhGftc>l@|%@@;&zMuhOX2J>=7`U=qAAWY&MeJvNNKCx$ z&UJsX#I)7zSCNP+hq~QhswA`ReI3_=&yo4hpT{RwS0mr>i|h+g3RIP6xn%|81l|i5 zT;tHoFJdNq1cH@ET3W<^1B>9KZ?GOvhF+2-CjR{SGrpHDr5#K|zWa2`2h>rT)2e6x^{=hP8Mwg~aaf-+#c0Y*jvfxu;-p8!A=o zJ`E1yjd8iPH67Dz2|YB4k@8Y>!$GETa;mdwbbQOr60gQWE@tPr?-6?{i*zFzOQe3R zQMt6QjEIK}13`Al`WlPY`9xQvu)t9zIHUOGHM0QX{dXTeew&i8cE!U-hO^Si23zW? z3BkQ8Ej+KQUJ3EK)1djatL;fI(yaxu7nTsDE3!6vb*>PkdkIAh5 z!!b^xj8yzrx5q_lCS88fJgk^%BdL)h|9qmX9W?$98%Y5b6X>X031q!Lg3rmK^$>GF zAw7R>Y&(g*R@BiG-O)2X-$<|->KOKNAi);yNR44>72{L^uuAMO&{X&x{z>`#`7?ay z6_-Mbfah^=Z~%2Z@G|X;KRosiejD1x#4sFyxf<$$4(QEB3Se$D)Ys>JSRTUc)A2GZ z%UnsRtEb0q8d76c24Mzvb9bkO5E=p;2mJy%k?4>GZbWr?c{#iv=re)>0+vklm);1X zMkpr%0V10;uEVdDl|yjr{z0Qhw?E<&E^BI%y2Ejx8TJ1CB)I%b-3s7B3eMPl?d@HI zG@X5lFE4SBm)F{A7MQLB_;F$(f8)B zkx??TvPkGFu!yl@=5^YQe5K?yfyDK^0u_Xwu5LKC0Kaf$c{#XcVaQi>cK!=iODL$Q z1vde))b}_h84Y^TOe065)WU0t8icO{nwqTGG*lq)7vvs2vGnJ^zy87jA3690w|=xx zT)(bwXmEayf9?~)6JO+=kGwJgn}qj#kvCdM1VwN7-s)U>#*PwlWqIA}bb{xPr6>3j zZnn2u+FOpa&T(>cTdySst{!g2XJ#HbTr1$ScuP)hwDogVj*vQJFlxP2b)*rzEpWE72@nr6ud2#psOF5b?iZmYy-5*`uXa{Uuik- zM^Ei1P@oWk`L)_ZsQ%@}KH@E!?g;lNVX#>v-sJqI*A; zU)1WA05Hku>j>uxx~C(SDCt&PD!+_wt znL2o1nhQ@rq}bEb!_LA2ZjwD%S%()snzB9YcbRK6(A6bX&z^c$UA>>98gOedFievuUn`B62100Ue^?<6&)xhBLpMv~f( zRJ|SgE+Id1zbLG2Y$8=z_KkENKGeaDI^$;J{rhY4d@_j(fN^z|8| zI-8@9tAN~q@L=DO=hJM4=~iFzf|WuF7~Sthx(0$aj>k6@*&fQ&6DXgU!+j z&EA7N0gQzt%dlhnNhE<8wRsdenT zAB*iff{sFIJ{idj$HZ>yOQoiD0*WQthCh85l3zya1yIXiB3R6^dxDtm8@_<}CJKh^ z-Cg(O`Mtf}`Q||X$C8qGr`pRjSL}w`5w^UA(mf_>EsA04NiS$XP?3I>5&g)1=Imt79qNI zJIR2Xw&~={=-VAVy01URWCP7C)(2g+)%^zaM2nhzSJ^LZFy!9zn7qBI{DSzCxKEVA zzv)bPweeVufWR{-B_+w2i!oG#vR<;+eG+(P6=l>J^fnA93=WALMw_hAw zX+hVmJlg*6%2ilkC4gJ;?c2w8cKKkWW@pFIo(6yk@_}GcC|JY3i_ADV+WC;5&z{L# z$p;}@FuPmr*=Pf0BdRw!0bNfz_=>axHAouppMd1?%L&@LHZ}?gxuI(t*Qe3UM~-RZW1~+4jvvJE-o(RarkyX#*Bafv*qIR7Yf^DV5oqaJ^nfv#zgLT zzl9esCZ^=Y3l=h8MfI64Ur>XCv%t!X_H}_JXM_*>=zhPstlgX;Pk8&Ib+y|qF2&v5 z{r=mxgq{c6`q6=U4|K+K41~UZtE;L!JlJKV67il-7dIAlY1y4jf@kf4fq~bZ-L(gx zN*VkuvO#e@3#Tqs4#v;Jur*x*3m4_DqlY_E4%!=!Xy&X#%-PeM2U3PwI?3 zxa!g?{0kzQFq~wFSg{Qn^7FV(KQ6FGJTfnEnK7GUz9;a4_2)gLgael5Zhxe(xeaR&b*$do;1~P7upCA~es^ z2)E_Wgb_~dG$DC9oDW>?rp9VFos9Wv5IWly$+!x?x7M$4R@WPi+fAep7c61%32UsFz4 z7$&8>TaJKLcfi@yVbd3#Rsy&ueg2-F)!?^OyfF|Cz*l~Lz7O<5-U*gD1La^-Qxp6s z`{Q@i1EHYEgG@uye%Cr2Erh3m!8lZuQlHdzMCxEpwho=0ZnY8-5nZVl;GVJ%5PDy| zLOEUP@2Jy~V|NKuG}Me*-S+%o`iFlvl2UiWdv^@AjmvIF^){mWO}5Gzk(9ly{apXQ z6f@k=pc79)GLlqbjr;c>`PG9i5MoWX>yqn_kn$`q6ck>lY!i`Mktcwhp~1~R^ywLJmBF1jXF4YZ8y-aM1TD6A0lAu=E8RmmOWa0r zaxz|AU#MqvLy7lluhB6a>FDx0n_2@MU8%_hmyl4^-d~eo^bqgLs>|1~LHCagNcx{> z=-*|H&dy;iyT%}Lcu+h!wQT%b6wRJUDkQ?-H>4&dcRe^fVC*|^_F^c#^3|rxFqo^N z*L-K!Oiw;Ompr6i*)ReH{N+Wchq9SxVIg0#WZB0sXKhX`%%`U?meH(jD0~1Wq-jKo zg%l`MvJsRIN0`o?p%3S!5%B$p+MgDe0GRT$}15bnqL%3S~qv>*A0pP zAl5)rFS@m-`Db^C;kSA9^iF-@M{Z?Rx~$Ezvj%ijO2Cqr4--FY_K-dzgX2d0O=X9( zee-W|OCS}l%|I&IY&g%}%P^3HgC%0%JoDNaX-F?vTUcFUU?%3Jh{zCmSo`;{tjdCI zI^@1-=KhjJXF*Y681!iAIZQpWXGT0yWjFs(w5a;1%)5`uRO3*UkO(DRASuT^iLW91I*kw~^D(;MqNw zj^G67R#DjLQF=>@<q#sAI(z}_#)+} z@1yDc`XlcXc_HCjzF3ebA{H4|Uk{06AU8^B-5nTi;LqNG<3qFv7qR{&A$uf8z1s- z8;l#wcmv`)^Q`6zd+f7Q1=+WK@JLiea@L|DX`<8-LnaJ_NIJR!Ti!v+_anD9#?6}0 zmB(dOxjeT!?bNI2jSd{_4{IsM|363;{LVs`IPM>Wj}rP)p5E8d5%c*=Kl!xg2LQ;0 zTo+&r1zHT)^#XvV2p72tBK!<(cSY~G6XrLJBB~3sM%!<@@K2xwE#VM2zcZS6S@3o68Aa$voU6nz}EdGuuDaoDfRlY zH<42gNd0hpSONo|dJw2kt}$IBrIZoB;|*w;YcBm%-PYd>Nf@od2pfWkzUZYbQN!n4 zB7dWX7LIBJ2+uAw?MN+X`?8D?rY+BI2}x*Rpuo4#CVoMvJ5)()fpQkfAJLS z8+0Q4^e`QKS2cU7Hn-Q{l9eQ|YjwtCSvXqs686=68;j!1{QDOU_7PY@aSm?)gVD(r z(&BdB*jiQwF4U*r^!4oa!Z8VxLv(}cT;^?)I1g)is2Kh>Y+(B}7En~mP4 z{%Bm@N|UiKu-0+E`V;-gAnfE%2xG;J`F%Db&8RWobZ4tM<#=c)&~BuQ$C!8$;YA^6 z6lLw?T;aI$HnGm7!R#`PmQMXubT%IR4@tQFIXe6_^N!C{!gr|?TEZ@56#tYsAtSv+ zYAlkv2XU!c%Za}jK3G{nYY9SO>c4H@+JURfA>qUu-bdewus`~Ec(UopVH3bc%~kH^(m&wR-#hs2PH4m)z8vT&!@}+& zZ!XAkh5Y5wZE^PSpb1T_!b28&GxQpK&Gs=~p_S_>ER@;N&b-%?rV+HqG`-E1d$GO; zI!Gys^GoWG(&oUe;u@~>{;x(VSHD<8ZLP2^aj`k${=3^L62%+|@&OKhq=*x{>uAVe zytZ=_FvtKd1!WxDvv2f(#C{4DVS@_)DnHcMivdl@oDMa;Gth{Tuwia)9$`!hC2Otx`sIa%FBHw1 z9jN?cYZmqeibEhU<2eRSIZpX?ZY+?r=sK4LK+;TB7ai^&0Wm@^0a#z0R#0swao0Hm6A4{^c*B2L| z@_Igf)+F|eyePrzE>h{oA7e^P%mpff`29M&ygWT`vfRutHNocM?1#plzCPwQ(L~5H z8mp=Cdi0O6+Q-$+%je(r?njI(UlJbZQVI(JaBrRfa>KyO3xN7E;=i)|HcPBH#>Nvn zl=VWt+Frf0wNz~!| zX^-Fd+#2w!G|;~829xvDJIROSSwchj&59dY-V0Sqh_x(T%#e#Xg3O^E^`+g86oJX(=y6!!R>j!gT?W1;o*1 zK$k#?Q5Epxf%|K^1+GAGi3MI=qG4Pq;j0@l_4sW=A$a(qY7SV?+uw zo=v^^HK}7LhDJ+EEdn@tW5eMO+8|al(H>SBfdSZOUNl;gtf}x5(+szO{nggJa)6xq zA<>H$ieuv<5fLO=lEJW253CFti;38t6pUJ7geq?SVW_V00>bh1V2h8Hn+2u(CWYKn z8}0c88JV?#2z)IF)YV12yp-AVO+Wjr?h1rP;6deD&!|;*Z*PCjzLANE0>nou>gk!8 znLWamd1s)bqjR4nzOTrB-|osAWGoqTDuj<*{9)A-D;w~iDoeFVj-MM-q^z_^E%5RJ zf^C0Klw|OGrxzzw!M2>1S?-t`-*WMyVQ;?Gek2U0r=oj1UkmXTCC=iD9J? zS|ahZoA9uQnxJj7=LZf61am3QU<8~q&bG#BwwcV~A$8lx`@v^W>p1(<-Y!mX?U(wy z_8OA3SSp8{h`_R<63Dz6>g#*n-SX_>?4sEptVWT!R^K9sJZy*YNr20E+5`DT6{y`XkS;%LcJu;SBel@ixF-L5ah@Wiy*Y&xB5_c?kt60TQgt_7 zzDx0Z>)wx9$yq|TGd~JtM>H-Tq?uH<1-D@|zJK=pl@^*kC$*Cgr3402-YZJ9m3G>b z+B_H4R=l{r*G-b8#@0P?Iq;=9D*T8>PS4@D+T+!gms~%erjD_h=Bi8;3+M_}f;jow|=l z%5rzjkgTK{Y+JOnsq$^i72j-!(?xt72KNP)*^q6bF){Xl+M^+)Q5!1&HA-+dvHJYWC`M;;!kn(FG)e^mKPc6e@h zQE0GD-w*wmOM1f17l-*nN0+&{@afvx1`S=>Ln9+e2;yp#^*=_f3GF9EN6!GD>b*5D z1mPF#+F~9j3y%Oq>+0$Popcl)9?s#GIR%p}4;!2Nl&cSwA%UI{hN<)!UyZmRIYx)q z(}~JfRRjSkB9-k9+CBg^7S9&HxjH!XbHexlNDa@=pL3j-S6O*Dhnpb4ZO_N?@L6Z; z?Tx)Z-=Z|iKHsbMG38sH2c}&xK7uHz$@e@Wt%{&}bbfjT2;n{Jnn(h@-2`^|3pCaZ z&T5-ww!`2DQxKaLPRqmrjS|}~?(Po85Uw>s5`C#r zgToQ(D(2Ah$SBR2+pK&W9$48<*_BpyEi_?`q(*;|&!9;Up?h!l`K$p8m8)kbaMP0z zP54ar&Qo1VkBvzObd3JxNj(RLr^BZAws{BQ``AM61-QH~88i^e@j4j6NBXPlm;C52 zUvWfVi^v}#&9X5vpcVYf8WPd}zI(7QUcddlii^tFvX^lxLF>4g*4<_Oa=sv-HrCBs zjw5Uc2yf%l&TnsQ{u|Mo58fkuywiB=bD0Ln5fl9$yyu7W6~lt?w8N^q=}M0GkN1lD z?aN{#>UbiDp69`Sb6`ME z41=uO*RfHn9-`=%yCk@k6*ZHnG2o38p!>JF>>(3#wi!AhLRcXShY`pZ)xV!4i0Qc_*T$cUb9E&uZqk=iC+N|Vmti!z@J z)*z+lQpVq#M_^~aP%o009z0L#XRj#sR)=4wNZIJ`H)$c9F@L%n+Cr54*`!v%Vv`5x|(kh-We#9du zD>eHiyg&^ZwohzjSOoEZU}RK_3)#)p2Qlc8Q_rjpr`Oa}w;N&%j-l+gGNl9B$KVsk zw6+>o#u_EjYshUpJPHPIe1xfqev{2N+kGX&vF(?Mbw8K}Bn!`)q>^>4Z@uMI^&RZh z&5{l7&a=p2LPr`GV2JSXxxmNn-pPs1>YFb}pwk|cd%cHAV$uE0*-@XlB8r+f2#}iB&&{zm{UPHOr)Je*MsCui`C@z!ZCL&L(ox=v zt*y6~V-#`8UpBndU825X4Xly0MI%tMA8-*vxeLyCbJ`exp8_jf0|*;*1A~?l?6DlT zZ}(n^U7;<&f;9UbEU8u##oQQK?1=1O;H;Izz4|&%egmrzM5qB&sL;`V^TWL`lDO@I zJ!Ra=>`@D)F8HWDK0hIdewni)%8y7@%er;2PXvJ$O@RTCM<*XvAnbBs?PGC%+1>lz zZeBA(b`@d;`{IzKOE>rI&Bl4mk<7QXDiBBT97SC1j&CEzb zOww?sbY$RzuU;?aL9|~`TExlAJKWcIe6-jBim5HuTc%>c7hhyL4gJ0z?Z6Bwk($~c zO0m-vW6PcFESaF6pKNTc^&bd6GwFP(i6E-8`?ob0 zM}Nme(+Df8QW3t1_(b^Afa^foaKoDM1j6s(6sj`}Gl0bNtHn~H;7QjG@UMYVG?I{n z_-5I#6NKQFaqHfIKSIE3Ce3Y8)F`9^5-2}i&_+X=O9hjPNTE9|6dfgvH%XF~2beax zj!w^_3;NXhhyjk9*8J}S!jr&jTvJ^$jgq`r?LPPcjT)c1`TC0A(G|td+=YDuyhG)# z7Xe_K*cTqkkd};osXy@qW>gMWC9TtBp3)Fy^&cK<^dZhCiJ3f{p(BU>7qjU0tPh0} z-UkdKgbUo~DD6pQL}QpG=^uDEIlCf|#%~HA(h7-w=KD$~%MlWhZ2<9$ie`f{A=o0T zKPkohvfFwjNsy#J4sY#<3vMc?3-Lckvmr3_oU(eh!*|6oiC(4@7i8Ql)c4xP&HXEh zn-S7ndGtmj@bVslEzR} ziTi3N8~oGtGUSGND}rx}`S36n8~YyUiOuHcVKrPN$DX^FRRgF>)WPq_h@g8%dtaW1 zViUcU->yn;YI^bZEty)DF>FEA3WtoIZXQ1W?yh@CA~shRfBt;zY+0CHSo7o?8*k26 zpEOjl>=^&S`RUWBD5zqhg%5(cF;3i|y{+?Ozq7Sbt&8;ZGQ>VaA$@1=GG)Dk=1Gu* zZr<{&Z^RDw)J=aZ(V%3A=iWUfm+#fbZRe-3*Ub9jRAHm6?#22|uKSSv`Q5d*I!YRU zqkmpeCcv_G1%QtLuiQfOOD8EbO$>rF_eujnbieu-h9T*V%nCD)ht{oqYpwQ8I7wPDa@)t9{{7rL>rfn+kFr(Y~NK0cDF?~&i%;3Xsc zc$Mg9RMX%2<;~smpD_qf4(PYZe5l17PrS8#fH6xs@=SLt} zCe*px7*1NUfGgJ##g)~{t*k~PQzqkETZN!K;XF{x9=JxSLRBMGDeu%yG~vd+y=06a zI)5PLcJG0CN?F8&muSKpCd@A}a-SL+944Pj87Ky<2y>NtmZ~>K4kc=308Nk%I5kiG zJR8FE{VN2D9+!w5`=1@`H2M!7TzJ(Bak88UF)M`Oj{d#~#ev}dB{H(q)JL&e!^6C# zl$5=^FG8_ZqYs|ujx-yu@BL%I^~c9mQ6U}^6?XU9LuRjDHLSehKcc!gJRsOUWQ4iS z*2(Fqg~i(6zilKtNZ?-#JG-7h(?;8fQ=90uEjtbv0y>Y4w1T!Rlx%D$M7)T2co;A+ zZn7RlWJp*3L#E--24G+yfg6O86edi_|Io^mI;fJG={T1&da@1_+|oOqy1H7+d*X|G z5Ypr6j=_ZvpcvfDq<8Py*@q8_OBlxc)L7XiPgTj;N=%zi#`^StWD60RNonW?RR)lt z_inV%7PPTyXsCQ){I&@JXI6j4r~?jkfEW2`&))nl3r)_>)|xk2)Lbd*V_u1%*H`OI zm(PZf?8p@g0m=JvZ-T$T@1%l*dc6mIjdFt$hBV%Ajwc2At={7fhaU`mGAuYkaI!!{ z{N#;6_Ty}M`9GKhES(DU81`ZWHovfl&7M{7OGgf)t-VW4MW^!#sQt0GiZ5pu-G4(8 zQ;?JZG55lCa9RBP*N*Sw68kB8%&ZC!Xs6ycHMQ8p*5pdz|J2a*f|nh8379(<`;M4u zcko=Bq_?J*+2DH>H>AJhSHsW6`m1Y177P*UJ6Q9LLUElN2KbMgZ>i-CIu7aQA`Hoc0?Vdn z1`Vqga~8AZTmjw7m9{@U<6UJN8=_F$jAfZCFx_ zCHkw=o_#Z8x9ja*Eh5ZyLQl2r3dQd=C z3k&t+=cmB%q`&`CQdRZVty@yZ&#BDO)}~WaW$RofbGj?K(qKGVc}*vdUpD9g&!DNg z#=fgphhf~^KxuveHDd_#u0>`a(aR|wIR9weRCN;6SX}GvwNj4Dl<<8U>Al+-1;u*V zGi&m)jx&`>49TY*Xu|rt8%bQ0d3mn$%6ISWs5TLZnS+^xSzexXqb0U{;nHN1BZ!>p z>d5E;&dJCVIP%;HL1e_NL*aov^;m^gc zX3F=em1-*R1h^MZ`_!X`T32Rm2$MRZp{94Io?1*~Q{A2d{_6#AS_AUgQF8ZK9ExY? z7^@~qlx{w^a*grv;FfN!73O4oO6SLF5Yge=Qg8=e*{l*lUYFS_X)Gq$AP z@3=h4d=(U1@~Bvqql$Pd^VV&n3R`jl?XzLT+RBUerJV{Rman(S^F)H4_%ty8?+5N=ryEw5zvNU{H0A||g(h4Y$J+n48jML-!(c-sw*LizL4 z=zkRCPl;Ma)v{VY%l;#OkD3(q^!c~HM*;_vh=_m*bhK03y_Tf97KJ0gffNuV?8 z3?meFk&JW`C;;fQHek1V-|D>yLA>ih*cnMlH7vYZ0EbkUQ9tAgycg=CLLA2S;qM4O zerQsn3a51?t;l{=MMYF@Zc>Kn(`_Q!z;2w7+RDm_uV2k7tvRZRjSokV`y=b$^1As6 zZdh14c>f(t8GfUA^+|)dgXRPz5%3ksVU5k^AbH?f>vMFvz^o-oXU*0E>NjS9{yZzI zdM3l}9-x?n9IlhpwJs~bE}VA95%svz>LWyyZWt7cDg6u}Myji;)w;|@v{2kgxaMN+ zPiLH&79LgDh)=_z5GCxqpB%$yUViXp9h59O8>j!Q?y>xi%P*dJ{d6i&BskH+ZR)ds zTxuo0TKC@GUIhnk1&{L;C^J5qzqbm%+o?f`NL0QB1=4)Rv4kO%L%J zlAUrZ@YY}DW$uKhBB=9UNuO!{2Exz|Dg}uy>tGE^6ite{nzNwO#F&c!a&uY-~527DAUNOe4 z)1%Az6yhT0o?BpDKursDEk;q>%0xUV12X6YhcLyMZB4?8=Rx;mG#a=ypv@HscE8V=45 zGQ0mQC7;qs$=uYX+4HLh8N-?41BD$NSr}`RWoB_P zuewERe-C!GkxG2*aQ=ydAQ~}&LZV2HCzcMiPgl(uCK~)PF+`60$k!;>=t&GGK@VW5 zckQvU3Bq0s?e!BTkAmk>G_=Nu#)bwUP0}xRzq5Qz3f}ApGSW|1p6FSfO_g0m(5e-F z_D;a+*o0&}TH06bIaMiEZVnu#7|8FGt!QgfLxrD1;@pYug+OXS*5tw~j)HMcETZ(^FR ztr-5hO_S!8R*!?y)J!tI>^B%2227(4w#E|7f6|brKuAD9PDa+Q@ZJ2LwBJ4?FV}l- z(q_$KkzGyem$;elKIG)!sHx&bzubox3-S9VE+8#Aj~_$EHmc_bJ3byCy^P^ z1t|p_zX`RhLOK%I7l>%X{)=l?Qo2=;nVQ;ITN|6rOm_0UdjgihE(+zWCkWaWcQ-c= zXkIL@P8Iif@QC&so!`Y|2M)=VHE=fau2;EftGDSBc4uQeAT6liYM$JzJ@wgABBm=< zyWh?9k~RPh|J7%B;1{Q7KM+Dg zA*TnTaJg*-rz$TL5sd=3YEU;3cnAjR7Ulzt+`K#iuFpHmc7DZY9@;InG%~@dzY1w4 z(pv`|-*^xq)72?XuGjI5 z=oTELY;z;m+^OJ&kj$1Ka{&Q)=0&p~4IZBt=-?#%53Ls!C1E|frve+e<)Z#SJKDfl zMge?oPCsinR`85jE_Q?}MxNkc9uw}s4hK;plPHwF4z=UflqMl<_K4e#PC@z00)7hF zZ%$GwM9rhLCq~2$;f_)Td8N6zP8Rmow$7GTR==T%absgRMg|@>fW=U}qCRoo@-G+n z4G#ly;Ww|Oe0K?t)}eT58A7Wgq$8ZJ;MKpdwNv1mH@UV=&P0Cqj<w0-fU4t>h-5Jk8UyV-Gi7H!36Bdn-`)Vqu{u8-|I^#xCT=qS!@(piT5V zy%VFFT%?o2R6lK@|L8vYx3Igp#{{b*Jwci;oxpTtOiTBh%*Fa3p#y`BHIf)jIQ(m3 zN=Dg}SFC^e%>6gnWaO7WkI`ch1+4Tkr=#0j78W%}CNm4^gQq83Ubg6v{v-d473^NZ zwpx@PxJOUX*A%j@MM!7mJD}d2#k5N;(SV6nymcWhSd6f$gcxmqzSSeivl0sjqJ|VK z$?(GN>lq9B+8!d50ZdpL?>GIrrzrk*6-h$=vV^$jqkfn+GXlF480m=CZ;rHz4cCS4E%=^AS?aK@7 zJMm~`B3%^Q+d&bxcukv+338%@JP0a_HY}8sPD}rV4i65-L`NIyDFCqIVeeq;Y-0nc*UzO5;?0BY?HB$2H8`m42JlJ? zji1wyjbun}fz)2=EM7Cn?4tSezpK>NMfhvB8-~xv#!uiTz{zup%Ki`;_c1q5M8JP) z`2ombs41k;wNX0J<`p^R<=6ZPXILP}#smf#Rn@+>bDZ=)e^M5;$h`Dgz}^|WF>C)* z6hQ=}Z?d$RaUKRowfCtC|3)kf} zBB$1o@WHFn?IePqgH|?(H+ zs$<;jZez!fj|L7)!Vu@|mXxV#%h2#bnhV!dqvT{r94-~|kqNkRdi>U=mvy$Yd0j^0 z@Mwd!0$eyBKf22MH;@m$oD1Hpyi@nZ5c?`(*hc-GtEk%%rQB5~?QDypv1w?@U0Moh z7Lg>pu7wN>RzCB_M2-6oZgFT+OY(9htj`OdRBdx^HNBo&Nc5kAkGO z8b|TA*sq=6IzNn?Dlm|AvUydwSwx59y|oR=6+RA;4jSjlACOTad;Tl$N(WKQnq8yI zMu@xcC)Pi3>F_sf-IrQRsV)AsxuB7c((&7*bhNb+XiXn)ZZ43 zm{XtGej@#Ir?8Ep)%sw#_!9mRi~S*w>$CId$tjG&oUw@{t*nv9 zXBKF}vur_JyvB|AD;zvTXvoh-m?5?h!d(QBKu|Sm8TF^^BcO}mc(Sc*GPruOnH7-j z?d{;NQd1wm1rhi@=|?nsOI=-GUo4OTgYOH@AIWvOQ^0v4r;FGr8tt{1d}y{7YU~>Q zXa@_+qOnRSWUuovfl$~VM;TO-;5&wt`RDiVPt?{1U-bE7Yi^y+s%4E%(fwGjx?ZhM z$;r%K2K^ont|E!PeiP!qBT!RbUR_h=qp&SRc0!XoLjBK<^#WVuq$;C1v?}^{7W^z8 zQl9J_eIq?Man&kgFYA6!mTYRs5k@q+gBPxtu0@P&yxH?x#OqwKEZRdQ06Zk{kn)g8 z@JP%>rV80nAU-v{Ia5^^HiP3XgNfqNXFp%BsIb081`&QKw6tA3=J6?`3_gMUVZ&L{ zI9%vLE}loTlRc~OkTYcJL;&t|emF!&jrdr*#?qA0KxklY9=v$w%jMxgM!^LW!}Bgl zY#B7vNTv+K(l4U%m-KfC11T`QBAy)FIv;>S1?1b=*RQnj@ipuHk`Sm2s(*+^EE-gj zKk*77tEE7~myiI(w3V|L-c2Oknwm~}54?Z9%Mc%P#evPDi`0sPC`MXd5bDo*~Pb2 z7NNQxBvNg=#xo-jlIPsqo7%X-`TLGMn@Oj!i*u>}K5PDvNOHEk9gB$d9lJ$(D5^&C zSbexU&uTUX;};$KYu5yr3(+P2MNGQPWZY=lpi|2qvLZcViN;1|%l@WI9q6a~Ci9Nx zVFYkMzDN+NqTXy+<+~dYjauXWn+-%Z6eJtX(CN)nNgN|BAM$qtxGYmr9IE?1-#KjL zl`WR@#X($_?gDoeHIK0F?I9&4SF`Q1hxYHjnP;DDEyvunf=uOKKi_F(B;72;mRR4Ra4y1OzOky0AD=A=Q!e9Rq4?L4n~AT~sKMp7 zEZ}?!@SiOyf|e_}%ec-Nq#$3&zMHE;B5tOD6+HfwwLVuo*~@D;rTxyPI=)qz2fKk} zb3PCgP-NWbSN*;H(62}f9RE#$c+*+wCSb280~Xio&PY z;&1+Fvi+mlmI1eYqSVX^v5I9#nGASJHU^HQVk#)R^j6V?S)c7Jb#?A6rzi#8&|S~3 zi(G9GvTKM|AI?Ht#V!r52gB^KV~|rbJ@M6Hulx1)@873FeL2^%I>W-Y-{?>Z&&nzL z*qBK9{2k#r!@qhVvLC$D$2kjOZt+R@YoX`)dcHMS)@}CL8a_M=fC3uu6UxX@z>M*e zU3-BAZTw~IOYYmZ?X$IJ4EfBp;-bQ}CrTc5DLvdCF0QE9X+Pe(9l{8>(AC0JtcRcl z*B@i+9bg&AlUR)JHhQkS$Sa`_dkJSG*G6-lX1qt6O5)V^LyR^=PtOyOSqODPB%S}Z zn0XI9`A=p;-Om@t#S>1y0NSncATSMBC%48U?2Xs`G}~Aeu3CVm593Q?XPR#++y22w z5{rT3MHR;-S9ss85{Dl@3UEZbA3|cJrjug->038Fnn?U?Bc{Hug;l@*&bpjD=qy2h zCcpBUqwvSyY=Rfv~Jak_l@kPfK4w?+*%cA2Tn z&C!0b$&T3xK7qTfxQ}eus7bCnq&ZU8rujp4#%jP&u%cXvxXd{T&h%gYE z3@(t&6(myol)+IyR6wudiL##Uo#i$1p&<&oS^+|fcC-*@$1rA7fAI>`P!?7g)&MD! z39V$^-}#Oen5qSO3SVS~Npbo7k-x{mo3%AzoO9dp?%WWoy`;2JUZSBK2Rv>bmTzls z4eNB*Qb0J^2z@9@N{p$_zMo1hb*UDg}_R4Jt&deI{x1clZ0?dv7jYr zWJ$3%w-!}^7pG7@RE5$ zpWVH_aY0Xg9X$%LPHCwodWH-hZf=NjuCDgd*Y5@C{?ZcJdDpEHC#QdZXAB=c>}@Z5 zt;rlOx-_jkZ?F9y4l%%TWE2E`79eEbJXBUDak@d~+wpyz;q~d@8<`I+Bc*?}hD&2( zwe<44S=nVj^=xX)`O3R=YGk$(x)>#=GD`YY7rok9;ZHAWXgD72h_q~D9(?6dU8P>{ zSEI<^uK=rh;T`}J5(Y4aW0)2JEU_9&8z3Ugx_|h|*d#{#0^XuAC#Cn?k4iq3J};wi z0Iqw}$x+m4HaR7J^!OkwyMJJy%&bK)z`h=EPjj>}3r_hLFs-|p;=qrE%Za)~N$`jq z0I%f!dQ#i~k`^ukul$op(e@WFE&~q<({eNe(2-kn4Nsk%(P|ik+*un%*i}mV>Q$TRGK4%1>=MQkyWqYW;5ECoV=2JU8*J0xlhdY2ez!(znDH#$(jbRO9EM zf`e@er^evAI5ApT%vfTEu^8+15wq_&DE^PFM0i%OGh2;a@TmQNsT)=hzxsN_OCiqp z=jV507CWSS*Sn0NTb~51Zxazwq+JdgjM`06{D=q}lRbYQR6jAV!5LE6x%)}(_o4y@ z!5^s&XSUJ2hw#P#jQ3sR4%>Oq4Sb~ffLh*gdnB>)>;K^^b>y>VG4$g1Ra9^ZY=UVx zI5{D2x$aa{aaELA7O&_N&+XesyT8;Sh$WJQX`$~^b#>yw1311+$zk?MDX4M5->kQwoo7LZq$F)d*-P}9@W zlTp%AYJpf;oSWs9LwrYsas&}WM7=ByX13(DcDFlsYP-AHrX?hb95x_A0~B+hp`)TC z9Uq%8YqeA~Gh>`A$EJwc_~qtiEh2&@-zM`yfh8x0a2=ZFV$#!_UbI`ht*pEP2xFb; z0`pE}>PGFfIRo?1)*hV9*8W2#R1N&>)3dPfShbwaw!kv3auBKFC*XXg1_V45H?H*g zf$O`)VqVJD=K1TGSANI4w+{LLW5k|a35H^h;K5OBN{$G*p?m5*yCwh=+*9YrT-vFy z%5;RZ27`5D;aW2K(W~r;6Zx`NaghYqT=+!G;~KzEOX%@5coqn&16c~ zbKskVykzCfU08+Re8@QOTV|i4FV3L4zJE0OLC##3Z1-fy5kz&#Jb`BjB&d9$ggF00 zcA@-(3=u1AC;yP2?0v3y&l|ChPMV)xG@Z-OG1vD(vqR!tuaP)&(#Wf&XhZ717`)4E z)%eSDdZU^@ErQkC*SCGfNA-AWyt=Y=d)b?BRfgmI>Mf#Fj&&Mb`O~blSC<34#O~PZ z$}Wm)auderC1MWrB{t*Ak3wCn|HX&(ul5&H5i#jqp77iuNQXm>r6r@&=Scdb=~sVI zWlkSe*-w=Of&a(eSGZNZZry?bDhkpmNC-*^(hVXYoze}`A>AbcA}xX-4bm;$A|cX9 zcSv`4+_~KQeD{9$FSz%4zWto%ob7RmYyH;y&KPsdF#_h_(>;6>`HTFuq{_lws#!FB z3y;@#v^<_zP?jUc9vifJ~EO@L&<(F?hz=Zmli)iRqEOx>{Tk-0b zBPL_{H(9-gFWnjXEh2=Do1J|v=$tmJO^HPg!XnY?-g5KiGu@aVXG;d-FnJGS66UO4wN&2 zSO}hFpjXLlJKqNP*;#uK)*4_j8g#>82N$@ZU|%OICx=*@06G^I9v)}~CPOkZF|kP<_~qyJtWaxXh8YZ-RbIXS_U#l7VNF3e2)jB{itXk3L_5Nh%6C$_S( zvd?1ap-ZNs8VI627JmNxT84jN;T9M-!pc$mW#lZx8yNQ%hs-9xPR8cM$tBMa#D=)o zSUv2BmvSyyF4u!VR{)>&aJ0mDZ7AQj81+O@<-4x8m5&dy*GqA6P<9P@z;n3Vmlhiv ztC@u9{Nh~`1_>vV{jwY^bQ8FRdxfz#?&I9QAO7jnKz~0-TZ7^yDBIt0>LMRVZ|~_z z;xzqO8f*bO4?xq!!9mH*$^y+b5L3`lN3d$Es;k_bT@?vg=TJkQnGE2%E*HH|0kH#O zZhBUF5(lrtUDCPL#o3t|Lp{ChLhIy=jDoDMR1B2Y|Hw(@k*U41T+ADNs$Dx{#gz(6 z?dC3;@gKgZ7xH^tE>CJChb6x^(Lvddh56M3k0EAO)@PZm9!6kJikMrm9{Yn_u{Kf;xVxui zYutpkUyyoPckT#Q$UQv*%CD0l>BnnQ;9>?=OW9kBTj zdi11;;5fWBk%l^KpG|x_#mb~-c69S%HKLh7Hn#ssCOWS{Uv?f%GY~FW5F`g z(hoR{z|*eJ%5Amzc4!8b>^_JN^(H^WI=K1k7xsH%dh*R?Q0&1?ug+lHF|ye|cK7I* zX#B#Z&|Ys=1ZgS)bgn_PW)ySdbi4c zj`g@`aX{^BUgMV&QT9jJnS%n(&D`rKb*omBKGhy*B%#Hr;aMM~dDLU3JLHLo+85~= z?}S=7DVf;YU8)!%RfC}DM{$<=n<6toO&UAzZ-W)nH?rH+JAambk5t8+GL@z#zyBse z_hq-};2qwpjWHc8W<0N3b-1WvLjvy4vkMFPZNG84KHtguwP0hOLX5fJ`E^!R{_!A9 z4^CLT>VUk;Kw`s$rrIl~HLIPG>|<+ry11HB=9&wvu?bDL1@)bGE5}oh?-2XEczfBp zf5g}Y%14qa9_C)UzbXmX>kgg_4!%0O^tW0mGJW~_ErkTNOkvH{`_?yvIaQ^lwr2&Y zdTBdcOl@y8r7r3MrkB9#=4hVqu98&HeP2vKG!+odWU%w?OcH2dIS!^|(5|iY0vBtEA{wGFY#bb*#emtnfv70TSx@}k!I6<;*yaEb z=<$;$eOYAXGwgj573JkqW#;&qp#kaX>98EpV$@!4@?tkk;4=Kkr^^ErKEFFJ!$3xVs$yW4Vh1nTq> zLW92C*8qmnJ%@$cO9)K~Bv(d}$ zUaV#2WU!mFTQBc@sN}>2(*=It1DyNIlvFoFblsCQR#?OdKlry3z?3^qqnF4pv*(t# zRa%cOw9$+OR*0U#f!PD4aXx`n?;4tzBwW`g*ff*%4nZ|K=L16nywWn-wQnM#xD-#N zQf=d*zm{?Rhy`;f(rYGi+E!Ek4r>fxrd`#C;7A{|N0C?tHuk`bC?N{BLEn#@?F%wm z|4{F(d+7ds2bULT-L>lhjY)BFZeZX0B$7Ji1%VD;7i(KiTB;Zo4UL^W=?+k@?$LO_ zY)P?>!P5Zv@` z5fhOY|A&sK<#Ds|*3by-u@EjV{~)}JeoM;Z3ami^ttF3&i=9A1@K}SRSzyCwDi9kwQ6*TZD$wb_Py;~*kk=O{-0>5+|eEhBZ=X&p=VV+p#u%>({Y~+L`XmG>fJ{}(W?b|N9^S3jD zuUm~MC@LCv#*o8ymex4zVt=Mr`xw!;`I;Imy^`GyR$DN=VUn!|f;)E^9N#n5JuD6n z4-X3JNSFLP@&vtgBj;5vNMUi&-o$mqgR-3l%oevAu57uHZaMhPWM58QoXs}*-}@Mi ziHEr{t)r=~?*BqzXkg$O6YDdu{VXoF1KoCqJ(C09!-FGWGVWpQW8As}+_O9AeqM;t zSHm_JmU~mnS|kKC-`TWZYMw|!>{w$Gg6nz!`svC&B}gEtM(;UHj#5@d6qJJYuc`Q zPXpu>?p^|*%Ai)+Hre9WGX8L&jY=UoNkFrgrz)dto6K9ZNkTPFBRmU}YIC|KOuFyj z^mtZy{Zn=?xvTnE$7lN2oIzfkfR$L(M#sEI@g~J%$H}1Cpd*lBik7+;GDElAl=!J+=tc?4{10}0EwY#vQrHvw)S<{W~7lMWP%+?mZc16lG7O8ZOT z9WXjJmdI(7`t|F(E>vsnKoz7f8GyD2`1{v8?_ATslElTwFVv_AeL-c!iD*1 zb9Of3)2C+oJkmO6v}Adq3t01H?0z)KgR~0|JCDr%Pcko6b#;(Y_a{U? zSwUjuofq^!JUrxcJ8WM3Q4Wfu=yv!Mo<>@LoAnLbZ27a30&+v$MiswdvsCo|IHqUcRL|VQB!UmWEY?ZQZB!7ZqnE zIG<%{?&)xQ9~T!;R|kb%RcI*Vx1Ho*sjJI53Xij#teo!oh%FW@m+N9l@~~Hh_c=WL zFMPk`}^GS>Mx5Fug?1iWJgoJa7?91QB=A?sT6}3;&3)5N=(IRAOp+MyQw; ztD?NpGY)n_KHGV~N31m)K&?<)O)b#tbN|GIrlJ;%(_OAM>-cIMr4yO@hJJsL(yv{6 zhyYD4YCWtbD}P;n>y&~;6(E_Nul)IW&L}5tV-w)f0<_WRd?im41(G>d2gRKS|f3Da5$a( zPS2rw-R$Nx#ap@7nz8KKSj204T*N;Zz(LPB|0s=kkqkWD)vdb`bAyAU$Ufl4e2DCZ}Xg-MaUvYD3Q{>0QM1H*=H%MF0BR+mqFEhh!`gY5qgjR#lKBhD{ z&qRkA&~H7tCv>?cCT1&Z#4!qlrN9~r3L<`P%~w}f8{m9)oa&!yeB9eL*$-@>_IA3k!w%YC*Jb4x_2Qp?soq&f!*>Hv55m9VcQ*NRdQg(la|`_z6Eo8~ld!_L zk1ZeGCfR_Or9vL*FrRuQBp;7@D_xSi6oauVd|y|$`12B(F%axydo zu&-C=;9yHlz81_Q#UXXA(mkHnlf>bzqn#ta*QRDbVIKF>$8#`NEaDH(5}NfIIP<;w z2r(Weiv0NhWL>L4kSc}_wfM!702QmUub*E_dla3bg2D{>YCy8Fk`{8Z5JWe(7cZLK z*q@iwIq&=!9{wD#zU4JQ8vW@LsMR@`KFM9k4nmws+*`M9IRkuC-gXE=eSfm#)11-$ z&JH(t3|fP0YaPkYH9TpN?uFIE77;!+_Mh7xq6g#F4KS_&S!E1t;gZzUYp$YT8Q~^K zV`yMtceJT>s&MN<@YO378Y*g(fOqZfzYg#}6R^H?1)@LB%x{9yx;o#;NVE?rlYF&f z=YDmd`!!>SDrO8D!v+S54i5L!)VN?16zJQuhJ%DG#yju$f42NAN$$xy*z8Abj{*rX z*fV|=F^SXAZ!Cr&J~}zcVyMsGv(_Q<&ge5{1sNrlt%<7sgnhzSYHFiXlQjVSs@`Z% z=D-zsbnjbEalKDT-F4R6W7n^K#KKt{P0mi1BV%zQwuH$rDmDxo+Rv=zWne@8DSjZDhs>JY!3{R1V1ZU#rpIXF z@OxuCO)Pks2@kcdk;JFKn{ak@eqErMgNliX^)V;wQ^9*n^aD0P1cF2D56!r2KcOq9wtDap*i5l)4<5YuxS4(eoS0Wb}PBh znUh67GeVr|fpk|Dn991l9X;X$E9qD!Bi$7LS39%5s1Le-c4xr|2$WU&4Kjtogi@Hs z$OK&9Nxyh64D>7*RJ$|&Lwyaol{gP{#cG_$i~ArpAqiHlZEYryh2c2(@<=)}zI9_u zbLP1Kt9N&?_YYF;a#OJ?$!KfCmVl@zUSp*pNG~7+wXpvRYH0v#-whR2jOvqLP*5Bk zuk+hR1_s7@dZs3Zc^Mgubabecig`K$z*mye?p^Bal){VPSkC|KjWpm6=Q682I+|S~ zDc{lp^T<-gsY3Zw>R!!kh=W(g3BPO-^WNs9Ti)k|0!0uThS7Cja;o$3AZ|u(HX8%= zAqdY%cYf9nZMH_L4zhS*I1q-|36T6=U+0EUQ03~Eg7Ut#)D#kdfd}>=x%Vc89c;9D z9g>u@)8C|Ag82sUKO8G*^zaGq&V+ye-k&M6GnD%YcBO!I2`fxUY(A$z_fL<^buzv& zX?rooM#JN_>>{gD@h(?|neqhn+A@fJY-m&vt`TQFraf_Wo33_=oTbk#EcAMF<@r+X z{(fet|Kf;z8X3QHuZU-FILH%N!(#kBN$!C=i^Dr~H1P+c7# z#w6*LSh@`CCWE64i*a{wSeVYFmU@NNWatAP_>be>Vi%~QSElD?XPa7B%%g5fVY47z zK#=$ar^noUjR}zEZ&_I;0XttFJb17*ZCu?bDI?SUSmxZz4b(&`taToL?t1_I%@maT z7^J*y!PiQVdf^j^ii*|;-?;v(u~A4wB)#cN7zr0vD@(aj&J40Oi0Jl`tTRj9koNZ% zc0JU)0M98{Dwhlss<0TPfIhcXQ|^ZSGj__H8BJj~a4q{B8QJepJ#A`>nmoG)R{;t% zeGCJt{hv3OShm*?aVt)9^j6$+62?%F@Wu%~;@%7S0zK(rlK8;5Us7L&lb>!pqtEC@sWwM33hj&N)PB4gGT zEu>g;Bg0@=FKj(UvFV65W~ypSJq-jJX4JPj&jYiTZwr$wLd#upvR-qQJ0S07z*5P6 zHb;hZZ_;h&$4w1Mx-9WGf`9Gao>Z&AB5)#?C3n0DG}@I5bUC?Wb)}c&VC!b$tTftS zofPCtiDPM{Uv1fehbMe(GZ|Rne-<`w_Dxb-oC=JP53<}u_C>6QzPb4FS z4V-GP99n~8C#9e*xsUVij_@P-)AjlJxev$ul*qzk{YzUmN%BX=$4+CVWbm5C^^WfD zsf%I8tXP<}V*GH{hdI(AJNr(@=LSh}y~ArCc_0sEFaoJWhW~4NR~(BNERH4#biqET zqs6%YNj^qhju$QT8ut|LI&q#IZKB=2GxwMTbr`ayqs<#lLVOJ4S~^XMxMLK-rKN?R zh3|#S$p(5=S-pgw$Udm6x_`GDsCtctJ_lyNj~*`6?)MtMfCJqARHuG@q*@2)q6{46HXg#F~i&$+&Ex5_1!JkbI3&EY$El&z0E0J3g;V-ov8k zXqbjnS66?_VsTGTNht=kK+yX)vJA-NjG;#y(LJ!Gb%FZrY&Z4}wWNgwJv)9*vhd;h zs4t&)I_&aqZ?jlUa2j2l2;@-0Jz)>-1Z^5U*9<>b(U~TQ*D(DAI>?XvsRGJCavjK4 zB%uBn6LagyN@U*+McvhbY-l8o2e0${1mwc*%BN4C;2kuoB}aj+@SP5HYCiHyc?aox zL>THa+MeH$#7zka2Qbt{{PHpiK$Q@*Jk#J*W?I88L}7Y*enCn~ULFM{C6A!3uGj6` z-*Siw-ILU|xZ8Sf+_;Gw^%80|5S5aVVYHmh=}YdJ?z5{2L7n8|3)ezDdCT9gq@d}( zGS>nW+&BV)U0#xaf(&J6lM#SnzLk9AZW1Jf6(sZr08cAi)+HH*-ym2+V!6S2XRb3sXshuC35RRvFKil+6WUSXPJAzm#CkU~ z0E6_#>4|WY7A(yq=$XeY7vO11Q2Ypp$SPET`R2G3s$~tfps)%KKJIj_josx$mGZ!S zaR62nsij?kC(o0XTKaV!NJP(l-wwWNgZr~codMA_@nk7!f8V}f`l`{V$XloSjoxP7 zsH5_b+@m%UM$@yS4Z33?;bnm**8~yU<2^7#W5flK)-FJ=_lkAg_Lpaum!07(7Hlsb zhXumwA2zQR1SthsS&X=wuh`kyouAHtvJt4^GY1`R3z*#q21B~cuV0G}Q@nW$I9nj) z!Y0Z0UQyQ+Rzff0NpZNO`QITbA*NkF72%lKtPVWsB!|ua-X;3-+BGPa04+p_sPFAK zWZbTQOKkrVB~a}JL?GSoAZv`<0 zOz}iRRlUD&X{fTNw1{}W2Av{>i?Ag z(K$Usg zMzeCbvBJ!}*h(#D%=Q{rjK{5QetX(WK-yCs$a_Z0OzSDKx|UoZ3RcTyH=(6TPM7sD zC6Rx>6;=>q>dYvb6aKHf&FZ+W3Q36m&YT<^8(0N-EuDQU1t#iNDgHijOnTRZlcgvk z5LWe4r;3>}#kWc+9zW(ZAIbwiU%cZe?e_*9O#xO`R-FX)kmGbUXOlR%F_ML#9(?2I z*h(rJ6%o+~7%$Tb#2iaw;{fTg41vpIgOMUVgh19_--C~hiW1O)WGaDC{*rR z4)og~J#`;)UuOz&=$+fQ^%{&H56Jn0Tq+hUZTp!`jy62IR z*|Vv18vn^YU>YGh&-pq%{b*|n_OB+lQ)an0`mSt+-Vaf4ZIYmX~z|i;Z$1_5do1qI!H+!>m%fuz6`Oc4~0d$#&*DB?C z@JTr8tA(W%Gp%bgC}<_}x?JP1wwN-pVIcDM|E*Vc3LXw7GAiR`%`YmElM6**RxuzR zsfQCw9JfAE?AukbtHrP!|8=|q)Vfpbbi;LIYejh;up(K)exGyW50pwwr5+eKkJJ~wG{~dUJrTKPEG_z8;6XbOk zJ=AP)ytgR=s)T)Qrc4~>Ik|nurYlMPfJwQ4h3QMY1`70yk}|)WvF|j|VKS^|IbEvP z1ibe5Ke-bDszboXcK=WXOkSjiVP1Q z0YW7odRAUuJhuaF$%At!;Eo_?d!FtGrUg(`%FDcbSyWouY&1*_(To>$>9)7${&GZ6 z5Za-04rp|cmQOFBoa%uhBSgrNuSy5YeN8<*AgyamGI=B{1`_LTZf-2X6#yB72bFH7 zfET?tzs=l@Ch~m&vmQa?AN|iV`4p9uO2MlZB4%cKI`g^@2!F7k=`%ilOn{Bu#9t-M zR~U%;HA#fc@~*nJ-ZpG7$54Z866_SifCUE^cNPN};V_nKX?fIqdY~xe_#pll_Gzc- zgmLigzz?gdD_{xO-*5bsdf#P?)pOsY^;S!-Y!cPhc!@DI4xmkimtnL@Yl!z9lMSK( z3#EaM^;XOiMOa|*JnBw(UVgD&Ib;U_=!y$iG~=x%)-kbz0PoJtJ}F5pN~7IR93Xi& zbdvNyFb&wzS067I=KRpil?#a2^uLz)rUAfzR)h0H>G#82^*D{Y_iiIJXd;>dps~8w zajnUmqDKTHS+ARDd6{T+#i1;Olz9#$K1#X2JDjwqcQCz6rh{}BnZ%otJAcf)aRXp?Y%vN#h>k= zaQn+_7bN@(Lf;X5P(eCf9n1wdbVT#=NB~e_DSq%N)?0jb_gh(6ox>U)K1P9DGgved zVD((dqoS1Jl(3P=A|dQ(V*JQrSxvEwGCzUlUr`a0(N|RsjWejLG?8#PZRG2;k-sqhBouyIzNo_m>U<2;?6$|-4?#99tfbC2Eg_-A zmy#8OKahmoeQY2m%4P?&)EaCqT49OP%-p;tAj9K)8@c@f%!;fjDbflwt2;G%+6zxk z;QLbhW+KlqKHG3*RB3aq?qMUG8x?vj|B8ESLwwSn?M*BnBO~qB+vMGa_RB3hqCY>? z){S;0Pyz&hmUv_k{;wgJp!E%Z-FutS$k!P+Y~7u}Rr9)kag@ba-~MbtONZO>=oPg4#Y%Qr(TDf*XuLauKqY*+z#9@9tblr-B8xZe>JG{ znMkWa;Z^L5xAz0(+zKD280Gz@l?aRx$|4dnh#q@Rd?R8kRXlt??&{Z(#*tI}RvX73 z_ObfJu(M^#&5L+DO=)LyT#x!?5{J{M#dgz&i(OjpscMJZ7UZ_j&`=hm&hWEhcIDIC2I?0r_&W2P%-~fj z1vp|ppFoo5=}H7;*ts9m5%NAviBI=e3~X2{N=o+N@J2PFKemhJzsuh^BJmF~5Eyo%W^(113|fZl+YED+H#jfKhiGxV(nHW=>dm zazXg!9h}E11@X|;nMzodF<+j-nW5yc?Cwm?{qk(Ny}SEW^X`_H3av_k`>5@L*J#aUm0c+8!e6yUH z-XqxHHB$an2@GaG{u8l<<(@Ela(G7)R5Yg}p zl2$ZOp6_-%or?}^ierD>-bX1WO;@N{wY!npvX~i7(=p?7T@0O!^qJ?6_}CI0Nwop{RGh7|h7m zC|%z+b(?0?5<@3uB%SPvV-Ga{SkchwD{`Y3iPY{!Z#9K?wtYv4Ej^%}=$xvQt)Sz1l77g+yrU8UwqL4J7Jg1sF2>NBggM6Fo6B4R`i<&cK7?_K z81E^?=ZPD=bH;1%tb*){IemPc{p)ILK|_R&19OQHz0mz+*Q_jpvzE<#X#RLhBy|=% z+RX=Z(htp*^3}_3qTyv(cFr3h)j`sRIG>#^+QYloS?jX5%RbcizG43O_&BTP(|cK5 zRMdbwxU{uLQ*Lui&CR6OIUHmM|yI{aR`H=vT})wfS5SpNR% zEQrjrSyt_c3`?LYkd5oohN361r-yjhTJsD$FllpM2kAYU?b*;q=K+T`pMP;ir5dPZ zVg}F@d}O~FP_BTXF9vYaT>V%6mxE_q%zR1EJ%w^TC~@jZYzra&>~%Ex<0e{tqQF(I z%U;8YYon87w3TWD|LIRM*#z$N3C`s%eIyU_w#UK>&j7;c_)|nxs2bV3Eq%=-0Yt6X zy%|*sCg8yzT&k^FQx{%aek2(^kguV#u@Fd;=)U_)BnZzmRtwBx0s;bBxE?pR`&>uW z?wH)dfRZY&Zm#gT-yOu4%iG)}io{S57!)M={!wFNBQQAD;O@hWA_(V+z9*x8mxguS zYyOD$$2JYn4uET+@~Skq!zvCjG2O87F!ZbfC-c~4I%%mQL7}94+#NNsI>@D!Pv-PW z4xfEj&R2)&pWrIz)4+xxI0-dUvCmeu$sTQ*rabpUNY3v~fV>}^BET1^tZCdt_}e>^ zIV@La=F}HfGCR(5*BgQ*yVPS1lJ=-PNO`O@H~Wo?acpQ$?FY>Yua>%wrmnVqG5DX| z><#()fMCC>^A(hM`z<(Fv2S!>IaHeT;gy3_eVBL*uE{ur2*FW9ISR5;YF zYX%{y)7fAHoU+SVvsfrD58z1WLYAj zb4)NH{M_Mnoq&vNy4XN0C@kF9x9Qz&?0NcMqDYoRN2?|x5fmcL%6rRdkCn3I0Kx~{ zT%Z3vB_ksoL>aT|e;*Yg3-xCo@M5R{)2^rJQX7+;K_am2`YnE%vfmqL7Nwve;>TqG<<~X%Y4bX&_=jZna6lEO%>Y$}< z@3zL{Etev+M^zaXmHx87B`{+nnS^$+gYx4vU%f69j(~c@(`avR?eU68{_9BId6SWV3__XU?BtWKR!r&}=$D#$CR_ z(<7+iMX@w1rZLrKeNxgi%&1R(e)TdH7Z(SIU7c$C?8+#YkNiuQ`M$dpsJ#-W6S*o6 z;g};8si==#Tn}K3&N58g1w+yz-cZRt8tX8UB+s+ZQ&WEuazu!Sq0AN-l;OxZ}Xk8EDI#Ye_BIAh}aa?f~Q=T9=PP$hf2qD_KgKy0<(G_ zExE-`SH+8> zoJdaa?^GFIUc@oLoIyxP$R&uzZtbd3&$T9 zyyRkA0OLD7m-Fb3Zimfo4U^uzY0;~+zY!C5`fNh z+}woOD83~cF6~!#&*l7zsj)Ki&$TseZ7n7O! z&Lh;)U-C4o6kJA68+}oyy{vq9aKP@AAhM+MaSb+x&&^SQ(D9FEfX6$-?wkS zYCC+2j;0PZoc-Zf*l_6zp<+O|*!Ni5WTYsi@*vpvlBPo{Y0q`tF6FAuXFFtQtFAD; zCqIP6+G4ysWS&nwHglHqtaoUYR4KQnHwf`m0V zHn~2K-4FR;fIY0e5NWKHq<2nyZMwmWkw&;EsZkff7ETX+@7nw9sxRGAsBJ)f{{k{ zpeVo!XP{+gck*-^^o79Rik=^mAubVzRXoY-v0KRr@$nFXu6!QlYgBmt3E=>ET`@z7 zkQlReWB|ee)Y#(Vlkh>g9oMsl2*Hj@imRNY66UQil3?)Pn$rdQVV8|Dwz96YT@cru zJ_aUone+Ud@`oTtm`7wAOa=xV!f}~%P2_Q6q^Ixv`7;x09;oeC8`b1fU);7K#Ky)R z^xSbWmPj%mEZ9)Wi%C=L`w<$-jv^E!?I#_??8@$8N$Jaevv;f9WU%_+4A=m8K>~=& z0RMaoMEZinO}3092wzZcL`1}*;!7}Tg$S=s@``p7ls$y4Cn{4|e?>1ZFGDiR)gp#3 z#tI7U-Sb-ZYfyB1y223HDX-Gw>=3chk;n@jMkgF~PEO9OjS!gW!E^f zUxOYGAB@45An1)1FMQ8d%!H^nP!RF~kAd54;PGpT`*slRGcz&}6@bO)H?!=%IO}>s zT--+v=S$4@-oyNF6T}^1>`j~4BN{@a7Pq5GxoM=`MdRCWpTUDsx%Qpu6ZO1G#Xfxz zK(3!fzW(EEY(QlhoR~n8@PQkTf%Xo`mFkBN?E!?PM8a{@-k#Z)9q?pEQ(v3!=m-dS z{WeBC7ePl%L2SV#UtWx4G@QrVW9Ee-FJ3^V&EjE^!6vtgEW7Op8`gDBTiRtIlfMDm z=}ENQU~q?Pp;2NKqG?U(U7%jJ3budq`gZzGb0ipK{Ofab`ZoIol{gz?WjwC?S=rk0 zXJ5r5^IG!m5wlMe={2RfwIyzEzp)85YY>&?I~ag+b^yf)aV+36X;oNhl?}YKe+CTQ zbB_|ZBrk!UAtCW0EG#TIcz=7l=t4`e%J#KQXXlNJ9!z|Ej-lhB#?+P%pC8MAvyXlD z`tqRg*%d9_r3R?n73Zi1Vu?VMSFUvq{FpBB3CmmpkHdc;RWtn5)VIOheJkpNd$(j{=x?{!|mCml_HvICau-?5}E=TsNzhK!?0+?p3aG z-TrS$vxtbav_EhJw|Kc!V-3+qoGgaK7z3)+)u1(hKr%$Jj*1PK&9NY$I)cR(*_G0`YY^KJ0QMfo-`nRpZ@s7!!tuPghECJ z@rX3aHPgZVP01z4Y|v~l(M8mqJUTP6V0B${VLFO7@M#O#i^fP z#AcTD^E4b-|BOqKjXH0Sle$Nela0-X!0MZRZeA(J-qd)Bwha1ol`TINZPVT?`9CQ4 zh}o>pSIxV$t;hDuQ0@`c{Z39%)wxxR?AT@Glsu8e%VQsO-50Sr+kir#j^xo(_d5Cd+8JyK385w0PqobgBsW9pLq5f7aO*91Nnoq1bJ9CjvVee}$ zh9R_;QL}P8hS{KWejhdp=E5FHe3DxWeo-6{AQp|^I|1k~cwjPgGcxd=%t!WTNS7Gj z339)i+>(C+bmn~NBnp0PB36?k(6wBzPkRPZH(g)Y0N{ps?J9fP$OvcneUqNGK(_3kiu|Jyf-k?vvr9GPNYQ1D|t>Ijv#OTU&s#c^! zIe*&is_Dve;xN*2W8C9ZEsj0OeS206i1V{2KP?YbL~$h;WJ6huh$L_XF}pe%CPhCwjGK zOibH9{LmDR)GA@9?!24tH}*VNIUk1FY3wCbWYq+cPiM?enLk2GWmko ziy#3UY?Cv-8F2!618U zZJAn$5tP0q%qv3v%*6-0yWiOLRqPd@;a(~b z+yG_`4DZVEd=bF|tZQwrSqWdF^y+s3e%PF^Ld;9WXasfrXGSfvGY%5M)<8D+Jix1< zaivo&Nt(-pwB%MzyEl;(osk(<*?X@(&Q;2*}g8W1>zS5t$5t!1#2M0nfY?oYC zwx_xIMcKVe#%2!5H!s^t23eLo*z0JE-rE!eLub&iuW$hfQHAulqt#}jc7jLs$>H+g z+1^yr(H8Epi(nytoxm1NbmFB?{p3Y$js4#Gz-Y_hMSqVbztx`j+7Z8V=1ENmaf5o{ z6dEK9I*8#bzhB2g5U-y?+uS4tD$;!5HN=6R$3kXBw&yRx6zqv*`8lRBU^P{{0>Oh> zghAo!%jtR#%jx$JaUu5-+u`9qmPYLfH~^lOjQBe1L%Gc6xIO~l zP0*6!nuL1}@6m+awEJENJ8oblCNaB(9O-j;>VhS-r?pH#h=;+I#)MzsanTnZr-G7_ zk_k;p#|tRC;;y={dl5tCxJ0g_RmxKxVcvV?lYxnd!S)3T={Aun(^uPRJ(?5TZcYbl zguQ2bm)B7-h`HSdiZIjs?~$>&&oApyieeD)=3RL=dYm2}Z4HiBh{;+f6>O3|5LlZz zwHbG0P%pRo{dJ|JN4ZdLRvdCf58qL3rtwecj9Q_^hJS3s{+65b^xo_Qci{`N7|4o% zhHxb_NgMjs4eo;Ja#kU zz+-nXHNol`2_W<2^gKU?CVpeO;R<5ThhVTvef_~%-3-`6P?ztGes8h+4fP%TH8G+b zSCB#GZkIYEi;T~;1gXF=fXEEFBXtk%h^1h4r3Weo>H2VCQYG~ee_k`S%^y-Oj`O%}=S{NfB|78%p)Q!%SNg&p}MrsN5+g+&iE3m?ff)(xz>ztJN?3~&6!dN(pC__01909H0R1YN z1n#+JIP$7c3)?)A_{_+_7_?!BU-4>U0?ty_SvlkM_2s*NqzR!rt1L1r5sPJ%3B-ET zY|3w{RBkZ})%b@F{Xf@VOyoIDdpH4lt1kxbk){)s#p>kDd~S26(25rub$x-a#&R%c z5WvhwiNZG8bHvT9p-}tQ!%*O0V*7}KzZoR(x%B7TT%Xn}Ji0UY zue_(Gw5rP>B?&c&4(1NupSFSXxcVLWI>x#{vebjWw?{DapQeNawHFdIqz$M)6g7yV zXkXECavsChycvtQy80O63Ff?_+iT2Ogu%;0(%`qEj{M}W+dp9%!|GD<<}o=ja_LBd zudi7`Pzu9WAT{rh!}vRj7DA8Io9m{_4R)P2%t7V{%EjpUIT zQnBm?W1XY@#$A{lrM+sccU{MhXT;N5gNa!UV|04784EV0b z7x0>zYnVcuU8IM`w~}us?+Jw>&cwwx_7G8@Yd5t4n}BdMj-0jZ1sEkzHY@l-MA0Fs z83AV%aHpajb}nLM`^`W)L@xo-Y@=hi&}kup5`^T9U&$pR6E*NVt#)C>iLIPVsWe~XluZ?Rh_)wz*0ilf1srlzHDw+Em@T)t>+dg_! z{~3dXm(yr{WO1uJOEudTstCHvv*{i_oh*z9BZUXnH}c6D()m{C}FJSUm2UO~#Q-wf|D=HD`8>7NH- zbl@2{`Yq{C+fkJFfuQ z9!5MA6ciRZ-@%J9O)LziY@?Scm1&S;h7@W%4&S0+?w z!@(TSy|77$ z(p2RohkAg=X>%WnzGJO$h74nCxX*_ZPUl*IVKq$xSG?9!wc0*DmSg3X6JNy>s(S|W z)XrAS2IG@lS66ZdbNKf1OflDpSuF-qMLJ{!EXnTonlI+50Yrw0@pX*y@5got_~&Ck z^Tvb5WT4u?JP0$TGD^Q6EWZZWjR3g-c&8XfXcz+LMNsybi5+GGZ~?l7IrLnJy z`0#X*F)>iZJAyDrkgRDmAb2wy8;ohKO8RWAkxW2)faMRj ztYdeW;W*%a*aS?BRNe)6F}v(~`&{>^aXMNVD+{uL|I#?xer3oaa18J*rd8no8G$oUzW2z7j#kl;YRCJ**>D6NClW-PG9}* zepjGfp>qD0q@<*g!pb(Tk-~Kwv}Eb#s6g1ysIW??sc{3flEF=S%p`IEbv{*A0TUeW z^P`TRBf3oQ|A2qpGlPbaaWddRzZkWu0e2MJXvwl1F9+cQg>h9qCIV>Z0eb`eYBD0Cu@qs{ zK4B<3VNP=Fg#}|#SO?;OA{VYJc=$pR2Sn}Lo(ZBxr4$X8!x-kDGt_}#iN~Ps?BWsv z66??y0H#xA^T&VXrqgQTy1@q_pa>iSVhRR54e&Ug)^0zC;uYqtR7o|*27oy=Pr;k1 zhN=OWrc1UBUjSj1jbRevdzFS54a0LWyB-H)zXL#6a~}*Az=D3AcqFx{9rJjT{cA)K z=ZlKb{e`UqbSE^uc9hywM0v^KPhqd|@$xng0*JN@kYh>N=evJQOl&Z_;cdWavk3?O z;?st6TbL&`?@=ExN0=# z+TU9(WI||ut{Dv@e|Yf$Gybn{lVOIvCa&2Bt!HASWb}68)ABU%yh7{S)5dX<{PpUrp3| zxGi?Z0wmXNOJbkE?Etsder<@s$m>5}fcdRtG8-s(;GC2Legzny*{a1n3{3mP$N<@f z9}xFXu>kMU;-R-P&?Z4%_7s|;)&H9C?>1x0a&mJI<*D^|by-3e3sEE0|9}56%K!Sh z;p_U(mxcIF|MR)wi~sj)N1TrTytV&%@&7!Hzd_;u`?Fzq2Lr38LhGu0F)L{{&J*ApG|RA)@8K z|N1{K{+m1g=V|;ucs53{VZk^`8Zw}Q+pl(*jL(zm>+27E_!-5icG70|X7gkLfzm_0 zMUXZ#GXvxC&8fPx?gSpQ1mIhDw`IEjX}wqW+!lyDRmZ?8G85qkpb5|i?0{0MN$#C# zAU8pR$`$$N+ZSYq1m6UsADG@~eOuX{Z88&68!fktn`nY84}a|-$Fw^onG1UN1wt)^ z-DTJCq=*fhT1qar>&DHSzoa2sepP-6HCVgHvCD44Ka6|03-zqq1D1tzj%wP!J@g6p(JDq?8T; z=?0}6=?07L?hpi|yGx}zr9)C0>4tYby7xKzjPK{i4;@2+=f1C4G1pvkEr4u;N%On} zno4sOB?Uz`c5pJ3$|XSi2Z7S&kmxhv)z;N5QfOA&I{p6nPR$S$R#sP5vJ`;|rwz;w6tV*|8ura@4@FmGBD)IK#O1mG1}wtI2I1WFjrFuB-OT6?pee5=MZdLBe1X`KmI8M zl}E5xOd}cSD4OQC|9;}=JEW%s5k;wxVz5f#n+d9@#EeTS5d*VycJ|XoIw;y8Uh}pJ zv;^lX@wn^g>MrUc$REa=^RT55*W~;#fX|}l;J|0r`KV@C3O&G8X3%$U(ww{ob)KE= zTaYbG0Uu6C=p!bg`%}aGPn{GA>F-A?1ZCaElK5H;nReOSb_onNJ&q|!uo3*!<&+2H zLwQM77K7d-dYCHgteAIXBLyi1U(#0nsq_T2Ek8szxI{h2`I|N&1!G?|aseqgL6kKO z9$wzTfq|h^5BA5ec=ZYjq=ZWeLx!5g2ZrShx7H(?^zR1Daey0h&b{Bf`X!G>FXO`<^}`=fF|MXJ9pNBNoK?cP0o;IBFby@ZixOE zK$19TfOM%3aTW}l-2(w5PzXUogJFoj(>92$;9CHE4uqqF<*+i44!s-zV>Cc<%{Yg! z5Jq=Vzsq=th5!`i3WaToM6DL){8Lg=fb>PHoKMv}hKY+?0UTRq-If@%7iSK@;DN-D z(|$ch&BXTi&*y{b&_xlxneAWcl>s7HiSiRM(3uzReY+EmaO9x!Rc$Mbg**!653>** zBB-I2@#DUC;G9E-xzuX2Jtw^3g)*l^bAw`KWo0g|GcqG|1ey(Sc>OhFf+(LnQ7}FN zI=YeQ+UlywMz`PJb$omGdf&mxX3bV9k2LU)KB>JP1PVlHIb0YFQ{V-_?JkQ1ZdXPJ zM5acxkOM~iWQ3W#PfDX){PqE}4O?RaDbazBrh3SQ9#aMs1M2c6Fc31}g2hM{2ZzJ9 zwlvHWUWHIQJpH$&AYqQWnp$`TtY%Fr>1R3$=@zX8Dh;+oCoBv;id*bzCy z%MzzugDSJ-zE2&(2-*s?Bt?$U`?UhrV4;@dN>cFG!@q5yf-`#K#tm_5dm%*?Wz_Iu0y*0o>m zMUJN!NGT{pOn#_>Ifb1b2~Y(ZW`{LOD6akS!0CaGOH)&mN>>v$Xh>Lpr3uP}6KWFL zG&MEldQ{G(-?Ijt>YW~hEP6f~gcul)EEPrKlDTYJ4j#q&w2GLf1~epri5i+AX>!B; zmHz{(@nc+8$A>c>1!dO9{XDt(N&`wygSI2mX=&$6F>>^%{2-a0(zG=F)_E< zDAVIEqNlcBvUVU_-8li6TY?XHG0zIHALv(50gWXvq>E^X?XBWOBjKYfJ57c$;Pjf+ z3M)z~WhRl6Jdn%c+e|?z6VL*XOH?87KW{|mH|ieAQ>U~sH&fp{fWnw0k)9mIBMhmCs}l=*wUW^ca~kEqw%R{z(~n?$f26ib zA&8I0*zon`URHop1JDG^73)m_a_KOmpTlVj7J?-LkO!#L2qNG4FrD!cV} zDGJfAlxiZpgQ)cZ9=Cn&F!=hw4kdnmet*kAfQO4UGK5p1Ubmxx0^GMR8zgeh0cgzEZpb$)eja=Ay|c5EhmX%;Gr{#0Fh%507C7$t5f`&vE8}E5PJa(@ zRWDOd=m2b0m5lt7%dZ|H^K^gaZ_zafCNGd50IP1YDMobvQ)nSh_>l1L3Op;n_iRF#INg-e>FGSkRJ6qdMP84uz1SDi z*F?4;tU}FICWS|6ys=Sm`g7Nj8Q;5oI0y?@i@41P!F!hNL(l;x`Bb~a+v^TKX<5&m ztTHK}QAzNCQyYTCpl!4(k&La;h1#2mXDH8pc}-0XVd4xp!PSR0T4q_A!%#?j%z=)I z%9^L#5*DBTndJub>^70PjbS}_KvNOvv7J)u{W2&fEj4vPD#LZM{OHJ0f4v0CtDI70 ze>o&bg-}D%jNqcGcGw~g!9J$9dH3#}^)4ddal7n=vdSWhQ`2Up)%=1K&aSEcmtuCw z7HB$O+=r?|$327B03uiivOk9A29AXz72;v~cwU$CA+GGSk<>49)2rd^Kv@Hz zlnHPiaJ4d|;xk^bl5Qqy6u?WZ3q$uw>bSFP+8sv@2k=u>zFCnw8A5-z=3&jPloA*K zon33P1@v8$P&??LR-C>E@<(5A@S%_B<+yJrRQ65PESc~Ye2La=FE8BWccJvvwg$b* zmkpYHLqK=W`w9kCO?}rmLMlSG8u-*v6`*U_GG2pn1BhSHLP*b0(F)PTBC*+>Eydz3 zK!i==fs#RCiRmT*Q>WZcY^$nxWIWF-o;^>fPxw#+?wpeLq7uB)BS9}J!RBta?Pgr- z#sJYnHoH}Y!47EW5DUz`_%3*#fM5a?r>n#fttl7BhX3wX8^zV-+&_^Y`v^pcW#2QP zLekc4{y7~&!)V$vd~Cq_7xoW+@9hm$E6%mS8yUsF08g{95^(eGgU{x2XyO_uSyPu( zj2-^L!OAEAfcDooSXPPjXUdSA>YhU^(r&Jtr$+PzeIRqTuN}Y*A~o~lAVQ;HigPa&N^#li1!QLFY^4~`sFApo7@84$!|U5f@^ zExAz~l9aT$xR_q1NP)rb3zY0?hcQ^-a`9LKwIo5+U4Z91*&t{#X-5=_X-ED z1Z_B5r;SH^?^Nh(_QgHK0oHScP(-3`9lEn1N()sp4BD*VtmFKFf_^B9Api&Vlk5M3+o;+C8kA-e$$6v`+~Y!Rq>J4ZTBR|CQRQ6F1F_QVq_0-gaP*$$UmkT8`M z)Io+8n}wZfqrZx7RfTAPL41Wk-%CkKS1=6S1k7HutgRZhK^!Bw6QYPAKW`8pqE`l9 zrNA_eVH1QgO9jjtg|FU0=?BE1h4mdBE7`}2SV)SF$-fJLLsmkZJ+3w-WYNw+(HO{= zI&Uz@`4Hmca{+J8oW&240N(K@gSgtRk1ERW-o(qdF#2m!*}!m#bn4yMw)^U+z=(;c z=ol;IU69?F3l)=)kfUW`@j@qI*qB8JQ#m(U8V6k=rtu5RTNE7&Q@}Hkc;~y*BiId$ zydQGDJD9>&uewhE=3H-iM3E9bfkWECrTOYr3IyHtZl?=N;P(}2iPcJtLN%P0`yM&v z>`*j9ws3ZFx|MY8cNb>DIY7g_Zz)`ts1<>}Sc)oRGoyy+ltnw%;3O>i;v&LB^I}e=LjV-yOzqwqfB5$5B8mqrIN0I#t+By2n-~7&5-;d{ z=3~vDfUc1KAo%IeyoR$-i(6u3#ky&*{RK!Floe8vl53+yY2|DWA3mFod#tZ;3VgdE zlr#iAfTRlnPK;35_*UYfXFz#jC-=bvCku;MCrXH^%C*Tkffb+b%Qi=9+OYcPFal}c zAL%J-$5|VYUKD7}1Y&hhz?eb~71oi50+#U*1NB{=S8SfiuH` zi;tg$trD-VQVEHoii*n1moGt;#|MLKsVC7i?g8hcN6*Ivk08w%iV}f3EjD)?{1PxZ z&iKBJKvP1X+|!|#U(R5XYX{}nl!H9!X7|4VR8{K=w&$9S;H=^mD1yHM_#d=OL-E-}cbN>*yLt?Si=xdHgG`1X zZg6u2+>Zw+yOkj-l_8>k3P~Zx5C${Y6M(OV)cOZQ$OzEj8MF@|H_E|Qf-X@Z3!`{I z$zHy^$AJYu=KQDT^en^dM&J7yM6qq+hgL}DMFP^e&}1LP?pZ{LcdP;#c>wEQjNMJS z!NE}hRL(4-M_XsHS4#c2z5cX4qQZ9}I)~sdJ-Mq!+S)VBVbCxOshSi6)5=s$#kW^4 zPX&nEbCAYcTSe>RAMCn31^Q4KBmBTa+`o@5emg}_Q1A!JJoh#v`&7}lDWwx$u7!v~ zS2Y?6iX3gg9b`cmc%(u72jfB*GHbs-HMQ9F+bN!q#6!uyEfQ#ia{eKrduSQ&|#|&_Xdi)65FtZhLVsc zN4`gdhBqLbRYn11k_MNG7aWi8eE_RK0m^~7{!IapG-0+qF92s68RF8?Q7)%Y{DvS3 z(r;WmJgT_U<73DG7~%L@0M~*X1yGdPwKZB3XrF@40O)ckrn`y7@A&&63yU?dB{NVT z=YueynVDHjGBkQ}Q{kG92vse?wmOzhksyN5V(9qE!G`xby7{Nb_>AK8+A=tcj!5tX zN+_^afFwd>p+Ny%y)UP=9Cw$bl$Dk9UIG5Kqx2d$X>vCifaW0a4NP-cp3Vr=#rxQs zo16DHCX`LiZ}l^AajC9S4?>IWVjN_SG(|PW=}L$mX24?r#DbPT@OP!`@&|ybH2-)x zfj4ur5?%w5@X^|#q%30ef!hU1kBoi54R)kpp(-44Ko}`aiOltI%E9CYJp0ABzgypa zf#eK&N!E)9yeJRwF>4&N!1#DX_vnI`bp@<`1P5Tyi*`D&&0}fO+^g{^FkbuAfKMKew4zp|mm9y=6o-pi88Q~3&l&0q5Z-uiHBW(46y9$8L7f2;91bSpq)=-?F28PG1$fn=^v zSb_O0esC2SX0*~m(?c*3+iD+V>Y7yB)|%799YUj`4EPvMAx;IPX$f?XBg!eb&{CTB zqYc}cVqtD20LP0g9v+wAXWGt%goWF0bTAUIK)`lu`|9$-~+@D^`9IRCwo%K&uV z4}%=ua;jPpe~rd=b!Y{wv>GJ>$ZWX^F7O;+zKlFzG$L@Q+A;tc#{1?W$#n|HAw>lg zGlV0yE!?p!tAfY32}0X+9EQ20GO4-3#o4;KLPm|s6gwm4w@FZ{00-l+KFVy+6$6a2 zI2us)V;DiG>BeW0GBEV?_R35wz`>WfQw*w=S&9D8UQf6Pn0&vhNf3}Y5AfK{VvcQp zz!eM+KY`GLrsR#!1o{4KF+@#H2;SPWzqUvM$((XA&;}vrQPIO5PkaI^44`m?DZ9nw z7xN9E4VPP)MN^2P+`(_e~6Mu_dFK+w0yAz zE}rAZ(Fe&>BT!$BhcX0t2@%JoBak~w#1&{GZOqOV8TF?EJtc=oU#}~XTR9(ET0$#2 z2JVJji9f}S{yAKD-t!1G;plzVsuK;1?jnO@TU0pdOy7X1uTpsEV zjZkhhK5g%j*QlTl-RBPF=l&t?TY5!9pGxwv(5;=coCpMrriP`)9&C3 zylpvdc)y2({IqY;AXsvDL=Iuw)=YCJy8;hhjQtFPDMppTXGt$f8|i+tk)WCBKz8=7 zJv9f?n86Il?6R)mZnp-AUh4yS(W8@-xO$wsQe;3Pp3cSgi5!o32z2tx~_P?i1y{ z_`u@nT(edQ;F(y}_3rJu=Uok-&o6Wk9UK7?^7&kL>epZ7n1zA_Kw-2Cb3I{9y)c+mY)oa&&;N$NxgPM(9&}||*)nXxHowWPg zhcb4X=tS$|Ws-@lLL`XJtT)s|Mg^Kzk-jEiK7&aW+1RQWmopJAx1jf)miPf6kQ(?T zBn9SbP;e-pe$wv6bGI3cJlyI}=i4<~^$?sU@pv7A0_aFJCV#lm+J@~?7rwmu)6H;xc5S!)wqyO{a3ucT}c;%kDoaCmd$i{+B%qZQS^@_BZp79 zxr39B0v8tamR={5z7?5$!wDLmOWOftI4jQ1&N1=HP{w?uOQalA<8tHrbtdgUT`x#T zQy#Scux#!hA0ehSL_2xG9ZOSs zpPK5%du3sfl$_jq1cMl#>?+90$_7Ng1QXhL@g_-FQ1IHh+6yFeOUvGvsMx$b`dubT zNy+s9x@Fw*iV9Zkwj)jv4GqHGLK&=QWOSAbz~qd%(lybwvQcGYZ5@;Um00>8K$z$s zO2gZw`D@D=85B$S5HcsxDk^%@4Jl*f<~3a;qDTpSzJZsZ)fv80DyWxR#$4BM^JD-HJkWr@Io`hi ztfsP(N%IhQZ-a^QCn$fDWBNKF>CP8sXRku06doRq&p1`<6mIpXV|X~qiIQ^_3{BUy z&wNryn(FHQD?T6v+sMw0j)rCs@Y;Q3xqw;bFUJxI`A_fzPu^V$Lt*LZ1ERWZk&$6d zjX@kBFE0c*Qh6IjcFK5f7iPC{vz^9bW04uI~y@`z|_ zH`LcBUC3ElGVX4BeE~?d#Z&%HO6YcXs4P(11Ner@%F3WVZQx#(lJeH-(MfKMu0~do zX}@G&G<<4b^DC{$6CWfs5Pfz*Il$|J?ox{JO-*HGZ=$EjZULj8{w{Itzto$Q*eH;h zB_itP=_Mcf@q<}C3|~W2llcQ=m$5e?b9;phfeABu3K^y^;zEX2S65R|P=G9U7s=4T zzyo~z*BYGrNY&}-o>l2(L!)Rqw(i%iT^n}Ax1qOT#3*NC^>~BCYX`?{V`F1|P+>+1Gb(Nq{uehwD5T|0!jL_CY3EilTx7 zpa(@Z%-{BEqaOITS#c8D|5L88{FjMjl)?N~1Sb3-gvgx)yr6REwF{($dIw9Ss2KF)=r9*4WXy@+7;s7={`69AhHeZbR#7 z*F;WisMzO_kb8TvukxRQHJ6peI3bMTKU%qJ1k>l(fzy82Bzt z>{Ib9~}HZ^!KbHq25Z#G`S^{yMZsBy}IAK z@}}*9t-J66_4BKpA%!LjuZebY@!O3=QEEb0-9KY{@7=$5m9X_HA1?~_x6lv$5W7u1y|;elKd?UGNji1-ox&uXhZz-*jx~qE zEOewuKHV36Kq1$Lb<}Wt$#6AxoL|C%m^{jE-oLsHUNAywyqAg8B!nQ+N1J+1>mw&l zpL@^iB*D{IZ~F(S#|a0~Qf_D3{AF0HSzmDW`veX@4nzz@og7$T;@JrcbE0f=0sA!|h5i|Y1X!a&iw`C)* zC0;<@dV~DreDsJNTh7#paE`HD$!2)VuJJWo?E!b^Ps#5Rqy(QN>0aU#(MOUG_^L1Z zjmS;SS>H#^exSxJM(&gJ*J>aR%~yWptINRx>2f!{y-;aPw#e(Bph7mq-X(QxAksiT-s47&;*IC;6GwRyC+fo8^60z#srzPpdRdp#j~vCi zC?$J%hxeb4hv$A@Do0~kcm9__4KH>11n%8UdM}2&O6<$I$ue(IqD}vsI5fQ9iZVXN z$cbn8@GtYcNqe8fB`tT;GGcco3#(53F}`BXJJ=sC;g1&`fkopRwf6K>yaeuE}j%w%~hJfzY@3o91T*ZxG6atIKzqBQ( zx4Vqqv>BB-Pn(1deW`{h_J7c;KUW)oiTe-VCP{w~Z1hZDtkAi?KP4#4;6-RJ6PZ}b z=>47b`>YSn@NcVP6ov_cm!*xYrt^`Rrrl&p50LXY=ZiGq~$$igEJ}J?nl0t z>z$P5@NFw{Q}`LHV9i=WQ=RgtEH$q%D>L8ai;JVJV+4-0t};5svxl~5m@f;jm0Q(>o$8EI$3JY1M^FB2P+IN~LVb_- zzgG$awvog_Z^6#^xcla|bI-f0ElIWI=$|*)zL1BiunMglJ)iD-I=p}(pYZ%Kx0{xQ zy=(a}i*@Yt-$-Jo!Z|7w7i!HABTtaAxi$6UuobXMmQZ)Mh?A z>WrCP@~*_4nIA`FNw-<9w3~1Y>}?vv^0(Q!YAi&n;Y4ZPzpSP%$w(_LD@aG@cp`De znEc+S{&@3|tJJTbIbNF)e&weHtAYcxQ%TqpP4z56ZT;1W`FJtT-#KJQH*QA{rX{kY zkEjbVaMZOr{xqWJb8uk@ra5VSH_SsES@5yf-J$C!Q$8AphWp~vMn%V7k5^~W#N@yM zGFH(AbxD+w9_<8@+VR+z80{XdYb4^RGTv(r_meUmZ0%$p{-0CNckh`84Vj~>l~eP^ z&EP#ESEsU#!+2!>o7l{Ui!Y@<5&Lg)IhOdbIW63w8(R#oY36d8O266J>v`rrZu2hn zJU9$zhLHJ8?&qF{g}!qW7pU_zMAx``XQ$>$*2INhFWRJG5o` z(K^{ZLJY0evK0H=f_*Y8l0NfyI7S{aqX+p5W)9ccXG+Gk3W z%aYIO4}4iA-CVxA7R{OuE9@nfH63k^OETYd+vOy}yr6u27GAg%Hh)5}@WLTSdttDI z-+5YbcwDrT>!@~y_f(zP9XS0a2TwmX69!YyaNzdci%SFVaTOu{)8aY&@9c<*m=<1y1H$SVc zIFTS16*H;tCLy0SWBWnnQe!l7jz??%n6#Qdq4GCBSG@KQ&FIye`9(gHdY87|hB$_= zn0epwy-5>@Ism_6g0in?)cw4(up4Cw(^)Bq@0~a!i}UGCzQbgIPX7IRi;sSFxuJ2H z>QiwB`}*^M&Bv`DP=1wXYb+H7;@qW~-iklXl$y-m{Um9dPcLV8q1v)qqf4QE+q8FH zG4U1IhTeW&)A7u{po)BdyXn}m4=kntM^2Dv`DS52Tx;IZ`ky0bB3(bd z!Qmw~aK$c}t>_rAaQ=Ff) zTn;^nwR2sBkBPDbIdPq4ol1X_-MBc4=B@@qa{DaTsrmbwiZwHx)D69U;O65$>OzU(>?g~b@J3&rHOd+X8Q z;r`Ff?7elp&x3>1S7D?NYoAK1chZEcU^LqF@J;ToxVrhabvKq=)diOyBXcbcs~U#l z!eb+HgD&ywNhlu`+M4cWXS|F|#;DYg`-N&!Bz@X*@?P8K-TSE%$-~1_Trs{?!#z4B zk07JEXL|z}^K?x+k9DGD{C#<~<=s%XIZ&!4YeWizy%JwveO#0=bE;op$=sZG%gWJH z<<}$JVvNQYLX<@Pu06I-v@ylWYmL`_L@b`XoAD*+<~y2xcab0{f%=(laqedSm_k?^ zCRmSJt$ZebJvZn33d@YEo4JGahV8x6wCzQzA-elXTO8}y;mS5ZccA6)Fv7dp50ZWV zejTkpLptw$7$$aCsvCViIjKgkB=EBBjE$RXcyoS~hR3B~X#Ls_-KT|!)ittTIEi&F zVSdF~ia6*8m4?3F6;ym=eS_**r$lRa-%oRtCU)lSoHK{(BR^Nylsmi;`P=hHCoj?Q zS|3l8oU;fg@ih6HIx&`7n%vJfPhXB26On3(narV6W1!K525#skhRf2lFU&Jfoxblp z&~PdH_S7KMZx{D!rs@cfQ#Wqk++oirnJ=*Sfh3r>7-o!4^(53k1qyg5SsNtUWZOLp z(v7~}n#@B;YkyYkyZhyRobu1W=mL~CWMuz$-?6UuU6QkS4h)UsobJW??ne?`6|GmR ziq!pK5hQh1a+q-R7SFl<%fPUMiloUbkpa-V@xtzR6DW7*jRv^$ERn^WJYqgH3yh$@%lJOM6dEXMz+FRBx-&P{19w?-S3F#yzyQ8jDK0lG*{2)sz~*hA4!Os`Tp&lMC?2tEm(uyHY^EK7nidVJsWOHX$)UWCG$mwIa`XV}%ry=gG|^JHC9o3BnPlDt4bx`!TQZ$(M8x>8zi5+GBTfUO z{*t@bfO6rU#crabBOuC;o)u7HLi=D+wzzj5(FY z_9ENC_xuj+M$6YHQRwLyh#uc7?EMG4MJ&hL-g|H)aIjF)>q=-JEqbP(|2R)=ZBQ@s z)E&bKvb)Jy?%T5z(SjVkzAE`TQG1BDFl$+bp1G=K`f1QagBVM-sHd?DV_lHvFoR-U z$J0lq-CUpd%3?0&O8DZ}lQ(ZWcKbN@&sD41tv{zUFo@LKX>S{ziq?%zJkT(9HT5=R z*R2(>)wKG>J^l;vC zd+t>&JZB!(>^4=3kjR?4eaj4Dz++`qlkgQPstM%Q9EeInH+1{HhjikS<)3@MIP2OD zVU5fiv$=VdGSlI?egBKetT!V3Kb2IhRB!mcXyN+M47Z-L z%W4NPUYEYiUFah8Wq@-tOmmQ;{jJ?wlSW)`SO}Yb#aojg&{B%Hvu9E1So(W}{pimh z3O!#jhd<)dUlckYSEz?Y-uG=4SSye7X5_J3X$?0p?P_$;rsagFn?LtCUG}+$YD(Qxl@XDgpT$!nT6q;gTA%06(%(|7x#;B|3mAU9hN7B`U59mFL|9<0f;*jo zt*-T~6qjJ?csr@MAzkt=QM$%m2(|L$oAl>1ga^Mgm?-;zSr1{A2^ ziXw>%Jy-B%;3g^h@ksSZDR6TBb$MRw%41B0;%6OKzrOE!Rc3B+(Zr@svKa~I;? z=zgjg*1mJwQ4N!5ir%)scaXy}EB|uD+s-|VX-dyobX~;LgvHbTkh1ccu1oqD&itG2 zB+_wkssh|f0w~+lilrH!na_EPNLG{BlkB;ajqg0NLE_aGLg${iay;fDZEc40Q>84v zJl3t>FOnPGD2dzS@c9cnbKY2iZ}sN?IavnxZlzpqd{HpbzIg_GYwB}rdPhvacRWIr6xo=I*%MyW$7(R=T^^+0h+#vS&mJ<{x zHS^-jLHJ&?;ygNmp8CyJQiJB}wMJrNB5CpbM69t6Vk@}RHqs-wt;Dt+Jj6<_@!YJ@ zQhWnX1Kb+!II`^2V#^w;mPoJ&^SxGB?SK1wBW|CfqG3T^a1H&jw;S5+f&M9!-dm-b z19Y@{`9+PlNiHvz-UzJMh?t^9&jrtk44~JjB;(n%^AlmzE0#3#g}SrM>ZEaNqnKW@ z^}(1A@QAQCm(V*26kuT^Lg&y$%^nL(CKz% zwmZ|Bm!OwnzrFq_-Lh%#l5?%3{n2ZgXBehl{?z&Lk?%P=xXg7tqN{&5Z|V{fpmk3) zoqE?|dl)4ieUQ$oeD3*CcDOla@u8FEXhnOU%rk+ULR2M!2^9L%>kg#lGUTat1uo+p zr_yh7r}$k;zB<(Ut@S!@pVnjD$I6m;?|sJki{L~F`0U21xex>(|_-dLo=-RdQ1V=qakP&ly`YDZTr zwSxbSjb8t*%yDgT11{@Vy#h;gf&d+t(Vvxg9EEz(%EJcnF?P=No#cTH6V%^}T-S1& z*9_^^xD4C9X2|yr0?mV#?&jUE91xau&j|?Gkn;;KYG~K$M>ESw_X+U zJH>7M=TDP{Jp{>%6@OY>J6HL>(c`;&ImvJOtyr}oZ%L)tYID2}^=M(_3%6P28`Bz_ z0jX$9ly!J(D2}s;OQ+aG|MH_y!mR?W}aV4 z-jush<~oyD^ze+o@DQd24yy|@%GI6R zxU6iv-N2RWwqWph%|@3Vs7U=OBRP@gh6Q8XYln6vICP1pYdeW&sTsq{#E;4C?Co@C z{;44Txn`Gl;g+?$sSHgGD}Jl~sW{Uv+G2{^q2JF*lKsrGqV1{i;0N5T#9H~hFwQYP zqauREh7L{l@QML6f+2U8(N}yF=|mGN33X!>&Uy7)a>xFt!bYEVM+ne+G*2j7zb_J^ zM!DDNZTJzL9?P_uBylsQW$i9*<@5z#+KQnhngn}XIhi5S8dKLyvb2{NU-)g7HZ?EB z9$Y@&v{~%0+z>i8!0O|sdkcUcUIw$#t>1d_-D%zIMFi$BdWe5&J*E;6s2cTghMCE! z5fnr&u`Rc-KBLsD1#~iY>A}u}fmK6?9p8&$`?X1JOO-EozSv3v}nf0-S5orGh2loJZ(qvv9Vr?~9d@N@f@rgIhL2f0u4Dq`Y*;^b|d7P}*&giu`LLi2$I;n%WW$?I5kRUmQ43P3vzb zMw`Wyy`pY?pT2vkQeyq<%Del;nSl(P&L`6@_rjCH)n`Wc==PhFPmeiq=uVwxn0s>C z3A#4VGd9bg^-Ue*@fbHM_q;Le%P!d4>Ok{p`WIFm1hRwe)O&yNso_m1^_+ z=b^QV?_KMJ=|6fm=WtK;r<827N)!A2m0YV#PA3xkku4jSo9eE3c=o$MYT&roK34%t zJS%r5{#}8=;_d!QR{BeQV*8zO{_!t*Wy?Q(8KW+Ir{{8-a}J643>KH;LQYaM3(!xa z{^K)`w%#E*d*Klg(HCUbTX6^|_BwVG=T|l4|5PcRTVt!i+3hsv-k@8EA|;q=cD?fx z!f>{yBQ0nIPgGo648wUh3cqoDDv{cI;MVYX2N&0`{%+#Trft$?YaBNB0ct`BK5F9m zCj6tf6Fd2`4^ zrKhv_Ub!0vM)db`-}rS_Z!pX#S+7;R)F&%WfZN;jYo*TZ{KeM)0^vw+5d{sy2-&Nj zvdb@|7p`^N{JNa%<=Co-MBLCKv-$UKq8x4NZzgy`YSjzEgyf_$ZXGSsbK0%odeWM9 zZJJw&rEJ&^XKvVPJz=ob60t1!RysHqTt8U#Da+}4aOO*@QS66peM}6?J-mC_+6O*< zH*p{*T$n=^7VJ0}Z!X~3I8Kb=+qLqTLCwzLMlMe3QC69%T#8_}**m*YF#cUWnmu-M zX}^mHqdzr4Ou>g_?#XRflVdbK3|rn9{ADpcYi$n?IB_By&(rygPPWO2`{Jd%^D?4M`HPzahpH5*+f!a{ zgs9PJ(H=EsM3>w0Xi4?dE3tqYjZXY$#eUBqU)j4O^bpf@mEb!`OA;INjAMw$v?ZONE3$^xzYrCJvWisV)KI-I+zk^GCYu?wPFge-6{)#Nq9PkRL4UHhViY zhfM5bloj<^_#7+n%gJj0U0YZgO(%5Y!*I3 z0}_Q82QgpeLy4JtK~Bh|zGvh$%cguk#&q47mQWK_vsY_7pQ;@>hrW2hs7q3DmR4`B z#xTR6p85Kf1ZilD;VV>46-#XotK;~@egpfU%qKmY3%KV_-Ai2lMz8nQiWJy!}!MD>}SJYX!9+3B>%aC&u(7X3lJs` zU40NwsxxQHs4?j*ww%MSG{`axCPK5}P}ySTMs4xTCRS3zHq!3%_c``NpSNu7cT&q> zo_xExkwr`3PgMsh83_Hv2Y;U(#-neKim)CQq*N4!xQ{+wj&7J@u?E!y*XMck5=&K! zc3shjcV{Ozd1C>5nrEIbdpd%~NdahO-fz9@tdZ-D`TNBN{v2ENF8p4{J6U)W_`EMQ zw6-~!J%=+s)U&ERGy7I;ZqsTnGB{!DT9bHL!>i-(spv6A%kHrKbUbL~hNOpDnd}Rt z(~2O34LE!m|3{pyKL&)*GBo5I#|>1p$`2Vwe#TlggTc!UWBB3HOmOLA-h)ts@B*iYKv1@J0X{ zYlTe&F>0EJR-Lc=+edYM+G!*h$}e|s-Euz8z86f=Ed3An;m?NQ0j7R=9W!=vavW!O zA=Wo?gD+Hf?Z8^s{^!l z8tk0m%gKjRX}gt@)GO^|BfDs0;=DAl>J>)gyrV|p_jKu|e(#1t$uC|H!M1}>F_Ht! z4bfp7oqD8kSj79Zls^^2+m}n_W~EGp6iWlT+w@rMZ;<$P4P`W54if<=WgJ*ib0e5; zrhcok*h{9byY~CYQGWxYGcm;@wrgT+@@4ZzB*qT}sB}G)uCvH-`;^`xTUa?x`_FBC z^%SB#5`j-I$GN%hCua)t`el}l7?}_!Zqi}bH8WX!t|~iMHL3obF?yCKab}58t|vpVg&n0;t*%F3Ku2#hZRfbc?k-YUYeujYHzmOB z^cc;zV||Xh-jr< zV2H<|eZ0NqOX<~`xZast_{MkwelGW>Y{}~qaX2{8$ud-ZjM>Ac-u-MBGh(x52#*pC z{==pI*C1TDL2a4qmXMsj{KR0w2#I$4aXMqCiK*ICWUH-_=(O^B@x%8-yf0gZip^`+ z4vD85Jp_JNrbZ0l+nJ}?`~qH&8C;zk!L+&4kHyeaqRrcwTolCH2%$n^vNoQJ==>mtj~A7u@2oe_8&%`i_PB~;P$&mJ*xhq z`POaf*NOJ7cQK9B<74;l-}~@F?blrtY`r)G@-QMJDh!6nrMXy!{`AHF%{n zg6BxQHUR;cx&zwcYR;P@#nT0L$GU!Nxq-jADDi1Y$hHc9R^saK=Y7uFS7o3sh*DzJ zTln5tv2FenYmAbd+FDK(=B$?H`xcF*ciN-nTWs?2)w}D~*16@Ht;=L3O!d&$zuRT% z#ZV<`L{-+DS}92Vz*ARCl&DDu3lY#2QFbS&(>PVP?^}+;#gBU3`FCVJ)6YkLFg=nB z`EJpagIBe)n%0qpv`v?>*brJKW^gl4xW#3KBwe%fDScXgbpLzn7fZpnQ@XWCJ$Zd} zub8{)pkivgo6P?B|9#Dnwg|r#cHQJcPaQ);5{09qxW@FOyA9)U@-5cG^@m}_r(SPH z)r%dEvtxe6VDe&Vm8gp?SI*(P$;+eCa=TDEILK(vaC~A#KCv>y{xa{~3+!Cb3Dib)l^Z|4NWW3Tyzk>oBfyQjohG?< zLnrFP^@vBRL|gqHvClr4s}D;x_J|ae7?<)9H0pLI#_k7^BIAoG@(VmS%1dTkcxd-Q zOpPG#)P^#t{G6+j@Z0&+M%Mo_Q;-}vAn1vMLT5a7x60r`aMp1x4YwlwIWfiEpA-|@ zk`(e`gT)oQ#F^gTYmS1n3u^-cLnxQ#=jS(Peb1MhCMS8Zp4hHz@F{I#g^aK~!t@S2_HLz6A$xlSgN1Vc**m*_&;WfGyRudneCbox~j^mPm zs+8Vd!B2N9G(mbK8&WI1p#RH-?%#_8f4^-2p%&Ke@by$p6I@lnh2UJq%E*3WGI?vN z`xPH4?)mg@i|07p?#G-Zx|$kBvxQC7z(_)(jY>Nacq$wnz!t+tm)AeS|5kGlfK#E} z8|(+?ZE`Zg0W6`AM!HCSHpc50ZT^gFxD{dRPS^kG0awo|N4B+S>Y((^z|;+qIy>+08|_{%_5Lr&7Uaa7dV` zu}{2A^T~F~_N7MI0!IQxrAw~&J;?Gg#?eV)eO~vutd^-qphtgT+@dg}Wv@C$azcw2 z8>7tUq^YK`PSVIF;Xj)kgAb<1coezMR6l_JDk)i*4Mo=Pg@iuC*ymQgnp?bl8-%sb zdu-8g**{EY>k%hL1=lUA6KV3IC5^NmknLLew{vo8rnfd(Bu%gWGC!bBl5cpmws7#y zCAH4gm400_$XK25C3~^8t=sN%MoYK*{)*f%Dq<5mOu9Grjk&GaHKW8Q*2Af30Xf^OYrX0d`Oqp zsgj3Ya9Y-CBrO;4xiN~P$1ZMbOVI@fbNOA`)-<#nwrZbN8`G-D^pH;#QctHET;IJ` zw~8Hp_@aI0C18q&W4=wF##Xzs)TpiUylES40TY8krqm2C+yc(xrdCQDWAd?E--vlc8jaa2uu0q} zi_qVaQ*Hz~Ar&6Z+fV$aP@*&99d|}%5WKs{q_yNJ-)E_CdDR>P*#Z~Y1Q(s%tjYb@ z;Mslg8A9<{LJ0^oHnEqJppUcoN98P5jkfE&sqC1c(#IQm&aRq#n)EXIEWO-J;zd8o zbS6wHh39)A|7RsY174hA{iCgedy?HHq@RY+#iIff&L>hZO;;_Q!0(we`ALsNGZneH znijtOZf-_YLVLiJGm>PCl{lv4ubMyc%g<(|BeL=;XR9hc8<_~`Dk3riN2=uR>(bnkf^@-2GIKBcwn>h~GcnD8g;_vR_T zL^PjsXGOK4)r~T^i6$i@-vb>AM~nQpyZOU1#F6g9~1FOs+nqb(Vu9PqB#aP<(F?F}cZ57kP@xE`V(j3;P{BxcqYi zOjk5%MQchM5LS2YD~oe#^m1B`on<m=z^d}-uBH^U^N z%U>P*|J$B(HLv8;CPa5|zZpkPuic~(Yom{-{XL%$7vhPHQPh%dib5kgIo>^_OBcso{n*tf zGps5KUoSJZ=A!O2#>^3!FlOY) zOXE&cY+d&8@qa%d+|8E6QcBW!Bi)BX&79yb6JU1g~yDN?tL4!wK&9U>fXj~>g zxT?8bu61>Kr?!2Y5NvIDd8!Rs8_N3}3k^g+Ny)fwe%gKdEZkEkp?UV-kd%|* zMuL(vW>sgM_7?p#phW#i8P}d=z0|*6nJ&KfL=O}^)t4FY+0pw7cCQ&(oX&yW+uCy* zIGdFGsuL!~gYgW)uD`0AG!!=b<0L$O=Hc!!qzbujE$MCazH00>3o}DD(PB5V=^SrQ zaB1`z9rqy}(GXozpM{js=rPj5GsgvSSs7sBrbr3SSWmUt`v`SVqeB+*Uf>c1|J1&C zW2B;#l^|`lbzCfn3JrlOdw!L6211}}>cE1*f= zHEk3#Z|p$UR+ji)OEASw)nCdc!O1{b_XebW7&6GsyvKj0PyalhMST7q5hXFKjOWi9 zj@d0DpNF{-y@*=Djt{Bmo+3wtT~5W3jm@V-R!9K8ZvwZZLLrV7}P7{{I&l^U#zqxs^Ui4X?+sParmOQL3vn{rz z@>+$1y}?aa^ybRN*)>0jWsaUSO8L`QP28w(^EUo0(hU6>RwMivW>s$NvVP$s{k^xp zRVt6uQSnvGctjaz7wz%hetNMfz?zhL*Y_R65fcoV_48%P1uB?MzT?Q!``whjt1DaJ z=3?aD$2a;k0(Kuk&x!9VK*B&0)pMAjm5~8C$^<9JWPU@DRt-nALq`=F^GZM|{(suZ zJgR!^8;d8 zm3j7_nxp9PGm2P(vyg$Z=?}=-+PpaE^cj6&lxOeJ^Ip@uR@D=|ywbF>!5L#8$}!A$ zTsk4SSY-F@WX?ab6VpAdt#tkV5jl@RDt-9*J3)~&R3y01hWxxY%T#0K@V5i? zLT>7p<@$2ebDhExw zd0fX97Np5vF0%QFh+bg!Omnj@=3DMgS^y)mbF+Rd#CSF8^yHsw@s30I6LJ889{&1J zgFmmy_D<(QNYCy_kljYLvgSP}{!!AF2gmz;%~JXhLQP}S-Myuqk`s9I z;wU4-nd_Pu4_32!L&ZjR2 z6g-u_6kFTx!Iujho-`*M1PZ-eJtzzyXODdSKTqU-{>O;2xxJIQC))IaBje({E`<_O zhpj4u#Cr#dia*OQx5PH&Ok(3$!Da>at`~G09$)K;b4ORxUh8l(N_H$D6t9THwsJIcH;)mD}C`IQeK9TVmLhTgQ~K3&#T}?Lriz<-tAkLZ1r#9?T~#}<&AoP47X1M?VsxNb1+%?3K=&{!QEe>TW8dm|febWvoB zP*A+Wq37{DL1mRvapc%@>5lv#wjXH#VbBO^I$uu#R7=V;y04=$2g9UbD7dC+$)xU~C;B1B}fQY(x|R*g$Ru6H>>REHRW zK<5WcMCxwFGd}W-XPby<&(7q|FDr5Rb_7pY@F1-~jeQqCHL;K!c|F%TE+r)>uOb9{ zfliOZhHe%ZKsd~4H!(ZOM#fYDoZ&}uY3_$k=a`?lxrM)6gQ;S2r!Xuuo4zO1^wLmU zv(G6+&b76XusNX<@pw*5FBCvGF<-o%T3ifoYI6p=kJHt)Fm`k|^M}2&PzOZb+;8ir zN?FEA;VCpHC@iq;AIG6}^G|^8X93qFyWD7}qS0GWE=J8zg3!duN&B!7Wj~8QtV==n zfI0lD|FoetOc8pgbNg;1d+Q>1f1eNt2uyvvJj5m-NKA?nB$uw$`DuZSHn92I*Suq8 zy`k^qZIOJ1Jp77s&|E>b@KBWxCeh0P&)QNXj&|FIQqyljX$B01<6M%#IjS=3DAYLy z(;yU&6X2DZK*hA0yqJluTrAwp!y~x69KGGy-Bkn+EVL>GE651+wYb)v1^g&n)NJ2V zbA|JuE4dQ-cq`JDaZ~Jf^0vm7JNHt4;b@BL3V?w}L}ARSZ$8o_+qdK`f+$wa!AXpl zNanuv$GJS1;tUN9l|?7o+S&#$V0mL*Z-7k|na~f7^#d0U=4ozK6?PH=e6?=E)m8nLhUC=NJ#VC_swwEK=v>S0lKx~0E5|SHM?(?Z>Q_& zQlO^lmEru)=fEA+f!o3VJ43dAq(IFW&~Eff8QeU1(xi5A4H3`@7gtnIXR%C~bu|xz zdD+l_fY$6^Ua_sMtw4gIr>43LHPvC{d!`|I&FOsfq%YPP^qi~)g-lS0)q?pa#=FY9 z;h%14>xv=Z*uI!n6hgI7W-)^+ibeaA{H45wjgm(Q&=S8sE7&QgvJAH%+QNJ5aht8f zMRd5jnvq&)ju%ZHJ>K)ssFzbV>M2Zn_8Tjsk-`-&G{)Aw4b5bnyDO~GCfa?QJMq2; zUr3rm%K?mrr}Spe1g0vZMZD`F6a-1n0k$T`eC@J{v&`r-4<~kl17WrJo~u)T;Z7C3 zfxI7E5gCTBV(9JcL`6gvoM>Pwa7F!`n@B&PIvBG+SF*2{!pfJ6nVH!F9(MLeqiYI3 zB=%t^x;U9V zSY@tm{o0UJ(|HewoC<#*(PEuV;jJ}JNTs})0NRB24$z@*?2KhF{foC!@c)!=O;A$R>s>=VetnSl7|6-j^-B8C<|M!6fQTtj z0-s8B*4=Wd&aReD$h5~HQomWdTf+PC`Ej30<5!R^!8C2-_q4R3l9Fdp;$iw1$3`%s z=Y2EIuraFe7wv_38*pm8HL7z==BGBD9?jvGDNdbrm-;yo_tCq#m z&O9975~3&EKpuus;sw&iCMMKWwAOI2bVC_=v%jGQJ3G6w)>KL!&%e#de3zG7J}6is z$kjO4=)UEJEBrva`*DMlgz9W*S&%FF4#%7TjZF+Lw2th0H*lvkSEco=)d>;$0FkCwQ?gM ztIc^P`g36e3*pf+P|J@^(ioF7{l@5K0uKuv8%U=#D4FNJDEHKWOPYL-i`B6Gb51M# zUGtcuJ-LA3xO#b$*qN-x3!^`5b8zKx|3NlLilHAPP!mlx3 zBTuMQIbc4I1b#))cb%+s?%=WJd`bm#1w_eOKt^J#Uv4A^>PAIXkei#AmtO#+ZT@v|L~JwS9n)A!o*5NNjaW31ekpIYb+8A?&ZQ7 zF{3Pr^jWBcJ+U)&<4|F_b$c5d2FHc)(Jz$3%;&SQ)x4(LhZNtluefE}0O+^%Y*#eA z>~O91ouw9!Cx2Rdx~^zPc%TGlYgE%j8ts(`Dbh_TTjcVz@+2e z<+BO1;YfCyll#a21OfgYVTNjtjJdx2RF=(gU|EB}JM=5`N#i*Sadm37e)W8_4D|F$ z;~0>YWvp=9CG6_3w&7u*3?dP3syfr~xw{NN1OfsBx3{)h7u;4FF#wNoXDmw+Ca{i4DCNtd9qS+4tk6{+~tV-_MYOot_u~JHahz2tY3w@m{ zRrW*^&>@Sg2@vnSsX$J(;#zQ*bWBn2NPy$avI{Qu37Lr-8Ijv-Brb~m*gVNMi$pld z2^IYL!QvJxfP5q>=b3mfb=0@tz=QlECB@6B;^JyrSs}*3!cqW{os!bV#yWRMn4`6A zq_P6milHTc>twwqjM)_&EL+>=nTZKjS+cO& z%gew=TdS1P3oQ?D|BqwGtA>YB_##_g5P`r85}@mBR?C=s7mPf?}(VC#Gz62y`Vq;N+^ zM>8{OdzzGIdFs}wi>jfk*bCJ1All};lR{Vz&;(AB3tDMxauN}RZ;vvj_` z;2D*DhxPce&*=nfZy@%sxJn}EoYM#!NL8H|S^M)lVcs!E!E@v!Et5ASiz?{G2j4c% zPB!gBBms7z^i(Fv_nSbfrpVTFAoW7ES3}*7!m}U zUrRAV1HEolRuFE0$8jFJyP5iQ!4!}cCq8#Y2uOU^|4Ad zdb1N=j3Th~KgFQe!myAsnJMUR$o&;9;T_*Epq2>adUZ}$kLe%{7O}%QNKoEq`C{AJ z9bumt&U}#KJa*vXC4~hF%1%=}jy*wDL!>8Q6QuoV|FeF7$*c)9j5!)k%|1vUqG(_W zqlbf&(oJUN7pmdRnPEvy7PuY?Y)n54~;^Q&~24z;J>Z*k6@d@(Q&aRPx z0mf5=z0Oq-i|{&Me&QE%i9}J;6l8)>8bwlIZz(@)(re}B=T&I+ow0g%NxkSyp9FFy zrn_6xmC>^&k=V1WP-m`%zHXy*IJ~3}R%%XsI8B}w&VvDGBAD3MTxTdKwx=gkJT;e> z-Q6;Y`b|M*g-?+F^ZJP1e=`yp2@lFt&$+ofgK0N&&TQh3uQ4%w2Ex1T&W<8STG)Yy zcPCos4NGLLux1f5VrH@a^FD0-@Rd?0ZZ6%A zbqmZ5=y~PEizC8IzE8PHi#Q1bVagg=^OCex56{O}LR!&YmIjUke}-LP>{U7cD;?S_c^%yNL%=L>-mJ&^Esv;VS{E*F1w0e za;^p)OW5Z5&eYwwD1Qq*w6@)Bz4}hxW0@{jXlKC1PTF3_ZwFu z<WVaY~}^GB+JhQ;Uf7>D0_9O?IihAQ@(tr&UfBlIAt+`vOL9 zb)Qc8&p?c2Xc8JL#BhR5#ns*s-qq2Ov(gQ^pW*2`24b&bQ(TaeO^31@<`oyCCB?E- zIRq5x82Cro87jSM@Bqy>n3JkSluAFANxcVqde&d?9nMujAP^+&oQfz(3M7Q&OG9{j z*XxF&S;9b?6%EOLrG;8t{=@(>YR<0Dbr?1-3ifBH#hNW)>VF%}y${h4puAyWXlkks z^UKIiIa>hCxwa6U##r!>Q|{TuiIWqh9J#nIo69Yc!T?fnAQQTN%lFGe^tMB)TJn>& zmGR+Muh3_J9(i(Nd&^nxV;P12E-AcEMbcC!b}a7G%(PIO@-k8VlM;(vceRB&?k9`2 ziULN?T1G5&W^qJmj`m=>o0GSlEdYD5MibW_xvG2M5gcDi#)z@ZM=5_MnqrT_qW$e} z7fk{g!$1$K6nHq&rLUd>W#V#GWA~%MQt#i>iCDNDf5G5Omtt3JWX&h)vLT;M|}oz5X(Z)n;Sr7k$X6_{qa+5*bP!{ zzS)Zk3Eb;7d{8&+eEXh4YGaS~j>K*6WNuxg%LbjO6kl#Y7>;lOWDzAI{yRJpQdMPD zdK~w7cTkVR=0{55UT_Si%3Rq3fsAF%K!gQc=ygx=!?}8O zqx112HVjb(d**I9JdwXJJnuuIKd)nBqPjt>gqg7E3Wd-arP^I;nQeyC>5L9{_A!Me zyk#5PA$imLg5uSa;*h6`V8^;&!%VELKk&%j_xtl8`={PhY(6&MT-=)XL1F+_&NiMT zItn3_KkB?v-7JF@U(vZ1B~9RJk8GbJE;1^e=Qwn^?_Fo?5m8wkG=86~JH>5gDP-r~ zMd{t^P0i!B0HvF2EKY_sw#e`e1;lklYzJ=V7cFpz$d{#CuRIJmFfGHG+1MKD>+y=K z7@JOn$O6focyyApBYMj0&}2@_VACW;DzgtK=GaMDl!sJ2#VRl3+12~(YYJw8u-B1= z4ZK4wkmUu!W}x0kO1c8%RslaLEHKdJ?wTb~h?HjnJ&#nrJ=oN}T#P1#)gub)AgF6tPG1)i~-5`BAMMqPEaCYDjS9SW77+w!2|xU#>!k`k~^Akp~bI$ z(P==9n23ENgYp-LGk~n%r6~*p`h{{T%v;OI#NlFr8+MM3g|${eT?af72S-X!u)2C( zd{hCAw;|IbynTYoMdm68m1Zhaym(M=1?4GfFP6l4;jG(zPbRJ;EgD{t z`~tpAG*tR)ssg%ZA>f|SeN)rL~COv82s4GBtn8J7A1*&bF^Of zcr4ig4|nN5Ek;{>&nyfT0Nf81QM)Crqej_7525q6y0~4FjVRa)kcWW!8KLlvk zPfRGms2S?l5%bSB2Jr1p$GfE6&PW%jx*Jontct5nz3(z=yky{#PGkq$H2a;C`a5$N zYwQe5Y{uBs0mE4`4Yr4_#mUL}iFdKdZF7Cy!1(jKy3fPzWlw_1v&cJe1{27d&GYtL z3sBjtM1I7^X2itRJlPNFuUC2?khA7L%P|x$sV;Y$=HLKZ;)Rby;na+kw2YU3Nqf@I zn+xUm5{l|TZ0(ke^>yq^PJ3fYataF7;zho~w}R-YL;jk%B_&2EoL#9hk7kC#Q@F|& z^!qIDQ3<)5j)HOU@$ilc_V&!p%pf+A`%jGx6j?H`DK-iwQ)wC14W!>w7}A7FNphnR z_V)F4vkmk%goNyeK>}zZ(}vFWYTHd}f{`0^wrM1#tmc~sxKN1qch!;hBDqCAQKL`u%#@^kU(#hEb5luMB=xPC@8MF3LFhLcVMa% zP&Ng)bvY$DRxL|g>u~L#R&(p^Lq17z(&o~ziqL5tyj)5nDvJ8w?&CYa3_;RK&t$A7 zAiUZZaaC3q}}Wa3MCn2T3U#Gu@waDA=rIq z_>qxOs%R;j1blX?^&8cL{lC}-p6^twqEYH1Gs56zCWj7(Gn_>!jU2`5=Qjp`d}0Xd z_Vd)~5bufM(FWK3nOK^?>Lg1ykKvgw9YGO6T}=&d+Su5FQC0+%e zg_L#uipqR~^PA3bb~h|WpDpc!(bkwX@U{+udS`-0ShvMN+wpieh*n~~^C!0%=flBv z4g;@2g9s2&~ny!%qWwFuIQkjd3E^%3kCfa1YXdB^A(y;Z82{`g|(cDuDd1#R%QXZ4VA zX{n#YP3WP&#SUlY3x^p#M2Af5ry5dM9CN z{^4IE)Ibw;-v66;7C57J5}rWg;#9+=2RmNrgIuyEoIVfil>A=aH=jWHs}_a2FI0Ih zq}JDB@NSA7c&!eDPs0=A4esmDJ_{nbb08-j8|Dt3AnRIy`u$CM)T`iCt= zMZ1AgF)Avm%caYu&=cpkzB#|2g)_x-%!t@;S-QR)z!F5Cqh~a&_^oe}hMG_azNjl3 z4pN}d2@LjQbRu?)F_E&M_M{&*!;YZ>i^Yg~JCIJ++F?;$Mdi2c0wP*`LIM!8ZvHi8 zf0)gr9;3yUWP5)IbI~nbz#`^IM%BHwUHR3g@V0;QHPFP=z0qyD8otA7H4kiUgZT0!J=P{}O2g3cMsGP6OjNS_ewPa*uj~AI~;^N}M zq|HV7+-@$FzJD}kfQj*0P`{(X#EFvsG*|908(hgec6w7*)@8mR zBuvuGt-C7R0f~v4apx$`)WP8lDAspici4mFQp7hi81|J#1e(>C}b z^1{7IagDmm)E~9FfT1BJg=%NUakkdl*c&X&l11xr#r*61{5$~zqsv@`uRxb9)oj|Y zI#KcaM*6g(NwltCUk}11!nLBt{TU(uGd5MZ@ zr8ZxdblXhi1oS2c@ODT!mr|eV5x^coeBWYcvG|M)0S*_CGsMKj0j>1e)2Dz* zcnUCZO&?Q0fqY(Le&VP3RH3V372jk(K3GLq&h`6cmN?3HMQ`hD{Aa4})MQ={Gbx>$ zkQb{g@kEKv_4n@q5JJBgXI;`OMKv$-0@AGfxVaoAeo+zOxUwF$Z9Be{yn}rLCv4|~rDuFDekiyW*ufNpX(#?-<7k{-W#}nIB41P=#K0+v$E6n)nWF1@v ziY@|P*L<@_f+WQ%&WeDDh~E|UY3TJq%;{j!Pv;3|-O}^Q$B~UQ!NTJcJuY>o3yUSu z0TLfp$Z1Ql&; z!iBd7ZZM(7iqMI@ERhJ|4yv=u3S+f-o=X2e=n1gUX1BlLoVPl&nvtx00w|xH_{Yar zJ8CE;#>8Z&9#;dT#H#aOAhcW`la5}W_l>+iQg_bLMIDF8T|d z<%5auLx4RCY6%94noPVun~rbmrRdQ{`D%!@iltdJyYo`%CD({ROUWzTy@_ak^hGL8 zi#TxKK*{Q5N#D`k-PhYI4R9?xV|ll?w>4Ia2f}~$Oc`JR*%cL;Z6L%+@)U3kXeKJF zI^4$>QR4)TT7EWYagA((Wo7;9%W@e|$k~f$5nFgr;j^%ywW|Q{SNbu>o0=1_dtP1z zw+(A&WM>;*5aH-r2V#Sk5|C=x=#|xhFx&M+;d^#9TA38?_0-O(4o-Z0{L%Q9F*AUB z0?VvO{wk;?YjP*DOQy8|{O0SoM;(FRzkjDuF6r#11a+?Q$;k|!3*$FjYV2DuTu1oI zaX(ZUdATEpW&uam9JuS>vGDek9s>KVQIzA;nf;6~P4Qb2>Ud3f<`D0{>$%AN_X4D~ zs9dSHr?I!A!kyLTVkJ~>>+bjq*!;;6ma_FNH%%{__>}D#(7^8|DJT!(*eO5+o_#y_;&lfPxYZi>gk&ZCoGTa>cd0 z2hNdk-++3v@HW9LOqFa{j80A1zWtG>VyKCw8YZ{!L*(fxhe)D(UEGY#x?qtND^){t zRR-lYH4=zwn?=LR-4s&4MwW-CXag#MO#56&e?Lg0rq=7~uMvm8afrMwg>%h7-Nk@X zo0^iOr8zh>xigbO5+57?NdQuKIw!B5gH(I40d!sh=g`~R{4}*JkiGhFs?Ac`0`#^? zKyFF@|BKVnD-s@ZA|xj-0ST_d0EC39epvRtI_HGv!J6+aqk8OXVY7tnmC?;1+{)xZ zk5FOtS0{AX*B0~r{BFWsg(+0;xgo4{#ZKX*{ey#!;QEY}m6e&fvaHPI>f~+1_a8oh zez?8+8h_yY!^*KXmq%T{xd$X`$wTc~#};n1$4+2axs#%iMdDCSA*V&oa3)hcCYPa9 z3`-Wn7`S(XscE1~iKeGhEo5ezBgIntE-2 z9a3FPR0awz=CkJ;`|6HSdQjuavCW|(3swx8-djSxl%j%yZsi-(oFVsyKVSo=&4c+4 z>>hWtREfS`YelFi10(hGCc-^LNX2_RwHcgW^egdy`^uIYKwk9cEUq1y;D;!ytR^WQ zr`3h7zg_^~ob9;RLBtc+>!p`gyTaKL$h7nC4Xj1mVYoaVh9N!^p`#j~soKaOdLf~| ze_N17#XKWIhf$&tZ-}_Lxyj4RqiO=C6a9!$$K<54j7wGX5{XN4Xe85*qBZvmQyx;0 zA*8KS9;=AvY*JII!JJ%9kM0vO{w~ZxYAO3yIFWTWtB8GVlLkd&-@oqI9TxDiRJ`V0 zfB-Ob!}*@9f)YtrEF-Lru$tZ6BO<2DAbW9U29MCr>r4&Vks~iC{<=p#hg~x27l*f! zk|HwNv!eZQaOytZa!}fhii|o2yG`2iU{Dh@)U_vz2Ea3eiyWdCuXEF~qzXHWbInXO zpQ3lO{hh*Xy+HhXx>~R6F?F*2iT5pNvj`vXGyx@L!S=CKN|o2;dl1LME~-dJUp;$Y z{bMW~`a!HMXD9?k?bL5aW3^6R(J-Vug+p2jI@4ASu;Zc?BCu!>g@MSz?0w}YoCWH^ z9DVXCy&4g62G@m>xUKA4=O~4rFcd)sEtjh8L6vLs%Sn@MvE8w?vt%xpU$)-XW)Rx) z%zG8IZ3heFtZ>%vk`(^hjmD*O0igO0ishXqo5e7v!`v}PG&k%fp|np#Jg${G87vLt zIMLA?1(-y1OBh+z+E!@OXUu#MvafLQAHVPB9_pkfz z9X$<=S=RQ>lZqJH#H>oKgLir-p?C3tH)AQa?|5xhTt`*rJR-htXlSXrB%@x-hvCYg zWaSJQp;9JBCL5D9d}JXdeYB6{KANH!k!SQgUjR8@LajC)(+3~x`TF@j#IT^>VEy*~!nJtF;WPDQfztMASV|ce?@Z zE04g7jjvsVuL9KR5RfiJ;Rp#{_F59XJY_K>Bg^=Ru#SL@jSW)N_o5|Cs|0**1U&t8 z-Xhc=$sfG=1{>mK$HrI&{WV+J{?-%z&n~quX^ggg?{9ylXCb8y@#jwmgsbuK@p+pu zhhdXd9np=uH3rP+wi{4B;I|)`+{9Y-LmdwK;_BvM-- zK<4`c=)OKX^LR+)90c_vVlL;@^z>hlq{Yt>?UrY5H!LZ8ie4-Ew^L$tu1Rap##LGE zF0+JPUeT^Z zhrXbIm(d+&!_iV&*!a~iJ0>k6V%e5V(E-)c@Ef0^Jq%_gB4mBa)m~W{%UbQ&Ap)ub zux%!zW*fMo%yd&@{l*b@7CQ#by56Y{EM=vi@lTNaL2Jd5`h%P@-2O_hEtZ`!oX6IS z%^^Nl*Xwga2z!W%5f%&Tz$wmOBLt;L5LkGrFkcgor_Y+eq7zyu|7Z=eJ1dUbe#qd8 z7@L}D+fE#ro7>ypx2PW(r}adJ6sfgIPah3T;a+*XH7o3Xf=X`wrNwYQ?mCb(N(mK0L0d0 zBC%%x>z1nG=`ewufUq7@LY$;4B^%ph=YW*3RMJm9PmkfTiDyr_WV?2zU4=|c=-H;H zG4Ug=2>6^5j?M)>ZmBxL+lL2%3MQDjXg;G~-`qTOw9z5vBc`J>ggmN`y3gzeHF3p_ zg1~Vb;VLr(``67pe@;%YZlKYvMLEPBm%nB}SXg*#@Cvp)3UH+p6B7V?0ysOsq1f8m zf`k;jV?#qj;P;ULa?k7j7EtT}r=myS7K!j83Uph+U0Us=I*3G1xaljbF@NXS7}5k~ zJ2rqSwjJ-~j^*l<-bFu6gtQ8N!w^KDhL}|*{Z2t-$`N~hGPsTtkwm3KO}AOAaQd@+ zfn(qa#YpBRzW5I%#5yBJN*QY!( zt|m*ZqX)GG4MeD$h6eDcTQ@tY-Dxr@?4jNwQ0lO&8=Ab_m+GJCK*z=AU`n_0zf8gn z$T(g+pI>PPWLQ9=0|n%ms3=S$FgzAh08MWP)KX6St?wj)P&SbL zi}K=fyTYtm?)+lnxLnEu0s`vmuK{8X0D8a+7~bdB|1&rGcA__=5dy@aWJ}*?0UB`fVP-h!s- zb%n%z(D@234~mK`z&)ggn0YnIlj3Z;_Z_6-5_#1ZjI9|hp0dw&D zsXurjmCoVLdxEkk;(QFfdNs;Cc{rfCF13ES^sZ9|Sms+`^(RM1#>U2gVY;@t+3x-4 zmG#^1^faqx-WZ^Uju9b5fIq>(F%2j=09*!ESyoI|U0d18_e)qrab$MTj}REwvoaUm z(;q*6Oq6IJZf@dWV>=woaadSb=;$o>*n^HCVWTWhZsN2qCPqdW43-UGsSY~|pxx^C z@86(LiaJ_NTRVIrIWp1~tUKu8T3T8H;7Q=%G7e>xwE>Y?Q**vyc={MyO0=9>YogK` zd-LMOOnUy#jzlbdiI!PfYUW&U8r1fAkT`Wdev!q&VmL6Ej*?VJiB?N>#iFFSlHWa7 zYZN~}xEs6V)TV{)ae(`}6+Ab!+GnY!$j>$A=2Q< zrPruE0{>7~SEqs!dWZ?;{vTf3Q+&)b<%v{1t}M^1@(-lq4F8lj2I>G{T>v1@@7p&5 zUeCG3MPd)T-H9Twq~XN8py5#`{OwjI`V#HCch+`xQ9piw(wqk{_7*g+u2jJE5&<=H zz+7UZq|9-Ovkh;|vpDW8{FrNOzCF76f>BUCA|cjkWsM3%N70JoYirmlh}r$ZG0h=C z@%35b*Wfmj*WDG)EBvqYDy_YkPX z@yR)8z@c+~SQwutoP1wRDr^w_m}m|Wx;2^{5m9FMH$s-r1>Fv<|G9w2-f-88$n)qK z_$`f%T=u;0>FDl(?V;f@KP4u*@JaEycL60GMn*<3t2PCLejo#PZ?~~&n0%6|O(4-V z^q{F4+6?)e1+a18KY<|toOUZWe@O})9GuC?Nw0)cO&!cDXUp}Z$ZJkjlKn>5(TY$B&@(MP2(g+A0ZtlR;I`LvR?RhXfk{2oiHThE z%Cc#FBdeB9c$!%P!o#^bpL3?>LFEQ~ToW^^@S-1esc2J-4&HSjqdwSL+pDyrw5zi@ zH3Z%Xocw9w;(VV|JL?_H-oG#X+FUbJ3STtXHAqL!T-n%2pzV(e#J`R%%L*DGcEpYc z3#%&|8#^1#53OyN-w@pYO{_PAajge8&cOp0^*3laiEq#;*0Fg5qbE^r4j0YI!-Q1$5Zs-+$ln%-{*8ec}N| z37jn`H4z@4I4;&$flNPlZ@y)5cJktv?@>o)=16(D)rV^^f$`@#(*C1+j2y}{ZwGR6 z;CTS3v$XUG{QVYHSU*a_x*j+xf$Ic2fGpzU)F)&2@IPN<%eZoI>8t5OVsE+9^LrL3 zO_jwMvM)>8lkt>zH(qG16EqqTOk4oxkS@3gOy7#>;J-h(i16nRvML*bi`}@mXa%NqM<=H^hE?4>5VwjzO(dt+--iZ6^q7&|D@1n2x;oz29GU`Kjk`!(B^`y9;JlbmJ zw>%7>vv}HH;HrERn!Nk;@3O-^V1cjfGIg{GoB|-j4S+}k9EJ~(Wkda?HqbvHEbtXmDBT^$kJ$y#^gK4Z#eiylGj!J&{PxR+V5S+opU9WF| z+u{OLQxZYHrHn-{)V;EAWFjHoBO>a+M}lI1h|iyo{2KWxAADp}m1Aa2^HgSom2{hmlKuj`Pu zF8**l7I83V{Pt~DQxl<^JE`X>Sf<-|K=DHIY3j|(e_fW~E2Du`W0)En0CfQR^}vfqkc*jPp5-K?px$mg5)a3eA)__PnN zo=89i`TzVXBp|25n_X7M@ZrOEfB($}i4A~$L2Fkj!!Ql0ktq&r;N z50-Q6V6zH(?1Qe$|BtAzj*0^P{svJ|DV1&&0cmOJ5)df?>5^_x89*9Akw&_tOF%%R zyFqdW=@zM>C8XoM?Du!hyMOH2J!cQHGtYCMdp~vA+1Zo$?0$`mklejX!D-wEml+no zKk>nzB?v?9T%62aIB!g-!y?MriIhj|>>$wtmB=cu2COahn(qjO4{d#YJvKHLU(kaP zF-GQi>mxKbcZrE%dRtyyt+7DTC@UjjS^2`!Qe>B@EeTP_4oDQgY7ZTrAr0!?+U*ti zW1ho{{EC&Fk}@eK=40U zpTfr4OUGhSQCMs$ms$5AoD?XaR=*w&1$_DPB`Qh{hEga(6~fk8)zwEp-r|t3#{Wym z%d;{ze$W$Ao0VmHaefv;4~!2oRV+OAX(;U0Em;?qx9l?OiA^VvVmwoOv)D}xz&`%) zz!DM`V@pUh_xH0iFyv%sUz_CymX{X&!o9JsnI$+{d-l1R)9*!G>ibEFiEsrR9UTV; z2SI?5l$?Av*NTlGll{bnKrztt0#!Ts>lYkAR9?_xdX+=E`cqliOhQ@EORO>%iD~hL z9?LOA4HN)8gk}*8d?mu@oI<%p+12UDvK2cttnx|&jx?HMv`C-(dYX5dGU+)MG$TF$ za5OR@l5}3Ycx7cJVEXg+<%b{g`SS}cEi&ie$xmcw=>uB@0~-iuXFi>Xii&+$)$AsS zpU8QGqP*3zF4umhjtLfE8r5qRUxyCSZ4V7g+NnTunw9k-Lhse&4 ziz?xq?q$OTO)9iSF#1`Vtbqqm=W7HsiT%k!q)B(oFg%TI92^|>9Q7CnT3}d&z6fgt z6&Xy*5f&WWdIb$!Hm6JZq@9G#_V)Hl7B&jxkmrkWhJSlJZ?1_=48v)ckdUx+@b=SN zlD)!zKh5y0>EJ&4UXsr6dW^K2&iKg}Wx8%)Y*`l?X znK&x76xEwiFT4jCDB%}J5V0{cOLp=wPs5)B#2UP`?d|PAOE&BvZEB=5FU%q%BJL0n z5a?LJxB&{t)$4-kY3%9ej0_AJw3|*Yv$hW5{l5!fIJ7W&-yqw03LC_UNk|~5-OnK| zbCj3O(wpHWPesMSEARh%ftwC)TtPcv;|PM?kAa!FTOiq#;o(EXT+1~N-7~CdD>3hL z4`@#Ya}+6gEN{n&wBb@cc>y9bXdg$xbVkXK>UdkKTGopcy8uC?2kgr3@F3zjUYJl$ z5r$2iC;vtb1r&-x^TWwFB~?{-;DLt%p!Lt!_X^xInL^kHUSv>Tla+OKRIW@|X9aFZ z35MI@-#O^q!6%kB@JWUHilr^v^r{s`-(xs2OiXz|{QTqDf#WpOWq0qyJSszejq>Z^ zY;OsW9*(O+a`e&y@nKbcue&r?ugGx4G%HA9sCdFhimx;RS z43M3}b0F%r_Z5by{R0+41_g18SXeRrrx@$xg`M37X!W3nWKjANpo9V|Bs}}uprd~K zR#j7zFiFJyob?VEZy4lBwOQC5hg8Sy(#j`?EY}j%mpWv6va++|SjRz^0{x=f-Y2H; zZp!)}&p2#!+&DA-Z=Wqj0|$(lLOc|Ou!?qk+!gvkt@D6zV(n%baT7KK0aDsl+pj z0h0L?A3x@cTtcRDJbH8pTjwADnuO1sf|An0!U8_oz54QU-dIr+5!s$J15kZ+b$4g1 z!pko%W@_3A#R+h-rhJpGovJtU-|ZxV+4QO&Gc(_sWYZ!T;0JggI61M4Q|^%)92^d> zUfqj%o_z;?CVQr6@xo82LV|)dQMbk6XT1B<76`J-kUp3-mgnZk`E1EDJFqL~E{?q&5&MyMXm{^||A|-J&~LN8FwEB+|~Li{HVKl$|H+ksRC1 z7wX>cJtwaY5nsD{EoXaocWbZX!_VBl0ZXk@V%$EIr#2w)v!`bb zfJzXkp#O@>ex6m(8*<<7Ex^f{P32}c+8@g`o=-tutbJ_zKpGj9!m+44tbaOWWpX&9 zPr|^0C@Y7Z+E6mdb8+#1QkUtrJDm%}2>cVFc=1082%wPkP~4~yYKf7N896x}o>?b@ zahN3@IK+*=GDWG>L`Rvy%gEv2B74@}W;3aY(r|(!YLUIQsjmF-s> z9}%8rq|U{i+&au@{9st){d@GePce$xqR!N2DOr2y{MZonP79Hr#^ z^z=7Ny$R3;D!KIr)f}InolpxohrnEmN|QJgE#UTI1zQ z=+^d^Q@xGrrEA}e|1g8bb2V$0GOceG3K1$@$~&X8R>~ijlZ#8T&)8GE2W~5^n3qRy zJg1h^9*ONtU3Gdo=9(D0Ps)=w5BTb@^S{RAadUa0{D|n5$)pPHiwnr!A!XC~V0Z7h z2Bwt5!*?E+?HmD0<#EopxmPl-N?6s&CdR&DLG0}AAqex?8^8O4jvl!62P}mPu%PR8 zwKNr9&rqe%2Ooz1ds=CiNV~vB?XE6Br$V=t(82!cV^8dJR=uhhzyv;id^>54wlS>1 zAHNiSN8xvW)1dDH__?K*+(@2;k(tFXE>7$Lo1KVvLrp-y6WZAaUNkIoa_?JAws1n9 zlaO@>q^I7*SFRkwbDYh~&30N{kXt`Uq*~xDOX)4BQP@;tf3uix4A!<+pDk+eEVl;?rOA3Z*xGyC02%+<12vhk}A) zn|EA1P9{~$0?NIOuS8W#3+SW|1prsTKL+s4P1oWY z6Ym*q{xb|T`fih6b9{Vyumw~fjFQq*HBQ^V7D1*tT09#N7AWU+%p=5@o0H?unV%+> zCK??ZE2=2xdb)74c85=-zM+A}FbZ$)din*g6^G&7Nh5ZsDaz!H3Ck4Lu}?B7u6w?N z#=B|fOfvN=Kxtlmvh`8tUg<7W`UWr!2(oWV9Jb2?;z}*zbvm{^4-yh!Ogtjv03V1dv)V$1>Y@=N6}uVmX{;rZ)})cEB%TJL9g-&pL${t zBasuEr-ll|@e}~h1OIohHS4{>qF4QP5&i{juB)l(((>UZyi@Ve0KyX;92E5Z4LN#b zu|1d+D*RNqkIdvhIKngnZ?jxn+@=9nJAq;g4T4PE;n9(wpI^z^@2aXN@Zxne05v0h zg15mb<3w>*foB(;k}@NHe3cptUHbUs_}EyP>*?r_WhGdZUvb=bTF?=* zB1#?mGgH%|H~!7}scT0E_+=*K=&1(QtG1b2msXLx1`4ZreEB zO6>rddhk?fKRe?AGcqQchUt-BY>db~`fPsp{kxc=BF#*)A3CKu+fjKBGG~iM55AY^ z*x84L4O6|9K|s#3tze?s(Fd@H&m;}ftJv7sXpMaY{tXm=f$eQWxXHI`L)*!jpJjo9 z12>j(E+{=>8)WO9SXUrvNGW*_aWb+oNfN8k=4C9cIoo6&wUH^ittVst@b1b>l+|EN zK>)Rk*~h%`#U4#JPG6dTyXR+GHc5(b*7fHWH!kN9y~2Z!A89_}XDXuj^}=oy1vPqO z)O>GNpSxRg9Z)^j?FSoefl(FC`|0{%=dj{g41ch$sm~ z7i47o0nYM&cO~C4n+ADb|MZfla>hlds}IMrIVX(<3RmwPWFcMOcywuAbJ4sX6j4K0zaD?B@pAnCKojsR%ti=c zk$)U<-Bp_&_|pBY0WNv6kJ7c|kzWJt^m-x2#uZ$1U4-nNk#VuH8x!@P#%^ffzoR<- zyG_@CKZ;^pNb9_>_cn+HSVBgnl>BesS~ShCC5sp9H`;@GXbfLNZ+dFBk)v&Rm853% z$tG>uC5>{MSkW%|_vaUXh8Dp2w(gt^B8ASm)92C|j1 z1a&i$#Kw}gw2Bdv9@v{=$bS_hPe>5@@+Hv5lzo*VG1u3(>h!0|qv(Z&T$@qtyOT%9 z85Xa|obOXLJ{R1Ro2m2`E4jsNskt>X-(rvE!M4 zZg1Bgdfv>xOH5suUnBu;BbGkRTv6}-#dd*XJvt zg()RQJu2;|6NOyt!+$NOj$OS3JfRqCmBw>ubX@yJ!beou&>(vMesf!!=^lFE!Gj0) z@82J5GD1xHT>GK55VS?kV#!HN42t3*1H{dP+bwx&xq#f*z*zh%ub`mk=TG3k9@Bt& z;_oXF5fRoaGL`JI;WTE#nFMSmHa5ZS-Bsmj8`39^zp~^)h{23t6e#d(Yz#d=@3;SW zpE;Jx!qhY^4rKrh*Ky4G2dTZ)u@V!Pt(nlGL-GQ+ftIo|3ERuk6-7I=`)apN(ErL@{E}OMZqvKc-=@d9h!M$s^DGb$D5d|Mv~S zH`HtiFfY6nz^HJ5R0?}N3nN}X*-(e>Rc&sEXY%#;Oi@-rK}@%9#hIFZYil>Vc2sdg zG2k{uDBee}?1yf@tQVh~&>=L;NbA+hRg{-ClRXXfC%QL$+U%XKp>lmejt1g@qRmZL zSJ$bl5!q&E#NwIUOM|&gYy3K3Yh?dgiLYB(uc+KPYBArJRH;ijJN{h9TkU<|(TFPCa`?p|ay$&Y%5OQv^-FIYUWH7@U$!92rg@(4%a0e6ya3VVZ zo>EX$EIB2O`24xb^MucY*7@#|&k}|c5^7_$d=*BbALd^3sHHsg?cw`m+nbm6Upef; z-8a%v!1Fy{~DhG4ii~AN50#*%*YfB>4nJ-Uco%TYWYx~bs z69#qfaEGsxjGJ+o*Ej!XXbm(<6=#_)w~33n(OB?1HSvmC@={-5jCO2Jnomz!3E6RA z#vchLrjO*31y>mjNhk(mByjM<%XuEFYKSx1|GCrzgMF%YpbeY<%yMz#Q)vL z#sXB#x{zZ}h&Misu$GqAA@Ja1 zA;VfeUWb2;mM|u|x&qBGLcQ+Zxzk+t0JOdc%0M=`OtDYh{rzE_1G~UeR@#oMs;T)| z_krT2#~a>vs7gV@#-!qeCrF=~O3xGLC4)OymP@N+2V-IYZsNg-yK(E5q|1#qysH>K z=W0`u+kFiB%=vkCCe$hLYC8^2R0qbn4%%-EP2ON;ofq4@ACr_}Fu>cxE$U*Pg*0L$ z8jdlqakx|C?KLAf=qft%iP=q5O~UXd0fFKD`X|vBqf#yNL|1dR;%>XMGVZEINMD~; zkB%<-*+t%)n9ax|qJ{QWKKw+_jGWG5_HG-DS{&<&-4Ao8|JV8UnS-_NBU{N+150}= z0oPTc<;!ou;!yc6VXb2MF^7-WMFVQMFFC0>{nqntSQw($-y{!Qn6yk{As1r>onPp= zvm>BjO`0af04yG9J$sHNlIZB? zsG^}Rb#fm<92$S5h*P^Ct;U2FQkQrdtBei_dQCMJ6`i0jQg5Z<1P4tWY|?2&q+?&o zs)Dr7I2a@^a>1xMursx=qent5;<=Za*poLoaOWOZKC})0>_XdXqN6RQ4FokbG~7lIEnMB|KE{mzZTJL?~1$w6SyK?y8n@Qg5+)AT77jr`c;7UY9G&DNecv&LC z=3%@6T4%2HlfzV12@vJAlAa)aNlBqO5TO-#y8s#sx9x>TQ$$oC&1z-%CO#=-J@mEN z51U&OEU+pN`~#z%@d~#u``Li9ObFR~HXjn1B-GH*)4HEB>HwE_3P#_7%6vmX|#0(iK`aMXMjYW_BIW!ootEg|{k=Vx#pB$*qF3QF&0$ z?F2nPA)pc`efY&&4qm1opxZEZ`bM?)WN&+((Sv$Z_}$^_e~IO7e1R|;G>`P3!+iRs zHYk0-f_|o-9Oibj4q3oAd0{fBLO-GFB zGY1V61L%UNo}dRlt#E5Zs}JM9d*^XH)Kgg;b&mK9e!{5`qquZ2qBm>KonNN(JXo%J zD>oaJgpK_iB^8ls#UVX`T){0(XXkx}aga%v{>r?|hS*PtZ4Bb8rPY zVz5c22DG*h7fy~(R^3ROonQOkam2nnj?=w>Xfx@Q9!MrN zOqbGSqo;4bkv3oqZ64bBV*~{q#JGhdBtykcNWC2(aq+FF$l1k3b~1o1u;Iw)SB?li5^P2AbMwAC*|*!me*=WV zQ|cxfn%@fx(aFiv!MGIE85_2C)Y9+hj$}k{Q&mg6^8InoyGb%Icjs-}UV=(dS2sof z-SXRs9@@4)qRQfTWBUXISp5@43qaQh`l`+CYl$wN|SVTu3S<6PFecxTfWk57rmy-c$Ss(WZ6_|Y5WL`@nn|}XV?D;|vGL4M_r@;T84s^veXV69O zR(@q`)s4}e5s6=bbr~BI^F0dnLMD=uh=>O`Y}P+*6UBLXZ=9XYd+nR?p!ONyhP}d1 z1L>Hlkx_HJaRx|CFVV`=ZS4J3Kv8@9`UdQ1cG(m?JcK7&L(-`1NGC`-e%s^L{8my> zc&VY$Q9;cHS?_z$!`nXO9l<}oZnpzbLNZ}gfoyyr`~h%XF-4eM_XMbP2)7KJmwm1F zTH(u+fzD3sO5E#fxVy@)G53la&ubi)mDY>11|mU?46HQ`2w|1?Tdqm z8&1wOlyXwKi*LCa>W@69r}2Rj>BQ4UBKNmOfc@vQp;!F-)xv5VjMB?LXUw2S`(pbc zvUq2?eLK4|FP?&ZZ*TqVloBn@%)r3+-X1_&pxaiQYi>Tc_0e~N)9~S)ZXt#QZ>?w) zM};I|%(Ww7(g3koGZ9)IW~=AVNlnelpPJhz@Op-h;gWW4wC%nkET4a?QfSD>RQX@5o>cbB_)t^sR>f< zbZpnnlYQd1afu-JKf=EHozm0xRn_Tgxy`x^+7UmEz@27=&&S@%s`}k_WK9l9$;Sp} z7R`j&TGFmLtfpp|E|zSnC|bjaA)<$km zkzKDeYMY9;>f*lxdo+t+5ldXL-VFjV_w&=30?om3VbsnVmgU_%uD@+^&W%T}*uSPi zM}QW~OL?E30>QWb*K09Ps&sN#G}}8wep8W~fE@Z}+JJWxxRU@u{gd=cRa<*yabEr# z3xt?#tn)h_8Q#3Cf<6!`GJ7RdrWQ_fo$D^y%EJTbJI(`B%)@BKXv({Oat_qwC?+x7 zzyDq^SZJj1>Ctj(i8n}5uU@}?moa`x)Q92EZE=@hvj?CNy@o^bSdbeu?{FY9u2mgP zngTl`Wk+4+o{jgZ^jWcu?!=A3*B|`-pFiu3)A>p3q}{MUy|!1)D#gL1I_TAUMcYb( zK(=`(=}PsRx+4N6Z%0Ea`h z^ZP22jkZKz((e6SbQ#fD^kedT+HRyuu9;uU}H8gY1hgj|cC*e(k|` zoT@@T+h+<23t#>iK~#hD6Cpy~rl8Qm>D^_HzfPh(U0r6CYoJvfFmQ`av^KGk{}yKR zTu$0)k$D#jTckgk02o%Kr)?W3R4Mq#SrqzECp`k=FJGuSJ6`50d@%HMK{@qj_rz7p z36Ni_r`}JhtaCd{?BS0{1~q@9NoT-@Nx|!}iqORn{%PPL-%j>0`LMGs5?%W8+u)%6 z{r)@w=`gZdPSSaHcF8#(|4RZdrVBZF9x_#Ss;1mMJQ(7mmk)5%XXh5$1a_rGs$DoD zdn#qL)%)7}k{8ufIrZt%YHy~wpU(X!wE{sXC1Bz~pdF)2UN8{526>({@3CsmM{F=K zUNp)*m3WDrv42VW@P4!>faqBUh4}C9w*SP(ldapQH#B$wi70X_4b&y%)AKZ+V2bu0 z`O_7}n5&q?_xVyW39mQkPAqT@WP~o+RZB(_9NaWyONiTBV5i*C`NCX)4Za5G&vNbi zwa=h#$y5Y!cdJWwDdBXNmX40G>w$8r3u#4XbWBXbXpaw08fXb@%*+6CC+?a7I=e*Z z7JRQdDz2_a0OJoJOHwlF*;rXop@W(;1JDX`cvxER_CNSYI`T3iugdSXpm2U(o_0}U z(!O}1aFq1k7m9+#xgUt}yGBxq?+eZ)$yGd#2;S||?lguC>(r_m%^JAGA~)w%U%Wu& zs~7aAh^)=eo0yw-nEihClpzX+E<1K+M9e{4uE)jbjAxVdn+;E_#%6&cJ~( z8wH*>Y&>}nZalcN#K5#kV;_YD#XwcP)o6-f-@% zyqRYppxi(Dd>>>VB!L4x`8LH_#Onb)w|WQ(ry6rXQ|fs_U@1o`u9omE-F5Ty<1f|i zQr)cVdY#&S|5_4VQ=GIFtZC0q{FUX|SHZDi{U!6A*$G&2FU00hb957Gi0Xfj)fEN2 z{+O1uRm-l%TeTI>{^d+G>MfLLQpnh-Fu^`hPW$jDMa+GykT}74vY`IJVq*hMG&hUEBmj6AwXe zN(=4euBoYcjc>WS2fBR)vi9fmm9t*E z2L~56$ZQ4ypB#5d>s*jp^~ACf$Gh@@9t;GWOAzHc$2D}{pwwIu5Xv!(#8|80{9zwV zEE!IJTyn|U{o+cK)X54vAt-jBA`YN_s;8q<1D0)grg)|}_=YNe{KyF9qY%g8Kcn?R zd{O2k=!l>aALaD>u@X{{pMR^>|plD9uoc*a)0FPPxW|=T4>)nGJAjo_X=y) zfe~Tmzxar zKF3v}Q0QC|W=GH>M}QOn>)!~^6x?P-1qHBQn+4Z@Wj-cKN>6Wa`kCt`H6cu;qlE=|itz!Du*b>|v#E)z3KT^} zdGyW;T=i95_KtOk@dwG}wV0TW%YDlubg!Jpy@E}iKc^cIa1!3%oX#yu?<#yB)Qrdk zXM%kEXZ}rBI=VFat0+YC)vMS0Q-tDbi@slnlV*TTpxUWyJNi+&QKAHyB4GO_`nIX^ zOVyblP6Z!2UaORQou8K~w1*pw4vnN&1oq`v=^_@)Qk)XiX(5%dt5EOn3}#tjGIga= zvmwF#vY6ZAanaFwRqs?`y0%Tj;xBxggc|;^j-z+!b&u zK+F))brj+{SU{e>eGg(S0)lYRYnH?eTKyUvge(tJq<$+sCdNY0%$p1x=BX63WVt?M z0a2d@dR|Ui+A4qlr(!{;+Y{xtKd3FLEr(2FiW{jpU#cxS#yJ^Yedm3?K4Lh)9h2pA z$4fJoQfV;lptZMGAz>B;^fuf8z#q2f!^Og=1R`gOrl+Urk?-zFKv0*tkWi*WqbO&U z$!+c%iER904qJcQa|u7~U6`RiSJU~spY5pm{I2|b)0dI3^OFLZH)5S^W2<3$^UnjW z3mIvrjnKUoJI-R-#aNBMAayip;F;#|#9({H@MmmuDTTqlPN2SO#9z$E+4?PpB5k5Y zf68!@c~fC%cVu=fr5*nYCD~i=r?Bs&YuX2csrZ&PnPJK^vzgOtq35n7P9H|T=`->@ zGxJqigGlcICyI9@q@8iogR7Hzdi16Z5TNINuod_CnMZ!Z`5c{WmfYJs0;%LmsISl8 zait_wS1$zmNJF+gm7s^#deiF7sOVU>%86>5xUkKao632tT)N=%G8uq{;dI=Co}Hf{ zJuE8fh@Fhj$E%%CeDF?KxUyEgze_%e;Ep_N+)decJdj;@xD&%-y;^tkM^Vc8fJrBX zlE~-_H7rX^Zjl2GMhmx0!ovS4t$)IJAO7Ag6*v;=$FF=YYbp^M^hpMF?;`BW#;(TN{#YQVmv&Deb~^C=*U|CO3#>$#gO$Ig|pY zoY!U~WIr29>+Vt?q3DP2VFLc;y<`?FECuj0OEK+1CAuOu-sCG?y9&1?yT? zMw`7wlK;SLHwrAz=xE%@a)IQY8@Oi^d2Mtm?fZ)4PoxWf4G(+nuP2~?_3j`rl9Q7u zJEkI4cp8_yY+l*A~^c@O^W_362AfklyFJM;2He;nYv0?SAUDrn9d#)H&@1 z2dqi`@wn(t9#7ymI2G6~nPs4p>H}piYz=!xHp+xqkJ-`LiA?2pY12&g=KDHp&DOr+ z>qX~}M3Z3&|S0&YZ5yJ~1PEW7UKk-CyE8p}{MijiNjWS&sV`6A@f zP^E=n9+SC(&I$L)xka7)rA?-wJKtK!*$osBy2U!}q$h4?UN3AFI;1^)AY$BdO<@kI zvCPV4L}$xTB_et1?=~k-Vza(xQ}35rms!!SqvBpn9y>u{;g)VUdYAUbilY3b1`~@+ z!st{c#dsZcjF!bFF4E%Lr4YFa~VxrkPrW!Txi?iP#9_xz>@K#EL;|O=4 zr@Kx0=mB0A=3Y=t!CgW^>Cp(uw{Lek6n|Vrm2r4a)$po&Q@BW1$T5^IPc_Ulp3Tu~ zkB=kXZ;PC%%4wSoukt>awY@ovjT%iN5m%%8mrMRdz&%z-TnNrq35KAwDe>jdCIc`K z*;gz#&>#Zd8O%_w`dQQ`XB3)Tnc_h_t!$z(qdL}OBuHf;z|aYek}xz(t#{2C?dj=p z*_apxWftH2m|viP1Wo$M>{*1xcAE?*ZKJ92t@4Riwn3WG$;E~A1i`A4vMGaBP$}qm_I_Fs6f)Kp)ZAARE3&h)+KNDc?gpmM zh&!lqg$~cTmXGkI--7Cl%}}std1b3EC*p&NWytd$SaMX~;kzsD#Xitbd5!|t=I9>!b4zE47zGZJj_d~x?9;^hu_6^_PbQ96toWz z5}2D;*oPPLFdBLbwsO6W_B+vHdzs8WrY_P7ZT?2;`QGLrR~PMW*ba300PJ< zD<6#2>*}kWwvpP5j<*G!ti~01=}JhL2lrLS+=yTAeN!+POf<9p&5L;h3;R$8k5YlZ zW^SJDNYa|s$9XPEEKzjZ(|%^5r-4yHm8(+g?&m^c^1GSrQQ=5Mtk#vO ziUF^_q(+~b*9i5hD*I*jd$0T_Cnv_U`L^AI{eN+DiF#qw4>%LrX(FhvC(9~xaFpzJ zya2vnVyQ-ipFk&`H-QzVQ=GaM2uCj{kr{G@L|M3L<0_S4YUbzW4h{<|GI~~3Yxxda z@__i|BrFJlk(ST6Ex@ERtR(8~3p}Xg+`PPYzpkP_B$855vDS2Tm|9qfc^slH*e1%x z9%q13L|9m_ip&lTWf<=udYVa41KdCeSp-CCf=m(oEhr=Sjy3f3sGJ7rX}{D|#-K`o zqG&+sf_)1@-|p(~6|SzXMn@AFwNii$MD|lMwX8-DZWTT*uBkU-Yer<^cIC&tThqHd z!p{Nh#p?{j!tmfB=H=x#AQqIixNa5`tr^^a0|VkfZBgW}th|YN7YA6JK2L}Dyxm>=*>-~t>p}x=kIl#=8v89+1RGjv4pQX)pCE;|NGm;1bv^) zfxv2+uPkZxvsVrHN;&WE(9?U4Jza>aZZ}Vs??bxM&5QEo4`Arn9`-bBE;UF!G_!f= z<7}@3ZNnlk^K+ER^@#DE@43p*@dLAAB=e7nArD+~|Gn9(q*u%n*G1eZ$vxOK5g*t0{@xaP z;*t>wW%8f5#fuAd*y&{)m(JzBDSVMbpm;k{7DLf9cY%}xroz0a3iA!YO&wd#6th30 zd$Q?6oYngcXV_K5Kcr<{RwF9I@kk{`k;68li4@{$Dx;{Wr9aGXt#~QK`32+t17t}r z`-0;tNT9pe?xdBD(qVcA%_6Pj*CquPRx092OLlD)6^Tk+cxM4D0Fyr%pY0e(-nnM> zD^}Q#p-ly;sB5KkXyyc%{b#3#W^(`y!;@NX>%+jfN9iWk$67o=9aqo7E+jpLngQ7( zG*3V(K%nEcuc1>}^fEHpzT))=>%0{+^K+2r$BVe{gFFkC=p0>XmRVd{f>4k(92u&S zGeoWV?25^8GWXh%hvV|QPyIsX|B{c^emiy;R*{E}sL;49{GF>~z<*{sU;yt@ab=0h zvFqMKI+iBxYPHGh11YH*8meDT%V|YD{(Bmfg0EVXGi~|pvj{{t=T{8kG^ypgr-|c; z)n44^((J;1DBv@~|M*MetMq*${mt;PRD&9i>Z&R&tw~@&j#OVV_xDf!O1vwsW=F|4 z5v16}$W-v|-MVojW!g-@zkKy6v#Sb3RW`_!(O-(oipy(z38iW|t8dosJeJ#J+8Q*z zxEe(5Cp`G{RBE56tgPNkYz$5R)V(=Qegqp;R5UJ(i7+?M>2cnUpe3G9D7BwCl|m$k ze>_%7H!=$DW2?f;%*ZZuM5B!Mak5aT?;nGEt9owCn4^ucNuJS((6#n?Vpr;5a)-nf zQQe%!Iz)NV)AMp&5u}qkjV6WV52Xqgj-z)UGfuo6-@#3(=6aaPfBT}=g>hFH%aMza zB)Hyf4GUdRhgp_b)1g2}STbLpS3ZcFJ18XH%5QWWlZE$&l;l5`qt5|gR3>Sn_Tu7A z;O)3PKOWodq%gW$tfhy6Qm5bpMq)C>J5Vv}R@g%Mw)nI?!ex>$KR*w-F&ir@uD=VR zx`ACBWU{{)fJVQVcA2fNtpzpfkDe$Pz|m`Km6ij4rb&2xCtjJFM(>0ltqIRb+&%+@ ztd7>r1n-^xI$5&onYxjY8Qwu?MeGq>5(ELlmozBb3!S9KH?0!rO<}+Vk?UiAe$A!# zNT=R|;Ju@-f?mDseJS@HYjsX@Eb~UTX1a^y@RK7e5XY@GxdRSoF4lN%mZHW{bH!>r zFVD`-%z7HF6UaaN`uSDbP4Y?0E>F(2iqWDjO1*;U8je+U<$J8wM!=Te(jvKut*EG< zYdBJ%_49672SLEiv;&)+5U!NUSIu$`TZ^1@m_j1-Y@Uww+goXCxxc?pw-u^2DID3=b}89nD8@C{ zpBx-D3_zyl^!o3nGxp~cdnqLQdrY%i!9VgRj`1iB8%cH*aqF@4s=M98aC0fC|L%RM ze0Bdo*0(Vw&T{Ey=_O?dl2&Yydwzx?-sT({jV{ zY5}Y$GqbQL9cQ5wA56Z0Vsjbn5-?C~*mxOZ>M)yPkN^T!hcgxB3q&P0@-lEgEH2APbh%ghw>6;sJQppgC{b{7 zamis%zBX{mZ ztjnx;7tj7q{wU=hwAn5nmwkGEnc?Z#sZ@d#Ci-6b`YN^~_H^eCvQeVlzVr-CZcjDl zB5#W;$7e14@Dwt{pr)ZI>9xM*!d78hHvMql{S0kOav{8_S)YR zsM*?nv&^P~#-QwBrTP^H+w#&%(y)NswZAZJ)(E#ES9WT>T4gJ?zI^ptU0UidRJ(V8 zq0{R^ycg`xOXW*LqPXR|k!C8s9cWpi(OBTel3>I>C_%kCFMgc+`O zCRKE4lz3@kM+;FTY&xfZTM1yQ&7Lu3b3`gnn;9A!f`D|SP*;t+#%1e#P+3edS* zI|fb`$jI;ZhbVk)-Pqg|wH^Cz4%unEKAWg?ztB)Tq2N7lg7-uq3(AvC8p1fUfP*u= z;aFfiDJPaO9Uc+_OCFfmSe-M=)DTz-XaD?p2UpTa+m*VTM-PWt=f z3|>J*Q;dw?o(_qLj=n*fVG&3fK=v}u1Id<1lZgzKX52d#yR2B)hB^+W%NS1ax@v6B?b?dY( zIHqR?JNJZYzjivF^urZxZ+$xOdi}GF4Axz?>nDXO{Hml74rAKAm%GjNlf76l<;(Ip zD+_C}Lhn_2S|ZOsJa@&#Zua%}Z;zGh%FCW|4ThTQHNNo^HCv5uig2L0DM5&Z98-BG#_9E+6{{a@5M&>PGMkPp`IvpRelO$_^q0`&e;!RLpb9 zwiRhPLJO1p@4!a&VFp?Ow=&5f&ceNcI=8|@BHgzb)OYV1+u!#J!(qnPrD|6)W_O)J zWSVuU%CEi)&^Tsay?7C4a^506HJpcDU7;0y8(nX^8XtA|kOvVlZoCOEOtLdo=CQKfE;W&0f4YkqVMf9)CtwlW+0$`-&?K1Z}bkw0eXE#)0)JO0-U)_sg zxqAf-N<7%!<+?t@wto=pMV^a%oMVVtA;u)&V^A@5>pzS7PtbjJil0^g`D3iBJE>8q z!^?{#zL`dk#AJJ_Yc>935kilgfyo9&Dlw5_^%krWgW233xL`J&GQivP%B_ci2U*dm z1G`Qi|9CqrWvzyO9W3cQ4l{Lmh4g2~|0ZEG$(XI?kKeyvO+-L7qiis3vteg(>%2vY=rcNU=aWVQH%9W(4S^6g4-EA zBAI0_NCBR*m&EGNhF8((_=JRk$d+`Yb>mAvF+M)tnyn?=0GMP+D&E`wMO%Vx_*Wj0 z02e~3wY3$%7Ap%#vjTTgjd6PrSdqTZan<6U`RV*yALr)h=ZB5q4NH0kBs^~%hx*e70Qh^EcsWYMjmci_TGAc~?Y-QQtE`Zudv~F1(?iSt zNxi2zCG5=16W&cx9F0lve8V%Cy)q1Ur`8_Xb%$0QR%X3!ez1Li>nXjN;F58w0U7Cm zM{=LrCbKGu706IqT0pz8qp!0|!U~`63OV_tYosxAPoc&OiyVo6zapu`i}mUSRYow} zT4i+YldlBAIcBp!CeP!f<*q6VEpD(aeq{Dde46}P$Q|3h{L0ev$A&+61Q}!yyi3&L zaF1@2J#$EcB`g458_!}!N2|x`YL^IzPHR7LnC#G(`O623>)MGfJ*lY6g6glsF!>K8 z1~3Gw(M>ameE5EU;zh0V2DlaAy99h9pB#Y&-DUb#!%30n$v&(Xd9UF8a!hIey$XQh zXR8IpI*L_-T>Qp}O!p4S(B`OW7>OhVzw+{4s7xJ=A-n0{%%Gn)Nmo3oFHw9WzrKLqv?~Xfxf#yAxe~a@WSa$$*HUaBRzdzGWVqY zPMU0KEkmM{n{Z`kT}jY>O}*5J6LAjmrSGUyszAe*^9C}dpZ2rq2*;4dphu1W;K=3X zF56Ycmr+BEJLB2x0N)rcSVU_ECQzP8MRED$7_`B}W|0^lq?iUNj9(-7LPB8RzKK~@ z8f@v-!w5ZJZBo*Ye2U7u)=gXm#L&>=#TA>G&&3-FN7dH(xZ?hX5u>HKA~GgA)@^-; z&Y8=YIk=bPa^BZ2IoFO$`U^*DBskTf^q^2+I}Fh;V5HJGj3rBii&!*c7z&pkhu+v^s}!`f0Bxgivxv{x~vPci!1Z>!O(ug zs(Vto^dch3MfCO`uee@EQSy%mp84H8cB*~1a(6cKO77}fDxi=4p5IIoqazcoYvoSU z*5$gH<>S~gqu6UGD8=Q!`1&R1CV6?!L3$2~$|U&6z~ zk;-CckT-td3Dn@Vk05_kO5$UC@+s9ThEd7Yjy(3sd5-oE(ovdRovy8f)Bw8)aQ?(* zoh6ET*8HXMTLsTFB`}t}Qa})aRCwsFb&|h-M>LZfur3cKOYJc*MAJfc@}=ae*-MJ0 zcDo-cTtAZlpRJ0f>-xCW^gOAP64x5SLW96h#q$8UN?5;=j3H>O#xMU*)JFZ&btHK_ z9B9`KOJ$q+Yo&_5zP=0mHxl_D&fDh7szAdW~MK zJ}!UD_p5i8KaOHJeYQWE;Y$$tRGa0KpfVR-=|aAn)h08n{IJgv{SX^v)LZhWs6Dn1 zAAjYGiTc4gT^d{5fU`7s-P?Kp{*o}k`z)MdFPgh= zjA~3_?1_qugc7-8Ts1eJ+02by2GhTC@hm0&)@K@CCP<|LPSo!1v$y2Lx;JX)d-w)AI8U_*30TA%UQHDB^_MU~`zUh=({~Ojc z+<>DPCJq@@jg5`5u^Pc7Y-4_OwiuM+l~#jGpJ=_FxbL&F$yIu57U==@XlQ8!X3G6d z-@;SC)InChcMRcv@Hkk>e%*o84A0#@emLpb9jKO;)=PnL_Y+uU01j7Z1sBoipAC}y zjO%R-wv-U*Api&^#LN_Gl}NrTkdYO7{P@Ma+bD^_krDixH`9q%MvkGDm!$)>)KPcF z(xYRJ%Uk|_UlJT+n|Kh(kdc{bLMU>rJzcDm2xk(*`)hD$<>7ZTEw63dr6eiGQq<`C>^ICn^RviU zT)cnZAOF%2aou_7*%mNz^8R5=oxtbM^XIH;(6Njb%z* zLHGVCpC)mp&R9`*lFC?%7u-TPojq~h?Pvt&y4Y*+^=Ed>OC<6%;H|Zvt^;eH=C*Lx zi?Ew~ph+|yB`7UKhIp_O~V`Zgfi zcpb*IqLPf*t0KO2ek*PCC!J44`6Mf!Pfq2%8vLUiy_C=AV({%HV-Op*4Us6e{nhK;chpxzkb+Icvl*+Fa7PeB{9ynQFg;X zH$x>H&3bikhGjXS2QMT9Q>N#iceZTkfC%hjXj{QOQp~GUW zYhBlQ{^|q<$&rJD1NPwihAmII;pZ%@t@T!XjC9-lImlT8U!41mN zk;w5ZCze&61`2O=|5XkuxNjl@e~#0m)DoONtz8Q{Fj1FnZ;f{S$)QM;1T`DGZoS1f z?m-{4Z$`K8MLVb4oNtEkcxYzXLP88w9y6}xQnlK<_VmLIh>;3xNa-ttTECj-kKc<>0tKPQDR}cKjt~SNd z1Ex?m)h)~cR0qvDnt}YDp2_ktF6)!GP__jYsECS6zpe8oh$8LtQ7v+u?Z2c}pySPBO>s*3MJ7&MiYNjV&5m&UzJt5c4 zykD+>sPkx&wu*mSiN`ZTE#>5NW`>?+aY@Un{E@Hnw7r7pDIBzJhPlH-yq_y9ukLD5 zhl+R=7)rmMIr>Rb@3Z*La?G@opM>}CocWKG0C{Jk(SR%CgqQ;|=u_-vNY#rrb%M}O z2oG-b=UAj&!!d0py{iN1ohFIy^D8R~@vgg1f;_;iNN;FpIPnw~V;WtFqBi8OHrBbG z{`tvANG%459JOE@hqjHD$Mh*Kco{TCfL2|k<||SnHln7xe*J;R(LTTyUGYNvLPFYA zGJ1M?0Dx+!s&22ZUsh-NE6MBtHcOx>lcDg=hs_&$Oehcv_39>GKi&j|yJ>2o2X+z8 z0Uu{aW8-4k8H~YFdO$+d=p=5dca? zOP>(JcnWh-UNzvt!SaJDZM7srd}-Ct)TRzsn*%JB^FDuh*)v*tFGB-^d4+&a+9#Ya(0uYFcwJ0mTn!#$pjec)^#L|YM4 z#Mo$1a1u^M=4(Ts%D}gct-Z?U=1ib1%CGu5!UtY^l;K=o>)fC3jtT%`FDIvDivF2A z*aD5?w&t4a;V!Gn#-}}=ro{ygoom#vTAM^a#tI}P_=ee^R5v2<~F>HJbBTS~OUKt__!PFuygMU@A5Jhtiy$t->_M*l1_+cK{I`Na?LfMBmkm<}G!>lLE(xJa)$091RJoEp#5LOqwTzLy(< zP9L>zX`@i~B(#Ee(3etW@pzJ-?0`h0*z=q?#jW_sZ0ul_DCvJO^;9AflF`AzR|9Qx zB&1@efwF!LN1L5EJQRr><|2_wo5%`<%GCiV0B~WzR%n>{B!jMB5l3D_)(7l|=_E-| zRT{p0d290l)go-NpiVN4Qd3nOV@k-z1ECOVa_seIG8mDQlLHUU4ixs#Wbj3295%%Z zISzcKgftDX{JeW((R8H`wXTt`g|xI3246bl3+-b1LAlKw9Mf46NqOwP`Kuj-^Z}j|!+Z43Y6=a+Yb^1V{|&net{ttOe_^#?!1El{ZK8xk~1QzHG= z1Q-I9L+YbR86Cuw*;?LeY3s$>%^AdmXYaX44nLKS zT-2XA?>+#KPd9}ahmH~BW}X1{IiV{SbwKp0zPOk)fQLQ_BCFvsr3oX69PKOaWkW`* zsjc7gGi8<0kA2MEi_(>AlUJr_Nd!A9;`sa(?H8pcJ_U6tU@e4Se^FWav4Ks_V^QyW zL+WvZpIMO6*Ls(<9goLefL*^nr8NxOKMP6ye5(g`R`Sz-c;Bal1h4%Oac|TZ%#<9`U zH5|q_-DtKhy=+TbDn8X2GruhKS$bk5dq0(9r|ppgs{!>3D)znz^YwP3|{hm)jmv z)HrTp`fvwdaNuw@?uj6B3= z{@1XAt1=|yEG$$aJIMt@S0pZ&*_9C;$5D9#{tVq@-=e&tqOj-5L)~*_#1NnrEYeJi z_+iMLTTmULT*V-`_4?UrU?@M`uDIK_mB8rO7l~Z!Uq=o27;l$=b8ya^_bZ}^LfGPk zjdx@3UPrmNB%LuP3W++TVB?OT2R~6l0=w3lYg<8&|Ck#}JO#@TWRDmHbpWh2|d?t;`CsOr=)ywR|5H3yd!^8px2?uD8Vwj zaAy=k@F+NR=i{v73ybhjscW$85C|P;mzdFCtAM=2h0py4G-96M94<%zPDLufk`A^O z77Kzbq0;LIR0pS+0KFN2nI~{0qU68qMnn8Ktil6O8p?5BKtp~b;8({VoTOlCRVRdqRUD`*m8#Yx~x%$wBmz9-4YNptqG;c9^+>Q%N z0b2LQD}#*)EM%^Mcp%1s>%$@G6WRv9x-e>MdInFKLsHEIWu!-h+6o^(Od%In#jBLYIlmesUCqXB-^~S zfK`b)xA!ghjdTsE?QL&&_4ZQaYp54?UQ5qKpuUQWzi@MaoKxiCbC^j>$EnFpAaLK0idSm>>Pkx zLC%a@!~C4J9>=~s)T97})>PNXSFP*S$<3_J_e4z$hQ!5aLnfw%hP-RA^7*x{@lrQC zCR3&UnTiwKR9L45AjXgRbH=y(ABK@ z+c)-7t*^SD*if{Ka*yTYLN~E57my(R11-+s;U+<^>q+J9LA`K?)@w8CS*;3 z{^r!coF4jPH5HYuogHMR$Q=kche{6;ji9je2KYWPk%gS|N;56nbBe>e zOT*w}$u28X?I_@js;t}tFaYpO@twYrMRJx0527aqAvYVe697^ZaU-!ZC{&q`j}#D| zi;p2E90QAS1;%(boE}>P2?mfotj6N*q*FF__UQ^sZCF;A%o(?>?guT zC?YpEx0VldN_TyzBwAy3efVU)+uPZ(u(BFaq)J?&rha8+Cfl84Cn+SK>jtRA&?A5_ zpt-xf*zy(+;ih11Zcb1dplhI4Bib3z#=>$crX-)PrK6)WuYjGeu!)jn^s49ksWO#_ zrHdcsw%QWHLj~!M@(Q|Z-1BtkMZE~sY`7NJ&p)Nn!!wlmfp8Ut=1QV*(NneIyx)4C zc}v|zdD<`0N>Xb_<@L%$GqqS88@bRh2Tl83rAJZE*cr~b_7j%z*%S43x*-qK2y3_jOKnt@E}#m0Fa-FI*K zYZYyg?7*)7lIF&dSgcLo&_T7sv2Xi|fSW{4H+dXruimdYa)J5PCO2#>d5|vcX_M7V zqyuw=Afs0#Agm_{Ej7!qDPzLHj^cy*cZ&v$uS_SbN&-!25$tGNf}x<4IRii~8Qe@H z?%GVh^&vI(P9Aih9blM7OgLqX2`FJgL9Wde&mC1mq%V%Z%{AbXKpYn89fG#8CikKGxmM=6{8P!o<*Uu-LE_{sZ7U(tQJ5J-8#ajPDjuk2&H>N^7u<4taOA zwIvC)!xP8lcY^%^YC?#W-`v~Fr0ZYa9t5ixe95;eUvuu2v-gKcydThOKW9qU6`J}0}c+)lY@n_(K2FCuK2%yfBD=h0&#kejJN!QoxHAr zz6xOvJJX>UApmQR_QMe*i@QO(eFE)yDqgaVsfpOt&;3FMhEcwFC``*Lu^M(lk#^I` z>iA5bf5ExkNV~keNEsO!`H**M%4*sa5&9K}&byJAE}@v_vvf`=pEAHw!l7ls4!+%p zhzO`V=HgtQTbS{9d7K;_bg1Q4vv_22fkbw+UttCS$tl-VWy0@`0?JP!P&Ft<9cG6bT=f(1Z!P)S0heblgi38zUw#>U|8=#}$_Y>%n_Zp=*uq0*$@;RWCkU`s)Vm3y}akp1}3#Zm!!f5X?`zdcJi z6=_ zMbvN_$zga;b^%945qIc@N_9H{ZJ;Y%Qbn1v94%^>QJ3@wLj3~LXv@1EVBWnu1HqwS zEYqoX6JTY{d+FQ5-n42BXUF5mZSY&b*k@_t!TP}~Nxr-~*AYMH($N`w#lO1W?0;6g zvPhv%Zlvbs#RL;0x8R;V&^O!frko-rx4)#{=iXjbpUZq_vAc&vnOrhh74-PBkt~E~#DkX8jAH*uq}M?UGG;iyg!HcVtA}2fh)8w)lsgr^&Hkzlu8ACb`?sFS z$QML#xyX6v7m2T+W3g*j(LUO8-)z4NmFhr$KLk(D)`Wp4{IoYcQawSg-R^zI!|>>> z+N?RnZdZ4`zE(w3)2vz5KfGbsbG@tN)+5bQsfF#<#|2s_CDZjAk9w;E(5dJ3d9FvB z9{e_p64<)PlOM?eB$#KSQtr~kL{xf&i3)LrWW%mPn?DV z9LL$YX4}P<-Vr6#@{$~x?eeFm7JL>#N{=t+=iUcgxROQ)C>Q&#D36g@XEGqWl->X~#_*~yfBpD=E~IfVhBhE$qvFvNPY%)gLyq5uZj`$ks$ zdeqs6y!lcFq)<9FdZH$=nEB>xk|bRnkaSOT+|KOk%qdxI4e0V&B}Nx$P3a_Mp_q!`zhTC)gB1Ny0$@k{m11)EH#a6FNy+ z(KjBo&MJxPFbFS;onFKk!w|pDuV0fPRQq%gk4GpP8^}?=zPU*%g1!s_q};3$tiy5E ztO7HhX0uPk8ZVs;lvdDil&LZ1sXY5DVQS}tW}W@Xp!Gt~v~q>+X)I!*z9z<8bZbLm zwU^Fe#-RgWxO_Y#19)oElb||f+yP=M4=^M!l2-n@%5y0DMn#ytPohqA_2^yS zcAg1bKr!ZaQ3|aW>Cu(i5SSc|tSyDEZtTViH$H$jS7{7Uu2_GXZ4UK+`@$=_nIm53+_PG&@NAOUc zcbIh_C9=UO?Ui{~|Hu+vsG|d7>*=8nhfz9X^{ZihPfMHUCtc1cpwXaCZ)>~PPyT*u z6L~CVXvKg(|NTAh@<^T2tLIQsSeV9&!#&LwRY#XK>#VF4KxAV!x z5Q=_pr}0)+{!mYyW%R0uYM0Elk|k6#RJxXz3yHWpksoZK6<(cbur}w~8nqUsaN~aQ&E#02g+;Gq z#jZ=iW>e|dW8eJ!KvrSv(LxPS?7n_}oey`bi%JB4*cTs`vVL#t8syjv3!G6sX=0q_ zfQDvvb^`zhDD<8_UB~xVQZ?*!*rRd7L&5Cy)Uc~FdBlSz*AEX4T0}E|Za2cAs8;#6*A*l2dD0(K$sMR4rJ>Qzb3Il>0crv3DQuJZ!KVLJMmKJlXXbbDU@0(}zW%@% zL~Gq>`wz4tX}wH~c1pL*6~&SEPaU#lbSf+*(Hf7eoWz|M&pY|f`DuvaPc%uy-}Tb7 z=yyWswmx)O1teTLGv^1`5bj=v&i=?J@SE}L?K$c1ia2|V#!neC?EZciXF_{^Fywju z2@4IA(zxo^b5g!MdNDYtD&louWKTFKqoBg|AGz(5m4%5o!Yexri3dBaeb*|CCYWp= z*Yq|aB3>k>IWqGdB%u=sjgM!mZc?((aX>-KtxV9Z+$6mlV_sQl^>7BFFCWvR%#asc5iuVrQZe{C^H z-u->#vj@WGv1^BoQuQ1k|JUhm-E?AO0P~0en*k>Aw2})aI)RYx@8jcjE;~7Bv{uIn zJPSl_Mny$Y30C>Z;>rH!?oX6LKTFn($U)mPLc&h2TVb;;tNQ$T^ykkvK<oF41H1*yjY9wHh<}lITSFsHP^^W8;J!r=c3K ziXXVfYafhMXK}Nh0VSL{DihGG>`!hKUbCoFVhRpf52}O<4&;{t)8(5JBwhL~iVt91 z6f6O#tnF;4Tk^%%H4v8ciHHgD*hP+an6_F4c_D|Rcb_&C%nEp{k_l1-HK%N=)x=)B zfM>Wq$&6@L#i(zkjkev4f1HBWzq^x3Ryn3i7)>Roq2$ztOY1N=RaN$3{o>$kM`x zdTnl@%dU-CJNOvwTcp@?buh2hvrlY2bTyQ0sfg~LM$^aZ6a^xi*5oNx1s{*KjN@RF%OTE)jHQ<-sbbm zv04*;m*^t7hI+a?zn5Bz&ag5xJ`$=QZWmzt6M?}F902%9V&SE=3JeWcsy!gCRZ4uK z)N8tAEeFhb^%jK_0Y;Ni{40s%=#|Wrl-r=JYLxs}$b#FLjE9;bBMF9VWyHp60XTVb zKv@P}V(|I~2L)yMF?Sc?fvLn>Ohsw#-( zd06FH*?=IF9Gz9x(ShkJgb)}ox#G`eCFJm$z*JBgq~+b(MNg$HOU4WunjvpZ(xx+*H5*EFta54rRn%GWUtLW~sl)8dN2?j>z_L7YL+3p%s3 z+P7x1bct~-r3$FbC7aG|%9g`jx{q9|*6*Lpfhw9(fq!5irjguy*65;Qg{e7iyfZKO zQ&8oOu%(zoDfsfDb>Jm6sneqG7KyRgXQ=p=BD^vvye7AJm1Q;UOVIIk?oXavl@=o& z-uu(pk=v@+JxbzMtK=4ks`+ut29>~o^R^02onNz^$Pe62tn z;^E;9%nacEvU;b2+b4ZZnizc#K?cuP+snaMC<{Q&_-vE*SGZB6hMMZ=06yKiVWw|P zfZ(2@nv=(Pdf8hV8_l@e+|w6Yzp(JHZOdTKkf6$}7bszF9Dq^liq-^0L*{bnr^ z9Cqo6F40Cv6H%9=vECVd3ukf)4Lv>f5~7{+kXS$lG|{yitQB_{?so28Zu>kr`SGsO z(UD}_*GLd|2?Qgf9)Z9Ybx-nA$m3+9(vlLR)#+>G6%^~R((LHU)`sIvyluKjaO+qf$ zV{6}Om6z$_lEH@jrxS5A3j8@@&$k>^SX5C` zG5z9I&D7NK%DA2931xV6_?@N$^Y;^!Vi{}7@XUUk+QIAVQ62Hi zwUdGR+AA^kS4;k~JQMNT|+Z*HMRAUEcoHCoxhS@-yM1LwBc6?Wh!lZ|7tuj+U?W z7~KxYWOiUlA?Ck{ev6{YSI6qUD$9w@pkuibZ1o6rHl`>ps{m(*1|PaX@oA!7o>|BJ zD_<-0@%zcr=(UJVaz1M@nR`^eZ79m^bbH|w@m`HZDwm33@|%8Uc5>xHuJp(*ziQ{N zDOIM*kBpXY^8-j|k8z@=65J@er=3COV9nXt*ytY_Ssls_GesOZsyuy)8O-cGI@khl z#?9}EI1SX}!i<7~v>e9NTCO{l+d6V51yvTU*l_`_O34NX_sHKdAMN-UP92PG9$RTz zyp(#@9v6j-p!}^?*qK|xk^{Gpt21|?gL!eMDkY$=wxmXr0gTZkjPq#G&YVOedakBh z%wZ?)n^JmDpP0!l#?9qWjVE|sfXjrhQ0RqZUbMN5fGG zI3M6JIvR&Lo=0qBLDBJ52a1Z8W@?t=NJTA8q{7RC)X&dON1I3W$@HGEJV=c7WMo*g zFh6OIIy*W*Y;8h*ej!|%JUla8eZDT6IXj1`upm1pKer%pnSjpZpKG8az{hx!JG{1m z&H%zB0Ct0Y1>n7wOFrZmeSiBa*2RH)zQ)lShU>x$Kz8k#Th=0O@0-?lzoV-yYQht- zG@+HzCkiK*c{b;1!KwZ%!o=9wE#U7=E`*x}yg$_-90MO<6oI7J|mEhmS0vJ6+P{QV`Sf_r|s+1c$~T@US-a4FM3HUgT-GyH=VS3*xrOu+gFI}bqdfLdRo=F0;mCY{@}OF2bAlIo&S z2Ov^5>G|voA|?0`gO>k(f)gEF^!cLFM=<9Y;_GH%qX1*zDJY^{0gMF5s{f`YBcSTE z4_^NOp(p%n56CzGc?_S6Gz4e<=W7i{6`UgYh>Z;~a0Ossf?QdUBrh*5oh*o+QgXf! z10^4vebcWMc!Xlk9R{q=Jc5#pO}kv<6XvO=ATH(12SMb|9*A1OMwHJ>eoX zU>qUjB|wISvy&66W{FWpI8(z37Rp+B2sqBk%GzJdOaz70&dlkFBm6X6P7_=>4q;uJ zIR)qjXE6Y7P83GOK`1Gh+Chv36K@RKU|8P2Uv95)KZy4RxvsMErt^y3>n*Ej!rYc|0Md& Date: Thu, 15 Feb 2024 12:18:54 -0800 Subject: [PATCH 05/36] Update README.md (#4114) Signed-off-by: Steve Macenski --- nav2_graceful_controller/README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_graceful_controller/README.md b/nav2_graceful_controller/README.md index 322485cec8a..9cb5a72ec70 100644 --- a/nav2_graceful_controller/README.md +++ b/nav2_graceful_controller/README.md @@ -1,7 +1,7 @@ # Graceful Motion Controller The graceful motion controller implements a controller based on the works of Jong Jin Park in "Graceful Navigation for Mobile Robots in Dynamic and Uncertain Environments". (2016). In this implementation, a `motion_target` is set at a distance away from the robot that is exponentially stable to generate a smooth trajectory for the robot to follow. -See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-graceful-motion.html) for additional parameter descriptions. +See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-graceful-motion-controller.html) for additional parameter descriptions. ## Smooth control law The smooth control law is a pose-following kinematic control law that generates a smooth and confortable trajectory for the robot to follow. It is Lyapunov-based feedback control law made of three components: @@ -39,4 +39,4 @@ The smooth control law is a pose-following kinematic control law that generates | `transformed_global_plan` | `nav_msgs/Path` | The global plan transformed into the robot frame. | | `local_plan` | `nav_msgs/Path` | The local plan generated by appliyng iteratively the control law upon a set of motion targets along the global plan. | | `motion_target` | `geometry_msgs/PointStamped` | The current motion target used by the controller to compute the velocity commands. | -| `slowdown` | `visualization_msgs/Marker` | A flat circle marker of radius slowdown_radius around the motion target. | \ No newline at end of file +| `slowdown` | `visualization_msgs/Marker` | A flat circle marker of radius slowdown_radius around the motion target. | From a3cdbbf7d0a9be9ec50f876ab365a45eff2214d9 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 15 Feb 2024 17:11:01 -0800 Subject: [PATCH 06/36] CI green P3 (#4117) --- .../src/behaviors/spin/spin_behavior_tester.cpp | 15 ++++++++------- .../behaviors/spin/test_spin_behavior_launch.py | 2 ++ .../behaviors/spin/test_spin_behavior_node.cpp | 10 +++++----- 3 files changed, 15 insertions(+), 12 deletions(-) diff --git a/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.cpp b/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.cpp index 706767b9ced..1e6d57e0f41 100644 --- a/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.cpp +++ b/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.cpp @@ -136,6 +136,7 @@ bool SpinBehaviorTester::defaultSpinBehaviorTest( std::this_thread::sleep_for(5s); if (make_fake_costmap_) { + sendFakeCostmap(target_yaw); sendFakeOdom(0.0); } @@ -211,7 +212,7 @@ bool SpinBehaviorTester::defaultSpinBehaviorTest( { sendFakeOdom(command_yaw); sendFakeCostmap(target_yaw); - rclcpp::sleep_for(std::chrono::milliseconds(1)); + rclcpp::sleep_for(std::chrono::milliseconds(3)); } sendFakeOdom(target_yaw); sendFakeCostmap(target_yaw); @@ -277,7 +278,7 @@ void SpinBehaviorTester::sendFakeCostmap(float angle) nav2_msgs::msg::Costmap fake_costmap; fake_costmap.header.frame_id = "odom"; - fake_costmap.header.stamp = stamp_; + fake_costmap.header.stamp = node_->now(); fake_costmap.metadata.layer = "master"; fake_costmap.metadata.resolution = .1; fake_costmap.metadata.size_x = 100; @@ -288,9 +289,9 @@ void SpinBehaviorTester::sendFakeCostmap(float angle) float costmap_val = 0; for (int ix = 0; ix < 100; ix++) { for (int iy = 0; iy < 100; iy++) { - if (abs(angle) > M_PI_2f32) { + if (fabs(angle) > M_PI_2f32) { // fake obstacles in the way so we get failure due to potential collision - costmap_val = 100; + costmap_val = 253; } fake_costmap.data.push_back(costmap_val); } @@ -302,7 +303,7 @@ void SpinBehaviorTester::sendInitialPose() { geometry_msgs::msg::PoseWithCovarianceStamped pose; pose.header.frame_id = "map"; - pose.header.stamp = stamp_; + pose.header.stamp = node_->now(); pose.pose.pose.position.x = -2.0; pose.pose.pose.position.y = -0.5; pose.pose.pose.position.z = 0.0; @@ -325,7 +326,7 @@ void SpinBehaviorTester::sendFakeOdom(float angle) { geometry_msgs::msg::TransformStamped transformStamped; - transformStamped.header.stamp = stamp_; + transformStamped.header.stamp = node_->now(); transformStamped.header.frame_id = "odom"; transformStamped.child_frame_id = "base_link"; transformStamped.transform.translation.x = 0.0; @@ -342,7 +343,7 @@ void SpinBehaviorTester::sendFakeOdom(float angle) geometry_msgs::msg::PolygonStamped footprint; footprint.header.frame_id = "odom"; - footprint.header.stamp = stamp_; + footprint.header.stamp = node_->now(); footprint.polygon.points.resize(4); footprint.polygon.points[0].x = 0.22; footprint.polygon.points[0].y = 0.22; diff --git a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py index c9df3d32432..8edbcf8fe80 100755 --- a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py @@ -60,6 +60,8 @@ def generate_launch_description(): 'gzserver', '-s', 'libgazebo_ros_init.so', + '-s', + 'libgazebo_ros_factory.so', '--minimal_comms', world, ], diff --git a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_node.cpp b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_node.cpp index f16db47851a..6838e012219 100644 --- a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_node.cpp +++ b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_node.cpp @@ -118,12 +118,12 @@ INSTANTIATE_TEST_SUITE_P( SpinRecoveryTests, SpinBehaviorTestFixture, ::testing::Values( - std::make_tuple(-M_PIf32 / 6.0, 0.15), - std::make_tuple(M_PI_4f32, 0.15), - std::make_tuple(-M_PI_2f32, 0.15), - std::make_tuple(M_PIf32, 0.10), + std::make_tuple(-M_PIf32 / 6.0, 0.1), + // std::make_tuple(M_PI_4f32, 0.1), + // std::make_tuple(-M_PI_2f32, 0.1), + std::make_tuple(M_PIf32, 0.1), std::make_tuple(3.0 * M_PIf32 / 2.0, 0.15), - std::make_tuple(-2.0 * M_PIf32, 0.15), + std::make_tuple(-2.0 * M_PIf32, 0.1), std::make_tuple(4.0 * M_PIf32, 0.15)), testNameGenerator); From bc242b6168e73327ee28d568f5d5e3669404b56e Mon Sep 17 00:00:00 2001 From: Alan Date: Sat, 17 Feb 2024 10:10:41 +0800 Subject: [PATCH 07/36] Refactors `possible_inscribed_cost` to `possible_circumscribed_cost` in collision checker for smac_planner and mppi_controller (#4113). (#4118) * Utilizes circumscribed radius to assess potential collision points on the robot. * Ensures safety when the robot's center is outside the circumscribed radius, eliminating additional checks and optimizing CPU usage. Signed-off-by: Alan Xue --- .../include/nav2_mppi_controller/critics/cost_critic.hpp | 2 +- .../nav2_mppi_controller/critics/obstacles_critic.hpp | 2 +- nav2_mppi_controller/src/critics/cost_critic.cpp | 8 ++++---- nav2_mppi_controller/src/critics/obstacles_critic.cpp | 8 ++++---- .../include/nav2_smac_planner/collision_checker.hpp | 4 ++-- nav2_smac_planner/src/collision_checker.cpp | 8 ++++---- 6 files changed, 16 insertions(+), 16 deletions(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp index 77df20adcb5..14c0fe93f4a 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp @@ -78,7 +78,7 @@ class CostCritic : public CriticFunction protected: nav2_costmap_2d::FootprintCollisionChecker collision_checker_{nullptr}; - float possibly_inscribed_cost_; + float possible_collision_cost_; bool consider_footprint_{true}; float circumscribed_radius_{0}; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp index 358994c238b..31b6a3d0df8 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp @@ -90,7 +90,7 @@ class ObstaclesCritic : public CriticFunction float collision_cost_{0}; float inflation_scale_factor_{0}, inflation_radius_{0}; - float possibly_inscribed_cost_; + float possible_collision_cost_; float collision_margin_distance_; float near_goal_distance_; float circumscribed_cost_{0}, circumscribed_radius_{0}; diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp index 137e9433760..7a9b7729ed5 100644 --- a/nav2_mppi_controller/src/critics/cost_critic.cpp +++ b/nav2_mppi_controller/src/critics/cost_critic.cpp @@ -34,9 +34,9 @@ void CostCritic::initialize() weight_ /= 254.0f; collision_checker_.setCostmap(costmap_); - possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_); + possible_collision_cost_ = findCircumscribedCost(costmap_ros_); - if (possibly_inscribed_cost_ < 1.0f) { + if (possible_collision_cost_ < 1.0f) { RCLCPP_ERROR( logger_, "Inflation layer either not found or inflation is not set sufficiently for " @@ -96,7 +96,7 @@ void CostCritic::score(CriticData & data) if (consider_footprint_) { // footprint may have changed since initialization if user has dynamic footprints - possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_); + possible_collision_cost_ = findCircumscribedCost(costmap_ros_); } // If near the goal, don't apply the preferential term since the goal is near obstacles @@ -161,7 +161,7 @@ bool CostCritic::inCollision(float cost, float x, float y, float theta) // If consider_footprint_ check footprint scort for collision if (consider_footprint_ && - (cost >= possibly_inscribed_cost_ || possibly_inscribed_cost_ < 1.0f)) + (cost >= possible_collision_cost_ || possible_collision_cost_ < 1.0f)) { cost = static_cast(collision_checker_.footprintCostAtPose( x, y, theta, costmap_ros_->getRobotFootprint())); diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp index 21d82396159..cdf0113564a 100644 --- a/nav2_mppi_controller/src/critics/obstacles_critic.cpp +++ b/nav2_mppi_controller/src/critics/obstacles_critic.cpp @@ -32,9 +32,9 @@ void ObstaclesCritic::initialize() getParam(inflation_layer_name_, "inflation_layer_name", std::string("")); collision_checker_.setCostmap(costmap_); - possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_); + possible_collision_cost_ = findCircumscribedCost(costmap_ros_); - if (possibly_inscribed_cost_ < 1.0f) { + if (possible_collision_cost_ < 1.0f) { RCLCPP_ERROR( logger_, "Inflation layer either not found or inflation is not set sufficiently for " @@ -111,7 +111,7 @@ void ObstaclesCritic::score(CriticData & data) if (consider_footprint_) { // footprint may have changed since initialization if user has dynamic footprints - possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_); + possible_collision_cost_ = findCircumscribedCost(costmap_ros_); } // If near the goal, don't apply the preferential term since the goal is near obstacles @@ -212,7 +212,7 @@ CollisionCost ObstaclesCritic::costAtPose(float x, float y, float theta) cost = collision_checker_.pointCost(x_i, y_i); if (consider_footprint_ && - (cost >= possibly_inscribed_cost_ || possibly_inscribed_cost_ < 1.0f)) + (cost >= possible_collision_cost_ || possible_collision_cost_ < 1.0f)) { cost = static_cast(collision_checker_.footprintCostAtPose( x, y, theta, costmap_ros_->getRobotFootprint())); diff --git a/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp b/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp index 53e019a3aed..205c3f0471a 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp @@ -66,7 +66,7 @@ class GridCollisionChecker void setFootprint( const nav2_costmap_2d::Footprint & footprint, const bool & radius, - const double & possible_inscribed_cost); + const double & possible_collision_cost); /** * @brief Check if in collision with costmap and footprint at pose @@ -130,7 +130,7 @@ class GridCollisionChecker float footprint_cost_; bool footprint_is_radius_; std::vector angles_; - float possible_inscribed_cost_{-1}; + float possible_collision_cost_{-1}; rclcpp::Logger logger_{rclcpp::get_logger("SmacPlannerCollisionChecker")}; rclcpp::Clock::SharedPtr clock_; }; diff --git a/nav2_smac_planner/src/collision_checker.cpp b/nav2_smac_planner/src/collision_checker.cpp index 918a8a9b4eb..2f23442097a 100644 --- a/nav2_smac_planner/src/collision_checker.cpp +++ b/nav2_smac_planner/src/collision_checker.cpp @@ -51,9 +51,9 @@ GridCollisionChecker::GridCollisionChecker( void GridCollisionChecker::setFootprint( const nav2_costmap_2d::Footprint & footprint, const bool & radius, - const double & possible_inscribed_cost) + const double & possible_collision_cost) { - possible_inscribed_cost_ = static_cast(possible_inscribed_cost); + possible_collision_cost_ = static_cast(possible_collision_cost); footprint_is_radius_ = radius; // Use radius, no caching required @@ -113,8 +113,8 @@ bool GridCollisionChecker::inCollision( footprint_cost_ = static_cast(costmap_->getCost( static_cast(x + 0.5), static_cast(y + 0.5))); - if (footprint_cost_ < possible_inscribed_cost_) { - if (possible_inscribed_cost_ > 0) { + if (footprint_cost_ < possible_collision_cost_) { + if (possible_collision_cost_ > 0) { return false; } else { RCLCPP_ERROR_THROTTLE( From c59e0f362a867244d589562ecff56a9a5f23df59 Mon Sep 17 00:00:00 2001 From: hsong-MLE <158601314+hsong-MLE@users.noreply.github.com> Date: Mon, 19 Feb 2024 19:47:10 -0500 Subject: [PATCH 08/36] Added cast to float to getClosestAngularBin return for behavior consistency (#4123) * Added cast to float to getClosestAngularBin return for behavior consistency. Signed-off-by: Hunter Song * Revised test name Signed-off-by: Hunter Song --------- Signed-off-by: Hunter Song --- nav2_smac_planner/src/node_hybrid.cpp | 2 +- nav2_smac_planner/test/test_nodehybrid.cpp | 30 ++++++++++++++++++++++ 2 files changed, 31 insertions(+), 1 deletion(-) diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp index 71158f32ff1..2e81a5f47ce 100644 --- a/nav2_smac_planner/src/node_hybrid.cpp +++ b/nav2_smac_planner/src/node_hybrid.cpp @@ -332,7 +332,7 @@ MotionPoses HybridMotionTable::getProjections(const NodeHybrid * node) unsigned int HybridMotionTable::getClosestAngularBin(const double & theta) { - return static_cast(floor(theta / bin_size)); + return static_cast(floor(static_cast(theta) / bin_size)); } float HybridMotionTable::getAngleFromBin(const unsigned int & bin_idx) diff --git a/nav2_smac_planner/test/test_nodehybrid.cpp b/nav2_smac_planner/test/test_nodehybrid.cpp index 7ed61c4ec44..1c127c47990 100644 --- a/nav2_smac_planner/test/test_nodehybrid.cpp +++ b/nav2_smac_planner/test/test_nodehybrid.cpp @@ -374,3 +374,33 @@ TEST(NodeHybridTest, test_node_reeds_neighbors) // should be empty since totally invalid EXPECT_EQ(neighbors.size(), 0u); } + +TEST(NodeHybridTest, basic_get_closest_angular_bin_test) +{ + // Tests to check getClosestAngularBin behavior for different input types + nav2_smac_planner::HybridMotionTable motion_table; + + { + motion_table.bin_size = 3.1415926; + double test_theta = 3.1415926; + unsigned int expected_angular_bin = 1; + unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta); + EXPECT_EQ(expected_angular_bin, calculated_angular_bin); + } + + { + motion_table.bin_size = M_PI; + double test_theta = M_PI; + unsigned int expected_angular_bin = 1; + unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta); + EXPECT_EQ(expected_angular_bin, calculated_angular_bin); + } + + { + motion_table.bin_size = M_PI; + float test_theta = M_PI; + unsigned int expected_angular_bin = 1; + unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta); + EXPECT_EQ(expected_angular_bin, calculated_angular_bin); + } +} From b7a9f7f54f0c7a29fa0eea4ecb81a780c0b8ef73 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 20 Feb 2024 10:41:31 -0800 Subject: [PATCH 09/36] Adding context to MPPI readme Signed-off-by: Steve Macenski --- nav2_mppi_controller/README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index b6cfc0e452e..2a9a25a8638 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -288,6 +288,8 @@ If you don't require path following behavior (e.g. just want to follow a goal po By default, the controller is tuned and has the capabilities established in the PathAlign/Obstacle critics to generally follow the path closely when no obstacles prevent it, but able to deviate from the path when blocked. See `PathAlignCritic::score()` for details, but it is disabled when the local path is blocked so the obstacle critic takes over in that state. +If you want to slow further on approach to goal, consider increasing the ``threshold_to_consider`` parameters to give a hand off from the path tracking critics to the goal approach critics sooner - then tune those critics for your profile of interest. + ### Prediction Horizon, Costmap Sizing, and Offsets As this is a predictive planner, there is some relationship between maximum speed, prediction times, and costmap size that users should keep in mind while tuning for their application. If a controller server costmap is set to 3.0m in size, that means that with the robot in the center, there is 1.5m of information on either side of the robot. When your prediction horizon (time_steps * model_dt) at maximum speed (vx_max) is larger than this, then your robot will be artificially limited in its maximum speeds and behavior by the costmap limitation. For example, if you predict forward 3 seconds (60 steps @ 0.05s per step) at 0.5m/s maximum speed, the **minimum** required costmap radius is 1.5m - or 3m total width. From 8458c8a1f565c2a3b1816a2d7cff1b970fb22d29 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Tue, 20 Feb 2024 22:00:42 +0100 Subject: [PATCH 10/36] [behavior_tree] Add generate_nav2_tree_nodes_xml (#4073) * Add generate_nav2_tree_nodes_xml Signed-off-by: Davide Faconti * behavior_tree: removed list of plugins from yaml Signed-off-by: Davide Faconti * check result of get_parameter(plugin_lib_names) Signed-off-by: Davide Faconti * fix previous commit (uncrustify) Signed-off-by: Davide Faconti * revert change Signed-off-by: Davide Faconti --------- Signed-off-by: Davide Faconti --- nav2_behavior_tree/CMakeLists.txt | 12 ++++ nav2_behavior_tree/plugins_list.hpp.in | 6 ++ .../src/generate_nav2_tree_nodes_xml.cpp | 47 +++++++++++++ .../params/nav2_multirobot_params_1.yaml | 59 ++-------------- .../params/nav2_multirobot_params_2.yaml | 59 ++-------------- .../params/nav2_multirobot_params_all.yaml | 59 ++-------------- nav2_bringup/params/nav2_params.yaml | 59 ++-------------- nav2_bt_navigator/src/bt_navigator.cpp | 70 ++++--------------- .../behavior_tree/test_behavior_tree_node.cpp | 58 ++------------- .../src/costmap_filters/keepout_params.yaml | 54 ++------------ .../costmap_filters/speed_global_params.yaml | 55 ++------------- .../costmap_filters/speed_local_params.yaml | 55 ++------------- .../gps_navigation/nav2_no_map_params.yaml | 59 ++-------------- 13 files changed, 120 insertions(+), 532 deletions(-) create mode 100644 nav2_behavior_tree/plugins_list.hpp.in create mode 100644 nav2_behavior_tree/src/generate_nav2_tree_nodes_xml.cpp diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index fe539b0965f..fad9886c77e 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -222,6 +222,17 @@ install(TARGETS ${library_name} RUNTIME DESTINATION bin ) +# we will embed the list of plugin names inside a header file +set(GENERATED_DIR ${CMAKE_CURRENT_BINARY_DIR}/gen) +configure_file(plugins_list.hpp.in ${GENERATED_DIR}/plugins_list.hpp) + +add_executable(generate_nav2_tree_nodes_xml src/generate_nav2_tree_nodes_xml.cpp) +ament_target_dependencies(generate_nav2_tree_nodes_xml ${dependencies}) +# allow generate_nav2_tree_nodes_xml to find plugins_list.hpp +target_include_directories(generate_nav2_tree_nodes_xml PRIVATE ${GENERATED_DIR}) +install(TARGETS generate_nav2_tree_nodes_xml DESTINATION lib/${PROJECT_NAME}) + + install(DIRECTORY include/ DESTINATION include/ ) @@ -231,6 +242,7 @@ install(DIRECTORY test/utils/ ) install(FILES nav2_tree_nodes.xml DESTINATION share/${PROJECT_NAME}) +install(FILES ${GENERATED_DIR}/plugins_list.hpp DESTINATION include/${PROJECT_NAME}) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) diff --git a/nav2_behavior_tree/plugins_list.hpp.in b/nav2_behavior_tree/plugins_list.hpp.in new file mode 100644 index 00000000000..148583b9271 --- /dev/null +++ b/nav2_behavior_tree/plugins_list.hpp.in @@ -0,0 +1,6 @@ + +// This was automativally generated by cmake +namespace nav2::details +{ + const char* BT_BUILTIN_PLUGINS = "@plugin_libs@"; +} diff --git a/nav2_behavior_tree/src/generate_nav2_tree_nodes_xml.cpp b/nav2_behavior_tree/src/generate_nav2_tree_nodes_xml.cpp new file mode 100644 index 00000000000..01bdf6a6854 --- /dev/null +++ b/nav2_behavior_tree/src/generate_nav2_tree_nodes_xml.cpp @@ -0,0 +1,47 @@ +// Copyright (c) 2024 Davide Faconti +// +// 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. Reserved. + +#include +#include +#include +#include + +#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp_v3/utils/shared_library.h" +#include "behaviortree_cpp_v3/xml_parsing.h" + +#include "plugins_list.hpp" + +int main() +{ + BT::BehaviorTreeFactory factory; + + std::vector plugins_list; + boost::split(plugins_list, nav2::details::BT_BUILTIN_PLUGINS, boost::is_any_of(";")); + + for (const auto & plugin : plugins_list) { + std::cout << "Loading: " << plugin << "\n"; + factory.registerFromPlugin(BT::SharedLibrary::getOSName(plugin)); + } + std::cout << "\nGenerating file: nav2_tree_nodes.xml\n" + << "\nCompare it with the one in the git repo and update the latter if necessary.\n"; + + std::ofstream xml_file; + xml_file.open("nav2_tree_nodes.xml"); + xml_file << BT::writeTreeNodesModelXML(factory) << std::endl; + xml_file.close(); + + return 0; +} diff --git a/nav2_bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/params/nav2_multirobot_params_1.yaml index 172a832100d..91401fc21f9 100644 --- a/nav2_bringup/params/nav2_multirobot_params_1.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_1.yaml @@ -56,60 +56,11 @@ bt_navigator: # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. - plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_smooth_path_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_assisted_teleop_action_bt_node - - nav2_back_up_action_bt_node - - nav2_drive_on_heading_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_globally_updated_goal_condition_bt_node - - nav2_is_path_valid_condition_bt_node - - nav2_are_error_codes_active_condition_bt_node - - nav2_would_a_controller_recovery_help_condition_bt_node - - nav2_would_a_planner_recovery_help_condition_bt_node - - nav2_would_a_smoother_recovery_help_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_truncate_path_local_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_path_expiring_timer_condition - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_goal_updated_controller_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_planner_selector_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - - nav2_controller_cancel_bt_node - - nav2_path_longer_on_approach_bt_node - - nav2_wait_cancel_bt_node - - nav2_spin_cancel_bt_node - - nav2_back_up_cancel_bt_node - - nav2_assisted_teleop_cancel_bt_node - - nav2_drive_on_heading_cancel_bt_node - - nav2_is_battery_charging_condition_bt_node - - nav2_progress_checker_selector_bt_node - - nav2_smoother_selector_bt_node + + # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). + # Built-in plugins are added automatically + # plugin_lib_names: [] + error_code_names: - compute_path_error_code - follow_path_error_code diff --git a/nav2_bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/params/nav2_multirobot_params_2.yaml index da426b4722f..732fa24eb12 100644 --- a/nav2_bringup/params/nav2_multirobot_params_2.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_2.yaml @@ -56,60 +56,11 @@ bt_navigator: # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. - plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_smooth_path_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_assisted_teleop_action_bt_node - - nav2_back_up_action_bt_node - - nav2_drive_on_heading_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_globally_updated_goal_condition_bt_node - - nav2_is_path_valid_condition_bt_node - - nav2_are_error_codes_active_condition_bt_node - - nav2_would_a_controller_recovery_help_condition_bt_node - - nav2_would_a_planner_recovery_help_condition_bt_node - - nav2_would_a_smoother_recovery_help_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_truncate_path_local_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_path_expiring_timer_condition - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_goal_updated_controller_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_planner_selector_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - - nav2_controller_cancel_bt_node - - nav2_path_longer_on_approach_bt_node - - nav2_wait_cancel_bt_node - - nav2_spin_cancel_bt_node - - nav2_back_up_cancel_bt_node - - nav2_assisted_teleop_cancel_bt_node - - nav2_drive_on_heading_cancel_bt_node - - nav2_is_battery_charging_condition_bt_node - - nav2_progress_checker_selector_bt_node - - nav2_smoother_selector_bt_node + + # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). + # Built-in plugins are added automatically + # plugin_lib_names: [] + error_code_names: - compute_path_error_code - follow_path_error_code diff --git a/nav2_bringup/params/nav2_multirobot_params_all.yaml b/nav2_bringup/params/nav2_multirobot_params_all.yaml index 8249df84d17..bb812d276b8 100644 --- a/nav2_bringup/params/nav2_multirobot_params_all.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_all.yaml @@ -56,60 +56,11 @@ bt_navigator: # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. - plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_smooth_path_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_assisted_teleop_action_bt_node - - nav2_back_up_action_bt_node - - nav2_drive_on_heading_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_globally_updated_goal_condition_bt_node - - nav2_is_path_valid_condition_bt_node - - nav2_are_error_codes_active_condition_bt_node - - nav2_would_a_controller_recovery_help_condition_bt_node - - nav2_would_a_planner_recovery_help_condition_bt_node - - nav2_would_a_smoother_recovery_help_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_truncate_path_local_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_path_expiring_timer_condition - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_goal_updated_controller_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_planner_selector_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - - nav2_controller_cancel_bt_node - - nav2_path_longer_on_approach_bt_node - - nav2_wait_cancel_bt_node - - nav2_spin_cancel_bt_node - - nav2_back_up_cancel_bt_node - - nav2_assisted_teleop_cancel_bt_node - - nav2_drive_on_heading_cancel_bt_node - - nav2_is_battery_charging_condition_bt_node - - nav2_progress_checker_selector_bt_node - - nav2_smoother_selector_bt_node + + # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). + # Built-in plugins are added automatically + # plugin_lib_names: [] + error_code_names: - compute_path_error_code - follow_path_error_code diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 28b23ab2897..28e7d273fba 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -56,60 +56,11 @@ bt_navigator: # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. - plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_smooth_path_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_assisted_teleop_action_bt_node - - nav2_back_up_action_bt_node - - nav2_drive_on_heading_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_globally_updated_goal_condition_bt_node - - nav2_is_path_valid_condition_bt_node - - nav2_are_error_codes_active_condition_bt_node - - nav2_would_a_controller_recovery_help_condition_bt_node - - nav2_would_a_planner_recovery_help_condition_bt_node - - nav2_would_a_smoother_recovery_help_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_truncate_path_local_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_path_expiring_timer_condition - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_goal_updated_controller_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_planner_selector_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - - nav2_controller_cancel_bt_node - - nav2_path_longer_on_approach_bt_node - - nav2_wait_cancel_bt_node - - nav2_spin_cancel_bt_node - - nav2_back_up_cancel_bt_node - - nav2_assisted_teleop_cancel_bt_node - - nav2_drive_on_heading_cancel_bt_node - - nav2_is_battery_charging_condition_bt_node - - nav2_progress_checker_selector_bt_node - - nav2_smoother_selector_bt_node + + # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). + # Built-in plugins are added automatically + # plugin_lib_names: [] + error_code_names: - compute_path_error_code - follow_path_error_code diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index c588e6ff4d0..4c1b4f47bca 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -18,12 +18,15 @@ #include #include #include +#include #include "nav2_util/geometry_utils.hpp" #include "nav2_util/node_utils.hpp" #include "nav2_util/robot_utils.hpp" #include "nav2_behavior_tree/bt_utils.hpp" +#include "nav2_behavior_tree/plugins_list.hpp" + using nav2_util::declare_parameter_if_not_declared; namespace nav2_bt_navigator @@ -36,64 +39,8 @@ BtNavigator::BtNavigator(rclcpp::NodeOptions options) { RCLCPP_INFO(get_logger(), "Creating"); - const std::vector plugin_libs = { - "nav2_compute_path_to_pose_action_bt_node", - "nav2_compute_path_through_poses_action_bt_node", - "nav2_smooth_path_action_bt_node", - "nav2_follow_path_action_bt_node", - "nav2_spin_action_bt_node", - "nav2_wait_action_bt_node", - "nav2_assisted_teleop_action_bt_node", - "nav2_back_up_action_bt_node", - "nav2_drive_on_heading_bt_node", - "nav2_clear_costmap_service_bt_node", - "nav2_is_stuck_condition_bt_node", - "nav2_goal_reached_condition_bt_node", - "nav2_initial_pose_received_condition_bt_node", - "nav2_goal_updated_condition_bt_node", - "nav2_globally_updated_goal_condition_bt_node", - "nav2_is_path_valid_condition_bt_node", - "nav2_are_error_codes_active_condition_bt_node", - "nav2_would_a_controller_recovery_help_condition_bt_node", - "nav2_would_a_planner_recovery_help_condition_bt_node", - "nav2_would_a_smoother_recovery_help_condition_bt_node", - "nav2_reinitialize_global_localization_service_bt_node", - "nav2_rate_controller_bt_node", - "nav2_distance_controller_bt_node", - "nav2_speed_controller_bt_node", - "nav2_truncate_path_action_bt_node", - "nav2_truncate_path_local_action_bt_node", - "nav2_goal_updater_node_bt_node", - "nav2_recovery_node_bt_node", - "nav2_pipeline_sequence_bt_node", - "nav2_round_robin_node_bt_node", - "nav2_transform_available_condition_bt_node", - "nav2_time_expired_condition_bt_node", - "nav2_path_expiring_timer_condition", - "nav2_distance_traveled_condition_bt_node", - "nav2_single_trigger_bt_node", - "nav2_goal_updated_controller_bt_node", - "nav2_is_battery_low_condition_bt_node", - "nav2_navigate_through_poses_action_bt_node", - "nav2_navigate_to_pose_action_bt_node", - "nav2_remove_passed_goals_action_bt_node", - "nav2_planner_selector_bt_node", - "nav2_controller_selector_bt_node", - "nav2_goal_checker_selector_bt_node", - "nav2_controller_cancel_bt_node", - "nav2_path_longer_on_approach_bt_node", - "nav2_wait_cancel_bt_node", - "nav2_spin_cancel_bt_node", - "nav2_assisted_teleop_cancel_bt_node", - "nav2_back_up_cancel_bt_node", - "nav2_drive_on_heading_cancel_bt_node", - "nav2_is_battery_charging_condition_bt_node", - "nav2_progress_checker_selector_bt_node", - "nav2_smoother_selector_bt_node" - }; - declare_parameter_if_not_declared( - this, "plugin_lib_names", rclcpp::ParameterValue(plugin_libs)); + this, "plugin_lib_names", rclcpp::ParameterValue(std::vector{})); declare_parameter_if_not_declared( this, "transform_tolerance", rclcpp::ParameterValue(0.1)); declare_parameter_if_not_declared( @@ -126,7 +73,14 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) odom_topic_ = get_parameter("odom_topic").as_string(); // Libraries to pull plugins (BT Nodes) from - auto plugin_lib_names = get_parameter("plugin_lib_names").as_string_array(); + std::vector plugin_lib_names; + boost::split(plugin_lib_names, nav2::details::BT_BUILTIN_PLUGINS, boost::is_any_of(";")); + + auto user_defined_plugins = get_parameter("plugin_lib_names").as_string_array(); + // append user_defined_plugins to plugin_lib_names + plugin_lib_names.insert( + plugin_lib_names.end(), user_defined_plugins.begin(), + user_defined_plugins.end()); nav2_core::FeedbackUtils feedback_utils; feedback_utils.tf = tf_; diff --git a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp index 51558d9ba16..b3ac057172b 100644 --- a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp +++ b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include #include "gtest/gtest.h" @@ -31,6 +32,8 @@ #include "nav2_util/odometry_utils.hpp" +#include "nav2_behavior_tree/plugins_list.hpp" + #include "rclcpp/rclcpp.hpp" #include "ament_index_cpp/get_package_share_directory.hpp" @@ -58,58 +61,9 @@ class BehaviorTreeHandler odom_smoother_ = std::make_shared(node_); - const std::vector plugin_libs = { - "nav2_compute_path_to_pose_action_bt_node", - "nav2_compute_path_through_poses_action_bt_node", - "nav2_smooth_path_action_bt_node", - "nav2_follow_path_action_bt_node", - "nav2_spin_action_bt_node", - "nav2_wait_action_bt_node", - "nav2_assisted_teleop_action_bt_node", - "nav2_back_up_action_bt_node", - "nav2_drive_on_heading_bt_node", - "nav2_clear_costmap_service_bt_node", - "nav2_is_stuck_condition_bt_node", - "nav2_goal_reached_condition_bt_node", - "nav2_initial_pose_received_condition_bt_node", - "nav2_goal_updated_condition_bt_node", - "nav2_globally_updated_goal_condition_bt_node", - "nav2_is_path_valid_condition_bt_node", - "nav2_are_error_codes_active_condition_bt_node", - "nav2_would_a_controller_recovery_help_condition_bt_node", - "nav2_would_a_planner_recovery_help_condition_bt_node", - "nav2_would_a_smoother_recovery_help_condition_bt_node", - "nav2_reinitialize_global_localization_service_bt_node", - "nav2_rate_controller_bt_node", - "nav2_distance_controller_bt_node", - "nav2_speed_controller_bt_node", - "nav2_truncate_path_action_bt_node", - "nav2_truncate_path_local_action_bt_node", - "nav2_goal_updater_node_bt_node", - "nav2_recovery_node_bt_node", - "nav2_pipeline_sequence_bt_node", - "nav2_round_robin_node_bt_node", - "nav2_transform_available_condition_bt_node", - "nav2_time_expired_condition_bt_node", - "nav2_path_expiring_timer_condition", - "nav2_distance_traveled_condition_bt_node", - "nav2_single_trigger_bt_node", - "nav2_is_battery_low_condition_bt_node", - "nav2_navigate_through_poses_action_bt_node", - "nav2_navigate_to_pose_action_bt_node", - "nav2_remove_passed_goals_action_bt_node", - "nav2_planner_selector_bt_node", - "nav2_controller_selector_bt_node", - "nav2_goal_checker_selector_bt_node", - "nav2_controller_cancel_bt_node", - "nav2_path_longer_on_approach_bt_node", - "nav2_assisted_teleop_cancel_bt_node", - "nav2_wait_cancel_bt_node", - "nav2_spin_cancel_bt_node", - "nav2_back_up_cancel_bt_node", - "nav2_drive_on_heading_cancel_bt_node", - "nav2_goal_updated_controller_bt_node" - }; + std::vector plugin_libs; + boost::split(plugin_libs, nav2::details::BT_BUILTIN_PLUGINS, boost::is_any_of(";")); + for (const auto & p : plugin_libs) { factory_.registerFromPlugin(BT::SharedLibrary::getOSName(p)); } diff --git a/nav2_system_tests/src/costmap_filters/keepout_params.yaml b/nav2_system_tests/src/costmap_filters/keepout_params.yaml index 5c948331f00..9f9bdd33dea 100644 --- a/nav2_system_tests/src/costmap_filters/keepout_params.yaml +++ b/nav2_system_tests/src/costmap_filters/keepout_params.yaml @@ -47,56 +47,10 @@ bt_navigator: # files to allow for a commandline change default used is the # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml & # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml - plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_smooth_path_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_assisted_teleop_action_bt_node - - nav2_back_up_action_bt_node - - nav2_drive_on_heading_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_are_error_codes_active_condition_bt_node - - nav2_would_a_controller_recovery_help_condition_bt_node - - nav2_would_a_planner_recovery_help_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_is_path_valid_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_truncate_path_local_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_goal_updated_controller_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_controller_cancel_bt_node - - nav2_path_longer_on_approach_bt_node - - nav2_wait_cancel_bt_node - - nav2_spin_cancel_bt_node - - nav2_back_up_cancel_bt_node - - nav2_assisted_teleop_cancel_bt_node - - nav2_drive_on_heading_cancel_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - - nav2_planner_selector_bt_node - - nav2_smoother_selector_bt_node - - nav2_progress_checker_selector_bt_node + + # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). + # Built-in plugins are added automatically + # plugin_lib_names: [] controller_server: ros__parameters: diff --git a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml index c887ac3ce57..10040d87983 100644 --- a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml @@ -47,57 +47,10 @@ bt_navigator: # files to allow for a commandline change default used is the # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml & # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml - plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_smooth_path_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_assisted_teleop_action_bt_node - - nav2_back_up_action_bt_node - - nav2_drive_on_heading_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_are_error_codes_active_condition_bt_node - - nav2_would_a_controller_recovery_help_condition_bt_node - - nav2_would_a_planner_recovery_help_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_globally_updated_goal_condition_bt_node - - nav2_is_path_valid_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_truncate_path_local_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_goal_updated_controller_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_controller_cancel_bt_node - - nav2_path_longer_on_approach_bt_node - - nav2_wait_cancel_bt_node - - nav2_spin_cancel_bt_node - - nav2_back_up_cancel_bt_node - - nav2_assisted_teleop_cancel_bt_node - - nav2_drive_on_heading_cancel_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - - nav2_planner_selector_bt_node - - nav2_smoother_selector_bt_node - - nav2_progress_checker_selector_bt_node + + # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). + # Built-in plugins are added automatically + # plugin_lib_names: [] controller_server: ros__parameters: diff --git a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml index 8bfe04488d5..b50108a13fc 100644 --- a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml @@ -47,57 +47,10 @@ bt_navigator: # files to allow for a commandline change default used is the # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml & # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml - plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_smooth_path_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_assisted_teleop_action_bt_node - - nav2_back_up_action_bt_node - - nav2_drive_on_heading_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_are_error_codes_active_condition_bt_node - - nav2_would_a_controller_recovery_help_condition_bt_node - - nav2_would_a_planner_recovery_help_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_globally_updated_goal_condition_bt_node - - nav2_is_path_valid_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_truncate_path_local_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_goal_updated_controller_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_controller_cancel_bt_node - - nav2_path_longer_on_approach_bt_node - - nav2_wait_cancel_bt_node - - nav2_spin_cancel_bt_node - - nav2_back_up_cancel_bt_node - - nav2_assisted_teleop_cancel_bt_node - - nav2_drive_on_heading_cancel_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - - nav2_planner_selector_bt_node - - nav2_smoother_selector_bt_node - - nav2_progress_checker_selector_bt_node + + # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). + # Built-in plugins are added automatically + # plugin_lib_names: [] controller_server: ros__parameters: diff --git a/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml b/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml index d11ef6ab535..6e9f3f490bf 100644 --- a/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml +++ b/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml @@ -15,60 +15,11 @@ bt_navigator: # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. - plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_smooth_path_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_assisted_teleop_action_bt_node - - nav2_back_up_action_bt_node - - nav2_drive_on_heading_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_globally_updated_goal_condition_bt_node - - nav2_is_path_valid_condition_bt_node - - nav2_are_error_codes_active_condition_bt_node - - nav2_would_a_controller_recovery_help_condition_bt_node - - nav2_would_a_planner_recovery_help_condition_bt_node - - nav2_would_a_smoother_recovery_help_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_truncate_path_local_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_path_expiring_timer_condition - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_goal_updated_controller_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_planner_selector_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - - nav2_controller_cancel_bt_node - - nav2_path_longer_on_approach_bt_node - - nav2_wait_cancel_bt_node - - nav2_spin_cancel_bt_node - - nav2_back_up_cancel_bt_node - - nav2_assisted_teleop_cancel_bt_node - - nav2_drive_on_heading_cancel_bt_node - - nav2_is_battery_charging_condition_bt_node - - nav2_progress_checker_selector_bt_node - - nav2_smoother_selector_bt_node + + # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). + # Built-in plugins are added automatically + # plugin_lib_names: [] + error_code_names: - compute_path_error_code - follow_path_error_code From 55c952d953ce4dd9a141844d4c80894968a5bb56 Mon Sep 17 00:00:00 2001 From: Ramon Wijnands Date: Wed, 21 Feb 2024 17:59:36 +0100 Subject: [PATCH 11/36] Fix compilation with clang (#4131) Signed-off-by: Ramon Wijnands --- nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp | 2 +- nav2_collision_monitor/test/velocity_polygons_test.cpp | 1 - nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp | 2 +- nav2_rviz_plugins/src/selector.cpp | 2 +- 4 files changed, 3 insertions(+), 4 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp index 7e8ea23ec73..a3b42011f0a 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp @@ -145,7 +145,7 @@ T1 deconflictPortAndParamFrame( const T2 * behavior_tree_node) { T1 param_value; - bool param_from_input = behavior_tree_node->getInput(param_name, param_value); + bool param_from_input = behavior_tree_node->getInput(param_name, param_value).has_value(); if constexpr (std::is_same_v) { // not valid if port doesn't exist or it is an empty string diff --git a/nav2_collision_monitor/test/velocity_polygons_test.cpp b/nav2_collision_monitor/test/velocity_polygons_test.cpp index 7030ddd2b20..4fd4f8bc0b3 100644 --- a/nav2_collision_monitor/test/velocity_polygons_test.cpp +++ b/nav2_collision_monitor/test/velocity_polygons_test.cpp @@ -40,7 +40,6 @@ using namespace std::chrono_literals; static constexpr double EPSILON = std::numeric_limits::epsilon(); static const char BASE_FRAME_ID[]{"base_link"}; -static const char BASE2_FRAME_ID[]{"base2_link"}; static const char POLYGON_PUB_TOPIC[]{"polygon_pub"}; static const char POLYGON_NAME[]{"TestVelocityPolygon"}; static const char SUB_POLYGON_FORWARD_NAME[]{"Forward"}; diff --git a/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp b/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp index d674ee76691..849edf282c7 100644 --- a/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp +++ b/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp @@ -53,7 +53,7 @@ class TestCostmapSubscriberShould : public ::testing::Test costmapToSend = std::make_shared(10, 10, 1.0, 0.0, 0.0); } - void TearDown() + void TearDown() override { costmapSubscriber.reset(); costmapToSend.reset(); diff --git a/nav2_rviz_plugins/src/selector.cpp b/nav2_rviz_plugins/src/selector.cpp index f0f3d4e3358..1df075f82d3 100644 --- a/nav2_rviz_plugins/src/selector.cpp +++ b/nav2_rviz_plugins/src/selector.cpp @@ -169,7 +169,7 @@ void Selector::pluginLoader( } RCLCPP_INFO( node->get_logger(), - (server_name + " service not available").c_str()); + "%s service not available", server_name.c_str()); server_unavailable = true; server_failed_ = true; break; From 2e491b18328ede064c75bfa024f57c7e49578eba Mon Sep 17 00:00:00 2001 From: iceuw Date: Sat, 24 Feb 2024 00:55:37 +0800 Subject: [PATCH 12/36] Fix goal handle was not freed correctly (#4137) * Fix goal handle was not freed correctly * Update nav2_util/include/nav2_util/simple_action_server.hpp Signed-off-by: Steve Macenski --------- Signed-off-by: Steve Macenski Co-authored-by: Ziohang Co-authored-by: Steve Macenski --- nav2_util/include/nav2_util/simple_action_server.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_util/include/nav2_util/simple_action_server.hpp b/nav2_util/include/nav2_util/simple_action_server.hpp index 71828321f00..eec9c2dd9f7 100644 --- a/nav2_util/include/nav2_util/simple_action_server.hpp +++ b/nav2_util/include/nav2_util/simple_action_server.hpp @@ -574,7 +574,7 @@ class SimpleActionServer * @param the Results object to terminate the action with */ void terminate( - std::shared_ptr> handle, + std::shared_ptr> & handle, typename std::shared_ptr result = std::make_shared()) { From 865fd3274fff213fd5dfa488b49cd8b2361018e4 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Sat, 24 Feb 2024 10:28:36 -0800 Subject: [PATCH 13/36] Adding Nvidia as Gold Sponsor! (#4144) --- README.md | 2 +- doc/sponsors_feb_2024.png | Bin 0 -> 196892 bytes 2 files changed, 1 insertion(+), 1 deletion(-) create mode 100644 doc/sponsors_feb_2024.png diff --git a/README.md b/README.md index e8eecf89439..3fd23f972eb 100644 --- a/README.md +++ b/README.md @@ -27,7 +27,7 @@ If you need professional services related to Nav2, please contact Open Navigatio Please thank our amazing sponsors for their generous support of Nav2 on behalf of the community to allow the project to continue to be professionally maintained, developed, and supported for the long-haul! [Open Navigation LLC](https://www.opennav.org/) provides project leadership, maintenance, development, and support services to the Nav2 & ROS community.

- +

### [Dexory](https://www.dexory.com/) develops robotics and AI logistics solutions to drive better business decisions using a digital twin of warehouses to provide inventory insights. diff --git a/doc/sponsors_feb_2024.png b/doc/sponsors_feb_2024.png new file mode 100644 index 0000000000000000000000000000000000000000..87dbbef555cfa2b60c975fef9b00d9e248f5298e GIT binary patch literal 196892 zcmeEt=UbCo`z4BiO7k291O%)oz4xl9NEM}bq<18gh@q&cC{=nFkS1M9XrV|ifrJjB zMS*}o2m+D-q0GkfzBA`{%|9?7hA&*{&$IVk*1gu+A9Qup=+CpCr=XyqSATf#2?fPj zS_+CY@6S?$XM)d+f`2b5)bHKVe{PANJr}@fU*EkMSN+y*4jNlst?e+Uo_3cR&gXKm zRgzT$JkmS=B1LhWdz%w$s^iH@ z-cN>ld1qsPr*9uMXQGewI%*ab9221qpZu{JRN(jj|KI;#f&WKWz_e#|tk|l-Wvqz5 z<`%zEnZm^3{=#NJ`ynQ}zOmscII{YTSW7`bVDs=Ov1l)RiufG%7CRrWZR@uHz*s21gtJTn>#q54ub1DmGl=0*8Jp1{P-jPjHf66vH!n5F-YN;Ym(J} z*ld(=Jd=3Ye=#On;5g`iz9G*|pums%Bpc_rG3~c**I`a*2b(ReAMG4%b2un#MvZ@i^TM*O!o#bbR?=?{vCg=F0MH zFAd3@H)Fg^(*Co5rvKMUt__@)PqU(V(B_s6HEnqy4jIhIwKVRZrMlg@bY52znW!P%Q4L z|1?j>@|Qg%43{iKR`ZX>1a?iqX8rG@0|P_nrut^M8teas;YCK0~gNV9x}{pJ+QC(#?m;7;!0S;#XgeZxOe; zP735$d%-mFrD;A&+kATB+JyLr%bBMrzA3L6%QQT^wResWrE@isi<~*Nc#Ed1`A!Pn zmw?Uql7m0P=_t%#18dZ%%<;_(tU+Aq^RdaqjAbhFMpOmLM{3tdu>Ynj`58l_lF%wi zc)fE<$WXL&kw@dB0P>c{^qXA{&Sl%D@tou0BBOe9j(%yKe&{RgY@=Pxg$LXF$=~ce zM-TVJ>spsG{WXhByY%t2f0K2RjA#z`n4BMU))D7h4;!z}Hd)&d6Z2D1yMsq%c9`~= zo!*7^NhO2(ev^lCMcRf2kDI3~x-WCwSlMpxC>yE>ObHuKng~A#ennQ?g5l70uAdU* zG%}e}15(703_?d9aRejtjgIz1NYiyj)x{SD&y9^FE`BYwF@^V1+SY0dWj?37S;|(g zW_xUVaMbTQyGdY%1Un!_HEXD5ljy0u_TMPR`FJ^2_Wzvwu?I=#OD(u$rjs%`R_oW? zgH_suZ3_~WRcj;tNad3S=C`UFYiuGM9u)DRu%cemzXXXH9szs9gts~2eQ!@we1p0_ zmDuuuRvy6Rot0El`O*?9IA*Ra>yXg3b>Ty<)xAgaN2L2g%HGEW8bWaJz~Me`611Ln zthNq&TqKgn;f;CPN%|xZ>}CY%Y0phjkt=hx*ziHvuAVsz?SN#PI>JC;&amHGQN!I) z^UuV%vrH@c#?sekuG1y$_y#$Y&!e)T4c7k-axZ8hmzjE>uwbEhGJKxM2N z&uDL9PQ`yazi^)$jtpV-?-~0GN2?mA+*lKAN8=jD_16O;)v(s*NJaJg7})Gi&#jqb z(#@oey5OS^)6qdVNCjCjsm^^xgS1iZ-964>PIz{FY~gE*Unh``wNrk}F&7xsW66$5 z;WOA{P12ePVPmg`eEi7!_&}XRYLb4xElOJBZ2})2X0sm zb_MBstb$HT&!vWVv3{yS*g~w^qGUWj1?Rxd!#HsI&8YT*ByG>`6|M|^&%mPj2abNh z8JW`sJ@j4t2JtP8buqNu)leRql)>C`=)xD*H8-YDoej1xoAgr-E+sDhy6^k$@{W?uS71k>-iS5TS4j61o#g*n0;w;kSe2wie5mkhJtcygbz`^`psZBNL& z@);h^rw?eY^d0djm#>K{9vbON9O_K+1Rx(U=Pmcjd`$5>MUnj8;Z)0YR$4LZ#)`UD zR{IwH;d|DV=$}2G7KNTfCpfEy{QP(e^&ynZ>g|0A0p5IfHUFmIK(sE)Gs$~ zk3V%M7AJ?Y`ujnB|KGpv8%R*)wipb{Fc)kM;w;B5d%9@kgddbhJI7h@MAF}9x|1Gg zc@u*O%dfjVAQyMgD|WNA|C6o|-B(H{mMik9G#qE=Kfn5Z8CnwiphPeC&ZETE;h~=E zuEG%OP{BgN)ny5LJDyZG)r@^|A}qebD&DmbQaJd+PMs%VQ8>u~1+LOFX7js&hZcGG zj>EH46j?&4R1am-y5w~{A@wu3)650fSenM{-YA2|pLna>TBgzxRK>fo9og5Jrr-Wq zk~?5YUS&sZ;?6b~e6~qU-9i)e799wseRi4Shwafz+M|Y2h|Y-v9+*X)7sf!Y;n+rl$S5WR0QG4lieU_<)HUqATfoom*owE|nZV;GqQ#iK>iq zrf~NPadlu8=Vi7{k6%|p5#miM*tb3bP(c6M;Z$X5PiIHq@u12t8cs(mC~IN4cS{*v z&hM3s&7ZGr&ZA#tGhp)jIM}{Y{gY`@v@(!@B0H21D=198v#&0^h9g!-! znpX~|#4s^S`Jeq&#<=zmq1MB!MK{oLk=jcr3ogTVxrOH#v$Y4T=GI(DItFHZ1?INc zdDJG61W}ImHZ3kf0nnoMr6Q6!seZXUE=HQ-M~{&BuoXRQ!bCY2DIwR>Al! zF3tksn47c>u|5?FDo=Y)j{-<^)pulY;PhRO2SqoOJJ(xgtoc#F#DI*5%R+i-iLP(+ z$E9E!F129~qJ$D1`GQGD_qlY<`K2dtKaA>)ozA<9@g#-b5HovlY5Fmg4wk1g%*bnt z9^pZ#?u}jiniF$#+5^%Z;%z=_C;Kj`kZSKLCv7O}3TTq)l)5wnr|pWacjFN=q6f7Q z3qfrw3-yK;bI0xPq^oc!X*w{zKhcl_8^5+{HeSClDxIKC;%7atT|Ry}&aT|I`+eG4 z0IB(Y>dm8zU3*^YPu2xkK^Q22Vj#vAgIeU6>FhF*tKojaLzeb8E95sJiul7m6sA`E zv-j=NwEBD@RdV)C=*b|fu{jUEU~16#pitMiKy?yE%^>-4fp1SkO>gDltnt#C$K{1X zKLW()g1yx_8}cql)Sm#&Q5*jLc@(1t?cTfkG(Gdn!fz%*oo`uLQwASnD-tX1|8ta` zM$0cTmZo*s=PCY<*v(x$MiarGPOA$RKBYO?P1PE2|0>4Yci%yl$M-jba9GN@FG@zN z{%Mx?J0WRIYb#a+eXo7Ol%jm~`|aklMN1ADgL-Q0&$Pjzi!2q6b@kU z5gS-fu`2t(rsCQAlWcO8+zGr}atnB8&d1FkS2h9Y23vh*;BbpEq1a@W>|%PRLKxrGEMm#l}sm?dRs`d9*l1cNs7IDF`E^ z3I956e)PQtKGG%=801t*iC~TB=dxOh3paW+D4_CiRzkf>B~MBlQ9<)_GDx-J@+%3t zo)_56a_tdRto%T+oNCH}mmDg$eu!)$?t*liEbnVyhU5*3O!0+;RM`p4a^fE8d%Z~S zzjh%RFJ{5vUDf~Xw^|w0!yu+NTO(G{Hyzm}uN zYM4R`2fN%l)R$S<=BLX^s&R_aUnJk}&*?5JwKSRJJ2SeBRC{}tnF!Xvia`1k$c>f6 zCY{}~z_5U$Y5mwh6%xiD_v94C%YD!^N7&fDx{r3v5g>k-9B=vE9A{xI+kRI)&E-Eh zJP%&a^++u+$$Ell&DRckL#_`lsQc?`<}tN>Wi7HgpV{N9WOjughU8=3__keQ{XRXu zynQmNZyJhod0OkL2?{nCPr$HJIR9I+FBd!J-V|Ncs}d?+`?yzAYzR=OY5LLe1`bX`f@BE;gbHSgXg5A2anv`G!OcrgXV*F$h02=TSH$xjg6&sAR<>dyC6a9YMQH--(}bHPu~Ce0TOzvJA%PcqTfAWfg z0)OFT!R$(ro-^Nxwtb7;J=L{fb{4`YAvpw6IsWwB5rA;D&m}BIdnuDw!f)XfG6~a$=X3=X)dAU=C zaWU<)dpngM;9DX*i0kjyL!BD-vgaf>YTpF)<(D?x!CUR9|&gNb+ll+`z&tcFSW7S4Cw+c)dvkQE|2Bte-+ z5TDNhnCrY1%5RG;cX4f;*wbn1d*HvIJ7?;+_v*pp*&J~$iwhlo`rX73g8aT@RlP*u zA`g*MNcp@iS;sz+vxIsvI|A)Vs<{kSb>UVY!FxGDQ#CG7Ck1bvBC1t0yVZq%B_( z`RXn~`|$zNfzuI+FrTkMj>zTUf0f;^z&~g`@@uRGTY0uHE#Hl0vT__|jZcg3Ei2!y z4+t+|4a%0<`%9uhQpqGprF%tl2I_Y|V`I2|vSmevySU$i3jl`|!;GtI)5!Rp1zVof zGp536Gn>mSoYUSjvI~)NCQ=ff>IOy!0mc(9AOZ*e_7@3=mS;Ka$Z)l^P%z(yB)$Se zr@-_hH6njlXlX1xp@GnS$BLMHemVES1P^^EU$4$%_=Aou=!zL(qRTR0+ptu>SiG+j z@ITWJswkXCdwlA|I#)C8{a*{kMfFS@h%pzt)#B*c2L4Gfb%2b)Ts!Nvegn z7!JK;;FCTl=;`M}%p;38`B0j z`+sT-%UX1CW!aJ)!0=MPp*H?$Cy>J(_b?9QP9Q58r5#v>936b9aui-qd>6B|^eX=j zxVW00{ke&^JjXssY;1)CA8{DbFHmFks&X-+O{eTcB<=dTM?`>2w~CbRk2$l2*!3D^ zGhX@@-)1%JHBz)^V%G;iL}@IDQ&7l%a5yzghsj#NlL!-fi-N`0PuWEUO}+nVfpZS0 z3P2g4I^4^?V~1-^Y|PU?3TVa|rieA(>SHymYaM@CB;F=7ZN&Z;^K`LY)3jyqFlT{g z6HI1Mq)5C0bQy;E-{%O3=P6;^8@ybGyNQBGrU3Z}Jo~KjmXmC)p-ia6br6@{U3R4U zilMiClSVx<&RN9RcfwO`&z;J|_SVb=J<@eKj_13s5H*aVjmFt02cM7kpWm;#Rewmt z%V9G;F_(J;czMXC(3aT(bKd{d$ANf+orVN+!7*3Y^+$!iO;ufv;@N-N`l`dJtKGKH zfS|mvy4L(fdO_P0GU{Nrol7FhPcqoQeC8~4nora5qF%OR@VN#2J^!30a{;Y-DgR4M zsRuv$a?xJc&f|uSMbrCC;b_caSFcgM`;WM%>0DD+wH*aqN8CU-YNy-OoFVurW-2I`JIT>TpjI1{Jf1AiPTdX(=&^r zh|Jb@$QkAd&L&5d@di5{b|i1UCdH&Li3f zwN5$&7FpoqX3x31MZ%elBepjm+0jYL)HxW|nyM~n1AxW!>afUH6z(*X?uD-!HtduY0sdy`d(+I3e7*~P0SD)C7@0iMqk*?~#(ITi%`2e|xHF^g4se^kAEaX>bP12;INIoq=Tde@1o=Hu} z62A`jZNL}9`YR9lHl40a`}(V@*@6id;7^d51={qBfe-~lxHP9irpEMKxh>2&IYOa4 zR$m_3*;dWxndaLpTs}|Kh9!h;C85opjNxpa7WG{0rrBWrop+rezgvwQ3&Wi0QB_0b z3{QF&Dg-2&P%lQ_wc?tR=ELr~5uwKVQ2fP}s!8pGvH;}aRBjTOGcU28`X-TWl7$03)AVZE z3|0S*97?Gi^hw++pFFW1GYI`Z1dBl`6<_D*)X=b+OI*fkRBvOGg+Q-$@ZeuK^p{^m zqK97pi=j+GsO?+Yc#^ij*51f;Ld7wsfL0EqROIH=@&Ly$UoY3%>QUG?yB2*o&nax} z{mMfO9#sf+u|@m&4{=rcJcL})c8WZyYA!LgNYC*K#R1M>hExELp#C|YvYSp9y7sd9 zP-r6ovz?dnpJvy)1~ZGBng&!cv-YUV(|HRAH^bP5Z-~8Xew>D1?7Kn=I%PdIC@{DM zg1wZg#gs%|RDJ7`ji+9v zP-8DQioRy{&fEngcR^%=bjNcU4oA|ejJ)cct^+KcK~)^r`n2L+c(2mJYH*c}{0(_- z*#b+>OXSDQ1)3@LSyxY%RdS1Gn*{HT{=x|}HBBoCCUpHeCMa$D`nvou%YC}ID$;Ao zk=jn=W2GIjR`+F`%~V!pgFVjJ=&tRN*%zt}{>1WII2xIEnnLSpDwQc0z#?PUSj8sl z@BDUzvjzDTRGgC@Vh8gypbs@AY2Q!1$Ryf_@nUbLy_O+|hvE5OP`?9XO`ATut1#Ep zB{DThamI8Ru_?$gE4v#lr(-2Ir~9FBglCPsszm-GQd2(e<;S4c72B(Ep#X|-&DbcC zQS^A^KQaPW>8|nFBm^_obZ_d0Ns{ihtLt-(=uDk!q zF8&AktBso7D`d;Wn3^d+seC0OT3z{yJ zFclyEmUoDUv13-s*uaZ#q(?*i=^kv8x6>yBuyu9(Ce3wE{9mp=>x+OR&64DL-&I#^ zKz&KsI-TLC$JwpOp6I-%Y6ko9|@ z^yoXpg^jd;!lj+q@V(1l6;!3&3(ki77Dlnn=E6b_mz+$74{ix()onKj@Eewe0IIQ{ zUd%Shd^y4OEw8S?MiOp;gb(x#D3dJdAhyV{;AsEEALW1J?>egl&GVg<;-tNGgFd@ssS{DG-Z)vyc0FTrm`bj~)7Q79 z(2#r%8>&`8TAQ?F?^@Z8%bhYQW8t}<*7)jD7pD1Xd=qNykaTDLY1r~KSIWB{DV_n9 zmT{iIrvNX6I{QQUQ>oqyV}90bYc|-BE5|Kkv*c8TzedMOuQCr5#A;XOCWNUaLdsHH z#@Gm^`+s>%lV^65D-SGoS!9>Y2La>T|B;#kaclW4OnW+IEt$}3pa9B(lU{cTrM9VA~8SrRA2h`cr0T3g}`-&N)m z49tyq)rV`#b)p?;g@G>2`+i-~x0w=qZDDoouC3KgI4|JCM6{h)oczH}WL{&f3I>2d z-5q~tOXLpm)t1hy!hh($HZZ){G@)%up5mbi>Umzp2YZKM>YheCZt;w${aB)JK!znMp|Aj11#^1V@7%)IrmGaJhZ5>?0&luqi4OJ3Op?4H1V3 zB50HdI^?D425(#=d3i!xg2KG$Dew;e6o&WEA)Y3SZj7{Vtg_}n60K=Nb^-05%y9*T zp37Pwwb#+X71Ys_#amb@exyx?5#wo~93giH)9Gyp^q-Rw#yrn1W{; zEEIm?|0GXGw;L$1wKMnD!qzpz;fkVy|0GVcN!B%K^uiDoU+<&GjsE-SP3O|3 z)xi&e`2x_tdN^+Tx)4mpH3ZuxmEccsSAXPcFyQ0!gkJXqjgIZP+_ydNJerJG;+)o; zt=KGW2wogY$j(l|4Y0*Ufwc8>5ofo!z=L_JD)^!?CQrZsl(;l}6u709p50{!AUO96 ziO9Zyg(iThwXCeg2(a`gs)EhlsoL^Dy91?jB-c~c(42oTPyl-Og)1gBJnH9(Tp-*h zf1c-0XN}|@IIZ&t9rj5C6+HsSovk}%vv^>OC9^aPwnse-{Pz`Y7XL5y!B+-^ww6A-jo>0L#{oI!?z~$DG8zmf zyn?dL7Efk#a)K9E2+#M$Z_=kSDYsa~n7C;XY3;YVH+7)}aGFD8IL!Wz`*Dh~WBHT* zyU*cLd$jx!Mig0I|3Pqz8x^%{x5=iLmP5A^mq-5q?aeQM5S+259jK)Ab!+*dMBbO% z`Ndbp06oF%#PNHsIvBaU6M;KkS@k|98VTQ#Ak}T-SK#aC;%rMS0Q>l~#p1^*xk*m) zR;~#oT7LQ9io~9nb7j#p${;>akRPVIk3aTH3L6oorhPzd=5_9(^A#WRUU))pSpwRu zfLbZfI7)@E{j<~0vfgl#0gM6Suj|)W8sv6%1S=J1iy{x5(BD2r7y!f1>;G)X@db;L zs~Lyi4jU@@3sJlqB9oGT+Dwont^OkQ^stBhmaV>>aG4$>R_ti$jiUP8ZQhCVjP@l> z2rS{cblmFbSW84XGrrR0oL;L=>?)oAFEH-O7-7YeSrV&N##bjltw8LIVj-pCdnc8J z#s520@4_@=pK)npjzr5U9iX|b=7b_Z z$~{q7go8F+T(js?s+U`Hpt2ue)XslvbQE!M#MhEv`_>jq09Y+H+x7c7DzPo*MD!Ju zlvauSRqYgCIB(8~ZL+reF!qOo7CzdKe0rp(%nM&-N^o(rz%c3mYD2C&nkd=`-aq_&Ls%M(k1jaKI z-iumt_x#YL*-c!tIK;&9+0JBToLLuhILAb0cgNei+2-Q@&V#2s)bew4Vwel}&A$c8 z@0$Z}sd~{qZp3VIsTdew{X?DowwDax9Q=*nI5juEY-5_jIl||-x`-AMpWTzBOi-Vb zoA`8ueV@wGnxNWpHw?G8njsZH==^V9n}sVh0?G*5y8T2_m(H;MLrwUfKBC|b1{u3l zjf_R~aF=CnSlS0BmjyGA^RuF{Chj%%&k_Nf@4dV{; zl62JrymNGD<&CHU0X1EPU)tr~JI0-E&ePvlbC}3l?v+*g?j{Wo!9e7TB+ud-!CG%& zdXjRp&!+`+wVIsV{syy(QK+lIdgN2Wxx4{AF#Wm-pEEZ@K~ePfm*)J3hWm`8rMKoJ%hb z3}N09X0~nodn|rXAC{f)V;^EF`rJJlk2e9|IRlAQ2U^I?PX8&n8f9Ca&)3SRiLY&H ziqG4Q>~~F(@U>*8xQrs>B@IaM1;^kOLKRxb=sd-s%71R4a@}oMtX(Dk(DkcGq$y$E zkNuydm;tbHoE$G5IaBdNqSG`o7G~%CPRN$=BTW(xvL^&AT%d$TM_UAX3Ka@kJsmFh zyNTp71oc(l_kp8A!#8)c@*xD_g`1ape9=?JULzv~`|LI% z7lyLT`6*N=F{V}D`V#n+*Tuf(6dD%O_8rsX4M9Ds48&aMA_mk`lt|J&YpoLh6I%x! zEYD65UX@2+@Mh!I(nkeT)xGolD;o@nQHUb z`9$tXdv}+kElcJ+VIwgF%{1$#Fo*rSQ_v>w0H8S(1L%M_N%-;kF2O0gWxo`k+?_d@ za@1K8ikqN>?PeZ8$7;*fB=sGN*Q5B2kgfpqyzwGR)G8lT>dXf8n=((2z<@G)exiwD zaI$7x;45u?&H?tD2N342PG|0TSjk$A@5G}vw@`1~!3_4XVYAufn{(WQ5|KX30t>~} z(Vp6ijPB-9rN7>FvbZ-upErvU4a$_H#|l$CV?r@TUv3;iyKW##N`Zs!i;OaA>^FJ= zE;x~PfxFU@Yw;})OsG2{15nRb88curUQ3#S{~o z{&Wcm+KXLNzE%zQ((uSK9uWJzRg*A_W?|PXLz#){UNdz-W)^+M8-<0|r_I)QA&|#? zTC@p$l+B*p2(Fp9%+Jo>f#nxi2F%agrXdnF)fA7zYP{h zdZvBAj##P{R1!8~wi@8KF(367+OXYwYw1#2yelrdX7F-jQgOGYg?pKyL_9Di(gUwx z)tSF3B+mVVbKl^f;SVd(Z+NV_w5vJ)3>&Id-&jt@+sgL7i;Qoi#|smzM2UXxc(om+ z(1G&Z8IiA(1YT_>xf-nQ;}_UBa(hWTTjQo5jrkc7NL-trE8{}8X^u+z;e(;uDm{rM zBRul*5$oq2YB5hwB6i*FNkx-x=XFh-`@xpZC_XIZs}0%ej%d^V8#KXe@zri5A$fQH zhV0iWbN=$WZD*rc+`>dXPHyiKM!ET7S5EZpB#_lv|IKP|lY$6E@n<228nk{Lj{sYF zwvZ#0ifc3tqcW20_#808S#ZuRp}3`!))t=HW!By8 zzAnshNv0;pT+pqy1_KjbA6NIQlb=g3y_Onxme)Q*p7B`s7BOYG$7t@=SA&0U10aq9 zF=Ey1p&vKKvrqns9>?YMv6Ngg0)1#aidR10m=14Tf%Y3ijLuwlcVA1A&WYcVME26dW*`p+O(E@(IRx8E-c*;gK~*vU;; z#NMFs^Gm~9*IEQhzpvWyUDZ1KVcnu~M%~e>p+#P{c4oRo?+qwdH9-Ce+A;#1KnlVW zDcMo81FSD^4vEj5_YESfxSwISXFU$~R}$SAFbtK7b=xbhKsgYc=xXmddKRaHH{%D{ z9`6ctxIbJSX3x!!wXXIZ4>|KnU*>JHod#bF-%KuzYfj?Uft>U=q7!U7qLLjcT?2WH#X2hINl4~hSQCceRk#b`0N}PRAT$% z*ELK*pFA5Epv$lNf*iT$-hf8wbv55j!SBmSZVQSTEO5`(FTLdwR2vlXM7!EPdeSd) z6|?B#Rj5GdWjbu*Xw)|{Ba zYLwwH-WbKg;V|2@n`4S?GO$!cqO9i7!NJSo_80DzR$p$Q`9fS3QcCl|t%V^(4~+EJ z1LP`%lsYdWA?7DyWh(RpPEjCO!D`@NNFnZQzzJ>3i&=Fa#$7BnndcS(7c3YWc)-{xk%P{Vq&ZW+ zcLfvNC$C?XCS6`+5L@n-xw$HrIg9vdlSlD`8OJtAVPxg~g}8fs#_D z!?8Ass+}o}IP6b1v?^DU{i=5h^O4+$HTnySh%MaG_Ips<%aDK7s{menpN7;o)gz{= zn!(O=mrE`!9A29f;F||EPJoIWaGtE)jkX%Snz2m^CP|E!$DNBQ=VMD1O-|#IdPK5(15s z*n~TDC?f*DQ=M3Qa#Z&<GPPMvn{^G#?hn;Qy z$1gJFCl2oJOg}But~VXfN#s5ocd$2Q7i=IA`p|=R9)BIMwfqz3rwfh6pD*&3B`WLw zD7_>thbXk7Bgw-I7 zbp<_EyB;mTTFmStd?}?=i%85e_IuiPVi;_~eu+Cc4rW$`0+cMx zj4mDj(M>=qxU&JOBe1jjxAN^dw{&CG%xlu-9z#6j=V}|nO~^bK7XUuxA{DcqZ->=B zyRl{=h+?oMkDf#beAa^GeHRVH`yn zJb`aV^URBvB*y2Op83RxVPLOi%#4=B$%&DKfIT&m;<2mnTF6Fj1raFNytY=jRe$)r zcJpkE;KEh^FUE4m7?_0z^f(|#Az++`j0}mP@g$m(j%RZheiNrQ1T*FJoG-_84M>kXG3 z;RLK>4W)nhfslc`NBy>3cd`2-7Zi2mh6Wn^T!m>HVBRwvFt0QKiknv5+a@P`KBb_j z43LNaH%lQSZ}wESjuobv=3vwt^qQ>;lJ#&na*e@8>Ke$&%tr&KhkrB~j*Yi8!Ku7M z`Q$L+XC-oH;mXWm-Op07!GM7{ZIGh6m~ z`Ib^h@bLIh61`Jg;Jq=_GVTyn+vC5~hPo}^NHzD(C2mQsR=9C8Fn?_|zz|z$l7fF> zk4JQ;y?2tE@p7-seH%7B#-|g!+(wyN7(ML~PP4mZtyQM&;WfP}=^^0LOkVaxY*+_q z**)SwJL<=`sCDldUs}H9CwNKrg=X8)LEYeDIX>)x&iNd?;?}R9+(+s6j)eTLkkU5u zUz>#;JLor-@=n|h!0Kgc^YtDLrr3qTzsWj@`Hu=%m#^ODqF`RW9W6d?sc?qFTwIgu zc^6^l5Y(OCNF-}I(*?GW7wLb_hya1TQpR1HfQ*C?x|5uZo_eh z$H~9;0{cyCJp-D$1*XfNOc$h@kastaL;4*I?h!Pt%H|L1Zfcr(2UAmIjqQSscN8rx zRmU2^j4>L$bIg~>qV9O3ybuG{ky9*smc3^B6D#wjoXE(km1y%T0p&kG+dnh^m1pv8 zJ*4&x!P{_tD9bo0@?)rmK?5H5z;*nKV%C4D=gu-~La*Lp6W`gG1^&6T=b(=}W3v6MV8~P8po~vK4QTW=TAcCpyn?zr< zRg7!0xViV?#6Rb~F+>Hg_?o`KUIvmW)O2JfZ@FLWp&ic3Ws|rtu zZSA8->54<`vDCEH(ebqz;CQshMEAXQW42J2Q@hzI8@{w`O7K~D2W?nO!Qf0KV`?x} zNq)sWTzR(q;$iX1Ii4|H%kp=jjd2dgClrK=?ba8QD@}Y_l}yY16&AQ_C!`Xa@fuZ* zrhcqrB`S?0t66KF+Ogcg>xh?xhEN`n7W97f#!swWTwBpr^I*^la#`mw-4q#gJ(8fE zOjCunYQ~)v*$b0~XaxbIe@i?`QQ5XU!Fe1fxC$vvZ5tfaI+-ioEA_guHO1R(z(vK0 z0Tf)ccC3qIz4@fj`mhR92a$;&b5&0{$yLiwY4~|XD<{;$&o)Cdp*QGe)k59zp^G~{ zd}h+IT3o5R<>0MW^eP=Va~B4w4G7xvQ~r@nUhi3?pa(~5{f$t zwRBrfX!+~Wkig4OAm()Q8nqZq<$8Kg_dc>`V@XZb5XJ03lz5HGee1S?N7*}bTc8>C zp-K6D6|()go&N|*ziN_Dm~#LWs3kXtDo9M;XDvU!OL>ok!L&gk0bKp2v%6{1TYd(Y z3PSsQN(h;cAlSXy_!3S7gg=kxnTn<5(ZwM5N~C^li($U8P_SY%I5PLGtQhrEgTX&p zjCk1BV>-@6x`VD9iu4NNcU_WN5LE-rX5V7DPv%0ZT;>3x&MPHdd9D=3m}XVRToWyU zdJ_})s(3jPw;!8<;*tK8a1(u$WmiLOi!F$a$uWq}gJg)_9`Te;y{Sq3nEw}$XzRQ% zF`YY)M3L!DlbquAnA7>EGShoxf0D9$KgmN0dL}P`Z^bQ>IKK{o2E<~h%@u=9)97Q# zq6;w?s%Bj_w_%#@XRK4&OYnTLDKZLgetH*ouh|Yw;9&&Wu7@|HIn4 z-XLDRynQ7E1vm!C^LD!Sgn5=jH5Vqg8E}bUSEA$#Bxm&pw^(#U4FW3iWu-M1+%m{Q zE#>`JqxqB0PxAr{msqXa^~tE98K6pOm~;=ll|OlGy_g%?J~OUFm_ws{s@CW zA_N^?EoyuBI&j)2?Fdvy3mcXAM!=!$X>Zhn!Xg$E z?qNZ`vIX~7Wa%iK@eLlKK z$~XySoZ_3MqutAJuVJBO7^oA;XnIO;ell6Wds&FlH@N1>*qqPK;T`m$7;4C2mwxzy zv2)nFSE~^sDB>7T5{|~{i2yt8`rEBU%skM$fr_CBM6wV!D8rG>g{p&ZIu1xN%GzG| zODSS8w@X=9HD`5hLuli-oNMIS z&$6C(=^K$eQ0-qdM$tKorvmED>79|#OOC9!yBK442nWz84%>Ar=diWM6coRromjq| z0Zqn%X0rzZZQZgoLUGCT$;TgakZH zvVxCYK+D|N#?+OX<+^~c;%u_!iQ@dM4V~2kvrzv@i~eV^O-P)8ft#}5`g83*-)m;k zqicXM4hQBTU7>0^~m6&MWZE?c$c7ruI$LWMTRgu+Z( zWHg%`%FTJhFOuU|M$t9cCxu^za13?btC}29XJ8r!jo1+MjQSa+EU*-d?5;-YN0*@H zdx`Vf^E9tz5Z$;mMK4iz*Bq%lcyGxC4=ulBa!PQH3uE7{0G8;=dgBLec2*4RWSFa) zhdD6jXP15AG9S@$u~QIU*mpIE&sgsg_B7n=0sae1Aupt6QO1wcWPPKMT0YyBH5W*p zd~MI~eOzixshY!wJvaxY^4LPk=*fz#u^A8L%>Q7T_1a7FFl&I>tT(+EcEdOF|EcGZ}A3O4%sXquKa-v}TX7pQDG|2Y0#RUis#|4jvgFk-ZwgfCi`5%w~@E zm7LECpBxpDpaG%B2|`a1ex-(gap1W@8v6$OeG)!OZ7xWA3yn-IjKuOP0ttme1hWL} zOH-4M{I8cF=bI!`j;vFZfsc_jZY}*bz{u*?LwCY*@kheFY-$(C;i@T_UOeUJoLSe6l*OG zBPBbaw}%gWiX!%qojc&fMlzlI76ry#jel13=kIy0wm18*7&u?wk>j5uhgsuKGOi*I z>s(XB4c2GO%R(G0#1ave$lUH8Hx8VO8m-_y?%J#2dYaRQ@!~QLm(o)?3Es$W5Ut;u1BV) z^^tQzF}c@Z6EpjqDG{Pr(QJTGH1JIE_4t}m7>n^AWu%a*sppUn28me zTZxH^rFG7FC=fXkCXapK@?UBW6~7J4(!hKDn7c3|K8q;9u20LWH#PG`s-|kLua#KM zN4a*x8ZMs#DoMp$!zNx)0aNIzs^33gJ z_Bi)v6Jm~qH(+jgKbm1CLDne(%2y)MV)7#$nb`7+B$+(_7O~N+0Zs}EYs8?y%M9I& zA#k49FR6m2u9fUMdu6Y?i1_jW^#}7wH~Gr4-}T^7VDo?wm~&t9UiB;{IX;3c=Ln0s zS6>dPL|8P>aYoWf4g2!|8^Kp>*|YA4Xynu!B-{EG-^^ zE}B-vpze$S9!;wZ)M1+lvouA=^^T2foOd`GUrx;0`=UL7B+tn1-_YK+jj>57>j~0A zwc~8ob%4jM4CrAVg2(+_57>1#p6&etlrGk>;50>WDX^hR78{l}eAWpvD&2aP`vEM5 z#gGt~_Pff$wxM-$R#{=;viUmGQ8PKV*((u>sr2AuU zR7dtqnmcUoHcMR>UX2JP`;%SO=mti~bm$F@2T_JypR$cpt<7Q_!qW6@u!Zcfft5nA z3Hd7Sba{5_uczoJl_Ujvhm#!rZm!Hj3{0csg@Fg^)=!zkuH9v!iMHChwYR^wve3;B z9V~Arxz+-^!Lx-g!CtnH9A4hG*%+%h%ofObEW}Lljmg&5cKu`}rf{!H7vFDX-f91A zLJ@bj;bB5A14G1NGI{4N4tpQZ&~0IDp09n>)~66W9-0Trr2dm_MuGuPaE477?1RGP zgTPCU=rqK})>rf570K~AXSZu>pSN{uA6BpwORc2Y<>!MYY7w@Ro}iw&P*d14S}4kE zvmu;#`8~eVBj9LG#ew4~&npWN2|d6!(8V2GG@8aXOwHxh&0$QX#uGa#&}I6K?2ID< zFAGJ1w%{16Ec9-7P5f+f z!~CP7I|;47&_!{0J(|liAn~SHpK>vG=!`P*}S}!qhJhB;&dBP7a zZS}^Oj=vLO^_WsR>X&1>Yo)v$kERtN<>B>0dB5hz`9CssX_c>6L4rOzUg5Ys;ld85 z@g8}Y_5&PwaVx@Qf}>}3%ha_QpUefEsVII;041!&(Br~w`|Tk$?%934dvIVo6xjpA zV6pJ5l^RjmZuZ{|LV7tFD5fM>Qkj6rhl15^bfRuEMu7PxS3{*rKrE}7qlNdFm zrMp9E27=_M!F$d7d7j_>@-Kem>|7_l=X+xPri2*&6Eo3Efu<556gPvZ9vyzcA_eTt zKJdwvIYM6JhO;}+EwscUkHB536 z$Q#ux3pT(+38P38?FyW0ThL;{+VNxD2tFFC9#; z`Z1}b+y*-xwUZ@-oLvNVGuEus(*avAjjW}8s)Ins%g>^poJB}T#opy3`*}Wxx_*f<7~rahcZd2h)?@ z1rR~aYF#$p$r*lN%-6fWMzYf7Ma~BIM%`y+28dqzQDE0fu_`e=Qn`)Sk^!!ZQd6~i zo1NSY7PP){l)jxUgWp)#>+k&ev(T{uEc!rEj!OmW+*gOY!bYBx$|QnfB3{;J=c75c zY2azcdsSAePN~+UH`C*|mFaoQ-CyAi;7UYn!(TRC~%6I&+zW_LS{y)Y@LmkJ;X&X|nPkdRx<* zmQLBcQ1t_}<2D$b27AWg)Yu~Xit})`?5<_)75aw?Dtwq{f*uFf?3FUWK4~XGPj2<` zdFBb*=$lhp?Y}*>q|Si1MmC(x@!^H?UW&~&tX2{xBFu6ZWPCcWBt5io3?Gv@Ag>}R zXZawQV}*iSY)r*hTqjV%-$g-SxUX90?rza(0d0I<5{dW-x_8+UI`<=bNld)zsI|_- zDZOfqF~yE&h3Ols8t`g)U4?dx z*Dd+M$6ru+>W7;+griTnj$)qG_J#TT&PI)!$1(@}=6msCcmO6PHLW$DNW^m>=onht zM7sqU05I2#*MG*yeslwDhEkv7s=66C&r+h%PYsX{J|UgdznBkAlvXBz3}(T}U%)Jb z9cxR?JLR7(Bo4({!pdflx0g!Snci(7IgJ*Ai;6fZ|K7H)$`e*}-mBHEsv93HGB6ZJ(&3;asXyH~tC15VvjNa%J}M@PwU`cYkW=6|JRAmd(}|CEd7xKsY(dcbz9%NX%kEA)@H^Nb!v$-P&t zCL=EQ?%65dUW}AmY}k4vdM)zg{+arg;AsHnqb`?6Z9xNKkTU=k_%Rr6;$fyM?=x)w#F0X zFs}qSsp<8^D~4E1$+t=@3CR~F?@3||X@&dGgd0mcyqybG+2!c6ev*M-JZl zqkx-Z*QqRw-I-fUwms8}WO_^P$C%qa%-$``2ASun`N`2N`pDDO-^qt=%SNM{9AqfJ znPT*y>_uow-PreisgfMpHuKo(8$ButL8TfGt>4P{XzP08ijevvr*8Rl(swO~M!){@ zo4hee!McOuZHF z(=D^TFkTX`gt(f%eN5NxC#z30Va}dYbZP!V>Ijn_&T*T%th2MwAo{G~*4EXsKamuj zs(Vg>&>|+K4Gh)xl)Ljq*v|3~cZsIVbSNh_!D)c>K@}O>xxUC= z(iP+da5w5Jr8n=iWb@(L(luT9uZ*hmo4BR(#^{ohe6+nz4MpDh>xbL--k50k*Io;m zo14)~GpjH>abeRMEXEv44Zh{%S>G^rChLSjLf#~wHW9PiB6G%P`G#dYP6b>H+-V(5 z6t-<%*^Y?R4d=Y5qyV<+#{A{zkuawE_*)H|7VeECt)^oBxzNxL_M{Qv6I0b|u^Lvd z`1}l3us@owtNN0H2~Xiqq>nw1F0kZeBcUleXM=4S54a(h2t;Sb$Hx(>R2Mc&gcroi ztS8FZvrj2!lwKTnqlr1eeS>~^)=ZJj{fpZgF^Vg|2-qPmQP!`1I`CyR@kRW>p5i!+|y_PXtE_&U@ z-^2Tn*E=ZkyfU0#|2*AbIIh-~zUUE@-%fp>`k>oVta8;{QJaqHiLSn6KMgP=J@}bj zuU|#q;y;#YOzBP*Hdhc{FRfy^U_7eI=Bs2Z z9pmED{qA=rxz9~amovTy8R7R)M}5w$`%oHhwl}HAj84D|vKGPxZQ;lZhMeaI-6B=M zHR7^PDN}Y4M=K1f!1xr#2H?(wFk>~vp^%RkXmR(_y$y^Ix91AgXa9bDMxk$+n^$6U zP4>nvi;N2`mA!_i@Vrb}yeq#Vz+hfQSe{?Ja!fqgBGUJL^<(Iq>&HhY30d!GGqz@Y z7-tJN>FM6&A>|Ts6)$zQT;UEiP<r zr#1ggHT@D)X&sk^g-bmDP%85+uN2@acv0Z@+MTVa-z=UMMi_xiE>Kv5S?-#b3W`$- z(9AJ}JWXx5WJ#J&t?ip37Y?fm6Gn5}pYvUpxvsBTryavw8D@v<*2=|em6h3T#60%U zOfIfW`%)KTL@ER34_^p53yiJ?SY#r^WEjFVPj;>wV8lVY0qu}D8N$v zG#C_e_U=^t3=<3vz3mg>foNY$VzJ?!tSr?~nSy`IZ;rVhTvG#NmX5Y%< z(&zC4w$l_15BDNUSHLtZads}Pr;kKXc+hJUomQyg34*hbya8O&KXA-~~DKz;Cx zf=3=z5TWjnS5xIaRLqAJMGjM6)!LJ)5V2#h<gncJfj(Nb6dUM-1qazr89XJ^%w{4CYmFm`m=~P5x5KsW@vSC2pKN*?4t?A~fh%0#*uGhzxW&lEcf-Cc@#8k7X@d)r7!Zw# z*%=F~H>rZLx!YKGM>sU9BD!^M^V+<}@%Nqpdc@H3jg8@x zSEpjGWVpgP@spKu-jwVk_^S{0`&kUpJ8d z`GlGRd!n0M*Q~m%`J7AUK3WS$Yrepo>{+SAW|y7j?RI~(Fp)BsX}olWaQ=RMri5$} zZP0Xnc%K(oev{7d;|V?{bVPKK7OV^r2Gx!+_%zgIv)(O(4N(JDHql(P*Ooctb(jc_ z?gnEau;=ULX$-3sZ;}IW8!qqoox-L8YIabg7>4381;o zDgSDL<@vF7wVYz})(E`gbFuFc>N{}@)@Ant^y@!5cqR9gdh?O4x452_MpAp}`-$|; zD7G8f)=HtLGFd$1YBc+?zl!yxQxn246Pshmb!3*!R>brIYtim7 zjxo-l?S{p^{3@ek=VI*(32;9}sB3j~wO!Ts&s>Z=4g7TbB>tS{?Q`IRW-)uDFXeq@ z(a)Z|ft;?E{8%)wfQH(hH@~UGCk9U7C$e5k1=lE6@)lg*d(a5RS40QV^E3=k9`NMU z){0x)LQBmU=plC8Uf$0$*EmF_T6{0OxACD=W5=C46XOXhAAR#`4hu=|Rh;_+-M?}r z#4O=mj=GuapSN(ASzPbpQ8UdYHdAo}30WiWPtX(^P+f7y*fylq&30Il?#ArFB%eTDF~AlkJ<>UBvEy?AcB;PkJQge1`-h2T6b7jLJdi$4?;h@4n zgXn>P5tNzB&NUDNBA@J2ywa`Hr>z>h{WNJX^vK zM}dU$Ve?L6_%M_4OYdkk_BtPhUfoeM-in~@qs})Ny}Dz*KLL4>y>V|;)+R05(!f9* zqx$S=`@2gX@gH{H=Tl8tt8@|JQz8fNM&8M1j*5uAy*;Lro{z6c^G`+=T-|FtsQ1xn z6~45;J~xE_>~gPfAqgqABW~UHe94T|?MshyeAZr)DRgYrIk#ZUn{PEHndKqD&L;;( zp~9)b&zqw(`1768XvfN-IfbGFiVy*0jM2e`dBb*UiQunT7Vie%sd*|jwe9Y55mwmY8tate`dva zI;Kf>RpG;5;Xwad-U(_Ef@CZZViP$P!sfFv9iPSv6UD(xFphSzMY`Rd`z*UrZ7TQr zcyuO*ker{8L}vH;C-O1&g0KNWFVFmzzwzG|y!3Q5{c+B7lVuUoM6jhB>2chpIPrr3;7aq7x2KXmu|xD$QQkoE@2 zYnYX8%dIl@RW%67mG{F-h9NE~;0F_U;-R(dH;OY|x8=5m_SDwsC=A*n5*4ZKEZDmO zJdfSqOKDj_?hi7z{o>-*P%Ka-nnD)*lMhzq88Gxwz7Qc!Pj|2!HPP1ut*q z#OY@^r~=)>!oxKx`7nL($x}hbU9VjPkpft4>UjS!0bLyk##Cg!qG!3VMf*^>>cg@z zKJHvABl4%(B@_*7uu5T0IR7z)pU5Pvz2rh zF%iFmc47`=Z!FnHx#I{LY?Ge19-ti4YQlZxW@>9)Sw_c#_b(p#;7l^0_e-HLGxKK$ zR-s;H5SaBiUH#~R8_UrzaM!Wl`CE>F=1hT4!njvSr9NM!>u;|4q^Zq9aD>En?sH%s z&X+6^;o?lPm|`Oznm^y|X_hBmfB6sX4ovWRl0X(7OrxzCZJH(RP;s z@dlLP>exwoDu8%#zOkD(VmSkL&^ea268O&!7qA@XLflspuQ2d|L@X+N_pE9XVfE!? z43kUk?jihZo5%xU2}C1^RFusEANFwwUqr0gyJc4-p^ zRD5ZFgj;BBZK-FM*gx6}b|q)!==vf7LQwYlYKe2CXmTNS|Cf`VcdKR!Y`yKcG2|Mf zfy<^xV2vg`@+VZv9O*kYGr{$3YcBTDeY=F_d>HPY&w|9`BfqKJ0%Lg#Ya{{5@c7FF z1Vb780s@|LKo5$n1`Vf@%k;D4`{)yc-K~UfbOM|_@gi`BN6cmeN@~OIxaP$%Z>E!p z-rT^3_7QmMT8wl&G)4$S2LjX4pTST7z~WEoU1&DZv91lR z*r_8#a;*kuK82rVPy~L>8AxE(#(TEk`s~&G=F6+AEg&|Kt;sLjbUr1u6q=6X zl(O#2zX~d7U{gC5?>0G%_^1z1d&qg^pQKw4ZTat zh#9)BSk@5LGq#k#tZpntc`Qh(i@|*#YlnNzrbZNg_p}tPja23nriAz~-sH3T@)Qq# zS%yRg71*ThpOiVLjL~*nCfXZg++I5lB%NXTx|soa)iogUD2iyQcG%|!aks-|!2;f9 zlL0w(Hd)FDUg2LB;z1Ql>wTO8+*6i@}hmJxw7fQ*aes-Qf6atF4(!bDLJ zbuxhw5=EWnh==D8g6v9Qs{47by&e)ruhG6EyF2d6CgwwqV7?a8zNbC-E3M+rPSxHz zyW#ee-spj$l{<7eT8FLish=F%(oL=NO@~86i|>%b^%-=;AG(rM+Jaj@>j<;iX6^51 zIr->=4HMI*jdI=GVpcasY!Yo6OAQqM+Sr!wZty@p(Y?}rkx9v>Py8#O4b8D{Ni zWdt!vfMVFOmKWTtAYeGL3-E-OOEZ4U>hSXbux6x{*F2h(TQrG>QhFDaEZ! z1U!?H!fm(kVBGaR@9f}g4i5i_l z5aY86T8Ad0r`?K2g6kebNwXh#D3ieCe59P|*=thelcD*%{ZwYns(`W=>z^s`2B^DQ z^3AiZX4qt&{N2(uwRGt{Qu=S2u1Q%JdHjj}i4df*@O3yytej~TntNn)I3CoTbx<9Wi)Ny4+SXkg zX6E%05?Oj^IX~(%;v_lt6Cx6-I4s8#j~{3U69L4czV`&1pRi~9I~n*wGAob1)j@uO z|Bk>hJr}+4Sj>CSR5fvzec~BK)or%(H+(Abtry4DkL8S;TD`z9{rlx{B)<+58DB-f zygQPP;5b2@maHVlzb=g5g(P@WWh_ZMJSP~mOiaZyprX1_({`1wW=Cc<5&PFu-=~&! zkbcyNY}%IoVQBc?rG`fJmd8$Gx@;Ed$59j6P2%n;i2&%w{ zpvTDBX1Z9U>F{#yWN@moON&+ytGX9+L^u&5#Zu64)$4I|g;pjS#D*(jRoyourbFzv zdFK1&>7peg21LDytk@)KCU5wlOE#I#b;9CYmJiGLt(eH>Os&Vcn7@#LzbjvghKf#4 zK@{nXZ25%bIqhUzE|5i zNxk^1*eO-TktC81itiqG#B5&QkH4?nzSr7P1hpPaN5A&`RC*|)a}u%1zW9ggG9?d# zxhh-4pX%you<3~e(Ye07754gXYOGZTO3ltD&8eQg!XL|5J?FbM(;N#tZPEEHu(z>~ zo`TGtVCaJ+03{nRr)k?4Ecw5GaXIjlv}_Tj!kjBHP^MwzOl?h#R4EH5UZ-3@XFEl; z;!F_+Ichi?!fWg{V`8o+&^8(ejUE=;s^`?%`w6 zA2*)GYE{UiPD$rGiLG%N>=bbo+i4H#ab{u!so-WkIcs~#uz@&Z+m|d0`8e8?m?0~C zj3(0tFFE2{j(vZfn|Q--JND*`&|v)Mp)8n9Dp#~5M&ME3^Rg#CW}sI_K%fg&N;-sb z0X|Hj1{rPJOJRA;JwQ%gFEM8#qK9d`=+;8%SbRyflgBwknywl7^=$B-sypXVylrYD zG4HrbOoEM(_qAX1!HfJz&VA^WXAuyM+@Dswghb*I_0j1sAWHw1Wlk)-K z%q!6$Qvfx5v3Gz|M>r4BTE|U>zVz9Vq$^-QT9k5H-Zd8&9xKWeJJo3WD2&v`i^&Gc zdabm2Y2h1O4pIwpcb(>R5(M8qn;n`-DY5uE0Ib$S9nlokZl~IQICGhOBGs}G%QUMD z!ctyf%_Yb@0Xp`5lpg|YG-`;3&(=?C7Vpe zx>**Uxgl{9B~s0Rq|W$^74aPHW@&dmd! zlX-7Xjc93m{TUhGV{CoEFXM{#Mfl3^5NdpJ;yYq?xZ4WBXzzNs6nQiWZKe=YB&pG| zsjSpAm(D)jV3}mDWr}#C5X!ETu{9@tglgi~mu;>Zh8$b%P0hK}foFRR79_NF3EUB5i;)q5y|v{|12E74RTrQ4HAN+dZ7i3KnN zz%0NQ8j>J_e4a(kJ+_syIVl6LP*NY?{^;aD{j5-g?U+ia<7iXn;$3O}FYzBrNpZN2CtRB3tcm6=mx1c2- zh9sv1apJ82?t!EZj(BIaY|$6V=r-7pp>Jv{Xe|db6AaOpO*nnxuuwlynT{~0Q~F+O z3?#ItOT5xZm3DUfX>=D6MZuSoxlUbaRPI`dUXQjZ*xw^ySP6zuZ>Fbt#|eyXy@>E3 zM7A!>lQ%8Rxv}L|B(+|2_XX#aZso1q+E(fuFK64Ql&L<=*B@*tyxdqB26(#xKRO8v z)I8&5Lm^QA-WS@(pFTi?LhT ziA{8I;jXcmpFFOu_T)c#T8 zmaDe1VyzRylcS@mORpyBvnRHT ztgedGG#5%};JuthdE@8y1&r3%P|noxL)Th~1o!EcopneAcC>;cwMZ zpA6$JyiEG=Iag6=F*)madb3N z4gik-dO-Z}Ghr@=lZ7$M1O@KOVv1+yoMbh-pI#<9Q{4oE!|3k@MVFcBj21Yxczknn zNF99wM&-EUYI^&X8f1SX01EB?vKqk{U;WkT`yCel3S!VFnRNGlm%rBeDr~0vLr$+1 z_pSD2Ybk0Vsvi?LOgUWanR+M*0%um z`o|)Iocv7&3{d(CTHdB zFs(*V+Ub}4J8s11Zk&iynwU+}FJm5}^b*B472y=tf0Dg@yHha&N09lpG3x>$#@@!s z&KK&8TRD9XZM!X<_=x6V0ZoSbiT!v9B?0J@c_yOZoS|)ufJV7IpWAG+)=>S*IRq*Z z8IzY6XTHKlCo5LhF^@1bkZ>3q5U>#+Q%`Ob$1ec{P*gHxj`d7U-6R0^=;3hz%&HTV zhC$Brt$LG~DEMyk`Br;0SdU^ z)BLYyzq&sv8!cV(=#4iKm2h?{Eu;t?%ex+_41f}2e)r?F@d*2E6JxCK+QRdkz+Y)k zkQYmvg9CcCYunSw_P@xqY&sz82e8>=pUdwzpWwgD#~v(9H_9ScIs>>rHHdD&gESED zBKFz|<9RwB;vYUBIO~es@wEtu#QWo}u9lbI^X$uHQdZb=e)X4mE?+@v5qR=GMucr- zv~{_WEwneRo_sTi9an}SuM-HCfNE{$iLFrcSll*$pLq-$@;W7mEN|&}H+ZOm(rdq* zbmbTBNgTzB!@RSh7sr`ei+8T=CbS^C=gPzFkI3w1|H$ofhCIb2a9^PIkaac!FC z^OT=Xr6JakbQ3oC6}L2CMpBMv4-NAk{-)N?3_nNk`LZacQs`cX&C7HyY<+p*TUHzf zWwD?3OI84g{!TD)cbSP1oo0J8WreoQN9a{QJ*B;6N&lKzZ}G6Q9;Suf^i}BmqB?DT z`W%b66bLVgS-EixUv-nEE>86k&)b5c(GU!Is3i?Ri#OK^=Y>G+KsC@+z~LZ}ATkKh zE?#0OU;{e$HyUO=cpVW|`}-5-1tbPz{o_V%$r@o}kd zlCRD`vJACWpycC)F)z&<XMcSy43v z)Iq_~0D4#0N}PfHfzad}%Y_XA5EqVgPpQk`FZ+JWG4SS@V8kc6zS&*qI|jrv2v+=H z71gk)je>8RnkdfmxHOu>NG`p77@nFCxb~9PlH5Jn@d(X*xBVE#@vb^@_1^K5l$Xoj zIY9TRuAIL^wOwRjoh2uko~A4D7-c`(PfQH5T~x<(t1$PioUBlasr1b&Ephm+GZlfn zzj2UySG1)0t**F-x$HL0Et=gIO~Kvnuav<>VnT{DPAN&#ilZ=L*p4pF=o9pE{PmcG z5AB4v>c3INK5`d`AY+59@0uKNgWC%PKQI4Bc4T@@o`F0ILaN8axTq7QuS5$@CG!-6 zO<)|)12VR3}UVA=QVE`oFJPd(QWG2(=c3viG!O_L#faw$$ z^8ObD0o&w=cCUiu6i$8Xu9U~0V((c_7%*PnkwIt@#hvG-I?>Be!g*mClWm9C0}g!pw8*~#3P0G_{~83$wikXO?Nt)Q^Pu#4yzg4wEFX6qN!oco~AZY zNQ9cn@8D>Tx2l!7D=Bg-e@rBhmnlKiAMdf`@@70x#-@loK7bPy!>fH8&9{SUj`|BP z%O^Zc5FC_1o!+39!kH<8_M&Y0sAz3vvQy z8#-JdaVr%utz!Ay?x^+hGg;=PoeEEq@kRc!=bo-?^t{VKsfkJ3`h z)~^my&|8o*yazd_R#h2pr08+VD;%ZjNDqxGIwsyyxsPsRxh!ANDbK07k<$x1&)GER z&E6nhNjDEy!!+P8D``H`1l~BCT+IxCF?M87Oiio9?R* zIeiwL{pOo+qL4eE2s6Zh$j{v)sG^}!-|%k72ekdYvc1${;}R@?oq9fbfuW}6i? zl7y0YJ-*wX6H1ztZjP+cn@OTY>{B8b&YdNK`_lRHPu2rf+tOJF+J_~grO&Nx;a5R$ zca0-2S${2c$>aGiV|B-Ktppy-ApI(TX5c8V6{8L+kIn8w=c>2lqJ`j}FsQ>AiCTR>w6Ekk zlt19GoenAf&hDac-TnLWV*!TRscL>!K-fozMN3iumbm*jVZ7%8D{y+Xz6({^p^{!r zt(GpjIL(`*OaS3+&>a`pWuuQ{`N&S}MA7+-%;u7Iap zV`-xho(sMXu~a7w+ISt0mpt^8e_oj<`@vzPKB~&tn^nIfxqM&_(*G?hV8!liMn#JO z>Uq5ba1|3tMS}zyk`^Yn3HTgv}d1$iA)E|W- z7)oOez4|PUBFSVR+x1=X>k38UpJAV;9^K zNBk_>KGv%|3wJ5xSn}XR(Oihydj26Ww%ZXXmLDKz4M0|CMF98kl=qGvuMbD0&K}LH zXbHDWpRoXv(AWm}NuPv4pf{UH;TVTvC@r=;#?c{3!0gR)Lt(d-XwUpWZr?q*P~xPW zT!B%M_vSap_4Xo$AQYZgshwCRqxYYS?0eKzp>UMo!^fLdP!{3R=j?=3Yj>M%7OegIXmyO9_JBk>g>P#gq|D_RNx&17LSAZYw?+I8v z4pn@3ATq8yL;mSm*ATdcPEf`Rd}2j&*ueJ=9iph$pBiAKdh4(9$#cqxY+9?D+RVFc zfm#ilzc2q9@WG%KE(K7v@ov!J2LOE$x}Q1eY&xY}O7;VRN155e+kk^)54eY<@+gm- z<6iX*M~^?%Fc&u3#$Jz)$(MP~CCq4$-fMF%UEPS3e`GZx{JV;A?t-E2MFPL6|BBgE zs$oeiX??Pn{G3loL%8@w{7f%6PsT8ce=LQV1PHa~(V}pipusy~H@Yl&@8Y|3-+N^J z>%r0Pex#P3oX4Jk)6|7*g(BYfA9vJ0geWkX42G|k1SeEC5odApiI{+g8OmA(S#1yM zola#?^%xtt1mO45&Vre;As-l;JZ2G)4MS6towWDJvz0KOiLsCTr6L-^7-1<$K8+U^ zSRaQP6t;cGepzTrUA0K57cG8vqhrGCN;xQ5KUIMDw+=j(=hLmz3`{e%cCVr9wqdhq?m-e-M{+)h4qh4^*~b6@VNk(vRJrYz0WV> zsP@Hxtl8tPxo)sSg|7_2^n0{6Sb-LNw0Er83tB&2odZXT$5JTWHmx(5y~a``jp2 z&d~sM-^bu&XjXs129HV`DgUElBAZ{Iy*@J>3zXFo!?Wny5F4At%1$T9|LL~`pR@)F zMyii0f>SkKzD%XLzO=*9*&4S`Rd>bvvk4ErJY!T$NPZ*4LC*3+J7#N@U9<-TFR9iE z7%I|!dm);`LUoVo_tuz*M#7bjTinErYR_F=!VWWq?g9ck-_mxsHgqLaAT{+X>}V@M z7M2ZVc>4~N%a_Y#v~X`Fs1&jnX~b@+o~-S6341T_@vNWx)kXI44Ru6i><NRbepG1NM_X(n%TsL}JNJ}Sbz$YA@ot3R6eZAh-=gMJ)7wUt zd*l8GQX#_)4NcQnSIJpPafdq8Z-Sw4kddRif?AQew}qVu$qW*jVQ+1HKY~c++LJy> zXY*^Yeoy{^CCyB9VXc!#i}^h6#!#(!A1L4VW%D+Die)D3*rfQHA@UO21HHXZew(2{ z%q;}e3?3~^D;4zo=^9{u&A_G*P7>18x+P2e@ua)w|GbY(fP%%M1kOYL>4eOrvwoGr z+O5FTTNLfhZFS{{Z-T{epW;jOkEAK+x0Y#FKa2K{!ksO-=G)dhVKyOWIYnNNyA)s` zgr}(EM}vJBNq14F0^j#G1$0Krobhc~7IM?~XUe+lPqGh2YNBx~#sHJnSb-7m9&KQ> zS^$9IjkY>n3lnzpK+9R#(R!S0|Ami#>hjq1VIla{-%KV5tIMiqq4?4kaeefx5qSxW zJdLsCi0#LCUe`xzN7{YUor%+$W%=r;2Y?13_6#QjD((PGX?X*Z$jN5&L%Va4)S|@u zGwO&E96udbmK07B4Wd;)K)Kgb2_E~j9-G$BgygqIefUv=al2+Vz`zyk8FZ~PV^;GBYX+IWccZwU|hG&|EU#;VgFRXuDP(<{$bq( zs@fK(1C_Y6jRUT)#HR{oa!xI|iC<}EZ8(KIEP2i%GK51idb%PRtYoz2Rb&g z^>^I{-QRG~zNs@UusffL?t&jsHguHzSnR&2`4o#%KSHKTP2%mK>OXbq7+RLMJ&SeX z^~G&3_U1kTkm(zF!>U$c63h0K`P(#W<|b4fQ`ZtNcUvnRh1F1a`$NS#l0i#a=_$zq zHrJ<;6J}6zU%0Eu1+Z=8*DHkoQ7^da`!|*~6VG2NvLiQ~?}HRHOiv&D&md~EwTzp6 zAR&r5x`l5-*|FuHIUUOA={!QuFug3~i)5^%zY8`moP8-rgY=O4D^De_n;V$LvAP*% zqV?@FcswWX@7D~xpGEam660pfQ$Si}(DTW=Fhes*E_KQC$1SShK4tQ?s{*(TcUd33 z2=CC+!zi-{4+;Q`M0nr+H81%5{!mz%s#8KjW#DBSsQn00g$LIDR zDQ=MDm)VOsT5H|gHzcRGu=Uaa!Q*d8W9lDLeezqLPCB7F>F1NA*|U>pQ8{;On1>7R zEx!5u=a9gq_|&Siq4Ej!h`X?+T>)yFpjK=kiQ>wKkiP6t1)Xw9VQJkAxHAtQL?YRe z{A*o4HUP5nGJZSuU8;=K`89q9bGHJod&2i>^~GgGb_ZvFN$yi#u0D;=A5qM0$^hIe zfy^s0HO1FHc}&~Djw+uzrU=COlqk8&j!`+jLfhF8FV%9WP7V!lnRbxA;N3{50LbJ2 zK3eKoZ26&dtjJdo|HTq-1M?hyhAHWrbl>Bk`tS<>l{;*Z&z!tAL8&e)+7)3TJvM!}+!C)`bU;s1 z*o{BB@0k{o<};A4g5XciwW1~o3|C({hYz2mIBxr>$8`s*^=vy>e{(c3e#8PGnrM1f%=sdn)b?gIO9rl!JhU59P_fkiRID7||JR zCvA&|rHo_W_GSP5PJ(`Kgfp)mU}3Oh6oP#JSCmjKXgJ`8 z&1YmbjKVtOotSj3Ra~m;W^arcSnz~gdHGWm=|`1QP_J~R2~cYPu?@dKik$4x1gyZ%DX%htF)yRf1U|AdaXl{%QQp{Fgza>2Io1 zwGr(-IP8xUFrQ7hZ)a;()EP12vM8!f>unRu2LSPlvfvNme7*P)V*n=cL-3a;P}Yq8 zrv>!ECNB#7o($I`Qi;rVe2_h3Su4bV1|=Y1-~(Fg6-A2TcSHWVW!}i(tZK^E?jaJB zeDun=4wV#0M1im&wsPuzC)S1W=gb1bzY@yIE1h$< zxUXlzh+zYgv_g|;zolzC&7aLKAbymP}*D{MWt+9KmP#c>|>LVXZ?ZqQcF$U?$`p_alID zm^fw!M`=lv*w8;Y${-xQk)F?@3P3j~r@nlukim4=FL{Rird(3e6}8br7Dz34*|%Rj zkpvNtHybwGLn54da@AVawqAH>C+7NC>|4;~9a7ML<0WM*mDU{lv=!hOFu6kdxXo_{ z6mZ~Y5lffSYJ-hMK7aQW#tbElgandf2&10@k()KeY)|#5`><5UN;Me$NgVB zNegXR7^tKNv9=6&MV$mGbEtvBiDMK2zWMf+GtW4d%(MMkD9ZG{?bLA?6~5nh)y zww{b){QznYT`<1l{Z}=WB8s^uvn4KjA-=n!7l}zaCXP>wid8AdyyChz02u)^(*e#t z`f=gQRq$Bw0~G)8zpSJ-bGw3sonbD1AGvawE2i;m`$?Z4g;wc3}C%sU>^u`Sswi-1bC^~l}5!?ZNGhuKzCmv+8CZd zW$Mk;qV@C&HWfpO1ph6NZ2iD@x)3(mzeqdF2Idfn`V9q!{g(lC)&Ja0yQaLd1y?dz zYiJo4uZ$7&f$SZN8-W&pRtGt_c!I$i1xWI+gk7ke11rxe_i5!(hYH5JlV2DnUnLx> z26e?HBq1IEL=kBtg(>OW+C}kGj>%&W!Avq!2s*v(d+g$PDcj#j?Kk}wRj-1s5N!S- zj&t_kV7eejR)~(|Hl5y=`8u6JksqR*f_FJ^-1G;k75@@OEvm<5BC& zjc3ASclq&KMc`)wn6UjjO*$jY_dPXGii1K?t_h4Yrqkp5BcL_$KMWfPs7m%nOi^qo2pB#=@ka39FMgp_tl7g4G_f|sM6)gd&6$VrTtTV zU6$#iYHIb!n$04lxW2%;?0?DjpLi#FfX}7<0DoI1BP8D)tFF8SUeyyBMTz9{t-_8+ zH7`d%$+9*I_<2=2bl5O@Rv}bQ?vC)X(-EZ$TY`x#{BPXpUS(utW8Y1n;U&FqPxjYb z2+Bbr@|V5R5zb7FqM^`OGTJ9NE18+OXK4%npS}E6I~RKjrmT7c*yL%4_oI^~^CE@VgXZH6Am|%SSqU@=uv@O3AbJ&uZzp8#nP|f&i9pfJYlIQAwB} zmm(x%gMXo+IA|emvC(IkQ@J0B?73t5zsI@>=6Y1HkLZ<5%+!rd@L}N*8l6KH@jNh}MR`bnEQtRmfEScoYr|eXhO$ExR%2(&Bx#%~(A^&iZmn}@i zQ=;g0B|Jt=cNrVc|0Rx=qT-SKySJ3E0g@jMM(VcDI6RVlDeXoX{^wAYVCqmz%R7^b z-R)@`DJSj3>x8=Tg^{fII}kxxU7GyOOQTsE?1FbbDMSZ}3@9~p;%nVumBFw9`9hgc z8L#v>c9b&M67LS{yANFI8W|ZKnPUD)DZQtdw_%(4C4WW`c7bwWpkoYH06RNy%ZT^# z02%_SzSA}$B;rV_}5?$va09jj~ zL2rJN$Xz+*Le>jN3;2pn#>#vSH>;ZszE_jfo^vt9N$0NUC6%y)LC!zoOg%mS5zA`i zXmuqk^$@@$Q9xV)k+cAPjW15MB0v@bo8B?wxJx~kVe|8qN+OZwZa)7LB{}suSUhLa z9D4v3?_J@G?`sZdaWOZY`WiRK0L3lxF#hC6eqmPFU&RL8veI7wZhnSh%=}Lk=2yP(} zRcpvcM9TVEiYx~Ox9*z3ldt9!9C8FNVlX89@(qe$hcPEqLWeW0a$5{g**(Ae7=U9H z!!7}A4U}!-$+cIf#(_OnM^kJqQe#8!wTp?#IWs zR&uRn!zHZJ`zWfa0vu7mdVsaG#H=njrgYl@73O$|jqW zy>~`*C}ot)?3umyb~4J|A&$clkz*W=WS`^kyN}-Q@8|P*{QfPEdb#gwJg?{VysrC* zcvz=pw`0H^^8{f%dJ*eGFDWD-4@o^=WBoc;^02mQ$7Quk6r*+AmDTkfoD zlH2zH^zP8<0!RahKnna-C6hf|WK&W@C-`v{VEzb>^sJ-<&3hr738`LJ&@C3jN~4XuFk*wHvV_L0JM-FMsA5!+`!Or1S9Tgj$wrB$OFPw<))=t!N z+~b01S;-iIV - + diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index 797d53270b7..b9ed847ce38 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -17,7 +17,7 @@ rclcpp rclcpp_action rclcpp_lifecycle - behaviortree_cpp_v3 + behaviortree_cpp builtin_interfaces geometry_msgs sensor_msgs @@ -36,7 +36,7 @@ rclcpp_action rclcpp_lifecycle std_msgs - behaviortree_cpp_v3 + behaviortree_cpp builtin_interfaces geometry_msgs sensor_msgs diff --git a/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp b/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp index 64eb61d788e..e876d2ce40b 100644 --- a/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp +++ b/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp @@ -70,7 +70,7 @@ BT::NodeStatus AssistedTeleopAction::on_cancelled() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/plugins/action/assisted_teleop_cancel_node.cpp b/nav2_behavior_tree/plugins/action/assisted_teleop_cancel_node.cpp index e226ba19753..362499c9f2e 100644 --- a/nav2_behavior_tree/plugins/action/assisted_teleop_cancel_node.cpp +++ b/nav2_behavior_tree/plugins/action/assisted_teleop_cancel_node.cpp @@ -32,7 +32,7 @@ AssistedTeleopCancel::AssistedTeleopCancel( } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/plugins/action/back_up_action.cpp b/nav2_behavior_tree/plugins/action/back_up_action.cpp index 09d79c7e230..e17580fe71c 100644 --- a/nav2_behavior_tree/plugins/action/back_up_action.cpp +++ b/nav2_behavior_tree/plugins/action/back_up_action.cpp @@ -76,7 +76,7 @@ BT::NodeStatus BackUpAction::on_cancelled() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/plugins/action/back_up_cancel_node.cpp b/nav2_behavior_tree/plugins/action/back_up_cancel_node.cpp index 3baa1ffb1e3..21835c326a8 100644 --- a/nav2_behavior_tree/plugins/action/back_up_cancel_node.cpp +++ b/nav2_behavior_tree/plugins/action/back_up_cancel_node.cpp @@ -32,7 +32,7 @@ BackUpCancel::BackUpCancel( } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp b/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp index 08d2b00632b..2d007a6623e 100644 --- a/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp +++ b/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp @@ -60,7 +60,7 @@ void ClearCostmapAroundRobotService::on_tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("ClearEntireCostmap"); diff --git a/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp b/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp index 8bfffed8434..a0572ecad61 100644 --- a/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp +++ b/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp @@ -65,7 +65,7 @@ BT::NodeStatus ComputePathThroughPosesAction::on_cancelled() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp b/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp index c4b93afa854..c7f1a0164e8 100644 --- a/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp +++ b/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp @@ -71,7 +71,7 @@ void ComputePathToPoseAction::halt() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/plugins/action/controller_cancel_node.cpp b/nav2_behavior_tree/plugins/action/controller_cancel_node.cpp index 966ee01e10a..b2eb4358c20 100644 --- a/nav2_behavior_tree/plugins/action/controller_cancel_node.cpp +++ b/nav2_behavior_tree/plugins/action/controller_cancel_node.cpp @@ -32,7 +32,7 @@ ControllerCancel::ControllerCancel( } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/plugins/action/controller_selector_node.cpp b/nav2_behavior_tree/plugins/action/controller_selector_node.cpp index 7d77adbee39..b58d78bb78f 100644 --- a/nav2_behavior_tree/plugins/action/controller_selector_node.cpp +++ b/nav2_behavior_tree/plugins/action/controller_selector_node.cpp @@ -84,7 +84,7 @@ ControllerSelector::callbackControllerSelect(const std_msgs::msg::String::Shared } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("ControllerSelector"); diff --git a/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp b/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp index 7e0b613f62f..03c00344141 100644 --- a/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp +++ b/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp @@ -74,7 +74,7 @@ BT::NodeStatus DriveOnHeadingAction::on_cancelled() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/plugins/action/drive_on_heading_cancel_node.cpp b/nav2_behavior_tree/plugins/action/drive_on_heading_cancel_node.cpp index b29de63df55..8ac530f8df0 100644 --- a/nav2_behavior_tree/plugins/action/drive_on_heading_cancel_node.cpp +++ b/nav2_behavior_tree/plugins/action/drive_on_heading_cancel_node.cpp @@ -32,7 +32,7 @@ DriveOnHeadingCancel::DriveOnHeadingCancel( } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/plugins/action/follow_path_action.cpp b/nav2_behavior_tree/plugins/action/follow_path_action.cpp index 00acf5c3495..b662221de0f 100644 --- a/nav2_behavior_tree/plugins/action/follow_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/follow_path_action.cpp @@ -96,7 +96,7 @@ void FollowPathAction::on_wait_for_result( } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/plugins/action/goal_checker_selector_node.cpp b/nav2_behavior_tree/plugins/action/goal_checker_selector_node.cpp index 1a2e9c703b0..3ee9d832609 100644 --- a/nav2_behavior_tree/plugins/action/goal_checker_selector_node.cpp +++ b/nav2_behavior_tree/plugins/action/goal_checker_selector_node.cpp @@ -75,7 +75,7 @@ GoalCheckerSelector::callbackGoalCheckerSelect(const std_msgs::msg::String::Shar } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("GoalCheckerSelector"); diff --git a/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp b/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp index 57db8942481..9213cd564f4 100644 --- a/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp +++ b/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp @@ -61,7 +61,7 @@ BT::NodeStatus NavigateThroughPosesAction::on_cancelled() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp b/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp index 8022697e0e8..07bea22436f 100644 --- a/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp +++ b/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp @@ -59,7 +59,7 @@ BT::NodeStatus NavigateToPoseAction::on_cancelled() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/plugins/action/planner_selector_node.cpp b/nav2_behavior_tree/plugins/action/planner_selector_node.cpp index ccb85184127..5d0610c0fa5 100644 --- a/nav2_behavior_tree/plugins/action/planner_selector_node.cpp +++ b/nav2_behavior_tree/plugins/action/planner_selector_node.cpp @@ -84,7 +84,7 @@ PlannerSelector::callbackPlannerSelect(const std_msgs::msg::String::SharedPtr ms } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("PlannerSelector"); diff --git a/nav2_behavior_tree/plugins/action/progress_checker_selector_node.cpp b/nav2_behavior_tree/plugins/action/progress_checker_selector_node.cpp index e8c418ddc17..fea2194158b 100644 --- a/nav2_behavior_tree/plugins/action/progress_checker_selector_node.cpp +++ b/nav2_behavior_tree/plugins/action/progress_checker_selector_node.cpp @@ -74,7 +74,7 @@ ProgressCheckerSelector::callbackProgressCheckerSelect(const std_msgs::msg::Stri } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("ProgressCheckerSelector"); diff --git a/nav2_behavior_tree/plugins/action/reinitialize_global_localization_service.cpp b/nav2_behavior_tree/plugins/action/reinitialize_global_localization_service.cpp index 388f16ccb25..30b876d5346 100644 --- a/nav2_behavior_tree/plugins/action/reinitialize_global_localization_service.cpp +++ b/nav2_behavior_tree/plugins/action/reinitialize_global_localization_service.cpp @@ -26,7 +26,7 @@ ReinitializeGlobalLocalizationService::ReinitializeGlobalLocalizationService( } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType( diff --git a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp index fd255f78c54..86f5fffd6b2 100644 --- a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp +++ b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp @@ -89,7 +89,7 @@ inline BT::NodeStatus RemovePassedGoals::tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("RemovePassedGoals"); diff --git a/nav2_behavior_tree/plugins/action/smooth_path_action.cpp b/nav2_behavior_tree/plugins/action/smooth_path_action.cpp index da547b15876..3a67904558b 100644 --- a/nav2_behavior_tree/plugins/action/smooth_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/smooth_path_action.cpp @@ -64,7 +64,7 @@ BT::NodeStatus SmoothPathAction::on_cancelled() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/plugins/action/smoother_selector_node.cpp b/nav2_behavior_tree/plugins/action/smoother_selector_node.cpp index 0a84062e088..c717332c79b 100644 --- a/nav2_behavior_tree/plugins/action/smoother_selector_node.cpp +++ b/nav2_behavior_tree/plugins/action/smoother_selector_node.cpp @@ -85,7 +85,7 @@ SmootherSelector::callbackSmootherSelect(const std_msgs::msg::String::SharedPtr } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("SmootherSelector"); diff --git a/nav2_behavior_tree/plugins/action/spin_action.cpp b/nav2_behavior_tree/plugins/action/spin_action.cpp index cab9c8f8211..d10bb83f32b 100644 --- a/nav2_behavior_tree/plugins/action/spin_action.cpp +++ b/nav2_behavior_tree/plugins/action/spin_action.cpp @@ -72,7 +72,7 @@ BT::NodeStatus SpinAction::on_cancelled() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/plugins/action/spin_cancel_node.cpp b/nav2_behavior_tree/plugins/action/spin_cancel_node.cpp index 62ddbc4719c..1d75c5cf275 100644 --- a/nav2_behavior_tree/plugins/action/spin_cancel_node.cpp +++ b/nav2_behavior_tree/plugins/action/spin_cancel_node.cpp @@ -32,7 +32,7 @@ SpinCancel::SpinCancel( } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/plugins/action/truncate_path_action.cpp b/nav2_behavior_tree/plugins/action/truncate_path_action.cpp index b394a804c73..7bebfbfba38 100644 --- a/nav2_behavior_tree/plugins/action/truncate_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/truncate_path_action.cpp @@ -20,7 +20,7 @@ #include "nav_msgs/msg/path.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_util/geometry_utils.hpp" -#include "behaviortree_cpp_v3/decorator_node.h" +#include "behaviortree_cpp/decorator_node.h" #include "nav2_behavior_tree/plugins/action/truncate_path_action.hpp" @@ -82,7 +82,7 @@ inline BT::NodeStatus TruncatePath::tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("TruncatePath"); diff --git a/nav2_behavior_tree/plugins/action/truncate_path_local_action.cpp b/nav2_behavior_tree/plugins/action/truncate_path_local_action.cpp index 934f2b86bb8..35dc8635c8c 100644 --- a/nav2_behavior_tree/plugins/action/truncate_path_local_action.cpp +++ b/nav2_behavior_tree/plugins/action/truncate_path_local_action.cpp @@ -17,7 +17,7 @@ #include #include -#include "behaviortree_cpp_v3/decorator_node.h" +#include "behaviortree_cpp/decorator_node.h" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_util/robot_utils.hpp" @@ -150,7 +150,7 @@ TruncatePathLocal::poseDistance( } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType( "TruncatePathLocal"); diff --git a/nav2_behavior_tree/plugins/action/wait_action.cpp b/nav2_behavior_tree/plugins/action/wait_action.cpp index b9317076737..b607d026e59 100644 --- a/nav2_behavior_tree/plugins/action/wait_action.cpp +++ b/nav2_behavior_tree/plugins/action/wait_action.cpp @@ -56,7 +56,7 @@ void WaitAction::on_tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/plugins/action/wait_cancel_node.cpp b/nav2_behavior_tree/plugins/action/wait_cancel_node.cpp index 9365b7e06fa..b45db33396f 100644 --- a/nav2_behavior_tree/plugins/action/wait_cancel_node.cpp +++ b/nav2_behavior_tree/plugins/action/wait_cancel_node.cpp @@ -32,7 +32,7 @@ WaitCancel::WaitCancel( } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.cpp b/nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.cpp index 6764cccd116..f39dd8fbce4 100644 --- a/nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.cpp @@ -14,7 +14,7 @@ #include "nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType( diff --git a/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp b/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp index 167caec938c..7db1817c65e 100644 --- a/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp @@ -56,7 +56,7 @@ BT::NodeStatus DistanceTraveledCondition::tick() initialize(); } - if (status() == BT::NodeStatus::IDLE) { + if (!BT::isStatusActive(status())) { if (!nav2_util::getCurrentPose( start_pose_, *tf_, global_frame_, robot_base_frame_, transform_tolerance_)) @@ -92,7 +92,7 @@ BT::NodeStatus DistanceTraveledCondition::tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("DistanceTraveled"); diff --git a/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp b/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp index 61c45696f57..dbd84d8b2ef 100644 --- a/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp @@ -55,7 +55,7 @@ BT::NodeStatus GloballyUpdatedGoalCondition::tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("GlobalUpdatedGoal"); diff --git a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp index 8609ab55902..70243562034 100644 --- a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp @@ -90,7 +90,7 @@ bool GoalReachedCondition::isGoalReached() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("GoalReached"); diff --git a/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp b/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp index f2f112c348e..88d329efc2a 100644 --- a/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp @@ -28,7 +28,7 @@ GoalUpdatedCondition::GoalUpdatedCondition( BT::NodeStatus GoalUpdatedCondition::tick() { - if (status() == BT::NodeStatus::IDLE) { + if (!BT::isStatusActive(status())) { BT::getInputOrBlackboard("goals", goals_); BT::getInputOrBlackboard("goal", goal_); return BT::NodeStatus::FAILURE; @@ -50,7 +50,7 @@ BT::NodeStatus GoalUpdatedCondition::tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("GoalUpdated"); diff --git a/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.cpp b/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.cpp index 0a74ce68c95..9d930229124 100644 --- a/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.cpp @@ -33,7 +33,7 @@ BT::NodeStatus InitialPoseReceived::tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("InitialPoseReceived"); diff --git a/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.cpp b/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.cpp index c9c05f68753..27e1bd55fc7 100644 --- a/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.cpp @@ -59,7 +59,7 @@ void IsBatteryChargingCondition::batteryCallback(sensor_msgs::msg::BatteryState: } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("IsBatteryCharging"); diff --git a/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp b/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp index 9d221cf8c81..a0e3761f28c 100644 --- a/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp @@ -77,7 +77,7 @@ void IsBatteryLowCondition::batteryCallback(sensor_msgs::msg::BatteryState::Shar } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("IsBatteryLow"); diff --git a/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp b/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp index 59d593a1475..7274743e1e9 100644 --- a/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp @@ -64,7 +64,7 @@ BT::NodeStatus IsPathValidCondition::tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("IsPathValid"); diff --git a/nav2_behavior_tree/plugins/condition/is_stuck_condition.cpp b/nav2_behavior_tree/plugins/condition/is_stuck_condition.cpp index a898dec9473..d0ca48cd7f5 100644 --- a/nav2_behavior_tree/plugins/condition/is_stuck_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_stuck_condition.cpp @@ -143,7 +143,7 @@ bool IsStuckCondition::isStuck() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("IsStuck"); diff --git a/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.cpp b/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.cpp index 1347998602f..540af1d83d3 100644 --- a/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.cpp @@ -15,7 +15,7 @@ #include #include -#include "behaviortree_cpp_v3/condition_node.h" +#include "behaviortree_cpp/condition_node.h" #include "nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp" @@ -68,7 +68,7 @@ BT::NodeStatus PathExpiringTimerCondition::tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("PathExpiringTimer"); diff --git a/nav2_behavior_tree/plugins/condition/time_expired_condition.cpp b/nav2_behavior_tree/plugins/condition/time_expired_condition.cpp index 64afd1a34be..f03c9711aa8 100644 --- a/nav2_behavior_tree/plugins/condition/time_expired_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/time_expired_condition.cpp @@ -16,7 +16,7 @@ #include #include -#include "behaviortree_cpp_v3/condition_node.h" +#include "behaviortree_cpp/condition_node.h" #include "nav2_behavior_tree/plugins/condition/time_expired_condition.hpp" @@ -46,7 +46,7 @@ BT::NodeStatus TimeExpiredCondition::tick() initialize(); } - if (status() == BT::NodeStatus::IDLE) { + if (!BT::isStatusActive(status())) { start_ = node_->now(); return BT::NodeStatus::FAILURE; } @@ -67,7 +67,7 @@ BT::NodeStatus TimeExpiredCondition::tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("TimeExpired"); diff --git a/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp b/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp index 33e94178deb..0ee491fbfd2 100644 --- a/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp @@ -83,7 +83,7 @@ BT::NodeStatus TransformAvailableCondition::tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("TransformAvailable"); diff --git a/nav2_behavior_tree/plugins/condition/would_a_controller_recovery_help_condition.cpp b/nav2_behavior_tree/plugins/condition/would_a_controller_recovery_help_condition.cpp index f8c62064f6c..87625d4511f 100644 --- a/nav2_behavior_tree/plugins/condition/would_a_controller_recovery_help_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/would_a_controller_recovery_help_condition.cpp @@ -33,7 +33,7 @@ WouldAControllerRecoveryHelp::WouldAControllerRecoveryHelp( } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType( diff --git a/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.cpp b/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.cpp index 91de9c2e22a..603eb60f107 100644 --- a/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.cpp @@ -35,7 +35,7 @@ WouldAPlannerRecoveryHelp::WouldAPlannerRecoveryHelp( } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType( diff --git a/nav2_behavior_tree/plugins/condition/would_a_smoother_recovery_help_condition.cpp b/nav2_behavior_tree/plugins/condition/would_a_smoother_recovery_help_condition.cpp index fb903e1cbfc..50665256c25 100644 --- a/nav2_behavior_tree/plugins/condition/would_a_smoother_recovery_help_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/would_a_smoother_recovery_help_condition.cpp @@ -33,7 +33,7 @@ WouldASmootherRecoveryHelp::WouldASmootherRecoveryHelp( } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType( diff --git a/nav2_behavior_tree/plugins/control/pipeline_sequence.cpp b/nav2_behavior_tree/plugins/control/pipeline_sequence.cpp index 640c3ed7b4c..bc2327af284 100644 --- a/nav2_behavior_tree/plugins/control/pipeline_sequence.cpp +++ b/nav2_behavior_tree/plugins/control/pipeline_sequence.cpp @@ -35,6 +35,7 @@ PipelineSequence::PipelineSequence( BT::NodeStatus PipelineSequence::tick() { + unsigned skipped_count = 0; for (std::size_t i = 0; i < children_nodes_.size(); ++i) { auto status = children_nodes_[i]->executeTick(); switch (status) { @@ -42,6 +43,10 @@ BT::NodeStatus PipelineSequence::tick() ControlNode::haltChildren(); last_child_ticked_ = 0; // reset return status; + case BT::NodeStatus::SKIPPED: + skipped_count++; + // do nothing and continue on to the next child. + break; case BT::NodeStatus::SUCCESS: // do nothing and continue on to the next child. If it is the last child // we'll exit the loop and hit the wrap-up code at the end of the method. @@ -63,6 +68,10 @@ BT::NodeStatus PipelineSequence::tick() // Wrap up. ControlNode::haltChildren(); last_child_ticked_ = 0; // reset + if (skipped_count == children_nodes_.size()) { + // All the children were skipped + return BT::NodeStatus::SKIPPED; + } return BT::NodeStatus::SUCCESS; } diff --git a/nav2_behavior_tree/plugins/control/recovery_node.cpp b/nav2_behavior_tree/plugins/control/recovery_node.cpp index 44e56dc55ec..6eb3c6e99e5 100644 --- a/nav2_behavior_tree/plugins/control/recovery_node.cpp +++ b/nav2_behavior_tree/plugins/control/recovery_node.cpp @@ -45,12 +45,18 @@ BT::NodeStatus RecoveryNode::tick() if (current_child_idx_ == 0) { switch (child_status) { + case BT::NodeStatus::SKIPPED: + // If first child is skipped, the entire branch is considered skipped + halt(); + return BT::NodeStatus::SKIPPED; + case BT::NodeStatus::SUCCESS: - { - // reset node and return success when first child returns success - halt(); - return BT::NodeStatus::SUCCESS; - } + // reset node and return success when first child returns success + halt(); + return BT::NodeStatus::SUCCESS; + + case BT::NodeStatus::RUNNING: + return BT::NodeStatus::RUNNING; case BT::NodeStatus::FAILURE: { @@ -66,44 +72,41 @@ BT::NodeStatus RecoveryNode::tick() } } - case BT::NodeStatus::RUNNING: - { - return BT::NodeStatus::RUNNING; - } - default: - { - throw BT::LogicError("A child node must never return IDLE"); - } + throw BT::LogicError("A child node must never return IDLE"); } // end switch } else if (current_child_idx_ == 1) { switch (child_status) { + case BT::NodeStatus::SKIPPED: + { + // if we skip the recovery (maybe a precondition fails), then we + // should assume that no recovery is possible. For this reason, + // we should return FAILURE and reset the index. + // This does not count as a retry. + current_child_idx_ = 0; + ControlNode::haltChild(1); + return BT::NodeStatus::FAILURE; + } + case BT::NodeStatus::RUNNING: + return child_status; + case BT::NodeStatus::SUCCESS: { // halt second child, increment recovery count, and tick first child in next iteration ControlNode::haltChild(1); retry_count_++; - current_child_idx_--; + current_child_idx_ = 0; } break; case BT::NodeStatus::FAILURE: - { - // reset node and return failure if second child fails - halt(); - return BT::NodeStatus::FAILURE; - } - - case BT::NodeStatus::RUNNING: - { - return BT::NodeStatus::RUNNING; - } + // reset node and return failure if second child fails + halt(); + return BT::NodeStatus::FAILURE; default: - { - throw BT::LogicError("A child node must never return IDLE"); - } + throw BT::LogicError("A child node must never return IDLE"); } // end switch } } // end while loop @@ -122,7 +125,7 @@ void RecoveryNode::halt() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("RecoveryNode"); diff --git a/nav2_behavior_tree/plugins/control/round_robin_node.cpp b/nav2_behavior_tree/plugins/control/round_robin_node.cpp index 135ea09b744..9293a5b1181 100644 --- a/nav2_behavior_tree/plugins/control/round_robin_node.cpp +++ b/nav2_behavior_tree/plugins/control/round_robin_node.cpp @@ -36,18 +36,22 @@ BT::NodeStatus RoundRobinNode::tick() const auto num_children = children_nodes_.size(); setStatus(BT::NodeStatus::RUNNING); + unsigned num_skipped_children = 0; - while (num_failed_children_ < num_children) { + while (num_failed_children_ + num_skipped_children < num_children) { TreeNode * child_node = children_nodes_[current_child_idx_]; const BT::NodeStatus child_status = child_node->executeTick(); + if (child_status != BT::NodeStatus::RUNNING) { + // Increment index and wrap around to the first child + if (++current_child_idx_ == num_children) { + current_child_idx_ = 0; + } + } + switch (child_status) { case BT::NodeStatus::SUCCESS: { - // Wrap around to the first child - if (++current_child_idx_ >= num_children) { - current_child_idx_ = 0; - } num_failed_children_ = 0; ControlNode::haltChildren(); return BT::NodeStatus::SUCCESS; @@ -55,27 +59,27 @@ BT::NodeStatus RoundRobinNode::tick() case BT::NodeStatus::FAILURE: { - if (++current_child_idx_ >= num_children) { - current_child_idx_ = 0; - } num_failed_children_++; break; } - case BT::NodeStatus::RUNNING: + case BT::NodeStatus::SKIPPED: { - return BT::NodeStatus::RUNNING; + num_skipped_children++; + break; } + case BT::NodeStatus::RUNNING: + return BT::NodeStatus::RUNNING; default: - { - throw BT::LogicError("Invalid status return from BT node"); - } + throw BT::LogicError("Invalid status return from BT node"); } } + const bool all_skipped = (num_skipped_children == num_children); halt(); - return BT::NodeStatus::FAILURE; + // If all the children were skipped, this node is considered skipped + return all_skipped ? BT::NodeStatus::SKIPPED : BT::NodeStatus::FAILURE; } void RoundRobinNode::halt() diff --git a/nav2_behavior_tree/plugins/decorator/distance_controller.cpp b/nav2_behavior_tree/plugins/decorator/distance_controller.cpp index 3cef0ea978c..7f87695416d 100644 --- a/nav2_behavior_tree/plugins/decorator/distance_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/distance_controller.cpp @@ -23,7 +23,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "tf2_ros/buffer.h" -#include "behaviortree_cpp_v3/decorator_node.h" +#include "behaviortree_cpp/decorator_node.h" #include "nav2_behavior_tree/plugins/decorator/distance_controller.hpp" #include "nav2_behavior_tree/bt_utils.hpp" @@ -51,7 +51,7 @@ DistanceController::DistanceController( inline BT::NodeStatus DistanceController::tick() { - if (status() == BT::NodeStatus::IDLE) { + if (!BT::isStatusActive(status())) { // Reset the starting position since we're starting a new iteration of // the distance controller (moving from IDLE to RUNNING) if (!nav2_util::getCurrentPose( @@ -90,8 +90,9 @@ inline BT::NodeStatus DistanceController::tick() const BT::NodeStatus child_state = child_node_->executeTick(); switch (child_state) { + case BT::NodeStatus::SKIPPED: case BT::NodeStatus::RUNNING: - return BT::NodeStatus::RUNNING; + return child_state; case BT::NodeStatus::SUCCESS: if (!nav2_util::getCurrentPose( @@ -114,7 +115,7 @@ inline BT::NodeStatus DistanceController::tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("DistanceController"); diff --git a/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp b/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp index 049bc61f7ec..d0de9205452 100644 --- a/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp @@ -17,7 +17,7 @@ #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" -#include "behaviortree_cpp_v3/decorator_node.h" +#include "behaviortree_cpp/decorator_node.h" #include "nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp" #include "nav2_behavior_tree/bt_utils.hpp" @@ -33,7 +33,7 @@ GoalUpdatedController::GoalUpdatedController( BT::NodeStatus GoalUpdatedController::tick() { - if (status() == BT::NodeStatus::IDLE) { + if (!BT::isStatusActive(status())) { // Reset since we're starting a new iteration of // the goal updated controller (moving from IDLE to RUNNING) @@ -61,19 +61,7 @@ BT::NodeStatus GoalUpdatedController::tick() // 'til completion if ((child_node_->status() == BT::NodeStatus::RUNNING) || goal_was_updated_) { goal_was_updated_ = false; - const BT::NodeStatus child_state = child_node_->executeTick(); - - switch (child_state) { - case BT::NodeStatus::RUNNING: - return BT::NodeStatus::RUNNING; - - case BT::NodeStatus::SUCCESS: - return BT::NodeStatus::SUCCESS; - - case BT::NodeStatus::FAILURE: - default: - return BT::NodeStatus::FAILURE; - } + return child_node_->executeTick(); } return status(); @@ -81,7 +69,7 @@ BT::NodeStatus GoalUpdatedController::tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("GoalUpdatedController"); diff --git a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp index 1f7226e6aac..fa5badf750d 100644 --- a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp +++ b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp @@ -17,7 +17,7 @@ #include #include "geometry_msgs/msg/pose_stamped.hpp" -#include "behaviortree_cpp_v3/decorator_node.h" +#include "behaviortree_cpp/decorator_node.h" #include "nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp" @@ -84,7 +84,7 @@ GoalUpdater::callback_updated_goal(const geometry_msgs::msg::PoseStamped::Shared } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("GoalUpdater"); diff --git a/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp b/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp index d4b40b0964e..cb39a245579 100644 --- a/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp +++ b/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp @@ -62,7 +62,7 @@ inline BT::NodeStatus PathLongerOnApproach::tick() getInput("prox_len", prox_len_); getInput("length_factor", length_factor_); - if (status() == BT::NodeStatus::IDLE) { + if (!BT::isStatusActive(status())) { // Reset the starting point since we're starting a new iteration of // PathLongerOnApproach (moving from IDLE to RUNNING) first_time_ = true; @@ -77,14 +77,14 @@ inline BT::NodeStatus PathLongerOnApproach::tick() { const BT::NodeStatus child_state = child_node_->executeTick(); switch (child_state) { + case BT::NodeStatus::SKIPPED: case BT::NodeStatus::RUNNING: - return BT::NodeStatus::RUNNING; + return child_state; case BT::NodeStatus::SUCCESS: - old_path_ = new_path_; - return BT::NodeStatus::SUCCESS; case BT::NodeStatus::FAILURE: old_path_ = new_path_; - return BT::NodeStatus::FAILURE; + resetChild(); + return child_state; default: old_path_ = new_path_; return BT::NodeStatus::FAILURE; @@ -97,7 +97,7 @@ inline BT::NodeStatus PathLongerOnApproach::tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("PathLongerOnApproach"); diff --git a/nav2_behavior_tree/plugins/decorator/rate_controller.cpp b/nav2_behavior_tree/plugins/decorator/rate_controller.cpp index b710ec30091..9592d119c96 100644 --- a/nav2_behavior_tree/plugins/decorator/rate_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/rate_controller.cpp @@ -43,7 +43,7 @@ BT::NodeStatus RateController::tick() initialize(); } - if (status() == BT::NodeStatus::IDLE) { + if (!BT::isStatusActive(status())) { // Reset the starting point since we're starting a new iteration of // the rate controller (moving from IDLE to RUNNING) start_ = std::chrono::high_resolution_clock::now(); @@ -70,14 +70,15 @@ BT::NodeStatus RateController::tick() const BT::NodeStatus child_state = child_node_->executeTick(); switch (child_state) { + case BT::NodeStatus::SKIPPED: case BT::NodeStatus::RUNNING: - return BT::NodeStatus::RUNNING; + case BT::NodeStatus::FAILURE: + return child_state; case BT::NodeStatus::SUCCESS: start_ = std::chrono::high_resolution_clock::now(); // Reset the timer return BT::NodeStatus::SUCCESS; - case BT::NodeStatus::FAILURE: default: return BT::NodeStatus::FAILURE; } @@ -88,7 +89,7 @@ BT::NodeStatus RateController::tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("RateController"); diff --git a/nav2_behavior_tree/plugins/decorator/single_trigger_node.cpp b/nav2_behavior_tree/plugins/decorator/single_trigger_node.cpp index 84f0fadfbbc..c95d646438a 100644 --- a/nav2_behavior_tree/plugins/decorator/single_trigger_node.cpp +++ b/nav2_behavior_tree/plugins/decorator/single_trigger_node.cpp @@ -30,7 +30,7 @@ SingleTrigger::SingleTrigger( BT::NodeStatus SingleTrigger::tick() { - if (status() == BT::NodeStatus::IDLE) { + if (!BT::isStatusActive(status())) { first_time_ = true; } @@ -40,16 +40,14 @@ BT::NodeStatus SingleTrigger::tick() const BT::NodeStatus child_state = child_node_->executeTick(); switch (child_state) { + case BT::NodeStatus::SKIPPED: case BT::NodeStatus::RUNNING: - return BT::NodeStatus::RUNNING; - - case BT::NodeStatus::SUCCESS: - first_time_ = false; - return BT::NodeStatus::SUCCESS; + return child_state; case BT::NodeStatus::FAILURE: + case BT::NodeStatus::SUCCESS: first_time_ = false; - return BT::NodeStatus::FAILURE; + return child_state; default: first_time_ = false; @@ -62,7 +60,7 @@ BT::NodeStatus SingleTrigger::tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("SingleTrigger"); diff --git a/nav2_behavior_tree/plugins/decorator/speed_controller.cpp b/nav2_behavior_tree/plugins/decorator/speed_controller.cpp index 8d160aa76e1..b8e5b3915a3 100644 --- a/nav2_behavior_tree/plugins/decorator/speed_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/speed_controller.cpp @@ -59,7 +59,7 @@ SpeedController::SpeedController( inline BT::NodeStatus SpeedController::tick() { - if (status() == BT::NodeStatus::IDLE) { + if (!BT::isStatusActive(status())) { // Reset since we're starting a new iteration of // the speed controller (moving from IDLE to RUNNING) BT::getInputOrBlackboard("goals", goals_); @@ -101,19 +101,7 @@ inline BT::NodeStatus SpeedController::tick() start_ = node_->now(); } - const BT::NodeStatus child_state = child_node_->executeTick(); - - switch (child_state) { - case BT::NodeStatus::RUNNING: - return BT::NodeStatus::RUNNING; - - case BT::NodeStatus::SUCCESS: - return BT::NodeStatus::SUCCESS; - - case BT::NodeStatus::FAILURE: - default: - return BT::NodeStatus::FAILURE; - } + return child_node_->executeTick(); } return status(); @@ -121,7 +109,7 @@ inline BT::NodeStatus SpeedController::tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("SpeedController"); diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp index 33100d76bb3..8ed1cd4e71b 100644 --- a/nav2_behavior_tree/src/behavior_tree_engine.cpp +++ b/nav2_behavior_tree/src/behavior_tree_engine.cpp @@ -20,7 +20,7 @@ #include #include "rclcpp/rclcpp.hpp" -#include "behaviortree_cpp_v3/utils/shared_library.h" +#include "behaviortree_cpp/utils/shared_library.h" namespace nav2_behavior_tree { @@ -32,6 +32,11 @@ BehaviorTreeEngine::BehaviorTreeEngine( for (const auto & p : plugin_libraries) { factory_.registerFromPlugin(loader.getOSName(p)); } + + // FIXME: the next two line are needed for back-compatibility with BT.CPP 3.8.x + // Note that the can be removed, once we migrate from BT.CPP 4.5.x to 4.6+ + BT::ReactiveSequence::EnableException(false); + BT::ReactiveFallback::EnableException(false); } BtStatus @@ -52,7 +57,7 @@ BehaviorTreeEngine::run( return BtStatus::CANCELED; } - result = tree->tickRoot(); + result = tree->tickOnce(); onLoop(); diff --git a/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_action.cpp b/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_action.cpp index cbb5606d8bd..c5d40f0f4e6 100644 --- a/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_action.cpp @@ -17,7 +17,7 @@ #include #include -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp" @@ -123,7 +123,7 @@ TEST_F(AssistedTeleopActionTestFixture, test_ports) { std::string xml_txt = R"( - + @@ -134,7 +134,7 @@ TEST_F(AssistedTeleopActionTestFixture, test_ports) xml_txt = R"( - + @@ -148,7 +148,7 @@ TEST_F(AssistedTeleopActionTestFixture, test_tick) { std::string xml_txt = R"( - + @@ -172,7 +172,7 @@ TEST_F(AssistedTeleopActionTestFixture, test_failure) { std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_cancel_node.cpp index a9261fe4b8d..a516d868e4f 100644 --- a/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_cancel_node.cpp @@ -17,7 +17,7 @@ #include #include -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/assisted_teleop_cancel_node.hpp" @@ -122,7 +122,7 @@ TEST_F(CancelAssistedTeleopActionTestFixture, test_ports) { std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp b/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp index 9df8b3da154..846e7e86db9 100644 --- a/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp @@ -18,7 +18,7 @@ #include #include -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/back_up_action.hpp" @@ -122,7 +122,7 @@ TEST_F(BackUpActionTestFixture, test_ports) { std::string xml_txt = R"( - + @@ -134,7 +134,7 @@ TEST_F(BackUpActionTestFixture, test_ports) xml_txt = R"( - + @@ -149,7 +149,7 @@ TEST_F(BackUpActionTestFixture, test_tick) { std::string xml_txt = R"( - + @@ -174,7 +174,7 @@ TEST_F(BackUpActionTestFixture, test_failure) { std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_back_up_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_back_up_cancel_node.cpp index c7f379a64c2..a3ea28791c6 100644 --- a/nav2_behavior_tree/test/plugins/action/test_back_up_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_back_up_cancel_node.cpp @@ -17,7 +17,7 @@ #include #include -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/back_up_cancel_node.hpp" @@ -120,7 +120,7 @@ TEST_F(CancelBackUpActionTestFixture, test_ports) { std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp b/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp index ec3aa03a079..8aa1c3366eb 100644 --- a/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp @@ -23,7 +23,7 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "nav2_behavior_tree/bt_action_node.hpp" #include "test_msgs/action/fibonacci.hpp" @@ -238,7 +238,7 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_success) // create tree std::string xml_txt = R"( - + @@ -264,7 +264,7 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_success) // main BT execution loop while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) { - result = tree_->tickRoot(); + result = tree_->tickOnce(); ticks++; loopRate.sleep(); } @@ -307,7 +307,7 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_success) // main BT execution loop while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) { - result = tree_->tickRoot(); + result = tree_->tickOnce(); ticks++; loopRate.sleep(); } @@ -316,8 +316,10 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_success) // the BT should have failed EXPECT_EQ(result, BT::NodeStatus::FAILURE); - // since the server timeout is 20ms and bt loop duration is 10ms, number of ticks should be 2 - EXPECT_EQ(ticks, 2); + // since the server timeout is 20ms and bt loop duration is 10ms, number of ticks should + // be at most 2, but it can be 1 too, because the tickOnce may execute two ticks. + EXPECT_LE(ticks, 2); + EXPECT_GE(ticks, 1); } TEST_F(BTActionNodeTestFixture, test_server_timeout_failure) @@ -325,7 +327,7 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_failure) // create tree std::string xml_txt = R"( - + @@ -352,7 +354,7 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_failure) // main BT execution loop while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) { - result = tree_->tickRoot(); + result = tree_->tickOnce(); ticks++; loopRate.sleep(); } @@ -386,7 +388,7 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_failure) // main BT execution loop while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) { - result = tree_->tickRoot(); + result = tree_->tickOnce(); ticks++; loopRate.sleep(); } @@ -401,7 +403,7 @@ TEST_F(BTActionNodeTestFixture, test_server_cancel) // create tree std::string xml_txt = R"( - + @@ -429,7 +431,7 @@ TEST_F(BTActionNodeTestFixture, test_server_cancel) // main BT execution loop while (rclcpp::ok() && result == BT::NodeStatus::RUNNING && ticks < 5) { - result = tree_->tickRoot(); + result = tree_->tickOnce(); ticks++; loopRate.sleep(); } @@ -461,7 +463,7 @@ TEST_F(BTActionNodeTestFixture, test_server_cancel) // main BT execution loop while (rclcpp::ok() && result == BT::NodeStatus::RUNNING && ticks < 7) { - result = tree_->tickRoot(); + result = tree_->tickOnce(); ticks++; loopRate.sleep(); } diff --git a/nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp b/nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp index e21f9802d7e..76f3b1d025a 100644 --- a/nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp @@ -18,7 +18,7 @@ #include #include -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_service.hpp" #include "nav2_behavior_tree/plugins/action/clear_costmap_service.hpp" @@ -100,7 +100,7 @@ TEST_F(ClearEntireCostmapServiceTestFixture, test_tick) { std::string xml_txt = R"( - + @@ -195,7 +195,7 @@ TEST_F(ClearCostmapExceptRegionServiceTestFixture, test_tick) { std::string xml_txt = R"( - + @@ -290,7 +290,7 @@ TEST_F(ClearCostmapAroundRobotServiceTestFixture, test_tick) { std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp b/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp index 20c4461acf5..24d10b63d63 100644 --- a/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp @@ -22,7 +22,7 @@ #include "nav_msgs/msg/path.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp" @@ -128,7 +128,7 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick) // create tree std::string xml_txt = R"( - + @@ -187,7 +187,7 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick_use_start) // create tree std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp b/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp index 6065f4543d2..29ebaef9362 100644 --- a/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp @@ -21,7 +21,7 @@ #include "nav_msgs/msg/path.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp" @@ -125,7 +125,7 @@ TEST_F(ComputePathToPoseActionTestFixture, test_tick) // create tree std::string xml_txt = R"( - + @@ -184,7 +184,7 @@ TEST_F(ComputePathToPoseActionTestFixture, test_tick_use_start) // create tree std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_controller_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_controller_cancel_node.cpp index 7be55c08ae0..564e72d3d54 100644 --- a/nav2_behavior_tree/test/plugins/action/test_controller_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_controller_cancel_node.cpp @@ -17,7 +17,7 @@ #include #include -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/controller_cancel_node.hpp" @@ -120,7 +120,7 @@ TEST_F(CancelControllerActionTestFixture, test_ports) { std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_controller_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_controller_selector_node.cpp index 3218877c847..c7f04ae9a95 100644 --- a/nav2_behavior_tree/test/plugins/action/test_controller_selector_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_controller_selector_node.cpp @@ -20,7 +20,7 @@ #include #include "utils/test_action_server.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "nav2_behavior_tree/plugins/action/controller_selector_node.hpp" #include "nav_msgs/msg/path.hpp" #include "std_msgs/msg/string.hpp" @@ -80,7 +80,7 @@ TEST_F(ControllerSelectorTestFixture, test_custom_topic) // create tree std::string xml_txt = R"( - + @@ -128,7 +128,7 @@ TEST_F(ControllerSelectorTestFixture, test_default_topic) // create tree std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp b/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp index c4439f1e1cf..baa5ea02a2c 100644 --- a/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp @@ -18,7 +18,7 @@ #include #include -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp" @@ -118,7 +118,7 @@ TEST_F(DriveOnHeadingActionTestFixture, test_ports) { std::string xml_txt = R"( - + @@ -131,7 +131,7 @@ TEST_F(DriveOnHeadingActionTestFixture, test_ports) xml_txt = R"( - + @@ -146,7 +146,7 @@ TEST_F(DriveOnHeadingActionTestFixture, test_tick) { std::string xml_txt = R"( - + @@ -169,7 +169,7 @@ TEST_F(DriveOnHeadingActionTestFixture, test_failure) { std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_cancel_node.cpp index 762351bdd1b..c94ed1d89f8 100644 --- a/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_cancel_node.cpp @@ -17,7 +17,7 @@ #include #include -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/drive_on_heading_cancel_node.hpp" @@ -123,7 +123,7 @@ TEST_F(CancelDriveOnHeadingTestFixture, test_ports) { std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp b/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp index 47766d8e9d9..4f272208198 100644 --- a/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp @@ -21,7 +21,7 @@ #include "nav_msgs/msg/path.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/follow_path_action.hpp" @@ -118,7 +118,7 @@ TEST_F(FollowPathActionTestFixture, test_tick) // create tree std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_goal_checker_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_goal_checker_selector_node.cpp index 747686000ed..19089fb6f3e 100644 --- a/nav2_behavior_tree/test/plugins/action/test_goal_checker_selector_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_goal_checker_selector_node.cpp @@ -20,7 +20,7 @@ #include #include "utils/test_action_server.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "nav2_behavior_tree/plugins/action/goal_checker_selector_node.hpp" #include "nav_msgs/msg/path.hpp" #include "std_msgs/msg/string.hpp" @@ -80,7 +80,7 @@ TEST_F(GoalCheckerSelectorTestFixture, test_custom_topic) // create tree std::string xml_txt = R"( - + @@ -128,7 +128,7 @@ TEST_F(GoalCheckerSelectorTestFixture, test_default_topic) // create tree std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp b/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp index 73de1ac53f5..a805a721b9f 100644 --- a/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp @@ -23,7 +23,7 @@ #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/quaternion.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp" @@ -124,7 +124,7 @@ TEST_F(NavigateThroughPosesActionTestFixture, test_tick) // create tree std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_navigate_to_pose_action.cpp b/nav2_behavior_tree/test/plugins/action/test_navigate_to_pose_action.cpp index aef672b940c..4ed5120b986 100644 --- a/nav2_behavior_tree/test/plugins/action/test_navigate_to_pose_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_navigate_to_pose_action.cpp @@ -22,7 +22,7 @@ #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/quaternion.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp" @@ -119,7 +119,7 @@ TEST_F(NavigateToPoseActionTestFixture, test_tick) // create tree std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_planner_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_planner_selector_node.cpp index efc59adb6c8..ade80c57daf 100644 --- a/nav2_behavior_tree/test/plugins/action/test_planner_selector_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_planner_selector_node.cpp @@ -20,7 +20,7 @@ #include #include "utils/test_action_server.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "nav2_behavior_tree/plugins/action/planner_selector_node.hpp" #include "nav_msgs/msg/path.hpp" #include "std_msgs/msg/string.hpp" @@ -78,7 +78,7 @@ TEST_F(PlannerSelectorTestFixture, test_custom_topic) // create tree std::string xml_txt = R"( - + @@ -126,7 +126,7 @@ TEST_F(PlannerSelectorTestFixture, test_default_topic) // create tree std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_progress_checker_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_progress_checker_selector_node.cpp index 574a514886b..c97bbc19699 100644 --- a/nav2_behavior_tree/test/plugins/action/test_progress_checker_selector_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_progress_checker_selector_node.cpp @@ -19,7 +19,7 @@ #include #include "utils/test_action_server.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp" #include "nav_msgs/msg/path.hpp" #include "std_msgs/msg/string.hpp" @@ -79,7 +79,7 @@ TEST_F(ProgressCheckerSelectorTestFixture, test_custom_topic) // create tree std::string xml_txt = R"( - + @@ -94,7 +94,9 @@ TEST_F(ProgressCheckerSelectorTestFixture, test_custom_topic) // check default value std::string selected_progress_checker_result; - config_->blackboard->get("selected_progress_checker", selected_progress_checker_result); + [[maybe_unused]] auto res = config_->blackboard->get( + "selected_progress_checker", + selected_progress_checker_result); EXPECT_EQ(selected_progress_checker_result, "SimpleProgressCheck"); @@ -119,7 +121,7 @@ TEST_F(ProgressCheckerSelectorTestFixture, test_custom_topic) } // check progress_checker updated - config_->blackboard->get("selected_progress_checker", selected_progress_checker_result); + res = config_->blackboard->get("selected_progress_checker", selected_progress_checker_result); EXPECT_EQ("AngularProgressChecker", selected_progress_checker_result); } @@ -128,7 +130,7 @@ TEST_F(ProgressCheckerSelectorTestFixture, test_default_topic) // create tree std::string xml_txt = R"( - + @@ -143,7 +145,9 @@ TEST_F(ProgressCheckerSelectorTestFixture, test_default_topic) // check default value std::string selected_progress_checker_result; - config_->blackboard->get("selected_progress_checker", selected_progress_checker_result); + [[maybe_unused]] auto res = config_->blackboard->get( + "selected_progress_checker", + selected_progress_checker_result); EXPECT_EQ(selected_progress_checker_result, "GridBased"); @@ -167,7 +171,7 @@ TEST_F(ProgressCheckerSelectorTestFixture, test_default_topic) } // check goal_checker updated - config_->blackboard->get("selected_progress_checker", selected_progress_checker_result); + res = config_->blackboard->get("selected_progress_checker", selected_progress_checker_result); EXPECT_EQ("RRT", selected_progress_checker_result); } diff --git a/nav2_behavior_tree/test/plugins/action/test_reinitialize_global_localization_service.cpp b/nav2_behavior_tree/test/plugins/action/test_reinitialize_global_localization_service.cpp index 73e72fa764d..04b0e704903 100644 --- a/nav2_behavior_tree/test/plugins/action/test_reinitialize_global_localization_service.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_reinitialize_global_localization_service.cpp @@ -18,7 +18,7 @@ #include #include -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_service.hpp" #include "nav2_behavior_tree/plugins/action/reinitialize_global_localization_service.hpp" @@ -97,7 +97,7 @@ TEST_F(ReinitializeGlobalLocalizationServiceTestFixture, test_tick) { std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp b/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp index a8ac368a160..ce9d0debf8e 100644 --- a/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp @@ -23,7 +23,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_util/geometry_utils.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp" @@ -105,7 +105,7 @@ TEST_F(RemovePassedGoalsTestFixture, test_tick) // create tree std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp b/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp index 917b696acf8..d60ed2ffb78 100644 --- a/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp @@ -21,7 +21,7 @@ #include "nav_msgs/msg/path.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/smooth_path_action.hpp" @@ -118,7 +118,7 @@ TEST_F(SmoothPathActionTestFixture, test_tick) // create tree std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp index c3476615069..f93f6df878f 100644 --- a/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp @@ -20,7 +20,7 @@ #include #include "utils/test_action_server.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "nav2_behavior_tree/plugins/action/smoother_selector_node.hpp" #include "nav_msgs/msg/path.hpp" #include "std_msgs/msg/string.hpp" @@ -80,7 +80,7 @@ TEST_F(SmootherSelectorTestFixture, test_custom_topic) // create tree std::string xml_txt = R"( - + @@ -128,7 +128,7 @@ TEST_F(SmootherSelectorTestFixture, test_default_topic) // create tree std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp b/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp index e1285d28a90..e3d2e80c85d 100644 --- a/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp @@ -18,7 +18,7 @@ #include #include -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/spin_action.hpp" @@ -122,7 +122,7 @@ TEST_F(SpinActionTestFixture, test_ports) { std::string xml_txt = R"( - + @@ -133,7 +133,7 @@ TEST_F(SpinActionTestFixture, test_ports) xml_txt = R"( - + @@ -147,7 +147,7 @@ TEST_F(SpinActionTestFixture, test_tick) { std::string xml_txt = R"( - + @@ -169,7 +169,7 @@ TEST_F(SpinActionTestFixture, test_failure) { std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp index 8cc3083eb3e..54a0270e786 100644 --- a/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp @@ -17,7 +17,7 @@ #include #include -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/spin_cancel_node.hpp" @@ -120,7 +120,7 @@ TEST_F(CancelSpinActionTestFixture, test_ports) { std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_truncate_path_action.cpp b/nav2_behavior_tree/test/plugins/action/test_truncate_path_action.cpp index 3610cd41a9e..2e7ac3fccd2 100644 --- a/nav2_behavior_tree/test/plugins/action/test_truncate_path_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_truncate_path_action.cpp @@ -22,7 +22,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_util/geometry_utils.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/truncate_path_action.hpp" @@ -87,7 +87,7 @@ TEST_F(TruncatePathTestFixture, test_tick) // create tree std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_truncate_path_local_action.cpp b/nav2_behavior_tree/test/plugins/action/test_truncate_path_local_action.cpp index 0559d6effe3..c3333605f89 100644 --- a/nav2_behavior_tree/test/plugins/action/test_truncate_path_local_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_truncate_path_local_action.cpp @@ -21,7 +21,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_util/geometry_utils.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_behavior_tree_fixture.hpp" #include "nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp" @@ -107,7 +107,7 @@ TEST_F(TruncatePathLocalTestFixture, test_tick) // create tree std::string xml_txt = R"( - + + + + + #include -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/wait_action.hpp" @@ -113,7 +113,7 @@ TEST_F(WaitActionTestFixture, test_ports) { std::string xml_txt = R"( - + @@ -124,7 +124,7 @@ TEST_F(WaitActionTestFixture, test_ports) xml_txt = R"( - + @@ -138,7 +138,7 @@ TEST_F(WaitActionTestFixture, test_tick) { std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_wait_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_wait_cancel_node.cpp index ebce2f63413..4b55e3d1335 100644 --- a/nav2_behavior_tree/test/plugins/action/test_wait_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_wait_cancel_node.cpp @@ -17,7 +17,7 @@ #include #include -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/wait_cancel_node.hpp" @@ -120,7 +120,7 @@ TEST_F(CancelWaitActionTestFixture, test_ports) { std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/condition/test_are_error_codes_present.cpp b/nav2_behavior_tree/test/plugins/condition/test_are_error_codes_present.cpp index d1716397df0..111cda7d005 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_are_error_codes_present.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_are_error_codes_present.cpp @@ -34,7 +34,7 @@ class AreErrorCodesPresentFixture : public nav2_behavior_tree::BehaviorTreeTestF std::string xml_txt = R"( - + @@ -65,7 +65,7 @@ TEST_F(AreErrorCodesPresentFixture, test_condition) for (const auto & error_to_status : error_to_status_map) { config_->blackboard->set("error_code", error_to_status.first); - EXPECT_EQ(tree_->tickRoot(), error_to_status.second); + EXPECT_EQ(tree_->tickOnce(), error_to_status.second); } } diff --git a/nav2_behavior_tree/test/plugins/condition/test_goal_reached.cpp b/nav2_behavior_tree/test/plugins/condition/test_goal_reached.cpp index 1580a8d1bc0..1034c4f9b6f 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_goal_reached.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_goal_reached.cpp @@ -43,7 +43,7 @@ class GoalReachedConditionTestFixture : public nav2_behavior_tree::BehaviorTreeT std::string xml_txt = R"( - + @@ -66,32 +66,32 @@ std::shared_ptr GoalReachedConditionTestFixture::tree_ = nullptr; TEST_F(GoalReachedConditionTestFixture, test_behavior) { - EXPECT_EQ(tree_->tickRoot(), BT::NodeStatus::FAILURE); + EXPECT_EQ(tree_->tickOnce(), BT::NodeStatus::FAILURE); geometry_msgs::msg::Pose pose; pose.position.x = 0.0; pose.position.y = 0.0; transform_handler_->updateRobotPose(pose); std::this_thread::sleep_for(500ms); - EXPECT_EQ(tree_->tickRoot(), BT::NodeStatus::FAILURE); + EXPECT_EQ(tree_->tickOnce(), BT::NodeStatus::FAILURE); pose.position.x = 0.5; pose.position.y = 0.5; transform_handler_->updateRobotPose(pose); std::this_thread::sleep_for(500ms); - EXPECT_EQ(tree_->tickRoot(), BT::NodeStatus::FAILURE); + EXPECT_EQ(tree_->tickOnce(), BT::NodeStatus::FAILURE); pose.position.x = 0.9; pose.position.y = 0.9; transform_handler_->updateRobotPose(pose); std::this_thread::sleep_for(500ms); - EXPECT_EQ(tree_->tickRoot(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(tree_->tickOnce(), BT::NodeStatus::SUCCESS); pose.position.x = 1.0; pose.position.y = 1.0; transform_handler_->updateRobotPose(pose); std::this_thread::sleep_for(500ms); - EXPECT_EQ(tree_->tickRoot(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(tree_->tickOnce(), BT::NodeStatus::SUCCESS); } int main(int argc, char ** argv) diff --git a/nav2_behavior_tree/test/plugins/condition/test_is_battery_charging.cpp b/nav2_behavior_tree/test/plugins/condition/test_is_battery_charging.cpp index 79fa1e08c20..8cd761aadf1 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_is_battery_charging.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_is_battery_charging.cpp @@ -73,7 +73,7 @@ TEST_F(IsBatteryChargingConditionTestFixture, test_behavior_power_supply_status) { std::string xml_txt = R"( - + @@ -86,32 +86,32 @@ TEST_F(IsBatteryChargingConditionTestFixture, test_behavior_power_supply_status) battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); rclcpp::spin_some(node_); - EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); battery_msg.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_CHARGING; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); rclcpp::spin_some(node_); - EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS); battery_msg.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_DISCHARGING; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); rclcpp::spin_some(node_); - EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); battery_msg.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_NOT_CHARGING; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); rclcpp::spin_some(node_); - EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); battery_msg.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_FULL; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); rclcpp::spin_some(node_); - EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); } int main(int argc, char ** argv) diff --git a/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp b/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp index a39a4140890..5e763f56d42 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp @@ -74,7 +74,7 @@ TEST_F(IsBatteryLowConditionTestFixture, test_behavior_percentage) { std::string xml_txt = R"( - + @@ -87,32 +87,32 @@ TEST_F(IsBatteryLowConditionTestFixture, test_behavior_percentage) battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); rclcpp::spin_some(node_); - EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); battery_msg.percentage = 0.49; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); rclcpp::spin_some(node_); - EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS); battery_msg.percentage = 0.51; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); rclcpp::spin_some(node_); - EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); battery_msg.percentage = 0.0; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); rclcpp::spin_some(node_); - EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS); } TEST_F(IsBatteryLowConditionTestFixture, test_behavior_voltage) { std::string xml_txt = R"( - + @@ -125,25 +125,25 @@ TEST_F(IsBatteryLowConditionTestFixture, test_behavior_voltage) battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); rclcpp::spin_some(node_); - EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); battery_msg.voltage = 4.9; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); rclcpp::spin_some(node_); - EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS); battery_msg.voltage = 5.1; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); rclcpp::spin_some(node_); - EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); battery_msg.voltage = 0.0; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); rclcpp::spin_some(node_); - EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS); } int main(int argc, char ** argv) diff --git a/nav2_behavior_tree/test/plugins/condition/test_is_path_valid.cpp b/nav2_behavior_tree/test/plugins/condition/test_is_path_valid.cpp index 9c022359a11..df973cb280a 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_is_path_valid.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_is_path_valid.cpp @@ -92,7 +92,7 @@ TEST_F(IsPathValidTestFixture, test_behavior) { std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/condition/test_transform_available.cpp b/nav2_behavior_tree/test/plugins/condition/test_transform_available.cpp index 2b21fd416c4..73316dfa63f 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_transform_available.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_transform_available.cpp @@ -65,7 +65,7 @@ class TransformAvailableConditionTestFixture : public ::testing::Test { std::string xml_txt = R"( - + @@ -99,10 +99,10 @@ std::shared_ptr TransformAvailableConditionTestFixture::tree_ = nullpt TEST_F(TransformAvailableConditionTestFixture, test_behavior) { - EXPECT_EQ(tree_->tickRoot(), BT::NodeStatus::FAILURE); + EXPECT_EQ(tree_->tickOnce(), BT::NodeStatus::FAILURE); transform_handler_->activate(); transform_handler_->waitForTransform(); - EXPECT_EQ(tree_->tickRoot(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(tree_->tickOnce(), BT::NodeStatus::SUCCESS); } int main(int argc, char ** argv) diff --git a/nav2_behavior_tree/test/plugins/condition/test_would_a_controller_recovery_help.cpp b/nav2_behavior_tree/test/plugins/condition/test_would_a_controller_recovery_help.cpp index d93249879d5..839a4003fe8 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_would_a_controller_recovery_help.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_would_a_controller_recovery_help.cpp @@ -32,7 +32,7 @@ class WouldAControllerRecoveryHelpFixture : public nav2_behavior_tree::BehaviorT std::string xml_txt = R"( - + @@ -69,7 +69,7 @@ TEST_F(WouldAControllerRecoveryHelpFixture, test_condition) for (const auto & error_to_status : error_to_status_map) { config_->blackboard->set("error_code", error_to_status.first); - EXPECT_EQ(tree_->tickRoot(), error_to_status.second); + EXPECT_EQ(tree_->tickOnce(), error_to_status.second); } } diff --git a/nav2_behavior_tree/test/plugins/condition/test_would_a_planner_recovery_help.cpp b/nav2_behavior_tree/test/plugins/condition/test_would_a_planner_recovery_help.cpp index 4dc0f8bb5cb..89e09265dc4 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_would_a_planner_recovery_help.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_would_a_planner_recovery_help.cpp @@ -32,7 +32,7 @@ class WouldAPlannerRecoveryHelpFixture : public nav2_behavior_tree::BehaviorTree std::string xml_txt = R"( - + @@ -64,7 +64,7 @@ TEST_F(WouldAPlannerRecoveryHelpFixture, test_condition) for (const auto & error_to_status : error_to_status_map) { config_->blackboard->set("error_code", error_to_status.first); - EXPECT_EQ(tree_->tickRoot(), error_to_status.second); + EXPECT_EQ(tree_->tickOnce(), error_to_status.second); } } diff --git a/nav2_behavior_tree/test/plugins/condition/test_would_a_smoother_recovery_help.cpp b/nav2_behavior_tree/test/plugins/condition/test_would_a_smoother_recovery_help.cpp index 337d5133c38..7a6dff9be97 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_would_a_smoother_recovery_help.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_would_a_smoother_recovery_help.cpp @@ -32,7 +32,7 @@ class WouldASmootherRecoveryHelpFixture : public nav2_behavior_tree::BehaviorTre std::string xml_txt = R"( - + @@ -66,7 +66,7 @@ TEST_F(WouldASmootherRecoveryHelpFixture, test_condition) for (const auto & error_to_status : error_to_status_map) { config_->blackboard->set("error_code", error_to_status.first); - EXPECT_EQ(tree_->tickRoot(), error_to_status.second); + EXPECT_EQ(tree_->tickOnce(), error_to_status.second); } } diff --git a/nav2_behavior_tree/test/plugins/control/test_pipeline_sequence.cpp b/nav2_behavior_tree/test/plugins/control/test_pipeline_sequence.cpp index d50a6d4c51c..517ee1f04a8 100644 --- a/nav2_behavior_tree/test/plugins/control/test_pipeline_sequence.cpp +++ b/nav2_behavior_tree/test/plugins/control/test_pipeline_sequence.cpp @@ -125,6 +125,33 @@ TEST_F(PipelineSequenceTestFixture, test_behavior) EXPECT_EQ(third_child_->status(), BT::NodeStatus::IDLE); } +TEST_F(PipelineSequenceTestFixture, test_skipped) +{ + first_child_->changeStatus(BT::NodeStatus::SKIPPED); + second_child_->changeStatus(BT::NodeStatus::SUCCESS); + third_child_->changeStatus(BT::NodeStatus::SUCCESS); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE); + EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE); + EXPECT_EQ(third_child_->status(), BT::NodeStatus::IDLE); + + first_child_->changeStatus(BT::NodeStatus::SKIPPED); + second_child_->changeStatus(BT::NodeStatus::SUCCESS); + third_child_->changeStatus(BT::NodeStatus::FAILURE); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); + EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE); + EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE); + EXPECT_EQ(third_child_->status(), BT::NodeStatus::IDLE); + + first_child_->changeStatus(BT::NodeStatus::SKIPPED); + second_child_->changeStatus(BT::NodeStatus::SKIPPED); + third_child_->changeStatus(BT::NodeStatus::SKIPPED); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SKIPPED); + EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE); + EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE); + EXPECT_EQ(third_child_->status(), BT::NodeStatus::IDLE); +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp b/nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp index cc5e2d29699..1ca41e798e8 100644 --- a/nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp +++ b/nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp @@ -144,6 +144,26 @@ TEST_F(RecoveryNodeTestFixture, test_failure_one_retry) EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE); } +TEST_F(RecoveryNodeTestFixture, test_skipping) +{ + // first child skipped + first_child_->changeStatus(BT::NodeStatus::SKIPPED); + second_child_->changeStatus(BT::NodeStatus::SUCCESS); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SKIPPED); + EXPECT_EQ(bt_node_->status(), BT::NodeStatus::IDLE); + EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE); + EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE); + + // first child fails, second child skipped + first_child_->changeStatus(BT::NodeStatus::FAILURE); + second_child_->changeStatus(BT::NodeStatus::SKIPPED); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); + EXPECT_EQ(bt_node_->status(), BT::NodeStatus::FAILURE); + EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE); + EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE); +} + + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/nav2_behavior_tree/test/plugins/control/test_round_robin_node.cpp b/nav2_behavior_tree/test/plugins/control/test_round_robin_node.cpp index 2996bca25a8..7daa91c0697 100644 --- a/nav2_behavior_tree/test/plugins/control/test_round_robin_node.cpp +++ b/nav2_behavior_tree/test/plugins/control/test_round_robin_node.cpp @@ -114,6 +114,33 @@ TEST_F(RoundRobinNodeTestFixture, test_behavior) EXPECT_EQ(third_child_->status(), BT::NodeStatus::IDLE); } +TEST_F(RoundRobinNodeTestFixture, test_skikpped) +{ + first_child_->changeStatus(BT::NodeStatus::SKIPPED); + second_child_->changeStatus(BT::NodeStatus::SUCCESS); + third_child_->changeStatus(BT::NodeStatus::FAILURE); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE); + EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE); + EXPECT_EQ(third_child_->status(), BT::NodeStatus::IDLE); + + first_child_->changeStatus(BT::NodeStatus::SKIPPED); + second_child_->changeStatus(BT::NodeStatus::SKIPPED); + third_child_->changeStatus(BT::NodeStatus::FAILURE); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); + EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE); + EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE); + EXPECT_EQ(third_child_->status(), BT::NodeStatus::IDLE); + + first_child_->changeStatus(BT::NodeStatus::SKIPPED); + second_child_->changeStatus(BT::NodeStatus::SKIPPED); + third_child_->changeStatus(BT::NodeStatus::SKIPPED); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SKIPPED); + EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE); + EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE); + EXPECT_EQ(third_child_->status(), BT::NodeStatus::IDLE); +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp b/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp index 1adde82318b..fa56a2064ab 100644 --- a/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp +++ b/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp @@ -21,7 +21,7 @@ #include "nav_msgs/msg/path.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp" @@ -86,7 +86,7 @@ TEST_F(GoalUpdaterTestFixture, test_tick) // create tree std::string xml_txt = R"( - + @@ -115,7 +115,7 @@ TEST_F(GoalUpdaterTestFixture, test_older_goal_update) // create tree std::string xml_txt = R"( - + @@ -153,7 +153,7 @@ TEST_F(GoalUpdaterTestFixture, test_get_latest_goal_update) // create tree std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/decorator/test_path_longer_on_approach.cpp b/nav2_behavior_tree/test/plugins/decorator/test_path_longer_on_approach.cpp index 0f08bafe691..5d521e205c1 100644 --- a/nav2_behavior_tree/test/plugins/decorator/test_path_longer_on_approach.cpp +++ b/nav2_behavior_tree/test/plugins/decorator/test_path_longer_on_approach.cpp @@ -89,7 +89,7 @@ TEST_F(PathLongerOnApproachTestFixture, test_tick) // create tree std::string xml_txt = R"( - + @@ -115,7 +115,7 @@ TEST_F(PathLongerOnApproachTestFixture, test_tick) // create tree xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/test_bt_utils.cpp b/nav2_behavior_tree/test/test_bt_utils.cpp index 238dee27dcd..1c008d6478a 100644 --- a/nav2_behavior_tree/test/test_bt_utils.cpp +++ b/nav2_behavior_tree/test/test_bt_utils.cpp @@ -22,7 +22,7 @@ #include "geometry_msgs/msg/quaternion.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "nav2_behavior_tree/bt_utils.hpp" template @@ -51,7 +51,7 @@ TEST(PointPortTest, test_wrong_syntax) { std::string xml_txt = R"( - + @@ -69,7 +69,7 @@ TEST(PointPortTest, test_wrong_syntax) xml_txt = R"( - + @@ -86,7 +86,7 @@ TEST(PointPortTest, test_correct_syntax) { std::string xml_txt = R"( - + @@ -107,7 +107,7 @@ TEST(QuaternionPortTest, test_wrong_syntax) { std::string xml_txt = R"( - + @@ -126,7 +126,7 @@ TEST(QuaternionPortTest, test_wrong_syntax) xml_txt = R"( - + @@ -144,7 +144,7 @@ TEST(QuaternionPortTest, test_correct_syntax) { std::string xml_txt = R"( - + @@ -166,7 +166,7 @@ TEST(PoseStampedPortTest, test_wrong_syntax) { std::string xml_txt = R"( - + @@ -190,7 +190,7 @@ TEST(PoseStampedPortTest, test_wrong_syntax) xml_txt = R"( - + @@ -213,7 +213,7 @@ TEST(PoseStampedPortTest, test_correct_syntax) { std::string xml_txt = R"( - + @@ -241,7 +241,7 @@ TEST(MillisecondsPortTest, test_correct_syntax) { std::string xml_txt = R"( - + @@ -257,7 +257,7 @@ TEST(MillisecondsPortTest, test_correct_syntax) xml_txt = R"( - + @@ -272,7 +272,7 @@ TEST(ErrorCodePortTest, test_correct_syntax) { std::string xml_txt = R"( - + @@ -295,7 +295,7 @@ TEST(deconflictPortAndParamFrameTest, test_correct_syntax) { std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/utils/test_behavior_tree_fixture.hpp b/nav2_behavior_tree/test/utils/test_behavior_tree_fixture.hpp index 6215909ae9c..7bf08ddd440 100644 --- a/nav2_behavior_tree/test/utils/test_behavior_tree_fixture.hpp +++ b/nav2_behavior_tree/test/utils/test_behavior_tree_fixture.hpp @@ -20,7 +20,7 @@ #include #include -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "rclcpp/rclcpp.hpp" #include "test_transform_handler.hpp" diff --git a/nav2_behavior_tree/test/utils/test_dummy_tree_node.hpp b/nav2_behavior_tree/test/utils/test_dummy_tree_node.hpp index d277e7f4d25..dcfa52927ac 100644 --- a/nav2_behavior_tree/test/utils/test_dummy_tree_node.hpp +++ b/nav2_behavior_tree/test/utils/test_dummy_tree_node.hpp @@ -16,8 +16,8 @@ #ifndef UTILS__TEST_DUMMY_TREE_NODE_HPP_ #define UTILS__TEST_DUMMY_TREE_NODE_HPP_ -#include -#include +#include +#include namespace nav2_behavior_tree { @@ -36,7 +36,11 @@ class DummyNode : public BT::ActionNodeBase void changeStatus(BT::NodeStatus status) { - setStatus(status); + if (status == BT::NodeStatus::IDLE) { + resetStatus(); + } else { + setStatus(status); + } } BT::NodeStatus executeTick() override diff --git a/nav2_bt_navigator/CMakeLists.txt b/nav2_bt_navigator/CMakeLists.txt index cfe6a0d4869..7de1592106a 100644 --- a/nav2_bt_navigator/CMakeLists.txt +++ b/nav2_bt_navigator/CMakeLists.txt @@ -12,7 +12,7 @@ find_package(geometry_msgs REQUIRED) find_package(nav2_behavior_tree REQUIRED) find_package(nav_msgs REQUIRED) find_package(nav2_msgs REQUIRED) -find_package(behaviortree_cpp_v3 REQUIRED) +find_package(behaviortree_cpp REQUIRED) find_package(std_srvs REQUIRED) find_package(nav2_util REQUIRED) find_package(nav2_core REQUIRED) @@ -43,7 +43,7 @@ set(dependencies nav2_behavior_tree nav_msgs nav2_msgs - behaviortree_cpp_v3 + behaviortree_cpp std_srvs nav2_util nav2_core diff --git a/nav2_bt_navigator/behavior_trees/follow_point.xml b/nav2_bt_navigator/behavior_trees/follow_point.xml index 6941a8c7e60..9f55ecd66ec 100644 --- a/nav2_bt_navigator/behavior_trees/follow_point.xml +++ b/nav2_bt_navigator/behavior_trees/follow_point.xml @@ -2,7 +2,7 @@ This Behavior Tree follows a dynamic pose to a certain distance --> - + diff --git a/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml b/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml index 8fa2eca237c..6973057e93a 100644 --- a/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml +++ b/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml @@ -3,7 +3,7 @@ recovery actions specific to planning / control as well as general system issues. This will be continuous if a kinematically valid planner is selected. --> - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml index 7a235ff0407..28eda8ed893 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml @@ -3,7 +3,7 @@ This Behavior Tree replans the global path periodically at 1 Hz through an array of poses continuously and it also has recovery actions specific to planning / control as well as general system issues. --> - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml index 8cb0eb749ca..772d36fded8 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml @@ -4,7 +4,7 @@ recovery actions specific to planning / control as well as general system issues. This will be continuous if a kinematically valid planner is selected. --> - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml index ea2a7315909..a705aa6b3a2 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml @@ -5,7 +5,7 @@ make the robot wait for a specific time, to see if the obstacle clears out before navigating along a significantly longer path to reach the goal location. --> - + @@ -20,10 +20,10 @@ - + - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml b/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml index 444c9458ff1..872dbf80187 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml @@ -4,7 +4,7 @@ recovery actions specific to planning / control as well as general system issues. This will be continuous if a kinematically valid planner is selected. --> - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml index 61132d5f95d..16f7ede4b3e 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml @@ -2,7 +2,7 @@ This Behavior Tree replans the global path after every 1m. --> - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml index 56e360933c8..a214c9b64fd 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml @@ -2,7 +2,7 @@ This Behavior Tree replans the global path only when the goal is updated. --> - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml index b9f903fd334..effe9a2bf82 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml @@ -1,7 +1,7 @@ - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml index a48ae89a76a..e0d5d6a5a41 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml @@ -2,7 +2,7 @@ This Behavior Tree replans the global path periodically proprortional to speed. --> - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml index f18111f5bd1..11332fb24e9 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml @@ -2,7 +2,7 @@ This Behavior Tree replans the global path periodically at 1 Hz. --> - + diff --git a/nav2_bt_navigator/behavior_trees/odometry_calibration.xml b/nav2_bt_navigator/behavior_trees/odometry_calibration.xml index 49000e14fa6..fdc459f40ff 100644 --- a/nav2_bt_navigator/behavior_trees/odometry_calibration.xml +++ b/nav2_bt_navigator/behavior_trees/odometry_calibration.xml @@ -2,7 +2,7 @@ his Behavior Tree drives in a square for odometry calibration experiments --> - + diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml index 289f08d7ea6..d4eaa63c3c5 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -17,7 +17,7 @@ nav2_behavior_tree nav_msgs nav2_msgs - behaviortree_cpp_v3 + behaviortree_cpp std_msgs geometry_msgs std_srvs @@ -25,7 +25,7 @@ pluginlib nav2_core - behaviortree_cpp_v3 + behaviortree_cpp rclcpp rclcpp_action rclcpp_lifecycle diff --git a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp index 53769abfcd0..a4b4df52d1b 100644 --- a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp @@ -103,7 +103,7 @@ NavigateThroughPosesNavigator::onLoop() auto blackboard = bt_action_server_->getBlackboard(); Goals goal_poses; - blackboard->get(goals_blackboard_id_, goal_poses); + [[maybe_unused]] auto res = blackboard->get(goals_blackboard_id_, goal_poses); if (goal_poses.size() == 0) { bt_action_server_->publishFeedback(feedback_msg); @@ -123,7 +123,7 @@ NavigateThroughPosesNavigator::onLoop() try { // Get current path points nav_msgs::msg::Path current_path; - blackboard->get(path_blackboard_id_, current_path); + res = blackboard->get(path_blackboard_id_, current_path); // Find the closest pose to current pose on global path auto find_closest_pose_idx = @@ -166,7 +166,7 @@ NavigateThroughPosesNavigator::onLoop() } int recovery_count = 0; - blackboard->get("number_recoveries", recovery_count); + res = blackboard->get("number_recoveries", recovery_count); feedback_msg->number_of_recoveries = recovery_count; feedback_msg->current_pose = current_pose; feedback_msg->navigation_time = clock_->now() - start_time_; diff --git a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp index 326a0bd9fb6..2270b6add56 100644 --- a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp @@ -126,7 +126,7 @@ NavigateToPoseNavigator::onLoop() try { // Get current path points nav_msgs::msg::Path current_path; - blackboard->get(path_blackboard_id_, current_path); + [[maybe_unused]] auto res = blackboard->get(path_blackboard_id_, current_path); // Find the closest pose to current pose on global path auto find_closest_pose_idx = @@ -169,7 +169,7 @@ NavigateToPoseNavigator::onLoop() } int recovery_count = 0; - blackboard->get("number_recoveries", recovery_count); + [[maybe_unused]] auto res = blackboard->get("number_recoveries", recovery_count); feedback_msg->number_of_recoveries = recovery_count; feedback_msg->current_pose = current_pose; feedback_msg->navigation_time = clock_->now() - start_time_; diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 2674b65e742..bfab0a79fed 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -22,7 +22,7 @@ find_package(nav2_navfn_planner REQUIRED) find_package(nav2_planner REQUIRED) find_package(navigation2) find_package(angles REQUIRED) -find_package(behaviortree_cpp_v3 REQUIRED) +find_package(behaviortree_cpp REQUIRED) find_package(pluginlib REQUIRED) nav2_package() @@ -45,7 +45,7 @@ set(dependencies nav2_planner nav2_navfn_planner angles - behaviortree_cpp_v3 + behaviortree_cpp pluginlib ) diff --git a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp index b3ac057172b..0a4b320d8f4 100644 --- a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp +++ b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp @@ -22,9 +22,9 @@ #include "gtest/gtest.h" -#include "behaviortree_cpp_v3/behavior_tree.h" -#include "behaviortree_cpp_v3/bt_factory.h" -#include "behaviortree_cpp_v3/utils/shared_library.h" +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/bt_factory.h" +#include "behaviortree_cpp/utils/shared_library.h" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" @@ -224,7 +224,7 @@ TEST_F(BehaviorTreeTestFixture, TestAllSuccess) BT::NodeStatus result = BT::NodeStatus::RUNNING; while (result == BT::NodeStatus::RUNNING) { - result = bt_handler->tree.tickRoot(); + result = bt_handler->tree.tickOnce(); std::this_thread::sleep_for(10ms); } @@ -275,7 +275,7 @@ TEST_F(BehaviorTreeTestFixture, TestAllFailure) BT::NodeStatus result = BT::NodeStatus::RUNNING; while (result == BT::NodeStatus::RUNNING) { - result = bt_handler->tree.tickRoot(); + result = bt_handler->tree.tickOnce(); std::this_thread::sleep_for(10ms); } @@ -324,7 +324,7 @@ TEST_F(BehaviorTreeTestFixture, TestNavigateSubtreeRecoveries) BT::NodeStatus result = BT::NodeStatus::RUNNING; while (result == BT::NodeStatus::RUNNING) { - result = bt_handler->tree.tickRoot(); + result = bt_handler->tree.tickOnce(); std::this_thread::sleep_for(10ms); } @@ -382,7 +382,7 @@ TEST_F(BehaviorTreeTestFixture, TestNavigateRecoverySimple) BT::NodeStatus result = BT::NodeStatus::RUNNING; while (result == BT::NodeStatus::RUNNING) { - result = bt_handler->tree.tickRoot(); + result = bt_handler->tree.tickOnce(); std::this_thread::sleep_for(10ms); } @@ -476,7 +476,7 @@ TEST_F(BehaviorTreeTestFixture, TestNavigateRecoveryComplex) BT::NodeStatus result = BT::NodeStatus::RUNNING; while (result == BT::NodeStatus::RUNNING) { - result = bt_handler->tree.tickRoot(); + result = bt_handler->tree.tickOnce(); std::this_thread::sleep_for(10ms); } @@ -540,7 +540,7 @@ TEST_F(BehaviorTreeTestFixture, TestRecoverySubtreeGoalUpdated) BT::NodeStatus result = BT::NodeStatus::RUNNING; while (result == BT::NodeStatus::RUNNING) { - result = bt_handler->tree.tickRoot(); + result = bt_handler->tree.tickOnce(); // Update goal on blackboard after Spin has been triggered once // to simulate a goal update during a recovery action diff --git a/tools/bt2img.py b/tools/bt2img.py index b3c2b34eaf5..2b12d777d82 100755 --- a/tools/bt2img.py +++ b/tools/bt2img.py @@ -27,7 +27,7 @@ 'ReactiveFallback', 'ReactiveSequence', 'Sequence', - 'SequenceStar', + 'SequenceWithMemory', 'BlackboardCheckInt', 'BlackboardCheckDouble', 'BlackboardCheckString', From 623434b09f25afac3872411b8fda786a6f6dce95 Mon Sep 17 00:00:00 2001 From: Chortine <107395103+Chortine@users.noreply.github.com> Date: Sat, 2 Mar 2024 01:24:30 +0800 Subject: [PATCH 18/36] bug fix (#4160) Signed-off-by: Tianchu --- .../include/nav2_mppi_controller/tools/utils.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp index fb22b38b248..49ff2cb010a 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -314,6 +314,7 @@ inline size_t findPathFurthestReachedPoint(const CriticData & data) for (size_t i = 0; i < dists.shape(0); i++) { min_id_by_path = 0; + min_distance_by_path = std::numeric_limits::max(); for (size_t j = 0; j < dists.shape(1); j++) { cur_dist = dists(i, j); if (cur_dist < min_distance_by_path) { From 211273f831108a767a42ccc8bfc5d5de50761cc1 Mon Sep 17 00:00:00 2001 From: Kemal Bektas <34746077+bektaskemal@users.noreply.github.com> Date: Fri, 1 Mar 2024 18:46:59 +0100 Subject: [PATCH 19/36] Stop planner if the goal is cancelled (#4148) * Add support for stopping planners when the action is cancelled Signed-off-by: Kemal Bektas * Support cancel in Smac planner Signed-off-by: Kemal Bektas * Support cancel in Theta* planner Signed-off-by: Kemal Bektas * Support cancel in Navfn planner Signed-off-by: Kemal Bektas * Update nav2_system_tests to use cancel checker Signed-off-by: Kemal Bektas * Add terminal_checking_interval parameter Signed-off-by: Kemal Bektas * Handle timeout and cancel check on the same branch and default to 5000 iterations Signed-off-by: Kemal Bektas * Add system tests for cancel Signed-off-by: Kemal Bektas --------- Signed-off-by: Kemal Bektas Co-authored-by: Kemal Bektas --- .../include/nav2_core/global_planner.hpp | 4 +- .../include/nav2_core/planner_exceptions.hpp | 7 ++ .../include/nav2_navfn_planner/navfn.hpp | 18 +++-- .../nav2_navfn_planner/navfn_planner.hpp | 6 +- nav2_navfn_planner/src/navfn.cpp | 21 ++++-- nav2_navfn_planner/src/navfn_planner.cpp | 10 +-- .../include/nav2_planner/planner_server.hpp | 5 +- nav2_planner/src/planner_server.cpp | 27 +++++-- nav2_smac_planner/README.md | 1 + .../include/nav2_smac_planner/a_star.hpp | 7 +- .../nav2_smac_planner/smac_planner_2d.hpp | 5 +- .../nav2_smac_planner/smac_planner_hybrid.hpp | 5 +- .../smac_planner_lattice.hpp | 5 +- nav2_smac_planner/src/a_star.cpp | 13 +++- nav2_smac_planner/src/smac_planner_2d.cpp | 13 +++- nav2_smac_planner/src/smac_planner_hybrid.cpp | 13 +++- .../src/smac_planner_lattice.cpp | 31 +++++--- nav2_smac_planner/test/test_a_star.cpp | 64 +++++++++++++---- nav2_smac_planner/test/test_smac_2d.cpp | 10 ++- nav2_smac_planner/test/test_smac_hybrid.cpp | 8 ++- nav2_smac_planner/test/test_smac_lattice.cpp | 7 +- nav2_smac_planner/test/test_smoother.cpp | 11 ++- .../src/error_codes/error_code_param.yaml | 4 +- .../planner/planner_error_plugin.cpp | 1 + .../planner/planner_error_plugin.hpp | 47 +++++++++--- .../src/error_codes/planner_plugins.xml | 4 ++ .../src/error_codes/test_error_codes.py | 20 +++++- .../src/planning/planner_tester.hpp | 3 +- .../src/planning/test_planner_plugins.cpp | 72 +++++++++++++++++-- .../nav2_theta_star_planner/theta_star.hpp | 4 +- .../theta_star_planner.hpp | 13 +++- nav2_theta_star_planner/src/theta_star.cpp | 9 ++- .../src/theta_star_planner.cpp | 18 +++-- .../test/test_theta_star.cpp | 18 +++-- 34 files changed, 413 insertions(+), 91 deletions(-) diff --git a/nav2_core/include/nav2_core/global_planner.hpp b/nav2_core/include/nav2_core/global_planner.hpp index dff1207b88f..0eff064a5d2 100644 --- a/nav2_core/include/nav2_core/global_planner.hpp +++ b/nav2_core/include/nav2_core/global_planner.hpp @@ -71,11 +71,13 @@ class GlobalPlanner * @brief Method create the plan from a starting and ending goal. * @param start The starting pose of the robot * @param goal The goal pose of the robot + * @param cancel_checker Function to check if the action has been canceled * @return The sequence of poses to get from start to goal, if any */ virtual nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal) = 0; + const geometry_msgs::msg::PoseStamped & goal, + std::function cancel_checker) = 0; }; } // namespace nav2_core diff --git a/nav2_core/include/nav2_core/planner_exceptions.hpp b/nav2_core/include/nav2_core/planner_exceptions.hpp index 2680b3c6695..c16c1c10967 100644 --- a/nav2_core/include/nav2_core/planner_exceptions.hpp +++ b/nav2_core/include/nav2_core/planner_exceptions.hpp @@ -113,6 +113,13 @@ class NoViapointsGiven : public PlannerException : PlannerException(description) {} }; +class PlannerCancelled : public PlannerException +{ +public: + explicit PlannerCancelled(const std::string & description) + : PlannerException(description) {} +}; + } // namespace nav2_core #endif // NAV2_CORE__PLANNER_EXCEPTIONS_HPP_ diff --git a/nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp b/nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp index a4ce84404a8..966a8458521 100644 --- a/nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp +++ b/nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp @@ -43,6 +43,7 @@ #include #include #include +#include namespace nav2_navfn_planner { @@ -131,14 +132,16 @@ class NavFn /** * @brief Calculates a plan using the A* heuristic, returns true if one is found + * @param cancelChecker Function to check if the task has been canceled * @return True if a plan is found, false otherwise */ - bool calcNavFnAstar(); + bool calcNavFnAstar(std::function cancelChecker); /** - * @brief Caclulates the full navigation function using Dijkstra + * @brief Calculates the full navigation function using Dijkstra + * @param cancelChecker Function to check if the task has been canceled */ - bool calcNavFnDijkstra(bool atStart = false); + bool calcNavFnDijkstra(std::function cancelChecker, bool atStart = false); /** * @brief Accessor for the x-coordinates of a path @@ -179,6 +182,9 @@ class NavFn float curT; /**< current threshold */ float priInc; /**< priority threshold increment */ + /**< number of cycles between checks for cancellation */ + static constexpr int terminal_checking_interval = 5000; + /** goal and start positions */ /** * @brief Sets the goal position for the planner. @@ -229,18 +235,20 @@ class NavFn * @brief Run propagation for iterations, or until start is reached using * breadth-first Dijkstra method * @param cycles The maximum number of iterations to run for + * @param cancelChecker Function to check if the task has been canceled * @param atStart Whether or not to stop when the start point is reached * @return true if the start point is reached */ - bool propNavFnDijkstra(int cycles, bool atStart = false); + bool propNavFnDijkstra(int cycles, std::function cancelChecker, bool atStart = false); /** * @brief Run propagation for iterations, or until start is reached using * the best-first A* method with Euclidean distance heuristic * @param cycles The maximum number of iterations to run for + * @param cancelChecker Function to check if the task has been canceled * @return true if the start point is reached */ - bool propNavFnAstar(int cycles); /**< returns true if start point found */ + bool propNavFnAstar(int cycles, std::function cancelChecker); /** gradient and paths */ float * gradx, * grady; /**< gradient arrays, size of potential array */ diff --git a/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp b/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp index 2330a5165b9..c1cebb1bef0 100644 --- a/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp +++ b/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp @@ -81,11 +81,13 @@ class NavfnPlanner : public nav2_core::GlobalPlanner * @brief Creating a plan from start and goal poses * @param start Start pose * @param goal Goal pose + * @param cancel_checker Function to check if the task has been canceled * @return nav_msgs::Path of the generated path */ nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal) override; + const geometry_msgs::msg::PoseStamped & goal, + std::function cancel_checker) override; protected: /** @@ -93,12 +95,14 @@ class NavfnPlanner : public nav2_core::GlobalPlanner * @param start Start pose * @param goal Goal pose * @param tolerance Relaxation constraint in x and y + * @param cancel_checker Function to check if the task has been canceled * @param plan Path to be computed * @return true if can find the path */ bool makePlan( const geometry_msgs::msg::Pose & start, const geometry_msgs::msg::Pose & goal, double tolerance, + std::function cancel_checker, nav_msgs::msg::Path & plan); /** diff --git a/nav2_navfn_planner/src/navfn.cpp b/nav2_navfn_planner/src/navfn.cpp index 8259950b038..2cce713b0f2 100644 --- a/nav2_navfn_planner/src/navfn.cpp +++ b/nav2_navfn_planner/src/navfn.cpp @@ -44,6 +44,7 @@ #include "nav2_navfn_planner/navfn.hpp" #include +#include "nav2_core/planner_exceptions.hpp" #include "rclcpp/rclcpp.hpp" namespace nav2_navfn_planner @@ -293,12 +294,12 @@ NavFn::setCostmap(const COSTTYPE * cmap, bool isROS, bool allow_unknown) } bool -NavFn::calcNavFnDijkstra(bool atStart) +NavFn::calcNavFnDijkstra(std::function cancelChecker, bool atStart) { setupNavFn(true); // calculate the nav fn and path - return propNavFnDijkstra(std::max(nx * ny / 20, nx + ny), atStart); + return propNavFnDijkstra(std::max(nx * ny / 20, nx + ny), cancelChecker, atStart); } @@ -307,12 +308,12 @@ NavFn::calcNavFnDijkstra(bool atStart) // bool -NavFn::calcNavFnAstar() +NavFn::calcNavFnAstar(std::function cancelChecker) { setupNavFn(true); // calculate the nav fn and path - return propNavFnAstar(std::max(nx * ny / 20, nx + ny)); + return propNavFnAstar(std::max(nx * ny / 20, nx + ny), cancelChecker); } // @@ -571,7 +572,7 @@ NavFn::updateCellAstar(int n) // bool -NavFn::propNavFnDijkstra(int cycles, bool atStart) +NavFn::propNavFnDijkstra(int cycles, std::function cancelChecker, bool atStart) { int nwv = 0; // max priority block size int nc = 0; // number of cells put into priority blocks @@ -581,6 +582,10 @@ NavFn::propNavFnDijkstra(int cycles, bool atStart) int startCell = start[1] * nx + start[0]; for (; cycle < cycles; cycle++) { // go for this many cycles, unless interrupted + if (cycle % terminal_checking_interval == 0 && cancelChecker()) { + throw nav2_core::PlannerCancelled("Planner was cancelled"); + } + if (curPe == 0 && nextPe == 0) { // priority blocks empty break; } @@ -652,7 +657,7 @@ NavFn::propNavFnDijkstra(int cycles, bool atStart) // bool -NavFn::propNavFnAstar(int cycles) +NavFn::propNavFnAstar(int cycles, std::function cancelChecker) { int nwv = 0; // max priority block size int nc = 0; // number of cells put into priority blocks @@ -667,6 +672,10 @@ NavFn::propNavFnAstar(int cycles) // do main cycle for (; cycle < cycles; cycle++) { // go for this many cycles, unless interrupted + if (cycle % terminal_checking_interval == 0 && cancelChecker()) { + throw nav2_core::PlannerCancelled("Planner was cancelled"); + } + if (curPe == 0 && nextPe == 0) { // priority blocks empty break; } diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index 2ee8d69f53e..9985280159e 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -129,7 +129,8 @@ NavfnPlanner::cleanup() nav_msgs::msg::Path NavfnPlanner::createPlan( const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal) + const geometry_msgs::msg::PoseStamped & goal, + std::function cancel_checker) { #ifdef BENCHMARK_TESTING steady_clock::time_point a = steady_clock::now(); @@ -183,7 +184,7 @@ nav_msgs::msg::Path NavfnPlanner::createPlan( return path; } - if (!makePlan(start.pose, goal.pose, tolerance_, path)) { + if (!makePlan(start.pose, goal.pose, tolerance_, cancel_checker, path)) { throw nav2_core::NoValidPathCouldBeFound( "Failed to create plan with tolerance of: " + std::to_string(tolerance_) ); } @@ -214,6 +215,7 @@ bool NavfnPlanner::makePlan( const geometry_msgs::msg::Pose & start, const geometry_msgs::msg::Pose & goal, double tolerance, + std::function cancel_checker, nav_msgs::msg::Path & plan) { // clear the plan, just in case @@ -261,9 +263,9 @@ NavfnPlanner::makePlan( planner_->setStart(map_goal); planner_->setGoal(map_start); if (use_astar_) { - planner_->calcNavFnAstar(); + planner_->calcNavFnAstar(cancel_checker); } else { - planner_->calcNavFnDijkstra(true); + planner_->calcNavFnDijkstra(cancel_checker, true); } double resolution = costmap_->getResolution(); diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp index 7d2c9d03f00..bdffda281a2 100644 --- a/nav2_planner/include/nav2_planner/planner_server.hpp +++ b/nav2_planner/include/nav2_planner/planner_server.hpp @@ -68,12 +68,15 @@ class PlannerServer : public nav2_util::LifecycleNode * @brief Method to get plan from the desired plugin * @param start starting pose * @param goal goal request + * @param planner_id The planner to plan with + * @param cancel_checker A function to check if the action has been canceled * @return Path */ nav_msgs::msg::Path getPlan( const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal, - const std::string & planner_id); + const std::string & planner_id, + std::function cancel_checker); protected: /** diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index de239b3587e..7f8dc4a72de 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -394,6 +394,10 @@ void PlannerServer::computePlanThroughPoses() throw nav2_core::PlannerTFError("Unable to get start pose"); } + auto cancel_checker = [this]() { + return action_server_poses_->is_cancel_requested(); + }; + // Get consecutive paths through these points for (unsigned int i = 0; i != goal->goals.size(); i++) { // Get starting point @@ -413,7 +417,9 @@ void PlannerServer::computePlanThroughPoses() } // Get plan from start -> goal - nav_msgs::msg::Path curr_path = getPlan(curr_start, curr_goal, goal->planner_id); + nav_msgs::msg::Path curr_path = getPlan( + curr_start, curr_goal, goal->planner_id, + cancel_checker); if (!validatePath(curr_goal, curr_path, goal->planner_id)) { throw nav2_core::NoValidPathCouldBeFound(goal->planner_id + " generated a empty path"); @@ -476,6 +482,9 @@ void PlannerServer::computePlanThroughPoses() exceptionWarning(curr_start, curr_goal, goal->planner_id, ex); result->error_code = ActionThroughPosesResult::NO_VIAPOINTS_GIVEN; action_server_poses_->terminate_current(result); + } catch (nav2_core::PlannerCancelled &) { + RCLCPP_INFO(get_logger(), "Goal was canceled. Canceling planning action."); + action_server_poses_->terminate_all(); } catch (std::exception & ex) { exceptionWarning(curr_start, curr_goal, goal->planner_id, ex); result->error_code = ActionThroughPosesResult::UNKNOWN; @@ -516,7 +525,11 @@ PlannerServer::computePlan() throw nav2_core::PlannerTFError("Unable to transform poses to global frame"); } - result->path = getPlan(start, goal_pose, goal->planner_id); + auto cancel_checker = [this]() { + return action_server_pose_->is_cancel_requested(); + }; + + result->path = getPlan(start, goal_pose, goal->planner_id, cancel_checker); if (!validatePath(goal_pose, result->path, goal->planner_id)) { throw nav2_core::NoValidPathCouldBeFound(goal->planner_id + " generated a empty path"); @@ -567,6 +580,9 @@ PlannerServer::computePlan() exceptionWarning(start, goal->goal, goal->planner_id, ex); result->error_code = ActionToPoseResult::TF_ERROR; action_server_pose_->terminate_current(result); + } catch (nav2_core::PlannerCancelled &) { + RCLCPP_INFO(get_logger(), "Goal was canceled. Canceling planning action."); + action_server_pose_->terminate_all(); } catch (std::exception & ex) { exceptionWarning(start, goal->goal, goal->planner_id, ex); result->error_code = ActionToPoseResult::UNKNOWN; @@ -578,7 +594,8 @@ nav_msgs::msg::Path PlannerServer::getPlan( const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal, - const std::string & planner_id) + const std::string & planner_id, + std::function cancel_checker) { RCLCPP_DEBUG( get_logger(), "Attempting to a find path from (%.2f, %.2f) to " @@ -586,14 +603,14 @@ PlannerServer::getPlan( goal.pose.position.x, goal.pose.position.y); if (planners_.find(planner_id) != planners_.end()) { - return planners_[planner_id]->createPlan(start, goal); + return planners_[planner_id]->createPlan(start, goal, cancel_checker); } else { if (planners_.size() == 1 && planner_id.empty()) { RCLCPP_WARN_ONCE( get_logger(), "No planners specified in action call. " "Server will use only plugin %s in server." " This warning will appear once.", planner_ids_concat_.c_str()); - return planners_[planners_.begin()->first]->createPlan(start, goal); + return planners_[planners_.begin()->first]->createPlan(start, goal, cancel_checker); } else { RCLCPP_ERROR( get_logger(), "planner %s is not a valid planner. " diff --git a/nav2_smac_planner/README.md b/nav2_smac_planner/README.md index 3a992088b69..6e92209068d 100644 --- a/nav2_smac_planner/README.md +++ b/nav2_smac_planner/README.md @@ -109,6 +109,7 @@ planner_server: allow_unknown: false # allow traveling in unknown space max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable max_on_approach_iterations: 1000 # maximum number of iterations to attempt to reach goal once in tolerance + terminal_checking_interval: 5000 # number of iterations between checking if the goal has been cancelled or planner timed out max_planning_time: 3.5 # max time in s for planner to plan, smooth, and upsample. Will scale maximum smoothing and upsampling times based on remaining time after planning. motion_model_for_search: "DUBIN" # For Hybrid Dubin, Redds-Shepp cost_travel_multiplier: 2.0 # For 2D: Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*. diff --git a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp index e4ecf38f035..668aff5e4d8 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -88,6 +88,8 @@ class AStarAlgorithm * @param max_on_approach_iterations Maximum number of iterations before returning a valid * path once within thresholds to refine path * comes at more compute time but smoother paths. + * @param terminal_checking_interval Number of iterations to check if the task has been canceled or + * or planning time exceeded * @param max_planning_time Maximum time (in seconds) to wait for a plan, createPath returns * false after this timeout */ @@ -95,6 +97,7 @@ class AStarAlgorithm const bool & allow_unknown, int & max_iterations, const int & max_on_approach_iterations, + const int & terminal_checking_interval, const double & max_planning_time, const float & lookup_table_size, const unsigned int & dim_3_size); @@ -104,11 +107,13 @@ class AStarAlgorithm * @param path Reference to a vector of indicies of generated path * @param num_iterations Reference to number of iterations to create plan * @param tolerance Reference to tolerance in costmap nodes + * @param cancel_checker Function to check if the task has been canceled * @param expansions_log Optional expansions logged for debug * @return if plan was successful */ bool createPath( CoordinateVector & path, int & num_iterations, const float & tolerance, + std::function cancel_checker, std::vector> * expansions_log = nullptr); /** @@ -250,11 +255,11 @@ class AStarAlgorithm */ void clearStart(); - int _timing_interval = 5000; bool _traverse_unknown; int _max_iterations; int _max_on_approach_iterations; + int _terminal_checking_interval; double _max_planning_time; float _tolerance; unsigned int _x_size; diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp index c499d0796fb..059871d8677 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp @@ -82,11 +82,13 @@ class SmacPlanner2D : public nav2_core::GlobalPlanner * @brief Creating a plan from start and goal poses * @param start Start pose * @param goal Goal pose + * @param cancel_checker Function to check if the action has been canceled * @return nav2_msgs::Path of the generated path */ nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal) override; + const geometry_msgs::msg::PoseStamped & goal, + std::function cancel_checker) override; protected: /** @@ -112,6 +114,7 @@ class SmacPlanner2D : public nav2_core::GlobalPlanner bool _allow_unknown; int _max_iterations; int _max_on_approach_iterations; + int _terminal_checking_interval; bool _use_final_approach_orientation; SearchInfo _search_info; std::string _motion_model_for_search; diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp index fae139aa2f8..157bbcea839 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp @@ -81,11 +81,13 @@ class SmacPlannerHybrid : public nav2_core::GlobalPlanner * @brief Creating a plan from start and goal poses * @param start Start pose * @param goal Goal pose + * @param cancel_checker Function to check if the action has been canceled * @return nav2_msgs::Path of the generated path */ nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal) override; + const geometry_msgs::msg::PoseStamped & goal, + std::function cancel_checker) override; protected: /** @@ -113,6 +115,7 @@ class SmacPlannerHybrid : public nav2_core::GlobalPlanner bool _allow_unknown; int _max_iterations; int _max_on_approach_iterations; + int _terminal_checking_interval; SearchInfo _search_info; double _max_planning_time; double _lookup_table_size; diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp index ce8bafd2fd0..57e225f2876 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp @@ -80,11 +80,13 @@ class SmacPlannerLattice : public nav2_core::GlobalPlanner * @brief Creating a plan from start and goal poses * @param start Start pose * @param goal Goal pose + * @param cancel_checker Function to check if the action has been canceled * @return nav2_msgs::Path of the generated path */ nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal) override; + const geometry_msgs::msg::PoseStamped & goal, + std::function cancel_checker) override; protected: /** @@ -108,6 +110,7 @@ class SmacPlannerLattice : public nav2_core::GlobalPlanner bool _allow_unknown; int _max_iterations; int _max_on_approach_iterations; + int _terminal_checking_interval; float _tolerance; rclcpp_lifecycle::LifecyclePublisher::SharedPtr _raw_plan_publisher; double _max_planning_time; diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index ebade0f4aa0..1f0d7207631 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -37,6 +37,7 @@ AStarAlgorithm::AStarAlgorithm( const SearchInfo & search_info) : _traverse_unknown(true), _max_iterations(0), + _terminal_checking_interval(5000), _max_planning_time(0), _x_size(0), _y_size(0), @@ -59,6 +60,7 @@ void AStarAlgorithm::initialize( const bool & allow_unknown, int & max_iterations, const int & max_on_approach_iterations, + const int & terminal_checking_interval, const double & max_planning_time, const float & lookup_table_size, const unsigned int & dim_3_size) @@ -66,6 +68,7 @@ void AStarAlgorithm::initialize( _traverse_unknown = allow_unknown; _max_iterations = max_iterations; _max_on_approach_iterations = max_on_approach_iterations; + _terminal_checking_interval = terminal_checking_interval; _max_planning_time = max_planning_time; NodeT::precomputeDistanceHeuristic(lookup_table_size, _motion_model, dim_3_size, _search_info); _dim3_size = dim_3_size; @@ -78,6 +81,7 @@ void AStarAlgorithm::initialize( const bool & allow_unknown, int & max_iterations, const int & max_on_approach_iterations, + const int & terminal_checking_interval, const double & max_planning_time, const float & /*lookup_table_size*/, const unsigned int & dim_3_size) @@ -85,6 +89,7 @@ void AStarAlgorithm::initialize( _traverse_unknown = allow_unknown; _max_iterations = max_iterations; _max_on_approach_iterations = max_on_approach_iterations; + _terminal_checking_interval = terminal_checking_interval; _max_planning_time = max_planning_time; if (dim_3_size != 1) { @@ -245,6 +250,7 @@ template bool AStarAlgorithm::createPath( CoordinateVector & path, int & iterations, const float & tolerance, + std::function cancel_checker, std::vector> * expansions_log) { steady_clock::time_point start_time = steady_clock::now(); @@ -285,8 +291,11 @@ bool AStarAlgorithm::createPath( }; while (iterations < getMaxIterations() && !_queue.empty()) { - // Check for planning timeout only on every Nth iteration - if (iterations % _timing_interval == 0) { + // Check for planning timeout and cancel only on every Nth iteration + if (iterations % _terminal_checking_interval == 0) { + if (cancel_checker()) { + throw nav2_core::PlannerCancelled("Planner was cancelled"); + } std::chrono::duration planning_duration = std::chrono::duration_cast>(steady_clock::now() - start_time); if (static_cast(planning_duration.count()) >= _max_planning_time) { diff --git a/nav2_smac_planner/src/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp index 70f8852f28c..38fd12b9e17 100644 --- a/nav2_smac_planner/src/smac_planner_2d.cpp +++ b/nav2_smac_planner/src/smac_planner_2d.cpp @@ -83,6 +83,9 @@ void SmacPlanner2D::configure( nav2_util::declare_parameter_if_not_declared( node, name + ".max_on_approach_iterations", rclcpp::ParameterValue(1000)); node->get_parameter(name + ".max_on_approach_iterations", _max_on_approach_iterations); + nav2_util::declare_parameter_if_not_declared( + node, name + ".terminal_checking_interval", rclcpp::ParameterValue(5000)); + node->get_parameter(name + ".terminal_checking_interval", _terminal_checking_interval); nav2_util::declare_parameter_if_not_declared( node, name + ".use_final_approach_orientation", rclcpp::ParameterValue(false)); node->get_parameter(name + ".use_final_approach_orientation", _use_final_approach_orientation); @@ -120,6 +123,7 @@ void SmacPlanner2D::configure( _allow_unknown, _max_iterations, _max_on_approach_iterations, + _terminal_checking_interval, _max_planning_time, 0.0 /*unused for 2D*/, 1.0 /*unused for 2D*/); @@ -192,7 +196,8 @@ void SmacPlanner2D::cleanup() nav_msgs::msg::Path SmacPlanner2D::createPlan( const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal) + const geometry_msgs::msg::PoseStamped & goal, + std::function cancel_checker) { std::lock_guard lock_reinit(_mutex); steady_clock::time_point a = steady_clock::now(); @@ -257,7 +262,7 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( // Note: All exceptions thrown are handled by the planner server and returned to the action if (!_a_star->createPath( path, num_iterations, - _tolerance / static_cast(costmap->getResolution()))) + _tolerance / static_cast(costmap->getResolution()), cancel_checker)) { // Note: If the start is blocked only one iteration will occur before failure if (num_iterations == 1) { @@ -377,6 +382,9 @@ SmacPlanner2D::dynamicParametersCallback(std::vector paramete "disabling tolerance and on approach iterations."); _max_on_approach_iterations = std::numeric_limits::max(); } + } else if (name == _name + ".terminal_checking_interval") { + reinit_a_star = true; + _terminal_checking_interval = parameter.as_int(); } } } @@ -390,6 +398,7 @@ SmacPlanner2D::dynamicParametersCallback(std::vector paramete _allow_unknown, _max_iterations, _max_on_approach_iterations, + _terminal_checking_interval, _max_planning_time, 0.0 /*unused for 2D*/, 1.0 /*unused for 2D*/); diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index 727e4c08b29..b0bdf042dc2 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -94,6 +94,9 @@ void SmacPlannerHybrid::configure( nav2_util::declare_parameter_if_not_declared( node, name + ".max_on_approach_iterations", rclcpp::ParameterValue(1000)); node->get_parameter(name + ".max_on_approach_iterations", _max_on_approach_iterations); + nav2_util::declare_parameter_if_not_declared( + node, name + ".terminal_checking_interval", rclcpp::ParameterValue(5000)); + node->get_parameter(name + ".terminal_checking_interval", _terminal_checking_interval); nav2_util::declare_parameter_if_not_declared( node, name + ".smooth_path", rclcpp::ParameterValue(true)); node->get_parameter(name + ".smooth_path", smooth_path); @@ -228,6 +231,7 @@ void SmacPlannerHybrid::configure( _allow_unknown, _max_iterations, _max_on_approach_iterations, + _terminal_checking_interval, _max_planning_time, _lookup_table_dim, _angle_quantizations); @@ -318,7 +322,8 @@ void SmacPlannerHybrid::cleanup() nav_msgs::msg::Path SmacPlannerHybrid::createPlan( const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal) + const geometry_msgs::msg::PoseStamped & goal, + std::function cancel_checker) { std::lock_guard lock_reinit(_mutex); steady_clock::time_point a = steady_clock::now(); @@ -394,7 +399,7 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( // Note: All exceptions thrown are handled by the planner server and returned to the action if (!_a_star->createPath( path, num_iterations, - _tolerance / static_cast(costmap->getResolution()), expansions.get())) + _tolerance / static_cast(costmap->getResolution()), cancel_checker, expansions.get())) { if (_debug_visualizations) { geometry_msgs::msg::PoseArray msg; @@ -598,6 +603,9 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para "disabling tolerance and on approach iterations."); _max_on_approach_iterations = std::numeric_limits::max(); } + } else if (name == _name + ".terminal_checking_interval") { + reinit_a_star = true; + _terminal_checking_interval = parameter.as_int(); } else if (name == _name + ".angle_quantization_bins") { reinit_collision_checker = true; reinit_a_star = true; @@ -653,6 +661,7 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para _allow_unknown, _max_iterations, _max_on_approach_iterations, + _terminal_checking_interval, _max_planning_time, _lookup_table_dim, _angle_quantizations); diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index 1666c7a9ce0..cd98cf64408 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -77,6 +77,9 @@ void SmacPlannerLattice::configure( nav2_util::declare_parameter_if_not_declared( node, name + ".max_on_approach_iterations", rclcpp::ParameterValue(1000)); node->get_parameter(name + ".max_on_approach_iterations", _max_on_approach_iterations); + nav2_util::declare_parameter_if_not_declared( + node, name + ".terminal_checking_interval", rclcpp::ParameterValue(5000)); + node->get_parameter(name + ".terminal_checking_interval", _terminal_checking_interval); nav2_util::declare_parameter_if_not_declared( node, name + ".smooth_path", rclcpp::ParameterValue(true)); node->get_parameter(name + ".smooth_path", smooth_path); @@ -193,6 +196,7 @@ void SmacPlannerLattice::configure( _allow_unknown, _max_iterations, _max_on_approach_iterations, + _terminal_checking_interval, _max_planning_time, lookup_table_dim, _metadata.number_of_headings); @@ -261,7 +265,8 @@ void SmacPlannerLattice::cleanup() nav_msgs::msg::Path SmacPlannerLattice::createPlan( const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal) + const geometry_msgs::msg::PoseStamped & goal, + std::function cancel_checker) { std::lock_guard lock_reinit(_mutex); steady_clock::time_point a = steady_clock::now(); @@ -316,7 +321,7 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( // Note: All exceptions thrown are handled by the planner server and returned to the action if (!_a_star->createPath( path, num_iterations, - _tolerance / static_cast(_costmap->getResolution()), expansions.get())) + _tolerance / static_cast(_costmap->getResolution()), cancel_checker, expansions.get())) { if (_debug_visualizations) { geometry_msgs::msg::PoseArray msg; @@ -504,15 +509,18 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector par "disabling maximum iterations."); _max_iterations = std::numeric_limits::max(); } - } - } else if (name == _name + ".max_on_approach_iterations") { - reinit_a_star = true; - _max_on_approach_iterations = parameter.as_int(); - if (_max_on_approach_iterations <= 0) { - RCLCPP_INFO( - _logger, "On approach iteration selected as <= 0, " - "disabling tolerance and on approach iterations."); - _max_on_approach_iterations = std::numeric_limits::max(); + } else if (name == _name + ".max_on_approach_iterations") { + reinit_a_star = true; + _max_on_approach_iterations = parameter.as_int(); + if (_max_on_approach_iterations <= 0) { + RCLCPP_INFO( + _logger, "On approach iteration selected as <= 0, " + "disabling tolerance and on approach iterations."); + _max_on_approach_iterations = std::numeric_limits::max(); + } + } else if (name == _name + ".terminal_checking_interval") { + reinit_a_star = true; + _terminal_checking_interval = parameter.as_int(); } } else if (type == ParameterType::PARAMETER_STRING) { if (name == _name + ".lattice_filepath") { @@ -565,6 +573,7 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector par _allow_unknown, _max_iterations, _max_on_approach_iterations, + _terminal_checking_interval, _max_planning_time, lookup_table_dim, _metadata.number_of_headings); diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp index 13c2c802982..bdcf37c7215 100644 --- a/nav2_smac_planner/test/test_a_star.cpp +++ b/nav2_smac_planner/test/test_a_star.cpp @@ -47,10 +47,13 @@ TEST(AStarTest, test_a_star_2d) float tolerance = 0.0; float some_tolerance = 20.0; int it_on_approach = 10; + int terminal_checking_interval = 5000; double max_planning_time = 120.0; int num_it = 0; - a_star.initialize(false, max_iterations, it_on_approach, max_planning_time, 0.0, 1); + a_star.initialize( + false, max_iterations, it_on_approach, terminal_checking_interval, + max_planning_time, 0.0, 1); nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0); @@ -67,6 +70,10 @@ TEST(AStarTest, test_a_star_2d) auto costmap = costmap_ros->getCostmap(); *costmap = *costmapA; + auto dummy_cancel_checker = []() { + return false; + }; + // functional case testing std::unique_ptr checker = std::make_unique(costmap_ros, 1, lnode); @@ -75,7 +82,7 @@ TEST(AStarTest, test_a_star_2d) a_star.setStart(20u, 20u, 0); a_star.setGoal(80u, 80u, 0); nav2_smac_planner::Node2D::CoordinateVector path; - EXPECT_TRUE(a_star.createPath(path, num_it, tolerance)); + EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, dummy_cancel_checker)); EXPECT_EQ(num_it, 2414); // check path is the right size and collision free @@ -92,21 +99,32 @@ TEST(AStarTest, test_a_star_2d) // failure cases with invalid inputs nav2_smac_planner::AStarAlgorithm a_star_2( nav2_smac_planner::MotionModel::TWOD, info); - a_star_2.initialize(false, max_iterations, it_on_approach, max_planning_time, 0, 1); + a_star_2.initialize( + false, max_iterations, it_on_approach, terminal_checking_interval, + max_planning_time, 0, 1); num_it = 0; - EXPECT_THROW(a_star_2.createPath(path, num_it, tolerance), std::runtime_error); + EXPECT_THROW( + a_star_2.createPath( + path, num_it, tolerance, + dummy_cancel_checker), std::runtime_error); a_star_2.setCollisionChecker(checker.get()); num_it = 0; - EXPECT_THROW(a_star_2.createPath(path, num_it, tolerance), std::runtime_error); + EXPECT_THROW( + a_star_2.createPath( + path, num_it, tolerance, + dummy_cancel_checker), std::runtime_error); a_star_2.setStart(0, 0, 0); // valid a_star_2.setGoal(50, 50, 0); // invalid num_it = 0; - EXPECT_THROW(a_star_2.createPath(path, num_it, tolerance), std::runtime_error); + EXPECT_THROW( + a_star_2.createPath( + path, num_it, tolerance, + dummy_cancel_checker), std::runtime_error); num_it = 0; // invalid goal but liberal tolerance a_star_2.setStart(20, 20, 0); // valid a_star_2.setGoal(50, 50, 0); // invalid - EXPECT_TRUE(a_star_2.createPath(path, num_it, some_tolerance)); + EXPECT_TRUE(a_star_2.createPath(path, num_it, some_tolerance, dummy_cancel_checker)); EXPECT_EQ(path.size(), 21u); for (unsigned int i = 0; i != path.size(); i++) { EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0); @@ -141,10 +159,13 @@ TEST(AStarTest, test_a_star_se2) int max_iterations = 10000; float tolerance = 10.0; int it_on_approach = 10; + int terminal_checking_interval = 5000; double max_planning_time = 120.0; int num_it = 0; - a_star.initialize(false, max_iterations, it_on_approach, max_planning_time, 401, size_theta); + a_star.initialize( + false, max_iterations, it_on_approach, terminal_checking_interval, + max_planning_time, 401, size_theta); nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0); @@ -173,7 +194,11 @@ TEST(AStarTest, test_a_star_se2) std::unique_ptr>> expansions = nullptr; expansions = std::make_unique>>(); - EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, expansions.get())); + auto dummy_cancel_checker = []() { + return false; + }; + + EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, dummy_cancel_checker, expansions.get())); // check path is the right size and collision free EXPECT_EQ(num_it, 3146); @@ -214,11 +239,14 @@ TEST(AStarTest, test_a_star_lattice) int max_iterations = 10000; float tolerance = 10.0; int it_on_approach = 10; + int terminal_checking_interval = 5000; double max_planning_time = 120.0; int num_it = 0; a_star.initialize( - false, max_iterations, std::numeric_limits::max(), max_planning_time, 401, size_theta); + false, max_iterations, + std::numeric_limits::max(), terminal_checking_interval, max_planning_time, 401, + size_theta); nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D(100, 100, 0.05, 0.0, 0.0, 0); @@ -239,12 +267,16 @@ TEST(AStarTest, test_a_star_lattice) std::make_unique(costmap_ros, size_theta, lnode); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); + auto dummy_cancel_checker = []() { + return false; + }; + // functional case testing a_star.setCollisionChecker(checker.get()); a_star.setStart(5u, 5u, 0u); a_star.setGoal(40u, 40u, 1u); nav2_smac_planner::NodeLattice::CoordinateVector path; - EXPECT_TRUE(a_star.createPath(path, num_it, tolerance)); + EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, dummy_cancel_checker)); // check path is the right size and collision free EXPECT_EQ(num_it, 22); @@ -278,10 +310,13 @@ TEST(AStarTest, test_se2_single_pose_path) int max_iterations = 100; float tolerance = 10.0; int it_on_approach = 10; + int terminal_checking_interval = 5000; double max_planning_time = 120.0; int num_it = 0; - a_star.initialize(false, max_iterations, it_on_approach, max_planning_time, 401, size_theta); + a_star.initialize( + false, max_iterations, it_on_approach, terminal_checking_interval, + max_planning_time, 401, size_theta); nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0); @@ -296,13 +331,16 @@ TEST(AStarTest, test_se2_single_pose_path) std::make_unique(costmap_ros, size_theta, lnode); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); + auto dummy_cancel_checker = []() { + return false; + }; // functional case testing a_star.setCollisionChecker(checker.get()); a_star.setStart(10u, 10u, 0u); // Goal is one costmap cell away a_star.setGoal(12u, 10u, 0u); nav2_smac_planner::NodeHybrid::CoordinateVector path; - EXPECT_TRUE(a_star.createPath(path, num_it, tolerance)); + EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, dummy_cancel_checker)); // Check that the path is length one // With the current implementation, this produces a longer path diff --git a/nav2_smac_planner/test/test_smac_2d.cpp b/nav2_smac_planner/test/test_smac_2d.cpp index 1ebe215b30f..e2dff5a56f4 100644 --- a/nav2_smac_planner/test/test_smac_2d.cpp +++ b/nav2_smac_planner/test/test_smac_2d.cpp @@ -57,6 +57,10 @@ TEST(SmacTest, test_smac_2d) { node2D->declare_parameter("test.downsampling_factor", 2); node2D->set_parameter(rclcpp::Parameter("test.downsampling_factor", 2)); + auto dummy_cancel_checker = []() { + return false; + }; + geometry_msgs::msg::PoseStamped start, goal; start.pose.position.x = 0.0; start.pose.position.y = 0.0; @@ -69,7 +73,7 @@ TEST(SmacTest, test_smac_2d) { planner_2d->configure(node2D, "test", nullptr, costmap_ros); planner_2d->activate(); try { - planner_2d->createPlan(start, goal); + planner_2d->createPlan(start, goal, dummy_cancel_checker); } catch (...) { } @@ -108,6 +112,7 @@ TEST(SmacTest, test_smac_2d_reconfigure) { rclcpp::Parameter("test.downsampling_factor", 2), rclcpp::Parameter("test.max_iterations", -1), rclcpp::Parameter("test.max_on_approach_iterations", -1), + rclcpp::Parameter("test.terminal_checking_interval", 100), rclcpp::Parameter("test.use_final_approach_orientation", false)}); rclcpp::spin_until_future_complete( @@ -127,6 +132,9 @@ TEST(SmacTest, test_smac_2d_reconfigure) { EXPECT_EQ( node2D->get_parameter("test.max_on_approach_iterations").as_int(), -1); + EXPECT_EQ( + node2D->get_parameter("test.terminal_checking_interval").as_int(), + 100); results = rec_param->set_parameters_atomically( {rclcpp::Parameter("test.downsample_costmap", true)}); diff --git a/nav2_smac_planner/test/test_smac_hybrid.cpp b/nav2_smac_planner/test/test_smac_hybrid.cpp index aae4666edc5..a2af405d51f 100644 --- a/nav2_smac_planner/test/test_smac_hybrid.cpp +++ b/nav2_smac_planner/test/test_smac_hybrid.cpp @@ -56,6 +56,10 @@ TEST(SmacTest, test_smac_se2) nodeSE2->declare_parameter("test.downsampling_factor", 2); nodeSE2->set_parameter(rclcpp::Parameter("test.downsampling_factor", 2)); + auto dummy_cancel_checker = []() { + return false; + }; + geometry_msgs::msg::PoseStamped start, goal; start.pose.position.x = 0.0; start.pose.position.y = 0.0; @@ -68,7 +72,7 @@ TEST(SmacTest, test_smac_se2) planner->activate(); try { - planner->createPlan(start, goal); + planner->createPlan(start, goal, dummy_cancel_checker); } catch (...) { } @@ -119,6 +123,7 @@ TEST(SmacTest, test_smac_se2_reconfigure) rclcpp::Parameter("test.smooth_path", false), rclcpp::Parameter("test.analytic_expansion_max_length", 42.0), rclcpp::Parameter("test.max_on_approach_iterations", 42), + rclcpp::Parameter("test.terminal_checking_interval", 42), rclcpp::Parameter("test.motion_model_for_search", std::string("REEDS_SHEPP"))}); rclcpp::spin_until_future_complete( @@ -144,6 +149,7 @@ TEST(SmacTest, test_smac_se2_reconfigure) EXPECT_EQ(nodeSE2->get_parameter("test.lookup_table_size").as_double(), 30.0); EXPECT_EQ(nodeSE2->get_parameter("test.analytic_expansion_max_length").as_double(), 42.0); EXPECT_EQ(nodeSE2->get_parameter("test.max_on_approach_iterations").as_int(), 42); + EXPECT_EQ(nodeSE2->get_parameter("test.terminal_checking_interval").as_int(), 42); EXPECT_EQ( nodeSE2->get_parameter("test.motion_model_for_search").as_string(), std::string("REEDS_SHEPP")); diff --git a/nav2_smac_planner/test/test_smac_lattice.cpp b/nav2_smac_planner/test/test_smac_lattice.cpp index b791f9961c2..dcc925e9e72 100644 --- a/nav2_smac_planner/test/test_smac_lattice.cpp +++ b/nav2_smac_planner/test/test_smac_lattice.cpp @@ -60,6 +60,10 @@ TEST(SmacTest, test_smac_lattice) std::make_shared("global_costmap"); costmap_ros->on_configure(rclcpp_lifecycle::State()); + auto dummy_cancel_checker = []() { + return false; + }; + geometry_msgs::msg::PoseStamped start, goal; start.pose.position.x = 0.0; start.pose.position.y = 0.0; @@ -76,7 +80,7 @@ TEST(SmacTest, test_smac_lattice) planner->activate(); try { - planner->createPlan(start, goal); + planner->createPlan(start, goal, dummy_cancel_checker); } catch (...) { } @@ -128,6 +132,7 @@ TEST(SmacTest, test_smac_lattice_reconfigure) rclcpp::Parameter("test.tolerance", 42.0), rclcpp::Parameter("test.rotation_penalty", 42.0), rclcpp::Parameter("test.max_on_approach_iterations", 42), + rclcpp::Parameter("test.terminal_checking_interval", 42), rclcpp::Parameter("test.allow_reverse_expansion", true)}); try { diff --git a/nav2_smac_planner/test/test_smoother.cpp b/nav2_smac_planner/test/test_smoother.cpp index 38449234980..616c674595a 100644 --- a/nav2_smac_planner/test/test_smoother.cpp +++ b/nav2_smac_planner/test/test_smoother.cpp @@ -89,11 +89,14 @@ TEST(SmootherTest, test_full_smoother) int max_iterations = 10000; float tolerance = 10.0; int it_on_approach = 10; + int terminal_checking_interval = 5000; double max_planning_time = 120.0; int num_it = 0; a_star.initialize( - false, max_iterations, std::numeric_limits::max(), max_planning_time, 401, size_theta); + false, max_iterations, + std::numeric_limits::max(), terminal_checking_interval, max_planning_time, 401, + size_theta); // Convert raw costmap into a costmap ros object auto costmap_ros = std::make_shared(); @@ -105,12 +108,16 @@ TEST(SmootherTest, test_full_smoother) std::make_unique(costmap_ros, size_theta, node); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); + auto dummy_cancel_checker = []() { + return false; + }; + // Create A* search to smooth a_star.setCollisionChecker(checker.get()); a_star.setStart(5u, 5u, 0u); a_star.setGoal(45u, 45u, 36u); nav2_smac_planner::NodeHybrid::CoordinateVector path; - EXPECT_TRUE(a_star.createPath(path, num_it, tolerance)); + EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, dummy_cancel_checker)); // Convert to world coordinates and get length to compare to smoothed length nav_msgs::msg::Path plan; diff --git a/nav2_system_tests/src/error_codes/error_code_param.yaml b/nav2_system_tests/src/error_codes/error_code_param.yaml index 388e6f2cdb2..7886fe66866 100644 --- a/nav2_system_tests/src/error_codes/error_code_param.yaml +++ b/nav2_system_tests/src/error_codes/error_code_param.yaml @@ -154,7 +154,7 @@ planner_server: expected_planner_frequency: 20.0 planner_plugins: [ "unknown", "tf_error", "start_outside_map", "goal_outside_map", "start_occupied", "goal_occupied", "timeout","no_valid_path", - "no_viapoints_given" ] + "no_viapoints_given", "cancelled" ] unknown: plugin: "nav2_system_tests::UnknownErrorPlanner" tf_error: @@ -173,6 +173,8 @@ planner_server: plugin: "nav2_system_tests::NoValidPathErrorPlanner" no_viapoints_given: plugin: "nav2_system_tests::NoViapointsGivenErrorPlanner" + cancelled: + plugin: "nav2_system_tests::CancelledPlanner" smoother_server: ros__parameters: diff --git a/nav2_system_tests/src/error_codes/planner/planner_error_plugin.cpp b/nav2_system_tests/src/error_codes/planner/planner_error_plugin.cpp index 90b18c5b117..1c2eecf23a4 100644 --- a/nav2_system_tests/src/error_codes/planner/planner_error_plugin.cpp +++ b/nav2_system_tests/src/error_codes/planner/planner_error_plugin.cpp @@ -24,3 +24,4 @@ PLUGINLIB_EXPORT_CLASS(nav2_system_tests::NoValidPathErrorPlanner, nav2_core::Gl PLUGINLIB_EXPORT_CLASS(nav2_system_tests::TimedOutErrorPlanner, nav2_core::GlobalPlanner) PLUGINLIB_EXPORT_CLASS(nav2_system_tests::TFErrorPlanner, nav2_core::GlobalPlanner) PLUGINLIB_EXPORT_CLASS(nav2_system_tests::NoViapointsGivenErrorPlanner, nav2_core::GlobalPlanner) +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::CancelledPlanner, nav2_core::GlobalPlanner) diff --git a/nav2_system_tests/src/error_codes/planner/planner_error_plugin.hpp b/nav2_system_tests/src/error_codes/planner/planner_error_plugin.hpp index 2b6ae7971d3..77b23aeccef 100644 --- a/nav2_system_tests/src/error_codes/planner/planner_error_plugin.hpp +++ b/nav2_system_tests/src/error_codes/planner/planner_error_plugin.hpp @@ -51,7 +51,8 @@ class UnknownErrorPlanner : public nav2_core::GlobalPlanner nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped &, - const geometry_msgs::msg::PoseStamped &) override + const geometry_msgs::msg::PoseStamped &, + std::function) override { throw nav2_core::PlannerException("Unknown Error"); } @@ -61,7 +62,8 @@ class StartOccupiedErrorPlanner : public UnknownErrorPlanner { nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped &, - const geometry_msgs::msg::PoseStamped &) override + const geometry_msgs::msg::PoseStamped &, + std::function) override { throw nav2_core::StartOccupied("Start Occupied"); } @@ -71,7 +73,8 @@ class GoalOccupiedErrorPlanner : public UnknownErrorPlanner { nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped &, - const geometry_msgs::msg::PoseStamped &) override + const geometry_msgs::msg::PoseStamped &, + std::function) override { throw nav2_core::GoalOccupied("Goal occupied"); } @@ -81,7 +84,8 @@ class StartOutsideMapErrorPlanner : public UnknownErrorPlanner { nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped &, - const geometry_msgs::msg::PoseStamped &) override + const geometry_msgs::msg::PoseStamped &, + std::function) override { throw nav2_core::StartOutsideMapBounds("Start OutsideMapBounds"); } @@ -91,7 +95,8 @@ class GoalOutsideMapErrorPlanner : public UnknownErrorPlanner { nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped &, - const geometry_msgs::msg::PoseStamped &) override + const geometry_msgs::msg::PoseStamped &, + std::function) override { throw nav2_core::GoalOutsideMapBounds("Goal outside map bounds"); } @@ -101,7 +106,8 @@ class NoValidPathErrorPlanner : public UnknownErrorPlanner { nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped &, - const geometry_msgs::msg::PoseStamped &) override + const geometry_msgs::msg::PoseStamped &, + std::function) override { return nav_msgs::msg::Path(); } @@ -112,7 +118,8 @@ class TimedOutErrorPlanner : public UnknownErrorPlanner { nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped &, - const geometry_msgs::msg::PoseStamped &) override + const geometry_msgs::msg::PoseStamped &, + std::function) override { throw nav2_core::PlannerTimedOut("Planner Timed Out"); } @@ -122,7 +129,8 @@ class TFErrorPlanner : public UnknownErrorPlanner { nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped &, - const geometry_msgs::msg::PoseStamped &) override + const geometry_msgs::msg::PoseStamped &, + std::function) override { throw nav2_core::PlannerTFError("TF Error"); } @@ -132,12 +140,33 @@ class NoViapointsGivenErrorPlanner : public UnknownErrorPlanner { nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped &, - const geometry_msgs::msg::PoseStamped &) override + const geometry_msgs::msg::PoseStamped &, + std::function) override { throw nav2_core::NoViapointsGiven("No Via points given"); } }; +class CancelledPlanner : public UnknownErrorPlanner +{ + nav_msgs::msg::Path createPlan( + const geometry_msgs::msg::PoseStamped &, + const geometry_msgs::msg::PoseStamped &, + std::function cancel_checker) override + { + auto start_time = std::chrono::steady_clock::now(); + while (rclcpp::ok() && + std::chrono::steady_clock::now() - start_time < std::chrono::seconds(5)) + { + if (cancel_checker()) { + throw nav2_core::PlannerCancelled("Planner Cancelled"); + } + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } + throw nav2_core::PlannerException("Cancel is not called in time."); + } +}; + } // namespace nav2_system_tests diff --git a/nav2_system_tests/src/error_codes/planner_plugins.xml b/nav2_system_tests/src/error_codes/planner_plugins.xml index 9d6f7f0545b..ab10ae800bb 100644 --- a/nav2_system_tests/src/error_codes/planner_plugins.xml +++ b/nav2_system_tests/src/error_codes/planner_plugins.xml @@ -39,4 +39,8 @@ base_class_type="nav2_core::GlobalPlanner"> This global planner produces a no via points exception. + + This global planner produces a cancelled exception. + diff --git a/nav2_system_tests/src/error_codes/test_error_codes.py b/nav2_system_tests/src/error_codes/test_error_codes.py index f023bbb27c1..a5f2b84d975 100755 --- a/nav2_system_tests/src/error_codes/test_error_codes.py +++ b/nav2_system_tests/src/error_codes/test_error_codes.py @@ -14,6 +14,7 @@ # limitations under the License. import sys +import threading import time from geometry_msgs.msg import PoseStamped @@ -23,7 +24,7 @@ FollowPath, SmoothPath, ) -from nav2_simple_commander.robot_navigator import BasicNavigator +from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult from nav_msgs.msg import Path import rclpy @@ -111,6 +112,17 @@ def main(argv=sys.argv[1:]): result.error_code == error_code ), 'Compute path to pose error does not match' + def cancel_task(): + time.sleep(1) + navigator.goal_handle.cancel_goal_async() + + # Check compute path to pose cancel + threading.Thread(target=cancel_task).start() + result = navigator._getPathImpl(initial_pose, goal_pose, 'cancelled') + assert ( + navigator.getResult() == TaskResult.CANCELED + ), 'Compute path to pose cancel failed' + # Check compute path through error codes goal_pose1 = goal_pose goal_poses = [goal_pose, goal_pose1] @@ -133,6 +145,12 @@ def main(argv=sys.argv[1:]): assert ( result.error_code == error_code ), 'Compute path through pose error does not match' + # Check compute path to pose cancel + threading.Thread(target=cancel_task).start() + result = navigator._getPathThroughPosesImpl(initial_pose, goal_poses, 'cancelled') + assert ( + navigator.getResult() == TaskResult.CANCELED + ), 'Compute path through poses cancel failed' # Check compute path to pose error codes pose = PoseStamped() diff --git a/nav2_system_tests/src/planning/planner_tester.hpp b/nav2_system_tests/src/planning/planner_tester.hpp index 7a5f8d4b7f9..421a00f4b7d 100644 --- a/nav2_system_tests/src/planning/planner_tester.hpp +++ b/nav2_system_tests/src/planning/planner_tester.hpp @@ -87,7 +87,8 @@ class NavFnPlannerTester : public nav2_planner::PlannerServer return false; } try { - path = planners_["GridBased"]->createPlan(start, goal); + auto dummy_cancel_checker = []() {return false;}; + path = planners_["GridBased"]->createPlan(start, goal, dummy_cancel_checker); // The situation when createPlan() did not throw any exception // does not guarantee that plan was created correctly. // So it should be checked additionally that path is correct. diff --git a/nav2_system_tests/src/planning/test_planner_plugins.cpp b/nav2_system_tests/src/planning/test_planner_plugins.cpp index 906e8f44d3a..01d264006ac 100644 --- a/nav2_system_tests/src/planning/test_planner_plugins.cpp +++ b/nav2_system_tests/src/planning/test_planner_plugins.cpp @@ -57,9 +57,11 @@ void testSmallPathValidityAndOrientation(std::string plugin, double length) goal.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(-M_PI); goal.header.frame_id = "map"; + auto dummy_cancel_checker = []() {return false;}; + // Test without use_final_approach_orientation // expecting end path pose orientation to be equal to goal orientation - auto path = obj->getPlan(start, goal, "GridBased"); + auto path = obj->getPlan(start, goal, "GridBased", dummy_cancel_checker); EXPECT_GT((int)path.poses.size(), 0); EXPECT_NEAR(tf2::getYaw(path.poses.back().pose.orientation), -M_PI, 0.01); // obj->onCleanup(state); @@ -93,7 +95,9 @@ void testSmallPathValidityAndNoOrientation(std::string plugin, double length) goal.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(-M_PI); goal.header.frame_id = "map"; - auto path = obj->getPlan(start, goal, "GridBased"); + auto dummy_cancel_checker = []() {return false;}; + + auto path = obj->getPlan(start, goal, "GridBased", dummy_cancel_checker); EXPECT_GT((int)path.poses.size(), 0); int path_size = path.poses.size(); @@ -114,6 +118,36 @@ void testSmallPathValidityAndNoOrientation(std::string plugin, double length) obj.reset(); } +void testCancel(std::string plugin) +{ + auto obj = std::make_shared(); + rclcpp_lifecycle::State state; + obj->set_parameter(rclcpp::Parameter("GridBased.plugin", plugin)); + obj->declare_parameter("GridBased.terminal_checking_interval", rclcpp::ParameterValue(1)); + obj->onConfigure(state); + + geometry_msgs::msg::PoseStamped start; + geometry_msgs::msg::PoseStamped goal; + + start.pose.position.x = 0.0; + start.pose.position.y = 0.0; + start.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(M_PI_2); + start.header.frame_id = "map"; + + goal.pose.position.x = 0.5; + goal.pose.position.y = 0.6; + goal.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(-M_PI); + goal.header.frame_id = "map"; + + auto always_cancelled = []() {return true;}; + + EXPECT_THROW( + obj->getPlan(start, goal, "GridBased", always_cancelled), + nav2_core::PlannerCancelled); + // obj->onCleanup(state); + obj.reset(); +} + TEST(testPluginMap, Failures) { auto obj = std::make_shared(); @@ -127,11 +161,14 @@ TEST(testPluginMap, Failures) geometry_msgs::msg::PoseStamped goal; std::string plugin_fake = "fake"; std::string plugin_none = ""; - auto path = obj->getPlan(start, goal, plugin_none); + + auto dummy_cancel_checker = []() {return false;}; + + auto path = obj->getPlan(start, goal, plugin_none, dummy_cancel_checker); EXPECT_EQ(path.header.frame_id, std::string("map")); try { - path = obj->getPlan(start, goal, plugin_fake); + path = obj->getPlan(start, goal, plugin_fake, dummy_cancel_checker); FAIL() << "Failed to throw invalid planner id exception"; } catch (const nav2_core::InvalidPlanner & ex) { EXPECT_EQ(ex.what(), std::string("Planner id fake is invalid")); @@ -140,6 +177,7 @@ TEST(testPluginMap, Failures) obj->onCleanup(state); } + TEST(testPluginMap, Smac2dEqualStartGoal) { testSmallPathValidityAndOrientation("nav2_smac_planner/SmacPlanner2D", 0.0); @@ -290,6 +328,32 @@ TEST(testPluginMap, ThetaStarAboveCostmapResolutionN) testSmallPathValidityAndNoOrientation("nav2_theta_star_planner/ThetaStarPlanner", 1.5); } +TEST(testPluginMap, NavFnCancel) +{ + testCancel("nav2_navfn_planner/NavfnPlanner"); +} + +TEST(testPluginMap, ThetaStarCancel) +{ + testCancel("nav2_theta_star_planner/ThetaStarPlanner"); +} + +TEST(testPluginMap, Smac2dCancel) +{ + testCancel("nav2_smac_planner/SmacPlanner2D"); +} + +TEST(testPluginMap, SmacLatticeCancel) +{ + testCancel("nav2_smac_planner/SmacPlannerLattice"); +} + +TEST(testPluginMap, SmacHybridAStarCancel) +{ + testCancel("nav2_smac_planner/SmacPlannerHybrid"); +} + + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp index a58a4f87eda..1c819a882ee 100644 --- a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp +++ b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp @@ -76,6 +76,8 @@ class ThetaStar bool allow_unknown_; /// the x-directional and y-directional lengths of the map respectively int size_x_, size_y_; + /// the interval at which the planner checks if it has been cancelled + int terminal_checking_interval_; ThetaStar(); @@ -87,7 +89,7 @@ class ThetaStar * @param raw_path is used to return the path obtained by executing the algorithm * @return true if a path is found, false if no path is found in between the start and goal pose */ - bool generatePath(std::vector & raw_path); + bool generatePath(std::vector & raw_path, std::function cancel_checker); /** * @brief this function checks whether the cost of a point(cx, cy) on the costmap is less than the LETHAL_COST diff --git a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp index bcbff2ae7c0..09f87142196 100644 --- a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp +++ b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp @@ -54,9 +54,17 @@ class ThetaStarPlanner : public nav2_core::GlobalPlanner void deactivate() override; + /** + * @brief Creating a plan from start and goal poses + * @param start Start pose + * @param goal Goal pose + * @param cancel_checker Function to check if the action has been canceled + * @return nav2_msgs::Path of the generated path + */ nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal) override; + const geometry_msgs::msg::PoseStamped & goal, + std::function cancel_checker) override; protected: std::shared_ptr tf_; @@ -76,9 +84,10 @@ class ThetaStarPlanner : public nav2_core::GlobalPlanner /** * @brief the function responsible for calling the algorithm and retrieving a path from it + * @param cancel_checker is a function to check if the action has been canceled * @return global_path is the planned path to be taken */ - void getPlan(nav_msgs::msg::Path & global_path); + void getPlan(nav_msgs::msg::Path & global_path, std::function cancel_checker); /** * @brief interpolates points between the consecutive waypoints of the path diff --git a/nav2_theta_star_planner/src/theta_star.cpp b/nav2_theta_star_planner/src/theta_star.cpp index f83b745b22e..ba3629bcbfa 100644 --- a/nav2_theta_star_planner/src/theta_star.cpp +++ b/nav2_theta_star_planner/src/theta_star.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include +#include "nav2_core/planner_exceptions.hpp" #include "nav2_theta_star_planner/theta_star.hpp" namespace theta_star @@ -26,6 +27,7 @@ ThetaStar::ThetaStar() allow_unknown_(true), size_x_(0), size_y_(0), + terminal_checking_interval_(5000), index_generated_(0) { exp_node = new tree_node; @@ -43,7 +45,7 @@ void ThetaStar::setStartAndGoal( dst_ = {static_cast(d[0]), static_cast(d[1])}; } -bool ThetaStar::generatePath(std::vector & raw_path) +bool ThetaStar::generatePath(std::vector & raw_path, std::function cancel_checker) { resetContainers(); addToNodesData(index_generated_); @@ -60,6 +62,11 @@ bool ThetaStar::generatePath(std::vector & raw_path) while (!queue_.empty()) { nodes_opened++; + if (nodes_opened % terminal_checking_interval_ == 0 && cancel_checker()) { + clearQueue(); + throw nav2_core::PlannerCancelled("Planner was canceled"); + } + if (isGoal(*curr_data)) { break; } diff --git a/nav2_theta_star_planner/src/theta_star_planner.cpp b/nav2_theta_star_planner/src/theta_star_planner.cpp index ca9391111b3..61064efbf45 100644 --- a/nav2_theta_star_planner/src/theta_star_planner.cpp +++ b/nav2_theta_star_planner/src/theta_star_planner.cpp @@ -59,6 +59,10 @@ void ThetaStarPlanner::configure( planner_->w_heuristic_cost_ = planner_->w_euc_cost_ < 1.0 ? planner_->w_euc_cost_ : 1.0; + nav2_util::declare_parameter_if_not_declared( + node, name_ + ".terminal_checking_interval", rclcpp::ParameterValue(5000)); + node->get_parameter(name_ + ".terminal_checking_interval", planner_->terminal_checking_interval_); + nav2_util::declare_parameter_if_not_declared( node, name + ".use_final_approach_orientation", rclcpp::ParameterValue(false)); node->get_parameter(name + ".use_final_approach_orientation", use_final_approach_orientation_); @@ -86,7 +90,8 @@ void ThetaStarPlanner::deactivate() nav_msgs::msg::Path ThetaStarPlanner::createPlan( const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal) + const geometry_msgs::msg::PoseStamped & goal, + std::function cancel_checker) { nav_msgs::msg::Path global_path; auto start_time = std::chrono::steady_clock::now(); @@ -140,7 +145,7 @@ nav_msgs::msg::Path ThetaStarPlanner::createPlan( RCLCPP_DEBUG( logger_, "Got the src and dst... (%i, %i) && (%i, %i)", planner_->src_.x, planner_->src_.y, planner_->dst_.x, planner_->dst_.y); - getPlan(global_path); + getPlan(global_path, cancel_checker); // check if a plan is generated size_t plan_size = global_path.poses.size(); if (plan_size > 0) { @@ -173,13 +178,15 @@ nav_msgs::msg::Path ThetaStarPlanner::createPlan( return global_path; } -void ThetaStarPlanner::getPlan(nav_msgs::msg::Path & global_path) +void ThetaStarPlanner::getPlan( + nav_msgs::msg::Path & global_path, + std::function cancel_checker) { std::vector path; if (planner_->isUnsafeToPlan()) { global_path.poses.clear(); throw nav2_core::PlannerException("Either of the start or goal pose are an obstacle! "); - } else if (planner_->generatePath(path)) { + } else if (planner_->generatePath(path, cancel_checker)) { global_path = linearInterpolation(path, planner_->costmap_->getResolution()); } else { global_path.poses.clear(); @@ -229,6 +236,9 @@ ThetaStarPlanner::dynamicParametersCallback(std::vector param if (name == name_ + ".how_many_corners") { planner_->how_many_corners_ = parameter.as_int(); } + if (name == name_ + ".terminal_checking_interval") { + planner_->terminal_checking_interval_ = parameter.as_int(); + } } else if (type == ParameterType::PARAMETER_DOUBLE) { if (name == name_ + ".w_euc_cost") { planner_->w_euc_cost_ = parameter.as_double(); diff --git a/nav2_theta_star_planner/test/test_theta_star.cpp b/nav2_theta_star_planner/test/test_theta_star.cpp index 0476290f934..a8db0ddbfe0 100644 --- a/nav2_theta_star_planner/test/test_theta_star.cpp +++ b/nav2_theta_star_planner/test/test_theta_star.cpp @@ -60,10 +60,12 @@ class test_theta_star : public theta_star::ThetaStar void uresetContainers() {nodes_data_.clear(); resetContainers();} - bool runAlgo(std::vector & path) + bool runAlgo( + std::vector & path, + std::function cancel_checker = [] () {return false;}) { if (!isUnsafeToPlan()) { - return generatePath(path); + return generatePath(path, cancel_checker); } return false; } @@ -162,7 +164,11 @@ TEST(ThetaStarPlanner, test_theta_star_planner) { planner_2d->configure(life_node, "test", nullptr, costmap_ros); planner_2d->activate(); - nav_msgs::msg::Path path = planner_2d->createPlan(start, goal); + auto dummy_cancel_checker = []() { + return false; + }; + + nav_msgs::msg::Path path = planner_2d->createPlan(start, goal, dummy_cancel_checker); EXPECT_GT(static_cast(path.poses.size()), 0); // test if the goal is unsafe @@ -174,7 +180,7 @@ TEST(ThetaStarPlanner, test_theta_star_planner) { goal.pose.position.x = 1.0; goal.pose.position.y = 1.0; - EXPECT_THROW(planner_2d->createPlan(start, goal), nav2_core::GoalOccupied); + EXPECT_THROW(planner_2d->createPlan(start, goal, dummy_cancel_checker), nav2_core::GoalOccupied); planner_2d->deactivate(); planner_2d->cleanup(); @@ -212,7 +218,8 @@ TEST(ThetaStarPlanner, test_theta_star_reconfigure) rclcpp::Parameter("test.w_euc_cost", 1.0), rclcpp::Parameter("test.w_traversal_cost", 2.0), rclcpp::Parameter("test.use_final_approach_orientation", false), - rclcpp::Parameter("test.allow_unknown", false)}); + rclcpp::Parameter("test.allow_unknown", false), + rclcpp::Parameter("test.terminal_checking_interval", 100)}); rclcpp::spin_until_future_complete( life_node->get_node_base_interface(), @@ -225,6 +232,7 @@ TEST(ThetaStarPlanner, test_theta_star_reconfigure) EXPECT_EQ(life_node->get_parameter("test.w_traversal_cost").as_double(), 2.0); EXPECT_EQ(life_node->get_parameter("test.use_final_approach_orientation").as_bool(), false); EXPECT_EQ(life_node->get_parameter("test.allow_unknown").as_bool(), false); + EXPECT_EQ(life_node->get_parameter("test.terminal_checking_interval").as_int(), 100); rclcpp::spin_until_future_complete( life_node->get_node_base_interface(), From 26363716536425ff6479610c18639c853e7537be Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 5 Mar 2024 19:28:08 +0100 Subject: [PATCH 20/36] Fix BT.CPP import (#4165) Signed-off-by: Tony Najjar --- nav2_behavior_tree/src/generate_nav2_tree_nodes_xml.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/nav2_behavior_tree/src/generate_nav2_tree_nodes_xml.cpp b/nav2_behavior_tree/src/generate_nav2_tree_nodes_xml.cpp index 01bdf6a6854..4992e2e7775 100644 --- a/nav2_behavior_tree/src/generate_nav2_tree_nodes_xml.cpp +++ b/nav2_behavior_tree/src/generate_nav2_tree_nodes_xml.cpp @@ -17,10 +17,10 @@ #include #include -#include "behaviortree_cpp_v3/behavior_tree.h" -#include "behaviortree_cpp_v3/bt_factory.h" -#include "behaviortree_cpp_v3/utils/shared_library.h" -#include "behaviortree_cpp_v3/xml_parsing.h" +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/bt_factory.h" +#include "behaviortree_cpp/utils/shared_library.h" +#include "behaviortree_cpp/xml_parsing.h" #include "plugins_list.hpp" From c9d9b5c698d1f5aff060806481d18ba7e9b18cb2 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 7 Mar 2024 14:12:33 -0800 Subject: [PATCH 21/36] Update default recommendation from Obstacles to Cost critic in MPPI (#4170) Signed-off-by: Steve Macenski --- nav2_mppi_controller/README.md | 34 +++++++++++++++++----------------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index 2a9a25a8638..3bf779e0938 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -201,7 +201,7 @@ controller_server: time_step: 3 AckermannConstraints: min_turning_r: 0.2 - critics: ["ConstraintCritic", "ObstaclesCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"] + critics: ["ConstraintCritic", "CostCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"] ConstraintCritic: enabled: true cost_power: 1 @@ -221,24 +221,24 @@ controller_server: cost_power: 1 cost_weight: 5.0 threshold_to_consider: 0.5 - ObstaclesCritic: - enabled: true - cost_power: 1 - repulsion_weight: 1.5 - critical_weight: 20.0 - consider_footprint: false - collision_cost: 10000.0 - collision_margin_distance: 0.1 - near_goal_distance: 0.5 - # Option to replace Obstacles and use Cost instead - # CostCritic: + # ObstaclesCritic: # enabled: true # cost_power: 1 - # cost_weight: 3.81 - # critical_cost: 300.0 - # consider_footprint: true - # collision_cost: 1000000.0 - # near_goal_distance: 1.0 + # repulsion_weight: 1.5 + # critical_weight: 20.0 + # consider_footprint: false + # collision_cost: 10000.0 + # collision_margin_distance: 0.1 + # near_goal_distance: 0.5 + CostCritic: + enabled: true + cost_power: 1 + cost_weight: 3.81 + critical_cost: 300.0 + consider_footprint: true + collision_cost: 1000000.0 + near_goal_distance: 1.0 + PathAlignCritic: PathAlignCritic: enabled: true cost_power: 1 From 47374622dee01a27e5f9b8ae08f3d19a15de9b3a Mon Sep 17 00:00:00 2001 From: Reza Kermani Date: Fri, 8 Mar 2024 13:25:25 -0500 Subject: [PATCH 22/36] nav2_controller: add loop rate log (#4171) * update smac_planner README Signed-off-by: ARK3r * added current controller loop rate logging Signed-off-by: ARK3r * linting Signed-off-by: ARK3r * uncrustify lint Signed-off-by: ARK3r * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski --------- Signed-off-by: ARK3r Signed-off-by: Steve Macenski Co-authored-by: Steve Macenski --- nav2_controller/src/controller_server.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index fa25e0b1379..85aae171cc7 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -419,6 +419,7 @@ void ControllerServer::computeControl() { std::lock_guard lock(dynamic_params_lock_); + auto start_time = this->now(); RCLCPP_INFO(get_logger(), "Received a goal, begin computing control effort."); try { @@ -479,10 +480,12 @@ void ControllerServer::computeControl() break; } + auto cycle_duration = this->now() - start_time; if (!loop_rate.sleep()) { RCLCPP_WARN( - get_logger(), "Control loop missed its desired rate of %.4fHz", - controller_frequency_); + get_logger(), + "Control loop missed its desired rate of %.4f Hz. Current loop rate is %.4f Hz.", + controller_frequency_, 1 / cycle_duration.seconds()); } } } catch (nav2_core::InvalidController & e) { From 0897da9252b4cb537b1110e88831a1e63ac157d3 Mon Sep 17 00:00:00 2001 From: GoesM <130988564+GoesM@users.noreply.github.com> Date: Tue, 12 Mar 2024 07:08:28 +0800 Subject: [PATCH 23/36] completely shutdown inital_pose_sub_ (#4176) Signed-off-by: GoesM Co-authored-by: GoesM --- nav2_amcl/src/amcl_node.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 192111439f4..d1c47979321 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -326,6 +326,7 @@ AmclNode::on_cleanup(const rclcpp_lifecycle::State & /*state*/) // don't continue to process incoming messages global_loc_srv_.reset(); nomotion_update_srv_.reset(); + executor_thread_.reset(); // to make sure initial_pose_sub_ completely exit initial_pose_sub_.reset(); laser_scan_connection_.disconnect(); tf_listener_.reset(); // listener may access lase_scan_filter_, so it should be reset earlier From 09ba08b7f45004df92f77fa683ea33d8afa2acce Mon Sep 17 00:00:00 2001 From: bi0ha2ard Date: Wed, 13 Mar 2024 19:38:04 +0100 Subject: [PATCH 24/36] chore(nav2_behavior_tree): log actual wait period in bt_action_node (#4178) Signed-off-by: Felix Co-authored-by: Felix --- .../include/nav2_behavior_tree/bt_action_node.hpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp index 6193a91a868..846f8d13853 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp @@ -97,8 +97,9 @@ class BtActionNode : public BT::ActionNodeBase RCLCPP_DEBUG(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str()); if (!action_client_->wait_for_action_server(wait_for_service_timeout_)) { RCLCPP_ERROR( - node_->get_logger(), "\"%s\" action server not available after waiting for 1 s", - action_name.c_str()); + node_->get_logger(), "\"%s\" action server not available after waiting for %f s", + action_name.c_str(), + wait_for_service_timeout_.count() / 1000.0); throw std::runtime_error( std::string("Action server ") + action_name + std::string(" not available")); From 9f13cb1b29ca14c11bcbe11ab02d35c71f35de9d Mon Sep 17 00:00:00 2001 From: GoesM <130988564+GoesM@users.noreply.github.com> Date: Fri, 15 Mar 2024 01:05:08 +0800 Subject: [PATCH 25/36] replace throw-error with error-log to avoid UAF mentioned in #4175 (#4180) * replace throw-error with error-log to avoid UAF Signed-off-by: GoesM * fix typo Signed-off-by: GoesM --------- Signed-off-by: GoesM Co-authored-by: GoesM --- nav2_util/include/nav2_util/simple_action_server.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_util/include/nav2_util/simple_action_server.hpp b/nav2_util/include/nav2_util/simple_action_server.hpp index eec9c2dd9f7..c8b1660840f 100644 --- a/nav2_util/include/nav2_util/simple_action_server.hpp +++ b/nav2_util/include/nav2_util/simple_action_server.hpp @@ -315,7 +315,7 @@ class SimpleActionServer if (steady_clock::now() - start_time >= server_timeout_) { terminate_all(); if (completion_callback_) {completion_callback_();} - throw std::runtime_error("Action callback is still running and missed deadline to stop"); + error_msg("Action callback is still running and missed deadline to stop"); } } From 53f1f4289fb8f5630c4e22ba5b57ceb9c0c25ff6 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 15 Mar 2024 11:25:57 -0700 Subject: [PATCH 26/36] Fixing a typo in Smac Heuristic (#4184) Signed-off-by: Steve Macenski --- nav2_smac_planner/src/node_hybrid.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp index 2e81a5f47ce..aa1773e682d 100644 --- a/nav2_smac_planner/src/node_hybrid.cpp +++ b/nav2_smac_planner/src/node_hybrid.cpp @@ -658,10 +658,10 @@ float NodeHybrid::getObstacleHeuristic( if (existing_cost <= 0.0f) { if (motion_table.use_quadratic_cost_penalty) { travel_cost = - (i <= 3 ? 1.0f : sqrt2) * (1.0f + (cost_penalty * cost * cost / 64516.0f)); // 254^2 + (i <= 3 ? 1.0f : sqrt2) * (1.0f + (cost_penalty * cost * cost / 63504.0f)); // 252^2 } else { travel_cost = - ((i <= 3) ? 1.0f : sqrt2) * (1.0f + (cost_penalty * cost / 254.0f)); + ((i <= 3) ? 1.0f : sqrt2) * (1.0f + (cost_penalty * cost / 252.0f)); } new_cost = c_cost + travel_cost; From db974ea91b1eb01d7abf094aad511c0f2306d55f Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Fri, 15 Mar 2024 14:40:52 -0400 Subject: [PATCH 27/36] RPP Dexory tweaks (#4140) * RPP Dexory tweaks Signed-off-by: Guillaume Doisy * Update nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp Signed-off-by: Steve Macenski * Update nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp Signed-off-by: Steve Macenski * projectCarrotPastGoal test Signed-off-by: Guillaume Doisy * Use circleSegmentIntersection when projecting past end of path This guarantees that the look ahead distance is maintained Signed-off-by: James Ward * lint Signed-off-by: Guillaume Doisy --------- Signed-off-by: Guillaume Doisy Signed-off-by: Steve Macenski Signed-off-by: James Ward Co-authored-by: Guillaume Doisy Co-authored-by: Steve Macenski Co-authored-by: James Ward --- .../CMakeLists.txt | 2 + .../README.md | 2 + .../parameter_handler.hpp | 1 + .../path_handler.hpp | 3 +- .../regulated_pure_pursuit_controller.hpp | 12 +- .../package.xml | 1 + .../src/parameter_handler.cpp | 28 ++-- .../src/path_handler.cpp | 16 ++- .../src/regulated_pure_pursuit_controller.cpp | 64 ++++++++-- .../test/test_regulated_pp.cpp | 120 +++++++++++++++++- 10 files changed, 218 insertions(+), 31 deletions(-) diff --git a/nav2_regulated_pure_pursuit_controller/CMakeLists.txt b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt index 6f84b05382d..78beab89fea 100644 --- a/nav2_regulated_pure_pursuit_controller/CMakeLists.txt +++ b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5) project(nav2_regulated_pure_pursuit_controller) find_package(ament_cmake REQUIRED) +find_package(nav2_amcl REQUIRED) find_package(nav2_common REQUIRED) find_package(nav2_core REQUIRED) find_package(nav2_costmap_2d REQUIRED) @@ -26,6 +27,7 @@ set(dependencies nav2_costmap_2d pluginlib nav_msgs + nav2_amcl nav2_util nav2_core tf2 diff --git a/nav2_regulated_pure_pursuit_controller/README.md b/nav2_regulated_pure_pursuit_controller/README.md index 350b1926da4..57b820b16d5 100644 --- a/nav2_regulated_pure_pursuit_controller/README.md +++ b/nav2_regulated_pure_pursuit_controller/README.md @@ -90,6 +90,7 @@ Note: The maximum allowed time to collision is thresholded by the lookahead poin | `rotate_to_heading_min_angle` | The difference in the path orientation and the starting robot orientation to trigger a rotate in place, if `use_rotate_to_heading` is enabled. | | `max_angular_accel` | Maximum allowable angular acceleration while rotating to heading, if enabled | | `max_robot_pose_search_dist` | Maximum integrated distance along the path to bound the search for the closest pose to the robot. This is set by default to the maximum costmap extent, so it shouldn't be set manually unless there are loops within the local costmap. | +| `interpolate_curvature_after_goal` | Needs use_fixed_curvature_lookahead to be true. Interpolate a carrot after the goal dedicated to the curvate calculation (to avoid oscilaltions at the end of the path) | Example fully-described XML with default parameter values: @@ -137,6 +138,7 @@ controller_server: rotate_to_heading_min_angle: 0.785 max_angular_accel: 3.2 max_robot_pose_search_dist: 10.0 + interpolate_curvature_after_goal: false cost_scaling_dist: 0.3 cost_scaling_gain: 1.0 inflation_cost_scaling_factor: 3.0 diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp index 39f5de6fa0c..d6ca585a49a 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp @@ -56,6 +56,7 @@ struct Parameters double rotate_to_heading_min_angle; bool allow_reversing; double max_robot_pose_search_dist; + bool interpolate_curvature_after_goal; bool use_collision_detection; double transform_tolerance; }; diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp index 22bc0266a66..1e603f3c8bb 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp @@ -58,11 +58,12 @@ class PathHandler * - Outside the local_costmap (collision avoidance cannot be assured) * @param pose pose to transform * @param max_robot_pose_search_dist Distance to search for matching nearest path point + * @param reject_unit_path If true, fail if path has only one pose * @return Path in new frame */ nav_msgs::msg::Path transformGlobalPlan( const geometry_msgs::msg::PoseStamped & pose, - double max_robot_pose_search_dist); + double max_robot_pose_search_dist, bool reject_unit_path = false); /** * @brief Transform a pose to another frame. diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp index 7b28ca9720e..417dc1c811f 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp @@ -131,10 +131,12 @@ class RegulatedPurePursuitController : public nav2_core::Controller * @brief Whether robot should rotate to rough path heading * @param carrot_pose current lookahead point * @param angle_to_path Angle of robot output relatie to carrot marker + * @param x_vel_sign Velocoty sign (forward or backward) * @return Whether should rotate to path heading */ bool shouldRotateToPath( - const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path); + const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path, + double & x_vel_sign); /** * @brief Whether robot should rotate to final goal orientation @@ -185,9 +187,13 @@ class RegulatedPurePursuitController : public nav2_core::Controller * @brief Get lookahead point * @param lookahead_dist Optimal lookahead distance * @param path Current global path + * @param interpolate_after_goal If true, interpolate the lookahead point after the goal based + * on the orientation given by the position of the last two pose of the path * @return Lookahead point */ - geometry_msgs::msg::PoseStamped getLookAheadPoint(const double &, const nav_msgs::msg::Path &); + geometry_msgs::msg::PoseStamped getLookAheadPoint( + const double &, const nav_msgs::msg::Path &, + bool interpolate_after_goal = false); /** * @brief checks for the cusp position @@ -210,6 +216,8 @@ class RegulatedPurePursuitController : public nav2_core::Controller std::shared_ptr> global_path_pub_; std::shared_ptr> carrot_pub_; + std::shared_ptr> + curvature_carrot_pub_; std::shared_ptr> carrot_arc_pub_; std::unique_ptr path_handler_; std::unique_ptr param_handler_; diff --git a/nav2_regulated_pure_pursuit_controller/package.xml b/nav2_regulated_pure_pursuit_controller/package.xml index 5b62ce5aeaa..c56147a9693 100644 --- a/nav2_regulated_pure_pursuit_controller/package.xml +++ b/nav2_regulated_pure_pursuit_controller/package.xml @@ -10,6 +10,7 @@ ament_cmake + nav2_amcl nav2_common nav2_core nav2_util diff --git a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp index 016a626c288..dea2bc59a43 100644 --- a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp @@ -90,6 +90,9 @@ ParameterHandler::ParameterHandler( declare_parameter_if_not_declared( node, plugin_name_ + ".max_robot_pose_search_dist", rclcpp::ParameterValue(costmap_size_x / 2.0)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".interpolate_curvature_after_goal", + rclcpp::ParameterValue(false)); declare_parameter_if_not_declared( node, plugin_name_ + ".use_collision_detection", rclcpp::ParameterValue(true)); @@ -159,6 +162,15 @@ ParameterHandler::ParameterHandler( params_.max_robot_pose_search_dist = std::numeric_limits::max(); } + node->get_parameter( + plugin_name_ + ".interpolate_curvature_after_goal", + params_.interpolate_curvature_after_goal); + if (!params_.use_fixed_curvature_lookahead && params_.interpolate_curvature_after_goal) { + RCLCPP_WARN( + logger_, "For interpolate_curvature_after_goal to be set to true, " + "use_fixed_curvature_lookahead should be true, it is currently set to false. Disabling."); + params_.interpolate_curvature_after_goal = false; + } node->get_parameter( plugin_name_ + ".use_collision_detection", params_.use_collision_detection); @@ -170,16 +182,6 @@ ParameterHandler::ParameterHandler( params_.use_cost_regulated_linear_velocity_scaling = false; } - /** Possible to drive in reverse direction if and only if - "use_rotate_to_heading" parameter is set to false **/ - - if (params_.use_rotate_to_heading && params_.allow_reversing) { - RCLCPP_WARN( - logger_, "Disabling reversing. Both use_rotate_to_heading and allow_reversing " - "parameter cannot be set to true. By default setting use_rotate_to_heading true"); - params_.allow_reversing = false; - } - dyn_params_handler_ = node->add_on_set_parameters_callback( std::bind( &ParameterHandler::dynamicParametersCallback, @@ -250,12 +252,6 @@ ParameterHandler::dynamicParametersCallback( } else if (name == plugin_name_ + ".use_collision_detection") { params_.use_collision_detection = parameter.as_bool(); } else if (name == plugin_name_ + ".use_rotate_to_heading") { - if (parameter.as_bool() && params_.allow_reversing) { - RCLCPP_WARN( - logger_, "Both use_rotate_to_heading and allow_reversing " - "parameter cannot be set to true. Rejecting parameter update."); - continue; - } params_.use_rotate_to_heading = parameter.as_bool(); } else if (name == plugin_name_ + ".allow_reversing") { if (params_.use_rotate_to_heading && parameter.as_bool()) { diff --git a/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp index 90b7c4a3e0c..66b19be3237 100644 --- a/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp @@ -47,12 +47,17 @@ double PathHandler::getCostmapMaxExtent() const nav_msgs::msg::Path PathHandler::transformGlobalPlan( const geometry_msgs::msg::PoseStamped & pose, - double max_robot_pose_search_dist) + double max_robot_pose_search_dist, + bool reject_unit_path) { if (global_plan_.poses.empty()) { throw nav2_core::InvalidPath("Received plan with zero length"); } + if (reject_unit_path && global_plan_.poses.size() == 1) { + throw nav2_core::InvalidPath("Received plan with length of one"); + } + // let's get the pose of the robot in the frame of the plan geometry_msgs::msg::PoseStamped robot_pose; if (!transformPose(global_plan_.header.frame_id, pose, robot_pose)) { @@ -73,6 +78,15 @@ nav_msgs::msg::Path PathHandler::transformGlobalPlan( return euclidean_distance(robot_pose, ps); }); + // Make sure we always have at least 2 points on the transformed plan and that we don't prune + // the global plan below 2 points in order to have always enough point to interpolate the + // end of path direction + if (global_plan_.poses.begin() != closest_pose_upper_bound && global_plan_.poses.size() > 1 && + transformation_begin == std::prev(closest_pose_upper_bound)) + { + transformation_begin = std::prev(std::prev(closest_pose_upper_bound)); + } + // We'll discard points on the plan that are outside the local costmap const double max_costmap_extent = getCostmapMaxExtent(); auto transformation_end = std::find_if( diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index 76837754e86..09012a9fb3e 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -20,6 +20,7 @@ #include #include +#include "nav2_amcl/angleutils.hpp" #include "nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp" #include "nav2_core/controller_exceptions.hpp" #include "nav2_util/node_utils.hpp" @@ -73,6 +74,8 @@ void RegulatedPurePursuitController::configure( global_path_pub_ = node->create_publisher("received_global_plan", 1); carrot_pub_ = node->create_publisher("lookahead_point", 1); + curvature_carrot_pub_ = node->create_publisher( + "curvature_lookahead_point", 1); } void RegulatedPurePursuitController::cleanup() @@ -84,6 +87,7 @@ void RegulatedPurePursuitController::cleanup() plugin_name_.c_str()); global_path_pub_.reset(); carrot_pub_.reset(); + curvature_carrot_pub_.reset(); } void RegulatedPurePursuitController::activate() @@ -95,6 +99,7 @@ void RegulatedPurePursuitController::activate() plugin_name_.c_str()); global_path_pub_->on_activate(); carrot_pub_->on_activate(); + curvature_carrot_pub_->on_activate(); } void RegulatedPurePursuitController::deactivate() @@ -106,6 +111,7 @@ void RegulatedPurePursuitController::deactivate() plugin_name_.c_str()); global_path_pub_->on_deactivate(); carrot_pub_->on_deactivate(); + curvature_carrot_pub_->on_deactivate(); } std::unique_ptr RegulatedPurePursuitController::createCarrotMsg( @@ -171,7 +177,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity // Transform path to robot base frame auto transformed_plan = path_handler_->transformGlobalPlan( - pose, params_->max_robot_pose_search_dist); + pose, params_->max_robot_pose_search_dist, params_->interpolate_curvature_after_goal); global_path_pub_->publish(transformed_plan); // Find look ahead distance and point on path and publish @@ -190,6 +196,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity // Get the particular point on the path at the lookahead distance auto carrot_pose = getLookAheadPoint(lookahead_dist, transformed_plan); + auto rotate_to_path_carrot_pose = carrot_pose; carrot_pub_->publish(createCarrotMsg(carrot_pose)); double linear_vel, angular_vel; @@ -200,33 +207,39 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity if (params_->use_fixed_curvature_lookahead) { auto curvature_lookahead_pose = getLookAheadPoint( params_->curvature_lookahead_dist, - transformed_plan); + transformed_plan, params_->interpolate_curvature_after_goal); + rotate_to_path_carrot_pose = curvature_lookahead_pose; regulation_curvature = calculateCurvature(curvature_lookahead_pose.pose.position); + curvature_carrot_pub_->publish(createCarrotMsg(curvature_lookahead_pose)); } // Setting the velocity direction - double sign = 1.0; + double x_vel_sign = 1.0; if (params_->allow_reversing) { - sign = carrot_pose.pose.position.x >= 0.0 ? 1.0 : -1.0; + x_vel_sign = carrot_pose.pose.position.x >= 0.0 ? 1.0 : -1.0; } linear_vel = params_->desired_linear_vel; // Make sure we're in compliance with basic constraints + // For shouldRotateToPath, using x_vel_sign in order to support allow_reversing + // and rotate_to_path_carrot_pose for the direction carrot pose: + // - equal to "normal" carrot_pose when curvature_lookahead_pose = false + // - otherwise equal to curvature_lookahead_pose (which can be interpolated after goal) double angle_to_heading; if (shouldRotateToGoalHeading(carrot_pose)) { double angle_to_goal = tf2::getYaw(transformed_plan.poses.back().pose.orientation); rotateToHeading(linear_vel, angular_vel, angle_to_goal, speed); - } else if (shouldRotateToPath(carrot_pose, angle_to_heading)) { + } else if (shouldRotateToPath(rotate_to_path_carrot_pose, angle_to_heading, x_vel_sign)) { rotateToHeading(linear_vel, angular_vel, angle_to_heading, speed); } else { applyConstraints( regulation_curvature, speed, collision_checker_->costAtPose(pose.pose.position.x, pose.pose.position.y), transformed_plan, - linear_vel, sign); + linear_vel, x_vel_sign); // Apply curvature to angular velocity after constraining linear velocity - angular_vel = linear_vel * lookahead_curvature; + angular_vel = linear_vel * regulation_curvature; } // Collision checking on this velocity heading @@ -246,10 +259,15 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity } bool RegulatedPurePursuitController::shouldRotateToPath( - const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path) + const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path, + double & x_vel_sign) { // Whether we should rotate robot to rough path heading angle_to_path = atan2(carrot_pose.pose.position.y, carrot_pose.pose.position.x); + // In case we are reversing + if (x_vel_sign < 0.0) { + angle_to_path = nav2_amcl::angleutils::normalize(angle_to_path + M_PI); + } return params_->use_rotate_to_heading && fabs(angle_to_path) > params_->rotate_to_heading_min_angle; } @@ -314,7 +332,8 @@ geometry_msgs::msg::Point RegulatedPurePursuitController::circleSegmentIntersect geometry_msgs::msg::PoseStamped RegulatedPurePursuitController::getLookAheadPoint( const double & lookahead_dist, - const nav_msgs::msg::Path & transformed_plan) + const nav_msgs::msg::Path & transformed_plan, + bool interpolate_after_goal) { // Find the first pose which is at a distance greater than the lookahead distance auto goal_pose_it = std::find_if( @@ -324,7 +343,32 @@ geometry_msgs::msg::PoseStamped RegulatedPurePursuitController::getLookAheadPoin // If the no pose is not far enough, take the last pose if (goal_pose_it == transformed_plan.poses.end()) { - goal_pose_it = std::prev(transformed_plan.poses.end()); + if (interpolate_after_goal) { + auto last_pose_it = std::prev(transformed_plan.poses.end()); + auto prev_last_pose_it = std::prev(last_pose_it); + + double end_path_orientation = atan2( + last_pose_it->pose.position.y - prev_last_pose_it->pose.position.y, + last_pose_it->pose.position.x - prev_last_pose_it->pose.position.x); + + // Project the last segment out to guarantee it is beyond the look ahead + // distance + auto projected_position = last_pose_it->pose.position; + projected_position.x += cos(end_path_orientation) * lookahead_dist; + projected_position.y += sin(end_path_orientation) * lookahead_dist; + + // Use the circle intersection to find the position at the correct look + // ahead distance + const auto interpolated_position = circleSegmentIntersection( + last_pose_it->pose.position, projected_position, lookahead_dist); + + geometry_msgs::msg::PoseStamped interpolated_pose; + interpolated_pose.header = last_pose_it->header; + interpolated_pose.pose.position = interpolated_position; + return interpolated_pose; + } else { + goal_pose_it = std::prev(transformed_plan.poses.end()); + } } else if (goal_pose_it != transformed_plan.poses.begin()) { // Find the point on the line segment between the two poses // that is exactly the lookahead distance away from the robot pose (the origin) diff --git a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp index 1557dad2d2d..38cc195bc61 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -68,6 +68,14 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure return circleSegmentIntersection(p1, p2, r); } + geometry_msgs::msg::PoseStamped + projectCarrotPastGoalWrapper( + const double & dist, + const nav_msgs::msg::Path & path) + { + return getLookAheadPoint(dist, path, true); + } + geometry_msgs::msg::PoseStamped getLookAheadPointWrapper( const double & dist, const nav_msgs::msg::Path & path) { @@ -77,7 +85,8 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure bool shouldRotateToPathWrapper( const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path) { - return shouldRotateToPath(carrot_pose, angle_to_path); + double x_vel_sign = 1.0; + return shouldRotateToPath(carrot_pose, angle_to_path, x_vel_sign); } bool shouldRotateToGoalHeadingWrapper(const geometry_msgs::msg::PoseStamped & carrot_pose) @@ -330,6 +339,115 @@ INSTANTIATE_TEST_SUITE_P( } )); +TEST(RegulatedPurePursuitTest, projectCarrotPastGoal) { + auto ctrl = std::make_shared(); + auto node = std::make_shared("testRPP"); + std::string name = "PathFollower"; + auto tf = std::make_shared(node->get_clock()); + auto costmap = + std::make_shared("fake_costmap"); + rclcpp_lifecycle::State state; + costmap->on_configure(state); + ctrl->configure(node, name, tf, costmap); + + double EPSILON = std::numeric_limits::epsilon(); + + nav_msgs::msg::Path path; + // More than 2 poses + path.poses.resize(4); + path.poses[0].pose.position.x = 0.0; + path.poses[1].pose.position.x = 1.0; + path.poses[2].pose.position.x = 2.0; + path.poses[3].pose.position.x = 3.0; + auto pt = ctrl->projectCarrotPastGoalWrapper(10.0, path); + EXPECT_NEAR(pt.pose.position.x, 10.0, EPSILON); + EXPECT_NEAR(pt.pose.position.y, 0.0, EPSILON); + + // 2 poses fwd + path.poses.clear(); + path.poses.resize(2); + path.poses[0].pose.position.x = 2.0; + path.poses[1].pose.position.x = 3.0; + pt = ctrl->projectCarrotPastGoalWrapper(10.0, path); + EXPECT_NEAR(pt.pose.position.x, 10.0, EPSILON); + EXPECT_NEAR(pt.pose.position.y, 0.0, EPSILON); + + // 2 poses at 45° + path.poses.clear(); + path.poses.resize(2); + path.poses[0].pose.position.x = 2.0; + path.poses[0].pose.position.y = 2.0; + path.poses[1].pose.position.x = 3.0; + path.poses[1].pose.position.y = 3.0; + pt = ctrl->projectCarrotPastGoalWrapper(10.0, path); + EXPECT_NEAR(pt.pose.position.x, cos(45.0 * M_PI / 180) * 10.0, EPSILON); + EXPECT_NEAR(pt.pose.position.y, sin(45.0 * M_PI / 180) * 10.0, EPSILON); + + // 2 poses at 90° + path.poses.clear(); + path.poses.resize(2); + path.poses[0].pose.position.x = 0.0; + path.poses[0].pose.position.y = 2.0; + path.poses[1].pose.position.x = 0.0; + path.poses[1].pose.position.y = 3.0; + pt = ctrl->projectCarrotPastGoalWrapper(10.0, path); + EXPECT_NEAR(pt.pose.position.x, cos(90.0 * M_PI / 180) * 10.0, EPSILON); + EXPECT_NEAR(pt.pose.position.y, sin(90.0 * M_PI / 180) * 10.0, EPSILON); + + // 2 poses at 135° + path.poses.clear(); + path.poses.resize(2); + path.poses[0].pose.position.x = -2.0; + path.poses[0].pose.position.y = 2.0; + path.poses[1].pose.position.x = -3.0; + path.poses[1].pose.position.y = 3.0; + pt = ctrl->projectCarrotPastGoalWrapper(10.0, path); + EXPECT_NEAR(pt.pose.position.x, cos(135.0 * M_PI / 180) * 10.0, EPSILON); + EXPECT_NEAR(pt.pose.position.y, sin(135.0 * M_PI / 180) * 10.0, EPSILON); + + // 2 poses bck + path.poses.clear(); + path.poses.resize(2); + path.poses[0].pose.position.x = -2.0; + path.poses[1].pose.position.x = -3.0; + pt = ctrl->projectCarrotPastGoalWrapper(10.0, path); + EXPECT_NEAR(pt.pose.position.x, -10.0, EPSILON); + EXPECT_NEAR(pt.pose.position.y, 0.0, EPSILON); + + // 2 poses at -135° + path.poses.clear(); + path.poses.resize(2); + path.poses[0].pose.position.x = -2.0; + path.poses[0].pose.position.y = -2.0; + path.poses[1].pose.position.x = -3.0; + path.poses[1].pose.position.y = -3.0; + pt = ctrl->projectCarrotPastGoalWrapper(10.0, path); + EXPECT_NEAR(pt.pose.position.x, cos(-135.0 * M_PI / 180) * 10.0, EPSILON); + EXPECT_NEAR(pt.pose.position.y, sin(-135.0 * M_PI / 180) * 10.0, EPSILON); + + // 2 poses at -90° + path.poses.clear(); + path.poses.resize(2); + path.poses[0].pose.position.x = 0.0; + path.poses[0].pose.position.y = -2.0; + path.poses[1].pose.position.x = 0.0; + path.poses[1].pose.position.y = -3.0; + pt = ctrl->projectCarrotPastGoalWrapper(10.0, path); + EXPECT_NEAR(pt.pose.position.x, cos(-90.0 * M_PI / 180) * 10.0, EPSILON); + EXPECT_NEAR(pt.pose.position.y, sin(-90.0 * M_PI / 180) * 10.0, EPSILON); + + // 2 poses at -45° + path.poses.clear(); + path.poses.resize(2); + path.poses[0].pose.position.x = 2.0; + path.poses[0].pose.position.y = -2.0; + path.poses[1].pose.position.x = 3.0; + path.poses[1].pose.position.y = -3.0; + pt = ctrl->projectCarrotPastGoalWrapper(10.0, path); + EXPECT_NEAR(pt.pose.position.x, cos(-45.0 * M_PI / 180) * 10.0, EPSILON); + EXPECT_NEAR(pt.pose.position.y, sin(-45.0 * M_PI / 180) * 10.0, EPSILON); +} + TEST(RegulatedPurePursuitTest, lookaheadAPI) { auto ctrl = std::make_shared(); From aa849aede4fd9d230220d196a277b171b4f7c557 Mon Sep 17 00:00:00 2001 From: Antonio Park Date: Mon, 18 Mar 2024 20:44:40 +0900 Subject: [PATCH 28/36] fix typos in description messages (#4188) Signed-off-by: Antonio Park --- nav2_amcl/src/amcl_node.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index d1c47979321..6f2415cf505 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -1104,20 +1104,20 @@ AmclNode::initParameters() // Semantic checks if (laser_likelihood_max_dist_ < 0) { RCLCPP_WARN( - get_logger(), "You've set laser_likelihood_max_dist to be negtive," + get_logger(), "You've set laser_likelihood_max_dist to be negative," " this isn't allowed so it will be set to default value 2.0."); laser_likelihood_max_dist_ = 2.0; } if (max_particles_ < 0) { RCLCPP_WARN( - get_logger(), "You've set max_particles to be negtive," + get_logger(), "You've set max_particles to be negative," " this isn't allowed so it will be set to default value 2000."); max_particles_ = 2000; } if (min_particles_ < 0) { RCLCPP_WARN( - get_logger(), "You've set min_particles to be negtive," + get_logger(), "You've set min_particles to be negative," " this isn't allowed so it will be set to default value 500."); min_particles_ = 500; } @@ -1131,7 +1131,7 @@ AmclNode::initParameters() if (resample_interval_ <= 0) { RCLCPP_WARN( - get_logger(), "You've set resample_interval to be zero or negtive," + get_logger(), "You've set resample_interval to be zero or negative," " this isn't allowed so it will be set to default value to 1."); resample_interval_ = 1; } From f484b0970327906d54b0e049326d645c8810e126 Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Mon, 18 Mar 2024 13:41:28 +0100 Subject: [PATCH 29/36] AMCL: Set an initial guess by service call (#4182) * Added initial guess service. Signed-off-by: Alexander Mock Signed-off-by: Alexander Mock * - Removed added empty line - Renamed initialGuessCallback to initialPoseReceivedSrv - Added new line to SetInitialPose service definition - Removed mutex from initialPoseReceived - Cleanup service server Signed-off-by: Alexander Mock * added whitespace Signed-off-by: Alexander Mock * renamed initial pose service in callback bind Signed-off-by: Alexander Mock --------- Signed-off-by: Alexander Mock --- nav2_amcl/include/nav2_amcl/amcl_node.hpp | 11 +++++++++++ nav2_amcl/src/amcl_node.cpp | 14 ++++++++++++++ nav2_msgs/CMakeLists.txt | 1 + nav2_msgs/srv/SetInitialPose.srv | 3 +++ 4 files changed, 29 insertions(+) create mode 100644 nav2_msgs/srv/SetInitialPose.srv diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp index 2360d09c253..030a85f38cd 100644 --- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp +++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp @@ -35,6 +35,7 @@ #include "nav2_amcl/sensors/laser/laser.hpp" #include "nav2_msgs/msg/particle.hpp" #include "nav2_msgs/msg/particle_cloud.hpp" +#include "nav2_msgs/srv/set_initial_pose.hpp" #include "nav_msgs/srv/set_map.hpp" #include "sensor_msgs/msg/laser_scan.hpp" #include "std_srvs/srv/empty.hpp" @@ -210,6 +211,16 @@ class AmclNode : public nav2_util::LifecycleNode const std::shared_ptr request, std::shared_ptr response); + // service server for providing an initial pose guess + rclcpp::Service::SharedPtr initial_guess_srv_; + /* + * @brief Service callback for an initial pose guess request + */ + void initialPoseReceivedSrv( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + // Let amcl update samples without requiring motion rclcpp::Service::SharedPtr nomotion_update_srv_; /* diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 6f2415cf505..b311b7ac5e9 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -325,6 +325,7 @@ AmclNode::on_cleanup(const rclcpp_lifecycle::State & /*state*/) // Get rid of the inputs first (services and message filter input), so we // don't continue to process incoming messages global_loc_srv_.reset(); + initial_guess_srv_.reset(); nomotion_update_srv_.reset(); executor_thread_.reset(); // to make sure initial_pose_sub_ completely exit initial_pose_sub_.reset(); @@ -495,6 +496,15 @@ AmclNode::globalLocalizationCallback( pf_init_ = false; } +void +AmclNode::initialPoseReceivedSrv( + const std::shared_ptr/*request_header*/, + const std::shared_ptr req, + std::shared_ptr/*res*/) +{ + initialPoseReceived(std::make_shared(req->pose)); +} + // force nomotion updates (amcl updating without requiring motion) void AmclNode::nomotionUpdateCallback( @@ -1544,6 +1554,10 @@ AmclNode::initServices() "reinitialize_global_localization", std::bind(&AmclNode::globalLocalizationCallback, this, _1, _2, _3)); + initial_guess_srv_ = create_service( + "set_initial_pose", + std::bind(&AmclNode::initialPoseReceivedSrv, this, _1, _2, _3)); + nomotion_update_srv_ = create_service( "request_nomotion_update", std::bind(&AmclNode::nomotionUpdateCallback, this, _1, _2, _3)); diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index 206ae86322b..12637abca6a 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -38,6 +38,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/ManageLifecycleNodes.srv" "srv/LoadMap.srv" "srv/SaveMap.srv" + "srv/SetInitialPose.srv" "action/AssistedTeleop.action" "action/BackUp.action" "action/ComputePathToPose.action" diff --git a/nav2_msgs/srv/SetInitialPose.srv b/nav2_msgs/srv/SetInitialPose.srv new file mode 100644 index 00000000000..ec1e50b65f1 --- /dev/null +++ b/nav2_msgs/srv/SetInitialPose.srv @@ -0,0 +1,3 @@ +geometry_msgs/PoseWithCovarianceStamped pose +--- + From 914d881f6e154d8d7bb1975892d4642eaf4c7366 Mon Sep 17 00:00:00 2001 From: Antonio Park Date: Tue, 19 Mar 2024 22:11:29 +0900 Subject: [PATCH 30/36] Move lines for pre-computation to outside a loop (#4191) Signed-off-by: Kyungsik Park --- .../sensors/laser/likelihood_field_model.cpp | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/nav2_amcl/src/sensors/laser/likelihood_field_model.cpp b/nav2_amcl/src/sensors/laser/likelihood_field_model.cpp index 491e64ecf1e..1148c2828db 100644 --- a/nav2_amcl/src/sensors/laser/likelihood_field_model.cpp +++ b/nav2_amcl/src/sensors/laser/likelihood_field_model.cpp @@ -53,6 +53,17 @@ LikelihoodFieldModel::sensorFunction(LaserData * data, pf_sample_set_t * set) self = reinterpret_cast(data->laser); + // Pre-compute a couple of things + double z_hit_denom = 2 * self->sigma_hit_ * self->sigma_hit_; + double z_rand_mult = 1.0 / data->range_max; + + step = (data->range_count - 1) / (self->max_beams_ - 1); + + // Step size must be at least 1 + if (step < 1) { + step = 1; + } + total_weight = 0.0; // Compute the sample weights @@ -65,17 +76,6 @@ LikelihoodFieldModel::sensorFunction(LaserData * data, pf_sample_set_t * set) p = 1.0; - // Pre-compute a couple of things - double z_hit_denom = 2 * self->sigma_hit_ * self->sigma_hit_; - double z_rand_mult = 1.0 / data->range_max; - - step = (data->range_count - 1) / (self->max_beams_ - 1); - - // Step size must be at least 1 - if (step < 1) { - step = 1; - } - for (i = 0; i < data->range_count; i += step) { obs_range = data->ranges[i][0]; obs_bearing = data->ranges[i][1]; From 2eb99e31ab747662932033b5d29950b6cdc7f690 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 19 Mar 2024 17:00:11 +0100 Subject: [PATCH 31/36] Fix typo (#4196) * Fix BT.CPP import Signed-off-by: Tony Najjar * Update README.md --------- Signed-off-by: Tony Najjar --- nav2_smac_planner/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_smac_planner/README.md b/nav2_smac_planner/README.md index 6e92209068d..a9f59c4e6a0 100644 --- a/nav2_smac_planner/README.md +++ b/nav2_smac_planner/README.md @@ -134,7 +134,7 @@ planner_server: max_iterations: 1000 w_smooth: 0.3 w_data: 0.2 - tolerance: 1e-10 + tolerance: 1.0e-10 do_refinement: true # Whether to recursively run the smoother 3 times on the results from prior runs to refine the results further ``` From 12292d17c5db73b1fb387d30c0c426415ff2defe Mon Sep 17 00:00:00 2001 From: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Date: Wed, 20 Mar 2024 22:14:15 +0100 Subject: [PATCH 32/36] Update footprint iif changed (#4193) Signed-off-by: Brice --- .../include/nav2_smac_planner/analytic_expansion.hpp | 2 +- nav2_smac_planner/src/analytic_expansion.cpp | 2 +- nav2_smac_planner/src/collision_checker.cpp | 1 + nav2_smac_planner/src/smac_planner_hybrid.cpp | 4 ++++ nav2_smac_planner/src/smac_planner_lattice.cpp | 4 ++++ 5 files changed, 11 insertions(+), 2 deletions(-) diff --git a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp index 10c4224a077..9dc317dd8a4 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp @@ -73,7 +73,7 @@ class AnalyticExpansion * @brief Sets the collision checker and costmap to use in expansion validation * @param collision_checker Collision checker to use */ - void setCollisionChecker(GridCollisionChecker * & collision_checker); + void setCollisionChecker(GridCollisionChecker * collision_checker); /** * @brief Attempt an analytic path completion diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp index 8dcc1314357..e14033d0f1c 100644 --- a/nav2_smac_planner/src/analytic_expansion.cpp +++ b/nav2_smac_planner/src/analytic_expansion.cpp @@ -41,7 +41,7 @@ AnalyticExpansion::AnalyticExpansion( template void AnalyticExpansion::setCollisionChecker( - GridCollisionChecker * & collision_checker) + GridCollisionChecker * collision_checker) { _collision_checker = collision_checker; } diff --git a/nav2_smac_planner/src/collision_checker.cpp b/nav2_smac_planner/src/collision_checker.cpp index 2f23442097a..c9ce030fab5 100644 --- a/nav2_smac_planner/src/collision_checker.cpp +++ b/nav2_smac_planner/src/collision_checker.cpp @@ -66,6 +66,7 @@ void GridCollisionChecker::setFootprint( return; } + oriented_footprints_.clear(); oriented_footprints_.reserve(angles_.size()); double sin_th, cos_th; geometry_msgs::msg::Point new_pt; diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index b0bdf042dc2..592ec40f1e9 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -338,6 +338,10 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( } // Set collision checker and costmap information + _collision_checker.setFootprint( + _costmap_ros->getRobotFootprint(), + _costmap_ros->getUseRadius(), + findCircumscribedCost(_costmap_ros)); _a_star->setCollisionChecker(&_collision_checker); // Set starting point, in A* bin search coordinates diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index cd98cf64408..87f2e365639 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -274,6 +274,10 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( std::unique_lock lock(*(_costmap->getMutex())); // Set collision checker and costmap information + _collision_checker.setFootprint( + _costmap_ros->getRobotFootprint(), + _costmap_ros->getUseRadius(), + findCircumscribedCost(_costmap_ros)); _a_star->setCollisionChecker(&_collision_checker); // Set starting point, in A* bin search coordinates From 1ef462e71d12fd3e4ffb45e332a7d9883e921921 Mon Sep 17 00:00:00 2001 From: nelson Date: Thu, 21 Mar 2024 20:44:39 +0000 Subject: [PATCH 33/36] fix missing param declare (#4203) Signed-off-by: nelson --- .../include/nav2_behavior_tree/bt_action_server_impl.hpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index 5e005fc610d..12dffd28865 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -64,6 +64,9 @@ BtActionServer::BtActionServer( if (!node->has_parameter("action_server_result_timeout")) { node->declare_parameter("action_server_result_timeout", 900.0); } + if (!node->has_parameter("wait_for_service_timeout")) { + node->declare_parameter("wait_for_service_timeout", 1000); + } std::vector error_code_names = { "follow_path_error_code", From 5bf14fc81ad81b815eecf4f153a3ba3cc60e5675 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 22 Mar 2024 16:56:27 -0700 Subject: [PATCH 34/36] Revert "nav2_controller: add loop rate log (#4171)" (#4210) This reverts commit 47374622dee01a27e5f9b8ae08f3d19a15de9b3a. --- nav2_controller/src/controller_server.cpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 85aae171cc7..fa25e0b1379 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -419,7 +419,6 @@ void ControllerServer::computeControl() { std::lock_guard lock(dynamic_params_lock_); - auto start_time = this->now(); RCLCPP_INFO(get_logger(), "Received a goal, begin computing control effort."); try { @@ -480,12 +479,10 @@ void ControllerServer::computeControl() break; } - auto cycle_duration = this->now() - start_time; if (!loop_rate.sleep()) { RCLCPP_WARN( - get_logger(), - "Control loop missed its desired rate of %.4f Hz. Current loop rate is %.4f Hz.", - controller_frequency_, 1 / cycle_duration.seconds()); + get_logger(), "Control loop missed its desired rate of %.4fHz", + controller_frequency_); } } } catch (nav2_core::InvalidController & e) { From 46ff0ecc4cc7d42cc92477e6159b29d69fd23050 Mon Sep 17 00:00:00 2001 From: Michael Wrock Date: Mon, 25 Mar 2024 11:44:29 -0700 Subject: [PATCH 35/36] Add uint suffix (#4213) --- nav2_graceful_controller/test/test_graceful_controller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_graceful_controller/test/test_graceful_controller.cpp b/nav2_graceful_controller/test/test_graceful_controller.cpp index 26e929a5805..fd5f986c76a 100644 --- a/nav2_graceful_controller/test/test_graceful_controller.cpp +++ b/nav2_graceful_controller/test/test_graceful_controller.cpp @@ -814,7 +814,7 @@ TEST(GracefulControllerTest, pruningPlan) { // Check results: the plan should be pruned auto transformed_plan = controller->transformGlobalPlan(robot_pose); - EXPECT_EQ(transformed_plan.poses.size(), 3); + EXPECT_EQ(transformed_plan.poses.size(), 3u); } TEST(GracefulControllerTest, pruningPlanOutsideCostmap) { @@ -880,7 +880,7 @@ TEST(GracefulControllerTest, pruningPlanOutsideCostmap) { // Check results: the plan should be pruned auto transformed_plan = controller->transformGlobalPlan(robot_pose); - EXPECT_EQ(transformed_plan.poses.size(), 2); + EXPECT_EQ(transformed_plan.poses.size(), 2u); } TEST(GracefulControllerTest, computeVelocityCommandRotate) { From a1997db142bef4aff898ab72fc250adba09825e6 Mon Sep 17 00:00:00 2001 From: Johannes Huemer Date: Mon, 25 Mar 2024 19:45:14 +0100 Subject: [PATCH 36/36] Enable reloading BT xml file with same name (#4209) * Let BtActionServer overwrite xml Co-authored-by: Johannes Huemer Signed-off-by: Johannes Huemer Signed-off-by: Christoph Froehlich * Make a ROS parameter for it Signed-off-by: Christoph Froehlich * Rename flag to always reload BT xml file Signed-off-by: Johannes Huemer --------- Signed-off-by: Johannes Huemer Signed-off-by: Christoph Froehlich Co-authored-by: Christoph Froehlich --- .../include/nav2_behavior_tree/bt_action_server.hpp | 3 +++ .../include/nav2_behavior_tree/bt_action_server_impl.hpp | 8 ++++++-- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp index 331ca237c8f..2b323a67985 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp @@ -251,6 +251,9 @@ class BtActionServer // The timeout value for waiting for a service to response std::chrono::milliseconds wait_for_service_timeout_; + // should the BT be reloaded even if the same xml filename is requested? + bool always_reload_bt_xml_ = false; + // User-provided callbacks OnGoalReceivedCallback on_goal_received_callback_; OnLoopCallback on_loop_callback_; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index 12dffd28865..deaeea1bdc2 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -64,6 +64,9 @@ BtActionServer::BtActionServer( if (!node->has_parameter("action_server_result_timeout")) { node->declare_parameter("action_server_result_timeout", 900.0); } + if (!node->has_parameter("always_reload_bt_xml")) { + node->declare_parameter("always_reload_bt_xml", false); + } if (!node->has_parameter("wait_for_service_timeout")) { node->declare_parameter("wait_for_service_timeout", 1000); } @@ -164,6 +167,7 @@ bool BtActionServer::on_configure() int wait_for_service_timeout; node->get_parameter("wait_for_service_timeout", wait_for_service_timeout); wait_for_service_timeout_ = std::chrono::milliseconds(wait_for_service_timeout); + node->get_parameter("always_reload_bt_xml", always_reload_bt_xml_); // Get error code id names to grab off of the blackboard error_code_names_ = node->get_parameter("error_code_names").as_string_array(); @@ -223,8 +227,8 @@ bool BtActionServer::loadBehaviorTree(const std::string & bt_xml_filena // Empty filename is default for backward compatibility auto filename = bt_xml_filename.empty() ? default_bt_xml_filename_ : bt_xml_filename; - // Use previous BT if it is the existing one - if (current_bt_xml_filename_ == filename) { + // Use previous BT if it is the existing one and always reload flag is not set to true + if (!always_reload_bt_xml_ && current_bt_xml_filename_ == filename) { RCLCPP_DEBUG(logger_, "BT will not be reloaded as the given xml is already loaded"); return true; }