From 77c1b742e959dda02acf6830c2a814f2278bb4a4 Mon Sep 17 00:00:00 2001 From: Homalozoa X <21300069+paeunt@users.noreply.github.com> Date: Sat, 2 Jan 2021 04:23:23 +0800 Subject: [PATCH 01/14] Fix deprecated usage of FutureReturnCode::SUCCESS (#2140) Use rclcpp::FutureReturnCode::SUCCESS replace rclcpp::executor::FutureReturnCode::SUCCESS --- .../include/nav2_behavior_tree/bt_action_node.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 082a830076..eed3626601 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 @@ -231,7 +231,7 @@ class BtActionNode : public BT::ActionNodeBase auto future_goal_handle = action_client_->async_send_goal(goal_, send_goal_options); if (rclcpp::spin_until_future_complete(node_, future_goal_handle, server_timeout_) != - rclcpp::executor::FutureReturnCode::SUCCESS) + rclcpp::FutureReturnCode::SUCCESS) { throw std::runtime_error("send_goal failed"); } From 63b9510ddf16abe2b6cc0f21c4d4824ed4e99331 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 24 Feb 2021 16:22:55 -0800 Subject: [PATCH 02/14] backporting to foxy checking goal index on backchecks (#2202) --- smac_planner/src/a_star.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/smac_planner/src/a_star.cpp b/smac_planner/src/a_star.cpp index e3e3190bff..b9668810df 100644 --- a/smac_planner/src/a_star.cpp +++ b/smac_planner/src/a_star.cpp @@ -411,7 +411,7 @@ AStarAlgorithm::NodePtr AStarAlgorithm::getAnalyticPath( prev = node; for (const auto & node_pose : possible_nodes) { const auto & n = node_pose.first; - if (!n->wasVisited()) { + if (!n->wasVisited() && n->getIndex() != _goal->getIndex()) { // Make sure this node has not been visited by the regular algorithm. // If it has been, there is the (slight) chance that it is in the path we are expanding // from, so we should skip it. From af8a857699c6f9f64cfccb7d98b60230ddc3ee94 Mon Sep 17 00:00:00 2001 From: Albert Yen Date: Thu, 25 Feb 2021 11:25:14 -0800 Subject: [PATCH 03/14] Fix recovery action collision check. (#2193) * Fix recovery action collision check. * Fix linting issue. --- nav2_recoveries/plugins/back_up.cpp | 5 +++-- nav2_recoveries/plugins/spin.cpp | 3 ++- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/nav2_recoveries/plugins/back_up.cpp b/nav2_recoveries/plugins/back_up.cpp index 585d8a7741..b429c8bdcf 100644 --- a/nav2_recoveries/plugins/back_up.cpp +++ b/nav2_recoveries/plugins/back_up.cpp @@ -120,11 +120,12 @@ bool BackUp::isCollisionFree( double sim_position_change; const double diff_dist = abs(command_x_) - distance; const int max_cycle_count = static_cast(cycle_frequency_ * simulate_ahead_time_); + geometry_msgs::msg::Pose2D init_pose = pose2d; while (cycle_count < max_cycle_count) { sim_position_change = cmd_vel->linear.x * (cycle_count / cycle_frequency_); - pose2d.x += sim_position_change * cos(pose2d.theta); - pose2d.y += sim_position_change * sin(pose2d.theta); + pose2d.x = init_pose.x + sim_position_change * cos(init_pose.theta); + pose2d.y = init_pose.y + sim_position_change * sin(init_pose.theta); cycle_count++; if (diff_dist - abs(sim_position_change) <= 0.) { diff --git a/nav2_recoveries/plugins/spin.cpp b/nav2_recoveries/plugins/spin.cpp index e24be17d89..103623b15c 100644 --- a/nav2_recoveries/plugins/spin.cpp +++ b/nav2_recoveries/plugins/spin.cpp @@ -150,10 +150,11 @@ bool Spin::isCollisionFree( int cycle_count = 0; double sim_position_change; const int max_cycle_count = static_cast(cycle_frequency_ * simulate_ahead_time_); + geometry_msgs::msg::Pose2D init_pose = pose2d; while (cycle_count < max_cycle_count) { sim_position_change = cmd_vel->angular.z * (cycle_count / cycle_frequency_); - pose2d.theta += sim_position_change; + pose2d.theta = init_pose.theta + sim_position_change; cycle_count++; if (abs(relative_yaw) - abs(sim_position_change) <= 0.) { From 52abee596117441311ce8fcb625fc2504e345ea2 Mon Sep 17 00:00:00 2001 From: Albert Yen Date: Thu, 25 Feb 2021 17:33:19 -0800 Subject: [PATCH 04/14] Fix back up action behavior tree node name issue (#2206) --- nav2_bringup/bringup/params/nav2_params.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_bringup/bringup/params/nav2_params.yaml b/nav2_bringup/bringup/params/nav2_params.yaml index d0e1ad4c34..07817d4318 100644 --- a/nav2_bringup/bringup/params/nav2_params.yaml +++ b/nav2_bringup/bringup/params/nav2_params.yaml @@ -264,10 +264,10 @@ recoveries_server: costmap_topic: local_costmap/costmap_raw footprint_topic: local_costmap/published_footprint cycle_frequency: 10.0 - recovery_plugins: ["spin", "backup", "wait"] + recovery_plugins: ["spin", "back_up", "wait"] spin: plugin: "nav2_recoveries/Spin" - backup: + back_up: plugin: "nav2_recoveries/BackUp" wait: plugin: "nav2_recoveries/Wait" From 64bf1463fe9fc92f99f8722624a4920d578b661f Mon Sep 17 00:00:00 2001 From: MarcoLm993 <65171491+MarcoLm993@users.noreply.github.com> Date: Wed, 3 Mar 2021 19:22:48 +0100 Subject: [PATCH 05/14] Export nav2_bt_navigator library and dependencies (#2212) Signed-off-by: Marco Lampacrescia --- nav2_bt_navigator/CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/nav2_bt_navigator/CMakeLists.txt b/nav2_bt_navigator/CMakeLists.txt index da362c9106..22d8f4884c 100644 --- a/nav2_bt_navigator/CMakeLists.txt +++ b/nav2_bt_navigator/CMakeLists.txt @@ -82,5 +82,6 @@ if(BUILD_TESTING) endif() ament_export_include_directories(include) - +ament_export_libraries(${library_name}) +ament_export_dependencies(${dependencies}) ament_package() From 41996f409fcd7b005cf779e0376273cee3de6d76 Mon Sep 17 00:00:00 2001 From: Victor Lopez <3469405+v-lopez@users.noreply.github.com> Date: Wed, 3 Mar 2021 19:27:51 +0100 Subject: [PATCH 06/14] Optional transient map saver (#2215) * Added transient local subscription qos profile parameter to map saver (#1871) * Added transient local subscription qos profile parameter to map saver * Made transient local default true * Fixed linter problem * switched back house world to waffle model * Make transient map subscribe backwards compatible for foxy Co-authored-by: Michael Equi <32988490+Michael-Equi@users.noreply.github.com> --- nav2_bringup/bringup/params/nav2_params.yaml | 1 + nav2_map_server/include/nav2_map_server/map_saver.hpp | 2 ++ nav2_map_server/src/map_saver/map_saver.cpp | 11 ++++++++++- 3 files changed, 13 insertions(+), 1 deletion(-) diff --git a/nav2_bringup/bringup/params/nav2_params.yaml b/nav2_bringup/bringup/params/nav2_params.yaml index 07817d4318..bbfc35d969 100644 --- a/nav2_bringup/bringup/params/nav2_params.yaml +++ b/nav2_bringup/bringup/params/nav2_params.yaml @@ -243,6 +243,7 @@ map_saver: save_map_timeout: 5000 free_thresh_default: 0.25 occupied_thresh_default: 0.65 + map_subscribe_transient_local: False planner_server: ros__parameters: diff --git a/nav2_map_server/include/nav2_map_server/map_saver.hpp b/nav2_map_server/include/nav2_map_server/map_saver.hpp index 3aa4e2e233..9c9b57d292 100644 --- a/nav2_map_server/include/nav2_map_server/map_saver.hpp +++ b/nav2_map_server/include/nav2_map_server/map_saver.hpp @@ -103,6 +103,8 @@ class MapSaver : public nav2_util::LifecycleNode // Default values for map thresholds double free_thresh_default_; double occupied_thresh_default_; + // param for handling QoS configuration + bool map_subscribe_transient_local_; // The name of the service for saving a map from topic const std::string save_map_service_name_{"save_map"}; diff --git a/nav2_map_server/src/map_saver/map_saver.cpp b/nav2_map_server/src/map_saver/map_saver.cpp index 7555ebd632..563e0d5290 100644 --- a/nav2_map_server/src/map_saver/map_saver.cpp +++ b/nav2_map_server/src/map_saver/map_saver.cpp @@ -50,6 +50,8 @@ MapSaver::MapSaver() free_thresh_default_ = declare_parameter("free_thresh_default", 0.25), occupied_thresh_default_ = declare_parameter("occupied_thresh_default", 0.65); + // false only of foxy for backwards compatibility + map_subscribe_transient_local_ = declare_parameter("map_subscribe_transient_local", false); } MapSaver::~MapSaver() @@ -173,8 +175,15 @@ bool MapSaver::saveMapTopicToFile( // Add new subscription for incoming map topic. // Utilizing local rclcpp::Node (rclcpp_node_) from nav2_util::LifecycleNode // as a map listener. + rclcpp::QoS map_qos = rclcpp::SystemDefaultsQoS(); // initialize to default + if (map_subscribe_transient_local_) { + map_qos = rclcpp::QoS(10); + map_qos.transient_local(); + map_qos.reliable(); + map_qos.keep_last(1); + } auto map_sub = rclcpp_node_->create_subscription( - map_topic_loc, rclcpp::SystemDefaultsQoS(), mapCallback); + map_topic_loc, map_qos, mapCallback); rclcpp::Time start_time = now(); while (rclcpp::ok()) { From 3befe9aa1c2546f2178c7ced6fe55fc3ba56642a Mon Sep 17 00:00:00 2001 From: Victor Lopez <3469405+v-lopez@users.noreply.github.com> Date: Thu, 18 Mar 2021 01:07:40 +0100 Subject: [PATCH 07/14] Add has_node_params launch utility (#2257) --- nav2_common/nav2_common/launch/__init__.py | 1 + .../nav2_common/launch/has_node_params.py | 61 +++++++++++++++++++ 2 files changed, 62 insertions(+) create mode 100644 nav2_common/nav2_common/launch/has_node_params.py diff --git a/nav2_common/nav2_common/launch/__init__.py b/nav2_common/nav2_common/launch/__init__.py index 1f0638f81a..c25fadb220 100644 --- a/nav2_common/nav2_common/launch/__init__.py +++ b/nav2_common/nav2_common/launch/__init__.py @@ -12,5 +12,6 @@ # See the License for the specific language governing permissions and # limitations under the License. +from .has_node_params import HasNodeParams from .rewritten_yaml import RewrittenYaml from .replace_string import ReplaceString diff --git a/nav2_common/nav2_common/launch/has_node_params.py b/nav2_common/nav2_common/launch/has_node_params.py new file mode 100644 index 0000000000..21b8a32f9e --- /dev/null +++ b/nav2_common/nav2_common/launch/has_node_params.py @@ -0,0 +1,61 @@ +# Copyright (c) 2021 PAL Robotics S.L. +# +# 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. + +from typing import List +from typing import Text + +import yaml +import launch + + +class HasNodeParams(launch.Substitution): + """ + Substitution that checks if a param file contains parameters for a node. + + Used in launch system + """ + + def __init__(self, + source_file: launch.SomeSubstitutionsType, + node_name: Text) -> None: + super().__init__() + """ + Construct the substitution + + :param: source_file the parameter YAML file + :param: node_name the name of the node to check + """ + + # import here to avoid loop + from launch.utilities import normalize_to_list_of_substitutions + self.__source_file = normalize_to_list_of_substitutions(source_file) + self.__node_name = node_name + + @property + def name(self) -> List[launch.Substitution]: + """Getter for name.""" + return self.__source_file + + def describe(self) -> Text: + """Return a description of this substitution as a string.""" + return '' + + def perform(self, context: launch.LaunchContext) -> Text: + yaml_filename = launch.utilities.perform_substitutions( + context, self.name) + data = yaml.safe_load(open(yaml_filename, 'r')) + + if self.__node_name in data.keys(): + return "True" + return "False" From 9831d83f5dfce01d5842c867a0540dede4dcabf9 Mon Sep 17 00:00:00 2001 From: shonigmann Date: Fri, 2 Apr 2021 17:01:59 -0700 Subject: [PATCH 08/14] corrected backup plugin name for multirobot params (#2288) Co-authored-by: Simon Honigmann --- nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml | 2 +- nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml index 26b8a54274..4d1e964381 100644 --- a/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml +++ b/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml @@ -226,7 +226,7 @@ recoveries_server: costmap_topic: local_costmap/costmap_raw footprint_topic: local_costmap/published_footprint cycle_frequency: 10.0 - recovery_plugins: ["spin", "back_up", "wait"] + recovery_plugins: ["spin", "backup", "wait"] spin: plugin: "nav2_recoveries/Spin" backup: diff --git a/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml index b5d0700074..39d1ff0e45 100644 --- a/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml +++ b/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml @@ -226,7 +226,7 @@ recoveries_server: costmap_topic: local_costmap/costmap_raw footprint_topic: local_costmap/published_footprint cycle_frequency: 10.0 - recovery_plugins: ["spin", "back_up", "wait"] + recovery_plugins: ["spin", "backup", "wait"] spin: plugin: "nav2_recoveries/Spin" backup: From 5c9cb96e0c887a6a51506342b8fc50d2ab34643c Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 5 Apr 2021 16:11:11 -0700 Subject: [PATCH 09/14] foxy Sync 5.1 (#2291) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * merge conflict * Add groot monitoring behavior tree visualization (#1958) * include ZMQ publisher for Groot very plain integration, should be made optionally through a launch parameter * fix Groot crashing finding custom nodes in monitor mode straight forward working fix. The manifest was missing, so Groot searched custom node IDs that it did not have. This is implemented correctly directly in BT.CPP V3 and should be used instead of an implementation in nav2_bt_engine * refactor buildTreeFromText to createTreeFromText as in BT.CPP v3 * forward XML to createTreeFromText from BT.CPP v3 factory function * Add createTreeFromFile forware to BT-factory function * fix createTreeFromFile args.. * add personal copyright I think this is okay for finding a nasty bug.. :) * move creating ZMQ Publisher from run to dedicated function this way the ZMQ Publisher ca be added to individual trees within the same factory. Should be important for switching trees (XML files) * Add parameter for Groot Monitoring - default true. Also cleanup ZMQ * Move haltAllActions() Implementation from .hpp to .cpp * update Copyright in hpp of BT-engine * make linters happy.. :) * Update Groot parameter naming and chg default=0 * rename resetZMQGrootMonitor -> resetGrootMonitor * add parameter to nav2_params.yaml - default = false * add ZMQ params and logic for server/pub ports * Fix RewrittenYaml ignoring Integers Integers where converted as floats before which crashes get_parameter.. fun thing.... * add launch based tests for params and ZMQ * Activate Dijkstra and A* switching tests, thanks to RewrittenYaml * add pyzmq==19.0.2 via pip3 to CI test_workspace * make flake8 linter happy * make cpp linters happy * add personal copyright * add GoalUpdated BT node description in order to view the full default BT only affects editor mode of Groot and not live monitoring * make linter happy (unused import) * remove unused groot-port replacement functions in test_system_launch.py * add groot parameters to params.md * get reloading BTs to work nicely with Groot * pretty space for smac :) * switch from unsinged to uint16_t * fix converting string into float or int * Revert "add pyzmq==19.0.2 via pip3 to CI test_workspace" This reverts commit 7bca08121c88db3763771911e3c6b4c6f4f8ddeb. * Switch to 4 spaces indent and other linter stuff for RewrittenYaml * removed prints in test_system_launch.py * linter stuff * add python-zmq as test_depend in package.xml (instead of .CI_conf) * enable groot monitoring by default * remove ZMQ from naming (function / variable) * remove variable zmq ports from testing scripts * remove default ports in BT_engine, as they are set through (def-)params * Remove complete test for "dynamic" ZMQ ports testing * fix python-zmq depend location * fix style * swap missing Groot to default True * fix rosdep zmq + flake8 fixes in system_tests * remove debug logs + c_str() * remove final debug_log * return failure on plugin failure (#2119) * Move voxel publisher activation into conditional that its on * fix boundary point exclusion in convexFillCells (#2161) * Regulated pure pursuit controller (#2152) * regulated pure pursuit migration commit * adding speed limit API * adding review comments + adding rotate to goal heading * adding test dir * add some initial tests * more tests * remove old comment * improve readme * fix CI * first attempt at changing algos in tests * allowing full path parameter substitutions * adding integration tests * enable SMAC testing too with new changes * swap algos * revert * Update angular velocity after constraining linear velocity (#2165) This ensures the robot moves towards the lookahead point more closely. If the angular velocity is not updated, then the robot tries to take cuts while turning, which could lead to collisions when near obstacles Signed-off-by: Shrijit Singh * Update cost scaling heuristic to vary speed linearly with distance (#2164) * Update cost scaling to vary linearly with distance instead of relying on costmap cost Signed-off-by: Shrijit Singh * Resolve suggested changes Signed-off-by: Shrijit Singh * Add documentation for cost scaling parameters Signed-off-by: Shrijit Singh * Improve parameter descriptions Signed-off-by: Shrijit Singh * Comment cost scaling tests since layered costmap is not initialized A valid layered costmap reference is needed to get the inscribed radius Signed-off-by: Shrijit Singh Co-authored-by: Shrijit Singh * Updating example yaml to include extra params (#2183) * Fixing control_frequency to controller_frequency typo (#2182) * Write doxygen for navfn (#2184) * Write doxygen for navfn * Remove forward slashes * expose dwb's shorten_transformed_plan param * Adding RPP to metapackage.xml * [NavFn] Make the 3 parameters changeable at runtime (#2181) * make the 3 params changeable at runtime * use parameter events callbacks * doxygen * lint * Install test_updown to lib/ (#2208) * Remove optimization check on carrot, incorrect optimization (#2209) * [RPP] Remove dependency on collision checking to carrot location (#2211) * Remove dependency on collision checking to carrot location * Fix i removal * changing API to be consistent with collision updates * fix typo in regulated pure pursuit readme (#2228) * Rviz state machine waypoint follower updates (#2227) * working on canceling state machine for waypoint mode * fixing cancelation logic * fix linting isue * adding cherry pick fixes Co-authored-by: Sarthak Mittal Co-authored-by: Florian Gramß <6034322+gramss@users.noreply.github.com> Co-authored-by: ChristofDubs Co-authored-by: Shrijit Singh Co-authored-by: Phone Thiha Kyaw Co-authored-by: simutisernestas <35775651+simutisernestas@users.noreply.github.com> Co-authored-by: G.Doisy Co-authored-by: Uladzslau <79460842+Uladzslau@users.noreply.github.com> Co-authored-by: Erwin Lejeune --- doc/parameters/param_list.md | 4 + .../behavior_tree_engine.hpp | 32 +- nav2_behavior_tree/nav2_tree_nodes.xml | 2 + .../src/behavior_tree_engine.cpp | 52 +- nav2_bringup/bringup/params/nav2_params.yaml | 3 + nav2_bt_navigator/src/bt_navigator.cpp | 31 +- .../nav2_common/launch/rewritten_yaml.py | 273 +++++---- nav2_controller/src/nav2_controller.cpp | 3 + nav2_costmap_2d/plugins/voxel_layer.cpp | 3 +- nav2_costmap_2d/src/costmap_2d.cpp | 2 +- .../include/dwb_core/dwb_local_planner.hpp | 6 + .../dwb_core/src/dwb_local_planner.cpp | 13 +- .../include/nav2_navfn_planner/navfn.hpp | 10 + .../nav2_navfn_planner/navfn_planner.hpp | 115 +++- nav2_navfn_planner/src/navfn_planner.cpp | 35 ++ nav2_planner/src/planner_server.cpp | 1 + .../nav2_recoveries/recovery_server.hpp | 2 +- nav2_recoveries/src/recovery_server.cpp | 10 +- .../CMakeLists.txt | 70 +++ .../README.md | 147 +++++ .../doc/lookahead_algorithm.png | Bin 0 -> 108322 bytes .../regulated_pure_pursuit_controller.hpp | 251 ++++++++ ...nav2_regulated_pure_pursuit_controller.xml | 10 + .../package.xml | 29 + .../src/regulated_pure_pursuit_controller.cpp | 563 ++++++++++++++++++ .../test/CMakeLists.txt | 10 + .../test/test_regulated_pp.cpp | 324 ++++++++++ nav2_rviz_plugins/src/nav2_panel.cpp | 33 +- nav2_system_tests/package.xml | 1 + nav2_system_tests/src/system/CMakeLists.txt | 27 +- .../src/system/test_system_launch.py | 25 +- nav2_system_tests/src/system/tester_node.py | 97 +++ nav2_system_tests/src/updown/CMakeLists.txt | 2 +- navigation2/package.xml | 1 + .../smac_planner/costmap_downsampler.hpp | 26 +- .../include/smac_planner/smac_planner.hpp | 3 +- .../include/smac_planner/smac_planner_2d.hpp | 3 +- smac_planner/src/costmap_downsampler.cpp | 33 +- smac_planner/src/smac_planner.cpp | 106 ++-- smac_planner/src/smac_planner_2d.cpp | 88 +-- .../test/test_costmap_downsampler.cpp | 13 +- 41 files changed, 2142 insertions(+), 317 deletions(-) create mode 100644 nav2_regulated_pure_pursuit_controller/CMakeLists.txt create mode 100644 nav2_regulated_pure_pursuit_controller/README.md create mode 100644 nav2_regulated_pure_pursuit_controller/doc/lookahead_algorithm.png create mode 100644 nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp create mode 100644 nav2_regulated_pure_pursuit_controller/nav2_regulated_pure_pursuit_controller.xml create mode 100644 nav2_regulated_pure_pursuit_controller/package.xml create mode 100644 nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp create mode 100644 nav2_regulated_pure_pursuit_controller/test/CMakeLists.txt create mode 100644 nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp diff --git a/doc/parameters/param_list.md b/doc/parameters/param_list.md index ecc954dedc..9278a512a5 100644 --- a/doc/parameters/param_list.md +++ b/doc/parameters/param_list.md @@ -5,6 +5,9 @@ | Parameter | Default | Description | | ----------| --------| ------------| | default_bt_xml_filename | N/A | path to the default behavior tree XML description | +| enable_groot_monitoring | True | enable Groot live monitoring of the behavior tree | +| groot_zmq_publisher_port | 1666 | change port of the zmq publisher needed for groot | +| groot_zmq_server_port | 1667 | change port of the zmq server needed for groot | | plugin_lib_names | ["nav2_compute_path_to_pose_action_bt_node", "nav2_follow_path_action_bt_node", "nav2_back_up_action_bt_node", "nav2_spin_action_bt_node", "nav2_wait_action_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_reinitialize_global_localization_service_bt_node", "nav2_rate_controller_bt_node", "nav2_distance_controller_bt_node", "nav2_recovery_node_bt_node", "nav2_pipeline_sequence_bt_node", "nav2_round_robin_node_bt_node", "nav2_transform_available_condition_bt_node"] | list of behavior tree node shared libraries | | transform_tolerance | 0.1 | TF transform tolerance | | global_frame | "map" | Reference frame | @@ -237,6 +240,7 @@ When `controller_plugins`\`progress_checker_plugin`\`goal_checker_plugin` parame | ``.critics | N/A | List of critic plugins to use | | ``.default_critic_namespaces | ["dwb_critics"] | Namespaces to load critics in | | ``.prune_plan | true | Whether to prune the path of old, passed points | +| ``.shorten_transformed_plan | true | Determines whether we will pass the full plan on to the critics | | ``.prune_distance | 1.0 | Distance (m) to prune backward until | | ``.debug_trajectory_details | false | Publish debug information | | ``.trajectory_generator_name | "dwb_plugins::StandardTrajectoryGenerator" | Trajectory generator plugin name | 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 ca627bbe7e..61816a9950 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 @@ -1,4 +1,5 @@ // Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Florian Gramss // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -22,6 +23,8 @@ #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" + namespace nav2_behavior_tree { @@ -40,28 +43,29 @@ class BehaviorTreeEngine std::function cancelRequested, std::chrono::milliseconds loopTimeout = std::chrono::milliseconds(10)); - BT::Tree buildTreeFromText( + BT::Tree createTreeFromText( const std::string & xml_string, BT::Blackboard::Ptr blackboard); + BT::Tree createTreeFromFile( + const std::string & file_path, + BT::Blackboard::Ptr blackboard); + + void addGrootMonitoring( + BT::Tree * tree, + uint16_t publisher_port, + uint16_t server_port, + uint16_t max_msg_per_second = 25); + + void resetGrootMonitor(); + // In order to re-run a Behavior Tree, we must be able to reset all nodes to the initial state - void haltAllActions(BT::TreeNode * root_node) - { - // this halt signal should propagate through the entire tree. - root_node->halt(); - - // but, just in case... - auto visitor = [](BT::TreeNode * node) { - if (node->status() == BT::NodeStatus::RUNNING) { - node->halt(); - } - }; - BT::applyRecursiveVisitor(root_node, visitor); - } + void haltAllActions(BT::TreeNode * root_node); protected: // The factory that will be used to dynamically construct the behavior tree BT::BehaviorTreeFactory factory_; + std::unique_ptr groot_monitor_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 94e3c2591b..81df7b4641 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -54,6 +54,8 @@ Parent frame for transform + + diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp index 36b852e61c..93841615d9 100644 --- a/nav2_behavior_tree/src/behavior_tree_engine.cpp +++ b/nav2_behavior_tree/src/behavior_tree_engine.cpp @@ -1,4 +1,5 @@ // Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Florian Gramss // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -21,8 +22,6 @@ #include "rclcpp/rclcpp.hpp" #include "behaviortree_cpp_v3/utils/shared_library.h" -using namespace std::chrono_literals; - namespace nav2_behavior_tree { @@ -62,13 +61,54 @@ BehaviorTreeEngine::run( } BT::Tree -BehaviorTreeEngine::buildTreeFromText( +BehaviorTreeEngine::createTreeFromText( const std::string & xml_string, BT::Blackboard::Ptr blackboard) { - BT::XMLParser p(factory_); - p.loadFromText(xml_string); - return p.instantiateTree(blackboard); + return factory_.createTreeFromText(xml_string, blackboard); +} + +BT::Tree +BehaviorTreeEngine::createTreeFromFile( + const std::string & file_path, + BT::Blackboard::Ptr blackboard) +{ + return factory_.createTreeFromFile(file_path, blackboard); +} + +void +BehaviorTreeEngine::addGrootMonitoring( + BT::Tree * tree, + uint16_t publisher_port, + uint16_t server_port, + uint16_t max_msg_per_second) +{ + // This logger publish status changes using ZeroMQ. Used by Groot + groot_monitor_ = std::make_unique( + *tree, max_msg_per_second, publisher_port, + server_port); +} + +void +BehaviorTreeEngine::resetGrootMonitor() +{ + groot_monitor_.reset(); +} + +// In order to re-run a Behavior Tree, we must be able to reset all nodes to the initial state +void +BehaviorTreeEngine::haltAllActions(BT::TreeNode * root_node) +{ + // this halt signal should propagate through the entire tree. + root_node->halt(); + + // but, just in case... + auto visitor = [](BT::TreeNode * node) { + if (node->status() == BT::NodeStatus::RUNNING) { + node->halt(); + } + }; + BT::applyRecursiveVisitor(root_node, visitor); } } // namespace nav2_behavior_tree diff --git a/nav2_bringup/bringup/params/nav2_params.yaml b/nav2_bringup/bringup/params/nav2_params.yaml index bbfc35d969..bae3aa36c0 100644 --- a/nav2_bringup/bringup/params/nav2_params.yaml +++ b/nav2_bringup/bringup/params/nav2_params.yaml @@ -53,6 +53,9 @@ bt_navigator: global_frame: map robot_base_frame: base_link odom_topic: /odom + enable_groot_monitoring: True + groot_zmq_publisher_port: 1666 + groot_zmq_server_port: 1667 default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml" plugin_lib_names: - nav2_compute_path_to_pose_action_bt_node diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index fcb761322c..afd20df0e6 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include "nav2_util/geometry_utils.hpp" #include "nav2_util/robot_utils.hpp" @@ -68,6 +69,9 @@ BtNavigator::BtNavigator() declare_parameter("global_frame", std::string("map")); declare_parameter("robot_base_frame", std::string("base_link")); declare_parameter("odom_topic", std::string("odom")); + declare_parameter("enable_groot_monitoring", true); + declare_parameter("groot_zmq_publisher_port", 1666); + declare_parameter("groot_zmq_server_port", 1667); } BtNavigator::~BtNavigator() @@ -146,9 +150,13 @@ BtNavigator::loadBehaviorTree(const std::string & bt_xml_filename) { // Use previous BT if it is the existing one if (current_bt_xml_filename_ == bt_xml_filename) { + RCLCPP_DEBUG(get_logger(), "BT will not be reloaded as the given xml is already loaded"); return true; } + // if a new tree is created, than the ZMQ Publisher must be destroyed + bt_->resetGrootMonitor(); + // Read the input BT XML from the specified file into a string std::ifstream xml_file(bt_xml_filename); @@ -161,13 +169,21 @@ BtNavigator::loadBehaviorTree(const std::string & bt_xml_filename) std::istreambuf_iterator(xml_file), std::istreambuf_iterator()); - RCLCPP_DEBUG(get_logger(), "Behavior Tree file: '%s'", bt_xml_filename.c_str()); - RCLCPP_DEBUG(get_logger(), "Behavior Tree XML: %s", xml_string.c_str()); - // Create the Behavior Tree from the XML input - tree_ = bt_->buildTreeFromText(xml_string, blackboard_); + tree_ = bt_->createTreeFromText(xml_string, blackboard_); current_bt_xml_filename_ = bt_xml_filename; + // get parameter for monitoring with Groot via ZMQ Publisher + if (get_parameter("enable_groot_monitoring").as_bool()) { + uint16_t zmq_publisher_port = get_parameter("groot_zmq_publisher_port").as_int(); + uint16_t zmq_server_port = get_parameter("groot_zmq_server_port").as_int(); + // optionally add max_msg_per_second = 25 (default) here + try { + bt_->addGrootMonitoring(&tree_, zmq_publisher_port, zmq_server_port); + } catch (const std::logic_error & e) { + RCLCPP_ERROR(get_logger(), "ZMQ already enabled, Error: %s", e.what()); + } + } return true; } @@ -211,6 +227,7 @@ BtNavigator::on_cleanup(const rclcpp_lifecycle::State & /*state*/) current_bt_xml_filename_.clear(); blackboard_.reset(); bt_->haltAllActions(tree_.rootNode()); + bt_->resetGrootMonitor(); bt_.reset(); RCLCPP_INFO(get_logger(), "Completed Cleaning up"); @@ -243,15 +260,15 @@ BtNavigator::navigateToPose() return action_server_->is_cancel_requested(); }; - auto bt_xml_filename = action_server_->get_current_goal()->behavior_tree; + std::string bt_xml_filename = action_server_->get_current_goal()->behavior_tree; // Empty id in request is default for backward compatibility bt_xml_filename = bt_xml_filename == "" ? default_bt_xml_filename_ : bt_xml_filename; if (!loadBehaviorTree(bt_xml_filename)) { RCLCPP_ERROR( - get_logger(), "BT file not found: %s. Navigation canceled", - bt_xml_filename.c_str(), current_bt_xml_filename_.c_str()); + get_logger(), "BT file not found: %s. Navigation canceled.", + bt_xml_filename.c_str()); action_server_->terminate_current(); return; } diff --git a/nav2_common/nav2_common/launch/rewritten_yaml.py b/nav2_common/nav2_common/launch/rewritten_yaml.py index 4d9b86373f..40332a6427 100644 --- a/nav2_common/nav2_common/launch/rewritten_yaml.py +++ b/nav2_common/nav2_common/launch/rewritten_yaml.py @@ -21,131 +21,166 @@ import tempfile import launch + class DictItemReference: - def __init__(self, dictionary, key): - self.dictionary = dictionary - self.dictKey = key + def __init__(self, dictionary, key): + self.dictionary = dictionary + self.dictKey = key + + def key(self): + return self.dictKey - def key(self): - return self.dictKey + def setValue(self, value): + self.dictionary[self.dictKey] = value - def setValue(self, value): - self.dictionary[self.dictKey] = value class RewrittenYaml(launch.Substitution): - """ - Substitution that modifies the given YAML file. - - Used in launch system - """ - - def __init__(self, - source_file: launch.SomeSubstitutionsType, - param_rewrites: Dict, - root_key: Optional[launch.SomeSubstitutionsType] = None, - key_rewrites: Optional[Dict] = None, - convert_types = False) -> None: - super().__init__() """ - Construct the substitution + Substitution that modifies the given YAML file. - :param: source_file the original YAML file to modify - :param: param_rewrites mappings to replace - :param: root_key if provided, the contents are placed under this key - :param: key_rewrites keys of mappings to replace - :param: convert_types whether to attempt converting the string to a number or boolean + Used in launch system """ - from launch.utilities import normalize_to_list_of_substitutions # import here to avoid loop - self.__source_file = normalize_to_list_of_substitutions(source_file) - self.__param_rewrites = {} - self.__key_rewrites = {} - self.__convert_types = convert_types - self.__root_key = None - for key in param_rewrites: - self.__param_rewrites[key] = normalize_to_list_of_substitutions(param_rewrites[key]) - if key_rewrites is not None: - for key in key_rewrites: - self.__key_rewrites[key] = normalize_to_list_of_substitutions(key_rewrites[key]) - if root_key is not None: - self.__root_key = normalize_to_list_of_substitutions(root_key) - - @property - def name(self) -> List[launch.Substitution]: - """Getter for name.""" - return self.__source_file - - def describe(self) -> Text: - """Return a description of this substitution as a string.""" - return '' - - def perform(self, context: launch.LaunchContext) -> Text: - yaml_filename = launch.utilities.perform_substitutions(context, self.name) - rewritten_yaml = tempfile.NamedTemporaryFile(mode='w', delete=False) - param_rewrites, keys_rewrites = self.resolve_rewrites(context) - data = yaml.safe_load(open(yaml_filename, 'r')) - self.substitute_params(data, param_rewrites) - self.substitute_keys(data, keys_rewrites) - if self.__root_key is not None: - root_key = launch.utilities.perform_substitutions(context, self.__root_key) - if root_key: - data = {root_key: data} - yaml.dump(data, rewritten_yaml) - rewritten_yaml.close() - return rewritten_yaml.name - - def resolve_rewrites(self, context): - resolved_params = {} - for key in self.__param_rewrites: - resolved_params[key] = launch.utilities.perform_substitutions(context, self.__param_rewrites[key]) - resolved_keys = {} - for key in self.__key_rewrites: - resolved_keys[key] = launch.utilities.perform_substitutions(context, self.__key_rewrites[key]) - return resolved_params, resolved_keys - - def substitute_params(self, yaml, param_rewrites): - for key in self.getYamlLeafKeys(yaml): - if key.key() in param_rewrites: - raw_value = param_rewrites[key.key()] - key.setValue(self.convert(raw_value)) - - def substitute_keys(self, yaml, key_rewrites): - if len(key_rewrites) != 0: - for key, val in yaml.items(): - if isinstance(val, dict) and key in key_rewrites: - new_key = key_rewrites[key] - yaml[new_key] = yaml[key] - del yaml[key] - self.substitute_keys(val, key_rewrites) - - def getYamlLeafKeys(self, yamlData): - try: - for key in yamlData.keys(): - for k in self.getYamlLeafKeys(yamlData[key]): - yield k - yield DictItemReference(yamlData, key) - except AttributeError: - return - - def convert(self, text_value): - if self.__convert_types: - # try converting to float - try: - return float(text_value) - except ValueError: - pass - - # try converting to int - try: - return int(text_value) - except ValueError: - pass - - # try converting to bool - if text_value.lower() == "true": - return True - if text_value.lower() == "false": - return False - - #nothing else worked so fall through and return text - return text_value + def __init__(self, + source_file: launch.SomeSubstitutionsType, + param_rewrites: Dict, + root_key: Optional[launch.SomeSubstitutionsType] = None, + key_rewrites: Optional[Dict] = None, + convert_types = False) -> None: + super().__init__() + """ + Construct the substitution + + :param: source_file the original YAML file to modify + :param: param_rewrites mappings to replace + :param: root_key if provided, the contents are placed under this key + :param: key_rewrites keys of mappings to replace + :param: convert_types whether to attempt converting the string to a number or boolean + """ + + from launch.utilities import normalize_to_list_of_substitutions # import here to avoid loop + self.__source_file = normalize_to_list_of_substitutions(source_file) + self.__param_rewrites = {} + self.__key_rewrites = {} + self.__convert_types = convert_types + self.__root_key = None + for key in param_rewrites: + self.__param_rewrites[key] = normalize_to_list_of_substitutions(param_rewrites[key]) + if key_rewrites is not None: + for key in key_rewrites: + self.__key_rewrites[key] = normalize_to_list_of_substitutions(key_rewrites[key]) + if root_key is not None: + self.__root_key = normalize_to_list_of_substitutions(root_key) + + @property + def name(self) -> List[launch.Substitution]: + """Getter for name.""" + return self.__source_file + + def describe(self) -> Text: + """Return a description of this substitution as a string.""" + return '' + + def perform(self, context: launch.LaunchContext) -> Text: + yaml_filename = launch.utilities.perform_substitutions(context, self.name) + rewritten_yaml = tempfile.NamedTemporaryFile(mode='w', delete=False) + param_rewrites, keys_rewrites = self.resolve_rewrites(context) + data = yaml.safe_load(open(yaml_filename, 'r')) + self.substitute_params(data, param_rewrites) + self.substitute_keys(data, keys_rewrites) + if self.__root_key is not None: + root_key = launch.utilities.perform_substitutions(context, self.__root_key) + if root_key: + data = {root_key: data} + yaml.dump(data, rewritten_yaml) + rewritten_yaml.close() + return rewritten_yaml.name + + def resolve_rewrites(self, context): + resolved_params = {} + for key in self.__param_rewrites: + resolved_params[key] = launch.utilities.perform_substitutions(context, self.__param_rewrites[key]) + resolved_keys = {} + for key in self.__key_rewrites: + resolved_keys[key] = launch.utilities.perform_substitutions(context, self.__key_rewrites[key]) + return resolved_params, resolved_keys + + def substitute_params(self, yaml, param_rewrites): + # substitute leaf-only parameters + for key in self.getYamlLeafKeys(yaml): + if key.key() in param_rewrites: + raw_value = param_rewrites[key.key()] + key.setValue(self.convert(raw_value)) + + # substitute total path parameters + yaml_paths = self.pathify(yaml) + for path in yaml_paths: + if path in param_rewrites: + # this is an absolute path (ex. 'key.keyA.keyB.val') + rewrite_val = param_rewrites[path] + yaml_keys = path.split('.') + yaml = self.updateYamlPathVals(yaml, yaml_keys, rewrite_val) + + + def updateYamlPathVals(self, yaml, yaml_key_list, rewrite_val): + for key in yaml_key_list: + if key == yaml_key_list[-1]: + yaml[key] = rewrite_val + break + key = yaml_key_list.pop(0) + yaml[key] = self.updateYamlPathVals(yaml.get(key, {}), yaml_key_list, rewrite_val) + + return yaml + + def substitute_keys(self, yaml, key_rewrites): + if len(key_rewrites) != 0: + for key, val in yaml.items(): + if isinstance(val, dict) and key in key_rewrites: + new_key = key_rewrites[key] + yaml[new_key] = yaml[key] + del yaml[key] + self.substitute_keys(val, key_rewrites) + + def getYamlLeafKeys(self, yamlData): + try: + for key in yamlData.keys(): + for k in self.getYamlLeafKeys(yamlData[key]): + yield k + yield DictItemReference(yamlData, key) + except AttributeError: + return + + def pathify(self, d, p=None, paths=None, joinchar='.'): + if p is None: + paths = {} + self.pathify(d, "", paths, joinchar=joinchar) + return paths + pn = p + if p != "": + pn += '.' + if isinstance(d, dict): + for k in d: + v = d[k] + self.pathify(v, pn + k, paths, joinchar=joinchar) + elif isinstance(d, list): + for idx, e in enumerate(d): + self.pathify(e, pn + str(idx), paths, joinchar=joinchar) + else: + paths[p] = d + + def convert(self, text_value): + if self.__convert_types: + # try converting to int or float + try: + return float(text_value) if '.' in text_value else int(text_value) + except ValueError: + pass + + # try converting to bool + if text_value.lower() == "true": + return True + if text_value.lower() == "false": + return False + + # nothing else worked so fall through and return text + return text_value diff --git a/nav2_controller/src/nav2_controller.cpp b/nav2_controller/src/nav2_controller.cpp index 13284395f0..dd938c4dbc 100644 --- a/nav2_controller/src/nav2_controller.cpp +++ b/nav2_controller/src/nav2_controller.cpp @@ -115,6 +115,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) RCLCPP_FATAL( get_logger(), "Failed to create progress_checker. Exception: %s", ex.what()); + return nav2_util::CallbackReturn::FAILURE; } try { goal_checker_type_ = nav2_util::get_plugin_type_param(node, goal_checker_id_); @@ -127,6 +128,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) RCLCPP_FATAL( get_logger(), "Failed to create goal_checker. Exception: %s", ex.what()); + return nav2_util::CallbackReturn::FAILURE; } for (size_t i = 0; i != controller_ids_.size(); i++) { @@ -145,6 +147,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) RCLCPP_FATAL( get_logger(), "Failed to create controller. Exception: %s", ex.what()); + return nav2_util::CallbackReturn::FAILURE; } } diff --git a/nav2_costmap_2d/plugins/voxel_layer.cpp b/nav2_costmap_2d/plugins/voxel_layer.cpp index 5235e23675..9a960dc689 100644 --- a/nav2_costmap_2d/plugins/voxel_layer.cpp +++ b/nav2_costmap_2d/plugins/voxel_layer.cpp @@ -88,10 +88,9 @@ void VoxelLayer::onInitialize() if (publish_voxel_) { voxel_pub_ = node_->create_publisher( "voxel_grid", custom_qos); + voxel_pub_->on_activate(); } - voxel_pub_->on_activate(); - clearing_endpoints_pub_ = node_->create_publisher( "clearing_endpoints", custom_qos); diff --git a/nav2_costmap_2d/src/costmap_2d.cpp b/nav2_costmap_2d/src/costmap_2d.cpp index 976d09ce26..21a96bcf8b 100644 --- a/nav2_costmap_2d/src/costmap_2d.cpp +++ b/nav2_costmap_2d/src/costmap_2d.cpp @@ -435,7 +435,7 @@ void Costmap2D::convexFillCells( MapLocation pt; // loop though cells in the column - for (unsigned int y = min_pt.y; y < max_pt.y; ++y) { + for (unsigned int y = min_pt.y; y <= max_pt.y; ++y) { pt.x = x; pt.y = y; polygon_cells.push_back(pt); diff --git a/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp b/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp index 2da4b44a52..f4a588611a 100644 --- a/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp +++ b/nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp @@ -160,6 +160,11 @@ class DWBLocalPlanner : public nav2_core::Controller * 3) If prune_plan_ is true, it will remove all points that we've already passed from both the transformed plan * and the saved global_plan_. Technically, it iterates to a pose on the path that is within prune_distance_ * of the robot and erases all poses before that. + * + * Additionally, shorten_transformed_plan_ determines whether we will pass the full plan all + * the way to the nav goal on to the critics or just a subset of the plan near the robot. + * True means pass just a subset. This gives DWB less discretion to decide how it gets to the + * nav goal. Instead it is encouraged to try to get on to the path generated by the global planner. */ virtual nav_2d_msgs::msg::Path2D transformGlobalPlan( const nav_2d_msgs::msg::Pose2DStamped & pose); @@ -168,6 +173,7 @@ class DWBLocalPlanner : public nav2_core::Controller double prune_distance_; bool debug_trajectory_details_; rclcpp::Duration transform_tolerance_{0, 0}; + bool shorten_transformed_plan_; /** * @brief try to resolve a possibly shortened critic name with the default namespaces and the suffix "Critic" diff --git a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp index 245323db51..793e9c90f7 100644 --- a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp +++ b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp @@ -89,6 +89,9 @@ void DWBLocalPlanner::configure( declare_parameter_if_not_declared( node_, dwb_plugin_name_ + ".transform_tolerance", rclcpp::ParameterValue(0.1)); + declare_parameter_if_not_declared( + node_, dwb_plugin_name_ + ".shorten_transformed_plan", + rclcpp::ParameterValue(true)); declare_parameter_if_not_declared( node_, dwb_plugin_name_ + ".short_circuit_trajectory_evaluation", rclcpp::ParameterValue(true)); @@ -107,6 +110,7 @@ void DWBLocalPlanner::configure( node_->get_parameter( dwb_plugin_name_ + ".short_circuit_trajectory_evaluation", short_circuit_trajectory_evaluation_); + node->get_parameter(dwb_plugin_name_ + ".shorten_transformed_plan", shorten_transformed_plan_); pub_ = std::make_unique(node_, dwb_plugin_name_); pub_->on_configure(); @@ -518,18 +522,11 @@ DWBLocalPlanner::transformGlobalPlan( sq_transform_start_threshold = sq_dist_threshold; } - // Determines whether we will pass the full plan all the way to the nav goal on - // to the critics or just a subset of the plan near the robot. True means pass - // just a subset. This gives DWB less discretion to decide how it gets to the - // nav goal. Instead it is encouraged to try to get on to the path generated - // by the global planner. - bool shorten_transformed_plan = true; - // Set the maximum distance we'll include points after the part of the part of // the plan near the robot (the end of the plan). This determines the amount // of the plan passed on to the critics double sq_transform_end_threshold; - if (shorten_transformed_plan) { + if (shorten_transformed_plan_) { sq_transform_end_threshold = std::min(sq_dist_threshold, sq_prune_dist); } else { sq_transform_end_threshold = sq_dist_threshold; diff --git a/nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp b/nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp index c6a4fa9699..a4ce84404a 100644 --- a/nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp +++ b/nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp @@ -219,6 +219,10 @@ class NavFn */ void updateCellAstar(int n); + /** + * @brief Set up navigation potential arrays for new propagation + * @param keepit whether or not use COST_NEUTRAL + */ void setupNavFn(bool keepit = false); /** @@ -253,7 +257,13 @@ class NavFn */ int calcPath(int n, int * st = NULL); + /** + * @brief Calculate gradient at a cell + * @param n Cell number + * @return float norm + */ float gradCell(int n); /**< calculates gradient at cell , returns norm */ + float pathStep; /**< step size for following gradient */ /** display callback */ 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 e9d34279a5..66f57790b7 100644 --- a/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp +++ b/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp @@ -37,52 +37,100 @@ namespace nav2_navfn_planner class NavfnPlanner : public nav2_core::GlobalPlanner { public: + /** + * @brief constructor + */ NavfnPlanner(); + + /** + * @brief destructor + */ ~NavfnPlanner(); - // plugin configure + /** + * @brief Configuring plugin + * @param parent Lifecycle node pointer + * @param name Name of plugin map + * @param tf Shared ptr of TF2 buffer + * @param costmap_ros Costmap2DROS object + */ void configure( rclcpp_lifecycle::LifecycleNode::SharedPtr parent, std::string name, std::shared_ptr tf, std::shared_ptr costmap_ros) override; - // plugin cleanup + /** + * @brief Cleanup lifecycle node + */ void cleanup() override; - // plugin activate + /** + * @brief Activate lifecycle node + */ void activate() override; - // plugin deactivate + /** + * @brief Deactivate lifecycle node + */ void deactivate() override; - // plugin create path + /** + * @brief Creating a plan from start and goal poses + * @param start Start pose + * @param goal Goal pose + * @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; protected: - // Compute a plan given start and goal poses, provided in global world frame. + /** + * @brief Compute a plan given start and goal poses, provided in global world frame. + * @param start Start pose + * @param goal Goal pose + * @param tolerance Relaxation constraint in x and y + * @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, nav_msgs::msg::Path & plan); - // Compute the navigation function given a seed point in the world to start from + /** + * @brief Compute the navigation function given a seed point in the world to start from + * @param world_point Point in world coordinate frame + * @return true if can compute + */ bool computePotential(const geometry_msgs::msg::Point & world_point); - // Compute a plan to a goal from a potential - must call computePotential first + /** + * @brief Compute a plan to a goal from a potential - must call computePotential first + * @param goal Goal pose + * @param plan Path to be computed + * @return true if can compute a plan path + */ bool getPlanFromPotential( const geometry_msgs::msg::Pose & goal, nav_msgs::msg::Path & plan); - // Remove artifacts at the end of the path - originated from planning on a discretized world + /** + * @brief Remove artifacts at the end of the path - originated from planning on a discretized world + * @param goal Goal pose + * @param plan Computed path + */ void smoothApproachToGoal( const geometry_msgs::msg::Pose & goal, nav_msgs::msg::Path & plan); - // Compute the potential, or navigation cost, at a given point in the world - // - must call computePotential first + /** + * @brief Compute the potential, or navigation cost, at a given point in the world + * must call computePotential first + * @param world_point Point in world coordinate frame + * @return double point potential (navigation cost) + */ double getPointPotential(const geometry_msgs::msg::Point & world_point); // Check for a valid potential value at a given point in the world @@ -91,7 +139,12 @@ class NavfnPlanner : public nav2_core::GlobalPlanner // bool validPointPotential(const geometry_msgs::msg::Point & world_point); // bool validPointPotential(const geometry_msgs::msg::Point & world_point, double tolerance); - // Compute the squared distance between two points + /** + * @brief Compute the squared distance between two points + * @param p1 Point 1 + * @param p2 Point 2 + * @return double squared distance between two points + */ inline double squared_distance( const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Pose & p2) @@ -101,16 +154,36 @@ class NavfnPlanner : public nav2_core::GlobalPlanner return dx * dx + dy * dy; } - // Transform a point from world to map frame + /** + * @brief Transform a point from world to map frame + * @param wx double of world X coordinate + * @param wy double of world Y coordinate + * @param mx int of map X coordinate + * @param my int of map Y coordinate + * @return true if can transform + */ bool worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my); - // Transform a point from map to world frame + /** + * @brief Transform a point from map to world frame + * @param mx double of map X coordinate + * @param my double of map Y coordinate + * @param wx double of world X coordinate + * @param wy double of world Y coordinate + */ void mapToWorld(double mx, double my, double & wx, double & wy); - // Set the corresponding cell cost to be free space + /** + * @brief Set the corresponding cell cost to be free space + * @param mx int of map X coordinate + * @param my int of map Y coordinate + */ void clearRobotCell(unsigned int mx, unsigned int my); - // Determine if a new planner object should be made + /** + * @brief Determine if a new planner object should be made + * @return true if planner object is out of date + */ bool isPlannerOutOfDate(); // Planner based on ROS1 NavFn algorithm @@ -137,6 +210,16 @@ class NavfnPlanner : public nav2_core::GlobalPlanner // Whether to use the astar planner or default dijkstras bool use_astar_; + + // Subscription for parameter change + rclcpp::AsyncParametersClient::SharedPtr parameters_client_; + rclcpp::Subscription::SharedPtr parameter_event_sub_; + + /** + * @brief Callback executed when a paramter change is detected + * @param event ParameterEvent message + */ + void on_parameter_event_callback(const rcl_interfaces::msg::ParameterEvent::SharedPtr event); }; } // namespace nav2_navfn_planner diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index a2b412b776..975e5f8288 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -40,6 +40,8 @@ using namespace std::chrono_literals; using nav2_util::declare_parameter_if_not_declared; +using rcl_interfaces::msg::ParameterType; +using std::placeholders::_1; namespace nav2_navfn_planner { @@ -85,6 +87,16 @@ NavfnPlanner::configure( planner_ = std::make_unique( costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY()); + + // Setup callback for changes to parameters. + parameters_client_ = std::make_shared( + node_->get_node_base_interface(), + node_->get_node_topics_interface(), + node_->get_node_graph_interface(), + node_->get_node_services_interface()); + + parameter_event_sub_ = parameters_client_->on_parameter_event( + std::bind(&NavfnPlanner::on_parameter_event_callback, this, _1)); } void @@ -453,6 +465,29 @@ NavfnPlanner::clearRobotCell(unsigned int mx, unsigned int my) costmap_->setCost(mx, my, nav2_costmap_2d::FREE_SPACE); } +void +NavfnPlanner::on_parameter_event_callback( + const rcl_interfaces::msg::ParameterEvent::SharedPtr event) +{ + for (auto & changed_parameter : event->changed_parameters) { + const auto & type = changed_parameter.value.type; + const auto & name = changed_parameter.name; + const auto & value = changed_parameter.value; + + if (type == ParameterType::PARAMETER_DOUBLE) { + if (name == name_ + ".tolerance") { + tolerance_ = value.double_value; + } + } else if (type == ParameterType::PARAMETER_BOOL) { + if (name == name_ + ".use_astar") { + use_astar_ = value.bool_value; + } else if (name == name_ + ".allow_unknown") { + allow_unknown_ = value.bool_value; + } + } + } +} + } // namespace nav2_navfn_planner #include "pluginlib/class_list_macros.hpp" diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index b6b4615bb0..48eede3686 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -102,6 +102,7 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & state) RCLCPP_FATAL( get_logger(), "Failed to create global planner. Exception: %s", ex.what()); + return nav2_util::CallbackReturn::FAILURE; } } diff --git a/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp b/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp index bb9efa571c..26c497d458 100644 --- a/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp +++ b/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp @@ -37,7 +37,7 @@ class RecoveryServer : public nav2_util::LifecycleNode RecoveryServer(); ~RecoveryServer(); - void loadRecoveryPlugins(); + bool loadRecoveryPlugins(); protected: // Implement the lifecycle interface diff --git a/nav2_recoveries/src/recovery_server.cpp b/nav2_recoveries/src/recovery_server.cpp index d657a9d056..bd78ae85f8 100644 --- a/nav2_recoveries/src/recovery_server.cpp +++ b/nav2_recoveries/src/recovery_server.cpp @@ -88,13 +88,15 @@ RecoveryServer::on_configure(const rclcpp_lifecycle::State & /*state*/) } } recovery_types_.resize(recovery_ids_.size()); - loadRecoveryPlugins(); + if (!loadRecoveryPlugins()) { + return nav2_util::CallbackReturn::FAILURE; + } return nav2_util::CallbackReturn::SUCCESS; } -void +bool RecoveryServer::loadRecoveryPlugins() { auto node = shared_from_this(); @@ -112,9 +114,11 @@ RecoveryServer::loadRecoveryPlugins() get_logger(), "Failed to create recovery %s of type %s." " Exception: %s", recovery_ids_[i].c_str(), recovery_types_[i].c_str(), ex.what()); - exit(-1); + return false; } } + + return true; } nav2_util::CallbackReturn diff --git a/nav2_regulated_pure_pursuit_controller/CMakeLists.txt b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt new file mode 100644 index 0000000000..9c87714fad --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt @@ -0,0 +1,70 @@ +cmake_minimum_required(VERSION 3.5) +project(nav2_regulated_pure_pursuit_controller) + +find_package(ament_cmake REQUIRED) +find_package(nav2_common REQUIRED) +find_package(nav2_core REQUIRED) +find_package(nav2_costmap_2d REQUIRED) +find_package(nav2_util REQUIRED) +find_package(rclcpp REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(pluginlib REQUIRED) +find_package(tf2 REQUIRED) + +nav2_package() +set(CMAKE_CXX_STANDARD 17) + +include_directories( + include +) + +set(dependencies + rclcpp + geometry_msgs + nav2_costmap_2d + pluginlib + nav_msgs + nav2_util + nav2_core + tf2 +) + +set(library_name nav2_regulated_pure_pursuit_controller) + +add_library(${library_name} SHARED + src/regulated_pure_pursuit_controller.cpp) + +# prevent pluginlib from using boost +target_compile_definitions(${library_name} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") + +ament_target_dependencies(${library_name} + ${dependencies} +) + +install(TARGETS ${library_name} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(DIRECTORY include/ + DESTINATION include/ +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + set(ament_cmake_copyright_FOUND TRUE) + ament_lint_auto_find_test_dependencies() + add_subdirectory(test) +endif() + +ament_export_include_directories(include) +ament_export_libraries(${library_name}) +ament_export_dependencies(${dependencies}) + +pluginlib_export_plugin_description_file(nav2_core nav2_regulated_pure_pursuit_controller.xml) + +ament_package() + diff --git a/nav2_regulated_pure_pursuit_controller/README.md b/nav2_regulated_pure_pursuit_controller/README.md new file mode 100644 index 0000000000..285797499c --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/README.md @@ -0,0 +1,147 @@ +# Nav2 Regulated Pure Pursuit Controller + +This is a controller (local trajectory planner) that implements a variant on the pure pursuit algorithm to track a path. This variant we call the Regulated Pure Pursuit Algorithm, due to its additional regulation terms on collision and linear speed. It also implements the basics behind the Adaptive Pure Pursuit algorithm to vary lookahead distances by current speed. It was developed by [Shrijit Singh](https://www.linkedin.com/in/shrijitsingh99/) and [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) while at [Samsung Research](https://www.sra.samsung.com/) as part of the Nav2 working group. + +Code based on a simplified version of this controller is referenced in the [Writing a New Nav2 Controller](https://navigation.ros.org/plugin_tutorials/docs/writing_new_nav2controller_plugin.html) tutorial. + +This plugin implements the `nav2_core::Controller` interface allowing it to be used across the navigation stack as a local trajectory planner in the controller server's action server (`controller_server`). + +It builds on top of the ordinary pure pursuit algorithm in a number of ways. It also implements all the common variants of the pure pursuit algorithm such as adaptive pure pursuit. This controller is suitable for use on all types of robots, including differential, legged, and ackermann steering vehicles. It may also be used on omni-directional platforms, but won't be able to fully leverage the lateral movements of the base (you may consider DWB instead). + +This controller has been measured to run at well over 1 kHz on a modern intel processor. + +

+ +

+ +## Pure Pursuit Basics + +The Pure Pursuit algorithm has been in use for over 30 years. You can read more about the details of the pure pursuit controller in its [introduction paper](http://www.enseignement.polytechnique.fr/profs/informatique/Eric.Goubault/MRIS/coulter_r_craig_1992_1.pdf). The core idea is to find a point on the path in front of the robot and find the linear and angular velocity to help drive towards it. Once it moves forward, a new point is selected, and the process repeats until the end of the path. The distance used to find the point to drive towards is the `lookahead` distance. + +In order to simply book-keeping, the global path is continuously pruned to the closest point to the robot (see the figure below) so that we only have to process useful path points. Then, the section of the path within the local costmap bounds is transformed to the robot frame and a lookahead point is determined using a predefined distance. + +Finally, the lookahead point will be given to the pure pursuit algorithm which finds the curvature of the path required to drive the robot to the lookahead point. This curvature is then applied to the velocity commands to allow the robot to drive. + +![Lookahead algorithm](./doc/lookahead_algorithm.png) + +## Regulated Pure Pursuit Features + +We have created a new variation on the pure pursuit algorithm that we dubb the Regulated Pure Pursuit algorithm. We combine the features of the Adaptive Pure Pursuit algorithm with rules around linear velocity with a focus on consumer, industrial, and service robot's needs. We also implement several common-sense safety mechanisms like collision detection and ensuring that commands are kinematically feasible that are missing from all other variants of pure pursuit (for some remarkable reason). + +The Regulated Pure Pursuit controller implements active collision detection. We use a parameter to set the maximum allowable time before a potential collision on the current velocity command. Using the current linear and angular velocity, we project forward in time that duration and check for collisions. Intuitively, you may think that collision checking between the robot and the lookahead point seems logical. However, if you're maneuvering in tight spaces, it makes alot of sense to only search forward a given amount of time to give the system a little leeway to get itself out. In confined spaces especially, we want to make sure that we're collision checking a reasonable amount of space for the current action being taken (e.g. if moving at 0.1 m/s, it makes no sense to look 10 meters ahead to the carrot, or 100 seconds into the future). This helps look further at higher speeds / angular rotations and closer with fine, slow motions in constrained environments so it doesn't over report collisions from valid motions near obstacles. If you set the maximum allowable to a large number, it will collision check all the way, but not exceeding, the lookahead point. We visualize the collision checking arc on the `lookahead_arc` topic. + +The regulated pure pursuit algorithm also makes use of the common variations on the pure pursuit algorithm. We implement the adaptive pure pursuit's main contribution of having velocity-scaled lookahead point distances. This helps make the controller more stable over a larger range of potential linear velocities. There are parameters for setting the lookahead gain (or lookahead time) and thresholded values for minimum and maximum. + +We also implement kinematic speed limits on the linear velocities in operations and angular velocities during pure rotations. This makes sure that the output commands are smooth, safe, and kinematically feasible. This is especially important at the beginning and end of a path tracking task, where you are ramping up to speed and slowing down to the goal. + +The final minor improvement we make is slowing on approach to the goal. Knowing that the optimal lookahead distance is `X`, we can take the difference in `X` and the actual distance of the lookahead point found to find the lookahead point error. During operations, the variation in this error should be exceptionally small and won't be triggered. However, at the end of the path, there are no more points at a lookahead distance away from the robot, so it uses the last point on the path. So as the robot approaches a target, its error will grow and the robot's velocity will be reduced proportional to this error until a minimum threshold. This is also tracked by the kinematic speed limits to ensure drivability. + +The major improvements that this work implements is the regulations on the linear velocity based on some cost functions. They were selected to remove long-standing bad behavior within the pure pursuit algorithm. Normal Pure Pursuit has an issue with overshoot and poor handling in particularly high curvature (or extremely rapidly changing curvature) environments. It is commonly known that this will cause the robot to overshoot from the path and potentially collide with the environment. These cost functions in the Regulated Pure Pursuit algorithm were also chosen based on common requirements and needs of mobile robots uses in service, commercial, and industrial use-cases; scaling by curvature creates intuitive behavior of slowing the robot when making sharp turns and slowing when its near a potential collision so that small variations don't clip obstacles. This is also really useful when working in partially observable environments (like turning in and out of aisles / hallways often) so that you slow before a sharp turn into an unknown dynamic environment to be more conservative in case something is in the way immediately requiring a stop. + +The cost functions penalize the robot's speed based on its proximity to obstacles and the curvature of the path. This is helpful to slow the robot when moving close to things in narrow spaces and scaling down the linear velocity by curvature helps to stabilize the controller over a larger range of lookahead point distances. This also has the added benefit of removing the sensitive tuning of the lookahead point / range, as the robot will track paths far better. Tuning is still required, but it is substantially easier to get reasonable behavior with minor adjustments. + +An unintended tertiary benefit of scaling the linear velocities by curvature is that a robot will natively rotate to rough path heading when using holonomic planners that don't start aligned with the robot pose orientation. As the curvature will be very high, the linear velocity drops and the angular velocity takes over to rotate to heading. While not perfect, it does dramatically reduce the need to rotate to a close path heading before following and opens up a broader range of planning techniques. Pure Pursuit controllers otherwise would be completely unable to recover from this in even modestly confined spaces. + +Mixing the proximity and curvature regulated linear velocities with the time-scaled collision checker, we see a near-perfect combination allowing the regulated pure pursuit algorithm to handle high starting deviations from the path and navigate collision-free in tight spaces without overshoot. + +## Configuration + +| Parameter | Description | +|-----|----| +| `desired_linear_vel` | The desired maximum linear velocity to use. | +| `max_linear_accel` | Acceleration for linear velocity | +| `max_linear_decel` | Deceleration for linear velocity | +| `lookahead_dist` | The lookahead distance to use to find the lookahead point | +| `min_lookahead_dist` | The minimum lookahead distance threshold when using velocity scaled lookahead distances | +| `max_lookahead_dist` | The maximum lookahead distance threshold when using velocity scaled lookahead distances | +| `lookahead_time` | The time to project the velocity by to find the velocity scaled lookahead distance. Also known as the lookahead gain. | +| `rotate_to_heading_angular_vel` | If rotate to heading is used, this is the angular velocity to use. | +| `transform_tolerance` | The TF transform tolerance | +| `use_velocity_scaled_lookahead_dist` | Whether to use the velocity scaled lookahead distances or constant `lookahead_distance` | +| `min_approach_linear_velocity` | The minimum velocity threshold to apply when approaching the goal | +| `use_approach_linear_velocity_scaling` | Whether to scale the linear velocity down on approach to the goal for a smooth stop | +| `max_allowed_time_to_collision` | The time to project a velocity command to check for collisions | +| `use_regulated_linear_velocity_scaling` | Whether to use the regulated features for curvature | +| `use_cost_regulated_linear_velocity_scaling` | Whether to use the regulated features for proximity to obstacles | +| `cost_scaling_dist` | The minimum distance from an obstacle to trigger the scaling of linear velocity, if `use_cost_regulated_linear_velocity_scaling` is enabled. The value set should be smaller or equal to the `inflation_radius` set in the inflation layer of costmap, since inflation is used to compute the distance from obstacles | +| `cost_scaling_gain` | A multiplier gain, which should be <= 1.0, used to further scale the speed when an obstacle is within `cost_scaling_dist`. Lower value reduces speed more quickly. | +| `inflation_cost_scaling_factor` | The value of `cost_scaling_factor` set for the inflation layer in the local costmap. The value should be exactly the same for accurately computing distance from obstacles using the inflated cell values | +| `regulated_linear_scaling_min_radius` | The turning radius for which the regulation features are triggered. Remember, sharper turns have smaller radii | +| `regulated_linear_scaling_min_speed` | The minimum speed for which the regulated features can send, to ensure process is still achievable even in high cost spaces with high curvature. | +| `use_rotate_to_heading` | Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types except ackermann, which cannot rotate in place. | +| `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 | +| `goal_dist_tol` | XY tolerance from goal to rotate to the goal heading, if `use_rotate_to_heading` is enabled. This should match or be smaller than the `GoalChecker`'s translational goal tolerance. | + + +Example fully-described XML with default parameter values: + +``` +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + progress_checker_plugin: "progress_checker" + goal_checker_plugin: "goal_checker" + controller_plugins: ["FollowPath"] + + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + stateful: True + FollowPath: + plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController" + desired_linear_vel: 0.5 + max_linear_accel: 2.5 + max_linear_decel: 2.5 + lookahead_dist: 0.6 + min_lookahead_dist: 0.3 + max_lookahead_dist: 0.9 + lookahead_time: 1.5 + rotate_to_heading_angular_vel: 1.8 + transform_tolerance: 0.1 + use_velocity_scaled_lookahead_dist: false + min_approach_linear_velocity: 0.05 + use_approach_linear_velocity_scaling: true + max_allowed_time_to_collision: 1.0 + use_regulated_linear_velocity_scaling: true + use_cost_regulated_linear_velocity_scaling: false + regulated_linear_scaling_min_radius: 0.9 + regulated_linear_scaling_min_speed: 0.25 + use_rotate_to_heading: true + rotate_to_heading_min_angle: 0.785 + max_angular_accel: 3.2 + goal_dist_tol: 0.25 + cost_scaling_dist: 0.3 + cost_scaling_gain: 1.0 + inflation_cost_scaling_factor: 3.0 +``` + +## Topics + +| Topic | Type | Description | +|-----|----|----| +| `lookahead_point` | `geometry_msgs/PointStamped` | The current lookahead point on the path | +| `lookahead_arc` | `nav_msgs/Path` | The drivable arc between the robot and the carrot. Arc length depends on `max_allowed_time_to_collision`, forward simulating from the robot pose at the commanded `Twist` by that time. In a collision state, the last published arc will be the points leading up to, and including, the first point in collision. | + +Note: The `lookahead_arc` is also a really great speed indicator, when "full" to carrot or max time, you know you're at full speed. If 20% less, you can tell the robot is approximately 20% below maximum speed. Think of it as the collision checking bounds but also a speed guage. + +## Notes to users + +By default, the `use_cost_regulated_linear_velocity_scaling` is set to `false` because the generic sandbox environment we have setup is the TB3 world. This is a highly constrained environment so it overly triggers to slow the robot as everywhere has high costs. This is recommended to be set to `true` when not working in constantly high-cost spaces. + +To tune to get Adaptive Pure Pursuit behaviors, set all boolean parameters to false except `use_velocity_scaled_lookahead_dist` and make sure to tune `lookahead_time`, `min_lookahead_dist` and `max_lookahead_dist`. + +To tune to get Pure Pursuit behaviors, set all boolean parameters to false and make sure to tune `lookahead_dist`. + +Currently, there is no rotate to goal behaviors, so it is expected that the path approach orientations are the orientations of the goal or the goal checker has been set with a generous `min_theta_velocity_threshold`. Implementations for rotating to goal heading are on the way. + +The choice of lookahead distances are highly dependent on robot size, responsiveness, controller update rate, and speed. Please make sure to tune this for your platform, although the `regulated` features do largely make heavy tuning of this value unnecessary. If you see wiggling, increase the distance or scale. If it's not converging as fast to the path as you'd like, decrease it. diff --git a/nav2_regulated_pure_pursuit_controller/doc/lookahead_algorithm.png b/nav2_regulated_pure_pursuit_controller/doc/lookahead_algorithm.png new file mode 100644 index 0000000000000000000000000000000000000000..8507ddd3e03bb6846236dd10fe8f945fe807cf0a GIT binary patch literal 108322 zcmb@u1yodP`#uavcS*MaA}QS4XRl5=rvP`u)5zR&j24qpRb7oRPU zh3lpoHkRw1ur+7C8?_~UsV#x<8%(5Uuhz=1O2fc-h5zS+9FBSQDG$N>^L3B;|EIH^ zK7R`OU&{TzoJE!6{<-?^pZ{(X#@~(k&so%I;O?R~RM}$GSDG|v19kc%xR>DV zr@uR3=2?5~>RbpKW?tUX)h<8{@A5I1$sStwJXvCn>Eig){Z7=-b;8$d-m=+UK6{yl z5gqK2`rOHXhJ+k0=P8-_DizHK+bL~~<9El2`hMJf6EmzHKzwcYt zy!~##RwMr*bS_Ktc1qtZy~SDQy3n?dKkTVvx)TC%aRxz_zTIayy_y|*idHcD)DZ>eXrK!=Qg0JBVzmWt{cE;J-0 zq^hc_xjE3xY%WJJB2sK)s@m?|Z7o6~qS5hj+;9njWGt;~xD0O?nV6)XeNW=l$+FzQ z3TJO4PR!0GGqiZ}gn$;gvGFPfIazswoBM&fIzg&PNDezYJ7RJ1p4Y}>OVaj!tmfwC z`1tr%Y}{DgAdCPX3`d*u9=EeQW0c(Fl%K|LI^aL}(pIARkqd*IoBBa)WLTBuJ#6|r z?ZYpupk^ME3pf$Fh4f)PWx{EU$Cd2Na9gdXp-eNl;;}E3MDuTMeWmEc-?;3W;(ks4V z3cE3CQRkc)qZIVu4Z}@ETP^nw?+~X^RGe%49NQ%AZn9*|lD|044<{_jn~xZ1b;zTB z`HU6m^NoN`H2NS}&fm)?&E{*QRND>NazgB0cVA{KV+Oge^rzvHFkrK*Rf5&Doei^W z!y3Q#irn-JVr7BH)a&1J7G)R zaWQnhD6?%hSG$f%o1Q8rSasE6@L;Okr$rsv? ziEJr+oF11NkFiPwUqBL3kmKJty#kIy+#jg^X}TUdUhdLmqRi}NrProG5dNE3PGb0- zs>yn{8x+!)w~3r5%9s_-5^rg&#nFp8rDkT}ykP9V2USXapC5QjfXkA-)3hZ>MzJj~ zNZ3r+VWe{Tk>x=2G&LR>izhvkEM!tUIP^8W?80=7 zqr^6tJ9eU-;ha$BXuN_Sx7vFNh>PBEd08c;%W-wS`#!^te;e_)d! zbCi8|7wj`W3Mie545$smlTT!KGYC&pDyC(myW*G;ZNq=Zx!^ z&c}j?m8aiflXJEMnisg?rLDD%UYoX`e(8Nuxy2&`uduq-^5b^iS@h4>NTuGR3B8el zdm$QOM(;k3vOWP@KWvW8a}lrm+uIxKE8e(9PHbc~O99*9&;`! z_c0{KmEPFFu=H#Fe2hIl*h*^wp<8T9mQbO4&2tuq(FX$Sxy?J;>!xOJ2O}!I)mm9)cpc%B4m0YkG3*tg7Q`W8I7Bx z)Ubdp;$BbJ$?k-$^o@k4xKqz6fG%jYl%0NUvoSRo&Tm>T;DwM{>a8V_Zd?ohaoEaM z=!H9&DW!_PNoE=PfcBN<6wRDK$~JL$$@WLVnSe&u67Nas^wrON5W8AQ0fREr_51x9 z5<&v0@|QZ2taO?&P}frwvQAI7SbPwJYWYIXnaA}l-P#eJ&=XcUNP>|yCbPni)i&>Q zi+b0#TkTDLozXb;OY8#g`7L(Zr$|p2M4s0xK}_N;8W#-6XkY2dhR`4TOtEwP{mR|V z%#FRF8ei`kTRDSpOHnA$&OF0R=GOl$gtRjETfPo;cpBaA(FDc5L}o7v9t>Buc6(JEGe-Kss@4S_q}7ha}WKyk3ZXQg?@V5FG)k} z-G8P_i5m8Zdz<5jjCF)v;LTMbXQ>9wI;h49*t=B??8^dQ@ZsBMBItZR`Ukz+oH8CO zu|kl!2G-$t`3L5u#v&vAqA#WeLR>VQO2m(E3l?5BdzKe)K{$UOjh+PU%3(?P4tx*+ z`|>!RSxP%qpl#*Hnb!HH>w_gzGK%Ax7s4DyK2K7*b~8~_`{f;pv8C+MEW9X3$|q0( z`#3V<52$zH$b%)QnyJ~#oRu`Ef5aq^@J@m29Opo!zqCr%k&kJ75rC4aQo)PqioU)- z#4)R*jFxp>kiXjG4L|*%%9_swvGbpErrfNG*0KlVq@mQmLAk??U+kAd z)qA1$d$~_lGWcNaM+O00Bbj@dL+0@3qYKON08?>M`#3bqGa4uwx0_7vjGP%ZQEDPK zSd`j)YBgD5#n<<*`OcR18&S4C9gZB{jT4*SC6hrfggP$fJe-v)_Szf=6FzlvF9-oO ztaW;F$#Dk_-I+A#Y!mfwqy z!;rPH0l*%5LMMcrt|`7eE_qVTK?to531XqN^M;ZvYCJeteQZf??M?Z`7?_NhhOYIAYy1F`U=UzPFHU3&r$|Jh%HKiB_ z%Ql5+x1)eTcT8)QZIq%RJ;Jo>3iGxCsxSr%b3+D&aB-b|KsHps&Hi8 zynk^YZevT}?;X2l`dQn)M@9XY=O~M!IuF?XTsYEmp}u611U->61@98FY{j@NS?r2Y zbusyV8*{AKkc=@5FJI+xF5DF%B}Fzv_FB?w11NNxo_J~l~| z0b6z5CeA7CD>5iGeunV!KipW()-N-apzQ$m;>u9=5d7PCQhc#vx5sCBy&&bpv_?rzf1cB78z6NEvNPD z!=o9O(`i{0qpK2i`Cj0=OwSjx^r~VISN#$s2#MT@a0?Ipo$M|a@ ze;PqGuxB)1BjXswynBazo*IT1-gp16mAdD4z&}vB8wiA)=)g^**lM!pMjzE9GsIlX zFNf>hmJ(M|gBc9VOc4>OvVn~Q^?{O1wm@`AR3o@&(r|HQmiT~OlyX*JLpXB@*`BU> z+19m^h?t!ZM9ooT3&PG%QE<;ut&4o%(*=GCLjrwMqmSfE15&__8)BC?cgq94i)b~f zbxP~w{g<>{FXGev7?SxOG)>08J44+nrN6?e<7`i9+xvvB=J{B3%dI<=E|zC|J*;Qs zc}Eu!Uq_%%Cl5sI+I<*CwtwzNQrs}T=}c6?*nN`iAtAWC!wS@SlEw-Y2_qsNXQZ4@ zB5&43B=ee#w@{ZuK*O{?2=9^oNJny1zF#_hYaNahcU=gH5eA~B)zZ$A^Pi?!-+|OU zF#%>)#d?15!Am?A%iGhp6jd~#-@ZDG27w(#{TkW-D23(a9qL=!T?$`_wbWAk)0*&U>$Tmvlm%7$@sc#T)E7a_Z2xp4clZ2P@4Fw>^Pti%Tm-Se!f#Zg zx~;j3N0of9J3V@rITVlV2oGFi2b>>1CZW@pl@6n~@s_rxsyml!8N-_9Z?)fPT+IqN zd)q{ENzTuhnKnJD9UkCm7Le-$4uxwM=g?XM0=*|fdFx%eTqQy=>L<0dC$HS{!Mr*V@{ zbj6Duh`FBLg2D*0wd|^=y@`1UDW5_nNzZSuEe)?h5tu$N#s9V0g1l!|Qm^5W_NVPK z12sez8+3kht6#l$YQ3noVY@Sm>XGnqJg`C{2%Lj8E3LaISCC;9PK=K4dfxMzbQ7$l zQ3F@|YbfwP%|C~q3xd7=HF+%qbH-&*<M_+jE7b5tNLcLF|3kk_? z&gU~OEz63>HqXaP*6-O*fBj;1^l^RuR+Uu;?gsh=e?vlFU{st;2u?zz$*rK%Z86;K z^R|I>F&_bga&u<7y9OooLG3>NHdsn!Rjzxx`P!E!u3fSR!-^Nawunz1sDn`>0G-GL z9Muz(vPk=a6?YnnY}y|Lu9nolIzRs@L3V>m_f(4S7V+5hk?)dQISa7OJOHF7e=%^#5E{1~iul691 zpRoW4q3&Et!8Y=YDl0;r6={}5AY0CU#~?+bTkqj+@_9dLU%U<)4D|7>@2CKTiv3LO zc5q7Pe+&|8SoM?xuxC7OLGVbp9~+)(Ckpf%Bru8%B8$^DcA4`gNf}rsNpZ z#{~<%b}Jg}Z=u<34#CwO69bNl2W5WE=ri};tZblb?W;t7D@ zoqK(ILFEQUsc8tinKgSTvgPL+cEx^e+<)X0u$xJ`d%vU>>gKd>U-o!scJ2X(-14K; z7y=4UO)5(UE<_^nJ(yOm>L^a~Xj5F78Fa)4VKV5!13@u%#M24p0%Pr`303AJ%lkxU zHxi`pOOPcq-|XTO63^O6qTw}8;z<-7Z2RL_Y0-&ph!^83VEOgeXcfT`sIhXvYn$6C>cU>+Teb`|wrMnY{Eu@%{( zmqcO&FmXI%<=bdt+ek~g)J`-sBcsA6{c7fWWIp;a1-lP&2?rp1hO7{!HO7u1OZ+(p z4Lx+s5%xVWy5;PC)>Jm08;Tqs4At|i8h_nXL(bXrlSOCkGJ0Ug)YaduUq$(ap8xDi&?=U@F)+SfXJKfY2)OtdGeyn6d{*VUW-_evZLj9r~$ zJWS9HaP`&OpO%8Rj}-qL{pZxx&E8NmwcGF1onA<0hChYQsmL0a8nCAg*zT1}WNNe1 z6XR0+Q^j6$z@iizfaSrzJUINHFA0zJYs|~5c~0ru0Q!IK-te?$X^#tb4Z!-X$Venc z>oJ712H`sS1QIOGQkTaS{Y}*`3!b>TPcKyoM=HLkH0t#JUe$C9bt;1eF2?}(9n2$K z?OEXK<;ReD0ob*_&sLc70m9&lB=|EopnD&4_I!C@`-}d~;Yxqa(;hS0x+nexMkEGC zbRt)>zl|D?6*X0=f&)pjRqV%yP)FeusOiBa$u1rm!__vHJ^v|eDQEmU1h4gqvatmH z3lS<6+2ia+OTsff95a&{1^}e!tCr;Q&rDep(c08$Z>FR>I1;m#-%tr-d}x%L-gv^s}KVZ zH@C{FDuBqRrKOGi{E1kIDjb}^Z7l(-swm*OBKY#mW3g@oIYn~2I9wK^%Y9jX3(vina*Qw2# z`ctQqOvdj$c;V@0t|r#y{=%WU-|H3;v%_;*k)OgT?;VCNLw|n@U7EF%3Jz=u3Hg#) z4e#DQhYX=Guhtn@uIK$%D-5+hx~imWKxIf*gMUv*=q@++!zd&?5M^dy@V+bY?c2S@ z82*!_j6CPL#ktEDFbNGf zu{|>dQ;Q$ml<$l(5B7)TbogN9PpUryzm2A@EfjB4y7iXEt?8!3M9O3z9JiQ!iTj_NVdM~>yq@6*$Zo5g78fSVB`uhWessWJQ%1zK2KTnt#T!s7$5 zh96P-tqx|^lyOF(W$lOA_JQeA!YkvQ@qWK)x19(HJ9Tz&SO*k?N<5R__qW}^Mm3X{ zf_SqOj0vwe5cChWS_Gf#BliMZLojL9X6ldt*EpaE1BQ$P(J;V)4$WqPuT^lyoRamI zjJuKo3zKuT}ECeM6eZVWgKpTWeXPc(Wt%BUFj02uxK$?IabZXcNu*X#ss15@m- zt}Xxt3b_DRffOucO|W{d7Ig@E$3ZSC#`5%NyODFWX??VSlL6iVDVOl1)vup2MSW__ zoAuu;EhzE41Pj*;JNmbvM}x{m2lE}tcdl<`PS!qI4H$mb5r%Ev8+kMz?B~}2W~3ME zL_ndV8wECLsu%!yXp?0lEwh|wy#S4BM+|DdTD+Yo0FayCm>GmP+wj3D)*go_nwNfnr)in{L67=5*W8}l5sGY6m^ z0iC-B4`Y#y#N*1Jn*|Yl4u`s+`Uuvb$tEx|4TWDx)a|bOE4efuj;B}zt%DgGq65vi zQbCc(X_lQ_LVkuZx`5ohD@<^u`TS^xkKjv}&o;e}oh| zA8f2RH26jDHGlrxt$ck)2LO8Rpt{`}>Tza_#nUN*KWh7GU%s2>D{60V7q|rm^=irO zd)`_H{@+C(y390aYf_MQEJ5zV{w3I&v}<$FIzKY$-H@2j+RfC0nw9B}EKkTpX(`|P zcVbo1eBm!9(JW=?iK3Ho*vWTHHdyux(1WX0M-HEAKe#|uFR*#ZXsN|DP$T8>xJb$lQxz&&31^~(sm)THhT)LI?NE{_vFWPx9Y zm@xrX(|I1C6&ZDY`?f?$DJf9#vx`$C&G3s{^&S*pMlS$8pC;_^5P*EVvNvZ^VcDi{X!h)_gG?iL(~eYW54ucQFzVvn?5yePi^+;Mtn8qZ zFEJf&Z>aL@UP1gX_RN3*Ua;xM%Pn(MRdp}r=3kQWYGt#*0`&FufrlW|u##T}G??^$ zzfj|?8uP_d;8&S4wVUXVRb?8QnZ0-k*vif>1+fz2kA;P6B{4{IEv*57ZU89mx&yg6 zE-6#t5aE9}Ri*;SAOONX|9M3y&=B5m|GsB-szYfe^)4law$_HI#JqXBOX1Xm=VNx? z69)LT0MrieojX6I2!_nGH-JU~OugxY|B+cCXqMqO(A(dEN|*;6E79Y}kMmgIJqfJY zYp;vJsCmrq%t3i8ToLW!L)UpK+?O9LLc>%UhJc%@Ol$TOGq7un_TOEPkTUl?F7iXo zdgw&&%}s36U7Vxf0A!hvp2K0d_<=)fYH1!61dHha;@{;ZvgI-h9rVz*;zzZe&ORV{ z-_4e>|GT)J-ILAo^b}x*_(LYtpfS&YprX+=^?nJq(T4(_okd+EBYX2=gVOfTnh>({ z-3~Iqibi`rzyyZ#--1Sv9Pa4on0a%g%}d#CW6!MCX=*W-n~$%y6?9??OGobftKWxL zz($GuqZ?Nak#KnL6B;-ft1QzhT>wzQ113=K(rbTg0C~xtKSCqL%=?LCCo09)h>06$oDu<#4*K}E3m ze`}(+p;B*TJDA}xzg-76=if7nu>gMb3I`hSufoS~1)-OQ}z?iyXkI%qGgerxkw7AyZ`9fQ$ z|H6vH`5-WROtI5NUu=xz$M`9R6TAK0qK>N;9c@ifnOgu0RwI7~m>+TPR+pDWeh&I< zvscOkGqW3UcxehirVk>H-~0N2g(~b4)W=(AB)@6#)lB9PE$jFDX>)Fx19D&&(Q$|f z+1S_snC-?>>-I1L&@mW}ZGS3)lNZxzE-Rxmk^$HPfu!0>GSH0#OHyCF8(PRQNLcydF2{I6K3k z`ym@N2JG=A>mP+sXR4aQgZpi_iXAB6&wk`8pD?3c7NPco=NrZ4wMGXDaaU$$v@sCh zV0J=;6M#~ZPlWE*E@paeivXc8E4NGFpotW%74(b8~if2FTDsuD>_{zDLU#x8JGT zBYHtJ1c35NZ_gJS^2^N7^mwfeE5O?UTXID#*DdE6LQ;x~iZU{oG6(G8G2<(qJF`z> z+`iuWe6a)#)@2PmcpJ1=Rb5?UWD)$Ryx4;@v#qdi6EBBVD=EfxPtJRbR_P^M$l~rn zJaa(BOm!P~>HzMi%wF%x&?8dsl2?zk7`?5Qn0Sbtnq9ZDPhsU5%uC&e7Klt|72@LB0IRroa?M40{ul!$r9-_V~@T%;;Z~9|U{WbXD{C^Iv zs{2#t)n$LK{m%iY`v1#K^0JxQIg?JJ$3rC(o4TEJtAaUMLw2~S4iG%^a>KPRo04SF z+un@POM9uof8=DR&mJ?7mwU{AIl~}s4v!qJ08zdAzyyX71aB7);oqDkrLPYJlKP!Y zDby)A*@=?Cz?iNrjsEO#RRTF2k!{37le2#e!A(G#@}&$*uvuA_nnT&;F_NH!bzbgr8zT$Jiteh5MS!A150UjeNm&P2Ktg@m-gbt92DSECb& zd~}Ts*73ZCK?kzB=S$4=2Ss7ar;cXzF2T5OB@oNvSo%Kvi54&6lR`p;S1MK%nB4bo zQR53}Vn298i52-W41*5yCWd@&$irwgrR1G!*Cs?f7miw`FXvIsGX0ka{>|-3)Tu%7 z^_Z?LZ>exAl}|sG-Mg0dvS)4RiFns|3RjTIgaI4%OT)R~~ax4i202jSr{rcN1G<;_J=#aF{ za##Pj>?~fV;8FR+l3$h;9dQRSZs>sRl6aYUdHICn;VDyss(PY(L&nhRCkF;)y~LYO z-fP{Y-1ZKrG=hBhmCZIZ6=ooLo${=~h^O826`Q9dv$EbUnB$j*{6VtWs-N_}JSL3j zSitIWdxLeuxsI{XT>u18nm6B#UYt?tUfN4ohO*%k%4KTbO(uMbr$UZ>Yzrq9PKsLO z3c2LO)@8#`7#hZiWW&PZ!dKH1CTGLJ$N6?ZsDMLGp1VfyKq-&DJyK}I=+BI~_3k(G7dkzItYPMIgS&@RVKeRxv5;H-=tFO4V6(aYn`QT?@8-h0 zciunCnYi#j-f86H;A^jKdX4QkQ%iU+Q~S}EBy0tyE}bGc8waS92hMug%#zT<^+ovx zLtL)2fx_^T4|L9m=eGY_HEhP+4#Y=UgW|DYy;VFJObI=jw{Q5TdP?`;8T+!;|3tj! za6zPj;e{+C-y>61rOO_^zOQf~us(@IMMp+1U$Gajr)YX(M%Tj-$zj9KYh>@h?W*a$ zcPOzq?jC6c^T7pM5;-+{1r`nDo|I3d8mNnZcHl_8)7)iEu`0O^v zzScuugs@@S2g}Q=E0Kq&-Dk%jpope^i>;2GqjX1^3!6KKi(IKlmkk}%J=Y_~&@(tY zm3fXONjAQ?0}--GLuCcb?{mztFf(r>k^DS7g)@>Kbd{{9WP5>reoqEg0Ay1cTToC? zTU*OEoH>w59tDDG-FW z>Y4!h!I%|^&A9zWUTR@|QKy~GW^XQ4h3a_}2m_WDhRM?#?wOeoNR+C+T17%Kql6Kk zAVERe&cxX+ua;iPlarW-CB4lpF20MO*RN-(+u8(!{T{n5=JNEZ(ygHTkLTfV8zS9O zhg^T>>#Ma<-*c3j>gyL2hL>k%W-jZ+Brg-G9B~rKK*#TpMwR5WE_{fIiMhFe^D-pU zdeZU}0aD09R8(}Fyf8?k%jfxzoSdBDyn+HdFR#O2zrtW)+uPewQ3TJI*esh9y02PJ zXA>(JtnU;mzbhHcAsieO)QuGo5P)B`abksfXb7=;)G60LQK1gb-4$i#=jFwD8E0y? zm${q4wzaeKc^>0bevZJx+`Rjqn%X0zfxIX%s7{!XmJ zKAY|1vmUI+p~g=+UeTIu%OAkk)(T~jAg21H<|kvlq_}u$riZH&Igx8(&WM~g5pQUm zVD}*K6qE z#@EX@c6^iEPoju)HS*LO&5a% zeTn^Y3uZ%CJ{ZaQ@}`BftgN{Pq$hf8YATKYBd6qNtJGxwkM^gfG;psQMF?&W4-Y}i z3my{n?6kC~8FU~r`K;xD1G zFxjYYUC+m0msh65zh8)vNPZoIfDq%>OALI4o9vXB)L7*51Yv~u{pTIbOU;r}b7r!v zLuA7{{*H>P(gSJqbRXx=5Qv=XaR~{h5$^7@SWQh$o}PKUpK?e##ErYrm=ft@DQjEs>G!K0plL2}3&h+TXWUu8g>ytlGOf z2W>WNo0&JJ)xWmvcd%AsSDH`C#n+@dEi|qDeMr1 zcai!PE)C&NX+Hg3LTIl$`jno^WLwXB*Z{3PrDVmzBOoEMj`CpnsRTk4LHv`NETo)Q z1nZq>amWi3S%{s>is8D)^A|pNv!FFJy_kcr*?J+@(8Q0> z4_{Ug0V+9yGNaCy(Dk@?j~x=0l09Dl4RqDIp`oGDwY|McLKqCj^Ko0<^c`Ah+`VjW zbaY9FVyBK0CMP5FgxR>`^qknb2A2+Id>-@g02CN>r@o?M0VPB>{y*)5!{K$3#9KvJ z(LMW!fQ+{__4Sj^XzhWZv&Dl0&llI_;GyuA#>Njk+zHLrzKoUn{!aP+5QvkPZ$R)( z2z+`vT4>e|{j{=>t=Cxd5aO>$nY zuEWw0kiqc1gnVUXr#YGO$NSC`1kls)!36e7MSw#Mnq@2_dj4f}bTr;zedqV@ zdzbf2h~dYX=77`L+4+6phxn?K-(AXxc`3&gfjszx+Bu8In@Vv%P6r{7Q9hnHSIk?QwC zsw~7!%#gsv#KcNTm*TJH{?9@^&PJR9E@|xe?5u>GA$Fw$m6OBX(pXnFVU1?dXizwC zA_xr))fa+zs;V;dT!@0^5&-bA<&M=Sir{2je0X37(#+G8skFAT(#^}OE8@e|4mg&1 zjTA0-q=X4getsSI`26{I%EHE7RilvepE}`OE#DnQAa+VEpb>g#J`RrGb*B-{ zQX!E}F&!8|KBYA=`dw^a$d$M$g&!6@RK|Sx_$Ce^C1no2x^m6~_WRL#Sh4rRqA2Af z!&tbkk13tO@sLQmm05=&O_$YXB%UjB)-J$x$Y<#IBG?mMhA2PheE;^z(XNb?lu3E6 zi_8-QG>u}DSdcfY^4s_Czh^ZyHP=S9b#!E|eK6`Gi+6xVAg6s20(W}Nd|R5D78nU? zQ~mZ3E|FbD-Iz*A{>1PoI+$ZX7S*qn9YnyRM~_~~RcdbaP;>(wuOkTIsWqSMk{BRA5%jT5dw>qE$ZS=;CvZ@&;@O-MjZSgsUbp5+Oy=;j_)^6FPJR zTA`s4bJG$6F%5ooUs|u5DDUKG+r-6%-<+3^pWjVWcXicqHQ*!+SiPX}k&$#TDdBvl z7cb7uw~61s=p})BxVw*!kN3YVNJVlRtSL^U1FEV^t3Uc=ZB6aW@_J^{F0;&57G}Wk z7AwmZ0{U!n3*f~4{lvC4=V!;GLaizM7T(_8VUe;p9X=f4NZT0y7cWZ1T?ych&!5X0 zONG)32wLv)%)|T7sS1C;qg!WzQBhot6c{R4OUvaC`Wzs8-|x=dTcD1~y%~;t_Uu`z z4>w0-I4|SA4FE6KGH=|tv50$jo9tY$=Jvb{1TYFC5r9LqR%|xPDVwWAg8h441AcOH zlDd7wa6$#fCO66YYDW~M0K7;}4pQfmT}N!%MU+D)P^8Kqcz!aL)zooxBnl#e6#p3& zU7v~@DWN->ho8uyAH6sJV!3c;1su)fdZ)|L|uI+`&Ly5eD zH?Pss(sq&kc0}zm>q$xO9J254?=KbrTTu{VxVgD$#5r>@rqsEc$%R7)D=#bCxJ7J# ze70*=ym}KeE-vomU2UzX;9~=Wjr^k{@4e6l1!DLp*+0V*ucf|d=ixEx#GtU4;fs#X z@@bB~@Zsf1SOxY_d9bC1SJQ5wx3>W?kOsC=egITy%3S#N<*@eED=fG0h_!1G;o+TR zD>aP~NTANM5NPzISjpw#W=a6M%-`Stz=4^W`TkY^A0{A9kB=KJgM|+fQVT|TXhA66 zUL#j{QD#K6En`WXrQ zuS^Ya1|k6pTF2gzO|!K+fS_}4T(S?tcEI} z(7U+$01-!d`uN0TL!nZ68*7?mukQm*kA-`1l-h$;VXNKLs@mbxQ+BL&lVfjgc6Q-u zqg|`uZfYy@C6(Yq5pw*GR2FBQOaVO{23LFW$E6gcC+DjUK3v3lJcs{>{o}KRPg0^{ zA4N#Zv3m*OfW+(~EcNskYW%%R7Ukng-Vw0HU)E+hECc}rycnd1LI#X0SR)|p_9AkZ z+3zl;ca)8TL+`w+H5t6dUfJ&X>kg}^Ypd}CFSHU85?Bf0l^1}E7qNQ@bVnb2D-%=c zsS9m4zjvt_5s?Y^P0A>)>vwY1sM*;lBeB%;??0dp%cbBTP|oAlzm-JE{qA8BWe&$3 zd~76Y>CmiaHcd*T8hXBd_GT$wAivU030!SLs0Z)0WT)aYwzY93!4w(Z(Li6yalksZwo0N}`}$&%CYlx9mzS64voQF#4%h8R*ScljvmQ7h}onnMbv(#lJe505-Mrl+StV8N{AQ`2imYWSTuu1854 zjWjegO$=iGji%<)Q&SELGK*3fnVEr?7iRMU$cdRgpsaamzuqHc{!;e-`ZN$+AP+G% zVq@dE>hS#hPV?IKHg8S7j4N)p4GsJmZMyJi3k)X85}%L&lr{m#R4UHCGE~EIS7d}64KD8C>@BbV$a?eap*5dxHlx!a_FVJh|)b4kJrKP3)qzq(h zTY-Ur_Zk@J>7%Om&1@46FiT2G=*XISzJGt9S^9)(|!h$7m z0^Qvc^}pV~g>n>Vsjs)hGFGvD`t;!7KvPF&Xkfri4$zQ0%iZ1G4KIwmYTY7A2^0E*0h7a}PsNw?Aw z6>o9!)Gg1h3dOXWvebLRN&)W`tjMekR$=8#xuM*dmPs7lbNuDYm-vr>Mkpy@{qlCp zg>7?T&@CD$b0w34xJ+S9Z+;LZT4bD#BOKq3&uwGV=&lFEo_@{;v~f@@VPk=Vc}t?>ov}%W@aKkjEs$|b|kG7J`}XE4$*J` z3t<-N;^dTSWR;O|>cbKGy#)&0>>iCm-hf?O7CgUz5NZpf-+oLcu4dbR^4I!A5O@X! zT|`78y`RU0?*N`j0(N$B9$;^;cJV#l;!1}vN&$=X!jIX+T)p)SRd?!XYx!6$gt5P# zrFmC&QL4fzyHXB}&7U0w_EpklG&;!Fcdb^Jgp_nqBP%H>Opjo2UHCyYsiUXo!SQig z2)fIMNae0k+i+P|q_AO|m9&IJn%>&QMbJ-IMk+vknV6VVUC&HgQ~NZY6#r5l1MUqm zOtt@Pd%CxTSEnQsWjeUo6sVFO&2WR6^reG+_?Neber@gT_c!!UzRwAeN<4k?L>fD^ z?epi)B_%vG{qFle^J8--#>V!(8*HX5d@+9fxU*Srb7WOpKXyP{t^fR7<`JpzI?!W_ zwjFkz>c4f+ZCreOw!|4$uAaf?X;8c=T@S zefT!lJaSLey<>lUE;vV*>jF=a-Oc@DL&pBnEiJF_d#O%^&OWs8p{L9x4dV>B@n12C zRwI2NGb+eH&%ytG`@e6pjD35*bnL>#p)`LuK(47zqM%}2@`{?d`d@^eFGA6z zf73T?uowX9{KZFrpZUPo|IZhuL?QoOqInDw74@f-t73!WCjyWXc6XLFMc_kdRndz=7Y*DCYd;=_#ND4N8pOe)v!XfFoYsfws1`fdO7lPCIMs#PoD1i0EgP z3)1~@4&ruQyMP}AGGTG?i>rV}*aCiDVd1ylUUd)v2P_UC)YPz;fk$lc=#j_ahJGS0 z8~_uD#jA9k{(kHeTI-U3A^>N@Nf-gyrwSR)l=iF2$!QxL)CQIxh;hrzG*eOO{`QSa zN-ALVv(`t~8#ty7?)mxoICN2fl5VwsW@QB)6OfpgSoQU5e_!8BbAXt-vy|^H+b}&a zxq+!C{|SA3OhQZy*wlC?2`oz_=|`uMeYyBcCvpP)F#Md42I)?EFAd(b?r^{1`mW+o zG-rfS@=dM^`&F`Nx_k%+rQ$2hwn$uDY=Pl5)MZA-Y8KpKMHVF}^!Z1Vh20=a@M>e$ z>8!+M|0=3EyBvgj<6kW!c`&^g2&f}qG64-P0t3ktBOsz9Q_5#r4dguS0(Spyh6s$B zD}h1eL9fe3c?$?_?#?Nw={-Ie)$w|7$??Uod>%wMV1{y4s|{V_llpO zkVvXwyu5eUv`+8{;3AL9ewmH#sBpYq0Z(!GiqOgs*Q@?@I~Alc?}5NO(IyZ(3c~GX zus=zhgXFR4T~b!rk|D)ZH4u|j2GZ4Jk0x!>B>#Je81x>GEYj2ho>|H52qaevi#3Ck zd_kM;pJiq<;z>VUX6t|4UhQEGh5)KN_^tTnJsLh!Ix4Da5LXGp6q+wiHgO24(aIND zi-6g;f4kupTi~%eC@S&becTPZ!3+>D!~(_+gtWfrGb`dt0eN{E9qZkCASLvA6L3o+ zi0N}3h2$Oy#c~8CZ-bm|vr!a?CDUz-acer+C>D(cPrP7M1;GX&f7VYX!Fl?#R{Fuc zfP=N1BbFuI^B59ER$bl0xYI9JDS9A4QR>6F;b#2&YY?NPW3#nmQeLNzBO*%dwLll~ z$~UlH0nT!ATMrRy?qqM-0(R3$cyALz>d$h)Xb18I_ydDmD+?g^#kA&mvR3?}1I~E& zeuUhaf&^6SzSeSg4)<`{+Jq9_DGxF#%1*fGSXZ2N1jPoG+&YtP=92xvhjTInID4g!dQ6 zrgM;%FWQM}GlqQFDgu#UpFyswXp=%HE`7p9(nX2ZeJS@jp9jcH5}Vn2x7#;C9wr9j zisZi9lx&a*{3G7FS<1YQo1MJ2U-XOLU+)32CUXv(%>gK5iz0bJ((7~BEf*lO$5DFL zm&NZFg%6F?x{^0YfBS0iSZl|>cgM3S*r4q80f&Eq9fNZpGPE@`$Q79wuM?E+@=0sM3 zq{SsPcp}4DhtJrT2o@^s{0GUgSIM;oOd$5h?XDY#dCuAqgANFsc)0JcOy8_iD+?mA zQCf!2bVFPc`x*Wt6+j&5?p5ex_ooXQ#t@aFIK|=za}4x8OklPD5DjU$I4tje1QKo7 zCd!4MWG?L(|3ssG)4d-bTQh;X4$0smAuc!cZp9DLhZ^KG@*ay~h>;ssxjZTW8+zx$ z=x#hns;ggR5={hQLeJ)Rnc4n?Q(w(b7_!E3LX_ov8uugabup`n*y=+iRaM4_3mryz zkUAR6=`!D>P-cOsfwFsp=W>W@eHrc+2anKLVugvh%u9cx57kkPtF2%dnyj>WfdNuO zg;}=!$4;#uDd327f7)TYODb|imtbBWcT-SMZ|pKn_`hY4s9m^5SYQ&DUc0Y(z_v)8 z;hgxHVR*Ral?c?%mfa3M2Xb5}3v{EavvacbKFP|}0-mXJDgWTB1%bQuFsH;UDBjoe zZQX4K0e_;DaT^iPG46iBc%apSDO!Q#kF^1UQIuHb5@h(*JTd%#h%u zwPO!EcJiN-LIW5~T}#N?+ZN7N=u0!<>-(pl4$E-%ZmE;Y@|5Y$yChG}|9W(zVeyWh z{;Za&dNCR_on}3NoP4>U6-oFbpmUfi0Pgrx2X`YcXrvN4{l0w%C?lup$Dr6B=mUfZ zRjp#DEcyx&yc-?!fYRd?_O;72&$i`9i_s-3$#9&&4BSV$t^yQQ<= zgxJ+Qx*vjiH(~8+4O?7uKdYn%O3?2S;KOGsi(9@tu!E*t0H}!q_8B-u+{E#jkaq6U zk1)$Is?GKX7*XMGOfa90W`t_%QP2KTs`Tq8s@rYi>#B?Y1m zp4<;2;LTYA*iD}+TNlhBknkB3XBso(M5P!j*15Sk&_I9_d_~KlTkK?uKB=2KLcV3t zOY=l=c#=O?|Fh>x#-|=pXkIi@Ne44u-Uwh_TP5X6|A#g z62O>+87}ATj z-Oyc;k|hV|;$Pg4ua6WnWCb;pmtwa(^?>aHqY|uv#c9owN!VZ>^%|#d1+BY3g|7s^ zBp}+)B&TTC=A6CE#oaRXaM(FXFvh|tp$`Qcyg;g9f2-Wj5SpNcy`X)ZG2n<+%q0K4 z&y|3{`vNZEc@r*2SR?%oELUbZV_^CZ^q}YO9@RUg zl^MrMg!kq~9PLTQ6Privv!GkM_c{Pt6O@KJ%@1#{K6>NsXkt?=yA6jz$Io5O4E(zG z)ER0A4RR0xnv~vRXV|Ry?({X;+wCpy15)qrE1iZ`7%!TZ}wgb))Tc;EGSPl?9sJ3%u;Lj&_3d;$Q7%j4J4(9?zjFm1rg&_LEl?Kx^pPFNocmK7p+UN7$=Jn|c3NBy>xPg9&D8)f z62z#fz}67G;tm#6xEMM|W9`SA2ADk-u`3+xFWgAe`JvC}!FyZFROMBUdoefGuFzr0 z=;CnW9s_nKSddSFIuxsm@?#v_;9MuZ-Mwk*uiT);hr|`_U9rsX{a``P*zXP&Y$ml8 zoZHSn`&oo*M0MTft~*qB97?)466?)=i9MvO;@7Xw=OMP&1;&k`LO`8EQ-L|#gx|$M z7Cv*QVz6+b#-&RNNKd$A_)8`4SsfLMc)pEXcTW*ZJE%(4Vk=4R-;Hc5W#)|m!{zaO z$MgLvq)ZFu%31hvAubWVN%s;I$ZEp(06_YASimLTfv2W^20TVp@B-k3E>3kezeqBW zN;k>{#RSe&Ak|M|gUjvoVf#_M+K!Az>H`G8*|(a-Jw@-z#;vs26$D7~Bx+nTXEjS5 z2gU^t(VpzbZ=+SNUkq16r}bU$wDQT+6SnB+pK)gfH zldM`B8Ev~}rGc>o`kBURWO?l5d>IcHop2fOX%v#MW?dWV(#6;gl{dV=>~Kf7=~_X@ z1!xi=h%yC7y6B0!Aok|FlOx99XPb0pCnyCRKqgG}^3~}dh0q|eNK3yDyfp~RnJ&16 zA~8i(NcBG|u`;_Hg1cvr967Rx4tsd?jQRb`yV?oTwwdx5C-2U1_>kPs=4$kS^UNpp zQ|$o$`hz^zLxW1zVd9QI+?hW?*vR=_<|WO!N9{xC=`MOR588YS$L>5!njXc|YpzO> zqKz3k5<}zAfT4I85&K2F%Mee?^=1H-PV`f4nu|vTvpPqAz`?<<+`A>8Z`BLVf#s=2 zGxdZ47^Mi7-D72dAGdx74o+w#`XvCuIF0?8MBt;*t!{LjWeOq;+-{P)Oh>)a^n}N zm}P~tiIlPU0D#!r5lV<=3C6G=^mua}W6-nU>SuKqr z9|QA*S4~1n|SBh|CCx#G3MZa>)B@n@QR?r=+=kx zDIzk)dy5>_a;<&<%KY0|utwy_FahJ#CLKcJorCtJHqDA6DOYdOUz86WPE8(7&+p^b z!xq!HwOr0@b1j=IN|2AStBVu^XHl&lYxS1zc4!ic5l%qjjk_PM zG6KoY@t!+7Y!FWpy#3A90G~-%<4%OmH+645<}GpBj8AU9+OY^7U*SiU+1$V<^ISb4 z4WT*`Bp_DFYl3il0`&YCFur^}v9+x57ce5AQNvVK82pZ{-iO2x;zDeO6}N#;>GNTS z#>I)&w|~EL#RXOY56lhIng1X^1)s32yxar8p+!HgX{ckkEd6u@db zLTli$EoIT&x=yg~Xh0NY;`!ZTVEwh%*EHwO9;Xv`i1j6X23 zKyZ(+7G8Q>b@c4rZH@ww6D0VG{=x1Nrva-=p$3H0e=c;LcphX28+M; ztK+kLkc_hn@J>MM-5U_WU}%kb`2CsKlRp=jZo^ZphK5vID|?f}lS0D#jY9jvR~WDV z?QsY#0Z^_9IS4sGQbQc#IN6pCo*4L{E`Q$1!H6}WmmG2xecCzTb2_iD*lr*0D@Dq` z9!dh$+s9on%k_~+QCkDp5Flg#=yv&7V|;5u#)GA#XqQw_+t5i8;G zq2LMKRMm4!A0zBP*CsOuAw)JDE@LNNl7NQ7b37r`ULKkol%o)@fZh?DGu@yO$dZwu z3(I0NrO<*&I<)m-uc$P{R#;xtDRIet^_^Y>x7=k8i2l}Z1+UtgojFXwYXloQ0JLN{ z@Mf8nIWD`cfQ*DBS;Oi;vyHUFM6jYBG?OItv2B`4RB4?W?5$S}G+3pw-bkNhe%S8m zqH3O@lUNm+iJ1|%3rTUlp^Viu5zNTA!Mssn1^(wmva*UFpM1K|L`D{quT^lNjEiXt zX6M#1>L{dvgM_;ZL4z`(3RV=1L#%r&w|FLaaKGQ5QS-9O2nW6twKN4poxDR>$(F~P zjUl%H)S?vVCUBhUh_y^xX8s&%xT9Gy@xP9=T{yYH4QWXlty3#cFSk3O-TvjlLI!50 zDoh|r-dv=|M)u8-GwhHq0#9ikTtSsjnP%aTAT!Z2g@mP>+d2r$l(qlC3%4%-txNpB z-`;Az8wBw*C+7ri&%R^l86_OQnEe6^p#>Hj`Zh)&v-LT*+X^!c4Hp{4eUMgz1kO1{ z^*k(izd>9|UAN^&nXW2Kq@e2ywme+arUdvSd>Eh!EhU_!6&=8(u*!1u|d z7FYlA;t;r=wx$bv{fqJ{XNi#2d$1N{7QaYs}$b)`&8T)QmPcKzomuw9we zS91+uyeN}!k>(L-L9=_ja5jNrHboCdPRqom8tWF=3_v@Q#Ve|kAzU&V>0DBz`!5Ts z>4r+=o0V{!uvx%Z)4=0wtDan0EZ{K07hESEFj+$8Z9GwC8ZFQSlo@b&?JOtf2oS0*`@j!3CI73P8ZNJ06y5?|jT*#i zpgqw9PaoLHPMuTemfl!YS;A?-!<%W$&re2E6Z@xgccTVKV5g4;L$iMpIJ9RJf{$C` z4F1akW5K#ZvKtE#131q>qJ=Vby#c=K_M)cVtlm*t?;(o5FfyybblMHkZ_v@X@{3Lh zf}+rG8_ZxY{yyvX?AVnN$BDtLbZNH|lltJj5v1b$0gk=W(Zc?V7+>IHtBq&}^#!I@ z5L~R!)9ui}yqP@qg8sedAU;R438H!Aw*>)Q)*GRhhz;!{@r~uSx!v8luLl!CbyM%g z?l2|g0^j)Nq96jtrz?&l{rNCVGTI<^=gU6Dkc4;~zu9(t%Y(LkXcEgeYgNG9lkd)i zPUB4}Mda!$vBiIEV}YFSo_xfSk{PjYSg|{XIzB!gXl8q$6IaWsuQwd2mC*4uiasUALZ>CK7& z1}C<%y2y4aIF))mb-86DW!-reObYl>Q+*|_;ACVQ)tWskgdl6OB?Zh!44AeU33MG$ ze!~o35MB_NXy7D*_AGt(VuR%%Zv>kJ=g|~uii?A`z);rHvWf~|IH<@hp7-HwQ+#;l z)(6#HwXTGceA}TL`!?715S9*WdboK6&q&Ud!mc3BhD715sOY z<(lj*>CQRp>jStdjTGsl7p1F#e;P_$P(92;8o!bG5o^3=0jsyPbNBva;JyHNs8uHd z+*zPCF)wTmf=+8s3egj+P>#kR_(f|y?(jh%Bej7<19ZACf&1{24c#+Wx|7y^qQw-L z|De-c>E|Sr=St?*XV5qj5Fl7SIYLK=gML;ZVuuV(s*M0tRVJESC=)**cAJCBlx1i3 zu+I-7l5uRyfuIU9-(yR<9$k<^?ggU0dguiQXU}tOoQtIcoOf5Kk3x|_0vImJS*&ZI zzx>_F>oYhC8R;xeh>0Z~3-m|^Vw9M^Uywu?w*+Ux97xGPw}R0EBjK@{Q-2u~XAb^E z7~c@^F1LDIkP7jzF9v%`Ue3q4FSTmGNuWcPUbToIiPdk6J{NCTbe4)jkBfGi)89z* zRbbj$zn8P_|7WX~UN^q|m!c(QV)OXM=Gdkktcf|2YEQ|<=|(gRa*=|-2B8LAN3O19 zoUpA`k9Zh--ekG+6&Sp|2SOrBVUZHQUKVPXHf=gRGzJtifnI$vcK}_%aoVkY<^6lK zA|~SI(Qt%`^_2)@*lOQLJcPVZ8Pt?IXMy<=^oSLm@5mz9RI{HFZYy6e)9RlroXo)Z zvrDaY7zP~1QZ$jFUHK7^B*NiGwr>|5?q!&P?eYjEer)nUxsuF?q;L|6X-VIH*X$5dI7>zU69EgyJ*%= zdMeE=0d4%vCDHQcLObIK)-%=R?*xqNz|ICh76vH%VAyJ*)!8yeUSZ`Mi+pN z5Lqv}cpo$~RhMAV~<_=WMm)GzU4t573ZF7afV|?cEd2;;9E&hwdKAq!k(K zmc^guj%<8;;J_3xWw|q?6_Z}DC#>daoUMEn6MA*9H{)2T5kgLFJboD0G{f+ z0fhgrJofbo(_l|AS9;~E#Rrtc<{=H>I;2cb3em-Jx8w@7I0@QJM^hIFj#;uY|I8mq zDd=$$EF*AWy)X0iUrwSlxMx?eScMjqcL%VF8OXYEd?`!%odJZV=0*1720vK@O#S+-KOGBCE(^&T80o-@JL)H*z2qzI0@jMlXP5&9ZK0VMeXrVLvjE>yjl z$UdcPS|Bl#-kaJM%f~zfnSfZoYb2T8MQI4D`j;j4y1&I&F$~=Yk_o6QQMs|a;kI?F zWBf+bYWX{#s^DqCd-n)&xL^WJtkCoZ0sk&F`t_YfcOI|hPc-V`D7P0u$x4e^#L~?q z$gG^aHrstw^NY)A{g{m~+m0GoFYq4ze<`!NJoPi4>c|rIg3JzcfMdyodsfP)XUs#& z=s<_p%&xw2{5?0`fBcv{K@r@$<)=ibwzF%_&n@e>UmLmXJc;{2h;Zm%L<^s`gUmN_ zTA(sNQgh+W*O((`6d3D)+G%7tl||0vaWYOmh?A)Tl-5S3ClF|t2SFh6@gpP4{p9cK zcnTQ}zP}n><{JV~D-At4>~`}k4J{)UJ=M{8BTjk4CSUWVSa~DPlfu8_qp)ST?Nm=; z^Q+iV(D&KGlc~FO?IvAFKrQL{R#w%LK!=^o*(G1Gya9yr++|y?&6U9f4m7J4&W8BM z+#qP;SFJnlHAxiIJInjuu;^Pdh*87?cgL;o!S>)ue36q9C_ij-%oQAm@+?n^N6e(g z;1nOH+O%tOgXEr&uJ&YQS^8;Cul@ndo#XfDu$al)Iw`oDmiIvO`i%@Kq9ErB*8un? z0eh%0DwmRUVaxxqxF>ai_&j~5fXB#X50%U^yys%?o7%SP2+n9zjsAx1pgGb6**iuh zP~z#vK*wDkbV}|ZOHJJMx4woDkPT$$o$8ji`}1@sb=N}}ARTwj@4Ew-S472BUftpj z@jq~<$6mkLq)TqJ?lJ%12G&%m23&2Bz_4FturNj>#sBw87zw;qhUu{EjgF#Q(r3CJ z9Y*q9hp^9Q|3^jLKWiKRFGbyT34Esd|6)=1`TF6_6L4xTNn<946vMmolGDg#@x;k> z0;i=^(VCmVvn9E|EA{`SCkz zUL{a}DQ}LIrDpLJ4(60-5c%I$1@3SAh1vGiJ2{lfSar$2{RfyN?PAy@ghwjpgBbj~ zc>X0w#I>bKmH-+vOO)ApTap$Kqm6&?WvPHPjU`H_$o?YxnY_#GdY^9|MqBCtHLEWI zw7E+50sq=t!RUu_KyB+myt^GK=EYaG`u zfayme8Gs|6M)mXVIh+;~LGJ{JVOjBQY-@wwc5}jIn^7?Y4l4+;he*i5B`?z5AMAV| zu}*Vw5;o~i7+JgyqI!7tdu;glLW<5zQj=za&)fkV>8f;>{CA7f-6ByxiNIOF7{F`@ znN%Q3vAPvt6}*&Sc!csY&FW1pn0~`kjmsK8&q)ZYAlH7c zr{$W;IVby9cF-~ZlajHW+&;A%ncv+au0eXpHwjQ`ZK3{$TGE2*21vke?c)W>aG+Se&SRFI32{XYD;z8d4OuT027gnZbGthn;41S=g0N40hc%!3NSN^#*%lc?Ng#ZNexY0}=bgy0#G=>= z>SB*o$gB9u7Uofw5i<1MzPDul*GtE;|Gjh^Cb^}gNOvmu{78>_%4e8_9Vvp2Js;xJ zj(A8KHnW2GAramGLmk`GkGV~f|#j0+92 zv`hh2gN6nM_l{wCZu4v9eTlH0>czB+x3C!g647p(b%G=rf4$da$cJ+(z z27+lb>LqWpdynejEPx!XcL`Dj0jbJd+Ds*a79BR=(}(ln0a^#SbJa(g8h`Fg@@S#c zrqu%CZ4lym4SqM;*0L94$+GP!G~tkW8N-fywob%XHxn=LVpBF29wl;c!`3ph7r+B1KXjgACCvE&+G!4T6Ua>ii@b%-DzH!8 z14Xrv1FXZ};M$3#irt7YTmRtZ`J{W0kmT<_{{E^+$_yiRx&J-gXh4Vkjei~VFlkQ@ zh_G*8pyyV-D_3}c|HO%_o}qhhC}+PKR&m_8Oh*Jp@NWl)$*~1^?|ZIgwN?jbngq04 zXLW)fW2jG%7eAyJa`hxpLE6;#am z4aqDm#D>jA*qxZ`fux-P+P~^kgsKQpw}?o`w!cH~@W0N`^%EA5;qc?n4Yr?bn_e7m z7Cw@0q{bW#LmrwO@a7-`HC^+cwd=GtqeP)5@>|YDKsiNR_{P2rse-nwryHFBo)YAZ zCJ$gtF6pGL-ag&MnkJ*qqU+}72HwwYC!ixN=&p|+VTzfRT2vgQL_%U&0+`X*nds>s zOHM3gA-S-Akv}(okV8nLWaDbD{YCN)?f0W-B1@dQjQ;tTYNB1rPd@z_a^(|KbwOMK ziC^_aTxU@%*U#8-5`8%F`Nk&%5tk4<3-x2&j9@Wn`N z*^hNS@4VN~etM)mTIqqew!C|2ZCrAc_Mshp+l#rvzV(qs(Fjr?EB5F>dwZfL)b`uBS#>U5%3O^gV5m-ijvB)lv4cneR^zUQ%O+x-YAVGVsV{juKP z3@0Tgu-1$96zzK@xra1g{`(a-vWKY4$;Y(p>iJ7n?U%0%y-$TddcnU#{1It33w3^L z(G%9EBq#bj=iWx3GrW(TggGGZ@6axbo){1y%uG>iRckk_rSME;L;L0U%Xh>?UEk?fVIt<;aydcz2axUzXyg!eoVuT@*$aGw2^l##b}GG~ z!NYw=UV593kj{lD@XT*OujgpMNl($$=T#hjY&X;b55){?OM zV(FS=V)q3jOAqj5{XdD3BW;K$r{Nv|o`6KL!>S+np!<5l1Y2lkJ z>w^rqn`GWU>O6yJn3R$Qc~P7*pjJ)OMQJIhbHkIcs(#HdRaV(Q1)(g1+5FL$QkwR| zB-Y+octtl^W{{;F+A^Fak#62NG$5~2$mePDayKMJTH?uBQPQoq^4q|bP0&n#HKuPz zZ*)u!AK`W}`(mqQeCL~nqV1*`bnd8LQW)K&e(%l6_nzh>zt2hDO;I(ownm@5ECZ5e z;L-0l$2%uTv~xcho0GOzb;Nv2;{o>Mj{ z`knN0G-YaSIt_Y*kUpONH&gLeFrJ*8998b&#&u=PEF?lC#KqsA?ksW5XKM_(m)83J zw5hpybxhw6_u%T`L(~=-KNf#fYSBrYG@bDjTU{b1=|*cZZgu96xOaX6#&PbD<>lq| zOLKD5g%3W+Pp46+vkI#;^c`)drr$)pdqiZQ z_VoBuEbBl;ytnoZCaZSqa>mup%3=*R>B^%>Y1A`36A_WgMa9}_9h z_pe`i#dQ+|3t0-h$cOU_EhleJW^v-fPzJhw>GAcD;7B zdVB5Z(PuxgIQq8guCyQ~PU??L{Uot2$7gjl9C$RYUPTD%1t8(cubGfu0}FTUC%?~> z#}6FI+btW|sIBs&mMV32jRGaUNSBPg3xiAKPkQ;T<7EkN`SQ+#HT)_eVcJG8JQZ6= z`8qMZ`kT(tm`iK3zY3P*H6gh;V-4yo!M?Wbs$mazzvL-!4Y;PrD)pJmi)GLO|NOun zD7Y*?N~)3f@q00K*AICpg)iWy5c7H`1RYjlfWtsun<34<<=eEjDyeJ zCdYaT7hrLdxT4eL!DaY7JoPc+DA7_T=BqJLc~bgLhmc7Ah3-)iIhqg2rlAWZq;`RNjnds_hx!aVJa-iM($lrgM$Y9Hd*+f zudQ;>)yT-Gh)YDIJ#Nj%&rj%aW0UGW)g5esBGmCiv#L(;lC|HJ-8sjd@2L((=-1Dv zCwY{F#Nidtb-wB!q~Uvo+Y&{PUS6b5i!$l=gM{e12;}TyTo?X)N3F z6uFw>ce|>`0vdid$pMdJy-^Axk9^2DN2B)=hpSxqdxX-l2)Jw?e6^~orv859C0-W? z+^jG$`B(nP89#hf)PMeZ|51^U%S(9iUDj|H*$d@)Rl&i2(+Ahl8GYcb<{8PVld%aY z0tV`|d#oKfik78eBc`f*4gdZ9c$r^iWv1?DWDW1q^As;j0`^PnOuoT=NM5SlUC$7c zZCh7z${|%Qag0}2p6jvx`%AUZwjkzu@OrFh&z?WO4z${Qg@4`yVyMs)uc4q&1xQnO zC=*9Jv^}U`wYv@FqKWoQ&mP-~D1b{qGXpc+e0Jwf-HF@)DD+M&MB6vXgSTVe^jRK? zL`&ZR#PZZo7V*#)ck*F@+f3mofQM(uYhI~03G_%wE->KRJ1RL=+iM5# zg+8_8dxwNQnEIyJG2@dCuAxV}|0NsA6T!ZIawV8Q1Qcbfk5h7f}3b?n7xp+We0iG+TEwxVWp&PJx-UE=d9q`12o-7PGeH@ow*zA{eDk<{UAp#=jz#N+;r+R*ZP$ zH+Gbxcf~{XA>tEq|6QAO6kNe4P1di9Q~3iG4Au(|uY4{eLM6WPpLD?eAi&--Dskp7 zbC=4?DlJ-0y98W1Si&l3_xt1y$BYS=uh(k>!(uK=I~%|zw^&~6CZ^{g4i9LLrxS9@ z-A)K5=s|vYy!<8cA`bwTY0By9M!Vj8AlP5vNg>0fAdNjCfG#}9bm^sKXfi(QVQxL4NCPpTEj`Y6MYM@u2)6()Yqx0<{4;x zDV%t&L3vQ5OS-cnR#649m2Zbv8!e>*jnJ?MMD>)6t9Q#gQ8H1jMBv2WLEm;FRtNbD)q9%s`Ebzp;j=IA(AFldwx4dzZ z6FWLOiqU(I{ZWdsba;YyvbX)wW}zu#E{R}DJRVp?U#n#$Wk@x->}8ZTFf`=tDD?3O zdY^0Al^6R6y{)vboGq|3`V0*PPZ_@T0o^TJ0yJQn6qQIfDSq5~=LFnDJGVH~+r5Us zg6HfBeZ9LEYXs`Ascs&>1Lawd8?Ue{wyZ*c0}Y5Z12gaOyM`X$^`YP6BF5-_wn_`M z1^u7;;&0CKbd@vP^7C`&FCqZ@r@C^hch|Cr4z)u8mf=%+rVge}w5mri`Ragv0D+2Z zs-WOg7CT$AgK9Refkk8dsuP<}KG>kD^yC7Q;f&7b?fXw+o^KfrT9!k2q1+zzWH{wb zP5|P*GZ_d)TV^?+IOnb2%N2P@Y)+@0y+<(>iU7Ra+$Y(Vr@J+BQhD!MvA!@qMkk@6 z>=Ds4RAI3a`FBsGYOc7!-B_bM))_>k8GtQLHp;uGcLs>B1Cn2+!9hy4)wV687ci=6 zu!#;AyK$qo#NHs=O6BXoPQVphe=^dR()}v1_pe?&d>Aj}-?KaY)2D}3!RM@t=Wc*> zfEX6Tkp+y-oSejGxbKu|ko27PvWX-DN;~iafi5V|Nr{QAg3lln-So%~fDLkTa^`7f z2gKsKq3k$xh^;FZ#f^u9lQyXvk|pspDh)JCc5?Xg@N`s!VD#VVu)Y4Yk|8l;d1!uq z9^fucH5V&B9{=Yx0gZRhpVPLh6%yVf9>D>`7cd-+B8|3C;VPs%6t3lKqODbzU~bNw zH9j~3MBe!*Zc0<=+*lc-liluvOJwfa8#!tRe0=t4%tQ(~Eupd{kaz;_5vbfTNLra2 zvV5>S)0<{DQ?3%d;?ZrH&r}aBe+JJ#Jzsuz41Y#Kf&@6@C!X)I{*v>Rkjto;#q`-%(27^i(&uAKOgvg5=jt=qn$i$7Ybo7-+qX_u9M3o#V-xZwy~wvY;1VSfV* zv!b?f2X5}W4uJ0`Lk( z&FPr7z7E1QH#LzbU#3ykN2UEn0Y{W~)d_f;PYGZ#K5yRwGYJ{sw|#(}0a(nXL4ZHB z!5wK9^hp=5mdD~A{SiEr@mAl3YVFo67>D$}{8%_Fox8#o-g5=^rb%vVhT~X0p!P?4 zNqPLVvZzdZ3R&~?eC_q@e7@>4Cj%t9#Bk=s^h+2vT~!n zAUl#A?@!}VE_1yM`gth5yWwinxcOgb4QP3U1c~1$0334EB6nltR2`{5g??=k7HD!mo?|(DQPCFXJTK%05y~ES0qY#Jl0n zrrWEK7Y^paCVl?Q=v}_A!wU;&K470|O4kw@YCPdR~Una7rk{+Kr z050mu{0pvE=l?Vq6T$W;7+L0o&}>p63`$&a<3D@Wrg{nRnqN4e=!Tifu3r2|cSrQZ ztU{}9i3?MJLjJ*aE$JJ;DgW7RPKOUU6NUZH_5}}3fB!Qw;u5gvI$r_}gyjox(RXKd z4`8gzku1w~14p8^>vXR0B~-r=gJXt)oy(8p12Hc|F#+9+!ykOFZF$G930L+2)wsPc z<|IIE)7#`^WMn|?EP4R|wMeBG`XfTYtd=3|3!1rRO!a`Ajs|F>I{=UnE$4Ct$^%tQ zc-R_61PFp#dKVe#_Va)Uqak0>*hw}EA_E=|;D({bP$KwtYqwU#hwzzt(nF2;#V$)` zEKK&5Iw`#?{j;u$sId^;CnbKNk`fxHRBp`7=y`I`gG|`hdLD7C&)u{*uu(5AVZp}r ziXKgrUZ0g6o`<`aTK@oD|N5jJg`27M9&~;~3NAx4cY@y#FEpz2b8yLNa=bC#+jfk$ z58@7JtLp__2Dz^cBX@8cnZ@q*A$TLNc&BQALeay!`BpkZ1ta0+i#on6B6kFam{5STQ}Pfn8B2+iA@}`q!*=gq`Nf7#!Gsh$3*M zJ)Ko!M(EHyA+v|y(q~Ww);rcHXF?h0nr`uD*$EvvJrkWNloVStga1Wrvx(SPC7s{Y zxG>if{)8sTpr{;Ud%_`Pruy600sT=v1?k6g0lEdmFH!jbo;WYuK_%Ab1Q6|_{vk}Y z#0&PDaHPEloRqXQ<((Y+MY8Eg4IpQFUK~D) zrMCLcPCpA>l=saYM0=eCgEnw)2}P$Hpt+)Hfq=D-(~GHOzw0uiYafVT1F|O9u~={W zlJCL8$o))ptw0FGVg<;TlbD?ix5%FeyThF_M1Frgjf7%flOM`Y_*x&)-S^m$1Gn0p z&XmUccVPy63G4~80AwBn??pAfF_MYx76E#b!Pr28I&F0X+r17xTots937a%r8T|!- zgc-nfYI2BokgIqyP4=d?eM|KPY|wobZNX9jTiGvolHG>HCcorV`4XO{sX+B@ z@ny=JcIkeo>0klxC)CL)Akgfxh#GqE_2JW{Q$j*ZfcPiCiZ$F+*mTTg|I_*|1)@ zFRs>0(J%AjHnk9vBa(!6$}=ohhK7c#rl35BY1Umfv zNO9gI8ww={tG0WKw{&E;|&VB!wfB191rfW5Hy|bPr-g_{aRzcA_E{oVLP*9j& z;5?7rcO_l@?U3k1lef95utulLqF;Bu2QFrKV^xaC9wl)I)aNFr%i40ScCO}heGWPz z&CSV$5|pk*3hU+Qb^aM0_8XZUGR?0is}F@Jl#Q5WM5Gnlt*o+$c;s4lZp@^_I7WAP zKbGCfu@r0Si3?&z-$k_(c`s0iG6@w?>TFOKT|)Bm$#g!@)Yw8XsX#t17}JL`Xld)T9r+99>zCgg2I3 ztodvYJ8ApC4UFf3WZ7lOtGf#3PbDuHdrU6C=rG24eHhC)=}!0F#7rFe&&%z>eYL1H zvb(sKm-{G9lr#rwZRx7&*`smQXXKxZ?=7xX<9o3$lfx<7nzk>d*S$TfPm=!hb2+>WKAGLpWgnN`f z(sQGf&b@L*#A3aw+dEpqb3psPFfycHXqYq*H^!Y+fE7540=B8kj&>nCb1>{(S$VX> z8!2s;a(4){CEH@Q0zhka-m%jwju$qQKsSkzP{fyc_Y#^Lg^MN#FZLJv;7;LQfM~Hn910#t)ES;j^yU?c_Y1Q+r_j>&ao`;m8Vh&+>nCaZLi;=%c|xo2l<0MP zJt<_>v2t>9dZ>|TnGr1|jq||ogzx0CU{_`ajJBsfwoyq6Nis6a)kNLj1^>4%JS{)5 zR8-q~)$Y$Cw%i~8N2iaYFKfBff@fgeJ_eE)3k#(ucz;j9mLh>wau8eK<8vMRSm^d}HuVGWHQ7V4N9 z2@D>$G3si>xqahDAa#ZD#F7i@ei{6qwL&@c&O4Bwa?#hxbZQ$WhY=Vx|B7BJ$IlA*Wr` z7c4Tc=s#t}9K|BUJ`ote;(@JQL!o%o`e50_D9Uz(jY1=$mawPmzjY0&)a;?%Qo~eq zxxe11W?zh1Zy$N==HE5#?da$LE|4)U)I45$L(6m0`lWeg%VPJWu)7p)%3~-o(^7Q) z-uBa#f1b=CGNoLiOS^L~R8}(GXPq)&bGvhAW>eg^6~XbK=Vh|8vW?G9%V$gJ@9*DN zmu6+>fWp0w3X@)2$%wGyB@kF$_>esc+&jnisnNu~JFDTo zVhf8_)UF=AzH^D!!sX9PZH{8;tjUqE)p11UTv7jpE-f~*FH}f4f~uV9s+pa&X?bj6 zVG$^z!GL?`Hzc~>xBZ~x2Xkl7rRVs)h=?D@8Xv4lbL@Dqt5Uv1H-NAG_Z;k9gRlnN zr7<>U^6is0pZE8#9{7#b$3zxkkH^)G?VHl^njl8Se{o3EN;rb5lG+MeVY{xiQDQBp zGbl=~scqdf7}V2~9Rj1HFW`bDQ{@+U^$Sjzv5QE*XhlJ^T!sfuAumE3~$Sl zZ$a@R{NRrfvl3msVg+1stJzC>hxwX0IXvzJZl1h2BXK86Z1YZ4c)=x&6jigGXg`lcF37*1EK;oGiFK8iGAD|DG&D4Tqz>wYD|Km42d|B(q_>D=gR!j5ESAO* z`vU5s4z^$T`^)fJBYr;##papM9tkrT!QO~8&sBcP@R$ln%GV>p?kftg)KRkao0r)f z!iWBYDqUDjSPk-=SGsWvigU8~5{KFG$ZS%&7IuL=r?@HD;HmPYtMh!}Lyc$&1SXn} zWsWZQ1gSWCI*mFs+fi?C_!adgLB;ovEG*7zH>1uTd#mgSw`6QAHTdU>g%BBtLUmEn zEZe(#>?~me$Bw!}$<<63^^f-j6WzdXTvw0BE|K8j19G}F&f5nsn0UR`NC0I+uQ_%b!Vi)P3?Ar2v81ZSU+*Dp^3#X*y-=Arl1^xdxR@(K1 zoDxT6N-^5MECP%D$GUy476y=n#$WWb;`_51 zO3ty}?PD-JfYRvA4LdFWf&31;qU12)-0~c_RswVC&Mik)k*Y}?+s9>RmBf`1x35X2 zUDYdQu*uFYqy{^9CHkP$wro=e6IoF!PI~J}A5wa)yn8--w~wm!aV6nt3U_SzT`f$* zY*#!pRk<5SHMm1$tdMQHGwfbpqgqM3rT9M8X{Qadm2$bUR>b5Z2gGFjN#9uClSMzt zY-rHMtqzQDoUq=uQQ|>~rUfG?P!gllgmm@E1$NK+afWv(VRis-a4w}FdNPvUP#S`v8&^u4K)tEBTm{e_i=@HC7 zL+JhiZW_wYq5P_Ymn<6+b|a#KJi_>^q>aaZ!mYCIy9x#dOl^eTwe`2BHM}Inlc<)- zrLiEm1i{;vRjfohV7@O};>bLr?%z_YFBg#{R~-ec*fwBOe9nT1+Z#pIkzrVvZ{|gWswYw%1fRqH!3!!?D`&94@=F z1l-b_dZV#xID>}-$qf<0a@Bh@T^tD9xkv^dk9!%INlA)o!bSfd-RtQFNxfqPJ(V*H09*GT5n z?DcuYpbMxsFPi&>{$%}uIKqZHrc@-i@BL}{X2C^^>PC@lUP8X*s@0~#glTxCT?h4W zELb6-57*p_Hjv93-e}{+@7+G`C>tSvCpb+FIbaMes*yuNzpY|W{oL*p_Us1dglK(d zz5H<&eW9I*@PYt(QRh)E{haq`F3)_~x*t)p$bv;%^oHv@iRLB8^C(6Hv8+bWUrJodNGwZ(=XZFsP1@a)GeF*8lxF+88yAFyk2$2M za_hfDUCHVEqMkPJL|~yAtwCG1~Rebs9 zJQ*C#O8PCSdH6_Z;@;V#>JwcODBpm9?e*4P?1{z*oF~*zVFmu~oAIOEQ~pC`HB_jL z&_q+#ipFO!6S(83{5uysNoz$>+uoV6(Mqt|w5SU=D;mhHl(i(L4@gXS{O^mVB6!D_I+LmGEtq7HbiMDrl)4q@mGbt*=MlO~@ZTay>3c-x!NE&S# z)7HsCp(4d7jXS$N!_=HTJqRWGA^#GB&}C^DkEca@(bp;@QH$t;Qt%WXgb-cx6(glQ zIlmZf=~@)Z#$u*{Hv$(W?~Hp~ogWa{$Dl6uSJOGUwbzCP6YK6zLd5taV``?iJk2K*-{cg6{|yKOutC(5?vb`>edb%w@y z=azKBDm7l0e=itOi$Xn5DkPHO?>a%!(NcDje`vM_0%|GwR-yL6ox4$atnN~2&jlcO zJ7K-`<>xAdO%GPopL%+JKZ;c>*Arr7<9mI0@x|MfEL+no1(n+J7-e0DDvg)0uK8Rn zjh!3`6xmkGH{uUFFmZ6u!u$I~R~~T=Z@4N^LP_ixfj`?wCL0@0`S&KEX!pZ2ngUwc z;!9)=GW5B+g<{2nh+)U^Ej1P&gduszq|lbktX3id5cT{x%6i{Z26>~b{&bl z884t_yPj4EoFhoPF2BVQh?G8)tLf_doVVqJHS*k1n#O1MQjYH4kD8J$JQW1TI&mVl zQwr>zX5eZ0v{st!FNc}uYDSH)Usq?DS9}EXDW;z z3YBl`6ZT$b6~eIlw5whR=&dow)wwWHe-^(RsEpOW(pFBZSNEs?wu{pSSClqJP#DV{ zU4qp0HboIK5$z=l!+q7HW(;9Zj0L@v6H@cpRAA*FCSCanXseBZH>-x*M*)TU80jGp z^jrYrZ+&ok&ZyEitIL^s6a&KWMwz=`!{&{c@OBWy{lcgMQo7!fZXcmz?Rlqwz+YL} zX&TNcf9c}I!@KEGx7nQ~;Eh4e)-Nw_uQ(9v`}0k#-w99DFGWFrz6#wWMGsy--ivl|@a=*F+mG(VydBHS_{aZe=2zzT)cH+U921rTOZ1M0?p)4GyY&2(bKIqT zg`ej`PxJ?$_++D95jASVdhg?q#N`>~J7F1LuvPecJmWPSu^j`p=G2d;cIr9%z1X}( z@cE17EAv>B=EzrirFFwkI6SGK{)=Q?Z0f@RcG#I&v$NE97+n!cls6}1DfcbES@%K! z@ONV41AX)w+V8&GsZ)JI69-@Q*cjDjKR{tg3_WiV|bkCj}f+Po`f(Mv(qf{6~`XgJT{c>dNwv82t;c0o`Xg%vf5#3HagJJUZkY%9XlkL3jCfBUOJY9c&PkxoPerik z^^Gp~N*sM9vw(~d7R!hLm%W|1g?iXNkWS<+xp*0i!=TkFKSCg*d1UMNcez8dC0(h# z3e(tq_zC-jYbv|MMgdkT{~igV47OFUW;)z-^PvTNv%oW`20g9AdVL%ZoJljMx@C=2 zxkHfkZ!yc=_9?%;H^}VegOnzMXhcJ!0oA<5pfP0 zb-zI0$jv!Ml2cE$?f&t52PS`qxUIA;UM+y!$%JAl;x~K{zwtWtU==Dw92(tI2fa$QW zYxQ$;?(XSpxf1^uS?>W(_5Z$slbIDsw#=+DBAMA+wvd@wGLw~KMv+Zsc4S2|%Q*HH z3T4ExMP!d-{-4wL^Z#Af|N38_tIJg%=e*bJ{k)#%e(vYK&l|M4aQ}Cou(2L}{#hf* zXfbWUkiwA0|NJC#aCul5QjY&c)*%^*OD;aH9be-XPjjg5Tw# z-^zvkvY>zyOU8~t+T)qh#$osNU7vMnGDc}~%%Tpo>-5U5tQ0DeI6v9J!LsSBSSOyh z-{AE`UdH@y_>%@z2Y6m^c);zp!NFn>MILP34hX-oQF;7V{Pr`R|ISxf4h@KVLSe}gIM?$WH4*gInOc5)LyVqD#edcMcVQ{1w7%sX_)W|8vf4TO4Hp+*!KymM-K-HnU1GWaw@R5? zEPx4vxwnvMGKk-OS9`^Hy5Pg8MNpa)O!eaQTFvXKWP>(mPFEtu({n>T=`izh!^M^1mfH+L8>{ zLBC&0oCT56ecU#g!vkEs?>)NM47y@dqocP$TFk|N=PgDDjxSjPNO{orqfzQQ0CCv7 zzd-#WVms{M_m3Mn1<@X|ca9*)raHBWlCtR!r{s)3Fa~K_nn|G8B{mGgWM})qH$1H7 zExoSyrN>$}K3UQNHzN%&b^M($SXhnq$qGpa;ZM$<|876~_dN@o(vRVLU}i35>oBun5T=MOpYOcJ7%MPs zHB+FvASr##8($Z}SH}hGPyYwXWq(>G@bE4;dXC)YLE>QKIDT>=n?RFh>%W6jz`_02 zz-Kvj1gO}X1~`F8snO1XY3%j`u!eovsLREGOeJs`sJUk(|`dGmV z7q}Hzv8WVfc(AeLu!t6!`j&s4PK-{S;&@N2G^VD;nAe+>jQS7Z`CmaiMjG-^1-ppQ z|N6ZA^os&3MMTHthzm3Ug6pw1*TT~;bWxq_p43F4 z(v!hfWpv}YB-8JXWxNYP<{vNNzu0>YHpv05DVJWgXfYrY&4L8CcL3cI%PR)ObXg0~uqUW$;AIWIVu1GU-g6y>&pQ1H z*T`N$S{&1L?|Om)>%+?N?w9)H5p!)}d5MA2>4c4N&Ou%C6jWe<%&auqE{vcwN($JH z9|I6AT%q4i=t@waDPQfl<4~AV)}VbF!LOCMXX1i6Ek+wZmyWGIWj8PHh%vl$*BL4uvLJY=gPA>1y(`2DD7*k^YKlBs5qxD zrb%iXaK0TmNd0_WSr3u7>j%d_|0I4vNB3QfzHCr3D#Og2&?7@*PF@TUnf7SbS_VPY z6$9kQTDShkHf2B4g`&q>c-aVFi_>(4krAbAz814&e)0OJAgP_=m3}c6u5kgYnG3gd3DV+F(k1?iv~wv^u!k9Hb8r1m)97WeDEO%7%;$g=gE)Aw^*;7*QiDb9=gN~B7*+R(WG%Q4OI{T*aI7B3~cM{|+4p=4PH{v&dlF`e}nHR}RBIg4# z@Tzn))9Ev0%}zuKiS5>=(oEx2Sd|)N{1HKBB(qemMk(57+c5fOHOQD}>wT+(qqh_9 z@vqN;HmSV*K#Ay`Is?^D!Vd%zOUc(n9BBg3VABEmltzXrr$A}}>WnG_ zDZR)pZ=QT`6K0y2p{E&X>OC6@Qd}25Uj4d!z=b8nL31AYlejCjDVtiyY(Sd{eHSK; z^ec!fdU;CHpZ0cmMh0qko+K5Xb`f|EYt0bJ5KzsDdeuIA31$;t z9-#-Ed=93(ykvhjXGc5(LbCCK!~;n$p49ai`33mhG0-rJ7yccSN7Uuo$9%coWf~=4Bn8ys?YiR2F}+!sdpTT^U5i+2f`2t#Ci8 z>t2A^rheRqDngb+2YVt9z6B6x-(>d!9lq@$kO@}L+~kU$DY!8h@#W~&d3lHQVbe)m zvQc1%QA+Km()56u5f!gZH8M>;?AGQ{6*8a8_vgauw6HBAnH=-DMvi9VwQ8CI$=?@5zFfomYb&&nLbOj{q@u~*j%N1 zirEG^rC!k?v^ivA>J* zfk0`?!CjZW>VNf%@aNH?E}1)d>vY_Fo-c1X6i=CfK9`(pL9Z7+LAhOL@@HWh)!08Y zyb7R;Wok#>Me>cS2vOiZKubd|lkvF^Yw^6H@yeYfZ}h;O3jsbrEPABE7X<`dz3oOh zir^5hfSZn$UqyO)UyxhDmM*Ejb+Zr4Rq6XUb^}Ln%*=~J(YtSA>~Jo#s5O7+NKn(+ z=R3^(n5EZ1$b!~eru(S}{V31ETXJ zol_ERw#jBmR|3658q^wLrsCBeNcQ9Zlu7jg#XZ`48zgx{>~7^i`uxw{z?hz7V@XgRtAf?kMX|MgODXXeGFTVodv-sG#6zqs;g!dgWX z$3K0&Ak1c_IS3>b`KeB8MyNmDqw*}av(;Kj**Z@?@}j$VZoBveA0M(0FUt#v6WdU3 zzUR;J>#I-bA7Vz7zuzcoTYM(H*^I-$M_?B-cDep?9qCDuD30!)I?k!QD*o#+3tkj@ zUFL@B-!*NbuOBbcN#Dz0>k*BYtUUzkd2-IF{j2K>BpnB_XWPM(Bai7}}O9Cjuyx&7Ry?jH; z>Miv`t`@j>#VMCyyIN#d7! zu8ppDW@6Tm(q~NKbqpufbj?ZvsIpNyc+ zy*#@zWI_Bal2cA!q+(q%OfRqvhnK>o?7|FobkxIyF@Z0KX>ufKTPeNM>OrM}LeEtQ zdk&trqnERqaw*G}bgd2z20xw_tFtzQ@z>fW-v4M6`cVDtH+z!e5|Mkkz;q{bysCx^ zdd(jAj*6p3<{>JYXO|13rU*6ERHcKa@Io}pNEmYF6F9bWbutm#s`jFwMo>~p%lRqU z9NUCLP19|awjFm0vGdgv5qR@^*Y+_-4_=ML-<4Z16Q2ssfh|U(fiw8 zH0X~}N zZc4AOySksnNX9+ObKfmDr6ek8F8`NtdFdxYbo7|*#+oR-$*8mPCc_{M1A*(Mb#l&LHJLcJ7KNnQGZ{fCd*6G!{;CW#U&Xho_)SvNnQqsE zOg`$2-)jA|one>$$0_$ku|v5;XS; zqxiGJI+DcgfoAsf*}P-ZSx&5-uXbYJg0k%$UgL0zv)#WtF_`~UjGW=hkBD9Su?uLB z@e$el*3C`7qU~fss5rhqD`G!Z)P;((Eba97UljtDB9 z>M0v(3=Sbe0_&8^zvl&+c%j1fR9V;ka6IG_5V=?)Avxu4iQ>>izTGh>tV|%Ik-!RW zMf4nK5a#sLvf$$2yIaRbkpI=s6111X=}A?5WkoJ2iZlGwx&-tj2ULWa@D90uU$aLO zoF_|fUnVZ6A>O$<-92f1vBWC$Lk%ItAYVC~-HElh97*b4Uhj0GBpVM=k5|uu>j`mh zn_WD)azbr0sQo!bu?*s?k`qytZfEa{aT7758mNF$5AluuDqp+>-|lcI;xjY9iGLmu z4E~X;o83$KAUieyX4h&wZ;kfmH0x>!C#1d9W7LSN7W?*XJ3|n6c2Q8x|DjUA9Kp_e z`Meu%hHT8$3`Qll#O13w`h9f13A-#kjePTMd{)%NK7#|%v*hc_#iNdV)h0zE(shx> zQRh3X>Z5hXDs|EPSE6Yu=xD!zXG0nZHPqLGbRMDyrc%_e|8k-D>zLhn4vH4G{yO*{ zTwky6(frL=yxV`Nt)j=vfAFQyoYS!+jDVr7F^S%G#_SbPh`opqZW?#`SiVqS_~?yT z0&yv&+z9K*2>agm`vf7gZ&N6G_0z!AoNpQ3e6GTa_aXhi9Zx`8q}ldGhSkrp7KJco z3%*pH?-gDxKO5uiE120)94Mz6oT;7J(QW;i<@@+LCG@VSksN#4O`B?JZpw$~yFS|sn?x5+ z4~W4Mp!)s7G1Q^<7WXm)vbFC^*gQ59X^6XB(0c$`p{Ti%{i|2dQ{LT7mT*XvTOoS0 zK|*=)&(o?8LIQI+dF#>=Yl@xQ_J^>)ur5l>Gdad4yPc*wr?mr-Xk{?k& zhp}<7pP9^)+|H=lZ9eJ8^1OWI@A*kS?wuq;G}GaG5rNPbjVI&!Jzgw4#ATkyf|cG% zLnL`_zko<=OX-_SpGgFysid(|oWfle^DbBljnRmNle*Jd-Wea4!_f?-5jjrPO>X>C zV$maH{4e(s`6cO54FZ_EeL`B6*^XlRCQg=``sY{s{dKPZjX%QTqI>##zQL1EyzHpd zo$KWyZ&IVaaK^CqV6l;%nv~V87R!0G?1tH-8eJl`E{N^;hN~&e(OAUGPCcFZn+EN? z$EqnZoSZC`La_$(1)g_j_0gJFw8R!AoJN=IaFePy>IRx4lc~5+m8Qp@-|^ACvQ{=Zo+Jy!GU`7^JHPl@ zsU(XvCB*UhQ+uzMZ++i)R9l!Q<|bW9^}-aGX}b{kl7)yW?t&A+sYB zeb4)f7DrEqo09b|1tFK`O7h+H_s#KtKQr9c@C+t0Vq7(B$H6)3vHPjb8+&y17SVkbq+X7r1u1Sj&Zliv{IE~^E-*3Vlf(S<=}v;< z*x^#9lrO%fFz2T*N=7zR<;+P0SGezJY?#_X>J5Gd^lDyn15#4cVT zrd47pS!0N7t9(enp*?X;^c%IQt1JblxH*!tKV$Lcl_h&s*4)Ou4OG?)S!-Gk9_uP@ z*0C~9tW3Naa%Z20)(lrtlG-hHT<^S$x9oE+Yz!Ggw*qhKDKy!F2R_^Z-lf&SPF1oYf`1eRW7 z+@4>lUba~kv{UTGc1Yj!K1>+k@ip9zEELAXA_^Zt&8xTPU$g3VAwY*>Akaw5uMf1d z4ONiVx)+|XB}{b4Iz*YV6m9X5$yu{%J}X-A#prW4*k<|dUnKaQAm5s&Kx@z5%ZIwi z&r^x0?fLes^NNm#FHC&g&qj0+Dt8~mSDi;v7OrSd&Ym4c@6`y?5JT1IYJhUel(2n zUBBjq$K3zMSOZQ6W1JxCnULXuH$(20Eo!QIDvvlB6-aDM7V~*IUWJ*aj_>wj`4@b} zeAt!V*(}4zV7KT~RP>|?3p)C>^<>f8MN~yv(8%WR*PQxhy~xrbFPEi+f>%@WeaCf{(b z5Tg*nv%>A%c-yciy4ekZ3Y?A_bDySu?e2^2rnG5~7no%cBGCT+34wmfIv+8BK*8W{ z?+w(tWMe;e?3Nh}+ih=%uU8GPzCrNI9uJb8UfZuV@qWl#ACa(8Xm6iQYM(yWN(-ZT zNV&;MuxqfN!ozv~6R8$dP%1gYSi5)$klB;@n(}QrtEp zWC`I4n!GjKzU&S&l(D{r@7iT&v_D^P#rmT<7?QF(rOe|?EvJI~NsDG89Ay-HYNLC# zGv?X`iSeyd$BCy6h-(6tff>>$hqO6Sm^C~hQuPfpHCgOF08O?JkJdCfaxNBr2Qi zIN+4v`V!_~Z&`k?{1x$gEmcItnF9cTH?Rr;5s$eQ);f$$f=4O%6;` za$-GEu5F|syxrR{f!%^VTkh&g#O+P?Shs-Uqo$vV?S>pwAL87osRb zCaFUN|Mf20;xR@gk|ct<%U^4&eBoye)Vn`u(E|)GdBtaW++B{E_|59BoGx34pY9G< z4nBI2y6smx^y=s2;?u^6gcg8HZKWJ@?+^B~lIGJddG~wC*mgGHVxUl|A zse0}1ewaBxbTN9xaumP2Lik`2-YX)!Z?4`6bzz5duekqvU>sD;_?Wtp=PrzXU zpY(}_R5Up(Q^P`pG|c(7U8B=7TCh$3Tmfa{*7lTIRg}m?H~&dDTMWCKWxyP%3tq36 z+Js&*#uHPh0ig8JtJgn%>ZIc;-z=U6Xn}z01`6~x9Tx49V#(i!lC1QJGh?p!gm8*V z>Y2n^x%{_nMi5<|vsfg$cgyupY3&0;=Z_b7QBI%LKn!%^aPgAlrp)no>9eg%^n00g zJV%#$`B6@y28V_P7XFC__j4v1-`u(SZ)0F}7S3f{SrMBF#0fj$b zoKP2hc-HlCRQBqHFLSSgW+C1k;OvG)L^G{=)Zq^b3Me+tOY(O`a>sK=62Ma}$T z{UTgs)y`>q^7HIb$33s9*bNyaZErMKMN+F}+x^Nrm#`O5ebJ-BtBoWmPT^+I^izkq zvGvF_Q!5WOq;l1{_h+FG$fMOW_iw*azn8Es)Ze^WK$JJU&4c%|GGI449$bW%dr)*f zQU!3cNTEEkoJ?_#;&rCq;L&7+6dJrrS%rT;jQ=VAlHN^W(iI(L#zfpNtHmV@G%C-z zV-T-keiSdMAf{I-*9^pKIO;y(@-aYQiXaCihMQkASz0&etUuEM_73 zP<6P{!csk`v~M`4mLbRHk6th6l3r)Do+cRlon@m$2-_(?OrAE)kJu)cG9QIdYL<6k z5OtCd>#$Euf+WJpcFIt5>v*N;g_WL@8&)L_rbsrg4;t;CLp@l2Tr{8#3%>^bG%xxK zUWWJ`N2=XXvhDTaNyE7p>`zF)u9E0w&aGjzZmgIn(f8J=CN<;lZF6J*jQ!3&*h}$e z6EA~FT}Go6x=q*msKgD5V|sp+`otN_lfsCgz&KYKL>^*r_;k{-lC-v6rh%&R`eEpn zBm`kXFw{eV1?t|@zHAZiQ-?l4Mdoqnw^c(u;IjY|%egNQZPbPHi)WgjdYe&+Qq(ZWe=tQHt^e+> zICxAft$GXK|M-nJG$!-m!|)giL=q<4n@IL#LP?0HAA&z|)i-SHFA2RcH~Hkr;JVO@ zsPJyv8`@8!e@7;7_#E}n9)^MLCe`x>j9TJpJZHu;-RcN4*A~11R{*tdjoVc}zVu9F z%t}LzmZx$%!^r)tzK^e7K+$QG15@;mf8u87_fD-X5HV>28SwqUG0HgNn_{q;h`kiy zxMhyCK6UK-0+ZbFqA-db`E-w0LDl5kxr28&b|UlQesoh;-@)PO%}_*)_ej3`Scb#o5CcZ=`95!#c zL4Wc|;Z9O>C>z@R4n;6+R@-pzD+;;IMf+=Rl9BcpyA@q4T!K{j_2OpLi_*zqy$kt8 z2OTK~)bD>~K*F(bOmvl86z6ifxYYvTd4A@nJk1}`a|DqrP0C>}m}#*lTLU(}EHk~g zTiozWs|UdpP73{-=mj>ICU(0U=SZ-$&fd*hVELPVM(`cLJpwahw1>=g7Zk>|rH`S) zf?YLMsX#rUZ#13XHAi-BxU4Mr>L152f5Z>d{R?B&mC9GyVfYaakLAsLOIrRPx5wvB`yh*(Gx$ z?u(7@*_?qtIw-=DH-N?Jh|DI@Gb@QV$S zptzXaTeP?S^w8`~2h2rDpVp=Cg&jI87T)i+mPf9cHOR71r9OU8`A8y_DLpj;_Ji>e6=a=d@tG0u50Wiqf4`6 zG`qi6tcSELE%u>o0DCFiAF7lS0Hd2>clUn**?^TlduG)7h-72r@^i96txLO2hzOJ;xg$ zI3FyNT1jmYGF?1bw0^A-8VeuC*xu)1JJzkd=em3}ogx$3aKZn^o3A~&GZzg+*UH;K z0*OGkL1&s!;Vi1L4wAWhj*3yTR@ zrtBMU%KX-0VycREegQJ9AeBxj6S>2f@U$M>v@`Gtdo=u>)GA_Co#b}C(s#i>oN+O6 z!6JE|trjIJji1Z^Yzhcy{VR6+%ZnFN?>bZ8%c(wI6&Le+#+lHdp(I%sRrB=|M@ncp z*+$y>muy@QX!Op6eG`-T~@2QPz-xe#yoP>tP?$pP;$JclQ+dD^*`OLTty4 ziL4eRhJ0kANs~E`6(5l# z_ma_GVjN=36cyl2pP@^}EzUjvUyvXsbo*g!kG31kVC^#rAwMB6_Ou#R&?gZI5tN*Y zEo7K}d;C1or^&^!3s0I(o`rQLuGvCVl81ediUrMwyR-c`+x6nb?ykDfp%l5dtchWl zDPs>8l>lXW*tKPw`bC9MmipOfP!G6{s49fEsLqav;Y(PlTv zlFLeDlGX7}PS=7WenU=sc$LI6NfHjAu%FJ0YAA}+TD=7SzHRpC>h0UUEER1eYZ?^u zBtu}(YBd30=IS6SYp{%-)xVzGN^|~IA}&?P{i>Fx1KBLep67%C=1hnGqVLOYt5Q6H z_gPNhpy>28WrUI)6$<CmDTY?{%U0z7lqZ{qpk#QKf9lnV)IGMk8KEn^^Nq9Zr??@3P zCDR(n)GWMw74b>scg?T_h<~(&$0DG2L!5&el_Ok=a>KV2k@NHUsNh<0f*a4I`Gl3{ z#R{=>_zmAIsN}=7FEzrESlbkQG}M1{T)b86pr+k{mu>0a$OC3+4IJjg~BKbkkDWAoR5K~Zr;22CA0YWXTRMKVUO1*XqruVp{8g>!1N2KRuvXa{w`o% z1?9bxpDxfs(jS&q|FBvzXu`*wM#x1<$8&}?kl@~3$H>lsQ<3#A4alFo#%-zX$9$GE zZa=PEsF&StoQC}I=%FXjA|!A~Xatxkc*W8c?6EY33nYp-+CP6pMGJ0R3i!VGDvSDh zE^a#9&9htJ7NxHHiX0wj5s0$sqRWM2`x+31UXnz)O9H&=W@e+$D~mXRB%2RI2w>04B!g(oENJ#2V6llk==u?h1(OK4(ls3H}zc4 zpmS8*h92_guGfFR`oABA-ugS4_S0asmN0^Y%PBtCRC3lR2^=97IMU(ndy9Z`4nR5o zeffW%uz7=(nJnTC!fKp*_$?~~eOWqZzOlS|Mm{Q z%ga&7@HqEKT2u$$1|+_W3yc4>yWHU?y+X%^T0YArgHJ&q!F&qyBTUT}Ru15Rx?g<_ z^v^ZhvVGWdg*6VBP=ZUR=pMiS-!T}#k300~bm%tfF;h4f=|?SJQ6*%6WPlMXrslzX zE#`;*6?&&Cis6f(!Z{9l-|DM(acPOM z;DC>!`7rkvekd$%Hk}_gAu}MMc&RF-^e-yN1GD1r9l4CPD}I{V*D?!)=y@rGyG{}h zCSr&Jp_q@tk3s<^p9Kok02KB^c}UiFcth7opn61sej>UaKAx>AibMJy|0jRu>B#`x z$#Ji1?EjyGEB@PNJf}g_CeNI;9c&LW9zqv89ZGLA8NxJN(l**hB4FMp_~ct2oIrQT z-@X9j`2TL`|9(()KsK{Ji3T28!t?N0eW#)9AfZUQ+r_I^azg^?L3Vm<8Nya0>D1N> z$D8AW9Ja~`z^nK{bqE%9s?c=`x$VX>jGx*_Po5h1JH-RtHUoR;t#jQ9Xs00oiFNtZ zaTQ2snhsPgv|EP1z%BUAh)!A4+4H8i0mqZ5_y=+)KLbvar76yLq|Xx$R>1@#BWPys z!Bpn65TO%b<0b*CqJmCbIxpeq6*=aeeF?m&D-U!D(_RS;d9xaF`JZ@Lp%{*0H#6jvB=$xSmofc7Fuj!m?o8fm*g1ND7k3WIcYjT z4x0JYQX9a;c^OvUA7Nk}_8pwZK+FGnVhafT_7?>~=(JHp@nR}Wsh$x6^j+|4yMKYi zH~~T3(ernM6u`@p1z#w5`4ntFt=CNcm=v6sV}Mk4U=y}3yMT}6!Zh~TVpK53K7gRH zp0>z5lF`NH`V!q0p0|d1U21+kRqr{-m3@p}YJ|P-y2q8CgI7$?RkqFEn0TrqG5-nPvdd@y;@^*7Gd&|8V@@;A@>CvNv0JxJ$hPFir+i12^co7oo`(O6lB1mq_hSgXh%@L#XM3lx$BH( zr02J7WoGc}Q5^khSJQmg??}M74|FfDO=rN~I)(9?%kX>vTm2o#5v0K!4zPixth3#M z#{G#BP|yM|bD);`T($voT7R7AyGM`Cy{?xwLQ$eKZQ}R|uf*(0Y4sVr!j-UdE41`y zNZe@{NSW`CbE6f79HJ!ay5E-7-8agNN_RgkI_`r!Up)}mV8=QG)(b`{O^^3M8IZn< z8*6|`_!v^0+6jk}+dquj&KNOpdD97$Fmva39}QgXW6BMzr65R0BnH=r?rb1-ab z5#xFaX9^*#1!B*@$~2JjM4f?a_kbnz>&o4oej%X!YekDpe(AaXb`9z&Wk!z?r`jnI zpKY6@o&n%VLMT6%_*wY$Z51T+n9d4tVKndc9DwOXU%p!pM>6iMj7Mm|xh`{D7xv!w zdIx~|!&i)B#RC}*T zW)-^X0+{00jZJoRf}}lFg9rV+WHi z(oF*PHlVbu%0PKtxi)THcX2^Om^Y?~PIo5EOi7B}PQ>8I}%|75TJH%`g2um`#E-EZV z4`}|3CAME%_C}w9;NeydAsaPrkT=NsJ$zpwc?$%lc@GSN`bFV5F&Ekx3s*$WfV3RX{7AV*2vKEd=3#Ju52Sk89}i`?1J|Oiwfu<~TV(bJ zDj@#N2doUb%92TLHiIkqk2N-7<(EweaE`@jOMf=-?V=7lW zX5N#o{Fbh*oT3N&(9g~0?Z$h^wGO=kbM&^Wlx%lU?=iQCra4;h=CW!}&T$ zmSLZa^QY+feC|DS@Sc7aS9f7Zy8Wv_^`IS;&#riui*TA$D7W~H>TpOUkeMsU8m*|? z)pjh8G?9ZaFH1ur4 z=psqQi);jE!my;^d-rdO)Es!txabb9OUwMwxe1KHzPR}@;Iyxd^Gvj>+Y520bJ%tX z8h6S+I7elriY(S%(mQ9r$$YL$fVWUDWnpOU!%=e22`j7*L@2kWK?w84p0-{W-6Ka# z60SJRN3S3DO4$Q(y7yCJy(&TdMezN*$=t4e{3yYgNNLyc?7o)XVQ;kmTsWuy&euD} zd{Pr6)Q@vKzN=^)Xo!;p=lm8G(7?|BbIZN;B4@TSacL>kG5gV1UY<$cH(2?1Ls?L? ztyWI{Hm}Gn+IU3Ana=b z;v*wubYnqf+ArjMOYIxAwEnpZ9`)owX|0s9$si}h*!{(&{;&YH-;n?Ve@xcCvU zcgHwJ+%p5Tig9A>t|hJg)jro;=RB|8&JcPcAqMfCF6)w}b{ljhHG%{>`a*I=af&2> z&}(n-Woo&^p*axy1}58^S1P4kknw}`K2%QK6XoY+&+JTC(3$i8bLP7!6{)ks`Spph zNFd#_<7Kms(UNa{sm^J*nEtRG%f5x6Ja&YF4fVJ)UD>kQ)3h}i|M}fD6X>EQ8Ww1D z;VcK^($vJpPugetyhJ26b}JyD;M;<}T*?<7j_f9hbCAWTW^7`+I08 zW!bM9Q<2U9FwFo11hT>k!LD5C*n$rVy8{*2B%Z${=c&4NfK7*W>HN1LUBL8&NG;>E5N$@ZT@I{441PD6$K3UsaG#x@cIc^#w*i(EKZnDHI#Y zZXY~-Y;uLNr%HPd`qRaRov~y)M_(Fd7nD{GaNsjXuizKf3Fvmxl5^!t42*M{a%7Ct zhF!M+H}+~-DFt>3oU+nYD_o~zyx6=#t~1%hJ4D=G>FAGU(m~(ui#(L(mRIsyDoyI9 z&Rupo)EIaM((q=0x5s+!wf9lvO#| z`9k^L=OfU|6jyocn&?#Xf?lBDlJtpdwYoo$1Zz+c=3FKrOl+vyNoR=wrAMni>fcRV zp@AuZ!&g7?wetC!C#4g`o@94J!3*R;&RR?Agg&`A3z3J%P|JLBUfW=e_=cyGGV`gp zUV&z*$WkG6w+8|ZfmXp3@QzmwIea%`7DF+c%WfKa;Bh2ZY!*4(awp6p{%@@Vb>$tt zvJa-p%%6k|hEG;-`XX40MZ{h?)7Z}_J#b<`{2hC0@bscO^5+F8w7fsmwuw=)sA1jf zyCalln$yv))pX{K8jDtZd$4?6NXdXuhZ_@E6Z&T?^0kGN#pqS6WIcTD={caCIj&m_ zr`#*PCA8G7lSez??1@jNQ=K3zjzi8A#maq!+uB;Su>Qu1+W;$yTdg{ApdkIJnkZAo zglp3#NVw8@>ZV7hHxk)A(eB=0UKho2YNib&EuR~0J$qng?hypuQDd|!u4Dv)lPlQ8 zrOg}>ZO%r__UX4C+H*%B3Yve>(io5%asC+Vv=~ySfR;Or4l`c&kDTGL)_j3(XZ+qP z59poR1|BwNzb@`G;_ZEgR3(4AG*}qcT=J$?uRGn%K%?IzXuC1*PTk1*?Das?=q8yD z*tNVijtZ6!Cpp^cyQDb14 z%Xus&pOecX=>Dxh(01cIQ$m13PM-u*Mv>&!4sdulWt{ON)WJ{yck6*J+S%PMaH6eQ z{M+~=eo1cIk>4F7Do@@|RgNweZD$1cf`bUbeHHW9qklN024GHSqR;sxUCce6SwUD7 zho$2ory{LyCKD+?#(9G3LHzgQhRDb9d_*@jf50)a`N+ z$7Bs*hSrTrM1jh9=gab@@ixgXEMq)pz?k$f^r*oi{=zY!UG1GtoPNQ_VTX3xsw7?y z$Dw!)57n)eT~M$&c7$&k?zHttE4O51nqDBB=OK^DfWE zYhOhR^3*>OSxpcmjf`jM3lr3M^mlWCR6}iz1Xv(yjc(swT>i^jJte6?zu1TwpP100 zGkDu*`}fzED--`)jyXQ}ed7C!D%lj+g$0f&b0<;_I|}*DU&n1!Qfq03kW;;YYEmNj zBVwMIZ9(Eov%yYi8=bJPt}LX|dW*-j_URPnEXc9WxJqn@2*wM?kW(h`AxTwCe4BeF z$Rvt35e1%T{VcLDA&JwXYO^3g+}P!`@R!+tI6l4>BN4wf)X&9UTaf4xdEny*85dESPN$7{2xIUs z8T%Ayr?M*aWC!Krhrd3{n%1xK*^u6Pj|x~;ep-sbm68#c2+9Rm9@_RE$die&hcl+h*YT$An5s^EmKjoS`wIYU&8?73 zf3!#mH^5QX%#;*!O;1%H_U8&MGY(-)ojEfDIQDrC(UVr)3TcfbWhAA`I*A?r1i8pc zW;nKk0#a!2(W=QwsvKqTndifE&+mNGcnK&#Ft5Ol$x8%deuU5P4vPQzur=QWEWsuyrQ_*3 z1}aosm5ULaznE{IHq#QmjFuXp*k-iV3u(z&YZgf?Pmp^LTvH7edh?Wl#7MN<=kjK^ znwH&mcg}H4d-$Xt`s9x-#CM0}Pt^m?k5PJtieK;Q9NWUc42oaAC!kZ;>U~x9JJz|Z z)RmPj?pbU?Q=(V=w#$%9S7C1jy<5J-TuUdR=&mF(Zj(TjLHDMJ`@}?rqUDw$5)sq% z4X@^vQ+YrhIyIcKmlSr{9as#Y9iLYgON$32jnjt5`c5+vPc*MRlyrCla3EC+09F>y zS2)~f zZfMg-&*oZHU52;CI`kueyZM2A|^=>T<{y@^$&B~ z?p@KMnJ^h&IL4zk)s{I4g>G6@z)!%Isrgaki#J>zgEn{6cd}$d_un9@uDB>#8v%LE zNeq7Wgwzex3f`6<&;3xn9*oICj$Xwx{TrsAhiISgt=^Wz@!3(imwGF|b zSN_`*+joYd#j1}jzxJ2OYTe}C{N)ZHzQkXBpPR2z-mh&>X-YAsgys(2?$)3mXUo)B z&c)nHEUgDY8nxmt$e})TeF?w-yR9rC-CLodE2rvnp*>#IPSe*I0@}V?T;-GtBa#Qw zGNU-br zV5mw<+^G44?U$>1k=gP!AID@q-Su~fwy_Mm#2028BuuEHU z`2i7lLfoUeorr$JI2Q6cEPX=!iy zcPKESSQPEN6dA?bWYO~DyPfa~u%46Nb=R30@TH6!5YnrK@9yv3 zWS3oReE#+JPf;APZE#ldhe{(=G3lwLl1qpXs*M@(Wxz$aO=(gva zs27gV!P0*atxttwnYaf`(%sg`-B z_c}^d@t}`cN~jDCepxksl3Uvu!^w*`L)@rehZkfBu?6o{k~DBRw~h)77icZ7W{+fi z_cg@3Zb^vi8p9xVD?EF;*K28bE@g7Z7b@bUR(h;=ysfqMx_t-DpBP0PjaDZ_GvhaM z<+R1h(HF{i2`LDbty>Ef!oE{x7Dd_>S$}tti-B=m;)L5y@}a?H;!vl(6@Hv6%LI+3 zZK-S=uHk_Q=R=qhf1tBHW#>meDR*k=?PFJbqX`isJwUs744}$72`(!qEwK1L3b~RP zilhC3*L9S<4`nSoo&U3As=4Sk;>nc5?d}8UQEJ{L~xG6wOj$#^yP9 z@A$ILjOQ5+Q$wY6ApvyoKag~MgZah$c4RDzS%rLrQaqE)*;LB~2(;l|+(#s86o^ZcK6u4LdEw*RGzdx3&=U_sa@HnMI zr6+HI1^1idKr@M$>P;5huY%Mp<8+q%T+5nVHSRDIX%Y8IPAp^fP~0 z7IpyGms%%T`6NjX8ABu3N?S3efh#r4VsuW!j|vJ&8mV=JTj!64?#=|VYV^JdI=Rkk z1IWD}oIfD$NCF@pT9@(rVmFflaN4Jt{570_vU1h;~up z^lFVss1J35sPmV_t4$)WjZ(SqG@JnZXRe9XtjwF4;#ZRLUUgVWD;DR0ZReeATi;;j z|3lSRKtcL~x+2-1RrboWpODJ9a~T_U0&LxX@c2#7E=QV!kmo$+~| z_rJci6qjppVeY=??z7L{zmS=uzBom=S~gyztiCE$@g&kpdH_qJO?qYpG!aCPN_3-S ze2YNGnu7!2m>0GG;2~i|Bpvj=zm|*|!Fj!uHv|lJlb^sC!94P23OJya)Wp_tgN~`N zs9EH@uGg2a#6#zdNlwKXRY@8QVisQ+0eSKS> z&FK*n+Z0O>f-K;EC3>B!{pK!yfaOEWzl3?(FtzDWm(2D60J7Aa;Curu<^MWr?exXc z2}}gkwQ?jt&-(fROh2l5DB=S)dIJ4Mov0L6Kt!4$_%2xx-_)$&#&?HE8{(CaTyl`olk&-Z;!V?E=Ac8Fp(w zL9xQDCBTHeMR*#!ZTt~>>vm%rWlX&)6;F*EmmI6X<^puSKd8tpe*0cC{A)T+3=qOH ztpsS?eRro2`EoztT>Tn-A^07g}x0l)Oy0`dchUXsCMJ zf;1}}A2bS~d8VD1gTl@~gp@7 zc+Q|<2YBd;Z7i?ZxAP?QDGV~P74(mW-mzs&YQQW+@3ZX%^>#+7JzzQVVxFucmr`Pi z+6_~(K(~D-y`E-MRf0D;6RzNTeoW`_jTyU<>ikbk>UAf;h4zD|TFEN}tWnrS(W+)P$9#iZ$1|D_^`Kw=>}yRRZk~fS+WrfEpQ7@dN7}Lyvcn^#tE>cl_sG2Owj90UL7wo|-+R_7b z`SD6WJj>1TU5OPN_U6W?6R-Q}L>K7a35=2sQaZ*ZyEOP(4ZLOOd=#ZUD0jdJEo%vI zYnn7*Ks{twzJfvDk_}^VK@lEqc?Phm!=|ANf^+db$Afg$J}7|`buuxguC0yNX8pb+ z0BZrF?A%60U*MHTAtod8XQ8I_C)YI~ybl*Y5-9k#Kt~|H7^*kuD-L$k?66saia$Gm zjOIB(?D^(HIq1m8>+*Ar+_TEbl0l?K+^xvF;%y+8hNUPEHsBj;r4iXM_txe|~ayq{ph0*W*Sys99KH#D%B5 z2cRR^UMFr(zi$CU7Rnn1oIT)Rq!yb7)S&=&M4_MALXc9gqsc$VLy^@;4;xJ_MoF$;C>xk#7h4%UV0U+<;P|RZQ)`I~qWfJ$3 z*?lBBnBuaFSe34WxbpGdtck7F_`|K*^z>Zv@t9Y0^B#jd=Tcejz0{z4YwJg(qT)96 z$tHB-HM}%Yw~K&WuoftTD&KAPL)BUM9F{Zki8}Xw|RQ% zycP5=S^?WMR7FW^i&4o|_Q^;8koCjpoUaNmRH&KHYjBK+BTKLa=*bu^_@df>6r1cw z=Yq|Ppn>2Ou~PF|_s7_ykDtnv|2=_<4zyFyM<~$iagVe-wmx_H-nd_}DeSr+t&15q zeq132>CFwp1sSO3Rs)QD-(l}f4qFe*9j0_kvF3qM$e3n}$txSB{#j5Z=ODgg%Rd#T zR$Y36)oB*6S!{URy!+sAJl%8a@#}e?x!BibMUbOLw^J~cDaEgKfb*U+Fw}PpML>%0 z#>(rMQiT&S&prwM`OzLmIiPr7J!(~Iuegh#S-5VG+o|Q^H#l6yKL7|5t*wt}RgvPF zoS4i`>umT_c;r=l()*h{V1py}^pY@=G;lqq&~`Rr0fe6GL_}}fGcrRkif)Q!isuWC zPepuEOqX;qqAJ4dr}a@BdZtuM0^Ld|{E;_OPMz7d~ zw79|pSp3cTKdw}4lr2}Sf&79Qw=>W&39_!{K?FmsS*?9O<`H|GQpLHC6 z=cFRNHt{m+u0Nqz_sPSFPR0_}GbZn++_cYe#(HCZI!0G!5!CGNt7y?pafUnQW5U0Mw$udPzV9ZnP|$uLa4^qr`#lZJVG|k? z;hiT9!9JzH%k6Hg0lvzh1(OP1xWHm^ckZLODK4e^FD; zBNEd3h+#x@&f~NNn0&uJJRbyOw0X@FTHJ`qlIf{zi@~pJpbx%EYzY&OFNNWJiX+eN`q%C~6oPyCAvSi%^_bkOUMwgV(y-zjiDH z_*Leb9h>HxV;cWdL^)UQx|Gh>eV%5>p!ID@(l0(4Zf((8`6JxWcnt5`bK~Yld|U~F zAD)YgL$Y!M;I94*`}CowY_|`jpg3xic&kqcZzbhP?8~$j|CMqD zGLf-HH?Mj_ZeKx%md$lt2L}gREk<%%k(!d=ud|St zSZXvd;wiiLZAJ3|gY( z&iYPLtwxow=!mJbr{11Hh4!yorU&bf|LRPF8wM_424uvAl=Tx!X;~L_bZU)w z5sdI#e&qM4%w&qW!h+zBA3sI~|Gg=lnwk=R_VnrB3nmhz7hkkE5(XPfz;0dkVyK=X zby0m03tEUdV6OxIgD|R{zO?GPy1KIB$A0m~ak{418f=@70_(tc8LXnH&Rt){944;> z-eyWY@D|7+y(G(j9|xce zHGTe^77dQi)wQ*%y9x{rIgXqMZwp&t0b{jh3n2lB!2gA*+xIhIQ**}ANsx>Z`PYzxZ2j?>O0lBk)(ti$NzoEb>RLCjxMiX$fjjvsP~*7ZWJ;zjfjx@_l{YF_+3?k z2RYH#_IBNhp9+|+$~k|Fr$bo|1X8e(h{bCZEEpUXBt?UIV5Yh{a|)trAOQUya`|d4*hxUzK2!}UO$~n7`o@O7 z``O;o3$ky8AP}A($F`fclLrHDpod%bi)v^Nvi3-=Q4oi65RWma8yJrmWQn`8&|njBNRykaMU=imGz zL5ulHp=mBVD{IF~67-7jUW_?cg86hj=1b{$ANZG_fd^`9XTckrwKg%#uz$6I-M~1; z0611WKni?w9HJ6p23J8!S7ZMUe!GB+x87;l} zB2~=QawFX0{LtE&g9zv$Q^JYi`RCoI;l%JBUr29XpT7G_Z)_;$z=YO=9Iuaqa7!7u zCN$*qk24dvQC^tXXfl^kGi^$0YGE*VNE7o3A-drAk;55f0w(9(0MfRMdcSo5aSuiq za|d!p80g#Hp-hG8c6CmM7BPFDDsm`f9kUAa`=rQ+%D4@j+8>LG z@oiQ%MHs>pPABI*Z`x}2q~}g<;!khVeOh>ITP+;9k1m4*o}_Gsysm4u?o@YdAZrsH zTLTacsXMu@z9WaftV!jet}m}+2iFQhP^uqm)O0o#{1~y~PpF)AdYpSku;?bfGqmY7 zE|;!d>LJ1qAB9Mk%U#G^*{2fYTMpD(k%BBjWe4#u9Z~J}>7RP9sTeysC&bIaAM~B7 zGXAMh0?WDGr-fX9a=!z=nrD!a9W-*SKB5V6NlhylU{ECo+TDHSVv-xMu0v$ybOmTMlw$P6%X_xBr zrZyOJi>EL}0%>t4QO0;IEeRHf29s(yLmM11?;@7?Ff?Ah_~oTyL!P5(CQr7u(pUnF zcnBUi8Qx|YnS?lpGAzaPuq3N1fHJdeu1>i@r^DVkbhP#K^7=U78Y-(#hkZmX$Ev3pMZi+60u9FTLAQ{`YIi=_hKJJL1k0kx74iWln z$C4XU1h5M;(!0#;pu`yBt)1T1D?f^8Z3sNe3;ff!lYpkN zB>wnoxZBC>Qn!1WO@u!O0gPY6wI5L$vTb=Gd^()otly;S88}>;oZPBEF`;W7bYWBq zYXFCl>vgfVbl(MkYUVFhX8M?#e|~@?LTaEiC2Ww%4FJW2yxhW~Ycy)6RA`;OfcNs2 ze7EH?rt6orl(W-!n0jn9Ejb}M`a2u%GVS9nbEhXjh+2Z8S4PEr%T*}AcfJ!y4)pJKK|}zO^l$=z zJa2)KEEKfo1=q!)zF*90w24=O>2d*e`qx3o=u%5yY05oGH)%;ppJamFsM9}AG%&gA z4jHZ6nn4hM=mtIQg;~N&mqq}!q)Z^r%6}Yezt>7Zb8wRL*@nO4j3@MAlXKqvyj-hj z>Ni0CWBe>1aG#_^86>^fL&Ea1f#haFqpqs1#ueNI5B^0D2{g63mxhXq9MGx5OmN`& zau@@2!Y>l`y_>s1o1<7RBP2-Z7>vHrAB9ySC0!EC%2?URk@KpLc(85nr#}=}yJ}9e zw?O0WIo8{oxAOh0s&Q99qxEoP6&&~}#Rj`^Lj1uUGx_GnrHP7zelJqf;zzshXSXXa zfXL;76|_wAK-<9ZDa9gSbM&F6)98nvd)J`Px&UTgN~|aY=>h!_h+p&HkHaOc)#IDm z3W`N-Sd>3K8j!EJ9GUc9^1U0Qmj^SOrE6FM7YL17~MN z`mvmH+zfKLp_^%1)0tv`VVzkDH2-4Ob*sAiB@Y(N&6|n|Lmz#9{kF^ZRGY`rYcZqg z!@%z)-x6VbcLf#D`~&bS{8)gc565^a5yMfth(5N z6t!P5a2A^;M~bcd2nL#QWnK$(VRMYg^7Od{aQ0DTO{9|nVr(fLr^pr`I7|@qTW_KE zP6nK53v#b!UlYbE2Ooh(K|UZn1bL!g;THfflrD2B^SZqW8eFX@9v^UlRN^>inwg=w z0ydbQ516$;>xC_q?@&|Fu!PH;3DJwLLN=c%P=v8}4a}FLaz6yWY^$ST%DdRR1}*eE z{TIW&u&ziW!?R&^T>5n9j%VexiD>XAE)RyiUlbwrNP8t;by9juJ;y70>s?QcE50~( z_XcZH@oYFPPbrpJ{7h&>dSW;1#jV{|q?fl<5P#!@wiG!W$v{wIt#{~HY8f;P8c|Qx zV7w2cbvd@}=wL0r4(A1EeT9WKMw_E0BhB^pXE)=jMN>!t{JxQ)$l_%b9BMfL9jJQ_ zSql69ZHu1YN`9YM0K(yvA&@};SNm$H?l++&Ci1JE*7fV7G0x=QcpsudG5B>q(AU?6 zioPovfl7;Ft9NfbWx7(bZeWd+IvB! z5q#Vtb@$T21i(KUn)=LccmqI9M;6OlACbhG;=|i6wz#xWJ8zY_a$ETvhO_SY5(i{3 ztf!c|^y^H}N)WzP6wg8u0Ro!c99UwTCC-L}t!+q@&&ZZ8j)B~uz@M=grZ&x&6oH*p zq1u8`%!W_U$^+)C6}r&nui94I8icMKyH3|@K(-wAfm$V2059`CA^d*amCx=gZd^SA zCF?)_oXDqzPS1Ymd=u_Y2$!fTDACSVzo0Di)e4ZBp#9b3-hs7kd&bKulr(Er!qoXa z0{xueC4x7#%QyAVnqex)<<_%m%|ZF2`a|WSK|%R~J3kxr;ui@s6)B#SX3@!{OFxN` z1JM54yOMX6tprK}#7PE-_DdP*b)GMj1E1}?iaspN7UbB|*fc5TK2H|>Bbgs63=A-B zUoO-1ur8YzY|6H7Fp_45BWgn=yRamIdE&mOrz`^F3p={~8lO4i2kDHJ%0MSPRgDM) z_TbLhm`ls<4>vfr5gspTy}`#1%-eOQP58x-QMId<7E#t}iwRd|wg-})MzTG%W!3!e z%oLTh^c2lg>6HDvyc`J?hXpCVqijm6hRbux@O(DOVjN~pqWHx_vMxr9*jcZhkROqZGfUOKGucq|xZYrlHi8F#Le3reM;q#u&;)3{nhKfT? zfG+f#MJum~u;Q(o!O!tJTP3^fZ*zi&Becx;-XMNC_MpW`pfPlqPh1^`6{q~+9G{ue z3lHJIQ0Py}u>D@kul}2P@pvSJ|BX(T?nIjX4pV`K0mv92-J0``I`52pw?D0K9jQyv zPvld1QNuUGImDJ1r^^DpzsN347v+sdsA^5TeyAT^L41R$|Li?cW1Zp`naBGFMv^AA_Z}$|iJ-uzB=D74(&X=^hdPMp zCXL~(q0iW})g~HZaj@?4in2YQvZlt_OFYNHAWCyd#fPiOGQFO7`_Lvc%XqEYV91#s znz|RSIUW%EB||~^Za<^OR~WUA{Iet~;gIe~Vu~Ct6c`sP7hU?)LU;HpH|K+2vAs^} zJjuVa($jT*GQQ3IAvla}Ya|Mtb_+;+K*2mhe_&C&z%6cpkWImzK})5H*X?Xl-^<3~ zr@ts8vlfoqoQZVZpU>&^b&=6(?7j2-Ti-Og{~N?T{31!m%2U<}yk4uR!9DZt^s4E! zaT?8}-)NOf5CT0r3!&!XqOURnZpqe4w z!Lm|DWc-uIStObbWZ4)&t0F~y;xvx5YIh#i33fFJNs+-> zoaZB=*&c~=i8tSlGwcy-t(Xn;oI`>_2KUZ}Y45q%eARQ0O{)q5WAL}VcY`q!dA~e- zjR0lcRC!@d)C{ME25sl*RoG;(8-LZ)nI5<)9z6YS9i#Ct%M-Miu%2+73?52l>4|L~ zO;x<8`gmKO~_DJ|JYHIKotGl{c{80wFHZ( z?Hif7Kj^Z`Ku-r4xUXlfKUP1GxB*rF8S&+ARcnH!^5kBY&a4_Xw|p2yxI2~LhlBSI z9Oz1TE#<#z$fJ0C)xRtgPACSut8DB3F&$4brO4EcGVL3ZJ^gAUT=KBdyn3m2!r6WNbRyxD2C@Q-Py4ITXjS|$|Lnto!9#-*4Tn*p*_0rR-mJ= zmHSR20d0DtpmpWV#+|g-GI{6m<6)(aD-I5>0;T6TXyqs!>7p(&(7rIy+pwPYcP~gD zH0c~UDpo~J!@bY`co<$ipIPhdmOimtn+#H+{^8LrV3OOgv}E(VkQh_VjRLB(CsC~O z*y*j2(@-hZAUZMi5x$m@S3u~yhHOmumzP_^mc>f6mz@bEOP6A|JNwZTseQD(sizyD zgXic?QCYtH74NJ@&f)7SQdL2PUIOpWOKh99OHySI$E!Of{wmO9z5F9^z;vXBwgEk> zm*2{6pSYr;C3c#LLYt=9Xe`>>Pf2QBF`<3xuqC61=r|AQT}?3H>Fsq*pMVdJ!^-8v z$8HjhvRge6A#b)du4YJT#TT#?eeeyH_xo$68u;%%&56omkJvhYsmR^4o_J8s!EslavpKk0 zaBsfPjw)c{_HrYHruK!qe~8bQuX427^t#9IYmp@3^u22^W1g@N9;abx>Pwno%-DdzW)T@H{K`>Nc}}fUQE>nKj^3c;FVaE z>Vv&(GxiG4?K?P1T3x>0@`|C}H#2T=-Ju9@F1lE>DsvKniZK{d?`3Ce5_*45MwTo$ zf;%3DjIg;M9^B}-ERAMjA4e5&dus6*Wq)Cpl&}0GN+m_m;3$A|pwHC{aN7~HN2ZFX*RM}jqr*_Efb2qKj^i3XM>xz)V8eYddKd8t5T;an!Ia9x{Kj1+_ z0hM40-Q?N}`kp4h5$EYcgx&ApcxKUgJQRv}Joz+a4WI~JsG;}Y^eoVX1$-m@CCv!S z1!p^%>j40S0NH}4iHI2Yr2uo{oh%lAiGtfCLvzEk8{kHjz(;Pd*6yoU@#=T1*uq1x zjmgWV$uI*(y6+4U&{{T7DqO8~ta;2)OBvstiRE#3d^BjFG|4hu*B<#+Y{u#pStc2g z|J48xu9y<~Q!00DucLGb+R?OpCoWR@0-KI)Fqbt@gCX}N>VO(U^xfV8b&o}9-v7$`DI%q)~$3WjDgb#j(QSV(NvG`}nA)v}vQ0`-= zg_Y*oLHQMP7BA2Dt^rY3<2>4SbyQ{`0-=>5sNe@$I~3AKl*U$Jx~f&1%mT}ky< zQlU>c0#GEMle*ySDa;uc@+<}7AQ!bw=z{(nwGS+4v8A~TAnppj9@=)DD(A>A*TJkm z@<69q>hYdy@;Cvt5R-SDD%C(&M5$-7<;2bl=poN_JtBaHfTq_tGbnyJp?v;KAmi=L zuYv9cOU1k1{qsS-ONXbuY*lOg9#OHNzMAjO6$d~c;p`6V5*?{s;cNEdU!>3EZs=*V z3~=ekfY2g^GLVGXVn5Ty`r*-%+~!}t{)}vAC~@#s`Lek0xez z!0>h_g&2tmoDZff%?K6#SSbuewuD|`cdfnWi3b9dk#65#gq*op!hdv~0-E5_$3@O5 zYtKcg^T}3hI3Tl5&7`fdRYxFHF22ebx&hkldi$vu9NGXqvyeJ^N&o|NZS*ne!|zHM zF?46z9djpP2src*-(N1=vP@uD3?{$B)o3pD&kPo9H}AD-%X zp&`N&!yJrz>&u$P07PVTQWFoxYb=6QUcdkR^>kc=Zx?yXqA9WlIy5S^jXzB0DHe-4 zDG|-z2aaA0@JW=AEd2*PeTvOv(is^d0FDq`ubKdH3 z1S+-uhi~LN%g?OaPoFf{&YB{Dzn~aveD>xd`(}wSqznsQKK9ei5r8~gVwM7%Q3e4d zhSub{-oVBw|!v_K$fq!^0_NP(LZY)RPQy%w?O){8g)-*|A__>DQyFbo}4*xS+S)jS7@Y>vJ;73^5 z(*E?Bb3R7-d9ybUgMmZ9=1nwUni`c7e|U8~0l3khRnt4s7r=TzN_NuDf5L<%jpyue za#TK5K8wO4o)Fq>vOC|KH+l3CNJvf=b^&L&7Z=Xs1bf$j>u<|CMq0IWh}xX3??%q^ zb+1#V0>5{e1PzwTpCAeK1v}aoGu|<<0n_$vY~+T(JQpt+0(XdMNzw1$bOYvf^|0ug zzqsBf*~mYDWd6RoK3FKyugsSW)JcCkTsyy=J`I~zIx7`Tkl;@&nGld-XdEivg4Z z*C%tSC~$Avd+xHfpwIXtp52gK?&9XOO(&$`FxGhWr!4ppsh`VxC`_xKUV_d!4=K#G zt^;#lpUJPQacn0U1s>@6e{YA+)hXxzc@se8fP9?w3fM3~Gyp4%w5<_nQ>AV!l@V}a z--k_3x{d)j>)4~cYyv0EG6S90U-tdV59|y=D(C;FUq6n6P2iHLE4)nlcaK5*2m9MY&y;g-i65(>T2^2nc^xCoovI*h% z6T3)}il>)4vggl0*PnH{VEC@LZvZZgkX}-_^-Rc6vW3@&7?ab2*W0O_{bv>~{CUFb z2OmPI2;w4w2kN))7EpY6Jg7CNrzSKMNDQ0k2;gE4AAD*D)Bh5dct3I&kg34QoI!1W z0;GR5)(qtk=o>^aK~Zt1Z;g@VO9Zm7kviC8@FfMvL7fG6U{H4VA|*6#uMr`cL-#o3 zDKt)MB3N6B7{TqCWoV7QURne)T;qQux*BSr^Yx}@MF#lv?0(k*PwqZ_4*$=Ir8(7R zHu+lwXA2ip_F)(lWgLWM1y(k?pii{E2u!`3LqmuwyD>CHOV9%Scw6S-(K@AJLZ*V~yW@TKaV zxAZBrLMmH15ukPFfaTXp02F5~;rIVQP)iADgdcXeyLOyPY-AlW-kwF&SLp|hr$;w% zJ#>%wv3+|)>YPppAL9!-ADlS=vK}jZn<7mPw)E`N{duW+7^xE?AXl$-oq`6gpv?3f z1-DdWnU8Mmy@uzSBIGy4KEo;a@TubYV8d0C!x&L0pAFMc1mfg)OJi>yoO8fFhHW8Y zR|FcZ$4;|flr$ddavUHpX`*@3<2&dNH|cIPV?g4f&qx0mV%>`4{gTHrcMZ_C`Qw+ z`PuMyl&IBxt}3r|ad+!hf2MhH|Bhd6U-rzkSvA-vE6r0sZoQ*9@Q?)zM&~@WQPI2* zP-TkEJ|mSy%0i!wExbg0m)OL4R#f3j$!`!xC7|e842W^X8KQu!le3VcIpQ@xoTMWg zwf3l^P-sc&$P%7uJUe&sbmD0s6dBtz$)jm_fF zIK}lx{iBw_v>V|P>OPn9%vEvWVhap1TF(`7_zw17k2>l!cJ#ep%nUfCs70jKEN^im zlXNVJvIvvF;o3E06MiISRK(d=8zthPnSPn?paxE;11g*M$qmTrb2uU&+82Gq`yKHc zBM@u-(*K1b6!o1Y^9rb;q)fo*EBVI~z6a2hQHqR{++uGu4Lc1>c^OF;Iaa7Dl;BbP z@EX)onH-4nKXllc>)-$67l#u4PWX=Zi&h(u{N#=O_hfD_Kz&n#YE<-a5#yo49E#hK zgSJTUkF5%n1JG*eYo1?U?0EGm(jQfuLx$0kAnpw<=MNR@O!2_7O=Suwy zi-))utu~ufSJa!MCv40hm8s^jMA5$NpZ&a9l6hucCj2XX5|I%E1ac3zX0APV$U}?C{89hwMxIq(EaqN5#3no;xw4m>~UB96mU)2``;P&UKp5NBW1lBum z8w&OB$)p>~-fhKXrSiyLP~>S-nFmcNK>C_N>} z_E8Nf32`H#SIi}zUS&c$Ea=D=h z#Gc*u@d2(22N~v3ACyVyyBL)y)7|7!uRyoDADpB8(zLKSJ|-FSXZ^|J?;YN>2_$|a zV&mK?f=%n1=i!_U=RLBqER@jHdXGa5hFvf2`ZVrgtUb`&DErvy!va;+TzIYWvP~%S zfekHegRlrB@yh_vsQp`=IP<*Z7ZE_Pp9KT`8Vq7;78RTENPt;;DKpXK4uH=z9>l_j zsy4^>+^g&9-n}qoP_>!8(W`dU=XpCdL zR=DecZp2#B`oz&t+|sVym$Wnb+vPf9vw10;mh5BsBsXejoGFR6w-{P)> zyiUC?qOpvN_>KE*O$@LNOE~n5pkY0J_3%egM+IbiC%^RxiO1zBf^jS6cg>8yB7Jca zNLa7!3D3X&8tfn&H2cO@5>(sn8+6H}l=U%3S2gb(U`yR^b|v-Pj8}+0^A(iKP?HZ} zc`gQeLF|Is34`}ujXw0k(MygEC|V67wfnir)Kfuu6-U+wiu*BeX#I9+&I?VIF0HA# z;EbNwQnZ7b3oc2qu+=_c&R<^dEQ_!eg^n;j9E?B&w9>((pol?G@%?d<|2+B#R8xlK zmDbx|7UX~%>T9NEbi{UT+o!?$4<0}E+*Y+LxX8jbr@9^`xeLEtM_rnkajw9qoPQPh zS+q;4@s23B*UkX^>u1Kh3pp;##x#7^6V@v`HXQ5<_kKdjMFdV0CiodP~}J<{bS`=-&J_xSEECY+d< z(jajcl1QvJK!>dcNAfva$XPS$JXi=VN@>#wG`-4D`fNy);`V74r4x-U_cvyg;V%hvUh4iG^cgB1lBxr~ z!Am4jZHO#g)Hy@(JJ>l)qNBc`f8&$oG*k6K(+Kz)kQOMlS(H@vrZ8#)G<_w!$3lZU zU*n`5AP-o)yeu0@Vz{>MPK2V}GvLo0+9K5z(X*X4Dj+}`>0zo$w?IO#PTeEwUW%3e+oU5Fmk?bMIbvVU2G~Q?O!{Pjgij=}xoMD5 zx=!y5^t5VWf72RH67L=2HDV;!!05V-#_}ocI7OcONxuJ{?vJ@6z^H+GPI*Yda7$A~ z1ntH{EgD}I>ehRWVJ<^gFI!X)R1jgKSvc|Dr$CGd(whNbA9S^*#^}A|`vL+I6H<-w z(FAnyad9#&TN%J_NCbta)t!1d?BSM{VQv(t7{Lkd@(IRvaEn=@w?l9Z7cH!*mu+m# zI$xfV>l!SuKOO?*##5{6sjB1L2+)mXh#^a)CmL5S$*k`r&={C`H|yl&9L*{AiD^+F zxx}8fkNrUFTXz4c_O55aHYPEBpwHBUUe$|dXr7|^P?XH9Hs31b0Anjfp6$R1nOVV~ zN_^$v$3#6JYLJr+qf1Vfx1V43uWe1}B`K(^YjdXf;I0w?4A{`BOKUCg}^6rqvU)l0V z9h?gDI2)F3=56m(u>J%Ly63^4Da=k{_7lnRVcQmDw{4g2*mJebuEi|K>^*ywdBDJX z4$y!ti9!* zw6BZH@Cb2sfU6u@B%<3_9zOcKVKzCLoc{iNT`le!u;?|mu%G~l=w+!jiW4$g6wnnZHB!F6J^WEB^q@p_G)=f)WQMC8bDnQ80zY z^;4N7_P=me;ngQx*EPnmj6QUIepy*r*Z|wTPyeD0D(hP1CCMWX+5AnTp=y#bKR=&c z&ypBAszl-!!f>{Yaz`1gBL6j*L<-ZnN}XQDDP*B2L@-k3!2 zC`9}9b*qfT30yIMbZIFc9mF@ZwNx4EDo8~|#s6byC>H%ro|0o#M~8wvN?%`JZ$?%Y z8N6Vu?3}!%rDZ$=z%i?>>#1UTG_`_)f@YmgwzoBCP?=z)@UV@I4Fk@pA3tmzGBPrn zTU(PE(f`KD*4FlIV^h=pFx20pG#U|4Oxim-!s;m~DSMjCcu6q`)Lwkd!L+C>0l)WNt9rVO- zwA$I~zrTApJ(n)QMe*7EX-V1pm4xRX@V}IBb7gxAYq(-O`}|z3$6HtRtJWQKY(ejV4X74@W90XE3JCo=i&B7@5Ot_!A$#fQ7lzFpYyt@}OAn z#Eiu{24AqZW>?y}yFZFSw{j;ZEpo2x4KDq8BM*Pu(EePx!! zFJB}uwC>3lp+F$)a1;pFv+d4q3Gc1<6gch6oN+zIua;EhE)vDg>1b$a zr-VdB!Ar)JqsuNVAyMw%`)O*276o}ZMEB#G6eO>y$tcS;Xxr4}E+nQ>oWce_yeon* zW@37}wDK9^lLo<$LB~&A*8n1xHp`+Mt2F;|l{>5_N)n>TM>2y^BP45)v~(S@PCM^s`$AoAw~khej% zL$1Mv7=(3sm~M+L=L@H>`@;Zu?tQFR{{C0=Iz@b6kDtyaCMKpZ;V5|bu1(RAaf3j5 z%mDNmAZ5jo7$*wp_LrP-7{+KTV0yhgW9(y|6dir1|Ht@vL6i6DH)8h#u`R6rXL@v( z4HP1OuSb5d;;WBtaGd7l@oP_QyL#@9dQ9<2PT1DL!J)OmkuGq_1X0w`p#K56S=<3j z1Nm!_UlL+sy(#)ywM{=mV#@ru+i9qP)@7SGkTlD$QDaL=bovx&Xq4B|V)`K)EV9Jh z2(Irxut2E)KDE3_+b5u*h-=agE*7US$0xaTjuvkw04Zd7a&ofj`7L6!%$7F zG`395$Tr!3cxI<&t}PL67*tIONA zseJ5w)>8B8IwVr1aJjsyYE>@I5#?_Hz{jZT@mju{q=upX4eaIJ62VuWDX7}5IpTT@ zUu^&rfY0KGhlkCw{m(p<7#ez3-1nWpI<~Ts|GztMHdHvbsMxEtyjIWJ+H%0S^R`Ei zE~XN5N z+5q$IM|oDpOg;5^4opE?`ne=XGsjWXiTQb+S?w(Ge6D{!GUPoX^!lM?7VzaU)xdSz z9KW}3*5acThHCYQEFQxUt^d|}pzY?kR`Di?N=F?m5j&6sK>j*2LPyT>qd~uFgk2ZH z9ldOVNj}Bjgj?BRabx|8c(o$>fIbd4Ie*_K8z2!Qo@~?nK4#UdgTXp z(ACwVm$8EXU7w&47}z2X-fs;8U?ZgFzO}vm_p`WjXVF;QY_Cq&T26O@&6u7bqDSAz zNK8<$z_=Ht9cg1JME38`2(;P{FyT!8aGRZ-J*9^Z_-BzD;Gn2$6=QtWZL_p$#L-LE zW4elqi#zthK>FlBR*q)ni|pS#ihSf9$q(;#13kHTRMYO|`*A&buXbZBuL{IP@0rCY zOEXaFJb5zP-X6*lO1k^s3W2nP2gQ{GgUMouW@G3`Q)u9S0;|mZ25bh7`{H0piE|H| z*L?bvV*cm98-<`haCEE`2eKxHRufP5y6fdVm;jrIXu~z+GLs_tzm=4ihdvAPEAll$ z*J4W|9{J+AK{lgIAtCUUAZz9NciYt(pNUg*OUu2vDAM}|$>xH{+{6CwCp(o#X&*$i z_#z%#kv>`V+r4J9yckdI{^GNnL6L93d2g0#E#&6P-`(k~B}MwbmkqK9Fx@d5KsNZ1 zmA>&zx6LC1!^x?sC?M#Jc;QY%N5}oo<~IS+U`tw_$^GB6hoCDU7db$~nwyy!FpY5A zQiv|RwWt9pC9A$(fXC$jUqJE>_V&d0lhrx?7e?%t<>lpo!4@?!HkL1+^W=MX{n-<2 zaLmHeWmNNG-_+Rn-vp%zijBV+68W-noS#mHE-YB=ADx~$GC(MJ_Ko)@ zU-kB?xbs@Vs)|_{;$upys+^EH>Ax^{yw@%zWRO0)^)yN9FG`WLy1ltxo}8Zkd$~Ke zN3@gW^_EeOKWNr|eEqSpF=}ZH_rH(%UrH(_`0Ta$&M%HpuBJWwgKktGOX)Wsq|C$K zxtEWR`lp&2UX1^i0%Uo}*WJOfQ;<@-|+klQRsrjhTTUmS7Ap?6mEAAd@#w>wVhT&fdCirH#hMLgGLNz{cv zP5-XbK&$#Q-fV3yBFHC<#8kcdCGLmIPiJj;!G^bthmUU`^?wUUA4Co;?L4Te+f13S zdq6+{88*_&R8QI!c3x70W<^XAkD=+=4kWFQL$9V9RB;Ur4VR;oSwOb`U+A%4J`?27 z2Couoht+lJt^|=CupfwtiD_mFebdLXT08n#t0USKxWSmusJr6A&xd`x&w-0T4N6^I9iad8=s$VV zpF{r7nklw~p^1oy?3g{ULSpHczd+AF)KFQ45y$taA<%)EbDFr^dB2V@|MLE4zX4Ee zS#4=cX5Rq5%Q=u)o{OgvOn-~|Z}_lgzTBbmTpJ+Y7Zw$*aXXW;dQ{&Y*|%Ke`|{+( z$2}N4CC3D?g48~N70P3K(x^apDVg7d{@+U6F`I94WpstTs%Jj&si=WiJ$lkL!vYpi zbkf7SFE97A!7}18`Bag;aoU9TZ%y&49&u$rhhm4H=LZi%Dq5vZj z+iehj&YwRn{Xf8q+*AJP(}_>g_W#Ne2t&Yc=KU<-{NM@UW>~J@`yIVR*>?6u9DZ}s zB$5EC#b+9KNp_sD)a(A;RUlSS=vTlUlkV#3YR0qbAd2tl?JVR92N~Dh$*2@_YH`uZ z-~WrBb-nSJv*|IV?Qb5G|AGSnd|(A>!1EPLt_Xe5`I4C-Ce~Uc7#K`MH`t9Z;Me8n z=V#`eKVV5bKR+*+qqcF1uUSX^FE$Xj_3zaC#zNv^Vx?jb3e1?-e%$=ghRuHa1>)Y` z-ZV~yg@yR|_{%tN6!rD>P4Jv_TExo@)Lqg3m**FO!*%QC1A|tSI{wJMf&$}KkPF>m zdvEyMnEpvh|BtTsj>o!v|9~%hWJi$|NeUU+JK3QSib(d3$h<8B| zs3u4hUPuFeLTApG^dX({FYIu=OAJ$>qI5O$?SEng&4^Wg7J>}UVCr4Z{%@lxn8Z$} zvF?o%g`r-dKq4~j0+9|AsjFA7dg1+*EbzxGudlDIg&;l-_kTix@}OR~yR-Y;!)L3N z^#X2Jsj<`vvhd(iytY-K(9QR+t`_O~Hau*&HC?-ZOP~0^<(gPp&T!w^4)Xwqp50{O zOS@U@OOu~FP?mOJOqB1$a4Oh z9Im6+jf{AE`-}AH2LDq_T=R(U4vIsL^NBcg{s6)PMv<4HZeXBCXK2V^rt#A3sHw@_ z>-GVFB)XO&yFYjBEg6Uuy^kl}Q>A|{U6OiO-_Q^cW6(1)))x56y8eub7aE|$e62Gi zqh)NnlMmw@3F_&{{^yMQnerf!7931=pG~8UHf7T;R`f$UXd@RF7lZpI-)$4T`zjg6 zJTW&9NH*8kZ7AqZohmFWq={x8-P^0f<$C+~#r^2x$EQ&gUrRzyzoq$(l05A!S{Ahd3`Kg z2?H7veVdaPSZW<>UE7rqXh@qU@J_B*BkiPo~U!KYno_99J%8ES-Om;5IdD)Q7jMFw7T5pzn)k8 zT=DCPy<2;jhWHY69M5;3{y&C}_Uhl=mXk_SSGq^g1PyB3gT*He!8xF%tF1^T)i6sa z+73udV{x{}k1VAh8XDp-ymzk*mn-bwAz!^3F7_?jngZ3EB7SBNf?z%{r(85{x7;k^ zK9uUNk|F+2d--%xSpPrCBF%XHPe+}kDGB*Lf+&DYP#zVj(%yQNxvUs%xw7ragKxPraKNaxKT#!3j+HZMz85u}Yfv}nrNIoWX z6Lz~C_u=B_Qf*NJpUqlKXy3?4aZxbhRZ+0Y6mOr?)Y0iH%D7ANeGFEfNr7vczC;r19$C1-B10v|J+lK zCtVvG8^9FL?N=2o;QZ<|DdB=doBev?X$TM;u1MSl8#^kreSs#l=i^5^p83jG8o{#{QvT^nr1$lmX?$}v)^#Dg=or=ec;h>`BjnHQ?;i@wO{rcr33~N z7>TA?DP(Ca-_X;`?VFP-@c=WGeq{6p;t&a8tla}H@ovFj3T7|x`Gh{21lud~zDM@- zr=Gt?GW9Hs!u=lU2l+-8?-l`m{tU+KoE*gzkT`-r8~QR32~WSF7f0T##)scT@}ra{ zMBeY)3*D$oLq@z_N*7J;t(W=slRy5e?}`{NBhq}7HL-UpwGN=`7+VB(Co6zPWN|_@_i=b#++Z*jVXN zcuiUecCu+kc446WO8j80`@K7N(o0DeelDOa7$j{aop@C|K+M~>VI?I_B0jQ(E?Y}? zjfxQWbb;67Xe+M!!B+U47Xr3|k!puz9T*6QcONSW)IXAR0z*hx@Cyq=81+LB!nbSbG7pm<6u)dOKU3i!XISoE zHPzn|1T#zIQvmU!yNU-LP6U0n+vm)$8}L!Kj@nnPS0K_%Fp+hNwR`z zJ-JH*j|R#ICf=0{+#QO&5Y`Oh*uD+mi9%2R&!0ahr`-sYXJfgJV+elMpV%P3q=c^1 zz+==JC77gMN;)P5CBOas{ggQ7$IO90Oo*&I^SM~M%mwXETFXEK+%F_p00XaIW~_@c zy!rP_tRZ57$U%7(4~h7a?WiIPN=s#Y_B>F#7Q62@nir?8Qn*>WxrUh{2#N63uR7Ok z(k8%!hy72JniTy@%YdJdlAFu<;T#GG7OKy$0li`@)5h=7UBL53MMvAIzM`~d?UO0~ zVWMnKWYL+!r3tX;=VFL+=t$ZIX%5Jqkdu*d`SBtGgqS9{Vqt?LU6)W^-rlU}rORC< zZ{BPNo-_t4*GFT@+9~t&p-|U{sCEivtsgr(J8j#)-5ZON2^<_8;&`2WGmjhx(DCpM zz_+sIixf#*?$CEG=Y^8$#5`q>QNt&bKd(w6hfgz37>M%sQTwpla0Ba0ht0Irl;k+f@3iBJT7pTH(ZZsl*VNVR9UWz)rMLOA zD|GAPKYjUP%G#&+Dd!V`Gi^Xg3GXpjfOK$-X5Qi~7}`1|7>t2~)Mdtzp0Mm@mc z9!?9A5^Hhy>xC8a@j<#BDkHqdNQJ7Rr@wsBRyXL&C!@=XyOC&d`TbYZDsyEvunH9! zRaO0~A=ke7gXczcy=MYR08U8C%`%|i3&4&}7;lw5OGvO={Q~ueMbo$C<<9r-uVmG9 z*g0urh6<%}YUF5NXsXCkw6QVB$jX>=xqa-^sZ&y{FQH6fU|@TXjFM6kUh2=AoxZc# zDJ&x5C57@pqi-7-MZU-{DClY^fL}FCrXVLT!0fE8Rf-thxZTj$SY{XpxqC-P$KSty z(`Gf+)opQ0f6}SXuW|LG=xdfxJU8!3{vuN-BdF@0_Oij_6ECL4!de#>O9-AldzN-? zV0gHww6t0jB;hv}J6c~$ik>j37Ix=RWxu$XVRFbt?(}q@*x0FfcGNEkIOgXQ$&@D)OnzyrQ_6 zCst+P|7RQf#}A$z4%Fg7mxgTARC+q^1?}=H?|4UuZBz}}m@~=Qn>fkk3{f16cqAex z2}})f<%m$z)l@+hY$TD4Y(hi$?Ypv#jg*-ECcTg%(}cet!1zmxuKusxf_%kO;7{d} zKtULDUefmft-!;}`%Eq8h7+LXAl`>UBFc{<0v;%Z-r~1_WCyXtRP*NjHz@~I`^<+1 zMn;QJ`69r>!~0-)>CeOaCgI`N(xg|IJfzZoxWz^E%_UhBoH!kNS7@^h(?r=dyUY?b+vw*>|qg{ez;=u!93-IZ~ z{X-#>1%VG~P*9MFsOVp|sIB8w1|JsY@1e$_HgsOnd&8c|{6Kl`f9K%Cc=YZs%gbX) z=hwYZO&8HSYv(PVsh!IY%4R9?8KJV$%b%*AJVP z%Pi-(K71lOC&1{g}Q&a*(=lxIT6G z#+)p|o|<_sK!moH z4&fph12}W4aUP0@k^my458>>n+iC?jNDBarEIoqz7p;S9c@dRP4*ckfE9X?>z{7-S-GpLxxz{>iy(zq3}c?d|O`T3oboFgN9og#G!|Ihb=nQ2^ES z3-_5do3G}4!K9Brd^(PpEA9oU%!4Wv!gr!8Y@2!^TZ-}5f zTK+mZs%d!9E%EE3o7su~R$kZ-`BmEy%ZAVT>@fj-5`RIzZToxVej(yA4&?~)f=gDtZS}1XM!g$o z-252F0S80E09NOK@t>)L=F8F;3DAcll3>LwI>)AbME>vj92`G-OE8Wwb6F)-)>p=K zVgC+Ua4Xz@!TI%_Z{PGHo*W4%*lcLuk+-$7I`^9zxZ-ln7NqTaA3^?_MVt6c?S@J|)@8qQE zJ1$z%LrIhWmI|yQ#08+QUc28kGAaSt4XCQB5{m@i;m@%gNi`1Y=AAo~1;E|YCf#l= zq=vbbstKe}>ho^3%ZGn$plTdM5*uFi#Bv8{086v2zdIAH@LK-gIUK!v%&JW%oRgDQ z!41$!)P5;QX8OuJH0(txP_Qy+eZJ zkm~yGexm4+Y!MT+n+jyd$H$vepq8MEI_N8~9MHZA!i#`4fY+ROa^O3J zc{Sw&bFRoIAleN43iT73!C(CjK-k7#FzVQO`|qpTR(b7hIqP_D<1E1tsi3Fl1j<@Q zMsBMxOM3|!lLDKdI+$+(*`w=l`d-l(jt6u+%FhF>Rj7V*PWjU+Y(gwY7r2-2z}`?F zJs)LqVbtZLG}}7lSvt8V+TPx{uz2u>_WvH1^4|;QcOH1vuFOLRS!iuXON+b;UASUR zr<)m2li60qXq9~j?9pF6iE0uOcN&*d*#|92jV(5|`rkPLw&Jh;M)5Pdxw*-Az)*EM z_s*82!NFVbt5twifvt|wx|mSE8UEJ}sfR+oEEt*Rdcr*zL{JcH;?y(nHvS&f2=<>6 z0O~|WZ6ZpR2z^boEQ~n*0ee!yUx2}OJ|zEl55coxdjLxOwOw&p8ITrM8kBfht)HqI zUgR$rUhLTWXQMu7BsAmX_7v#k$=ibRH5O;>d?)OR$(p`; zK$`fu)SGmu3OM0DAOXFypOGO~9hH80VGe!JL4*=)^p z8RiMgFZLo=lrpcY#BtnzavZ#!Hut6#2Gnn8u!-40W}eF~K?kO|eZwQzlEkmr%`2f*@@ZV? zXl+i!DqtVoZhP8tHsXTB_;&iXcZY(JcVF*DZ_liM)v=d}R-3yGAF(HIm3_b3)Y4+` z{vFf~)3;s~7E0YK4F$0mXGe~$jpk^H!23vN=HGl8YJ&|hZu=bti&Wx4&)x6fR&a-z z3Y|K8AesB2_(%eX2>l-s;=KF`oj$GaAh(>7_yu%Mm~{F-8)E++lGX?AW^GbtQ9XRK zf`5ez1>ht;1vkXsWf1GL7GfW+buW8w{NJa(1Gm@lLEzYmrMIgY7RV_q{1ZqZ+dB;7 zyeJhnK$ml9gBKcco{^F9Z@hv+Rb!#UCH5f(poQUqp&WM4_;8SU!_m z?=F!_4~r3F%6*Uf&v<%oEQFRrPrOL%GmBfaijA0Sz!1s(Cs=ZLIqH~Tkz`Behj?`r z@I4&4uZf(fg&!Zumw$7{XKRJF_A*`FoVU2+zzH3>0JE0VstAr&G%nrzf{Wd*%Ao3@ z3zZ*opDMs0is2xSe!$t_FaAa#XLyklWjhC*oZ$;lu_Zz7YW}5VX5fq!uyAp?gmYaM zZe+v)bgHO@gknv$)L#uooDXWm`}zrdS=K?&h)E!%D8J_b*qm?@lisWUY)yYa zaa|PYGBihKRO2F-Z2lUW2+W|mP0cWgpTwKPgWWSQbN;r00CMa{GR*S=ZmZ74gG5w* zB#e7LQvurS0l_sc!)8QlL*S=a)zH9lON%z>BkF6XhUh4*8El|xQ8v%}cR*q3B&Pl+ zB5`VSe~Eqoz$|KTHlMls8N31VFw+HSPuJkiDlCK>IM{xrdl~VLVO2b~Gq7HGrEQ=K z(|6+Sv=$nwxrbSMRt(iDWGfdCOX!DK=?aAKuq4Q>mRYB};?J}GJWt0MCY8Q3hIX(@ z+_$5p!sE!Nke{B<;Q2BSg4L>Izp8f*1mpO>|FuvI8B@uM38%#PAobBh38;7@?m04-Yy$Wkx#q?`|9?|}XM8?D_W7?Z;vu(*R> zRy2=>WNuiBu%Z?u6*-YJg`J}-U<0;HN2I?wuZz1;R!XHZ%q$i_6xj@%) zjiMW9IQ*9*NAJ6~sg!m1@Iq6ak#jqpfEy4{p7WcI0`UvMnyl<*H-h7b--L$3_r(hE zR`~u%CO7yp`}O^{@Ur+#Ofa*OvjQqJK>n!`jKgXb$_ zr(E2SlYpPSE56uuFPl6ZV3|HMJT#Rw0*jVh~uI z6K0%S+VCi)V*WlMQRGzQ^ADDJeX6LQPN3jnH9OgOt6^o#UPQX~?@7EApfu4(d|rs+ zt18F6>eE&zj-G&O^0l|K=;r4%Q*P$280+f( z7i*8Y!!(O`2(WCL8M!UU&p`>)P(&-BGG=*M-=9`dD`AD=pW@ivSPUuJa5L3?m zdJOj5(Mc1znGZFQ@rmOu!AKtkHyA&Y7brZ3wYc?H5{TSvd+%j^{)EA?B>ol)BRRQv zUw^iK`giRE&E(!Yl1*e|UcXA-f$JBK+-&$sq{6Jb`wcS5`bbqkA}r7MkFJ21+-LR! zHGZxF34V-lkwX9p-|+@Tc8Ko>qw6nr0C(nmW!yMa$s-n^_ODSp4Zx`gdr&V%G?U)y z+t8of_ZttMA?KUU^0c;`elRH!;QcTIr6*$90I<4i<4EYtgl5pep! z7%&!NPzb^(b{`(UerS@uwpl6d1R29Fi`Q@4H(@N4!&4(5P8T@7{IJ^Y+N;9?vOk~n zne#n|+CswBS_o<4?`PAG!1%LQotryX(CJ+;u7DG+{iGy+?bU0^IO7l`M=roGU#oHj zsP1%X(*9B_r2uCGpTM7Qo!qValR}Xz`yRdkZuQmmJ_!FnU=oDW=z(ODdHLa+Q0%2G z7^*TOmP!3bCT>E;U}0hK)jA(mSNCk<+)MzFNQYyOTlJ<;gUBp1n;4@GI>(shzET7_ zqYew{405zzD01J@Vo+1$I{TDHl|();jOuJZsVWz5EDB!^ML;b_Do16bN^?WunRe>t z=FE?kArZ%V(XmZD@d~$Z?j8HSn_^I4fE3-Dp{;FU2o00W;K0DUF7LH=5fyOG11f83 zMEUuzYiU6$CzDG}QA zj1Zdv!YJN?nU!^y^&Deufb48VdHJJIvlHc0PCh=G0`Q7~Wxsye?f8cXNK0#cSy`!w z>Fw=>g3ITJ3KlQ&p|DAM>=-26MMXtfSy>gWqo%;+1wTFO!u2E%L9VYWD=UD&kt$D4 zW~3ha^^4ohW^H5RRL~>S)|~q;E?ONBMZF%zl41nxNGD&_67${VS&ffBkwP8!?Ai2~ zIe2Hv%0%C$y1GK01Ul%=fQ}XUTR?{1KO4$5`GMGwA7Y--4N!E61GwL)RAoUyXkF%j zdE!qJ@s;jw(l6>O&*I_)Wn>P3kiULHa#Ay5WOVd_yZe(G0A4`uzmci|yRdNjSN7?5 zzRSt%GEnv4Sp5Cl^vBZD%l!NhDGpA~sHmtH8k7B(PLo_Nk$jy0JUg8N6HEIXWrBIfCSc>=aA`cIbl(cl{X=P>QkcBS11JV;GSw>~aFy+F9 zRaMW>XE`|5*4CiNOo}YWd8?P+ES+@~-z;K30eooH3ZAP9WMAAweML2Ng0gw;b8>R3 z$P}`QxZ? zg`Pc*WQ#X9&^~_sbAAEJ{ENEsNO&CAVJX!u;GgiulI|`nBl1z8>a>Mhi(BE`~_-iY60{)cKJ>2 zjp*4Kp|jLl$s!JI%XqJ54G*H!B3H#d^KkG82WG5gFGS(Yvt0=~{mQV;V;SPG*!O4R zj%TSoyAWb3%<#1?Q*sHCa1Y2*h&vG+VzBgN}ak z>Qg;X2WbzZ3uza~i1h7(dVTxM)H}@bg}EeoZJu6WJ-yx$cPI^$6dzllo-U5RGKD6! z*PldmgOyo46`qtwFG3X1tV2cgjBjAXk8ujV{q^MJ+V4@u50~zrN;_HMF_PpVaj$Nf zklhATGgTvOP0A?hsGt!^PfyYr*hIGtuw4BTgTPva%7)E

JXPjJ~b2hf{SaGyzGmZ+{;(Z1j2f#ew0b zC=Jm^$F8`u&wBz$jxX%AL8Kzgz+({dh%PGo!P2&&I@QjUx8;wfek3&Lc~k_w$~F3L zZWL(WH2Y)@Jk2kgEI4$?CjpB}&Yq2lh+ORbc`qh#R8MTw-iEb6@c=R4h@D zBF(zv1dg-Y?~#HD{O{}I51OoB<@#MWZiygdd8Xi@bikyK1=#=xqtN3Ko0vVO zVN_}O<^^I$oEEPuP2qS0@^_2um#mLd)-2^2<3;g(`-;A+nKB_M;x_QjN2t-xcHqkC z4saxetp47tmV2jg2}R!7Ii6;h*OHT;JWeNoF1uOI&Tghb{wS9|(M6x1|5*f%r)#@O zF8v3UQ^W+qZX=RjTP(LPNHaF@LLw=Q=*Q=Z~$#fsUZVWcyHq6sHxX~ ze!h~T`h@em*4ol&bbYXhx$MFnpaa-IY!m>JkFa=?)i@wW=88x|;72%{m+(@FoOR?A~ zT=lu}lwZXI=R)10+__4}QHkEAeQ<5)@IGh~cr^@&OuP>afj<+H)YFa!KJ9e+A&>6( zR2(?Tva?I3zk9OGr87}`FNfHV)7jaHsz}MNRlmKN%PY5Kv-=shodmBzB^6PqgJr9G zXi^TL_wl@~;Y`=I#$MjNqCv!s|N8eN8KVf#X|(dAEpal;AHqJO9O=ms zpn|uRgZ`WLxlft?L4Mym-P((d>xbNShFjdFZfFo?Z8`&8@N*$*BHwRRpWglOTK&NL z@2+io_ZRB6zlJ$;uO1wvh7eoe&Q`~7LQL7-Sse0|?~aU}j6Flcl$Zv2reTdEjv~{y zw-0_PMbJOuVq)fd_esFpI}#_+?W4!mZ%PXC4VcE;M%9otQ)@q1VTwua}y!p7(;z0NEh33i0nxxYi1UWD$$?szY;>;U{>% zxbnYYwc3i(Um$Z_A*H(zvbr@D&HeX=;bzOO|N8zhP>Y%U1&q8?O1bP$*pAt3uP7@f z!~E(m%40LxyTw%8kUjt8@>9ME3n8?!sYhA>*6)(dL-A0nYp)jvU(G^F%olPspdPm} zK2a6wm$5jxMUi7>dy-rzTHRe86v;5owQ557RjNZOcp-DbsZdIQTQh}kRO)~wd*{bg;8gxU}`gU z@Ps6jv=?o-xk$UFE6g&RxXTLJH2(?7YG&HCvP$TzJB{v4OO|JDm{N?lUhwJ|s>{5Kvk*l)L&HgKz2Xc}JQymf%#^}a^!MhzTWL(PI?K-PN|nH+C7%K)=Jx95cL^fq z(;{brus6Scrdl{3b@pof4ZJk;imJdTR;c37LV$V*Q3L(1VwS7ZtoKGBb`0#+Ntm=* z=v;hLg;cDSuuK>3(n({F%#6F-#_@hj`;gG#{yHIPFm?9Suvc_teTJ$;{9U+Gi=K_p zp;Q;71K3DsNOF|D@jtq^aBgg6)Oq)(yUUqNCKwsfY9T*vydrLOudJb~bR&E)VQWA7 zwx8wlt7X9P!q{2x)8xDw7RpypHq@<#bQRa zZjn~XS_jL8W8|VEeqzS2j@KoShF;kC1qz2l|l(yw0G$tTtK*H0Gq270}ODV^}-YK-#^NQm*F2bUOob3 zgN@kDOULtk4-cM8!j{m3vY@7pbuda7v^VYGz0I%;QC)F&WDraQ9u2n#Tq=z$=i_nDrc_pYPY%N`$D*N6Nqm#@G?~t zB7l~!;O?(b&TDBY)sOKmRllKXFjFwNk|^`=tr!?a{k0`#NaSZevg}RS#`Gt^djSg< zTSpijw-8mea@)y$HlzK|owvC~I&*<$9GO33eM0S>w<8qIW zpVPE{MDhYC>* z>9;kroD3wMkhCl+522|DC2(1O9SAa=2}+Y-hj<^VKe2_I3xh7rj|l+GppEC$zNq@f zu-cxa-1YhP60YUMT3bl~9Sq9Ajubw6ZgSmkZh=$dd5FL5mS4yTX6CKu@bFNW33gY*36Rra{!v@< zLfekJo=%naTLQMM&%tBit6iu_m2|fyMK~cvSO;=5z-F6hw0BGw6ly}#)bBXy`cE?9VhbuoJ8z$?CO?d^b=m~?03NVtS#-K z&IP1(Uwf}li3NvD)HY@eNW@fn??ck7_-6SFF?g$Ozz>67m02XGSc5&A-FwjXT-a{% zatA|0mNEg}!N%LI`Rq2v{;?Y;ja*~*XJ0r7(q@4MNMyZ+ZnzoV-N=5EqD$p%b;x{YYs))TwsiM@tW3WT|6F|QRW>tog0WY@2kzbVU%>&?8q1)o zu2e@k8?nlm_UP$`pX9sP3l*s?u+D6>2au!ghx~0c{rY_Wl{Q%@;6N~#LGlAlfq^>s z%vH@jl6;TL#*jBJHY^5!)nO|5rz|Dp&6$A7B8_czPpJug!nPD#S62tHVFUpoCVRa6 z0ES8~@$WsW5Wf7m)%~{2ZFhnB?`6?;k>P;?s~`q1GMJ7@Z8wWf+AFkz%6w_l$GJO# zV~2WO5YKb#@pmG3$%|?*N!4DB0{97T7p^b~afUwXr>I+jaWBoXVvz0}$h<;yIF+lO zq@AnI;Cx*n%ZjutkKV)EG!+j=;y}{0jaXP_`^Rw}*TL;g(AkYZJGL!~=e6zpV2N~Z zw$|B<>+jXLg!n((Z_ep`qSCtih|t)X|6CD6OSDW77FA+c1G=q-k_az@K(fE%7{_}W zc^JC4E~HQ%m*KfOQ$W#G8AQqIBNL2$3B&`e7lQ-NJywg42*7$s5d##M36(RE_^?-H zi^3;N78O)1aR0aD{uCJoN4aFDJB$D2m#hNNz#l(G5Xq57^ECi5Sz9x1MB4eafgfeX z94)1|!Y0d;?GO?_G0EM*q}E;qjfsRz>;W2Ef$4NY3O8k$T5Ne9qDeAlhOme31O5M%b2yzj#M6w%UW zZDe)gTqk*~PbfWw9BX3@ilhuv6+`@4>eRNqjvA;}aT%q?{k_`hT_^OQ#K9b$?-m8L z*Dij}B4RNgdceu?{W*m(16qctSV(HQ?Ks!T7X5GeT349tFMlG2^e=Bjc`ykE$M7cF z;l>4za}Zfm5$GVEG_oSW{s|LqYt(UDH3ip$%j#z|=f30!9|um~GZYCd3=LucfpMm76IG1*FVQ)I$6ub0f-RgMmcwg;m{Y zV)M^R_o4chr9$K&_4CIzIF9{cObyX2(nBXxKc}x~tX$S`(vU42*4x(Hi0={UzD%$jjAe;`4sR4Z=0=M@(a=&7MdB=_SuD$T zQTc56C>0giHPWhs0hz}q8T3!oNH?&J;d+GM#S4(&^1bH$+Svn|b32AF92y1DD6XBF zc!PiAybrwK&yO&u8|Qh{r-B?t~V~R4odR`ewz}$`PB z?7?9=ap*bYnPH0Lm$|K1q^@1E^QLb4(PFFe<@VC!?uwoY9{c#&ZI%!$^53&RrLCtH z44bF?DoaIsAZOm%f<1LE_~NX)6D6VlYhO0V%^%gjcuW9&$?@bt{jqz4&l?>EY^wmJ zHIj!U_KzwYC-{BUSHMeO{z#`tJp&kx*O>qF>2YYMfmGVDE=XRT94@A_{Q7I1bRfPrT?h9QMNqC)+x0 z}@}=>jM;zf`w_z3rHhMql6LGMB=to`3b8GfVJm)F!iaa7$rR*#=k6VmzdNuY1YL zwk1g^`#oen+O?hi1HLm$9h#3)Aq9{%$Y(#9Qo+L*siY#2s);{SxHCxWG`AgErmo>i%y zTLuPd!8iZZ!K)+TSxB9a1L^SgoE7f@0H30kf}cO*(UQJYgkT2Wku5=CBZf)B^(RD+ z&V~K9fd$jk7T%U{(#Q#&hPoU-zz?e%dr6&O3T({nIu8NP)`=xdYavpdh99|_Ve(`E z0`unS;Q$u)K2SWzAYkhj78lS^F(WX6l?4(=5071fLp(FNdvAAV-7aw%c!H23mmlv( z4$E62n6D@nSs$i2@GR?m4`m!l{*;iA9e`h{;1P`%{s??e{>@E?L<7VZ!K{*ZRoQ)? z+>nFepY1S<62Yha+}s5yRS(p+m^u*#<~g=1Q)MTGF;{0#+8_ZF$3sClslXnofowz^!VAOZtJI{KrSU*SIY7kxHqW z=Qu#3kQ)ZYpQN?e*R|u|Q`{}YU=Qy=&Fdsa)Q1cph-UqfR_Mb6>4S%5#`UB+@LG-} zT(hYv{WSzuJs zMBfuS)!XjgGwXt!5EK*BqjxOn3C*xRc;Cj>7D~CFsmZ^>U~_8y-cPgJBLg^tm!+R{ ziJioJcC2}b=n3TX_jJZ^Slr%MAU(RK^c{{hjM1GfdKVLNe)&z`S4D410a?_&n&g+_ z)R@?hv$DwS&_5F4XY*X%DmWVZ3xB<9v&)>_h-l4AdQx(Ebaku2poA$BTZ~VH`v~Tk z{<#WGI8Yv{-;I!J^55gG{pIXZJ4*Ut8y)X-?839Kt?&biFH=z1OsMXg`#R?u)^5sk z;uxX7ZMyQs1ekkSnA70n{nBr&BHM4ynQ;1RW0iOy5`ItOB^UaAFa)r?bZaK z%GQa~ciifY^ZMpadb{H=et#C|ycf@C8Dy%h^5#M1UCXHIp>Z_}OUnm30N{o{GeO-W zN}0SiS+9K0uk?Q&zsU=&+^L;?n4muZcB)UM?jVQBPw?*jB|2g^=sgLp+T29NeP1;(xHO&2bs-szc|C-b?{v!86W%HvMx)s8e zS(qaCB-)T|?BS+Sni~v^f)0xDC({q7E{(##lymBdPQBu@=#4?E_;~dhO0ceA{e&@x#sQ_B<2(u^O%aU zZSunbJkeX-BJeC40sX+nkU0-gQZJ3XZj1ZS4R||N)BTu2n0Z~?ajOt&$B~JNbWt&3 zr;#a5Miu6vIqXHF770dQ(xLiedBY6m>tf^QA;e(;T=KhMr$eBp`LWM)d8+|F+aAmc zL)XziAR$jL;cBfSVn6Vb;Mj?qGJupEOB67ZzhK#Oi#bX3p7G}iNjrn(Z$JUm5yaKo z730Tq03b1)t&8AKkYW@SNR#7Bu+()S4Jk>4C0pyefI%yTaRZqnFlNafS#=(J=#NyJ z%^1pao@%6fK)4hwByQfCvH}?wlm39R?wAjtk3Cb{1F_j(AOmo0uoz1D0eR=~u;s5L zk8xJOK`38E*ikM`uvBb!${sXNRO8zp*HB^=Ocu20hEU#C0xb*Bsj1e@G4Ffdc(qR$ zuy=I5p>2vAh4ysw5jMuh_s3EuTQ+OPytv2Swo*tg#d4RvX5Z+@ehpx)a1vOBg2F^? zNQl>qW{a=*o}BFK7+_eZ?iRNB5}-$V{5;#Fishq;yj~JtU#GoKHIx{@IoS^>ZsAn} z@1eNM41c{35|OwKS^(tP9t72iLgHsegzUmkGjRY6oJVCg1-K@j%ckm(Wt=$E+Nfv2 z!}3Gj99!9^nH_&4i+{e?ORg6!VD?op0IR;=IU9O@0}>LuBQ77QyDz=_D2n4#JWJNT zid!rQC>06{jWbLHZ@qs0xU2SjB?Y*olgwgWSc}NW^(%MqgS8wle(+qZ`tETT0$g^! zxF%6Cc*#Y$NxC*qJiyV}|FjRGZAfSr+EZlSRm}Lu_ze<*b5_&-m&k9>wMX-d9Qe0t z=g|EG^zj<`$}&wPZ_P-Y4WZx~0F2f;81Hc;d=ytQbg0s8uZkZ~`-mHQhbIXT;gDha z$aZtPVL8-=T&Gt^0_#RSA-TG~tQY1i*2W{VY&CRWOuy{y{Dr737<%ylW@T*7^_>%o zJSDsXVB-=dQB zyJ)aVs+)X`d#1WefDMVE-c1pWda#?`LhC>@3uzo)F0|EXOF~LaiZ=VbX9%o%e}|~t zqn&jbs)Iov(K>pk#n@#&_yVS$%*lpl`+gnX-O|QrWmRdu;d3*3%?ws4)Yv92X@*Mi z13rtoAwE&Py@yWFH=5r;t{)A&*NuaOhZ@Zkfm?J}160xhTEob6-?qq4A-O%%gEgM% zrqRo8s_-X%yAO`_#>=;E0x25ij*R>H3{i|uB*gbrd zNz)GdD@8(x1(?p{aC2vd{qkqOwx&qlGi)HuMv5`R?@5b^_j45!qcc;R^kB6R^wgkJ z?Y`CuuH$S^=E+DtV_C-E{-0Akp6*|ok{&d8m9dm6zde5Ye%IM}dvGCTO3wy~hyl>( z#hsj&H|)3O^nd9%o{hFN9*z^Z&5Bjey{75lbTg=HELGTfvW=?WY0C!~VFS1vYoGf~ zGx>@L+BniV;RLfTSqYBDE%Qo?;bQ`Wu($W9$3GQ*JZMrKKKvp`i|taRAqW87$^HVU zpA}TDpz*P1XsFNU0Jg_5*`c~%N1y#nPJ@NVV1X@aqUpS#=sV+d&F`yRJW6N}p^It*2I-Ct2&5LwW7ETnH(6<&c$k^H1Ws`W-g z%jRTgNC+4DB()ZP*_{-%Ypx8W!p%OuUqdJjCK6Z4q35aEOOdvruRg7T?=0B1$$eEq z@~ATN(7wSrn}%z$s`pB`AnkO7ZLb0WQ$iZkjETFbR4G@a4~(0QFfnJXULbo23) z%XUur7X>Kgnr{I{8uB?507aOVj zn~z%?e3A3mgxwoiVUYDf*Bt@`%btSKVCUPt4dyB`E~{utahPXO%muvzc6 z*7eJqW|^ktT%ap4B>ajY3N~^pqG`Z@0gn&6#4mPx|9W04yfd}dAJZiNl7%LQyq%o> zn4(=kbWw2wnC45CJulJuoYxBHmXcLXq2d>h)B3*Qa42lDa~3KVd#6Y-zKJKePQOq1 zUa*wfd*Yin536S37FB5vzZC@1%bW)(%Vf*J5tp}MgB7+3EcPo0zI*&};0H=@)&m^bjf^Rp@+MRZucyLHwjZTQ~`Oi`T}Uxs|@4Ma8ZGC4}w^HVg~UoTEx0>HKTdL(r3scW=tZ%z3zA~Jdt24tw2YO<#* zP^65tvBy}v0{!2q4E~j)E(yl-$*=4G?Rne1(G$K;zMigAp*Aeh!(KclfC-I_QMqyq zGv`gB+tKrYuEx*B7WogifhQ9o$*@etzqF z&)z_X3j;r8*EeHt2kkAGDwMxAAsL+UDY(*M_#2bg#*~ID=`E3myBm?wlP-52Ah+vC zQuiiL(X?ImpeFd0bI1s!acPoQE_`Wf*@XPG*bN3|L0vKoNrfF3npM>Cc3r66iT5-4 zSy5rp^8@8sXOkh=HB<&Li=+(|i@k^8vLW!1EXw!4L|b=R2Uz*|BGa!)zTGq<7yM1f zt@l(A&3egFJu{pY0Ga%qSjwI;ym%Pq@^CT>^6b@Wi1T`+fD72^7#d37PM$`S-BLJ3 zdun*FD25NGFsZl_{Pq{)h^x%`WvOK|+41f9F$FY}U9;Uy$HgA-MQ&Z0kYxO_Bkua% z5;Ba@(UTcB31QsvF!)oK?Tu{4>cRcm1J873t2WhRGE~nD2>_s&&;NUI%Ewm*UU7Cl zN1pHMWTar8%WUs5XWC%JplwmfWY*S#UNd4G6lfd5Lx`{ZR@GUm?0l|rMIO5Z{Btg6 zqG?wAV}L+IB$kk&K_`qRCFg2_fhv!2otRi#EOJ2r`?#F@L1eh6VjGHx%&hG2h6};}ZCX|X9nr*gMyL=98VFe(uJRgIlf{O6sBH^AM#|R7D*j{`&@!vz zp9sxzzCf%ayJK`^V)vF+>U3>yLEgs(v03hx@Fhq*17vRd_x(w%n)_ow{1iT;x<{Q{ z_DyYT?1kd7_a+d(EVIGXZT?`2bdSE5cUyVzCLi1(b?rKvfY_I#AcWkS%I8bAYIWgp z2T3&a6lwDvkJGwlpHl(;(9AcvaG`fgsc8(2^m^SCLaZZP_lb7=)P>DQ%;YnNIob#rWiwv9| zI}AA!#UGqo;c;A1;#OE30`MKV^vAdUWBY8xB9|D3Mdq=tb;QkHDlg%NKG}SBzg0Ib zVSL1?m7Md-^;Y)?7HJ8q?$<(50M5T&3K$nCtK{X_LPDkP5bR6};MOi!j7yJs4cX+~ zlHsdcl6NwOhB>w1f+>3M4W=Zk>iY;V!U_6L9f~M;u9`25l$9){j$pJO0yw<+t|6|= z>+9#m>LTMmA`|XB4q@6jz(TB+GUb|_+)Ho6f#4o^iQc@&`Sdw8b z-&3!vQjTWvOt#AJSdo0O8-u_g&PI#@Zo@Mx|9uS^c+gLU+#4a=-;oUN?K6vO`Umq6 zwyr||N~Vyk6_L$bJK~-1x2%|dTW(($eJX&qx*9Q0jsZ3)>w=4~#~*vl3V*wK=3L#( z(gR>F9lM*C>efB1_G7a;=L6=tRm&zM=8|A_h=jyS2#eij)$yR_9ML~B<4g&?p1wlqon6Q{S-puKLK#s zuMP%?FYC@lcaLpH!E{OqF&J4_ebyUP4>5Ww6%6VP7YGQw4*e5HzQyd=FCQR2UGGiA z>t{_NV(~cru=Xo*4$HkMcr!7VJCjvrdDAcTik&%&#z2TYrTm)zg;%Tl2p|B?KiT!s zNy)qtoO|a|bF=B)jyUJn{;TsvW#7EEd>{&LHw5fn&z!?QHD;@?eM^MZDnj(2luO%- z;hHS&=FKdv@V(#R_))S4TUnuB8OR8N@9*z!o_of9c`FJs64GMoRp9<@>M>ZB8bSpP ztIA~0V6OhwdwqT#FxHGIfSlN>S6v=3i>9M(FaxlmOsU4(!f~i@uY|j(tIxJgAgDTabMlBloBqR2aNaHX&6r|z*wh4gU3Xy=$IbS zvuJp!sVQZsA9ip(d=QA`#?*uVfsp3RwACc+-&p+}RPQ>cjq7!oIx>s(`7i@~H=Bkf z_t_b2CDFcMP9GzEV2S^Zd1x`vO6OZl7;PH!ZH??3;OEM8Gl(sl8ffv^gFed~f!{j#fVS=!FU>Rc$#Xx-9>b?5qCqtL)g``PxF>jLQ?Xds_s=(RE&Q*SOyiP zYql!+&A_e%OmT%uhb0*l7tfb9TkU61w_i8e>J>BuXs64VVca0t%p{HL+gQ+~LFuo2 zTy8v{OI48gO~XdqciM>Is5*F0#9jQu*um^Uxa$xhN- z?UglIk{i2tkbD)Vkf!04!Eyb>jwt5$7v4cjLkrvZrD^>4Ntfs^Sw@^4wUsCxg_Fil(v*B!>4O}&RZP(Y;l>)nojh2LBG2*2NcefVY2 zh2)bl+!ZpLBY2rD$1|>rs=t&ysD zge|%!t4De|le3>Y=Fk_kdC~n-R(ZSQ3h&#RDRu7b!vh&B&fAiiew)8%AM#lwx~*G& z^9RG)&eg6nJ+p$1JLxx2pur?;WYk=V6P$#@ehJ{aq;%>Q!EtrClno~92?Z`%JlDN((& z^WBZPGv+2)ESU7~U2^B?o3qj8_}PEPzhBU8kO;T9ap^VRE|V3R*OO^)XSDKGnHd>H zmk)I1hV}q~Mg#f>%L|&zUs*J_aiXEfP_Uq1o6!=uG$k=J{NeUH3CqJJ^D;xNvR7I$ z&CFt7U2QPX$v>o#>Am~KyG_!oEs~$jQ1SiJ_vuDqk@feRDW~5*6LRS+8EPHqk^Bri zikDX>bW&;@J}egPzt{ddJfx~Mmy^s%!tIcD_{!U%BbxFh%2l1WFIu!4TfNouuGrQc z+PyZbwzE~Xb4yRvtW!wL<)D?M@RLa~`sa39iH7L8-U+*}wW-2Q>-_4PH=5o3erc`3 zvITF8E;X*3_hMHbKhZaq&$?E4mYW20EMtYeOLpcbM2dV1?fbp2wBw*3-+;Vk=dr;< zMccS1larE83CdOMuU(UP+3U5+38%bP9OV=c#bMAI|vi?Bt7JaJafWF_0+zUP4O zXNd@7uj)fZV?DWpNY`s0@+;PDT#0v5bj7ONA^!lrRl5Tw#%1h~7@@?v z4C|9&?9S*YT<5%b)BetR)^*>!lG|}vg*#1|h3^_Sn10rlJzWv;IDP!qJ^IC0iki5u zvc~XP_l5P&o;e%4eUk&nhL@SY8PU2^m-3H{W=pq3C%~x?gOJvTuq+Iob=TD7d#Jhm z^582M(Tcu{HNDDJxpm(%_W0Ru-q`q7`HV-~2gB)Nv%_^$^a5{Aj*75;J z-COrbq~-7c-|)(pSN0?ph#Kb%6_}B^muKkE z(9hj20zAj2sIL7hnl8QZTx)|uKs5bIMBBna`_MSPXSx0#d{zM&ru()@Uc+@tfbLFf z?Eak+y9~%64ToJWy~tIT)<~(@Aj~}P`!hz*Mq3AgFhWvD`2>iD7gYb0;LeU#3j!7$_9$jfIfs3>^u!|ce}W!^*$7vaO9L{S0)0>DeL zm7>V*9UIdD6i_r!jMzlD+_saSBPHeM>l@B)vZtp9P*ZfISds7S%A92D#{sJgnkwC#a`dhA!n zMn}yoERH+x+O=Tn;fh1v-iB*{bijTQ<#~{RIO8c$Sdaxfoi%%&$fXK)c600c`Ez5; z531v)FJI`QTeogCB239?Ci#pdiEO(lK+ji5Q%#l)5Br%QDGRx2^C+%ZzVGhdtEjE5 z9hSjCjjgn_6xhu2xbvUlQ)Ny;fdoI|2b5=0tn=~l0lL^kLk?FY&oMxIuT$ie=e%_E zmVUg+v_+|Nc)5k8OzE7}`i}aRHZ;Ceo|)X74;i8crb42@g=G~ScRB74XxJ;Fn$9}n z3ucHE)^&g<2|*rVc6N6BOagO8!)O4PXZU$^boDMSeek~W&)BRpMfnkfkRwMvAb)9@ zU1#X%?0gA`5^mJ9eFqMR2qR~Z{VTOqtLE|p4W0euR#DMhVK8gyTmlHGWDnqD@FU_JgU9v(85 zg?j7;v<3P7K0cCDg}xG!lOy3>w=ceXH=ZT3QV`Kz8mJcYFEofON|WKUmQ zx@=iLVsBWjTUMcU#EB?xxuY!Q!7MQ+Sx>Y=YU=O%9?)!SZ)dfxzXGiTOhfV%3RwMl z@daLl|IWDOQMkzFDLI3led-Au_;~`X_|-xW3dzxrr!V<9nCu)%w%>m zD0|`J_C9|RyBS2fzk(@cIMs6*_SH%zd$RFNdZIPH_ejcChKb2z51%Bp(aO__Iyka*)MwJyiO0pkdqAm^~ zrs6sWDed4XLt=@&Q4jJ-C#m5gmSN-(pAfh$%pm2Aqy{b{vEuW~Jr)qlCaFo%IjrS} zSR`}8l@tU#)%G9U_v@xZX)^Ja+(hy876Qw@LQ~Ra^@WG2yAXN$3N<`TsL4_sMDQ(= zh|+TqnEuo`nzcpaRm)z8DDWIzu5T;)4WAPqC8ykgH$466-Hk0G)a<&tGfQNOk!H4l zlHIptpq3wIV(P7Bxc>rvrKz5EH)_o z5ltvakeiRr$ERVK8XP7dZ^ox4WF{2WJiwjARgWALiG6_~ zavvr*_469KdU|H_0j3Jj2gx&8+5AaKNe8{WLW1Ec=AG0;3Fka$F|PqTIgbpvBcDb+ z09G!E`;=Ap4LdOuP||4Fm%S32k}#BfB%V9*5H!dHUMK^Ou5QX~zL=O8K)EjEqJExg zcjI_I$_=KZq|~cY51U$AE@E%ZN4cMP4yrqxl^QEoN(n|F{2grSfPYz8X<{-MXuoZn zF+~RL7(N(96ppS>Oy++EPO2$D5)_)Wl8DMCMEL=poI2+c1FIIsHy z1LZjMA^BhwLjaKPz#%qxh>U#l_?WbV2z4kd^1ylf4-EhSgw#f?8M<{#x#Iz|)Fr+s}_&;fl56@-`C zFZV1v{~R{x@d*?VfO)}5?qjsQ80T0(;E^LbbQM)q5@L`1IU^tYoV1J#s(nV~0|-?1 z^bRG8@O@rhxAXHwqkjyILTt>qsZHQ`c{8-ScxZ8uw*UMbrw_EkXza6qhIV2%6By{A z6#Fn3g*>4iP7(QLWOVd;-8bY_@UDjf3Zw@ivIF9Wju_!m&n7Hu6C^RXRE z3h+Q)9SsV%2Ke}du?CE0qD-@D6M@gT9(9_bC$sp5vqE19bR&+`(cs1=1pn^$qkgX@ zbqdlQf9u}8_uu*zSTltYT=Lw%|4db@!Ryk%2@*?$)SmWj@7aJ%?waC|&+BPaF8iLn%!RWDs` zAZ*irUxQa+*@>eejtZuV+ycj<6r*`A@fa;SDj8xB!aXBRSdpJ=zL1z@1TBQ`p}WGF zXG3yLCnr{y4d{@DInc+IV)+$-cZ|VEicnK^aRIO@pbIIskBs z1sR6o6o0me|GebuyID~*XcurQ+}+(D#Pgx1reGGE0#@K2Lu?n%n4>ZLOMD+uzK0O{2CuG$<2+nxN*#<4wObnKTfH{wYE}16LK>NrRh~3+h02!Y-aRxpWxX z$tfv-*j;&HR^l8P87Q^99OQgrP_)R<&`_8R@7}#jO@J-rdRyuV2etUYW*}Wd5@sf% z7Neey&de5;nt=&npeGo5ka2F*SxvW9ws&v{-Oo}>FjZx+HPS436H!tU`wZ+QM`@N; z17Qb5GI~6=3$;&7Gpvj4XS18cadPzN(bNP$lmK)I8`+rluWzm1|Qo)-~a#{B~FM#Z%RI z!>92n&=rIqSc)EQ3q2cDM<>qVCmE?g+Sckje0mamdLucGOj9UOYTf1gz<${%8y z;py!yK}LELsX|Z=u$=;o8E7>j4ksRe1_tcd*pX@EammmlU2lME0UC9GeKE?@?mhw! z3QO<6v0awpdiy5N95kV>e_|hDT?H%mwz#INKPbw-Kqt%NHo zuoGLTww9K-I_h@e%zgK*pBfGEC9BpIOB70*GYKlqWWtkmn$VvG?vq>sKrd9Pl9H0B zG)u+N-958tNuw>=k$OTQ;kP(L#4X?Jy9qbR2mNg z$LExx85Sl`x}Fq>Tp{dFf$OayjoFxQlB`lcfvNQb5wwSnK`RJZy4?rPYs z#yg#p(z1~Hkk_Cr9+7FIWcM9u5eP^h|JAWkTaY!-KU@SHbBng!q6-#i#^MSVyx8w4 zB^l_eP;Eu|Uzx)0_CYCu(Ajd(#LELHvh_=ufy2K#fW4C*o7>R!z2SQwG>Lky|8D>p zz+)-)ANm2`u%q78M6JAVr?J~Q^B_ofDTITcd3Qh1sV4wt-US^8yu>;Ju<@?E&8U%_ zX_Nl}mQT;eA~MktTiGqS|1**fVEDt>>^is-L+|3C+JH9=(H@03Y4}52GLRLe=lkOe z`ipq1z)D~s`#Fqk0l(q^+`%H^*=TX=&mkC;{`3 zz13Sa=xS<^(HUCR2}McKHA)7&;jSDP&qVRs95oWQ_yS9 z=`ZH_{63=B!bJ>PL1iF<31ajr{!JJVM1T$XAMymDr^L%KUITpYzP|lRv7KMP@+#n5 zPB3eO_Xw`#Y2?p_X7zSsO+!C|> zyY)=w1UN=Z{PgJ)kjf0w1Uc~w9DajYw#q0#VqtEM66Mp+0DxfO?vvjh|#Htcxf1OY7Y{yiH}V_^C%>!XjQ;ufJY@XJvi#?~{*T5GwC1 z&#ABQN)+BHCc0`d$N9|oGM;e$e2tAa6c;M(IU_Z{VZgiReZWN4J+YdKiN=wMcHP5A z)~*HlDC5V;$?3LtudxQ{SZ{7dEOa=k$Yo|Oa-2mn5g#m@x32w2;);%rMq)J1Ui^6P z-tPAHxaer>En8NtUaefc=Fe|&MChZ)4t5Kt9Xpo0f4IA^+2EBKDj^}aV*%>NFJAl& z%NjeJXd5b_lOG(++nt=!QSSk|q^9QE2M?MN&n5gEmEMvgBC4<$A-RYB{DKRy^^+g$ z7?N^wfNE1#R5UR$sYfap4rG_(2)^&^?pBGz{8pvt99~ep=+Ey#QW@k37a)fY{f2)8 zjGx%%ma<~is-m*8GHe5ynwlpgBl!=IHHq=T+42&l3aza6BjFDidJ7j`32cAA+Z{3G zkT58)@D<9)$UwQ9xv2sf$0}sat<|^kaPs@9q{x-^%f|J*gaUE_*}>*M+@Q#L4^O2 zBftg2lAJ%R1Wz(RWl?FnYX zVFJzD*w`2zU75LaMRrY2Iy9~()Sgp{1r{J#G)7hR1+3Ty4;%sYknVxyu*cC+KGqak zwiv&0K)^VX>67&WRVO=#NS2P6Rs-1qm6}dOL_juhd?>~|{qW(#BPV3NM=aCSG-4i0 zNFY&RaMW*AB@5JS+z8-qabh&`Pcs<8;GhpC7z!%j4w0wkqe_Q^!^D@TB9n8Cm0Bu<<%u$1P&ZMm_)A-xlIV%b%1U_N*VCin zu3ln$OaIpn$vJaYtXMHLG9stzuMPRe)Wh!!_k(x<%n;TG#2}~_MGs+k{p;!q@J4HE zYtS<~HikMeIQ_6Zzpi^-S67gihb+t3r_R`mai#!L5=S#sbfgWlK232addeO>deqdE zYMS8+vr$E*80RWVULiFrC@2U!3XUW+n3mQYXC5#2tgK95S={5}y=AzUlb4sDH?O6$ zHVf*MRaPxReub%KuT*&7BCuf`gq8&WNXYTx*k>s2g)}^*Md6fiM^*z4A2_TaE596G z;XqSYA8x-{RCL(Zw*Jh#b-8(Y*eamO3C=VzNa%oB*Vxw*NOmDSp{ z#|_fL!o$xzb;h_IWUtMgGiT4g|3-`-AS1Z2;xR#Sadm+Fi+{H4kcq#5pKy0~fByWry_Cic>_2qU)pX&;9B%iD|wlKPBus!I10M5ORXaE2J literal 0 HcmV?d00001 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 new file mode 100644 index 0000000000..eb2026a429 --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp @@ -0,0 +1,251 @@ +// Copyright (c) 2020 Shrijit Singh +// Copyright (c) 2020 Samsung Research America +// +// 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_REGULATED_PURE_PURSUIT_CONTROLLER__PURE_PURSUIT_CONTROLLER_HPP_ +#define NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__PURE_PURSUIT_CONTROLLER_HPP_ + +#include +#include +#include +#include + +#include "nav2_core/controller.hpp" +#include "rclcpp/rclcpp.hpp" +#include "pluginlib/class_loader.hpp" +#include "pluginlib/class_list_macros.hpp" +#include "nav2_util/odometry_utils.hpp" +#include "geometry_msgs/msg/pose2_d.hpp" + +namespace nav2_regulated_pure_pursuit_controller +{ + +/** + * @class nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController + * @brief Regulated pure pursuit controller plugin + */ +class RegulatedPurePursuitController : public nav2_core::Controller +{ +public: + /** + * @brief Constructor for nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController + */ + RegulatedPurePursuitController() = default; + + /** + * @brief Destrructor for nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController + */ + ~RegulatedPurePursuitController() override = default; + + /** + * @brief Configure controller state machine + * @param parent WeakPtr to node + * @param name Name of plugin + * @param tf TF buffer + * @param costmap_ros Costmap2DROS object of environment + */ + void configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr & parent, + std::string name, const std::shared_ptr & tf, + const std::shared_ptr & costmap_ros) override; + + /** + * @brief Cleanup controller state machine + */ + void cleanup() override; + + /** + * @brief Activate controller state machine + */ + void activate() override; + + /** + * @brief Deactivate controller state machine + */ + void deactivate() override; + + /** + * @brief Compute the best command given the current pose and velocity, with possible debug information + * + * Same as above computeVelocityCommands, but with debug results. + * If the results pointer is not null, additional information about the twists + * evaluated will be in results after the call. + * + * @param pose Current robot pose + * @param velocity Current robot velocity + * @param results Output param, if not NULL, will be filled in with full evaluation results + * @return Best command + */ + geometry_msgs::msg::TwistStamped computeVelocityCommands( + const geometry_msgs::msg::PoseStamped & pose, + const geometry_msgs::msg::Twist & velocity) override; + + /** + * @brief nav2_core setPlan - Sets the global plan + * @param path The global plan + */ + void setPlan(const nav_msgs::msg::Path & path) override; + +protected: + /** + * @brief Transforms global plan into same frame as pose, clips far away poses and possibly prunes passed poses + * @param pose pose to transform + * @return Path in new frame + */ + nav_msgs::msg::Path transformGlobalPlan(const geometry_msgs::msg::PoseStamped & pose); + + /** + * @brief Transform a pose to another frame. + * @param frame Frame ID to transform to + * @param in_pose Pose input to transform + * @param out_pose transformed output + * @return bool if successful + */ + bool transformPose( + const std::string frame, + const geometry_msgs::msg::PoseStamped & in_pose, + geometry_msgs::msg::PoseStamped & out_pose) const; + + /** + * @brief Get lookahead distance + * @param cmd the current speed to use to compute lookahead point + * @return lookahead distance + */ + double getLookAheadDistance(const geometry_msgs::msg::Twist &); + + /** + * @brief Creates a PointStamped message for visualization + * @param carrot_pose Input carrot point as a PoseStamped + * @return CarrotMsg a carrot point marker, PointStamped + */ + std::unique_ptr createCarrotMsg( + const geometry_msgs::msg::PoseStamped & carrot_pose); + + /** + * @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 + * @return Whether should rotate to path heading + */ + bool shouldRotateToPath( + const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path); + + /** + * @brief Whether robot should rotate to final goal orientation + * @param carrot_pose current lookahead point + * @return Whether should rotate to goal heading + */ + bool shouldRotateToGoalHeading(const geometry_msgs::msg::PoseStamped & carrot_pose); + + /** + * @brief Create a smooth and kinematically smoothed rotation command + * @param linear_vel linear velocity + * @param angular_vel angular velocity + * @param angle_to_path Angle of robot output relatie to carrot marker + * @param curr_speed the current robot speed + */ + void rotateToHeading(double & linear_vel, double & angular_vel, + const double & angle_to_path, const geometry_msgs::msg::Twist & curr_speed); + + /** + * @brief Whether collision is imminent + * @param robot_pose Pose of robot + * @param carrot_pose Pose of carrot + * @param linear_vel linear velocity to forward project + * @param angular_vel angular velocity to forward project + * @return Whether collision is imminent + */ + bool isCollisionImminent( + const geometry_msgs::msg::PoseStamped &, + const double &, const double &); + + /** + * @brief Whether point is in collision + * @param x Pose of pose x + * @param y Pose of pose y + * @return Whether in collision + */ + bool inCollision(const double & x, const double & y); + + /** + * @brief Cost at a point + * @param x Pose of pose x + * @param y Pose of pose y + * @return Cost of pose in costmap + */ + double costAtPose(const double & x, const double & y); + + /** + * @brief apply regulation constraints to the system + * @param linear_vel robot command linear velocity input + * @param dist_error error in the carrot distance and lookahead distance + * @param lookahead_dist optimal lookahead distance + * @param curvature curvature of path + * @param speed Speed of robot + * @param pose_cost cost at this pose + */ + void applyConstraints( + const double & dist_error, const double & lookahead_dist, + const double & curvature, const geometry_msgs::msg::Twist & speed, + const double & pose_cost, double & linear_vel); + + /** + * @brief Get lookahead point + * @param lookahead_dist Optimal lookahead distance + * @param path Current global path + * @return Lookahead point + */ + geometry_msgs::msg::PoseStamped getLookAheadPoint(const double &, const nav_msgs::msg::Path &); + + std::shared_ptr tf_; + std::string plugin_name_; + std::shared_ptr costmap_ros_; + nav2_costmap_2d::Costmap2D * costmap_; + rclcpp::Logger logger_ {rclcpp::get_logger("RegulatedPurePursuitController")}; + + double desired_linear_vel_; + double lookahead_dist_; + double rotate_to_heading_angular_vel_; + double max_lookahead_dist_; + double min_lookahead_dist_; + double lookahead_time_; + double max_linear_accel_; + double max_linear_decel_; + bool use_velocity_scaled_lookahead_dist_; + tf2::Duration transform_tolerance_; + bool use_approach_vel_scaling_; + double min_approach_linear_velocity_; + double control_duration_; + double max_allowed_time_to_collision_; + bool use_regulated_linear_velocity_scaling_; + bool use_cost_regulated_linear_velocity_scaling_; + double cost_scaling_dist_; + double cost_scaling_gain_; + double inflation_cost_scaling_factor_; + double regulated_linear_scaling_min_radius_; + double regulated_linear_scaling_min_speed_; + bool use_rotate_to_heading_; + double max_angular_accel_; + double rotate_to_heading_min_angle_; + double goal_dist_tol_; + + nav_msgs::msg::Path global_plan_; + std::shared_ptr> global_path_pub_; + std::shared_ptr> carrot_pub_; + std::shared_ptr> carrot_arc_pub_; +}; + +} // namespace nav2_regulated_pure_pursuit_controller + +#endif // NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__PURE_PURSUIT_CONTROLLER_HPP_ diff --git a/nav2_regulated_pure_pursuit_controller/nav2_regulated_pure_pursuit_controller.xml b/nav2_regulated_pure_pursuit_controller/nav2_regulated_pure_pursuit_controller.xml new file mode 100644 index 0000000000..d695bcec7f --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/nav2_regulated_pure_pursuit_controller.xml @@ -0,0 +1,10 @@ + + + + + nav2_regulated_pure_pursuit_controller + + + + + diff --git a/nav2_regulated_pure_pursuit_controller/package.xml b/nav2_regulated_pure_pursuit_controller/package.xml new file mode 100644 index 0000000000..39a894477a --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/package.xml @@ -0,0 +1,29 @@ + + + + nav2_regulated_pure_pursuit_controller + 1.0.0 + Regulated Pure Pursuit Controller + Steve Macenski + Shrijit Singh + Apache-2.0 + + ament_cmake + + nav2_common + nav2_core + nav2_util + nav2_costmap_2d + rclcpp + geometry_msgs + nav2_msgs + pluginlib + tf2 + + ament_cmake_gtest + + + ament_cmake + + + 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 new file mode 100644 index 0000000000..f5017ca058 --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -0,0 +1,563 @@ +// Copyright (c) 2020 Shrijit Singh +// Copyright (c) 2020 Samsung Research America +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +#include "nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp" +#include "nav2_core/exceptions.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_util/geometry_utils.hpp" + +using std::hypot; +using std::min; +using std::max; +using std::abs; +using nav2_util::declare_parameter_if_not_declared; +using nav2_util::geometry_utils::euclidean_distance; +using namespace nav2_costmap_2d; // NOLINT + +namespace nav2_regulated_pure_pursuit_controller +{ + +/** + * Find element in iterator with the minimum calculated value + */ +template +Iter min_by(Iter begin, Iter end, Getter getCompareVal) +{ + if (begin == end) { + return end; + } + auto lowest = getCompareVal(*begin); + Iter lowest_it = begin; + for (Iter it = ++begin; it != end; ++it) { + auto comp = getCompareVal(*it); + if (comp < lowest) { + lowest = comp; + lowest_it = it; + } + } + return lowest_it; +} + +void RegulatedPurePursuitController::configure( + const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, + std::string name, const std::shared_ptr & tf, + const std::shared_ptr & costmap_ros) +{ + costmap_ros_ = costmap_ros; + costmap_ = costmap_ros_->getCostmap(); + tf_ = tf; + plugin_name_ = name; + logger_ = node->get_logger(); + + double transform_tolerance = 0.1; + double control_frequency = 20.0; + + declare_parameter_if_not_declared( + node, plugin_name_ + ".desired_linear_vel", rclcpp::ParameterValue(0.5)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_linear_accel", rclcpp::ParameterValue(2.5)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_linear_decel", rclcpp::ParameterValue(2.5)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".lookahead_dist", rclcpp::ParameterValue(0.6)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".min_lookahead_dist", rclcpp::ParameterValue(0.3)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_lookahead_dist", rclcpp::ParameterValue(0.9)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".lookahead_time", rclcpp::ParameterValue(1.5)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".rotate_to_heading_angular_vel", rclcpp::ParameterValue(1.8)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".transform_tolerance", rclcpp::ParameterValue(0.1)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_velocity_scaled_lookahead_dist", + rclcpp::ParameterValue(false)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".min_approach_linear_velocity", rclcpp::ParameterValue(0.05)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_approach_linear_velocity_scaling", rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_allowed_time_to_collision", rclcpp::ParameterValue(1.0)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_regulated_linear_velocity_scaling", rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_cost_regulated_linear_velocity_scaling", rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".cost_scaling_dist", rclcpp::ParameterValue(0.6)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".cost_scaling_gain", rclcpp::ParameterValue(1.0)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".inflation_cost_scaling_factor", rclcpp::ParameterValue(3.0)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".regulated_linear_scaling_min_radius", rclcpp::ParameterValue(0.90)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".regulated_linear_scaling_min_speed", rclcpp::ParameterValue(0.25)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_rotate_to_heading", rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".rotate_to_heading_min_angle", rclcpp::ParameterValue(0.785)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_angular_accel", rclcpp::ParameterValue(3.2)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".goal_dist_tol", rclcpp::ParameterValue(0.25)); + + node->get_parameter(plugin_name_ + ".desired_linear_vel", desired_linear_vel_); + node->get_parameter(plugin_name_ + ".max_linear_accel", max_linear_accel_); + node->get_parameter(plugin_name_ + ".max_linear_decel", max_linear_decel_); + node->get_parameter(plugin_name_ + ".lookahead_dist", lookahead_dist_); + node->get_parameter(plugin_name_ + ".min_lookahead_dist", min_lookahead_dist_); + node->get_parameter(plugin_name_ + ".max_lookahead_dist", max_lookahead_dist_); + node->get_parameter(plugin_name_ + ".lookahead_time", lookahead_time_); + node->get_parameter(plugin_name_ + ".rotate_to_heading_angular_vel", rotate_to_heading_angular_vel_); + node->get_parameter(plugin_name_ + ".transform_tolerance", transform_tolerance); + node->get_parameter(plugin_name_ + ".use_velocity_scaled_lookahead_dist", + use_velocity_scaled_lookahead_dist_); + node->get_parameter(plugin_name_ + ".min_approach_linear_velocity", min_approach_linear_velocity_); + node->get_parameter(plugin_name_ + ".use_approach_linear_velocity_scaling", use_approach_vel_scaling_); + node->get_parameter(plugin_name_ + ".max_allowed_time_to_collision", max_allowed_time_to_collision_); + node->get_parameter(plugin_name_ + ".use_regulated_linear_velocity_scaling", use_regulated_linear_velocity_scaling_); + node->get_parameter(plugin_name_ + ".use_cost_regulated_linear_velocity_scaling", use_cost_regulated_linear_velocity_scaling_); + node->get_parameter(plugin_name_ + ".cost_scaling_dist", cost_scaling_dist_); + node->get_parameter(plugin_name_ + ".cost_scaling_gain", cost_scaling_gain_); + node->get_parameter(plugin_name_ + ".inflation_cost_scaling_factor", inflation_cost_scaling_factor_); + node->get_parameter(plugin_name_ + ".regulated_linear_scaling_min_radius", regulated_linear_scaling_min_radius_); + node->get_parameter(plugin_name_ + ".regulated_linear_scaling_min_speed", regulated_linear_scaling_min_speed_); + node->get_parameter(plugin_name_ + ".use_rotate_to_heading", use_rotate_to_heading_); + node->get_parameter(plugin_name_ + ".rotate_to_heading_min_angle", rotate_to_heading_min_angle_); + node->get_parameter(plugin_name_ + ".max_angular_accel", max_angular_accel_); + node->get_parameter(plugin_name_ + ".goal_dist_tol", goal_dist_tol_); + node->get_parameter("controller_frequency", control_frequency); + + transform_tolerance_ = tf2::durationFromSec(transform_tolerance); + control_duration_ = 1.0 / control_frequency; + + if (inflation_cost_scaling_factor_ <= 0.0) { + RCLCPP_WARN(logger_, "The value inflation_cost_scaling_factor is incorrectly set, " + "it should be >0. Disabling cost regulated linear velocity scaling."); + use_cost_regulated_linear_velocity_scaling_ = false; + } + + global_path_pub_ = node->create_publisher("received_global_plan", 1); + carrot_pub_ = node->create_publisher("lookahead_point", 1); + carrot_arc_pub_ = node->create_publisher("lookahead_collision_arc", 1); +} + +void RegulatedPurePursuitController::cleanup() +{ + RCLCPP_INFO( + logger_, + "Cleaning up controller: %s of type" + " regulated_pure_pursuit_controller::RegulatedPurePursuitController", + plugin_name_.c_str()); + global_path_pub_.reset(); + carrot_pub_.reset(); + carrot_arc_pub_.reset(); +} + +void RegulatedPurePursuitController::activate() +{ + RCLCPP_INFO( + logger_, + "Activating controller: %s of type " + "regulated_pure_pursuit_controller::RegulatedPurePursuitController", + plugin_name_.c_str()); + global_path_pub_->on_activate(); + carrot_pub_->on_activate(); + carrot_arc_pub_->on_activate(); +} + +void RegulatedPurePursuitController::deactivate() +{ + RCLCPP_INFO( + logger_, + "Deactivating controller: %s of type " + "regulated_pure_pursuit_controller::RegulatedPurePursuitController", + plugin_name_.c_str()); + global_path_pub_->on_deactivate(); + carrot_pub_->on_deactivate(); + carrot_arc_pub_->on_deactivate(); +} + +std::unique_ptr RegulatedPurePursuitController::createCarrotMsg( + const geometry_msgs::msg::PoseStamped & carrot_pose) +{ + auto carrot_msg = std::make_unique(); + carrot_msg->header = carrot_pose.header; + carrot_msg->point.x = carrot_pose.pose.position.x; + carrot_msg->point.y = carrot_pose.pose.position.y; + carrot_msg->point.z = 0.01; // publish right over map to stand out + return carrot_msg; +} + +double RegulatedPurePursuitController::getLookAheadDistance(const geometry_msgs::msg::Twist & speed) +{ + // If using velocity-scaled look ahead distances, find and clamp the dist + // Else, use the static look ahead distance + double lookahead_dist = lookahead_dist_; + if (use_velocity_scaled_lookahead_dist_) { + lookahead_dist = speed.linear.x * lookahead_time_; + lookahead_dist = std::clamp(lookahead_dist, min_lookahead_dist_, max_lookahead_dist_); + } + + return lookahead_dist; +} + +geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocityCommands( + const geometry_msgs::msg::PoseStamped & pose, + const geometry_msgs::msg::Twist & speed) +{ + // Transform path to robot base frame + auto transformed_plan = transformGlobalPlan(pose); + + // Find look ahead distance and point on path and publish + const double lookahead_dist = getLookAheadDistance(speed); + auto carrot_pose = getLookAheadPoint(lookahead_dist, transformed_plan); + carrot_pub_->publish(std::move(createCarrotMsg(carrot_pose))); + + double linear_vel, angular_vel; + + // Find distance^2 to look ahead point (carrot) in robot base frame + // This is the chord length of the circle + const double carrot_dist2 = + (carrot_pose.pose.position.x * carrot_pose.pose.position.x) + + (carrot_pose.pose.position.y * carrot_pose.pose.position.y); + + // Find curvature of circle (k = 1 / R) + double curvature = 0.0; + if (carrot_dist2 > 0.001) { + curvature = 2.0 * carrot_pose.pose.position.y / carrot_dist2; + } + + linear_vel = desired_linear_vel_; + + // Make sure we're in compliance with basic constraints + 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)) { + rotateToHeading(linear_vel, angular_vel, angle_to_heading, speed); + } else { + applyConstraints( + fabs(lookahead_dist - sqrt(carrot_dist2)), + lookahead_dist, curvature, speed, + costAtPose(pose.pose.position.x, pose.pose.position.y), linear_vel); + + // Apply curvature to angular velocity after constraining linear velocity + angular_vel = linear_vel * curvature; + } + + // Collision checking on this velocity heading + if (isCollisionImminent(pose, linear_vel, angular_vel)) { + throw std::runtime_error("RegulatedPurePursuitController detected collision ahead!"); + } + + // populate and return message + geometry_msgs::msg::TwistStamped cmd_vel; + cmd_vel.header = pose.header; + cmd_vel.twist.linear.x = linear_vel; + cmd_vel.twist.angular.z = angular_vel; + return cmd_vel; +} + +bool RegulatedPurePursuitController::shouldRotateToPath( + const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path) +{ + // Whether we should rotate robot to rough path heading + angle_to_path = atan2(carrot_pose.pose.position.y, carrot_pose.pose.position.x); + return use_rotate_to_heading_ && fabs(angle_to_path) > rotate_to_heading_min_angle_; +} + +bool RegulatedPurePursuitController::shouldRotateToGoalHeading( + const geometry_msgs::msg::PoseStamped & carrot_pose) +{ + // Whether we should rotate robot to goal heading + double dist_to_goal = std::hypot(carrot_pose.pose.position.x, carrot_pose.pose.position.y); + return use_rotate_to_heading_ && dist_to_goal < goal_dist_tol_; +} + +void RegulatedPurePursuitController::rotateToHeading( + double & linear_vel, double & angular_vel, + const double & angle_to_path, const geometry_msgs::msg::Twist & curr_speed) +{ + // Rotate in place using max angular velocity / acceleration possible + linear_vel = 0.0; + const double sign = angle_to_path > 0.0 ? 1.0 : -1.0; + angular_vel = sign * rotate_to_heading_angular_vel_; + + const double & dt = control_duration_; + const double min_feasible_angular_speed = curr_speed.angular.z - max_angular_accel_ * dt; + const double max_feasible_angular_speed = curr_speed.angular.z + max_angular_accel_ * dt; + angular_vel = std::clamp(angular_vel, min_feasible_angular_speed, max_feasible_angular_speed); +} + +geometry_msgs::msg::PoseStamped RegulatedPurePursuitController::getLookAheadPoint( + const double & lookahead_dist, + const nav_msgs::msg::Path & transformed_plan) +{ + // Find the first pose which is at a distance greater than the lookahead distance + auto goal_pose_it = std::find_if( + transformed_plan.poses.begin(), transformed_plan.poses.end(), [&](const auto & ps) { + return hypot(ps.pose.position.x, ps.pose.position.y) >= lookahead_dist; + }); + + // 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()); + } + + return *goal_pose_it; +} + +bool RegulatedPurePursuitController::isCollisionImminent( + const geometry_msgs::msg::PoseStamped & robot_pose, + const double & linear_vel, const double & angular_vel) +{ + // Note(stevemacenski): This may be a bit unusual, but the robot_pose is in + // odom frame and the carrot_pose is in robot base frame. + + // check current point is OK + if (inCollision(robot_pose.pose.position.x, robot_pose.pose.position.y)) { + return true; + } + + // visualization messages + nav_msgs::msg::Path arc_pts_msg; + arc_pts_msg.header.frame_id = costmap_ros_->getGlobalFrameID(); + arc_pts_msg.header.stamp = robot_pose.header.stamp; + geometry_msgs::msg::PoseStamped pose_msg; + pose_msg.header.frame_id = arc_pts_msg.header.frame_id; + pose_msg.header.stamp = arc_pts_msg.header.stamp; + + const double projection_time = costmap_->getResolution() / linear_vel; + + geometry_msgs::msg::Pose2D curr_pose; + curr_pose.x = robot_pose.pose.position.x; + curr_pose.y = robot_pose.pose.position.y; + curr_pose.theta = tf2::getYaw(robot_pose.pose.orientation); + + int i = 1; + while (true) { + // only forward simulate within time requested + if (i * projection_time > max_allowed_time_to_collision_) { + break; + } + + i++; + + // apply velocity at curr_pose over distance + curr_pose.x += projection_time * (linear_vel * cos(curr_pose.theta)); + curr_pose.y += projection_time * (linear_vel * sin(curr_pose.theta)); + curr_pose.theta += projection_time * angular_vel; + + // store it for visualization + pose_msg.pose.position.x = curr_pose.x; + pose_msg.pose.position.y = curr_pose.y; + pose_msg.pose.position.z = 0.01; + arc_pts_msg.poses.push_back(pose_msg); + + // check for collision at this point + if (inCollision(curr_pose.x, curr_pose.y)) { + carrot_arc_pub_->publish(arc_pts_msg); + return true; + } + } + + carrot_arc_pub_->publish(arc_pts_msg); + + return false; +} + +bool RegulatedPurePursuitController::inCollision(const double & x, const double & y) +{ + unsigned int mx, my; + costmap_->worldToMap(x, y, mx, my); + + unsigned char cost = costmap_->getCost(mx, my); + + if (costmap_ros_->getLayeredCostmap()->isTrackingUnknown()) { + return cost >= INSCRIBED_INFLATED_OBSTACLE && cost != NO_INFORMATION; + } else { + return cost >= INSCRIBED_INFLATED_OBSTACLE; + } +} + +double RegulatedPurePursuitController::costAtPose(const double & x, const double & y) +{ + unsigned int mx, my; + costmap_->worldToMap(x, y, mx, my); + + unsigned char cost = costmap_->getCost(mx, my); + return static_cast(cost); +} + +void RegulatedPurePursuitController::applyConstraints( + const double & dist_error, const double & lookahead_dist, + const double & curvature, const geometry_msgs::msg::Twist & curr_speed, + const double & pose_cost, double & linear_vel) +{ + double curvature_vel = linear_vel; + double cost_vel = linear_vel; + double approach_vel = linear_vel; + + // limit the linear velocity by curvature + const double radius = fabs(1.0 / curvature); + const double & min_rad = regulated_linear_scaling_min_radius_; + if (use_regulated_linear_velocity_scaling_ && radius < min_rad) { + curvature_vel *= 1.0 - (fabs(radius - min_rad) / min_rad); + } + + // limit the linear velocity by proximity to obstacles + if (use_cost_regulated_linear_velocity_scaling_ && + pose_cost != static_cast(NO_INFORMATION) && + pose_cost != static_cast(FREE_SPACE)) + { + const double inscribed_radius = costmap_ros_->getLayeredCostmap()->getInscribedRadius(); + const double min_distance_to_obstacle = (-1.0 / inflation_cost_scaling_factor_) * + std::log(pose_cost / (INSCRIBED_INFLATED_OBSTACLE - 1)) + inscribed_radius; + + if (min_distance_to_obstacle < cost_scaling_dist_) { + cost_vel *= cost_scaling_gain_ * min_distance_to_obstacle / cost_scaling_dist_; + } + } + + // Use the lowest of the 2 constraint heuristics, but above the minimum translational speed + linear_vel = std::min(cost_vel, curvature_vel); + linear_vel = std::max(linear_vel, regulated_linear_scaling_min_speed_); + + // if the actual lookahead distance is shorter than requested, that means we're at the + // end of the path. We'll scale linear velocity by error to slow to a smooth stop + if (use_approach_vel_scaling_ && dist_error > 2.0 * costmap_->getResolution()) { + double velocity_scaling = 1.0 - (dist_error / lookahead_dist); + double unbounded_vel = approach_vel * velocity_scaling; + if (unbounded_vel < min_approach_linear_velocity_) { + approach_vel = min_approach_linear_velocity_; + } else { + approach_vel *= velocity_scaling; + } + + // Use the lowest velocity between approach and other constraints, if all overlapping + linear_vel = std::min(linear_vel, approach_vel); + } + + // Limit linear velocities to be valid and kinematically feasible, v = v0 + a * dt + double & dt = control_duration_; + const double max_feasible_linear_speed = curr_speed.linear.x + max_linear_accel_ * dt; + const double min_feasible_linear_speed = curr_speed.linear.x - max_linear_decel_ * dt; + linear_vel = std::clamp(linear_vel, min_feasible_linear_speed, max_feasible_linear_speed); + linear_vel = std::clamp(linear_vel, 0.0, desired_linear_vel_); +} + +void RegulatedPurePursuitController::setPlan(const nav_msgs::msg::Path & path) +{ + global_plan_ = path; +} + +nav_msgs::msg::Path RegulatedPurePursuitController::transformGlobalPlan( + const geometry_msgs::msg::PoseStamped & pose) +{ + if (global_plan_.poses.empty()) { + throw nav2_core::PlannerException("Received plan with zero length"); + } + + // 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)) + { + throw nav2_core::PlannerException("Unable to transform robot pose into global plan's frame"); + } + + // We'll discard points on the plan that are outside the local costmap + nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap(); + const double max_costmap_dim = std::max(costmap->getSizeInCellsX(), costmap->getSizeInCellsY()); + const double max_transform_dist = max_costmap_dim * costmap->getResolution() / 2.0; + + // First find the closest pose on the path to the robot + auto transformation_begin = + min_by( + global_plan_.poses.begin(), global_plan_.poses.end(), + [&robot_pose](const geometry_msgs::msg::PoseStamped & ps) { + return euclidean_distance(robot_pose, ps); + }); + + // Find points definitely outside of the costmap so we won't transform them. + auto transformation_end = std::find_if( + transformation_begin, end(global_plan_.poses), + [&](const auto & global_plan_pose) { + return euclidean_distance(robot_pose, global_plan_pose) > max_transform_dist; + }); + + // Lambda to transform a PoseStamped from global frame to local + auto transformGlobalPoseToLocal = [&](const auto & global_plan_pose) { + geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose; + stamped_pose.header.frame_id = global_plan_.header.frame_id; + stamped_pose.header.stamp = robot_pose.header.stamp; + stamped_pose.pose = global_plan_pose.pose; + transformPose(costmap_ros_->getBaseFrameID(), stamped_pose, transformed_pose); + return transformed_pose; + }; + + // Transform the near part of the global plan into the robot's frame of reference. + nav_msgs::msg::Path transformed_plan; + std::transform( + transformation_begin, transformation_end, + std::back_inserter(transformed_plan.poses), + transformGlobalPoseToLocal); + transformed_plan.header.frame_id = costmap_ros_->getBaseFrameID(); + transformed_plan.header.stamp = robot_pose.header.stamp; + + // Remove the portion of the global plan that we've already passed so we don't + // process it on the next iteration (this is called path pruning) + global_plan_.poses.erase(begin(global_plan_.poses), transformation_begin); + global_path_pub_->publish(transformed_plan); + + if (transformed_plan.poses.empty()) { + throw nav2_core::PlannerException("Resulting plan has 0 poses in it."); + } + + return transformed_plan; +} + +bool RegulatedPurePursuitController::transformPose( + const std::string frame, + const geometry_msgs::msg::PoseStamped & in_pose, + geometry_msgs::msg::PoseStamped & out_pose) const +{ + if (in_pose.header.frame_id == frame) { + out_pose = in_pose; + return true; + } + + try { + tf_->transform(in_pose, out_pose, frame, transform_tolerance_); + out_pose.header.frame_id = frame; + return true; + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR(logger_, "Exception in transformPose: %s", ex.what()); + } + return false; +} + +} // namespace nav2_pure_pursuit_controller + +// Register this controller as a nav2_core plugin +PLUGINLIB_EXPORT_CLASS( + nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController, + nav2_core::Controller) diff --git a/nav2_regulated_pure_pursuit_controller/test/CMakeLists.txt b/nav2_regulated_pure_pursuit_controller/test/CMakeLists.txt new file mode 100644 index 0000000000..aee6297fcb --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/test/CMakeLists.txt @@ -0,0 +1,10 @@ +# tests for regulated PP +ament_add_gtest(test_regulated_pp + test_regulated_pp.cpp +) +ament_target_dependencies(test_regulated_pp + ${dependencies} +) +target_link_libraries(test_regulated_pp + ${library_name} +) diff --git a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp new file mode 100644 index 0000000000..9389b43bff --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -0,0 +1,324 @@ +// Copyright (c) 2021 Samsung Research America +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController +{ +public: + BasicAPIRPP() : nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController() {}; + + nav_msgs::msg::Path getPlan() {return global_plan_;}; + + double getSpeed() {return desired_linear_vel_;}; + + std::unique_ptr createCarrotMsgWrapper( + const geometry_msgs::msg::PoseStamped & carrot_pose) + { + return createCarrotMsg(carrot_pose); + }; + + void setVelocityScaledLookAhead() {use_velocity_scaled_lookahead_dist_ = true;}; + void setCostRegulationScaling() {use_cost_regulated_linear_velocity_scaling_ = true;}; + void resetVelocityRegulationScaling() {use_regulated_linear_velocity_scaling_ = false;}; + void resetVelocityApproachScaling() {use_approach_vel_scaling_ = false;}; + + double getLookAheadDistanceWrapper(const geometry_msgs::msg::Twist & twist) + { + return getLookAheadDistance(twist); + }; + + geometry_msgs::msg::PoseStamped getLookAheadPointWrapper( + const double & dist, const nav_msgs::msg::Path & path) + { + return getLookAheadPoint(dist, path); + }; + + bool shouldRotateToPathWrapper( + const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path) + { + return shouldRotateToPath(carrot_pose, angle_to_path); + } + + bool shouldRotateToGoalHeadingWrapper(const geometry_msgs::msg::PoseStamped & carrot_pose) + { + return shouldRotateToGoalHeading(carrot_pose); + } + + void rotateToHeadingWrapper(double & linear_vel, double & angular_vel, + const double & angle_to_path, const geometry_msgs::msg::Twist & curr_speed) + { + return rotateToHeading(linear_vel, angular_vel, angle_to_path, curr_speed); + } + + void applyConstraintsWrapper( + const double & dist_error, const double & lookahead_dist, + const double & curvature, const geometry_msgs::msg::Twist & curr_speed, + const double & pose_cost, double & linear_vel) + { + return applyConstraints(dist_error, lookahead_dist, curvature, curr_speed, pose_cost, linear_vel); + } + +}; + +TEST(RegulatedPurePursuitTest, basicAPI) +{ + 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"); + + // instantiate + auto ctrl = std::make_shared(); + ctrl->configure(node, name, tf, costmap); + ctrl->activate(); + ctrl->deactivate(); + ctrl->cleanup(); + + // setPlan and get plan + nav_msgs::msg::Path path; + path.poses.resize(2); + path.poses[0].header.frame_id = "fake_frame"; + ctrl->setPlan(path); + EXPECT_EQ(ctrl->getPlan().poses.size(), 2ul); + EXPECT_EQ(ctrl->getPlan().poses[0].header.frame_id, std::string("fake_frame")); +} + +TEST(RegulatedPurePursuitTest, createCarrotMsg) +{ + auto ctrl = std::make_shared(); + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = "Hi!"; + pose.pose.position.x = 1.0; + pose.pose.position.y = 12.0; + pose.pose.orientation.w = 0.5; + + auto rtn = ctrl->createCarrotMsgWrapper(pose); + EXPECT_EQ(rtn->header.frame_id, std::string("Hi!")); + EXPECT_EQ(rtn->point.x, 1.0); + EXPECT_EQ(rtn->point.y, 12.0); + EXPECT_EQ(rtn->point.z, 0.01); +} + +TEST(RegulatedPurePursuitTest, lookaheadAPI) +{ + 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"); + ctrl->configure(node, name, tf, costmap); + + geometry_msgs::msg::Twist twist; + + // test getLookAheadDistance + double rtn = ctrl->getLookAheadDistanceWrapper(twist); + EXPECT_EQ(rtn, 0.6); // default lookahead_dist + + // shouldn't be a function of speed + twist.linear.x = 10.0; + rtn = ctrl->getLookAheadDistanceWrapper(twist); + EXPECT_EQ(rtn, 0.6); + + // now it should be a function of velocity, max out + ctrl->setVelocityScaledLookAhead(); + rtn = ctrl->getLookAheadDistanceWrapper(twist); + EXPECT_EQ(rtn, 0.9); // 10 speed maxes out at max_lookahead_dist + + // check normal range + twist.linear.x = 0.35; + rtn = ctrl->getLookAheadDistanceWrapper(twist); + EXPECT_NEAR(rtn, 0.525, 0.0001); // 1.5 * 0.35 + + // check minimum range + twist.linear.x = 0.0; + rtn = ctrl->getLookAheadDistanceWrapper(twist); + EXPECT_EQ(rtn, 0.3); + + // test getLookAheadPoint + double dist = 1.0; + nav_msgs::msg::Path path; + path.poses.resize(10); + for (uint i = 0; i != path.poses.size(); i++) { + path.poses[i].pose.position.x = static_cast(i); + } + + // test exact hits + auto pt = ctrl->getLookAheadPointWrapper(dist, path); + EXPECT_EQ(pt.pose.position.x, 1.0); + + // test getting next closest point + dist = 3.8; + pt = ctrl->getLookAheadPointWrapper(dist, path); + EXPECT_EQ(pt.pose.position.x, 4.0); + + // test end of path + dist = 100.0; + pt = ctrl->getLookAheadPointWrapper(dist, path); + EXPECT_EQ(pt.pose.position.x, 9.0); +} + +TEST(RegulatedPurePursuitTest, rotateTests) +{ + 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"); + ctrl->configure(node, name, tf, costmap); + + // shouldRotateToPath + geometry_msgs::msg::PoseStamped carrot; + double angle_to_path_rtn; + EXPECT_EQ(ctrl->shouldRotateToPathWrapper(carrot, angle_to_path_rtn), false); + + carrot.pose.position.x = 0.5; + carrot.pose.position.y = 0.25; + EXPECT_EQ(ctrl->shouldRotateToPathWrapper(carrot, angle_to_path_rtn), false); + + carrot.pose.position.x = 0.5; + carrot.pose.position.y = 1.0; + EXPECT_EQ(ctrl->shouldRotateToPathWrapper(carrot, angle_to_path_rtn), true); + + // shouldRotateToGoalHeading + carrot.pose.position.x = 0.0; + carrot.pose.position.y = 0.0; + EXPECT_EQ(ctrl->shouldRotateToGoalHeadingWrapper(carrot), true); + + carrot.pose.position.x = 0.0; + carrot.pose.position.y = 0.24; + EXPECT_EQ(ctrl->shouldRotateToGoalHeadingWrapper(carrot), true); + + carrot.pose.position.x = 0.0; + carrot.pose.position.y = 0.26; + EXPECT_EQ(ctrl->shouldRotateToGoalHeadingWrapper(carrot), false); + + // rotateToHeading + double lin_v = 10.0; + double ang_v = 0.5; + double angle_to_path = 0.4; + geometry_msgs::msg::Twist curr_speed; + curr_speed.angular.z = 1.75; + + // basic full speed at a speed + ctrl->rotateToHeadingWrapper(lin_v, ang_v, angle_to_path, curr_speed); + EXPECT_EQ(lin_v, 0.0); + EXPECT_EQ(ang_v, 1.8); + + // negative direction + angle_to_path = -0.4; + curr_speed.angular.z = -1.75; + ctrl->rotateToHeadingWrapper(lin_v, ang_v, angle_to_path, curr_speed); + EXPECT_EQ(ang_v, -1.8); + + // kinematic clamping, no speed, some speed accelerating, some speed decelerating + angle_to_path = 0.4; + curr_speed.angular.z = 0.0; + ctrl->rotateToHeadingWrapper(lin_v, ang_v, angle_to_path, curr_speed); + EXPECT_NEAR(ang_v, 0.16, 0.01); + + curr_speed.angular.z = 1.0; + ctrl->rotateToHeadingWrapper(lin_v, ang_v, angle_to_path, curr_speed); + EXPECT_NEAR(ang_v, 1.16, 0.01); + + angle_to_path = -0.4; + curr_speed.angular.z = 1.0; + ctrl->rotateToHeadingWrapper(lin_v, ang_v, angle_to_path, curr_speed); + EXPECT_NEAR(ang_v, 0.84, 0.01); +} + +TEST(RegulatedPurePursuitTest, applyConstraints) +{ + 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"); + ctrl->configure(node, name, tf, costmap); + + double dist_error = 0.0; + double lookahead_dist = 0.6; + double curvature = 0.5; + geometry_msgs::msg::Twist curr_speed; + double pose_cost = 0.0; + double linear_vel = 0.0; + + // since costmaps here are bogus, we can't access them + ctrl->resetVelocityApproachScaling(); + + // test curvature regulation (default) + curr_speed.linear.x = 0.25; + ctrl->applyConstraintsWrapper(dist_error, lookahead_dist, curvature, curr_speed, pose_cost, linear_vel); + EXPECT_EQ(linear_vel, 0.25); // min set speed + + linear_vel = 1.0; + curvature = 0.7407; + curr_speed.linear.x = 0.5; + ctrl->applyConstraintsWrapper(dist_error, lookahead_dist, curvature, curr_speed, pose_cost, linear_vel); + EXPECT_NEAR(linear_vel, 0.5, 0.01); // lower by curvature + + linear_vel = 1.0; + curvature = 1000.0; + curr_speed.linear.x = 0.25; + ctrl->applyConstraintsWrapper(dist_error, lookahead_dist, curvature, curr_speed, pose_cost, linear_vel); + EXPECT_NEAR(linear_vel, 0.25, 0.01); // min out by curvature + + + // now try with cost regulation (turn off velocity and only cost) + // ctrl->setCostRegulationScaling(); + // ctrl->resetVelocityRegulationScaling(); + // curvature = 0.0; + + // min changable cost + // pose_cost = 1; + // linear_vel = 0.5; + // curr_speed.linear.x = 0.5; + // ctrl->applyConstraintsWrapper(dist_error, lookahead_dist, curvature, curr_speed, pose_cost, linear_vel); + // EXPECT_NEAR(linear_vel, 0.498, 0.01); + + // max changing cost + // pose_cost = 127; + // curr_speed.linear.x = 0.255; + // ctrl->applyConstraintsWrapper(dist_error, lookahead_dist, curvature, curr_speed, pose_cost, linear_vel); + // EXPECT_NEAR(linear_vel, 0.255, 0.01); + + // over max cost thresh + // pose_cost = 200; + // curr_speed.linear.x = 0.25; + // ctrl->applyConstraintsWrapper(dist_error, lookahead_dist, curvature, curr_speed, pose_cost, linear_vel); + // EXPECT_NEAR(linear_vel, 0.25, 0.01); + + // test kinematic clamping + // pose_cost = 200; + // curr_speed.linear.x = 1.0; + // ctrl->applyConstraintsWrapper(dist_error, lookahead_dist, curvature, curr_speed, pose_cost, linear_vel); + // EXPECT_NEAR(linear_vel, 0.5, 0.01); +} diff --git a/nav2_rviz_plugins/src/nav2_panel.cpp b/nav2_rviz_plugins/src/nav2_panel.cpp index 396eafd4fa..e227489171 100644 --- a/nav2_rviz_plugins/src/nav2_panel.cpp +++ b/nav2_rviz_plugins/src/nav2_panel.cpp @@ -117,7 +117,7 @@ Nav2Panel::Nav2Panel(QWidget * parent) // State entered when navigate_to_pose action is not active accumulating_ = new QState(); accumulating_->setObjectName("accumulating"); - accumulating_->assignProperty(start_reset_button_, "text", "Reset"); + accumulating_->assignProperty(start_reset_button_, "text", "Cancel Waypoint Mode"); accumulating_->assignProperty(start_reset_button_, "toolTip", cancel_waypoint_msg); accumulating_->assignProperty(start_reset_button_, "enabled", true); @@ -130,6 +130,18 @@ Nav2Panel::Nav2Panel(QWidget * parent) accumulating_->assignProperty(navigation_mode_button_, "toolTip", waypoint_goal_msg); accumulated_ = new QState(); + accumulated_->setObjectName("accumulated"); + accumulated_->assignProperty(start_reset_button_, "text", "Cancel"); + accumulated_->assignProperty(start_reset_button_, "toolTip", cancel_msg); + accumulated_->assignProperty(start_reset_button_, "enabled", true); + + accumulated_->assignProperty(pause_resume_button_, "text", "Pause"); + accumulated_->assignProperty(pause_resume_button_, "enabled", false); + accumulated_->assignProperty(pause_resume_button_, "toolTip", pause_msg); + + accumulated_->assignProperty(navigation_mode_button_, "text", "Start Navigation"); + accumulated_->assignProperty(navigation_mode_button_, "enabled", false); + accumulated_->assignProperty(navigation_mode_button_, "toolTip", waypoint_goal_msg); // State entered to cancel the navigate_to_pose action canceled_ = new QState(); @@ -185,12 +197,12 @@ Nav2Panel::Nav2Panel(QWidget * parent) idle_->addTransition(navigation_mode_button_, SIGNAL(clicked()), accumulating_); accumulating_->addTransition(navigation_mode_button_, SIGNAL(clicked()), accumulated_); accumulating_->addTransition(start_reset_button_, SIGNAL(clicked()), idle_); + accumulated_->addTransition(start_reset_button_, SIGNAL(clicked()), canceled_); // Internal state transitions canceled_->addTransition(canceled_, SIGNAL(entered()), idle_); reset_->addTransition(reset_, SIGNAL(entered()), initial_); resumed_->addTransition(resumed_, SIGNAL(entered()), idle_); - accumulated_->addTransition(accumulated_, SIGNAL(entered()), idle_); // Pause/Resume button click transitions idle_->addTransition(pause_resume_button_, SIGNAL(clicked()), paused_); @@ -205,6 +217,15 @@ Nav2Panel::Nav2Panel(QWidget * parent) runningTransition->setTargetState(idle_); running_->addTransition(runningTransition); + ROSActionQTransition * idleAccumulatedTransition = + new ROSActionQTransition(QActionState::INACTIVE); + idleAccumulatedTransition->setTargetState(accumulated_); + idle_->addTransition(idleAccumulatedTransition); + + ROSActionQTransition * accumulatedTransition = new ROSActionQTransition(QActionState::ACTIVE); + accumulatedTransition->setTargetState(idle_); + accumulated_->addTransition(accumulatedTransition); + initial_thread_ = new InitialThread(client_nav_, client_loc_); connect(initial_thread_, &InitialThread::finished, initial_thread_, &QObject::deleteLater); @@ -407,7 +428,7 @@ Nav2Panel::onNewGoal(double x, double y, double theta, QString frame) void Nav2Panel::onCancelButtonPressed() { - if (state_machine_.configuration().contains(accumulating_)) { + if (waypoint_follower_goal_handle_) { auto future_cancel = waypoint_follower_action_client_->async_cancel_goal(waypoint_follower_goal_handle_); @@ -417,7 +438,9 @@ Nav2Panel::onCancelButtonPressed() RCLCPP_ERROR(client_node_->get_logger(), "Failed to cancel waypoint follower"); return; } - } else { + } + + if (navigation_goal_handle_) { auto future_cancel = navigation_action_client_->async_cancel_goal(navigation_goal_handle_); if (rclcpp::spin_until_future_complete(client_node_, future_cancel, server_timeout_) != @@ -448,7 +471,7 @@ Nav2Panel::onAccumulating() void Nav2Panel::timerEvent(QTimerEvent * event) { - if (state_machine_.configuration().contains(accumulating_)) { + if (state_machine_.configuration().contains(accumulated_)) { if (event->timerId() == timer_.timerId()) { if (!waypoint_follower_goal_handle_) { RCLCPP_DEBUG(client_node_->get_logger(), "Waiting for Goal"); diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index 7c07bb08ae..27e1412add 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -59,6 +59,7 @@ launch launch_ros launch_testing + python3-zmq ament_cmake diff --git a/nav2_system_tests/src/system/CMakeLists.txt b/nav2_system_tests/src/system/CMakeLists.txt index a7ae2aa43f..b176fcddfc 100644 --- a/nav2_system_tests/src/system/CMakeLists.txt +++ b/nav2_system_tests/src/system/CMakeLists.txt @@ -12,7 +12,9 @@ ament_add_test(test_bt_navigator TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models BT_NAVIGATOR_XML=navigate_w_replanning_and_recovery.xml - ASTAR=False + ASTAR=True + CONTROLLER=nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController + PLANNER=nav2_navfn_planner/NavfnPlanner ) ament_add_test(test_bt_navigator_with_dijkstra @@ -27,6 +29,25 @@ ament_add_test(test_bt_navigator_with_dijkstra GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models BT_NAVIGATOR_XML=navigate_w_replanning_and_recovery.xml ASTAR=False + CONTROLLER=dwb_core::DWBLocalPlanner + PLANNER=nav2_navfn_planner/NavfnPlanner +) + +ament_add_test(test_bt_navigator_with_groot_monitoring + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + TIMEOUT 180 + ENV + TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR} + TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml + TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world + GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models + BT_NAVIGATOR_XML=navigate_w_replanning_and_recovery.xml + ASTAR=False + GROOT_MONITORING=True + CONTROLLER=dwb_core::DWBLocalPlanner + PLANNER=nav2_navfn_planner/NavfnPlanner ) ament_add_test(test_dynamic_obstacle @@ -41,6 +62,8 @@ ament_add_test(test_dynamic_obstacle GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models BT_NAVIGATOR_XML=navigate_w_replanning_and_recovery.xml ASTAR=False + CONTROLLER=dwb_core::DWBLocalPlanner + PLANNER=nav2_navfn_planner/NavfnPlanner ) # ament_add_test(test_multi_robot @@ -55,4 +78,6 @@ ament_add_test(test_dynamic_obstacle # TEST_URDF=${PROJECT_SOURCE_DIR}/urdf/turtlebot3_waffle.urdf # TEST_SDF=${PROJECT_SOURCE_DIR}/models/turtlebot3_waffle/model.sdf # BT_NAVIGATOR_XML=navigate_w_replanning_and_recovery.xml +# CONTROLLER=dwb_core::DWBLocalPlanner +# PLANNER=nav2_navfn_planner/NavfnPlanner # ) diff --git a/nav2_system_tests/src/system/test_system_launch.py b/nav2_system_tests/src/system/test_system_launch.py index 92ea252eba..8a15b9a63c 100755 --- a/nav2_system_tests/src/system/test_system_launch.py +++ b/nav2_system_tests/src/system/test_system_launch.py @@ -1,6 +1,7 @@ #!/usr/bin/env python3 # Copyright (c) 2018 Intel Corporation +# Copyright (c) 2020 Florian Gramss # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -22,6 +23,7 @@ from launch import LaunchDescription from launch import LaunchService from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable +from launch.launch_context import LaunchContext from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node from launch_testing.legacy import LaunchTestService @@ -40,15 +42,30 @@ def generate_launch_description(): bringup_dir = get_package_share_directory('nav2_bringup') params_file = os.path.join(bringup_dir, 'params', 'nav2_params.yaml') - # Replace the `use_astar` setting on the params file - param_substitutions = { - 'planner_server.ros__parameters.GridBased.use_astar': os.getenv('ASTAR')} + # Replace the default parameter values for testing special features + # without having multiple params_files inside the nav2 stack + context = LaunchContext() + param_substitutions = {} + + if (os.getenv('ASTAR') == 'True'): + param_substitutions.update({'use_astar': 'True'}) + + if (os.getenv('GROOT_MONITORING') == 'True'): + param_substitutions.update({'enable_groot_monitoring': 'True'}) + + param_substitutions.update( + {'planner_server.ros__parameters.GridBased.plugin': os.getenv('PLANNER')}) + param_substitutions.update( + {'controller_server.ros__parameters.FollowPath.plugin': os.getenv('CONTROLLER')}) + configured_params = RewrittenYaml( source_file=params_file, root_key='', param_rewrites=param_substitutions, convert_types=True) + new_yaml = configured_params.perform(context) + return LaunchDescription([ SetEnvironmentVariable('RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1'), @@ -79,7 +96,7 @@ def generate_launch_description(): 'use_namespace': 'False', 'map': map_yaml_file, 'use_sim_time': 'True', - 'params_file': configured_params, + 'params_file': new_yaml, 'bt_xml_file': bt_navigator_xml, 'autostart': 'True'}.items()), ]) diff --git a/nav2_system_tests/src/system/tester_node.py b/nav2_system_tests/src/system/tester_node.py index 83e9df9dc9..d84724ea5c 100755 --- a/nav2_system_tests/src/system/tester_node.py +++ b/nav2_system_tests/src/system/tester_node.py @@ -1,5 +1,6 @@ #! /usr/bin/env python3 # Copyright 2018 Intel Corporation. +# Copyright 2020 Florian Gramss # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -15,8 +16,10 @@ import argparse import math +import os import sys import time + from typing import Optional from action_msgs.msg import GoalStatus @@ -34,6 +37,8 @@ from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy from rclpy.qos import QoSProfile +import zmq + class NavTester(Node): @@ -94,6 +99,13 @@ def runNavigateAction(self, goal_pose: Optional[Pose] = None): while not self.action_client.wait_for_server(timeout_sec=1.0): self.info_msg("'NavigateToPose' action server not available, waiting...") + if (os.getenv('GROOT_MONITORING') == 'True'): + if self.grootMonitoringGetStatus(): + self.error_msg('Behavior Tree must not be running already!') + self.error_msg('Are you running multiple goals/bts..?') + return False + self.info_msg('This Error above MUST Fail and is o.k.!') + self.goal_pose = goal_pose if goal_pose is not None else self.goal_pose goal_msg = NavigateToPose.Goal() goal_msg.pose = self.getStampedPoseMsg(self.goal_pose) @@ -111,6 +123,19 @@ def runNavigateAction(self, goal_pose: Optional[Pose] = None): self.info_msg('Goal accepted') get_result_future = goal_handle.get_result_async() + future_return = True + if (os.getenv('GROOT_MONITORING') == 'True'): + try: + if not self.grootMonitoringReloadTree(): + self.error_msg('Failed GROOT_BT - Reload Tree from ZMQ Server') + future_return = False + if not self.grootMonitoringGetStatus(): + self.error_msg('Failed GROOT_BT - Get Status from ZMQ Publisher') + future_return = False + except Exception as e: + self.error_msg('Failed GROOT_BT - ZMQ Tests: ' + e.__doc__ + e.message) + future_return = False + self.info_msg("Waiting for 'NavigateToPose' action to complete") rclpy.spin_until_future_complete(self, get_result_future) status = get_result_future.result().status @@ -118,9 +143,81 @@ def runNavigateAction(self, goal_pose: Optional[Pose] = None): self.info_msg('Goal failed with status code: {0}'.format(status)) return False + if not future_return: + return False + self.info_msg('Goal succeeded!') return True + def grootMonitoringReloadTree(self): + # ZeroMQ Context + context = zmq.Context() + + sock = context.socket(zmq.REQ) + port = 1667 # default server port for groot monitoring + # # Set a Timeout so we do not spin till infinity + sock.setsockopt(zmq.RCVTIMEO, 1000) + # sock.setsockopt(zmq.LINGER, 0) + + sock.connect('tcp://localhost:' + str(port)) + self.info_msg('ZMQ Server Port: ' + str(port)) + + # this should fail + try: + sock.recv() + self.error_msg('ZMQ Reload Tree Test 1/3 - This should have failed!') + # Only works when ZMQ server receives a request first + sock.close() + return False + except zmq.error.ZMQError: + self.info_msg('ZMQ Reload Tree Test 1/3: Check') + try: + # request tree from server + sock.send_string('') + # receive tree from server as flat_buffer + sock.recv() + self.info_msg('ZMQ Reload Tree Test 2/3: Check') + except zmq.error.Again: + self.info_msg('ZMQ Reload Tree Test 2/3 - Failed to load tree') + sock.close() + return False + + # this should fail + try: + sock.recv() + self.error_msg('ZMQ Reload Tree Test 3/3 - This should have failed!') + # Tree should only be loadable ONCE after ZMQ server received a request + sock.close() + return False + except zmq.error.ZMQError: + self.info_msg('ZMQ Reload Tree Test 3/3: Check') + + return True + + def grootMonitoringGetStatus(self): + # ZeroMQ Context + context = zmq.Context() + # Define the socket using the 'Context' + sock = context.socket(zmq.SUB) + # Set a Timeout so we do not spin till infinity + sock.setsockopt(zmq.RCVTIMEO, 2000) + # sock.setsockopt(zmq.LINGER, 0) + + # Define subscription and messages with prefix to accept. + sock.setsockopt_string(zmq.SUBSCRIBE, '') + port = 1666 # default publishing port for groot monitoring + sock.connect('tcp://127.0.0.1:' + str(port)) + + for request in range(3): + try: + sock.recv() + except zmq.error.Again: + self.error_msg('ZMQ - Did not receive any status') + sock.close() + return False + self.info_msg('ZMQ - Did receive status') + return True + def poseCallback(self, msg): self.info_msg('Received amcl_pose') self.current_pose = msg.pose.pose diff --git a/nav2_system_tests/src/updown/CMakeLists.txt b/nav2_system_tests/src/updown/CMakeLists.txt index a56bff34b2..273e1828ea 100644 --- a/nav2_system_tests/src/updown/CMakeLists.txt +++ b/nav2_system_tests/src/updown/CMakeLists.txt @@ -6,6 +6,6 @@ ament_target_dependencies(test_updown ${dependencies} ) -install(TARGETS test_updown RUNTIME DESTINATION share/${PROJECT_NAME}) +install(TARGETS test_updown RUNTIME DESTINATION lib/${PROJECT_NAME}) install(FILES test_updown_launch.py DESTINATION share/${PROJECT_NAME}) diff --git a/navigation2/package.xml b/navigation2/package.xml index 7d0558bfda..5169c4471f 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -31,6 +31,7 @@ nav2_controller nav2_waypoint_follower smac_planner + nav2_regulated_pure_pursuit_controller ament_cmake diff --git a/smac_planner/include/smac_planner/costmap_downsampler.hpp b/smac_planner/include/smac_planner/costmap_downsampler.hpp index 6f23c696e2..bdfb5cdf33 100644 --- a/smac_planner/include/smac_planner/costmap_downsampler.hpp +++ b/smac_planner/include/smac_planner/costmap_downsampler.hpp @@ -34,9 +34,8 @@ class CostmapDownsampler public: /** * @brief A constructor for CostmapDownsampler - * @param node Lifecycle node pointer */ - explicit CostmapDownsampler(const nav2_util::LifecycleNode::SharedPtr & node); + CostmapDownsampler(); /** * @brief A destructor for CostmapDownsampler @@ -44,13 +43,15 @@ class CostmapDownsampler ~CostmapDownsampler(); /** - * @brief Initialize the downsampled costmap object and the ROS publisher + * @brief Configure the downsampled costmap object and the ROS publisher + * @param node Lifecycle node pointer * @param global_frame The ID of the global frame used by the costmap * @param topic_name The name of the topic to publish the downsampled costmap * @param costmap The costmap we want to downsample * @param downsampling_factor Multiplier for the costmap resolution */ - void initialize( + void on_configure( + const nav2_util::LifecycleNode::SharedPtr & node, const std::string & global_frame, const std::string & topic_name, nav2_costmap_2d::Costmap2D * const costmap, @@ -59,18 +60,17 @@ class CostmapDownsampler /** * @brief Activate the publisher of the downsampled costmap */ - void activatePublisher() - { - _downsampled_costmap_pub->on_activate(); - } + void on_activate(); /** * @brief Deactivate the publisher of the downsampled costmap */ - void deactivatePublisher() - { - _downsampled_costmap_pub->on_deactivate(); - } + void on_deactivate(); + + /** + * @brief Cleanup the publisher of the downsampled costmap + */ + void on_cleanup(); /** * @brief Downsample the given costmap by the downsampling factor, and publish the downsampled costmap @@ -105,8 +105,6 @@ class CostmapDownsampler unsigned int _downsampled_size_y; unsigned int _downsampling_factor; float _downsampled_resolution; - std::string _topic_name; - nav2_util::LifecycleNode::SharedPtr _node; nav2_costmap_2d::Costmap2D * _costmap; std::unique_ptr _downsampled_costmap; std::unique_ptr _downsampled_costmap_pub; diff --git a/smac_planner/include/smac_planner/smac_planner.hpp b/smac_planner/include/smac_planner/smac_planner.hpp index 379f0c92a5..347bd562c9 100644 --- a/smac_planner/include/smac_planner/smac_planner.hpp +++ b/smac_planner/include/smac_planner/smac_planner.hpp @@ -111,7 +111,8 @@ class SmacPlanner : public nav2_core::GlobalPlanner protected: std::unique_ptr> _a_star; std::unique_ptr _smoother; - nav2_util::LifecycleNode::SharedPtr _node; + rclcpp::Clock::SharedPtr _clock; + rclcpp::Logger _logger{rclcpp::get_logger("SmacPlanner")}; nav2_costmap_2d::Costmap2D * _costmap; std::unique_ptr _costmap_downsampler; std::string _global_frame, _name; diff --git a/smac_planner/include/smac_planner/smac_planner_2d.hpp b/smac_planner/include/smac_planner/smac_planner_2d.hpp index 6d89887c3d..7cf18a673c 100644 --- a/smac_planner/include/smac_planner/smac_planner_2d.hpp +++ b/smac_planner/include/smac_planner/smac_planner_2d.hpp @@ -106,7 +106,8 @@ class SmacPlanner2D : public nav2_core::GlobalPlanner std::unique_ptr _smoother; nav2_costmap_2d::Costmap2D * _costmap; std::unique_ptr _costmap_downsampler; - nav2_util::LifecycleNode::SharedPtr _node; + rclcpp::Clock::SharedPtr _clock; + rclcpp::Logger _logger{rclcpp::get_logger("SmacPlanner2D")}; std::string _global_frame, _name; float _tolerance; int _downsampling_factor; diff --git a/smac_planner/src/costmap_downsampler.cpp b/smac_planner/src/costmap_downsampler.cpp index 6955469831..025a30b351 100644 --- a/smac_planner/src/costmap_downsampler.cpp +++ b/smac_planner/src/costmap_downsampler.cpp @@ -22,9 +22,8 @@ namespace smac_planner { -CostmapDownsampler::CostmapDownsampler(const nav2_util::LifecycleNode::SharedPtr & node) -: _node(node), - _costmap(nullptr), +CostmapDownsampler::CostmapDownsampler() +: _costmap(nullptr), _downsampled_costmap(nullptr), _downsampled_costmap_pub(nullptr) { @@ -34,13 +33,13 @@ CostmapDownsampler::~CostmapDownsampler() { } -void CostmapDownsampler::initialize( +void CostmapDownsampler::on_configure( + const nav2_util::LifecycleNode::SharedPtr & node, const std::string & global_frame, const std::string & topic_name, nav2_costmap_2d::Costmap2D * const costmap, const unsigned int & downsampling_factor) { - _topic_name = topic_name; _costmap = costmap; _downsampling_factor = downsampling_factor; updateCostmapSize(); @@ -50,7 +49,24 @@ void CostmapDownsampler::initialize( _costmap->getOriginX(), _costmap->getOriginY(), UNKNOWN); _downsampled_costmap_pub = std::make_unique( - _node, _downsampled_costmap.get(), global_frame, _topic_name, false); + node, _downsampled_costmap.get(), global_frame, topic_name, false); +} + +void CostmapDownsampler::on_activate() +{ + _downsampled_costmap_pub->on_activate(); +} + +void CostmapDownsampler::on_deactivate() +{ + _downsampled_costmap_pub->on_deactivate(); +} + +void CostmapDownsampler::on_cleanup() +{ + _costmap = nullptr; + _downsampled_costmap.reset(); + _downsampled_costmap_pub.reset(); } nav2_costmap_2d::Costmap2D * CostmapDownsampler::downsample( @@ -74,10 +90,7 @@ nav2_costmap_2d::Costmap2D * CostmapDownsampler::downsample( } } - if (_node->count_subscribers(_topic_name) > 0) { - _downsampled_costmap_pub->publishCostmap(); - } - + _downsampled_costmap_pub->publishCostmap(); return _downsampled_costmap.get(); } diff --git a/smac_planner/src/smac_planner.cpp b/smac_planner/src/smac_planner.cpp index f3db10830a..71ba1d3333 100644 --- a/smac_planner/src/smac_planner.cpp +++ b/smac_planner/src/smac_planner.cpp @@ -31,7 +31,6 @@ using namespace std::chrono; // NOLINT SmacPlanner::SmacPlanner() : _a_star(nullptr), _smoother(nullptr), - _node(nullptr), _costmap(nullptr), _costmap_downsampler(nullptr) { @@ -40,16 +39,17 @@ SmacPlanner::SmacPlanner() SmacPlanner::~SmacPlanner() { RCLCPP_INFO( - _node->get_logger(), "Destroying plugin %s of type SmacPlanner", + _logger, "Destroying plugin %s of type SmacPlanner", _name.c_str()); } void SmacPlanner::configure( - rclcpp_lifecycle::LifecycleNode::SharedPtr parent, + rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::string name, std::shared_ptr/*tf*/, std::shared_ptr costmap_ros) { - _node = parent; + _logger = node->get_logger(); + _clock = node->get_clock(); _costmap = costmap_ros->getCostmap(); _name = name; _global_frame = costmap_ros->getGlobalFrameID(); @@ -64,62 +64,62 @@ void SmacPlanner::configure( // General planner params nav2_util::declare_parameter_if_not_declared( - _node, name + ".tolerance", rclcpp::ParameterValue(0.125)); - _tolerance = static_cast(_node->get_parameter(name + ".tolerance").as_double()); + node, name + ".tolerance", rclcpp::ParameterValue(0.125)); + _tolerance = static_cast(node->get_parameter(name + ".tolerance").as_double()); nav2_util::declare_parameter_if_not_declared( - _node, name + ".downsample_costmap", rclcpp::ParameterValue(true)); - _node->get_parameter(name + ".downsample_costmap", _downsample_costmap); + node, name + ".downsample_costmap", rclcpp::ParameterValue(false)); + node->get_parameter(name + ".downsample_costmap", _downsample_costmap); nav2_util::declare_parameter_if_not_declared( - _node, name + ".downsampling_factor", rclcpp::ParameterValue(1)); - _node->get_parameter(name + ".downsampling_factor", _downsampling_factor); + node, name + ".downsampling_factor", rclcpp::ParameterValue(1)); + node->get_parameter(name + ".downsampling_factor", _downsampling_factor); nav2_util::declare_parameter_if_not_declared( - _node, name + ".angle_quantization_bins", rclcpp::ParameterValue(1)); - _node->get_parameter(name + ".angle_quantization_bins", angle_quantizations); + node, name + ".angle_quantization_bins", rclcpp::ParameterValue(72)); + node->get_parameter(name + ".angle_quantization_bins", angle_quantizations); _angle_bin_size = 2.0 * M_PI / angle_quantizations; _angle_quantizations = static_cast(angle_quantizations); nav2_util::declare_parameter_if_not_declared( - _node, name + ".allow_unknown", rclcpp::ParameterValue(true)); - _node->get_parameter(name + ".allow_unknown", allow_unknown); + node, name + ".allow_unknown", rclcpp::ParameterValue(true)); + node->get_parameter(name + ".allow_unknown", allow_unknown); nav2_util::declare_parameter_if_not_declared( - _node, name + ".max_iterations", rclcpp::ParameterValue(-1)); - _node->get_parameter(name + ".max_iterations", max_iterations); + node, name + ".max_iterations", rclcpp::ParameterValue(-1)); + node->get_parameter(name + ".max_iterations", max_iterations); nav2_util::declare_parameter_if_not_declared( - _node, name + ".smooth_path", rclcpp::ParameterValue(false)); - _node->get_parameter(name + ".smooth_path", smooth_path); + node, name + ".smooth_path", rclcpp::ParameterValue(false)); + node->get_parameter(name + ".smooth_path", smooth_path); nav2_util::declare_parameter_if_not_declared( - _node, name + ".minimum_turning_radius", rclcpp::ParameterValue(0.2)); - _node->get_parameter(name + ".minimum_turning_radius", search_info.minimum_turning_radius); + node, name + ".minimum_turning_radius", rclcpp::ParameterValue(0.2)); + node->get_parameter(name + ".minimum_turning_radius", search_info.minimum_turning_radius); nav2_util::declare_parameter_if_not_declared( - _node, name + ".reverse_penalty", rclcpp::ParameterValue(2.0)); - _node->get_parameter(name + ".reverse_penalty", search_info.reverse_penalty); + node, name + ".reverse_penalty", rclcpp::ParameterValue(2.0)); + node->get_parameter(name + ".reverse_penalty", search_info.reverse_penalty); nav2_util::declare_parameter_if_not_declared( - _node, name + ".change_penalty", rclcpp::ParameterValue(0.5)); - _node->get_parameter(name + ".change_penalty", search_info.change_penalty); + node, name + ".change_penalty", rclcpp::ParameterValue(0.5)); + node->get_parameter(name + ".change_penalty", search_info.change_penalty); nav2_util::declare_parameter_if_not_declared( - _node, name + ".non_straight_penalty", rclcpp::ParameterValue(1.05)); - _node->get_parameter(name + ".non_straight_penalty", search_info.non_straight_penalty); + node, name + ".non_straight_penalty", rclcpp::ParameterValue(1.05)); + node->get_parameter(name + ".non_straight_penalty", search_info.non_straight_penalty); nav2_util::declare_parameter_if_not_declared( - _node, name + ".cost_penalty", rclcpp::ParameterValue(1.2)); - _node->get_parameter(name + ".cost_penalty", search_info.cost_penalty); + node, name + ".cost_penalty", rclcpp::ParameterValue(1.2)); + node->get_parameter(name + ".cost_penalty", search_info.cost_penalty); nav2_util::declare_parameter_if_not_declared( - _node, name + ".analytic_expansion_ratio", rclcpp::ParameterValue(2.0)); - _node->get_parameter(name + ".analytic_expansion_ratio", search_info.analytic_expansion_ratio); + node, name + ".analytic_expansion_ratio", rclcpp::ParameterValue(2.0)); + node->get_parameter(name + ".analytic_expansion_ratio", search_info.analytic_expansion_ratio); nav2_util::declare_parameter_if_not_declared( - _node, name + ".max_planning_time_ms", rclcpp::ParameterValue(5000.0)); - _node->get_parameter(name + ".max_planning_time_ms", _max_planning_time); + node, name + ".max_planning_time_ms", rclcpp::ParameterValue(5000.0)); + node->get_parameter(name + ".max_planning_time_ms", _max_planning_time); nav2_util::declare_parameter_if_not_declared( - _node, name + ".motion_model_for_search", rclcpp::ParameterValue(std::string("DUBIN"))); - _node->get_parameter(name + ".motion_model_for_search", motion_model_for_search); + node, name + ".motion_model_for_search", rclcpp::ParameterValue(std::string("DUBIN"))); + node->get_parameter(name + ".motion_model_for_search", motion_model_for_search); MotionModel motion_model = fromString(motion_model_for_search); if (motion_model == MotionModel::UNKNOWN) { RCLCPP_WARN( - _node->get_logger(), + _logger, "Unable to get MotionModel search type. Given '%s', " "valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP.", motion_model_for_search.c_str()); @@ -127,14 +127,14 @@ void SmacPlanner::configure( if (max_on_approach_iterations <= 0) { RCLCPP_INFO( - _node->get_logger(), "On approach iteration selected as <= 0, " + _logger, "On approach iteration selected as <= 0, " "disabling tolerance and on approach iterations."); max_on_approach_iterations = std::numeric_limits::max(); } if (max_iterations <= 0) { RCLCPP_INFO( - _node->get_logger(), "maximum iteration selected as <= 0, " + _logger, "maximum iteration selected as <= 0, " "disabling maximum iterations."); max_iterations = std::numeric_limits::max(); } @@ -153,22 +153,23 @@ void SmacPlanner::configure( if (smooth_path) { _smoother = std::make_unique(); - _optimizer_params.get(_node.get(), name); - _smoother_params.get(_node.get(), name); + _optimizer_params.get(node.get(), name); + _smoother_params.get(node.get(), name); _smoother_params.max_curvature = 1.0f / minimum_turning_radius_global_coords; _smoother->initialize(_optimizer_params); } if (_downsample_costmap && _downsampling_factor > 1) { std::string topic_name = "downsampled_costmap"; - _costmap_downsampler = std::make_unique(_node); - _costmap_downsampler->initialize(_global_frame, topic_name, _costmap, _downsampling_factor); + _costmap_downsampler = std::make_unique(); + _costmap_downsampler->on_configure( + node, _global_frame, topic_name, _costmap, _downsampling_factor); } - _raw_plan_publisher = _node->create_publisher("unsmoothed_plan", 1); + _raw_plan_publisher = node->create_publisher("unsmoothed_plan", 1); RCLCPP_INFO( - _node->get_logger(), "Configured plugin %s of type SmacPlanner with " + _logger, "Configured plugin %s of type SmacPlanner with " "tolerance %.2f, maximum iterations %i, " "max on approach iterations %i, and %s. Using motion model: %s.", _name.c_str(), _tolerance, max_iterations, max_on_approach_iterations, @@ -179,32 +180,33 @@ void SmacPlanner::configure( void SmacPlanner::activate() { RCLCPP_INFO( - _node->get_logger(), "Activating plugin %s of type SmacPlanner", + _logger, "Activating plugin %s of type SmacPlanner", _name.c_str()); _raw_plan_publisher->on_activate(); if (_costmap_downsampler) { - _costmap_downsampler->activatePublisher(); + _costmap_downsampler->on_activate(); } } void SmacPlanner::deactivate() { RCLCPP_INFO( - _node->get_logger(), "Deactivating plugin %s of type SmacPlanner", + _logger, "Deactivating plugin %s of type SmacPlanner", _name.c_str()); _raw_plan_publisher->on_deactivate(); if (_costmap_downsampler) { - _costmap_downsampler->deactivatePublisher(); + _costmap_downsampler->on_deactivate(); } } void SmacPlanner::cleanup() { RCLCPP_INFO( - _node->get_logger(), "Cleaning up plugin %s of type SmacPlanner", + _logger, "Cleaning up plugin %s of type SmacPlanner", _name.c_str()); _a_star.reset(); _smoother.reset(); + _costmap_downsampler->on_cleanup(); _costmap_downsampler.reset(); _raw_plan_publisher.reset(); } @@ -251,7 +253,7 @@ nav_msgs::msg::Path SmacPlanner::createPlan( // Setup message nav_msgs::msg::Path plan; - plan.header.stamp = _node->now(); + plan.header.stamp = _clock->now(); plan.header.frame_id = _global_frame; geometry_msgs::msg::PoseStamped pose; pose.header = plan.header; @@ -282,7 +284,7 @@ nav_msgs::msg::Path SmacPlanner::createPlan( if (!error.empty()) { RCLCPP_WARN( - _node->get_logger(), + _logger, "%s: failed to create plan, %s.", _name.c_str(), error.c_str()); return plan; @@ -304,7 +306,7 @@ nav_msgs::msg::Path SmacPlanner::createPlan( } // Publish raw path for debug - if (_node->count_subscribers(_raw_plan_publisher->get_topic_name()) > 0) { + if (_raw_plan_publisher->get_subscription_count() > 0) { _raw_plan_publisher->publish(plan); } @@ -328,7 +330,7 @@ nav_msgs::msg::Path SmacPlanner::createPlan( // Smooth plan if (!_smoother->smooth(path_world, costmap, _smoother_params)) { RCLCPP_WARN( - _node->get_logger(), + _logger, "%s: failed to smooth plan, Ceres could not find a usable solution to optimize.", _name.c_str()); return plan; diff --git a/smac_planner/src/smac_planner_2d.cpp b/smac_planner/src/smac_planner_2d.cpp index 3c4b8ca83a..643ffe41a6 100644 --- a/smac_planner/src/smac_planner_2d.cpp +++ b/smac_planner/src/smac_planner_2d.cpp @@ -30,24 +30,24 @@ SmacPlanner2D::SmacPlanner2D() : _a_star(nullptr), _smoother(nullptr), _costmap(nullptr), - _costmap_downsampler(nullptr), - _node(nullptr) + _costmap_downsampler(nullptr) { } SmacPlanner2D::~SmacPlanner2D() { RCLCPP_INFO( - _node->get_logger(), "Destroying plugin %s of type SmacPlanner2D", + _logger, "Destroying plugin %s of type SmacPlanner2D", _name.c_str()); } void SmacPlanner2D::configure( - rclcpp_lifecycle::LifecycleNode::SharedPtr parent, + rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::string name, std::shared_ptr/*tf*/, std::shared_ptr costmap_ros) { - _node = parent; + _logger = node->get_logger(); + _clock = node->get_clock(); _costmap = costmap_ros->getCostmap(); _name = name; _global_frame = costmap_ros->getGlobalFrameID(); @@ -61,42 +61,42 @@ void SmacPlanner2D::configure( // General planner params nav2_util::declare_parameter_if_not_declared( - _node, name + ".tolerance", rclcpp::ParameterValue(0.125)); - _tolerance = static_cast(_node->get_parameter(name + ".tolerance").as_double()); + node, name + ".tolerance", rclcpp::ParameterValue(0.125)); + _tolerance = static_cast(node->get_parameter(name + ".tolerance").as_double()); nav2_util::declare_parameter_if_not_declared( - _node, name + ".downsample_costmap", rclcpp::ParameterValue(true)); - _node->get_parameter(name + ".downsample_costmap", _downsample_costmap); + node, name + ".downsample_costmap", rclcpp::ParameterValue(true)); + node->get_parameter(name + ".downsample_costmap", _downsample_costmap); nav2_util::declare_parameter_if_not_declared( - _node, name + ".downsampling_factor", rclcpp::ParameterValue(1)); - _node->get_parameter(name + ".downsampling_factor", _downsampling_factor); + node, name + ".downsampling_factor", rclcpp::ParameterValue(1)); + node->get_parameter(name + ".downsampling_factor", _downsampling_factor); nav2_util::declare_parameter_if_not_declared( - _node, name + ".allow_unknown", rclcpp::ParameterValue(true)); - _node->get_parameter(name + ".allow_unknown", allow_unknown); + node, name + ".allow_unknown", rclcpp::ParameterValue(true)); + node->get_parameter(name + ".allow_unknown", allow_unknown); nav2_util::declare_parameter_if_not_declared( - _node, name + ".max_iterations", rclcpp::ParameterValue(-1)); - _node->get_parameter(name + ".max_iterations", max_iterations); + node, name + ".max_iterations", rclcpp::ParameterValue(-1)); + node->get_parameter(name + ".max_iterations", max_iterations); 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); + 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 + ".smooth_path", rclcpp::ParameterValue(false)); - _node->get_parameter(name + ".smooth_path", smooth_path); + node, name + ".smooth_path", rclcpp::ParameterValue(false)); + node->get_parameter(name + ".smooth_path", smooth_path); nav2_util::declare_parameter_if_not_declared( - _node, name + ".minimum_turning_radius", rclcpp::ParameterValue(0.2)); - _node->get_parameter(name + ".minimum_turning_radius", minimum_turning_radius); + node, name + ".minimum_turning_radius", rclcpp::ParameterValue(0.2)); + node->get_parameter(name + ".minimum_turning_radius", minimum_turning_radius); nav2_util::declare_parameter_if_not_declared( - _node, name + ".max_planning_time_ms", rclcpp::ParameterValue(1000.0)); - _node->get_parameter(name + ".max_planning_time_ms", _max_planning_time); + node, name + ".max_planning_time_ms", rclcpp::ParameterValue(1000.0)); + node->get_parameter(name + ".max_planning_time_ms", _max_planning_time); nav2_util::declare_parameter_if_not_declared( - _node, name + ".motion_model_for_search", rclcpp::ParameterValue(std::string("MOORE"))); - _node->get_parameter(name + ".motion_model_for_search", motion_model_for_search); + node, name + ".motion_model_for_search", rclcpp::ParameterValue(std::string("MOORE"))); + node->get_parameter(name + ".motion_model_for_search", motion_model_for_search); MotionModel motion_model = fromString(motion_model_for_search); if (motion_model == MotionModel::UNKNOWN) { RCLCPP_WARN( - _node->get_logger(), + _logger, "Unable to get MotionModel search type. Given '%s', " "valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP.", motion_model_for_search.c_str()); @@ -104,14 +104,14 @@ void SmacPlanner2D::configure( if (max_on_approach_iterations <= 0) { RCLCPP_INFO( - _node->get_logger(), "On approach iteration selected as <= 0, " + _logger, "On approach iteration selected as <= 0, " "disabling tolerance and on approach iterations."); max_on_approach_iterations = std::numeric_limits::max(); } if (max_iterations <= 0) { RCLCPP_INFO( - _node->get_logger(), "maximum iteration selected as <= 0, " + _logger, "maximum iteration selected as <= 0, " "disabling maximum iterations."); max_iterations = std::numeric_limits::max(); } @@ -124,22 +124,23 @@ void SmacPlanner2D::configure( if (smooth_path) { _smoother = std::make_unique(); - _optimizer_params.get(_node.get(), name); - _smoother_params.get(_node.get(), name); + _optimizer_params.get(node.get(), name); + _smoother_params.get(node.get(), name); _smoother_params.max_curvature = 1.0f / minimum_turning_radius; _smoother->initialize(_optimizer_params); } if (_downsample_costmap && _downsampling_factor > 1) { std::string topic_name = "downsampled_costmap"; - _costmap_downsampler = std::make_unique(_node); - _costmap_downsampler->initialize(_global_frame, topic_name, _costmap, _downsampling_factor); + _costmap_downsampler = std::make_unique(); + _costmap_downsampler->on_configure( + node, _global_frame, topic_name, _costmap, _downsampling_factor); } - _raw_plan_publisher = _node->create_publisher("unsmoothed_plan", 1); + _raw_plan_publisher = node->create_publisher("unsmoothed_plan", 1); RCLCPP_INFO( - _node->get_logger(), "Configured plugin %s of type SmacPlanner2D with " + _logger, "Configured plugin %s of type SmacPlanner2D with " "tolerance %.2f, maximum iterations %i, " "max on approach iterations %i, and %s. Using motion model: %s.", _name.c_str(), _tolerance, max_iterations, max_on_approach_iterations, @@ -150,32 +151,33 @@ void SmacPlanner2D::configure( void SmacPlanner2D::activate() { RCLCPP_INFO( - _node->get_logger(), "Activating plugin %s of type SmacPlanner2D", + _logger, "Activating plugin %s of type SmacPlanner2D", _name.c_str()); _raw_plan_publisher->on_activate(); if (_costmap_downsampler) { - _costmap_downsampler->activatePublisher(); + _costmap_downsampler->on_activate(); } } void SmacPlanner2D::deactivate() { RCLCPP_INFO( - _node->get_logger(), "Deactivating plugin %s of type SmacPlanner2D", + _logger, "Deactivating plugin %s of type SmacPlanner2D", _name.c_str()); _raw_plan_publisher->on_deactivate(); if (_costmap_downsampler) { - _costmap_downsampler->deactivatePublisher(); + _costmap_downsampler->on_deactivate(); } } void SmacPlanner2D::cleanup() { RCLCPP_INFO( - _node->get_logger(), "Cleaning up plugin %s of type SmacPlanner2D", + _logger, "Cleaning up plugin %s of type SmacPlanner2D", _name.c_str()); _a_star.reset(); _smoother.reset(); + _costmap_downsampler->on_cleanup(); _costmap_downsampler.reset(); _raw_plan_publisher.reset(); } @@ -212,7 +214,7 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( // Setup message nav_msgs::msg::Path plan; - plan.header.stamp = _node->now(); + plan.header.stamp = _clock->now(); plan.header.frame_id = _global_frame; geometry_msgs::msg::PoseStamped pose; pose.header = plan.header; @@ -243,7 +245,7 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( if (!error.empty()) { RCLCPP_WARN( - _node->get_logger(), + _logger, "%s: failed to create plan, %s.", _name.c_str(), error.c_str()); return plan; @@ -268,7 +270,7 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( } // Publish raw path for debug - if (_node->count_subscribers(_raw_plan_publisher->get_topic_name()) > 0) { + if (_raw_plan_publisher->get_subscription_count() > 0) { _raw_plan_publisher->publish(plan); } @@ -292,7 +294,7 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( // Smooth plan if (!_smoother->smooth(path_world, costmap, _smoother_params)) { RCLCPP_WARN( - _node->get_logger(), + _logger, "%s: failed to smooth plan, Ceres could not find a usable solution to optimize.", _name.c_str()); return plan; diff --git a/smac_planner/test/test_costmap_downsampler.cpp b/smac_planner/test/test_costmap_downsampler.cpp index 040a2eb760..5353dcb387 100644 --- a/smac_planner/test/test_costmap_downsampler.cpp +++ b/smac_planner/test/test_costmap_downsampler.cpp @@ -13,13 +13,11 @@ // limitations under the License. Reserved. #include -#include #include #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" #include "nav2_costmap_2d/costmap_2d.hpp" -#include "nav2_costmap_2d/costmap_subscriber.hpp" #include "nav2_util/lifecycle_node.hpp" #include "smac_planner/costmap_downsampler.hpp" @@ -35,8 +33,7 @@ TEST(CostmapDownsampler, costmap_downsample_test) { nav2_util::LifecycleNode::SharedPtr node = std::make_shared( "CostmapDownsamplerTest"); - smac_planner::CostmapDownsampler downsampler(node); - auto map_sub = nav2_costmap_2d::CostmapSubscriber(node, "unused_topic"); + smac_planner::CostmapDownsampler downsampler; // create basic costmap nav2_costmap_2d::Costmap2D costmapA(10, 10, 0.05, 0.0, 0.0, 0); @@ -44,7 +41,7 @@ TEST(CostmapDownsampler, costmap_downsample_test) costmapA.setCost(5, 5, 50); // downsample it - downsampler.initialize("map", "unused_topic", &costmapA, 2); + downsampler.on_configure(node, "map", "unused_topic", &costmapA, 2); nav2_costmap_2d::Costmap2D * downsampledCostmapA = downsampler.downsample(2); // validate it @@ -57,10 +54,10 @@ TEST(CostmapDownsampler, costmap_downsample_test) nav2_costmap_2d::Costmap2D costmapB(4, 4, 0.10, 0.0, 0.0, 0); // downsample it - downsampler.initialize("map", "unused_topic", &costmapB, 4); - downsampler.activatePublisher(); + downsampler.on_configure(node, "map", "unused_topic", &costmapB, 4); + downsampler.on_activate(); nav2_costmap_2d::Costmap2D * downsampledCostmapB = downsampler.downsample(4); - downsampler.deactivatePublisher(); + downsampler.on_deactivate(); // validate size EXPECT_EQ(downsampledCostmapB->getSizeInCellsX(), 1u); From fcdc1d83fadc6ea19589f464486a27c6a70820e7 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 5 Apr 2021 16:13:04 -0700 Subject: [PATCH 10/14] bumping to 0.4.6 --- nav2_amcl/package.xml | 2 +- nav2_behavior_tree/package.xml | 2 +- nav2_bringup/bringup/package.xml | 2 +- nav2_bringup/nav2_gazebo_spawner/package.xml | 2 +- nav2_bt_navigator/package.xml | 2 +- nav2_common/package.xml | 2 +- nav2_controller/package.xml | 2 +- nav2_core/package.xml | 2 +- nav2_costmap_2d/package.xml | 2 +- nav2_dwb_controller/costmap_queue/package.xml | 2 +- nav2_dwb_controller/dwb_core/package.xml | 2 +- nav2_dwb_controller/dwb_critics/package.xml | 2 +- nav2_dwb_controller/dwb_msgs/package.xml | 2 +- nav2_dwb_controller/dwb_plugins/package.xml | 2 +- nav2_dwb_controller/nav2_dwb_controller/package.xml | 2 +- nav2_dwb_controller/nav_2d_msgs/package.xml | 2 +- nav2_dwb_controller/nav_2d_utils/package.xml | 2 +- nav2_lifecycle_manager/package.xml | 2 +- nav2_map_server/package.xml | 2 +- nav2_msgs/package.xml | 2 +- nav2_navfn_planner/package.xml | 2 +- nav2_planner/package.xml | 2 +- nav2_recoveries/package.xml | 2 +- nav2_regulated_pure_pursuit_controller/package.xml | 2 +- nav2_rviz_plugins/package.xml | 2 +- nav2_system_tests/package.xml | 2 +- nav2_util/package.xml | 2 +- nav2_voxel_grid/package.xml | 2 +- nav2_waypoint_follower/package.xml | 2 +- navigation2/package.xml | 2 +- smac_planner/package.xml | 2 +- 31 files changed, 31 insertions(+), 31 deletions(-) diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml index f22e3a42f4..1b64b6a694 100644 --- a/nav2_amcl/package.xml +++ b/nav2_amcl/package.xml @@ -2,7 +2,7 @@ nav2_amcl - 0.4.5 + 0.4.6

amcl is a probabilistic localization system for a robot moving in diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index 860a699034..5ab4e0387b 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -2,7 +2,7 @@ nav2_behavior_tree - 0.4.5 + 0.4.6 TODO Michael Jeronimo Carlos Orduno diff --git a/nav2_bringup/bringup/package.xml b/nav2_bringup/bringup/package.xml index 0a16ca2707..65e442c5b7 100644 --- a/nav2_bringup/bringup/package.xml +++ b/nav2_bringup/bringup/package.xml @@ -2,7 +2,7 @@ nav2_bringup - 0.4.5 + 0.4.6 Bringup scripts and configurations for the navigation2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_bringup/nav2_gazebo_spawner/package.xml b/nav2_bringup/nav2_gazebo_spawner/package.xml index 0032a5b847..8b14fba65e 100644 --- a/nav2_bringup/nav2_gazebo_spawner/package.xml +++ b/nav2_bringup/nav2_gazebo_spawner/package.xml @@ -2,7 +2,7 @@ nav2_gazebo_spawner - 0.4.5 + 0.4.6 Package for spawning a robot model into Gazebo for navigation2 lkumarbe lkumarbe diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml index 950ac6d9e6..e8fa15c88e 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -2,7 +2,7 @@ nav2_bt_navigator - 0.4.5 + 0.4.6 TODO Michael Jeronimo Apache-2.0 diff --git a/nav2_common/package.xml b/nav2_common/package.xml index 28e36f559a..5c98734e64 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -2,7 +2,7 @@ nav2_common - 0.4.5 + 0.4.6 Common support functionality used throughout the navigation 2 stack Carl Delsey Apache-2.0 diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index 93c2993f51..ebf32bd70f 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -2,7 +2,7 @@ nav2_controller - 0.4.5 + 0.4.6 Controller action interface Carl Delsey Apache-2.0 diff --git a/nav2_core/package.xml b/nav2_core/package.xml index 2b335e013a..7746a68f02 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -2,7 +2,7 @@ nav2_core - 0.4.5 + 0.4.6 A set of headers for plugins core to the navigation2 stack Steve Macenski Carl Delsey diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml index a4f899f9ee..55418b7f17 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -2,7 +2,7 @@ nav2_costmap_2d - 0.4.5 + 0.4.6 This package provides an implementation of a 2D costmap that takes in sensor data from the world, builds a 2D or 3D occupancy grid of the data (depending diff --git a/nav2_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml index f25f807f44..86b1e01d14 100644 --- a/nav2_dwb_controller/costmap_queue/package.xml +++ b/nav2_dwb_controller/costmap_queue/package.xml @@ -1,7 +1,7 @@ costmap_queue - 0.4.5 + 0.4.6 The costmap_queue package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_core/package.xml b/nav2_dwb_controller/dwb_core/package.xml index 0d77dac155..46a8771375 100644 --- a/nav2_dwb_controller/dwb_core/package.xml +++ b/nav2_dwb_controller/dwb_core/package.xml @@ -2,7 +2,7 @@ dwb_core - 0.4.5 + 0.4.6 TODO Carl Delsey BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_critics/package.xml b/nav2_dwb_controller/dwb_critics/package.xml index a0e474c728..bf3ab3e396 100644 --- a/nav2_dwb_controller/dwb_critics/package.xml +++ b/nav2_dwb_controller/dwb_critics/package.xml @@ -1,7 +1,7 @@ dwb_critics - 0.4.5 + 0.4.6 The dwb_critics package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_msgs/package.xml b/nav2_dwb_controller/dwb_msgs/package.xml index c0ee55637c..48ab814310 100644 --- a/nav2_dwb_controller/dwb_msgs/package.xml +++ b/nav2_dwb_controller/dwb_msgs/package.xml @@ -2,7 +2,7 @@ dwb_msgs - 0.4.5 + 0.4.6 Message/Service definitions specifically for the dwb_core David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_plugins/package.xml b/nav2_dwb_controller/dwb_plugins/package.xml index 348997568e..befb079cf2 100644 --- a/nav2_dwb_controller/dwb_plugins/package.xml +++ b/nav2_dwb_controller/dwb_plugins/package.xml @@ -1,7 +1,7 @@ dwb_plugins - 0.4.5 + 0.4.6 Standard implementations of the GoalChecker and TrajectoryGenerators for dwb_core diff --git a/nav2_dwb_controller/nav2_dwb_controller/package.xml b/nav2_dwb_controller/nav2_dwb_controller/package.xml index ba7901850c..20efeab7ca 100644 --- a/nav2_dwb_controller/nav2_dwb_controller/package.xml +++ b/nav2_dwb_controller/nav2_dwb_controller/package.xml @@ -2,7 +2,7 @@ nav2_dwb_controller - 0.4.5 + 0.4.6 ROS2 controller (DWB) metapackage diff --git a/nav2_dwb_controller/nav_2d_msgs/package.xml b/nav2_dwb_controller/nav_2d_msgs/package.xml index 291e7963fd..3f222dacc9 100644 --- a/nav2_dwb_controller/nav_2d_msgs/package.xml +++ b/nav2_dwb_controller/nav_2d_msgs/package.xml @@ -2,7 +2,7 @@ nav_2d_msgs - 0.4.5 + 0.4.6 Basic message types for two dimensional navigation, extending from geometry_msgs::Pose2D. David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/nav_2d_utils/package.xml b/nav2_dwb_controller/nav_2d_utils/package.xml index b3e6b16f43..65091db895 100644 --- a/nav2_dwb_controller/nav_2d_utils/package.xml +++ b/nav2_dwb_controller/nav_2d_utils/package.xml @@ -2,7 +2,7 @@ nav_2d_utils - 0.4.5 + 0.4.6 A handful of useful utility functions for nav_2d packages. David V. Lu!! BSD-3-Clause diff --git a/nav2_lifecycle_manager/package.xml b/nav2_lifecycle_manager/package.xml index 3eec000ae7..dcfe2e86e6 100644 --- a/nav2_lifecycle_manager/package.xml +++ b/nav2_lifecycle_manager/package.xml @@ -2,7 +2,7 @@ nav2_lifecycle_manager - 0.4.5 + 0.4.6 A controller/manager for the lifecycle nodes of the Navigation 2 system Michael Jeronimo Apache-2.0 diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml index c304ae5b5a..c22d79c42f 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -2,7 +2,7 @@ nav2_map_server - 0.4.5 + 0.4.6 Refactored map server for ROS2 Navigation diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index 979223f7ad..e5404314fd 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -2,7 +2,7 @@ nav2_msgs - 0.4.5 + 0.4.6 Messages and service files for the navigation2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_navfn_planner/package.xml b/nav2_navfn_planner/package.xml index ce103f0a2b..5b65d40866 100644 --- a/nav2_navfn_planner/package.xml +++ b/nav2_navfn_planner/package.xml @@ -2,7 +2,7 @@ nav2_navfn_planner - 0.4.5 + 0.4.6 TODO Steve Macenski Carlos Orduno diff --git a/nav2_planner/package.xml b/nav2_planner/package.xml index 1e6d33f02b..dcb2065e31 100644 --- a/nav2_planner/package.xml +++ b/nav2_planner/package.xml @@ -2,7 +2,7 @@ nav2_planner - 0.4.5 + 0.4.6 TODO Steve Macenski Apache-2.0 diff --git a/nav2_recoveries/package.xml b/nav2_recoveries/package.xml index a6c7a5aa0b..722659e98e 100644 --- a/nav2_recoveries/package.xml +++ b/nav2_recoveries/package.xml @@ -2,7 +2,7 @@ nav2_recoveries - 0.4.5 + 0.4.6 TODO Carlos Orduno Steve Macenski diff --git a/nav2_regulated_pure_pursuit_controller/package.xml b/nav2_regulated_pure_pursuit_controller/package.xml index 39a894477a..5357c9dda8 100644 --- a/nav2_regulated_pure_pursuit_controller/package.xml +++ b/nav2_regulated_pure_pursuit_controller/package.xml @@ -2,7 +2,7 @@ nav2_regulated_pure_pursuit_controller - 1.0.0 + 0.4.6 Regulated Pure Pursuit Controller Steve Macenski Shrijit Singh diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index e27a8aaec5..9352a853a8 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -2,7 +2,7 @@ nav2_rviz_plugins - 0.4.5 + 0.4.6 Navigation 2 plugins for rviz Michael Jeronimo Apache-2.0 diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index 27e1412add..5ee6f815be 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -2,7 +2,7 @@ nav2_system_tests - 0.4.5 + 0.4.6 TODO Carlos Orduno Apache-2.0 diff --git a/nav2_util/package.xml b/nav2_util/package.xml index 5f443d0e0d..19a6d18c6e 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -2,7 +2,7 @@ nav2_util - 0.4.5 + 0.4.6 TODO Michael Jeronimo Mohammad Haghighipanah diff --git a/nav2_voxel_grid/package.xml b/nav2_voxel_grid/package.xml index 29cdcc7fcb..4f2fd329b1 100644 --- a/nav2_voxel_grid/package.xml +++ b/nav2_voxel_grid/package.xml @@ -2,7 +2,7 @@ nav2_voxel_grid - 0.4.5 + 0.4.6 voxel_grid provides an implementation of an efficient 3D voxel grid. The occupancy grid can support 3 different representations for the state of a cell: marked, free, or unknown. Due to the underlying implementation relying on bitwise and and or integer operations, the voxel grid only supports 16 different levels per voxel column. However, this limitation yields raytracing and cell marking performance in the grid comparable to standard 2D structures making it quite fast compared to most 3D structures. diff --git a/nav2_waypoint_follower/package.xml b/nav2_waypoint_follower/package.xml index 676846b4d6..3b07a61a1a 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -2,7 +2,7 @@ nav2_waypoint_follower - 0.4.5 + 0.4.6 A waypoint follower navigation server Steve Macenski Apache-2.0 diff --git a/navigation2/package.xml b/navigation2/package.xml index 5169c4471f..05eb0e0c95 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -2,7 +2,7 @@ navigation2 - 0.4.5 + 0.4.6 ROS2 Navigation Stack diff --git a/smac_planner/package.xml b/smac_planner/package.xml index 14a04772e6..ca96ed5cfa 100644 --- a/smac_planner/package.xml +++ b/smac_planner/package.xml @@ -2,7 +2,7 @@ smac_planner - 0.4.5 + 0.4.6 Smac global planning plugin Steve Macenski Apache-2.0 From a947ab5656e46ae3aa1afdf95e1706a16e478974 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 5 Apr 2021 18:15:13 -0700 Subject: [PATCH 11/14] bump to 0.4.7 and add missing dep to RPP --- nav2_amcl/package.xml | 2 +- nav2_behavior_tree/package.xml | 2 +- nav2_bringup/bringup/package.xml | 2 +- nav2_bringup/nav2_gazebo_spawner/package.xml | 2 +- nav2_bt_navigator/package.xml | 2 +- nav2_common/package.xml | 2 +- nav2_controller/package.xml | 2 +- nav2_core/package.xml | 2 +- nav2_costmap_2d/package.xml | 2 +- nav2_dwb_controller/costmap_queue/package.xml | 2 +- nav2_dwb_controller/dwb_core/package.xml | 2 +- nav2_dwb_controller/dwb_critics/package.xml | 2 +- nav2_dwb_controller/dwb_msgs/package.xml | 2 +- nav2_dwb_controller/dwb_plugins/package.xml | 2 +- nav2_dwb_controller/nav2_dwb_controller/package.xml | 2 +- nav2_dwb_controller/nav_2d_msgs/package.xml | 2 +- nav2_dwb_controller/nav_2d_utils/package.xml | 2 +- nav2_lifecycle_manager/package.xml | 2 +- nav2_map_server/package.xml | 2 +- nav2_msgs/package.xml | 2 +- nav2_navfn_planner/package.xml | 2 +- nav2_planner/package.xml | 2 +- nav2_recoveries/package.xml | 2 +- nav2_regulated_pure_pursuit_controller/package.xml | 4 +++- nav2_rviz_plugins/package.xml | 2 +- nav2_system_tests/package.xml | 2 +- nav2_util/package.xml | 2 +- nav2_voxel_grid/package.xml | 2 +- nav2_waypoint_follower/package.xml | 2 +- navigation2/package.xml | 2 +- smac_planner/package.xml | 2 +- 31 files changed, 33 insertions(+), 31 deletions(-) diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml index 1b64b6a694..ff27677865 100644 --- a/nav2_amcl/package.xml +++ b/nav2_amcl/package.xml @@ -2,7 +2,7 @@ nav2_amcl - 0.4.6 + 0.4.7

amcl is a probabilistic localization system for a robot moving in diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index 5ab4e0387b..604c3b5c28 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -2,7 +2,7 @@ nav2_behavior_tree - 0.4.6 + 0.4.7 TODO Michael Jeronimo Carlos Orduno diff --git a/nav2_bringup/bringup/package.xml b/nav2_bringup/bringup/package.xml index 65e442c5b7..d34bd54d0a 100644 --- a/nav2_bringup/bringup/package.xml +++ b/nav2_bringup/bringup/package.xml @@ -2,7 +2,7 @@ nav2_bringup - 0.4.6 + 0.4.7 Bringup scripts and configurations for the navigation2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_bringup/nav2_gazebo_spawner/package.xml b/nav2_bringup/nav2_gazebo_spawner/package.xml index 8b14fba65e..c69941fb2a 100644 --- a/nav2_bringup/nav2_gazebo_spawner/package.xml +++ b/nav2_bringup/nav2_gazebo_spawner/package.xml @@ -2,7 +2,7 @@ nav2_gazebo_spawner - 0.4.6 + 0.4.7 Package for spawning a robot model into Gazebo for navigation2 lkumarbe lkumarbe diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml index e8fa15c88e..3528045247 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -2,7 +2,7 @@ nav2_bt_navigator - 0.4.6 + 0.4.7 TODO Michael Jeronimo Apache-2.0 diff --git a/nav2_common/package.xml b/nav2_common/package.xml index 5c98734e64..753275f3bd 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -2,7 +2,7 @@ nav2_common - 0.4.6 + 0.4.7 Common support functionality used throughout the navigation 2 stack Carl Delsey Apache-2.0 diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index ebf32bd70f..0e38b5994d 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -2,7 +2,7 @@ nav2_controller - 0.4.6 + 0.4.7 Controller action interface Carl Delsey Apache-2.0 diff --git a/nav2_core/package.xml b/nav2_core/package.xml index 7746a68f02..abe8c47870 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -2,7 +2,7 @@ nav2_core - 0.4.6 + 0.4.7 A set of headers for plugins core to the navigation2 stack Steve Macenski Carl Delsey diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml index 55418b7f17..5688118923 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -2,7 +2,7 @@ nav2_costmap_2d - 0.4.6 + 0.4.7 This package provides an implementation of a 2D costmap that takes in sensor data from the world, builds a 2D or 3D occupancy grid of the data (depending diff --git a/nav2_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml index 86b1e01d14..23a2029c47 100644 --- a/nav2_dwb_controller/costmap_queue/package.xml +++ b/nav2_dwb_controller/costmap_queue/package.xml @@ -1,7 +1,7 @@ costmap_queue - 0.4.6 + 0.4.7 The costmap_queue package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_core/package.xml b/nav2_dwb_controller/dwb_core/package.xml index 46a8771375..fbb064d070 100644 --- a/nav2_dwb_controller/dwb_core/package.xml +++ b/nav2_dwb_controller/dwb_core/package.xml @@ -2,7 +2,7 @@ dwb_core - 0.4.6 + 0.4.7 TODO Carl Delsey BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_critics/package.xml b/nav2_dwb_controller/dwb_critics/package.xml index bf3ab3e396..6b121ecd50 100644 --- a/nav2_dwb_controller/dwb_critics/package.xml +++ b/nav2_dwb_controller/dwb_critics/package.xml @@ -1,7 +1,7 @@ dwb_critics - 0.4.6 + 0.4.7 The dwb_critics package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_msgs/package.xml b/nav2_dwb_controller/dwb_msgs/package.xml index 48ab814310..057b293335 100644 --- a/nav2_dwb_controller/dwb_msgs/package.xml +++ b/nav2_dwb_controller/dwb_msgs/package.xml @@ -2,7 +2,7 @@ dwb_msgs - 0.4.6 + 0.4.7 Message/Service definitions specifically for the dwb_core David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_plugins/package.xml b/nav2_dwb_controller/dwb_plugins/package.xml index befb079cf2..f425fc26aa 100644 --- a/nav2_dwb_controller/dwb_plugins/package.xml +++ b/nav2_dwb_controller/dwb_plugins/package.xml @@ -1,7 +1,7 @@ dwb_plugins - 0.4.6 + 0.4.7 Standard implementations of the GoalChecker and TrajectoryGenerators for dwb_core diff --git a/nav2_dwb_controller/nav2_dwb_controller/package.xml b/nav2_dwb_controller/nav2_dwb_controller/package.xml index 20efeab7ca..1ae96ea402 100644 --- a/nav2_dwb_controller/nav2_dwb_controller/package.xml +++ b/nav2_dwb_controller/nav2_dwb_controller/package.xml @@ -2,7 +2,7 @@ nav2_dwb_controller - 0.4.6 + 0.4.7 ROS2 controller (DWB) metapackage diff --git a/nav2_dwb_controller/nav_2d_msgs/package.xml b/nav2_dwb_controller/nav_2d_msgs/package.xml index 3f222dacc9..3579746eaf 100644 --- a/nav2_dwb_controller/nav_2d_msgs/package.xml +++ b/nav2_dwb_controller/nav_2d_msgs/package.xml @@ -2,7 +2,7 @@ nav_2d_msgs - 0.4.6 + 0.4.7 Basic message types for two dimensional navigation, extending from geometry_msgs::Pose2D. David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/nav_2d_utils/package.xml b/nav2_dwb_controller/nav_2d_utils/package.xml index 65091db895..75d5cda8bb 100644 --- a/nav2_dwb_controller/nav_2d_utils/package.xml +++ b/nav2_dwb_controller/nav_2d_utils/package.xml @@ -2,7 +2,7 @@ nav_2d_utils - 0.4.6 + 0.4.7 A handful of useful utility functions for nav_2d packages. David V. Lu!! BSD-3-Clause diff --git a/nav2_lifecycle_manager/package.xml b/nav2_lifecycle_manager/package.xml index dcfe2e86e6..d56290206e 100644 --- a/nav2_lifecycle_manager/package.xml +++ b/nav2_lifecycle_manager/package.xml @@ -2,7 +2,7 @@ nav2_lifecycle_manager - 0.4.6 + 0.4.7 A controller/manager for the lifecycle nodes of the Navigation 2 system Michael Jeronimo Apache-2.0 diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml index c22d79c42f..c66428fb35 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -2,7 +2,7 @@ nav2_map_server - 0.4.6 + 0.4.7 Refactored map server for ROS2 Navigation diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index e5404314fd..6d2a6f2f35 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -2,7 +2,7 @@ nav2_msgs - 0.4.6 + 0.4.7 Messages and service files for the navigation2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_navfn_planner/package.xml b/nav2_navfn_planner/package.xml index 5b65d40866..bf52cb0e11 100644 --- a/nav2_navfn_planner/package.xml +++ b/nav2_navfn_planner/package.xml @@ -2,7 +2,7 @@ nav2_navfn_planner - 0.4.6 + 0.4.7 TODO Steve Macenski Carlos Orduno diff --git a/nav2_planner/package.xml b/nav2_planner/package.xml index dcb2065e31..e762c67d0e 100644 --- a/nav2_planner/package.xml +++ b/nav2_planner/package.xml @@ -2,7 +2,7 @@ nav2_planner - 0.4.6 + 0.4.7 TODO Steve Macenski Apache-2.0 diff --git a/nav2_recoveries/package.xml b/nav2_recoveries/package.xml index 722659e98e..62f2f79540 100644 --- a/nav2_recoveries/package.xml +++ b/nav2_recoveries/package.xml @@ -2,7 +2,7 @@ nav2_recoveries - 0.4.6 + 0.4.7 TODO Carlos Orduno Steve Macenski diff --git a/nav2_regulated_pure_pursuit_controller/package.xml b/nav2_regulated_pure_pursuit_controller/package.xml index 5357c9dda8..14c4e1c1d9 100644 --- a/nav2_regulated_pure_pursuit_controller/package.xml +++ b/nav2_regulated_pure_pursuit_controller/package.xml @@ -2,7 +2,7 @@ nav2_regulated_pure_pursuit_controller - 0.4.6 + 0.4.7 Regulated Pure Pursuit Controller Steve Macenski Shrijit Singh @@ -21,6 +21,8 @@ tf2 ament_cmake_gtest + ament_lint_common + ament_lint_auto ament_cmake diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index 9352a853a8..388cd4345c 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -2,7 +2,7 @@ nav2_rviz_plugins - 0.4.6 + 0.4.7 Navigation 2 plugins for rviz Michael Jeronimo Apache-2.0 diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index 5ee6f815be..01a41e39d0 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -2,7 +2,7 @@ nav2_system_tests - 0.4.6 + 0.4.7 TODO Carlos Orduno Apache-2.0 diff --git a/nav2_util/package.xml b/nav2_util/package.xml index 19a6d18c6e..9a645cbd70 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -2,7 +2,7 @@ nav2_util - 0.4.6 + 0.4.7 TODO Michael Jeronimo Mohammad Haghighipanah diff --git a/nav2_voxel_grid/package.xml b/nav2_voxel_grid/package.xml index 4f2fd329b1..7eaccca6e4 100644 --- a/nav2_voxel_grid/package.xml +++ b/nav2_voxel_grid/package.xml @@ -2,7 +2,7 @@ nav2_voxel_grid - 0.4.6 + 0.4.7 voxel_grid provides an implementation of an efficient 3D voxel grid. The occupancy grid can support 3 different representations for the state of a cell: marked, free, or unknown. Due to the underlying implementation relying on bitwise and and or integer operations, the voxel grid only supports 16 different levels per voxel column. However, this limitation yields raytracing and cell marking performance in the grid comparable to standard 2D structures making it quite fast compared to most 3D structures. diff --git a/nav2_waypoint_follower/package.xml b/nav2_waypoint_follower/package.xml index 3b07a61a1a..7da90b5ed9 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -2,7 +2,7 @@ nav2_waypoint_follower - 0.4.6 + 0.4.7 A waypoint follower navigation server Steve Macenski Apache-2.0 diff --git a/navigation2/package.xml b/navigation2/package.xml index 05eb0e0c95..8b6811af54 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -2,7 +2,7 @@ navigation2 - 0.4.6 + 0.4.7 ROS2 Navigation Stack diff --git a/smac_planner/package.xml b/smac_planner/package.xml index ca96ed5cfa..34d0e42ec9 100644 --- a/smac_planner/package.xml +++ b/smac_planner/package.xml @@ -2,7 +2,7 @@ smac_planner - 0.4.6 + 0.4.7 Smac global planning plugin Steve Macenski Apache-2.0 From 2590238de036513c96c6fd2c76884c7599b72a01 Mon Sep 17 00:00:00 2001 From: Jovian Dsouza Date: Tue, 6 Apr 2021 23:03:10 +0530 Subject: [PATCH 12/14] [nav2_bringup] Update waffle.model (#2296) Changed to the latest tire mesh file names for waffle as per the latest `turtlebot3_gazebo` package. This results in faster loading and resolves the errors that come in `gazebo --verbose` --- nav2_bringup/bringup/worlds/waffle.model | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_bringup/bringup/worlds/waffle.model b/nav2_bringup/bringup/worlds/waffle.model index 74af1c7670..bc984be6c4 100644 --- a/nav2_bringup/bringup/worlds/waffle.model +++ b/nav2_bringup/bringup/worlds/waffle.model @@ -266,7 +266,7 @@ 0.0 0.144 0.023 0 0 0 - model://turtlebot3_waffle/meshes/left_tire.dae + model://turtlebot3_waffle/meshes/tire.dae 0.001 0.001 0.001 @@ -324,7 +324,7 @@ 0.0 -0.144 0.023 0 0 0 - model://turtlebot3_waffle/meshes/right_tire.dae + model://turtlebot3_waffle/meshes/tire.dae 0.001 0.001 0.001 From 2e18dd1cae31807f68746eb1a2c702526a34f4cc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pau=20Carr=C3=A9=20Cardona?= Date: Mon, 3 May 2021 19:24:31 +0200 Subject: [PATCH 13/14] Update list of behavioural tree nodes (#2329) * Update list of nodes with nodes compiled in the branch and excluding unexistant to prevent runtime exceptions. * Updated documentation Co-authored-by: Pau Carre --- doc/parameters/param_list.md | 2 +- nav2_bt_navigator/src/bt_navigator.cpp | 8 ++++++-- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/doc/parameters/param_list.md b/doc/parameters/param_list.md index 9278a512a5..ada0e743f9 100644 --- a/doc/parameters/param_list.md +++ b/doc/parameters/param_list.md @@ -8,7 +8,7 @@ | enable_groot_monitoring | True | enable Groot live monitoring of the behavior tree | | groot_zmq_publisher_port | 1666 | change port of the zmq publisher needed for groot | | groot_zmq_server_port | 1667 | change port of the zmq server needed for groot | -| plugin_lib_names | ["nav2_compute_path_to_pose_action_bt_node", "nav2_follow_path_action_bt_node", "nav2_back_up_action_bt_node", "nav2_spin_action_bt_node", "nav2_wait_action_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_reinitialize_global_localization_service_bt_node", "nav2_rate_controller_bt_node", "nav2_distance_controller_bt_node", "nav2_recovery_node_bt_node", "nav2_pipeline_sequence_bt_node", "nav2_round_robin_node_bt_node", "nav2_transform_available_condition_bt_node"] | list of behavior tree node shared libraries | +| plugin_lib_names | ["nav2_compute_path_to_pose_action_bt_node", "nav2_follow_path_action_bt_node", "nav2_back_up_action_bt_node", "nav2_spin_action_bt_node", "nav2_wait_action_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_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_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_rotate_action_bt_node", "nav2_translate_action_bt_node", "nav2_is_battery_low_condition_bt_node", "nav2_goal_updater_node_bt_node", "nav2_navigate_to_pose_action_bt_node"] | list of behavior tree node shared libraries | | transform_tolerance | 0.1 | TF transform tolerance | | global_frame | "map" | Reference frame | | robot_base_frame | "base_link" | Robot base frame | diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index afd20df0e6..e44e64887e 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -53,13 +53,17 @@ BtNavigator::BtNavigator() "nav2_distance_controller_bt_node", "nav2_speed_controller_bt_node", "nav2_truncate_path_action_bt_node", - "nav2_change_goal_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_distance_traveled_condition_bt_node", + "nav2_rotate_action_bt_node", + "nav2_translate_action_bt_node", + "nav2_is_battery_low_condition_bt_node", + "nav2_goal_updater_node_bt_node", + "nav2_navigate_to_pose_action_bt_node", }; // Declare this node's parameters From 1160100d9689bea4c149031bb82bf7d182ec0645 Mon Sep 17 00:00:00 2001 From: Nisala Kalupahana Date: Tue, 8 Jun 2021 10:37:20 -0700 Subject: [PATCH 14/14] Moves changes from PR #2083 to foxy (#2375) * fix basename not defined issue * macos fixes * removed observationbuffer change * Update regulated_pure_pursuit_controller.cpp Fixes error: moving a temporary object prevents copy elision --- nav2_amcl/src/amcl_node.cpp | 4 ++-- .../include/nav2_behavior_tree/bt_action_node.hpp | 8 ++++---- .../include/nav2_behavior_tree/bt_service_node.hpp | 8 ++++---- nav2_bt_navigator/src/ros_topic_logger.cpp | 5 ----- nav2_costmap_2d/include/nav2_costmap_2d/observation.hpp | 5 +++++ nav2_recoveries/include/nav2_recoveries/recovery.hpp | 2 +- .../src/regulated_pure_pursuit_controller.cpp | 2 +- nav2_util/src/dump_params.cpp | 1 + nav2_waypoint_follower/package.xml | 3 +-- smac_planner/src/a_star.cpp | 2 +- 10 files changed, 20 insertions(+), 20 deletions(-) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 9ea4a9b80c..37da4a8253 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -401,7 +401,7 @@ AmclNode::checkLaserReceived() RCLCPP_WARN( get_logger(), "Laser scan has not been received" " (and thus no pose updates have been published)." - " Verify that data is being published on the %s topic.", scan_topic_); + " Verify that data is being published on the %s topic.", scan_topic_.c_str()); return; } @@ -410,7 +410,7 @@ AmclNode::checkLaserReceived() RCLCPP_WARN( get_logger(), "No laser scan received (and thus no pose updates have been published) for %f" " seconds. Verify that data is being published on the %s topic.", - d.nanoseconds() * 1e-9, scan_topic_); + d.nanoseconds() * 1e-9, scan_topic_.c_str()); } } 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 eed3626601..77aa8836d4 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 @@ -36,11 +36,11 @@ class BtActionNode : public BT::ActionNodeBase const BT::NodeConfiguration & conf) : BT::ActionNodeBase(xml_tag_name, conf), action_name_(action_name) { - node_ = config().blackboard->get("node"); + node_ = config().blackboard->template get("node"); // Get the required items from the blackboard server_timeout_ = - config().blackboard->get("server_timeout"); + config().blackboard->template get("server_timeout"); getInput("server_timeout", server_timeout_); // Initialize the input and output messages @@ -245,9 +245,9 @@ class BtActionNode : public BT::ActionNodeBase void increment_recovery_count() { int recovery_count = 0; - config().blackboard->get("number_recoveries", recovery_count); // NOLINT + config().blackboard->template get("number_recoveries", recovery_count); // NOLINT recovery_count += 1; - config().blackboard->set("number_recoveries", recovery_count); // NOLINT + config().blackboard->template set("number_recoveries", recovery_count); // NOLINT } std::string action_name_; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp index 2b7b16bfb9..8931783df6 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp @@ -35,11 +35,11 @@ class BtServiceNode : public BT::SyncActionNode const BT::NodeConfiguration & conf) : BT::SyncActionNode(service_node_name, conf), service_node_name_(service_node_name) { - node_ = config().blackboard->get("node"); + node_ = config().blackboard->template get("node"); // Get the required items from the blackboard server_timeout_ = - config().blackboard->get("server_timeout"); + config().blackboard->template get("server_timeout"); getInput("server_timeout", server_timeout_); // Now that we have node_ to use, create the service client for this BT service @@ -126,9 +126,9 @@ class BtServiceNode : public BT::SyncActionNode void increment_recovery_count() { int recovery_count = 0; - config().blackboard->get("number_recoveries", recovery_count); // NOLINT + config().blackboard->template get("number_recoveries", recovery_count); // NOLINT recovery_count += 1; - config().blackboard->set("number_recoveries", recovery_count); // NOLINT + config().blackboard->template set("number_recoveries", recovery_count); // NOLINT } std::string service_name_, service_node_name_; diff --git a/nav2_bt_navigator/src/ros_topic_logger.cpp b/nav2_bt_navigator/src/ros_topic_logger.cpp index 48bffd20c8..5b248014e8 100644 --- a/nav2_bt_navigator/src/ros_topic_logger.cpp +++ b/nav2_bt_navigator/src/ros_topic_logger.cpp @@ -39,12 +39,7 @@ void RosTopicLogger::callback( // BT timestamps are a duration since the epoch. Need to convert to a time_point // before converting to a msg. -#ifndef _WIN32 - event.timestamp = - tf2_ros::toMsg(std::chrono::time_point(timestamp)); -#else event.timestamp = tf2_ros::toMsg(tf2::TimePoint(timestamp)); -#endif event.node_name = node.name(); event.previous_status = toStr(prev_status, false); event.current_status = toStr(status, false); diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/observation.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/observation.hpp index dbbfb2caa9..e319502078 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/observation.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/observation.hpp @@ -59,6 +59,11 @@ class Observation delete cloud_; } + /** + * @brief Explicitly define copy assignment operator for Observation as it has a user-declared destructor + */ + Observation & operator=(const Observation &) = default; + /** * @brief Creates an observation from an origin point and a point cloud * @param origin The origin point of the observation diff --git a/nav2_recoveries/include/nav2_recoveries/recovery.hpp b/nav2_recoveries/include/nav2_recoveries/recovery.hpp index 5ba051b16b..7edb732d00 100644 --- a/nav2_recoveries/include/nav2_recoveries/recovery.hpp +++ b/nav2_recoveries/include/nav2_recoveries/recovery.hpp @@ -107,7 +107,7 @@ class Recovery : public nav2_core::Recovery collision_checker_ = collision_checker; - vel_pub_ = node_->create_publisher("cmd_vel", 1); + vel_pub_ = node_->template create_publisher("cmd_vel", 1); onConfigure(); } 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 f5017ca058..b8130f37e7 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 @@ -229,7 +229,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity // Find look ahead distance and point on path and publish const double lookahead_dist = getLookAheadDistance(speed); auto carrot_pose = getLookAheadPoint(lookahead_dist, transformed_plan); - carrot_pub_->publish(std::move(createCarrotMsg(carrot_pose))); + carrot_pub_->publish(createCarrotMsg(carrot_pose)); double linear_vel, angular_vel; diff --git a/nav2_util/src/dump_params.cpp b/nav2_util/src/dump_params.cpp index a29a21c37b..259479f1f6 100644 --- a/nav2_util/src/dump_params.cpp +++ b/nav2_util/src/dump_params.cpp @@ -14,6 +14,7 @@ #include #include +#include #include #include #include diff --git a/nav2_waypoint_follower/package.xml b/nav2_waypoint_follower/package.xml index 7da90b5ed9..a8aa4bb935 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -7,8 +7,6 @@ Steve Macenski Apache-2.0 - tf2_ros - ament_cmake nav2_common rclcpp @@ -17,6 +15,7 @@ nav_msgs nav2_msgs nav2_util + tf2_ros ament_lint_common ament_lint_auto diff --git a/smac_planner/src/a_star.cpp b/smac_planner/src/a_star.cpp index b9668810df..a8432b6c47 100644 --- a/smac_planner/src/a_star.cpp +++ b/smac_planner/src/a_star.cpp @@ -352,7 +352,7 @@ AStarAlgorithm::NodePtr AStarAlgorithm::getAnalyticPath( float d = node->motion_table.state_space->distance(from(), to()); NodePtr prev(node); // A move of sqrt(2) is guaranteed to be in a new cell - constexpr float sqrt_2 = std::sqrt(2.); + static const float sqrt_2 = std::sqrt(2.); unsigned int num_intervals = std::floor(d / sqrt_2); using PossibleNode = std::pair;