From e1b1f24fed16aede76af1c70f5b52ce4e6d019bb Mon Sep 17 00:00:00 2001 From: stevedan Date: Wed, 14 Feb 2024 02:47:22 +0100 Subject: [PATCH 01/41] Include goal heading mode to allow bidirectional and all direction goals MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: stevedan New MPPI Cost Critic (Contrib: Brice Renaudeau) (#4090) * Share code Signed-off-by: Brice * Update inflation_cost_critic.hpp - copyright - ifndef Signed-off-by: Brice * fix lint cpp - extra space Signed-off-by: Brice * Fix Smac Planner confined collision checker (#4055) * Update collision_checker.cpp Signed-off-by: Steve Macenski * Fix tests Signed-off-by: Steve Macenski * Update test_a_star.cpp Signed-off-by: Steve Macenski --------- Signed-off-by: Brice * Prevent analytic expansions from shortcutting Smac Planner feasible paths (#3962) * a potential solution to smac shortcutting * costmap reoslution * some fixes * completed prototype * some fixes for collision detection and performance * completing shortcutting fix * updating tests * adding readme --------- Signed-off-by: Steve Macenski Signed-off-by: Brice * change pointer free order in amcl to avoid use-after-free bug mentioned in #4068 (#4070) Signed-off-by: GoesM Co-authored-by: GoesM Signed-off-by: Brice * [Smac Planner] Massive Improvement of Behavior for SE2 Footprint Checking (ie non-circular robots) In Confined Settings (#4067) * prototype to test SE2 footprint H improvements * some fixes * fixed * invert logic * Working final prototype to be tested * complete unit test conversions * Update inflation_layer.hpp Signed-off-by: Steve Macenski --------- Signed-off-by: Steve Macenski Signed-off-by: Brice * Adding new Smac paper to readme Signed-off-by: Steve Macenski Signed-off-by: Brice * Update README.md Signed-off-by: Steve Macenski Signed-off-by: Brice * [behavior_tree] don't repeat yourself in "blackboard->set" (#4074) * don't repeat yourself: templates in tests Signed-off-by: Davide Faconti * misse change Signed-off-by: Davide Faconti --------- Signed-off-by: Davide Faconti Signed-off-by: Brice * Allow path end pose deviation revive (#4065) * Support stitching paths in compute path to poses * Update nav2_planner/src/planner_server.cpp Co-authored-by: Steve Macenski * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg Co-authored-by: pepisg Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Steve Macenski Signed-off-by: Brice * Updated code to use getInflationLayer() method (#4076) * updated code to use getInflationLayer method Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> * Fix linting Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> --------- Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Brice * 1594 twist stamped publisher (#4077) * Add TwistStamped to controller_server via TwistPublisher util * Add a new util class for publishing either Twist or TwistStamped * Add a new parameter for selecting to stamp the twist data * Consume TwistPublisher in nav2_controller Signed-off-by: Ryan Friedman * Fix small issues * Unused variable * Incorrect doxygen Signed-off-by: Ryan Friedman * Remove stored node and assert Signed-off-by: Ryan Friedman * Add tests for node * Facing timeout even though it does the same thing as velocity smoother test Signed-off-by: Ryan Friedman * Add missing spin call to solve timeout Signed-off-by: Ryan Friedman * Fix copyright (me instead of intel) Signed-off-by: Ryan Friedman * Add full test coverage with subscriber Signed-off-by: Ryan Friedman * Remove unused rclcpp fixture * Can't use it due to needing to join the pub thread after rclcpp shuts down Signed-off-by: Ryan Friedman * Use TwistStamped in nav2_behaviors Signed-off-by: Ryan Friedman * Use TwistStamped in collision monitor node Signed-off-by: Ryan Friedman * Add TwistStamped readme updates to velocity smoother Signed-off-by: Ryan Friedman * Add TwistSubscriber implementation Signed-off-by: Ryan Friedman * Fix syntax errors Signed-off-by: Ryan Friedman * Use TwistSubscriber in test_velocity_smoother Signed-off-by: Ryan Friedman * Use TwistSubscriber in assisted_teleop Signed-off-by: Ryan Friedman * Use TwistSubscriber in collision monitor node Signed-off-by: Ryan Friedman * Use TwistSubscriber in velocity smoother Signed-off-by: Ryan Friedman * Remove unused code Signed-off-by: Ryan Friedman * add timestamp and frame_id to TwistStamped message * Add missing utility include Signed-off-by: Ryan Friedman * Document TwistPublisher and TwistSubscriber usage Signed-off-by: Ryan Friedman * Use pass-by-reference * Instead of std::move(std::unique_ptr) Signed-off-by: Ryan Friedman * Finish twist subscriber tests Signed-off-by: Ryan Friedman * Add other constructor and docs Signed-off-by: Ryan Friedman * Fix linter issues Signed-off-by: Ryan Friedman * Manually fix paren alignment Signed-off-by: Ryan Friedman * Remove GSoC reference Signed-off-by: Ryan Friedman * Document twist bool param in README Signed-off-by: Ryan Friedman * Handle twistPublisher in collision monitor * Implement behavior in the stamped callback * Unstamped callback calls the stamped callback * Switch to unique pointer for publisher Signed-off-by: Ryan Friedman * Convert to using TwistStamped interally * Use incoming twistStamped timestamp if available * Convert all internal representations to use TwistStamped Signed-off-by: Ryan Friedman * Remove nav2_util usage instructions Signed-off-by: Ryan Friedman * Remove unused Twist only subscriber Signed-off-by: Ryan Friedman * More linter fixes Signed-off-by: Ryan Friedman * Prefer working with unique_ptr for cmd_vel * This makes it easier to switch to std::move instead of dereference on publish Signed-off-by: Ryan Friedman * Completing twist stamped migration * shared to unique ptr Signed-off-by: Steve Macenski * twist add stamps and properly propogated * nav2_util: fix for compiling with clang - Resolve error: moving a temporary object prevents copy elision [-Werror,-Wpessimizing-move] Signed-off-by: Rhys Mainwaring --------- Signed-off-by: Ryan Friedman Signed-off-by: Steve Macenski Signed-off-by: Rhys Mainwaring Co-authored-by: pedro-fuoco Co-authored-by: Steve Macenski Co-authored-by: Rhys Mainwaring Signed-off-by: Brice * Change costmap_queue to shared library (#4072) Signed-off-by: cybaol Signed-off-by: Brice * fix include of hpp Signed-off-by: Brice Renaudeau * inflation cost optmiizations and cleanu * rename, add defaults, and docs * smoke test addition * lintg * normalize weight * update readme * increment cache * Update cost_critic.hpp Signed-off-by: Steve Macenski * Update cost_critic.cpp Signed-off-by: Steve Macenski --------- Signed-off-by: Brice Signed-off-by: Steve Macenski Signed-off-by: GoesM Signed-off-by: Davide Faconti Signed-off-by: gg Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Ryan Friedman Signed-off-by: Rhys Mainwaring Signed-off-by: cybaol Signed-off-by: Brice Renaudeau Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: GoesM Co-authored-by: Davide Faconti Co-authored-by: Joshua Wallace Co-authored-by: pepisg Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: jncfa <20467009+jncfa@users.noreply.github.com> Co-authored-by: Ryan Co-authored-by: pedro-fuoco Co-authored-by: Rhys Mainwaring Co-authored-by: Kino [behavior_tree] support both ports and blackboard (#4060) * check result of blackboard->get and use halTree Signed-off-by: Davide Faconti * remove unused header Signed-off-by: Davide Faconti * BT: add port inputs when missing and use getInputPortOrBlackboard Signed-off-by: Davide Faconti * add description Signed-off-by: Davide Faconti * change return value of getInputPortOrBlackboard Signed-off-by: Davide Faconti * updated tree XML Signed-off-by: Davide Faconti --------- Signed-off-by: Davide Faconti Use ament_export_targets for all targets (#4112) * Matches new internal ALIAS targets * Use ALIAS targets for all internal linkage * Remove unnecessary calls to ament_target_dependencies in test code * Export includes in proper folders for overlays in colcon Signed-off-by: Ryan Friedman adding new Friends of Nav2 Sponsors! Update README.md (#4114) Signed-off-by: Steve Macenski CI green P3 (#4117) Refactors `possible_inscribed_cost` to `possible_circumscribed_cost` in collision checker for smac_planner and mppi_controller (#4113). (#4118) * Utilizes circumscribed radius to assess potential collision points on the robot. * Ensures safety when the robot's center is outside the circumscribed radius, eliminating additional checks and optimizing CPU usage. Signed-off-by: Alan Xue Added cast to float to getClosestAngularBin return for behavior consistency (#4123) * Added cast to float to getClosestAngularBin return for behavior consistency. Signed-off-by: Hunter Song * Revised test name Signed-off-by: Hunter Song --------- Signed-off-by: Hunter Song Adding context to MPPI readme Signed-off-by: Steve Macenski [behavior_tree] Add generate_nav2_tree_nodes_xml (#4073) * Add generate_nav2_tree_nodes_xml Signed-off-by: Davide Faconti * behavior_tree: removed list of plugins from yaml Signed-off-by: Davide Faconti * check result of get_parameter(plugin_lib_names) Signed-off-by: Davide Faconti * fix previous commit (uncrustify) Signed-off-by: Davide Faconti * revert change Signed-off-by: Davide Faconti --------- Signed-off-by: Davide Faconti Fix compilation with clang (#4131) Signed-off-by: Ramon Wijnands code improvement based on feedback. Name changes and minor code improvement Signed-off-by: stevedan Fix goal handle was not freed correctly (#4137) * Fix goal handle was not freed correctly * Update nav2_util/include/nav2_util/simple_action_server.hpp Signed-off-by: Steve Macenski --------- Signed-off-by: Steve Macenski Co-authored-by: Ziohang Co-authored-by: Steve Macenski Adding Nvidia as Gold Sponsor! (#4144) Prevent a possible segmentation fault (#4141) (#4147) * Prevent a possible segmentation fault Signed-off-by: Joni Pöllänen * Cleanup Signed-off-by: Joni Pöllänen --------- Signed-off-by: Joni Pöllänen Updating stereolabs logo (#4158) Update README.md Signed-off-by: Steve Macenski behavior_tree: migration to BT.CPP 4.5.x (#4128) * behavior_tree: migration to BT.CPP 4.5.x Signed-off-by: Davide Faconti * fix formatting Signed-off-by: Davide Faconti * fix lint warning Signed-off-by: Davide Faconti * remove setStatus(BT::NodeStatus::IDLE) Signed-off-by: Davide Faconti * handle SKIPPED state Signed-off-by: Davide Faconti * fix SequenceStar Signed-off-by: Davide Faconti * add tests Signed-off-by: Davide Faconti * fix Signed-off-by: Davide Faconti * fix unit tests Signed-off-by: Davide Faconti * add emitWakeUpSignal() Signed-off-by: Davide Faconti * fit test Signed-off-by: Davide Faconti * fix Signed-off-by: Davide Faconti * remove SequenceStar Signed-off-by: Davide Faconti * remove SequenceStar Signed-off-by: Davide Faconti * keep resetStatus() in action_node Signed-off-by: Davide Faconti * add suggestions from peer reviewer Signed-off-by: Davide Faconti * fix compilation Signed-off-by: Davide Faconti --------- Signed-off-by: Davide Faconti bug fix (#4160) Signed-off-by: Tianchu Stop planner if the goal is cancelled (#4148) * Add support for stopping planners when the action is cancelled Signed-off-by: Kemal Bektas * Support cancel in Smac planner Signed-off-by: Kemal Bektas * Support cancel in Theta* planner Signed-off-by: Kemal Bektas * Support cancel in Navfn planner Signed-off-by: Kemal Bektas * Update nav2_system_tests to use cancel checker Signed-off-by: Kemal Bektas * Add terminal_checking_interval parameter Signed-off-by: Kemal Bektas * Handle timeout and cancel check on the same branch and default to 5000 iterations Signed-off-by: Kemal Bektas * Add system tests for cancel Signed-off-by: Kemal Bektas --------- Signed-off-by: Kemal Bektas Co-authored-by: Kemal Bektas Improvement on the wat the distance heuristic is calculated Signed-off-by: stevedan Fix BT.CPP import (#4165) Signed-off-by: Tony Najjar Update default recommendation from Obstacles to Cost critic in MPPI (#4170) Signed-off-by: Steve Macenski nav2_controller: add loop rate log (#4171) * update smac_planner README Signed-off-by: ARK3r * added current controller loop rate logging Signed-off-by: ARK3r * linting Signed-off-by: ARK3r * uncrustify lint Signed-off-by: ARK3r * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski --------- Signed-off-by: ARK3r Signed-off-by: Steve Macenski Co-authored-by: Steve Macenski improvement based on feedback Signed-off-by: stevedan --- README.md | 4 +- doc/sponsors_feb_2024.png | Bin 0 -> 187601 bytes doc/sponsors_jan_2024.png | Bin 0 -> 184628 bytes nav2_behavior_tree/CMakeLists.txt | 16 +- nav2_behavior_tree/README.md | 2 +- .../behavior_tree_engine.hpp | 7 +- .../nav2_behavior_tree/bt_action_node.hpp | 14 +- .../bt_action_server_impl.hpp | 3 +- .../bt_cancel_action_node.hpp | 2 +- .../nav2_behavior_tree/bt_service_node.hpp | 8 +- .../include/nav2_behavior_tree/bt_utils.hpp | 48 ++++- .../action/controller_selector_node.hpp | 2 +- .../action/goal_checker_selector_node.hpp | 2 +- .../plugins/action/planner_selector_node.hpp | 2 +- .../action/progress_checker_selector_node.hpp | 2 +- .../action/remove_passed_goals_action.hpp | 2 +- .../plugins/action/smoother_selector_node.hpp | 2 +- .../plugins/action/truncate_path_action.hpp | 2 +- .../action/truncate_path_local_action.hpp | 2 +- .../are_error_codes_present_condition.hpp | 2 +- .../condition/distance_traveled_condition.hpp | 2 +- .../globally_updated_goal_condition.hpp | 9 +- .../condition/goal_reached_condition.hpp | 2 +- .../condition/goal_updated_condition.hpp | 9 +- .../initial_pose_received_condition.hpp | 21 +- .../is_battery_charging_condition.hpp | 2 +- .../condition/is_battery_low_condition.hpp | 2 +- .../condition/is_path_valid_condition.hpp | 2 +- .../plugins/condition/is_stuck_condition.hpp | 2 +- .../path_expiring_timer_condition.hpp | 2 +- .../condition/time_expired_condition.hpp | 2 +- .../transform_available_condition.hpp | 2 +- .../plugins/control/pipeline_sequence.hpp | 4 +- .../plugins/control/recovery_node.hpp | 2 +- .../plugins/control/round_robin_node.hpp | 4 +- .../plugins/decorator/distance_controller.hpp | 2 +- .../decorator/goal_updated_controller.hpp | 9 +- .../plugins/decorator/goal_updater_node.hpp | 2 +- .../decorator/path_longer_on_approach.hpp | 2 +- .../plugins/decorator/rate_controller.hpp | 2 +- .../plugins/decorator/single_trigger_node.hpp | 2 +- .../plugins/decorator/speed_controller.hpp | 6 +- .../nav2_behavior_tree/ros_topic_logger.hpp | 2 +- nav2_behavior_tree/nav2_tree_nodes.xml | 17 +- nav2_behavior_tree/package.xml | 4 +- .../plugins/action/assisted_teleop_action.cpp | 2 +- .../action/assisted_teleop_cancel_node.cpp | 2 +- .../plugins/action/back_up_action.cpp | 2 +- .../plugins/action/back_up_cancel_node.cpp | 2 +- .../plugins/action/clear_costmap_service.cpp | 2 +- .../compute_path_through_poses_action.cpp | 2 +- .../action/compute_path_to_pose_action.cpp | 2 +- .../plugins/action/controller_cancel_node.cpp | 2 +- .../action/controller_selector_node.cpp | 2 +- .../action/drive_on_heading_action.cpp | 2 +- .../action/drive_on_heading_cancel_node.cpp | 2 +- .../plugins/action/follow_path_action.cpp | 2 +- .../action/goal_checker_selector_node.cpp | 2 +- .../action/navigate_through_poses_action.cpp | 2 +- .../action/navigate_to_pose_action.cpp | 2 +- .../plugins/action/planner_selector_node.cpp | 2 +- .../action/progress_checker_selector_node.cpp | 2 +- ...initialize_global_localization_service.cpp | 2 +- .../action/remove_passed_goals_action.cpp | 4 +- .../plugins/action/smooth_path_action.cpp | 2 +- .../plugins/action/smoother_selector_node.cpp | 2 +- .../plugins/action/spin_action.cpp | 2 +- .../plugins/action/spin_cancel_node.cpp | 2 +- .../plugins/action/truncate_path_action.cpp | 4 +- .../action/truncate_path_local_action.cpp | 4 +- .../plugins/action/wait_action.cpp | 2 +- .../plugins/action/wait_cancel_node.cpp | 2 +- .../are_error_codes_present_condition.cpp | 2 +- .../condition/distance_traveled_condition.cpp | 8 +- .../globally_updated_goal_condition.cpp | 11 +- .../condition/goal_reached_condition.cpp | 4 +- .../condition/goal_updated_condition.cpp | 13 +- .../initial_pose_received_condition.cpp | 18 +- .../is_battery_charging_condition.cpp | 2 +- .../condition/is_battery_low_condition.cpp | 2 +- .../condition/is_path_valid_condition.cpp | 2 +- .../plugins/condition/is_stuck_condition.cpp | 2 +- .../path_expiring_timer_condition.cpp | 4 +- .../condition/time_expired_condition.cpp | 6 +- .../transform_available_condition.cpp | 2 +- ...d_a_controller_recovery_help_condition.cpp | 2 +- ...ould_a_planner_recovery_help_condition.cpp | 2 +- ...uld_a_smoother_recovery_help_condition.cpp | 2 +- .../plugins/control/pipeline_sequence.cpp | 9 + .../plugins/control/recovery_node.cpp | 59 +++--- .../plugins/control/round_robin_node.cpp | 32 +-- .../plugins/decorator/distance_controller.cpp | 13 +- .../decorator/goal_updated_controller.cpp | 30 +-- .../plugins/decorator/goal_updater_node.cpp | 4 +- .../decorator/path_longer_on_approach.cpp | 12 +- .../plugins/decorator/rate_controller.cpp | 9 +- .../plugins/decorator/single_trigger_node.cpp | 14 +- .../plugins/decorator/speed_controller.cpp | 27 +-- nav2_behavior_tree/plugins_list.hpp.in | 6 + .../src/behavior_tree_engine.cpp | 11 +- .../src/generate_nav2_tree_nodes_xml.cpp | 47 ++++ .../action/test_assisted_teleop_action.cpp | 10 +- .../test_assisted_teleop_cancel_node.cpp | 4 +- .../plugins/action/test_back_up_action.cpp | 10 +- .../action/test_back_up_cancel_node.cpp | 4 +- .../plugins/action/test_bt_action_node.cpp | 26 +-- .../action/test_clear_costmap_service.cpp | 8 +- ...test_compute_path_through_poses_action.cpp | 18 +- .../test_compute_path_to_pose_action.cpp | 18 +- .../action/test_controller_cancel_node.cpp | 4 +- .../action/test_controller_selector_node.cpp | 14 +- .../action/test_drive_on_heading_action.cpp | 10 +- .../test_drive_on_heading_cancel_node.cpp | 4 +- .../action/test_follow_path_action.cpp | 6 +- .../test_goal_checker_selector_node.cpp | 14 +- .../test_navigate_through_poses_action.cpp | 6 +- .../action/test_navigate_to_pose_action.cpp | 6 +- .../action/test_planner_selector_node.cpp | 14 +- .../test_progress_checker_selector_node.cpp | 18 +- ...initialize_global_localization_service.cpp | 4 +- .../test_remove_passed_goals_action.cpp | 6 +- .../action/test_smooth_path_action.cpp | 6 +- .../action/test_smoother_selector_node.cpp | 14 +- .../test/plugins/action/test_spin_action.cpp | 10 +- .../plugins/action/test_spin_cancel_node.cpp | 4 +- .../action/test_truncate_path_action.cpp | 6 +- .../test_truncate_path_local_action.cpp | 30 +-- .../test/plugins/action/test_wait_action.cpp | 8 +- .../plugins/action/test_wait_cancel_node.cpp | 4 +- .../test_are_error_codes_present.cpp | 4 +- .../plugins/condition/test_goal_reached.cpp | 12 +- .../condition/test_initial_pose_received.cpp | 30 +-- .../condition/test_is_battery_charging.cpp | 12 +- .../plugins/condition/test_is_battery_low.cpp | 20 +- .../plugins/condition/test_is_path_valid.cpp | 2 +- .../condition/test_transform_available.cpp | 6 +- .../test_would_a_controller_recovery_help.cpp | 4 +- .../test_would_a_planner_recovery_help.cpp | 4 +- .../test_would_a_smoother_recovery_help.cpp | 4 +- .../control/test_pipeline_sequence.cpp | 27 +++ .../plugins/control/test_recovery_node.cpp | 20 ++ .../plugins/control/test_round_robin_node.cpp | 27 +++ .../decorator/test_goal_updater_node.cpp | 14 +- .../test_path_longer_on_approach.cpp | 4 +- nav2_behavior_tree/test/test_bt_utils.cpp | 32 +-- .../test/utils/test_behavior_tree_fixture.hpp | 2 +- .../test/utils/test_dummy_tree_node.hpp | 10 +- .../params/nav2_multirobot_params_1.yaml | 59 +----- .../params/nav2_multirobot_params_2.yaml | 59 +----- .../params/nav2_multirobot_params_all.yaml | 59 +----- nav2_bringup/params/nav2_params.yaml | 59 +----- nav2_bt_navigator/CMakeLists.txt | 4 +- .../behavior_trees/follow_point.xml | 2 +- ...replanning_and_if_path_becomes_invalid.xml | 2 +- ...hrough_poses_w_replanning_and_recovery.xml | 2 +- ...gate_to_pose_w_replanning_and_recovery.xml | 2 +- ..._replanning_goal_patience_and_recovery.xml | 6 +- ...eplanning_only_if_path_becomes_invalid.xml | 2 +- .../navigate_w_replanning_distance.xml | 2 +- ...e_w_replanning_only_if_goal_is_updated.xml | 2 +- ...eplanning_only_if_path_becomes_invalid.xml | 2 +- .../navigate_w_replanning_speed.xml | 2 +- .../navigate_w_replanning_time.xml | 2 +- .../behavior_trees/odometry_calibration.xml | 2 +- nav2_bt_navigator/package.xml | 4 +- nav2_bt_navigator/src/bt_navigator.cpp | 70 ++---- .../src/navigators/navigate_through_poses.cpp | 6 +- .../src/navigators/navigate_to_pose.cpp | 4 +- .../test/velocity_polygons_test.cpp | 1 - nav2_controller/src/controller_server.cpp | 7 +- .../include/nav2_core/global_planner.hpp | 4 +- .../include/nav2_core/planner_exceptions.hpp | 7 + nav2_costmap_2d/CMakeLists.txt | 53 ++--- .../test/integration/CMakeLists.txt | 60 ++---- .../integration/test_costmap_subscriber.cpp | 2 +- .../test/regression/CMakeLists.txt | 6 +- nav2_costmap_2d/test/unit/CMakeLists.txt | 29 +-- nav2_graceful_controller/README.md | 4 +- nav2_mppi_controller/CMakeLists.txt | 1 + nav2_mppi_controller/README.md | 44 +++- nav2_mppi_controller/critics.xml | 4 + .../critics/cost_critic.hpp | 98 +++++++++ .../critics/obstacles_critic.hpp | 2 +- .../nav2_mppi_controller/tools/utils.hpp | 1 + .../src/critics/cost_critic.cpp | 200 ++++++++++++++++++ .../src/critics/obstacles_critic.cpp | 8 +- nav2_mppi_controller/test/critics_tests.cpp | 1 + .../test/optimizer_smoke_test.cpp | 2 +- .../include/nav2_navfn_planner/navfn.hpp | 18 +- .../nav2_navfn_planner/navfn_planner.hpp | 6 +- nav2_navfn_planner/src/navfn.cpp | 21 +- nav2_navfn_planner/src/navfn_planner.cpp | 10 +- .../include/nav2_planner/planner_server.hpp | 5 +- nav2_planner/src/planner_server.cpp | 27 ++- nav2_rviz_plugins/src/selector.cpp | 2 +- nav2_smac_planner/README.md | 1 + .../include/nav2_smac_planner/a_star.hpp | 27 ++- .../nav2_smac_planner/analytic_expansion.hpp | 5 +- .../nav2_smac_planner/collision_checker.hpp | 4 +- .../include/nav2_smac_planner/constants.hpp | 35 +++ .../include/nav2_smac_planner/node_hybrid.hpp | 11 +- .../nav2_smac_planner/node_lattice.hpp | 7 + .../nav2_smac_planner/smac_planner_2d.hpp | 5 +- .../nav2_smac_planner/smac_planner_hybrid.hpp | 6 +- .../smac_planner_lattice.hpp | 6 +- nav2_smac_planner/src/a_star.cpp | 140 +++++++++--- nav2_smac_planner/src/analytic_expansion.cpp | 171 ++++++++------- nav2_smac_planner/src/collision_checker.cpp | 8 +- nav2_smac_planner/src/node_hybrid.cpp | 22 +- nav2_smac_planner/src/node_lattice.cpp | 23 +- nav2_smac_planner/src/smac_planner_2d.cpp | 13 +- nav2_smac_planner/src/smac_planner_hybrid.cpp | 49 ++++- .../src/smac_planner_lattice.cpp | 68 ++++-- nav2_smac_planner/test/test_a_star.cpp | 162 ++++++++++++-- nav2_smac_planner/test/test_nodehybrid.cpp | 30 +++ nav2_smac_planner/test/test_smac_2d.cpp | 10 +- nav2_smac_planner/test/test_smac_hybrid.cpp | 14 +- nav2_smac_planner/test/test_smac_lattice.cpp | 10 +- nav2_smac_planner/test/test_smoother.cpp | 11 +- nav2_system_tests/CMakeLists.txt | 4 +- .../behavior_tree/test_behavior_tree_node.cpp | 76 ++----- .../behaviors/spin/spin_behavior_tester.cpp | 15 +- .../spin/test_spin_behavior_launch.py | 2 + .../spin/test_spin_behavior_node.cpp | 10 +- .../src/costmap_filters/keepout_params.yaml | 54 +---- .../costmap_filters/speed_global_params.yaml | 55 +---- .../costmap_filters/speed_local_params.yaml | 55 +---- .../src/error_codes/error_code_param.yaml | 4 +- .../planner/planner_error_plugin.cpp | 1 + .../planner/planner_error_plugin.hpp | 47 +++- .../src/error_codes/planner_plugins.xml | 4 + .../src/error_codes/test_error_codes.py | 20 +- .../gps_navigation/nav2_no_map_params.yaml | 59 +----- .../src/planning/planner_tester.hpp | 3 +- .../src/planning/test_planner_plugins.cpp | 72 ++++++- .../nav2_theta_star_planner/theta_star.hpp | 4 +- .../theta_star_planner.hpp | 13 +- nav2_theta_star_planner/src/theta_star.cpp | 9 +- .../src/theta_star_planner.cpp | 18 +- .../test/test_theta_star.cpp | 18 +- .../nav2_util/simple_action_server.hpp | 2 +- tools/bt2img.py | 2 +- 242 files changed, 2157 insertions(+), 1375 deletions(-) create mode 100644 doc/sponsors_feb_2024.png create mode 100644 doc/sponsors_jan_2024.png create mode 100644 nav2_behavior_tree/plugins_list.hpp.in create mode 100644 nav2_behavior_tree/src/generate_nav2_tree_nodes_xml.cpp create mode 100644 nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp create mode 100644 nav2_mppi_controller/src/critics/cost_critic.cpp diff --git a/README.md b/README.md index 3fbb186d458..6df210791bd 100644 --- a/README.md +++ b/README.md @@ -27,11 +27,13 @@ If you need professional services related to Nav2, please contact Open Navigatio Please thank our amazing sponsors for their generous support of Nav2 on behalf of the community to allow the project to continue to be professionally maintained, developed, and supported for the long-haul! [Open Navigation LLC](https://www.opennav.org/) provides project leadership, maintenance, development, and support services to the Nav2 & ROS community.

- +

### [Dexory](https://www.dexory.com/) develops robotics and AI logistics solutions to drive better business decisions using a digital twin of warehouses to provide inventory insights. +### [Nvidia](https://www.nvidia.com/en-us/deep-learning-ai/industries/robotics/) develops GPU and AI technologies that power modern robotics, autonomous driving, data centers, gaming, and more. + ### [Polymath Robotics](https://www.polymathrobotics.com/) creates safety-critical navigation systems for industrial vehicles that are radically simple to enable and deploy. ### [Stereolabs](https://www.stereolabs.com/) produces the high-quality ZED stereo cameras with a complete vision pipeline from neural depth to SLAM, 3D object tracking, AI and more. diff --git a/doc/sponsors_feb_2024.png b/doc/sponsors_feb_2024.png new file mode 100644 index 0000000000000000000000000000000000000000..50dfbc47a406b49ae84da62b56f66ba7ae727909 GIT binary patch literal 187601 zcmeEt=UbCo`z4BiO7k291O%)oz4xl9NEM}bq<18gh@q&cC{=nFkS1M9XrV|ifrJjB zMS*}o2m+D-q0GkfzBA`{%|9?7hA&*{&$IVk*1gu+A9Qup=+CpCr=XyqSATf#2?fPj zS_+CY@6S?$XM)d+f`2b5)bHKVe{PANJr}@fU*EkMSN+y*4jNlst?e+Uo_3cR&gXKm zRgzT$JkmS=B1LhWdz%w$s^iH@ z-cN>ld1qsPr*9uMXQGewI%*ab9221qpZu{JRN(jj|KI;#f&WKWz_e#|tk|l-Wvqz5 z<`%zEnZm^3{=#NJ`ynQ}zOmscII{YTSW7`bVDs=Ov1l)RiufG%7CRrWZR@uHz*s21gtJTn>#q54ub1DmGl=0*8Jp1{P-jPjHf66vH!n5F-YN;Ym(J} z*ld(=Jd=3Ye=#On;5g`iz9G*|pums%Bpc_rG3~c**I`a*2b(ReAMG4%b2un#MvZ@i^TM*O!o#bbR?=?{vCg=F0MH zFAd3@H)Fg^(*Co5rvKMUt__@)PqU(V(B_s6HEnqy4jIhIwKVRZrMlg@bY52znW!P%Q4L z|1?j>@|Qg%43{iKR`ZX>1a?iqX8rG@0|P_nrut^M8teas;YCK0~gNV9x}{pJ+QC(#?m;7;!0S;#XgeZxOe; zP735$d%-mFrD;A&+kATB+JyLr%bBMrzA3L6%QQT^wResWrE@isi<~*Nc#Ed1`A!Pn zmw?Uql7m0P=_t%#18dZ%%<;_(tU+Aq^RdaqjAbhFMpOmLM{3tdu>Ynj`58l_lF%wi zc)fE<$WXL&kw@dB0P>c{^qXA{&Sl%D@tou0BBOe9j(%yKe&{RgY@=Pxg$LXF$=~ce zM-TVJ>spsG{WXhByY%t2f0K2RjA#z`n4BMU))D7h4;!z}Hd)&d6Z2D1yMsq%c9`~= zo!*7^NhO2(ev^lCMcRf2kDI3~x-WCwSlMpxC>yE>ObHuKng~A#ennQ?g5l70uAdU* zG%}e}15(703_?d9aRejtjgIz1NYiyj)x{SD&y9^FE`BYwF@^V1+SY0dWj?37S;|(g zW_xUVaMbTQyGdY%1Un!_HEXD5ljy0u_TMPR`FJ^2_Wzvwu?I=#OD(u$rjs%`R_oW? zgH_suZ3_~WRcj;tNad3S=C`UFYiuGM9u)DRu%cemzXXXH9szs9gts~2eQ!@we1p0_ zmDuuuRvy6Rot0El`O*?9IA*Ra>yXg3b>Ty<)xAgaN2L2g%HGEW8bWaJz~Me`611Ln zthNq&TqKgn;f;CPN%|xZ>}CY%Y0phjkt=hx*ziHvuAVsz?SN#PI>JC;&amHGQN!I) z^UuV%vrH@c#?sekuG1y$_y#$Y&!e)T4c7k-axZ8hmzjE>uwbEhGJKxM2N z&uDL9PQ`yazi^)$jtpV-?-~0GN2?mA+*lKAN8=jD_16O;)v(s*NJaJg7})Gi&#jqb z(#@oey5OS^)6qdVNCjCjsm^^xgS1iZ-964>PIz{FY~gE*Unh``wNrk}F&7xsW66$5 z;WOA{P12ePVPmg`eEi7!_&}XRYLb4xElOJBZ2})2X0sm zb_MBstb$HT&!vWVv3{yS*g~w^qGUWj1?Rxd!#HsI&8YT*ByG>`6|M|^&%mPj2abNh z8JW`sJ@j4t2JtP8buqNu)leRql)>C`=)xD*H8-YDoej1xoAgr-E+sDhy6^k$@{W?uS71k>-iS5TS4j61o#g*n0;w;kSe2wie5mkhJtcygbz`^`psZBNL& z@);h^rw?eY^d0djm#>K{9vbON9O_K+1Rx(U=Pmcjd`$5>MUnj8;Z)0YR$4LZ#)`UD zR{IwH;d|DV=$}2G7KNTfCpfEy{QP(e^&ynZ>g|0A0p5IfHUFmIK(sE)Gs$~ zk3V%M7AJ?Y`ujnB|KGpv8%R*)wipb{Fc)kM;w;B5d%9@kgddbhJI7h@MAF}9x|1Gg zc@u*O%dfjVAQyMgD|WNA|C6o|-B(H{mMik9G#qE=Kfn5Z8CnwiphPeC&ZETE;h~=E zuEG%OP{BgN)ny5LJDyZG)r@^|A}qebD&DmbQaJd+PMs%VQ8>u~1+LOFX7js&hZcGG zj>EH46j?&4R1am-y5w~{A@wu3)650fSenM{-YA2|pLna>TBgzxRK>fo9og5Jrr-Wq zk~?5YUS&sZ;?6b~e6~qU-9i)e799wseRi4Shwafz+M|Y2h|Y-v9+*X)7sf!Y;n+rl$S5WR0QG4lieU_<)HUqATfoom*owE|nZV;GqQ#iK>iq zrf~NPadlu8=Vi7{k6%|p5#miM*tb3bP(c6M;Z$X5PiIHq@u12t8cs(mC~IN4cS{*v z&hM3s&7ZGr&ZA#tGhp)jIM}{Y{gY`@v@(!@B0H21D=198v#&0^h9g!-! znpX~|#4s^S`Jeq&#<=zmq1MB!MK{oLk=jcr3ogTVxrOH#v$Y4T=GI(DItFHZ1?INc zdDJG61W}ImHZ3kf0nnoMr6Q6!seZXUE=HQ-M~{&BuoXRQ!bCY2DIwR>Al! zF3tksn47c>u|5?FDo=Y)j{-<^)pulY;PhRO2SqoOJJ(xgtoc#F#DI*5%R+i-iLP(+ z$E9E!F129~qJ$D1`GQGD_qlY<`K2dtKaA>)ozA<9@g#-b5HovlY5Fmg4wk1g%*bnt z9^pZ#?u}jiniF$#+5^%Z;%z=_C;Kj`kZSKLCv7O}3TTq)l)5wnr|pWacjFN=q6f7Q z3qfrw3-yK;bI0xPq^oc!X*w{zKhcl_8^5+{HeSClDxIKC;%7atT|Ry}&aT|I`+eG4 z0IB(Y>dm8zU3*^YPu2xkK^Q22Vj#vAgIeU6>FhF*tKojaLzeb8E95sJiul7m6sA`E zv-j=NwEBD@RdV)C=*b|fu{jUEU~16#pitMiKy?yE%^>-4fp1SkO>gDltnt#C$K{1X zKLW()g1yx_8}cql)Sm#&Q5*jLc@(1t?cTfkG(Gdn!fz%*oo`uLQwASnD-tX1|8ta` zM$0cTmZo*s=PCY<*v(x$MiarGPOA$RKBYO?P1PE2|0>4Yci%yl$M-jba9GN@FG@zN z{%Mx?J0WRIYb#a+eXo7Ol%jm~`|aklMN1ADgL-Q0&$Pjzi!2q6b@kU z5gS-fu`2t(rsCQAlWcO8+zGr}atnB8&d1FkS2h9Y23vh*;BbpEq1a@W>|%PRLKxrGEMm#l}sm?dRs`d9*l1cNs7IDF`E^ z3I956e)PQtKGG%=801t*iC~TB=dxOh3paW+D4_CiRzkf>B~MBlQ9<)_GDx-J@+%3t zo)_56a_tdRto%T+oNCH}mmDg$eu!)$?t*liEbnVyhU5*3O!0+;RM`p4a^fE8d%Z~S zzjh%RFJ{5vUDf~Xw^|w0!yu+NTO(G{Hyzm}uN zYM4R`2fN%l)R$S<=BLX^s&R_aUnJk}&*?5JwKSRJJ2SeBRC{}tnF!Xvia`1k$c>f6 zCY{}~z_5U$Y5mwh6%xiD_v94C%YD!^N7&fDx{r3v5g>k-9B=vE9A{xI+kRI)&E-Eh zJP%&a^++u+$$Ell&DRckL#_`lsQc?`<}tN>Wi7HgpV{N9WOjughU8=3__keQ{XRXu zynQmNZyJhod0OkL2?{nCPr$HJIR9I+FBd!J-V|Ncs}d?+`?yzAYzR=OY5LLe1`bX`f@BE;gbHSgXg5A2anv`G!OcrgXV*F$h02=TSH$xjg6&sAR<>dyC6a9YMQH--(}bHPu~Ce0TOzvJA%PcqTfAWfg z0)OFT!R$(ro-^Nxwtb7;J=L{fb{4`YAvpw6IsWwB5rA;D&m}BIdnuDw!f)XfG6~a$=X3=X)dAU=C zaWU<)dpngM;9DX*i0kjyL!BD-vgaf>YTpF)<(D?x!CUR9|&gNb+ll+`z&tcFSW7S4Cw+c)dvkQE|2Bte-+ z5TDNhnCrY1%5RG;cX4f;*wbn1d*HvIJ7?;+_v*pp*&J~$iwhlo`rX73g8aT@RlP*u zA`g*MNcp@iS;sz+vxIsvI|A)Vs<{kSb>UVY!FxGDQ#CG7Ck1bvBC1t0yVZq%B_( z`RXn~`|$zNfzuI+FrTkMj>zTUf0f;^z&~g`@@uRGTY0uHE#Hl0vT__|jZcg3Ei2!y z4+t+|4a%0<`%9uhQpqGprF%tl2I_Y|V`I2|vSmevySU$i3jl`|!;GtI)5!Rp1zVof zGp536Gn>mSoYUSjvI~)NCQ=ff>IOy!0mc(9AOZ*e_7@3=mS;Ka$Z)l^P%z(yB)$Se zr@-_hH6njlXlX1xp@GnS$BLMHemVES1P^^EU$4$%_=Aou=!zL(qRTR0+ptu>SiG+j z@ITWJswkXCdwlA|I#)C8{a*{kMfFS@h%pzt)#B*c2L4Gfb%2b)Ts!Nvegn z7!JK;;FCTl=;`M}%p;38`B0j z`+sT-%UX1CW!aJ)!0=MPp*H?$Cy>J(_b?9QP9Q58r5#v>936b9aui-qd>6B|^eX=j zxVW00{ke&^JjXssY;1)CA8{DbFHmFks&X-+O{eTcB<=dTM?`>2w~CbRk2$l2*!3D^ zGhX@@-)1%JHBz)^V%G;iL}@IDQ&7l%a5yzghsj#NlL!-fi-N`0PuWEUO}+nVfpZS0 z3P2g4I^4^?V~1-^Y|PU?3TVa|rieA(>SHymYaM@CB;F=7ZN&Z;^K`LY)3jyqFlT{g z6HI1Mq)5C0bQy;E-{%O3=P6;^8@ybGyNQBGrU3Z}Jo~KjmXmC)p-ia6br6@{U3R4U zilMiClSVx<&RN9RcfwO`&z;J|_SVb=J<@eKj_13s5H*aVjmFt02cM7kpWm;#Rewmt z%V9G;F_(J;czMXC(3aT(bKd{d$ANf+orVN+!7*3Y^+$!iO;ufv;@N-N`l`dJtKGKH zfS|mvy4L(fdO_P0GU{Nrol7FhPcqoQeC8~4nora5qF%OR@VN#2J^!30a{;Y-DgR4M zsRuv$a?xJc&f|uSMbrCC;b_caSFcgM`;WM%>0DD+wH*aqN8CU-YNy-OoFVurW-2I`JIT>TpjI1{Jf1AiPTdX(=&^r zh|Jb@$QkAd&L&5d@di5{b|i1UCdH&Li3f zwN5$&7FpoqX3x31MZ%elBepjm+0jYL)HxW|nyM~n1AxW!>afUH6z(*X?uD-!HtduY0sdy`d(+I3e7*~P0SD)C7@0iMqk*?~#(ITi%`2e|xHF^g4se^kAEaX>bP12;INIoq=Tde@1o=Hu} z62A`jZNL}9`YR9lHl40a`}(V@*@6id;7^d51={qBfe-~lxHP9irpEMKxh>2&IYOa4 zR$m_3*;dWxndaLpTs}|Kh9!h;C85opjNxpa7WG{0rrBWrop+rezgvwQ3&Wi0QB_0b z3{QF&Dg-2&P%lQ_wc?tR=ELr~5uwKVQ2fP}s!8pGvH;}aRBjTOGcU28`X-TWl7$03)AVZE z3|0S*97?Gi^hw++pFFW1GYI`Z1dBl`6<_D*)X=b+OI*fkRBvOGg+Q-$@ZeuK^p{^m zqK97pi=j+GsO?+Yc#^ij*51f;Ld7wsfL0EqROIH=@&Ly$UoY3%>QUG?yB2*o&nax} z{mMfO9#sf+u|@m&4{=rcJcL})c8WZyYA!LgNYC*K#R1M>hExELp#C|YvYSp9y7sd9 zP-r6ovz?dnpJvy)1~ZGBng&!cv-YUV(|HRAH^bP5Z-~8Xew>D1?7Kn=I%PdIC@{DM zg1wZg#gs%|RDJ7`ji+9v zP-8DQioRy{&fEngcR^%=bjNcU4oA|ejJ)cct^+KcK~)^r`n2L+c(2mJYH*c}{0(_- z*#b+>OXSDQ1)3@LSyxY%RdS1Gn*{HT{=x|}HBBoCCUpHeCMa$D`nvou%YC}ID$;Ao zk=jn=W2GIjR`+F`%~V!pgFVjJ=&tRN*%zt}{>1WII2xIEnnLSpDwQc0z#?PUSj8sl z@BDUzvjzDTRGgC@Vh8gypbs@AY2Q!1$Ryf_@nUbLy_O+|hvE5OP`?9XO`ATut1#Ep zB{DThamI8Ru_?$gE4v#lr(-2Ir~9FBglCPsszm-GQd2(e<;S4c72B(Ep#X|-&DbcC zQS^A^KQaPW>8|nFBm^_obZ_d0Ns{ihtLt-(=uDk!q zF8&AktBso7D`d;Wn3^d+seC0OT3z{yJ zFclyEmUoDUv13-s*uaZ#q(?*i=^kv8x6>yBuyu9(Ce3wE{9mp=>x+OR&64DL-&I#^ zKz&KsI-TLC$JwpOp6I-%Y6ko9|@ z^yoXpg^jd;!lj+q@V(1l6;!3&3(ki77Dlnn=E6b_mz+$74{ix()onKj@Eewe0IIQ{ zUd%Shd^y4OEw8S?MiOp;gb(x#D3dJdAhyV{;AsEEALW1J?>egl&GVg<;-tNGgFd@ssS{DG-Z)vyc0FTrm`bj~)7Q79 z(2#r%8>&`8TAQ?F?^@Z8%bhYQW8t}<*7)jD7pD1Xd=qNykaTDLY1r~KSIWB{DV_n9 zmT{iIrvNX6I{QQUQ>oqyV}90bYc|-BE5|Kkv*c8TzedMOuQCr5#A;XOCWNUaLdsHH z#@Gm^`+s>%lV^65D-SGoS!9>Y2La>T|B;#kaclW4OnW+IEt$}3pa9B(lU{cTrM9VA~8SrRA2h`cr0T3g}`-&N)m z49tyq)rV`#b)p?;g@G>2`+i-~x0w=qZDDoouC3KgI4|JCM6{h)oczH}WL{&f3I>2d z-5q~tOXLpm)t1hy!hh($HZZ){G@)%up5mbi>Umzp2YZKM>YheCZt;w${aB)JK!znMp|Aj11#^1V@7%)IrmGaJhZ5>?0&luqi4OJ3Op?4H1V3 zB50HdI^?D425(#=d3i!xg2KG$Dew;e6o&WEA)Y3SZj7{Vtg_}n60K=Nb^-05%y9*T zp37Pwwb#+X71Ys_#amb@exyx?5#wo~93giH)9Gyp^q-Rw#yrn1W{; zEEIm?|0GXGw;L$1wKMnD!qzpz;fkVy|0GVcN!B%K^uiDoU+<&GjsE-SP3O|3 z)xi&e`2x_tdN^+Tx)4mpH3ZuxmEccsSAXPcFyQ0!gkJXqjgIZP+_ydNJerJG;+)o; zt=KGW2wogY$j(l|4Y0*Ufwc8>5ofo!z=L_JD)^!?CQrZsl(;l}6u709p50{!AUO96 ziO9Zyg(iThwXCeg2(a`gs)EhlsoL^Dy91?jB-c~c(42oTPyl-Og)1gBJnH9(Tp-*h zf1c-0XN}|@IIZ&t9rj5C6+HsSovk}%vv^>OC9^aPwnse-{Pz`Y7XL5y!B+-^ww6A-jo>0L#{oI!?z~$DG8zmf zyn?dL7Efk#a)K9E2+#M$Z_=kSDYsa~n7C;XY3;YVH+7)}aGFD8IL!Wz`*Dh~WBHT* zyU*cLd$jx!Mig0I|3Pqz8x^%{x5=iLmP5A^mq-5q?aeQM5S+259jK)Ab!+*dMBbO% z`Ndbp06oF%#PNHsIvBaU6M;KkS@k|98VTQ#Ak}T-SK#aC;%rMS0Q>l~#p1^*xk*m) zR;~#oT7LQ9io~9nb7j#p${;>akRPVIk3aTH3L6oorhPzd=5_9(^A#WRUU))pSpwRu zfLbZfI7)@E{j<~0vfgl#0gM6Suj|)W8sv6%1S=J1iy{x5(BD2r7y!f1>;G)X@db;L zs~Lyi4jU@@3sJlqB9oGT+Dwont^OkQ^stBhmaV>>aG4$>R_ti$jiUP8ZQhCVjP@l> z2rS{cblmFbSW84XGrrR0oL;L=>?)oAFEH-O7-7YeSrV&N##bjltw8LIVj-pCdnc8J z#s520@4_@=pK)npjzr5U9iX|b=7b_Z z$~{q7go8F+T(js?s+U`Hpt2ue)XslvbQE!M#MhEv`_>jq09Y+H+x7c7DzPo*MD!Ju zlvauSRqYgCIB(8~ZL+reF!qOo7CzdKe0rp(%nM&-N^o(rz%c3mYD2C&nkd=`-aq_&Ls%M(k1jaKI z-iumt_x#YL*-c!tIK;&9+0JBToLLuhILAb0cgNei+2-Q@&V#2s)bew4Vwel}&A$c8 z@0$Z}sd~{qZp3VIsTdew{X?DowwDax9Q=*nI5juEY-5_jIl||-x`-AMpWTzBOi-Vb zoA`8ueV@wGnxNWpHw?G8njsZH==^V9n}sVh0?G*5y8T2_m(H;MLrwUfKBC|b1{u3l zjf_R~aF=CnSlS0BmjyGA^RuF{Chj%%&k_Nf@4dV{; zl62JrymNGD<&CHU0X1EPU)tr~JI0-E&ePvlbC}3l?v+*g?j{Wo!9e7TB+ud-!CG%& zdXjRp&!+`+wVIsV{syy(QK+lIdgN2Wxx4{AF#Wm-pEEZ@K~ePfm*)J3hWm`8rMKoJ%hb z3}N09X0~nodn|rXAC{f)V;^EF`rJJlk2e9|IRlAQ2U^I?PX8&n8f9Ca&)3SRiLY&H ziqG4Q>~~F(@U>*8xQrs>B@IaM1;^kOLKRxb=sd-s%71R4a@}oMtX(Dk(DkcGq$y$E zkNuydm;tbHoE$G5IaBdNqSG`o7G~%CPRN$=BTW(xvL^&AT%d$TM_UAX3Ka@kJsmFh zyNTp71oc(l_kp8A!#8)c@*xD_g`1ape9=?JULzv~`|LI% z7lyLT`6*N=F{V}D`V#n+*Tuf(6dD%O_8rsX4M9Ds48&aMA_mk`lt|J&YpoLh6I%x! zEYD65UX@2+@Mh!I(nkeT)xGolD;o@nQHUb z`9$tXdv}+kElcJ+VIwgF%{1$#Fo*rSQ_v>w0H8S(1L%M_N%-;kF2O0gWxo`k+?_d@ za@1K8ikqN>?PeZ8$7;*fB=sGN*Q5B2kgfpqyzwGR)G8lT>dXf8n=((2z<@G)exiwD zaI$7x;45u?&H?tD2N342PG|0TSjk$A@5G}vw@`1~!3_4XVYAufn{(WQ5|KX30t>~} z(Vp6ijPB-9rN7>FvbZ-upErvU4a$_H#|l$CV?r@TUv3;iyKW##N`Zs!i;OaA>^FJ= zE;x~PfxFU@Yw;})OsG2{15nRb88curUQ3#S{~o z{&Wcm+KXLNzE%zQ((uSK9uWJzRg*A_W?|PXLz#){UNdz-W)^+M8-<0|r_I)QA&|#? zTC@p$l+B*p2(Fp9%+Jo>f#nxi2F%agrXdnF)fA7zYP{h zdZvBAj##P{R1!8~wi@8KF(367+OXYwYw1#2yelrdX7F-jQgOGYg?pKyL_9Di(gUwx z)tSF3B+mVVbKl^f;SVd(Z+NV_w5vJ)3>&Id-&jt@+sgL7i;Qoi#|smzM2UXxc(om+ z(1G&Z8IiA(1YT_>xf-nQ;}_UBa(hWTTjQo5jrkc7NL-trE8{}8X^u+z;e(;uDm{rM zBRul*5$oq2YB5hwB6i*FNkx-x=XFh-`@xpZC_XIZs}0%ej%d^V8#KXe@zri5A$fQH zhV0iWbN=$WZD*rc+`>dXPHyiKM!ET7S5EZpB#_lv|IKP|lY$6E@n<228nk{Lj{sYF zwvZ#0ifc3tqcW20_#808S#ZuRp}3`!))t=HW!By8 zzAnshNv0;pT+pqy1_KjbA6NIQlb=g3y_Onxme)Q*p7B`s7BOYG$7t@=SA&0U10aq9 zF=Ey1p&vKKvrqns9>?YMv6Ngg0)1#aidR10m=14Tf%Y3ijLuwlcVA1A&WYcVME26dW*`p+O(E@(IRx8E-c*;gK~*vU;; z#NMFs^Gm~9*IEQhzpvWyUDZ1KVcnu~M%~e>p+#P{c4oRo?+qwdH9-Ce+A;#1KnlVW zDcMo81FSD^4vEj5_YESfxSwISXFU$~R}$SAFbtK7b=xbhKsgYc=xXmddKRaHH{%D{ z9`6ctxIbJSX3x!!wXXIZ4>|KnU*>JHod#bF-%KuzYfj?Uft>U=q7!U7qLLjcT?2WH#X2hINl4~hSQCceRk#b`0N}PRAT$% z*ELK*pFA5Epv$lNf*iT$-hf8wbv55j!SBmSZVQSTEO5`(FTLdwR2vlXM7!EPdeSd) z6|?B#Rj5GdWjbu*Xw)|{Ba zYLwwH-WbKg;V|2@n`4S?GO$!cqO9i7!NJSo_80DzR$p$Q`9fS3QcCl|t%V^(4~+EJ z1LP`%lsYdWA?7DyWh(RpPEjCO!D`@NNFnZQzzJ>3i&=Fa#$7BnndcS(7c3YWc)-{xk%P{Vq&ZW+ zcLfvNC$C?XCS6`+5L@n-xw$HrIg9vdlSlD`8OJtAVPxg~g}8fs#_D z!?8Ass+}o}IP6b1v?^DU{i=5h^O4+$HTnySh%MaG_Ips<%aDK7s{menpN7;o)gz{= zn!(O=mrE`!9A29f;F||EPJoIWaGtE)jkX%Snz2m^CP|E!$DNBQ=VMD1O-|#IdPK5(15s z*n~TDC?f*DQ=M3Qa#Z&<GPPMvn{^G#?hn;Qy z$1gJFCl2oJOg}But~VXfN#s5ocd$2Q7i=IA`p|=R9)BIMwfqz3rwfh6pD*&3B`WLw zD7_>thbXk7Bgw-I7 zbp<_EyB;mTTFmStd?}?=i%85e_IuiPVi;_~eu+Cc4rW$`0+cMx zj4mDj(M>=qxU&JOBe1jjxAN^dw{&CG%xlu-9z#6j=V}|nO~^bK7XUuxA{DcqZ->=B zyRl{=h+?oMkDf#beAa^GeHRVH`yn zJb`aV^URBvB*y2Op83RxVPLOi%#4=B$%&DKfIT&m;<2mnTF6Fj1raFNytY=jRe$)r zcJpkE;KEh^FUE4m7?_0z^f(|#Az++`j0}mP@g$m(j%RZheiNrQ1T*FJoG-_84M>kXG3 z;RLK>4W)nhfslc`NBy>3cd`2-7Zi2mh6Wn^T!m>HVBRwvFt0QKiknv5+a@P`KBb_j z43LNaH%lQSZ}wESjuobv=3vwt^qQ>;lJ#&na*e@8>Ke$&%tr&KhkrB~j*Yi8!Ku7M z`Q$L+XC-oH;mXWm-Op07!GM7{ZIGh6m~ z`Ib^h@bLIh61`Jg;Jq=_GVTyn+vC5~hPo}^NHzD(C2mQsR=9C8Fn?_|zz|z$l7fF> zk4JQ;y?2tE@p7-seH%7B#-|g!+(wyN7(ML~PP4mZtyQM&;WfP}=^^0LOkVaxY*+_q z**)SwJL<=`sCDldUs}H9CwNKrg=X8)LEYeDIX>)x&iNd?;?}R9+(+s6j)eTLkkU5u zUz>#;JLor-@=n|h!0Kgc^YtDLrr3qTzsWj@`Hu=%m#^ODqF`RW9W6d?sc?qFTwIgu zc^6^l5Y(OCNF-}I(*?GW7wLb_hya1TQpR1HfQ*C?x|5uZo_eh z$H~9;0{cyCJp-D$1*XfNOc$h@kastaL;4*I?h!Pt%H|L1Zfcr(2UAmIjqQSscN8rx zRmU2^j4>L$bIg~>qV9O3ybuG{ky9*smc3^B6D#wjoXE(km1y%T0p&kG+dnh^m1pv8 zJ*4&x!P{_tD9bo0@?)rmK?5H5z;*nKV%C4D=gu-~La*Lp6W`gG1^&6T=b(=}W3v6MV8~P8po~vK4QTW=TAcCpyn?zr< zRg7!0xViV?#6Rb~F+>Hg_?o`KUIvmW)O2JfZ@FLWp&ic3Ws|rtu zZSA8->54<`vDCEH(ebqz;CQshMEAXQW42J2Q@hzI8@{w`O7K~D2W?nO!Qf0KV`?x} zNq)sWTzR(q;$iX1Ii4|H%kp=jjd2dgClrK=?ba8QD@}Y_l}yY16&AQ_C!`Xa@fuZ* zrhcqrB`S?0t66KF+Ogcg>xh?xhEN`n7W97f#!swWTwBpr^I*^la#`mw-4q#gJ(8fE zOjCunYQ~)v*$b0~XaxbIe@i?`QQ5XU!Fe1fxC$vvZ5tfaI+-ioEA_guHO1R(z(vK0 z0Tf)ccC3qIz4@fj`mhR92a$;&b5&0{$yLiwY4~|XD<{;$&o)Cdp*QGe)k59zp^G~{ zd}h+IT3o5R<>0MW^eP=Va~B4w4G7xvQ~r@nUhi3?pa(~5{f$t zwRBrfX!+~Wkig4OAm()Q8nqZq<$8Kg_dc>`V@XZb5XJ03lz5HGee1S?N7*}bTc8>C zp-K6D6|()go&N|*ziN_Dm~#LWs3kXtDo9M;XDvU!OL>ok!L&gk0bKp2v%6{1TYd(Y z3PSsQN(h;cAlSXy_!3S7gg=kxnTn<5(ZwM5N~C^li($U8P_SY%I5PLGtQhrEgTX&p zjCk1BV>-@6x`VD9iu4NNcU_WN5LE-rX5V7DPv%0ZT;>3x&MPHdd9D=3m}XVRToWyU zdJ_})s(3jPw;!8<;*tK8a1(u$WmiLOi!F$a$uWq}gJg)_9`Te;y{Sq3nEw}$XzRQ% zF`YY)M3L!DlbquAnA7>EGShoxf0D9$KgmN0dL}P`Z^bQ>IKK{o2E<~h%@u=9)97Q# zq6;w?s%Bj_w_%#@XRK4&OYnTLDKZLgetH*ouh|Yw;9&&Wu7@|HIn4 z-XLDRynQ7E1vm!C^LD!Sgn5=jH5Vqg8E}bUSEA$#Bxm&pw^(#U4FW3iWu-M1+%m{Q zE#>`JqxqB0PxAr{msqXa^~tE98K6pOm~;=ll|OlGy_g%?J~OUFm_ws{s@CW zA_N^?EoyuBI&j)2?Fdvy3mcXAM!=!$X>Zhn!Xg$E z?qNZ`vIX~7Wa%iK@eLlKK z$~XySoZ_3MqutAJuVJBO7^oA;XnIO;ell6Wds&FlH@N1>*qqPK;T`m$7;4C2mwxzy zv2)nFSE~^sDB>7T5{|~{i2yt8`rEBU%skM$fr_CBM6wV!D8rG>g{p&ZIu1xN%GzG| zODSS8w@X=9HD`5hLuli-oNMIS z&$6C(=^K$eQ0-qdM$tKorvmED>79|#OOC9!yBK442nWz84%>Ar=diWM6coRromjq| z0Zqn%X0rzZZQZgoLUGCT$;TgakZH zvVxCYK+D|N#?+OX<+^~c;%u_!iQ@dM4V~2kvrzv@i~eV^O-P)8ft#}5`g83*-)m;k zqicXM4hQBTU7>0^~m6&MWZE?c$c7ruI$LWMTRgu+Z( zWHg%`%FTJhFOuU|M$t9cCxu^za13?btC}29XJ8r!jo1+MjQSa+EU*-d?5;-YN0*@H zdx`Vf^E9tz5Z$;mMK4iz*Bq%lcyGxC4=ulBa!PQH3uE7{0G8;=dgBLec2*4RWSFa) zhdD6jXP15AG9S@$u~QIU*mpIE&sgsg_B7n=0sae1Aupt6QO1wcWPPKMT0YyBH5W*p zd~MI~eOzixshY!wJvaxY^4LPk=*fz#u^A8L%>Q7T_1a7FFl&I>tT(+EcEdOF|EcGZ}A3O4%sXquKa-v}TX7pQDG|2Y0#RUis#|4jvgFk-ZwgfCi`5%w~@E zm7LECpBxpDpaG%B2|`a1ex-(gap1W@8v6$OeG)!OZ7xWA3yn-IjKuOP0ttme1hWL} zOH-4M{I8cF=bI!`j;vFZfsc_jZY}*bz{u*?LwCY*@kheFY-$(C;i@T_UOeUJoLSe6l*OG zBPBbaw}%gWiX!%qojc&fMlzlI76ry#jel13=kIy0wm18*7&u?wk>j5uhgsuKGOi*I z>s(XB4c2GO%R(G0#1ave$lUH8Hx8VO8m-_y?%J#2dYaRQ@!~QLm(o)?3Es$W5Ut;u1BV) z^^tQzF}c@Z6EpjqDG{Pr(QJTGH1JIE_4t}m7>n^AWu%a*sppUn28me zTZxH^rFG7FC=fXkCXapK@?UBW6~7J4(!hKDn7c3|K8q;9u20LWH#PG`s-|kLua#KM zN4a*x8ZMs#DoMp$!zNx)0aNIzs^33gJ z_Bi)v6Jm~qH(+jgKbm1CLDne(%2y)MV)7#$nb`7+B$+(_7O~N+0Zs}EYs8?y%M9I& zA#k49FR6m2u9fUMdu6Y?i1_jW^#}7wH~Gr4-}T^7VDo?wm~&t9UiB;{IX;3c=Ln0s zS6>dPL|8P>aYoWf4g2!|8^Kp>*|YA4Xynu!B-{EG-^^ zE}B-vpze$S9!;wZ)M1+lvouA=^^T2foOd`GUrx;0`=UL7B+tn1-_YK+jj>57>j~0A zwc~8ob%4jM4CrAVg2(+_57>1#p6&etlrGk>;50>WDX^hR78{l}eAWpvD&2aP`vEM5 z#gGt~_Pff$wxM-$R#{=;viUmGQ8PKV*((u>sr2AuU zR7dtqnmcUoHcMR>UX2JP`;%SO=mti~bm$F@2T_JypR$cpt<7Q_!qW6@u!Zcfft5nA z3Hd7Sba{5_uczoJl_Ujvhm#!rZm!Hj3{0csg@Fg^)=!zkuH9v!iMHChwYR^wve3;B z9V~Arxz+-^!Lx-g!CtnH9A4hG*%+%h%ofObEW}Lljmg&5cKu`}rf{!H7vFDX-f91A zLJ@bj;bB5A14G1NGI{4N4tpQZ&~0IDp09n>)~66W9-0Trr2dm_MuGuPaE477?1RGP zgTPCU=rqK})>rf570K~AXSZu>pSN{uA6BpwORc2Y<>!MYY7w@Ro}iw&P*d14S}4kE zvmu;#`8~eVBj9LG#ew4~&npWN2|d6!(8V2GG@8aXOwHxh&0$QX#uGa#&}I6K?2ID< zFAGJ1w%{16Ec9-7P5f+f z!~CP7I|;47&_!{0J(|liAn~SHpK>vG=!`P*}S}!qhJhB;&dBP7a zZS}^Oj=vLO^_WsR>X&1>Yo)v$kERtN<>B>0dB5hz`9CssX_c>6L4rOzUg5Ys;ld85 z@g8}Y_5&PwaVx@Qf}>}3%ha_QpUefEsVII;041!&(Br~w`|Tk$?%934dvIVo6xjpA zV6pJ5l^RjmZuZ{|LV7tFD5fM>Qkj6rhl15^bfRuEMu7PxS3{*rKrE}7qlNdFm zrMp9E27=_M!F$d7d7j_>@-Kem>|7_l=X+xPri2*&6Eo3Efu<556gPvZ9vyzcA_eTt zKJdwvIYM6JhO;}+EwscUkHB536 z$Q#ux3pT(+38P38?FyW0ThL;{+VNxD2tFFC9#; z`Z1}b+y*-xwUZ@-oLvNVGuEus(*avAjjW}8s)Ins%g>^poJB}T#opy3`*}Wxx_*f<7~rahcZd2h)?@ z1rR~aYF#$p$r*lN%-6fWMzYf7Ma~BIM%`y+28dqzQDE0fu_`e=Qn`)Sk^!!ZQd6~i zo1NSY7PP){l)jxUgWp)#>+k&ev(T{uEc!rEj!OmW+*gOY!bYBx$|QnfB3{;J=c75c zY2azcdsSAePN~+UH`C*|mFaoQ-CyAi;7UYn!(TRC~%6I&+zW_LS{y)Y@LmkJ;X&X|nPkdRx<* zmQLBcQ1t_}<2D$b27AWg)Yu~Xit})`?5<_)75aw?Dtwq{f*uFf?3FUWK4~XGPj2<` zdFBb*=$lhp?Y}*>q|Si1MmC(x@!^H?UW&~&tX2{xBFu6ZWPCcWBt5io3?Gv@Ag>}R zXZawQV}*iSY)r*hTqjV%-$g-SxUX90?rza(0d0I<5{dW-x_8+UI`<=bNld)zsI|_- zDZOfqF~yE&h3Ols8t`g)U4?dx z*Dd+M$6ru+>W7;+griTnj$)qG_J#TT&PI)!$1(@}=6msCcmO6PHLW$DNW^m>=onht zM7sqU05I2#*MG*yeslwDhEkv7s=66C&r+h%PYsX{J|UgdznBkAlvXBz3}(T}U%)Jb z9cxR?JLR7(Bo4({!pdflx0g!Snci(7IgJ*Ai;6fZ|K7H)$`e*}-mBHEsv93HGB6ZJ(&3;asXyH~tC15VvjNa%J}M@PwU`cYkW=6|JRAmd(}|CEd7xKsY(dcbz9%NX%kEA)@H^Nb!v$-P&t zCL=EQ?%65dUW}AmY}k4vdM)zg{+arg;AsHnqb`?6Z9xNKkTU=k_%Rr6;$fyM?=x)w#F0X zFs}qSsp<8^D~4E1$+t=@3CR~F?@3||X@&dGgd0mcyqybG+2!c6ev*M-JZl zqkx-Z*QqRw-I-fUwms8}WO_^P$C%qa%-$``2ASun`N`2N`pDDO-^qt=%SNM{9AqfJ znPT*y>_uow-PreisgfMpHuKo(8$ButL8TfGt>4P{XzP08ijevvr*8Rl(swO~M!){@ zo4hee!McOuZHF z(=D^TFkTX`gt(f%eN5NxC#z30Va}dYbZP!V>Ijn_&T*T%th2MwAo{G~*4EXsKamuj zs(Vg>&>|+K4Gh)xl)Ljq*v|3~cZsIVbSNh_!D)c>K@}O>xxUC= z(iP+da5w5Jr8n=iWb@(L(luT9uZ*hmo4BR(#^{ohe6+nz4MpDh>xbL--k50k*Io;m zo14)~GpjH>abeRMEXEv44Zh{%S>G^rChLSjLf#~wHW9PiB6G%P`G#dYP6b>H+-V(5 z6t-<%*^Y?R4d=Y5qyV<+#{A{zkuawE_*)H|7VeECt)^oBxzNxL_M{Qv6I0b|u^Lvd z`1}l3us@owtNN0H2~Xiqq>nw1F0kZeBcUleXM=4S54a(h2t;Sb$Hx(>R2Mc&gcroi ztS8FZvrj2!lwKTnqlr1eeS>~^)=ZJj{fpZgF^Vg|2-qPmQP!`1I`CyR@kRW>p5i!+|y_PXtE_&U@ z-^2Tn*E=ZkyfU0#|2*AbIIh-~zUUE@-%fp>`k>oVta8;{QJaqHiLSn6KMgP=J@}bj zuU|#q;y;#YOzBP*Hdhc{FRfy^U_7eI=Bs2Z z9pmED{qA=rxz9~amovTy8R7R)M}5w$`%oHhwl}HAj84D|vKGPxZQ;lZhMeaI-6B=M zHR7^PDN}Y4M=K1f!1xr#2H?(wFk>~vp^%RkXmR(_y$y^Ix91AgXa9bDMxk$+n^$6U zP4>nvi;N2`mA!_i@Vrb}yeq#Vz+hfQSe{?Ja!fqgBGUJL^<(Iq>&HhY30d!GGqz@Y z7-tJN>FM6&A>|Ts6)$zQT;UEiP<r zr#1ggHT@D)X&sk^g-bmDP%85+uN2@acv0Z@+MTVa-z=UMMi_xiE>Kv5S?-#b3W`$- z(9AJ}JWXx5WJ#J&t?ip37Y?fm6Gn5}pYvUpxvsBTryavw8D@v<*2=|em6h3T#60%U zOfIfW`%)KTL@ER34_^p53yiJ?SY#r^WEjFVPj;>wV8lVY0qu}D8N$v zG#C_e_U=^t3=<3vz3mg>foNY$VzJ?!tSr?~nSy`IZ;rVhTvG#NmX5Y%< z(&zC4w$l_15BDNUSHLtZads}Pr;kKXc+hJUomQyg34*hbya8O&KXA-~~DKz;Cx zf=3=z5TWjnS5xIaRLqAJMGjM6)!LJ)5V2#h<gncJfj(Nb6dUM-1qazr89XJ^%w{4CYmFm`m=~P5x5KsW@vSC2pKN*?4t?A~fh%0#*uGhzxW&lEcf-Cc@#8k7X@d)r7!Zw# z*%=F~H>rZLx!YKGM>sU9BD!^M^V+<}@%Nqpdc@H3jg8@x zSEpjGWVpgP@spKu-jwVk_^S{0`&kUpJ8d z`GlGRd!n0M*Q~m%`J7AUK3WS$Yrepo>{+SAW|y7j?RI~(Fp)BsX}olWaQ=RMri5$} zZP0Xnc%K(oev{7d;|V?{bVPKK7OV^r2Gx!+_%zgIv)(O(4N(JDHql(P*Ooctb(jc_ z?gnEau;=ULX$-3sZ;}IW8!qqoox-L8YIabg7>4381;o zDgSDL<@vF7wVYz})(E`gbFuFc>N{}@)@Ant^y@!5cqR9gdh?O4x452_MpAp}`-$|; zD7G8f)=HtLGFd$1YBc+?zl!yxQxn246Pshmb!3*!R>brIYtim7 zjxo-l?S{p^{3@ek=VI*(32;9}sB3j~wO!Ts&s>Z=4g7TbB>tS{?Q`IRW-)uDFXeq@ z(a)Z|ft;?E{8%)wfQH(hH@~UGCk9U7C$e5k1=lE6@)lg*d(a5RS40QV^E3=k9`NMU z){0x)LQBmU=plC8Uf$0$*EmF_T6{0OxACD=W5=C46XOXhAAR#`4hu=|Rh;_+-M?}r z#4O=mj=GuapSN(ASzPbpQ8UdYHdAo}30WiWPtX(^P+f7y*fylq&30Il?#ArFB%eTDF~AlkJ<>UBvEy?AcB;PkJQge1`-h2T6b7jLJdi$4?;h@4n zgXn>P5tNzB&NUDNBA@J2ywa`Hr>z>h{WNJX^vK zM}dU$Ve?L6_%M_4OYdkk_BtPhUfoeM-in~@qs})Ny}Dz*KLL4>y>V|;)+R05(!f9* zqx$S=`@2gX@gH{H=Tl8tt8@|JQz8fNM&8M1j*5uAy*;Lro{z6c^G`+=T-|FtsQ1xn z6~45;J~xE_>~gPfAqgqABW~UHe94T|?MshyeAZr)DRgYrIk#ZUn{PEHndKqD&L;;( zp~9)b&zqw(`1768XvfN-IfbGFiVy*0jM2e`dBb*UiQunT7Vie%sd*|jwe9Y55mwmY8tate`dva zI;Kf>RpG;5;Xwad-U(_Ef@CZZViP$P!sfFv9iPSv6UD(xFphSzMY`Rd`z*UrZ7TQr zcyuO*ker{8L}vH;C-O1&g0KNWFVFmzzwzG|y!3Q5{c+B7lVuUoM6jhB>2chpIPrr3;7aq7x2KXmu|xD$QQkoE@2 zYnYX8%dIl@RW%67mG{F-h9NE~;0F_U;-R(dH;OY|x8=5m_SDwsC=A*n5*4ZKEZDmO zJdfSqOKDj_?hi7z{o>-*P%Ka-nnD)*lMhzq88Gxwz7Qc!Pj|2!HPP1ut*q z#OY@^r~=)>!oxKx`7nL($x}hbU9VjPkpft4>UjS!0bLyk##Cg!qG!3VMf*^>>cg@z zKJHvABl4%(B@_*7uu5T0IR7z)pU5Pvz2rh zF%iFmc47`=Z!FnHx#I{LY?Ge19-ti4YQlZxW@>9)Sw_c#_b(p#;7l^0_e-HLGxKK$ zR-s;H5SaBiUH#~R8_UrzaM!Wl`CE>F=1hT4!njvSr9NM!>u;|4q^Zq9aD>En?sH%s z&X+6^;o?lPm|`Oznm^y|X_hBmfB6sX4ovWRl0X(7OrxzCZJH(RP;s z@dlLP>exwoDu8%#zOkD(VmSkL&^ea268O&!7qA@XLflspuQ2d|L@X+N_pE9XVfE!? z43kUk?jihZo5%xU2}C1^RFusEANFwwUqr0gyJc4-p^ zRD5ZFgj;BBZK-FM*gx6}b|q)!==vf7LQwYlYKe2CXmTNS|Cf`VcdKR!Y`yKcG2|Mf zfy<^xV2vg`@+VZv9O*kYGr{$3YcBTDeY=F_d>HPY&w|9`BfqKJ0%Lg#Ya{{5@c7FF z1Vb780s@|LKo5$n1`Vf@%k;D4`{)yc-K~UfbOM|_@gi`BN6cmeN@~OIxaP$%Z>E!p z-rT^3_7QmMT8wl&G)4$S2LjX4pTST7z~WEoU1&DZv91lR z*r_8#a;*kuK82rVPy~L>8AxE(#(TEk`s~&G=F6+AEg&|Kt;sLjbUr1u6q=6X zl(O#2zX~d7U{gC5?>0G%_^1z1d&qg^pQKw4ZTat zh#9)BSk@5LGq#k#tZpntc`Qh(i@|*#YlnNzrbZNg_p}tPja23nriAz~-sH3T@)Qq# zS%yRg71*ThpOiVLjL~*nCfXZg++I5lB%NXTx|soa)iogUD2iyQcG%|!aks-|!2;f9 zlL0w(Hd)FDUg2LB;z1Ql>wTO8+*6i@}hmJxw7fQ*aes-Qf6atF4(!bDLJ zbuxhw5=EWnh==D8g6v9Qs{47by&e)ruhG6EyF2d6CgwwqV7?a8zNbC-E3M+rPSxHz zyW#ee-spj$l{<7eT8FLish=F%(oL=NO@~86i|>%b^%-=;AG(rM+Jaj@>j<;iX6^51 zIr->=4HMI*jdI=GVpcasY!Yo6OAQqM+Sr!wZty@p(Y?}rkx9v>Py8#O4b8D{Ni zWdt!vfMVFOmKWTtAYeGL3-E-OOEZ4U>hSXbux6x{*F2h(TQrG>QhFDaEZ! z1U!?H!fm(kVBGaR@9f}g4i5i_l z5aY86T8Ad0r`?K2g6kebNwXh#D3ieCe59P|*=thelcD*%{ZwYns(`W=>z^s`2B^DQ z^3AiZX4qt&{N2(uwRGt{Qu=S2u1Q%JdHjj}i4df*@O3yytej~TntNn)I3CoTbx<9Wi)Ny4+SXkg zX6E%05?Oj^IX~(%;v_lt6Cx6-I4s8#j~{3U69L4czV`&1pRi~9I~n*wGAob1)j@uO z|Bk>hJr}+4Sj>CSR5fvzec~BK)or%(H+(Abtry4DkL8S;TD`z9{rlx{B)<+58DB-f zygQPP;5b2@maHVlzb=g5g(P@WWh_ZMJSP~mOiaZyprX1_({`1wW=Cc<5&PFu-=~&! zkbcyNY}%IoVQBc?rG`fJmd8$Gx@;Ed$59j6P2%n;i2&%w{ zpvTDBX1Z9U>F{#yWN@moON&+ytGX9+L^u&5#Zu64)$4I|g;pjS#D*(jRoyourbFzv zdFK1&>7peg21LDytk@)KCU5wlOE#I#b;9CYmJiGLt(eH>Os&Vcn7@#LzbjvghKf#4 zK@{nXZ25%bIqhUzE|5i zNxk^1*eO-TktC81itiqG#B5&QkH4?nzSr7P1hpPaN5A&`RC*|)a}u%1zW9ggG9?d# zxhh-4pX%you<3~e(Ye07754gXYOGZTO3ltD&8eQg!XL|5J?FbM(;N#tZPEEHu(z>~ zo`TGtVCaJ+03{nRr)k?4Ecw5GaXIjlv}_Tj!kjBHP^MwzOl?h#R4EH5UZ-3@XFEl; z;!F_+Ichi?!fWg{V`8o+&^8(ejUE=;s^`?%`w6 zA2*)GYE{UiPD$rGiLG%N>=bbo+i4H#ab{u!so-WkIcs~#uz@&Z+m|d0`8e8?m?0~C zj3(0tFFE2{j(vZfn|Q--JND*`&|v)Mp)8n9Dp#~5M&ME3^Rg#CW}sI_K%fg&N;-sb z0X|Hj1{rPJOJRA;JwQ%gFEM8#qK9d`=+;8%SbRyflgBwknywl7^=$B-sypXVylrYD zG4HrbOoEM(_qAX1!HfJz&VA^WXAuyM+@Dswghb*I_0j1sAWHw1Wlk)-K z%q!6$Qvfx5v3Gz|M>r4BTE|U>zVz9Vq$^-QT9k5H-Zd8&9xKWeJJo3WD2&v`i^&Gc zdabm2Y2h1O4pIwpcb(>R5(M8qn;n`-DY5uE0Ib$S9nlokZl~IQICGhOBGs}G%QUMD z!ctyf%_Yb@0Xp`5lpg|YG-`;3&(=?C7Vpe zx>**Uxgl{9B~s0Rq|W$^74aPHW@&dmd! zlX-7Xjc93m{TUhGV{CoEFXM{#Mfl3^5NdpJ;yYq?xZ4WBXzzNs6nQiWZKe=YB&pG| zsjSpAm(D)jV3}mDWr}#C5X!ETu{9@tglgi~mu;>Zh8$b%P0hK}foFRR79_NF3EUB5i;)q5y|v{|12E74RTrQ4HAN+dZ7i3KnN zz%0NQ8j>J_e4a(kJ+_syIVl6LP*NY?{^;aD{j5-g?U+ia<7iXn;$3O}FYzBrNpZN2CtRB3tcm6=mx1c2- zh9sv1apJ82?t!EZj(BIaY|$6V=r-7pp>Jv{Xe|db6AaOpO*nnxuuwlynT{~0Q~F+O z3?#ItOT5xZm3DUfX>=D6MZuSoxlUbaRPI`dUXQjZ*xw^ySP6zuZ>Fbt#|eyXy@>E3 zM7A!>lQ%8Rxv}L|B(+|2_XX#aZso1q+E(fuFK64Ql&L<=*B@*tyxdqB26(#xKRO8v z)I8&5Lm^QA-WS@(pFTi?LhT ziA{8I;jXcmpFFOu_T)c#T8 zmaDe1VyzRylcS@mORpyBvnRHT ztgedGG#5%};JuthdE@8y1&r3%P|noxL)Th~1o!EcopneAcC>;cwMZ zpA6$JyiEG=Iag6=F*)madb3N z4gik-dO-Z}Ghr@=lZ7$M1O@KOVv1+yoMbh-pI#<9Q{4oE!|3k@MVFcBj21Yxczknn zNF99wM&-EUYI^&X8f1SX01EB?vKqk{U;WkT`yCel3S!VFnRNGlm%rBeDr~0vLr$+1 z_pSD2Ybk0Vsvi?LOgUWanR+M*0%um z`o|)Iocv7&3{d(CTHdB zFs(*V+Ub}4J8s11Zk&iynwU+}FJm5}^b*B472y=tf0Dg@yHha&N09lpG3x>$#@@!s z&KK&8TRD9XZM!X<_=x6V0ZoSbiT!v9B?0J@c_yOZoS|)ufJV7IpWAG+)=>S*IRq*Z z8IzY6XTHKlCo5LhF^@1bkZ>3q5U>#+Q%`Ob$1ec{P*gHxj`d7U-6R0^=;3hz%&HTV zhC$Brt$LG~DEMyk`Br;0SdU^ z)BLYyzq&sv8!cV(=#4iKm2h?{Eu;t?%ex+_41f}2e)r?F@d*2E6JxCK+QRdkz+Y)k zkQYmvg9CcCYunSw_P@xqY&sz82e8>=pUdwzpWwgD#~v(9H_9ScIs>>rHHdD&gESED zBKFz|<9RwB;vYUBIO~es@wEtu#QWo}u9lbI^X$uHQdZb=e)X4mE?+@v5qR=GMucr- zv~{_WEwneRo_sTi9an}SuM-HCfNE{$iLFrcSll*$pLq-$@;W7mEN|&}H+ZOm(rdq* zbmbTBNgTzB!@RSh7sr`ei+8T=CbS^C=gPzFkI3w1|H$ofhCIb2a9^PIkaac!FC z^OT=Xr6JakbQ3oC6}L2CMpBMv4-NAk{-)N?3_nNk`LZacQs`cX&C7HyY<+p*TUHzf zWwD?3OI84g{!TD)cbSP1oo0J8WreoQN9a{QJ*B;6N&lKzZ}G6Q9;Suf^i}BmqB?DT z`W%b66bLVgS-EixUv-nEE>86k&)b5c(GU!Is3i?Ri#OK^=Y>G+KsC@+z~LZ}ATkKh zE?#0OU;{e$HyUO=cpVW|`}-5-1tbPz{o_V%$r@o}kd zlCRD`vJACWpycC)F)z&<XMcSy43v z)Iq_~0D4#0N}PfHfzad}%Y_XA5EqVgPpQk`FZ+JWG4SS@V8kc6zS&*qI|jrv2v+=H z71gk)je>8RnkdfmxHOu>NG`p77@nFCxb~9PlH5Jn@d(X*xBVE#@vb^@_1^K5l$Xoj zIY9TRuAIL^wOwRjoh2uko~A4D7-c`(PfQH5T~x<(t1$PioUBlasr1b&Ephm+GZlfn zzj2UySG1)0t**F-x$HL0Et=gIO~Kvnuav<>VnT{DPAN&#ilZ=L*p4pF=o9pE{PmcG z5AB4v>c3INK5`d`AY+59@0uKNgWC%PKQI4Bc4T@@o`F0ILaN8axTq7QuS5$@CG!-6 zO<)|)12VR3}UVA=QVE`oFJPd(QWG2(=c3viG!O_L#faw$$ z^8ObD0o&w=cCUiu6i$8Xu9U~0V((c_7%*PnkwIt@#hvG-I?>Be!g*mClWm9C0}g!pw8*~#3P0G_{~83$wikXO?Nt)Q^Pu#4yzg4wEFX6qN!oco~AZY zNQ9cn@8D>Tx2l!7D=Bg-e@rBhmnlKiAMdf`@@70x#-@loK7bPy!>fH8&9{SUj`|BP z%O^Zc5FC_1o!+39!kH<8_M&Y0sAz3vvQy z8#-JdaVr%utz!Ay?x^+hGg;=PoeEEq@kRc!=bo-?^t{VKsfkJ3`h z)~^my&|8o*yazd_R#h2pr08+VD;%ZjNDqxGIwsyyxsPsRxh!ANDbK07k<$x1&)GER z&E6nhNjDEy!!+P8D``H`1l~BCT+IxCF?M87Oiio9?R* zIeiwL{pOo+qL4eE2s6Zh$j{v)sG^}!-|%k72ekdYvc1${;}R@?oq9fbfuW}6i? zl7y0YJ-*wX6H1ztZjP+cn@OTY>{B8b&YdNK`_lRHPu2rf+tOJF+J_~grO&Nx;a5R$ zca0-2S${2c$>aGiV|B-Ktppy-ApI(TX5c8V6{8L+kIn8w=c>2lqJ`j}FsQ>AiCTR>w6Ekk zlt19GoenAf&hDac-TnLWV*!TRscL>!K-fozMN3iumbm*jVZ7%8D{y+Xz6({^p^{!r zt(GpjIL(`*OaS3+&>a`pWuuQ{`N&S}MA7+-%;u7Iap zV`-xho(sMXu~a7w+ISt0mpt^8e_oj<`@vzPKB~&tn^nIfxqM&_(*G?hV8!liMn#JO z>Uq5ba1|3tMS}zyk`^Yn3HTgv}d1$iA)E|W- z7)oOez4|PUBFSVR+x1=X>k38UpJAV;9^K zNBk_>KGv%|3wJ5xSn}XR(Oihydj26Ww%ZXXmLDKz4M0|CMF98kl=qGvuMbD0&K}LH zXbHDWpRoXv(AWm}NuPv4pf{UH;TVTvC@r=;#?c{3!0gR)Lt(d-XwUpWZr?q*P~xPW zT!B%M_vSap_4Xo$AQYZgshwCRqxYYS?0eKzp>UMo!^fLdP!{3R=j?=3Yj>M%7OegIXmyO9_JBk>g>P#gq|D_RNx&17LSAZYw?+I8v z4pn@3ATq8yL;mSm*ATdcPEf`Rd}2j&*ueJ=9iph$pBiAKdh4(9$#cqxY+9?D+RVFc zfm#ilzc2q9@WG%KE(K7v@ov!J2LOE$x}Q1eY&xY}O7;VRN155e+kk^)54eY<@+gm- z<6iX*M~^?%Fc&u3#$Jz)$(MP~CCq4$-fMF%UEPS3e`GZx{JV;A?t-E2MFPL6|BBgE zs$oeiX??Pn{G3loL%8@w{7f%6PsT8ce=LQV1PHa~(V}pipusy~H@Yl&@8Y|3-+N^J z>%r0Pex#P3oX4Jk)6|7*g(BYfA9vJ0geWkX42G|k1SeEC5odApiI{+g8OmA(S#1yM zola#?^%xtt1mO45&Vre;As-l;JZ2G)4MS6towWDJvz0KOiLsCTr6L-^7-1<$K8+U^ zSRaQP6t;cGepzTrUA0K57cG8vqhrGCN;xQ5KUIMDw+=j(=hLmz3`{e%cCVr9wqdhq?m-e-M{+)h4qh4^*~b6@VNk(vRJrYz0WV> zsP@Hxtl8tPxo)sSg|7_2^n0{6Sb-LNw0Er83tB&2odZXT$5JTWHmx(5y~a``jp2 z&d~sM-^bu&XjXs129HV`DgUElBAZ{Iy*@J>3zXFo!?Wny5F4At%1$T9|LL~`pR@)F zMyii0f>SkKzD%XLzO=*9*&4S`Rd>bvvk4ErJY!T$NPZ*4LC*3+J7#N@U9<-TFR9iE z7%I|!dm);`LUoVo_tuz*M#7bjTinErYR_F=!VWWq?g9ck-_mxsHgqLaAT{+X>}V@M z7M2ZVc>4~N%a_Y#v~X`Fs1&jnX~b@+o~-S6341T_@vNWx)kXI44Ru6i><NRbepG1NM_X(n%TsL}JNJ}Sbz$YA@ot3R6eZAh-=gMJ)7wUt zd*l8GQX#_)4NcQnSIJpPafdq8Z-Sw4kddRif?AQew}qVu$qW*jVQ+1HKY~c++LJy> zXY*^Yeoy{^CCyB9VXc!#i}^h6#!#(!A1L4VW%D+Die)D3*rfQHA@UO21HHXZew(2{ z%q;}e3?3~^D;4zo=^9{u&A_G*P7>18x+P2e@ua)w|GbY(fP%%M1kOYL>4eOrvwoGr z+O5FTTNLfhZFS{{Z-T{epW;jOkEAK+x0Y#FKa2K{!ksO-=G)dhVKyOWIYnNNyA)s` zgr}(EM}vJBNq14F0^j#G1$0Krobhc~7IM?~XUe+lPqGh2YNBx~#sHJnSb-7m9&KQ> zS^$9IjkY>n3lnzpK+9R#(R!S0|Ami#>hjq1VIla{-%KV5tIMiqq4?4kaeefx5qSxW zJdLsCi0#LCUe`xzN7{YUor%+$W%=r;2Y?13_6#QjD((PGX?X*Z$jN5&L%Va4)S|@u zGwO&E96udbmK07B4Wd;)K)Kgb2_E~j9-G$BgygqIefUv=al2+Vz`zyk8FZ~PV^;GBYX+IWccZwU|hG&|EU#;VgFRXuDP(<{$bq( zs@fK(1C_Y6jRUT)#HR{oa!xI|iC<}EZ8(KIEP2i%GK51idb%PRtYoz2Rb&g z^>^I{-QRG~zNs@UusffL?t&jsHguHzSnR&2`4o#%KSHKTP2%mK>OXbq7+RLMJ&SeX z^~G&3_U1kTkm(zF!>U$c63h0K`P(#W<|b4fQ`ZtNcUvnRh1F1a`$NS#l0i#a=_$zq zHrJ<;6J}6zU%0Eu1+Z=8*DHkoQ7^da`!|*~6VG2NvLiQ~?}HRHOiv&D&md~EwTzp6 zAR&r5x`l5-*|FuHIUUOA={!QuFug3~i)5^%zY8`moP8-rgY=O4D^De_n;V$LvAP*% zqV?@FcswWX@7D~xpGEam660pfQ$Si}(DTW=Fhes*E_KQC$1SShK4tQ?s{*(TcUd33 z2=CC+!zi-{4+;Q`M0nr+H81%5{!mz%s#8KjW#DBSsQn00g$LIDR zDQ=MDm)VOsT5H|gHzcRGu=Uaa!Q*d8W9lDLeezqLPCB7F>F1NA*|U>pQ8{;On1>7R zEx!5u=a9gq_|&Siq4Ej!h`X?+T>)yFpjK=kiQ>wKkiP6t1)Xw9VQJkAxHAtQL?YRe z{A*o4HUP5nGJZSuU8;=K`89q9bGHJod&2i>^~GgGb_ZvFN$yi#u0D;=A5qM0$^hIe zfy^s0HO1FHc}&~Djw+uzrU=COlqk8&j!`+jLfhF8FV%9WP7V!lnRbxA;N3{50LbJ2 zK3eKoZ26&dtjJdo|HTq-1M?hyhAHWrbl>Bk`tS<>l{;*Z&z!tAL8&e)+7)3TJvM!}+!C)`bU;s1 z*o{BB@0k{o<};A4g5XciwW1~o3|C({hYz2mIBxr>$8`s*^=vy>e{(c3e#8PGnrM1f%=sdn)b?gIO9rl!JhU59P_fkiRID7||JR zCvA&|rHo_W_GSP5PJ(`Kgfp)mU}3Oh6oP#JSCmjKXgJ`8 z&1YmbjKVtOotSj3Ra~m;W^arcSnz~gdHGWm=|`1QP_J~R2~cYPu?@dKik$4x1gyZ%DX%htF)yRf1U|AdaXl{%QQp{Fgza>2Io1 zwGr(-IP8xUFrQ7hZ)a;()EP12vM8!f>unRu2LSPlvfvNme7*P)V*n=cL-3a;P}Yq8 zrv>!ECNB#7o($I`Qi;rVe2_h3Su4bV1|=Y1-~(Fg6-A2TcSHWVW!}i(tZK^E?jaJB zeDun=4wV#0M1im&wsPuzC)S1W=gb1bzY@yIE1h$< zxUXlzh+zYgv_g|;zolzC&7aLKAbymP}*D{MWt+9KmP#c>|>LVXZ?ZqQcF$U?$`p_alID zm^fw!M`=lv*w8;Y${-xQk)F?@3P3j~r@nlukim4=FL{Rird(3e6}8br7Dz34*|%Rj zkpvNtHybwGLn54da@AVawqAH>C+7NC>|4;~9a7ML<0WM*mDU{lv=!hOFu6kdxXo_{ z6mZ~Y5lffSYJ-hMK7aQW#tbElgandf2&10@k()KeY)|#5`><5UN;Me$NgVB zNegXR7^tKNv9=6&MV$mGbEtvBiDMK2zWMf+GtW4d%(MMkD9ZG{?bLA?6~5nh)y zww{b){QznYT`<1l{Z}=WB8s^uvn4KjA-=n!7l}zaCXP>wid8AdyyChz02u)^(*e#t z`f=gQRq$Bw0~G)8zpSJ-bGw3sonbD1AGvawE2i;m`$?Z4g;wc3}C%sU>^u`Sswi-1bC^~l}5!?ZNGhuKzCmv+8CZd zW$Mk;qV@C&HWfpO1ph6NZ2iD@x)3(mzeqdF2Idfn`V9q!{g(lC)&Ja0yQaLd1y?dz zYiJo4uZ$7&f$SZN8-W&pRtGt_c!I$i1xWI+gk7ke11rxe_i5!(hYH5JlV2DnUnLx> z26e?HBq1IEL=kBtg(>OW+C}kGj>%&W!Avq!2s*v(d+g$PDcj#j?Kk}wRj-1s5N!S- zj&t_kV7eejR)~(|Hl5y=`8u6JksqR*f_FJ^-1G;k75@@OEvm<5BC& zjc3ASclq&KMc`)wn6UjjO*$jY_dPXGii1K?t_h4Yrqkp5BcL_$KMWfPs7m%nOi^qo2pB#=@ka39FMgp_tl7g4G_f|sM6)gd&6$VrTtTV zU6$#iYHIb!n$04lxW2%;?0?DjpLi#FfX}7<0DoI1BP8D)tFF8SUeyyBMTz9{t-_8+ zH7`d%$+9*I_<2=2bl5O@Rv}bQ?vC)X(-EZ$TY`x#{BPXpUS(utW8Y1n;U&FqPxjYb z2+Bbr@|V5R5zb7FqM^`OGTJ9NE18+OXK4%npS}E6I~RKjrmT7c*yL%4_oI^~^CE@VgXZH6Am|%SSqU@=uv@O3AbJ&uZzp8#nP|f&i9pfJYlIQAwB} zmm(x%gMXo+IA|emvC(IkQ@J0B?73t5zsI@>=6Y1HkLZ<5%+!rd@L}N*8l6KH@jNh}MR`bnEQtRmfEScoYr|eXhO$ExR%2(&Bx#%~(A^&iZmn}@i zQ=;g0B|Jt=cNrVc|0Rx=qT-SKySJ3E0g@jMM(VcDI6RVlDeXoX{^wAYVCqmz%R7^b z-R)@`DJSj3>x8=Tg^{fII}kxxU7GyOOQTsE?1FbbDMSZ}3@9~p;%nVumBFw9`9hgc z8L#v>c9b&M67LS{yANFI8W|ZKnPUD)DZQtdw_%(4C4WW`c7bwWpkoYH06RNy%ZT^# z02%_SzSA}$B;rV_}5?$va09jj~ zL2rJN$Xz+*Le>jN3;2pn#>#vSH>;ZszE_jfo^vt9N$0NUC6%y)LC!zoOg%mS5zA`i zXmuqk^$@@$Q9xV)k+cAPjW15MB0v@bo8B?wxJx~kVe|8qN+OZwZa)7LB{}suSUhLa z9D4v3?_J@G?`sZdaWOZY`WiRK0L3lxF#hC6eqmPFU&RL8veI7wZhnSh%=}Lk=2yP(} zRcpvcM9TVEiYx~Ox9*z3ldt9!9C8FNVlX89@(qe$hcPEqLWeW0a$5{g**(Ae7=U9H z!!7}A4U}!-$+cIf#(_OnM^kJqQe#8!wTp?#IWs zR&uRn!zHZJ`zWfa0vu7mdVsaG#H=njrgYl@73O$|jqW zy>~`*C}ot)?3umyb~4J|A&$clkz*W=WS`^kyN}-Q@8|P*{QfPEdb#gwJg?{VysrC* zcvz=pw`0H^^8{f%dJ*eGFDWD-4@o^=WBoc;^02mQ$7Quk6r*+AmDTkfoD zlH2zH^zP8<0!RahKnna-C6hf|WK&W@C-`v{VEzb>^sJ-<&3hr738`LJ&@C3jN~4XuFk*wHvV_L0JM-FMsA5!+`!Or1S9Tgj$wrB$OFPw<))=t!N z+~b01S;-iIV88{P^pz)Zopwj3Hi#->ZS?bZ?oaNEuo* z9r((7P!Zfq%AkLK{2cbH)lJoPu)os0dKLsmon7IdIRdD3(l32CL;366Z}!xOcv?KC zx0VWwd7jE_()3fS$wQrWANURL4Wv&A2S6Spndmrx>H0KYM9*KQ~A|7 z6i(e;IYv{M7Z<*hHno674vgAazq+d(6)<$Fh$x4XCjCnG{Fl&u6MG=b0`mu^D zf`gh27bW4XqoWMkWATxC*j%-{$V7$5%sn{<*B-uDb0bKRD+Pr;Fio8SkG%gGR55Hk z_&6KZzCd6zmb93xXdVXg7z`tRPJOr4vg@eO;EkYlw=2Jtefjh;&lb&%$$~dH2O(Nn zQBz65;9*QR$XKuFuNn()lbiJKs(1cnUr40(D{@HE8a#G%Yd#kn8Fb$X#Y6k25HciL z?@Sr));A*ozo*81Rl>x!T}~uoWWxL^vR)x+5|mAvlw-g@uLZuHMQDe+7Z|(=W8U?U zwr5s*OpPm>LU>bX)To*fb1hr&rZ+tnwpzz%E|qnJ+mJh3=QVec?951U_@(crkw)AX z3Mw7zJaFn2+iAN6ormyp}IcrtRP$PsA3`@$N3MGXbhB%^peRGBKC-swsmzLt5*RE&Px z$WLpvKXW9Z{w$C$WIoArAiEemYN^`QDhQR}z3YA%$UAk7meSD%>t~mk=dQ3i5&`Bt z_L6G8)x#B1<#n`lkluHPo|22aigaPPJM~CoFa{a%V49t8rztQOVej6{YK-ZCNO-DZ z#02Pw?Tbk#R(Hb9K$Mf4_BQODMW7fbuZ1tF=7Oe-hMk3iL_p43IPf=j(^Pf5#YFGe zC+#qHftE481s(OFD%^61E?(;kfuU(oLA$p=V&=eH#E>B{`m&O3fAY(iTSNC7`w7m{ z=HHijhIZZr;e`OGrDxmxI%WK_+gac=;S9+!!h?IhFV3fke{T%4$wAy=+JQRMC`b%V z6LJF|A?nnyN#32{z@?Ds67h=0Dlgvfxdnx>LvF#_DJdXv7Vue-9|Uk6E6AVAQQvCe zwTEYrqan&7s;O8*(b$;)qbzqNkM??ax33MkYKm=317sjDD4$Q|%Ez1UgPF=&i1I@ZPUODFV)7G1#7- zp1!2jr?;oeOh-YY%E9vhZhr1@~w1V)E-df--Nww3kDX0mmcb#eqQ&DJ@=qeGSn_7_94hfC+(8kpNm*l_B3zQ0M9B%XAqS>O!* zJDK!zyO{wc<(%-TxPOWuebHb6brrAnz5;U?#RAK*`=y^#M5|WusQloc_Wp;adfZ8u z4F>>Gp7~l;Tz|f1kKv6FHxf7;HpZk%$Z+L2=8VwuoeUAp zcr{R+uyE8U>m-L63@YfTDn-H~oq6tlLrR$hOez%&g|Oca5mP zfV&?xyhTEHI9nV2r&M#Da&XA6tmZ%NJEf^5+48?oE>NT6w57+!D6e@YH`eou<%O zLRz|#Qzs-S_bQa@C9U(Ph3HZ3I(Y49N(0r@x1U1)GlByOD5dF205&H#@w$l>s}@V) zVdxRsbVYE#xtHLDi9Jd1v<730H_$kS6dG!&rDI->F0WDZ{$@ zO|Jq5J8LRp`$iEN`8u@FUiH~}mG0C}bbt|j9@E}7k91>`=DT#l?nTN&K_zW}KNx<* z%t^;Ni(N6Hs97{*7p7P2De3tq1C>tBwrD>5_TU3-;`4)2^*V^8*dP6#9JL#}F0T!G za%3MFBh}~J1nNb_57aT8wHp!ZqZs}Qm8_ACumNw~W_!IxU8S+Wv!D7nUJ#Z{%X}>uzYNaxh8*#5Qj6j|M2FL zM}uXEZD8-%?*0tBeQu2L*ho*x;s&rNT$x|krv*H}GoAiq){nn#PDkUC#mhAhtN*IfjV8Y2Hx)2jZiAnVFqxXuj9v<3 z82$8vzAfoocW&*2LKZ05-@XRVKklZ_?Sm3=U-L&EbZBrTDQ|stadl+U@o-D3INS7+ zJl6k$yuAENMhPnJj+P$Lf$`p1ue+P>M4!Gs9*R?esyG_9wJgF@Xp;_+!W`vtbsA5n z#Do{LyHnW>N1$YfLHiC+Y&YaR`GRe5X;;0Ajj3=sSp1KE@vFxGPIR()#jOMzZmXar zoJ7Dm%N6T1LJ}^~3De|nPOdHF9WA!h3!e|TZ)3_JsALqu_4CEWJ1CU5GUmL)Q2tj( z5d2Z&n#sMjjYMm#I4%HFKY7NWp$PDDo=`@SeI}{5?2VEKLW8qK7kYUKKpYBjX*lo4 zv)~=|pF6Qj;1KET(3Gr=piux%6pzGbs`uo=KQu3bC^zg+wSmB}f>#Hc8aH{Xf{3cv zi1;01tx!fIjYBH&KGN^uUC+T$?b#flH{DQ@k;=h)BHLFY9w4~c43-$+*Cx>BkbPsI zW06d}H(i4`3k_yqA2@caXrr`SNSP`ebl|6@+};?6exlvt-ur|+^XgFYsump-s?^rn zFuG{Kn+oY@@~9*1OPXerYEIx37tdH#u)Z~Kgq}KM(=Zpy(*76E z5#FU6>{M^FbLyz4Z1vpG-n%j%RxyZO^?k*74QSBv*5<&5T?`?0@VY)IM?!_6(K z7KR$vULO;+aU&^nBaib|!73*Y8qV~e8VJm8(W*zfd3e-#npsV3zP;p+@&tH?m<#rI`}}N25Xq+KEkvd zWLLk{v;X&W?PM+fat@ppX0!voT*w!QO}wqEC1_$MnJ77ZPjD$k)i8?8$*c2?tom6| zSYIphxa8VD6v%tBP@RG*+U1)HbIe=y>m5_edjGvTf6Zt@&_W@CTE;7S2&&kDf1h>3 z8ct6+2^E|?5Ck#>u_I>RaWut*JI3C#qF1i!qOP|FNo9|Xulc`B~$KH>|4rJ5;%~)^w3^(gp$An}wiGO(<1|l9kFclF0}vtoP`y!nS3M$Ze4* zY>D-dPX`|ssd-!ks0JdMAmdWf(cw1;9}{N_zy;` zBv%R!d!Q1z?Onvtewnkszt7i=Bn`}_2SB`;j)fc+)#gPy$_?nl20eZHB$v*k=0SqB zlcaCmBw;zee)>pxl-Z)j6_V!U7vDR^A4mvx4}ym(r2rVCifM`Z>hzzUeNyRs*{PNN8olJU1&&bu|ukQ>x0|0&lu8uW|nu zuF^HlZ{xJ&Cx>?13bV$mcY|qqrkrVE6t^lpE(0!pgmr$_Zz45KfpA)nx%(ZQ-G;Vx z35>t)v#v1+$itr`r=EA2lCw%J6bo-DslP4^%i7V2(^DibAE2kScJi;t|JVd2>sh~O z9B2YAhn?!qgIx_ubmpdyc8DgHL!zPw>ctU5>QFQl6;(&yr}v}BV4x!KWBTddk=UM5 z$sXfBWok2!jOR6uc`)Z&nI1gjmucd-5MyrDzY8MNtl%?Zr#Md`M9$FH+h3jG1VZ0a>(mdyR(|MTTZgv72%u@1KgJKGhK-@Ma=ywH=^xY4m&`QY9S8A@}? z!^r=lZ{(U;lleP;4wGFVCBM)aa%(W8#u!rv+T+)WH-sFVFR75Hb3R-lm8;eQ519Q9 zigevfBX{#0s-lOqctiy+ToUHX7a+yGQnk!A2Fl;HY#wcLVW{m0*5 z_#7~hN(+f?la@tUnC+oidZ}=DOZd$}cWDQfO_Q-R>!DApFU)Z>ko1$Lqmh7$>E$Zn zXlcqY7%t5SNb9V|Bl@%lR8=`EeyU^iN}hP;XL8-$-Kck+PoAqU4L#H30mf}wX27hH zYjO}Hn<-G)wwpQ?sP%U?1`axzshPrT==Bep87*LZcfe6RyyKU_apfQWw0`Yq7w{Q`PD3-ZBW08TgT2c`(Mp-B6gFHK=pWW;v@QDg|OPC7B- zNGMhnz}ffoLcUSt0RZ3%1|(3Xs$C>JhW3uGidNK$d)u@@$>!Php2tli;~xV|E3?Eh zgm}77j5k%^DbgfW7~FPY-q+B&2s|Qh{cxtObdT?uiIXIv+D93Z#!CdolIoF-;`BOi z_Q{Y70x|gU{jn6JqPPs>yXF{NXrqd}B@dt3w%-KFjZEJUwDPdn19p>@ocs`@et3}% zx6~Nni& z#BL@gv035(aw0-yI};+y_q;PWF|SQUw)k@yBSP{kfV|Rf?YNZ>gL#mdcdv8P&iq*? zv~XQj%eJat)#$a4Mu~g2^B?pJWvCn3WvY2xUMZ(mmD~vzp85Ed$#Ichfv|W*+uzDh zziV-!@c)WBJ^mCJJ;xPzKOz*F6~EVd*5BXnKjrXXLkBQ~Nwtg3yBBs{o=pbXX)`sW z+7d;gXfx>yTdBjIBasXiiB~(mAlueHWWO9_!rUB|eJj~`euE+9aqO;RJQ31|-?rocx2(Az-TVS{UYS%=MN9umwr$v-OEePfdNN zM)fQ`$Go;Te3u_{y4L!|t~wX)GI0h9`{Y7a&LBZo^lRz{nfzTW-}rlUEGxX>5#^xzm&*Z->T8 zm+*6>Mt#=W2r#=xI4S!Ifv+(wvwcT7`XA1URXG9CK!Ee{r)HGT%;9sS=jjM&c)&S6- z|5M@Xm*`Y};373>bDVv}{;BkM-hHHh2EIVw*(a<1m-M0(C=MJ{DqPFQf;mk5YCiX| z$;>SqB4W!Hd3EmO?%Y={2a=nH4nNN+fG@h%`mBVZ zWLr5a_{SKUWmM}bXHw#g%VcndFmne#X!NwY0*F4IL84*h|Ahzn*R3hxRk8h<2O=PJb?Qk=0kPqsOuUKHIZ-|glR|4=#C zH9zU$)WkfF5e~-v44ethMjVRy)v)_em@d zvGEGWCgj=a@bEg-LS78h)_7vcw=Tj;lZHA2>R?D-#=0NPOMQQToE!;~&la*r*;m!jd~ z>RsZMq{ciLTRPHhWG0cRT}t*0l#UXI|5-2aH~`F}9%D&z?F|U1Ew5La z@bAJjZe%Z!>ExIO5Br?@_nw~m2*AS0#N}&#eN6pt{>-3qsSMT5+`Tu(tLA-4zHFbe zsG7|z4oA2Rc56$bmIaZbs*;s1(m7~KqLwmWoU?cIP;|tDz1S*MZIOvy(jyJ}8&Y(Y zoJm~*ONVRo$p644jaT|g%VwiQyYf9HWb?dF#%6%T`G5BFv8K2Gf4GC!MMO)0IqgYW zqn!L>ztTehD>R;8VL(bb7oRGST~~ge9qjl}eym3GvT-v~lSguNqL&6F!R@*kKxxSi z6=V#PRp;SJWE-QeNS`u#Ubb*#%op>Vcud5xu`g>vTFkkVyj*A7R`P3csOJaj`s+&H z!5cR`&3$t4^6%zuZMkI^@7}_(;GmL&7ELZovUGZkbZd;U(jcQIn8Eg^Gn24SL{_;I zy@gvk14WhBi6wAwprU8Fqp??HKwDsg-v|o$jg)@|{vqt#OdSrf4Dl78mPa4uWQHa$ zgV;$S%vY5;H!@^o4hgcDnwSV#vd7})9(SMToB{XAg%_04q%E=xWibumVWSw5VTq~= zM;UCpoo(~IUZ%#&B9s?z8I^M`n6-0ZiBODNR zTRi<7);CsJD-$fUbkeljimfyybUoO(A4PU^C{#rNw4uvIAWEm4f2tR+&)8rnitJ~A zo-{KmB{6zX6|aQE|5|dx<`fkqeWvcW9MR&<2CLvO4OGLjN=|gwia)+xxL)7xB5g6h z8ld9Op0_-CY09S#2xqMbQ%bE(L-QLtPyL}P0-dABB^%a5bApD4{s5jC7MLl}OkNbJ z?%xiUj3tJ2W(P&}qB78(Ffnt{eD|h>lUsob<1N|lq=B-70|T6MbED~*b2Mhq%g<^=M=vi_j`5QNO;q|kYwZ=N^xp1<)#%4O%B}hs< zFS;IgQG5{)+jV)KW5kO42$jQ)2zV3XA1Qx;a~+UhdC^Qz(AjJ}WVGLhHAY|A-~&t6 zA^WvCIXTEV_%6iNXVrL{XEw3uBkOlKD&Q!;TAX~%j|FzUmo&GMnx}rgxX!&bOJ|y} zuO~%tI>WJp1R8w`a1kPNBh)jyF_GuB;vL~h-?T}L+q!M;YzEYbifon7D5CaJ15F0O zZIF|l2HZ5Exen44?l;rF4368i9v!UJsn6~D-gpVqa;!4QHeb;*HU7hK=Us=m2BX>D zuDqY~xRCIDM}HHbFWkyG>xML*C-6#_TDRg7^rsP?VU_Nl6oj7_hTU&B7;x6WCiZ&X zh`YtDNM1*wD3&O3?B3qzUOF$C>6clfFOfn_sKv}}v>6QnzlsN8-gT;|aGq#}E!G|S z3af|Qc=KPrr2$9*#Sj0bkoR8O;;&wrnVGqHjPdVA8wLnvP>VlqNu`BBn z46m9?C-`fOw$ZYoYfG-8r5`m~;F)rqFkkQtR-C0sTWZ4R!|0ZK_K2^Yid{JMe=GxgIOa$OL2}Mnh zx0AlLFZmMxjnZE#Z$B4T^2rZdFI$C54;7!OR9bsbGEkbSA2gvN-(m|P`UfBGqp6jD zwIkOAm}>`1Bkk#IJmLi_s$dU1xaT!qT{aGI79E~AvF{}Ew(5}Gu8fYMcF#9>lRV>B zdM|(BDZ_FORsp%587ISWw32({Cn8S`Q&rrk&;;8$og5UZ8$CWUc9bs5MbHGKnIXd} zn}}Z6W^Zi3WM|tY(2c zzI)hg6HUA@z|1l=;?w50;_LRtu%u}mBeJ>OXQL#T z{is_YrH+=en5ivGbVT;-eTCQ2;aUiS{(8P?Is>V!k~f{?)MPS|hd!5qE7zsFT zp%84o(j7ho?au_=Y6(z3|6jPFQf>Pqq(&1fV{G>UZG)79Yu@ikMmVQ0@*rG-ib*|x z-EQK0Uf;pPNE|*l8p1$x7eRLp#S=|5mY0Sa_J4N!dlXGZ5jsHH1xb9|s5R1{%d zimYha&c{jxW?|Dfb4AC2ZBsljNST}REW!`j0E{SS ze7o2d8dnK}5(5&w&xwk6-q-y4adD=N`{`g89n=Od{wGI%q&D&&b!n1jc?mwEP>ezx z#j#lCC!F*W{D|oF1|$3j12uS9qyM-x_4c~ogcKE44DhjQh8n!repMLMSWYMx9K12O z_pzqbgdfIud3Jhwnx8H)cjFRMo3^FAoWMp0TZSzFQ_geKl)cw0s3!5RCrJ+rW^QFzbs z$g7ld>1mLJ0%|q@&Uj>-6B^&F-53J{`bDZaY5GrZqR6a6E_OUY-0!ziR!Ue?%u&GC zV)UH79Sq}aHRa8A-{wfiJdfMGbMG>UL#`itY}Az5NNHv1=ryHA;|Is3xaSxae+|2I zPH2NtJsFN7D}+8vQB6^}o5pC?9{=R_mnYM-iZL}O;Zy&sAGJy{?t{oCmjTkjpS9Y) z_?twgUed3hx{d%cBql!V+ts(;1e3155 z9A+6(k-?(%4CnKlzfOa-jB{|9wiTqCU)_K$-%!9_=s@!_1a1N=$IRmNllHlf%?o4BiMCWi_6*eFozq2c*$A>J8p(kzM!Os)JG7OcZS^ zOs+R%sPtfVFE@CzjyiyWU1cGeXPar`VkbN(ku9eQoc}?9gqnICA((Hg3Kukh=oV^p zy2WSG>{91afM>|IBr&u+Q?r@|-yzIOXNh|Y=qWN97tznau72#LNsi$FR58KS8hgx| zzFQJ!j@}b`P3I>dv*@XjpOjsEK}T46W^-m@2WpxX-;#({ThxB}Pb-RyxgK_+{@}*j zd!b<+oad&!B>$(DkByKg8Md6Q1!_~K4R2r}BAFa{W=Yb}r>_{oXX0gS6foGp9QBU@ z18dYTyHwj8Vq@Po#!)xmWoFm~>W~18`~!SxkHknJs?6KvK1*i=brG!YcS@TyB~?nO zw}7vT@p5IAq(^wG9`)o|6E2WO5rvc|nmeS@s*shgL3LMfu!J?PUtH#0L^4ca@Tup3 z=wCt%3nlWxli$dhD#f+(e?H<7GJXHTesOPaZ(5T!^C3Za`6AKduACBu=iBBk*^~6> zuISA$fXC7`;QevK#ybS)ng@z2(EZP@XyHa45ksk)qvCd^lRJS4E}!seUwcWl)H7Hf z5x}Jr`<)V0rXqgaXPk}ZW-F~IO)jS_$jNcs?u+dFWk=_xN+cMPDQ$t#)SpwNHmLb* zTyHm~g@X^!s|@%3mr^*7lD-Ak;CrKDqt9JfGRxJ0(qgCS=qv<((vE{H+A6+A*C?KC zsTosW$Vf^{8aCZQdEsnbqyPY%q_!W_@h$ zCy=-Ng}z`m+hH^aUGLL~HSKz>G?q+of|lj&uq(Cd@;(7P$r9hjbr9L$RAH4h`f8V- zmO`e<(>D`a{&4fYd%Z#SPEDiCC2ATY5a)a)^H_bA+oH0kBPvAa-_*{9bZkN6!Y=gu zC>QHh*wmKw@vRo*m0c ztpvDDy~EqKq~%4Gh5*k`yLSv+$OZcEHQfM~*@qN`SQGrueoWvCieDyrB|0%GTz7A; zd|mQ*1Isn|SpA{-5O6r6KI`El(A6Z{q>%hcvmW`Bvlv=Iri_VC&8xzA20Q4%&f@Kg zV{cCKSX?0Z){E~p$i#RK(_{jGibk7Hv@^cjQzDpmf`W>ccoBw>aa}}c zXBz>U*PXa?S85Ck^@W+b8N@vE6|4p5W|C324Cb*6MO0R`$8!*qP79L6?Xi|CQm6n> z1QOOVE%!}QB-=x#_2r+6h{GK-&w|}L>q5gTMrr1J6W((Rj-Dz0f9+j+IFwtsC#j7s zZE931?FuD>Qn?!`TkIGmjC+XWn)_`AmCI(5c9ORHrCf3umobxjVyI-uZ7?nyX)wm5 z!I)tJ+_x=68@0E@HTG_JYVxvwuVz>d2 zEM((yV3xy&ST`rH$gIw0Pst7jrIz$0#6(B??JBwkqgM|JOOP8>yZW8Z%o@eq^@Ipr zZu-pBK<7g9T!HgyGVju}49mvWiQQzq0~Ioz9J#)O3Xn&-IQb1%@|Whfcwc&Jb)wW0 zjq5pIuhj-DHWa+U{#EA*ajo?q8B!aDbbJUfr;Fo0Q0FM+t^)LKp=lq9F0ke+OYB8m z==M6c(SlTItlQo`r1bvp1t#3r$b|7DRj*bg3B@NnKP%Gtywooql1LZ zWjRWEnrQfW7Ct&I=X@dj)Lt|pA0ik9*Sosd{uWkVa5F~qQT^qhmWvFM&wQ0X!jXE1 z*wxjQLByL9eLR=DrE#|Oc;uTa`Y(+#Drf=a+DSttI;+wq(f+)pBl9kF~wk zflGvsJY907;HB!Aem6BwKz7$tJc?TKOUP1x!poL&e$r#~AnDkZ`%w6*;mCHinU>yI zSEhtj0#mPn8uic~gR`Pk&zn#z=Bk@^NVwE3mu8^PWC2ih^Z5=2^kHwk>g;;`i;#mJ zqol!3$GN(zzq+mr;*8n?&~L@{3Jaig)xOPDxux!8TI_xtEhzX?=YC=R?h+%3lr|1d zn535wApF-1Q*ZAhxrDWI`84`6Yj^Z#n?_=1l~d7u8}l(3uaXyl2}|V19V^N^(3T^; zsW;9Gf#{yR++R@Ae2SM_TU(=nKOfG#sVH>3-EPUMWm#X zy27Iv0=;riwwvmhP=6sR{Oo_E>nn_cOy-rUPBMNNZjlG03b<=lb2=fJ{tf5CpD7x~ zk)c|2WYLoI=FF!6^EZ6SqXV7j^eMq~SjjE>zOxf`)!J#bd0-T}xOyq-fSq^PvzUC9 z4Jm7la?E7G(P$_%AYi?4al^Ny_QHuLPu8OuZH0$6!Kr7@e~ppz^pEe6i6S!XERih} zNyNY}L3NDSQ2$T>H^vb~yuX;C#cUW<1f_YL82F+<#=uXLio90-+X_2m8c9+Vr3Z$o z@a+|MVcGxskf@X{?#hRu>i0x3;@S=oGXX+YhHYmLqBPVKaneUKRamG`nV(G_BlKaY z;IZ2i7kk{aQZfa%pFLdhcp(PFk7%=NtT8wRUTA!xZlIrj2MxQVrJvIhjtq~?4#&hi zo-^1tQ%FZydp$nHfXG|F`m1BZ5LYTg?lk=qUU0jbcCZq#!GyN^?OG#F+(~Vmx3EzV zEg;`%78WO~F3ebt-_OYE5KSJe&SjQ|<%X8W-8p-<3GPOz>Brkf_7J@b?bMmQkBeb= z7X22iucXi}4!ii5cdg~e*Kk(>B3tu$53N7Ae&{nQg)>}qU3Nz4x6XNS+&o!#-F9Dw zudlq{K|hZu^1!`PM!c)dF{6qZbKQnJFB2ULZv&rAN=0ynC&Mh;?dfWcR-&hU531im zE#$1li2I=l=A@Oo#cGVwgZ<^CgJM#1_~&K8hE@e+6H!{F6wb!UsO+o7ZDo`ezJrM)@ton1C_2$2jx0C8JcP=`y2s*+lof=E)t~KFNk+ z8ssf2CR4^g;xQ-V8#*UAV5hnLqgOI6*N{qzEFMHIM75p2tFA1n<3Zqg8=}lPuY$8M-PPU(q1$adq!C^u8>YhXH z-VFrPs+P=*eP;KxW5rPAm`is>wZRUL>hG$AN;M3>Pw0!%8P`~4!K#j91k>N>B!!6$ zvgdD*zc{0hLBl+~qd4SqkVJ?{6{3Vz+4XA4pv>-OxK>ZtVXsva_#{&;dvzZfuTZh_ zjF4~?_OR};sX@hgbz62QM`h;KXpToLGC5XZ4@WGW{qv(2LWcRhts6_Y`OHcu$v>rB z-+j%GQJOs#k`R<83OrG|>uFtIAP4e}(A2uel5Sqlu^Hj>ta#OmV_FN@sMi9!F=ld# z`BRh(sa1s~lzCi3P0X4ON$(RHZke7iK3zF$qyGEI4kBNYEfY(srh>YQDM;G9w-dYP?^s$MG z4ii5SR4pM%-fCw=(6mOs*qlMvsq4j$n8RijYsy3!nic9~5}07$IZ1gXS;K`ivoWOG zbF5-=iGSY`F5S(D7PIuEpKgP@>8*jK&wH;61k>PBFDoI3S}57~q61$MEr0EN;lO(} z$HeZl?WeEw%8+S|w;}S8J)9v2Wy(f#&XS9Nx^xtPXCx@Aju9NYS=;Uwlb*!+8u>Sz z+SU(K!ylH={aA@6Ot{e~z&K^*D9LjaWe}iy2g$cggP;?mqoW%vLq|gADAc24S{87D z<++ysP5o5b8Xw$2OZ$s0STgDehuwSC@M z<+#YVx+)Nm_ONUWgM~GAtQUvImQEVYkGmLAIuesE;W->nDI;2^tTyZ6p#&4xwyDr% zdfQMVYBWq%byZqDT77V#m(@30Y0%icIxPmig6%|RNE#B`5@}qh{$pro7 z)r;=o^|gP;IJum&HiZqI_?jtV>qXrd9a7C+9l|@1S1u3G8}dRk>zDbK8C4Lrd^U;T z$FrHTqQ7kvYs}@2fOC1%3z&WBr5O>q#Y>mXLe%u&rWhZV*l>Dezv-c$dgHa>bJ)i( z5LwCP?^&LLC8GQW*J)G8H$l?O{P|NM*KDCIb=0g^>0Y5fh}jhk!jX}>w!A1oeowzJ zl~`IjOnGrJjp$Yqy21t?zR3gV`jKIlgT%LPfS(&#-#nStfxvUU{W^JLtfn=Xa1kzz z@ng024G*(El?^=$WURSETzN6Hpk&5m%dOL?ATD^yR$$!2`g|@?{6fL~rYpzetA4Jk zH2)Zr5jopyEeF}H*@Sz;x_7s@`36=(Ho(GTK~+feqjkQHurU7@rDTuk2+tM|h#hX= z1MfdQ$x^tw=~Lb8$)(B3!=I3~UjYT~)d1@>m7(EOe7sV0*tM{9oc~R5z_G>ci-#Z- zOS!;7HNoBvz%K1Kj~x($+W!6ZYI*A!68U*~KfD=Mz-bUvP@|nNRyGH9QcHhT51RZ`87#w8W6Eq49_`K<_b-&^hkq zGFI%fzV<0y0xsns{y zdv^NdT#=knKvNT&Hdz7y#CcKED?y6Z* z@L2?W-HVERYy!VlL}R;BC`HFh9~bCn-^{bjEkayq{uR8rrm`3u?d_t=80wzrASz5I6Y0ljMOaSZ*zHVA#j6(9ufHr; z1U;tCvkA9@{$N(Oa`uY&#|FVs_s-Lty`$XH0yUpp-^ghXk8hdLFsra_yS_TFOqBEz zzWOD?ZFo#@B==n1eZl>UA?X|RJL@4X;PU{(=J~vPFCTRG#u4agcH-%LvteHzSp{k? zGT95Hn(3u{1oG`fp?rAYu7Bjg^Fee|lNeAfkA)Y~3;bwO&9Z1%Dq?Q@0A+tP5ufhb zgZ-T#AYq|FVnr7ATiI+Q`fF8(U4Sl9eA5(+SL`TcOp&qupVVDa5BfY|!>vXWyV0C< zg+B&Hz--Q6ipRoCSF%u*`HWacni8!xv)Z4TFQ|jXh=BcycAGYwgI7APbGXL5{{6Xm zOwjajm=kz@&5j9ek_tWK;AiAVLv_;S6Jdv`L2jdCfz*t)#xj*LmeF=sI!1n*La`yK z166vDZ||2U8-7wwuQ~AovQh6%&+ADaFr?)FVNWH z0FWbdf|~X91O77Q>Mqz$EFCLCT{T%Xi?ye@Ek-ZX^5#D>5b1n}>0d=gJ_^hJW_;lR z@qU|XIO?|MiJ!mA5Fr8r16%2J1fckBr3l`OAWFBD_@Y7VW-IOZ8Ne#H($D)gBMslv zKE+L}?f0a3V-tAyJ>7WyuZB-rBD*JeNzWF&lpEa2>-oNyjHq@7FyvgH>N->9+-rq$ z!pVFeDuN*!PTmP_SJ-$7;3muAE}eI+5ZUYCkvv&);Z_hWZ`FI6ahPGtEj|Om~@V~ja4`xuJ8|6G?s$3Z$AC-vZkrcNl1L7H1Ze=lG zK27-qGb3jJBqdX{fm=O5gJ^!>A%i?=pF)e}nb*OPnq2oja|#A8fsmGt20CoEyh`&J zsvx`&=#ibTL>w>@;VS5CVNkA}zwq&28oro=$g-A~tn`_B&-#~+PXi)p2Ox}ljGy7E zM_Y1pWX?hKlG>-j$NWh)zyE9CZkx#&%O^KFmp5`)E~~+DoH|XUA+*=<@3CeUw^?iT zeUtbCiB?U?NI03?t|whrdfM;1RDsRFc-t+Sj-aAjkF_aW%lh*4|0jmJ%QlIx?~;c%T8P zQ9WD*A}sp#%OeJ;qe+S$nnr?lKaL8ZTRCTROb$oR6aj4y6p`V&lc%x(1gBEko-VU% zML%aF>Nn|Y<7c@_+<5Aji_mu0@`69p)7-V|$GYDEe1JUAs}04cP%B*U%8;3R4XT;d rI{{SO@kc3gRr&va|947YNB_po`%7bZeX(ud<_CRUqf4baH}3rhuGITT literal 0 HcmV?d00001 diff --git a/doc/sponsors_jan_2024.png b/doc/sponsors_jan_2024.png new file mode 100644 index 0000000000000000000000000000000000000000..601df4909c92e83c7773a2aa47669ddd76cc998b GIT binary patch literal 184628 zcmeFZRa}%`)IClp24#YTbcmGFA>A!4Ez;ec1|lV0LwDC84TI7(baxEh-OT@>-}im5 z|Mx#{{5)LDJm;K!_Fj9fwI|@cyae_`!iOj*DA-bxqDm+z_k2-M?vUL_1AqC!HoAy{ z;)^0BDx~77yES9#Y0ew~>qdUc=T z^$)T%vR8wWF1sI>SbuvZwR_kceYA|5*u1RqE|Fh@RH*lOjumPkSLwl*{P)lI*%j%& z&;LAu|MzIgd@=s-IZqG;`~RMlKBEx+-%}>{lBNMhj;q_?}_aB{Z!Qd zeH48CND%#hpL|vR-xvPxX8gY`{ok!X`TrR-+T>409)4p-`~2wm>ytSPGJ10h3qN+Q z8^w@kH5`GYOg@?+l2qrXgL?2%lj^+6lLEwScB8qud551${aX|=BSd{cp8f%iu+_p2 zO~8O1#b=^Ow&9S37w<^!aMgWwzn}W~X~*xk-0Yj9*Z3yP0^(jJy?V!W-n4rw4XJgw zdgt%F4v)^K@X2?ZC}q;--0ZjIKN>rK zK3h1D*gtP_DU(9%MognXHk(u%_c}t?+L3mK@9|$IP}MZkl^m*j&DDpr@Avm~3@GU; zMn|egMZ{b$7?_JX3a?FZ4YEG)BzXSl*}@Y3MaSuJ)$!SSf(dhaMYMfkwF^Gn>GDrd{g)?({XWFpBpr<3YnB#nmOc&^Na)=) z{f@DP`#yPnVINHJT7!ae)M0aX?P2D#?0VT`8Ym{4N5$SJf0D5 z;{O58@|hkP)E-TxMXnKITC5(cd_+CT9>w4$VJHryW z!GV69G9s;8P?7FTM4eRGRlDu6=JbAGv(UW1qAE?8^yzasOem3rtX*>YuCZamAMbsp zQ)~&Bm-W)_0)NuV9t05RK@T>S%nDb8;HR{Q#~;< z#&@+8m`clmAu!%+Ay{6`wY#>%x|g4;F3Nq;Q&rQgLGEE^c^iWhS~1VaEq9D|(O1lK z^_W*zRo%C@$5bA2m~ExuKk)KG3^#=E_AxmNyqU5qD4y8ZpOm>5>hhk{c*e`OrvAI# z*sOSxs)^H9o&paQa+AMY$3~)T_B(+#@Av8yQig^a5zn8+JhTv9Ou%G( z&70HTWBv48h`LX1cvnTlrg0m|r?F9cF*6(zb(wHXpwIdVdgw^$Lho-?Y=OJI4|86! ze(xwt!L1MOoQ2`hoyQL?JgK}{UW{3hPHNltx!>aMFmc8cc7H`XSdGp{~W@aVcf2&F5*Tk zdxcCRb-&jm(={`BWDF??uJ2E;-9k38int}o2b6OavEy^L*ISCWG zi6aPN|E}dS^u|0Rld=YxrCx-nxEb>@K1E%D7;NXPF_)Ll)puO`=8UyY;p$@CZM=Mx zqN%19z`e^Rh%)o#xRHca6+;s%fo<-$c>@Ueqsp8dTbTtSer|bk5ftU@`Y7o^huDj`%Z%+2&j&PS{$tJ zM~wGT*|4c|Qb(jlQcwH2mwv^>jazD09S@%uz!Y}{)K$BCTeQL@pPjyif0~T};VQL* z3Mts(cRIH-Bglr`FbyhldrY{TAtM2KQZ}cM2}s zR#Y_v!+14ObVqMB8rEGautLhTL^a&9Vo%vu#E-$&&Ww?Eb~EGI;9foA*0Vnh!q^E=LEdOt1S7-xD2Se(>G6NT#3Jb#oDrlzDxj_#0tj;1g8 z9swe6qKmXsRi@i;IVjAcmPk{ z#>E#gcQN~iPRz9tv#-|&Cm5$|+*WHOU?p>{G|!*M8mK8A{<|uSw;}O+@e;R_hJ&b+ zUC3T0ml0aA(OgJ+?)UIb2|_(w=X9@g^Cn}P%7uBE-7I^v{hK4mzAisRD>NqV+E6Sq zYacu5ELoXXmCdcyB&lL|?8Oz8-%Ld)_gm*^pE~0cEKG6P%>6nn)cfcPiqG=0>e_EM z+&le_Y9gnw$oxOxN@qJKIFr^dl0FL$kMFx6mL<_ET}`pzyQc|t8rdi`u6A7h^Q{Go zz!j@s31+y={Gl>bdz;#Y89w5%mgwbgW4QcrANwOZn(6$VJN;2A%{^-*5IjRNljUlw zc9_hm*Oy2omI18@jywIi;sxC3rs)t`*F7##tK4WrOuzPalnG?qwWWnR!vTreAurS2 zLCqWXr^O+)p(t zs4c-i8CHM!&ZAImCUK2#c1(D-1@C$6VHumhBbl!_yC$4iaBOrmA}zfAgf%Iia4h_$ z;gg;+Zr_kts}ZO}Na3AEdcL1!6-QlqP6{s_V~ekHO7AzGg{r6@J`9p3y=wPW9U5P#nK#K=4TAYALtk1d>Jme6D^-t1yykcSN_V(v5sJiq@o?t&Zbb3fAU zTk(Xsy;baV*)Se~TxfFTUbtQ^17j-=3oa^~EgG7sdC*bJ3FNkHT$POF?JfAd8 zk0GQ8dUWxLVw*lW4!)C?rU&IV8Md~BSY%hTu;X;faGr6RfN-5$iZNlnR5|j%h7{S< z#5jITZulWB-Pz=DB!Gj;rZr-#i*@EvyxsJ2wfM~YEUs&@spa`|)l1wt6#`T82|-9u zB2CaVZi|<+kvoR_p^R*A;%=?FZgX5(-HQp8kOhuQoVkup#wRwoD6OCw1U)B+6t7?9 z`F}uV-oo2?=eXEgF!t6p@$w$|-vcETe2N(M-a{PE+u=k^i8xd!4#ry}jXXL$DW-SWGgNR30JE8|9-J zJSG9#mBFRzP)VN~syqF`YIQrcU)yPE zeV^bzuVc~m#8p+)SjSAV9-*14WQceD!aytF)~eWN*^3a z_{myzCay(2lT~n&m%kWL4bX}>6wLJ(%6SO#dZUH##`cCF}T z?H)Y***5h;j{Mo^89!!j_?Y}|`R|}tdy}o_i&z8bt zF)_(VD}`jfmSGa{QhWIKj2NE?`V^6lQD+-MCan$Wn?d>`+&i6ih)vle@Jy?Y4;Pv_ zuKUb)PeXb~98wUH4({K*l^UO*#G-Y*gXBBkYKr*SbjQTfLZvLk+OmIfIskC0Okk?d z?=l2hgjsR#{M=k|tZUv!g5|o1(FqOsM`-dCM@dkc&C=@$w|aFY&9c)y>|=e0@rz9J zOA>bn{OzkpDS-s<1jLJb>{66g87F4slFDe8%|!}-Cb_+T{~<5|^Ub|~$0c46U9YC@ zlo3$Nw6hs{#)pZFhz=#%E)lv#4VtfgY4Y8!{9z^j3ij62%rI3pbqg^od$B-oqy~Tu z*!F2Z17wn|5ll7cbj-*4qcGHk7r!{kPN$~pKh2peSXT)%OWqTA+OzZu zs%Y=!{}C)dzrQ!Ufluxm6R!l8%4KpLoq$T9ckn@jlZ>0)0=7BJ)PPN$AmTuG%zAhL zOO90kW;cWz6{EVt?^@A!pQw`LjiVy#2ZZ2!QDKegEkn#P71<#D_d45QH z%9+3FVJXLvg2>;+yz#aE@WWs(ZBu9X1<@D)(2avjAGl%B7cUX9a5ytX-H)33h-nLs z-FN;a^Bj*J`t|9FmCQPRg2EEMe1D)7WH6{~*l6~%tm@YsbnMdIS)P1$`Qq;VRHM8} zy3HFtuYm(635q%DH)Z1ABvpvcSOP5_h(>kCA*ppo0Z1YS+GD=Vl7l^|4pE(xm=Ezh z76f>6D9-;vj1gVT-CO!vmKN?fQ@Pp%GC(W!@{11kmw#`}7u2J%3p*Vp=@nvHg-VzK zO-|~J$v60VP-ww^+Hl+6*%#JWPf$#xc6cB`#NTK*ND#iyY;fDd-nxm2jZEb#Ow}{V zvwkQZAA5#redT={UN70Eya#$Q$F3rj%i}v{);^QVQ=GSnJ}q_cOHfwU`oHaCV1iZ z54iWJ%vi2@8?(a4h`HS9U$3x|AUPiYT@DnL?GOI_>#r1`W^_yo6F+s-nFXCbiXhem z`Qw3CF0~We>=~8?9cCCq=Y$dG*R*eu|M>ByLpa_Ut^6$%xkl7 za2r{>92u(z_1dKdQEUH@flq(C(0smf<-NT*p7;BY`2PR}i`sVW<_W2l$x{9G^fK*4 zjbAl3lJ+l_Xhk=*KZuD^1B(l?3_z?5XeW(eCB|+Sw6K=igz2%Cq>dX!b83~mjx;Yx zuiBg+S&XpXr?HcA6I%}P=u6UW_#W)?(ITBxYea0s7Ls<^mSjRXM}7ZJT>RM_ScZ8; z#2C03Z(Rn8+ZE>HLPsXL^s6ydU0;Q;DZ~xs3qY}ikU%YvQSSefeO-pfl^Hb@IED(=kMo`o82h8_T zE-bDk$=-4g>35P;;w_R@Y!sur_3^IJ1oq z_9rw-s;xnuNVlb*HKq2S4%`2U+DBQ8L`%yc^s0}en?(B z22@371nZb_jD*{Xnfhg?@X{kehcwMji#%LVQM}36-u;}!(j+daZYQfXjBP z{*wQXs_HusvRLcpx-fz__FZU|F-qhn{y!)3V@u`?2zfhD{tGS`2(G$3B85SR8QAWD ze`ua3|4u&=;QU4QQ+KF3wmyRra2_fmWa{w zPz#AAA7K=DI0gRg`qh4FU8hJysj3XPQGgpmB5)~qixpzw9MU`vi5qx}k8C~ur2KGs zfcP|+H^$JZBWlI&Nfkd`!hI9}FpS_|FX@mu`Fb-Sy9`oiyE-FL0&-eQC_2vH0@}Nl;0+5{NDWx_cR9z!17Tvcw0?m%i@WPVR4zEnS=`3Drf2txuWdbHzQ5ez zD^I_+^;(Q2Wv%kD06t<;XCp@#w2@rwl>Qd&h){Ch6&0rqc8l!ujZGKyAek<@p43hn zqvdA|+Ld|$ITm4Q=B2D-^s(j%j$Wr7H- z@<;D5eMimb(d((OvY2+!%2{l;eQ5(qkP9h{rF@F2G-?yGbTf3+c^ZO}t zQd&X5NYHmg_|5~lkbuyooTbue$G;Cqy8M3svR2LxzNS$(4{7sCR}Vw=na*gP($oH9 zfN21dr#^iU7_pINrqAbd*-T6Re7Y`~?I9}Ly@H~M9q{G_RcLEHsoh<#ag(a2Y@%Lj zky=|7WEyaph=7h&J0Xek$q3Ai$i}cC%|M}T#|vG&ufhFN!_6()H4-K6U#$b51&9gI zXnA#4jP2f8!H@b;-5^YVTkOQNzy-c(HZ5u9H%%z~ME{irKXD;)_41zhk1C1m2j7}Z zm?`f4bPAg$j~e?>ME%F+Q#I>#eEX6_19Ge0GUYZk^21u-8^ac(T-((5e$HM*ME6zX zNon2CdOw6CkjEL* z8G(2nP$eNAe$$us)P-if=FWEDe|I|xl?`+vqN22>@r8!go1MfGIFTmAxi`8yglW3{psO4Os4qg(zA zEq1h}yOKnm@a!5uj%I@nhu{!I`1i2Kiqq{X=mk!eA#0N1ifi%R3i)nOB>Q=-4UwoBP5cs?%$F@G-nms$u%-eb!MlfHAN zXRQ;wdKGT=cemlFza*);dV2M$jtzN7+njVfh^%HD%F@#{>!n!IDl02Xb9`JIUXrETDF!U;@%amy^nF zSieL$H^x%)=-#;^;M^}mp^=tca53+wGEJHESsy%`(4NQlzX4EKZd_up^8j_D9gwm` z{5|evmw#HQtd07?u3(R7!27%Gh-!j|DqZcwjgv1gD5bAoXsxCNLH_pf?Bb6?s)tu z>&`9IOn(PRM}exf(XaR$%`1}D@8Baa|Kxxo30ZBZtH%oo45$4EW5c={F16INH4Wb4JCFVL7gUl8Mh>xZmblK0Mp`n`ypMfq_M&8u zOMFVSXii0231;0b)p??%yB+1EZo`wTPPPa|Y%bB!~>FMb31=KWEtaZz# zAR;|Q=luX+#g5FT7@6v)L=9VBiq!#~K$0V71vE8f4#?%6LQkfiAT*|b02l!F+kR)w z;1jn^fzNwUc7+%{xcF_;9(O;rag@1bf#^~8+i6Yc12%O4A`OT_fp*MN8bfNt5A%?G;(v|jU-u{@z2IWy& zvt=|v5OJ}xRe5^tPxblzNeUfW`~TclAt1HAfs`+-mshczK(DI)0vNSh*cCKoz4s1K zFj+NX*qC;O)2_k6KU$`mA%oDsb8C-ykDBUo$RBW3I@^Lk%6x_wy>kp3^%y6vY@5oR zE5Uhv0Xll`&o)LLb`E#?Uv&b0399!isPcDlexS=u;BMH1pFMn-#Ho&{p zoqxHOum**=c@&L3S3ot%9nJzN|9qrt==$%23p{)dw>_AFNcuzcS;KrpwDKD4=Dq`{ zo~old&;!y@{KX%Dz_pv5&8kW>L^mte{c5ty@5)YFB3aLe0tG74Egw7_TCmW4fOnFB zt|!`skOiC%XfV=HAi4P~fZ}CwOpi1aF=b*Y9%9iIY7+}K?ibdVvo+nK;^SPQ8Gn7u z>9uyLsrI#Ia&j_a@IV=#b&( zgyaC-V)33__u~L_$~oGeH@bgD@%JP_@5FC=WNhvIHbds(S!|Tev|Z)c{DfsgK42ez zPgu>>eW+bIK$pb*FC}KHn5d#ZB??tfdkf{nCA|HHPxQVtUny9Ak8gT4L~by2R`zRy zozq;f3vOL!G)ysBe#TSl`Zo5)QHlLS!#dYcWQ})Wef>epxddn^nx{=v?mg#IP6z>g zspn?~ejy2zkPG<=wc$|?4T8tV0r}o_k}4hx7s@Mmi($(BBz#s2*8u)Tk1Wci>#DLN zPrl+)W^b1rzq(=K0BUgBTEAg zCkQPwGS$H%q0R@MNOGw*cV@lEf*IZ)UV3bR4dh_0^UZgw%N%O}B!#^XYvnAlc}^iO zAVGizl&{8qu3%_EIpW}9) zE~F8;8{Ibmz<@xhL4nNg-^KM35trBaJ=&tcQk-DCcEcZ&g(-8rNc0vE7={-22?rpGa*4BE?2KyWy9mimA^= zM$wD@^4)oPkNqUPAnd>a@!YEuJG(}Ojq!w3-;(_2bbb2qlze&hS=*Q8%SHaIu32Rf zUsB*P1Kkhd$dNg9N!Iw7BdBJ1X53g#jotuBfcSXpOJiaLuss180Bq5gkKrRiVm9kt zJ_ip2TN6a7p(&Bd<+tpvT6|XLN+!?I@Eq*)Nf7g6PO%V4#37qwAWTauTX#pCU4ia| z-A>El6&sDnWIkx8!68znKh1S-mn18!@#eJcxFb4H zR8!;f9^in3#!_$+)>=lrturz+3~IXsUZDlqpp5l~=l<6Kxxb+j z^sj+ywFcrW|7%f)WI(&Yyq3P<`(MbtkR9|MY9e8Wa1r6YPjX7;F`$1F zL)6`2F*n*dRLeCk0CL{ciFQXPeMA!N8u(bG>H%Tf)TwS4^Q*@K?wTq>`s?{}v{4st zD<4UKO{dSXdTZBt)Yn7Z@MkYK6eP}<9xNO~+Fo(TWvsXzk11I3&3$S9gNDcDBjQpL zfW;x%wSbnf0Mbt-qtz5tOKa*4ZsR%X0Msv#Af+xz%mG10|_n4*It zrDJ!WOb1q}Gd8&|cmd`j%}ZECqpbL$uTMFruKOyc-%a8#N+Khw%$Ldl0SA~QAicS0 zY+tWC)XJWyqTGhX-$cl)qGdFb_B82dGE7}fexTOA5y&1OU^qcSNneaO0NFT#MzS(! zWnb0AqwzNs)(0!*#q5ZeB>ybiGw5q4j}%$ zYU|XBQ+UmD#rKRK3@Y#he4aBtQeq=N0+A`sUUm@*1=?$ABD6IKGpLKhHQ9Kl;5i~V zVY~X+>WCJ#$Sb<^hbwqNOC17938UY04hlMZQ zZw0^{fPLoirMdG3GS68iLdzq?5wddIs7Uoi2Pd^miJ+#HwymxwfGsRP7N(!jgg0(` zT?rf6i%PU1Id}MXc?ZRz3$KPxrR9aX2lXug%izerY0d+-fCi6SvM}l0(Lh24lEZJw4;s;`Lg? z#KaqFo9u~=%0jQDpTWYw+cm(kza9E$bof>tOI>OjrR7v)Q^99%rl!XotbFwca5o|5 z?jSMMc{Zn1dI*7%a*?#^qoCI^+Mp^gq;+U>3A{Y7U$0H37%i)|_fCVR8yi{yIuMVS zC!h19yHFtn|JBAhhUaqKRvt_%EBFDVTETx`){x8>FF+JHukcH_`pW=ahoPhe0BY0) zWJ|zZxt@Jysz2PGFmN!RH~c~M#CKGL4Tkk2Ev9D%{tuU<(@*ECMd zHgk5-b%%OCTt+?m{q_VP7x_^agLImW+TggwX`84dC7&UX4J{ki2U}Rmj?a#`lPdk0 z&hJtKY;g`K$m~d zi_f(_1Cc$b?L`0jXPM9W1)r5&GpVAkVukTgfP6j!pSD@D(vHon$k3-fSO0~`Bq{6! zwnm_a9Vh^F-DQ}4!h2hfC>!7BA7rEmyP-6APtid=04e%LG4y&)yOGEh`0V-`@MlIz zl6NGc&96@`Q-yiF`mt{GRUNm-B4S!IPG6zwDB zWcuv1Yjd=0+W3{z`r8fRF_8hC@gF?Vxqymbi?)pVEAfjOt?Jhuz$ON!Q%q5o=ud-# ztDMD;)CKJR5R1k-{#i_GF*+R85g75vmtKxpb4z^}$u%xUm$0G9d0Ss8i_TI^Uy@sE z3ooI!K#u^DJm}6OLE4<083)R(u zE%(pJE?zSoyl2oVw0?1B066#KtOG+TP^02_+40`7z`%K~{wB1+Z>q88HetRY4r03*ggP=C!5;v&o!0~Mq4k^&PowVfNo(5zhol?wz^e1i%bM+Wk zr+mWq;64$5XFRWrKx9n5sb*L*#=1?9&aa?EeEi~bof?8$^HW%yqMDk9Pv@fg;YMH| zegxRGIT{N@3eYYukb;6YG6!?3_%Wglm!7+ddteVT4ayCW&q1Tt>26J#!>~3u6~=oR zGi{QjeKOpqZ@BwK!41Xhc(gn}2IhLdq+C z?^KP541Lk&d!_A0)0k>_SnL2m$avscXO>dCqtge``c$z)WqZFCzL@jltptcPO2f*;-w+}p=I=C?ChH@9J# z=S(d31}t{wz26mYYFEj+b?%jbJ~VKbxpsh95aep094E%AGr$H=1iorRXT@oQ$8@_! z#R7u&Boy=zrwrQleDEKjA@@2`^2)8a6%`TwBi-b=vD%)mf#GZEAQoQa_AwA@Kg z0tOJC0s>p|fSoimyT$3|uGh(D@@XV!Yb&BVnR)BnpqWV4#m8Q?e7|MUrKWOWqtQDR zAw1XKES5gGHZJOL$=FlmCd@J3d??$^)UwaL-d#^!SC_|nQmh3Wk$_y5Mn=%jlAy@{ z!&n!gWM8j%zK>d-u!**3k}g+};h`qZQdU=pJ*6T*uh)HzT?bOAL6BGz+h$Dy)~ z=zglu3h-mbM5`w^T!{kPKxcFCiust%nCDJ*Phm`an+!&z;_g!*4Y94BuptiVxgJjAD(N@qOccx&*?IzX)S#eGLy@aVW z|$)0~m)bL#i2ZQuwxCAmlX&VA_9&I)jxgYYShwqq*;rUy-K%aF^s zNhOa<0Jp5^un*<0(B^83xih)jU@w(9)s(tNU-ovE5n5Q&J;l1 z?T#beVW)89qXI~~58~9lNjA2!i-9pk&b;N>Dxew6KJXLyKKZvfGDHMDh-G~LZ(h}o zOY0kQv{;3sz<=DV&72l65{6eaZCjXIE$b=K55Sr2of^CbWEh*qTu9MXZbkaZp41^J z{{yq+^%Ufj`{rPDLHT9RxXBLC?%Fo1>)l@-w#S;L3JHH|lPw>Co)X6gJE0MsWx^^#!^P+GTyDW+u7fG$r&J=)OUeliLcM_&5A+F0O!!nHr(Sq& zu^zA(G|1Sr%|4{4s(*RO6iFTj^d+u;czmr$h(#iHipK=AF5B>N!dQV~wF6dgz-y4AlcUzZ>G*Vf#}0iMY) z#SIgh33Ks+iZ7M+KIH^$^f<%~1W)i1V)dqb9Iku_LYX_d=sXF}oHutcfmsE?9z5Z$ zadU4(Q*o-xPV3V_EZ$YzxPlOhT!1-=z=2K^Xh?4za_KS(JNe?HM8Ie`7>@#cnpTIk z;|{6RTA|e)3MCdWo;@QKRVN3}fhdsjz4vbA<8jSYJ$a#mg&-jSs|2zB!DGfOYqOtU z76BCuAaXC8-jKzfWo4~~z^lP9P@%^x`NP|ValU{;<)`h&-;%STQ5gMh)|83+t`%rK zcF*r%W(shZLD~iEFGvV3q^&XFVi-&J<29z0CwH! za(u;iBn1z2?4)YInQw%&gW(A4IS~bDJVT5yX(T{vU^VQk9!HHJKvk6n9h_fQv)FFE zqhT=}RBXJS7gJSr5Y1{N+fmMs9^wG}(^|T5scY+>mMGb9$y#OGjug_Dk*4~M@kQr% zGTluf9@h>bSg^S2HZ(TiuDSKPYQ?<&5E-p}wk3x6=@r{)8&zZ7avJCelaO_WISKr@ z*#Sd$^MoXNwm@G?dCU>vPyA0vfVYLF47E-jN(6{j?0$MGp`i7u1~n>nBW!}UPhjyd zi=>~kF)%P6n8O{FIb$r@9d6C(CYjq!%a>lsu=9v$J<8=U;mJE_`sSJ^SwsqeOMefd z16|=hNEq$rjBbw)4NwW!hAA~VF8vJ|pH}6IP-|VP-^6sZsN$$A)(ErS!s02+w z`9Bi~{m{5Q3>pRA&X!d#N%}(KDw-@q#bh8wVic3-7+UKNVDt?g5He7^Bl2@o^<^=7 zqLsAf2#q7DIk^JcKKdBjh5SKg~StA$$h{<4w6x+w^o#LKM-v!Kh^C|tIh#l zZIYk$j2HO-XMH4j91nIxb;jiLdt5$tOd!@4qZ9OG-FBCOlzqWgIRDFQZuur-bx;7; z9VBVM?d9?H9%4#^YnItHPn6CYtoF|5xejARmDU;B0swJNK{jtj)Xj==mu7Rbgi*Jq4%ns*wkK`()(hF9eQ$uB1;H7!D>68`B58f=1VSn2VfGjl!HeyWN;Hn zn#dBK50HLCfyg7w6Qdx@(3SU`*!d+o2=5I*#jj%oarzP(D$E`}Fy7(y@EP$h0%n6- zlFeq&rn{&2blf}JKidzXC+!ga6|Ud81JooMHBL0s|CD&;+?~sB=dB7UjZRt~)pMr2 z7kZ1Ay0@avhDj~3S>Db%QRV?OT^$g-85r2jVJ*--`jyFEHGJ%iZq>1h&_YUmeprLh z&2Qi`ycI;FrgQ<-`PzmCc6L$T5qhxh&{jCHIi7Sgy+_jzAQBK!)ph>_Bx9YA06U-c z3@$XW$}cg$hp+d%>e)q{>&sgC^RfGn1(ju}^07lNK(eV}Z7p}{T+g$rk2tu;=qF~R zb*ioM(z125Ra9Qc0&JDgpWQQ`qwY>Xk%Qr~m+VfcS^@Qm!ZV9RxL$fgNX-X=nEya6 zMBeas^AaW?HbaxV-rQ1`X^B}Eav zzsnABlBy#UA=HK8UFmO!^H=sMGoFI6prIGm@U=s9d;5^&%C+UqQ?|Y_5fnMr!4RJ- zK>XgYnEQ|^d5MotS6^zo>#3c2A$IVPbl~KYnF_J}fNg?F8z!)$yg{nXwl%W(;#Q&e zg5DsRI$h7i<*mbAJy7NW4F7vkgO6$hWx7b;;n;nsue(HX=)7(gby7-VcNypfev|!` z0bo_L)nE@r_Maa22gpbGlfpr1^nm<1h^v0Ke{%KYv)~%(v$uwT+t3qr15EVsK4#X* z{;Z8Vek9kY2v)7~*_)!1*Y3b7n%Hpl?VyBAQdP-*1q>)k$Z7mG#h6jka5=xAyd7b{ z&cHx3?TU_3Bj$Bz93yiV$QT2>WX_nFS>dz&dn3a?fWjA&2=zT+ub?gf4Os0wd&yep zJE~qdy>8{s{hD2w8!)o0 z>4Ql4%_2Afra7!f?FdbrY*!Mo211`B80UpMZ#_`?lxGmQ|v zc9Y^(KMotr3opPJkia_aXTcZo~=mPt-=>$n4yJ z(TF#-RE~`U4!|wHe-~}P@^(|N!NiLeubUFDQAKn;pmArU=rp1PI;dQD6iQ@XLt=)- z&qLIJv2mYACWYTe3w}BCG%Y>CW&dIb%x_G%Crfyj&|P!`KY&q13ECbQD7YODg5UX^ zO2f_Gec6%({88I4h+D&xtCs5-NuLh35{aXN@@r=}9uG-2__X;)O<1MsQTtkr36sFW z^=vf|;q5n*F?$#thSj9nKb~BSpMmr3_%=CN0*p%-!O8R!wp|7KA@;QRC2`e;{3(OO z(D-O|l^M6QLEGN%VCu!vPr_$J;mfV*a7l(FX&*QmE&@c;!En+he}=^7OXrR|IrIH5 zAh8g$y>*B0OG?ac*iS2&D1j;q=9mbc#vbq<`g9_JxmtS8-U^6z5k7a}BNlsGlsP-g z_)%Bk-18_<>{~Vp%~UOn$r7jo$Go3egVo=h#xo3RY_XeG0;cDg_bMQFQS``;+%Aj} z!*?x1Hg87)AnSxA$BOlQa^e_H3TRDi-=`#R7TEC5<1d0sZ$ox3@IvC*^H^qGJKxrv zqM3Vfbv9fVZ^?Cz{PAAJV!wlty*f(@DW|Q;liFeHk};@0PP)guj-~WikTnhPl-HF* zh(*EGm$y!8k5Jv~n`b&^l&70HpKZVhR~APXQn%A0ZtI<`9~P%K_cyt|4xo#sg5lIo z0bsI7H?ICTar3PuSkIaZCFei0U)Xo*G@BWpG>b||jUq7t8648EBcld$Ezl%HGxhbs zUkn?26Y~zotQd&j!1EIwtscmk_dpb&eS8fbODizWcuotZAfH!oZoi`~2*Ql~Y!2o? zZ!)&1h~(XzR5>*pw;ewn;A?oj+2aKTJ)z3Z*f}A7vmtErYtiA)o-MS8tY9)p_7TuA znm*C?I;OF5M=STwZhD0TGq+SF@EcMB68V(akZ$53Bxq{^*l$577UTno(HspRw}Yz; zRG+ExiaQ+Zxv~Q;hi-(L=Q2eY^JM=J;Dm6S0Cm=La1bk?mhU9ljr>sqBrF zSKvn$^fo0r=<+6jhwDvM{m0!jhhAOxtMaAVi=GC@?HJ_)!eBW%Flh^%d|tP@Qlz1n z5p|#4I!UFPk2SS`h`~2FfKYUK8gZ9bd_X8Rbxj23(_Zu7qSmzGzOA%fi^;#@WO+t9 z6rxR<>(ycb0PCcJUyzhkUovpB;*W|80(uRMbO-%BXDB?c@#DJF1o2J15A4^sE`pmf zHg(|X1^Q1%H*@yhz)O^*f0@QuQI(w;A-7fES~u=a6y%tE#5e}*^c1tnc)(AX`uY8b zr$?{dpMAx&1LLU&JX_5QjDymI9;XBJD>O>sDaOCoYC=lPntM$;-qL`jEA!Bd`h72aCGFAk3j`~_TmN%Js$%Sl#dNg9SR0tK-}N$ zK4JGF#{s^a9fSOZMC=HkXUn&)qoK`Digxkfi0-aORu}IZZ$AF@<_MTLCzpHn_7({v1bSBVF$D0mtsr{OeHh38@|)CiF-i7P|;yTUHeaV8zO{l z7<{Zq?xlMkF;e<*v-1$QRcp{5gr%npS+3hLk1^o9I?*FmZoLn0-TnZw45|I&YahOD z?ZTYNw-5xWu)_!=Z2wdn4EM0SOQ&Zn&|bGId|6}$F{~)SrC#FChO#(>kr;;Z`4F({ zn$U@`US5PiDQQ0{M`Rd(MS(Du?bGpO7reS^0CRm)^KLZEDe<#3HMsm*-D^cLmO*MDmNW=F3Z80hRa zq^4)Zlj0m3b~ey|%vcfz8l^|d@Fh|{uN16Es!%CNfrx@vdzLO>vlh3{F$f;Fw%t-8 z@(aYjNMk?g4O!{pchN&GHkg7a=6pyv>|lmd>xH8aCu_3{K>vQtCE>eEPP=q&;r+FZ zyEH;5GU$7^`R?sZN6atcJN=gA_d+kfXFT%`EDeeHSw010);M3(l6}i^rEllYM~}(-$QSz~BY6VRq~i@Bxa0?q;@_G>H^Bhv_ly6 z(*}GcV9INu&KzJZ2mFgxN4Ue{Rr9P!kty$d=Sfd;RdmsDlKCco|EUpdQ`ZffvJ-FM zwtg_KT0ICoJ8KxDlO$b9?kfOzWCAibvlotFjZJ=k9EIci-=KCk%HV_UUdCx~IVT@& z@Kl7{PM&2I;M_W`eaB$NWRJQf(8a{S0vkxJOowhvN7I7w zWNM@YB#6zt|AT*QiGiUcL7;_GB`BDL>z`z8D$pCc3%j$Dt(uu@{_Hll`5yCl z%41DjEV7ZD#_E1*JRs3n-6w?~(r%j*l?4BkGTdZRZiILq4Qq6dKQu??BOSNF^uN*| zl*kJb9hGRdTpb-(b8wxT zQY_Ch&u{uDJG}HfgVxpHPp=%VID%wUv4A;YAs^_IXNC*)SBpBAB}as8S_jDV8p$AF z!N1!Wpuh77E%n|%o{8maJN`GZmcIDi3AOmoZ#@B@pR5~e8Yd3A@KnW$ii(=^RvZ}@ zWiIv2dt%~3hyx04nX>9kPuSEf&TGrUy*-;-uklhfBQ@jZRAomp7%@sXpNe<=ngeDY zTk9F6SQ?0>St&gZfH|%C|D)T5D!gbFtdyD6E>hf^(d*iVnqO;mwo#Vg$_B zsVj1^O6pvgv%_9l9rZ|pC)sZ6SIel-ow)_5lBn(<_gBb8x67{&ouxcv#V=FQpM5!A zKWO}V4Y=9cA1|GPkqtp0J(mzsDeLL)g@4n}-yG~v#eXI zZK037>UR!~hk~Es31>;QmL|U9%36w=p6j_qiu|nGMYribjX|gXqs`|LopN57R|CMw zE4rPPUeFfIqA)D7;1#-{rm0%Iu^E(fiBSDU+-}hV<78xL>_KhWLOc$N3BKjHZPZt_ zz_zn3(C830p?E3Yf9j#s=bzGe$&;}UD_~wg{+J>F?RsnFnm`$_Mu7kU$fZ#pY9*bL z<`ubS;@(v_a;E|y!;h{B1pRzh5I-&| z(Fz8?(s$Y{cj%aC`CWc~H{`9jpzX29O7D;l{khw6*ZBFl?@%t?BrvNn>P<=@l~;Ah zwOtOZK=vf&(z+Y9p2{J0R^GO^DbnWl^5YiP8+IV*7^)o_lfUY^8ngR+$l8dEQh8&| zgic47+pd#z!)V#-6{TvN*I07Z930P4>4;}+YT=+r^iax>`RzWM#y)Jf)jBsORU zL^SQQh|=W0J&EC-a%A@1IC!f9E;(woB7Cn2h(KSk*xY9w_A+t8TB*B4L{p)``2Y%g zzkbkfKwEWm_+h|16<+X3>mxaA(4pLYdr)&)OqoMZZ<^*o@J5eX>u6{EO2KS;&uven z?K7C_doSwHs%l=i>f~pe92L^S4%G(9F7dk-&aIpMxG(m-!Jr~QswtYnNJP|#giy(0 z&64H^TU_2VviBh$NBnj*`x5pt*| z&?^IBcI6}142Fp=00+hg1Nbor|+P{<37115x#DS3N&9A2@kuO#3@a>k%5!wYktC4d@tOW zNO2;jgjJ1H+~1u-I%rKm$kGHjb-2JHM3L)p{8-=bp&Pm_qFuJ`TpaovO<%ppL-LmT zoo%9c4WB5hD?7~%@HW>`g?)}lx}UfKoqiiU4iFCBRHPC@*?1^7`1l+OsWY)35+oV_ zG=dnI2lq(mnZ;6`OSRnr$4g7>%N{JcJyu?aZ6^GwkaD1USB}P(FVty%e5F2r#(4gX z3)VVR4;mL?IK_a@Zuu&AvY!0?x~f$g`n<Z7*6-nRQBh(k$K`>i zmZJ!L9=C=8D@and>lg34eY}==+)~C>&TttK?t7L(&$oN^B=To6kp`Q&0`EfmGw^|XVv0)3x=?a$JXh z-MlFtl~G1n%WeBB`SI`+h^{ll*i`uqWCM3#L_t&$&v@wZ2mKl8VJNljp5a;}#<_H* zC5(T)#;QYeBV}Uf4m1Q!y}G+SMSW`!hR?*O)#oR&WKcIBbVyY;m^Fd zAd6kLEt96BEr^+&llYmpG=<k-r~of~qPNbGZzN zRkTD}oBI_Rm(&OtFzwUv(((y_r&5K!Pz&;|LHm&TtrDye9Y10uPY9I%go6CI)&s17 zdmkRP-<6UJeHmUHB&J{+=;2I^%qD;nYHJFC5BlINYNtkFoKs%)_W+Dmx)3r&L{jLz zCL6|2r~;i3!X}UEct`Tva(-1EgemeX#Z6U!{Z;p;Ekk`uI@%>3h+kK(l~Jq577L4=pt^pgho8nS@%wG7owkj#8#fA``+@%!>_M%%B@|xFuDLn~ zu^>Xz)0+Ow@T~Gn$eQD3eLiIOZSjHZfaBYXh@n?`Hut=ZAS{~VTsPOP_2hRP`s6f` zbB_Jq5Yen|b98?AI;tiYNkB^QPuu7<)UVUMBMM3ceUH&oU_{b6Ij5GHCzNO2$%H+WDeP=cfTWLp=Ps|Pt7h&?+BcnLdVCGFe+lsun z)VW|GIY(9Y7q_rm2ujC^^slB6!hI;#45Za|6*ofo+3!a76L`R+Go{vD;Zw%o-)1L~6PDc<_~nGVI&8Z%nrz{)qi*Yz>{V zIfR`>$Y(|4r}{9*3im{@Hcalr-odU&UC&o7Cl41S6VE?5t)@CM<ggLcMtTKPoA{oc|y zClOSvwm0mb-2dKvrpBGucXF68&=gw;71m7rq&CcEShDYn26>#qSuHSnB~BQeYtI(~ zsBwpC+|P7dEmUAILs(EomAGEB%a2KrT|&i0J<*X&U|i%saD`-9GShgj+WOI`pl=^Z zOu{RtdIcw-p)Es`@L@mofO(7nvn*EfeEqJ zV&W!=-o3iz-giI(w3;gMUaMXd==j(c%*ZZ7g3J~OT^}&bN@~3jzX)b+7%$?s_jMJ2 z3UFSNord^J<26F70rC`=&916!lJwafq)mh0i)b$h{O;0)!A8wRn)q?q*_EqT&7&Cf zza(hbk2|($DC61-G$+_=$fLScJDWmtQ;|Lax*tp z>@Rj#OQcjB<(!8H2dApf(?u44p5Dx)Sm<)cFC1saUwQh0`l<<%CkhMgt!VLA70H=Z zH9Z7Mjib*O(h_0p&^Qw^qoS_v(FrNek(hr{>7mDABek{snMOr~BwbViuXfQ}>aqS_+_sXQq{W+~BMF=c2wW0GLIyjjVu{$5%UfbGVFMP^Gd%jHijw#h6LD8ih`f`9 zbCjtvj6(I8x>j(G$abo!?)s5jt9sd7z)iu;kz;#>Xc$SgxwrG#TvHkNG1NUW?brD| zC(2uIUw5|13ka>6jcH$TaLg4DC=s7tmBFp6q$H@_(~?^@!{p_fpVM2pd0;asFb@bhzdnS|z;5Pxbiyo1f)cJMitB z{o>C*(f_=;?_oMFw@m!7+~L;-7d;DI(imw9UYmvM;!E^0+QK#S@9qL487J4#qWD*m z^otF5=Nt#PRl_{jStn~`2HJn1Aa~s{6uA6eTlATVeqTD{=N|l(8L1~SN~*dpn}>r= zye%L1J|#5eW>|0T%j3=<3Eca4-RA5>43$rllUQ^1o&=2^-F@T~J7(2&2^r&yoG_J$ zNOOkakYuLBDNf8n2CbV$NfmeS9-(Q3|ihp5A)7?}WbEhD_QnCX?@4 z!q8NTEqM-S2F19K(oj(1xY7*5U3ug(Y%V6bgT|dnD(^9U7eAWxLO)Y|^5tbz{?LZ0JkVmRSyE1$U;y3Vk{Yuq zbA~R-P*MY_EdFuh#~$aZCmXe>-Lu9I%Pr3i#a@T0vyo!sC3=^C{M-cj)~5lS56H+niX%SUEEUbQ$@%J3W~SJ@Ov6Kp7y`bcMvc+J_xTYbp( zh}{QMS}>#)kYK5?RK^_l$! zDjQP$)mYrfh$zePn`Zq+*cWV>T(am!Z zEk$S@=$0{TX)7B;)EZybwuy;e^ zQay>E9V#55K~*;@e8_2u7vJ|0Qm@;+l$ZCsz43A3uI2dzEAXflugsA;E%Q};C%O#R zRZk3YT@7Uts+Kv;C7GG^0!@{QJj9r!_FRW#@aB{?Dkxu%9`RTGTIV^fkfvHJCtgCN zp_k;*vFhAgScDi4&42AJScdH%kiGUcx*93z$o@D@M^3WhtNnf=K|!FsdVI*|o{sDn z))&nVb(aoB+u&9#;oS@^RveE<>ec)A^H{28jth*9oF-3GGs&VDFXvA<2z8jhxtD@0 zk8W2r^E2G!qz?I%HFn{4+*?;JIgf1y(GE3|hWp{|cRdz1u}~>Z#|&=dev*G9oq1^9 zSbQw=_{jgA-|bi)?Yi?PCstQc;>gk!kHQA8ERW0fL-c8@RB6xoF)q4tH&XFOu@a}I z<6Vih))c)!6)8Qta}cd&_EEj-P=4Qw-Zw!~%ZnU7`~z(1RtCY>SUJp&Z2B2e;Ar2t zUCzeHTahLdkWC$_`1rFqri4aw_t|1WHiOMkkUK-u&D#+XX&3g^L^MU&`D1N9UbrT2 zUgnr=#zY>W>05n3MjNjtmLO5hDfn9ac*B`*Ve zK2m}sd0f=|RvcRC+YBYF*7hs!b-3(*GNncRvoypy*RMoMig?Q77+a+|eg4So*SW$F ze}ZbsmoueDswAZA`LL}2oCrgkV}qMKn!YmVGOoO`_UM$okt4d;`O6Pvm&BN0p5RXo z%|-~>ui&b9Y~GI@*c5x~H}J_;r(~;AV6v7r@>^T>i*26*gN{BQ$uhYfy2t?Q?GtJ1#hd}j7Uq`T&QzVAhlaYXYgqD}V6I5pAAeA`<9bAeOe!JY z0RwqqyEd`zU@ZT9#Kp>^*FlYi%0^E?VH85HQi7aeT;#%<{88$F9@YNiw{P@vz7tks z_BRqcZ0l~a@@$CYVNzrn(no664j>_wBK`SbVY=`sm(5=Xo@_&=>})t?%Y4UQ=ly># zHe1?0o7rM?QfLS7HE+#Oak}@fwK*++HIsV9DM9Kwe9aEh<@h;L8H@9>)%mp2EjZsZ zx7;VBnZ5Mfq}QnMyVm^cU!3W7=qU&YunKCp%qIy)$NlKn5@;z5_nOOlA;{GQ1XfkY zzIxei#=h?ofq}_FZ(ZvMIdu^MUNl5yq^EcO90@Uxi)FEsYBh;vF44gCyB@J>WR)HAW`$3Z^pEfx)2F<55jh#m0b?u3J8#@JtgR` z3euVoUDt9-9^77`ziM(!M*h)ajcnUSYG-1zAM-WZc;*TZ3_GCinLIrUMlz%C@M0{p z&?YDxPYw$Zx`VWY_wT1c&Oc}fRAuxyx>UF`V7|)rygTZZ(3<6Il;59+VK>3|;*^fi z=3F6#t0hWS_dF2o5#BJSuL$MD9h9R+Sz!jP!y4BhlwM`>$Zx|Ik9Avz61@%`NDQP} zed!M(O)6f)nV+Y@{kF*yT7M8PP%=YdIp($d=|PsJ;q{M(d&n!QY_f0a63cm8Lc|#C zq?&ork!q2DCmLjkd8B-A*L%(Hcztn9A{t_XyVPHreU&T)4ezQ+fgqI#L>88^5B60v zoQg%oi++|_&HC3Wq^6^*E)E^T-wPiZ$O_-7F*Y__c*Y{fWz01Gp~QmTv~PrV#eOHC z)aAOMK>s_%oOdq3H$Qc>q|=LafB!rXSgo$F>OCO)$7 zud_z7XYMfPbRe%_wCno5AjOR2z3}fx!tH;7snbMiLnO2i1vzOP-~7f^6G1c-%?@Q{ zNv!0`q0Xc}tgCm30bnbz`#+r z|G=KOYZ%&dAm`m0_Zax5*U-9@@KdV{uIH(aWuXolznDk{+f!E?40EF4?-k1&zZzya zS*rHy61=@L!#{0*Lsuhl!jWMqPuY@buQwPSFh7~JZ~n@}l_5`~S1WOw&5SE?Z@&H} zxxN3nZpqh;C)aoUFMB8b0gMkqJMf0BcxSO5VVDkdZ-}=!{h;!QIzc^_8CYc|gf@K(1@6aW9F5e&G#4F10wI$nTjzp+`zFk{J+;=F{Q9by@ zoHJRXOuhOFN}N#JH5>bEy7@r;V;+e2e;yBp5Y$_Z=iFMPkI-Li^C zLX!Tod{13YR~Zu{mpfN$2nt7gEDKuzsVUz}aYTt7Um(C`*6{e~iGy`6n95;@uUVfn zoECY~L^%>Ki(zQEz=Md*#zZqLHo7^eazqx`p45Gr>_LN?65-(0O_Q0wjx2)*en8HR zy3Kt(#fnfK-$iQvdJP`y2^^};W8!25x=3cddQuiAmD&~t-fZQ0xnM#-(z)5Vy*(I6 zXP16Qn}+TCbafJ~sizNzUCcNghQTTC!22$x(Qkn!-A_zQ6kXjwGkMC|!8C&q&z?1$PD4ndq;PYPq;FS}G2W z`1lkQq=7%U;T0v>!^TEyGOtcY5(9LxU^4WwecpS&w?*!mf<6+TETF|4S3;9-DUyXkyw3WLjW;uFe28&?6*m8xS=FGuL z_hy9@5|ZejNb?cqlXdEp`|~ew2ZvVqw3$Z@m2bWLz1de|GYA4HJA5#QEt^I}clP34 z8P)3ucB=I#W}11pSNI6y-pVFby?yR|@N zw}cs16M<66%awH>g68b{2J!`+?U0A`v2OAY+3c4_z53v!mT|m9LCPlhb1kG4Y)v=! z`w=_amcNa4K&YuD{5$`y;AbfAt$X}RQJ^1*%MN!*)S|(2HFGzyFQoL> zWFt++tt-)l=er3Kiyb#0B=4XHRCNu)hH}5we)!4$deKC{ZGKB1nRr15-SN6mt27C$ zq#D2cIpG$lch1%^Us9>Q!<)IM)@-Z9`J|O975fSj!ymx?6sYcrnw=YnE#1bRYIW3n zUye^K^bcO)&QhfNe2e7S6sNbBgUS2U*zD;3hzi^G@pJEu1ye(Y#OF9|>AW0FBIb_C zUy4`_J0)@G0!#M~5;D5vDB*7Trsr?}=x|;sf4zGUr}8vSn`>U4F$jLqPV!*fq!Yp`dWKucWWq`H`kwfg`ru$Jxa&;$e?fsYK&cnq_9jz zsjL6Q=9N1vv~=+K)jaPCG-D~!8+O!Euv>d}Bn0{^#d}A-(o$E1jA;uz+=<>O_~G4B zFj>8y91)&ZD}{<$8U>dU>XDTgEVp275;ilDa7^JP?Y9@nrsL0+V$q%3?}UsAZ`Rjb zgz%XR*ey&lg>WeYqF1we%*b3c=X4E8fb$QA!H+|8aBlsAB|!)|?eaf4E$*^1orCBAySEq~VT{D$STl(A7xz)l4EvB#EY{YlN(#sut(}KY`=0Kbnsq^KZ zz%+y@nIWmD3bg;pro`@k?c!-mzR7o&7Z~OQ+{`9Z;esEcs3|fHLRUHEX(FCQuRE{t zx5s=7W~{quC%8#k@k_^@?maV-#lNAC7C+kRhluI5_i!$XZE64e%3E`G%|QrQgq~tA zu{yk6ED+LB3Yg9Asp%3RO* zP0q5N>KF4j*UgbWqWuQ=uSB%(|ERin?9060Zm%27PhIB{-)32C^gBw^rsiU|X5LLj z6s?U3o*L+GZW#FlJ05-a+a#45TF#Wf< zBoUI&6(xtVB?K)7d)Xe~{U{}In48;UkzVr*na#59qfPPJ;6w&41eh*1>xD-C2<~7J zR?@3WJ%solUDq*nZJwV&d6TGRS#yPyj5<9c8BFBC4u%Rs>!x@X6sFDO7!{7U|H{-uI>Xt&F{uHSSG++WkQIHZ*~XsxS$O(PtSC~F60q&coyVR* z-IpdNo_&%ooaP{qmv*oSL(pBqIXeT^GqFk6kk9xb5@uViw@JS1ypt75Ao(n$?t?NQMJs0~wxA%5NYCBH_RM4QN01GNkbAleir`!>wqJL;fcL zW@13Gzo28>o{{AB(L`2;xZnj|0VI6^vV_0@`C3{$GC)+>6=x2u< z)Eng*^mxenr1F$-kJIx_<$-A%5_K;?}i5$`EJplK3<^V|}x`bbM%+N|#rqNiqhAw?VEdyeS zo!5U%I|vy$=v4mPCV6XBgn`MKUou14Y5CV|(t9Oocs6D+5PqSz$iLfw-KzrC8PtWP zWZ~@_eC~sgea{=x9>Zc4DIbv5tJ287nf;uiD)9M4P=NI^ssmI_Z3kIEixjwhg+i<< z>*gV*Bqbh*(WZURt|mG^lm!y!>CHb$2fYiW8uBX^N&U}qb=Kd1NjX(9=hG*2QCZE7 zY=N9-Yb>TPcVf4cMs^k(WhSt)?+W(Ss}}8AHi}dm8pNNr>UTQHFS`P%>mcLHLLpB`8Bm0^7inss$X_9o7BPeAi?xGy|zfA>P zyvlvV?{>3ejbz$(SNTyQO7#vYPR0x0FSy@}B^J6&+-;v*EhdJd!NI`;dJ~dmQ>|R{ z_M%>TcBR8g(nq8HMyQ{F8kPHdoxz6U&{4>WNbXQQ>zv31xI{Po?=H~_<|uoJci(yV ztjl_+GcWuX{`=X?LOF?!1*co5dQ93LPqMtN2A|`ivT4B`6kCkO!S8avmvVEoFY{!n zgEJ{YO+YO7<=nvP_weCC^VJ4YV^_%BNP+(bplrNe(FIJXg=ca40Ob8SyOhe3&*Pnw zic^YEd*zzGC>u_8@|bkA-x|-YYxkkHUc&9g(8TidHVTY)CST#)njw`;%z8G~5c}h3 ze8CLrvN?XGiqMmz?TMb(_*UYl#rqY|^p(dGt-guZX(!|E&hx3eJsm2Q&xf z#T?DCmMg!(GZ%noJ~B%nf=)j)I<)zkfB8t+o zAHn!yWpwu!_Z+U?Sz~1Ph4YvQO2g}fS{+5j7GB3MMWHbXTyzhznF@R#{e2QT#E@mo zuf4Cc>fb>xO}_*Z39tkcX^J@k&0#@f!d>x4o57eXInQT_N{%O?A{w>+i@cAi4aM&yKbkg5$~Tmgy=|9nj!JAAA|cI z74iW&M)AeceCA#MZLuUjIE-KJ|A9kM5d*6?l|9kqj3Q+(Eqt5P9KCh@_coA8X3&v6 zH2AWa;cA+kr*E-k+f7QG{B`Z|Aw|k^gdZH^MP~k{`+|N;KOe+)M-vLS6w>wTa{-8+ z3p}cfuQ2KJ?Z9eh2zKj3Vh3!4B+vZspIjQDIPpaO2VN@&NM~36gN-4Emln$BC<9eQ zT;V@twba7TbQ}?fp&eDo^~RJxd9i553}{L*=|zYgmJ>Lc@-&F`*OyyS!+lx8Nf0RY z8)LT-^jDsRbbanNT*RGe#O(V2e>y9)*um<)Uh5F=)6vWS6}Q$51 z$)gw(MvSAeAu;cx&4>^&H%0f~D^Wn+XEc*e?M7Po=ydfuDZemYMfcc zC_bY*#n}E+Ux&Y6Q}@`nR_wC_PJbSRNMS2qzQ(Ah-?`}#ai?(3f$z)exL2xlU>|6U zKhxhAoO}uG(5>@i4mZsSNqh?D?0@x)?^~hN8pjPZuP>%=pyv3DsXJY+4sh@vj#Df{ z5id|mt$_bHzRa6JB2jd`gkXaXU#hZ^>}i|HyV#od{Hko}!5ssg`6x9Hp1uMFB!0|A zi6$k8F;OnWCa*`C>2Sxp#jE zXr2jtUjAeHkb3_iG?RmyIjtUBj(Mou2^GvqdwN%k2>s2V^{UJAF9W;g+24OYS=fqu{|p7{K@D}~S5mR~P+$JH0upv+ z`)v~D{}4vbBqW}kP|^PT1%yDU(s4I*=+QKDp5KK+*m%mN=mS4ZBu&%N%}B{IvqpvM z1cj<6yYpR<#B!VE-kxog&%ccqz%}2-O6%@VZ|seC))OyNKSn|!%DG-OQM%0Mx_G`* zNB#kO{Oq6hjpZ!>T%?)`e4c>Lb$ z__Tn-x#YFCXs(B=oz-`j!|DRExtI9+8s>24|HliUJjqcO5pBMhWcKEt=9Y6rZ=LK` z!np-B@xNcf5!yaaP{c=J(9MsH^7ds;TVel4pwI37MYAnNU0Oz7wUlkniFaw$5u9N6 z$vrV#-8iFya&HsXgM2$_Z(rw=P--nR2_(i_E2@{G3wa|#B2v4eNnwTlTaA0SBwWNt z3aIXyxLf4@_%i6+^yyu`yVZw(St#~4>BGTzR0K~ZCMf@q#d&F-jgo^2&G*q&__j}0 z>>>P;Y=#h@xT+3a6P=iJT}D23J>?$$j)kr+lcf6(UyJ^D?p!Fz%!qmN?gg1nyfxn! zegGv%j-V_fbX_FqD%Ct4ehFOhDe|z(FZQhj7Hf0C)R-}oe~$-fK#}2EY{u>DnnGY_ z-?**C-K|*~u5DJ?NbwjfgsUfghg%pDtS8T)SQ{<%pB4*kpBvjx%9|PqM=;yK4{D%0 z;}b|#1`SbQ_oB#+t$&Cvi zOOFi;`xF{J)VHi<&S9egg42_*bWX&zQaiyntfW!u+jE565}0s#&?X){P-a zp0;>(?mgSY^Er(h@RME}A)lGL#gh4m7TkXL3?nT{I=Q|!<>PC&TS$8fbhJ|-kbiLU zJl!FDXgT93zG6;c3R=Go#qqzP9sT7z!B!SvLXp)VO;`xgc=AT&X7&$+_G#iiH>JvizlrEIG zPP#<*g-NenZYmB!1*w_ItZ#;zI;m7NxYy?cvnxq3!ASJ4=ga+lxk3~a%|~R>GX0sl23h&_ zPAeJ`rm<0aj!1lno3;0cs7krF^UGgulFfoO1|bg1k{@_4H&Bl9j)N5flKPb(?WlPA zUF&^hoRFgjN9;1m^%%{Zb)j$($*Bsv+HP6hje(g-W*)Jjv5l_@iOXt&?;OhN)&yAh zd$5%;S7dhnpkoTPAuf0ZuWoE!VGm;XLef(i^#V6><+x+Gcu zw9Mk+)2^0TP0x>h-j}C`$@s^@`aYdOQv2bu(_=U{m1DV5cQP%KBd<0S{@!_&^*ujB zM7n?D9C#`jr~JNnrODH9ZtzJlF%K{$^S?OF-OpQrOiBi>2`-G7&nsVi!Tx~jkqNo# zzf1KfgmBM4{lLGl(rk8vk+L~-qTlZ76Jy(c~} zFbKLF@r!J9J3ewJ`>lDn;-@NMTBNogc66&8Wz2Lpq)+4$KjX>WE!|pe<^gS1bRnXS z&l(uBGk=G*VJCm@p<$@qfRa9Nh`UvKGzXFoUL?zU@s(<@Rs0w1t{QeHhie@U`x!VA zyKJ_``YBh~IPAtMW$tBBB0eWnQR1lZ5gx+n9`7A+8e8!cAa_F`)VnAlUC+ee~+O4eN8RUIs` zI}4zF@lSupSJ;1OAS5T7Q1w9x_|bM_1upOv%w?7n zz@cc??PByydqh9E$8Mfrpxc|{DK`}?OWEr)L_**EH zIiC+6Tn|h!O{o#$c3vj2x4{F+@&a0K zq%+qh7#ie|{;m^{bEG{_&O1Qr-9-55K00mPsa_IRq^el{YD>8oFx8mVPqCCcBve=gt26xM(4hxY1Cbq{T7^xiDQnw=bwhO*S&wcL zULfV++$-P&|HIX_VszpB0dH=)bfr0+2yWk-EDgUn|DZN!ch}wVDmcJ`p_GMKu~Oj& zMu_?3inid)%%BzM-e{t%wpHgA%u&QhuJqH)6W7m293_nJ_ zk6v9S{zX!ijhY&|8d-{l^qQZQ;pqHBL$az>pk=g_wXg4WLK*pA%%22rTPX}Sx971w z-&DK>oJ^8zIO(r6q+JB)I(-CIroO>eNN^uC?y=yqyep`FS7S4unj+~1eH55-(`d_-7AUN|aBT-AW-oGj)K*q)orlaUQrpAZhRf3so1 z6cB)F*0;`gV+7Z>jl)1O^Mu8i!?V*W!h>ZyBmIu_P*E<$;^9O1*3o z`bO<>=GG`R z3;`JPy4E$AEMYNrEK>KbKUD%;Z4Vcm_yFoQ-ZvTgJGAcMZEMGeM#_l>Z(Aooorthh z%;Vbdq#%y(p)5EYPW>G=o=M5O3k&C1{aj+PnH;OcUGH5XpQCczu`&$DjF6qq(|=0$ zYmEiE5^-|HnmHWeV#0MfNKwpzhb`OC zsxSkrO;{X>-8!}ED&S6%AbGsj_MEuFoL<#}(r!J(1UE|jxt-O@%NB9*PMZTQ6MYfK zy~U~g1RoCnh=>SVts)?e31t|GhZSB5!W#wL+I(1^!1Mlx2$(_60{0nmzYlG4J;<1& z2Sjd^r#YAe(Sl&cx^yHORYnN;ApGPg#C>aDJyCx6kFD zyl+cd^MH|Dt|AL{9>sOE#XFC#dr=(FgIoD#bNh8<&I1wdvs2l0Onzk4dX&E} zwij%^G4rI$33KM@XQNx7R&@-MFhwqH9}2-@S;DmzOntHbiFdb1VlJ!Nm;6jk$P=y= zRHxLOau;4FczBB6oWB)F_P-Zq)^(v!qp7@$WJbwc|Dtd2Mbw+j38>-GIj1| z+!*qjP?a~I%YrJr?JL;`BOD4xZ91z51s0h0*zfbKv5x+-hOgbn7Q0)G<$%OjFbK-4# zsybrZSHjD|r4CW;)9EXHE7E_8iz%q`M4KrP<{y53+F%fo!J-3BD6P-SsDb#Uf`qKx@!m6?hszLeQ@RPEtZ2Ve$%F2(SUxV%Y->eeXYM?qJ}kM>`DQ`0zM41 zAaLC!(W|+Gy+K}{ib<|AkB6->Xc_1vWV=XM)|v60b8@Y=_%r;gu87uSM~k<}p=Wx1 zrJMcal>?_n^X?RLt}edo^O&f+#=!PcdfhEGTkg;%!|a=W zHhbZ^!*P=U9}z77#~|%p-ku{m;IrgFZ^p^ICzEaJXQjsgRHp))jI5bU#t`VMh9;!tJl-I#nTYQ1XS~&^0^-`=!uD9Nyzlr z?#4DY5x>&Jm{?%?p(C%3qLMC3BGPpFlh5bv@in*~U{-`@saw{3n7q9gY*y9--`BPh z#Q*`<;{ehuXZ{cDL_!o231;ECR z6r4LlY#+V$OkZ%O9wF+P@FB{#TkRM1wjt{wdeu`gcnks8aHO)fl!ghEHmhar+wcM~ z#$=1_901Mnc$^n1Q!P4OI>klvo!RPt6-!em*jGB+OI(qxWVcI}!X=)u4J2a>=gzYk z%b|L3icYW9Lp#P>zbgitynioh{s+Yzim+|)B{E%Fd|2%I2En<8*ye$!sWS098m#JO zXoSigE|;Lu_CZb|CLbSBMmh1$X$E=9l4C7ra*&T-a%ITs|Iq)E8a}-H8Cy(9-}S@3 z=V_f11b7Ahie=CcGn<51z-MIvCd;9M%b)aboVA_-`y}3e?0nUP4`eSNv7&5)j+T>m zURj-gxp^hztFj6^ug>#vM*XEN2Ga z`1k%LKBoQ~(p8HT^Pv8ke1F~qa%$ZRN7~4Ujd5Ui_U@h6i@S)|SD$h%pU)1v);xMY zeiLDb{bsHV%MCvF+IIo|bjNrIPjFHd0*0&VqKC$B`_f}AyKEsLcY%Rkr+%--f7Z|d zeV-!v6hkb|i@M@&Ws0{LAXfLbzu1-bT?aJ+lzD&mua zfZ^$8On77;9!f~Ohs_7{DTrjoF1^pJqR$EX$|80XfM4*-cPtpq`d0D*#pe^BWnOmC z>#4VfCaoRdenT;~rh;yV=o|*Sy@e7muxYrg2=w(>iW+fl9K)R>Jory zys$z{R=2`xzJs5PRu02F8dl5WWIjqL)qQ)_5^69Fhh8Y3oY(8+6>=`6ez*(On5eXR zN634&lAXy)@?9mt%T{_{D#+@a0MTk+VSG@mZxm~GSL*y?{Ud$+;cx!1fc_KIs5-8eO83+|1t-W1;C^PuEufQWi(KR zz|B^U83gvHH8;eIs?P%JNVmy@0?r@MJ$OULe^|U(ak5LGu%&!k4J8Q%pY*_=9{M{Hm2`Rd5`n+^Zaj_pevE$A~A^(A1KRt8a=JPIIu9L>`=54LDaG!N(XR-I<#}1@*t&pUiTTf01V>jPovLhB zIzW$)>@Iox#ojqRGxeAe@=k1IfYBHG%jV(IPdo{a`TP+okJH+z*XEq$p4p$=U3?vS zQl+98N9Y}8Fy#Zg$=ezf=`KEuT>SuGi5oM<+^}0Q)QX(d-zx13PAaFwEZMud%UlMq zsoh%T!_|?@TEm18Kfoy?CAp+|eu{;<>m?{#wF2W?PU0T=_&%~2JGzU7K@cr&BasTN z8~+2%ab#`w?jSWZ#S(X|)f^d!xIo1Y7BSqLNAJhzbpR-ME9SUfka;vUv3EvxfQKiR zbLZ%7VnbU5AAr5_QVj64A6;mCGccjr#yVU%QFAXr<01}Ds(v;Rma&GB>%on;c5?a7 zwW=h&dwvgQz>Nng^yujteixzEiT2v;_TpC2B*m$*hs)X}{EJCpMNz8Ozavcr1P;~}?AB7)Ht_)gll$|dEjrmA>w8w0*LJ^%iT6I1g{ zC$<7f@NrDt?C>n4`UI5V=AomrzrcdS?YB9J!_{5Xrg`FdOdvPZo)MIkU9BFy7@9%! zlk8K>0RuMGlF_MI+7HZ(J%0sv`MT8b9R>!v)Xf+nfeY%n>}JaJpoG$L@N0OY(_)$K z0B;AZp6AjDNy^>7&&4qrYXvkPgp^c3+nMnea`k9dkJ?FnETWDzSnWJ(7HEt&=gxW8rnh@2+2%dJ5U=E?j&FYybHfx`> zeaJ@M`g{Nsg7!qT?(Bd7$z`yi&1POMsoalcs|2%RYtQ62Lp!Th8YGlsw~!9JijDeJ zs_TZu*kT~BfcFmEgY)~>mx%s86?TYdto;qt1d#t?;;U4A*~ZG(rc=78Y4ej00cl`! zK)7Neb6l!aIqeVL4nYr8=|GNmdabH_+J`H3pEjAz9LK&)EZ2hw8Bkjvt6h~&ANsd% zbpG$OnqdU}AYY-@u6o(er4y`WmT&v{{d^D;;Iz8k&`@t2_2rk}%>rI+bHDf2wG6pb z?B1!j+Jz$@*OX`Xh1F})cWn~fFSg8SBZ~xH|IJp&u!emKxFV<9qh9$T_UwT$YYU&~ z`5m|y;3ZF+q_Fucb_LY(kAV2TriEdQ$;Go96y={xI*|_cb0NwGXZKUw* z$CyX)Xx4QK!T8;B?AF2fn{$NTQ|45V$&44i-h~`|*CAJO6#ogd?$Kp9;($PPPaRGN*iQI8bQ*XMXb`o7N6*6pl3G=o!3Sg)>ThNb z2Z2~T^m+rEV1ba={rDqSW+f2}JOG1-)pk+ox(0GkS*m?a#t)<%AdiZ}K{EdiA zeWaC|DZ^mBpv%O0mA4SeL@)K*l2D?9J2MO7@QDNlEO<!sllL;barY$hFUgRz)NJcL?n0OE)^qj&jVBCnFqw@p!hL z@5~{! zbl^5X%Au<(wiV01RTKZZ%_HfyZ{G)BB~;~#od)G zN*8H4;uJ6;(yLNz%1ZOpb+a++JCMo8wceq-Oq7h&bE}N4$oA^|W={K!LOoNAAI7F3 z)6hUc21*U|txft(LJbSpy8X_}12vqpmzv7_EHCd| z(3}-pavqL$U9Dwk3XMt{Yjq^Dr>3I@&*Y068kHcv>~^}xY8QGD#qDO-2oc>R5g`4KF*Biqa15#@6`3`4SR_V!4&d1ye(LBBA&?ze-UV$NJTR zd}S{g7nY2kcE&h9FE@m)mz?<+{4xM3>)##;9PRBdKC)fNbfi~xCO^HiZC_%x%;Wqc z32Kt?llOdU)T#3Nfk&#TBzgnme1=mSWfcu7IS09T#Cl- ze0?HAe03?AoSoNrrc;JNwVJiqjtKuF_qsAM=O#-EhesY-l9+Wo2C7%Y&yPeAu)uSJz-DcTHHeNUR{AEW z$>7xWytIT>6$wN{kyl`f3fm}Pvd?Hk8AG1>>;scw* zPJ;D{u5$PFyU+zrVYokpZZ>Eqn`)tCB|E&A;zeZl%gOmFo6$#Xe-Zm+up7Zsj5eQ9 z|DQlxz}{vdLlqq^)&j8>nioGgbfxk?HqtEHeO{}4pC6P709ap{CZZ*mC)_q!;g>vu zp9~MCyJ6}WHjNT9LtW@I0=CtH(@&vYnr6ywJU1J=qFE z#MJ*|@2%gW;I{W+&_^*rDU}ov6cA9lK@gCZmVP9pLAn_fl@93!rCV~S0hMl~d!(D8 zJKlxoIp=uZKjC|Q_(A8IftfvfuejH}VhaIYhGVkqe%rm@&8cfdyVzwqI_8yMrqz90 zKL>1LiTP5a<+U03`kzExA+7-PbkqGop<8}I+t3%xS#3x)b>szeZYLnO)y49pWqgh&cezoSYK}WwJ|Lega zyzG%Ct(rcY5;-&>^dLNe4zGw=OOQG&kh+ZdvqGEw^gA>A#m{ViRWMQw$4M#^O-2>G z5f8YyoV(5jwF0X*Zj;BRAaG;&3e3js8nRNEYX+Qe=v{pPMLsTEP!8re@F{qyQ>Wg* z;1T4|s^QOb z7>h9i4#Q@P#0e>NJ5uOh(Evb=ipyPx;>xZz_rZhQ^p|wi^3cK1<)TDSR4(1FRhnL! z%X`X}%m*|Hl)4Gfu*Z%tTo<{>$V8<)&0opm={MEX=eRMNdp1i%yFkwUsnxfwzll{J zxf?c}_bm9Jc$}61rLMPfhY{eSaOB3J|0E$+RY{(%uh@qcj|1H3!kzNzPqUJabrEBR z(5uls6Xa7uQt?EU>%>a|8rO8bi-VD7=l=o4at8y=Kyg5)M07l-P3L0DcZlSOh>oAq zCeM3bp7?KS6v|+7)ej&<=459~PkNCP3JrD5MaE*Uwbh+hAv^7`PM5DI@Jy^8>UiCz zb~Yh#0Cz5|4SY;4B3`*o@pTv~Ie>$JAWAKV>|iN#V9;i?_qu*U8-MdWr0D1Qp-KRf zEkkx2bBTIkjc0ja&R;NRApYg&EMc;C1Z9Wi?c)Mt{`p@8+0$J%F7*PCjyr~8GH#76 z0;oPJT$;x$^E!ZihonSe<<>8&XKj^xy~}aJnFnEt${zy4isOLRfE2CM-t=}og8cRq zD|5U?y4Uv3CEtGy2 zgd|SKA|=M2y=}Y9+x<6ynBVhPoFajpYG+U{Uc~Z?&Hbc-U+&E3M4U|EkS`C2xSCpj z7SlOOcigkzKAZ|=+vF}m4qFMVP=rtPR2(<4Y-hKmahio_pI(6K3H7F4L7*>mpKe?+ zb`muZO${U15DY;)JHdl^Axrp`vCT-L{T+~xqru^aVn2rwxErSWRGSBfi$ z4f2(2sYb)fJs`vMEmu*nYbDPX~DBYfrLs6RY6Lq!~!iS&WgF!u5A*eh#>JFBvyt`Z6(l02~KQ zCnYGX0rUdhiIB(iWW=LOQFBlrL#)#W+jG_W++Wgoc>__XG4^Iuv*BO}uVv8PeZ2qu>R{&<%PUNep>fe`NVs4Krx2&?l;j& zZ*^fN|9kmr&@oz6X-f#}fyo|Ldl(pH^LQ84+9*uMFP13akWTo4s~9>#{=@nQ9N9Ke zTLLtOV%uE`(E?LWSP}C_JM3JA%%*aFgH>VzsU2dzkn&LUVEBa(#l62&=)-~kh8pO{ zyLR>^au*K7W!1AsBxy;$eEG8Rt$=}%C+P3>vbz;B5E23PlszHGkjk>^Q;&P2Ymw$b zzR|(EmF?R(D|%)N&sJZ6i)xN?i~T|NXMnZQ^Q6Fb5quoJeeB9U>0fLcsgD@}Du$Om zvtAnvcfQ6n46`hbPa5CPF;A7Dy6K=#;b_$lD6}@LaE>QA%rUX+659S6ShmyAjX_UZEyM{J38Mo(U^@LBGU_u3LpcH#feGHig4hsL`a zbIaTO*@Q4i6bkBJsSCBv_%_~r!cUg|ST+6|(BFOC4n2V{F1zVMsKWzP9`HU#uN5g+ zvd9npUAEXh{uGu#)3Rp&y_J##1$X=8;=#zvY-YVa@th zy1QaNkVN$KLz_=eeI&O#JsT~dWw&|KJ=U8;55^iSARlii^Hq}KoseEKk`b~6?R(KDPsPf#f_@?~s59d1MV1o845Mj> zMzh3>_lEtOOn0DDIhBQ9jq(aMlyzUJ|EW0fTk7LgAc_t0`WP7zVdZk67nTB6s1sT!-E5-hFgK29Q(L40v(D+en zzIG1swZW!kyIu8X{oKI&D`c7ty!C^^E&ZQ&sA6Q{c%Y91p@Ybgqr`^x(m)uy0#SXz zOK;CQgI`DDa>?@EBFTRSrA-bW`Q8m!PA;s}>@~#C9c&p;{ywz_XQu{@Su3;9Pvy2Z z_W3Ma))IXSDY4g+MZPeW%RtD_Nn|X5)RUj57An3%HaFK!iWAB3 z?!&N|0w^!pxsq}Df5ayN?D&D#p+5|ukZhqZo4YY3Bt)%qP2-&U=NB-d(;ru}@r!M? z@f45i50X+2cI8YZ7}x|2Bt;KtnP^+B=kgdH?QB4=_UrzNw*lDU`6H1R*9?(9A zP9zR9ewzNCdMo-qL5a*E@GQW3v|@Pg#Ra2Zw=z+y;84nSw)LcUzelrYdJ+mg!F^C+Gln;J>bUpj&AoJI zK2tJp|IN30k*Q{@;Y!eiV&8L_mDkdJU>+};Tc_;k1-+j2bd6Wr^3lT8QVq72s=94Ckjv+5+} z2dZvgHwqeC>p6j9$%Enw^o15Zvk9p2aKy+H_BcO>63dFyFVXy=yDzMVA8EKe|B|kQ zJlb5?hCBXdM-5!sVqcLLdq7BR(q}LYBUTi9tfR75HlZBB6vsNqH&6z68<@xV0WH%a zA{zuLS|}-cSoazFqW;Uby{BZ6&qvqwn9ff^cHQ5dGMdO|EhL(ae znZtMd+tBnwIuSIXtuQ;N!-y&n#+fl`yS>aD7;v)k(Qo(>AHaLOX>ph(RiGSs z6LOdfGkKn5caut*0ga%dAJqX&HU+~0{W;F3eqFSG`raRBy)JDXE-;8~z(TBP=E~}w zWJr{P_8p&TUv+@g&YefbFdyjtF>5to!%5ei-_3WfyRlSp;baeh((qI`O;pB^3|FgKWK~E4B3fN2p zLId`;U8^Gm^G&xz$~|Z!S2rUmp^>D`k{gbX%xq-b##t{hJ0l~^V#uxZ4D3fP1Oa~r zX4}9T?9Rc5X9@*+=zWU7P#Gg4`zE<`dl94{zv0J(8V0)TUlfkVXA_7XhP6PGS_y`` zzX_T=5lieFgJs9I+U^BH+MPy!CWlBeePOa&y1>-Q@S8^fF9f2l;vte>5T{7#THs%|66sYdOcPL>p;I_w^6n{@fk;FV?WWm?N5JI`!uDNF6jUDM(@J9stEKV?76cog;AgKyUmS#-x z=BA0QT`sua_`^S{Hvz=}13Jup_y+{t(|ynFbU1ff;Bf1W^4KY5W4fSehfF-%`T2e7xkp@Q(bYT$YRPTQ+kGVoLSdU ze<-V_+&3srgtz^AXKaUzEA?Z|?Ist>op2;#eVxDNlz!-n`>Ou=Fk!q0Fg_ zvQT(t99yDM@ERFY)g}{n^5u)}?x|azLF2lzziYM1ccgQcMtJu|j!L!++-3;ao64Te zuEvAPd!Xv#nr#M7FANSoTG;A z$oZTn1k>{+krzvzVaXV%w-5_GprO&JwobQdo$w9KI#MnbD@yP*6m6`Nm|J9}u~?`N zaI1N>sq`djk3N?g_f%0e#b#Tr^3H8O+tGVhy(n}{v&Qij`_d&%op{*pY}Ix$B=WgQ zyxIAPb)a}P$os^pf7qmCI73&QtX~#|G!rCA=9rstJ-d(vaZ^Zy@rF>ga`=GpYPair zRlUaQU;LA^jO4I+#&pV9@o{lv8(g-7Wum@4nMd0)M7tL$&dX(}B}K=@S#PwlGc+dn zUVXMNl~cHS-cT%K$kfHr< zaN{pu4GOWqG4aQWX(BNDo8tMlZ52=ImTpb_Ns;^Xs$G zpYD_X)z$KMBw8Z-BT8xATVADdsJDEn-r$Wj%N^UioBuu2L^mH(T^SL}xml4preQMM z)61&S_@EhSseIVB!n$8hGZ~oLy&}4TWm7g(he%{_g4EWQoyu(UdW9q0hg%Z(=-WgthO7MPl+6 z>tHjlyN^$1$&Q?BF;eSLHd78KryMSWHBgtt7fSya)mdAC7nSL&)#I*MkE%ZO@WV!? z+N5%XgoK=O+w}dk37-5Re{xW&9%^xH_?{0pyd#|CMx>aA13Bi&o3ubD7v<4yfgpUG z8&X~gwGGTXlL$JhC-M~`LRh(M=P-QBy{3{*@w2{HpEfU%$~fR9%0pl0^XJkb9Ylq2 z&%nP$5s{}{m?du4Qz|nOs8;4Wa{TMT<;$a027$j1oytR2Lc+(#N26vYDeW_Il91jh zyz5Q&tA6_-%gPj+)Yo<7zc7g9Y_l#&tDAHPpX-+23v(BT7M|)VbjK%&E6(OPnDC^0 z(UwwnZH`)uDD(KqVmy}89#s;H`V5Z3X2AE)vtyNPq31Lfmai;F#Td5v||A$fNCLci7ehEQXT zESK|SNb=m>@Fmye_a8MbpomdkZ8vv|yUP^>fBv*WZ%3znA-C!+JU*F@HuYINcoSD9 z20NE|>b;+~UCo?6=6_p#^+3G`72?idg+0YKbhbB0ANVy|*5!nqpV? z*;)MZ`6XGHWs^c3s7VF45!GBu`b>N&NiZFMw_y@@DCvLaC4XBt5xAo< z+}Ldci-((gX590^*nu3ga-WFRMp%`k%C^y{$=-hJTUe!(u5yBa1L{@i_pH5$ECpqL z-rWRe%n5-VF0HVR*dw))WG$pm#Vonf6D@^s{6?P5gE5!u2`IN+pZR$%yYXpjZ!Go9 z(d*8=<-1|N&Wwb&K7Xkr~{?Fo%+3B$lj?yF%HD=RvL zF^O|$R1K91%F;jwm5ZJoe-jTvbI zd<&VaKE0_sRj@8p7yLx8qWet0t$-lX>-6hjsg+JZjCR)E^U6Zy%q)sNlalqdJM6}@ z!`YmxK7I-mt{reUi2(sQIj6x6PyFzT^7?45>9~7)!?A7Mr$3jwsA96EiOQ58V;a|@ zZQHkNt@ju)Nx+>xbCbqyKd^Y{==S?pes!SIBeE7Vxn4!!5oj#tS=x`tgMyI@F8mMv@Z&_e3hD`y6^)fen79wI z9a>dHMC1>&N}AwW2>PA_02@k5%cD9-rR+X2t1OUIqq2owi_?(-)#hpoUb2Hs{s!k+k9mDNdYF9f>fAFzcPI*%3vszE{{;R(Vs;lm=TNQrFDw11&c zf4k%B+QS9S39twM!Ar`SpvlGU!(h3(EcZsyr-O*ylc|)H`wNTDR^j5|=>7+nlX`g3 z^Sw)k%181DI!3wl*CG~w2?Tc#u{@WMxLig&J!+OgpQrnzJt0>d$BQqrBiva^LSkjl zE)w1Ri6DZ9oixjIyqfLF7DCF5d&W-VcHhcmkHHTxa~;}cH&1!qTCxa$t?j1K)ulRWzfSnWd8s!uppp%1dE)EMPv6sJm#(ZM*4FO0@y}8J z7L7e@+XI)y*NlTH#?!f6{F2%_rWK?w?bOR(%bg=Hua;9#*uaqPE@iwHCxchoqjtrW z*vPG(`y6vV;&Vn%6bW{!j}+EA>cwIlNqA5#OQji0qv=wKnKh60myTw>MMZIanL853 z$}O%FtDgIo*&E2Ew8pg-kaqa%=hZXBgmY!+52*+*s*`aCZ(kysDjz7h%b3 zw{x)BwO02m!MOu$w^G{n`uTYPDZIxgnNz(QM7-#XqygQgs1JaO!#aovQ&LjiS9{+; z?#>h|=Tqa#)0U!F#t|W*@@XNVia&lNGeHWt(8RDYA+-KZ)fa6)%WQV}dYo~=lnZiA!GDRaum;hROqsqr9%xG^)bA@){7@5c}!~IFE(RQ!d1U%1cK=9o44y zSnAO}Q{Tp1f}HW973FJE0h(;!H#*^TK0U;ZcS>zAeeCjwsSZ&xW|Y&~+$jw>aCV4( zY}^nNfH)?1d~jbN;bcHx{LEZavU<3W>^J<70gV1o*5?eBa`OA(f@h#|?Fg@{OEGsj zGL0%#)=X*9p~-f)tRUS7UtW^Pw#3mi=S zlb(iD%aJ^S^bR$o`R@8Jug<;W>QOL;n%34WH&3OuH4h_|zK2$Fb?n~env(6JJ~(bA z`~zcTE}F_9!6+J-vh`$+Rf8X1sg#Qou&kA>K`>n$#SQYFHNhnRHa0-@N9360Dj z5AY3~X}m#+gI&Dg7lh!qTfGRImQ|Ln%4uF{yWIhy*ibUjA9$IvsM~q)`O1(Qx=@dN zZd?F;Jm@eu%6*kHk;i4_RZ~$$6qkG2!TN6#Bz`q1G$iDF-UTPS9Y!_L?Sx?Z)R~1< z*2U3OjlVuQ-L+U?)R|vX8>~` zTja39%yc;`wv$+~e>7OuHwm<+ib0f`gYRD%P;pv?(rz zm5aYR?$Yb_tF46{6^6ix4CCtHT)SOncuHr5d|3k*wSkFmwyea#iGm|c)3 z@a4GNH+-G^PLFIF3taFLx|$mv{7cv#oZ(ofJH-1_z34EGr%h>fsNy_PFToR@xV9pB zr|_t#Vi&XoSUaLy`5UUnjGZUcvtHuQq~@-0hlYhcibz0zXWfiE;lzw#s>sUbzAXqkkj3&CqB0NO7auXJH*P&YA8j`1~MM!lU10wNr2xR zAC8S(oaOD~w%yuY1Geyx%f;$MMOiPg+^@9inqL^WE4XI$Um#gmPse=7y*I}l*{z-s zrt8z{Y=u$PPA|^@KtoUJ2uRXT>Iq|LKY*{BG!`{>?YuA9<|k&^znBjs@@yZJ+hFJb z)khZ@ZB9(xvb=Z^4Wk(*lIP@qS~WNG@h15v2wWxQd;NJbE4{aBi;lqS)Og-lsv{ z)SDk(`b#ut28rT~A@ChJ@BdmH0~apnxOL7-iz<&v9l`m%eE$W(2qr5wP21fYKl0vW zOiIX8h62GfOF^`f6(Uka3hc_Rg0ZQLW}njY%7kXeC&}jhvYO3o7Ti^U=qHORwxW3; zs6pX$YSuWN+IJ-fH-5IVTF)^s*ql~-dAJ%{H>@{Y>R(ww_J5Sr~UMd&0(y}^KF&9ll8^D^*Vy$p2tO{t3WFWyB;l3pF~z;ILF3Z z2vc7|#Am*F9JEkijl^cX7@+=mu`yLp=G3jbjXl&b_t+v9q5O7`b;{gP2ai~b|88&TvHCI{I*AKN4iI*JTWc|5P)Tz(iAK?l~UxE z<%Ct$ag^%!1$N!K>3VfL5ENbOKw4Q-rR9aL8##}20`oPjKDZFC$z2*qKP)7fPyP+M&7?qNjeN7 z5>%&=jANhH7BHc?cTe5S8iWj+nu49>vsOpKv}Mxhil{YMA{x4eI3#a9xH5{7hXeL3%#l)$4PY@G{`8Fsy(5cz`Lv{^xNa z;HrNfHoe0l`RC!?tN-`Q|F;|eCma8-X-cq3YXl?Q5{Kn+bT^MNH8K`v;{n@-_5AD~ zcoHlhmLuEV;6*o0rvm%qXyjjKM(Z*)co@>_`?7F^8f9| z|5LJIOA8!5g@9|B17_TZ;DNJ?i-v{PdT##XFndp z)E@2tlbo7EXxL49gI~k2Jy@*wO=@9b;gt42e`{%|$S_yEgkB-Vn@!K1l7Jwz=JX^s z;ooRN=pEKUuG`^Gbab>t&kWf^c$W|wDP29iRK|1vd=`WS?xw4w<;qz~pVC^nS@oNO zz3?a{HU9e7>sZ@jSjYYFom#@^r~@Kw(zGh=IW0zmOE3NN`eFcl1eM6gFnVQ462>`c zKVrr_&GLKZH~)FPmlS(TgIdi0p;kpPn|eodf3Aj3jl0(Hy?UzWSOJJ5~Y#Q*G<5bCB-O?X%sn{juFWH`OV`B^g}*jCelJftN$f&=HD z&oc?aIylurIrsJS6dC>e(jCQZ>g?=1QflrO6ckx{;m_ZqdGNq;@ay;P?!xDv?scbd zvl20=#PQfI_N2?DUjOG)u`VUiOioQjGHIC@8>{8Kd}2Hv_w@U_3+(Ld;z1P0zrLS0 zCiv$)J=p&0QdnPKC*`!*T3gFd%2b&6@qy;2&D(PqV7&$w76NVuTfxCM7MlM32awJ* zyvNPGKieLwm?mvyXUERLk*AHSNJ~qLh={1JuFlOhKRr1PqTnYXC1oK0k4wPDwvG7B z$U9mc?X9B`r*7~`;D|<0F1yEvyJxcK`Hy^H5&r+Y%C|XcTH4u$08-eJSV2T}em*AY zsi(`<{7|k2l48td3xh_$n-;xpABO&gTTGhX-rm5VehS1pTeVO%L_=6YGooT*2Zx89 z(X6od?&LhSM&nhY$qjk9xPkWD%c6bx=%j=_yh`WmBwSp)m1Py>F3ubaqve~Of^yu} z)6bthdtc1`dlNR`=4UJ{`sMx2Ia0X9t}h!I8d?;sX0D-8$)r^s%VT~JQ?g{gI%>Dp z^;xem@bK`ElH0ns6Aqja!K7Ab+ZI)ztvyam98qEtJv@+q>Nb$K)Sr*o*rLOG_S>0m zjLkv1`u_d*US5LETMxOo_CQ%DOZk|Xm^vML3knM468J7(xpL*&wJMvrK%>X^?%kuK zli<;wKe_MMRRne5pIvyt#ZHD@@JSb}Nvax55EqVPLLRk`>`6 zm7J-VzJf;l8ZI%Nd~{b9dwo5>o|}P2#@f1&i`sj0xHXcQlF#wRjT>Jr(I)XLsW1XGE?^2e2Bb$PsS$$3DjQl%nS$5Dr1U^y+vUAN}trKP1| z^|q5(SXp9m>2(r|I67$Oot>B%Ws~R_-_3X~etwr7z9&!Qd2EU6f;$rh1raAlaLQ&Y z%k-v3FV)oo$oLOh`m(t#e;Xr@Wj9>x;xatM!PWv;5e=k;7Fe2h7p}U0div>}XiiSf z>y9|5?ZrMwcuR<@1c!y$V#ZN4QW3piV;$Eg@5JIin^IR-ul;zP*J(o^j=+VIiTM2c zj~`*#m0*#>>6JsIV@I!mjfXGT63)PB)RlxdJ%K+b4rQEWqNn%0PlBb?yEKr$yFAoW zp!a~EA9aO@frp#-bmtI5MMV|T`;u&~UZ*?Jqhrs(r+Bm1PbQX=mxm{UPNP(Pn}iWn zWse!6?y%~9`rAA-@rUCj)yHpw+PS*9WrM~i31J)mb(sI-!&Riw&&jUOA|PSiv@ADv z7gh5#7eF?^@ubS=zxew1Q(oT6_E=6hJ$-~8B|bij+kq`Pd3+EVzoDeGNQp_`aH)Bv z>;8t#Tn9hmgip28>O7VJw-xc>@hZ-bWAH8zk^3+rj7(j;y)K*G0F1vY&iRg_P zhP2Ko?)^ECN14;@f%;hFk<(Eb;?0{kuC7(Z^=!;K)ypHu{D=sZA?ukI)r`L_bxx6J z_gjcC+}}1-h$+(*iyRj8=GV7EgI38Nl?xZH5Wcat=CPfBq0;%(uW}bQ5NX_#_RI&r z`xC2Two0x{EQi6S|4SpI`N0A`29=zLmyU7XUgfo(o*%NAwLn)-ffT|1VycB?mb#{#L5xLX!6h7(N=GZCN@}R7VFnAR zvsr0>TN>_^VGuq<0*is~$B!S`UbPE@U9PIC0#yn##vy$5>eYE1+#_)JsmTWI%waC#lJFUCbY`pMj~5wYK8twu=ctVo88Ry>vR=R5zClEva*P;-R6LfyK_w&JJx=ea--%2mVX-NUgxg95m(P#f#lH3zfhA{r#!u#S1t% zQtAYCZj~q$ie;upx-1PK9hi8dP?YBvI%F8O89345rFLqJLjE?bZX^q8ekyMtk?U;4< zwzgI(Gvt4~9&ggmmZOkrQtulMU`;j6Y)D008wZcV-^b^Z?7zHrdCCv$Ezr`7*KR}A zKAmyOfq=ceeY$LX$krGc1;sc3yfaAVzpT%WVmao?YT{hOMcWGUU*MI$s`zI=|; zj{O9?Cr#$+)vK+8>Y$BN4FQ{v%@c#8)TybHFI*+|_L<`xz)HH`-D4=;*t<2<)%9k1 z&}wokGWhlGDnIyGHV%%Tux#bl-jS*#}9IYh6u zt?k()Z#5>aETyM+Iy)cqWhoDIbv;teJ3<_Y(ofhlQhEP=F)jEtGq2>7Ii zh6cb@NQ*H^X=#6df6Wq;lGxZvcq%V1p`gI0yj)a^bmZc7viNvXh(#)0*d(qdoeH|` z>>)R=KX`EK@Zfq*?vi(bt*vdgN?+E?Oa(Ba)yJjM*(%yNxMWFa!!wBxIsv_O?#+-l zXbJn2md-u3xahpUG2@?m{SM20yfKM%R#sMC_an!K{?~0$5Qk8a%i0mrD>b#YwxZFA z*GRAge43h?z;^I*b1$W&bZ&Up9kT|HAu ze4*IbSU^>VX69gRVq#*tCzK>V3x9~=v}Bs~tLDP`MSXa>r6xX5SEv8uJ!MbN90V7# zu^jCk9eHB(=KTC_)YMbMCGAVSnWmOH=;r3sg6zW)tl^?)!_e23X#V+Cx|4(m0p7vE z!I+rng~inE!<~*hxzI z7XQSfCXs&r+}qa|pOTP_jEs=b_HbwA+qZAL4y&Uzh|~3{#>r&RQpowH>dg*<1=6l^ zq>9gGo3ybRDSSycF)`8ovD;gsePrOGF3CM=>eTksn~F?nr(=KXGCud(Q#CTmn7*Fq zbc2*Mww?H%fEJH%pn>4J?hd;09)!5vNP_)mfX?J3PPv}&>mokkeiF+G<9lRMKvB;V zQW`GS7HMnbdPSF4vq1MlQ4uUW4@58@^L{IV6Cyg^S3mwTHCln*Y`_xkyTnlQU@RgtPn098ycLvZ8{Qmop3f?faRl`O)3g^&;*w8#{ZeYQ~Gw z(Hcwg`)&|OQ;JF|C`gWGhYfQ1etAffTLx{!_~>be)s$H!E)M{WMQ`7}Wn*gx&v?b3 zgnbeA=Z=O(p}s_di}`AAW)GYSOX=*~oNA@L`NA9e3<#S5`cyk@&bla;YYtU9VCH`& z}Y1vsjn(6{dN{Kk4GGEt$KO-jfw2eExa`oy+v2mzvn=4?l zthKeZtsuXE03s5XdJw&)eqJ~Xdnm69_zXia zC+)DmG(I9O)d2>CN8eQ+S)0^>0zR+5z$sVC;Dh_DhMK`W-d?4 zrh$Q7*!*yJg+L-^ZB12GB%naRewQv?f{?LEgzOQJFA#Ih!x`%}_-6v%2R@_-9p_6( zrvTV*y&QmJh4Z$Fg`h)w4Ew>sLAO}X*2;*gw2$q3v-*Yx**LBzJ@pS&hTdN$xIZ%O zLPJZNr&BZD)bxCK^p*}GAVt8&Ag~J})sE}Ba2#sp8V&q`b?~AA(?(N4yJm&=Rz_Ac9jHh+7o0334xC)W{ z<;#~LYQD|&CMh#B6T*0|E5@cU3!3ycY3vc=AZ};b#`)kVZ)xr{rPI#bBvzxBjV()F zL_JL}nPSinjPBM@o^zD!C)fNkS({J)f>4ys zI6PyoEJl9sYk?2aLJCj+pF`dU?qiA%f9RSBKd1bZ2Fx+*wCgL7{tduJicl zsKRzZ0UWGZVTUvkqk6GyJkO+!*r^9No17lRaBM%ztfr)sB_P59Fg!z_igLhc z1D|)s?U*w#&TFEaw<~3-3h|OIh=|nME%l>F1yt+YQLw$sh```EEFLB%i=E}6Gyh(? z$Gk2F?ysp@2U2KF_rbf}iWKAFsknHN9h{ln$}qc?{^ss3W~e9~VgZ$`-o``#J^ubO z-8y{pgI&!tHOv&!zkRqx*SGHt{Y=M0b0ovY3pO9F0G8X8lt~oa%Jl?GKPgl(5ftFC zI^I6Q1YAe2sH9|fZSaEEZrZPM4kKg5u&fw)y6kNF8@>limzqeT+3M$2H5|e3nlZ`*8JEL|}3Z zSiO^zLk!-6Dpm5)m82ffqAE}!2$(trI9=!ZvZ1F18@NsY&%VXEqYu@KGhWtfl$u?J zXh4D@27_H3g@z#SH3=;-gNoVS+609Bp@E?vwgy$j#l?qTsJEa&1HdR(lkTCgi*KjO zB?^RSTUgJZ93PHVI&`{ow%K60!6rZ$B^BG6B=q*2OblB&;6&TDYnPI!yc`ya^78Tk zLVtZ#d+uDGx0~zf$@zF5yDYV$*AQ!$6lbMYJH9}FYZ|PB=tdhrh-^myNu@!eK>E(1 znX!Sseok&~vRZ1(NCIb7xAXYk*DdaQ_jG$^+qK69u9J~Hkd3ircpMCPM?WM)>_Anh zQi1F#1`0z{{=`uY+x4g=5x z94kdCNkB=7Sz6L4x1J#rz|+WhLG|E4dgueW*D)V_jKPKZR9Cy>LJ2MrlcwW05A5C5 zvCs~~glS8xV^MV^90l?B&a62zqFf!U3{2<+l z0xJ^}pB6Y&D(zRmA67eLbziu6vD6YR&|`0FX9r>)yPhxy`~CN;SD(BRV(RmXoi<FF8UoQ5N@cz`a+dAD0lu5|70&Zt=gDK^^%cnFhD(f|_n?j8c1%bwpoadawdCCGB@ zmq!<-deo2Ta_0gThmDV|Q>yK{Dgq${1Ax~6K$=1=y%QKW`|U@Jx&Ho&J#+NH0%yD^ z(A?h`9~Z!ZC*n?=sTG012C*?lSBvDTr}bqId@U%5XLGW?dxtGawwwCrXu{Gbjmem~ z{&o$9NM@ZvqxGNE;u}j=!JScqwDk(UfxhlDHjD<9Y49H%2OV>(-}k zF>4b>uFA{HKWYn-&WRd$&?#_~^+mTuM4mA9{q|bx#?2{OTCbTr2St6+lJoPN#tUsA zOKfaxg+aB|&t24qzJH(CTb~-zG~sjFNC93Mj8P}sYV#Sg01^~nS{v=Ue_-Iw_I6)) zcS|4Zu%y(ZX&^kcx*hDtr(NLmhKsTpHA?whcFZ`|y~?^qzrOMe5ASmsu*?HS+XTD< zVEvZYJ5BUu~6D`7{6bK7U{so8xC~thytYV*HW(sguRQr=u53p^) z#EJRe{^Z5U011g{R1Im92G%jGbys%hma)zwuv(0MEZ_tncQ`G73d_kcuY8~DwcxxK;NNZ%JCLb}ba%rsoD&Ps8Urc-(0zhhO6T@+ zZq5>T2ydBG$?z;9dX+~y)$fqVPB7F0kKF?O{RxrlXt!NAlT!)~`x4XGr6tnA!Qre` zo36>=5U?&JB+gR@0j%k=lmfPnjXNs{i=o2p&YGS5{rELOD^{ig%Ee}9Cw5GN5X^oNaR(fw3Ef0S&R$Omnis!jb z@%YU_v2iSiMV0%CdQMxQ|^B3+$=qF--CF0Wd)Cd zFKRPa^jbMEw!i~Rk}3pyVez2WAzAT!q{SZu9-{QZ80&LJ)5#DyDrc)e-s!ojs+^ziU)If4gSFD^2s zZ9+op!RfwXUrV^)f$aQ9wu4vcN|k!C+2X{|;81<)E0M^LvI%(>jIkW1Q&T<1hk40j z{+f>wCjx>szi{xhYuw}e1DOrQ1ik-B81X1CS!35;7HC2OfT(n43~1`fHQxcCT<-O|gK>aiN$ zS*8Xnf9vQhKN85T0vUAL>-xMo+jbMjmltX2Cp8m?q1zYMq*@+BH6|Bi2ZV2zJ<&@L zL@<#H_i3W6Q%82uOG?>!xymwRG4^I?Ch~iNqqx_|xXm23q_U3~HI{e+w@JOt(`>wO z^~8h5%O-+=^>-Z_V8JI{^gP_Hf2HF3?2yA;i;Og8fTeHX=S`_cvE4oWH$Azzxoph_ zw9Up5$NTAArE`bHRMhoY2d~zz$UP!`zkT~TnBZ&Mqa_Bds!JdSIBL-@13y$uX61IbMppJoR7SU{4k zg!Xr46r0LsYl z@M~6!3J6Dh?3D5}x!0|LpU^$LNl9OtDoyaszb=i%VzkVRM<9aEgP3%%%4xICR+3ca zurNg`k_l;PxI5gxwYy=!Q}MDFqEz>_UvKrp$et^~Q!UNZIAnbHS@h-nw;*oP2Dt&(ynS>0fc3g@yb;Ln#FAHYSavVDK#{ zCUtsT#=jO8o^ZPLXDMY`fAA&JK+4J_77c&3dAL0^*Sl37nVk*vjJWy$W&7wT{}Sh$ z$82o)I5Mm@o0NzHiG5b10y3sJ!5YV(U$Bo4O3eC4#~b?Vx}GDQuh+ky)LKx=w#Z1= zY5dWhTwofMHwIc^_v7?E*PEH??nzM7+KYHkGL4NM^`}{frNwR%+ zMRg>lUp((qtrr?)BhJHqYPT?1PoY&w-yucOQrFeh29U2!I>)&{cm!$83nrb$k2sOr zP0VbXTPKSr(?&MTi1)bj6Pj}2^UmK zEdcrImj_>}r8AGHi-{4quN|*RS&2^er#S>A_?oiTNao;DyLo=RPF9zmqk~+P=)cug zeow{W#fvWAnN}D3rPby6ho#+HK|KTgPuN-PO@7_oSzXnt#9bah)OEf3a&eoot4M=$ zy|JN#Nf+zM%Y$N{sUknUT6dI7zD5!4+^&C9uJpNig_xVwf>K`RJ>>3NG$y3mkzGzQ z*ogkCYvD_BauB89S-N+1tP%<814(xP@<&raf)sp?YvW!*L?67oFz6b@XqgoNYLo61 zA0*NNY&AF<>&;miE5?W92X1xOo8kM;b;L7}00D0d1f8Ojqq8#)7gtnNloFU{a9g6j zgie;zF${oM4LjoYzqJZb96v=6o z6c^VKhbUK5D`GKfR=cYGe_WkaR26KrwKuJVh)9=8Nq0+$qBKZ%Nw;)KcZ*7wAl)Dc z2q>kJn{E)0lrCxh<#*1x`G+@NH~`sitTpE|*K50*qL`Na&ptFCR_TR>)yYpw#TJv0 zkdW-#>6+1wIE$(+TZSLu&0r1uSX_*Zj8=pxKcx`9+cQE{?$0T@11HQt_5lNt!;U^i z3j)yw7c9CaX;qb#@pWnWZd+5}M!$i5XJX^`S6TseZ9x6Wla5FtmZ9NQa=zLom(%SN zr|a{-JJaxg4WcIDg(CKt7Y)$>+Odb{DxMkM7;iP)8>Ec>&pfAsV}wuF8x*eXrI1Md2Fox zDK?Q!TAEVoBcZi9(azj3ZVw_7_x&ute1(*ua#sq9Kf;*8jggW+z2z!G>tIn=5VJ8} zo($i-CS2T?I^()N+FTzFxIAO2JGj~NF=tpzTzoya#nXA|>}Y%5U7gZkn;cnTXk?WA z?{aH9J%D0cjZ(l+*7jzd>qg!2{+U%EriiIr%S`#4yboi6g=WW!<4 zc=7Apo-I7Z`p*(A`&?eTZ>2j*ZAg)JQfx*?*lUl|*lXjNWUiny{M*BNq9e(l_ zluOLHMxwVXZbsQBt&4HrWgvSZ^nePfXHZ>fO5<|JHH^#UB3VbXL228m%Tb zel{Zz49dXCRiDAqj^3~0`0}<^49l6q!%S+dAEh&|>-+bx(9ldlClhE5p<#vY@N@#@ z$GD+iGnirc@?6!}*{x?AC7n?u00KOJ_TwHm_bX7G`1$#@&5$PV>2oaL>`&%0l}%!2 zqdGJMFXry>(#i^h4zU1p*)0KIn>KTPD9~Gf}s3!n{_{bCKp9}e)Nar&YcHTi~=YwA=pIbAY>|BT*iLx z&f+qa=Ku3BcLpw{BWPmerKF^$l&8>kQrrgFkIAc^C+>S0zEvpYs2ji`VTT`_9Ocl0 ziJQ$=(XY2b8gC1f85%JkK~7G`uIQAbjY$xT!^JngcrRza`&6bciuH^!NxJ-6Axkw& z?0DC9WnplnZ9Z*Jv}qRdcK&R0c=)a6YS*KShRY2aQp0NeH!zV! zPIFO7uf6}qQu5D~shAQ<(RM2RbS7I{8>|2HXRvH~r^K(x@A@@0v28!SuQd3$Q$nvd zlLc2I%`DCAw=E3~FT!G6GvE$lWccm;_L|+_?Um#0l67OTn{e&B8U2E$#cgXs$hUih zSRpMk=Kkfc$pVXtrDt)=cBZ#YXPccZR%+L`WAuk9O6^{h_@>oC(dh@m$HyE>osp;hfx0(3}X;z|63CS#)pFFOG= z1$%&Zz_=@fLRurj&i?e|1RWjyxj2)67@fajz3U$hIk_HrRUtU9#IG+7X5$y9eBrd$ z(9jrKxKalRgR8qEf^hz*F^y<&1VA|*ASR$vDCJ%QHx&9!=3&i=pCl4E;}wPu;4J3n zID&o+I(gX%W!UG@M{b+RH8nLZ!Mw0OO}Na@*cAKGEWh>i`~eJMh~ci@^FKdzgkKTA zqL7$xaTF}&f=OoMa&skh>730jJx^@IIolX1D7m+jvRJ_*W3 z1r-ZokCxfwmX*=UDk;q)kv~RC>)yR{ZT32joy;4!cDvmCy=h|D`mL${7I0Jl{KukK z%lvjzD{^OFY*x#~n$IKS2;WhQdVAI%A6qRAH*a#uj~ zdVKY+1kf0)`d=nGkd^u6265&31@fscUkod=cD?pJYx!HuS3ie=EN72#+k$bVtN8De z;~&Nbe~+cU{OKOp98-33k5DQX%7~1M)hzi6{+j#SciHv7@Y?=}7bbo`W#w7r-lNg; zo|RzQQA$_kke9^+0=8;cZu0_4(<01T6`ue3|qk! zmkX0q1=a)&EeIOTwGiL zvtvd)kV=b)38gm{)!F#-r`~oFAkGlcipJilZxVX4*2cBwb zCXod?s6ImU5%Roy_ioENXJCIH8z?X+8@!HL?dHCmVq%7(2fnGUu8xdkj>yZ&EwOlM zYhq<)=dd%u;Opk-SY1~aaHw_`-Qf!n8fpe6vfHE{C9{VqjemZ9l&MOTR_>u%=jomo z=j7xR%aisg(*1yjU3*+8>ipkHfD=H$ZzHj{32>J2j$M12}c%;h( zlU6Sk((<^>T{_Cr1oj-xQ#}!>cz-kyEMt*f7YtWmK#L#bBb5ix2s-nH4d=W+pp+cXCV%o>Kz!L#03#n@z+l@Sqx-M*XOKf zb&<={hjDvnfr_LevAsy z0jBq{V0~gZuZnt|+%6w>5v%}QF&^CIko>v1Y>4n zvj?J4wG$2L7ipvbRt7OCsURR#Zrp27+xDjNv2bwU>taq@i~D2hH@ZaR7&?RF043~W zv~Ey7z~pXcupZo9Xa}CGcin*9VeZS#Teqk=jS&1I#beX$+qb8WMex!^g@>P<9kK9}2>YJw-e+Zv zzBK+x;Lpy+mLcvZHg`!AT2)^UjOkUA_h0+aO$uq+Xy@%8RQM7H9m>mQ%batRn|?zU zBqZY?4$rpw`E^B;f%R0l!~+ZE_Iq=2Wn*J|TMSil;756oIG&TV zv;eh>BdjZaF0RqB!4S&d%gY4w7BWnJ*T>t!;X|u9s~qy_w}Xfyk*ogM*}stS$X(>_ zV9o-;edF99sWxR?(Qb~f-gy;KQE5d*4WXg=4Ixjv7dy#X(uUO`pXs?yUEY)F60g++V&6w@ffd%@ zBC)2REjb7uqqI+3ZW#Wg-ceP>wE&{ArRC~k(%HO=!SR9dw{I1O&E37ddYleowVC`f zwDe37m%f>kD&vSXbKVI#r@aKJu#7FLY(Z72+#{QDjT(1$PLugoZyEvD)cjQ8+Mwo7 zeV_)mUi=Md5~Rk(K-*sm!!Kgd_#pM8ZN#M~Ux8|R#IM=|k;<6qv9YpiKx{1gA;aS=K3*`RaPeZ~75ad0q@pZ5oa_sV z6{Bss_2H0g&o_U_LQc-c)Rd4)U;pzMz*ir`@RQ!Y*~ec)oat$3xNV;7WoH{b>@*E# zj1db9je+=d-W2JHfUV0D%ab)&U0<`~RbHcH8qj8B=mXRNt(kGAJVbS~684%%d1sp9 zsD&0+Hs$lSl}0C}U#L(CxY_FHkekQC=fYp+jcSSdPSU&4TTjrB`r05M$LqF#=(z*H zI2R4h3Ng`5U-!rygvnDA7J#5?naNDS{W*n;s}oqj@e(& z`Ld3c=;4Gao13qss+Szqx@LQA_p?J+v9>2F#lpg2?g7F_GS(H@2mhY8Sp9s7)mye?}?DPOGqH#Cm@=qASKm%dHU-cu0&KsL{V|^;sV16 zmSTKLj}NQ|@UC|+A2KoB>;qRi{PYV6BZ9oq3c$R@#l;C$ucN@>R-e;XJnuTCH1`h< zOjQpz=-Jqs-L`btyM*A(3RWSz^Y@g*7cjJ$I=gcK4RUk8gP;6RrQP1r(t3S;oq~)k zEkzr;_9+oTAras}JU?^g4vhSu(9js`>dF^x^s?UAh(vGOUEl@__X;Iot6*Sx8Op@; zqrJUR0Ztc3mwoE>KdUsPckkZ0LryIw{L1BbayXZYI2$+i6$LRh!ClG?n%locy61;& z@)B!U(r-lM8C$+gPrvXsaf<)>n_|4;{560l!fKhdS{ZVgs5`B`m(qrbaQEdsjk0BBE$d?w3g5onuDwgoVfX zLd8x;%kf8ZllwYL1bpy&m`jWgSTBAqec!xYn2DOb;N^3CI9dBs71zaOuc3;Q)D?M8 zXM>J3rA|ZRR-K#M*jT=_G%VaT1IH67EmA#g$B;}>-+VMt`D;IfrPk_&^EFNKUe3E2 zeZBJ94ma<2SJu62H%^c<9WJZKRKH`>G@D2z+Hbx-JK?&|ev83CU!km1<|pnF$so=h z*ei40GpNi~R@QZ9!wfFk(`1kNg)Vklp~onG$kdMe=)Tv?IEP_XgvX-Z|G$85j0x=C zt+gIy1_}_C6#3Z#z%=UWAOUTFMA_ZlJ@Le7RvFa(%KCZ>(vkp$qwQG{SdxFt48ss> zfF*z$iH*-L32QK*!1uW`m{4m^HpkUK9y0#$!m$|4-l>TR&Qiq7%=Gl=h|NI^1Ua=L z^oe1A8KljOj2su*Fi7inIWgOl@4tH5+@{@>$ccu8)rf*g3FS8l3Gd!hQzHou$u@je4pYJh3G&TXG{)+b z%Xc1L`+xoQE4~uB8@?OPl}iiK&5y#W1k*0*C(#DVye$6%mOJbFIEUrHT`VJZiBEhGpGK&N`pE(Gg#+z8CqJ(YHOzxh2gJp|Dn*t#AC7N zYWsD#Z@y`tcz8|fXxkJez9qW5M`j|QmY0y8oII<6mo|-x#BB7U6>Z)vn17wd<2*L} zeIm<%i!en#^n4^QnUo4o!H1rVytXnWf$MIq^uHD8pmVYS5Mmd#8?Q=TI)1OsU zwfYHik#hWY6VG3$g!K4z=gH{pypPr-L0dE5=GqJmcx)0Zmhdc$8) zPR{w&t2Xn&63zJ*U)2{cl&il0aw4c;i(L$6v)1`(UD43c5S*So6pY~GC4c>{kI0sU ze;Wn$#~L~wjbaKnM&>S%O%WC?h0!rF*JmYoB+SH6q({M&A-GO@HKAXDE2m=#CM`ZV zp_~teq{z>oKjU@15xjXqZkwbD&pW8+OOm3ad*8F^S~7{hz91zd`?>oHIWqF-^@SjH zMvcZe!V&``1gzuBUr&&~9JhFIW!uq^8FVh=lHMZTsFE@U29nR72{1940UVqweG1+P ztl80#kx#X*xVX3q^6~^{wl=G4YnxD-E^cgZ@VE8N5~3iVUznjwF%L}MtfRFR;pq$`3Y>Wc<6k}2Wv+s>$_HyT7N-q@ zYHij1>^GB-@00`5=_5V;#1!Y9=+(m{jsk_u4+=wH$8Vc*_UG7L`ab(6lK}^sr;d(> zIm1lf-WEd6MC)<8$;xt6k|z80C*#@1@LfkKeNnfQmf|=2g$ZIL1BPqy-!87vBmg88 zb8CAdh{0o;xTRY^NKV zoT9+H=$a(bBM)~H;+(u~+(UoeYOxq+PqtFOA-nY9gGUoVo#Fp?gXk7jSA#*&2irx9 zKZp<#Hvn6&4rJ{q6^lL&2?+tv&X^Q6Spfe)H5?P!HtV_UkHZ;BSPkhq}pLb6Q)EiTW)!IEL#xs zi4TC*fO>fN;0Eq-g4r-M#GgM-(~EKl>pgkv%+k!v6|9T10*sS$fZtC#U<=pP)C3KF z0s;RRVM+QM$T$xdoYSU;1~AZ~-`@%ShlxXcf{V%flioTf57KO=t`SoL?nXe*q@5l@ zDm1iz=MiF-?D0BJw8dMol9I%Hg(#SZ@ygj5u1(s9Oe}=zBO@|U)Mp_=|aH8pq&3JRvAXj@uZGBe+a zR*+Z5?CYnSf88vr6o*UG_u#N5U)X8}(k_Y_S7*yCX$!tNzDE3TLUvA#+N5t%g-4?M z)D@Ebp`aA@sLXG~`_LWRNaSzuiTmB~X(iebhcm)1JJ;Ls`n8)q-eKUakg@Ztt?8IQ z<0O5CMrP*yi_NGme-A%@=D5BL>-+dg3vJCw#dN;eH%c@g2czI(ed#-N_v=c7_2NTB2+g@pKtJOx@@7k z#z*kVzk%{w0_$aV>vc^*s|IT}2ub~!T!gOZ25+iAi#m%nzsB(oJ$(sM1GDUN<)n@f z9Ey~As$aqxBHsMG;VrSZ^i24uT*gWf1<`j7Xk%v>@W47XI~;3 zq4MD8e{W1@!^BhsViLcqhzrD5;4+xFtA;LQ+Jzh1Dz@?*G*F0twy!9@`}lDgAgwh* zcRYVgGcz-VWX>Gu9wtn@?&mPReEAZ1vnoU7>5kssO<1xMn~o-U**P|l_uONj_%8{_ zbQmYIJCXp!)XtNWm%t<{%ffjd-3uEjIW5f;e|uC;S=q5aT`1Wx5!j4RpFVXme8d*} zcO>{O@{jT%8cpGlCUg1U&u9xjgHD2;S+D9hxmL5+WpC|iC z+@_;mSl>Exj(fu5>x)K2q^_+k^+L5!B_RSGF1TFT5m}t|ZKRx@3N5QcQDT;3Dq^88EpQ?A5``Ssm zwu9eJWO%#TsyH#fL(%Kq++5bJ3vmR}v!OwbghNH;#h99=p`f3nwl>*iJ*N$o=&L76 zx)fd3T8_`pdR)#N8pa$@kdAz_jh=)n)}*fpKYag|ik0@^sS>rgle=5MhG(OSv1Qb2--zGQ<79ycMNVZ*E$Y1 z_C4K40$4K7pG#CFL&+WISq~L z9T_>f>5h((vHrQq&c509;bEKK&$CpZs<_Q)xgi!j@{rQ>^_gTGnMf)*ns72X+SGvf z?IV9TC#N7v2%a4s9zt#NuSCI192p+pLB-UM&&uLtQ&g8B!*BS?t2PSA!sYgs?y;!w z9y1%0P^CjwVuq-g-^fgQy2GcdNi28xuu_HbC2|8#kC6drmX2tln6%77Tm-W(15+A# z{6iZv#9X^4za@MUuLiBlt2a4{g#D}QNI!#}eTwe(e3Q8QvF&i(*ZQ3DxoUd3DYCWf z0yIm@s%Q8#XC9tXL0@s5S9p&zu;@H8-xJ>zP^_s&Z{H8x=QnSnHcg#-SIO2slxx{ zvE%=h$FA>|GOqMz^f)@Ok6{Dd`{Tz~^Kq$veWE!5-IN-nU~@V@J3lZ-$p$4=PgQj| zoRC#?;Q)k4@XRZ;sshP;Lv<9=1qtw4^b8Dc`SCo7R7L_PHg%1>+MMJGz43|U-76Cl zd3SpM{+^zoM`Ug8p@D8SwY4vK@@3NyC+-r$nmA}+3bD`&uN9z zcfUyd$PVBC_38e{iVBY7=f(IQT3V#94JAF&MIcfgAn}!2Vm1=A@_1S$X=?wg_NP%_ za@ zO^u$&2M$`=)1>uC3r|l^ZS98GeP1!Ri_b|(0xxkePeJU^)t(f5A!qRmJ;wGasmyH5 zZLZ?;1IrV*%76TiIxpDq^JnMKoxKrA_Nl9?YAP$kWHNH@mxXozV1AaV8K-g(z<+&% z8qVWOh>oMguOL5a-nul}->b{?Y&spRZv);7Tg_*R>Un6HLOaXr3e)!TyX#WA2H~8%qP2*vp?lpv&_)B$nEG@pH z`n7Aux*apdQaQvqJX69#!U^H+wl&6JRe1Yt*RyASr~4Rbfx^n;r8*yr^GfsO)1fal zu*bTfJa9*`^oUO7w5NRg=a1;V=bvA$yx-ou{@btr5#X0{p)MM$%zN30mORh^76JoPCwM}C-RY( z$8CRp7Y!3vtJ#aseS7-Q>*9o~J?6Hgu&^*pOZcEP^A14{mou5ZzP_!DJl(^GH!@J! zLi<7zrqGeQn^?{F7#YK45FmPa%lRHFDk)jq2#&s5oo$e)aET^;?VDep%`a?%-H zg<(m!*m9EHzx%xX$S%;0Rfjn7fd>?Qng3VxDP;&cJyuY_i+VYs3`d))s_OJieVI2i z3ro-RblQC<(`Q##S2J5n-*F}CRe_<}J38_m+()7g{C0A3!u_jzK7W0c^!9CVMA7$< zWV_E;-2D7q9je8;KkicPA2tPZ!Vgqn6tPeeA^l#U(BH*&L14>Af)Dv zO9&+^4GSq9p#On5vM*$1YcWJ?1~RzD;$%NyVc-1uqnnJBR9j1nEG-#aZ`wm`X$@J~ zDe-&1%+KAZZ1W9l>UU-WO7rrMK9l-RxLc7ag*= z8zYH@5r!ESiWL@|gL!>~j~nXH{KXd-Kq|@%$jsy7#ylraRW{ZZYA}BDCaAL`7%So{ zSwsZ>&09AESUyU<7K$ZEA8h2aIdpZ|wZ#eGV=s5NpEnl`tE~7>c8ws;H_w;udY^9& zf@%M)*vxI}*J3UA10+T2>>MmO1Hk^vR|P|;v9a;e`)>+a+Aq2L%6n^PpKQpuCDmpL zCEB0vjeqR1SL{Dw9_)RtByKhML5W7(?Kuzk-H^utaw}i2F3S}aSxIYiGUuj7OBxLF zjw}(gNF+kk%+6RQ+Y5cZ^}9zWYG7t$mM~twijti7Cvre+r5f}f}m(ROTZH^I$s>Hlne`CqwNR7ShH!uQ43@OsT^cFX(s zMc_()e&}Bs@_9f10j5zt-WT#k4wfE!#aCA3AKz@%-+2D}yX-3s{x{ddt1)Ch2xV1Y z8_(tz;H_6swEEaEvsT0QFk4m-k4%f4=ESBI@RLNAA4;uRs%Vjs}jn_9x_>Vxe|tZSx1w%*5VGAak_9>JzOT@RC{=I zWS4IAls;NiR$mk&30EwS1Jx=Zb&7$QO~L_572O{m50_bp)Q#SOKtE=yE^eDbcN)Zp@->8k{ z-@k8B3QBmh28%2K^bTHGrA$$(pwk1fTWAFU_huItGxOUNQ&LgEBolv`{|@-83r}BP zUt42ANZtb2SF?UK(@X6i%2>!}GxpEi8~UmOARv@2!G3%GTC%d%plq$3W!fO{{`N(Y zaB$C1-U;>6)1!>Cn1aY9IT@KTes1^F6nP9=f2;@MY(UM=^aK|lp9{F`g=doRrd}>w zVD!2@q7dgl+e#CdF#SwaOa1$Je7Tj~@QcfGMQgcXliQz_KELf*zl$}MsTvEzRv$ZY zqJLE^2p>V2V!q_Wlz|)=pP=8W3>P00MLDdmm|yA3%*~hY(7eefIFczjx6qN&jWNUQGYR?-P^}DnO?EN@N&-v#gBQOk^z0@l(+E? zFVpy*+}G7>Ih(MObyXPrk?q41_C{vdCN|5;4`6- z5ie9GchBsOGJ`rzO%Cwvsy~L)n=-cF)$sjI_x4?hvD$A6orK1g?FGVDgZLfwsd#)X zzJ?8Eny1yHL~^P6zn{bP~t*d>vz=4r(ZO9S{xM};8}6}5Nfy~WVT8z$uK_a_hWI>QserB z>|^{M1XQ{Ke}` zF5ic)a^%R?23m6%mT5J_iYXioCjJg=>r%Q_-=2u#NHFKK;NZV}C>c6mH+z_+nOAP( zktUf#Ap{2xpEC9cR?^L}_xT$3lkjV#3OQx*{PJT`XcQ~Xq{f=-voN_><&mC5QE_>b zbF0ZLu}{+i9ywm90^xKOi0S_k8{+Tc=HK`%U&8NbPaG^x6E;)rHxv zHK`Vcwg#Edc5R8eVi2*^{e1rH z92{Y_slT zA@VsZQ^dRfTNkk*%}9J9iHgd=@X!y+hJ7a%Egy3;b9TeVXzdFZ&J*m^gr<3tH{PuN zq3=B})~Z@}9$KPM$d$B*-Ktt6cz*GF&lFs#=eB0%Eq?Q2#1WH&JE%yIf7e1I172T| z?acqXMA?6i5V72kNvZnTza>&>crfBT!i7kEY{kq}*5q36arn9PTXkzSIi0@m;oXnL zjbG=`F6nf&iobTWxJb$#<(P^~htOJK-YU^*Bu~39t)TF`w{$}k>mbIq8lCR-%7A#D zhb=2=c3ehas};>kcS1K|(gV_4OEQrpAuqTY7@WT%<;C{+%U-@f?NUukPszy54!9d5 zhpVwh6)a=e;$`~25Lb-8Yd^*|hl`@s;(p3f%Vmo60ICyXL3<#PjerHddPl)a%gG=! zn~M1lBlV~izjw4fz24i^V_wIns;;Km{OMjPxL{Rr)VifoLC2r7((U@nKuk*0$xDTq z8GGy5J*it+o;D?VO+`huwbrX*#0o0H76glCN?Cq(`c56_ZmHyob+&E~gbGISmo~EU zmeVG9wlzp!4JJ~w)(ZCW@xefSi>RN21x$y#K~dhS$^ay|lkk@N1s%~{cFqQkI=k5YbMgUQL zeb`+TXa>mcFfcH9V9OKYo&))>?sR7;Rso@}qJlyt>iA1So?EaIy1o-;NritTSJcfO zKL6JjCsidSEXSn9Fs1=U0v(1oz<39mAL+(9%EpF<^MB>3V>ma+5p1-yH87ZCH67mh z!y`{w(EE;t+`9Ydaac$Q3lr1NpFJhT#a4gTAoas?=SF+zeU&(jkl<5H&bim|zCrC+ zxRehW#L)MAw*}9lvDurmaCKy4Tq=`0l*BF771-?1(4=^HoGB9eSEx zd}+Mz-{0SF!07-E5O<3h`bLW%bWJ4vSvy~nH$A>>uw0sX^#(^gL0ystT@-g8-{WG9 z;P^TJh%*YWp5FQK=IH21;4t_Evny|C#X0-{wtEuzCY77ZSVvFty#KHsVTsN6rsZQ1 zS^ECxD$^8CA9#@1ezqE8h;(D4Ot&VK>RK?fu|OChq5AN5`b>Asw6ocWRm#278!{`_ zX`==WibX|i5s`ff<3I9PLPDbn5E<2X(ik(cL-8u+h~qMm#cwX8Ua23ljM-XC-N&yW z;{FZgT%}&dOxhwnO1_&BE|LYs`sdE0cr{ou7RE#M{C6c1YhJyQkxt>Anw#2p>El8) zS`U2FV;e4&9Gd9<#VRMyt~8vteI+GtiSf!!F(=G0wpNz-&zL0&a=Axi6<6~k2fOYY zEn#n!_8!eX)L zR7M_$KGYAPFlasJVrKOYN3DBub${|j$I$u#d8@^hyetOt&_VAj_Ola#FR!Bzmijn8 zPQ2#?#Iq+0sz*O-ZusY)Q@(0PU90tr@{y{czhM{m;{H!c&ljiNt_m1s;aE26_EVX1 z$S(6y6Hd}@RgDK%`*uYOTjz^~6Skz{cKql5Ig?UE2ZyXBN*NYB*jWGLY@Zc?KLHaL zL*XK3f-xwxK}8($0-Erc+P~p&1s$1fYhd;L0m^_O=kl1+a?#=q8DMO zd2#&`gn$7vs7iT3&&=EibJs%01g7Ruv%v>5?@J9S_*w{Ym95Rq-XaQ-G(=-cWNXUP z+OIv`-5W5CqSmwl4GD=nWh`*tHo4R!Gft=OND0P9KBxP~0B3y9EJs1YOLoxGwo!k5 zp?nHCB~G8!j(3TWt z`P)7Xr|sFKxc(ZjO-&0UEXS%2$%wkM8N0fenOJD61nKCPiPlJYU8NplVKFjVXx^3b z_KL}y3@0$yjYvDcVL>9GtLsgu$|EQ%sp3=^+;NY9;C{znvYdh4g~)d-^Itqc+V~68 zU|`hDpS2Ce4E`60U}JXn$N-_7J=Z`k2|q#WQtgI5iL1zo7*5 zJ{aY97f0Z&zB=eq;D{qQ>CQIKE~)o2W9Hc%_XMaw$ZtD6DwnWDwvY^@k-5(aIIMHd zD2LziX$llmaYx6-$7J+dKJ+2PD*L~!}?3(zpkUI z;5&GMk^GKF0eKfRA8;uGUqV#W_+IwHER@sxDu}4q?gNu+UlZ_?&>WJQeN+=d(5?`-zc}-3`7)`YGX6 zO3#=L9+CvXw%(^#nc7v>QUM>GSzh;byhfLPaNo)5gEIM?+1u!>RJNxV>Y`$MdST=0`CrgdlsFunmF+$(_d@gQx??zBQ)+e-3Vo%JG+0!uB8iCgr zdJXn+<&6Y3g^{RhjbU+eCQAo>RL(c2JlbNhkk7B+zAT;7GFnCrBFnO4St8^}5sbMw zIsFMN;cSUoZJ7lS)b76=Du$=-GKl{dU)z3rXKGWf@xnD=JSFUN_%6MhG)F#l!~g(?qbT!G?Wm6wOE%d7}CRlzz$ z@61<-WxJnH=r*Zb1>_j#N!a%c;QjA@+&Kc`}rei#FM7)?BBd0 z*zj5Ji%ZeLjeC9gIx^bYdOqQKZ{G$6ekd+Z)rg|0{q!j&BO@p`5$02+m86BiX8mm_ zfPXR$B0F$fKyQtV$m{BcNhm+2bx`)r3JD1g9vzh_Dk`ZqDw5TAYi|XgJyqB>HKuH< zXt+QqBV4O6C&#$6+H8sr@6izI!Tzp?<28-j9GxquYya(D$!W;pDXX`WJdr@SdPuub$xb(hcY~&!w z-;4TPyLOIsr}_{5wi%yXe+%wxr($wA_n&t#lJRG$_FDaaKwWRRlK`NBks{ zRF*-L*MCsPX7V6`r{`wq{_mouB7LZp?~`0gtnqZcd#ChBQwHZ|YP3vxqAs_+bzLE@ zuZhOLV%p<6=j*Y>NvvVmElds{fY<;I1Op3LCm40`j#_&W!@U~{;)g6O zrcqW2X=&3iQ&$kL17)_^ybtsyGFvybqxE4%Vc|0zd&6!}MZiD>x$G;lL{mXw3j$aZ zXj6C2!OMW?s`)wZkergz4uHppSg%mBbDI8*4`vJz&0czGZVsFapY1one;^C>UuB-a= zDeT~ti;LSUH&>TeZjLU2t#-DXy?tzer-0CP6P;d$iD{!eDEz!;H(N5k9V9`(6BPaS z+U5bR!@qe`ABKZMgv?L z+ILCTVhfZqjpD3S+|iKm8>}KU-Q|<)>-g$8JyzB`x9DDHbmV%_g-1L$3279)cqBD; zHqQQmYEp%=`U?$JhAxOWeV=oasl=UY@C}1d0kq9ewmkmcN(;1e74-+O`Lo+maoIb* zL$&OzYn%6bxLE}sV-62L#A2_yop~J`Qh?L00P4p~orv5GtTgCB(VuXhIEfNzUEd&= zqVW#E@V5`-`@41%_cP~pe7Bu9XT;BIU$c|xYrQur?M;6@G@?}cikv-mY`+i*Mq4tU z1)zNXJ@e%eA6jiDy-HQ`4+_!*alfZuLY;($H%a{M2|l*uSpWT|_l7G*k_QbLLnB6; zqv+^))jiXel0M#rt4B%J-tClpOCpGTX#Te~>C$;2(pOFD%w}w@^^ujQBF=)DWh=rZ z3dAKd9+KZbF7^k_v9r?>_B0xtY1+1(Ms3V|7R6C%DdtQCQfxhr-V^&Y@EWrVQ9Q%c zL0j`d`*YTWkNiux3FAFb=`_pdrEvm*Hdo#F;}=am;250yo-Ies`S?LlR5=^pMVi6d z@0fvj^9I=|J`48B6dq8oY|k!iSC+6sHI`SX?%tZk^ZZ|tNd3hNfNs9WmM};IZ9{4I zYpe{tXKXO{nX}Va;iE^7GI{WWemw3)xWEl&XKTx#m`*ya1HqY0A!l=J_qwoY-A)1SuL`|fUTeEeKclqJ5WW`DpetgLK_SoOZM zk59Swd-2s(TS)o0{D(oAyk?Qbsi_^6#>&p%G?O~ey$Nk;2>|jK9wBIG2zmD|CZ;Uo zQ%u>rcYLHiH|DLatq9-Vp0@}V!=V+yYtv>Sgkk{13!kF0vc4ZDj@vVy$(}%@qFt92 z7f)52!Tjq92;P%xiwQaAIa~A#Pfad)5(jMTlMz$yuPwCSVlDp{tm1Vg^AE+bb9#kc zqj2MUELnTtTUQnYp7Ygc>$RKNofT9?&PTk5o;u{mmF#Nvirb5MFU|Lgj%sVi%MIGs zIYzzU;pt4T<3p?XR`K*tAB!e4Gnyv!lG>8PzQ+^TZ$pn75_XIF<$=T(KoI>d_g#;B zwg2;uHGl^o>S>^7VX=Xl6Vwnd-z&C3MpND+T5LxqVo+q3N+HXGebo zV8nUmxl|?0cWC_l*=Qir4u&eXE^i1$lKE4^WXU?a>8U(#%A%?&C%`2^m7HkP=@=Lg zhcp|UvGv;QGwv`E2!OG66Wvxw(Vd*0ek>?3#lKQT;eB+&4yJ28JrBBpjw)8iO@cV< z&kzCYV`7Ag*-{#WAh1!4dhFSNv2aGG;^h}k&C6NX<}&FX`HT_u=9_tcs)(nx-b=Mw zKZ$rK)u6RedFndBDPz1KDL$Jebn{!l&?@_`Z$1fCkVcY!v78L>%SX0VX+%IlL|g8W#>q(k%` z5+_1}f{K~kM}te>q0s~)g$AOjFjx}@JGmTCDR6lL(NOmu;qWQ&9a$w7Jo4Rtt6F)a z`@ibwRs5du9p{RL^}W566t*~^W9>wtmZnBm)3NKJ+kI423-}f> ziIV2)%H_|_#57ssU7wOrRUNqaTxr1DeP>~hxY_7~i2Dkw-g^9;cSkr3^{G)DQ2l#% z9MM>mclgZlE;Z+Up&N4QdF9`!B~(pKM_bx!Y1Vl#k$16Ay)`n;OD-p0FAv<7SS5iZ zMM_}1HOEz7VDN|HCTDToRP_t&k$pTn&>xInv83?hL~O#!mO7&tyB#lwz$?yu@SIt0 zOBxb-ImmVnxp)-PzJwQ$hU3BS_(lbNL8CscaZ^$6TRwlx(R8t-??0YVn1+;#;c*yz z+7RJxpyL+#nLdvAgpezVrKP46RIl*wWwEcR;+g< z41ui3%?%Resw1MHINzEUu+6tEWC2qhf>vOvvr?K~L*7zISlFP^<)%ivvT%#kLJD!_ z;;#T6K0bBu47?v@gyPe*{L5n&>{_6%nKR$q2g+Y7z+mLhL*PQrZaCj9K#*t3Yg8Dp zfoGvGJNQCPZKlc;s|EECtW5D|Fk+~(u5N5_G$uS+=WX&ApXK6@J(}!VZ__z_$3E{7 zq6I$In|%}=qX1Gp*x^u5y&^}PG5T&Z&s%uq;Ck(k6cumW2!gcl`=lhshfI*tvVs#G zq=dDNfc1?H0G~?|pBGzOV`E3gzmS*Ld-B8uc`rfo!H0IUmj#`j&7eELd=XB8Sju&@ z?{^}Yt*tj;v#8atRX~=iC#k~!C9dJD6Jv5OrJeWPU?apC37eefWTO6Y9sk7g*3 z+h2lzYzH5LeB~z~jKU&!Pe(eEAY`u(DJVkQ$Xc+=)Z7Ym>=N z%zL9-luQRs_b4_RvSLP!ADH0ed>jO5f5AJ;}HOF zG8_8SVNl@;rZ2kaC%2;w{v9VyWuCd_tlOfuZVkR*RLI~xm5Q51xl$r_2iB&z*_Wfl zm3P(f-%2l?3>OVmROqBXHIEf1idSRTvi41&W6q9vjD#5(o0d<^xx!*3jlh7*R4lMDUoF6Rl+C3!Clo)vM z4LbPzL(^nW1L-aOSNIVws)Qdupf-N$J7NBjt=~!{&2U-u(aE_w?4d;3xcJMvD^a&3 zNE%Ogznm@K>Vyk3q`T_v__i{eJ@A>nbN8{9Wy|35nIrYF2DX^|9Uj=(j zW?O26;+54kEhGftc>l@|%@@;&zMuhOX2J>=7`U=qAAWY&MeJvNNKCx$ z&UJsX#I)7zSCNP+hq~QhswA`ReI3_=&yo4hpT{RwS0mr>i|h+g3RIP6xn%|81l|i5 zT;tHoFJdNq1cH@ET3W<^1B>9KZ?GOvhF+2-CjR{SGrpHDr5#K|zWa2`2h>rT)2e6x^{=hP8Mwg~aaf-+#c0Y*jvfxu;-p8!A=o zJ`E1yjd8iPH67Dz2|YB4k@8Y>!$GETa;mdwbbQOr60gQWE@tPr?-6?{i*zFzOQe3R zQMt6QjEIK}13`Al`WlPY`9xQvu)t9zIHUOGHM0QX{dXTeew&i8cE!U-hO^Si23zW? z3BkQ8Ej+KQUJ3EK)1djatL;fI(yaxu7nTsDE3!6vb*>PkdkIAh5 z!!b^xj8yzrx5q_lCS88fJgk^%BdL)h|9qmX9W?$98%Y5b6X>X031q!Lg3rmK^$>GF zAw7R>Y&(g*R@BiG-O)2X-$<|->KOKNAi);yNR44>72{L^uuAMO&{X&x{z>`#`7?ay z6_-Mbfah^=Z~%2Z@G|X;KRosiejD1x#4sFyxf<$$4(QEB3Se$D)Ys>JSRTUc)A2GZ z%UnsRtEb0q8d76c24Mzvb9bkO5E=p;2mJy%k?4>GZbWr?c{#iv=re)>0+vklm);1X zMkpr%0V10;uEVdDl|yjr{z0Qhw?E<&E^BI%y2Ejx8TJ1CB)I%b-3s7B3eMPl?d@HI zG@X5lFE4SBm)F{A7MQLB_;F$(f8)B zkx??TvPkGFu!yl@=5^YQe5K?yfyDK^0u_Xwu5LKC0Kaf$c{#XcVaQi>cK!=iODL$Q z1vde))b}_h84Y^TOe065)WU0t8icO{nwqTGG*lq)7vvs2vGnJ^zy87jA3690w|=xx zT)(bwXmEayf9?~)6JO+=kGwJgn}qj#kvCdM1VwN7-s)U>#*PwlWqIA}bb{xPr6>3j zZnn2u+FOpa&T(>cTdySst{!g2XJ#HbTr1$ScuP)hwDogVj*vQJFlxP2b)*rzEpWE72@nr6ud2#psOF5b?iZmYy-5*`uXa{Uuik- zM^Ei1P@oWk`L)_ZsQ%@}KH@E!?g;lNVX#>v-sJqI*A; zU)1WA05Hku>j>uxx~C(SDCt&PD!+_wt znL2o1nhQ@rq}bEb!_LA2ZjwD%S%()snzB9YcbRK6(A6bX&z^c$UA>>98gOedFievuUn`B62100Ue^?<6&)xhBLpMv~f( zRJ|SgE+Id1zbLG2Y$8=z_KkENKGeaDI^$;J{rhY4d@_j(fN^z|8| zI-8@9tAN~q@L=DO=hJM4=~iFzf|WuF7~Sthx(0$aj>k6@*&fQ&6DXgU!+j z&EA7N0gQzt%dlhnNhE<8wRsdenT zAB*iff{sFIJ{idj$HZ>yOQoiD0*WQthCh85l3zya1yIXiB3R6^dxDtm8@_<}CJKh^ z-Cg(O`Mtf}`Q||X$C8qGr`pRjSL}w`5w^UA(mf_>EsA04NiS$XP?3I>5&g)1=Imt79qNI zJIR2Xw&~={=-VAVy01URWCP7C)(2g+)%^zaM2nhzSJ^LZFy!9zn7qBI{DSzCxKEVA zzv)bPweeVufWR{-B_+w2i!oG#vR<;+eG+(P6=l>J^fnA93=WALMw_hAw zX+hVmJlg*6%2ilkC4gJ;?c2w8cKKkWW@pFIo(6yk@_}GcC|JY3i_ADV+WC;5&z{L# z$p;}@FuPmr*=Pf0BdRw!0bNfz_=>axHAouppMd1?%L&@LHZ}?gxuI(t*Qe3UM~-RZW1~+4jvvJE-o(RarkyX#*Bafv*qIR7Yf^DV5oqaJ^nfv#zgLT zzl9esCZ^=Y3l=h8MfI64Ur>XCv%t!X_H}_JXM_*>=zhPstlgX;Pk8&Ib+y|qF2&v5 z{r=mxgq{c6`q6=U4|K+K41~UZtE;L!JlJKV67il-7dIAlY1y4jf@kf4fq~bZ-L(gx zN*VkuvO#e@3#Tqs4#v;Jur*x*3m4_DqlY_E4%!=!Xy&X#%-PeM2U3PwI?3 zxa!g?{0kzQFq~wFSg{Qn^7FV(KQ6FGJTfnEnK7GUz9;a4_2)gLgael5Zhxe(xeaR&b*$do;1~P7upCA~es^ z2)E_Wgb_~dG$DC9oDW>?rp9VFos9Wv5IWly$+!x?x7M$4R@WPi+fAep7c61%32UsFz4 z7$&8>TaJKLcfi@yVbd3#Rsy&ueg2-F)!?^OyfF|Cz*l~Lz7O<5-U*gD1La^-Qxp6s z`{Q@i1EHYEgG@uye%Cr2Erh3m!8lZuQlHdzMCxEpwho=0ZnY8-5nZVl;GVJ%5PDy| zLOEUP@2Jy~V|NKuG}Me*-S+%o`iFlvl2UiWdv^@AjmvIF^){mWO}5Gzk(9ly{apXQ z6f@k=pc79)GLlqbjr;c>`PG9i5MoWX>yqn_kn$`q6ck>lY!i`Mktcwhp~1~R^ywLJmBF1jXF4YZ8y-aM1TD6A0lAu=E8RmmOWa0r zaxz|AU#MqvLy7lluhB6a>FDx0n_2@MU8%_hmyl4^-d~eo^bqgLs>|1~LHCagNcx{> z=-*|H&dy;iyT%}Lcu+h!wQT%b6wRJUDkQ?-H>4&dcRe^fVC*|^_F^c#^3|rxFqo^N z*L-K!Oiw;Ompr6i*)ReH{N+Wchq9SxVIg0#WZB0sXKhX`%%`U?meH(jD0~1Wq-jKo zg%l`MvJsRIN0`o?p%3S!5%B$p+MgDe0GRT$}15bnqL%3S~qv>*A0pP zAl5)rFS@m-`Db^C;kSA9^iF-@M{Z?Rx~$Ezvj%ijO2Cqr4--FY_K-dzgX2d0O=X9( zee-W|OCS}l%|I&IY&g%}%P^3HgC%0%JoDNaX-F?vTUcFUU?%3Jh{zCmSo`;{tjdCI zI^@1-=KhjJXF*Y681!iAIZQpWXGT0yWjFs(w5a;1%)5`uRO3*UkO(DRASuT^iLW91I*kw~^D(;MqNw zj^G67R#DjLQF=>@<q#sAI(z}_#)+} z@1yDc`XlcXc_HCjzF3ebA{H4|Uk{06AU8^B-5nTi;LqNG<3qFv7qR{&A$uf8z1s- z8;l#wcmv`)^Q`6zd+f7Q1=+WK@JLiea@L|DX`<8-LnaJ_NIJR!Ti!v+_anD9#?6}0 zmB(dOxjeT!?bNI2jSd{_4{IsM|363;{LVs`IPM>Wj}rP)p5E8d5%c*=Kl!xg2LQ;0 zTo+&r1zHT)^#XvV2p72tBK!<(cSY~G6XrLJBB~3sM%!<@@K2xwE#VM2zcZS6S@3o68Aa$voU6nz}EdGuuDaoDfRlY zH<42gNd0hpSONo|dJw2kt}$IBrIZoB;|*w;YcBm%-PYd>Nf@od2pfWkzUZYbQN!n4 zB7dWX7LIBJ2+uAw?MN+X`?8D?rY+BI2}x*Rpuo4#CVoMvJ5)()fpQkfAJLS z8+0Q4^e`QKS2cU7Hn-Q{l9eQ|YjwtCSvXqs686=68;j!1{QDOU_7PY@aSm?)gVD(r z(&BdB*jiQwF4U*r^!4oa!Z8VxLv(}cT;^?)I1g)is2Kh>Y+(B}7En~mP4 z{%Bm@N|UiKu-0+E`V;-gAnfE%2xG;J`F%Db&8RWobZ4tM<#=c)&~BuQ$C!8$;YA^6 z6lLw?T;aI$HnGm7!R#`PmQMXubT%IR4@tQFIXe6_^N!C{!gr|?TEZ@56#tYsAtSv+ zYAlkv2XU!c%Za}jK3G{nYY9SO>c4H@+JURfA>qUu-bdewus`~Ec(UopVH3bc%~kH^(m&wR-#hs2PH4m)z8vT&!@}+& zZ!XAkh5Y5wZE^PSpb1T_!b28&GxQpK&Gs=~p_S_>ER@;N&b-%?rV+HqG`-E1d$GO; zI!Gys^GoWG(&oUe;u@~>{;x(VSHD<8ZLP2^aj`k${=3^L62%+|@&OKhq=*x{>uAVe zytZ=_FvtKd1!WxDvv2f(#C{4DVS@_)DnHcMivdl@oDMa;Gth{Tuwia)9$`!hC2Otx`sIa%FBHw1 z9jN?cYZmqeibEhU<2eRSIZpX?ZY+?r=sK4LK+;TB7ai^&0Wm@^0a#z0R#0swao0Hm6A4{^c*B2L| z@_Igf)+F|eyePrzE>h{oA7e^P%mpff`29M&ygWT`vfRutHNocM?1#plzCPwQ(L~5H z8mp=Cdi0O6+Q-$+%je(r?njI(UlJbZQVI(JaBrRfa>KyO3xN7E;=i)|HcPBH#>Nvn zl=VWt+Frf0wNz~!| zX^-Fd+#2w!G|;~829xvDJIROSSwchj&59dY-V0Sqh_x(T%#e#Xg3O^E^`+g86oJX(=y6!!R>j!gT?W1;o*1 zK$k#?Q5Epxf%|K^1+GAGi3MI=qG4Pq;j0@l_4sW=A$a(qY7SV?+uw zo=v^^HK}7LhDJ+EEdn@tW5eMO+8|al(H>SBfdSZOUNl;gtf}x5(+szO{nggJa)6xq zA<>H$ieuv<5fLO=lEJW253CFti;38t6pUJ7geq?SVW_V00>bh1V2h8Hn+2u(CWYKn z8}0c88JV?#2z)IF)YV12yp-AVO+Wjr?h1rP;6deD&!|;*Z*PCjzLANE0>nou>gk!8 znLWamd1s)bqjR4nzOTrB-|osAWGoqTDuj<*{9)A-D;w~iDoeFVj-MM-q^z_^E%5RJ zf^C0Klw|OGrxzzw!M2>1S?-t`-*WMyVQ;?Gek2U0r=oj1UkmXTCC=iD9J? zS|ahZoA9uQnxJj7=LZf61am3QU<8~q&bG#BwwcV~A$8lx`@v^W>p1(<-Y!mX?U(wy z_8OA3SSp8{h`_R<63Dz6>g#*n-SX_>?4sEptVWT!R^K9sJZy*YNr20E+5`DT6{y`XkS;%LcJu;SBel@ixF-L5ah@Wiy*Y&xB5_c?kt60TQgt_7 zzDx0Z>)wx9$yq|TGd~JtM>H-Tq?uH<1-D@|zJK=pl@^*kC$*Cgr3402-YZJ9m3G>b z+B_H4R=l{r*G-b8#@0P?Iq;=9D*T8>PS4@D+T+!gms~%erjD_h=Bi8;3+M_}f;jow|=l z%5rzjkgTK{Y+JOnsq$^i72j-!(?xt72KNP)*^q6bF){Xl+M^+)Q5!1&HA-+dvHJYWC`M;;!kn(FG)e^mKPc6e@h zQE0GD-w*wmOM1f17l-*nN0+&{@afvx1`S=>Ln9+e2;yp#^*=_f3GF9EN6!GD>b*5D z1mPF#+F~9j3y%Oq>+0$Popcl)9?s#GIR%p}4;!2Nl&cSwA%UI{hN<)!UyZmRIYx)q z(}~JfRRjSkB9-k9+CBg^7S9&HxjH!XbHexlNDa@=pL3j-S6O*Dhnpb4ZO_N?@L6Z; z?Tx)Z-=Z|iKHsbMG38sH2c}&xK7uHz$@e@Wt%{&}bbfjT2;n{Jnn(h@-2`^|3pCaZ z&T5-ww!`2DQxKaLPRqmrjS|}~?(Po85Uw>s5`C#r zgToQ(D(2Ah$SBR2+pK&W9$48<*_BpyEi_?`q(*;|&!9;Up?h!l`K$p8m8)kbaMP0z zP54ar&Qo1VkBvzObd3JxNj(RLr^BZAws{BQ``AM61-QH~88i^e@j4j6NBXPlm;C52 zUvWfVi^v}#&9X5vpcVYf8WPd}zI(7QUcddlii^tFvX^lxLF>4g*4<_Oa=sv-HrCBs zjw5Uc2yf%l&TnsQ{u|Mo58fkuywiB=bD0Ln5fl9$yyu7W6~lt?w8N^q=}M0GkN1lD z?aN{#>UbiDp69`Sb6`ME z41=uO*RfHn9-`=%yCk@k6*ZHnG2o38p!>JF>>(3#wi!AhLRcXShY`pZ)xV!4i0Qc_*T$cUb9E&uZqk=iC+N|Vmti!z@J z)*z+lQpVq#M_^~aP%o009z0L#XRj#sR)=4wNZIJ`H)$c9F@L%n+Cr54*`!v%Vv`5x|(kh-We#9du zD>eHiyg&^ZwohzjSOoEZU}RK_3)#)p2Qlc8Q_rjpr`Oa}w;N&%j-l+gGNl9B$KVsk zw6+>o#u_EjYshUpJPHPIe1xfqev{2N+kGX&vF(?Mbw8K}Bn!`)q>^>4Z@uMI^&RZh z&5{l7&a=p2LPr`GV2JSXxxmNn-pPs1>YFb}pwk|cd%cHAV$uE0*-@XlB8r+f2#}iB&&{zm{UPHOr)Je*MsCui`C@z!ZCL&L(ox=v zt*y6~V-#`8UpBndU825X4Xly0MI%tMA8-*vxeLyCbJ`exp8_jf0|*;*1A~?l?6DlT zZ}(n^U7;<&f;9UbEU8u##oQQK?1=1O;H;Izz4|&%egmrzM5qB&sL;`V^TWL`lDO@I zJ!Ra=>`@D)F8HWDK0hIdewni)%8y7@%er;2PXvJ$O@RTCM<*XvAnbBs?PGC%+1>lz zZeBA(b`@d;`{IzKOE>rI&Bl4mk<7QXDiBBT97SC1j&CEzb zOww?sbY$RzuU;?aL9|~`TExlAJKWcIe6-jBim5HuTc%>c7hhyL4gJ0z?Z6Bwk($~c zO0m-vW6PcFESaF6pKNTc^&bd6GwFP(i6E-8`?ob0 zM}Nme(+Df8QW3t1_(b^Afa^foaKoDM1j6s(6sj`}Gl0bNtHn~H;7QjG@UMYVG?I{n z_-5I#6NKQFaqHfIKSIE3Ce3Y8)F`9^5-2}i&_+X=O9hjPNTE9|6dfgvH%XF~2beax zj!w^_3;NXhhyjk9*8J}S!jr&jTvJ^$jgq`r?LPPcjT)c1`TC0A(G|td+=YDuyhG)# z7Xe_K*cTqkkd};osXy@qW>gMWC9TtBp3)Fy^&cK<^dZhCiJ3f{p(BU>7qjU0tPh0} z-UkdKgbUo~DD6pQL}QpG=^uDEIlCf|#%~HA(h7-w=KD$~%MlWhZ2<9$ie`f{A=o0T zKPkohvfFwjNsy#J4sY#<3vMc?3-Lckvmr3_oU(eh!*|6oiC(4@7i8Ql)c4xP&HXEh zn-S7ndGtmj@bVslEzR} ziTi3N8~oGtGUSGND}rx}`S36n8~YyUiOuHcVKrPN$DX^FRRgF>)WPq_h@g8%dtaW1 zViUcU->yn;YI^bZEty)DF>FEA3WtoIZXQ1W?yh@CA~shRfBt;zY+0CHSo7o?8*k26 zpEOjl>=^&S`RUWBD5zqhg%5(cF;3i|y{+?Ozq7Sbt&8;ZGQ>VaA$@1=GG)Dk=1Gu* zZr<{&Z^RDw)J=aZ(V%3A=iWUfm+#fbZRe-3*Ub9jRAHm6?#22|uKSSv`Q5d*I!YRU zqkmpeCcv_G1%QtLuiQfOOD8EbO$>rF_eujnbieu-h9T*V%nCD)ht{oqYpwQ8I7wPDa@)t9{{7rL>rfn+kFr(Y~NK0cDF?~&i%;3Xsc zc$Mg9RMX%2<;~smpD_qf4(PYZe5l17PrS8#fH6xs@=SLt} zCe*px7*1NUfGgJ##g)~{t*k~PQzqkETZN!K;XF{x9=JxSLRBMGDeu%yG~vd+y=06a zI)5PLcJG0CN?F8&muSKpCd@A}a-SL+944Pj87Ky<2y>NtmZ~>K4kc=308Nk%I5kiG zJR8FE{VN2D9+!w5`=1@`H2M!7TzJ(Bak88UF)M`Oj{d#~#ev}dB{H(q)JL&e!^6C# zl$5=^FG8_ZqYs|ujx-yu@BL%I^~c9mQ6U}^6?XU9LuRjDHLSehKcc!gJRsOUWQ4iS z*2(Fqg~i(6zilKtNZ?-#JG-7h(?;8fQ=90uEjtbv0y>Y4w1T!Rlx%D$M7)T2co;A+ zZn7RlWJp*3L#E--24G+yfg6O86edi_|Io^mI;fJG={T1&da@1_+|oOqy1H7+d*X|G z5Ypr6j=_ZvpcvfDq<8Py*@q8_OBlxc)L7XiPgTj;N=%zi#`^StWD60RNonW?RR)lt z_inV%7PPTyXsCQ){I&@JXI6j4r~?jkfEW2`&))nl3r)_>)|xk2)Lbd*V_u1%*H`OI zm(PZf?8p@g0m=JvZ-T$T@1%l*dc6mIjdFt$hBV%Ajwc2At={7fhaU`mGAuYkaI!!{ z{N#;6_Ty}M`9GKhES(DU81`ZWHovfl&7M{7OGgf)t-VW4MW^!#sQt0GiZ5pu-G4(8 zQ;?JZG55lCa9RBP*N*Sw68kB8%&ZC!Xs6ycHMQ8p*5pdz|J2a*f|nh8379(<`;M4u zcko=Bq_?J*+2DH>H>AJhSHsW6`m1Y177P*UJ6Q9LLUElN2KbMgZ>i-CIu7aQA`Hoc0?Vdn z1`Vqga~8AZTmjw7m9{@U<6UJN8=_F$jAfZCFx_ zCHkw=o_#Z8x9ja*Eh5ZyLQl2r3dQd=C z3k&t+=cmB%q`&`CQdRZVty@yZ&#BDO)}~WaW$RofbGj?K(qKGVc}*vdUpD9g&!DNg z#=fgphhf~^KxuveHDd_#u0>`a(aR|wIR9weRCN;6SX}GvwNj4Dl<<8U>Al+-1;u*V zGi&m)jx&`>49TY*Xu|rt8%bQ0d3mn$%6ISWs5TLZnS+^xSzexXqb0U{;nHN1BZ!>p z>d5E;&dJCVIP%;HL1e_NL*aov^;m^gc zX3F=em1-*R1h^MZ`_!X`T32Rm2$MRZp{94Io?1*~Q{A2d{_6#AS_AUgQF8ZK9ExY? z7^@~qlx{w^a*grv;FfN!73O4oO6SLF5Yge=Qg8=e*{l*lUYFS_X)Gq$AP z@3=h4d=(U1@~Bvqql$Pd^VV&n3R`jl?XzLT+RBUerJV{Rman(S^F)H4_%ty8?+5N=ryEw5zvNU{H0A||g(h4Y$J+n48jML-!(c-sw*LizL4 z=zkRCPl;Ma)v{VY%l;#OkD3(q^!c~HM*;_vh=_m*bhK03y_Tf97KJ0gffNuV?8 z3?meFk&JW`C;;fQHek1V-|D>yLA>ih*cnMlH7vYZ0EbkUQ9tAgycg=CLLA2S;qM4O zerQsn3a51?t;l{=MMYF@Zc>Kn(`_Q!z;2w7+RDm_uV2k7tvRZRjSokV`y=b$^1As6 zZdh14c>f(t8GfUA^+|)dgXRPz5%3ksVU5k^AbH?f>vMFvz^o-oXU*0E>NjS9{yZzI zdM3l}9-x?n9IlhpwJs~bE}VA95%svz>LWyyZWt7cDg6u}Myji;)w;|@v{2kgxaMN+ zPiLH&79LgDh)=_z5GCxqpB%$yUViXp9h59O8>j!Q?y>xi%P*dJ{d6i&BskH+ZR)ds zTxuo0TKC@GUIhnk1&{L;C^J5qzqbm%+o?f`NL0QB1=4)Rv4kO%L%J zlAUrZ@YY}DW$uKhBB=9UNuO!{2Exz|Dg}uy>tGE^6ite{nzNwO#F&c!a&uY-~527DAUNOe4 z)1%Az6yhT0o?BpDKursDEk;q>%0xUV12X6YhcLyMZB4?8=Rx;mG#a=ypv@HscE8V=45 zGQ0mQC7;qs$=uYX+4HLh8N-?41BD$NSr}`RWoB_P zuewERe-C!GkxG2*aQ=ydAQ~}&LZV2HCzcMiPgl(uCK~)PF+`60$k!;>=t&GGK@VW5 zckQvU3Bq0s?e!BTkAmk>G_=Nu#)bwUP0}xRzq5Qz3f}ApGSW|1p6FSfO_g0m(5e-F z_D;a+*o0&}TH06bIaMiEZVnu#7|8FGt!QgfLxrD1;@pYug+OXS*5tw~j)HMcETZ(^FR ztr-5hO_S!8R*!?y)J!tI>^B%2227(4w#E|7f6|brKuAD9PDa+Q@ZJ2LwBJ4?FV}l- z(q_$KkzGyem$;elKIG)!sHx&bzubox3-S9VE+8#Aj~_$EHmc_bJ3byCy^P^ z1t|p_zX`RhLOK%I7l>%X{)=l?Qo2=;nVQ;ITN|6rOm_0UdjgihE(+zWCkWaWcQ-c= zXkIL@P8Iif@QC&so!`Y|2M)=VHE=fau2;EftGDSBc4uQeAT6liYM$JzJ@wgABBm=< zyWh?9k~RPh|J7%B;1{Q7KM+Dg zA*TnTaJg*-rz$TL5sd=3YEU;3cnAjR7Ulzt+`K#iuFpHmc7DZY9@;InG%~@dzY1w4 z(pv`|-*^xq)72?XuGjI5 z=oTELY;z;m+^OJ&kj$1Ka{&Q)=0&p~4IZBt=-?#%53Ls!C1E|frve+e<)Z#SJKDfl zMge?oPCsinR`85jE_Q?}MxNkc9uw}s4hK;plPHwF4z=UflqMl<_K4e#PC@z00)7hF zZ%$GwM9rhLCq~2$;f_)Td8N6zP8Rmow$7GTR==T%absgRMg|@>fW=U}qCRoo@-G+n z4G#ly;Ww|Oe0K?t)}eT58A7Wgq$8ZJ;MKpdwNv1mH@UV=&P0Cqj<w0-fU4t>h-5Jk8UyV-Gi7H!36Bdn-`)Vqu{u8-|I^#xCT=qS!@(piT5V zy%VFFT%?o2R6lK@|L8vYx3Igp#{{b*Jwci;oxpTtOiTBh%*Fa3p#y`BHIf)jIQ(m3 zN=Dg}SFC^e%>6gnWaO7WkI`ch1+4Tkr=#0j78W%}CNm4^gQq83Ubg6v{v-d473^NZ zwpx@PxJOUX*A%j@MM!7mJD}d2#k5N;(SV6nymcWhSd6f$gcxmqzSSeivl0sjqJ|VK z$?(GN>lq9B+8!d50ZdpL?>GIrrzrk*6-h$=vV^$jqkfn+GXlF480m=CZ;rHz4cCS4E%=^AS?aK@7 zJMm~`B3%^Q+d&bxcukv+338%@JP0a_HY}8sPD}rV4i65-L`NIyDFCqIVeeq;Y-0nc*UzO5;?0BY?HB$2H8`m42JlJ? zji1wyjbun}fz)2=EM7Cn?4tSezpK>NMfhvB8-~xv#!uiTz{zup%Ki`;_c1q5M8JP) z`2ombs41k;wNX0J<`p^R<=6ZPXILP}#smf#Rn@+>bDZ=)e^M5;$h`Dgz}^|WF>C)* z6hQ=}Z?d$RaUKRowfCtC|3)kf} zBB$1o@WHFn?IePqgH|?(H+ zs$<;jZez!fj|L7)!Vu@|mXxV#%h2#bnhV!dqvT{r94-~|kqNkRdi>U=mvy$Yd0j^0 z@Mwd!0$eyBKf22MH;@m$oD1Hpyi@nZ5c?`(*hc-GtEk%%rQB5~?QDypv1w?@U0Moh z7Lg>pu7wN>RzCB_M2-6oZgFT+OY(9htj`OdRBdx^HNBo&Nc5kAkGO z8b|TA*sq=6IzNn?Dlm|AvUydwSwx59y|oR=6+RA;4jSjlACOTad;Tl$N(WKQnq8yI zMu@xcC)Pi3>F_sf-IrQRsV)AsxuB7c((&7*bhNb+XiXn)ZZ43 zm{XtGej@#Ir?8Ep)%sw#_!9mRi~S*w>$CId$tjG&oUw@{t*nv9 zXBKF}vur_JyvB|AD;zvTXvoh-m?5?h!d(QBKu|Sm8TF^^BcO}mc(Sc*GPruOnH7-j z?d{;NQd1wm1rhi@=|?nsOI=-GUo4OTgYOH@AIWvOQ^0v4r;FGr8tt{1d}y{7YU~>Q zXa@_+qOnRSWUuovfl$~VM;TO-;5&wt`RDiVPt?{1U-bE7Yi^y+s%4E%(fwGjx?ZhM z$;r%K2K^ont|E!PeiP!qBT!RbUR_h=qp&SRc0!XoLjBK<^#WVuq$;C1v?}^{7W^z8 zQl9J_eIq?Man&kgFYA6!mTYRs5k@q+gBPxtu0@P&yxH?x#OqwKEZRdQ06Zk{kn)g8 z@JP%>rV80nAU-v{Ia5^^HiP3XgNfqNXFp%BsIb081`&QKw6tA3=J6?`3_gMUVZ&L{ zI9%vLE}loTlRc~OkTYcJL;&t|emF!&jrdr*#?qA0KxklY9=v$w%jMxgM!^LW!}Bgl zY#B7vNTv+K(l4U%m-KfC11T`QBAy)FIv;>S1?1b=*RQnj@ipuHk`Sm2s(*+^EE-gj zKk*77tEE7~myiI(w3V|L-c2Oknwm~}54?Z9%Mc%P#evPDi`0sPC`MXd5bDo*~Pb2 z7NNQxBvNg=#xo-jlIPsqo7%X-`TLGMn@Oj!i*u>}K5PDvNOHEk9gB$d9lJ$(D5^&C zSbexU&uTUX;};$KYu5yr3(+P2MNGQPWZY=lpi|2qvLZcViN;1|%l@WI9q6a~Ci9Nx zVFYkMzDN+NqTXy+<+~dYjauXWn+-%Z6eJtX(CN)nNgN|BAM$qtxGYmr9IE?1-#KjL zl`WR@#X($_?gDoeHIK0F?I9&4SF`Q1hxYHjnP;DDEyvunf=uOKKi_F(B;72;mRR4Ra4y1OzOky0AD=A=Q!e9Rq4?L4n~AT~sKMp7 zEZ}?!@SiOyf|e_}%ec-Nq#$3&zMHE;B5tOD6+HfwwLVuo*~@D;rTxyPI=)qz2fKk} zb3PCgP-NWbSN*;H(62}f9RE#$c+*+wCSb280~Xio&PY z;&1+Fvi+mlmI1eYqSVX^v5I9#nGASJHU^HQVk#)R^j6V?S)c7Jb#?A6rzi#8&|S~3 zi(G9GvTKM|AI?Ht#V!r52gB^KV~|rbJ@M6Hulx1)@873FeL2^%I>W-Y-{?>Z&&nzL z*qBK9{2k#r!@qhVvLC$D$2kjOZt+R@YoX`)dcHMS)@}CL8a_M=fC3uu6UxX@z>M*e zU3-BAZTw~IOYYmZ?X$IJ4EfBp;-bQ}CrTc5DLvdCF0QE9X+Pe(9l{8>(AC0JtcRcl z*B@i+9bg&AlUR)JHhQkS$Sa`_dkJSG*G6-lX1qt6O5)V^LyR^=PtOyOSqODPB%S}Z zn0XI9`A=p;-Om@t#S>1y0NSncATSMBC%48U?2Xs`G}~Aeu3CVm593Q?XPR#++y22w z5{rT3MHR;-S9ss85{Dl@3UEZbA3|cJrjug->038Fnn?U?Bc{Hug;l@*&bpjD=qy2h zCcpBUqwvSyY=Rfv~Jak_l@kPfK4w?+*%cA2Tn z&C!0b$&T3xK7qTfxQ}eus7bCnq&ZU8rujp4#%jP&u%cXvxXd{T&h%gYE z3@(t&6(myol)+IyR6wudiL##Uo#i$1p&<&oS^+|fcC-*@$1rA7fAI>`P!?7g)&MD! z39V$^-}#Oen5qSO3SVS~Npbo7k-x{mo3%AzoO9dp?%WWoy`;2JUZSBK2Rv>bmTzls z4eNB*Qb0J^2z@9@N{p$_zMo1hb*UDg}_R4Jt&deI{x1clZ0?dv7jYr zWJ$3%w-!}^7pG7@RE5$ zpWVH_aY0Xg9X$%LPHCwodWH-hZf=NjuCDgd*Y5@C{?ZcJdDpEHC#QdZXAB=c>}@Z5 zt;rlOx-_jkZ?F9y4l%%TWE2E`79eEbJXBUDak@d~+wpyz;q~d@8<`I+Bc*?}hD&2( zwe<44S=nVj^=xX)`O3R=YGk$(x)>#=GD`YY7rok9;ZHAWXgD72h_q~D9(?6dU8P>{ zSEI<^uK=rh;T`}J5(Y4aW0)2JEU_9&8z3Ugx_|h|*d#{#0^XuAC#Cn?k4iq3J};wi z0Iqw}$x+m4HaR7J^!OkwyMJJy%&bK)z`h=EPjj>}3r_hLFs-|p;=qrE%Za)~N$`jq z0I%f!dQ#i~k`^ukul$op(e@WFE&~q<({eNe(2-kn4Nsk%(P|ik+*un%*i}mV>Q$TRGK4%1>=MQkyWqYW;5ECoV=2JU8*J0xlhdY2ez!(znDH#$(jbRO9EM zf`e@er^evAI5ApT%vfTEu^8+15wq_&DE^PFM0i%OGh2;a@TmQNsT)=hzxsN_OCiqp z=jV507CWSS*Sn0NTb~51Zxazwq+JdgjM`06{D=q}lRbYQR6jAV!5LE6x%)}(_o4y@ z!5^s&XSUJ2hw#P#jQ3sR4%>Oq4Sb~ffLh*gdnB>)>;K^^b>y>VG4$g1Ra9^ZY=UVx zI5{D2x$aa{aaELA7O&_N&+XesyT8;Sh$WJQX`$~^b#>yw1311+$zk?MDX4M5->kQwoo7LZq$F)d*-P}9@W zlTp%AYJpf;oSWs9LwrYsas&}WM7=ByX13(DcDFlsYP-AHrX?hb95x_A0~B+hp`)TC z9Uq%8YqeA~Gh>`A$EJwc_~qtiEh2&@-zM`yfh8x0a2=ZFV$#!_UbI`ht*pEP2xFb; z0`pE}>PGFfIRo?1)*hV9*8W2#R1N&>)3dPfShbwaw!kv3auBKFC*XXg1_V45H?H*g zf$O`)VqVJD=K1TGSANI4w+{LLW5k|a35H^h;K5OBN{$G*p?m5*yCwh=+*9YrT-vFy z%5;RZ27`5D;aW2K(W~r;6Zx`NaghYqT=+!G;~KzEOX%@5coqn&16c~ zbKskVykzCfU08+Re8@QOTV|i4FV3L4zJE0OLC##3Z1-fy5kz&#Jb`BjB&d9$ggF00 zcA@-(3=u1AC;yP2?0v3y&l|ChPMV)xG@Z-OG1vD(vqR!tuaP)&(#Wf&XhZ717`)4E z)%eSDdZU^@ErQkC*SCGfNA-AWyt=Y=d)b?BRfgmI>Mf#Fj&&Mb`O~blSC<34#O~PZ z$}Wm)auderC1MWrB{t*Ak3wCn|HX&(ul5&H5i#jqp77iuNQXm>r6r@&=Scdb=~sVI zWlkSe*-w=Of&a(eSGZNZZry?bDhkpmNC-*^(hVXYoze}`A>AbcA}xX-4bm;$A|cX9 zcSv`4+_~KQeD{9$FSz%4zWto%ob7RmYyH;y&KPsdF#_h_(>;6>`HTFuq{_lws#!FB z3y;@#v^<_zP?jUc9vifJ~EO@L&<(F?hz=Zmli)iRqEOx>{Tk-0b zBPL_{H(9-gFWnjXEh2=Do1J|v=$tmJO^HPg!XnY?-g5KiGu@aVXG;d-FnJGS66UO4wN&2 zSO}hFpjXLlJKqNP*;#uK)*4_j8g#>82N$@ZU|%OICx=*@06G^I9v)}~CPOkZF|kP<_~qyJtWaxXh8YZ-RbIXS_U#l7VNF3e2)jB{itXk3L_5Nh%6C$_S( zvd?1ap-ZNs8VI627JmNxT84jN;T9M-!pc$mW#lZx8yNQ%hs-9xPR8cM$tBMa#D=)o zSUv2BmvSyyF4u!VR{)>&aJ0mDZ7AQj81+O@<-4x8m5&dy*GqA6P<9P@z;n3Vmlhiv ztC@u9{Nh~`1_>vV{jwY^bQ8FRdxfz#?&I9QAO7jnKz~0-TZ7^yDBIt0>LMRVZ|~_z z;xzqO8f*bO4?xq!!9mH*$^y+b5L3`lN3d$Es;k_bT@?vg=TJkQnGE2%E*HH|0kH#O zZhBUF5(lrtUDCPL#o3t|Lp{ChLhIy=jDoDMR1B2Y|Hw(@k*U41T+ADNs$Dx{#gz(6 z?dC3;@gKgZ7xH^tE>CJChb6x^(Lvddh56M3k0EAO)@PZm9!6kJikMrm9{Yn_u{Kf;xVxui zYutpkUyyoPckT#Q$UQv*%CD0l>BnnQ;9>?=OW9kBTj zdi11;;5fWBk%l^KpG|x_#mb~-c69S%HKLh7Hn#ssCOWS{Uv?f%GY~FW5F`g z(hoR{z|*eJ%5Amzc4!8b>^_JN^(H^WI=K1k7xsH%dh*R?Q0&1?ug+lHF|ye|cK7I* zX#B#Z&|Ys=1ZgS)bgn_PW)ySdbi4c zj`g@`aX{^BUgMV&QT9jJnS%n(&D`rKb*omBKGhy*B%#Hr;aMM~dDLU3JLHLo+85~= z?}S=7DVf;YU8)!%RfC}DM{$<=n<6toO&UAzZ-W)nH?rH+JAambk5t8+GL@z#zyBse z_hq-};2qwpjWHc8W<0N3b-1WvLjvy4vkMFPZNG84KHtguwP0hOLX5fJ`E^!R{_!A9 z4^CLT>VUk;Kw`s$rrIl~HLIPG>|<+ry11HB=9&wvu?bDL1@)bGE5}oh?-2XEczfBp zf5g}Y%14qa9_C)UzbXmX>kgg_4!%0O^tW0mGJW~_ErkTNOkvH{`_?yvIaQ^lwr2&Y zdTBdcOl@y8r7r3MrkB9#=4hVqu98&HeP2vKG!+odWU%w?OcH2dIS!^|(5|iY0vBtEA{wGFY#bb*#emtnfv70TSx@}k!I6<;*yaEb z=<$;$eOYAXGwgj573JkqW#;&qp#kaX>98EpV$@!4@?tkk;4=Kkr^^ErKEFFJ!$3xVs$yW4Vh1nTq> zLW92C*8qmnJ%@$cO9)K~Bv(d}$ zUaV#2WU!mFTQBc@sN}>2(*=It1DyNIlvFoFblsCQR#?OdKlry3z?3^qqnF4pv*(t# zRa%cOw9$+OR*0U#f!PD4aXx`n?;4tzBwW`g*ff*%4nZ|K=L16nywWn-wQnM#xD-#N zQf=d*zm{?Rhy`;f(rYGi+E!Ek4r>fxrd`#C;7A{|N0C?tHuk`bC?N{BLEn#@?F%wm z|4{F(d+7ds2bULT-L>lhjY)BFZeZX0B$7Ji1%VD;7i(KiTB;Zo4UL^W=?+k@?$LO_ zY)P?>!P5Zv@` z5fhOY|A&sK<#Ds|*3by-u@EjV{~)}JeoM;Z3ami^ttF3&i=9A1@K}SRSzyCwDi9kwQ6*TZD$wb_Py;~*kk=O{-0>5+|eEhBZ=X&p=VV+p#u%>({Y~+L`XmG>fJ{}(W?b|N9^S3jD zuUm~MC@LCv#*o8ymex4zVt=Mr`xw!;`I;Imy^`GyR$DN=VUn!|f;)E^9N#n5JuD6n z4-X3JNSFLP@&vtgBj;5vNMUi&-o$mqgR-3l%oevAu57uHZaMhPWM58QoXs}*-}@Mi ziHEr{t)r=~?*BqzXkg$O6YDdu{VXoF1KoCqJ(C09!-FGWGVWpQW8As}+_O9AeqM;t zSHm_JmU~mnS|kKC-`TWZYMw|!>{w$Gg6nz!`svC&B}gEtM(;UHj#5@d6qJJYuc`Q zPXpu>?p^|*%Ai)+Hre9WGX8L&jY=UoNkFrgrz)dto6K9ZNkTPFBRmU}YIC|KOuFyj z^mtZy{Zn=?xvTnE$7lN2oIzfkfR$L(M#sEI@g~J%$H}1Cpd*lBik7+;GDElAl=!J+=tc?4{10}0EwY#vQrHvw)S<{W~7lMWP%+?mZc16lG7O8ZOT z9WXjJmdI(7`t|F(E>vsnKoz7f8GyD2`1{v8?_ATslElTwFVv_AeL-c!iD*1 zb9Of3)2C+oJkmO6v}Adq3t01H?0z)KgR~0|JCDr%Pcko6b#;(Y_a{U? zSwUjuofq^!JUrxcJ8WM3Q4Wfu=yv!Mo<>@LoAnLbZ27a30&+v$MiswdvsCo|IHqUcRL|VQB!UmWEY?ZQZB!7ZqnE zIG<%{?&)xQ9~T!;R|kb%RcI*Vx1Ho*sjJI53Xij#teo!oh%FW@m+N9l@~~Hh_c=WL zFMPk`}^GS>Mx5Fug?1iWJgoJa7?91QB=A?sT6}3;&3)5N=(IRAOp+MyQw; ztD?NpGY)n_KHGV~N31m)K&?<)O)b#tbN|GIrlJ;%(_OAM>-cIMr4yO@hJJsL(yv{6 zhyYD4YCWtbD}P;n>y&~;6(E_Nul)IW&L}5tV-w)f0<_WRd?im41(G>d2gRKS|f3Da5$a( zPS2rw-R$Nx#ap@7nz8KKSj204T*N;Zz(LPB|0s=kkqkWD)vdb`bAyAU$Ufl4e2DCZ}Xg-MaUvYD3Q{>0QM1H*=H%MF0BR+mqFEhh!`gY5qgjR#lKBhD{ z&qRkA&~H7tCv>?cCT1&Z#4!qlrN9~r3L<`P%~w}f8{m9)oa&!yeB9eL*$-@>_IA3k!w%YC*Jb4x_2Qp?soq&f!*>Hv55m9VcQ*NRdQg(la|`_z6Eo8~ld!_L zk1ZeGCfR_Or9vL*FrRuQBp;7@D_xSi6oauVd|y|$`12B(F%axydo zu&-C=;9yHlz81_Q#UXXA(mkHnlf>bzqn#ta*QRDbVIKF>$8#`NEaDH(5}NfIIP<;w z2r(Weiv0NhWL>L4kSc}_wfM!702QmUub*E_dla3bg2D{>YCy8Fk`{8Z5JWe(7cZLK z*q@iwIq&=!9{wD#zU4JQ8vW@LsMR@`KFM9k4nmws+*`M9IRkuC-gXE=eSfm#)11-$ z&JH(t3|fP0YaPkYH9TpN?uFIE77;!+_Mh7xq6g#F4KS_&S!E1t;gZzUYp$YT8Q~^K zV`yMtceJT>s&MN<@YO378Y*g(fOqZfzYg#}6R^H?1)@LB%x{9yx;o#;NVE?rlYF&f z=YDmd`!!>SDrO8D!v+S54i5L!)VN?16zJQuhJ%DG#yju$f42NAN$$xy*z8Abj{*rX z*fV|=F^SXAZ!Cr&J~}zcVyMsGv(_Q<&ge5{1sNrlt%<7sgnhzSYHFiXlQjVSs@`Z% z=D-zsbnjbEalKDT-F4R6W7n^K#KKt{P0mi1BV%zQwuH$rDmDxo+Rv=zWne@8DSjZDhs>JY!3{R1V1ZU#rpIXF z@OxuCO)Pks2@kcdk;JFKn{ak@eqErMgNliX^)V;wQ^9*n^aD0P1cF2D56!r2KcOq9wtDap*i5l)4<5YuxS4(eoS0Wb}PBh znUh67GeVr|fpk|Dn991l9X;X$E9qD!Bi$7LS39%5s1Le-c4xr|2$WU&4Kjtogi@Hs z$OK&9Nxyh64D>7*RJ$|&Lwyaol{gP{#cG_$i~ArpAqiHlZEYryh2c2(@<=)}zI9_u zbLP1Kt9N&?_YYF;a#OJ?$!KfCmVl@zUSp*pNG~7+wXpvRYH0v#-whR2jOvqLP*5Bk zuk+hR1_s7@dZs3Zc^Mgubabecig`K$z*mye?p^Bal){VPSkC|KjWpm6=Q682I+|S~ zDc{lp^T<-gsY3Zw>R!!kh=W(g3BPO-^WNs9Ti)k|0!0uThS7Cja;o$3AZ|u(HX8%= zAqdY%cYf9nZMH_L4zhS*I1q-|36T6=U+0EUQ03~Eg7Ut#)D#kdfd}>=x%Vc89c;9D z9g>u@)8C|Ag82sUKO8G*^zaGq&V+ye-k&M6GnD%YcBO!I2`fxUY(A$z_fL<^buzv& zX?rooM#JN_>>{gD@h(?|neqhn+A@fJY-m&vt`TQFraf_Wo33_=oTbk#EcAMF<@r+X z{(fet|Kf;z8X3QHuZU-FILH%N!(#kBN$!C=i^Dr~H1P+c7# z#w6*LSh@`CCWE64i*a{wSeVYFmU@NNWatAP_>be>Vi%~QSElD?XPa7B%%g5fVY47z zK#=$ar^noUjR}zEZ&_I;0XttFJb17*ZCu?bDI?SUSmxZz4b(&`taToL?t1_I%@maT z7^J*y!PiQVdf^j^ii*|;-?;v(u~A4wB)#cN7zr0vD@(aj&J40Oi0Jl`tTRj9koNZ% zc0JU)0M98{Dwhlss<0TPfIhcXQ|^ZSGj__H8BJj~a4q{B8QJepJ#A`>nmoG)R{;t% zeGCJt{hv3OShm*?aVt)9^j6$+62?%F@Wu%~;@%7S0zK(rlK8;5Us7L&lb>!pqtEC@sWwM33hj&N)PB4gGT zEu>g;Bg0@=FKj(UvFV65W~ypSJq-jJX4JPj&jYiTZwr$wLd#upvR-qQJ0S07z*5P6 zHb;hZZ_;h&$4w1Mx-9WGf`9Gao>Z&AB5)#?C3n0DG}@I5bUC?Wb)}c&VC!b$tTftS zofPCtiDPM{Uv1fehbMe(GZ|Rne-<`w_Dxb-oC=JP53<}u_C>6QzPb4FS z4V-GP99n~8C#9e*xsUVij_@P-)AjlJxev$ul*qzk{YzUmN%BX=$4+CVWbm5C^^WfD zsf%I8tXP<}V*GH{hdI(AJNr(@=LSh}y~ArCc_0sEFaoJWhW~4NR~(BNERH4#biqET zqs6%YNj^qhju$QT8ut|LI&q#IZKB=2GxwMTbr`ayqs<#lLVOJ4S~^XMxMLK-rKN?R zh3|#S$p(5=S-pgw$Udm6x_`GDsCtctJ_lyNj~*`6?)MtMfCJqARHuG@q*@2)q6{46HXg#F~i&$+&Ex5_1!JkbI3&EY$El&z0E0J3g;V-ov8k zXqbjnS66?_VsTGTNht=kK+yX)vJA-NjG;#y(LJ!Gb%FZrY&Z4}wWNgwJv)9*vhd;h zs4t&)I_&aqZ?jlUa2j2l2;@-0Jz)>-1Z^5U*9<>b(U~TQ*D(DAI>?XvsRGJCavjK4 zB%uBn6LagyN@U*+McvhbY-l8o2e0${1mwc*%BN4C;2kuoB}aj+@SP5HYCiHyc?aox zL>THa+MeH$#7zka2Qbt{{PHpiK$Q@*Jk#J*W?I88L}7Y*enCn~ULFM{C6A!3uGj6` z-*Siw-ILU|xZ8Sf+_;Gw^%80|5S5aVVYHmh=}YdJ?z5{2L7n8|3)ezDdCT9gq@d}( zGS>nW+&BV)U0#xaf(&J6lM#SnzLk9AZW1Jf6(sZr08cAi)+HH*-ym2+V!6S2XRb3sXshuC35RRvFKil+6WUSXPJAzm#CkU~ z0E6_#>4|WY7A(yq=$XeY7vO11Q2Ypp$SPET`R2G3s$~tfps)%KKJIj_josx$mGZ!S zaR62nsij?kC(o0XTKaV!NJP(l-wwWNgZr~codMA_@nk7!f8V}f`l`{V$XloSjoxP7 zsH5_b+@m%UM$@yS4Z33?;bnm**8~yU<2^7#W5flK)-FJ=_lkAg_Lpaum!07(7Hlsb zhXumwA2zQR1SthsS&X=wuh`kyouAHtvJt4^GY1`R3z*#q21B~cuV0G}Q@nW$I9nj) z!Y0Z0UQyQ+Rzff0NpZNO`QITbA*NkF72%lKtPVWsB!|ua-X;3-+BGPa04+p_sPFAK zWZbTQOKkrVB~a}JL?GSoAZv`<0 zOz}iRRlUD&X{fTNw1{}W2Av{>i?Ag z(K$Usg zMzeCbvBJ!}*h(#D%=Q{rjK{5QetX(WK-yCs$a_Z0OzSDKx|UoZ3RcTyH=(6TPM7sD zC6Rx>6;=>q>dYvb6aKHf&FZ+W3Q36m&YT<^8(0N-EuDQU1t#iNDgHijOnTRZlcgvk z5LWe4r;3>}#kWc+9zW(ZAIbwiU%cZe?e_*9O#xO`R-FX)kmGbUXOlR%F_ML#9(?2I z*h(rJ6%o+~7%$Tb#2iaw;{fTg41vpIgOMUVgh19_--C~hiW1O)WGaDC{*rR z4)og~J#`;)UuOz&=$+fQ^%{&H56Jn0Tq+hUZTp!`jy62IR z*|Vv18vn^YU>YGh&-pq%{b*|n_OB+lQ)an0`mSt+-Vaf4ZIYmX~z|i;Z$1_5do1qI!H+!>m%fuz6`Oc4~0d$#&*DB?C z@JTr8tA(W%Gp%bgC}<_}x?JP1wwN-pVIcDM|E*Vc3LXw7GAiR`%`YmElM6**RxuzR zsfQCw9JfAE?AukbtHrP!|8=|q)Vfpbbi;LIYejh;up(K)exGyW50pwwr5+eKkJJ~wG{~dUJrTKPEG_z8;6XbOk zJ=AP)ytgR=s)T)Qrc4~>Ik|nurYlMPfJwQ4h3QMY1`70yk}|)WvF|j|VKS^|IbEvP z1ibe5Ke-bDszboXcK=WXOkSjiVP1Q z0YW7odRAUuJhuaF$%At!;Eo_?d!FtGrUg(`%FDcbSyWouY&1*_(To>$>9)7${&GZ6 z5Za-04rp|cmQOFBoa%uhBSgrNuSy5YeN8<*AgyamGI=B{1`_LTZf-2X6#yB72bFH7 zfET?tzs=l@Ch~m&vmQa?AN|iV`4p9uO2MlZB4%cKI`g^@2!F7k=`%ilOn{Bu#9t-M zR~U%;HA#fc@~*nJ-ZpG7$54Z866_SifCUE^cNPN};V_nKX?fIqdY~xe_#pll_Gzc- zgmLigzz?gdD_{xO-*5bsdf#P?)pOsY^;S!-Y!cPhc!@DI4xmkimtnL@Yl!z9lMSK( z3#EaM^;XOiMOa|*JnBw(UVgD&Ib;U_=!y$iG~=x%)-kbz0PoJtJ}F5pN~7IR93Xi& zbdvNyFb&wzS067I=KRpil?#a2^uLz)rUAfzR)h0H>G#82^*D{Y_iiIJXd;>dps~8w zajnUmqDKTHS+ARDd6{T+#i1;Olz9#$K1#X2JDjwqcQCz6rh{}BnZ%otJAcf)aRXp?Y%vN#h>k= zaQn+_7bN@(Lf;X5P(eCf9n1wdbVT#=NB~e_DSq%N)?0jb_gh(6ox>U)K1P9DGgved zVD((dqoS1Jl(3P=A|dQ(V*JQrSxvEwGCzUlUr`a0(N|RsjWejLG?8#PZRG2;k-sqhBouyIzNo_m>U<2;?6$|-4?#99tfbC2Eg_-A zmy#8OKahmoeQY2m%4P?&)EaCqT49OP%-p;tAj9K)8@c@f%!;fjDbflwt2;G%+6zxk z;QLbhW+KlqKHG3*RB3aq?qMUG8x?vj|B8ESLwwSn?M*BnBO~qB+vMGa_RB3hqCY>? z){S;0Pyz&hmUv_k{;wgJp!E%Z-FutS$k!P+Y~7u}Rr9)kag@ba-~MbtONZO>=oPg4#Y%Qr(TDf*XuLauKqY*+z#9@9tblr-B8xZe>JG{ znMkWa;Z^L5xAz0(+zKD280Gz@l?aRx$|4dnh#q@Rd?R8kRXlt??&{Z(#*tI}RvX73 z_ObfJu(M^#&5L+DO=)LyT#x!?5{J{M#dgz&i(OjpscMJZ7UZ_j&`=hm&hWEhcIDIC2I?0r_&W2P%-~fj z1vp|ppFoo5=}H7;*ts9m5%NAviBI=e3~X2{N=o+N@J2PFKemhJzsuh^BJmF~5Eyo%W^(113|fZl+YED+H#jfKhiGxV(nHW=>dm zazXg!9h}E11@X|;nMzodF<+j-nW5yc?Cwm?{qk(Ny}SEW^X`_H3av_k`>5@L*J#aUm0c+8!e6yUH z-XqxHHB$an2@GaG{u8l<<(@Ela(G7)R5Yg}p zl2$ZOp6_-%or?}^ierD>-bX1WO;@N{wY!npvX~i7(=p?7T@0O!^qJ?6_}CI0Nwop{RGh7|h7m zC|%z+b(?0?5<@3uB%SPvV-Ga{SkchwD{`Y3iPY{!Z#9K?wtYv4Ej^%}=$xvQt)Sz1l77g+yrU8UwqL4J7Jg1sF2>NBggM6Fo6B4R`i<&cK7?_K z81E^?=ZPD=bH;1%tb*){IemPc{p)ILK|_R&19OQHz0mz+*Q_jpvzE<#X#RLhBy|=% z+RX=Z(htp*^3}_3qTyv(cFr3h)j`sRIG>#^+QYloS?jX5%RbcizG43O_&BTP(|cK5 zRMdbwxU{uLQ*Lui&CR6OIUHmM|yI{aR`H=vT})wfS5SpNR% zEQrjrSyt_c3`?LYkd5oohN361r-yjhTJsD$FllpM2kAYU?b*;q=K+T`pMP;ir5dPZ zVg}F@d}O~FP_BTXF9vYaT>V%6mxE_q%zR1EJ%w^TC~@jZYzra&>~%Ex<0e{tqQF(I z%U;8YYon87w3TWD|LIRM*#z$N3C`s%eIyU_w#UK>&j7;c_)|nxs2bV3Eq%=-0Yt6X zy%|*sCg8yzT&k^FQx{%aek2(^kguV#u@Fd;=)U_)BnZzmRtwBx0s;bBxE?pR`&>uW z?wH)dfRZY&Zm#gT-yOu4%iG)}io{S57!)M={!wFNBQQAD;O@hWA_(V+z9*x8mxguS zYyOD$$2JYn4uET+@~Skq!zvCjG2O87F!ZbfC-c~4I%%mQL7}94+#NNsI>@D!Pv-PW z4xfEj&R2)&pWrIz)4+xxI0-dUvCmeu$sTQ*rabpUNY3v~fV>}^BET1^tZCdt_}e>^ zIV@La=F}HfGCR(5*BgQ*yVPS1lJ=-PNO`O@H~Wo?acpQ$?FY>Yua>%wrmnVqG5DX| z><#()fMCC>^A(hM`z<(Fv2S!>IaHeT;gy3_eVBL*uE{ur2*FW9ISR5;YF zYX%{y)7fAHoU+SVvsfrD58z1WLYAj zb4)NH{M_Mnoq&vNy4XN0C@kF9x9Qz&?0NcMqDYoRN2?|x5fmcL%6rRdkCn3I0Kx~{ zT%Z3vB_ksoL>aT|e;*Yg3-xCo@M5R{)2^rJQX7+;K_am2`YnE%vfmqL7Nwve;>TqG<<~X%Y4bX&_=jZna6lEO%>Y$}< z@3zL{Etev+M^zaXmHx87B`{+nnS^$+gYx4vU%f69j(~c@(`avR?eU68{_9BId6SWV3__XU?BtWKR!r&}=$D#$CR_ z(<7+iMX@w1rZLrKeNxgi%&1R(e)TdH7Z(SIU7c$C?8+#YkNiuQ`M$dpsJ#-W6S*o6 z;g};8si==#Tn}K3&N58g1w+yz-cZRt8tX8UB+s+ZQ&WEuazu!Sq0AN-l;OxZ}Xk8EDI#Ye_BIAh}aa?f~Q=T9=PP$hf2qD_KgKy0<(G_ zExE-`SH+8> zoJdaa?^GFIUc@oLoIyxP$R&uzZtbd3&$T9 zyyRkA0OLD7m-Fb3Zimfo4U^uzY0;~+zY!C5`fNh z+}woOD83~cF6~!#&*l7zsj)Ki&$TseZ7n7O! z&Lh;)U-C4o6kJA68+}oyy{vq9aKP@AAhM+MaSb+x&&^SQ(D9FEfX6$-?wkS zYCC+2j;0PZoc-Zf*l_6zp<+O|*!Ni5WTYsi@*vpvlBPo{Y0q`tF6FAuXFFtQtFAD; zCqIP6+G4ysWS&nwHglHqtaoUYR4KQnHwf`m0V zHn~2K-4FR;fIY0e5NWKHq<2nyZMwmWkw&;EsZkff7ETX+@7nw9sxRGAsBJ)f{{k{ zpeVo!XP{+gck*-^^o79Rik=^mAubVzRXoY-v0KRr@$nFXu6!QlYgBmt3E=>ET`@z7 zkQlReWB|ee)Y#(Vlkh>g9oMsl2*Hj@imRNY66UQil3?)Pn$rdQVV8|Dwz96YT@cru zJ_aUone+Ud@`oTtm`7wAOa=xV!f}~%P2_Q6q^Ixv`7;x09;oeC8`b1fU);7K#Ky)R z^xSbWmPj%mEZ9)Wi%C=L`w<$-jv^E!?I#_??8@$8N$Jaevv;f9WU%_+4A=m8K>~=& z0RMaoMEZinO}3092wzZcL`1}*;!7}Tg$S=s@``p7ls$y4Cn{4|e?>1ZFGDiR)gp#3 z#tI7U-Sb-ZYfyB1y223HDX-Gw>=3chk;n@jMkgF~PEO9OjS!gW!E^f zUxOYGAB@45An1)1FMQ8d%!H^nP!RF~kAd54;PGpT`*slRGcz&}6@bO)H?!=%IO}>s zT--+v=S$4@-oyNF6T}^1>`j~4BN{@a7Pq5GxoM=`MdRCWpTUDsx%Qpu6ZO1G#Xfxz zK(3!fzW(EEY(QlhoR~n8@PQkTf%Xo`mFkBN?E!?PM8a{@-k#Z)9q?pEQ(v3!=m-dS z{WeBC7ePl%L2SV#UtWx4G@QrVW9Ee-FJ3^V&EjE^!6vtgEW7Op8`gDBTiRtIlfMDm z=}ENQU~q?Pp;2NKqG?U(U7%jJ3budq`gZzGb0ipK{Ofab`ZoIol{gz?WjwC?S=rk0 zXJ5r5^IG!m5wlMe={2RfwIyzEzp)85YY>&?I~ag+b^yf)aV+36X;oNhl?}YKe+CTQ zbB_|ZBrk!UAtCW0EG#TIcz=7l=t4`e%J#KQXXlNJ9!z|Ej-lhB#?+P%pC8MAvyXlD z`tqRg*%d9_r3R?n73Zi1Vu?VMSFUvq{FpBB3CmmpkHdc;RWtn5)VIOheJkpNd$(j{=x?{!|mCml_HvICau-?5}E=TsNzhK!?0+?p3aG z-TrS$vxtbav_EhJw|Kc!V-3+qoGgaK7z3)+)u1(hKr%$Jj*1PK&9NY$I)cR(*_G0`YY^KJ0QMfo-`nRpZ@s7!!tuPghECJ z@rX3aHPgZVP01z4Y|v~l(M8mqJUTP6V0B${VLFO7@M#O#i^fP z#AcTD^E4b-|BOqKjXH0Sle$Nela0-X!0MZRZeA(J-qd)Bwha1ol`TINZPVT?`9CQ4 zh}o>pSIxV$t;hDuQ0@`c{Z39%)wxxR?AT@Glsu8e%VQsO-50Sr+kir#j^xo(_d5Cd+8JyK385w0PqobgBsW9pLq5f7aO*91Nnoq1bJ9CjvVee}$ zh9R_;QL}P8hS{KWejhdp=E5FHe3DxWeo-6{AQp|^I|1k~cwjPgGcxd=%t!WTNS7Gj z339)i+>(C+bmn~NBnp0PB36?k(6wBzPkRPZH(g)Y0N{ps?J9fP$OvcneUqNGK(_3kiu|Jyf-k?vvr9GPNYQ1D|t>Ijv#OTU&s#c^! zIe*&is_Dve;xN*2W8C9ZEsj0OeS206i1V{2KP?YbL~$h;WJ6huh$L_XF}pe%CPhCwjGK zOibH9{LmDR)GA@9?!24tH}*VNIUk1FY3wCbWYq+cPiM?enLk2GWmko ziy#3UY?Cv-8F2!618U zZJAn$5tP0q%qv3v%*6-0yWiOLRqPd@;a(~b z+yG_`4DZVEd=bF|tZQwrSqWdF^y+s3e%PF^Ld;9WXasfrXGSfvGY%5M)<8D+Jix1< zaivo&Nt(-pwB%MzyEl;(osk(<*?X@(&Q;2*}g8W1>zS5t$5t!1#2M0nfY?oYC zwx_xIMcKVe#%2!5H!s^t23eLo*z0JE-rE!eLub&iuW$hfQHAulqt#}jc7jLs$>H+g z+1^yr(H8Epi(nytoxm1NbmFB?{p3Y$js4#Gz-Y_hMSqVbztx`j+7Z8V=1ENmaf5o{ z6dEK9I*8#bzhB2g5U-y?+uS4tD$;!5HN=6R$3kXBw&yRx6zqv*`8lRBU^P{{0>Oh> zghAo!%jtR#%jx$JaUu5-+u`9qmPYLfH~^lOjQBe1L%Gc6xIO~l zP0*6!nuL1}@6m+awEJENJ8oblCNaB(9O-j;>VhS-r?pH#h=;+I#)MzsanTnZr-G7_ zk_k;p#|tRC;;y={dl5tCxJ0g_RmxKxVcvV?lYxnd!S)3T={Aun(^uPRJ(?5TZcYbl zguQ2bm)B7-h`HSdiZIjs?~$>&&oApyieeD)=3RL=dYm2}Z4HiBh{;+f6>O3|5LlZz zwHbG0P%pRo{dJ|JN4ZdLRvdCf58qL3rtwecj9Q_^hJS3s{+65b^xo_Qci{`N7|4o% zhHxb_NgMjs4eo;Ja#kU zz+-nXHNol`2_W<2^gKU?CVpeO;R<5ThhVTvef_~%-3-`6P?ztGes8h+4fP%TH8G+b zSCB#GZkIYEi;T~;1gXF=fXEEFBXtk%h^1h4r3Weo>H2VCQYG~ee_k`S%^y-Oj`O%}=S{NfB|78%p)Q!%SNg&p}MrsN5+g+&iE3m?ff)(xz>ztJN?3~&6!dN(pC__01909H0R1YN z1n#+JIP$7c3)?)A_{_+_7_?!BU-4>U0?ty_SvlkM_2s*NqzR!rt1L1r5sPJ%3B-ET zY|3w{RBkZ})%b@F{Xf@VOyoIDdpH4lt1kxbk){)s#p>kDd~S26(25rub$x-a#&R%c z5WvhwiNZG8bHvT9p-}tQ!%*O0V*7}KzZoR(x%B7TT%Xn}Ji0UY zue_(Gw5rP>B?&c&4(1NupSFSXxcVLWI>x#{vebjWw?{DapQeNawHFdIqz$M)6g7yV zXkXECavsChycvtQy80O63Ff?_+iT2Ogu%;0(%`qEj{M}W+dp9%!|GD<<}o=ja_LBd zudi7`Pzu9WAT{rh!}vRj7DA8Io9m{_4R)P2%t7V{%EjpUIT zQnBm?W1XY@#$A{lrM+sccU{MhXT;N5gNa!UV|04784EV0b z7x0>zYnVcuU8IM`w~}us?+Jw>&cwwx_7G8@Yd5t4n}BdMj-0jZ1sEkzHY@l-MA0Fs z83AV%aHpajb}nLM`^`W)L@xo-Y@=hi&}kup5`^T9U&$pR6E*NVt#)C>iLIPVsWe~XluZ?Rh_)wz*0ilf1srlzHDw+Em@T)t>+dg_! z{~3dXm(yr{WO1uJOEudTstCHvv*{i_oh*z9BZUXnH}c6D()m{C}FJSUm2UO~#Q-wf|D=HD`8>7NH- zbl@2{`Yq{C+fkJFfuQ z9!5MA6ciRZ-@%J9O)LziY@?Scm1&S;h7@W%4&S0+?w z!@(TSy|77$ z(p2RohkAg=X>%WnzGJO$h74nCxX*_ZPUl*IVKq$xSG?9!wc0*DmSg3X6JNy>s(S|W z)XrAS2IG@lS66ZdbNKf1OflDpSuF-qMLJ{!EXnTonlI+50Yrw0@pX*y@5got_~&Ck z^Tvb5WT4u?JP0$TGD^Q6EWZZWjR3g-c&8XfXcz+LMNsybi5+GGZ~?l7IrLnJy z`0#X*F)>iZJAyDrkgRDmAb2wy8;ohKO8RWAkxW2)faMRj ztYdeW;W*%a*aS?BRNe)6F}v(~`&{>^aXMNVD+{uL|I#?xer3oaa18J*rd8no8G$oUzW2z7j#kl;YRCJ**>D6NClW-PG9}* zepjGfp>qD0q@<*g!pb(Tk-~Kwv}Eb#s6g1ysIW??sc{3flEF=S%p`IEbv{*A0TUeW z^P`TRBf3oQ|A2qpGlPbaaWddRzZkWu0e2MJXvwl1F9+cQg>h9qCIV>Z0eb`eYBD0Cu@qs{ zK4B<3VNP=Fg#}|#SO?;OA{VYJc=$pR2Sn}Lo(ZBxr4$X8!x-kDGt_}#iN~Ps?BWsv z66??y0H#xA^T&VXrqgQTy1@q_pa>iSVhRR54e&Ug)^0zC;uYqtR7o|*27oy=Pr;k1 zhN=OWrc1UBUjSj1jbRevdzFS54a0LWyB-H)zXL#6a~}*Az=D3AcqFx{9rJjT{cA)K z=ZlKb{e`UqbSE^uc9hywM0v^KPhqd|@$xng0*JN@kYh>N=evJQOl&Z_;cdWavk3?O z;?st6TbL&`?@=ExN0=# z+TU9(WI||ut{Dv@e|Yf$Gybn{lVOIvCa&2Bt!HASWb}68)ABU%yh7{S)5dX<{PpUrp3| zxGi?Z0wmXNOJbkE?Etsder<@s$m>5}fcdRtG8-s(;GC2Legzny*{a1n3{3mP$N<@f z9}xFXu>kMU;-R-P&?Z4%_7s|;)&H9C?>1x0a&mJI<*D^|by-3e3sEE0|9}56%K!Sh z;p_U(mxcIF|MR)wi~sj)N1TrTytV&%@&7!Hzd_;u`?Fzq2Lr38LhGu0F)L{{&J*ApG|RA)@8K z|N1{K{+m1g=V|;ucs53{VZk^`8Zw}Q+pl(*jL(zm>+27E_!-5icG70|X7gkLfzm_0 zMUXZ#GXvxC&8fPx?gSpQ1mIhDw`IEjX}wqW+!lyDRmZ?8G85qkpb5|i?0{0MN$#C# zAU8pR$`$$N+ZSYq1m6UsADG@~eOuX{Z88&68!fktn`nY84}a|-$Fw^onG1UN1wt)^ z-DTJCq=*fhT1qar>&DHSzoa2sepP-6HCVgHvCD44Ka6|03-zqq1D1tzj%wP!J@g6p(JDq?8T; z=?0}6=?07L?hpi|yGx}zr9)C0>4tYby7xKzjPK{i4;@2+=f1C4G1pvkEr4u;N%On} zno4sOB?Uz`c5pJ3$|XSi2Z7S&kmxhv)z;N5QfOA&I{p6nPR$S$R#sP5vJ`;|rwz;w6tV*|8ura@4@FmGBD)IK#O1mG1}wtI2I1WFjrFuB-OT6?pee5=MZdLBe1X`KmI8M zl}E5xOd}cSD4OQC|9;}=JEW%s5k;wxVz5f#n+d9@#EeTS5d*VycJ|XoIw;y8Uh}pJ zv;^lX@wn^g>MrUc$REa=^RT55*W~;#fX|}l;J|0r`KV@C3O&G8X3%$U(ww{ob)KE= zTaYbG0Uu6C=p!bg`%}aGPn{GA>F-A?1ZCaElK5H;nReOSb_onNJ&q|!uo3*!<&+2H zLwQM77K7d-dYCHgteAIXBLyi1U(#0nsq_T2Ek8szxI{h2`I|N&1!G?|aseqgL6kKO z9$wzTfq|h^5BA5ec=ZYjq=ZWeLx!5g2ZrShx7H(?^zR1Daey0h&b{Bf`X!G>FXO`<^}`=fF|MXJ9pNBNoK?cP0o;IBFby@ZixOE zK$19TfOM%3aTW}l-2(w5PzXUogJFoj(>92$;9CHE4uqqF<*+i44!s-zV>Cc<%{Yg! z5Jq=Vzsq=th5!`i3WaToM6DL){8Lg=fb>PHoKMv}hKY+?0UTRq-If@%7iSK@;DN-D z(|$ch&BXTi&*y{b&_xlxneAWcl>s7HiSiRM(3uzReY+EmaO9x!Rc$Mbg**!653>** zBB-I2@#DUC;G9E-xzuX2Jtw^3g)*l^bAw`KWo0g|GcqG|1ey(Sc>OhFf+(LnQ7}FN zI=YeQ+UlywMz`PJb$omGdf&mxX3bV9k2LU)KB>JP1PVlHIb0YFQ{V-_?JkQ1ZdXPJ zM5acxkOM~iWQ3W#PfDX){PqE}4O?RaDbazBrh3SQ9#aMs1M2c6Fc31}g2hM{2ZzJ9 zwlvHWUWHIQJpH$&AYqQWnp$`TtY%Fr>1R3$=@zX8Dh;+oCoBv;id*bzCy z%MzzugDSJ-zE2&(2-*s?Bt?$U`?UhrV4;@dN>cFG!@q5yf-`#K#tm_5dm%*?Wz_Iu0y*0o>m zMUJN!NGT{pOn#_>Ifb1b2~Y(ZW`{LOD6akS!0CaGOH)&mN>>v$Xh>Lpr3uP}6KWFL zG&MEldQ{G(-?Ijt>YW~hEP6f~gcul)EEPrKlDTYJ4j#q&w2GLf1~epri5i+AX>!B; zmHz{(@nc+8$A>c>1!dO9{XDt(N&`wygSI2mX=&$6F>>^%{2-a0(zG=F)_E< zDAVIEqNlcBvUVU_-8li6TY?XHG0zIHALv(50gWXvq>E^X?XBWOBjKYfJ57c$;Pjf+ z3M)z~WhRl6Jdn%c+e|?z6VL*XOH?87KW{|mH|ieAQ>U~sH&fp{fWnw0k)9mIBMhmCs}l=*wUW^ca~kEqw%R{z(~n?$f26ib zA&8I0*zon`URHop1JDG^73)m_a_KOmpTlVj7J?-LkO!#L2qNG4FrD!cV} zDGJfAlxiZpgQ)cZ9=Cn&F!=hw4kdnmet*kAfQO4UGK5p1Ubmxx0^GMR8zgeh0cgzEZpb$)eja=Ay|c5EhmX%;Gr{#0Fh%507C7$t5f`&vE8}E5PJa(@ zRWDOd=m2b0m5lt7%dZ|H^K^gaZ_zafCNGd50IP1YDMobvQ)nSh_>l1L3Op;n_iRF#INg-e>FGSkRJ6qdMP84uz1SDi z*F?4;tU}FICWS|6ys=Sm`g7Nj8Q;5oI0y?@i@41P!F!hNL(l;x`Bb~a+v^TKX<5&m ztTHK}QAzNCQyYTCpl!4(k&La;h1#2mXDH8pc}-0XVd4xp!PSR0T4q_A!%#?j%z=)I z%9^L#5*DBTndJub>^70PjbS}_KvNOvv7J)u{W2&fEj4vPD#LZM{OHJ0f4v0CtDI70 ze>o&bg-}D%jNqcGcGw~g!9J$9dH3#}^)4ddal7n=vdSWhQ`2Up)%=1K&aSEcmtuCw z7HB$O+=r?|$327B03uiivOk9A29AXz72;v~cwU$CA+GGSk<>49)2rd^Kv@Hz zlnHPiaJ4d|;xk^bl5Qqy6u?WZ3q$uw>bSFP+8sv@2k=u>zFCnw8A5-z=3&jPloA*K zon33P1@v8$P&??LR-C>E@<(5A@S%_B<+yJrRQ65PESc~Ye2La=FE8BWccJvvwg$b* zmkpYHLqK=W`w9kCO?}rmLMlSG8u-*v6`*U_GG2pn1BhSHLP*b0(F)PTBC*+>Eydz3 zK!i==fs#RCiRmT*Q>WZcY^$nxWIWF-o;^>fPxw#+?wpeLq7uB)BS9}J!RBta?Pgr- z#sJYnHoH}Y!47EW5DUz`_%3*#fM5a?r>n#fttl7BhX3wX8^zV-+&_^Y`v^pcW#2QP zLekc4{y7~&!)V$vd~Cq_7xoW+@9hm$E6%mS8yUsF08g{95^(eGgU{x2XyO_uSyPu( zj2-^L!OAEAfcDooSXPPjXUdSA>YhU^(r&Jtr$+PzeIRqTuN}Y*A~o~lAVQ;HigPa&N^#li1!QLFY^4~`sFApo7@84$!|U5f@^ zExAz~l9aT$xR_q1NP)rb3zY0?hcQ^-a`9LKwIo5+U4Z91*&t{#X-5=_X-ED z1Z_B5r;SH^?^Nh(_QgHK0oHScP(-3`9lEn1N()sp4BD*VtmFKFf_^B9Api&Vlk5M3+o;+C8kA-e$$6v`+~Y!Rq>J4ZTBR|CQRQ6F1F_QVq_0-gaP*$$UmkT8`M z)Io+8n}wZfqrZx7RfTAPL41Wk-%CkKS1=6S1k7HutgRZhK^!Bw6QYPAKW`8pqE`l9 zrNA_eVH1QgO9jjtg|FU0=?BE1h4mdBE7`}2SV)SF$-fJLLsmkZJ+3w-WYNw+(HO{= zI&Uz@`4Hmca{+J8oW&240N(K@gSgtRk1ERW-o(qdF#2m!*}!m#bn4yMw)^U+z=(;c z=ol;IU69?F3l)=)kfUW`@j@qI*qB8JQ#m(U8V6k=rtu5RTNE7&Q@}Hkc;~y*BiId$ zydQGDJD9>&uewhE=3H-iM3E9bfkWECrTOYr3IyHtZl?=N;P(}2iPcJtLN%P0`yM&v z>`*j9ws3ZFx|MY8cNb>DIY7g_Zz)`ts1<>}Sc)oRGoyy+ltnw%;3O>i;v&LB^I}e=LjV-yOzqwqfB5$5B8mqrIN0I#t+By2n-~7&5-;d{ z=3~vDfUc1KAo%IeyoR$-i(6u3#ky&*{RK!Floe8vl53+yY2|DWA3mFod#tZ;3VgdE zlr#iAfTRlnPK;35_*UYfXFz#jC-=bvCku;MCrXH^%C*Tkffb+b%Qi=9+OYcPFal}c zAL%J-$5|VYUKD7}1Y&hhz?eb~71oi50+#U*1NB{=S8SfiuH` zi;tg$trD-VQVEHoii*n1moGt;#|MLKsVC7i?g8hcN6*Ivk08w%iV}f3EjD)?{1PxZ z&iKBJKvP1X+|!|#U(R5XYX{}nl!H9!X7|4VR8{K=w&$9S;H=^mD1yHM_#d=OL-E-}cbN>*yLt?Si=xdHgG`1X zZg6u2+>Zw+yOkj-l_8>k3P~Zx5C${Y6M(OV)cOZQ$OzEj8MF@|H_E|Qf-X@Z3!`{I z$zHy^$AJYu=KQDT^en^dM&J7yM6qq+hgL}DMFP^e&}1LP?pZ{LcdP;#c>wEQjNMJS z!NE}hRL(4-M_XsHS4#c2z5cX4qQZ9}I)~sdJ-Mq!+S)VBVbCxOshSi6)5=s$#kW^4 zPX&nEbCAYcTSe>RAMCn31^Q4KBmBTa+`o@5emg}_Q1A!JJoh#v`&7}lDWwx$u7!v~ zS2Y?6iX3gg9b`cmc%(u72jfB*GHbs-HMQ9F+bN!q#6!uyEfQ#ia{eKrduSQ&|#|&_Xdi)65FtZhLVsc zN4`gdhBqLbRYn11k_MNG7aWi8eE_RK0m^~7{!IapG-0+qF92s68RF8?Q7)%Y{DvS3 z(r;WmJgT_U<73DG7~%L@0M~*X1yGdPwKZB3XrF@40O)ckrn`y7@A&&63yU?dB{NVT z=YueynVDHjGBkQ}Q{kG92vse?wmOzhksyN5V(9qE!G`xby7{Nb_>AK8+A=tcj!5tX zN+_^afFwd>p+Ny%y)UP=9Cw$bl$Dk9UIG5Kqx2d$X>vCifaW0a4NP-cp3Vr=#rxQs zo16DHCX`LiZ}l^AajC9S4?>IWVjN_SG(|PW=}L$mX24?r#DbPT@OP!`@&|ybH2-)x zfj4ur5?%w5@X^|#q%30ef!hU1kBoi54R)kpp(-44Ko}`aiOltI%E9CYJp0ABzgypa zf#eK&N!E)9yeJRwF>4&N!1#DX_vnI`bp@<`1P5Tyi*`D&&0}fO+^g{^FkbuAfKMKew4zp|mm9y=6o-pi88Q~3&l&0q5Z-uiHBW(46y9$8L7f2;91bSpq)=-?F28PG1$fn=^v zSb_O0esC2SX0*~m(?c*3+iD+V>Y7yB)|%799YUj`4EPvMAx;IPX$f?XBg!eb&{CTB zqYc}cVqtD20LP0g9v+wAXWGt%goWF0bTAUIK)`lu`|9$-~+@D^`9IRCwo%K&uV z4}%=ua;jPpe~rd=b!Y{wv>GJ>$ZWX^F7O;+zKlFzG$L@Q+A;tc#{1?W$#n|HAw>lg zGlV0yE!?p!tAfY32}0X+9EQ20GO4-3#o4;KLPm|s6gwm4w@FZ{00-l+KFVy+6$6a2 zI2us)V;DiG>BeW0GBEV?_R35wz`>WfQw*w=S&9D8UQf6Pn0&vhNf3}Y5AfK{VvcQp zz!eM+KY`GLrsR#!1o{4KF+@#H2;SPWzqUvM$((XA&;}vrQPIO5PkaI^44`m?DZ9nw z7xN9E4VPP)MN^2P+`(_e~6Mu_dFK+w0yAz zE}rAZ(Fe&>BT!$BhcX0t2@%JoBak~w#1&{GZOqOV8TF?EJtc=oU#}~XTR9(ET0$#2 z2JVJji9f}S{yAKD-t!1G;plzVsuK;1?jnO@TU0pdOy7X1uTpsEV zjZkhhK5g%j*QlTl-RBPF=l&t?TY5!9pGxwv(5;=coCpMrriP`)9&C3 zylpvdc)y2({IqY;AXsvDL=Iuw)=YCJy8;hhjQtFPDMppTXGt$f8|i+tk)WCBKz8=7 zJv9f?n86Il?6R)mZnp-AUh4yS(W8@-xO$wsQe;3Pp3cSgi5!o32z2tx~_P?i1y{ z_`u@nT(edQ;F(y}_3rJu=Uok-&o6Wk9UK7?^7&kL>epZ7n1zA_Kw-2Cb3I{9y)c+mY)oa&&;N$NxgPM(9&}||*)nXxHowWPg zhcb4X=tS$|Ws-@lLL`XJtT)s|Mg^Kzk-jEiK7&aW+1RQWmopJAx1jf)miPf6kQ(?T zBn9SbP;e-pe$wv6bGI3cJlyI}=i4<~^$?sU@pv7A0_aFJCV#lm+J@~?7rwmu)6H;xc5S!)wqyO{a3ucT}c;%kDoaCmd$i{+B%qZQS^@_BZp79 zxr39B0v8tamR={5z7?5$!wDLmOWOftI4jQ1&N1=HP{w?uOQalA<8tHrbtdgUT`x#T zQy#Scux#!hA0ehSL_2xG9ZOSs zpPK5%du3sfl$_jq1cMl#>?+90$_7Ng1QXhL@g_-FQ1IHh+6yFeOUvGvsMx$b`dubT zNy+s9x@Fw*iV9Zkwj)jv4GqHGLK&=QWOSAbz~qd%(lybwvQcGYZ5@;Um00>8K$z$s zO2gZw`D@D=85B$S5HcsxDk^%@4Jl*f<~3a;qDTpSzJZsZ)fv80DyWxR#$4BM^JD-HJkWr@Io`hi ztfsP(N%IhQZ-a^QCn$fDWBNKF>CP8sXRku06doRq&p1`<6mIpXV|X~qiIQ^_3{BUy z&wNryn(FHQD?T6v+sMw0j)rCs@Y;Q3xqw;bFUJxI`A_fzPu^V$Lt*LZ1ERWZk&$6d zjX@kBFE0c*Qh6IjcFK5f7iPC{vz^9bW04uI~y@`z|_ zH`LcBUC3ElGVX4BeE~?d#Z&%HO6YcXs4P(11Ner@%F3WVZQx#(lJeH-(MfKMu0~do zX}@G&G<<4b^DC{$6CWfs5Pfz*Il$|J?ox{JO-*HGZ=$EjZULj8{w{Itzto$Q*eH;h zB_itP=_Mcf@q<}C3|~W2llcQ=m$5e?b9;phfeABu3K^y^;zEX2S65R|P=G9U7s=4T zzyo~z*BYGrNY&}-o>l2(L!)Rqw(i%iT^n}Ax1qOT#3*NC^>~BCYX`?{V`F1|P+>+1Gb(Nq{uehwD5T|0!jL_CY3EilTx7 zpa(@Z%-{BEqaOITS#c8D|5L88{FjMjl)?N~1Sb3-gvgx)yr6REwF{($dIw9Ss2KF)=r9*4WXy@+7;s7={`69AhHeZbR#7 z*F;WisMzO_kb8TvukxRQHJ6peI3bMTKU%qJ1k>l(fzy82Bzt z>{Ib9~}HZ^!KbHq25Z#G`S^{yMZsBy}IAK z@}}*9t-J66_4BKpA%!LjuZebY@!O3=QEEb0-9KY{@7=$5m9X_HA1?~_x6lv$5W7u1y|;elKd?UGNji1-ox&uXhZz-*jx~qE zEOewuKHV36Kq1$Lb<}Wt$#6AxoL|C%m^{jE-oLsHUNAywyqAg8B!nQ+N1J+1>mw&l zpL@^iB*D{IZ~F(S#|a0~Qf_D3{AF0HSzmDW`veX@4nzz@og7$T;@JrcbE0f=0sA!|h5i|Y1X!a&iw`C)* zC0;<@dV~DreDsJNTh7#paE`HD$!2)VuJJWo?E!b^Ps#5Rqy(QN>0aU#(MOUG_^L1Z zjmS;SS>H#^exSxJM(&gJ*J>aR%~yWptINRx>2f!{y-;aPw#e(Bph7mq-X(QxAksiT-s47&;*IC;6GwRyC+fo8^60z#srzPpdRdp#j~vCi zC?$J%hxeb4hv$A@Do0~kcm9__4KH>11n%8UdM}2&O6<$I$ue(IqD}vsI5fQ9iZVXN z$cbn8@GtYcNqe8fB`tT;GGcco3#(53F}`BXJJ=sC;g1&`fkopRwf6K>yaeuE}j%w%~hJfzY@3o91T*ZxG6atIKzqBQ( zx4Vqqv>BB-Pn(1deW`{h_J7c;KUW)oiTe-VCP{w~Z1hZDtkAi?KP4#4;6-RJ6PZ}b z=>47b`>YSn@NcVP6ov_cm!*xYrt^`Rrrl&p50LXY=ZiGq~$$igEJ}J?nl0t z>z$P5@NFw{Q}`LHV9i=WQ=RgtEH$q%D>L8ai;JVJV+4-0t};5svxl~5m@f;jm0Q(>o$8EI$3JY1M^FB2P+IN~LVb_- zzgG$awvog_Z^6#^xcla|bI-f0ElIWI=$|*)zL1BiunMglJ)iD-I=p}(pYZ%Kx0{xQ zy=(a}i*@Yt-$-Jo!Z|7w7i!HABTtaAxi$6UuobXMmQZ)Mh?A z>WrCP@~*_4nIA`FNw-<9w3~1Y>}?vv^0(Q!YAi&n;Y4ZPzpSP%$w(_LD@aG@cp`De znEc+S{&@3|tJJTbIbNF)e&weHtAYcxQ%TqpP4z56ZT;1W`FJtT-#KJQH*QA{rX{kY zkEjbVaMZOr{xqWJb8uk@ra5VSH_SsES@5yf-J$C!Q$8AphWp~vMn%V7k5^~W#N@yM zGFH(AbxD+w9_<8@+VR+z80{XdYb4^RGTv(r_meUmZ0%$p{-0CNckh`84Vj~>l~eP^ z&EP#ESEsU#!+2!>o7l{Ui!Y@<5&Lg)IhOdbIW63w8(R#oY36d8O266J>v`rrZu2hn zJU9$zhLHJ8?&qF{g}!qW7pU_zMAx``XQ$>$*2INhFWRJG5o` z(K^{ZLJY0evK0H=f_*Y8l0NfyI7S{aqX+p5W)9ccXG+Gk3W z%aYIO4}4iA-CVxA7R{OuE9@nfH63k^OETYd+vOy}yr6u27GAg%Hh)5}@WLTSdttDI z-+5YbcwDrT>!@~y_f(zP9XS0a2TwmX69!YyaNzdci%SFVaTOu{)8aY&@9c<*m=<1y1H$SVc zIFTS16*H;tCLy0SWBWnnQe!l7jz??%n6#Qdq4GCBSG@KQ&FIye`9(gHdY87|hB$_= zn0epwy-5>@Ism_6g0in?)cw4(up4Cw(^)Bq@0~a!i}UGCzQbgIPX7IRi;sSFxuJ2H z>QiwB`}*^M&Bv`DP=1wXYb+H7;@qW~-iklXl$y-m{Um9dPcLV8q1v)qqf4QE+q8FH zG4U1IhTeW&)A7u{po)BdyXn}m4=kntM^2Dv`DS52Tx;IZ`ky0bB3(bd z!Qmw~aK$c}t>_rAaQ=Ff) zTn;^nwR2sBkBPDbIdPq4ol1X_-MBc4=B@@qa{DaTsrmbwiZwHx)D69U;O65$>OzU(>?g~b@J3&rHOd+X8Q z;r`Ff?7elp&x3>1S7D?NYoAK1chZEcU^LqF@J;ToxVrhabvKq=)diOyBXcbcs~U#l z!eb+HgD&ywNhlu`+M4cWXS|F|#;DYg`-N&!Bz@X*@?P8K-TSE%$-~1_Trs{?!#z4B zk07JEXL|z}^K?x+k9DGD{C#<~<=s%XIZ&!4YeWizy%JwveO#0=bE;op$=sZG%gWJH z<<}$JVvNQYLX<@Pu06I-v@ylWYmL`_L@b`XoAD*+<~y2xcab0{f%=(laqedSm_k?^ zCRmSJt$ZebJvZn33d@YEo4JGahV8x6wCzQzA-elXTO8}y;mS5ZccA6)Fv7dp50ZWV zejTkpLptw$7$$aCsvCViIjKgkB=EBBjE$RXcyoS~hR3B~X#Ls_-KT|!)ittTIEi&F zVSdF~ia6*8m4?3F6;ym=eS_**r$lRa-%oRtCU)lSoHK{(BR^Nylsmi;`P=hHCoj?Q zS|3l8oU;fg@ih6HIx&`7n%vJfPhXB26On3(narV6W1!K525#skhRf2lFU&Jfoxblp z&~PdH_S7KMZx{D!rs@cfQ#Wqk++oirnJ=*Sfh3r>7-o!4^(53k1qyg5SsNtUWZOLp z(v7~}n#@B;YkyYkyZhyRobu1W=mL~CWMuz$-?6UuU6QkS4h)UsobJW??ne?`6|GmR ziq!pK5hQh1a+q-R7SFl<%fPUMiloUbkpa-V@xtzR6DW7*jRv^$ERn^WJYqgH3yh$@%lJOM6dEXMz+FRBx-&P{19w?-S3F#yzyQ8jDK0lG*{2)sz~*hA4!Os`Tp&lMC?2tEm(uyHY^EK7nidVJsWOHX$)UWCG$mwIa`XV}%ry=gG|^JHC9o3BnPlDt4bx`!TQZ$(M8x>8zi5+GBTfUO z{*t@bfO6rU#crabBOuC;o)u7HLi=D+wzzj5(FY z_9ENC_xuj+M$6YHQRwLyh#uc7?EMG4MJ&hL-g|H)aIjF)>q=-JEqbP(|2R)=ZBQ@s z)E&bKvb)Jy?%T5z(SjVkzAE`TQG1BDFl$+bp1G=K`f1QagBVM-sHd?DV_lHvFoR-U z$J0lq-CUpd%3?0&O8DZ}lQ(ZWcKbN@&sD41tv{zUFo@LKX>S{ziq?%zJkT(9HT5=R z*R2(>)wKG>J^l;vC zd+t>&JZB!(>^4=3kjR?4eaj4Dz++`qlkgQPstM%Q9EeInH+1{HhjikS<)3@MIP2OD zVU5fiv$=VdGSlI?egBKetT!V3Kb2IhRB!mcXyN+M47Z-L z%W4NPUYEYiUFah8Wq@-tOmmQ;{jJ?wlSW)`SO}Yb#aojg&{B%Hvu9E1So(W}{pimh z3O!#jhd<)dUlckYSEz?Y-uG=4SSye7X5_J3X$?0p?P_$;rsagFn?LtCUG}+$YD(Qxl@XDgpT$!nT6q;gTA%06(%(|7x#;B|3mAU9hN7B`U59mFL|9<0f;*jo zt*-T~6qjJ?csr@MAzkt=QM$%m2(|L$oAl>1ga^Mgm?-;zSr1{A2^ ziXw>%Jy-B%;3g^h@ksSZDR6TBb$MRw%41B0;%6OKzrOE!Rc3B+(Zr@svKa~I;? z=zgjg*1mJwQ4N!5ir%)scaXy}EB|uD+s-|VX-dyobX~;LgvHbTkh1ccu1oqD&itG2 zB+_wkssh|f0w~+lilrH!na_EPNLG{BlkB;ajqg0NLE_aGLg${iay;fDZEc40Q>84v zJl3t>FOnPGD2dzS@c9cnbKY2iZ}sN?IavnxZlzpqd{HpbzIg_GYwB}rdPhvacRWIr6xo=I*%MyW$7(R=T^^+0h+#vS&mJ<{x zHS^-jLHJ&?;ygNmp8CyJQiJB}wMJrNB5CpbM69t6Vk@}RHqs-wt;Dt+Jj6<_@!YJ@ zQhWnX1Kb+!II`^2V#^w;mPoJ&^SxGB?SK1wBW|CfqG3T^a1H&jw;S5+f&M9!-dm-b z19Y@{`9+PlNiHvz-UzJMh?t^9&jrtk44~JjB;(n%^AlmzE0#3#g}SrM>ZEaNqnKW@ z^}(1A@QAQCm(V*26kuT^Lg&y$%^nL(CKz% zwmZ|Bm!OwnzrFq_-Lh%#l5?%3{n2ZgXBehl{?z&Lk?%P=xXg7tqN{&5Z|V{fpmk3) zoqE?|dl)4ieUQ$oeD3*CcDOla@u8FEXhnOU%rk+ULR2M!2^9L%>kg#lGUTat1uo+p zr_yh7r}$k;zB<(Ut@S!@pVnjD$I6m;?|sJki{L~F`0U21xex>(|_-dLo=-RdQ1V=qakP&ly`YDZTr zwSxbSjb8t*%yDgT11{@Vy#h;gf&d+t(Vvxg9EEz(%EJcnF?P=No#cTH6V%^}T-S1& z*9_^^xD4C9X2|yr0?mV#?&jUE91xau&j|?Gkn;;KYG~K$M>ESw_X+U zJH>7M=TDP{Jp{>%6@OY>J6HL>(c`;&ImvJOtyr}oZ%L)tYID2}^=M(_3%6P28`Bz_ z0jX$9ly!J(D2}s;OQ+aG|MH_y!mR?W}aV4 z-jush<~oyD^ze+o@DQd24yy|@%GI6R zxU6iv-N2RWwqWph%|@3Vs7U=OBRP@gh6Q8XYln6vICP1pYdeW&sTsq{#E;4C?Co@C z{;44Txn`Gl;g+?$sSHgGD}Jl~sW{Uv+G2{^q2JF*lKsrGqV1{i;0N5T#9H~hFwQYP zqauREh7L{l@QML6f+2U8(N}yF=|mGN33X!>&Uy7)a>xFt!bYEVM+ne+G*2j7zb_J^ zM!DDNZTJzL9?P_uBylsQW$i9*<@5z#+KQnhngn}XIhi5S8dKLyvb2{NU-)g7HZ?EB z9$Y@&v{~%0+z>i8!0O|sdkcUcUIw$#t>1d_-D%zIMFi$BdWe5&J*E;6s2cTghMCE! z5fnr&u`Rc-KBLsD1#~iY>A}u}fmK6?9p8&$`?X1JOO-EozSv3v}nf0-S5orGh2loJZ(qvv9Vr?~9d@N@f@rgIhL2f0u4Dq`Y*;^b|d7P}*&giu`LLi2$I;n%WW$?I5kRUmQ43P3vzb zMw`Wyy`pY?pT2vkQeyq<%Del;nSl(P&L`6@_rjCH)n`Wc==PhFPmeiq=uVwxn0s>C z3A#4VGd9bg^-Ue*@fbHM_q;Le%P!d4>Ok{p`WIFm1hRwe)O&yNso_m1^_+ z=b^QV?_KMJ=|6fm=WtK;r<827N)!A2m0YV#PA3xkku4jSo9eE3c=o$MYT&roK34%t zJS%r5{#}8=;_d!QR{BeQV*8zO{_!t*Wy?Q(8KW+Ir{{8-a}J643>KH;LQYaM3(!xa z{^K)`w%#E*d*Klg(HCUbTX6^|_BwVG=T|l4|5PcRTVt!i+3hsv-k@8EA|;q=cD?fx z!f>{yBQ0nIPgGo648wUh3cqoDDv{cI;MVYX2N&0`{%+#Trft$?YaBNB0ct`BK5F9m zCj6tf6Fd2`4^ zrKhv_Ub!0vM)db`-}rS_Z!pX#S+7;R)F&%WfZN;jYo*TZ{KeM)0^vw+5d{sy2-&Nj zvdb@|7p`^N{JNa%<=Co-MBLCKv-$UKq8x4NZzgy`YSjzEgyf_$ZXGSsbK0%odeWM9 zZJJw&rEJ&^XKvVPJz=ob60t1!RysHqTt8U#Da+}4aOO*@QS66peM}6?J-mC_+6O*< zH*p{*T$n=^7VJ0}Z!X~3I8Kb=+qLqTLCwzLMlMe3QC69%T#8_}**m*YF#cUWnmu-M zX}^mHqdzr4Ou>g_?#XRflVdbK3|rn9{ADpcYi$n?IB_By&(rygPPWO2`{Jd%^D?4M`HPzahpH5*+f!a{ zgs9PJ(H=EsM3>w0Xi4?dE3tqYjZXY$#eUBqU)j4O^bpf@mEb!`OA;INjAMw$v?ZONE3$^xzYrCJvWisV)KI-I+zk^GCYu?wPFge-6{)#Nq9PkRL4UHhViY zhfM5bloj<^_#7+n%gJj0U0YZgO(%5Y!*I3 z0}_Q82QgpeLy4JtK~Bh|zGvh$%cguk#&q47mQWK_vsY_7pQ;@>hrW2hs7q3DmR4`B z#xTR6p85Kf1ZilD;VV>46-#XotK;~@egpfU%qKmY3%KV_-Ai2lMz8nQiWJy!}!MD>}SJYX!9+3B>%aC&u(7X3lJs` zU40NwsxxQHs4?j*ww%MSG{`axCPK5}P}ySTMs4xTCRS3zHq!3%_c``NpSNu7cT&q> zo_xExkwr`3PgMsh83_Hv2Y;U(#-neKim)CQq*N4!xQ{+wj&7J@u?E!y*XMck5=&K! zc3shjcV{Ozd1C>5nrEIbdpd%~NdahO-fz9@tdZ-D`TNBN{v2ENF8p4{J6U)W_`EMQ zw6-~!J%=+s)U&ERGy7I;ZqsTnGB{!DT9bHL!>i-(spv6A%kHrKbUbL~hNOpDnd}Rt z(~2O34LE!m|3{pyKL&)*GBo5I#|>1p$`2Vwe#TlggTc!UWBB3HOmOLA-h)ts@B*iYKv1@J0X{ zYlTe&F>0EJR-Lc=+edYM+G!*h$}e|s-Euz8z86f=Ed3An;m?NQ0j7R=9W!=vavW!O zA=Wo?gD+Hf?Z8^s{^!l z8tk0m%gKjRX}gt@)GO^|BfDs0;=DAl>J>)gyrV|p_jKu|e(#1t$uC|H!M1}>F_Ht! z4bfp7oqD8kSj79Zls^^2+m}n_W~EGp6iWlT+w@rMZ;<$P4P`W54if<=WgJ*ib0e5; zrhcok*h{9byY~CYQGWxYGcm;@wrgT+@@4ZzB*qT}sB}G)uCvH-`;^`xTUa?x`_FBC z^%SB#5`j-I$GN%hCua)t`el}l7?}_!Zqi}bH8WX!t|~iMHL3obF?yCKab}58t|vpVg&n0;t*%F3Ku2#hZRfbc?k-YUYeujYHzmOB z^cc;zV||Xh-jr< zV2H<|eZ0NqOX<~`xZast_{MkwelGW>Y{}~qaX2{8$ud-ZjM>Ac-u-MBGh(x52#*pC z{==pI*C1TDL2a4qmXMsj{KR0w2#I$4aXMqCiK*ICWUH-_=(O^B@x%8-yf0gZip^`+ z4vD85Jp_JNrbZ0l+nJ}?`~qH&8C;zk!L+&4kHyeaqRrcwTolCH2%$n^vNoQJ==>mtj~A7u@2oe_8&%`i_PB~;P$&mJ*xhq z`POaf*NOJ7cQK9B<74;l-}~@F?blrtY`r)G@-QMJDh!6nrMXy!{`AHF%{n zg6BxQHUR;cx&zwcYR;P@#nT0L$GU!Nxq-jADDi1Y$hHc9R^saK=Y7uFS7o3sh*DzJ zTln5tv2FenYmAbd+FDK(=B$?H`xcF*ciN-nTWs?2)w}D~*16@Ht;=L3O!d&$zuRT% z#ZV<`L{-+DS}92Vz*ARCl&DDu3lY#2QFbS&(>PVP?^}+;#gBU3`FCVJ)6YkLFg=nB z`EJpagIBe)n%0qpv`v?>*brJKW^gl4xW#3KBwe%fDScXgbpLzn7fZpnQ@XWCJ$Zd} zub8{)pkivgo6P?B|9#Dnwg|r#cHQJcPaQ);5{09qxW@FOyA9)U@-5cG^@m}_r(SPH z)r%dEvtxe6VDe&Vm8gp?SI*(P$;+eCa=TDEILK(vaC~A#KCv>y{xa{~3+!Cb3Dib)l^Z|4NWW3Tyzk>oBfyQjohG?< zLnrFP^@vBRL|gqHvClr4s}D;x_J|ae7?<)9H0pLI#_k7^BIAoG@(VmS%1dTkcxd-Q zOpPG#)P^#t{G6+j@Z0&+M%Mo_Q;-}vAn1vMLT5a7x60r`aMp1x4YwlwIWfiEpA-|@ zk`(e`gT)oQ#F^gTYmS1n3u^-cLnxQ#=jS(Peb1MhCMS8Zp4hHz@F{I#g^aK~!t@S2_HLz6A$xlSgN1Vc**m*_&;WfGyRudneCbox~j^mPm zs+8Vd!B2N9G(mbK8&WI1p#RH-?%#_8f4^-2p%&Ke@by$p6I@lnh2UJq%E*3WGI?vN z`xPH4?)mg@i|07p?#G-Zx|$kBvxQC7z(_)(jY>Nacq$wnz!t+tm)AeS|5kGlfK#E} z8|(+?ZE`Zg0W6`AM!HCSHpc50ZT^gFxD{dRPS^kG0awo|N4B+S>Y((^z|;+qIy>+08|_{%_5Lr&7Uaa7dV` zu}{2A^T~F~_N7MI0!IQxrAw~&J;?Gg#?eV)eO~vutd^-qphtgT+@dg}Wv@C$azcw2 z8>7tUq^YK`PSVIF;Xj)kgAb<1coezMR6l_JDk)i*4Mo=Pg@iuC*ymQgnp?bl8-%sb zdu-8g**{EY>k%hL1=lUA6KV3IC5^NmknLLew{vo8rnfd(Bu%gWGC!bBl5cpmws7#y zCAH4gm400_$XK25C3~^8t=sN%MoYK*{)*f%Dq<5mOu9Grjk&GaHKW8Q*2Af30Xf^OYrX0d`Oqp zsgj3Ya9Y-CBrO;4xiN~P$1ZMbOVI@fbNOA`)-<#nwrZbN8`G-D^pH;#QctHET;IJ` zw~8Hp_@aI0C18q&W4=wF##Xzs)TpiUylES40TY8krqm2C+yc(xrdCQDWAd?E--vlc8jaa2uu0q} zi_qVaQ*Hz~Ar&6Z+fV$aP@*&99d|}%5WKs{q_yNJ-)E_CdDR>P*#Z~Y1Q(s%tjYb@ z;Mslg8A9<{LJ0^oHnEqJppUcoN98P5jkfE&sqC1c(#IQm&aRq#n)EXIEWO-J;zd8o zbS6wHh39)A|7RsY174hA{iCgedy?HHq@RY+#iIff&L>hZO;;_Q!0(we`ALsNGZneH znijtOZf-_YLVLiJGm>PCl{lv4ubMyc%g<(|BeL=;XR9hc8<_~`Dk3riN2=uR>(bnkf^@-2GIKBcwn>h~GcnD8g;_vR_T zL^PjsXGOK4)r~T^i6$i@-vb>AM~nQpyZOU1#F6g9~1FOs+nqb(Vu9PqB#aP<(F?F}cZ57kP@xE`V(j3;P{BxcqYi zOjk5%MQchM5LS2YD~oe#^m1B`on<m=z^d}-uBH^U^N z%U>P*|J$B(HLv8;CPa5|zZpkPuic~(Yom{-{XL%$7vhPHQPh%dib5kgIo>^_OBcso{n*tf zGps5KUoSJZ=A!O2#>^3!FlOY) zOXE&cY+d&8@qa%d+|8E6QcBW!Bi)BX&79yb6JU1g~yDN?tL4!wK&9U>fXj~>g zxT?8bu61>Kr?!2Y5NvIDd8!Rs8_N3}3k^g+Ny)fwe%gKdEZkEkp?UV-kd%|* zMuL(vW>sgM_7?p#phW#i8P}d=z0|*6nJ&KfL=O}^)t4FY+0pw7cCQ&(oX&yW+uCy* zIGdFGsuL!~gYgW)uD`0AG!!=b<0L$O=Hc!!qzbujE$MCazH00>3o}DD(PB5V=^SrQ zaB1`z9rqy}(GXozpM{js=rPj5GsgvSSs7sBrbr3SSWmUt`v`SVqeB+*Uf>c1|J1&C zW2B;#l^|`lbzCfn3JrlOdw!L6211}}>cE1*f= zHEk3#Z|p$UR+ji)OEASw)nCdc!O1{b_XebW7&6GsyvKj0PyalhMST7q5hXFKjOWi9 zj@d0DpNF{-y@*=Djt{Bmo+3wtT~5W3jm@V-R!9K8ZvwZZLLrV7}P7{{I&l^U#zqxs^Ui4X?+sParmOQL3vn{rz z@>+$1y}?aa^ybRN*)>0jWsaUSO8L`QP28w(^EUo0(hU6>RwMivW>s$NvVP$s{k^xp zRVt6uQSnvGctjaz7wz%hetNMfz?zhL*Y_R65fcoV_48%P1uB?MzT?Q!``whjt1DaJ z=3?aD$2a;k0(Kuk&x!9VK*B&0)pMAjm5~8C$^<9JWPU@DRt-nALq`=F^GZM|{(suZ zJgR!^8;d8 zm3j7_nxp9PGm2P(vyg$Z=?}=-+PpaE^cj6&lxOeJ^Ip@uR@D=|ywbF>!5L#8$}!A$ zTsk4SSY-F@WX?ab6VpAdt#tkV5jl@RDt-9*J3)~&R3y01hWxxY%T#0K@V5i? zLT>7p<@$2ebDhExw zd0fX97Np5vF0%QFh+bg!Omnj@=3DMgS^y)mbF+Rd#CSF8^yHsw@s30I6LJ889{&1J zgFmmy_D<(QNYCy_kljYLvgSP}{!!AF2gmz;%~JXhLQP}S-Myuqk`s9I z;wU4-nd_Pu4_32!L&ZjR2 z6g-u_6kFTx!Iujho-`*M1PZ-eJtzzyXODdSKTqU-{>O;2xxJIQC))IaBje({E`<_O zhpj4u#Cr#dia*OQx5PH&Ok(3$!Da>at`~G09$)K;b4ORxUh8l(N_H$D6t9THwsJIcH;)mD}C`IQeK9TVmLhTgQ~K3&#T}?Lriz<-tAkLZ1r#9?T~#}<&AoP47X1M?VsxNb1+%?3K=&{!QEe>TW8dm|febWvoB zP*A+Wq37{DL1mRvapc%@>5lv#wjXH#VbBO^I$uu#R7=V;y04=$2g9UbD7dC+$)xU~C;B1B}fQY(x|R*g$Ru6H>>REHRW zK<5WcMCxwFGd}W-XPby<&(7q|FDr5Rb_7pY@F1-~jeQqCHL;K!c|F%TE+r)>uOb9{ zfliOZhHe%ZKsd~4H!(ZOM#fYDoZ&}uY3_$k=a`?lxrM)6gQ;S2r!Xuuo4zO1^wLmU zv(G6+&b76XusNX<@pw*5FBCvGF<-o%T3ifoYI6p=kJHt)Fm`k|^M}2&PzOZb+;8ir zN?FEA;VCpHC@iq;AIG6}^G|^8X93qFyWD7}qS0GWE=J8zg3!duN&B!7Wj~8QtV==n zfI0lD|FoetOc8pgbNg;1d+Q>1f1eNt2uyvvJj5m-NKA?nB$uw$`DuZSHn92I*Suq8 zy`k^qZIOJ1Jp77s&|E>b@KBWxCeh0P&)QNXj&|FIQqyljX$B01<6M%#IjS=3DAYLy z(;yU&6X2DZK*hA0yqJluTrAwp!y~x69KGGy-Bkn+EVL>GE651+wYb)v1^g&n)NJ2V zbA|JuE4dQ-cq`JDaZ~Jf^0vm7JNHt4;b@BL3V?w}L}ARSZ$8o_+qdK`f+$wa!AXpl zNanuv$GJS1;tUN9l|?7o+S&#$V0mL*Z-7k|na~f7^#d0U=4ozK6?PH=e6?=E)m8nLhUC=NJ#VC_swwEK=v>S0lKx~0E5|SHM?(?Z>Q_& zQlO^lmEru)=fEA+f!o3VJ43dAq(IFW&~Eff8QeU1(xi5A4H3`@7gtnIXR%C~bu|xz zdD+l_fY$6^Ua_sMtw4gIr>43LHPvC{d!`|I&FOsfq%YPP^qi~)g-lS0)q?pa#=FY9 z;h%14>xv=Z*uI!n6hgI7W-)^+ibeaA{H45wjgm(Q&=S8sE7&QgvJAH%+QNJ5aht8f zMRd5jnvq&)ju%ZHJ>K)ssFzbV>M2Zn_8Tjsk-`-&G{)Aw4b5bnyDO~GCfa?QJMq2; zUr3rm%K?mrr}Spe1g0vZMZD`F6a-1n0k$T`eC@J{v&`r-4<~kl17WrJo~u)T;Z7C3 zfxI7E5gCTBV(9JcL`6gvoM>Pwa7F!`n@B&PIvBG+SF*2{!pfJ6nVH!F9(MLeqiYI3 zB=%t^x;U9V zSY@tm{o0UJ(|HewoC<#*(PEuV;jJ}JNTs})0NRB24$z@*?2KhF{foC!@c)!=O;A$R>s>=VetnSl7|6-j^-B8C<|M!6fQTtj z0-s8B*4=Wd&aReD$h5~HQomWdTf+PC`Ej30<5!R^!8C2-_q4R3l9Fdp;$iw1$3`%s z=Y2EIuraFe7wv_38*pm8HL7z==BGBD9?jvGDNdbrm-;yo_tCq#m z&O9975~3&EKpuus;sw&iCMMKWwAOI2bVC_=v%jGQJ3G6w)>KL!&%e#de3zG7J}6is z$kjO4=)UEJEBrva`*DMlgz9W*S&%FF4#%7TjZF+Lw2th0H*lvkSEco=)d>;$0FkCwQ?gM ztIc^P`g36e3*pf+P|J@^(ioF7{l@5K0uKuv8%U=#D4FNJDEHKWOPYL-i`B6Gb51M# zUGtcuJ-LA3xO#b$*qN-x3!^`5b8zKx|3NlLilHAPP!mlx3 zBTuMQIbc4I1b#))cb%+s?%=WJd`bm#1w_eOKt^J#Uv4A^>PAIXkei#AmtO#+ZT@v|L~JwS9n)A!o*5NNjaW31ekpIYb+8A?&ZQ7 zF{3Pr^jWBcJ+U)&<4|F_b$c5d2FHc)(Jz$3%;&SQ)x4(LhZNtluefE}0O+^%Y*#eA z>~O91ouw9!Cx2Rdx~^zPc%TGlYgE%j8ts(`Dbh_TTjcVz@+2e z<+BO1;YfCyll#a21OfgYVTNjtjJdx2RF=(gU|EB}JM=5`N#i*Sadm37e)W8_4D|F$ z;~0>YWvp=9CG6_3w&7u*3?dP3syfr~xw{NN1OfsBx3{)h7u;4FF#wNoXDmw+Ca{i4DCNtd9qS+4tk6{+~tV-_MYOot_u~JHahz2tY3w@m{ zRrW*^&>@Sg2@vnSsX$J(;#zQ*bWBn2NPy$avI{Qu37Lr-8Ijv-Brb~m*gVNMi$pld z2^IYL!QvJxfP5q>=b3mfb=0@tz=QlECB@6B;^JyrSs}*3!cqW{os!bV#yWRMn4`6A zq_P6milHTc>twwqjM)_&EL+>=nTZKjS+cO& z%gew=TdS1P3oQ?D|BqwGtA>YB_##_g5P`r85}@mBR?C=s7mPf?}(VC#Gz62y`Vq;N+^ zM>8{OdzzGIdFs}wi>jfk*bCJ1All};lR{Vz&;(AB3tDMxauN}RZ;vvj_` z;2D*DhxPce&*=nfZy@%sxJn}EoYM#!NL8H|S^M)lVcs!E!E@v!Et5ASiz?{G2j4c% zPB!gBBms7z^i(Fv_nSbfrpVTFAoW7ES3}*7!m}U zUrRAV1HEolRuFE0$8jFJyP5iQ!4!}cCq8#Y2uOU^|4Ad zdb1N=j3Th~KgFQe!myAsnJMUR$o&;9;T_*Epq2>adUZ}$kLe%{7O}%QNKoEq`C{AJ z9bumt&U}#KJa*vXC4~hF%1%=}jy*wDL!>8Q6QuoV|FeF7$*c)9j5!)k%|1vUqG(_W zqlbf&(oJUN7pmdRnPEvy7PuY?Y)n54~;^Q&~24z;J>Z*k6@d@(Q&aRPx z0mf5=z0Oq-i|{&Me&QE%i9}J;6l8)>8bwlIZz(@)(re}B=T&I+ow0g%NxkSyp9FFy zrn_6xmC>^&k=V1WP-m`%zHXy*IJ~3}R%%XsI8B}w&VvDGBAD3MTxTdKwx=gkJT;e> z-Q6;Y`b|M*g-?+F^ZJP1e=`yp2@lFt&$+ofgK0N&&TQh3uQ4%w2Ex1T&W<8STG)Yy zcPCos4NGLLux1f5VrH@a^FD0-@Rd?0ZZ6%A zbqmZ5=y~PEizC8IzE8PHi#Q1bVagg=^OCex56{O}LR!&YmIjUke}-LP>{U7cD;?S_c^%yNL%=L>-mJ&^Esv;VS{E*F1w0e za;^p)OW5Z5&eYwwD1Qq*w6@)Bz4}hxW0@{jXlKC1PTF3_ZwFu z<WVaY~}^GB+JhQ;Uf7>D0_9O?IihAQ@(tr&UfBlIAt+`vOL9 zb)Qc8&p?c2Xc8JL#BhR5#ns*s-qq2Ov(gQ^pW*2`24b&bQ(TaeO^31@<`oyCCB?E- zIRq5x82Cro87jSM@Bqy>n3JkSluAFANxcVqde&d?9nMujAP^+&oQfz(3M7Q&OG9{j z*XxF&S;9b?6%EOLrG;8t{=@(>YR<0Dbr?1-3ifBH#hNW)>VF%}y${h4puAyWXlkks z^UKIiIa>hCxwa6U##r!>Q|{TuiIWqh9J#nIo69Yc!T?fnAQQTN%lFGe^tMB)TJn>& zmGR+Muh3_J9(i(Nd&^nxV;P12E-AcEMbcC!b}a7G%(PIO@-k8VlM;(vceRB&?k9`2 ziULN?T1G5&W^qJmj`m=>o0GSlEdYD5MibW_xvG2M5gcDi#)z@ZM=5_MnqrT_qW$e} z7fk{g!$1$K6nHq&rLUd>W#V#GWA~%MQt#i>iCDNDf5G5Omtt3JWX&h)vLT;M|}oz5X(Z)n;Sr7k$X6_{qa+5*bP!{ zzS)Zk3Eb;7d{8&+eEXh4YGaS~j>K*6WNuxg%LbjO6kl#Y7>;lOWDzAI{yRJpQdMPD zdK~w7cTkVR=0{55UT_Si%3Rq3fsAF%K!gQc=ygx=!?}8O zqx112HVjb(d**I9JdwXJJnuuIKd)nBqPjt>gqg7E3Wd-arP^I;nQeyC>5L9{_A!Me zyk#5PA$imLg5uSa;*h6`V8^;&!%VELKk&%j_xtl8`={PhY(6&MT-=)XL1F+_&NiMT zItn3_KkB?v-7JF@U(vZ1B~9RJk8GbJE;1^e=Qwn^?_Fo?5m8wkG=86~JH>5gDP-r~ zMd{t^P0i!B0HvF2EKY_sw#e`e1;lklYzJ=V7cFpz$d{#CuRIJmFfGHG+1MKD>+y=K z7@JOn$O6focyyApBYMj0&}2@_VACW;DzgtK=GaMDl!sJ2#VRl3+12~(YYJw8u-B1= z4ZK4wkmUu!W}x0kO1c8%RslaLEHKdJ?wTb~h?HjnJ&#nrJ=oN}T#P1#)gub)AgF6tPG1)i~-5`BAMMqPEaCYDjS9SW77+w!2|xU#>!k`k~^Akp~bI$ z(P==9n23ENgYp-LGk~n%r6~*p`h{{T%v;OI#NlFr8+MM3g|${eT?af72S-X!u)2C( zd{hCAw;|IbynTYoMdm68m1Zhaym(M=1?4GfFP6l4;jG(zPbRJ;EgD{t z`~tpAG*tR)ssg%ZA>f|SeN)rL~COv82s4GBtn8J7A1*&bF^Of zcr4ig4|nN5Ek;{>&nyfT0Nf81QM)Crqej_7525q6y0~4FjVRa)kcWW!8KLlvk zPfRGms2S?l5%bSB2Jr1p$GfE6&PW%jx*Jontct5nz3(z=yky{#PGkq$H2a;C`a5$N zYwQe5Y{uBs0mE4`4Yr4_#mUL}iFdKdZF7Cy!1(jKy3fPzWlw_1v&cJe1{27d&GYtL z3sBjtM1I7^X2itRJlPNFuUC2?khA7L%P|x$sV;Y$=HLKZ;)Rby;na+kw2YU3Nqf@I zn+xUm5{l|TZ0(ke^>yq^PJ3fYataF7;zho~w}R-YL;jk%B_&2EoL#9hk7kC#Q@F|& z^!qIDQ3<)5j)HOU@$ilc_V&!p%pf+A`%jGx6j?H`DK-iwQ)wC14W!>w7}A7FNphnR z_V)F4vkmk%goNyeK>}zZ(}vFWYTHd}f{`0^wrM1#tmc~sxKN1qch!;hBDqCAQKL`u%#@^kU(#hEb5luMB=xPC@8MF3LFhLcVMa% zP&Ng)bvY$DRxL|g>u~L#R&(p^Lq17z(&o~ziqL5tyj)5nDvJ8w?&CYa3_;RK&t$A7 zAiUZZaaC3q}}Wa3MCn2T3U#Gu@waDA=rIq z_>qxOs%R;j1blX?^&8cL{lC}-p6^twqEYH1Gs56zCWj7(Gn_>!jU2`5=Qjp`d}0Xd z_Vd)~5bufM(FWK3nOK^?>Lg1ykKvgw9YGO6T}=&d+Su5FQC0+%e zg_L#uipqR~^PA3bb~h|WpDpc!(bkwX@U{+udS`-0ShvMN+wpieh*n~~^C!0%=flBv z4g;@2g9s2&~ny!%qWwFuIQkjd3E^%3kCfa1YXdB^A(y;Z82{`g|(cDuDd1#R%QXZ4VA zX{n#YP3WP&#SUlY3x^p#M2Af5ry5dM9CN z{^4IE)Ibw;-v66;7C57J5}rWg;#9+=2RmNrgIuyEoIVfil>A=aH=jWHs}_a2FI0Ih zq}JDB@NSA7c&!eDPs0=A4esmDJ_{nbb08-j8|Dt3AnRIy`u$CM)T`iCt= zMZ1AgF)Avm%caYu&=cpkzB#|2g)_x-%!t@;S-QR)z!F5Cqh~a&_^oe}hMG_azNjl3 z4pN}d2@LjQbRu?)F_E&M_M{&*!;YZ>i^Yg~JCIJ++F?;$Mdi2c0wP*`LIM!8ZvHi8 zf0)gr9;3yUWP5)IbI~nbz#`^IM%BHwUHR3g@V0;QHPFP=z0qyD8otA7H4kiUgZT0!J=P{}O2g3cMsGP6OjNS_ewPa*uj~AI~;^N}M zq|HV7+-@$FzJD}kfQj*0P`{(X#EFvsG*|908(hgec6w7*)@8mR zBuvuGt-C7R0f~v4apx$`)WP8lDAspici4mFQp7hi81|J#1e(>C}b z^1{7IagDmm)E~9FfT1BJg=%NUakkdl*c&X&l11xr#r*61{5$~zqsv@`uRxb9)oj|Y zI#KcaM*6g(NwltCUk}11!nLBt{TU(uGd5MZ@ zr8ZxdblXhi1oS2c@ODT!mr|eV5x^coeBWYcvG|M)0S*_CGsMKj0j>1e)2Dz* zcnUCZO&?Q0fqY(Le&VP3RH3V372jk(K3GLq&h`6cmN?3HMQ`hD{Aa4})MQ={Gbx>$ zkQb{g@kEKv_4n@q5JJBgXI;`OMKv$-0@AGfxVaoAeo+zOxUwF$Z9Be{yn}rLCv4|~rDuFDekiyW*ufNpX(#?-<7k{-W#}nIB41P=#K0+v$E6n)nWF1@v ziY@|P*L<@_f+WQ%&WeDDh~E|UY3TJq%;{j!Pv;3|-O}^Q$B~UQ!NTJcJuY>o3yUSu z0TLfp$Z1Ql&; z!iBd7ZZM(7iqMI@ERhJ|4yv=u3S+f-o=X2e=n1gUX1BlLoVPl&nvtx00w|xH_{Yar zJ8CE;#>8Z&9#;dT#H#aOAhcW`la5}W_l>+iQg_bLMIDF8T|d z<%5auLx4RCY6%94noPVun~rbmrRdQ{`D%!@iltdJyYo`%CD({ROUWzTy@_ak^hGL8 zi#TxKK*{Q5N#D`k-PhYI4R9?xV|ll?w>4Ia2f}~$Oc`JR*%cL;Z6L%+@)U3kXeKJF zI^4$>QR4)TT7EWYagA((Wo7;9%W@e|$k~f$5nFgr;j^%ywW|Q{SNbu>o0=1_dtP1z zw+(A&WM>;*5aH-r2V#Sk5|C=x=#|xhFx&M+;d^#9TA38?_0-O(4o-Z0{L%Q9F*AUB z0?VvO{wk;?YjP*DOQy8|{O0SoM;(FRzkjDuF6r#11a+?Q$;k|!3*$FjYV2DuTu1oI zaX(ZUdATEpW&uam9JuS>vGDek9s>KVQIzA;nf;6~P4Qb2>Ud3f<`D0{>$%AN_X4D~ zs9dSHr?I!A!kyLTVkJ~>>+bjq*!;;6ma_FNH%%{__>}D#(7^8|DJT!(*eO5+o_#y_;&lfPxYZi>gk&ZCoGTa>cd0 z2hNdk-++3v@HW9LOqFa{j80A1zWtG>VyKCw8YZ{!L*(fxhe)D(UEGY#x?qtND^){t zRR-lYH4=zwn?=LR-4s&4MwW-CXag#MO#56&e?Lg0rq=7~uMvm8afrMwg>%h7-Nk@X zo0^iOr8zh>xigbO5+57?NdQuKIw!B5gH(I40d!sh=g`~R{4}*JkiGhFs?Ac`0`#^? zKyFF@|BKVnD-s@ZA|xj-0ST_d0EC39epvRtI_HGv!J6+aqk8OXVY7tnmC?;1+{)xZ zk5FOtS0{AX*B0~r{BFWsg(+0;xgo4{#ZKX*{ey#!;QEY}m6e&fvaHPI>f~+1_a8oh zez?8+8h_yY!^*KXmq%T{xd$X`$wTc~#};n1$4+2axs#%iMdDCSA*V&oa3)hcCYPa9 z3`-Wn7`S(XscE1~iKeGhEo5ezBgIntE-2 z9a3FPR0awz=CkJ;`|6HSdQjuavCW|(3swx8-djSxl%j%yZsi-(oFVsyKVSo=&4c+4 z>>hWtREfS`YelFi10(hGCc-^LNX2_RwHcgW^egdy`^uIYKwk9cEUq1y;D;!ytR^WQ zr`3h7zg_^~ob9;RLBtc+>!p`gyTaKL$h7nC4Xj1mVYoaVh9N!^p`#j~soKaOdLf~| ze_N17#XKWIhf$&tZ-}_Lxyj4RqiO=C6a9!$$K<54j7wGX5{XN4Xe85*qBZvmQyx;0 zA*8KS9;=AvY*JII!JJ%9kM0vO{w~ZxYAO3yIFWTWtB8GVlLkd&-@oqI9TxDiRJ`V0 zfB-Ob!}*@9f)YtrEF-Lru$tZ6BO<2DAbW9U29MCr>r4&Vks~iC{<=p#hg~x27l*f! zk|HwNv!eZQaOytZa!}fhii|o2yG`2iU{Dh@)U_vz2Ea3eiyWdCuXEF~qzXHWbInXO zpQ3lO{hh*Xy+HhXx>~R6F?F*2iT5pNvj`vXGyx@L!S=CKN|o2;dl1LME~-dJUp;$Y z{bMW~`a!HMXD9?k?bL5aW3^6R(J-Vug+p2jI@4ASu;Zc?BCu!>g@MSz?0w}YoCWH^ z9DVXCy&4g62G@m>xUKA4=O~4rFcd)sEtjh8L6vLs%Sn@MvE8w?vt%xpU$)-XW)Rx) z%zG8IZ3heFtZ>%vk`(^hjmD*O0igO0ishXqo5e7v!`v}PG&k%fp|np#Jg${G87vLt zIMLA?1(-y1OBh+z+E!@OXUu#MvafLQAHVPB9_pkfz z9X$<=S=RQ>lZqJH#H>oKgLir-p?C3tH)AQa?|5xhTt`*rJR-htXlSXrB%@x-hvCYg zWaSJQp;9JBCL5D9d}JXdeYB6{KANH!k!SQgUjR8@LajC)(+3~x`TF@j#IT^>VEy*~!nJtF;WPDQfztMASV|ce?@Z zE04g7jjvsVuL9KR5RfiJ;Rp#{_F59XJY_K>Bg^=Ru#SL@jSW)N_o5|Cs|0**1U&t8 z-Xhc=$sfG=1{>mK$HrI&{WV+J{?-%z&n~quX^ggg?{9ylXCb8y@#jwmgsbuK@p+pu zhhdXd9np=uH3rP+wi{4B;I|)`+{9Y-LmdwK;_BvM-- zK<4`c=)OKX^LR+)90c_vVlL;@^z>hlq{Yt>?UrY5H!LZ8ie4-Ew^L$tu1Rap##LGE zF0+JPUeT^Z zhrXbIm(d+&!_iV&*!a~iJ0>k6V%e5V(E-)c@Ef0^Jq%_gB4mBa)m~W{%UbQ&Ap)ub zux%!zW*fMo%yd&@{l*b@7CQ#by56Y{EM=vi@lTNaL2Jd5`h%P@-2O_hEtZ`!oX6IS z%^^Nl*Xwga2z!W%5f%&Tz$wmOBLt;L5LkGrFkcgor_Y+eq7zyu|7Z=eJ1dUbe#qd8 z7@L}D+fE#ro7>ypx2PW(r}adJ6sfgIPah3T;a+*XH7o3Xf=X`wrNwYQ?mCb(N(mK0L0d0 zBC%%x>z1nG=`ewufUq7@LY$;4B^%ph=YW*3RMJm9PmkfTiDyr_WV?2zU4=|c=-H;H zG4Ug=2>6^5j?M)>ZmBxL+lL2%3MQDjXg;G~-`qTOw9z5vBc`J>ggmN`y3gzeHF3p_ zg1~Vb;VLr(``67pe@;%YZlKYvMLEPBm%nB}SXg*#@Cvp)3UH+p6B7V?0ysOsq1f8m zf`k;jV?#qj;P;ULa?k7j7EtT}r=myS7K!j83Uph+U0Us=I*3G1xaljbF@NXS7}5k~ zJ2rqSwjJ-~j^*l<-bFu6gtQ8N!w^KDhL}|*{Z2t-$`N~hGPsTtkwm3KO}AOAaQd@+ zfn(qa#YpBRzW5I%#5yBJN*QY!( zt|m*ZqX)GG4MeD$h6eDcTQ@tY-Dxr@?4jNwQ0lO&8=Ab_m+GJCK*z=AU`n_0zf8gn z$T(g+pI>PPWLQ9=0|n%ms3=S$FgzAh08MWP)KX6St?wj)P&SbL zi}K=fyTYtm?)+lnxLnEu0s`vmuK{8X0D8a+7~bdB|1&rGcA__=5dy@aWJ}*?0UB`fVP-h!s- zb%n%z(D@234~mK`z&)ggn0YnIlj3Z;_Z_6-5_#1ZjI9|hp0dw&D zsXurjmCoVLdxEkk;(QFfdNs;Cc{rfCF13ES^sZ9|Sms+`^(RM1#>U2gVY;@t+3x-4 zmG#^1^faqx-WZ^Uju9b5fIq>(F%2j=09*!ESyoI|U0d18_e)qrab$MTj}REwvoaUm z(;q*6Oq6IJZf@dWV>=woaadSb=;$o>*n^HCVWTWhZsN2qCPqdW43-UGsSY~|pxx^C z@86(LiaJ_NTRVIrIWp1~tUKu8T3T8H;7Q=%G7e>xwE>Y?Q**vyc={MyO0=9>YogK` zd-LMOOnUy#jzlbdiI!PfYUW&U8r1fAkT`Wdev!q&VmL6Ej*?VJiB?N>#iFFSlHWa7 zYZN~}xEs6V)TV{)ae(`}6+Ab!+GnY!$j>$A=2Q< zrPruE0{>7~SEqs!dWZ?;{vTf3Q+&)b<%v{1t}M^1@(-lq4F8lj2I>G{T>v1@@7p&5 zUeCG3MPd)T-H9Twq~XN8py5#`{OwjI`V#HCch+`xQ9piw(wqk{_7*g+u2jJE5&<=H zz+7UZq|9-Ovkh;|vpDW8{FrNOzCF76f>BUCA|cjkWsM3%N70JoYirmlh}r$ZG0h=C z@%35b*Wfmj*WDG)EBvqYDy_YkPX z@yR)8z@c+~SQwutoP1wRDr^w_m}m|Wx;2^{5m9FMH$s-r1>Fv<|G9w2-f-88$n)qK z_$`f%T=u;0>FDl(?V;f@KP4u*@JaEycL60GMn*<3t2PCLejo#PZ?~~&n0%6|O(4-V z^q{F4+6?)e1+a18KY<|toOUZWe@O})9GuC?Nw0)cO&!cDXUp}Z$ZJkjlKn>5(TY$B&@(MP2(g+A0ZtlR;I`LvR?RhXfk{2oiHThE z%Cc#FBdeB9c$!%P!o#^bpL3?>LFEQ~ToW^^@S-1esc2J-4&HSjqdwSL+pDyrw5zi@ zH3Z%Xocw9w;(VV|JL?_H-oG#X+FUbJ3STtXHAqL!T-n%2pzV(e#J`R%%L*DGcEpYc z3#%&|8#^1#53OyN-w@pYO{_PAajge8&cOp0^*3laiEq#;*0Fg5qbE^r4j0YI!-Q1$5Zs-+$ln%-{*8ec}N| z37jn`H4z@4I4;&$flNPlZ@y)5cJktv?@>o)=16(D)rV^^f$`@#(*C1+j2y}{ZwGR6 z;CTS3v$XUG{QVYHSU*a_x*j+xf$Ic2fGpzU)F)&2@IPN<%eZoI>8t5OVsE+9^LrL3 zO_jwMvM)>8lkt>zH(qG16EqqTOk4oxkS@3gOy7#>;J-h(i16nRvML*bi`}@mXa%NqM<=H^hE?4>5VwjzO(dt+--iZ6^q7&|D@1n2x;oz29GU`Kjk`!(B^`y9;JlbmJ zw>%7>vv}HH;HrERn!Nk;@3O-^V1cjfGIg{GoB|-j4S+}k9EJ~(Wkda?HqbvHEbtXmDBT^$kJ$y#^gK4Z#eiylGj!J&{PxR+V5S+opU9WF| z+u{OLQxZYHrHn-{)V;EAWFjHoBO>a+M}lI1h|iyo{2KWxAADp}m1Aa2^HgSom2{hmlKuj`Pu zF8**l7I83V{Pt~DQxl<^JE`X>Sf<-|K=DHIY3j|(e_fW~E2Du`W0)En0CfQR^}vfqkc*jPp5-K?px$mg5)a3eA)__PnN zo=89i`TzVXBp|25n_X7M@ZrOEfB($}i4A~$L2Fkj!!Ql0ktq&r;N z50-Q6V6zH(?1Qe$|BtAzj*0^P{svJ|DV1&&0cmOJ5)df?>5^_x89*9Akw&_tOF%%R zyFqdW=@zM>C8XoM?Du!hyMOH2J!cQHGtYCMdp~vA+1Zo$?0$`mklejX!D-wEml+no zKk>nzB?v?9T%62aIB!g-!y?MriIhj|>>$wtmB=cu2COahn(qjO4{d#YJvKHLU(kaP zF-GQi>mxKbcZrE%dRtyyt+7DTC@UjjS^2`!Qe>B@EeTP_4oDQgY7ZTrAr0!?+U*ti zW1ho{{EC&Fk}@eK=40U zpTfr4OUGhSQCMs$ms$5AoD?XaR=*w&1$_DPB`Qh{hEga(6~fk8)zwEp-r|t3#{Wym z%d;{ze$W$Ao0VmHaefv;4~!2oRV+OAX(;U0Em;?qx9l?OiA^VvVmwoOv)D}xz&`%) zz!DM`V@pUh_xH0iFyv%sUz_CymX{X&!o9JsnI$+{d-l1R)9*!G>ibEFiEsrR9UTV; z2SI?5l$?Av*NTlGll{bnKrztt0#!Ts>lYkAR9?_xdX+=E`cqliOhQ@EORO>%iD~hL z9?LOA4HN)8gk}*8d?mu@oI<%p+12UDvK2cttnx|&jx?HMv`C-(dYX5dGU+)MG$TF$ za5OR@l5}3Ycx7cJVEXg+<%b{g`SS}cEi&ie$xmcw=>uB@0~-iuXFi>Xii&+$)$AsS zpU8QGqP*3zF4umhjtLfE8r5qRUxyCSZ4V7g+NnTunw9k-Lhse&4 ziz?xq?q$OTO)9iSF#1`Vtbqqm=W7HsiT%k!q)B(oFg%TI92^|>9Q7CnT3}d&z6fgt z6&Xy*5f&WWdIb$!Hm6JZq@9G#_V)Hl7B&jxkmrkWhJSlJZ?1_=48v)ckdUx+@b=SN zlD)!zKh5y0>EJ&4UXsr6dW^K2&iKg}Wx8%)Y*`l?X znK&x76xEwiFT4jCDB%}J5V0{cOLp=wPs5)B#2UP`?d|PAOE&BvZEB=5FU%q%BJL0n z5a?LJxB&{t)$4-kY3%9ej0_AJw3|*Yv$hW5{l5!fIJ7W&-yqw03LC_UNk|~5-OnK| zbCj3O(wpHWPesMSEARh%ftwC)TtPcv;|PM?kAa!FTOiq#;o(EXT+1~N-7~CdD>3hL z4`@#Ya}+6gEN{n&wBb@cc>y9bXdg$xbVkXK>UdkKTGopcy8uC?2kgr3@F3zjUYJl$ z5r$2iC;vtb1r&-x^TWwFB~?{-;DLt%p!Lt!_X^xInL^kHUSv>Tla+OKRIW@|X9aFZ z35MI@-#O^q!6%kB@JWUHilr^v^r{s`-(xs2OiXz|{QTqDf#WpOWq0qyJSszejq>Z^ zY;OsW9*(O+a`e&y@nKbcue&r?ugGx4G%HA9sCdFhimx;RS z43M3}b0F%r_Z5by{R0+41_g18SXeRrrx@$xg`M37X!W3nWKjANpo9V|Bs}}uprd~K zR#j7zFiFJyob?VEZy4lBwOQC5hg8Sy(#j`?EY}j%mpWv6va++|SjRz^0{x=f-Y2H; zZp!)}&p2#!+&DA-Z=Wqj0|$(lLOc|Ou!?qk+!gvkt@D6zV(n%baT7KK0aDsl+pj z0h0L?A3x@cTtcRDJbH8pTjwADnuO1sf|An0!U8_oz54QU-dIr+5!s$J15kZ+b$4g1 z!pko%W@_3A#R+h-rhJpGovJtU-|ZxV+4QO&Gc(_sWYZ!T;0JggI61M4Q|^%)92^d> zUfqj%o_z;?CVQr6@xo82LV|)dQMbk6XT1B<76`J-kUp3-mgnZk`E1EDJFqL~E{?q&5&MyMXm{^||A|-J&~LN8FwEB+|~Li{HVKl$|H+ksRC1 z7wX>cJtwaY5nsD{EoXaocWbZX!_VBl0ZXk@V%$EIr#2w)v!`bb zfJzXkp#O@>ex6m(8*<<7Ex^f{P32}c+8@g`o=-tutbJ_zKpGj9!m+44tbaOWWpX&9 zPr|^0C@Y7Z+E6mdb8+#1QkUtrJDm%}2>cVFc=1082%wPkP~4~yYKf7N896x}o>?b@ zahN3@IK+*=GDWG>L`Rvy%gEv2B74@}W;3aY(r|(!YLUIQsjmF-s> z9}%8rq|U{i+&au@{9st){d@GePce$xqR!N2DOr2y{MZonP79Hr#^ z^z=7Ny$R3;D!KIr)f}InolpxohrnEmN|QJgE#UTI1zQ z=+^d^Q@xGrrEA}e|1g8bb2V$0GOceG3K1$@$~&X8R>~ijlZ#8T&)8GE2W~5^n3qRy zJg1h^9*ONtU3Gdo=9(D0Ps)=w5BTb@^S{RAadUa0{D|n5$)pPHiwnr!A!XC~V0Z7h z2Bwt5!*?E+?HmD0<#EopxmPl-N?6s&CdR&DLG0}AAqex?8^8O4jvl!62P}mPu%PR8 zwKNr9&rqe%2Ooz1ds=CiNV~vB?XE6Br$V=t(82!cV^8dJR=uhhzyv;id^>54wlS>1 zAHNiSN8xvW)1dDH__?K*+(@2;k(tFXE>7$Lo1KVvLrp-y6WZAaUNkIoa_?JAws1n9 zlaO@>q^I7*SFRkwbDYh~&30N{kXt`Uq*~xDOX)4BQP@;tf3uix4A!<+pDk+eEVl;?rOA3Z*xGyC02%+<12vhk}A) zn|EA1P9{~$0?NIOuS8W#3+SW|1prsTKL+s4P1oWY z6Ym*q{xb|T`fih6b9{Vyumw~fjFQq*HBQ^V7D1*tT09#N7AWU+%p=5@o0H?unV%+> zCK??ZE2=2xdb)74c85=-zM+A}FbZ$)din*g6^G&7Nh5ZsDaz!H3Ck4Lu}?B7u6w?N z#=B|fOfvN=Kxtlmvh`8tUg<7W`UWr!2(oWV9Jb2?;z}*zbvm{^4-yh!Ogtjv03V1dv)V$1>Y@=N6}uVmX{;rZ)})cEB%TJL9g-&pL${t zBasuEr-ll|@e}~h1OIohHS4{>qF4QP5&i{juB)l(((>UZyi@Ve0KyX;92E5Z4LN#b zu|1d+D*RNqkIdvhIKngnZ?jxn+@=9nJAq;g4T4PE;n9(wpI^z^@2aXN@Zxne05v0h zg15mb<3w>*foB(;k}@NHe3cptUHbUs_}EyP>*?r_WhGdZUvb=bTF?=* zB1#?mGgH%|H~!7}scT0E_+=*K=&1(QtG1b2msXLx1`4ZreEB zO6>rddhk?fKRe?AGcqQchUt-BY>db~`fPsp{kxc=BF#*)A3CKu+fjKBGG~iM55AY^ z*x84L4O6|9K|s#3tze?s(Fd@H&m;}ftJv7sXpMaY{tXm=f$eQWxXHI`L)*!jpJjo9 z12>j(E+{=>8)WO9SXUrvNGW*_aWb+oNfN8k=4C9cIoo6&wUH^ittVst@b1b>l+|EN zK>)Rk*~h%`#U4#JPG6dTyXR+GHc5(b*7fHWH!kN9y~2Z!A89_}XDXuj^}=oy1vPqO z)O>GNpSxRg9Z)^j?FSoefl(FC`|0{%=dj{g41ch$sm~ z7i47o0nYM&cO~C4n+ADb|MZfla>hlds}IMrIVX(<3RmwPWFcMOcywuAbJ4sX6j4K0zaD?B@pAnCKojsR%ti=c zk$)U<-Bp_&_|pBY0WNv6kJ7c|kzWJt^m-x2#uZ$1U4-nNk#VuH8x!@P#%^ffzoR<- zyG_@CKZ;^pNb9_>_cn+HSVBgnl>BesS~ShCC5sp9H`;@GXbfLNZ+dFBk)v&Rm853% z$tG>uC5>{MSkW%|_vaUXh8Dp2w(gt^B8ASm)92C|j1 z1a&i$#Kw}gw2Bdv9@v{=$bS_hPe>5@@+Hv5lzo*VG1u3(>h!0|qv(Z&T$@qtyOT%9 z85Xa|obOXLJ{R1Ro2m2`E4jsNskt>X-(rvE!M4 zZg1Bgdfv>xOH5suUnBu;BbGkRTv6}-#dd*XJvt zg()RQJu2;|6NOyt!+$NOj$OS3JfRqCmBw>ubX@yJ!beou&>(vMesf!!=^lFE!Gj0) z@82J5GD1xHT>GK55VS?kV#!HN42t3*1H{dP+bwx&xq#f*z*zh%ub`mk=TG3k9@Bt& z;_oXF5fRoaGL`JI;WTE#nFMSmHa5ZS-Bsmj8`39^zp~^)h{23t6e#d(Yz#d=@3;SW zpE;Jx!qhY^4rKrh*Ky4G2dTZ)u@V!Pt(nlGL-GQ+ftIo|3ERuk6-7I=`)apN(ErL@{E}OMZqvKc-=@d9h!M$s^DGb$D5d|Mv~S zH`HtiFfY6nz^HJ5R0?}N3nN}X*-(e>Rc&sEXY%#;Oi@-rK}@%9#hIFZYil>Vc2sdg zG2k{uDBee}?1yf@tQVh~&>=L;NbA+hRg{-ClRXXfC%QL$+U%XKp>lmejt1g@qRmZL zSJ$bl5!q&E#NwIUOM|&gYy3K3Yh?dgiLYB(uc+KPYBArJRH;ijJN{h9TkU<|(TFPCa`?p|ay$&Y%5OQv^-FIYUWH7@U$!92rg@(4%a0e6ya3VVZ zo>EX$EIB2O`24xb^MucY*7@#|&k}|c5^7_$d=*BbALd^3sHHsg?cw`m+nbm6Upef; z-8a%v!1Fy{~DhG4ii~AN50#*%*YfB>4nJ-Uco%TYWYx~bs z69#qfaEGsxjGJ+o*Ej!XXbm(<6=#_)w~33n(OB?1HSvmC@={-5jCO2Jnomz!3E6RA z#vchLrjO*31y>mjNhk(mByjM<%XuEFYKSx1|GCrzgMF%YpbeY<%yMz#Q)vL z#sXB#x{zZ}h&Misu$GqAA@Ja1 zA;VfeUWb2;mM|u|x&qBGLcQ+Zxzk+t0JOdc%0M=`OtDYh{rzE_1G~UeR@#oMs;T)| z_krT2#~a>vs7gV@#-!qeCrF=~O3xGLC4)OymP@N+2V-IYZsNg-yK(E5q|1#qysH>K z=W0`u+kFiB%=vkCCe$hLYC8^2R0qbn4%%-EP2ON;ofq4@ACr_}Fu>cxE$U*Pg*0L$ z8jdlqakx|C?KLAf=qft%iP=q5O~UXd0fFKD`X|vBqf#yNL|1dR;%>XMGVZEINMD~; zkB%<-*+t%)n9ax|qJ{QWKKw+_jGWG5_HG-DS{&<&-4Ao8|JV8UnS-_NBU{N+150}= z0oPTc<;!ou;!yc6VXb2MF^7-WMFVQMFFC0>{nqntSQw($-y{!Qn6yk{As1r>onPp= zvm>BjO`0af04yG9J$sHNlIZB? zsG^}Rb#fm<92$S5h*P^Ct;U2FQkQrdtBei_dQCMJ6`i0jQg5Z<1P4tWY|?2&q+?&o zs)Dr7I2a@^a>1xMursx=qent5;<=Za*poLoaOWOZKC})0>_XdXqN6RQ4FokbG~7lIEnMB|KE{mzZTJL?~1$w6SyK?y8n@Qg5+)AT77jr`c;7UY9G&DNecv&LC z=3%@6T4%2HlfzV12@vJAlAa)aNlBqO5TO-#y8s#sx9x>TQ$$oC&1z-%CO#=-J@mEN z51U&OEU+pN`~#z%@d~#u``Li9ObFR~HXjn1B-GH*)4HEB>HwE_3P#_7%6vmX|#0(iK`aMXMjYW_BIW!ootEg|{k=Vx#pB$*qF3QF&0$ z?F2nPA)pc`efY&&4qm1opxZEZ`bM?)WN&+((Sv$Z_}$^_e~IO7e1R|;G>`P3!+iRs zHYk0-f_|o-9Oibj4q3oAd0{fBLO-GFB zGY1V61L%UNo}dRlt#E5Zs}JM9d*^XH)Kgg;b&mK9e!{5`qquZ2qBm>KonNN(JXo%J zD>oaJgpK_iB^8ls#UVX`T){0(XXkx}aga%v{>r?|hS*PtZ4Bb8rPY zVz5c22DG*h7fy~(R^3ROonQOkam2nnj?=w>Xfx@Q9!MrN zOqbGSqo;4bkv3oqZ64bBV*~{q#JGhdBtykcNWC2(aq+FF$l1k3b~1o1u;Iw)SB?li5^P2AbMwAC*|*!me*=WV zQ|cxfn%@fx(aFiv!MGIE85_2C)Y9+hj$}k{Q&mg6^8InoyGb%Icjs-}UV=(dS2sof z-SXRs9@@4)qRQfTWBUXISp5@43qaQh`l`+CYl$wN|SVTu3S<6PFecxTfWk57rmy-c$Ss(WZ6_|Y5WL`@nn|}XV?D;|vGL4M_r@;T84s^veXV69O zR(@q`)s4}e5s6=bbr~BI^F0dnLMD=uh=>O`Y}P+*6UBLXZ=9XYd+nR?p!ONyhP}d1 z1L>Hlkx_HJaRx|CFVV`=ZS4J3Kv8@9`UdQ1cG(m?JcK7&L(-`1NGC`-e%s^L{8my> zc&VY$Q9;cHS?_z$!`nXO9l<}oZnpzbLNZ}gfoyyr`~h%XF-4eM_XMbP2)7KJmwm1F zTH(u+fzD3sO5E#fxVy@)G53la&ubi)mDY>11|mU?46HQ`2w|1?Tdqm z8&1wOlyXwKi*LCa>W@69r}2Rj>BQ4UBKNmOfc@vQp;!F-)xv5VjMB?LXUw2S`(pbc zvUq2?eLK4|FP?&ZZ*TqVloBn@%)r3+-X1_&pxaiQYi>Tc_0e~N)9~S)ZXt#QZ>?w) zM};I|%(Ww7(g3koGZ9)IW~=AVNlnelpPJhz@Op-h;gWW4wC%nkET4a?QfSD>RQX@5o>cbB_)t^sR>f< zbZpnnlYQd1afu-JKf=EHozm0xRn_Tgxy`x^+7UmEz@27=&&S@%s`}k_WK9l9$;Sp} z7R`j&TGFmLtfpp|E|zSnC|bjaA)<$km zkzKDeYMY9;>f*lxdo+t+5ldXL-VFjV_w&=30?om3VbsnVmgU_%uD@+^&W%T}*uSPi zM}QW~OL?E30>QWb*K09Ps&sN#G}}8wep8W~fE@Z}+JJWxxRU@u{gd=cRa<*yabEr# z3xt?#tn)h_8Q#3Cf<6!`GJ7RdrWQ_fo$D^y%EJTbJI(`B%)@BKXv({Oat_qwC?+x7 zzyDq^SZJj1>Ctj(i8n}5uU@}?moa`x)Q92EZE=@hvj?CNy@o^bSdbeu?{FY9u2mgP zngTl`Wk+4+o{jgZ^jWcu?!=A3*B|`-pFiu3)A>p3q}{MUy|!1)D#gL1I_TAUMcYb( zK(=`(=}PsRx+4N6Z%0Ea`h z^ZP22jkZKz((e6SbQ#fD^kedT+HRyuu9;uU}H8gY1hgj|cC*e(k|` zoT@@T+h+<23t#>iK~#hD6Cpy~rl8Qm>D^_HzfPh(U0r6CYoJvfFmQ`av^KGk{}yKR zTu$0)k$D#jTckgk02o%Kr)?W3R4Mq#SrqzECp`k=FJGuSJ6`50d@%HMK{@qj_rz7p z36Ni_r`}JhtaCd{?BS0{1~q@9NoT-@Nx|!}iqORn{%PPL-%j>0`LMGs5?%W8+u)%6 z{r)@w=`gZdPSSaHcF8#(|4RZdrVBZF9x_#Ss;1mMJQ(7mmk)5%XXh5$1a_rGs$DoD zdn#qL)%)7}k{8ufIrZt%YHy~wpU(X!wE{sXC1Bz~pdF)2UN8{526>({@3CsmM{F=K zUNp)*m3WDrv42VW@P4!>faqBUh4}C9w*SP(ldapQH#B$wi70X_4b&y%)AKZ+V2bu0 z`O_7}n5&q?_xVyW39mQkPAqT@WP~o+RZB(_9NaWyONiTBV5i*C`NCX)4Za5G&vNbi zwa=h#$y5Y!cdJWwDdBXNmX40G>w$8r3u#4XbWBXbXpaw08fXb@%*+6CC+?a7I=e*Z z7JRQdDz2_a0OJoJOHwlF*;rXop@W(;1JDX`cvxER_CNSYI`T3iugdSXpm2U(o_0}U z(!O}1aFq1k7m9+#xgUt}yGBxq?+eZ)$yGd#2;S||?lguC>(r_m%^JAGA~)w%U%Wu& zs~7aAh^)=eo0yw-nEihClpzX+E<1K+M9e{4uE)jbjAxVdn+;E_#%6&cJ~( z8wH*>Y&>}nZalcN#K5#kV;_YD#XwcP)o6-f-@% zyqRYppxi(Dd>>>VB!L4x`8LH_#Onb)w|WQ(ry6rXQ|fs_U@1o`u9omE-F5Ty<1f|i zQr)cVdY#&S|5_4VQ=GIFtZC0q{FUX|SHZDi{U!6A*$G&2FU00hb957Gi0Xfj)fEN2 z{+O1uRm-l%TeTI>{^d+G>MfLLQpnh-Fu^`hPW$jDMa+GykT}74vY`IJVq*hMG&hUEBmj6AwXe zN(=4euBoYcjc>WS2fBR)vi9fmm9t*E z2L~56$ZQ4ypB#5d>s*jp^~ACf$Gh@@9t;GWOAzHc$2D}{pwwIu5Xv!(#8|80{9zwV zEE!IJTyn|U{o+cK)X54vAt-jBA`YN_s;8q<1D0)grg)|}_=YNe{KyF9qY%g8Kcn?R zd{O2k=!l>aALaD>u@X{{pMR^>|plD9uoc*a)0FPPxW|=T4>)nGJAjo_X=y) zfe~Tmzxar zKF3v}Q0QC|W=GH>M}QOn>)!~^6x?P-1qHBQn+4Z@Wj-cKN>6Wa`kCt`H6cu;qlE=|itz!Du*b>|v#E)z3KT^} zdGyW;T=i95_KtOk@dwG}wV0TW%YDlubg!Jpy@E}iKc^cIa1!3%oX#yu?<#yB)Qrdk zXM%kEXZ}rBI=VFat0+YC)vMS0Q-tDbi@slnlV*TTpxUWyJNi+&QKAHyB4GO_`nIX^ zOVyblP6Z!2UaORQou8K~w1*pw4vnN&1oq`v=^_@)Qk)XiX(5%dt5EOn3}#tjGIga= zvmwF#vY6ZAanaFwRqs?`y0%Tj;xBxggc|;^j-z+!b&u zK+F))brj+{SU{e>eGg(S0)lYRYnH?eTKyUvge(tJq<$+sCdNY0%$p1x=BX63WVt?M z0a2d@dR|Ui+A4qlr(!{;+Y{xtKd3FLEr(2FiW{jpU#cxS#yJ^Yedm3?K4Lh)9h2pA z$4fJoQfV;lptZMGAz>B;^fuf8z#q2f!^Og=1R`gOrl+Urk?-zFKv0*tkWi*WqbO&U z$!+c%iER904qJcQa|u7~U6`RiSJU~spY5pm{I2|b)0dI3^OFLZH)5S^W2<3$^UnjW z3mIvrjnKUoJI-R-#aNBMAayip;F;#|#9({H@MmmuDTTqlPN2SO#9z$E+4?PpB5k5Y zf68!@c~fC%cVu=fr5*nYCD~i=r?Bs&YuX2csrZ&PnPJK^vzgOtq35n7P9H|T=`->@ zGxJqigGlcICyI9@q@8iogR7Hzdi16Z5TNINuod_CnMZ!Z`5c{WmfYJs0;%LmsISl8 zait_wS1$zmNJF+gm7s^#deiF7sOVU>%86>5xUkKao632tT)N=%G8uq{;dI=Co}Hf{ zJuE8fh@Fhj$E%%CeDF?KxUyEgze_%e;Ep_N+)decJdj;@xD&%-y;^tkM^Vc8fJrBX zlE~-_H7rX^Zjl2GMhmx0!ovS4t$)IJAO7Ag6*v;=$FF=YYbp^M^hpMF?;`BW#;(TN{#YQVmv&Deb~^C=*U|CO3#>$#gO$Ig|pY zoY!U~WIr29>+Vt?q3DP2VFLc;y<`?FECuj0OEK+1CAuOu-sCG?y9&1?yT? zMw`7wlK;SLHwrAz=xE%@a)IQY8@Oi^d2Mtm?fZ)4PoxWf4G(+nuP2~?_3j`rl9Q7u zJEkI4cp8_yY+l*A~^c@O^W_362AfklyFJM;2He;nYv0?SAUDrn9d#)H&@1 z2dqi`@wn(t9#7ymI2G6~nPs4p>H}piYz=!xHp+xqkJ-`LiA?2pY12&g=KDHp&DOr+ z>qX~}M3Z3&|S0&YZ5yJ~1PEW7UKk-CyE8p}{MijiNjWS&sV`6A@f zP^E=n9+SC(&I$L)xka7)rA?-wJKtK!*$osBy2U!}q$h4?UN3AFI;1^)AY$BdO<@kI zvCPV4L}$xTB_et1?=~k-Vza(xQ}35rms!!SqvBpn9y>u{;g)VUdYAUbilY3b1`~@+ z!st{c#dsZcjF!bFF4E%Lr4YFa~VxrkPrW!Txi?iP#9_xz>@K#EL;|O=4 zr@Kx0=mB0A=3Y=t!CgW^>Cp(uw{Lek6n|Vrm2r4a)$po&Q@BW1$T5^IPc_Ulp3Tu~ zkB=kXZ;PC%%4wSoukt>awY@ovjT%iN5m%%8mrMRdz&%z-TnNrq35KAwDe>jdCIc`K z*;gz#&>#Zd8O%_w`dQQ`XB3)Tnc_h_t!$z(qdL}OBuHf;z|aYek}xz(t#{2C?dj=p z*_apxWftH2m|viP1Wo$M>{*1xcAE?*ZKJ92t@4Riwn3WG$;E~A1i`A4vMGaBP$}qm_I_Fs6f)Kp)ZAARE3&h)+KNDc?gpmM zh&!lqg$~cTmXGkI--7Cl%}}std1b3EC*p&NWytd$SaMX~;kzsD#Xitbd5!|t=I9>!b4zE47zGZJj_d~x?9;^hu_6^_PbQ96toWz z5}2D;*oPPLFdBLbwsO6W_B+vHdzs8WrY_P7ZT?2;`QGLrR~PMW*ba300PJ< zD<6#2>*}kWwvpP5j<*G!ti~01=}JhL2lrLS+=yTAeN!+POf<9p&5L;h3;R$8k5YlZ zW^SJDNYa|s$9XPEEKzjZ(|%^5r-4yHm8(+g?&m^c^1GSrQQ=5Mtk#vO ziUF^_q(+~b*9i5hD*I*jd$0T_Cnv_U`L^AI{eN+DiF#qw4>%LrX(FhvC(9~xaFpzJ zya2vnVyQ-ipFk&`H-QzVQ=GaM2uCj{kr{G@L|M3L<0_S4YUbzW4h{<|GI~~3Yxxda z@__i|BrFJlk(ST6Ex@ERtR(8~3p}Xg+`PPYzpkP_B$855vDS2Tm|9qfc^slH*e1%x z9%q13L|9m_ip&lTWf<=udYVa41KdCeSp-CCf=m(oEhr=Sjy3f3sGJ7rX}{D|#-K`o zqG&+sf_)1@-|p(~6|SzXMn@AFwNii$MD|lMwX8-DZWTT*uBkU-Yer<^cIC&tThqHd z!p{Nh#p?{j!tmfB=H=x#AQqIixNa5`tr^^a0|VkfZBgW}th|YN7YA6JK2L}Dyxm>=*>-~t>p}x=kIl#=8v89+1RGjv4pQX)pCE;|NGm;1bv^) zfxv2+uPkZxvsVrHN;&WE(9?U4Jza>aZZ}Vs??bxM&5QEo4`Arn9`-bBE;UF!G_!f= z<7}@3ZNnlk^K+ER^@#DE@43p*@dLAAB=e7nArD+~|Gn9(q*u%n*G1eZ$vxOK5g*t0{@xaP z;*t>wW%8f5#fuAd*y&{)m(JzBDSVMbpm;k{7DLf9cY%}xroz0a3iA!YO&wd#6th30 zd$Q?6oYngcXV_K5Kcr<{RwF9I@kk{`k;68li4@{$Dx;{Wr9aGXt#~QK`32+t17t}r z`-0;tNT9pe?xdBD(qVcA%_6Pj*CquPRx092OLlD)6^Tk+cxM4D0Fyr%pY0e(-nnM> zD^}Q#p-ly;sB5KkXyyc%{b#3#W^(`y!;@NX>%+jfN9iWk$67o=9aqo7E+jpLngQ7( zG*3V(K%nEcuc1>}^fEHpzT))=>%0{+^K+2r$BVe{gFFkC=p0>XmRVd{f>4k(92u&S zGeoWV?25^8GWXh%hvV|QPyIsX|B{c^emiy;R*{E}sL;49{GF>~z<*{sU;yt@ab=0h zvFqMKI+iBxYPHGh11YH*8meDT%V|YD{(Bmfg0EVXGi~|pvj{{t=T{8kG^ypgr-|c; z)n44^((J;1DBv@~|M*MetMq*${mt;PRD&9i>Z&R&tw~@&j#OVV_xDf!O1vwsW=F|4 z5v16}$W-v|-MVojW!g-@zkKy6v#Sb3RW`_!(O-(oipy(z38iW|t8dosJeJ#J+8Q*z zxEe(5Cp`G{RBE56tgPNkYz$5R)V(=Qegqp;R5UJ(i7+?M>2cnUpe3G9D7BwCl|m$k ze>_%7H!=$DW2?f;%*ZZuM5B!Mak5aT?;nGEt9owCn4^ucNuJS((6#n?Vpr;5a)-nf zQQe%!Iz)NV)AMp&5u}qkjV6WV52Xqgj-z)UGfuo6-@#3(=6aaPfBT}=g>hFH%aMza zB)Hyf4GUdRhgp_b)1g2}STbLpS3ZcFJ18XH%5QWWlZE$&l;l5`qt5|gR3>Sn_Tu7A z;O)3PKOWodq%gW$tfhy6Qm5bpMq)C>J5Vv}R@g%Mw)nI?!ex>$KR*w-F&ir@uD=VR zx`ACBWU{{)fJVQVcA2fNtpzpfkDe$Pz|m`Km6ij4rb&2xCtjJFM(>0ltqIRb+&%+@ ztd7>r1n-^xI$5&onYxjY8Qwu?MeGq>5(ELlmozBb3!S9KH?0!rO<}+Vk?UiAe$A!# zNT=R|;Ju@-f?mDseJS@HYjsX@Eb~UTX1a^y@RK7e5XY@GxdRSoF4lN%mZHW{bH!>r zFVD`-%z7HF6UaaN`uSDbP4Y?0E>F(2iqWDjO1*;U8je+U<$J8wM!=Te(jvKut*EG< zYdBJ%_49672SLEiv;&)+5U!NUSIu$`TZ^1@m_j1-Y@Uww+goXCxxc?pw-u^2DID3=b}89nD8@C{ zpBx-D3_zyl^!o3nGxp~cdnqLQdrY%i!9VgRj`1iB8%cH*aqF@4s=M98aC0fC|L%RM ze0Bdo*0(Vw&T{Ey=_O?dl2&Yydwzx?-sT({jV{ zY5}Y$GqbQL9cQ5wA56Z0Vsjbn5-?C~*mxOZ>M)yPkN^T!hcgxB3q&P0@-lEgEH2APbh%ghw>6;sJQppgC{b{7 zamis%zBX{mZ ztjnx;7tj7q{wU=hwAn5nmwkGEnc?Z#sZ@d#Ci-6b`YN^~_H^eCvQeVlzVr-CZcjDl zB5#W;$7e14@Dwt{pr)ZI>9xM*!d78hHvMql{S0kOav{8_S)YR zsM*?nv&^P~#-QwBrTP^H+w#&%(y)NswZAZJ)(E#ES9WT>T4gJ?zI^ptU0UidRJ(V8 zq0{R^ycg`xOXW*LqPXR|k!C8s9cWpi(OBTel3>I>C_%kCFMgc+`O zCRKE4lz3@kM+;FTY&xfZTM1yQ&7Lu3b3`gnn;9A!f`D|SP*;t+#%1e#P+3edS* zI|fb`$jI;ZhbVk)-Pqg|wH^Cz4%unEKAWg?ztB)Tq2N7lg7-uq3(AvC8p1fUfP*u= z;aFfiDJPaO9Uc+_OCFfmSe-M=)DTz-XaD?p2UpTa+m*VTM-PWt=f z3|>J*Q;dw?o(_qLj=n*fVG&3fK=v}u1Id<1lZgzKX52d#yR2B)hB^+W%NS1ax@v6B?b?dY( zIHqR?JNJZYzjivF^urZxZ+$xOdi}GF4Axz?>nDXO{Hml74rAKAm%GjNlf76l<;(Ip zD+_C}Lhn_2S|ZOsJa@&#Zua%}Z;zGh%FCW|4ThTQHNNo^HCv5uig2L0DM5&Z98-BG#_9E+6{{a@5M&>PGMkPp`IvpRelO$_^q0`&e;!RLpb9 zwiRhPLJO1p@4!a&VFp?Ow=&5f&ceNcI=8|@BHgzb)OYV1+u!#J!(qnPrD|6)W_O)J zWSVuU%CEi)&^Tsay?7C4a^506HJpcDU7;0y8(nX^8XtA|kOvVlZoCOEOtLdo=CQKfE;W&0f4YkqVMf9)CtwlW+0$`-&?K1Z}bkw0eXE#)0)JO0-U)_sg zxqAf-N<7%!<+?t@wto=pMV^a%oMVVtA;u)&V^A@5>pzS7PtbjJil0^g`D3iBJE>8q z!^?{#zL`dk#AJJ_Yc>935kilgfyo9&Dlw5_^%krWgW233xL`J&GQivP%B_ci2U*dm z1G`Qi|9CqrWvzyO9W3cQ4l{Lmh4g2~|0ZEG$(XI?kKeyvO+-L7qiis3vteg(>%2vY=rcNU=aWVQH%9W(4S^6g4-EA zBAI0_NCBR*m&EGNhF8((_=JRk$d+`Yb>mAvF+M)tnyn?=0GMP+D&E`wMO%Vx_*Wj0 z02e~3wY3$%7Ap%#vjTTgjd6PrSdqTZan<6U`RV*yALr)h=ZB5q4NH0kBs^~%hx*e70Qh^EcsWYMjmci_TGAc~?Y-QQtE`Zudv~F1(?iSt zNxi2zCG5=16W&cx9F0lve8V%Cy)q1Ur`8_Xb%$0QR%X3!ez1Li>nXjN;F58w0U7Cm zM{=LrCbKGu706IqT0pz8qp!0|!U~`63OV_tYosxAPoc&OiyVo6zapu`i}mUSRYow} zT4i+YldlBAIcBp!CeP!f<*q6VEpD(aeq{Dde46}P$Q|3h{L0ev$A&+61Q}!yyi3&L zaF1@2J#$EcB`g458_!}!N2|x`YL^IzPHR7LnC#G(`O623>)MGfJ*lY6g6glsF!>K8 z1~3Gw(M>ameE5EU;zh0V2DlaAy99h9pB#Y&-DUb#!%30n$v&(Xd9UF8a!hIey$XQh zXR8IpI*L_-T>Qp}O!p4S(B`OW7>OhVzw+{4s7xJ=A-n0{%%Gn)Nmo3oFHw9WzrKLqv?~Xfxf#yAxe~a@WSa$$*HUaBRzdzGWVqY zPMU0KEkmM{n{Z`kT}jY>O}*5J6LAjmrSGUyszAe*^9C}dpZ2rq2*;4dphu1W;K=3X zF56Ycmr+BEJLB2x0N)rcSVU_ECQzP8MRED$7_`B}W|0^lq?iUNj9(-7LPB8RzKK~@ z8f@v-!w5ZJZBo*Ye2U7u)=gXm#L&>=#TA>G&&3-FN7dH(xZ?hX5u>HKA~GgA)@^-; z&Y8=YIk=bPa^BZ2IoFO$`U^*DBskTf^q^2+I}Fh;V5HJGj3rBii&!*c7z&pkhu+v^s}!`f0Bxgivxv{x~vPci!1Z>!O(ug zs(Vto^dch3MfCO`uee@EQSy%mp84H8cB*~1a(6cKO77}fDxi=4p5IIoqazcoYvoSU z*5$gH<>S~gqu6UGD8=Q!`1&R1CV6?!L3$2~$|U&6z~ zk;-CckT-td3Dn@Vk05_kO5$UC@+s9ThEd7Yjy(3sd5-oE(ovdRovy8f)Bw8)aQ?(* zoh6ET*8HXMTLsTFB`}t}Qa})aRCwsFb&|h-M>LZfur3cKOYJc*MAJfc@}=ae*-MJ0 zcDo-cTtAZlpRJ0f>-xCW^gOAP64x5SLW96h#q$8UN?5;=j3H>O#xMU*)JFZ&btHK_ z9B9`KOJ$q+Yo&_5zP=0mHxl_D&fDh7szAdW~MK zJ}!UD_p5i8KaOHJeYQWE;Y$$tRGa0KpfVR-=|aAn)h08n{IJgv{SX^v)LZhWs6Dn1 zAAjYGiTc4gT^d{5fU`7s-P?Kp{*o}k`z)MdFPgh= zjA~3_?1_qugc7-8Ts1eJ+02by2GhTC@hm0&)@K@CCP<|LPSo!1v$y2Lx;JX)d-w)AI8U_*30TA%UQHDB^_MU~`zUh=({~Ojc z+<>DPCJq@@jg5`5u^Pc7Y-4_OwiuM+l~#jGpJ=_FxbL&F$yIu57U==@XlQ8!X3G6d z-@;SC)InChcMRcv@Hkk>e%*o84A0#@emLpb9jKO;)=PnL_Y+uU01j7Z1sBoipAC}y zjO%R-wv-U*Api&^#LN_Gl}NrTkdYO7{P@Ma+bD^_krDixH`9q%MvkGDm!$)>)KPcF z(xYRJ%Uk|_UlJT+n|Kh(kdc{bLMU>rJzcDm2xk(*`)hD$<>7ZTEw63dr6eiGQq<`C>^ICn^RviU zT)cnZAOF%2aou_7*%mNz^8R5=oxtbM^XIH;(6Njb%z* zLHGVCpC)mp&R9`*lFC?%7u-TPojq~h?Pvt&y4Y*+^=Ed>OC<6%;H|Zvt^;eH=C*Lx zi?Ew~ph+|yB`7UKhIp_O~V`Zgfi zcpb*IqLPf*t0KO2ek*PCC!J44`6Mf!Pfq2%8vLUiy_C=AV({%HV-Op*4Us6e{nhK;chpxzkb+Icvl*+Fa7PeB{9ynQFg;X zH$x>H&3bikhGjXS2QMT9Q>N#iceZTkfC%hjXj{QOQp~GUW zYhBlQ{^|q<$&rJD1NPwihAmII;pZ%@t@T!XjC9-lImlT8U!41mN zk;w5ZCze&61`2O=|5XkuxNjl@e~#0m)DoONtz8Q{Fj1FnZ;f{S$)QM;1T`DGZoS1f z?m-{4Z$`K8MLVb4oNtEkcxYzXLP88w9y6}xQnlK<_VmLIh>;3xNa-ttTECj-kKc<>0tKPQDR}cKjt~SNd z1Ex?m)h)~cR0qvDnt}YDp2_ktF6)!GP__jYsECS6zpe8oh$8LtQ7v+u?Z2c}pySPBO>s*3MJ7&MiYNjV&5m&UzJt5c4 zykD+>sPkx&wu*mSiN`ZTE#>5NW`>?+aY@Un{E@Hnw7r7pDIBzJhPlH-yq_y9ukLD5 zhl+R=7)rmMIr>Rb@3Z*La?G@opM>}CocWKG0C{Jk(SR%CgqQ;|=u_-vNY#rrb%M}O z2oG-b=UAj&!!d0py{iN1ohFIy^D8R~@vgg1f;_;iNN;FpIPnw~V;WtFqBi8OHrBbG z{`tvANG%459JOE@hqjHD$Mh*Kco{TCfL2|k<||SnHln7xe*J;R(LTTyUGYNvLPFYA zGJ1M?0Dx+!s&22ZUsh-NE6MBtHcOx>lcDg=hs_&$Oehcv_39>GKi&j|yJ>2o2X+z8 z0Uu{aW8-4k8H~YFdO$+d=p=5dca? zOP>(JcnWh-UNzvt!SaJDZM7srd}-Ct)TRzsn*%JB^FDuh*)v*tFGB-^d4+&a+9#Ya(0uYFcwJ0mTn!#$pjec)^#L|YM4 z#Mo$1a1u^M=4(Ts%D}gct-Z?U=1ib1%CGu5!UtY^l;K=o>)fC3jtT%`FDIvDivF2A z*aD5?w&t4a;V!Gn#-}}=ro{ygoom#vTAM^a#tI}P_=ee^R5v2<~F>HJbBTS~OUKt__!PFuygMU@A5Jhtiy$t->_M*l1_+cK{I`Na?LfMBmkm<}G!>lLE(xJa)$091RJoEp#5LOqwTzLy(< zP9L>zX`@i~B(#Ee(3etW@pzJ-?0`h0*z=q?#jW_sZ0ul_DCvJO^;9AflF`AzR|9Qx zB&1@efwF!LN1L5EJQRr><|2_wo5%`<%GCiV0B~WzR%n>{B!jMB5l3D_)(7l|=_E-| zRT{p0d290l)go-NpiVN4Qd3nOV@k-z1ECOVa_seIG8mDQlLHUU4ixs#Wbj3295%%Z zISzcKgftDX{JeW((R8H`wXTt`g|xI3246bl3+-b1LAlKw9Mf46NqOwP`Kuj-^Z}j|!+Z43Y6=a+Yb^1V{|&net{ttOe_^#?!1El{ZK8xk~1QzHG= z1Q-I9L+YbR86Cuw*;?LeY3s$>%^AdmXYaX44nLKS zT-2XA?>+#KPd9}ahmH~BW}X1{IiV{SbwKp0zPOk)fQLQ_BCFvsr3oX69PKOaWkW`* zsjc7gGi8<0kA2MEi_(>AlUJr_Nd!A9;`sa(?H8pcJ_U6tU@e4Se^FWav4Ks_V^QyW zL+WvZpIMO6*Ls(<9goLefL*^nr8NxOKMP6ye5(g`R`Sz-c;Bal1h4%Oac|TZ%#<9`U zH5|q_-DtKhy=+TbDn8X2GruhKS$bk5dq0(9r|ppgs{!>3D)znz^YwP3|{hm)jmv z)HrTp`fvwdaNuw@?uj6B3= z{@1XAt1=|yEG$$aJIMt@S0pZ&*_9C;$5D9#{tVq@-=e&tqOj-5L)~*_#1NnrEYeJi z_+iMLTTmULT*V-`_4?UrU?@M`uDIK_mB8rO7l~Z!Uq=o27;l$=b8ya^_bZ}^LfGPk zjdx@3UPrmNB%LuP3W++TVB?OT2R~6l0=w3lYg<8&|Ck#}JO#@TWRDmHbpWh2|d?t;`CsOr=)ywR|5H3yd!^8px2?uD8Vwj zaAy=k@F+NR=i{v73ybhjscW$85C|P;mzdFCtAM=2h0py4G-96M94<%zPDLufk`A^O z77Kzbq0;LIR0pS+0KFN2nI~{0qU68qMnn8Ktil6O8p?5BKtp~b;8({VoTOlCRVRdqRUD`*m8#Yx~x%$wBmz9-4YNptqG;c9^+>Q%N z0b2LQD}#*)EM%^Mcp%1s>%$@G6WRv9x-e>MdInFKLsHEIWu!-h+6o^(Od%In#jBLYIlmesUCqXB-^~S zfK`b)xA!ghjdTsE?QL&&_4ZQaYp54?UQ5qKpuUQWzi@MaoKxiCbC^j>$EnFpAaLK0idSm>>Pkx zLC%a@!~C4J9>=~s)T97})>PNXSFP*S$<3_J_e4z$hQ!5aLnfw%hP-RA^7*x{@lrQC zCR3&UnTiwKR9L45AjXgRbH=y(ABK@ z+c)-7t*^SD*if{Ka*yTYLN~E57my(R11-+s;U+<^>q+J9LA`K?)@w8CS*;3 z{^r!coF4jPH5HYuogHMR$Q=kche{6;ji9je2KYWPk%gS|N;56nbBe>e zOT*w}$u28X?I_@js;t}tFaYpO@twYrMRJx0527aqAvYVe697^ZaU-!ZC{&q`j}#D| zi;p2E90QAS1;%(boE}>P2?mfotj6N*q*FF__UQ^sZCF;A%o(?>?guT zC?YpEx0VldN_TyzBwAy3efVU)+uPZ(u(BFaq)J?&rha8+Cfl84Cn+SK>jtRA&?A5_ zpt-xf*zy(+;ih11Zcb1dplhI4Bib3z#=>$crX-)PrK6)WuYjGeu!)jn^s49ksWO#_ zrHdcsw%QWHLj~!M@(Q|Z-1BtkMZE~sY`7NJ&p)Nn!!wlmfp8Ut=1QV*(NneIyx)4C zc}v|zdD<`0N>Xb_<@L%$GqqS88@bRh2Tl83rAJZE*cr~b_7j%z*%S43x*-qK2y3_jOKnt@E}#m0Fa-FI*K zYZYyg?7*)7lIF&dSgcLo&_T7sv2Xi|fSW{4H+dXruimdYa)J5PCO2#>d5|vcX_M7V zqyuw=Afs0#Agm_{Ej7!qDPzLHj^cy*cZ&v$uS_SbN&-!25$tGNf}x<4IRii~8Qe@H z?%GVh^&vI(P9Aih9blM7OgLqX2`FJgL9Wde&mC1mq%V%Z%{AbXKpYn89fG#8CikKGxmM=6{8P!o<*Uu-LE_{sZ7U(tQJ5J-8#ajPDjuk2&H>N^7u<4taOA zwIvC)!xP8lcY^%^YC?#W-`v~Fr0ZYa9t5ixe95;eUvuu2v-gKcydThOKW9qU6`J}0}c+)lY@n_(K2FCuK2%yfBD=h0&#kejJN!QoxHAr zz6xOvJJX>UApmQR_QMe*i@QO(eFE)yDqgaVsfpOt&;3FMhEcwFC``*Lu^M(lk#^I` z>iA5bf5ExkNV~keNEsO!`H**M%4*sa5&9K}&byJAE}@v_vvf`=pEAHw!l7ls4!+%p zhzO`V=HgtQTbS{9d7K;_bg1Q4vv_22fkbw+UttCS$tl-VWy0@`0?JP!P&Ft<9cG6bT=f(1Z!P)S0heblgi38zUw#>U|8=#}$_Y>%n_Zp=*uq0*$@;RWCkU`s)Vm3y}akp1}3#Zm!!f5X?`zdcJi z6=_ zMbvN_$zga;b^%945qIc@N_9H{ZJ;Y%Qbn1v94%^>QJ3@wLj3~LXv@1EVBWnu1HqwS zEYqoX6JTY{d+FQ5-n42BXUF5mZSY&b*k@_t!TP}~Nxr-~*AYMH($N`w#lO1W?0;6g zvPhv%Zlvbs#RL;0x8R;V&^O!frko-rx4)#{=iXjbpUZq_vAc&vnOrhh74-PBkt~E~#DkX8jAH*uq}M?UGG;iyg!HcVtA}2fh)8w)lsgr^&Hkzlu8ACb`?sFS z$QML#xyX6v7m2T+W3g*j(LUO8-)z4NmFhr$KLk(D)`Wp4{IoYcQawSg-R^zI!|>>> z+N?RnZdZ4`zE(w3)2vz5KfGbsbG@tN)+5bQsfF#<#|2s_CDZjAk9w;E(5dJ3d9FvB z9{e_p64<)PlOM?eB$#KSQtr~kL{xf&i3)LrWW%mPn?DV z9LL$YX4}P<-Vr6#@{$~x?eeFm7JL>#N{=t+=iUcgxROQ)C>Q&#D36g@XEGqWl->X~#_*~yfBpD=E~IfVhBhE$qvFvNPY%)gLyq5uZj`$ks$ zdeqs6y!lcFq)<9FdZH$=nEB>xk|bRnkaSOT+|KOk%qdxI4e0V&B}Nx$P3a_Mp_q!`zhTC)gB1Ny0$@k{m11)EH#a6FNy+ z(KjBo&MJxPFbFS;onFKk!w|pDuV0fPRQq%gk4GpP8^}?=zPU*%g1!s_q};3$tiy5E ztO7HhX0uPk8ZVs;lvdDil&LZ1sXY5DVQS}tW}W@Xp!Gt~v~q>+X)I!*z9z<8bZbLm zwU^Fe#-RgWxO_Y#19)oElb||f+yP=M4=^M!l2-n@%5y0DMn#ytPohqA_2^yS zcAg1bKr!ZaQ3|aW>Cu(i5SSc|tSyDEZtTViH$H$jS7{7Uu2_GXZ4UK+`@$=_nIm53+_PG&@NAOUc zcbIh_C9=UO?Ui{~|Hu+vsG|d7>*=8nhfz9X^{ZihPfMHUCtc1cpwXaCZ)>~PPyT*u z6L~CVXvKg(|NTAh@<^T2tLIQsSeV9&!#&LwRY#XK>#VF4KxAV!x z5Q=_pr}0)+{!mYyW%R0uYM0Elk|k6#RJxXz3yHWpksoZK6<(cbur}w~8nqUsaN~aQ&E#02g+;Gq z#jZ=iW>e|dW8eJ!KvrSv(LxPS?7n_}oey`bi%JB4*cTs`vVL#t8syjv3!G6sX=0q_ zfQDvvb^`zhDD<8_UB~xVQZ?*!*rRd7L&5Cy)Uc~FdBlSz*AEX4T0}E|Za2cAs8;#6*A*l2dD0(K$sMR4rJ>Qzb3Il>0crv3DQuJZ!KVLJMmKJlXXbbDU@0(}zW%@% zL~Gq>`wz4tX}wH~c1pL*6~&SEPaU#lbSf+*(Hf7eoWz|M&pY|f`DuvaPc%uy-}Tb7 z=yyWswmx)O1teTLGv^1`5bj=v&i=?J@SE}L?K$c1ia2|V#!neC?EZciXF_{^Fywju z2@4IA(zxo^b5g!MdNDYtD&louWKTFKqoBg|AGz(5m4%5o!Yexri3dBaeb*|CCYWp= z*Yq|aB3>k>IWqGdB%u=sjgM!mZc?((aX>-KtxV9Z+$6mlV_sQl^>7BFFCWvR%#asc5iuVrQZe{C^H z-u->#vj@WGv1^BoQuQ1k|JUhm-E?AO0P~0en*k>Aw2})aI)RYx@8jcjE;~7Bv{uIn zJPSl_Mny$Y30C>Z;>rH!?oX6LKTFn($U)mPLc&h2TVb;;tNQ$T^ykkvK<oF41H1*yjY9wHh<}lITSFsHP^^W8;J!r=c3K ziXXVfYafhMXK}Nh0VSL{DihGG>`!hKUbCoFVhRpf52}O<4&;{t)8(5JBwhL~iVt91 z6f6O#tnF;4Tk^%%H4v8ciHHgD*hP+an6_F4c_D|Rcb_&C%nEp{k_l1-HK%N=)x=)B zfM>Wq$&6@L#i(zkjkev4f1HBWzq^x3Ryn3i7)>Roq2$ztOY1N=RaN$3{o>$kM`x zdTnl@%dU-CJNOvwTcp@?buh2hvrlY2bTyQ0sfg~LM$^aZ6a^xi*5oNx1s{*KjN@RF%OTE)jHQ<-sbbm zv04*;m*^t7hI+a?zn5Bz&ag5xJ`$=QZWmzt6M?}F902%9V&SE=3JeWcsy!gCRZ4uK z)N8tAEeFhb^%jK_0Y;Ni{40s%=#|Wrl-r=JYLxs}$b#FLjE9;bBMF9VWyHp60XTVb zKv@P}V(|I~2L)yMF?Sc?fvLn>Ohsw#-( zd06FH*?=IF9Gz9x(ShkJgb)}ox#G`eCFJm$z*JBgq~+b(MNg$HOU4WunjvpZ(xx+*H5*EFta54rRn%GWUtLW~sl)8dN2?j>z_L7YL+3p%s3 z+P7x1bct~-r3$FbC7aG|%9g`jx{q9|*6*Lpfhw9(fq!5irjguy*65;Qg{e7iyfZKO zQ&8oOu%(zoDfsfDb>Jm6sneqG7KyRgXQ=p=BD^vvye7AJm1Q;UOVIIk?oXavl@=o& z-uu(pk=v@+JxbzMtK=4ks`+ut29>~o^R^02onNz^$Pe62tn z;^E;9%nacEvU;b2+b4ZZnizc#K?cuP+snaMC<{Q&_-vE*SGZB6hMMZ=06yKiVWw|P zfZ(2@nv=(Pdf8hV8_l@e+|w6Yzp(JHZOdTKkf6$}7bszF9Dq^liq-^0L*{bnr^ z9Cqo6F40Cv6H%9=vECVd3ukf)4Lv>f5~7{+kXS$lG|{yitQB_{?so28Zu>kr`SGsO z(UD}_*GLd|2?Qgf9)Z9Ybx-nA$m3+9(vlLR)#+>G6%^~R((LHU)`sIvyluKjaO+qf$ zV{6}Om6z$_lEH@jrxS5A3j8@@&$k>^SX5C` zG5z9I&D7NK%DA2931xV6_?@N$^Y;^!Vi{}7@XUUk+QIAVQ62Hi zwUdGR+AA^kS4;k~JQMNT|+Z*HMRAUEcoHCoxhS@-yM1LwBc6?Wh!lZ|7tuj+U?W z7~KxYWOiUlA?Ck{ev6{YSI6qUD$9w@pkuibZ1o6rHl`>ps{m(*1|PaX@oA!7o>|BJ zD_<-0@%zcr=(UJVaz1M@nR`^eZ79m^bbH|w@m`HZDwm33@|%8Uc5>xHuJp(*ziQ{N zDOIM*kBpXY^8-j|k8z@=65J@er=3COV9nXt*ytY_Ssls_GesOZsyuy)8O-cGI@khl z#?9}EI1SX}!i<7~v>e9NTCO{l+d6V51yvTU*l_`_O34NX_sHKdAMN-UP92PG9$RTz zyp(#@9v6j-p!}^?*qK|xk^{Gpt21|?gL!eMDkY$=wxmXr0gTZkjPq#G&YVOedakBh z%wZ?)n^JmDpP0!l#?9qWjVE|sfXjrhQ0RqZUbMN5fGG zI3M6JIvR&Lo=0qBLDBJ52a1Z8W@?t=NJTA8q{7RC)X&dON1I3W$@HGEJV=c7WMo*g zFh6OIIy*W*Y;8h*ej!|%JUla8eZDT6IXj1`upm1pKer%pnSjpZpKG8az{hx!JG{1m z&H%zB0Ct0Y1>n7wOFrZmeSiBa*2RH)zQ)lShU>x$Kz8k#Th=0O@0-?lzoV-yYQht- zG@+HzCkiK*c{b;1!KwZ%!o=9wE#U7=E`*x}yg$_-90MO<6oI7J|mEhmS0vJ6+P{QV`Sf_r|s+1c$~T@US-a4FM3HUgT-GyH=VS3*xrOu+gFI}bqdfLdRo=F0;mCY{@}OF2bAlIo&S z2Ov^5>G|voA|?0`gO>k(f)gEF^!cLFM=<9Y;_GH%qX1*zDJY^{0gMF5s{f`YBcSTE z4_^NOp(p%n56CzGc?_S6Gz4e<=W7i{6`UgYh>Z;~a0Ossf?QdUBrh*5oh*o+QgXf! z10^4vebcWMc!Xlk9R{q=Jc5#pO}kv<6XvO=ATH(12SMb|9*A1OMwHJ>eoX zU>qUjB|wISvy&66W{FWpI8(z37Rp+B2sqBk%GzJdOaz70&dlkFBm6X6P7_=>4q;uJ zIR)qjXE6Y7P83GOK`1Gh+Chv36K@RKU|8P2Uv95)KZy4RxvsMErt^y3>n*Ej!rYc|0Md& + diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp index daf609d067c..9ee903fd78e 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 @@ -20,10 +20,9 @@ #include #include -#include "behaviortree_cpp_v3/behavior_tree.h" -#include "behaviortree_cpp_v3/bt_factory.h" -#include "behaviortree_cpp_v3/xml_parsing.h" -#include "behaviortree_cpp_v3/loggers/bt_zmq_publisher.h" +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/bt_factory.h" +#include "behaviortree_cpp/xml_parsing.h" #include "rclcpp/rclcpp.hpp" 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 39c5c5f7d95..6193a91a868 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 @@ -19,7 +19,7 @@ #include #include -#include "behaviortree_cpp_v3/action_node.h" +#include "behaviortree_cpp/action_node.h" #include "nav2_util/node_utils.hpp" #include "rclcpp_action/rclcpp_action.hpp" #include "nav2_behavior_tree/bt_utils.hpp" @@ -188,7 +188,7 @@ class BtActionNode : public BT::ActionNodeBase BT::NodeStatus tick() override { // first step to be done only at the beginning of the Action - if (status() == BT::NodeStatus::IDLE) { + if (!BT::isStatusActive(status())) { // setting the status to RUNNING to notify the BT Loggers (if any) setStatus(BT::NodeStatus::RUNNING); @@ -325,7 +325,9 @@ class BtActionNode : public BT::ActionNodeBase on_cancelled(); } - setStatus(BT::NodeStatus::IDLE); + // this is probably redundant, since the parent node + // is supposed to call it, but we keep it, just in case + resetStatus(); } protected: @@ -376,12 +378,14 @@ class BtActionNode : public BT::ActionNodeBase if (this->goal_handle_->get_goal_id() == result.goal_id) { goal_result_available_ = true; result_ = result; + emitWakeUpSignal(); } }; send_goal_options.feedback_callback = [this](typename rclcpp_action::ClientGoalHandle::SharedPtr, const std::shared_ptr feedback) { feedback_ = feedback; + emitWakeUpSignal(); }; future_goal_handle_ = std::make_shared< @@ -434,9 +438,9 @@ class BtActionNode : public BT::ActionNodeBase void increment_recovery_count() { int recovery_count = 0; - config().blackboard->template get("number_recoveries", recovery_count); // NOLINT + [[maybe_unused]] auto res = config().blackboard->get("number_recoveries", recovery_count); // NOLINT recovery_count += 1; - config().blackboard->template set("number_recoveries", recovery_count); // NOLINT + config().blackboard->set("number_recoveries", recovery_count); // NOLINT } std::string action_name_; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index eef3ad2c869..5e005fc610d 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -237,7 +237,8 @@ bool BtActionServer::loadBehaviorTree(const std::string & bt_xml_filena // Create the Behavior Tree from the XML input try { tree_ = bt_->createTreeFromFile(filename, blackboard_); - for (auto & blackboard : tree_.blackboard_stack) { + for (auto & subtree : tree_.subtrees) { + auto & blackboard = subtree->blackboard; blackboard->set("node", client_node_); blackboard->set("server_timeout", default_server_timeout_); blackboard->set("bt_loop_duration", bt_loop_duration_); diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp index ea3e41d859d..789e30133dd 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp @@ -19,7 +19,7 @@ #include #include -#include "behaviortree_cpp_v3/action_node.h" +#include "behaviortree_cpp/action_node.h" #include "nav2_util/node_utils.hpp" #include "rclcpp_action/rclcpp_action.hpp" #include "nav2_behavior_tree/bt_utils.hpp" 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 1afdb5adfd1..d51c7d28398 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 @@ -19,7 +19,7 @@ #include #include -#include "behaviortree_cpp_v3/action_node.h" +#include "behaviortree_cpp/action_node.h" #include "nav2_util/node_utils.hpp" #include "rclcpp/rclcpp.hpp" #include "nav2_behavior_tree/bt_utils.hpp" @@ -157,7 +157,7 @@ class BtServiceNode : public BT::ActionNodeBase void halt() override { request_sent_ = false; - setStatus(BT::NodeStatus::IDLE); + resetStatus(); } /** @@ -230,9 +230,9 @@ class BtServiceNode : public BT::ActionNodeBase void increment_recovery_count() { int recovery_count = 0; - config().blackboard->template get("number_recoveries", recovery_count); // NOLINT + [[maybe_unused]] auto res = config().blackboard->get("number_recoveries", recovery_count); // NOLINT recovery_count += 1; - config().blackboard->template set("number_recoveries", recovery_count); // NOLINT + config().blackboard->set("number_recoveries", recovery_count); // NOLINT } std::string service_name_, service_node_name_; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp index c0425d1711f..7075bc63f48 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp @@ -20,7 +20,7 @@ #include "rclcpp/time.hpp" #include "rclcpp/node.hpp" -#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp/behavior_tree.h" #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/quaternion.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" @@ -132,20 +132,27 @@ inline std::set convertFromString(StringView key) } /** - * @brief Return parameter value from behavior tree node or ros2 parameter file + * @brief Return parameter value from behavior tree node or ros2 parameter file. * @param node rclcpp::Node::SharedPtr * @param param_name std::string * @param behavior_tree_node T2 * @return */ -template +template T1 deconflictPortAndParamFrame( rclcpp::Node::SharedPtr node, std::string param_name, - T2 * behavior_tree_node) + const T2 * behavior_tree_node) { T1 param_value; - if (!behavior_tree_node->getInput(param_name, param_value)) { + bool param_from_input = behavior_tree_node->getInput(param_name, param_value).has_value(); + + if constexpr (std::is_same_v) { + // not valid if port doesn't exist or it is an empty string + param_from_input &= !param_value.empty(); + } + + if (!param_from_input) { RCLCPP_DEBUG( node->get_logger(), "Parameter '%s' not provided by behavior tree xml file, " @@ -162,6 +169,37 @@ T1 deconflictPortAndParamFrame( } } +/** + * @brief Try reading an import port first, and if that doesn't work + * fallback to reading directly the blackboard. + * The blackboard must be passed explitly because config() is private in BT.CPP 4.X + * + * @param bt_node node + * @param blackboard the blackboard ovtained with node->config().blackboard + * @param param_name std::string + * @param behavior_tree_node the node + * @return + */ +template inline +bool getInputPortOrBlackboard( + const BT::TreeNode & bt_node, + const BT::Blackboard & blackboard, + const std::string & param_name, + T & value) +{ + if (bt_node.getInput(param_name, value)) { + return true; + } + if (blackboard.get(param_name, value)) { + return true; + } + return false; +} + +// Macro to remove boiler plate when using getInputPortOrBlackboard +#define getInputOrBlackboard(name, value) \ + getInputPortOrBlackboard(*this, *(this->config().blackboard), name, value); + } // namespace BT #endif // NAV2_BEHAVIOR_TREE__BT_UTILS_HPP_ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/controller_selector_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/controller_selector_node.hpp index 968750a05e9..625f7e9483b 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/controller_selector_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/controller_selector_node.hpp @@ -21,7 +21,7 @@ #include "std_msgs/msg/string.hpp" -#include "behaviortree_cpp_v3/action_node.h" +#include "behaviortree_cpp/action_node.h" #include "rclcpp/rclcpp.hpp" diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/goal_checker_selector_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/goal_checker_selector_node.hpp index 11f0d0f423d..90a10f1675c 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/goal_checker_selector_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/goal_checker_selector_node.hpp @@ -21,7 +21,7 @@ #include "std_msgs/msg/string.hpp" -#include "behaviortree_cpp_v3/action_node.h" +#include "behaviortree_cpp/action_node.h" #include "rclcpp/rclcpp.hpp" diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/planner_selector_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/planner_selector_node.hpp index 3aa9e6573e1..4a1dd779a39 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/planner_selector_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/planner_selector_node.hpp @@ -21,7 +21,7 @@ #include "std_msgs/msg/string.hpp" -#include "behaviortree_cpp_v3/action_node.h" +#include "behaviortree_cpp/action_node.h" #include "rclcpp/rclcpp.hpp" diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp index e996bda55bf..c9c61151083 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp @@ -20,7 +20,7 @@ #include "std_msgs/msg/string.hpp" -#include "behaviortree_cpp_v3/action_node.h" +#include "behaviortree_cpp/action_node.h" #include "rclcpp/rclcpp.hpp" diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp index d9b3d4a59cf..344afd546d8 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp @@ -22,7 +22,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_util/robot_utils.hpp" -#include "behaviortree_cpp_v3/action_node.h" +#include "behaviortree_cpp/action_node.h" namespace nav2_behavior_tree { diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smoother_selector_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smoother_selector_node.hpp index f9bb293d264..da5a7274445 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smoother_selector_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smoother_selector_node.hpp @@ -21,7 +21,7 @@ #include "std_msgs/msg/string.hpp" -#include "behaviortree_cpp_v3/action_node.h" +#include "behaviortree_cpp/action_node.h" #include "rclcpp/rclcpp.hpp" diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_action.hpp index 2604bcfea13..5501d49cf32 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_action.hpp @@ -21,7 +21,7 @@ #include "nav_msgs/msg/path.hpp" -#include "behaviortree_cpp_v3/action_node.h" +#include "behaviortree_cpp/action_node.h" namespace nav2_behavior_tree { diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp index 113e0d83e72..94b4b66ca2a 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp @@ -22,7 +22,7 @@ #include "nav_msgs/msg/path.hpp" -#include "behaviortree_cpp_v3/action_node.h" +#include "behaviortree_cpp/action_node.h" #include "tf2_ros/buffer.h" namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.hpp index 42721f62668..209958c38d4 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.hpp @@ -21,7 +21,7 @@ #include #include "rclcpp/rclcpp.hpp" -#include "behaviortree_cpp_v3/condition_node.h" +#include "behaviortree_cpp/condition_node.h" namespace nav2_behavior_tree { diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp index 3c87c7370c7..67747c62b88 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp @@ -19,7 +19,7 @@ #include #include -#include "behaviortree_cpp_v3/condition_node.h" +#include "behaviortree_cpp/condition_node.h" #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp index 64f77f1473d..7e3e92e799d 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp @@ -20,7 +20,7 @@ #include "rclcpp/rclcpp.hpp" -#include "behaviortree_cpp_v3/condition_node.h" +#include "behaviortree_cpp/condition_node.h" #include "geometry_msgs/msg/pose_stamped.hpp" @@ -57,7 +57,12 @@ class GloballyUpdatedGoalCondition : public BT::ConditionNode */ static BT::PortsList providedPorts() { - return {}; + return { + BT::InputPort>( + "goals", "Vector of navigation goals"), + BT::InputPort( + "goal", "Navigation goal"), + }; } private: diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp index acbca8c960f..b79fabe2f9e 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp @@ -19,7 +19,7 @@ #include #include "rclcpp/rclcpp.hpp" -#include "behaviortree_cpp_v3/condition_node.h" +#include "behaviortree_cpp/condition_node.h" #include "tf2_ros/buffer.h" namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp index 1e8c9712c16..89d4b7a573c 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp @@ -18,7 +18,7 @@ #include #include -#include "behaviortree_cpp_v3/condition_node.h" +#include "behaviortree_cpp/condition_node.h" #include "geometry_msgs/msg/pose_stamped.hpp" namespace nav2_behavior_tree @@ -54,7 +54,12 @@ class GoalUpdatedCondition : public BT::ConditionNode */ static BT::PortsList providedPorts() { - return {}; + return { + BT::InputPort>( + "goals", "Vector of navigation goals"), + BT::InputPort( + "goal", "Navigation goal"), + }; } private: diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp index 2bb7d995b80..548c15268b0 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp @@ -15,7 +15,8 @@ #ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__INITIAL_POSE_RECEIVED_CONDITION_HPP_ #define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__INITIAL_POSE_RECEIVED_CONDITION_HPP_ -#include "behaviortree_cpp_v3/behavior_tree.h" +#include +#include "behaviortree_cpp/behavior_tree.h" namespace nav2_behavior_tree { @@ -23,7 +24,21 @@ namespace nav2_behavior_tree * @brief A BT::ConditionNode that returns SUCCESS if initial pose * has been received and FAILURE otherwise */ -BT::NodeStatus initialPoseReceived(BT::TreeNode & tree_node); -} +class InitialPoseReceived : public BT::ConditionNode +{ +public: + InitialPoseReceived( + const std::string & name, + const BT::NodeConfiguration & config); + + static BT::PortsList providedPorts() + { + return {BT::InputPort("initial_pose_received")}; + } + + BT::NodeStatus tick() override; +}; + +} // namespace nav2_behavior_tree #endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__INITIAL_POSE_RECEIVED_CONDITION_HPP_ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.hpp index 37a53b41e12..704154a31d0 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.hpp @@ -21,7 +21,7 @@ #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/battery_state.hpp" -#include "behaviortree_cpp_v3/condition_node.h" +#include "behaviortree_cpp/condition_node.h" namespace nav2_behavior_tree { diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp index ff479105807..59a023ff3c2 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp @@ -22,7 +22,7 @@ #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/battery_state.hpp" -#include "behaviortree_cpp_v3/condition_node.h" +#include "behaviortree_cpp/condition_node.h" namespace nav2_behavior_tree { diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp index 36890f2a8a0..5a9f255a9b7 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp @@ -19,7 +19,7 @@ #include #include "rclcpp/rclcpp.hpp" -#include "behaviortree_cpp_v3/condition_node.h" +#include "behaviortree_cpp/condition_node.h" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_msgs/srv/is_path_valid.hpp" diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stuck_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stuck_condition.hpp index 565f261100f..e532822d42a 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stuck_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stuck_condition.hpp @@ -20,7 +20,7 @@ #include #include "rclcpp/rclcpp.hpp" -#include "behaviortree_cpp_v3/condition_node.h" +#include "behaviortree_cpp/condition_node.h" #include "nav_msgs/msg/odometry.hpp" namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp index fb2f42f3d4d..8871892949a 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp @@ -18,7 +18,7 @@ #include #include "rclcpp/rclcpp.hpp" -#include "behaviortree_cpp_v3/condition_node.h" +#include "behaviortree_cpp/condition_node.h" #include "nav_msgs/msg/path.hpp" namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/time_expired_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/time_expired_condition.hpp index 356a79857db..26a3431b5db 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/time_expired_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/time_expired_condition.hpp @@ -19,7 +19,7 @@ #include #include "rclcpp/rclcpp.hpp" -#include "behaviortree_cpp_v3/condition_node.h" +#include "behaviortree_cpp/condition_node.h" namespace nav2_behavior_tree { diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp index 5fdefa9b8bf..511c381321b 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp @@ -20,7 +20,7 @@ #include #include "rclcpp/rclcpp.hpp" -#include "behaviortree_cpp_v3/condition_node.h" +#include "behaviortree_cpp/condition_node.h" #include "tf2_ros/buffer.h" namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pipeline_sequence.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pipeline_sequence.hpp index 0384381d2a9..ce3406904f5 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pipeline_sequence.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pipeline_sequence.hpp @@ -16,8 +16,8 @@ #define NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PIPELINE_SEQUENCE_HPP_ #include -#include "behaviortree_cpp_v3/control_node.h" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/control_node.h" +#include "behaviortree_cpp/bt_factory.h" namespace nav2_behavior_tree { diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/recovery_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/recovery_node.hpp index 89c0cfa300c..62759ea7113 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/recovery_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/recovery_node.hpp @@ -16,7 +16,7 @@ #define NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__RECOVERY_NODE_HPP_ #include -#include "behaviortree_cpp_v3/control_node.h" +#include "behaviortree_cpp/control_node.h" namespace nav2_behavior_tree { diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/round_robin_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/round_robin_node.hpp index 8c374ce6af0..86f2b38c5bb 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/round_robin_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/round_robin_node.hpp @@ -17,8 +17,8 @@ #include -#include "behaviortree_cpp_v3/control_node.h" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/control_node.h" +#include "behaviortree_cpp/bt_factory.h" namespace nav2_behavior_tree { diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/distance_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/distance_controller.hpp index f85a3975d06..7fbda19c63e 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/distance_controller.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/distance_controller.hpp @@ -22,7 +22,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "tf2_ros/buffer.h" -#include "behaviortree_cpp_v3/decorator_node.h" +#include "behaviortree_cpp/decorator_node.h" namespace nav2_behavior_tree { diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp index 4ac0ab44eeb..bdd4171185c 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp @@ -19,7 +19,7 @@ #include #include -#include "behaviortree_cpp_v3/decorator_node.h" +#include "behaviortree_cpp/decorator_node.h" #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" @@ -48,7 +48,12 @@ class GoalUpdatedController : public BT::DecoratorNode */ static BT::PortsList providedPorts() { - return {}; + return { + BT::InputPort>( + "goals", "Vector of navigation goals"), + BT::InputPort( + "goal", "Navigation goal"), + }; } private: diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp index 49dfbc1a0c5..ddce12cf02e 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp @@ -21,7 +21,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" -#include "behaviortree_cpp_v3/decorator_node.h" +#include "behaviortree_cpp/decorator_node.h" #include "rclcpp/rclcpp.hpp" diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.hpp index 6e41516434c..bdf4a080d18 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.hpp @@ -21,7 +21,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav_msgs/msg/path.hpp" -#include "behaviortree_cpp_v3/decorator_node.h" +#include "behaviortree_cpp/decorator_node.h" #include "rclcpp/rclcpp.hpp" namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/rate_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/rate_controller.hpp index 24a0207e3bd..c390357b342 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/rate_controller.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/rate_controller.hpp @@ -18,7 +18,7 @@ #include #include -#include "behaviortree_cpp_v3/decorator_node.h" +#include "behaviortree_cpp/decorator_node.h" namespace nav2_behavior_tree { diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/single_trigger_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/single_trigger_node.hpp index c16ef63efa9..02c0c1812ef 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/single_trigger_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/single_trigger_node.hpp @@ -18,7 +18,7 @@ #include #include -#include "behaviortree_cpp_v3/decorator_node.h" +#include "behaviortree_cpp/decorator_node.h" namespace nav2_behavior_tree { diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp index 8150eb31098..ed454c0aa1d 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp @@ -24,7 +24,7 @@ #include "nav_msgs/msg/odometry.hpp" #include "nav2_util/odometry_utils.hpp" -#include "behaviortree_cpp_v3/decorator_node.h" +#include "behaviortree_cpp/decorator_node.h" namespace nav2_behavior_tree { @@ -57,6 +57,10 @@ class SpeedController : public BT::DecoratorNode BT::InputPort("max_rate", 1.0, "Maximum rate"), BT::InputPort("min_speed", 0.0, "Minimum speed"), BT::InputPort("max_speed", 0.5, "Maximum speed"), + BT::InputPort>( + "goals", "Vector of navigation goals"), + BT::InputPort( + "goal", "Navigation goal"), }; } diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/ros_topic_logger.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/ros_topic_logger.hpp index 0e14a59ee87..d1ccef107e2 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/ros_topic_logger.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/ros_topic_logger.hpp @@ -19,7 +19,7 @@ #include #include -#include "behaviortree_cpp_v3/loggers/abstract_logger.h" +#include "behaviortree_cpp/loggers/abstract_logger.h" #include "rclcpp/rclcpp.hpp" #include "nav2_msgs/msg/behavior_tree_log.hpp" #include "nav2_msgs/msg/behavior_tree_status_change.h" diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 3e23aafbd0a..925e9c35797 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -4,7 +4,7 @@ please refer to the groot_instructions.md and REAMDE.md respectively located in the nav2_behavior_tree package. --> - + @@ -224,9 +224,15 @@ Parent frame for transform - + + Vector of navigation goals + Navigation goal + - + + Vector of navigation goals + Navigation goal + Min battery % or voltage before triggering @@ -255,6 +261,7 @@ + SUCCESS if initial_pose_received true @@ -313,6 +320,8 @@ Maximum rate Minimum speed Maximum speed + Vector of navigation goals + Navigation goal @@ -322,6 +331,8 @@ + Vector of navigation goals + Navigation goal diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index 797d53270b7..b9ed847ce38 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -17,7 +17,7 @@ rclcpp rclcpp_action rclcpp_lifecycle - behaviortree_cpp_v3 + behaviortree_cpp builtin_interfaces geometry_msgs sensor_msgs @@ -36,7 +36,7 @@ rclcpp_action rclcpp_lifecycle std_msgs - behaviortree_cpp_v3 + behaviortree_cpp builtin_interfaces geometry_msgs sensor_msgs diff --git a/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp b/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp index 64eb61d788e..e876d2ce40b 100644 --- a/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp +++ b/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp @@ -70,7 +70,7 @@ BT::NodeStatus AssistedTeleopAction::on_cancelled() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/plugins/action/assisted_teleop_cancel_node.cpp b/nav2_behavior_tree/plugins/action/assisted_teleop_cancel_node.cpp index e226ba19753..362499c9f2e 100644 --- a/nav2_behavior_tree/plugins/action/assisted_teleop_cancel_node.cpp +++ b/nav2_behavior_tree/plugins/action/assisted_teleop_cancel_node.cpp @@ -32,7 +32,7 @@ AssistedTeleopCancel::AssistedTeleopCancel( } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/plugins/action/back_up_action.cpp b/nav2_behavior_tree/plugins/action/back_up_action.cpp index 09d79c7e230..e17580fe71c 100644 --- a/nav2_behavior_tree/plugins/action/back_up_action.cpp +++ b/nav2_behavior_tree/plugins/action/back_up_action.cpp @@ -76,7 +76,7 @@ BT::NodeStatus BackUpAction::on_cancelled() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/plugins/action/back_up_cancel_node.cpp b/nav2_behavior_tree/plugins/action/back_up_cancel_node.cpp index 3baa1ffb1e3..21835c326a8 100644 --- a/nav2_behavior_tree/plugins/action/back_up_cancel_node.cpp +++ b/nav2_behavior_tree/plugins/action/back_up_cancel_node.cpp @@ -32,7 +32,7 @@ BackUpCancel::BackUpCancel( } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp b/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp index 08d2b00632b..2d007a6623e 100644 --- a/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp +++ b/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp @@ -60,7 +60,7 @@ void ClearCostmapAroundRobotService::on_tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("ClearEntireCostmap"); diff --git a/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp b/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp index 8bfffed8434..a0572ecad61 100644 --- a/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp +++ b/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp @@ -65,7 +65,7 @@ BT::NodeStatus ComputePathThroughPosesAction::on_cancelled() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp b/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp index c4b93afa854..c7f1a0164e8 100644 --- a/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp +++ b/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp @@ -71,7 +71,7 @@ void ComputePathToPoseAction::halt() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/plugins/action/controller_cancel_node.cpp b/nav2_behavior_tree/plugins/action/controller_cancel_node.cpp index 966ee01e10a..b2eb4358c20 100644 --- a/nav2_behavior_tree/plugins/action/controller_cancel_node.cpp +++ b/nav2_behavior_tree/plugins/action/controller_cancel_node.cpp @@ -32,7 +32,7 @@ ControllerCancel::ControllerCancel( } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/plugins/action/controller_selector_node.cpp b/nav2_behavior_tree/plugins/action/controller_selector_node.cpp index 7d77adbee39..b58d78bb78f 100644 --- a/nav2_behavior_tree/plugins/action/controller_selector_node.cpp +++ b/nav2_behavior_tree/plugins/action/controller_selector_node.cpp @@ -84,7 +84,7 @@ ControllerSelector::callbackControllerSelect(const std_msgs::msg::String::Shared } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("ControllerSelector"); diff --git a/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp b/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp index 7e0b613f62f..03c00344141 100644 --- a/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp +++ b/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp @@ -74,7 +74,7 @@ BT::NodeStatus DriveOnHeadingAction::on_cancelled() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/plugins/action/drive_on_heading_cancel_node.cpp b/nav2_behavior_tree/plugins/action/drive_on_heading_cancel_node.cpp index b29de63df55..8ac530f8df0 100644 --- a/nav2_behavior_tree/plugins/action/drive_on_heading_cancel_node.cpp +++ b/nav2_behavior_tree/plugins/action/drive_on_heading_cancel_node.cpp @@ -32,7 +32,7 @@ DriveOnHeadingCancel::DriveOnHeadingCancel( } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/plugins/action/follow_path_action.cpp b/nav2_behavior_tree/plugins/action/follow_path_action.cpp index 00acf5c3495..b662221de0f 100644 --- a/nav2_behavior_tree/plugins/action/follow_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/follow_path_action.cpp @@ -96,7 +96,7 @@ void FollowPathAction::on_wait_for_result( } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/plugins/action/goal_checker_selector_node.cpp b/nav2_behavior_tree/plugins/action/goal_checker_selector_node.cpp index 1a2e9c703b0..3ee9d832609 100644 --- a/nav2_behavior_tree/plugins/action/goal_checker_selector_node.cpp +++ b/nav2_behavior_tree/plugins/action/goal_checker_selector_node.cpp @@ -75,7 +75,7 @@ GoalCheckerSelector::callbackGoalCheckerSelect(const std_msgs::msg::String::Shar } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("GoalCheckerSelector"); diff --git a/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp b/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp index 57db8942481..9213cd564f4 100644 --- a/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp +++ b/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp @@ -61,7 +61,7 @@ BT::NodeStatus NavigateThroughPosesAction::on_cancelled() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp b/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp index 8022697e0e8..07bea22436f 100644 --- a/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp +++ b/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp @@ -59,7 +59,7 @@ BT::NodeStatus NavigateToPoseAction::on_cancelled() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/plugins/action/planner_selector_node.cpp b/nav2_behavior_tree/plugins/action/planner_selector_node.cpp index ccb85184127..5d0610c0fa5 100644 --- a/nav2_behavior_tree/plugins/action/planner_selector_node.cpp +++ b/nav2_behavior_tree/plugins/action/planner_selector_node.cpp @@ -84,7 +84,7 @@ PlannerSelector::callbackPlannerSelect(const std_msgs::msg::String::SharedPtr ms } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("PlannerSelector"); diff --git a/nav2_behavior_tree/plugins/action/progress_checker_selector_node.cpp b/nav2_behavior_tree/plugins/action/progress_checker_selector_node.cpp index e8c418ddc17..fea2194158b 100644 --- a/nav2_behavior_tree/plugins/action/progress_checker_selector_node.cpp +++ b/nav2_behavior_tree/plugins/action/progress_checker_selector_node.cpp @@ -74,7 +74,7 @@ ProgressCheckerSelector::callbackProgressCheckerSelect(const std_msgs::msg::Stri } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("ProgressCheckerSelector"); diff --git a/nav2_behavior_tree/plugins/action/reinitialize_global_localization_service.cpp b/nav2_behavior_tree/plugins/action/reinitialize_global_localization_service.cpp index 388f16ccb25..30b876d5346 100644 --- a/nav2_behavior_tree/plugins/action/reinitialize_global_localization_service.cpp +++ b/nav2_behavior_tree/plugins/action/reinitialize_global_localization_service.cpp @@ -26,7 +26,7 @@ ReinitializeGlobalLocalizationService::ReinitializeGlobalLocalizationService( } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType( diff --git a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp index bf8b95c84f6..86f5fffd6b2 100644 --- a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp +++ b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp @@ -41,7 +41,7 @@ void RemovePassedGoals::initialize() auto node = config().blackboard->get("node"); node->get_parameter("transform_tolerance", transform_tolerance_); - robot_base_frame_ = BT::deconflictPortAndParamFrame( + robot_base_frame_ = BT::deconflictPortAndParamFrame( node, "robot_base_frame", this); } @@ -89,7 +89,7 @@ inline BT::NodeStatus RemovePassedGoals::tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("RemovePassedGoals"); diff --git a/nav2_behavior_tree/plugins/action/smooth_path_action.cpp b/nav2_behavior_tree/plugins/action/smooth_path_action.cpp index da547b15876..3a67904558b 100644 --- a/nav2_behavior_tree/plugins/action/smooth_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/smooth_path_action.cpp @@ -64,7 +64,7 @@ BT::NodeStatus SmoothPathAction::on_cancelled() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/plugins/action/smoother_selector_node.cpp b/nav2_behavior_tree/plugins/action/smoother_selector_node.cpp index 0a84062e088..c717332c79b 100644 --- a/nav2_behavior_tree/plugins/action/smoother_selector_node.cpp +++ b/nav2_behavior_tree/plugins/action/smoother_selector_node.cpp @@ -85,7 +85,7 @@ SmootherSelector::callbackSmootherSelect(const std_msgs::msg::String::SharedPtr } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("SmootherSelector"); diff --git a/nav2_behavior_tree/plugins/action/spin_action.cpp b/nav2_behavior_tree/plugins/action/spin_action.cpp index cab9c8f8211..d10bb83f32b 100644 --- a/nav2_behavior_tree/plugins/action/spin_action.cpp +++ b/nav2_behavior_tree/plugins/action/spin_action.cpp @@ -72,7 +72,7 @@ BT::NodeStatus SpinAction::on_cancelled() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/plugins/action/spin_cancel_node.cpp b/nav2_behavior_tree/plugins/action/spin_cancel_node.cpp index 62ddbc4719c..1d75c5cf275 100644 --- a/nav2_behavior_tree/plugins/action/spin_cancel_node.cpp +++ b/nav2_behavior_tree/plugins/action/spin_cancel_node.cpp @@ -32,7 +32,7 @@ SpinCancel::SpinCancel( } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/plugins/action/truncate_path_action.cpp b/nav2_behavior_tree/plugins/action/truncate_path_action.cpp index b394a804c73..7bebfbfba38 100644 --- a/nav2_behavior_tree/plugins/action/truncate_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/truncate_path_action.cpp @@ -20,7 +20,7 @@ #include "nav_msgs/msg/path.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_util/geometry_utils.hpp" -#include "behaviortree_cpp_v3/decorator_node.h" +#include "behaviortree_cpp/decorator_node.h" #include "nav2_behavior_tree/plugins/action/truncate_path_action.hpp" @@ -82,7 +82,7 @@ inline BT::NodeStatus TruncatePath::tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("TruncatePath"); diff --git a/nav2_behavior_tree/plugins/action/truncate_path_local_action.cpp b/nav2_behavior_tree/plugins/action/truncate_path_local_action.cpp index 934f2b86bb8..35dc8635c8c 100644 --- a/nav2_behavior_tree/plugins/action/truncate_path_local_action.cpp +++ b/nav2_behavior_tree/plugins/action/truncate_path_local_action.cpp @@ -17,7 +17,7 @@ #include #include -#include "behaviortree_cpp_v3/decorator_node.h" +#include "behaviortree_cpp/decorator_node.h" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_util/robot_utils.hpp" @@ -150,7 +150,7 @@ TruncatePathLocal::poseDistance( } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType( "TruncatePathLocal"); diff --git a/nav2_behavior_tree/plugins/action/wait_action.cpp b/nav2_behavior_tree/plugins/action/wait_action.cpp index b9317076737..b607d026e59 100644 --- a/nav2_behavior_tree/plugins/action/wait_action.cpp +++ b/nav2_behavior_tree/plugins/action/wait_action.cpp @@ -56,7 +56,7 @@ void WaitAction::on_tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/plugins/action/wait_cancel_node.cpp b/nav2_behavior_tree/plugins/action/wait_cancel_node.cpp index 9365b7e06fa..b45db33396f 100644 --- a/nav2_behavior_tree/plugins/action/wait_cancel_node.cpp +++ b/nav2_behavior_tree/plugins/action/wait_cancel_node.cpp @@ -32,7 +32,7 @@ WaitCancel::WaitCancel( } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.cpp b/nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.cpp index 6764cccd116..f39dd8fbce4 100644 --- a/nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.cpp @@ -14,7 +14,7 @@ #include "nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType( diff --git a/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp b/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp index 64394ee3782..7db1817c65e 100644 --- a/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp @@ -43,9 +43,9 @@ void DistanceTraveledCondition::initialize() tf_ = config().blackboard->get>("tf_buffer"); node_->get_parameter("transform_tolerance", transform_tolerance_); - global_frame_ = BT::deconflictPortAndParamFrame( + global_frame_ = BT::deconflictPortAndParamFrame( node_, "global_frame", this); - robot_base_frame_ = BT::deconflictPortAndParamFrame( + robot_base_frame_ = BT::deconflictPortAndParamFrame( node_, "robot_base_frame", this); initialized_ = true; } @@ -56,7 +56,7 @@ BT::NodeStatus DistanceTraveledCondition::tick() initialize(); } - if (status() == BT::NodeStatus::IDLE) { + if (!BT::isStatusActive(status())) { if (!nav2_util::getCurrentPose( start_pose_, *tf_, global_frame_, robot_base_frame_, transform_tolerance_)) @@ -92,7 +92,7 @@ BT::NodeStatus DistanceTraveledCondition::tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("DistanceTraveled"); diff --git a/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp b/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp index fee838c9379..dbd84d8b2ef 100644 --- a/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp @@ -16,6 +16,7 @@ #include #include "nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp" +#include "nav2_behavior_tree/bt_utils.hpp" namespace nav2_behavior_tree { @@ -33,15 +34,15 @@ BT::NodeStatus GloballyUpdatedGoalCondition::tick() { if (first_time) { first_time = false; - config().blackboard->get>("goals", goals_); - config().blackboard->get("goal", goal_); + BT::getInputOrBlackboard("goals", goals_); + BT::getInputOrBlackboard("goal", goal_); return BT::NodeStatus::SUCCESS; } std::vector current_goals; - config().blackboard->get>("goals", current_goals); + BT::getInputOrBlackboard("goals", current_goals); geometry_msgs::msg::PoseStamped current_goal; - config().blackboard->get("goal", current_goal); + BT::getInputOrBlackboard("goal", current_goal); if (goal_ != current_goal || goals_ != current_goals) { goal_ = current_goal; @@ -54,7 +55,7 @@ BT::NodeStatus GloballyUpdatedGoalCondition::tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("GlobalUpdatedGoal"); diff --git a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp index 2697680415b..70243562034 100644 --- a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp @@ -33,7 +33,7 @@ GoalReachedCondition::GoalReachedCondition( { auto node = config().blackboard->get("node"); - robot_base_frame_ = BT::deconflictPortAndParamFrame( + robot_base_frame_ = BT::deconflictPortAndParamFrame( node, "robot_base_frame", this); } @@ -90,7 +90,7 @@ bool GoalReachedCondition::isGoalReached() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("GoalReached"); diff --git a/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp b/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp index 34930bb582e..88d329efc2a 100644 --- a/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp @@ -15,6 +15,7 @@ #include #include #include "nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp" +#include "nav2_behavior_tree/bt_utils.hpp" namespace nav2_behavior_tree { @@ -27,16 +28,16 @@ GoalUpdatedCondition::GoalUpdatedCondition( BT::NodeStatus GoalUpdatedCondition::tick() { - if (status() == BT::NodeStatus::IDLE) { - config().blackboard->get>("goals", goals_); - config().blackboard->get("goal", goal_); + if (!BT::isStatusActive(status())) { + BT::getInputOrBlackboard("goals", goals_); + BT::getInputOrBlackboard("goal", goal_); return BT::NodeStatus::FAILURE; } std::vector current_goals; - config().blackboard->get>("goals", current_goals); geometry_msgs::msg::PoseStamped current_goal; - config().blackboard->get("goal", current_goal); + BT::getInputOrBlackboard("goals", current_goals); + BT::getInputOrBlackboard("goal", current_goal); if (goal_ != current_goal || goals_ != current_goals) { goal_ = current_goal; @@ -49,7 +50,7 @@ BT::NodeStatus GoalUpdatedCondition::tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("GoalUpdated"); diff --git a/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.cpp b/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.cpp index 41796834c63..9d930229124 100644 --- a/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.cpp @@ -13,22 +13,28 @@ // limitations under the License. #include "nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp" +#include "nav2_behavior_tree/bt_utils.hpp" namespace nav2_behavior_tree { +InitialPoseReceived::InitialPoseReceived( + const std::string & name, + const BT::NodeConfiguration & config) +: BT::ConditionNode(name, config) +{ +} -BT::NodeStatus initialPoseReceived(BT::TreeNode & tree_node) +BT::NodeStatus InitialPoseReceived::tick() { - auto initPoseReceived = tree_node.config().blackboard->get("initial_pose_received"); + bool initPoseReceived = false; + BT::getInputOrBlackboard("initial_pose_received", initPoseReceived); return initPoseReceived ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE; } } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { - factory.registerSimpleCondition( - "InitialPoseReceived", - std::bind(&nav2_behavior_tree::initialPoseReceived, std::placeholders::_1)); + factory.registerNodeType("InitialPoseReceived"); } diff --git a/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.cpp b/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.cpp index c9c05f68753..27e1bd55fc7 100644 --- a/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.cpp @@ -59,7 +59,7 @@ void IsBatteryChargingCondition::batteryCallback(sensor_msgs::msg::BatteryState: } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("IsBatteryCharging"); diff --git a/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp b/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp index 9d221cf8c81..a0e3761f28c 100644 --- a/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp @@ -77,7 +77,7 @@ void IsBatteryLowCondition::batteryCallback(sensor_msgs::msg::BatteryState::Shar } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("IsBatteryLow"); diff --git a/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp b/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp index 59d593a1475..7274743e1e9 100644 --- a/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp @@ -64,7 +64,7 @@ BT::NodeStatus IsPathValidCondition::tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("IsPathValid"); diff --git a/nav2_behavior_tree/plugins/condition/is_stuck_condition.cpp b/nav2_behavior_tree/plugins/condition/is_stuck_condition.cpp index a898dec9473..d0ca48cd7f5 100644 --- a/nav2_behavior_tree/plugins/condition/is_stuck_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_stuck_condition.cpp @@ -143,7 +143,7 @@ bool IsStuckCondition::isStuck() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("IsStuck"); diff --git a/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.cpp b/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.cpp index 1347998602f..540af1d83d3 100644 --- a/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.cpp @@ -15,7 +15,7 @@ #include #include -#include "behaviortree_cpp_v3/condition_node.h" +#include "behaviortree_cpp/condition_node.h" #include "nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp" @@ -68,7 +68,7 @@ BT::NodeStatus PathExpiringTimerCondition::tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("PathExpiringTimer"); diff --git a/nav2_behavior_tree/plugins/condition/time_expired_condition.cpp b/nav2_behavior_tree/plugins/condition/time_expired_condition.cpp index 64afd1a34be..f03c9711aa8 100644 --- a/nav2_behavior_tree/plugins/condition/time_expired_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/time_expired_condition.cpp @@ -16,7 +16,7 @@ #include #include -#include "behaviortree_cpp_v3/condition_node.h" +#include "behaviortree_cpp/condition_node.h" #include "nav2_behavior_tree/plugins/condition/time_expired_condition.hpp" @@ -46,7 +46,7 @@ BT::NodeStatus TimeExpiredCondition::tick() initialize(); } - if (status() == BT::NodeStatus::IDLE) { + if (!BT::isStatusActive(status())) { start_ = node_->now(); return BT::NodeStatus::FAILURE; } @@ -67,7 +67,7 @@ BT::NodeStatus TimeExpiredCondition::tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("TimeExpired"); diff --git a/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp b/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp index 33e94178deb..0ee491fbfd2 100644 --- a/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/transform_available_condition.cpp @@ -83,7 +83,7 @@ BT::NodeStatus TransformAvailableCondition::tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("TransformAvailable"); diff --git a/nav2_behavior_tree/plugins/condition/would_a_controller_recovery_help_condition.cpp b/nav2_behavior_tree/plugins/condition/would_a_controller_recovery_help_condition.cpp index f8c62064f6c..87625d4511f 100644 --- a/nav2_behavior_tree/plugins/condition/would_a_controller_recovery_help_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/would_a_controller_recovery_help_condition.cpp @@ -33,7 +33,7 @@ WouldAControllerRecoveryHelp::WouldAControllerRecoveryHelp( } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType( diff --git a/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.cpp b/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.cpp index 91de9c2e22a..603eb60f107 100644 --- a/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.cpp @@ -35,7 +35,7 @@ WouldAPlannerRecoveryHelp::WouldAPlannerRecoveryHelp( } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType( diff --git a/nav2_behavior_tree/plugins/condition/would_a_smoother_recovery_help_condition.cpp b/nav2_behavior_tree/plugins/condition/would_a_smoother_recovery_help_condition.cpp index fb903e1cbfc..50665256c25 100644 --- a/nav2_behavior_tree/plugins/condition/would_a_smoother_recovery_help_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/would_a_smoother_recovery_help_condition.cpp @@ -33,7 +33,7 @@ WouldASmootherRecoveryHelp::WouldASmootherRecoveryHelp( } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType( diff --git a/nav2_behavior_tree/plugins/control/pipeline_sequence.cpp b/nav2_behavior_tree/plugins/control/pipeline_sequence.cpp index 640c3ed7b4c..bc2327af284 100644 --- a/nav2_behavior_tree/plugins/control/pipeline_sequence.cpp +++ b/nav2_behavior_tree/plugins/control/pipeline_sequence.cpp @@ -35,6 +35,7 @@ PipelineSequence::PipelineSequence( BT::NodeStatus PipelineSequence::tick() { + unsigned skipped_count = 0; for (std::size_t i = 0; i < children_nodes_.size(); ++i) { auto status = children_nodes_[i]->executeTick(); switch (status) { @@ -42,6 +43,10 @@ BT::NodeStatus PipelineSequence::tick() ControlNode::haltChildren(); last_child_ticked_ = 0; // reset return status; + case BT::NodeStatus::SKIPPED: + skipped_count++; + // do nothing and continue on to the next child. + break; case BT::NodeStatus::SUCCESS: // do nothing and continue on to the next child. If it is the last child // we'll exit the loop and hit the wrap-up code at the end of the method. @@ -63,6 +68,10 @@ BT::NodeStatus PipelineSequence::tick() // Wrap up. ControlNode::haltChildren(); last_child_ticked_ = 0; // reset + if (skipped_count == children_nodes_.size()) { + // All the children were skipped + return BT::NodeStatus::SKIPPED; + } return BT::NodeStatus::SUCCESS; } diff --git a/nav2_behavior_tree/plugins/control/recovery_node.cpp b/nav2_behavior_tree/plugins/control/recovery_node.cpp index 44e56dc55ec..6eb3c6e99e5 100644 --- a/nav2_behavior_tree/plugins/control/recovery_node.cpp +++ b/nav2_behavior_tree/plugins/control/recovery_node.cpp @@ -45,12 +45,18 @@ BT::NodeStatus RecoveryNode::tick() if (current_child_idx_ == 0) { switch (child_status) { + case BT::NodeStatus::SKIPPED: + // If first child is skipped, the entire branch is considered skipped + halt(); + return BT::NodeStatus::SKIPPED; + case BT::NodeStatus::SUCCESS: - { - // reset node and return success when first child returns success - halt(); - return BT::NodeStatus::SUCCESS; - } + // reset node and return success when first child returns success + halt(); + return BT::NodeStatus::SUCCESS; + + case BT::NodeStatus::RUNNING: + return BT::NodeStatus::RUNNING; case BT::NodeStatus::FAILURE: { @@ -66,44 +72,41 @@ BT::NodeStatus RecoveryNode::tick() } } - case BT::NodeStatus::RUNNING: - { - return BT::NodeStatus::RUNNING; - } - default: - { - throw BT::LogicError("A child node must never return IDLE"); - } + throw BT::LogicError("A child node must never return IDLE"); } // end switch } else if (current_child_idx_ == 1) { switch (child_status) { + case BT::NodeStatus::SKIPPED: + { + // if we skip the recovery (maybe a precondition fails), then we + // should assume that no recovery is possible. For this reason, + // we should return FAILURE and reset the index. + // This does not count as a retry. + current_child_idx_ = 0; + ControlNode::haltChild(1); + return BT::NodeStatus::FAILURE; + } + case BT::NodeStatus::RUNNING: + return child_status; + case BT::NodeStatus::SUCCESS: { // halt second child, increment recovery count, and tick first child in next iteration ControlNode::haltChild(1); retry_count_++; - current_child_idx_--; + current_child_idx_ = 0; } break; case BT::NodeStatus::FAILURE: - { - // reset node and return failure if second child fails - halt(); - return BT::NodeStatus::FAILURE; - } - - case BT::NodeStatus::RUNNING: - { - return BT::NodeStatus::RUNNING; - } + // reset node and return failure if second child fails + halt(); + return BT::NodeStatus::FAILURE; default: - { - throw BT::LogicError("A child node must never return IDLE"); - } + throw BT::LogicError("A child node must never return IDLE"); } // end switch } } // end while loop @@ -122,7 +125,7 @@ void RecoveryNode::halt() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("RecoveryNode"); diff --git a/nav2_behavior_tree/plugins/control/round_robin_node.cpp b/nav2_behavior_tree/plugins/control/round_robin_node.cpp index 135ea09b744..9293a5b1181 100644 --- a/nav2_behavior_tree/plugins/control/round_robin_node.cpp +++ b/nav2_behavior_tree/plugins/control/round_robin_node.cpp @@ -36,18 +36,22 @@ BT::NodeStatus RoundRobinNode::tick() const auto num_children = children_nodes_.size(); setStatus(BT::NodeStatus::RUNNING); + unsigned num_skipped_children = 0; - while (num_failed_children_ < num_children) { + while (num_failed_children_ + num_skipped_children < num_children) { TreeNode * child_node = children_nodes_[current_child_idx_]; const BT::NodeStatus child_status = child_node->executeTick(); + if (child_status != BT::NodeStatus::RUNNING) { + // Increment index and wrap around to the first child + if (++current_child_idx_ == num_children) { + current_child_idx_ = 0; + } + } + switch (child_status) { case BT::NodeStatus::SUCCESS: { - // Wrap around to the first child - if (++current_child_idx_ >= num_children) { - current_child_idx_ = 0; - } num_failed_children_ = 0; ControlNode::haltChildren(); return BT::NodeStatus::SUCCESS; @@ -55,27 +59,27 @@ BT::NodeStatus RoundRobinNode::tick() case BT::NodeStatus::FAILURE: { - if (++current_child_idx_ >= num_children) { - current_child_idx_ = 0; - } num_failed_children_++; break; } - case BT::NodeStatus::RUNNING: + case BT::NodeStatus::SKIPPED: { - return BT::NodeStatus::RUNNING; + num_skipped_children++; + break; } + case BT::NodeStatus::RUNNING: + return BT::NodeStatus::RUNNING; default: - { - throw BT::LogicError("Invalid status return from BT node"); - } + throw BT::LogicError("Invalid status return from BT node"); } } + const bool all_skipped = (num_skipped_children == num_children); halt(); - return BT::NodeStatus::FAILURE; + // If all the children were skipped, this node is considered skipped + return all_skipped ? BT::NodeStatus::SKIPPED : BT::NodeStatus::FAILURE; } void RoundRobinNode::halt() diff --git a/nav2_behavior_tree/plugins/decorator/distance_controller.cpp b/nav2_behavior_tree/plugins/decorator/distance_controller.cpp index b59fba77b45..7f87695416d 100644 --- a/nav2_behavior_tree/plugins/decorator/distance_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/distance_controller.cpp @@ -23,7 +23,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "tf2_ros/buffer.h" -#include "behaviortree_cpp_v3/decorator_node.h" +#include "behaviortree_cpp/decorator_node.h" #include "nav2_behavior_tree/plugins/decorator/distance_controller.hpp" #include "nav2_behavior_tree/bt_utils.hpp" @@ -43,15 +43,15 @@ DistanceController::DistanceController( tf_ = config().blackboard->get>("tf_buffer"); node_->get_parameter("transform_tolerance", transform_tolerance_); - global_frame_ = BT::deconflictPortAndParamFrame( + global_frame_ = BT::deconflictPortAndParamFrame( node_, "global_frame", this); - robot_base_frame_ = BT::deconflictPortAndParamFrame( + robot_base_frame_ = BT::deconflictPortAndParamFrame( node_, "robot_base_frame", this); } inline BT::NodeStatus DistanceController::tick() { - if (status() == BT::NodeStatus::IDLE) { + if (!BT::isStatusActive(status())) { // Reset the starting position since we're starting a new iteration of // the distance controller (moving from IDLE to RUNNING) if (!nav2_util::getCurrentPose( @@ -90,8 +90,9 @@ inline BT::NodeStatus DistanceController::tick() const BT::NodeStatus child_state = child_node_->executeTick(); switch (child_state) { + case BT::NodeStatus::SKIPPED: case BT::NodeStatus::RUNNING: - return BT::NodeStatus::RUNNING; + return child_state; case BT::NodeStatus::SUCCESS: if (!nav2_util::getCurrentPose( @@ -114,7 +115,7 @@ inline BT::NodeStatus DistanceController::tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("DistanceController"); diff --git a/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp b/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp index f8a2d8cefc2..d0de9205452 100644 --- a/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp @@ -17,9 +17,9 @@ #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" -#include "behaviortree_cpp_v3/decorator_node.h" +#include "behaviortree_cpp/decorator_node.h" #include "nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp" - +#include "nav2_behavior_tree/bt_utils.hpp" namespace nav2_behavior_tree { @@ -33,12 +33,12 @@ GoalUpdatedController::GoalUpdatedController( BT::NodeStatus GoalUpdatedController::tick() { - if (status() == BT::NodeStatus::IDLE) { + if (!BT::isStatusActive(status())) { // Reset since we're starting a new iteration of // the goal updated controller (moving from IDLE to RUNNING) - config().blackboard->get>("goals", goals_); - config().blackboard->get("goal", goal_); + BT::getInputOrBlackboard("goals", goals_); + BT::getInputOrBlackboard("goal", goal_); goal_was_updated_ = true; } @@ -46,9 +46,9 @@ BT::NodeStatus GoalUpdatedController::tick() setStatus(BT::NodeStatus::RUNNING); std::vector current_goals; - config().blackboard->get>("goals", current_goals); + BT::getInputOrBlackboard("goals", current_goals); geometry_msgs::msg::PoseStamped current_goal; - config().blackboard->get("goal", current_goal); + BT::getInputOrBlackboard("goal", current_goal); if (goal_ != current_goal || goals_ != current_goals) { goal_ = current_goal; @@ -61,19 +61,7 @@ BT::NodeStatus GoalUpdatedController::tick() // 'til completion if ((child_node_->status() == BT::NodeStatus::RUNNING) || goal_was_updated_) { goal_was_updated_ = false; - const BT::NodeStatus child_state = child_node_->executeTick(); - - switch (child_state) { - case BT::NodeStatus::RUNNING: - return BT::NodeStatus::RUNNING; - - case BT::NodeStatus::SUCCESS: - return BT::NodeStatus::SUCCESS; - - case BT::NodeStatus::FAILURE: - default: - return BT::NodeStatus::FAILURE; - } + return child_node_->executeTick(); } return status(); @@ -81,7 +69,7 @@ BT::NodeStatus GoalUpdatedController::tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("GoalUpdatedController"); diff --git a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp index 1f7226e6aac..fa5badf750d 100644 --- a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp +++ b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp @@ -17,7 +17,7 @@ #include #include "geometry_msgs/msg/pose_stamped.hpp" -#include "behaviortree_cpp_v3/decorator_node.h" +#include "behaviortree_cpp/decorator_node.h" #include "nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp" @@ -84,7 +84,7 @@ GoalUpdater::callback_updated_goal(const geometry_msgs::msg::PoseStamped::Shared } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("GoalUpdater"); diff --git a/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp b/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp index d4b40b0964e..cb39a245579 100644 --- a/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp +++ b/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp @@ -62,7 +62,7 @@ inline BT::NodeStatus PathLongerOnApproach::tick() getInput("prox_len", prox_len_); getInput("length_factor", length_factor_); - if (status() == BT::NodeStatus::IDLE) { + if (!BT::isStatusActive(status())) { // Reset the starting point since we're starting a new iteration of // PathLongerOnApproach (moving from IDLE to RUNNING) first_time_ = true; @@ -77,14 +77,14 @@ inline BT::NodeStatus PathLongerOnApproach::tick() { const BT::NodeStatus child_state = child_node_->executeTick(); switch (child_state) { + case BT::NodeStatus::SKIPPED: case BT::NodeStatus::RUNNING: - return BT::NodeStatus::RUNNING; + return child_state; case BT::NodeStatus::SUCCESS: - old_path_ = new_path_; - return BT::NodeStatus::SUCCESS; case BT::NodeStatus::FAILURE: old_path_ = new_path_; - return BT::NodeStatus::FAILURE; + resetChild(); + return child_state; default: old_path_ = new_path_; return BT::NodeStatus::FAILURE; @@ -97,7 +97,7 @@ inline BT::NodeStatus PathLongerOnApproach::tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("PathLongerOnApproach"); diff --git a/nav2_behavior_tree/plugins/decorator/rate_controller.cpp b/nav2_behavior_tree/plugins/decorator/rate_controller.cpp index b710ec30091..9592d119c96 100644 --- a/nav2_behavior_tree/plugins/decorator/rate_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/rate_controller.cpp @@ -43,7 +43,7 @@ BT::NodeStatus RateController::tick() initialize(); } - if (status() == BT::NodeStatus::IDLE) { + if (!BT::isStatusActive(status())) { // Reset the starting point since we're starting a new iteration of // the rate controller (moving from IDLE to RUNNING) start_ = std::chrono::high_resolution_clock::now(); @@ -70,14 +70,15 @@ BT::NodeStatus RateController::tick() const BT::NodeStatus child_state = child_node_->executeTick(); switch (child_state) { + case BT::NodeStatus::SKIPPED: case BT::NodeStatus::RUNNING: - return BT::NodeStatus::RUNNING; + case BT::NodeStatus::FAILURE: + return child_state; case BT::NodeStatus::SUCCESS: start_ = std::chrono::high_resolution_clock::now(); // Reset the timer return BT::NodeStatus::SUCCESS; - case BT::NodeStatus::FAILURE: default: return BT::NodeStatus::FAILURE; } @@ -88,7 +89,7 @@ BT::NodeStatus RateController::tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("RateController"); diff --git a/nav2_behavior_tree/plugins/decorator/single_trigger_node.cpp b/nav2_behavior_tree/plugins/decorator/single_trigger_node.cpp index 84f0fadfbbc..c95d646438a 100644 --- a/nav2_behavior_tree/plugins/decorator/single_trigger_node.cpp +++ b/nav2_behavior_tree/plugins/decorator/single_trigger_node.cpp @@ -30,7 +30,7 @@ SingleTrigger::SingleTrigger( BT::NodeStatus SingleTrigger::tick() { - if (status() == BT::NodeStatus::IDLE) { + if (!BT::isStatusActive(status())) { first_time_ = true; } @@ -40,16 +40,14 @@ BT::NodeStatus SingleTrigger::tick() const BT::NodeStatus child_state = child_node_->executeTick(); switch (child_state) { + case BT::NodeStatus::SKIPPED: case BT::NodeStatus::RUNNING: - return BT::NodeStatus::RUNNING; - - case BT::NodeStatus::SUCCESS: - first_time_ = false; - return BT::NodeStatus::SUCCESS; + return child_state; case BT::NodeStatus::FAILURE: + case BT::NodeStatus::SUCCESS: first_time_ = false; - return BT::NodeStatus::FAILURE; + return child_state; default: first_time_ = false; @@ -62,7 +60,7 @@ BT::NodeStatus SingleTrigger::tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("SingleTrigger"); diff --git a/nav2_behavior_tree/plugins/decorator/speed_controller.cpp b/nav2_behavior_tree/plugins/decorator/speed_controller.cpp index f47dfed38a8..b8e5b3915a3 100644 --- a/nav2_behavior_tree/plugins/decorator/speed_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/speed_controller.cpp @@ -19,6 +19,7 @@ #include "nav2_util/geometry_utils.hpp" #include "nav2_behavior_tree/plugins/decorator/speed_controller.hpp" +#include "nav2_behavior_tree/bt_utils.hpp" namespace nav2_behavior_tree { @@ -58,20 +59,20 @@ SpeedController::SpeedController( inline BT::NodeStatus SpeedController::tick() { - if (status() == BT::NodeStatus::IDLE) { + if (!BT::isStatusActive(status())) { // Reset since we're starting a new iteration of // the speed controller (moving from IDLE to RUNNING) - config().blackboard->get>("goals", goals_); - config().blackboard->get("goal", goal_); + BT::getInputOrBlackboard("goals", goals_); + BT::getInputOrBlackboard("goal", goal_); period_ = 1.0 / max_rate_; start_ = node_->now(); first_tick_ = true; } std::vector current_goals; - config().blackboard->get>("goals", current_goals); + BT::getInputOrBlackboard("goals", current_goals); geometry_msgs::msg::PoseStamped current_goal; - config().blackboard->get("goal", current_goal); + BT::getInputOrBlackboard("goal", current_goal); if (goal_ != current_goal || goals_ != current_goals) { // Reset state and set period to max since we have a new goal @@ -100,19 +101,7 @@ inline BT::NodeStatus SpeedController::tick() start_ = node_->now(); } - const BT::NodeStatus child_state = child_node_->executeTick(); - - switch (child_state) { - case BT::NodeStatus::RUNNING: - return BT::NodeStatus::RUNNING; - - case BT::NodeStatus::SUCCESS: - return BT::NodeStatus::SUCCESS; - - case BT::NodeStatus::FAILURE: - default: - return BT::NodeStatus::FAILURE; - } + return child_node_->executeTick(); } return status(); @@ -120,7 +109,7 @@ inline BT::NodeStatus SpeedController::tick() } // namespace nav2_behavior_tree -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" BT_REGISTER_NODES(factory) { factory.registerNodeType("SpeedController"); diff --git a/nav2_behavior_tree/plugins_list.hpp.in b/nav2_behavior_tree/plugins_list.hpp.in new file mode 100644 index 00000000000..148583b9271 --- /dev/null +++ b/nav2_behavior_tree/plugins_list.hpp.in @@ -0,0 +1,6 @@ + +// This was automativally generated by cmake +namespace nav2::details +{ + const char* BT_BUILTIN_PLUGINS = "@plugin_libs@"; +} diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp index 429368ea96a..8ed1cd4e71b 100644 --- a/nav2_behavior_tree/src/behavior_tree_engine.cpp +++ b/nav2_behavior_tree/src/behavior_tree_engine.cpp @@ -20,7 +20,7 @@ #include #include "rclcpp/rclcpp.hpp" -#include "behaviortree_cpp_v3/utils/shared_library.h" +#include "behaviortree_cpp/utils/shared_library.h" namespace nav2_behavior_tree { @@ -32,6 +32,11 @@ BehaviorTreeEngine::BehaviorTreeEngine( for (const auto & p : plugin_libraries) { factory_.registerFromPlugin(loader.getOSName(p)); } + + // FIXME: the next two line are needed for back-compatibility with BT.CPP 3.8.x + // Note that the can be removed, once we migrate from BT.CPP 4.5.x to 4.6+ + BT::ReactiveSequence::EnableException(false); + BT::ReactiveFallback::EnableException(false); } BtStatus @@ -48,11 +53,11 @@ BehaviorTreeEngine::run( try { while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) { if (cancelRequested()) { - tree->rootNode()->halt(); + tree->haltTree(); return BtStatus::CANCELED; } - result = tree->tickRoot(); + result = tree->tickOnce(); onLoop(); diff --git a/nav2_behavior_tree/src/generate_nav2_tree_nodes_xml.cpp b/nav2_behavior_tree/src/generate_nav2_tree_nodes_xml.cpp new file mode 100644 index 00000000000..4992e2e7775 --- /dev/null +++ b/nav2_behavior_tree/src/generate_nav2_tree_nodes_xml.cpp @@ -0,0 +1,47 @@ +// Copyright (c) 2024 Davide Faconti +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#include +#include +#include +#include + +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/bt_factory.h" +#include "behaviortree_cpp/utils/shared_library.h" +#include "behaviortree_cpp/xml_parsing.h" + +#include "plugins_list.hpp" + +int main() +{ + BT::BehaviorTreeFactory factory; + + std::vector plugins_list; + boost::split(plugins_list, nav2::details::BT_BUILTIN_PLUGINS, boost::is_any_of(";")); + + for (const auto & plugin : plugins_list) { + std::cout << "Loading: " << plugin << "\n"; + factory.registerFromPlugin(BT::SharedLibrary::getOSName(plugin)); + } + std::cout << "\nGenerating file: nav2_tree_nodes.xml\n" + << "\nCompare it with the one in the git repo and update the latter if necessary.\n"; + + std::ofstream xml_file; + xml_file.open("nav2_tree_nodes.xml"); + xml_file << BT::writeTreeNodesModelXML(factory) << std::endl; + xml_file.close(); + + return 0; +} diff --git a/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_action.cpp b/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_action.cpp index cbb5606d8bd..c5d40f0f4e6 100644 --- a/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_action.cpp @@ -17,7 +17,7 @@ #include #include -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp" @@ -123,7 +123,7 @@ TEST_F(AssistedTeleopActionTestFixture, test_ports) { std::string xml_txt = R"( - + @@ -134,7 +134,7 @@ TEST_F(AssistedTeleopActionTestFixture, test_ports) xml_txt = R"( - + @@ -148,7 +148,7 @@ TEST_F(AssistedTeleopActionTestFixture, test_tick) { std::string xml_txt = R"( - + @@ -172,7 +172,7 @@ TEST_F(AssistedTeleopActionTestFixture, test_failure) { std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_cancel_node.cpp index a9261fe4b8d..a516d868e4f 100644 --- a/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_cancel_node.cpp @@ -17,7 +17,7 @@ #include #include -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/assisted_teleop_cancel_node.hpp" @@ -122,7 +122,7 @@ TEST_F(CancelAssistedTeleopActionTestFixture, test_ports) { std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp b/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp index 9df8b3da154..846e7e86db9 100644 --- a/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp @@ -18,7 +18,7 @@ #include #include -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/back_up_action.hpp" @@ -122,7 +122,7 @@ TEST_F(BackUpActionTestFixture, test_ports) { std::string xml_txt = R"( - + @@ -134,7 +134,7 @@ TEST_F(BackUpActionTestFixture, test_ports) xml_txt = R"( - + @@ -149,7 +149,7 @@ TEST_F(BackUpActionTestFixture, test_tick) { std::string xml_txt = R"( - + @@ -174,7 +174,7 @@ TEST_F(BackUpActionTestFixture, test_failure) { std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_back_up_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_back_up_cancel_node.cpp index c7f379a64c2..a3ea28791c6 100644 --- a/nav2_behavior_tree/test/plugins/action/test_back_up_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_back_up_cancel_node.cpp @@ -17,7 +17,7 @@ #include #include -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/back_up_cancel_node.hpp" @@ -120,7 +120,7 @@ TEST_F(CancelBackUpActionTestFixture, test_ports) { std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp b/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp index ec3aa03a079..8aa1c3366eb 100644 --- a/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp @@ -23,7 +23,7 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "nav2_behavior_tree/bt_action_node.hpp" #include "test_msgs/action/fibonacci.hpp" @@ -238,7 +238,7 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_success) // create tree std::string xml_txt = R"( - + @@ -264,7 +264,7 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_success) // main BT execution loop while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) { - result = tree_->tickRoot(); + result = tree_->tickOnce(); ticks++; loopRate.sleep(); } @@ -307,7 +307,7 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_success) // main BT execution loop while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) { - result = tree_->tickRoot(); + result = tree_->tickOnce(); ticks++; loopRate.sleep(); } @@ -316,8 +316,10 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_success) // the BT should have failed EXPECT_EQ(result, BT::NodeStatus::FAILURE); - // since the server timeout is 20ms and bt loop duration is 10ms, number of ticks should be 2 - EXPECT_EQ(ticks, 2); + // since the server timeout is 20ms and bt loop duration is 10ms, number of ticks should + // be at most 2, but it can be 1 too, because the tickOnce may execute two ticks. + EXPECT_LE(ticks, 2); + EXPECT_GE(ticks, 1); } TEST_F(BTActionNodeTestFixture, test_server_timeout_failure) @@ -325,7 +327,7 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_failure) // create tree std::string xml_txt = R"( - + @@ -352,7 +354,7 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_failure) // main BT execution loop while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) { - result = tree_->tickRoot(); + result = tree_->tickOnce(); ticks++; loopRate.sleep(); } @@ -386,7 +388,7 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_failure) // main BT execution loop while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) { - result = tree_->tickRoot(); + result = tree_->tickOnce(); ticks++; loopRate.sleep(); } @@ -401,7 +403,7 @@ TEST_F(BTActionNodeTestFixture, test_server_cancel) // create tree std::string xml_txt = R"( - + @@ -429,7 +431,7 @@ TEST_F(BTActionNodeTestFixture, test_server_cancel) // main BT execution loop while (rclcpp::ok() && result == BT::NodeStatus::RUNNING && ticks < 5) { - result = tree_->tickRoot(); + result = tree_->tickOnce(); ticks++; loopRate.sleep(); } @@ -461,7 +463,7 @@ TEST_F(BTActionNodeTestFixture, test_server_cancel) // main BT execution loop while (rclcpp::ok() && result == BT::NodeStatus::RUNNING && ticks < 7) { - result = tree_->tickRoot(); + result = tree_->tickOnce(); ticks++; loopRate.sleep(); } diff --git a/nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp b/nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp index e21f9802d7e..76f3b1d025a 100644 --- a/nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp @@ -18,7 +18,7 @@ #include #include -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_service.hpp" #include "nav2_behavior_tree/plugins/action/clear_costmap_service.hpp" @@ -100,7 +100,7 @@ TEST_F(ClearEntireCostmapServiceTestFixture, test_tick) { std::string xml_txt = R"( - + @@ -195,7 +195,7 @@ TEST_F(ClearCostmapExceptRegionServiceTestFixture, test_tick) { std::string xml_txt = R"( - + @@ -290,7 +290,7 @@ TEST_F(ClearCostmapAroundRobotServiceTestFixture, test_tick) { std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp b/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp index 001c3de67f9..24d10b63d63 100644 --- a/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp @@ -22,7 +22,7 @@ #include "nav_msgs/msg/path.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp" @@ -128,7 +128,7 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick) // create tree std::string xml_txt = R"( - + @@ -156,13 +156,13 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick) // check if returned path is correct nav_msgs::msg::Path path; - config_->blackboard->get("path", path); + EXPECT_TRUE(config_->blackboard->get("path", path)); EXPECT_EQ(path.poses.size(), 2u); EXPECT_EQ(path.poses[0].pose.position.x, 0.0); EXPECT_EQ(path.poses[1].pose.position.x, 1.0); // halt node so another goal can be sent - tree_->rootNode()->halt(); + tree_->haltTree(); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); // set new goal @@ -176,7 +176,7 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick) EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_EQ(action_server_->getCurrentGoal()->goals[0].pose.position.x, -2.5); - config_->blackboard->get("path", path); + EXPECT_TRUE(config_->blackboard->get("path", path)); EXPECT_EQ(path.poses.size(), 2u); EXPECT_EQ(path.poses[0].pose.position.x, 0.0); EXPECT_EQ(path.poses[1].pose.position.x, -2.5); @@ -187,7 +187,7 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick_use_start) // create tree std::string xml_txt = R"( - + @@ -222,13 +222,13 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick_use_start) // check if returned path is correct nav_msgs::msg::Path path; - config_->blackboard->get("path", path); + EXPECT_TRUE(config_->blackboard->get("path", path)); EXPECT_EQ(path.poses.size(), 2u); EXPECT_EQ(path.poses[0].pose.position.x, 2.0); EXPECT_EQ(path.poses[1].pose.position.x, 1.0); // halt node so another goal can be sent - tree_->rootNode()->halt(); + tree_->haltTree(); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); // set new goal and new start @@ -244,7 +244,7 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick_use_start) EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_EQ(action_server_->getCurrentGoal()->goals[0].pose.position.x, -2.5); - config_->blackboard->get("path", path); + EXPECT_TRUE(config_->blackboard->get("path", path)); EXPECT_EQ(path.poses.size(), 2u); EXPECT_EQ(path.poses[0].pose.position.x, -1.5); EXPECT_EQ(path.poses[1].pose.position.x, -2.5); diff --git a/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp b/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp index f1da97cb25c..29ebaef9362 100644 --- a/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp @@ -21,7 +21,7 @@ #include "nav_msgs/msg/path.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp" @@ -125,7 +125,7 @@ TEST_F(ComputePathToPoseActionTestFixture, test_tick) // create tree std::string xml_txt = R"( - + @@ -153,13 +153,13 @@ TEST_F(ComputePathToPoseActionTestFixture, test_tick) // check if returned path is correct nav_msgs::msg::Path path; - config_->blackboard->get("path", path); + EXPECT_TRUE(config_->blackboard->get("path", path)); EXPECT_EQ(path.poses.size(), 2u); EXPECT_EQ(path.poses[0].pose.position.x, 0.0); EXPECT_EQ(path.poses[1].pose.position.x, 1.0); // halt node so another goal can be sent - tree_->rootNode()->halt(); + tree_->haltTree(); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); // set new goal @@ -173,7 +173,7 @@ TEST_F(ComputePathToPoseActionTestFixture, test_tick) EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_EQ(action_server_->getCurrentGoal()->goal.pose.position.x, -2.5); - config_->blackboard->get("path", path); + EXPECT_TRUE(config_->blackboard->get("path", path)); EXPECT_EQ(path.poses.size(), 2u); EXPECT_EQ(path.poses[0].pose.position.x, 0.0); EXPECT_EQ(path.poses[1].pose.position.x, -2.5); @@ -184,7 +184,7 @@ TEST_F(ComputePathToPoseActionTestFixture, test_tick_use_start) // create tree std::string xml_txt = R"( - + @@ -219,13 +219,13 @@ TEST_F(ComputePathToPoseActionTestFixture, test_tick_use_start) // check if returned path is correct nav_msgs::msg::Path path; - config_->blackboard->get("path", path); + EXPECT_TRUE(config_->blackboard->get("path", path)); EXPECT_EQ(path.poses.size(), 2u); EXPECT_EQ(path.poses[0].pose.position.x, 2.0); EXPECT_EQ(path.poses[1].pose.position.x, 1.0); // halt node so another goal can be sent - tree_->rootNode()->halt(); + tree_->haltTree(); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); // set new goal and new start @@ -241,7 +241,7 @@ TEST_F(ComputePathToPoseActionTestFixture, test_tick_use_start) EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_EQ(action_server_->getCurrentGoal()->goal.pose.position.x, -2.5); - config_->blackboard->get("path", path); + EXPECT_TRUE(config_->blackboard->get("path", path)); EXPECT_EQ(path.poses.size(), 2u); EXPECT_EQ(path.poses[0].pose.position.x, -1.5); EXPECT_EQ(path.poses[1].pose.position.x, -2.5); diff --git a/nav2_behavior_tree/test/plugins/action/test_controller_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_controller_cancel_node.cpp index 7be55c08ae0..564e72d3d54 100644 --- a/nav2_behavior_tree/test/plugins/action/test_controller_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_controller_cancel_node.cpp @@ -17,7 +17,7 @@ #include #include -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/controller_cancel_node.hpp" @@ -120,7 +120,7 @@ TEST_F(CancelControllerActionTestFixture, test_ports) { std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_controller_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_controller_selector_node.cpp index 8c98db78b30..c7f04ae9a95 100644 --- a/nav2_behavior_tree/test/plugins/action/test_controller_selector_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_controller_selector_node.cpp @@ -20,7 +20,7 @@ #include #include "utils/test_action_server.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "nav2_behavior_tree/plugins/action/controller_selector_node.hpp" #include "nav_msgs/msg/path.hpp" #include "std_msgs/msg/string.hpp" @@ -80,7 +80,7 @@ TEST_F(ControllerSelectorTestFixture, test_custom_topic) // create tree std::string xml_txt = R"( - + @@ -95,7 +95,7 @@ TEST_F(ControllerSelectorTestFixture, test_custom_topic) // check default value std::string selected_controller_result; - config_->blackboard->get("selected_controller", selected_controller_result); + EXPECT_TRUE(config_->blackboard->get("selected_controller", selected_controller_result)); EXPECT_EQ(selected_controller_result, "DWB"); @@ -119,7 +119,7 @@ TEST_F(ControllerSelectorTestFixture, test_custom_topic) } // check controller updated - config_->blackboard->get("selected_controller", selected_controller_result); + EXPECT_TRUE(config_->blackboard->get("selected_controller", selected_controller_result)); EXPECT_EQ("DWC", selected_controller_result); } @@ -128,7 +128,7 @@ TEST_F(ControllerSelectorTestFixture, test_default_topic) // create tree std::string xml_txt = R"( - + @@ -143,7 +143,7 @@ TEST_F(ControllerSelectorTestFixture, test_default_topic) // check default value std::string selected_controller_result; - config_->blackboard->get("selected_controller", selected_controller_result); + EXPECT_TRUE(config_->blackboard->get("selected_controller", selected_controller_result)); EXPECT_EQ(selected_controller_result, "GridBased"); @@ -167,7 +167,7 @@ TEST_F(ControllerSelectorTestFixture, test_default_topic) } // check controller updated - config_->blackboard->get("selected_controller", selected_controller_result); + EXPECT_TRUE(config_->blackboard->get("selected_controller", selected_controller_result)); EXPECT_EQ("RRT", selected_controller_result); } diff --git a/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp b/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp index c4439f1e1cf..baa5ea02a2c 100644 --- a/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp @@ -18,7 +18,7 @@ #include #include -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp" @@ -118,7 +118,7 @@ TEST_F(DriveOnHeadingActionTestFixture, test_ports) { std::string xml_txt = R"( - + @@ -131,7 +131,7 @@ TEST_F(DriveOnHeadingActionTestFixture, test_ports) xml_txt = R"( - + @@ -146,7 +146,7 @@ TEST_F(DriveOnHeadingActionTestFixture, test_tick) { std::string xml_txt = R"( - + @@ -169,7 +169,7 @@ TEST_F(DriveOnHeadingActionTestFixture, test_failure) { std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_cancel_node.cpp index 762351bdd1b..c94ed1d89f8 100644 --- a/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_cancel_node.cpp @@ -17,7 +17,7 @@ #include #include -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/drive_on_heading_cancel_node.hpp" @@ -123,7 +123,7 @@ TEST_F(CancelDriveOnHeadingTestFixture, test_ports) { std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp b/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp index fea7a2b2b96..4f272208198 100644 --- a/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp @@ -21,7 +21,7 @@ #include "nav_msgs/msg/path.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/follow_path_action.hpp" @@ -118,7 +118,7 @@ TEST_F(FollowPathActionTestFixture, test_tick) // create tree std::string xml_txt = R"( - + @@ -145,7 +145,7 @@ TEST_F(FollowPathActionTestFixture, test_tick) EXPECT_EQ(action_server_->getCurrentGoal()->controller_id, std::string("FollowPath")); // halt node so another goal can be sent - tree_->rootNode()->halt(); + tree_->haltTree(); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); // set new goal diff --git a/nav2_behavior_tree/test/plugins/action/test_goal_checker_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_goal_checker_selector_node.cpp index 1fd0e286cb3..19089fb6f3e 100644 --- a/nav2_behavior_tree/test/plugins/action/test_goal_checker_selector_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_goal_checker_selector_node.cpp @@ -20,7 +20,7 @@ #include #include "utils/test_action_server.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "nav2_behavior_tree/plugins/action/goal_checker_selector_node.hpp" #include "nav_msgs/msg/path.hpp" #include "std_msgs/msg/string.hpp" @@ -80,7 +80,7 @@ TEST_F(GoalCheckerSelectorTestFixture, test_custom_topic) // create tree std::string xml_txt = R"( - + @@ -95,7 +95,7 @@ TEST_F(GoalCheckerSelectorTestFixture, test_custom_topic) // check default value std::string selected_goal_checker_result; - config_->blackboard->get("selected_goal_checker", selected_goal_checker_result); + EXPECT_TRUE(config_->blackboard->get("selected_goal_checker", selected_goal_checker_result)); EXPECT_EQ(selected_goal_checker_result, "SimpleGoalCheck"); @@ -119,7 +119,7 @@ TEST_F(GoalCheckerSelectorTestFixture, test_custom_topic) } // check goal_checker updated - config_->blackboard->get("selected_goal_checker", selected_goal_checker_result); + EXPECT_TRUE(config_->blackboard->get("selected_goal_checker", selected_goal_checker_result)); EXPECT_EQ("AngularGoalChecker", selected_goal_checker_result); } @@ -128,7 +128,7 @@ TEST_F(GoalCheckerSelectorTestFixture, test_default_topic) // create tree std::string xml_txt = R"( - + @@ -143,7 +143,7 @@ TEST_F(GoalCheckerSelectorTestFixture, test_default_topic) // check default value std::string selected_goal_checker_result; - config_->blackboard->get("selected_goal_checker", selected_goal_checker_result); + EXPECT_TRUE(config_->blackboard->get("selected_goal_checker", selected_goal_checker_result)); EXPECT_EQ(selected_goal_checker_result, "GridBased"); @@ -167,7 +167,7 @@ TEST_F(GoalCheckerSelectorTestFixture, test_default_topic) } // check goal_checker updated - config_->blackboard->get("selected_goal_checker", selected_goal_checker_result); + EXPECT_TRUE(config_->blackboard->get("selected_goal_checker", selected_goal_checker_result)); EXPECT_EQ("RRT", selected_goal_checker_result); } diff --git a/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp b/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp index 90cdfbbb0ca..a805a721b9f 100644 --- a/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp @@ -23,7 +23,7 @@ #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/quaternion.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp" @@ -124,7 +124,7 @@ TEST_F(NavigateThroughPosesActionTestFixture, test_tick) // create tree std::string xml_txt = R"( - + @@ -148,7 +148,7 @@ TEST_F(NavigateThroughPosesActionTestFixture, test_tick) EXPECT_EQ(action_server_->getCurrentGoal()->poses, poses); // halt node so another goal can be sent - tree_->rootNode()->halt(); + tree_->haltTree(); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); } diff --git a/nav2_behavior_tree/test/plugins/action/test_navigate_to_pose_action.cpp b/nav2_behavior_tree/test/plugins/action/test_navigate_to_pose_action.cpp index 5b3764befb9..4ed5120b986 100644 --- a/nav2_behavior_tree/test/plugins/action/test_navigate_to_pose_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_navigate_to_pose_action.cpp @@ -22,7 +22,7 @@ #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/quaternion.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp" @@ -119,7 +119,7 @@ TEST_F(NavigateToPoseActionTestFixture, test_tick) // create tree std::string xml_txt = R"( - + @@ -139,7 +139,7 @@ TEST_F(NavigateToPoseActionTestFixture, test_tick) EXPECT_EQ(action_server_->getCurrentGoal()->pose, pose); // halt node so another goal can be sent - tree_->rootNode()->halt(); + tree_->haltTree(); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); // set new goal diff --git a/nav2_behavior_tree/test/plugins/action/test_planner_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_planner_selector_node.cpp index b8851d81c36..ade80c57daf 100644 --- a/nav2_behavior_tree/test/plugins/action/test_planner_selector_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_planner_selector_node.cpp @@ -20,7 +20,7 @@ #include #include "utils/test_action_server.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "nav2_behavior_tree/plugins/action/planner_selector_node.hpp" #include "nav_msgs/msg/path.hpp" #include "std_msgs/msg/string.hpp" @@ -78,7 +78,7 @@ TEST_F(PlannerSelectorTestFixture, test_custom_topic) // create tree std::string xml_txt = R"( - + @@ -93,7 +93,7 @@ TEST_F(PlannerSelectorTestFixture, test_custom_topic) // check default value std::string selected_planner_result; - config_->blackboard->get("selected_planner", selected_planner_result); + EXPECT_TRUE(config_->blackboard->get("selected_planner", selected_planner_result)); EXPECT_EQ(selected_planner_result, "GridBased"); @@ -117,7 +117,7 @@ TEST_F(PlannerSelectorTestFixture, test_custom_topic) } // check planner updated - config_->blackboard->get("selected_planner", selected_planner_result); + EXPECT_TRUE(config_->blackboard->get("selected_planner", selected_planner_result)); EXPECT_EQ("RRT", selected_planner_result); } @@ -126,7 +126,7 @@ TEST_F(PlannerSelectorTestFixture, test_default_topic) // create tree std::string xml_txt = R"( - + @@ -141,7 +141,7 @@ TEST_F(PlannerSelectorTestFixture, test_default_topic) // check default value std::string selected_planner_result; - config_->blackboard->get("selected_planner", selected_planner_result); + EXPECT_TRUE(config_->blackboard->get("selected_planner", selected_planner_result)); EXPECT_EQ(selected_planner_result, "GridBased"); @@ -165,7 +165,7 @@ TEST_F(PlannerSelectorTestFixture, test_default_topic) } // check planner updated - config_->blackboard->get("selected_planner", selected_planner_result); + EXPECT_TRUE(config_->blackboard->get("selected_planner", selected_planner_result)); EXPECT_EQ("RRT", selected_planner_result); } diff --git a/nav2_behavior_tree/test/plugins/action/test_progress_checker_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_progress_checker_selector_node.cpp index 574a514886b..c97bbc19699 100644 --- a/nav2_behavior_tree/test/plugins/action/test_progress_checker_selector_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_progress_checker_selector_node.cpp @@ -19,7 +19,7 @@ #include #include "utils/test_action_server.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp" #include "nav_msgs/msg/path.hpp" #include "std_msgs/msg/string.hpp" @@ -79,7 +79,7 @@ TEST_F(ProgressCheckerSelectorTestFixture, test_custom_topic) // create tree std::string xml_txt = R"( - + @@ -94,7 +94,9 @@ TEST_F(ProgressCheckerSelectorTestFixture, test_custom_topic) // check default value std::string selected_progress_checker_result; - config_->blackboard->get("selected_progress_checker", selected_progress_checker_result); + [[maybe_unused]] auto res = config_->blackboard->get( + "selected_progress_checker", + selected_progress_checker_result); EXPECT_EQ(selected_progress_checker_result, "SimpleProgressCheck"); @@ -119,7 +121,7 @@ TEST_F(ProgressCheckerSelectorTestFixture, test_custom_topic) } // check progress_checker updated - config_->blackboard->get("selected_progress_checker", selected_progress_checker_result); + res = config_->blackboard->get("selected_progress_checker", selected_progress_checker_result); EXPECT_EQ("AngularProgressChecker", selected_progress_checker_result); } @@ -128,7 +130,7 @@ TEST_F(ProgressCheckerSelectorTestFixture, test_default_topic) // create tree std::string xml_txt = R"( - + @@ -143,7 +145,9 @@ TEST_F(ProgressCheckerSelectorTestFixture, test_default_topic) // check default value std::string selected_progress_checker_result; - config_->blackboard->get("selected_progress_checker", selected_progress_checker_result); + [[maybe_unused]] auto res = config_->blackboard->get( + "selected_progress_checker", + selected_progress_checker_result); EXPECT_EQ(selected_progress_checker_result, "GridBased"); @@ -167,7 +171,7 @@ TEST_F(ProgressCheckerSelectorTestFixture, test_default_topic) } // check goal_checker updated - config_->blackboard->get("selected_progress_checker", selected_progress_checker_result); + res = config_->blackboard->get("selected_progress_checker", selected_progress_checker_result); EXPECT_EQ("RRT", selected_progress_checker_result); } diff --git a/nav2_behavior_tree/test/plugins/action/test_reinitialize_global_localization_service.cpp b/nav2_behavior_tree/test/plugins/action/test_reinitialize_global_localization_service.cpp index 73e72fa764d..04b0e704903 100644 --- a/nav2_behavior_tree/test/plugins/action/test_reinitialize_global_localization_service.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_reinitialize_global_localization_service.cpp @@ -18,7 +18,7 @@ #include #include -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_service.hpp" #include "nav2_behavior_tree/plugins/action/reinitialize_global_localization_service.hpp" @@ -97,7 +97,7 @@ TEST_F(ReinitializeGlobalLocalizationServiceTestFixture, test_tick) { std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp b/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp index 7bfda58e548..ce9d0debf8e 100644 --- a/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp @@ -23,7 +23,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_util/geometry_utils.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp" @@ -105,7 +105,7 @@ TEST_F(RemovePassedGoalsTestFixture, test_tick) // create tree std::string xml_txt = R"( - + @@ -137,7 +137,7 @@ TEST_F(RemovePassedGoalsTestFixture, test_tick) // check that it removed the point in range std::vector output_poses; - config_->blackboard->get("goals", output_poses); + EXPECT_TRUE(config_->blackboard->get("goals", output_poses)); EXPECT_EQ(output_poses.size(), 2u); EXPECT_EQ(output_poses[0], poses[2]); diff --git a/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp b/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp index 6460a5d9b95..d60ed2ffb78 100644 --- a/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp @@ -21,7 +21,7 @@ #include "nav_msgs/msg/path.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/smooth_path_action.hpp" @@ -118,7 +118,7 @@ TEST_F(SmoothPathActionTestFixture, test_tick) // create tree std::string xml_txt = R"( - + @@ -138,7 +138,7 @@ TEST_F(SmoothPathActionTestFixture, test_tick) EXPECT_EQ(action_server_->getCurrentGoal()->path, path); // halt node so another goal can be sent - tree_->rootNode()->halt(); + tree_->haltTree(); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); // set new goal diff --git a/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp index de73cc4fe58..f93f6df878f 100644 --- a/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp @@ -20,7 +20,7 @@ #include #include "utils/test_action_server.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "nav2_behavior_tree/plugins/action/smoother_selector_node.hpp" #include "nav_msgs/msg/path.hpp" #include "std_msgs/msg/string.hpp" @@ -80,7 +80,7 @@ TEST_F(SmootherSelectorTestFixture, test_custom_topic) // create tree std::string xml_txt = R"( - + @@ -95,7 +95,7 @@ TEST_F(SmootherSelectorTestFixture, test_custom_topic) // check default value std::string selected_smoother_result; - config_->blackboard->get("selected_smoother", selected_smoother_result); + EXPECT_TRUE(config_->blackboard->get("selected_smoother", selected_smoother_result)); EXPECT_EQ(selected_smoother_result, "DWB"); @@ -119,7 +119,7 @@ TEST_F(SmootherSelectorTestFixture, test_custom_topic) } // check smoother updated - config_->blackboard->get("selected_smoother", selected_smoother_result); + EXPECT_TRUE(config_->blackboard->get("selected_smoother", selected_smoother_result)); EXPECT_EQ("DWC", selected_smoother_result); } @@ -128,7 +128,7 @@ TEST_F(SmootherSelectorTestFixture, test_default_topic) // create tree std::string xml_txt = R"( - + @@ -143,7 +143,7 @@ TEST_F(SmootherSelectorTestFixture, test_default_topic) // check default value std::string selected_smoother_result; - config_->blackboard->get("selected_smoother", selected_smoother_result); + EXPECT_TRUE(config_->blackboard->get("selected_smoother", selected_smoother_result)); EXPECT_EQ(selected_smoother_result, "GridBased"); @@ -167,7 +167,7 @@ TEST_F(SmootherSelectorTestFixture, test_default_topic) } // check smoother updated - config_->blackboard->get("selected_smoother", selected_smoother_result); + EXPECT_TRUE(config_->blackboard->get("selected_smoother", selected_smoother_result)); EXPECT_EQ("RRT", selected_smoother_result); } diff --git a/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp b/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp index e1285d28a90..e3d2e80c85d 100644 --- a/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp @@ -18,7 +18,7 @@ #include #include -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/spin_action.hpp" @@ -122,7 +122,7 @@ TEST_F(SpinActionTestFixture, test_ports) { std::string xml_txt = R"( - + @@ -133,7 +133,7 @@ TEST_F(SpinActionTestFixture, test_ports) xml_txt = R"( - + @@ -147,7 +147,7 @@ TEST_F(SpinActionTestFixture, test_tick) { std::string xml_txt = R"( - + @@ -169,7 +169,7 @@ TEST_F(SpinActionTestFixture, test_failure) { std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp index 8cc3083eb3e..54a0270e786 100644 --- a/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp @@ -17,7 +17,7 @@ #include #include -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/spin_cancel_node.hpp" @@ -120,7 +120,7 @@ TEST_F(CancelSpinActionTestFixture, test_ports) { std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_truncate_path_action.cpp b/nav2_behavior_tree/test/plugins/action/test_truncate_path_action.cpp index 9ca640d47dd..2e7ac3fccd2 100644 --- a/nav2_behavior_tree/test/plugins/action/test_truncate_path_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_truncate_path_action.cpp @@ -22,7 +22,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_util/geometry_utils.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/truncate_path_action.hpp" @@ -87,7 +87,7 @@ TEST_F(TruncatePathTestFixture, test_tick) // create tree std::string xml_txt = R"( - + @@ -127,7 +127,7 @@ TEST_F(TruncatePathTestFixture, test_tick) } nav_msgs::msg::Path truncated_path; - config_->blackboard->get("truncated_path", truncated_path); + EXPECT_TRUE(config_->blackboard->get("truncated_path", truncated_path)); EXPECT_NE(path, truncated_path); EXPECT_EQ(truncated_path.poses.size(), 2u); diff --git a/nav2_behavior_tree/test/plugins/action/test_truncate_path_local_action.cpp b/nav2_behavior_tree/test/plugins/action/test_truncate_path_local_action.cpp index 00564d4e8fb..c3333605f89 100644 --- a/nav2_behavior_tree/test/plugins/action/test_truncate_path_local_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_truncate_path_local_action.cpp @@ -21,7 +21,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_util/geometry_utils.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_behavior_tree_fixture.hpp" #include "nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp" @@ -107,7 +107,7 @@ TEST_F(TruncatePathLocalTestFixture, test_tick) // create tree std::string xml_txt = R"( - + blackboard->get("truncated_path", truncated_path); + EXPECT_TRUE(config_->blackboard->get("truncated_path", truncated_path)); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_NE(path, truncated_path); @@ -160,7 +160,7 @@ TEST_F(TruncatePathLocalTestFixture, test_tick) { tree_->rootNode()->executeTick(); } - config_->blackboard->get("truncated_path", truncated_path); + EXPECT_TRUE(config_->blackboard->get("truncated_path", truncated_path)); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_NE(path, truncated_path); @@ -182,7 +182,7 @@ TEST_F(TruncatePathLocalTestFixture, test_tick) { tree_->rootNode()->executeTick(); } - config_->blackboard->get("truncated_path", truncated_path); + EXPECT_TRUE(config_->blackboard->get("truncated_path", truncated_path)); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_NE(path, truncated_path); @@ -201,7 +201,7 @@ TEST_F(TruncatePathLocalTestFixture, test_success_on_empty_path) // create tree std::string xml_txt = R"( - + rootNode()->executeTick(); } nav_msgs::msg::Path truncated_path; - config_->blackboard->get("truncated_path", truncated_path); + EXPECT_TRUE(config_->blackboard->get("truncated_path", truncated_path)); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_EQ(path, truncated_path); @@ -245,7 +245,7 @@ TEST_F(TruncatePathLocalTestFixture, test_failure_on_no_pose) // create tree std::string xml_txt = R"( - + rootNode()->executeTick(); } nav_msgs::msg::Path truncated_path; - config_->blackboard->get("truncated_path", truncated_path); + EXPECT_TRUE(config_->blackboard->get("truncated_path", truncated_path)); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::FAILURE); SUCCEED(); @@ -287,7 +287,7 @@ TEST_F(TruncatePathLocalTestFixture, test_failure_on_invalid_robot_frame) // create tree std::string xml_txt = R"( - + rootNode()->executeTick(); } nav_msgs::msg::Path truncated_path; - config_->blackboard->get("truncated_path", truncated_path); + EXPECT_TRUE(config_->blackboard->get("truncated_path", truncated_path)); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::FAILURE); SUCCEED(); @@ -328,7 +328,7 @@ TEST_F(TruncatePathLocalTestFixture, test_path_pruning) // create tree std::string xml_txt = R"( - + rootNode()->executeTick(); } - config_->blackboard->get("truncated_path", truncated_path); + EXPECT_TRUE(config_->blackboard->get("truncated_path", truncated_path)); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_NE(path, truncated_path); @@ -395,7 +395,7 @@ TEST_F(TruncatePathLocalTestFixture, test_path_pruning) { tree_->rootNode()->executeTick(); } - config_->blackboard->get("truncated_path", truncated_path); + EXPECT_TRUE(config_->blackboard->get("truncated_path", truncated_path)); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_NE(path, truncated_path); @@ -428,7 +428,7 @@ TEST_F(TruncatePathLocalTestFixture, test_path_pruning) { tree_->rootNode()->executeTick(); } - config_->blackboard->get("truncated_path", truncated_path); + EXPECT_TRUE(config_->blackboard->get("truncated_path", truncated_path)); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_NE(path, truncated_path); diff --git a/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp b/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp index 2e9fb8b1682..bc0acfd94d2 100644 --- a/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp @@ -18,7 +18,7 @@ #include #include -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/wait_action.hpp" @@ -113,7 +113,7 @@ TEST_F(WaitActionTestFixture, test_ports) { std::string xml_txt = R"( - + @@ -124,7 +124,7 @@ TEST_F(WaitActionTestFixture, test_ports) xml_txt = R"( - + @@ -138,7 +138,7 @@ TEST_F(WaitActionTestFixture, test_tick) { std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/action/test_wait_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_wait_cancel_node.cpp index ebce2f63413..4b55e3d1335 100644 --- a/nav2_behavior_tree/test/plugins/action/test_wait_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_wait_cancel_node.cpp @@ -17,7 +17,7 @@ #include #include -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/action/wait_cancel_node.hpp" @@ -120,7 +120,7 @@ TEST_F(CancelWaitActionTestFixture, test_ports) { std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/condition/test_are_error_codes_present.cpp b/nav2_behavior_tree/test/plugins/condition/test_are_error_codes_present.cpp index d1716397df0..111cda7d005 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_are_error_codes_present.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_are_error_codes_present.cpp @@ -34,7 +34,7 @@ class AreErrorCodesPresentFixture : public nav2_behavior_tree::BehaviorTreeTestF std::string xml_txt = R"( - + @@ -65,7 +65,7 @@ TEST_F(AreErrorCodesPresentFixture, test_condition) for (const auto & error_to_status : error_to_status_map) { config_->blackboard->set("error_code", error_to_status.first); - EXPECT_EQ(tree_->tickRoot(), error_to_status.second); + EXPECT_EQ(tree_->tickOnce(), error_to_status.second); } } diff --git a/nav2_behavior_tree/test/plugins/condition/test_goal_reached.cpp b/nav2_behavior_tree/test/plugins/condition/test_goal_reached.cpp index 1580a8d1bc0..1034c4f9b6f 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_goal_reached.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_goal_reached.cpp @@ -43,7 +43,7 @@ class GoalReachedConditionTestFixture : public nav2_behavior_tree::BehaviorTreeT std::string xml_txt = R"( - + @@ -66,32 +66,32 @@ std::shared_ptr GoalReachedConditionTestFixture::tree_ = nullptr; TEST_F(GoalReachedConditionTestFixture, test_behavior) { - EXPECT_EQ(tree_->tickRoot(), BT::NodeStatus::FAILURE); + EXPECT_EQ(tree_->tickOnce(), BT::NodeStatus::FAILURE); geometry_msgs::msg::Pose pose; pose.position.x = 0.0; pose.position.y = 0.0; transform_handler_->updateRobotPose(pose); std::this_thread::sleep_for(500ms); - EXPECT_EQ(tree_->tickRoot(), BT::NodeStatus::FAILURE); + EXPECT_EQ(tree_->tickOnce(), BT::NodeStatus::FAILURE); pose.position.x = 0.5; pose.position.y = 0.5; transform_handler_->updateRobotPose(pose); std::this_thread::sleep_for(500ms); - EXPECT_EQ(tree_->tickRoot(), BT::NodeStatus::FAILURE); + EXPECT_EQ(tree_->tickOnce(), BT::NodeStatus::FAILURE); pose.position.x = 0.9; pose.position.y = 0.9; transform_handler_->updateRobotPose(pose); std::this_thread::sleep_for(500ms); - EXPECT_EQ(tree_->tickRoot(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(tree_->tickOnce(), BT::NodeStatus::SUCCESS); pose.position.x = 1.0; pose.position.y = 1.0; transform_handler_->updateRobotPose(pose); std::this_thread::sleep_for(500ms); - EXPECT_EQ(tree_->tickRoot(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(tree_->tickOnce(), BT::NodeStatus::SUCCESS); } int main(int argc, char ** argv) diff --git a/nav2_behavior_tree/test/plugins/condition/test_initial_pose_received.cpp b/nav2_behavior_tree/test/plugins/condition/test_initial_pose_received.cpp index 1f9aa41bb84..7b8a67529ed 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_initial_pose_received.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_initial_pose_received.cpp @@ -21,48 +21,30 @@ #include "utils/test_behavior_tree_fixture.hpp" #include "nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp" -class TestNode : public BT::SyncActionNode -{ -public: - TestNode(const std::string & name, const BT::NodeConfiguration & config) - : SyncActionNode(name, config) - {} - - BT::NodeStatus tick() - { - return BT::NodeStatus::SUCCESS; - } - - static BT::PortsList providedPorts() - { - return {}; - } -}; - class InitialPoseReceivedConditionTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture { public: void SetUp() { - test_node_ = std::make_shared("TestNode", *config_); + bt_node_ = std::make_shared("TestNode", *config_); } void TearDown() { - test_node_.reset(); + bt_node_.reset(); } protected: - static std::shared_ptr test_node_; + static std::shared_ptr bt_node_; }; -std::shared_ptr InitialPoseReceivedConditionTestFixture::test_node_ = nullptr; +std::shared_ptr InitialPoseReceivedConditionTestFixture::bt_node_ = nullptr; TEST_F(InitialPoseReceivedConditionTestFixture, test_behavior) { - EXPECT_EQ(nav2_behavior_tree::initialPoseReceived(*test_node_), BT::NodeStatus::FAILURE); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); config_->blackboard->set("initial_pose_received", true); - EXPECT_EQ(nav2_behavior_tree::initialPoseReceived(*test_node_), BT::NodeStatus::SUCCESS); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); } int main(int argc, char ** argv) diff --git a/nav2_behavior_tree/test/plugins/condition/test_is_battery_charging.cpp b/nav2_behavior_tree/test/plugins/condition/test_is_battery_charging.cpp index 79fa1e08c20..8cd761aadf1 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_is_battery_charging.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_is_battery_charging.cpp @@ -73,7 +73,7 @@ TEST_F(IsBatteryChargingConditionTestFixture, test_behavior_power_supply_status) { std::string xml_txt = R"( - + @@ -86,32 +86,32 @@ TEST_F(IsBatteryChargingConditionTestFixture, test_behavior_power_supply_status) battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); rclcpp::spin_some(node_); - EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); battery_msg.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_CHARGING; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); rclcpp::spin_some(node_); - EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS); battery_msg.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_DISCHARGING; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); rclcpp::spin_some(node_); - EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); battery_msg.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_NOT_CHARGING; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); rclcpp::spin_some(node_); - EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); battery_msg.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_FULL; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); rclcpp::spin_some(node_); - EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); } int main(int argc, char ** argv) diff --git a/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp b/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp index a39a4140890..5e763f56d42 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp @@ -74,7 +74,7 @@ TEST_F(IsBatteryLowConditionTestFixture, test_behavior_percentage) { std::string xml_txt = R"( - + @@ -87,32 +87,32 @@ TEST_F(IsBatteryLowConditionTestFixture, test_behavior_percentage) battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); rclcpp::spin_some(node_); - EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); battery_msg.percentage = 0.49; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); rclcpp::spin_some(node_); - EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS); battery_msg.percentage = 0.51; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); rclcpp::spin_some(node_); - EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); battery_msg.percentage = 0.0; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); rclcpp::spin_some(node_); - EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS); } TEST_F(IsBatteryLowConditionTestFixture, test_behavior_voltage) { std::string xml_txt = R"( - + @@ -125,25 +125,25 @@ TEST_F(IsBatteryLowConditionTestFixture, test_behavior_voltage) battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); rclcpp::spin_some(node_); - EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); battery_msg.voltage = 4.9; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); rclcpp::spin_some(node_); - EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS); battery_msg.voltage = 5.1; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); rclcpp::spin_some(node_); - EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::FAILURE); battery_msg.voltage = 0.0; battery_pub_->publish(battery_msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); rclcpp::spin_some(node_); - EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(tree.tickOnce(), BT::NodeStatus::SUCCESS); } int main(int argc, char ** argv) diff --git a/nav2_behavior_tree/test/plugins/condition/test_is_path_valid.cpp b/nav2_behavior_tree/test/plugins/condition/test_is_path_valid.cpp index 9c022359a11..df973cb280a 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_is_path_valid.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_is_path_valid.cpp @@ -92,7 +92,7 @@ TEST_F(IsPathValidTestFixture, test_behavior) { std::string xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/plugins/condition/test_transform_available.cpp b/nav2_behavior_tree/test/plugins/condition/test_transform_available.cpp index 2b21fd416c4..73316dfa63f 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_transform_available.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_transform_available.cpp @@ -65,7 +65,7 @@ class TransformAvailableConditionTestFixture : public ::testing::Test { std::string xml_txt = R"( - + @@ -99,10 +99,10 @@ std::shared_ptr TransformAvailableConditionTestFixture::tree_ = nullpt TEST_F(TransformAvailableConditionTestFixture, test_behavior) { - EXPECT_EQ(tree_->tickRoot(), BT::NodeStatus::FAILURE); + EXPECT_EQ(tree_->tickOnce(), BT::NodeStatus::FAILURE); transform_handler_->activate(); transform_handler_->waitForTransform(); - EXPECT_EQ(tree_->tickRoot(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(tree_->tickOnce(), BT::NodeStatus::SUCCESS); } int main(int argc, char ** argv) diff --git a/nav2_behavior_tree/test/plugins/condition/test_would_a_controller_recovery_help.cpp b/nav2_behavior_tree/test/plugins/condition/test_would_a_controller_recovery_help.cpp index d93249879d5..839a4003fe8 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_would_a_controller_recovery_help.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_would_a_controller_recovery_help.cpp @@ -32,7 +32,7 @@ class WouldAControllerRecoveryHelpFixture : public nav2_behavior_tree::BehaviorT std::string xml_txt = R"( - + @@ -69,7 +69,7 @@ TEST_F(WouldAControllerRecoveryHelpFixture, test_condition) for (const auto & error_to_status : error_to_status_map) { config_->blackboard->set("error_code", error_to_status.first); - EXPECT_EQ(tree_->tickRoot(), error_to_status.second); + EXPECT_EQ(tree_->tickOnce(), error_to_status.second); } } diff --git a/nav2_behavior_tree/test/plugins/condition/test_would_a_planner_recovery_help.cpp b/nav2_behavior_tree/test/plugins/condition/test_would_a_planner_recovery_help.cpp index 4dc0f8bb5cb..89e09265dc4 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_would_a_planner_recovery_help.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_would_a_planner_recovery_help.cpp @@ -32,7 +32,7 @@ class WouldAPlannerRecoveryHelpFixture : public nav2_behavior_tree::BehaviorTree std::string xml_txt = R"( - + @@ -64,7 +64,7 @@ TEST_F(WouldAPlannerRecoveryHelpFixture, test_condition) for (const auto & error_to_status : error_to_status_map) { config_->blackboard->set("error_code", error_to_status.first); - EXPECT_EQ(tree_->tickRoot(), error_to_status.second); + EXPECT_EQ(tree_->tickOnce(), error_to_status.second); } } diff --git a/nav2_behavior_tree/test/plugins/condition/test_would_a_smoother_recovery_help.cpp b/nav2_behavior_tree/test/plugins/condition/test_would_a_smoother_recovery_help.cpp index 337d5133c38..7a6dff9be97 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_would_a_smoother_recovery_help.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_would_a_smoother_recovery_help.cpp @@ -32,7 +32,7 @@ class WouldASmootherRecoveryHelpFixture : public nav2_behavior_tree::BehaviorTre std::string xml_txt = R"( - + @@ -66,7 +66,7 @@ TEST_F(WouldASmootherRecoveryHelpFixture, test_condition) for (const auto & error_to_status : error_to_status_map) { config_->blackboard->set("error_code", error_to_status.first); - EXPECT_EQ(tree_->tickRoot(), error_to_status.second); + EXPECT_EQ(tree_->tickOnce(), error_to_status.second); } } diff --git a/nav2_behavior_tree/test/plugins/control/test_pipeline_sequence.cpp b/nav2_behavior_tree/test/plugins/control/test_pipeline_sequence.cpp index d50a6d4c51c..517ee1f04a8 100644 --- a/nav2_behavior_tree/test/plugins/control/test_pipeline_sequence.cpp +++ b/nav2_behavior_tree/test/plugins/control/test_pipeline_sequence.cpp @@ -125,6 +125,33 @@ TEST_F(PipelineSequenceTestFixture, test_behavior) EXPECT_EQ(third_child_->status(), BT::NodeStatus::IDLE); } +TEST_F(PipelineSequenceTestFixture, test_skipped) +{ + first_child_->changeStatus(BT::NodeStatus::SKIPPED); + second_child_->changeStatus(BT::NodeStatus::SUCCESS); + third_child_->changeStatus(BT::NodeStatus::SUCCESS); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE); + EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE); + EXPECT_EQ(third_child_->status(), BT::NodeStatus::IDLE); + + first_child_->changeStatus(BT::NodeStatus::SKIPPED); + second_child_->changeStatus(BT::NodeStatus::SUCCESS); + third_child_->changeStatus(BT::NodeStatus::FAILURE); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); + EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE); + EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE); + EXPECT_EQ(third_child_->status(), BT::NodeStatus::IDLE); + + first_child_->changeStatus(BT::NodeStatus::SKIPPED); + second_child_->changeStatus(BT::NodeStatus::SKIPPED); + third_child_->changeStatus(BT::NodeStatus::SKIPPED); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SKIPPED); + EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE); + EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE); + EXPECT_EQ(third_child_->status(), BT::NodeStatus::IDLE); +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp b/nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp index cc5e2d29699..1ca41e798e8 100644 --- a/nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp +++ b/nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp @@ -144,6 +144,26 @@ TEST_F(RecoveryNodeTestFixture, test_failure_one_retry) EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE); } +TEST_F(RecoveryNodeTestFixture, test_skipping) +{ + // first child skipped + first_child_->changeStatus(BT::NodeStatus::SKIPPED); + second_child_->changeStatus(BT::NodeStatus::SUCCESS); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SKIPPED); + EXPECT_EQ(bt_node_->status(), BT::NodeStatus::IDLE); + EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE); + EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE); + + // first child fails, second child skipped + first_child_->changeStatus(BT::NodeStatus::FAILURE); + second_child_->changeStatus(BT::NodeStatus::SKIPPED); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); + EXPECT_EQ(bt_node_->status(), BT::NodeStatus::FAILURE); + EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE); + EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE); +} + + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/nav2_behavior_tree/test/plugins/control/test_round_robin_node.cpp b/nav2_behavior_tree/test/plugins/control/test_round_robin_node.cpp index 2996bca25a8..7daa91c0697 100644 --- a/nav2_behavior_tree/test/plugins/control/test_round_robin_node.cpp +++ b/nav2_behavior_tree/test/plugins/control/test_round_robin_node.cpp @@ -114,6 +114,33 @@ TEST_F(RoundRobinNodeTestFixture, test_behavior) EXPECT_EQ(third_child_->status(), BT::NodeStatus::IDLE); } +TEST_F(RoundRobinNodeTestFixture, test_skikpped) +{ + first_child_->changeStatus(BT::NodeStatus::SKIPPED); + second_child_->changeStatus(BT::NodeStatus::SUCCESS); + third_child_->changeStatus(BT::NodeStatus::FAILURE); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE); + EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE); + EXPECT_EQ(third_child_->status(), BT::NodeStatus::IDLE); + + first_child_->changeStatus(BT::NodeStatus::SKIPPED); + second_child_->changeStatus(BT::NodeStatus::SKIPPED); + third_child_->changeStatus(BT::NodeStatus::FAILURE); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); + EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE); + EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE); + EXPECT_EQ(third_child_->status(), BT::NodeStatus::IDLE); + + first_child_->changeStatus(BT::NodeStatus::SKIPPED); + second_child_->changeStatus(BT::NodeStatus::SKIPPED); + third_child_->changeStatus(BT::NodeStatus::SKIPPED); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SKIPPED); + EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE); + EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE); + EXPECT_EQ(third_child_->status(), BT::NodeStatus::IDLE); +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp b/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp index 221371a4143..fa56a2064ab 100644 --- a/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp +++ b/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp @@ -21,7 +21,7 @@ #include "nav_msgs/msg/path.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp" @@ -86,7 +86,7 @@ TEST_F(GoalUpdaterTestFixture, test_tick) // create tree std::string xml_txt = R"( - + @@ -107,7 +107,7 @@ TEST_F(GoalUpdaterTestFixture, test_tick) // tick tree without publishing updated goal and get updated_goal tree_->rootNode()->executeTick(); geometry_msgs::msg::PoseStamped updated_goal; - config_->blackboard->get("updated_goal", updated_goal); + EXPECT_TRUE(config_->blackboard->get("updated_goal", updated_goal)); } TEST_F(GoalUpdaterTestFixture, test_older_goal_update) @@ -115,7 +115,7 @@ TEST_F(GoalUpdaterTestFixture, test_older_goal_update) // create tree std::string xml_txt = R"( - + @@ -141,7 +141,7 @@ TEST_F(GoalUpdaterTestFixture, test_older_goal_update) goal_updater_pub->publish(goal_to_update); tree_->rootNode()->executeTick(); geometry_msgs::msg::PoseStamped updated_goal; - config_->blackboard->get("updated_goal", updated_goal); + EXPECT_TRUE(config_->blackboard->get("updated_goal", updated_goal)); // expect to succeed and not update goal EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); @@ -153,7 +153,7 @@ TEST_F(GoalUpdaterTestFixture, test_get_latest_goal_update) // create tree std::string xml_txt = R"( - + @@ -184,7 +184,7 @@ TEST_F(GoalUpdaterTestFixture, test_get_latest_goal_update) goal_updater_pub->publish(goal_to_update_2); tree_->rootNode()->executeTick(); geometry_msgs::msg::PoseStamped updated_goal; - config_->blackboard->get("updated_goal", updated_goal); + EXPECT_TRUE(config_->blackboard->get("updated_goal", updated_goal)); // expect to succeed EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); diff --git a/nav2_behavior_tree/test/plugins/decorator/test_path_longer_on_approach.cpp b/nav2_behavior_tree/test/plugins/decorator/test_path_longer_on_approach.cpp index 0f08bafe691..5d521e205c1 100644 --- a/nav2_behavior_tree/test/plugins/decorator/test_path_longer_on_approach.cpp +++ b/nav2_behavior_tree/test/plugins/decorator/test_path_longer_on_approach.cpp @@ -89,7 +89,7 @@ TEST_F(PathLongerOnApproachTestFixture, test_tick) // create tree std::string xml_txt = R"( - + @@ -115,7 +115,7 @@ TEST_F(PathLongerOnApproachTestFixture, test_tick) // create tree xml_txt = R"( - + diff --git a/nav2_behavior_tree/test/test_bt_utils.cpp b/nav2_behavior_tree/test/test_bt_utils.cpp index 076b9c3b7c4..1c008d6478a 100644 --- a/nav2_behavior_tree/test/test_bt_utils.cpp +++ b/nav2_behavior_tree/test/test_bt_utils.cpp @@ -22,7 +22,7 @@ #include "geometry_msgs/msg/quaternion.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "nav2_behavior_tree/bt_utils.hpp" template @@ -51,7 +51,7 @@ TEST(PointPortTest, test_wrong_syntax) { std::string xml_txt = R"( - + @@ -69,7 +69,7 @@ TEST(PointPortTest, test_wrong_syntax) xml_txt = R"( - + @@ -86,7 +86,7 @@ TEST(PointPortTest, test_correct_syntax) { std::string xml_txt = R"( - + @@ -107,7 +107,7 @@ TEST(QuaternionPortTest, test_wrong_syntax) { std::string xml_txt = R"( - + @@ -126,7 +126,7 @@ TEST(QuaternionPortTest, test_wrong_syntax) xml_txt = R"( - + @@ -144,7 +144,7 @@ TEST(QuaternionPortTest, test_correct_syntax) { std::string xml_txt = R"( - + @@ -166,7 +166,7 @@ TEST(PoseStampedPortTest, test_wrong_syntax) { std::string xml_txt = R"( - + @@ -190,7 +190,7 @@ TEST(PoseStampedPortTest, test_wrong_syntax) xml_txt = R"( - + @@ -213,7 +213,7 @@ TEST(PoseStampedPortTest, test_correct_syntax) { std::string xml_txt = R"( - + @@ -241,7 +241,7 @@ TEST(MillisecondsPortTest, test_correct_syntax) { std::string xml_txt = R"( - + @@ -257,7 +257,7 @@ TEST(MillisecondsPortTest, test_correct_syntax) xml_txt = R"( - + @@ -272,7 +272,7 @@ TEST(ErrorCodePortTest, test_correct_syntax) { std::string xml_txt = R"( - + @@ -295,7 +295,7 @@ TEST(deconflictPortAndParamFrameTest, test_correct_syntax) { std::string xml_txt = R"( - + @@ -310,12 +310,12 @@ TEST(deconflictPortAndParamFrameTest, test_correct_syntax) node->declare_parameter("test", 2); node->declare_parameter("test_alternative", 3); - int value = BT::deconflictPortAndParamFrame( + int value = BT::deconflictPortAndParamFrame( node, "test_alternative", tree.rootNode()); EXPECT_EQ(value, 3); - value = BT::deconflictPortAndParamFrame( + value = BT::deconflictPortAndParamFrame( node, "test", tree.rootNode()); EXPECT_EQ(value, 1); diff --git a/nav2_behavior_tree/test/utils/test_behavior_tree_fixture.hpp b/nav2_behavior_tree/test/utils/test_behavior_tree_fixture.hpp index 6215909ae9c..7bf08ddd440 100644 --- a/nav2_behavior_tree/test/utils/test_behavior_tree_fixture.hpp +++ b/nav2_behavior_tree/test/utils/test_behavior_tree_fixture.hpp @@ -20,7 +20,7 @@ #include #include -#include "behaviortree_cpp_v3/bt_factory.h" +#include "behaviortree_cpp/bt_factory.h" #include "rclcpp/rclcpp.hpp" #include "test_transform_handler.hpp" diff --git a/nav2_behavior_tree/test/utils/test_dummy_tree_node.hpp b/nav2_behavior_tree/test/utils/test_dummy_tree_node.hpp index d277e7f4d25..dcfa52927ac 100644 --- a/nav2_behavior_tree/test/utils/test_dummy_tree_node.hpp +++ b/nav2_behavior_tree/test/utils/test_dummy_tree_node.hpp @@ -16,8 +16,8 @@ #ifndef UTILS__TEST_DUMMY_TREE_NODE_HPP_ #define UTILS__TEST_DUMMY_TREE_NODE_HPP_ -#include -#include +#include +#include namespace nav2_behavior_tree { @@ -36,7 +36,11 @@ class DummyNode : public BT::ActionNodeBase void changeStatus(BT::NodeStatus status) { - setStatus(status); + if (status == BT::NodeStatus::IDLE) { + resetStatus(); + } else { + setStatus(status); + } } BT::NodeStatus executeTick() override diff --git a/nav2_bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/params/nav2_multirobot_params_1.yaml index 172a832100d..91401fc21f9 100644 --- a/nav2_bringup/params/nav2_multirobot_params_1.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_1.yaml @@ -56,60 +56,11 @@ bt_navigator: # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. - plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_smooth_path_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_assisted_teleop_action_bt_node - - nav2_back_up_action_bt_node - - nav2_drive_on_heading_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_globally_updated_goal_condition_bt_node - - nav2_is_path_valid_condition_bt_node - - nav2_are_error_codes_active_condition_bt_node - - nav2_would_a_controller_recovery_help_condition_bt_node - - nav2_would_a_planner_recovery_help_condition_bt_node - - nav2_would_a_smoother_recovery_help_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_truncate_path_local_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_path_expiring_timer_condition - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_goal_updated_controller_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_planner_selector_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - - nav2_controller_cancel_bt_node - - nav2_path_longer_on_approach_bt_node - - nav2_wait_cancel_bt_node - - nav2_spin_cancel_bt_node - - nav2_back_up_cancel_bt_node - - nav2_assisted_teleop_cancel_bt_node - - nav2_drive_on_heading_cancel_bt_node - - nav2_is_battery_charging_condition_bt_node - - nav2_progress_checker_selector_bt_node - - nav2_smoother_selector_bt_node + + # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). + # Built-in plugins are added automatically + # plugin_lib_names: [] + error_code_names: - compute_path_error_code - follow_path_error_code diff --git a/nav2_bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/params/nav2_multirobot_params_2.yaml index da426b4722f..732fa24eb12 100644 --- a/nav2_bringup/params/nav2_multirobot_params_2.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_2.yaml @@ -56,60 +56,11 @@ bt_navigator: # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. - plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_smooth_path_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_assisted_teleop_action_bt_node - - nav2_back_up_action_bt_node - - nav2_drive_on_heading_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_globally_updated_goal_condition_bt_node - - nav2_is_path_valid_condition_bt_node - - nav2_are_error_codes_active_condition_bt_node - - nav2_would_a_controller_recovery_help_condition_bt_node - - nav2_would_a_planner_recovery_help_condition_bt_node - - nav2_would_a_smoother_recovery_help_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_truncate_path_local_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_path_expiring_timer_condition - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_goal_updated_controller_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_planner_selector_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - - nav2_controller_cancel_bt_node - - nav2_path_longer_on_approach_bt_node - - nav2_wait_cancel_bt_node - - nav2_spin_cancel_bt_node - - nav2_back_up_cancel_bt_node - - nav2_assisted_teleop_cancel_bt_node - - nav2_drive_on_heading_cancel_bt_node - - nav2_is_battery_charging_condition_bt_node - - nav2_progress_checker_selector_bt_node - - nav2_smoother_selector_bt_node + + # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). + # Built-in plugins are added automatically + # plugin_lib_names: [] + error_code_names: - compute_path_error_code - follow_path_error_code diff --git a/nav2_bringup/params/nav2_multirobot_params_all.yaml b/nav2_bringup/params/nav2_multirobot_params_all.yaml index 8249df84d17..bb812d276b8 100644 --- a/nav2_bringup/params/nav2_multirobot_params_all.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_all.yaml @@ -56,60 +56,11 @@ bt_navigator: # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. - plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_smooth_path_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_assisted_teleop_action_bt_node - - nav2_back_up_action_bt_node - - nav2_drive_on_heading_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_globally_updated_goal_condition_bt_node - - nav2_is_path_valid_condition_bt_node - - nav2_are_error_codes_active_condition_bt_node - - nav2_would_a_controller_recovery_help_condition_bt_node - - nav2_would_a_planner_recovery_help_condition_bt_node - - nav2_would_a_smoother_recovery_help_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_truncate_path_local_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_path_expiring_timer_condition - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_goal_updated_controller_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_planner_selector_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - - nav2_controller_cancel_bt_node - - nav2_path_longer_on_approach_bt_node - - nav2_wait_cancel_bt_node - - nav2_spin_cancel_bt_node - - nav2_back_up_cancel_bt_node - - nav2_assisted_teleop_cancel_bt_node - - nav2_drive_on_heading_cancel_bt_node - - nav2_is_battery_charging_condition_bt_node - - nav2_progress_checker_selector_bt_node - - nav2_smoother_selector_bt_node + + # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). + # Built-in plugins are added automatically + # plugin_lib_names: [] + error_code_names: - compute_path_error_code - follow_path_error_code diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 28b23ab2897..28e7d273fba 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -56,60 +56,11 @@ bt_navigator: # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. - plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_smooth_path_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_assisted_teleop_action_bt_node - - nav2_back_up_action_bt_node - - nav2_drive_on_heading_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_globally_updated_goal_condition_bt_node - - nav2_is_path_valid_condition_bt_node - - nav2_are_error_codes_active_condition_bt_node - - nav2_would_a_controller_recovery_help_condition_bt_node - - nav2_would_a_planner_recovery_help_condition_bt_node - - nav2_would_a_smoother_recovery_help_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_truncate_path_local_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_path_expiring_timer_condition - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_goal_updated_controller_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_planner_selector_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - - nav2_controller_cancel_bt_node - - nav2_path_longer_on_approach_bt_node - - nav2_wait_cancel_bt_node - - nav2_spin_cancel_bt_node - - nav2_back_up_cancel_bt_node - - nav2_assisted_teleop_cancel_bt_node - - nav2_drive_on_heading_cancel_bt_node - - nav2_is_battery_charging_condition_bt_node - - nav2_progress_checker_selector_bt_node - - nav2_smoother_selector_bt_node + + # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). + # Built-in plugins are added automatically + # plugin_lib_names: [] + error_code_names: - compute_path_error_code - follow_path_error_code diff --git a/nav2_bt_navigator/CMakeLists.txt b/nav2_bt_navigator/CMakeLists.txt index cfe6a0d4869..7de1592106a 100644 --- a/nav2_bt_navigator/CMakeLists.txt +++ b/nav2_bt_navigator/CMakeLists.txt @@ -12,7 +12,7 @@ find_package(geometry_msgs REQUIRED) find_package(nav2_behavior_tree REQUIRED) find_package(nav_msgs REQUIRED) find_package(nav2_msgs REQUIRED) -find_package(behaviortree_cpp_v3 REQUIRED) +find_package(behaviortree_cpp REQUIRED) find_package(std_srvs REQUIRED) find_package(nav2_util REQUIRED) find_package(nav2_core REQUIRED) @@ -43,7 +43,7 @@ set(dependencies nav2_behavior_tree nav_msgs nav2_msgs - behaviortree_cpp_v3 + behaviortree_cpp std_srvs nav2_util nav2_core diff --git a/nav2_bt_navigator/behavior_trees/follow_point.xml b/nav2_bt_navigator/behavior_trees/follow_point.xml index 6941a8c7e60..9f55ecd66ec 100644 --- a/nav2_bt_navigator/behavior_trees/follow_point.xml +++ b/nav2_bt_navigator/behavior_trees/follow_point.xml @@ -2,7 +2,7 @@ This Behavior Tree follows a dynamic pose to a certain distance --> - + diff --git a/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml b/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml index 8fa2eca237c..6973057e93a 100644 --- a/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml +++ b/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml @@ -3,7 +3,7 @@ recovery actions specific to planning / control as well as general system issues. This will be continuous if a kinematically valid planner is selected. --> - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml index 7a235ff0407..28eda8ed893 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml @@ -3,7 +3,7 @@ This Behavior Tree replans the global path periodically at 1 Hz through an array of poses continuously and it also has recovery actions specific to planning / control as well as general system issues. --> - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml index 8cb0eb749ca..772d36fded8 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml @@ -4,7 +4,7 @@ recovery actions specific to planning / control as well as general system issues. This will be continuous if a kinematically valid planner is selected. --> - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml index ea2a7315909..a705aa6b3a2 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml @@ -5,7 +5,7 @@ make the robot wait for a specific time, to see if the obstacle clears out before navigating along a significantly longer path to reach the goal location. --> - + @@ -20,10 +20,10 @@ - + - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml b/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml index 444c9458ff1..872dbf80187 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml @@ -4,7 +4,7 @@ recovery actions specific to planning / control as well as general system issues. This will be continuous if a kinematically valid planner is selected. --> - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml index 61132d5f95d..16f7ede4b3e 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml @@ -2,7 +2,7 @@ This Behavior Tree replans the global path after every 1m. --> - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml index 56e360933c8..a214c9b64fd 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml @@ -2,7 +2,7 @@ This Behavior Tree replans the global path only when the goal is updated. --> - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml index b9f903fd334..effe9a2bf82 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml @@ -1,7 +1,7 @@ - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml index a48ae89a76a..e0d5d6a5a41 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml @@ -2,7 +2,7 @@ This Behavior Tree replans the global path periodically proprortional to speed. --> - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml index f18111f5bd1..11332fb24e9 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml @@ -2,7 +2,7 @@ This Behavior Tree replans the global path periodically at 1 Hz. --> - + diff --git a/nav2_bt_navigator/behavior_trees/odometry_calibration.xml b/nav2_bt_navigator/behavior_trees/odometry_calibration.xml index 49000e14fa6..fdc459f40ff 100644 --- a/nav2_bt_navigator/behavior_trees/odometry_calibration.xml +++ b/nav2_bt_navigator/behavior_trees/odometry_calibration.xml @@ -2,7 +2,7 @@ his Behavior Tree drives in a square for odometry calibration experiments --> - + diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml index 289f08d7ea6..d4eaa63c3c5 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -17,7 +17,7 @@ nav2_behavior_tree nav_msgs nav2_msgs - behaviortree_cpp_v3 + behaviortree_cpp std_msgs geometry_msgs std_srvs @@ -25,7 +25,7 @@ pluginlib nav2_core - behaviortree_cpp_v3 + behaviortree_cpp rclcpp rclcpp_action rclcpp_lifecycle diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index c588e6ff4d0..4c1b4f47bca 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -18,12 +18,15 @@ #include #include #include +#include #include "nav2_util/geometry_utils.hpp" #include "nav2_util/node_utils.hpp" #include "nav2_util/robot_utils.hpp" #include "nav2_behavior_tree/bt_utils.hpp" +#include "nav2_behavior_tree/plugins_list.hpp" + using nav2_util::declare_parameter_if_not_declared; namespace nav2_bt_navigator @@ -36,64 +39,8 @@ BtNavigator::BtNavigator(rclcpp::NodeOptions options) { RCLCPP_INFO(get_logger(), "Creating"); - const std::vector plugin_libs = { - "nav2_compute_path_to_pose_action_bt_node", - "nav2_compute_path_through_poses_action_bt_node", - "nav2_smooth_path_action_bt_node", - "nav2_follow_path_action_bt_node", - "nav2_spin_action_bt_node", - "nav2_wait_action_bt_node", - "nav2_assisted_teleop_action_bt_node", - "nav2_back_up_action_bt_node", - "nav2_drive_on_heading_bt_node", - "nav2_clear_costmap_service_bt_node", - "nav2_is_stuck_condition_bt_node", - "nav2_goal_reached_condition_bt_node", - "nav2_initial_pose_received_condition_bt_node", - "nav2_goal_updated_condition_bt_node", - "nav2_globally_updated_goal_condition_bt_node", - "nav2_is_path_valid_condition_bt_node", - "nav2_are_error_codes_active_condition_bt_node", - "nav2_would_a_controller_recovery_help_condition_bt_node", - "nav2_would_a_planner_recovery_help_condition_bt_node", - "nav2_would_a_smoother_recovery_help_condition_bt_node", - "nav2_reinitialize_global_localization_service_bt_node", - "nav2_rate_controller_bt_node", - "nav2_distance_controller_bt_node", - "nav2_speed_controller_bt_node", - "nav2_truncate_path_action_bt_node", - "nav2_truncate_path_local_action_bt_node", - "nav2_goal_updater_node_bt_node", - "nav2_recovery_node_bt_node", - "nav2_pipeline_sequence_bt_node", - "nav2_round_robin_node_bt_node", - "nav2_transform_available_condition_bt_node", - "nav2_time_expired_condition_bt_node", - "nav2_path_expiring_timer_condition", - "nav2_distance_traveled_condition_bt_node", - "nav2_single_trigger_bt_node", - "nav2_goal_updated_controller_bt_node", - "nav2_is_battery_low_condition_bt_node", - "nav2_navigate_through_poses_action_bt_node", - "nav2_navigate_to_pose_action_bt_node", - "nav2_remove_passed_goals_action_bt_node", - "nav2_planner_selector_bt_node", - "nav2_controller_selector_bt_node", - "nav2_goal_checker_selector_bt_node", - "nav2_controller_cancel_bt_node", - "nav2_path_longer_on_approach_bt_node", - "nav2_wait_cancel_bt_node", - "nav2_spin_cancel_bt_node", - "nav2_assisted_teleop_cancel_bt_node", - "nav2_back_up_cancel_bt_node", - "nav2_drive_on_heading_cancel_bt_node", - "nav2_is_battery_charging_condition_bt_node", - "nav2_progress_checker_selector_bt_node", - "nav2_smoother_selector_bt_node" - }; - declare_parameter_if_not_declared( - this, "plugin_lib_names", rclcpp::ParameterValue(plugin_libs)); + this, "plugin_lib_names", rclcpp::ParameterValue(std::vector{})); declare_parameter_if_not_declared( this, "transform_tolerance", rclcpp::ParameterValue(0.1)); declare_parameter_if_not_declared( @@ -126,7 +73,14 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) odom_topic_ = get_parameter("odom_topic").as_string(); // Libraries to pull plugins (BT Nodes) from - auto plugin_lib_names = get_parameter("plugin_lib_names").as_string_array(); + std::vector plugin_lib_names; + boost::split(plugin_lib_names, nav2::details::BT_BUILTIN_PLUGINS, boost::is_any_of(";")); + + auto user_defined_plugins = get_parameter("plugin_lib_names").as_string_array(); + // append user_defined_plugins to plugin_lib_names + plugin_lib_names.insert( + plugin_lib_names.end(), user_defined_plugins.begin(), + user_defined_plugins.end()); nav2_core::FeedbackUtils feedback_utils; feedback_utils.tf = tf_; diff --git a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp index 53769abfcd0..a4b4df52d1b 100644 --- a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp @@ -103,7 +103,7 @@ NavigateThroughPosesNavigator::onLoop() auto blackboard = bt_action_server_->getBlackboard(); Goals goal_poses; - blackboard->get(goals_blackboard_id_, goal_poses); + [[maybe_unused]] auto res = blackboard->get(goals_blackboard_id_, goal_poses); if (goal_poses.size() == 0) { bt_action_server_->publishFeedback(feedback_msg); @@ -123,7 +123,7 @@ NavigateThroughPosesNavigator::onLoop() try { // Get current path points nav_msgs::msg::Path current_path; - blackboard->get(path_blackboard_id_, current_path); + res = blackboard->get(path_blackboard_id_, current_path); // Find the closest pose to current pose on global path auto find_closest_pose_idx = @@ -166,7 +166,7 @@ NavigateThroughPosesNavigator::onLoop() } int recovery_count = 0; - blackboard->get("number_recoveries", recovery_count); + res = blackboard->get("number_recoveries", recovery_count); feedback_msg->number_of_recoveries = recovery_count; feedback_msg->current_pose = current_pose; feedback_msg->navigation_time = clock_->now() - start_time_; diff --git a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp index 326a0bd9fb6..2270b6add56 100644 --- a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp @@ -126,7 +126,7 @@ NavigateToPoseNavigator::onLoop() try { // Get current path points nav_msgs::msg::Path current_path; - blackboard->get(path_blackboard_id_, current_path); + [[maybe_unused]] auto res = blackboard->get(path_blackboard_id_, current_path); // Find the closest pose to current pose on global path auto find_closest_pose_idx = @@ -169,7 +169,7 @@ NavigateToPoseNavigator::onLoop() } int recovery_count = 0; - blackboard->get("number_recoveries", recovery_count); + [[maybe_unused]] auto res = blackboard->get("number_recoveries", recovery_count); feedback_msg->number_of_recoveries = recovery_count; feedback_msg->current_pose = current_pose; feedback_msg->navigation_time = clock_->now() - start_time_; diff --git a/nav2_collision_monitor/test/velocity_polygons_test.cpp b/nav2_collision_monitor/test/velocity_polygons_test.cpp index 7030ddd2b20..4fd4f8bc0b3 100644 --- a/nav2_collision_monitor/test/velocity_polygons_test.cpp +++ b/nav2_collision_monitor/test/velocity_polygons_test.cpp @@ -40,7 +40,6 @@ using namespace std::chrono_literals; static constexpr double EPSILON = std::numeric_limits::epsilon(); static const char BASE_FRAME_ID[]{"base_link"}; -static const char BASE2_FRAME_ID[]{"base2_link"}; static const char POLYGON_PUB_TOPIC[]{"polygon_pub"}; static const char POLYGON_NAME[]{"TestVelocityPolygon"}; static const char SUB_POLYGON_FORWARD_NAME[]{"Forward"}; diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index fa25e0b1379..85aae171cc7 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -419,6 +419,7 @@ void ControllerServer::computeControl() { std::lock_guard lock(dynamic_params_lock_); + auto start_time = this->now(); RCLCPP_INFO(get_logger(), "Received a goal, begin computing control effort."); try { @@ -479,10 +480,12 @@ void ControllerServer::computeControl() break; } + auto cycle_duration = this->now() - start_time; if (!loop_rate.sleep()) { RCLCPP_WARN( - get_logger(), "Control loop missed its desired rate of %.4fHz", - controller_frequency_); + get_logger(), + "Control loop missed its desired rate of %.4f Hz. Current loop rate is %.4f Hz.", + controller_frequency_, 1 / cycle_duration.seconds()); } } } catch (nav2_core::InvalidController & e) { diff --git a/nav2_core/include/nav2_core/global_planner.hpp b/nav2_core/include/nav2_core/global_planner.hpp index dff1207b88f..0eff064a5d2 100644 --- a/nav2_core/include/nav2_core/global_planner.hpp +++ b/nav2_core/include/nav2_core/global_planner.hpp @@ -71,11 +71,13 @@ class GlobalPlanner * @brief Method create the plan from a starting and ending goal. * @param start The starting pose of the robot * @param goal The goal pose of the robot + * @param cancel_checker Function to check if the action has been canceled * @return The sequence of poses to get from start to goal, if any */ virtual nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal) = 0; + const geometry_msgs::msg::PoseStamped & goal, + std::function cancel_checker) = 0; }; } // namespace nav2_core diff --git a/nav2_core/include/nav2_core/planner_exceptions.hpp b/nav2_core/include/nav2_core/planner_exceptions.hpp index 2680b3c6695..c16c1c10967 100644 --- a/nav2_core/include/nav2_core/planner_exceptions.hpp +++ b/nav2_core/include/nav2_core/planner_exceptions.hpp @@ -113,6 +113,13 @@ class NoViapointsGiven : public PlannerException : PlannerException(description) {} }; +class PlannerCancelled : public PlannerException +{ +public: + explicit PlannerCancelled(const std::string & description) + : PlannerException(description) {} +}; + } // namespace nav2_core #endif // NAV2_CORE__PLANNER_EXCEPTIONS_HPP_ diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt index df0ddb2dc3a..723f81f0f9b 100644 --- a/nav2_costmap_2d/CMakeLists.txt +++ b/nav2_costmap_2d/CMakeLists.txt @@ -26,17 +26,10 @@ find_package(visualization_msgs REQUIRED) find_package(angles REQUIRED) remove_definitions(-DDISABLE_LIBUSB-1.0) -find_package(Eigen3 REQUIRED) +find_package(Eigen3 3.3 REQUIRED) nav2_package() -include_directories( - include - ${EIGEN3_INCLUDE_DIRS} -) - -add_definitions(${EIGEN3_DEFINITIONS}) - add_library(nav2_costmap_2d_core SHARED src/costmap_2d.cpp src/layer.cpp @@ -51,6 +44,13 @@ add_library(nav2_costmap_2d_core SHARED src/footprint_collision_checker.cpp plugins/costmap_filters/costmap_filter.cpp ) +add_library(${PROJECT_NAME}::nav2_costmap_2d_core ALIAS nav2_costmap_2d_core) + +target_include_directories(nav2_costmap_2d_core + PUBLIC + "$" + "$" +) set(dependencies geometry_msgs @@ -78,6 +78,7 @@ set(dependencies ament_target_dependencies(nav2_costmap_2d_core ${dependencies} ) +target_link_libraries(nav2_costmap_2d_core Eigen3::Eigen) add_library(layers SHARED plugins/inflation_layer.cpp @@ -88,11 +89,12 @@ add_library(layers SHARED plugins/range_sensor_layer.cpp plugins/denoise_layer.cpp ) +add_library(${PROJECT_NAME}::layers ALIAS layers) ament_target_dependencies(layers ${dependencies} ) target_link_libraries(layers - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) add_library(filters SHARED @@ -100,11 +102,14 @@ add_library(filters SHARED plugins/costmap_filters/speed_filter.cpp plugins/costmap_filters/binary_filter.cpp ) +add_library(${PROJECT_NAME}::filters ALIAS filters) + + ament_target_dependencies(filters ${dependencies} ) target_link_libraries(filters - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) add_library(nav2_costmap_2d_client SHARED @@ -112,18 +117,19 @@ add_library(nav2_costmap_2d_client SHARED src/costmap_subscriber.cpp src/costmap_topic_collision_checker.cpp ) +add_library(${PROJECT_NAME}::nav2_costmap_2d_client ALIAS nav2_costmap_2d_client) ament_target_dependencies(nav2_costmap_2d_client ${dependencies} ) target_link_libraries(nav2_costmap_2d_client - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) add_executable(nav2_costmap_2d_markers src/costmap_2d_markers.cpp) target_link_libraries(nav2_costmap_2d_markers - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) ament_target_dependencies(nav2_costmap_2d_markers @@ -132,7 +138,7 @@ ament_target_dependencies(nav2_costmap_2d_markers add_executable(nav2_costmap_2d_cloud src/costmap_2d_cloud.cpp) target_link_libraries(nav2_costmap_2d_cloud - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) add_executable(nav2_costmap_2d src/costmap_2d_node.cpp) @@ -141,26 +147,24 @@ ament_target_dependencies(nav2_costmap_2d ) target_link_libraries(nav2_costmap_2d - nav2_costmap_2d_core - layers - filters + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::layers + ${PROJECT_NAME}::filters ) install(TARGETS - nav2_costmap_2d_core layers filters + nav2_costmap_2d_core nav2_costmap_2d_client + EXPORT export_${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin ) -install(TARGETS - nav2_costmap_2d - nav2_costmap_2d_markers - nav2_costmap_2d_cloud - RUNTIME DESTINATION lib/${PROJECT_NAME} +install(TARGETS nav2_costmap_2d_markers nav2_costmap_2d_cloud nav2_costmap_2d + DESTINATION lib/${PROJECT_NAME} ) install(FILES costmap_plugins.xml @@ -168,7 +172,7 @@ install(FILES costmap_plugins.xml ) install(DIRECTORY include/ - DESTINATION include/ + DESTINATION include/${PROJECT_NAME} ) if(BUILD_TESTING) @@ -183,8 +187,7 @@ if(BUILD_TESTING) pluginlib_export_plugin_description_file(nav2_costmap_2d test/regression/order_layer.xml) endif() -ament_export_include_directories(include) -ament_export_libraries(layers filters nav2_costmap_2d_core nav2_costmap_2d_client) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) ament_export_dependencies(${dependencies}) pluginlib_export_plugin_description_file(nav2_costmap_2d costmap_plugins.xml) ament_package() diff --git a/nav2_costmap_2d/test/integration/CMakeLists.txt b/nav2_costmap_2d/test/integration/CMakeLists.txt index 868b5081a1b..7fbef3d41db 100644 --- a/nav2_costmap_2d/test/integration/CMakeLists.txt +++ b/nav2_costmap_2d/test/integration/CMakeLists.txt @@ -1,90 +1,66 @@ ament_add_gtest_executable(footprint_tests_exec footprint_tests.cpp ) -ament_target_dependencies(footprint_tests_exec - ${dependencies} -) target_link_libraries(footprint_tests_exec - nav2_costmap_2d_core - layers + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::layers ) ament_add_gtest_executable(test_collision_checker_exec test_costmap_topic_collision_checker.cpp ) -ament_target_dependencies(test_collision_checker_exec - ${dependencies} -) target_link_libraries(test_collision_checker_exec - nav2_costmap_2d_core - nav2_costmap_2d_client - layers + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_client + ${PROJECT_NAME}::layers ) ament_add_gtest_executable(inflation_tests_exec inflation_tests.cpp ) -ament_target_dependencies(inflation_tests_exec - ${dependencies} -) target_link_libraries(inflation_tests_exec - nav2_costmap_2d_core - layers + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::layers ) ament_add_gtest_executable(obstacle_tests_exec obstacle_tests.cpp ) -ament_target_dependencies(obstacle_tests_exec - ${dependencies} -) target_link_libraries(obstacle_tests_exec - nav2_costmap_2d_core - layers + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::layers ) ament_add_gtest_executable(range_tests_exec range_tests.cpp ) -ament_target_dependencies(range_tests_exec - ${dependencies} -) target_link_libraries(range_tests_exec - nav2_costmap_2d_core - layers + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::layers ) ament_add_gtest(dyn_params_tests dyn_params_tests.cpp ) -ament_target_dependencies(dyn_params_tests - ${dependencies} -) target_link_libraries(dyn_params_tests - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) ament_add_gtest_executable(test_costmap_publisher_exec test_costmap_2d_publisher.cpp ) -ament_target_dependencies(test_costmap_publisher_exec - ${dependencies} -) target_link_libraries(test_costmap_publisher_exec - nav2_costmap_2d_core - nav2_costmap_2d_client - layers + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_client + ${PROJECT_NAME}::layers ) ament_add_gtest_executable(test_costmap_subscriber_exec test_costmap_subscriber.cpp ) -ament_target_dependencies(test_costmap_subscriber_exec - ${dependencies} -) target_link_libraries(test_costmap_subscriber_exec - nav2_costmap_2d_core - nav2_costmap_2d_client + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_client ) ament_add_test(test_collision_checker @@ -166,6 +142,6 @@ ament_add_test(test_costmap_subscriber_exec # ${dependencies} # ) # target_link_libraries(costmap_tester -# nav2_costmap_2d_core +# ${PROJECT_NAME}::nav2_costmap_2d_core # layers # ) diff --git a/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp b/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp index d674ee76691..849edf282c7 100644 --- a/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp +++ b/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp @@ -53,7 +53,7 @@ class TestCostmapSubscriberShould : public ::testing::Test costmapToSend = std::make_shared(10, 10, 1.0, 0.0, 0.0); } - void TearDown() + void TearDown() override { costmapSubscriber.reset(); costmapToSend.reset(); diff --git a/nav2_costmap_2d/test/regression/CMakeLists.txt b/nav2_costmap_2d/test/regression/CMakeLists.txt index 78fbfa6cf79..aafc4d561be 100644 --- a/nav2_costmap_2d/test/regression/CMakeLists.txt +++ b/nav2_costmap_2d/test/regression/CMakeLists.txt @@ -1,7 +1,7 @@ # Bresenham2D corner cases test ament_add_gtest(costmap_bresenham_2d costmap_bresenham_2d.cpp) target_link_libraries(costmap_bresenham_2d - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) # OrderLayer for checking Costmap2D plugins API calling order @@ -11,7 +11,7 @@ ament_target_dependencies(order_layer ${dependencies} ) target_link_libraries(order_layer - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) install(TARGETS order_layer @@ -23,5 +23,5 @@ install(TARGETS # Costmap2D plugins API calling order test ament_add_gtest(plugin_api_order plugin_api_order.cpp) target_link_libraries(plugin_api_order - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) diff --git a/nav2_costmap_2d/test/unit/CMakeLists.txt b/nav2_costmap_2d/test/unit/CMakeLists.txt index e69501277ea..d3c5edd813c 100644 --- a/nav2_costmap_2d/test/unit/CMakeLists.txt +++ b/nav2_costmap_2d/test/unit/CMakeLists.txt @@ -1,57 +1,58 @@ ament_add_gtest(collision_footprint_test footprint_collision_checker_test.cpp) target_link_libraries(collision_footprint_test - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) ament_add_gtest(costmap_convesion_test costmap_conversion_test.cpp) target_link_libraries(costmap_convesion_test - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) ament_add_gtest(declare_parameter_test declare_parameter_test.cpp) target_link_libraries(declare_parameter_test - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) ament_add_gtest(costmap_filter_test costmap_filter_test.cpp) target_link_libraries(costmap_filter_test - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) ament_add_gtest(keepout_filter_test keepout_filter_test.cpp) target_link_libraries(keepout_filter_test - nav2_costmap_2d_core - filters + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::filters ) ament_add_gtest(speed_filter_test speed_filter_test.cpp) target_link_libraries(speed_filter_test - nav2_costmap_2d_core - filters + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::filters ) ament_add_gtest(binary_filter_test binary_filter_test.cpp) target_link_libraries(binary_filter_test - nav2_costmap_2d_core - filters + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::filters ) ament_add_gtest(copy_window_test copy_window_test.cpp) target_link_libraries(copy_window_test - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) ament_add_gtest(costmap_filter_service_test costmap_filter_service_test.cpp) target_link_libraries(costmap_filter_service_test - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) ament_add_gtest(denoise_layer_test denoise_layer_test.cpp image_test.cpp image_processing_test.cpp) target_link_libraries(denoise_layer_test - nav2_costmap_2d_core layers + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::layers ) ament_add_gtest(lifecycle_test lifecycle_test.cpp) target_link_libraries(lifecycle_test - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) \ No newline at end of file diff --git a/nav2_graceful_controller/README.md b/nav2_graceful_controller/README.md index 322485cec8a..9cb5a72ec70 100644 --- a/nav2_graceful_controller/README.md +++ b/nav2_graceful_controller/README.md @@ -1,7 +1,7 @@ # Graceful Motion Controller The graceful motion controller implements a controller based on the works of Jong Jin Park in "Graceful Navigation for Mobile Robots in Dynamic and Uncertain Environments". (2016). In this implementation, a `motion_target` is set at a distance away from the robot that is exponentially stable to generate a smooth trajectory for the robot to follow. -See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-graceful-motion.html) for additional parameter descriptions. +See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-graceful-motion-controller.html) for additional parameter descriptions. ## Smooth control law The smooth control law is a pose-following kinematic control law that generates a smooth and confortable trajectory for the robot to follow. It is Lyapunov-based feedback control law made of three components: @@ -39,4 +39,4 @@ The smooth control law is a pose-following kinematic control law that generates | `transformed_global_plan` | `nav_msgs/Path` | The global plan transformed into the robot frame. | | `local_plan` | `nav_msgs/Path` | The local plan generated by appliyng iteratively the control law upon a set of motion targets along the global plan. | | `motion_target` | `geometry_msgs/PointStamped` | The current motion target used by the controller to compute the velocity commands. | -| `slowdown` | `visualization_msgs/Marker` | A flat circle marker of radius slowdown_radius around the motion target. | \ No newline at end of file +| `slowdown` | `visualization_msgs/Marker` | A flat circle marker of radius slowdown_radius around the motion target. | diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt index 1501e4321a1..d2bb6d9c9da 100644 --- a/nav2_mppi_controller/CMakeLists.txt +++ b/nav2_mppi_controller/CMakeLists.txt @@ -79,6 +79,7 @@ add_library(mppi_controller SHARED add_library(mppi_critics SHARED src/critics/obstacles_critic.cpp + src/critics/cost_critic.cpp src/critics/goal_critic.cpp src/critics/goal_angle_critic.cpp src/critics/path_align_critic.cpp diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index fd7ad802142..3bf779e0938 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -98,6 +98,9 @@ This process is then repeated a number of times and returns a converged solution #### Obstacles Critic + +Uses estimated distances from obstacles using cost and inflation parameters to avoid obstacles + | Parameter | Type | Definition | | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | | consider_footprint | bool | Default: False. Whether to use point cost (if robot is circular or low compute power) or compute SE2 footprint cost. | @@ -109,6 +112,20 @@ This process is then repeated a number of times and returns a converged solution | near_goal_distance | double | Default 0.5. Distance near goal to stop applying preferential obstacle term to allow robot to smoothly converge to goal pose in close proximity to obstacles. | inflation_layer_name | string | Default "". Name of the inflation layer. If empty, it uses the last inflation layer in the costmap. If you have multiple inflation layers, you may want to specify the name of the layer to use. | +#### Cost Critic + +Uses inflated costmap cost directly to avoid obstacles + + | Parameter | Type | Definition | + | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | + | consider_footprint | bool | Default: False. Whether to use point cost (if robot is circular or low compute power) or compute SE2 footprint cost. | + | cost_weight | double | Default 3.81. Wight to apply to critic to avoid obstacles. | + | cost_power | int | Default 1. Power order to apply to term. | + | collision_cost | double | Default 1000000.0. Cost to apply to a true collision in a trajectory. | + | critical_cost | double | Default 300.0. Cost to apply to a pose with any point in in inflated space to prefer distance from obstacles. | + | near_goal_distance | double | Default 0.5. Distance near goal to stop applying preferential obstacle term to allow robot to smoothly converge to goal pose in close proximity to obstacles. + | inflation_layer_name | string | Default "". Name of the inflation layer. If empty, it uses the last inflation layer in the costmap. If you have multiple inflation layers, you may want to specify the name of the layer to use. | + #### Path Align Critic | Parameter | Type | Definition | | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | @@ -184,7 +201,7 @@ controller_server: time_step: 3 AckermannConstraints: min_turning_r: 0.2 - critics: ["ConstraintCritic", "ObstaclesCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"] + critics: ["ConstraintCritic", "CostCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"] ConstraintCritic: enabled: true cost_power: 1 @@ -204,15 +221,24 @@ controller_server: cost_power: 1 cost_weight: 5.0 threshold_to_consider: 0.5 - ObstaclesCritic: + # ObstaclesCritic: + # enabled: true + # cost_power: 1 + # repulsion_weight: 1.5 + # critical_weight: 20.0 + # consider_footprint: false + # collision_cost: 10000.0 + # collision_margin_distance: 0.1 + # near_goal_distance: 0.5 + CostCritic: enabled: true cost_power: 1 - repulsion_weight: 1.5 - critical_weight: 20.0 - consider_footprint: false - collision_cost: 10000.0 - collision_margin_distance: 0.1 - near_goal_distance: 0.5 + cost_weight: 3.81 + critical_cost: 300.0 + consider_footprint: true + collision_cost: 1000000.0 + near_goal_distance: 1.0 + PathAlignCritic: PathAlignCritic: enabled: true cost_power: 1 @@ -262,6 +288,8 @@ If you don't require path following behavior (e.g. just want to follow a goal po By default, the controller is tuned and has the capabilities established in the PathAlign/Obstacle critics to generally follow the path closely when no obstacles prevent it, but able to deviate from the path when blocked. See `PathAlignCritic::score()` for details, but it is disabled when the local path is blocked so the obstacle critic takes over in that state. +If you want to slow further on approach to goal, consider increasing the ``threshold_to_consider`` parameters to give a hand off from the path tracking critics to the goal approach critics sooner - then tune those critics for your profile of interest. + ### Prediction Horizon, Costmap Sizing, and Offsets As this is a predictive planner, there is some relationship between maximum speed, prediction times, and costmap size that users should keep in mind while tuning for their application. If a controller server costmap is set to 3.0m in size, that means that with the robot in the center, there is 1.5m of information on either side of the robot. When your prediction horizon (time_steps * model_dt) at maximum speed (vx_max) is larger than this, then your robot will be artificially limited in its maximum speeds and behavior by the costmap limitation. For example, if you predict forward 3 seconds (60 steps @ 0.05s per step) at 0.5m/s maximum speed, the **minimum** required costmap radius is 1.5m - or 3m total width. diff --git a/nav2_mppi_controller/critics.xml b/nav2_mppi_controller/critics.xml index 790568e7967..d578d23a9ec 100644 --- a/nav2_mppi_controller/critics.xml +++ b/nav2_mppi_controller/critics.xml @@ -5,6 +5,10 @@ mppi critic for obstacle avoidance + + mppi critic for obstacle avoidance using costmap score + + mppi critic for driving towards the goal diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp new file mode 100644 index 00000000000..14c0fe93f4a --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp @@ -0,0 +1,98 @@ +// Copyright (c) 2023 Robocc Brice Renaudeau +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_MPPI_CONTROLLER__CRITICS__COST_CRITIC_HPP_ +#define NAV2_MPPI_CONTROLLER__CRITICS__COST_CRITIC_HPP_ + +#include +#include + +#include "nav2_costmap_2d/footprint_collision_checker.hpp" +#include "nav2_costmap_2d/inflation_layer.hpp" + +#include "nav2_mppi_controller/critic_function.hpp" +#include "nav2_mppi_controller/models/state.hpp" +#include "nav2_mppi_controller/tools/utils.hpp" + +namespace mppi::critics +{ + +/** + * @class mppi::critics::CostCritic + * @brief Critic objective function for avoiding obstacles using costmap's inflated cost + */ +class CostCritic : public CriticFunction +{ +public: + /** + * @brief Initialize critic + */ + void initialize() override; + + /** + * @brief Evaluate cost related to obstacle avoidance + * + * @param costs [out] add obstacle cost values to this tensor + */ + void score(CriticData & data) override; + +protected: + /** + * @brief Checks if cost represents a collision + * @param cost Point cost at pose center + * @param x X of pose + * @param y Y of pose + * @param theta theta of pose + * @return bool if in collision + */ + bool inCollision(float cost, float x, float y, float theta); + + /** + * @brief cost at a robot pose + * @param x X of pose + * @param y Y of pose + * @return Collision information at pose + */ + float costAtPose(float x, float y); + + /** + * @brief Find the min cost of the inflation decay function for which the robot MAY be + * in collision in any orientation + * @param costmap Costmap2DROS to get minimum inscribed cost (e.g. 128 in inflation layer documentation) + * @return double circumscribed cost, any higher than this and need to do full footprint collision checking + * since some element of the robot could be in collision + */ + float findCircumscribedCost(std::shared_ptr costmap); + +protected: + nav2_costmap_2d::FootprintCollisionChecker + collision_checker_{nullptr}; + float possible_collision_cost_; + + bool consider_footprint_{true}; + float circumscribed_radius_{0}; + float circumscribed_cost_{0}; + float collision_cost_{0}; + float critical_cost_{0}; + float weight_{0}; + + float near_goal_distance_; + std::string inflation_layer_name_; + + unsigned int power_{0}; +}; + +} // namespace mppi::critics + +#endif // NAV2_MPPI_CONTROLLER__CRITICS__COST_CRITIC_HPP_ diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp index 358994c238b..31b6a3d0df8 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp @@ -90,7 +90,7 @@ class ObstaclesCritic : public CriticFunction float collision_cost_{0}; float inflation_scale_factor_{0}, inflation_radius_{0}; - float possibly_inscribed_cost_; + float possible_collision_cost_; float collision_margin_distance_; float near_goal_distance_; float circumscribed_cost_{0}, circumscribed_radius_{0}; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp index fb22b38b248..49ff2cb010a 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -314,6 +314,7 @@ inline size_t findPathFurthestReachedPoint(const CriticData & data) for (size_t i = 0; i < dists.shape(0); i++) { min_id_by_path = 0; + min_distance_by_path = std::numeric_limits::max(); for (size_t j = 0; j < dists.shape(1); j++) { cur_dist = dists(i, j); if (cur_dist < min_distance_by_path) { diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp new file mode 100644 index 00000000000..7a9b7729ed5 --- /dev/null +++ b/nav2_mppi_controller/src/critics/cost_critic.cpp @@ -0,0 +1,200 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include "nav2_mppi_controller/critics/cost_critic.hpp" + +namespace mppi::critics +{ + +void CostCritic::initialize() +{ + auto getParam = parameters_handler_->getParamGetter(name_); + getParam(consider_footprint_, "consider_footprint", false); + getParam(power_, "cost_power", 1); + getParam(weight_, "cost_weight", 3.81); + getParam(critical_cost_, "critical_cost", 300.0); + getParam(collision_cost_, "collision_cost", 1000000.0); + getParam(near_goal_distance_, "near_goal_distance", 0.5); + getParam(inflation_layer_name_, "inflation_layer_name", std::string("")); + + // Normalized by cost value to put in same regime as other weights + weight_ /= 254.0f; + + collision_checker_.setCostmap(costmap_); + possible_collision_cost_ = findCircumscribedCost(costmap_ros_); + + if (possible_collision_cost_ < 1.0f) { + RCLCPP_ERROR( + logger_, + "Inflation layer either not found or inflation is not set sufficiently for " + "optimized non-circular collision checking capabilities. It is HIGHLY recommended to set" + " the inflation radius to be at MINIMUM half of the robot's largest cross-section. See " + "github.com/ros-planning/navigation2/tree/main/nav2_smac_planner#potential-fields" + " for full instructions. This will substantially impact run-time performance."); + } + + RCLCPP_INFO( + logger_, + "InflationCostCritic instantiated with %d power and %f / %f weights. " + "Critic will collision check based on %s cost.", + power_, critical_cost_, weight_, consider_footprint_ ? + "footprint" : "circular"); +} + +float CostCritic::findCircumscribedCost( + std::shared_ptr costmap) +{ + double result = -1.0; + const double circum_radius = costmap->getLayeredCostmap()->getCircumscribedRadius(); + if (static_cast(circum_radius) == circumscribed_radius_) { + // early return if footprint size is unchanged + return circumscribed_cost_; + } + + // check if the costmap has an inflation layer + const auto inflation_layer = nav2_costmap_2d::InflationLayer::getInflationLayer( + costmap, + inflation_layer_name_); + if (inflation_layer != nullptr) { + const double resolution = costmap->getCostmap()->getResolution(); + result = inflation_layer->computeCost(circum_radius / resolution); + } else { + RCLCPP_WARN( + logger_, + "No inflation layer found in costmap configuration. " + "If this is an SE2-collision checking plugin, it cannot use costmap potential " + "field to speed up collision checking by only checking the full footprint " + "when robot is within possibly-inscribed radius of an obstacle. This may " + "significantly slow down planning times and not avoid anything but absolute collisions!"); + } + + circumscribed_radius_ = static_cast(circum_radius); + circumscribed_cost_ = static_cast(result); + + return circumscribed_cost_; +} + +void CostCritic::score(CriticData & data) +{ + using xt::evaluation_strategy::immediate; + if (!enabled_) { + return; + } + + if (consider_footprint_) { + // footprint may have changed since initialization if user has dynamic footprints + possible_collision_cost_ = findCircumscribedCost(costmap_ros_); + } + + // If near the goal, don't apply the preferential term since the goal is near obstacles + bool near_goal = false; + if (utils::withinPositionGoalTolerance(near_goal_distance_, data.state.pose.pose, data.path)) { + near_goal = true; + } + + auto && repulsive_cost = xt::xtensor::from_shape({data.costs.shape(0)}); + repulsive_cost.fill(0.0); + + const size_t traj_len = data.trajectories.x.shape(1); + bool all_trajectories_collide = true; + for (size_t i = 0; i < data.trajectories.x.shape(0); ++i) { + bool trajectory_collide = false; + const auto & traj = data.trajectories; + float pose_cost; + + for (size_t j = 0; j < traj_len; j++) { + // The costAtPose doesn't use orientation + // The footprintCostAtPose will always return "INSCRIBED" if footprint is over it + // So the center point has more information than the footprint + pose_cost = costAtPose(traj.x(i, j), traj.y(i, j)); + if (pose_cost < 1.0f) {continue;} // In free space + + if (inCollision(pose_cost, traj.x(i, j), traj.y(i, j), traj.yaws(i, j))) { + trajectory_collide = true; + break; + } + + // Let near-collision trajectory points be punished severely + // Note that we collision check based on the footprint actual, + // but score based on the center-point cost regardless + using namespace nav2_costmap_2d; // NOLINT + if (pose_cost >= INSCRIBED_INFLATED_OBSTACLE) { + repulsive_cost[i] += critical_cost_; + } else if (!near_goal) { // Generally prefer trajectories further from obstacles + repulsive_cost[i] += pose_cost; + } + } + + if (!trajectory_collide) { + all_trajectories_collide = false; + } else { + repulsive_cost[i] = collision_cost_; + } + } + + data.costs += xt::pow((weight_ * repulsive_cost / traj_len), power_); + data.fail_flag = all_trajectories_collide; +} + +/** + * @brief Checks if cost represents a collision + * @param cost Costmap cost + * @return bool if in collision + */ +bool CostCritic::inCollision(float cost, float x, float y, float theta) +{ + bool is_tracking_unknown = + costmap_ros_->getLayeredCostmap()->isTrackingUnknown(); + + // If consider_footprint_ check footprint scort for collision + if (consider_footprint_ && + (cost >= possible_collision_cost_ || possible_collision_cost_ < 1.0f)) + { + cost = static_cast(collision_checker_.footprintCostAtPose( + x, y, theta, costmap_ros_->getRobotFootprint())); + } + + switch (static_cast(cost)) { + using namespace nav2_costmap_2d; // NOLINT + case (LETHAL_OBSTACLE): + return true; + case (INSCRIBED_INFLATED_OBSTACLE): + return consider_footprint_ ? false : true; + case (NO_INFORMATION): + return is_tracking_unknown ? false : true; + } + + return false; +} + +float CostCritic::costAtPose(float x, float y) +{ + using namespace nav2_costmap_2d; // NOLINT + unsigned int x_i, y_i; + if (!collision_checker_.worldToMap(x, y, x_i, y_i)) { + return nav2_costmap_2d::NO_INFORMATION; + } + + return collision_checker_.pointCost(x_i, y_i); +} + +} // namespace mppi::critics + +#include + +PLUGINLIB_EXPORT_CLASS( + mppi::critics::CostCritic, + mppi::critics::CriticFunction) diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp index 21d82396159..cdf0113564a 100644 --- a/nav2_mppi_controller/src/critics/obstacles_critic.cpp +++ b/nav2_mppi_controller/src/critics/obstacles_critic.cpp @@ -32,9 +32,9 @@ void ObstaclesCritic::initialize() getParam(inflation_layer_name_, "inflation_layer_name", std::string("")); collision_checker_.setCostmap(costmap_); - possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_); + possible_collision_cost_ = findCircumscribedCost(costmap_ros_); - if (possibly_inscribed_cost_ < 1.0f) { + if (possible_collision_cost_ < 1.0f) { RCLCPP_ERROR( logger_, "Inflation layer either not found or inflation is not set sufficiently for " @@ -111,7 +111,7 @@ void ObstaclesCritic::score(CriticData & data) if (consider_footprint_) { // footprint may have changed since initialization if user has dynamic footprints - possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_); + possible_collision_cost_ = findCircumscribedCost(costmap_ros_); } // If near the goal, don't apply the preferential term since the goal is near obstacles @@ -212,7 +212,7 @@ CollisionCost ObstaclesCritic::costAtPose(float x, float y, float theta) cost = collision_checker_.pointCost(x_i, y_i); if (consider_footprint_ && - (cost >= possibly_inscribed_cost_ || possibly_inscribed_cost_ < 1.0f)) + (cost >= possible_collision_cost_ || possible_collision_cost_ < 1.0f)) { cost = static_cast(collision_checker_.footprintCostAtPose( x, y, theta, costmap_ros_->getRobotFootprint())); diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index 08f50945fd5..fee853cda43 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -24,6 +24,7 @@ #include "nav2_mppi_controller/critics/goal_angle_critic.hpp" #include "nav2_mppi_controller/critics/goal_critic.hpp" #include "nav2_mppi_controller/critics/obstacles_critic.hpp" +#include "nav2_mppi_controller/critics/cost_critic.hpp" #include "nav2_mppi_controller/critics/path_align_critic.hpp" #include "nav2_mppi_controller/critics/path_align_legacy_critic.hpp" #include "nav2_mppi_controller/critics/path_angle_critic.hpp" diff --git a/nav2_mppi_controller/test/optimizer_smoke_test.cpp b/nav2_mppi_controller/test/optimizer_smoke_test.cpp index d694496395d..9a77f6d8b06 100644 --- a/nav2_mppi_controller/test/optimizer_smoke_test.cpp +++ b/nav2_mppi_controller/test/optimizer_smoke_test.cpp @@ -103,7 +103,7 @@ INSTANTIATE_TEST_SUITE_P( std::make_tuple( "DiffDrive", std::vector( - {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"}, + {{"GoalCritic"}, {"GoalAngleCritic"}, {"CostCritic"}, {"PathAngleCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}}), true), std::make_tuple( diff --git a/nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp b/nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp index a4ce84404a8..966a8458521 100644 --- a/nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp +++ b/nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp @@ -43,6 +43,7 @@ #include #include #include +#include namespace nav2_navfn_planner { @@ -131,14 +132,16 @@ class NavFn /** * @brief Calculates a plan using the A* heuristic, returns true if one is found + * @param cancelChecker Function to check if the task has been canceled * @return True if a plan is found, false otherwise */ - bool calcNavFnAstar(); + bool calcNavFnAstar(std::function cancelChecker); /** - * @brief Caclulates the full navigation function using Dijkstra + * @brief Calculates the full navigation function using Dijkstra + * @param cancelChecker Function to check if the task has been canceled */ - bool calcNavFnDijkstra(bool atStart = false); + bool calcNavFnDijkstra(std::function cancelChecker, bool atStart = false); /** * @brief Accessor for the x-coordinates of a path @@ -179,6 +182,9 @@ class NavFn float curT; /**< current threshold */ float priInc; /**< priority threshold increment */ + /**< number of cycles between checks for cancellation */ + static constexpr int terminal_checking_interval = 5000; + /** goal and start positions */ /** * @brief Sets the goal position for the planner. @@ -229,18 +235,20 @@ class NavFn * @brief Run propagation for iterations, or until start is reached using * breadth-first Dijkstra method * @param cycles The maximum number of iterations to run for + * @param cancelChecker Function to check if the task has been canceled * @param atStart Whether or not to stop when the start point is reached * @return true if the start point is reached */ - bool propNavFnDijkstra(int cycles, bool atStart = false); + bool propNavFnDijkstra(int cycles, std::function cancelChecker, bool atStart = false); /** * @brief Run propagation for iterations, or until start is reached using * the best-first A* method with Euclidean distance heuristic * @param cycles The maximum number of iterations to run for + * @param cancelChecker Function to check if the task has been canceled * @return true if the start point is reached */ - bool propNavFnAstar(int cycles); /**< returns true if start point found */ + bool propNavFnAstar(int cycles, std::function cancelChecker); /** gradient and paths */ float * gradx, * grady; /**< gradient arrays, size of potential array */ diff --git a/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp b/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp index 2330a5165b9..c1cebb1bef0 100644 --- a/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp +++ b/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp @@ -81,11 +81,13 @@ class NavfnPlanner : public nav2_core::GlobalPlanner * @brief Creating a plan from start and goal poses * @param start Start pose * @param goal Goal pose + * @param cancel_checker Function to check if the task has been canceled * @return nav_msgs::Path of the generated path */ nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal) override; + const geometry_msgs::msg::PoseStamped & goal, + std::function cancel_checker) override; protected: /** @@ -93,12 +95,14 @@ class NavfnPlanner : public nav2_core::GlobalPlanner * @param start Start pose * @param goal Goal pose * @param tolerance Relaxation constraint in x and y + * @param cancel_checker Function to check if the task has been canceled * @param plan Path to be computed * @return true if can find the path */ bool makePlan( const geometry_msgs::msg::Pose & start, const geometry_msgs::msg::Pose & goal, double tolerance, + std::function cancel_checker, nav_msgs::msg::Path & plan); /** diff --git a/nav2_navfn_planner/src/navfn.cpp b/nav2_navfn_planner/src/navfn.cpp index 8259950b038..2cce713b0f2 100644 --- a/nav2_navfn_planner/src/navfn.cpp +++ b/nav2_navfn_planner/src/navfn.cpp @@ -44,6 +44,7 @@ #include "nav2_navfn_planner/navfn.hpp" #include +#include "nav2_core/planner_exceptions.hpp" #include "rclcpp/rclcpp.hpp" namespace nav2_navfn_planner @@ -293,12 +294,12 @@ NavFn::setCostmap(const COSTTYPE * cmap, bool isROS, bool allow_unknown) } bool -NavFn::calcNavFnDijkstra(bool atStart) +NavFn::calcNavFnDijkstra(std::function cancelChecker, bool atStart) { setupNavFn(true); // calculate the nav fn and path - return propNavFnDijkstra(std::max(nx * ny / 20, nx + ny), atStart); + return propNavFnDijkstra(std::max(nx * ny / 20, nx + ny), cancelChecker, atStart); } @@ -307,12 +308,12 @@ NavFn::calcNavFnDijkstra(bool atStart) // bool -NavFn::calcNavFnAstar() +NavFn::calcNavFnAstar(std::function cancelChecker) { setupNavFn(true); // calculate the nav fn and path - return propNavFnAstar(std::max(nx * ny / 20, nx + ny)); + return propNavFnAstar(std::max(nx * ny / 20, nx + ny), cancelChecker); } // @@ -571,7 +572,7 @@ NavFn::updateCellAstar(int n) // bool -NavFn::propNavFnDijkstra(int cycles, bool atStart) +NavFn::propNavFnDijkstra(int cycles, std::function cancelChecker, bool atStart) { int nwv = 0; // max priority block size int nc = 0; // number of cells put into priority blocks @@ -581,6 +582,10 @@ NavFn::propNavFnDijkstra(int cycles, bool atStart) int startCell = start[1] * nx + start[0]; for (; cycle < cycles; cycle++) { // go for this many cycles, unless interrupted + if (cycle % terminal_checking_interval == 0 && cancelChecker()) { + throw nav2_core::PlannerCancelled("Planner was cancelled"); + } + if (curPe == 0 && nextPe == 0) { // priority blocks empty break; } @@ -652,7 +657,7 @@ NavFn::propNavFnDijkstra(int cycles, bool atStart) // bool -NavFn::propNavFnAstar(int cycles) +NavFn::propNavFnAstar(int cycles, std::function cancelChecker) { int nwv = 0; // max priority block size int nc = 0; // number of cells put into priority blocks @@ -667,6 +672,10 @@ NavFn::propNavFnAstar(int cycles) // do main cycle for (; cycle < cycles; cycle++) { // go for this many cycles, unless interrupted + if (cycle % terminal_checking_interval == 0 && cancelChecker()) { + throw nav2_core::PlannerCancelled("Planner was cancelled"); + } + if (curPe == 0 && nextPe == 0) { // priority blocks empty break; } diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index 2ee8d69f53e..9985280159e 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -129,7 +129,8 @@ NavfnPlanner::cleanup() nav_msgs::msg::Path NavfnPlanner::createPlan( const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal) + const geometry_msgs::msg::PoseStamped & goal, + std::function cancel_checker) { #ifdef BENCHMARK_TESTING steady_clock::time_point a = steady_clock::now(); @@ -183,7 +184,7 @@ nav_msgs::msg::Path NavfnPlanner::createPlan( return path; } - if (!makePlan(start.pose, goal.pose, tolerance_, path)) { + if (!makePlan(start.pose, goal.pose, tolerance_, cancel_checker, path)) { throw nav2_core::NoValidPathCouldBeFound( "Failed to create plan with tolerance of: " + std::to_string(tolerance_) ); } @@ -214,6 +215,7 @@ bool NavfnPlanner::makePlan( const geometry_msgs::msg::Pose & start, const geometry_msgs::msg::Pose & goal, double tolerance, + std::function cancel_checker, nav_msgs::msg::Path & plan) { // clear the plan, just in case @@ -261,9 +263,9 @@ NavfnPlanner::makePlan( planner_->setStart(map_goal); planner_->setGoal(map_start); if (use_astar_) { - planner_->calcNavFnAstar(); + planner_->calcNavFnAstar(cancel_checker); } else { - planner_->calcNavFnDijkstra(true); + planner_->calcNavFnDijkstra(cancel_checker, true); } double resolution = costmap_->getResolution(); diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp index 7d2c9d03f00..bdffda281a2 100644 --- a/nav2_planner/include/nav2_planner/planner_server.hpp +++ b/nav2_planner/include/nav2_planner/planner_server.hpp @@ -68,12 +68,15 @@ class PlannerServer : public nav2_util::LifecycleNode * @brief Method to get plan from the desired plugin * @param start starting pose * @param goal goal request + * @param planner_id The planner to plan with + * @param cancel_checker A function to check if the action has been canceled * @return Path */ nav_msgs::msg::Path getPlan( const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal, - const std::string & planner_id); + const std::string & planner_id, + std::function cancel_checker); protected: /** diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index de239b3587e..7f8dc4a72de 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -394,6 +394,10 @@ void PlannerServer::computePlanThroughPoses() throw nav2_core::PlannerTFError("Unable to get start pose"); } + auto cancel_checker = [this]() { + return action_server_poses_->is_cancel_requested(); + }; + // Get consecutive paths through these points for (unsigned int i = 0; i != goal->goals.size(); i++) { // Get starting point @@ -413,7 +417,9 @@ void PlannerServer::computePlanThroughPoses() } // Get plan from start -> goal - nav_msgs::msg::Path curr_path = getPlan(curr_start, curr_goal, goal->planner_id); + nav_msgs::msg::Path curr_path = getPlan( + curr_start, curr_goal, goal->planner_id, + cancel_checker); if (!validatePath(curr_goal, curr_path, goal->planner_id)) { throw nav2_core::NoValidPathCouldBeFound(goal->planner_id + " generated a empty path"); @@ -476,6 +482,9 @@ void PlannerServer::computePlanThroughPoses() exceptionWarning(curr_start, curr_goal, goal->planner_id, ex); result->error_code = ActionThroughPosesResult::NO_VIAPOINTS_GIVEN; action_server_poses_->terminate_current(result); + } catch (nav2_core::PlannerCancelled &) { + RCLCPP_INFO(get_logger(), "Goal was canceled. Canceling planning action."); + action_server_poses_->terminate_all(); } catch (std::exception & ex) { exceptionWarning(curr_start, curr_goal, goal->planner_id, ex); result->error_code = ActionThroughPosesResult::UNKNOWN; @@ -516,7 +525,11 @@ PlannerServer::computePlan() throw nav2_core::PlannerTFError("Unable to transform poses to global frame"); } - result->path = getPlan(start, goal_pose, goal->planner_id); + auto cancel_checker = [this]() { + return action_server_pose_->is_cancel_requested(); + }; + + result->path = getPlan(start, goal_pose, goal->planner_id, cancel_checker); if (!validatePath(goal_pose, result->path, goal->planner_id)) { throw nav2_core::NoValidPathCouldBeFound(goal->planner_id + " generated a empty path"); @@ -567,6 +580,9 @@ PlannerServer::computePlan() exceptionWarning(start, goal->goal, goal->planner_id, ex); result->error_code = ActionToPoseResult::TF_ERROR; action_server_pose_->terminate_current(result); + } catch (nav2_core::PlannerCancelled &) { + RCLCPP_INFO(get_logger(), "Goal was canceled. Canceling planning action."); + action_server_pose_->terminate_all(); } catch (std::exception & ex) { exceptionWarning(start, goal->goal, goal->planner_id, ex); result->error_code = ActionToPoseResult::UNKNOWN; @@ -578,7 +594,8 @@ nav_msgs::msg::Path PlannerServer::getPlan( const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal, - const std::string & planner_id) + const std::string & planner_id, + std::function cancel_checker) { RCLCPP_DEBUG( get_logger(), "Attempting to a find path from (%.2f, %.2f) to " @@ -586,14 +603,14 @@ PlannerServer::getPlan( goal.pose.position.x, goal.pose.position.y); if (planners_.find(planner_id) != planners_.end()) { - return planners_[planner_id]->createPlan(start, goal); + return planners_[planner_id]->createPlan(start, goal, cancel_checker); } else { if (planners_.size() == 1 && planner_id.empty()) { RCLCPP_WARN_ONCE( get_logger(), "No planners specified in action call. " "Server will use only plugin %s in server." " This warning will appear once.", planner_ids_concat_.c_str()); - return planners_[planners_.begin()->first]->createPlan(start, goal); + return planners_[planners_.begin()->first]->createPlan(start, goal, cancel_checker); } else { RCLCPP_ERROR( get_logger(), "planner %s is not a valid planner. " diff --git a/nav2_rviz_plugins/src/selector.cpp b/nav2_rviz_plugins/src/selector.cpp index f0f3d4e3358..1df075f82d3 100644 --- a/nav2_rviz_plugins/src/selector.cpp +++ b/nav2_rviz_plugins/src/selector.cpp @@ -169,7 +169,7 @@ void Selector::pluginLoader( } RCLCPP_INFO( node->get_logger(), - (server_name + " service not available").c_str()); + "%s service not available", server_name.c_str()); server_unavailable = true; server_failed_ = true; break; diff --git a/nav2_smac_planner/README.md b/nav2_smac_planner/README.md index 3a992088b69..6e92209068d 100644 --- a/nav2_smac_planner/README.md +++ b/nav2_smac_planner/README.md @@ -109,6 +109,7 @@ planner_server: allow_unknown: false # allow traveling in unknown space max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable max_on_approach_iterations: 1000 # maximum number of iterations to attempt to reach goal once in tolerance + terminal_checking_interval: 5000 # number of iterations between checking if the goal has been cancelled or planner timed out max_planning_time: 3.5 # max time in s for planner to plan, smooth, and upsample. Will scale maximum smoothing and upsampling times based on remaining time after planning. motion_model_for_search: "DUBIN" # For Hybrid Dubin, Redds-Shepp cost_travel_multiplier: 2.0 # For 2D: Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*. diff --git a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp index e4ecf38f035..3fca0e8b558 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -51,6 +52,7 @@ class AStarAlgorithm typedef NodeT * NodePtr; typedef robin_hood::unordered_node_map Graph; typedef std::vector NodeVector; + typedef std::unordered_set NodeSet; typedef std::pair> NodeElement; typedef typename NodeT::Coordinates Coordinates; typedef typename NodeT::CoordinateVector CoordinateVector; @@ -88,6 +90,8 @@ class AStarAlgorithm * @param max_on_approach_iterations Maximum number of iterations before returning a valid * path once within thresholds to refine path * comes at more compute time but smoother paths. + * @param terminal_checking_interval Number of iterations to check if the task has been canceled or + * or planning time exceeded * @param max_planning_time Maximum time (in seconds) to wait for a plan, createPath returns * false after this timeout */ @@ -95,20 +99,24 @@ class AStarAlgorithm const bool & allow_unknown, int & max_iterations, const int & max_on_approach_iterations, + const int & terminal_checking_interval, const double & max_planning_time, const float & lookup_table_size, - const unsigned int & dim_3_size); + const unsigned int & dim_3_size, + const GoalHeadingMode & goal_heading_mode = GoalHeadingMode::DEFAULT); /** * @brief Creating path from given costmap, start, and goal * @param path Reference to a vector of indicies of generated path * @param num_iterations Reference to number of iterations to create plan * @param tolerance Reference to tolerance in costmap nodes + * @param cancel_checker Function to check if the task has been canceled * @param expansions_log Optional expansions logged for debug * @return if plan was successful */ bool createPath( CoordinateVector & path, int & num_iterations, const float & tolerance, + std::function cancel_checker, std::vector> * expansions_log = nullptr); /** @@ -155,7 +163,7 @@ class AStarAlgorithm * @brief Get pointer reference to goal node * @return Node pointer reference to goal node */ - NodePtr & getGoal(); + NodeSet & getGoals(); /** * @brief Get maximum number of on-approach iterations after within threshold @@ -187,6 +195,13 @@ class AStarAlgorithm */ unsigned int & getSizeDim3(); + /** + * @brief Return the first goal coordinate defined by the user + * before applying the heading mode + * @return Coordinate to the first goal + */ + Coordinates getInitialGoalCoordinate(); + protected: /** * @brief Get pointer to next goal in open set @@ -250,11 +265,10 @@ class AStarAlgorithm */ void clearStart(); - int _timing_interval = 5000; - bool _traverse_unknown; int _max_iterations; int _max_on_approach_iterations; + int _terminal_checking_interval; double _max_planning_time; float _tolerance; unsigned int _x_size; @@ -262,9 +276,10 @@ class AStarAlgorithm unsigned int _dim3_size; SearchInfo _search_info; - Coordinates _goal_coordinates; + CoordinateVector _goals_coordinates; NodePtr _start; - NodePtr _goal; + NodeSet _goalsSet; + GoalHeadingMode _goal_heading_mode; Graph _graph; NodeQueue _queue; diff --git a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp index 10c4224a077..967d4f65e94 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include "nav2_smac_planner/node_2d.hpp" #include "nav2_smac_planner/node_hybrid.hpp" @@ -34,6 +35,7 @@ class AnalyticExpansion { public: typedef NodeT * NodePtr; + typedef std::unordered_set NodeSet; typedef typename NodeT::Coordinates Coordinates; typedef std::function NodeGetter; @@ -87,7 +89,8 @@ class AnalyticExpansion */ NodePtr tryAnalyticExpansion( const NodePtr & current_node, - const NodePtr & goal_node, + const NodeSet & goals_node, + const Coordinates & initial_goal_coords, const NodeGetter & getter, int & iterations, int & best_cost); /** diff --git a/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp b/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp index 53e019a3aed..205c3f0471a 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp @@ -66,7 +66,7 @@ class GridCollisionChecker void setFootprint( const nav2_costmap_2d::Footprint & footprint, const bool & radius, - const double & possible_inscribed_cost); + const double & possible_collision_cost); /** * @brief Check if in collision with costmap and footprint at pose @@ -130,7 +130,7 @@ class GridCollisionChecker float footprint_cost_; bool footprint_is_radius_; std::vector angles_; - float possible_inscribed_cost_{-1}; + float possible_collision_cost_{-1}; rclcpp::Logger logger_{rclcpp::get_logger("SmacPlannerCollisionChecker")}; rclcpp::Clock::SharedPtr clock_; }; diff --git a/nav2_smac_planner/include/nav2_smac_planner/constants.hpp b/nav2_smac_planner/include/nav2_smac_planner/constants.hpp index a7ff03fdca2..9c28b1ab3a0 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/constants.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/constants.hpp @@ -28,6 +28,14 @@ enum class MotionModel STATE_LATTICE = 4, }; +enum class GoalHeadingMode +{ + UNKNOWN = 0, + DEFAULT = 1, + BIDIRECTIONAL = 2, + ALL_DIRECTION = 3, +}; + inline std::string toString(const MotionModel & n) { switch (n) { @@ -59,6 +67,33 @@ inline MotionModel fromString(const std::string & n) } } +inline std::string toString(const GoalHeadingMode & n) +{ + switch (n) { + case GoalHeadingMode::DEFAULT: + return "DEFAULT"; + case GoalHeadingMode::BIDIRECTIONAL: + return "BIDIRECTIONAL"; + case GoalHeadingMode::ALL_DIRECTION: + return "ALL_DIRECTION"; + default: + return "Unknown"; + } +} + +inline GoalHeadingMode fromStringToGH(const std::string & n) +{ + if (n == "DEFAULT") { + return GoalHeadingMode::DEFAULT; + } else if (n == "BIDIRECTIONAL") { + return GoalHeadingMode::BIDIRECTIONAL; + } else if (n == "ALL_DIRECTION") { + return GoalHeadingMode::ALL_DIRECTION; + } else { + return GoalHeadingMode::UNKNOWN; + } +} + const float UNKNOWN = 255.0; const float OCCUPIED = 254.0; const float INSCRIBED = 253.0; diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp index c46a9471869..56bf8c74f16 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp @@ -165,12 +165,12 @@ class NodeHybrid : x(x_in), y(y_in), theta(theta_in) {} - inline bool operator==(const Coordinates & rhs) + inline bool operator==(const Coordinates & rhs) const { return this->x == rhs.x && this->y == rhs.y && this->theta == rhs.theta; } - inline bool operator!=(const Coordinates & rhs) + inline bool operator!=(const Coordinates & rhs) const { return !(*this == rhs); } @@ -459,6 +459,12 @@ class NodeHybrid */ bool backtracePath(CoordinateVector & path); + /** + * \brief Sets the goal mode for the current search + * \param goal_heading_mode The goal heading mode to use + */ + static void setGoalHeadingMode(const GoalHeadingMode & current_goal_heading_mode); + NodeHybrid * parent; Coordinates pose; @@ -476,6 +482,7 @@ class NodeHybrid // Dubin / Reeds-Shepp lookup and size for dereferencing static LookupTable dist_heuristic_lookup_table; static float size_lookup; + static GoalHeadingMode goal_heading_mode; private: float _cell_cost; diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp index 99f50d6a92b..2ffc213087f 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp @@ -415,12 +415,19 @@ class NodeLattice */ void addNodeToPath(NodePtr current_node, CoordinateVector & path); + /** + * \brief Sets the goal mode for the current search + * \param goal_heading_mode The goal heading mode to use + */ + static void setGoalHeadingMode(const GoalHeadingMode & current_goal_heading_mode); + NodeLattice * parent; Coordinates pose; static LatticeMotionTable motion_table; // Dubin / Reeds-Shepp lookup and size for dereferencing static LookupTable dist_heuristic_lookup_table; static float size_lookup; + static GoalHeadingMode goal_heading_mode; private: float _cell_cost; diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp index c499d0796fb..059871d8677 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp @@ -82,11 +82,13 @@ class SmacPlanner2D : public nav2_core::GlobalPlanner * @brief Creating a plan from start and goal poses * @param start Start pose * @param goal Goal pose + * @param cancel_checker Function to check if the action has been canceled * @return nav2_msgs::Path of the generated path */ nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal) override; + const geometry_msgs::msg::PoseStamped & goal, + std::function cancel_checker) override; protected: /** @@ -112,6 +114,7 @@ class SmacPlanner2D : public nav2_core::GlobalPlanner bool _allow_unknown; int _max_iterations; int _max_on_approach_iterations; + int _terminal_checking_interval; bool _use_final_approach_orientation; SearchInfo _search_info; std::string _motion_model_for_search; diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp index fae139aa2f8..73f85aa8c0c 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp @@ -81,11 +81,13 @@ class SmacPlannerHybrid : public nav2_core::GlobalPlanner * @brief Creating a plan from start and goal poses * @param start Start pose * @param goal Goal pose + * @param cancel_checker Function to check if the action has been canceled * @return nav2_msgs::Path of the generated path */ nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal) override; + const geometry_msgs::msg::PoseStamped & goal, + std::function cancel_checker) override; protected: /** @@ -113,6 +115,7 @@ class SmacPlannerHybrid : public nav2_core::GlobalPlanner bool _allow_unknown; int _max_iterations; int _max_on_approach_iterations; + int _terminal_checking_interval; SearchInfo _search_info; double _max_planning_time; double _lookup_table_size; @@ -120,6 +123,7 @@ class SmacPlannerHybrid : public nav2_core::GlobalPlanner bool _debug_visualizations; std::string _motion_model_for_search; MotionModel _motion_model; + GoalHeadingMode _goal_heading_mode; rclcpp_lifecycle::LifecyclePublisher::SharedPtr _raw_plan_publisher; rclcpp_lifecycle::LifecyclePublisher::SharedPtr _planned_footprints_publisher; diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp index ce8bafd2fd0..04325bc7147 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp @@ -80,11 +80,13 @@ class SmacPlannerLattice : public nav2_core::GlobalPlanner * @brief Creating a plan from start and goal poses * @param start Start pose * @param goal Goal pose + * @param cancel_checker Function to check if the action has been canceled * @return nav2_msgs::Path of the generated path */ nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal) override; + const geometry_msgs::msg::PoseStamped & goal, + std::function cancel_checker) override; protected: /** @@ -108,6 +110,7 @@ class SmacPlannerLattice : public nav2_core::GlobalPlanner bool _allow_unknown; int _max_iterations; int _max_on_approach_iterations; + int _terminal_checking_interval; float _tolerance; rclcpp_lifecycle::LifecyclePublisher::SharedPtr _raw_plan_publisher; double _max_planning_time; @@ -117,6 +120,7 @@ class SmacPlannerLattice : public nav2_core::GlobalPlanner _planned_footprints_publisher; rclcpp_lifecycle::LifecyclePublisher::SharedPtr _expansions_publisher; + GoalHeadingMode _goal_heading_mode; std::mutex _mutex; rclcpp_lifecycle::LifecycleNode::WeakPtr _node; diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index ebade0f4aa0..e9529bbdca1 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -37,13 +37,14 @@ AStarAlgorithm::AStarAlgorithm( const SearchInfo & search_info) : _traverse_unknown(true), _max_iterations(0), + _terminal_checking_interval(5000), _max_planning_time(0), _x_size(0), _y_size(0), _search_info(search_info), - _goal_coordinates(Coordinates()), + _goals_coordinates(CoordinateVector()), _start(nullptr), - _goal(nullptr), + _goalsSet(NodeSet()), _motion_model(motion_model) { _graph.reserve(100000); @@ -59,15 +60,21 @@ void AStarAlgorithm::initialize( const bool & allow_unknown, int & max_iterations, const int & max_on_approach_iterations, + const int & terminal_checking_interval, const double & max_planning_time, const float & lookup_table_size, - const unsigned int & dim_3_size) + const unsigned int & dim_3_size, + const GoalHeadingMode & goal_heading_mode) { _traverse_unknown = allow_unknown; _max_iterations = max_iterations; _max_on_approach_iterations = max_on_approach_iterations; + _terminal_checking_interval = terminal_checking_interval; _max_planning_time = max_planning_time; - NodeT::precomputeDistanceHeuristic(lookup_table_size, _motion_model, dim_3_size, _search_info); + _goal_heading_mode = goal_heading_mode; + NodeT::setGoalHeadingMode(_goal_heading_mode); + NodeT::precomputeDistanceHeuristic( + lookup_table_size, _motion_model, dim_3_size, _search_info); _dim3_size = dim_3_size; _expander = std::make_unique>( _motion_model, _search_info, _traverse_unknown, _dim3_size); @@ -78,14 +85,18 @@ void AStarAlgorithm::initialize( const bool & allow_unknown, int & max_iterations, const int & max_on_approach_iterations, + const int & terminal_checking_interval, const double & max_planning_time, const float & /*lookup_table_size*/, - const unsigned int & dim_3_size) + const unsigned int & dim_3_size, + const GoalHeadingMode & goal_heading_mode) { _traverse_unknown = allow_unknown; _max_iterations = max_iterations; _max_on_approach_iterations = max_on_approach_iterations; + _terminal_checking_interval = terminal_checking_interval; _max_planning_time = max_planning_time; + _goal_heading_mode = goal_heading_mode; if (dim_3_size != 1) { throw std::runtime_error("Node type Node2D cannot be given non-1 dim 3 quantization."); @@ -184,9 +195,11 @@ void AStarAlgorithm::setGoal( if (dim_3 != 0) { throw std::runtime_error("Node type Node2D cannot be given non-zero goal dim 3."); } + _goals_coordinates.clear(); + _goalsSet.clear(); - _goal = addToGraph(Node2D::getIndex(mx, my, getSizeX())); - _goal_coordinates = Node2D::Coordinates(mx, my); + _goalsSet.insert(addToGraph(Node2D::getIndex(mx, my, getSizeX()))); + _goals_coordinates.push_back(Node2D::Coordinates(mx, my)); } template @@ -195,14 +208,64 @@ void AStarAlgorithm::setGoal( const unsigned int & my, const unsigned int & dim_3) { - _goal = addToGraph(NodeT::getIndex(mx, my, dim_3)); - - typename NodeT::Coordinates goal_coords( - static_cast(mx), - static_cast(my), - static_cast(dim_3)); + _goalsSet.clear(); + NodeVector goals; + CoordinateVector goals_coordinates; + unsigned int num_bins = NodeT::motion_table.num_angle_quantization; + unsigned int dim_3_half_bin = 0; + switch (_goal_heading_mode) { + case GoalHeadingMode::DEFAULT: + goals.push_back(addToGraph(NodeT::getIndex(mx, my, dim_3))); + goals_coordinates.push_back( + typename NodeT::Coordinates( + static_cast(mx), + static_cast(my), + static_cast(dim_3))); + break; + case GoalHeadingMode::BIDIRECTIONAL: + // Add two goals, one for each direction + goals.push_back(addToGraph(NodeT::getIndex(mx, my, dim_3))); + // 180 degrees + dim_3_half_bin = (dim_3 + (num_bins / 2)) % num_bins; + _goalsSet.insert(addToGraph(NodeT::getIndex(mx, my, dim_3_half_bin))); + goals_coordinates.push_back( + typename NodeT::Coordinates( + static_cast(mx), + static_cast(my), + static_cast(dim_3))); + goals_coordinates.push_back( + typename NodeT::Coordinates( + static_cast(mx), + static_cast(my), + static_cast(dim_3_half_bin))); + break; + case GoalHeadingMode::ALL_DIRECTION: + // Add all possible headings as goals + // set first goal to be the same as the input + goals.push_back(addToGraph(NodeT::getIndex(mx, my, dim_3))); + goals_coordinates.push_back( + typename NodeT::Coordinates( + static_cast(mx), + static_cast(my), + static_cast(dim_3))); + for (unsigned int i = 0; i < num_bins; ++i) { + if (i == dim_3) { + continue; + } + goals.push_back(addToGraph(NodeT::getIndex(mx, my, i))); + goals_coordinates.push_back( + typename NodeT::Coordinates( + static_cast(mx), + static_cast(my), + static_cast(i))); + } + break; + case GoalHeadingMode::UNKNOWN: + throw std::runtime_error("Goal heading is UNKNOWN."); + break; + } - if (!_search_info.cache_obstacle_heuristic || goal_coords != _goal_coordinates) { + if (!_search_info.cache_obstacle_heuristic || goals_coordinates != _goals_coordinates) { if (!_start) { throw std::runtime_error("Start must be set before goal."); } @@ -211,8 +274,11 @@ void AStarAlgorithm::setGoal( _collision_checker->getCostmapROS(), _start->pose.x, _start->pose.y, mx, my); } - _goal_coordinates = goal_coords; - _goal->setPose(_goal_coordinates); + _goals_coordinates = goals_coordinates; + for (unsigned int i = 0; i < goals.size(); i++) { + goals[i]->setPose(_goals_coordinates[i]); + _goalsSet.insert(goals[i]); + } } template @@ -224,15 +290,17 @@ bool AStarAlgorithm::areInputsValid() } // Check if points were filled in - if (!_start || !_goal) { + if (!_start || _goalsSet.empty()) { throw std::runtime_error("Failed to compute path, no valid start or goal given."); } // Check if ending point is valid - if (getToleranceHeuristic() < 0.001 && - !_goal->isNodeValid(_traverse_unknown, _collision_checker)) - { - throw nav2_core::GoalOccupied("Goal was in lethal cost"); + if (getToleranceHeuristic() < 0.001) { + // if any goal is valid, then all goals are valid + auto goal = *_goalsSet.begin(); + if (!goal->isNodeValid(_traverse_unknown, _collision_checker)) { + throw nav2_core::GoalOccupied("Goal was in lethal cost"); + } } // Note: We do not check the if the start is valid because it is cleared @@ -245,6 +313,7 @@ template bool AStarAlgorithm::createPath( CoordinateVector & path, int & iterations, const float & tolerance, + std::function cancel_checker, std::vector> * expansions_log) { steady_clock::time_point start_time = steady_clock::now(); @@ -285,8 +354,11 @@ bool AStarAlgorithm::createPath( }; while (iterations < getMaxIterations() && !_queue.empty()) { - // Check for planning timeout only on every Nth iteration - if (iterations % _timing_interval == 0) { + // Check for planning timeout and cancel only on every Nth iteration + if (iterations % _terminal_checking_interval == 0) { + if (cancel_checker()) { + throw nav2_core::PlannerCancelled("Planner was cancelled"); + } std::chrono::duration planning_duration = std::chrono::duration_cast>(steady_clock::now() - start_time); if (static_cast(planning_duration.count()) >= _max_planning_time) { @@ -316,7 +388,8 @@ bool AStarAlgorithm::createPath( // 2.1) Use an analytic expansion (if available) to generate a path expansion_result = nullptr; expansion_result = _expander->tryAnalyticExpansion( - current_node, getGoal(), neighborGetter, analytic_iterations, closest_distance); + current_node, getGoals(), + getInitialGoalCoordinate(), neighborGetter, analytic_iterations, closest_distance); if (expansion_result != nullptr) { current_node = expansion_result; } @@ -366,7 +439,7 @@ bool AStarAlgorithm::createPath( template bool AStarAlgorithm::isGoal(NodePtr & node) { - return node == getGoal(); + return _goalsSet.find(node) != _goalsSet.end(); } template @@ -376,9 +449,9 @@ typename AStarAlgorithm::NodePtr & AStarAlgorithm::getStart() } template -typename AStarAlgorithm::NodePtr & AStarAlgorithm::getGoal() +typename AStarAlgorithm::NodeSet & AStarAlgorithm::getGoals() { - return _goal; + return _goalsSet; } template @@ -403,9 +476,9 @@ float AStarAlgorithm::getHeuristicCost(const NodePtr & node) { const Coordinates node_coords = NodeT::getCoords(node->getIndex(), getSizeX(), getSizeDim3()); - float heuristic = NodeT::getHeuristicCost( - node_coords, _goal_coordinates); - + // Heurittic cost for more than one goal is already cotemplated in the + // precomputation stage + float heuristic = NodeT::getHeuristicCost(node_coords, getInitialGoalCoordinate()); if (heuristic < _best_heuristic_node.first) { _best_heuristic_node = {heuristic, node->getIndex()}; } @@ -478,6 +551,13 @@ void AStarAlgorithm::clearStart() _costmap->setCost(coords.x, coords.y, nav2_costmap_2d::FREE_SPACE); } + +template +typename AStarAlgorithm::Coordinates AStarAlgorithm::getInitialGoalCoordinate() +{ + return _goals_coordinates[0]; +} + // Instantiate algorithm for the supported template types template class AStarAlgorithm; template class AStarAlgorithm; diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp index 7c73fb91da7..893f2ceb484 100644 --- a/nav2_smac_planner/src/analytic_expansion.cpp +++ b/nav2_smac_planner/src/analytic_expansion.cpp @@ -48,7 +48,7 @@ void AnalyticExpansion::setCollisionChecker( template typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalyticExpansion( - const NodePtr & current_node, const NodePtr & goal_node, + const NodePtr & current_node, const NodeSet & goals_node, const Coordinates & initial_goal_coords, const NodeGetter & getter, int & analytic_iterations, int & closest_distance) { @@ -60,17 +60,20 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic const Coordinates node_coords = NodeT::getCoords( current_node->getIndex(), _collision_checker->getCostmap()->getSizeInCellsX(), _dim_3_size); + + float current_best_score = std::numeric_limits::max(); + AnalyticExpansionNodes current_best_analytic_nodes = {}; + NodePtr current_best_goal = nullptr; + NodePtr current_best_node = nullptr; closest_distance = std::min( closest_distance, - static_cast(NodeT::getHeuristicCost(node_coords, goal_node->pose))); - + static_cast(NodeT::getHeuristicCost(node_coords, initial_goal_coords))); // We want to expand at a rate of d/expansion_ratio, // but check to see if we are so close that we would be expanding every iteration // If so, limit it to the expansion ratio (rounded up) int desired_iterations = std::max( static_cast(closest_distance / _search_info.analytic_expansion_ratio), static_cast(std::ceil(_search_info.analytic_expansion_ratio))); - // If we are closer now, we should update the target number of iterations to go analytic_iterations = std::min(analytic_iterations, desired_iterations); @@ -78,83 +81,99 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic // Always run the expansion on the first run in case there is a // trivial path to be found if (analytic_iterations <= 0) { - // Reset the counter and try the analytic path expansion - analytic_iterations = desired_iterations; - AnalyticExpansionNodes analytic_nodes = - getAnalyticPath(current_node, goal_node, getter, current_node->motion_table.state_space); - if (!analytic_nodes.empty()) { - // If we have a valid path, attempt to refine it - NodePtr node = current_node; - NodePtr test_node = current_node; - AnalyticExpansionNodes refined_analytic_nodes; - for (int i = 0; i < 8; i++) { - // Attempt to create better paths in 5 node increments, need to make sure - // they exist for each in order to do so (maximum of 40 points back). - if (test_node->parent && test_node->parent->parent && test_node->parent->parent->parent && - test_node->parent->parent->parent->parent && - test_node->parent->parent->parent->parent->parent) - { - test_node = test_node->parent->parent->parent->parent->parent; - refined_analytic_nodes = - getAnalyticPath(test_node, goal_node, getter, test_node->motion_table.state_space); - if (refined_analytic_nodes.empty()) { + for (auto goal_node : goals_node) { + // Reset the counter and try the analytic path expansion + analytic_iterations = desired_iterations; + AnalyticExpansionNodes analytic_nodes = + getAnalyticPath( + current_node, goal_node, getter, + current_node->motion_table.state_space); + if (!analytic_nodes.empty()) { + // If we have a valid path, attempt to refine it + NodePtr node = current_node; + NodePtr test_node = current_node; + AnalyticExpansionNodes refined_analytic_nodes; + for (int i = 0; i < 8; i++) { + // Attempt to create better paths in 5 node increments, need to make sure + // they exist for each in order to do so (maximum of 40 points back). + if (test_node->parent && test_node->parent->parent && + test_node->parent->parent->parent && + test_node->parent->parent->parent->parent && + test_node->parent->parent->parent->parent->parent) + { + test_node = test_node->parent->parent->parent->parent->parent; + // print the goals pose + refined_analytic_nodes = + getAnalyticPath( + test_node, goal_node, getter, + test_node->motion_table.state_space); + if (refined_analytic_nodes.empty()) { + break; + } + analytic_nodes = refined_analytic_nodes; + node = test_node; + } else { break; } - analytic_nodes = refined_analytic_nodes; - node = test_node; - } else { - break; } - } - // The analytic expansion can short-cut near obstacles when closer to a goal - // So, we can attempt to refine it more by increasing the possible radius - // higher than the minimum turning radius and use the best solution based on - // a scoring function similar to that used in traveral cost estimation. - auto scoringFn = [&](const AnalyticExpansionNodes & expansion) { - if (expansion.size() < 2) { - return std::numeric_limits::max(); + // The analytic expansion can short-cut near obstacles when closer to a goal + // So, we can attempt to refine it more by increasing the possible radius + // higher than the minimum turning radius and use the best solution based on + // a scoring function similar to that used in traveral cost estimation. + auto scoringFn = [&](const AnalyticExpansionNodes & expansion) { + if (expansion.size() < 2) { + return std::numeric_limits::max(); + } + + float score = 0.0; + float normalized_cost = 0.0; + // Analytic expansions are consistently spaced + const float distance = hypotf( + expansion[1].proposed_coords.x - expansion[0].proposed_coords.x, + expansion[1].proposed_coords.y - expansion[0].proposed_coords.y); + const float & weight = expansion[0].node->motion_table.cost_penalty; + for (auto iter = expansion.begin(); iter != expansion.end(); ++iter) { + normalized_cost = iter->node->getCost() / 252.0f; + // Search's Traversal Cost Function + score += distance * (1.0 + weight * normalized_cost); + } + return score; + }; + + float best_score = scoringFn(analytic_nodes); + float score = std::numeric_limits::max(); + float min_turn_rad = node->motion_table.min_turning_radius; + const float max_min_turn_rad = 4.0 * min_turn_rad; // Up to 4x the turning radius + while (min_turn_rad < max_min_turn_rad) { + min_turn_rad += 0.5; // In Grid Coords, 1/2 cell steps + ompl::base::StateSpacePtr state_space; + if (node->motion_table.motion_model == MotionModel::DUBIN) { + state_space = std::make_shared(min_turn_rad); + } else { + state_space = std::make_shared(min_turn_rad); } - - float score = 0.0; - float normalized_cost = 0.0; - // Analytic expansions are consistently spaced - const float distance = hypotf( - expansion[1].proposed_coords.x - expansion[0].proposed_coords.x, - expansion[1].proposed_coords.y - expansion[0].proposed_coords.y); - const float & weight = expansion[0].node->motion_table.cost_penalty; - for (auto iter = expansion.begin(); iter != expansion.end(); ++iter) { - normalized_cost = iter->node->getCost() / 252.0f; - // Search's Traversal Cost Function - score += distance * (1.0 + weight * normalized_cost); + refined_analytic_nodes = getAnalyticPath(node, goal_node, getter, state_space); + score = scoringFn(refined_analytic_nodes); + if (score <= best_score) { + analytic_nodes = refined_analytic_nodes; + best_score = score; } - return score; - }; - - float best_score = scoringFn(analytic_nodes); - float score = std::numeric_limits::max(); - float min_turn_rad = node->motion_table.min_turning_radius; - const float max_min_turn_rad = 4.0 * min_turn_rad; // Up to 4x the turning radius - while (min_turn_rad < max_min_turn_rad) { - min_turn_rad += 0.5; // In Grid Coords, 1/2 cell steps - ompl::base::StateSpacePtr state_space; - if (node->motion_table.motion_model == MotionModel::DUBIN) { - state_space = std::make_shared(min_turn_rad); - } else { - state_space = std::make_shared(min_turn_rad); } - refined_analytic_nodes = getAnalyticPath(node, goal_node, getter, state_space); - score = scoringFn(refined_analytic_nodes); - if (score <= best_score) { - analytic_nodes = refined_analytic_nodes; - best_score = score; + if (best_score < current_best_score) { + current_best_score = best_score; + current_best_node = node; + current_best_goal = goal_node; + current_best_analytic_nodes = analytic_nodes; } } - - return setAnalyticPath(node, goal_node, analytic_nodes); } } - + if (!current_best_analytic_nodes.empty()) { + return setAnalyticPath( + current_best_node, current_best_goal, + current_best_analytic_nodes); + } analytic_iterations--; } @@ -179,16 +198,17 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansiondistance(from(), to()); + // A move of sqrt(2) is guaranteed to be in a new cell + static const float sqrt_2 = std::sqrt(2.0f); + // If the length is too far, exit. This prevents unsafe shortcutting of paths // into higher cost areas far out from the goal itself, let search to the work of getting // close before the analytic expansion brings it home. This should never be smaller than // 4-5x the minimum turning radius being used, or planning times will begin to spike. - if (d > _search_info.analytic_expansion_max_length) { + if (d > _search_info.analytic_expansion_max_length || d < sqrt_2) { return AnalyticExpansionNodes(); } - // A move of sqrt(2) is guaranteed to be in a new cell - static const float sqrt_2 = std::sqrt(2.0f); unsigned int num_intervals = static_cast(std::floor(d / sqrt_2)); AnalyticExpansionNodes possible_nodes; @@ -248,7 +268,8 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansion max_cost) { + auto max_cost_it = std::max_element(node_costs.begin(), node_costs.end()); + if (max_cost_it != node_costs.end() && *max_cost_it > max_cost) { // If any element is above the comfortable cost limit, check edge cases: // (1) Check if goal is in greater than max_cost space requiring // entering it, but only entering it on final approach, not in-and-out @@ -354,7 +375,7 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::setAnalyt template<> typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalyticExpansion( - const NodePtr & current_node, const NodePtr & goal_node, + const NodePtr & current_node, const NodeSet & goals_node, const Coordinates & initial_goal_coords, const NodeGetter & getter, int & analytic_iterations, int & closest_distance) { diff --git a/nav2_smac_planner/src/collision_checker.cpp b/nav2_smac_planner/src/collision_checker.cpp index 918a8a9b4eb..2f23442097a 100644 --- a/nav2_smac_planner/src/collision_checker.cpp +++ b/nav2_smac_planner/src/collision_checker.cpp @@ -51,9 +51,9 @@ GridCollisionChecker::GridCollisionChecker( void GridCollisionChecker::setFootprint( const nav2_costmap_2d::Footprint & footprint, const bool & radius, - const double & possible_inscribed_cost) + const double & possible_collision_cost) { - possible_inscribed_cost_ = static_cast(possible_inscribed_cost); + possible_collision_cost_ = static_cast(possible_collision_cost); footprint_is_radius_ = radius; // Use radius, no caching required @@ -113,8 +113,8 @@ bool GridCollisionChecker::inCollision( footprint_cost_ = static_cast(costmap_->getCost( static_cast(x + 0.5), static_cast(y + 0.5))); - if (footprint_cost_ < possible_inscribed_cost_) { - if (possible_inscribed_cost_ > 0) { + if (footprint_cost_ < possible_collision_cost_) { + if (possible_collision_cost_ > 0) { return false; } else { RCLCPP_ERROR_THROTTLE( diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp index 71158f32ff1..5c597c8c754 100644 --- a/nav2_smac_planner/src/node_hybrid.cpp +++ b/nav2_smac_planner/src/node_hybrid.cpp @@ -46,6 +46,7 @@ std::shared_ptr NodeHybrid::inflation_layer = n CostmapDownsampler NodeHybrid::downsampler; ObstacleHeuristicQueue NodeHybrid::obstacle_heuristic_queue; +GoalHeadingMode NodeHybrid::goal_heading_mode; // Each of these tables are the projected motion models through // time and space applied to the search on the current node in @@ -332,7 +333,7 @@ MotionPoses HybridMotionTable::getProjections(const NodeHybrid * node) unsigned int HybridMotionTable::getClosestAngularBin(const double & theta) { - return static_cast(floor(theta / bin_size)); + return static_cast(floor(static_cast(theta) / bin_size)); } float HybridMotionTable::getAngleFromBin(const unsigned int & bin_idx) @@ -440,6 +441,9 @@ float NodeHybrid::getHeuristicCost( { const float obstacle_heuristic = getObstacleHeuristic(node_coords, goal_coords, motion_table.cost_penalty); + if (goal_heading_mode == GoalHeadingMode::ALL_DIRECTION) { + return obstacle_heuristic; + } const float dist_heuristic = getDistanceHeuristic(node_coords, goal_coords, obstacle_heuristic); return std::max(obstacle_heuristic, dist_heuristic); } @@ -780,6 +784,11 @@ void NodeHybrid::precomputeDistanceHeuristic( const unsigned int & dim_3_size, const SearchInfo & search_info) { + if (goal_heading_mode == GoalHeadingMode::ALL_DIRECTION) { + // For all direction goal heading, we only consider the obstacle heuristic + // it does not make sense to precompute the distance heuristic to save computation + return; + } // Dubin or Reeds-Shepp shortest distances if (motion_model == MotionModel::DUBIN) { motion_table.state_space = std::make_shared( @@ -816,6 +825,13 @@ void NodeHybrid::precomputeDistanceHeuristic( from[1] = y; from[2] = heading * angular_bin_size; motion_heuristic = motion_table.state_space->distance(from(), to()); + // we need to check for bidirectional goal heading and take the minimum + if (goal_heading_mode == GoalHeadingMode::BIDIRECTIONAL) { + unsigned int heading_180_offset = (heading + (dim_3_size_int / 2)) % dim_3_size_int; + from[2] = heading_180_offset * angular_bin_size; + float motion_heuristic_180 = motion_table.state_space->distance(from(), to()); + motion_heuristic = std::min(motion_heuristic, motion_heuristic_180); + } dist_heuristic_lookup_table[index] = motion_heuristic; index++; } @@ -883,4 +899,8 @@ bool NodeHybrid::backtracePath(CoordinateVector & path) return true; } +void NodeHybrid::setGoalHeadingMode(const GoalHeadingMode & current_goal_heading_mode) +{ + goal_heading_mode = current_goal_heading_mode; +} } // namespace nav2_smac_planner diff --git a/nav2_smac_planner/src/node_lattice.cpp b/nav2_smac_planner/src/node_lattice.cpp index c3303e0a113..90ef8301464 100644 --- a/nav2_smac_planner/src/node_lattice.cpp +++ b/nav2_smac_planner/src/node_lattice.cpp @@ -38,6 +38,7 @@ namespace nav2_smac_planner LatticeMotionTable NodeLattice::motion_table; float NodeLattice::size_lookup = 25; LookupTable NodeLattice::dist_heuristic_lookup_table; +GoalHeadingMode NodeLattice::goal_heading_mode; // Each of these tables are the projected motion models through // time and space applied to the search on the current node in @@ -332,6 +333,9 @@ float NodeLattice::getHeuristicCost( // get obstacle heuristic value const float obstacle_heuristic = getObstacleHeuristic( node_coords, goal_coords, motion_table.cost_penalty); + if (goal_heading_mode == GoalHeadingMode::ALL_DIRECTION) { + return obstacle_heuristic; + } const float distance_heuristic = getDistanceHeuristic(node_coords, goal_coords, obstacle_heuristic); return std::max(obstacle_heuristic, distance_heuristic); @@ -426,6 +430,11 @@ void NodeLattice::precomputeDistanceHeuristic( const unsigned int & dim_3_size, const SearchInfo & search_info) { + if (goal_heading_mode == GoalHeadingMode::ALL_DIRECTION) { + // For all direction goal heading, we only consider the obstacle heuristic + // it does not make sense to precompute the distance heuristic to save computation + return; + } // Dubin or Reeds-Shepp shortest distances if (!search_info.allow_reverse_expansion) { motion_table.state_space = std::make_shared( @@ -449,7 +458,7 @@ void NodeLattice::precomputeDistanceHeuristic( int dim_3_size_int = static_cast(dim_3_size); // Create a lookup table of Dubin/Reeds-Shepp distances in a window around the goal - // to help drive the search towards admissible approaches. Deu to symmetries in the + // to help drive the search towards admissible approaches. Due to symmetries in the // Heuristic space, we need to only store 2 of the 4 quadrants and simply mirror // around the X axis any relative node lookup. This reduces memory overhead and increases // the size of a window a platform can store in memory. @@ -461,6 +470,13 @@ void NodeLattice::precomputeDistanceHeuristic( from[1] = y; from[2] = motion_table.getAngleFromBin(heading); motion_heuristic = motion_table.state_space->distance(from(), to()); + // we need to check for bidirectional goal heading and take the minimum + if (goal_heading_mode == GoalHeadingMode::BIDIRECTIONAL) { + unsigned int heading_180_offset = (heading + (dim_3_size_int / 2)) % dim_3_size_int; + from[2] = motion_table.getAngleFromBin(heading_180_offset); + float motion_heuristic_180 = motion_table.state_space->distance(from(), to()); + motion_heuristic = std::min(motion_heuristic, motion_heuristic_180); + } dist_heuristic_lookup_table[index] = motion_heuristic; index++; } @@ -593,4 +609,9 @@ void NodeLattice::addNodeToPath( } } +void NodeLattice::setGoalHeadingMode(const GoalHeadingMode & current_goal_heading_mode) +{ + goal_heading_mode = current_goal_heading_mode; +} + } // namespace nav2_smac_planner diff --git a/nav2_smac_planner/src/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp index 70f8852f28c..38fd12b9e17 100644 --- a/nav2_smac_planner/src/smac_planner_2d.cpp +++ b/nav2_smac_planner/src/smac_planner_2d.cpp @@ -83,6 +83,9 @@ void SmacPlanner2D::configure( nav2_util::declare_parameter_if_not_declared( node, name + ".max_on_approach_iterations", rclcpp::ParameterValue(1000)); node->get_parameter(name + ".max_on_approach_iterations", _max_on_approach_iterations); + nav2_util::declare_parameter_if_not_declared( + node, name + ".terminal_checking_interval", rclcpp::ParameterValue(5000)); + node->get_parameter(name + ".terminal_checking_interval", _terminal_checking_interval); nav2_util::declare_parameter_if_not_declared( node, name + ".use_final_approach_orientation", rclcpp::ParameterValue(false)); node->get_parameter(name + ".use_final_approach_orientation", _use_final_approach_orientation); @@ -120,6 +123,7 @@ void SmacPlanner2D::configure( _allow_unknown, _max_iterations, _max_on_approach_iterations, + _terminal_checking_interval, _max_planning_time, 0.0 /*unused for 2D*/, 1.0 /*unused for 2D*/); @@ -192,7 +196,8 @@ void SmacPlanner2D::cleanup() nav_msgs::msg::Path SmacPlanner2D::createPlan( const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal) + const geometry_msgs::msg::PoseStamped & goal, + std::function cancel_checker) { std::lock_guard lock_reinit(_mutex); steady_clock::time_point a = steady_clock::now(); @@ -257,7 +262,7 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( // Note: All exceptions thrown are handled by the planner server and returned to the action if (!_a_star->createPath( path, num_iterations, - _tolerance / static_cast(costmap->getResolution()))) + _tolerance / static_cast(costmap->getResolution()), cancel_checker)) { // Note: If the start is blocked only one iteration will occur before failure if (num_iterations == 1) { @@ -377,6 +382,9 @@ SmacPlanner2D::dynamicParametersCallback(std::vector paramete "disabling tolerance and on approach iterations."); _max_on_approach_iterations = std::numeric_limits::max(); } + } else if (name == _name + ".terminal_checking_interval") { + reinit_a_star = true; + _terminal_checking_interval = parameter.as_int(); } } } @@ -390,6 +398,7 @@ SmacPlanner2D::dynamicParametersCallback(std::vector paramete _allow_unknown, _max_iterations, _max_on_approach_iterations, + _terminal_checking_interval, _max_planning_time, 0.0 /*unused for 2D*/, 1.0 /*unused for 2D*/); diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index 727e4c08b29..16fa22b032c 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -94,6 +94,9 @@ void SmacPlannerHybrid::configure( nav2_util::declare_parameter_if_not_declared( node, name + ".max_on_approach_iterations", rclcpp::ParameterValue(1000)); node->get_parameter(name + ".max_on_approach_iterations", _max_on_approach_iterations); + nav2_util::declare_parameter_if_not_declared( + node, name + ".terminal_checking_interval", rclcpp::ParameterValue(5000)); + node->get_parameter(name + ".terminal_checking_interval", _terminal_checking_interval); nav2_util::declare_parameter_if_not_declared( node, name + ".smooth_path", rclcpp::ParameterValue(true)); node->get_parameter(name + ".smooth_path", smooth_path); @@ -164,6 +167,19 @@ void SmacPlannerHybrid::configure( 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); + std::string goal_heading_type; + nav2_util::declare_parameter_if_not_declared( + node, name + ".goal_heading_mode", rclcpp::ParameterValue("DEFAULT")); + node->get_parameter(name + ".goal_heading_mode", goal_heading_type); + GoalHeadingMode goal_heading_mode = fromStringToGH(goal_heading_type); + if (goal_heading_mode == GoalHeadingMode::UNKNOWN) { + std::string error_msg = "Unable to get GoalHeader type. Given '" + goal_heading_type + "' " + "Valid options are DEFAULT, BIDIRECTIONAL, ALL_DIRECTION. "; + throw nav2_core::PlannerException(error_msg); + } else { + _goal_heading_mode = goal_heading_mode; + } + _motion_model = fromString(_motion_model_for_search); if (_motion_model == MotionModel::UNKNOWN) { RCLCPP_WARN( @@ -228,9 +244,11 @@ void SmacPlannerHybrid::configure( _allow_unknown, _max_iterations, _max_on_approach_iterations, + _terminal_checking_interval, _max_planning_time, _lookup_table_dim, - _angle_quantizations); + _angle_quantizations, + _goal_heading_mode); // Initialize path smoother if (smooth_path) { @@ -318,7 +336,8 @@ void SmacPlannerHybrid::cleanup() nav_msgs::msg::Path SmacPlannerHybrid::createPlan( const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal) + const geometry_msgs::msg::PoseStamped & goal, + std::function cancel_checker) { std::lock_guard lock_reinit(_mutex); steady_clock::time_point a = steady_clock::now(); @@ -394,7 +413,7 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( // Note: All exceptions thrown are handled by the planner server and returned to the action if (!_a_star->createPath( path, num_iterations, - _tolerance / static_cast(costmap->getResolution()), expansions.get())) + _tolerance / static_cast(costmap->getResolution()), cancel_checker, expansions.get())) { if (_debug_visualizations) { geometry_msgs::msg::PoseArray msg; @@ -598,6 +617,9 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para "disabling tolerance and on approach iterations."); _max_on_approach_iterations = std::numeric_limits::max(); } + } else if (name == _name + ".terminal_checking_interval") { + reinit_a_star = true; + _terminal_checking_interval = parameter.as_int(); } else if (name == _name + ".angle_quantization_bins") { reinit_collision_checker = true; reinit_a_star = true; @@ -616,6 +638,23 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para "valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP.", _motion_model_for_search.c_str()); } + } else if (name == _name + ".goal_heading_mode") { + std::string goal_heading_type = parameter.as_string(); + GoalHeadingMode goal_heading_mode = fromStringToGH(goal_heading_type); + RCLCPP_INFO( + _logger, + "GoalHeadingMode type set to '%s'.", + goal_heading_type.c_str()); + if (goal_heading_mode == GoalHeadingMode::UNKNOWN) { + RCLCPP_WARN( + _logger, + "Unable to get GoalHeader type. Given '%s', " + "Valid options are DEFAULT, BIDIRECTIONAL, ALL_DIRECTION. ", + goal_heading_type.c_str()); + } else { + reinit_a_star = true; + _goal_heading_mode = goal_heading_mode; + } } } } @@ -653,9 +692,11 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para _allow_unknown, _max_iterations, _max_on_approach_iterations, + _terminal_checking_interval, _max_planning_time, _lookup_table_dim, - _angle_quantizations); + _angle_quantizations, + _goal_heading_mode); } // Re-Initialize costmap downsampler diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index 1666c7a9ce0..3e1e0962acc 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -77,6 +77,9 @@ void SmacPlannerLattice::configure( nav2_util::declare_parameter_if_not_declared( node, name + ".max_on_approach_iterations", rclcpp::ParameterValue(1000)); node->get_parameter(name + ".max_on_approach_iterations", _max_on_approach_iterations); + nav2_util::declare_parameter_if_not_declared( + node, name + ".terminal_checking_interval", rclcpp::ParameterValue(5000)); + node->get_parameter(name + ".terminal_checking_interval", _terminal_checking_interval); nav2_util::declare_parameter_if_not_declared( node, name + ".smooth_path", rclcpp::ParameterValue(true)); node->get_parameter(name + ".smooth_path", smooth_path); @@ -139,6 +142,20 @@ void SmacPlannerLattice::configure( node, name + ".debug_visualizations", rclcpp::ParameterValue(false)); node->get_parameter(name + ".debug_visualizations", _debug_visualizations); + std::string goal_heading_type; + nav2_util::declare_parameter_if_not_declared( + node, name + ".goal_heading_mode", rclcpp::ParameterValue("DEFAULT")); + node->get_parameter(name + ".goal_heading_mode", goal_heading_type); + GoalHeadingMode goal_heading_mode = fromStringToGH(goal_heading_type); + goal_heading_mode = fromStringToGH(goal_heading_type); + if (goal_heading_mode == GoalHeadingMode::UNKNOWN) { + std::string error_msg = "Unable to get GoalHeader type. Given '" + goal_heading_type + "' " + "Valid options are DEFAULT, BIDIRECTIONAL, ALL_DIRECTION. "; + throw nav2_core::PlannerException(error_msg); + } else { + _goal_heading_mode = goal_heading_mode; + } + _metadata = LatticeMotionTable::getLatticeMetadata(_search_info.lattice_filepath); _search_info.minimum_turning_radius = _metadata.min_turning_radius / (_costmap->getResolution()); @@ -193,9 +210,11 @@ void SmacPlannerLattice::configure( _allow_unknown, _max_iterations, _max_on_approach_iterations, + _terminal_checking_interval, _max_planning_time, lookup_table_dim, - _metadata.number_of_headings); + _metadata.number_of_headings, + _goal_heading_mode); // Initialize path smoother if (smooth_path) { @@ -261,7 +280,8 @@ void SmacPlannerLattice::cleanup() nav_msgs::msg::Path SmacPlannerLattice::createPlan( const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal) + const geometry_msgs::msg::PoseStamped & goal, + std::function cancel_checker) { std::lock_guard lock_reinit(_mutex); steady_clock::time_point a = steady_clock::now(); @@ -316,7 +336,7 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( // Note: All exceptions thrown are handled by the planner server and returned to the action if (!_a_star->createPath( path, num_iterations, - _tolerance / static_cast(_costmap->getResolution()), expansions.get())) + _tolerance / static_cast(_costmap->getResolution()), cancel_checker, expansions.get())) { if (_debug_visualizations) { geometry_msgs::msg::PoseArray msg; @@ -504,15 +524,18 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector par "disabling maximum iterations."); _max_iterations = std::numeric_limits::max(); } - } - } else if (name == _name + ".max_on_approach_iterations") { - reinit_a_star = true; - _max_on_approach_iterations = parameter.as_int(); - if (_max_on_approach_iterations <= 0) { - RCLCPP_INFO( - _logger, "On approach iteration selected as <= 0, " - "disabling tolerance and on approach iterations."); - _max_on_approach_iterations = std::numeric_limits::max(); + } else if (name == _name + ".max_on_approach_iterations") { + reinit_a_star = true; + _max_on_approach_iterations = parameter.as_int(); + if (_max_on_approach_iterations <= 0) { + RCLCPP_INFO( + _logger, "On approach iteration selected as <= 0, " + "disabling tolerance and on approach iterations."); + _max_on_approach_iterations = std::numeric_limits::max(); + } + } else if (name == _name + ".terminal_checking_interval") { + reinit_a_star = true; + _terminal_checking_interval = parameter.as_int(); } } else if (type == ParameterType::PARAMETER_STRING) { if (name == _name + ".lattice_filepath") { @@ -524,6 +547,23 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector par _metadata = LatticeMotionTable::getLatticeMetadata(_search_info.lattice_filepath); _search_info.minimum_turning_radius = _metadata.min_turning_radius / (_costmap->getResolution()); + } else if (name == _name + ".goal_heading_mode") { + std::string goal_heading_type = parameter.as_string(); + GoalHeadingMode goal_heading_mode = fromStringToGH(goal_heading_type); + RCLCPP_INFO( + _logger, + "GoalHeadingMode type set to '%s'.", + goal_heading_type.c_str()); + if (goal_heading_mode == GoalHeadingMode::UNKNOWN) { + RCLCPP_WARN( + _logger, + "Unable to get GoalHeader type. Given '%s', " + "Valid options are DEFAULT, BIDIRECTIONAL, ALL_DIRECTION. ", + goal_heading_type.c_str()); + } else { + reinit_a_star = true; + _goal_heading_mode = goal_heading_mode; + } } } } @@ -565,9 +605,11 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector par _allow_unknown, _max_iterations, _max_on_approach_iterations, + _terminal_checking_interval, _max_planning_time, lookup_table_dim, - _metadata.number_of_headings); + _metadata.number_of_headings, + _goal_heading_mode); } } diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp index 13c2c802982..ead3b7e6854 100644 --- a/nav2_smac_planner/test/test_a_star.cpp +++ b/nav2_smac_planner/test/test_a_star.cpp @@ -47,10 +47,13 @@ TEST(AStarTest, test_a_star_2d) float tolerance = 0.0; float some_tolerance = 20.0; int it_on_approach = 10; + int terminal_checking_interval = 5000; double max_planning_time = 120.0; int num_it = 0; - a_star.initialize(false, max_iterations, it_on_approach, max_planning_time, 0.0, 1); + a_star.initialize( + false, max_iterations, it_on_approach, terminal_checking_interval, + max_planning_time, 0.0, 1); nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0); @@ -67,6 +70,10 @@ TEST(AStarTest, test_a_star_2d) auto costmap = costmap_ros->getCostmap(); *costmap = *costmapA; + auto dummy_cancel_checker = []() { + return false; + }; + // functional case testing std::unique_ptr checker = std::make_unique(costmap_ros, 1, lnode); @@ -75,7 +82,7 @@ TEST(AStarTest, test_a_star_2d) a_star.setStart(20u, 20u, 0); a_star.setGoal(80u, 80u, 0); nav2_smac_planner::Node2D::CoordinateVector path; - EXPECT_TRUE(a_star.createPath(path, num_it, tolerance)); + EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, dummy_cancel_checker)); EXPECT_EQ(num_it, 2414); // check path is the right size and collision free @@ -92,28 +99,40 @@ TEST(AStarTest, test_a_star_2d) // failure cases with invalid inputs nav2_smac_planner::AStarAlgorithm a_star_2( nav2_smac_planner::MotionModel::TWOD, info); - a_star_2.initialize(false, max_iterations, it_on_approach, max_planning_time, 0, 1); + a_star_2.initialize( + false, max_iterations, it_on_approach, terminal_checking_interval, + max_planning_time, 0, 1); num_it = 0; - EXPECT_THROW(a_star_2.createPath(path, num_it, tolerance), std::runtime_error); + EXPECT_THROW( + a_star_2.createPath( + path, num_it, tolerance, + dummy_cancel_checker), std::runtime_error); a_star_2.setCollisionChecker(checker.get()); num_it = 0; - EXPECT_THROW(a_star_2.createPath(path, num_it, tolerance), std::runtime_error); + EXPECT_THROW( + a_star_2.createPath( + path, num_it, tolerance, + dummy_cancel_checker), std::runtime_error); a_star_2.setStart(0, 0, 0); // valid a_star_2.setGoal(50, 50, 0); // invalid num_it = 0; - EXPECT_THROW(a_star_2.createPath(path, num_it, tolerance), std::runtime_error); + EXPECT_THROW( + a_star_2.createPath( + path, num_it, tolerance, + dummy_cancel_checker), std::runtime_error); num_it = 0; // invalid goal but liberal tolerance a_star_2.setStart(20, 20, 0); // valid a_star_2.setGoal(50, 50, 0); // invalid - EXPECT_TRUE(a_star_2.createPath(path, num_it, some_tolerance)); + EXPECT_TRUE(a_star_2.createPath(path, num_it, some_tolerance, dummy_cancel_checker)); EXPECT_EQ(path.size(), 21u); for (unsigned int i = 0; i != path.size(); i++) { EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0); } EXPECT_TRUE(a_star_2.getStart() != nullptr); - EXPECT_TRUE(a_star_2.getGoal() != nullptr); + EXPECT_TRUE(!a_star_2.getGoals().empty()); + EXPECT_TRUE(a_star_2.getGoals().size() == 1); EXPECT_EQ(a_star_2.getSizeX(), 100u); EXPECT_EQ(a_star_2.getSizeY(), 100u); EXPECT_EQ(a_star_2.getSizeDim3(), 1u); @@ -141,10 +160,13 @@ TEST(AStarTest, test_a_star_se2) int max_iterations = 10000; float tolerance = 10.0; int it_on_approach = 10; + int terminal_checking_interval = 5000; double max_planning_time = 120.0; int num_it = 0; - a_star.initialize(false, max_iterations, it_on_approach, max_planning_time, 401, size_theta); + a_star.initialize( + false, max_iterations, it_on_approach, terminal_checking_interval, + max_planning_time, 401, size_theta); nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0); @@ -173,7 +195,11 @@ TEST(AStarTest, test_a_star_se2) std::unique_ptr>> expansions = nullptr; expansions = std::make_unique>>(); - EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, expansions.get())); + auto dummy_cancel_checker = []() { + return false; + }; + + EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, dummy_cancel_checker, expansions.get())); // check path is the right size and collision free EXPECT_EQ(num_it, 3146); @@ -214,11 +240,14 @@ TEST(AStarTest, test_a_star_lattice) int max_iterations = 10000; float tolerance = 10.0; int it_on_approach = 10; + int terminal_checking_interval = 5000; double max_planning_time = 120.0; int num_it = 0; a_star.initialize( - false, max_iterations, std::numeric_limits::max(), max_planning_time, 401, size_theta); + false, max_iterations, + std::numeric_limits::max(), terminal_checking_interval, max_planning_time, 401, + size_theta); nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D(100, 100, 0.05, 0.0, 0.0, 0); @@ -239,12 +268,16 @@ TEST(AStarTest, test_a_star_lattice) std::make_unique(costmap_ros, size_theta, lnode); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); + auto dummy_cancel_checker = []() { + return false; + }; + // functional case testing a_star.setCollisionChecker(checker.get()); a_star.setStart(5u, 5u, 0u); a_star.setGoal(40u, 40u, 1u); nav2_smac_planner::NodeLattice::CoordinateVector path; - EXPECT_TRUE(a_star.createPath(path, num_it, tolerance)); + EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, dummy_cancel_checker)); // check path is the right size and collision free EXPECT_EQ(num_it, 22); @@ -278,10 +311,13 @@ TEST(AStarTest, test_se2_single_pose_path) int max_iterations = 100; float tolerance = 10.0; int it_on_approach = 10; + int terminal_checking_interval = 5000; double max_planning_time = 120.0; int num_it = 0; - a_star.initialize(false, max_iterations, it_on_approach, max_planning_time, 401, size_theta); + a_star.initialize( + false, max_iterations, it_on_approach, terminal_checking_interval, + max_planning_time, 401, size_theta); nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0); @@ -296,13 +332,16 @@ TEST(AStarTest, test_se2_single_pose_path) std::make_unique(costmap_ros, size_theta, lnode); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); + auto dummy_cancel_checker = []() { + return false; + }; // functional case testing a_star.setCollisionChecker(checker.get()); a_star.setStart(10u, 10u, 0u); // Goal is one costmap cell away a_star.setGoal(12u, 10u, 0u); nav2_smac_planner::NodeHybrid::CoordinateVector path; - EXPECT_TRUE(a_star.createPath(path, num_it, tolerance)); + EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, dummy_cancel_checker)); // Check that the path is length one // With the current implementation, this produces a longer path @@ -312,6 +351,80 @@ TEST(AStarTest, test_se2_single_pose_path) delete costmapA; } +TEST(AStarTest, test_goal_heading_mode) +{ + auto lnode = std::make_shared("test"); + nav2_smac_planner::SearchInfo info; + info.change_penalty = 0.1; + info.non_straight_penalty = 1.1; + info.reverse_penalty = 2.0; + info.minimum_turning_radius = 8; // in grid coordinates + info.retrospective_penalty = 0.015; + info.analytic_expansion_max_length = 20.0; // in grid coordinates + info.analytic_expansion_ratio = 3.5; + unsigned int size_theta = 72; + info.cost_penalty = 1.7; + nav2_smac_planner::AStarAlgorithm a_star( + nav2_smac_planner::MotionModel::DUBIN, info); + int max_iterations = 10000; + float tolerance = 10.0; + int it_on_approach = 10; + int terminal_checking_interval = 5000; + double max_planning_time = 120.0; + int num_it = 0; + + // BIDIRECTIONAL goal heading mode + a_star.initialize( + false, max_iterations, it_on_approach, terminal_checking_interval, + max_planning_time, 401, size_theta, nav2_smac_planner::GoalHeadingMode::BIDIRECTIONAL); + + nav2_costmap_2d::Costmap2D * costmapA = + new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0); + + // Convert raw costmap into a costmap ros object + auto costmap_ros = std::make_shared(); + costmap_ros->on_configure(rclcpp_lifecycle::State()); + auto costmap = costmap_ros->getCostmap(); + *costmap = *costmapA; + + std::unique_ptr checker = + std::make_unique(costmap_ros, size_theta, lnode); + checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); + + a_star.setCollisionChecker(checker.get()); + a_star.setStart(10u, 10u, 0u); + a_star.setGoal(80u, 80u, 40u); + nav2_smac_planner::NodeHybrid::CoordinateVector path; + std::unique_ptr>> expansions = nullptr; + expansions = std::make_unique>>(); + + auto dummy_cancel_checker = []() { + return false; + }; + + EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, dummy_cancel_checker, expansions.get())); + + EXPECT_TRUE(a_star.getGoals().size() == 2); + EXPECT_TRUE(a_star.getInitialGoalCoordinate().theta == 40u); + + + // ALL_DIRECTION goal heading mode + a_star.initialize( + false, max_iterations, it_on_approach, terminal_checking_interval, + max_planning_time, 401, size_theta, nav2_smac_planner::GoalHeadingMode::ALL_DIRECTION); + + a_star.setCollisionChecker(checker.get()); + a_star.setStart(10u, 10u, 0u); + a_star.setGoal(80u, 80u, 40u); + + unsigned int num_bins = nav2_smac_planner::NodeHybrid::motion_table.num_angle_quantization; + EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, dummy_cancel_checker, expansions.get())); + EXPECT_TRUE(a_star.getGoals().size() == num_bins); + // first goal should be the same the one set by the user + EXPECT_TRUE(a_star.getInitialGoalCoordinate().theta == 40u); + +} + TEST(AStarTest, test_constants) { nav2_smac_planner::MotionModel mm = nav2_smac_planner::MotionModel::UNKNOWN; // unknown @@ -323,6 +436,15 @@ TEST(AStarTest, test_constants) mm = nav2_smac_planner::MotionModel::REEDS_SHEPP; // reeds-shepp EXPECT_EQ(nav2_smac_planner::toString(mm), std::string("Reeds-Shepp")); + nav2_smac_planner::GoalHeadingMode gh = nav2_smac_planner::GoalHeadingMode::UNKNOWN; + EXPECT_EQ(nav2_smac_planner::toString(gh), std::string("Unknown")); + gh = nav2_smac_planner::GoalHeadingMode::DEFAULT; // default + EXPECT_EQ(nav2_smac_planner::toString(gh), std::string("DEFAULT")); + gh = nav2_smac_planner::GoalHeadingMode::BIDIRECTIONAL; // bidirectional + EXPECT_EQ(nav2_smac_planner::toString(gh), std::string("BIDIRECTIONAL")); + gh = nav2_smac_planner::GoalHeadingMode::ALL_DIRECTION; // all_direction + EXPECT_EQ(nav2_smac_planner::toString(gh), std::string("ALL_DIRECTION")); + EXPECT_EQ( nav2_smac_planner::fromString( "2D"), nav2_smac_planner::MotionModel::TWOD); @@ -331,4 +453,16 @@ TEST(AStarTest, test_constants) nav2_smac_planner::fromString( "REEDS_SHEPP"), nav2_smac_planner::MotionModel::REEDS_SHEPP); EXPECT_EQ(nav2_smac_planner::fromString("NONE"), nav2_smac_planner::MotionModel::UNKNOWN); + + EXPECT_EQ( + nav2_smac_planner::fromStringToGH( + "DEFAULT"), nav2_smac_planner::GoalHeadingMode::DEFAULT); + EXPECT_EQ( + nav2_smac_planner::fromStringToGH( + "BIDIRECTIONAL"), nav2_smac_planner::GoalHeadingMode::BIDIRECTIONAL); + EXPECT_EQ( + nav2_smac_planner::fromStringToGH( + "ALL_DIRECTION"), nav2_smac_planner::GoalHeadingMode::ALL_DIRECTION); + EXPECT_EQ( + nav2_smac_planner::fromStringToGH("NONE"), nav2_smac_planner::GoalHeadingMode::UNKNOWN); } diff --git a/nav2_smac_planner/test/test_nodehybrid.cpp b/nav2_smac_planner/test/test_nodehybrid.cpp index 7ed61c4ec44..1c127c47990 100644 --- a/nav2_smac_planner/test/test_nodehybrid.cpp +++ b/nav2_smac_planner/test/test_nodehybrid.cpp @@ -374,3 +374,33 @@ TEST(NodeHybridTest, test_node_reeds_neighbors) // should be empty since totally invalid EXPECT_EQ(neighbors.size(), 0u); } + +TEST(NodeHybridTest, basic_get_closest_angular_bin_test) +{ + // Tests to check getClosestAngularBin behavior for different input types + nav2_smac_planner::HybridMotionTable motion_table; + + { + motion_table.bin_size = 3.1415926; + double test_theta = 3.1415926; + unsigned int expected_angular_bin = 1; + unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta); + EXPECT_EQ(expected_angular_bin, calculated_angular_bin); + } + + { + motion_table.bin_size = M_PI; + double test_theta = M_PI; + unsigned int expected_angular_bin = 1; + unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta); + EXPECT_EQ(expected_angular_bin, calculated_angular_bin); + } + + { + motion_table.bin_size = M_PI; + float test_theta = M_PI; + unsigned int expected_angular_bin = 1; + unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta); + EXPECT_EQ(expected_angular_bin, calculated_angular_bin); + } +} diff --git a/nav2_smac_planner/test/test_smac_2d.cpp b/nav2_smac_planner/test/test_smac_2d.cpp index 1ebe215b30f..e2dff5a56f4 100644 --- a/nav2_smac_planner/test/test_smac_2d.cpp +++ b/nav2_smac_planner/test/test_smac_2d.cpp @@ -57,6 +57,10 @@ TEST(SmacTest, test_smac_2d) { node2D->declare_parameter("test.downsampling_factor", 2); node2D->set_parameter(rclcpp::Parameter("test.downsampling_factor", 2)); + auto dummy_cancel_checker = []() { + return false; + }; + geometry_msgs::msg::PoseStamped start, goal; start.pose.position.x = 0.0; start.pose.position.y = 0.0; @@ -69,7 +73,7 @@ TEST(SmacTest, test_smac_2d) { planner_2d->configure(node2D, "test", nullptr, costmap_ros); planner_2d->activate(); try { - planner_2d->createPlan(start, goal); + planner_2d->createPlan(start, goal, dummy_cancel_checker); } catch (...) { } @@ -108,6 +112,7 @@ TEST(SmacTest, test_smac_2d_reconfigure) { rclcpp::Parameter("test.downsampling_factor", 2), rclcpp::Parameter("test.max_iterations", -1), rclcpp::Parameter("test.max_on_approach_iterations", -1), + rclcpp::Parameter("test.terminal_checking_interval", 100), rclcpp::Parameter("test.use_final_approach_orientation", false)}); rclcpp::spin_until_future_complete( @@ -127,6 +132,9 @@ TEST(SmacTest, test_smac_2d_reconfigure) { EXPECT_EQ( node2D->get_parameter("test.max_on_approach_iterations").as_int(), -1); + EXPECT_EQ( + node2D->get_parameter("test.terminal_checking_interval").as_int(), + 100); results = rec_param->set_parameters_atomically( {rclcpp::Parameter("test.downsample_costmap", true)}); diff --git a/nav2_smac_planner/test/test_smac_hybrid.cpp b/nav2_smac_planner/test/test_smac_hybrid.cpp index aae4666edc5..db24daa6610 100644 --- a/nav2_smac_planner/test/test_smac_hybrid.cpp +++ b/nav2_smac_planner/test/test_smac_hybrid.cpp @@ -56,6 +56,10 @@ TEST(SmacTest, test_smac_se2) nodeSE2->declare_parameter("test.downsampling_factor", 2); nodeSE2->set_parameter(rclcpp::Parameter("test.downsampling_factor", 2)); + auto dummy_cancel_checker = []() { + return false; + }; + geometry_msgs::msg::PoseStamped start, goal; start.pose.position.x = 0.0; start.pose.position.y = 0.0; @@ -68,7 +72,7 @@ TEST(SmacTest, test_smac_se2) planner->activate(); try { - planner->createPlan(start, goal); + planner->createPlan(start, goal, dummy_cancel_checker); } catch (...) { } @@ -119,7 +123,9 @@ TEST(SmacTest, test_smac_se2_reconfigure) rclcpp::Parameter("test.smooth_path", false), rclcpp::Parameter("test.analytic_expansion_max_length", 42.0), rclcpp::Parameter("test.max_on_approach_iterations", 42), - rclcpp::Parameter("test.motion_model_for_search", std::string("REEDS_SHEPP"))}); + rclcpp::Parameter("test.terminal_checking_interval", 42), + rclcpp::Parameter("test.motion_model_for_search", std::string("REEDS_SHEPP")), + rclcpp::Parameter("test.goal_heading_mode", std::string("BIDIRECTIONAL"))}); rclcpp::spin_until_future_complete( nodeSE2->get_node_base_interface(), @@ -144,7 +150,11 @@ TEST(SmacTest, test_smac_se2_reconfigure) EXPECT_EQ(nodeSE2->get_parameter("test.lookup_table_size").as_double(), 30.0); EXPECT_EQ(nodeSE2->get_parameter("test.analytic_expansion_max_length").as_double(), 42.0); EXPECT_EQ(nodeSE2->get_parameter("test.max_on_approach_iterations").as_int(), 42); + EXPECT_EQ(nodeSE2->get_parameter("test.terminal_checking_interval").as_int(), 42); EXPECT_EQ( nodeSE2->get_parameter("test.motion_model_for_search").as_string(), std::string("REEDS_SHEPP")); + EXPECT_EQ( + nodeSE2->get_parameter("test.goal_heading_mode").as_string(), + std::string("BIDIRECTIONAL")); } diff --git a/nav2_smac_planner/test/test_smac_lattice.cpp b/nav2_smac_planner/test/test_smac_lattice.cpp index b791f9961c2..128db09fdc8 100644 --- a/nav2_smac_planner/test/test_smac_lattice.cpp +++ b/nav2_smac_planner/test/test_smac_lattice.cpp @@ -60,6 +60,10 @@ TEST(SmacTest, test_smac_lattice) std::make_shared("global_costmap"); costmap_ros->on_configure(rclcpp_lifecycle::State()); + auto dummy_cancel_checker = []() { + return false; + }; + geometry_msgs::msg::PoseStamped start, goal; start.pose.position.x = 0.0; start.pose.position.y = 0.0; @@ -76,7 +80,7 @@ TEST(SmacTest, test_smac_lattice) planner->activate(); try { - planner->createPlan(start, goal); + planner->createPlan(start, goal, dummy_cancel_checker); } catch (...) { } @@ -128,7 +132,9 @@ TEST(SmacTest, test_smac_lattice_reconfigure) rclcpp::Parameter("test.tolerance", 42.0), rclcpp::Parameter("test.rotation_penalty", 42.0), rclcpp::Parameter("test.max_on_approach_iterations", 42), - rclcpp::Parameter("test.allow_reverse_expansion", true)}); + rclcpp::Parameter("test.terminal_checking_interval", 42), + rclcpp::Parameter("test.allow_reverse_expansion", true), + rclcpp::Parameter("test.goal_heading_mode", std::string("BIDIRECTIONAL"))}); try { // All of these params will re-init A* which will involve loading the control set file diff --git a/nav2_smac_planner/test/test_smoother.cpp b/nav2_smac_planner/test/test_smoother.cpp index 38449234980..616c674595a 100644 --- a/nav2_smac_planner/test/test_smoother.cpp +++ b/nav2_smac_planner/test/test_smoother.cpp @@ -89,11 +89,14 @@ TEST(SmootherTest, test_full_smoother) int max_iterations = 10000; float tolerance = 10.0; int it_on_approach = 10; + int terminal_checking_interval = 5000; double max_planning_time = 120.0; int num_it = 0; a_star.initialize( - false, max_iterations, std::numeric_limits::max(), max_planning_time, 401, size_theta); + false, max_iterations, + std::numeric_limits::max(), terminal_checking_interval, max_planning_time, 401, + size_theta); // Convert raw costmap into a costmap ros object auto costmap_ros = std::make_shared(); @@ -105,12 +108,16 @@ TEST(SmootherTest, test_full_smoother) std::make_unique(costmap_ros, size_theta, node); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); + auto dummy_cancel_checker = []() { + return false; + }; + // Create A* search to smooth a_star.setCollisionChecker(checker.get()); a_star.setStart(5u, 5u, 0u); a_star.setGoal(45u, 45u, 36u); nav2_smac_planner::NodeHybrid::CoordinateVector path; - EXPECT_TRUE(a_star.createPath(path, num_it, tolerance)); + EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, dummy_cancel_checker)); // Convert to world coordinates and get length to compare to smoothed length nav_msgs::msg::Path plan; diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 2674b65e742..bfab0a79fed 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -22,7 +22,7 @@ find_package(nav2_navfn_planner REQUIRED) find_package(nav2_planner REQUIRED) find_package(navigation2) find_package(angles REQUIRED) -find_package(behaviortree_cpp_v3 REQUIRED) +find_package(behaviortree_cpp REQUIRED) find_package(pluginlib REQUIRED) nav2_package() @@ -45,7 +45,7 @@ set(dependencies nav2_planner nav2_navfn_planner angles - behaviortree_cpp_v3 + behaviortree_cpp pluginlib ) diff --git a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp index 51558d9ba16..0a4b320d8f4 100644 --- a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp +++ b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp @@ -17,13 +17,14 @@ #include #include #include +#include #include #include "gtest/gtest.h" -#include "behaviortree_cpp_v3/behavior_tree.h" -#include "behaviortree_cpp_v3/bt_factory.h" -#include "behaviortree_cpp_v3/utils/shared_library.h" +#include "behaviortree_cpp/behavior_tree.h" +#include "behaviortree_cpp/bt_factory.h" +#include "behaviortree_cpp/utils/shared_library.h" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" @@ -31,6 +32,8 @@ #include "nav2_util/odometry_utils.hpp" +#include "nav2_behavior_tree/plugins_list.hpp" + #include "rclcpp/rclcpp.hpp" #include "ament_index_cpp/get_package_share_directory.hpp" @@ -58,58 +61,9 @@ class BehaviorTreeHandler odom_smoother_ = std::make_shared(node_); - const std::vector plugin_libs = { - "nav2_compute_path_to_pose_action_bt_node", - "nav2_compute_path_through_poses_action_bt_node", - "nav2_smooth_path_action_bt_node", - "nav2_follow_path_action_bt_node", - "nav2_spin_action_bt_node", - "nav2_wait_action_bt_node", - "nav2_assisted_teleop_action_bt_node", - "nav2_back_up_action_bt_node", - "nav2_drive_on_heading_bt_node", - "nav2_clear_costmap_service_bt_node", - "nav2_is_stuck_condition_bt_node", - "nav2_goal_reached_condition_bt_node", - "nav2_initial_pose_received_condition_bt_node", - "nav2_goal_updated_condition_bt_node", - "nav2_globally_updated_goal_condition_bt_node", - "nav2_is_path_valid_condition_bt_node", - "nav2_are_error_codes_active_condition_bt_node", - "nav2_would_a_controller_recovery_help_condition_bt_node", - "nav2_would_a_planner_recovery_help_condition_bt_node", - "nav2_would_a_smoother_recovery_help_condition_bt_node", - "nav2_reinitialize_global_localization_service_bt_node", - "nav2_rate_controller_bt_node", - "nav2_distance_controller_bt_node", - "nav2_speed_controller_bt_node", - "nav2_truncate_path_action_bt_node", - "nav2_truncate_path_local_action_bt_node", - "nav2_goal_updater_node_bt_node", - "nav2_recovery_node_bt_node", - "nav2_pipeline_sequence_bt_node", - "nav2_round_robin_node_bt_node", - "nav2_transform_available_condition_bt_node", - "nav2_time_expired_condition_bt_node", - "nav2_path_expiring_timer_condition", - "nav2_distance_traveled_condition_bt_node", - "nav2_single_trigger_bt_node", - "nav2_is_battery_low_condition_bt_node", - "nav2_navigate_through_poses_action_bt_node", - "nav2_navigate_to_pose_action_bt_node", - "nav2_remove_passed_goals_action_bt_node", - "nav2_planner_selector_bt_node", - "nav2_controller_selector_bt_node", - "nav2_goal_checker_selector_bt_node", - "nav2_controller_cancel_bt_node", - "nav2_path_longer_on_approach_bt_node", - "nav2_assisted_teleop_cancel_bt_node", - "nav2_wait_cancel_bt_node", - "nav2_spin_cancel_bt_node", - "nav2_back_up_cancel_bt_node", - "nav2_drive_on_heading_cancel_bt_node", - "nav2_goal_updated_controller_bt_node" - }; + std::vector plugin_libs; + boost::split(plugin_libs, nav2::details::BT_BUILTIN_PLUGINS, boost::is_any_of(";")); + for (const auto & p : plugin_libs) { factory_.registerFromPlugin(BT::SharedLibrary::getOSName(p)); } @@ -270,7 +224,7 @@ TEST_F(BehaviorTreeTestFixture, TestAllSuccess) BT::NodeStatus result = BT::NodeStatus::RUNNING; while (result == BT::NodeStatus::RUNNING) { - result = bt_handler->tree.tickRoot(); + result = bt_handler->tree.tickOnce(); std::this_thread::sleep_for(10ms); } @@ -321,7 +275,7 @@ TEST_F(BehaviorTreeTestFixture, TestAllFailure) BT::NodeStatus result = BT::NodeStatus::RUNNING; while (result == BT::NodeStatus::RUNNING) { - result = bt_handler->tree.tickRoot(); + result = bt_handler->tree.tickOnce(); std::this_thread::sleep_for(10ms); } @@ -370,7 +324,7 @@ TEST_F(BehaviorTreeTestFixture, TestNavigateSubtreeRecoveries) BT::NodeStatus result = BT::NodeStatus::RUNNING; while (result == BT::NodeStatus::RUNNING) { - result = bt_handler->tree.tickRoot(); + result = bt_handler->tree.tickOnce(); std::this_thread::sleep_for(10ms); } @@ -428,7 +382,7 @@ TEST_F(BehaviorTreeTestFixture, TestNavigateRecoverySimple) BT::NodeStatus result = BT::NodeStatus::RUNNING; while (result == BT::NodeStatus::RUNNING) { - result = bt_handler->tree.tickRoot(); + result = bt_handler->tree.tickOnce(); std::this_thread::sleep_for(10ms); } @@ -522,7 +476,7 @@ TEST_F(BehaviorTreeTestFixture, TestNavigateRecoveryComplex) BT::NodeStatus result = BT::NodeStatus::RUNNING; while (result == BT::NodeStatus::RUNNING) { - result = bt_handler->tree.tickRoot(); + result = bt_handler->tree.tickOnce(); std::this_thread::sleep_for(10ms); } @@ -586,7 +540,7 @@ TEST_F(BehaviorTreeTestFixture, TestRecoverySubtreeGoalUpdated) BT::NodeStatus result = BT::NodeStatus::RUNNING; while (result == BT::NodeStatus::RUNNING) { - result = bt_handler->tree.tickRoot(); + result = bt_handler->tree.tickOnce(); // Update goal on blackboard after Spin has been triggered once // to simulate a goal update during a recovery action diff --git a/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.cpp b/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.cpp index 706767b9ced..1e6d57e0f41 100644 --- a/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.cpp +++ b/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.cpp @@ -136,6 +136,7 @@ bool SpinBehaviorTester::defaultSpinBehaviorTest( std::this_thread::sleep_for(5s); if (make_fake_costmap_) { + sendFakeCostmap(target_yaw); sendFakeOdom(0.0); } @@ -211,7 +212,7 @@ bool SpinBehaviorTester::defaultSpinBehaviorTest( { sendFakeOdom(command_yaw); sendFakeCostmap(target_yaw); - rclcpp::sleep_for(std::chrono::milliseconds(1)); + rclcpp::sleep_for(std::chrono::milliseconds(3)); } sendFakeOdom(target_yaw); sendFakeCostmap(target_yaw); @@ -277,7 +278,7 @@ void SpinBehaviorTester::sendFakeCostmap(float angle) nav2_msgs::msg::Costmap fake_costmap; fake_costmap.header.frame_id = "odom"; - fake_costmap.header.stamp = stamp_; + fake_costmap.header.stamp = node_->now(); fake_costmap.metadata.layer = "master"; fake_costmap.metadata.resolution = .1; fake_costmap.metadata.size_x = 100; @@ -288,9 +289,9 @@ void SpinBehaviorTester::sendFakeCostmap(float angle) float costmap_val = 0; for (int ix = 0; ix < 100; ix++) { for (int iy = 0; iy < 100; iy++) { - if (abs(angle) > M_PI_2f32) { + if (fabs(angle) > M_PI_2f32) { // fake obstacles in the way so we get failure due to potential collision - costmap_val = 100; + costmap_val = 253; } fake_costmap.data.push_back(costmap_val); } @@ -302,7 +303,7 @@ void SpinBehaviorTester::sendInitialPose() { geometry_msgs::msg::PoseWithCovarianceStamped pose; pose.header.frame_id = "map"; - pose.header.stamp = stamp_; + pose.header.stamp = node_->now(); pose.pose.pose.position.x = -2.0; pose.pose.pose.position.y = -0.5; pose.pose.pose.position.z = 0.0; @@ -325,7 +326,7 @@ void SpinBehaviorTester::sendFakeOdom(float angle) { geometry_msgs::msg::TransformStamped transformStamped; - transformStamped.header.stamp = stamp_; + transformStamped.header.stamp = node_->now(); transformStamped.header.frame_id = "odom"; transformStamped.child_frame_id = "base_link"; transformStamped.transform.translation.x = 0.0; @@ -342,7 +343,7 @@ void SpinBehaviorTester::sendFakeOdom(float angle) geometry_msgs::msg::PolygonStamped footprint; footprint.header.frame_id = "odom"; - footprint.header.stamp = stamp_; + footprint.header.stamp = node_->now(); footprint.polygon.points.resize(4); footprint.polygon.points[0].x = 0.22; footprint.polygon.points[0].y = 0.22; diff --git a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py index c9df3d32432..8edbcf8fe80 100755 --- a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py @@ -60,6 +60,8 @@ def generate_launch_description(): 'gzserver', '-s', 'libgazebo_ros_init.so', + '-s', + 'libgazebo_ros_factory.so', '--minimal_comms', world, ], diff --git a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_node.cpp b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_node.cpp index f16db47851a..6838e012219 100644 --- a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_node.cpp +++ b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_node.cpp @@ -118,12 +118,12 @@ INSTANTIATE_TEST_SUITE_P( SpinRecoveryTests, SpinBehaviorTestFixture, ::testing::Values( - std::make_tuple(-M_PIf32 / 6.0, 0.15), - std::make_tuple(M_PI_4f32, 0.15), - std::make_tuple(-M_PI_2f32, 0.15), - std::make_tuple(M_PIf32, 0.10), + std::make_tuple(-M_PIf32 / 6.0, 0.1), + // std::make_tuple(M_PI_4f32, 0.1), + // std::make_tuple(-M_PI_2f32, 0.1), + std::make_tuple(M_PIf32, 0.1), std::make_tuple(3.0 * M_PIf32 / 2.0, 0.15), - std::make_tuple(-2.0 * M_PIf32, 0.15), + std::make_tuple(-2.0 * M_PIf32, 0.1), std::make_tuple(4.0 * M_PIf32, 0.15)), testNameGenerator); diff --git a/nav2_system_tests/src/costmap_filters/keepout_params.yaml b/nav2_system_tests/src/costmap_filters/keepout_params.yaml index 5c948331f00..9f9bdd33dea 100644 --- a/nav2_system_tests/src/costmap_filters/keepout_params.yaml +++ b/nav2_system_tests/src/costmap_filters/keepout_params.yaml @@ -47,56 +47,10 @@ bt_navigator: # files to allow for a commandline change default used is the # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml & # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml - plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_smooth_path_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_assisted_teleop_action_bt_node - - nav2_back_up_action_bt_node - - nav2_drive_on_heading_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_are_error_codes_active_condition_bt_node - - nav2_would_a_controller_recovery_help_condition_bt_node - - nav2_would_a_planner_recovery_help_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_is_path_valid_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_truncate_path_local_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_goal_updated_controller_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_controller_cancel_bt_node - - nav2_path_longer_on_approach_bt_node - - nav2_wait_cancel_bt_node - - nav2_spin_cancel_bt_node - - nav2_back_up_cancel_bt_node - - nav2_assisted_teleop_cancel_bt_node - - nav2_drive_on_heading_cancel_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - - nav2_planner_selector_bt_node - - nav2_smoother_selector_bt_node - - nav2_progress_checker_selector_bt_node + + # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). + # Built-in plugins are added automatically + # plugin_lib_names: [] controller_server: ros__parameters: diff --git a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml index c887ac3ce57..10040d87983 100644 --- a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml @@ -47,57 +47,10 @@ bt_navigator: # files to allow for a commandline change default used is the # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml & # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml - plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_smooth_path_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_assisted_teleop_action_bt_node - - nav2_back_up_action_bt_node - - nav2_drive_on_heading_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_are_error_codes_active_condition_bt_node - - nav2_would_a_controller_recovery_help_condition_bt_node - - nav2_would_a_planner_recovery_help_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_globally_updated_goal_condition_bt_node - - nav2_is_path_valid_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_truncate_path_local_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_goal_updated_controller_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_controller_cancel_bt_node - - nav2_path_longer_on_approach_bt_node - - nav2_wait_cancel_bt_node - - nav2_spin_cancel_bt_node - - nav2_back_up_cancel_bt_node - - nav2_assisted_teleop_cancel_bt_node - - nav2_drive_on_heading_cancel_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - - nav2_planner_selector_bt_node - - nav2_smoother_selector_bt_node - - nav2_progress_checker_selector_bt_node + + # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). + # Built-in plugins are added automatically + # plugin_lib_names: [] controller_server: ros__parameters: diff --git a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml index 8bfe04488d5..b50108a13fc 100644 --- a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml @@ -47,57 +47,10 @@ bt_navigator: # files to allow for a commandline change default used is the # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml & # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml - plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_smooth_path_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_assisted_teleop_action_bt_node - - nav2_back_up_action_bt_node - - nav2_drive_on_heading_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_are_error_codes_active_condition_bt_node - - nav2_would_a_controller_recovery_help_condition_bt_node - - nav2_would_a_planner_recovery_help_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_globally_updated_goal_condition_bt_node - - nav2_is_path_valid_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_truncate_path_local_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_goal_updated_controller_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_controller_cancel_bt_node - - nav2_path_longer_on_approach_bt_node - - nav2_wait_cancel_bt_node - - nav2_spin_cancel_bt_node - - nav2_back_up_cancel_bt_node - - nav2_assisted_teleop_cancel_bt_node - - nav2_drive_on_heading_cancel_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - - nav2_planner_selector_bt_node - - nav2_smoother_selector_bt_node - - nav2_progress_checker_selector_bt_node + + # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). + # Built-in plugins are added automatically + # plugin_lib_names: [] controller_server: ros__parameters: diff --git a/nav2_system_tests/src/error_codes/error_code_param.yaml b/nav2_system_tests/src/error_codes/error_code_param.yaml index 388e6f2cdb2..7886fe66866 100644 --- a/nav2_system_tests/src/error_codes/error_code_param.yaml +++ b/nav2_system_tests/src/error_codes/error_code_param.yaml @@ -154,7 +154,7 @@ planner_server: expected_planner_frequency: 20.0 planner_plugins: [ "unknown", "tf_error", "start_outside_map", "goal_outside_map", "start_occupied", "goal_occupied", "timeout","no_valid_path", - "no_viapoints_given" ] + "no_viapoints_given", "cancelled" ] unknown: plugin: "nav2_system_tests::UnknownErrorPlanner" tf_error: @@ -173,6 +173,8 @@ planner_server: plugin: "nav2_system_tests::NoValidPathErrorPlanner" no_viapoints_given: plugin: "nav2_system_tests::NoViapointsGivenErrorPlanner" + cancelled: + plugin: "nav2_system_tests::CancelledPlanner" smoother_server: ros__parameters: diff --git a/nav2_system_tests/src/error_codes/planner/planner_error_plugin.cpp b/nav2_system_tests/src/error_codes/planner/planner_error_plugin.cpp index 90b18c5b117..1c2eecf23a4 100644 --- a/nav2_system_tests/src/error_codes/planner/planner_error_plugin.cpp +++ b/nav2_system_tests/src/error_codes/planner/planner_error_plugin.cpp @@ -24,3 +24,4 @@ PLUGINLIB_EXPORT_CLASS(nav2_system_tests::NoValidPathErrorPlanner, nav2_core::Gl PLUGINLIB_EXPORT_CLASS(nav2_system_tests::TimedOutErrorPlanner, nav2_core::GlobalPlanner) PLUGINLIB_EXPORT_CLASS(nav2_system_tests::TFErrorPlanner, nav2_core::GlobalPlanner) PLUGINLIB_EXPORT_CLASS(nav2_system_tests::NoViapointsGivenErrorPlanner, nav2_core::GlobalPlanner) +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::CancelledPlanner, nav2_core::GlobalPlanner) diff --git a/nav2_system_tests/src/error_codes/planner/planner_error_plugin.hpp b/nav2_system_tests/src/error_codes/planner/planner_error_plugin.hpp index 2b6ae7971d3..77b23aeccef 100644 --- a/nav2_system_tests/src/error_codes/planner/planner_error_plugin.hpp +++ b/nav2_system_tests/src/error_codes/planner/planner_error_plugin.hpp @@ -51,7 +51,8 @@ class UnknownErrorPlanner : public nav2_core::GlobalPlanner nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped &, - const geometry_msgs::msg::PoseStamped &) override + const geometry_msgs::msg::PoseStamped &, + std::function) override { throw nav2_core::PlannerException("Unknown Error"); } @@ -61,7 +62,8 @@ class StartOccupiedErrorPlanner : public UnknownErrorPlanner { nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped &, - const geometry_msgs::msg::PoseStamped &) override + const geometry_msgs::msg::PoseStamped &, + std::function) override { throw nav2_core::StartOccupied("Start Occupied"); } @@ -71,7 +73,8 @@ class GoalOccupiedErrorPlanner : public UnknownErrorPlanner { nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped &, - const geometry_msgs::msg::PoseStamped &) override + const geometry_msgs::msg::PoseStamped &, + std::function) override { throw nav2_core::GoalOccupied("Goal occupied"); } @@ -81,7 +84,8 @@ class StartOutsideMapErrorPlanner : public UnknownErrorPlanner { nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped &, - const geometry_msgs::msg::PoseStamped &) override + const geometry_msgs::msg::PoseStamped &, + std::function) override { throw nav2_core::StartOutsideMapBounds("Start OutsideMapBounds"); } @@ -91,7 +95,8 @@ class GoalOutsideMapErrorPlanner : public UnknownErrorPlanner { nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped &, - const geometry_msgs::msg::PoseStamped &) override + const geometry_msgs::msg::PoseStamped &, + std::function) override { throw nav2_core::GoalOutsideMapBounds("Goal outside map bounds"); } @@ -101,7 +106,8 @@ class NoValidPathErrorPlanner : public UnknownErrorPlanner { nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped &, - const geometry_msgs::msg::PoseStamped &) override + const geometry_msgs::msg::PoseStamped &, + std::function) override { return nav_msgs::msg::Path(); } @@ -112,7 +118,8 @@ class TimedOutErrorPlanner : public UnknownErrorPlanner { nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped &, - const geometry_msgs::msg::PoseStamped &) override + const geometry_msgs::msg::PoseStamped &, + std::function) override { throw nav2_core::PlannerTimedOut("Planner Timed Out"); } @@ -122,7 +129,8 @@ class TFErrorPlanner : public UnknownErrorPlanner { nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped &, - const geometry_msgs::msg::PoseStamped &) override + const geometry_msgs::msg::PoseStamped &, + std::function) override { throw nav2_core::PlannerTFError("TF Error"); } @@ -132,12 +140,33 @@ class NoViapointsGivenErrorPlanner : public UnknownErrorPlanner { nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped &, - const geometry_msgs::msg::PoseStamped &) override + const geometry_msgs::msg::PoseStamped &, + std::function) override { throw nav2_core::NoViapointsGiven("No Via points given"); } }; +class CancelledPlanner : public UnknownErrorPlanner +{ + nav_msgs::msg::Path createPlan( + const geometry_msgs::msg::PoseStamped &, + const geometry_msgs::msg::PoseStamped &, + std::function cancel_checker) override + { + auto start_time = std::chrono::steady_clock::now(); + while (rclcpp::ok() && + std::chrono::steady_clock::now() - start_time < std::chrono::seconds(5)) + { + if (cancel_checker()) { + throw nav2_core::PlannerCancelled("Planner Cancelled"); + } + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } + throw nav2_core::PlannerException("Cancel is not called in time."); + } +}; + } // namespace nav2_system_tests diff --git a/nav2_system_tests/src/error_codes/planner_plugins.xml b/nav2_system_tests/src/error_codes/planner_plugins.xml index 9d6f7f0545b..ab10ae800bb 100644 --- a/nav2_system_tests/src/error_codes/planner_plugins.xml +++ b/nav2_system_tests/src/error_codes/planner_plugins.xml @@ -39,4 +39,8 @@ base_class_type="nav2_core::GlobalPlanner"> This global planner produces a no via points exception. + + This global planner produces a cancelled exception. + diff --git a/nav2_system_tests/src/error_codes/test_error_codes.py b/nav2_system_tests/src/error_codes/test_error_codes.py index f023bbb27c1..a5f2b84d975 100755 --- a/nav2_system_tests/src/error_codes/test_error_codes.py +++ b/nav2_system_tests/src/error_codes/test_error_codes.py @@ -14,6 +14,7 @@ # limitations under the License. import sys +import threading import time from geometry_msgs.msg import PoseStamped @@ -23,7 +24,7 @@ FollowPath, SmoothPath, ) -from nav2_simple_commander.robot_navigator import BasicNavigator +from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult from nav_msgs.msg import Path import rclpy @@ -111,6 +112,17 @@ def main(argv=sys.argv[1:]): result.error_code == error_code ), 'Compute path to pose error does not match' + def cancel_task(): + time.sleep(1) + navigator.goal_handle.cancel_goal_async() + + # Check compute path to pose cancel + threading.Thread(target=cancel_task).start() + result = navigator._getPathImpl(initial_pose, goal_pose, 'cancelled') + assert ( + navigator.getResult() == TaskResult.CANCELED + ), 'Compute path to pose cancel failed' + # Check compute path through error codes goal_pose1 = goal_pose goal_poses = [goal_pose, goal_pose1] @@ -133,6 +145,12 @@ def main(argv=sys.argv[1:]): assert ( result.error_code == error_code ), 'Compute path through pose error does not match' + # Check compute path to pose cancel + threading.Thread(target=cancel_task).start() + result = navigator._getPathThroughPosesImpl(initial_pose, goal_poses, 'cancelled') + assert ( + navigator.getResult() == TaskResult.CANCELED + ), 'Compute path through poses cancel failed' # Check compute path to pose error codes pose = PoseStamped() diff --git a/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml b/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml index d11ef6ab535..6e9f3f490bf 100644 --- a/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml +++ b/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml @@ -15,60 +15,11 @@ bt_navigator: # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. - plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_smooth_path_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_assisted_teleop_action_bt_node - - nav2_back_up_action_bt_node - - nav2_drive_on_heading_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_globally_updated_goal_condition_bt_node - - nav2_is_path_valid_condition_bt_node - - nav2_are_error_codes_active_condition_bt_node - - nav2_would_a_controller_recovery_help_condition_bt_node - - nav2_would_a_planner_recovery_help_condition_bt_node - - nav2_would_a_smoother_recovery_help_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_truncate_path_local_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_path_expiring_timer_condition - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_goal_updated_controller_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_planner_selector_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - - nav2_controller_cancel_bt_node - - nav2_path_longer_on_approach_bt_node - - nav2_wait_cancel_bt_node - - nav2_spin_cancel_bt_node - - nav2_back_up_cancel_bt_node - - nav2_assisted_teleop_cancel_bt_node - - nav2_drive_on_heading_cancel_bt_node - - nav2_is_battery_charging_condition_bt_node - - nav2_progress_checker_selector_bt_node - - nav2_smoother_selector_bt_node + + # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). + # Built-in plugins are added automatically + # plugin_lib_names: [] + error_code_names: - compute_path_error_code - follow_path_error_code diff --git a/nav2_system_tests/src/planning/planner_tester.hpp b/nav2_system_tests/src/planning/planner_tester.hpp index 7a5f8d4b7f9..421a00f4b7d 100644 --- a/nav2_system_tests/src/planning/planner_tester.hpp +++ b/nav2_system_tests/src/planning/planner_tester.hpp @@ -87,7 +87,8 @@ class NavFnPlannerTester : public nav2_planner::PlannerServer return false; } try { - path = planners_["GridBased"]->createPlan(start, goal); + auto dummy_cancel_checker = []() {return false;}; + path = planners_["GridBased"]->createPlan(start, goal, dummy_cancel_checker); // The situation when createPlan() did not throw any exception // does not guarantee that plan was created correctly. // So it should be checked additionally that path is correct. diff --git a/nav2_system_tests/src/planning/test_planner_plugins.cpp b/nav2_system_tests/src/planning/test_planner_plugins.cpp index 906e8f44d3a..01d264006ac 100644 --- a/nav2_system_tests/src/planning/test_planner_plugins.cpp +++ b/nav2_system_tests/src/planning/test_planner_plugins.cpp @@ -57,9 +57,11 @@ void testSmallPathValidityAndOrientation(std::string plugin, double length) goal.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(-M_PI); goal.header.frame_id = "map"; + auto dummy_cancel_checker = []() {return false;}; + // Test without use_final_approach_orientation // expecting end path pose orientation to be equal to goal orientation - auto path = obj->getPlan(start, goal, "GridBased"); + auto path = obj->getPlan(start, goal, "GridBased", dummy_cancel_checker); EXPECT_GT((int)path.poses.size(), 0); EXPECT_NEAR(tf2::getYaw(path.poses.back().pose.orientation), -M_PI, 0.01); // obj->onCleanup(state); @@ -93,7 +95,9 @@ void testSmallPathValidityAndNoOrientation(std::string plugin, double length) goal.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(-M_PI); goal.header.frame_id = "map"; - auto path = obj->getPlan(start, goal, "GridBased"); + auto dummy_cancel_checker = []() {return false;}; + + auto path = obj->getPlan(start, goal, "GridBased", dummy_cancel_checker); EXPECT_GT((int)path.poses.size(), 0); int path_size = path.poses.size(); @@ -114,6 +118,36 @@ void testSmallPathValidityAndNoOrientation(std::string plugin, double length) obj.reset(); } +void testCancel(std::string plugin) +{ + auto obj = std::make_shared(); + rclcpp_lifecycle::State state; + obj->set_parameter(rclcpp::Parameter("GridBased.plugin", plugin)); + obj->declare_parameter("GridBased.terminal_checking_interval", rclcpp::ParameterValue(1)); + obj->onConfigure(state); + + geometry_msgs::msg::PoseStamped start; + geometry_msgs::msg::PoseStamped goal; + + start.pose.position.x = 0.0; + start.pose.position.y = 0.0; + start.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(M_PI_2); + start.header.frame_id = "map"; + + goal.pose.position.x = 0.5; + goal.pose.position.y = 0.6; + goal.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(-M_PI); + goal.header.frame_id = "map"; + + auto always_cancelled = []() {return true;}; + + EXPECT_THROW( + obj->getPlan(start, goal, "GridBased", always_cancelled), + nav2_core::PlannerCancelled); + // obj->onCleanup(state); + obj.reset(); +} + TEST(testPluginMap, Failures) { auto obj = std::make_shared(); @@ -127,11 +161,14 @@ TEST(testPluginMap, Failures) geometry_msgs::msg::PoseStamped goal; std::string plugin_fake = "fake"; std::string plugin_none = ""; - auto path = obj->getPlan(start, goal, plugin_none); + + auto dummy_cancel_checker = []() {return false;}; + + auto path = obj->getPlan(start, goal, plugin_none, dummy_cancel_checker); EXPECT_EQ(path.header.frame_id, std::string("map")); try { - path = obj->getPlan(start, goal, plugin_fake); + path = obj->getPlan(start, goal, plugin_fake, dummy_cancel_checker); FAIL() << "Failed to throw invalid planner id exception"; } catch (const nav2_core::InvalidPlanner & ex) { EXPECT_EQ(ex.what(), std::string("Planner id fake is invalid")); @@ -140,6 +177,7 @@ TEST(testPluginMap, Failures) obj->onCleanup(state); } + TEST(testPluginMap, Smac2dEqualStartGoal) { testSmallPathValidityAndOrientation("nav2_smac_planner/SmacPlanner2D", 0.0); @@ -290,6 +328,32 @@ TEST(testPluginMap, ThetaStarAboveCostmapResolutionN) testSmallPathValidityAndNoOrientation("nav2_theta_star_planner/ThetaStarPlanner", 1.5); } +TEST(testPluginMap, NavFnCancel) +{ + testCancel("nav2_navfn_planner/NavfnPlanner"); +} + +TEST(testPluginMap, ThetaStarCancel) +{ + testCancel("nav2_theta_star_planner/ThetaStarPlanner"); +} + +TEST(testPluginMap, Smac2dCancel) +{ + testCancel("nav2_smac_planner/SmacPlanner2D"); +} + +TEST(testPluginMap, SmacLatticeCancel) +{ + testCancel("nav2_smac_planner/SmacPlannerLattice"); +} + +TEST(testPluginMap, SmacHybridAStarCancel) +{ + testCancel("nav2_smac_planner/SmacPlannerHybrid"); +} + + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp index a58a4f87eda..1c819a882ee 100644 --- a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp +++ b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp @@ -76,6 +76,8 @@ class ThetaStar bool allow_unknown_; /// the x-directional and y-directional lengths of the map respectively int size_x_, size_y_; + /// the interval at which the planner checks if it has been cancelled + int terminal_checking_interval_; ThetaStar(); @@ -87,7 +89,7 @@ class ThetaStar * @param raw_path is used to return the path obtained by executing the algorithm * @return true if a path is found, false if no path is found in between the start and goal pose */ - bool generatePath(std::vector & raw_path); + bool generatePath(std::vector & raw_path, std::function cancel_checker); /** * @brief this function checks whether the cost of a point(cx, cy) on the costmap is less than the LETHAL_COST diff --git a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp index bcbff2ae7c0..09f87142196 100644 --- a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp +++ b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp @@ -54,9 +54,17 @@ class ThetaStarPlanner : public nav2_core::GlobalPlanner void deactivate() override; + /** + * @brief Creating a plan from start and goal poses + * @param start Start pose + * @param goal Goal pose + * @param cancel_checker Function to check if the action has been canceled + * @return nav2_msgs::Path of the generated path + */ nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal) override; + const geometry_msgs::msg::PoseStamped & goal, + std::function cancel_checker) override; protected: std::shared_ptr tf_; @@ -76,9 +84,10 @@ class ThetaStarPlanner : public nav2_core::GlobalPlanner /** * @brief the function responsible for calling the algorithm and retrieving a path from it + * @param cancel_checker is a function to check if the action has been canceled * @return global_path is the planned path to be taken */ - void getPlan(nav_msgs::msg::Path & global_path); + void getPlan(nav_msgs::msg::Path & global_path, std::function cancel_checker); /** * @brief interpolates points between the consecutive waypoints of the path diff --git a/nav2_theta_star_planner/src/theta_star.cpp b/nav2_theta_star_planner/src/theta_star.cpp index f83b745b22e..ba3629bcbfa 100644 --- a/nav2_theta_star_planner/src/theta_star.cpp +++ b/nav2_theta_star_planner/src/theta_star.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include +#include "nav2_core/planner_exceptions.hpp" #include "nav2_theta_star_planner/theta_star.hpp" namespace theta_star @@ -26,6 +27,7 @@ ThetaStar::ThetaStar() allow_unknown_(true), size_x_(0), size_y_(0), + terminal_checking_interval_(5000), index_generated_(0) { exp_node = new tree_node; @@ -43,7 +45,7 @@ void ThetaStar::setStartAndGoal( dst_ = {static_cast(d[0]), static_cast(d[1])}; } -bool ThetaStar::generatePath(std::vector & raw_path) +bool ThetaStar::generatePath(std::vector & raw_path, std::function cancel_checker) { resetContainers(); addToNodesData(index_generated_); @@ -60,6 +62,11 @@ bool ThetaStar::generatePath(std::vector & raw_path) while (!queue_.empty()) { nodes_opened++; + if (nodes_opened % terminal_checking_interval_ == 0 && cancel_checker()) { + clearQueue(); + throw nav2_core::PlannerCancelled("Planner was canceled"); + } + if (isGoal(*curr_data)) { break; } diff --git a/nav2_theta_star_planner/src/theta_star_planner.cpp b/nav2_theta_star_planner/src/theta_star_planner.cpp index ca9391111b3..61064efbf45 100644 --- a/nav2_theta_star_planner/src/theta_star_planner.cpp +++ b/nav2_theta_star_planner/src/theta_star_planner.cpp @@ -59,6 +59,10 @@ void ThetaStarPlanner::configure( planner_->w_heuristic_cost_ = planner_->w_euc_cost_ < 1.0 ? planner_->w_euc_cost_ : 1.0; + nav2_util::declare_parameter_if_not_declared( + node, name_ + ".terminal_checking_interval", rclcpp::ParameterValue(5000)); + node->get_parameter(name_ + ".terminal_checking_interval", planner_->terminal_checking_interval_); + nav2_util::declare_parameter_if_not_declared( node, name + ".use_final_approach_orientation", rclcpp::ParameterValue(false)); node->get_parameter(name + ".use_final_approach_orientation", use_final_approach_orientation_); @@ -86,7 +90,8 @@ void ThetaStarPlanner::deactivate() nav_msgs::msg::Path ThetaStarPlanner::createPlan( const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal) + const geometry_msgs::msg::PoseStamped & goal, + std::function cancel_checker) { nav_msgs::msg::Path global_path; auto start_time = std::chrono::steady_clock::now(); @@ -140,7 +145,7 @@ nav_msgs::msg::Path ThetaStarPlanner::createPlan( RCLCPP_DEBUG( logger_, "Got the src and dst... (%i, %i) && (%i, %i)", planner_->src_.x, planner_->src_.y, planner_->dst_.x, planner_->dst_.y); - getPlan(global_path); + getPlan(global_path, cancel_checker); // check if a plan is generated size_t plan_size = global_path.poses.size(); if (plan_size > 0) { @@ -173,13 +178,15 @@ nav_msgs::msg::Path ThetaStarPlanner::createPlan( return global_path; } -void ThetaStarPlanner::getPlan(nav_msgs::msg::Path & global_path) +void ThetaStarPlanner::getPlan( + nav_msgs::msg::Path & global_path, + std::function cancel_checker) { std::vector path; if (planner_->isUnsafeToPlan()) { global_path.poses.clear(); throw nav2_core::PlannerException("Either of the start or goal pose are an obstacle! "); - } else if (planner_->generatePath(path)) { + } else if (planner_->generatePath(path, cancel_checker)) { global_path = linearInterpolation(path, planner_->costmap_->getResolution()); } else { global_path.poses.clear(); @@ -229,6 +236,9 @@ ThetaStarPlanner::dynamicParametersCallback(std::vector param if (name == name_ + ".how_many_corners") { planner_->how_many_corners_ = parameter.as_int(); } + if (name == name_ + ".terminal_checking_interval") { + planner_->terminal_checking_interval_ = parameter.as_int(); + } } else if (type == ParameterType::PARAMETER_DOUBLE) { if (name == name_ + ".w_euc_cost") { planner_->w_euc_cost_ = parameter.as_double(); diff --git a/nav2_theta_star_planner/test/test_theta_star.cpp b/nav2_theta_star_planner/test/test_theta_star.cpp index 0476290f934..a8db0ddbfe0 100644 --- a/nav2_theta_star_planner/test/test_theta_star.cpp +++ b/nav2_theta_star_planner/test/test_theta_star.cpp @@ -60,10 +60,12 @@ class test_theta_star : public theta_star::ThetaStar void uresetContainers() {nodes_data_.clear(); resetContainers();} - bool runAlgo(std::vector & path) + bool runAlgo( + std::vector & path, + std::function cancel_checker = [] () {return false;}) { if (!isUnsafeToPlan()) { - return generatePath(path); + return generatePath(path, cancel_checker); } return false; } @@ -162,7 +164,11 @@ TEST(ThetaStarPlanner, test_theta_star_planner) { planner_2d->configure(life_node, "test", nullptr, costmap_ros); planner_2d->activate(); - nav_msgs::msg::Path path = planner_2d->createPlan(start, goal); + auto dummy_cancel_checker = []() { + return false; + }; + + nav_msgs::msg::Path path = planner_2d->createPlan(start, goal, dummy_cancel_checker); EXPECT_GT(static_cast(path.poses.size()), 0); // test if the goal is unsafe @@ -174,7 +180,7 @@ TEST(ThetaStarPlanner, test_theta_star_planner) { goal.pose.position.x = 1.0; goal.pose.position.y = 1.0; - EXPECT_THROW(planner_2d->createPlan(start, goal), nav2_core::GoalOccupied); + EXPECT_THROW(planner_2d->createPlan(start, goal, dummy_cancel_checker), nav2_core::GoalOccupied); planner_2d->deactivate(); planner_2d->cleanup(); @@ -212,7 +218,8 @@ TEST(ThetaStarPlanner, test_theta_star_reconfigure) rclcpp::Parameter("test.w_euc_cost", 1.0), rclcpp::Parameter("test.w_traversal_cost", 2.0), rclcpp::Parameter("test.use_final_approach_orientation", false), - rclcpp::Parameter("test.allow_unknown", false)}); + rclcpp::Parameter("test.allow_unknown", false), + rclcpp::Parameter("test.terminal_checking_interval", 100)}); rclcpp::spin_until_future_complete( life_node->get_node_base_interface(), @@ -225,6 +232,7 @@ TEST(ThetaStarPlanner, test_theta_star_reconfigure) EXPECT_EQ(life_node->get_parameter("test.w_traversal_cost").as_double(), 2.0); EXPECT_EQ(life_node->get_parameter("test.use_final_approach_orientation").as_bool(), false); EXPECT_EQ(life_node->get_parameter("test.allow_unknown").as_bool(), false); + EXPECT_EQ(life_node->get_parameter("test.terminal_checking_interval").as_int(), 100); rclcpp::spin_until_future_complete( life_node->get_node_base_interface(), diff --git a/nav2_util/include/nav2_util/simple_action_server.hpp b/nav2_util/include/nav2_util/simple_action_server.hpp index 71828321f00..eec9c2dd9f7 100644 --- a/nav2_util/include/nav2_util/simple_action_server.hpp +++ b/nav2_util/include/nav2_util/simple_action_server.hpp @@ -574,7 +574,7 @@ class SimpleActionServer * @param the Results object to terminate the action with */ void terminate( - std::shared_ptr> handle, + std::shared_ptr> & handle, typename std::shared_ptr result = std::make_shared()) { diff --git a/tools/bt2img.py b/tools/bt2img.py index b3c2b34eaf5..2b12d777d82 100755 --- a/tools/bt2img.py +++ b/tools/bt2img.py @@ -27,7 +27,7 @@ 'ReactiveFallback', 'ReactiveSequence', 'Sequence', - 'SequenceStar', + 'SequenceWithMemory', 'BlackboardCheckInt', 'BlackboardCheckDouble', 'BlackboardCheckString', From cedfd53c9c1e46e08e343dab49805ebf13983a97 Mon Sep 17 00:00:00 2001 From: stevedan Date: Mon, 25 Mar 2024 22:33:32 +0100 Subject: [PATCH 02/41] reset controller server back to previous state Signed-off-by: stevedan --- nav2_controller/src/controller_server.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 85aae171cc7..e5fdfbf1338 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -419,7 +419,6 @@ void ControllerServer::computeControl() { std::lock_guard lock(dynamic_params_lock_); - auto start_time = this->now(); RCLCPP_INFO(get_logger(), "Received a goal, begin computing control effort."); try { @@ -483,9 +482,8 @@ void ControllerServer::computeControl() auto cycle_duration = this->now() - start_time; if (!loop_rate.sleep()) { RCLCPP_WARN( - get_logger(), - "Control loop missed its desired rate of %.4f Hz. Current loop rate is %.4f Hz.", - controller_frequency_, 1 / cycle_duration.seconds()); + get_logger(), "Control loop missed its desired rate of %.4fHz", + controller_frequency_); } } } catch (nav2_core::InvalidController & e) { From 39be473f713d6a078778885775beb44dccffa786 Mon Sep 17 00:00:00 2001 From: stevedan Date: Mon, 25 Mar 2024 22:34:21 +0100 Subject: [PATCH 03/41] reset controller server back to previous stat: remove cycle duration Signed-off-by: stevedan --- nav2_controller/src/controller_server.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index e5fdfbf1338..fa25e0b1379 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -479,7 +479,6 @@ void ControllerServer::computeControl() break; } - auto cycle_duration = this->now() - start_time; if (!loop_rate.sleep()) { RCLCPP_WARN( get_logger(), "Control loop missed its desired rate of %.4fHz", From a3261a7e59ce584d7493698daf702652032484d4 Mon Sep 17 00:00:00 2001 From: stevedan Date: Wed, 27 Mar 2024 06:50:50 +0100 Subject: [PATCH 04/41] prune goals that are in collission Signed-off-by: stevedan --- nav2_smac_planner/src/a_star.cpp | 12 +++++++++--- nav2_smac_planner/test/test_a_star.cpp | 3 ++- 2 files changed, 11 insertions(+), 4 deletions(-) diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index e9529bbdca1..e6a4c832141 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -296,9 +296,15 @@ bool AStarAlgorithm::areInputsValid() // Check if ending point is valid if (getToleranceHeuristic() < 0.001) { - // if any goal is valid, then all goals are valid - auto goal = *_goalsSet.begin(); - if (!goal->isNodeValid(_traverse_unknown, _collision_checker)) { + // if a node is not valid, prune it from the goals set + for (auto it = _goalsSet.begin(); it != _goalsSet.end(); ) { + if (!(*it)->isNodeValid(_traverse_unknown, _collision_checker)) { + it = _goalsSet.erase(it); + } else { + ++it; + } + } + if (_goalsSet.empty()) { throw nav2_core::GoalOccupied("Goal was in lethal cost"); } } diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp index ead3b7e6854..e5fd8e589cf 100644 --- a/nav2_smac_planner/test/test_a_star.cpp +++ b/nav2_smac_planner/test/test_a_star.cpp @@ -418,10 +418,11 @@ TEST(AStarTest, test_goal_heading_mode) a_star.setGoal(80u, 80u, 40u); unsigned int num_bins = nav2_smac_planner::NodeHybrid::motion_table.num_angle_quantization; - EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, dummy_cancel_checker, expansions.get())); EXPECT_TRUE(a_star.getGoals().size() == num_bins); // first goal should be the same the one set by the user EXPECT_TRUE(a_star.getInitialGoalCoordinate().theta == 40u); + EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, dummy_cancel_checker, expansions.get())); + } From 10900a48efd92adfcd08ecf4e172968dab7ce41c Mon Sep 17 00:00:00 2001 From: stevedan Date: Tue, 2 Apr 2024 23:44:03 +0200 Subject: [PATCH 05/41] comment improvement Signed-off-by: stevedan --- nav2_smac_planner/include/nav2_smac_planner/a_star.hpp | 2 +- nav2_smac_planner/src/a_star.cpp | 7 ++++--- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp index 3fca0e8b558..e9a170e09ae 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -200,7 +200,7 @@ class AStarAlgorithm * before applying the heading mode * @return Coordinate to the first goal */ - Coordinates getInitialGoalCoordinate(); + const Coordinates & getInitialGoalCoordinate(); protected: /** diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index e6a4c832141..9227d56fc44 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -482,8 +482,9 @@ float AStarAlgorithm::getHeuristicCost(const NodePtr & node) { const Coordinates node_coords = NodeT::getCoords(node->getIndex(), getSizeX(), getSizeDim3()); - // Heurittic cost for more than one goal is already cotemplated in the - // precomputation stage + // The heuristic cost utilizes the initial goal coordinate for potential nodes + // according to the goal heading mode because we already consider the goal + // heading mode during heuristic precomputation. float heuristic = NodeT::getHeuristicCost(node_coords, getInitialGoalCoordinate()); if (heuristic < _best_heuristic_node.first) { _best_heuristic_node = {heuristic, node->getIndex()}; @@ -559,7 +560,7 @@ void AStarAlgorithm::clearStart() template -typename AStarAlgorithm::Coordinates AStarAlgorithm::getInitialGoalCoordinate() +const typename AStarAlgorithm::Coordinates & AStarAlgorithm::getInitialGoalCoordinate() { return _goals_coordinates[0]; } From 5ac4d41e0b23713208dac073f676247d317c8ff8 Mon Sep 17 00:00:00 2001 From: stevedan Date: Mon, 15 Apr 2024 19:08:27 +0200 Subject: [PATCH 06/41] WIP Signed-off-by: stevedan --- .../include/nav2_smac_planner/a_star.hpp | 19 +++++++---- .../nav2_smac_planner/analytic_expansion.hpp | 8 +++-- .../include/nav2_smac_planner/node_hybrid.hpp | 4 +-- .../nav2_smac_planner/node_lattice.hpp | 8 ++--- nav2_smac_planner/src/a_star.cpp | 33 ++++++++----------- nav2_smac_planner/test/test_a_star.cpp | 9 +++-- 6 files changed, 40 insertions(+), 41 deletions(-) diff --git a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp index e9a170e09ae..b38f203884e 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -94,6 +94,9 @@ class AStarAlgorithm * or planning time exceeded * @param max_planning_time Maximum time (in seconds) to wait for a plan, createPath returns * false after this timeout + * @param lookup_table_size Size of the lookup table to store heuristic values + * @param dim_3_size Number of quantization bins + * @param goal_heading_mode Heading mode for goal heading */ void initialize( const bool & allow_unknown, @@ -161,7 +164,7 @@ class AStarAlgorithm /** * @brief Get pointer reference to goal node - * @return Node pointer reference to goal node + * @return unordered_set of node pointers reference to the goal nodes */ NodeSet & getGoals(); @@ -195,12 +198,14 @@ class AStarAlgorithm */ unsigned int & getSizeDim3(); - /** - * @brief Return the first goal coordinate defined by the user - * before applying the heading mode - * @return Coordinate to the first goal - */ - const Coordinates & getInitialGoalCoordinate(); + // todo: remove getInitialGoalCoordinate + + // /** + // * @brief Return the first goal coordinate defined by the user + // * before applying the heading mode + // * @return Coordinate to the first goal + // */ + // const Coordinates & getInitialGoalCoordinate(); protected: /** diff --git a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp index ba728247b4a..e1f13315724 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp @@ -80,12 +80,14 @@ class AnalyticExpansion /** * @brief Attempt an analytic path completion * @param node The node to start the analytic path from - * @param goal The goal node to plan to + * @param goals_node set of goals node to plan to + * @param initial_goal_coords The coordinates of the initial goal node before goal heading + * was applied * @param getter Gets a node at a set of coordinates * @param iterations Iterations to run over * @param best_cost Best heuristic cost to propertionally expand more closer to the goal - * @return Node pointer reference to goal node if successful, else - * return nullptr + * @return Node pointer reference to goal node with the best score out of the goals node if + * successful, else return nullptr */ NodePtr tryAnalyticExpansion( const NodePtr & current_node, diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp index 56bf8c74f16..434fd588b07 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp @@ -460,8 +460,8 @@ class NodeHybrid bool backtracePath(CoordinateVector & path); /** - * \brief Sets the goal mode for the current search - * \param goal_heading_mode The goal heading mode to use + * @brief Sets the goal mode for the current search + * @param goal_heading_mode The goal heading mode to use */ static void setGoalHeadingMode(const GoalHeadingMode & current_goal_heading_mode); diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp index 2ffc213087f..1be04147ca6 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp @@ -410,14 +410,14 @@ class NodeLattice bool backtracePath(CoordinateVector & path); /** - * \brief add node to the path - * \param current_node + * @brief add node to the path + * @param current_node */ void addNodeToPath(NodePtr current_node, CoordinateVector & path); /** - * \brief Sets the goal mode for the current search - * \param goal_heading_mode The goal heading mode to use + * @brief Sets the goal mode for the current search + * @param goal_heading_mode The goal heading mode to use */ static void setGoalHeadingMode(const GoalHeadingMode & current_goal_heading_mode); diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index 9227d56fc44..479ad66dc40 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -240,18 +240,8 @@ void AStarAlgorithm::setGoal( static_cast(dim_3_half_bin))); break; case GoalHeadingMode::ALL_DIRECTION: - // Add all possible headings as goals - // set first goal to be the same as the input - goals.push_back(addToGraph(NodeT::getIndex(mx, my, dim_3))); - goals_coordinates.push_back( - typename NodeT::Coordinates( - static_cast(mx), - static_cast(my), - static_cast(dim_3))); + // Add all goals for each direction for (unsigned int i = 0; i < num_bins; ++i) { - if (i == dim_3) { - continue; - } goals.push_back(addToGraph(NodeT::getIndex(mx, my, i))); goals_coordinates.push_back( typename NodeT::Coordinates( @@ -265,7 +255,9 @@ void AStarAlgorithm::setGoal( break; } - if (!_search_info.cache_obstacle_heuristic || goals_coordinates != _goals_coordinates) { + // we just have to check whether the x and y are the same because the dim3 is not used + // in the computation of the obstacle heuristic + if (!_search_info.cache_obstacle_heuristic || (goals_coordinates[0].x == _goals_coordinates[0].x && goals_coordinates[0].y == _goals_coordinates[0].y)) { if (!_start) { throw std::runtime_error("Start must be set before goal."); } @@ -300,6 +292,7 @@ bool AStarAlgorithm::areInputsValid() for (auto it = _goalsSet.begin(); it != _goalsSet.end(); ) { if (!(*it)->isNodeValid(_traverse_unknown, _collision_checker)) { it = _goalsSet.erase(it); + _goals_coordinates.erase(_goals_coordinates.begin() + std::distance(_goalsSet.begin(), it)); } else { ++it; } @@ -392,6 +385,7 @@ bool AStarAlgorithm::createPath( current_node->visited(); // 2.1) Use an analytic expansion (if available) to generate a path + // todo -> remove the getInitialGoalCoordinate() function expansion_result = nullptr; expansion_result = _expander->tryAnalyticExpansion( current_node, getGoals(), @@ -482,9 +476,7 @@ float AStarAlgorithm::getHeuristicCost(const NodePtr & node) { const Coordinates node_coords = NodeT::getCoords(node->getIndex(), getSizeX(), getSizeDim3()); - // The heuristic cost utilizes the initial goal coordinate for potential nodes - // according to the goal heading mode because we already consider the goal - // heading mode during heuristic precomputation. + // TODO: remove the getInitialGoalCoordinate() function float heuristic = NodeT::getHeuristicCost(node_coords, getInitialGoalCoordinate()); if (heuristic < _best_heuristic_node.first) { _best_heuristic_node = {heuristic, node->getIndex()}; @@ -559,11 +551,12 @@ void AStarAlgorithm::clearStart() } -template -const typename AStarAlgorithm::Coordinates & AStarAlgorithm::getInitialGoalCoordinate() -{ - return _goals_coordinates[0]; -} +// template +// const typename AStarAlgorithm::Coordinates & +// AStarAlgorithm::getInitialGoalCoordinate() +// { +// return _goals_coordinates[0]; +// } // Instantiate algorithm for the supported template types template class AStarAlgorithm; diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp index e5fd8e589cf..b5715a7ced9 100644 --- a/nav2_smac_planner/test/test_a_star.cpp +++ b/nav2_smac_planner/test/test_a_star.cpp @@ -132,7 +132,7 @@ TEST(AStarTest, test_a_star_2d) EXPECT_TRUE(a_star_2.getStart() != nullptr); EXPECT_TRUE(!a_star_2.getGoals().empty()); - EXPECT_TRUE(a_star_2.getGoals().size() == 1); + EXPECT_EQ(a_star_2.getGoals().size(), 1); EXPECT_EQ(a_star_2.getSizeX(), 100u); EXPECT_EQ(a_star_2.getSizeY(), 100u); EXPECT_EQ(a_star_2.getSizeDim3(), 1u); @@ -404,8 +404,8 @@ TEST(AStarTest, test_goal_heading_mode) EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, dummy_cancel_checker, expansions.get())); - EXPECT_TRUE(a_star.getGoals().size() == 2); - EXPECT_TRUE(a_star.getInitialGoalCoordinate().theta == 40u); + EXPECT_EQ(a_star.getGoals().size(), 2); + EXPECT_EQ(a_star.getInitialGoalCoordinate().theta, 40u); // ALL_DIRECTION goal heading mode @@ -420,10 +420,9 @@ TEST(AStarTest, test_goal_heading_mode) unsigned int num_bins = nav2_smac_planner::NodeHybrid::motion_table.num_angle_quantization; EXPECT_TRUE(a_star.getGoals().size() == num_bins); // first goal should be the same the one set by the user - EXPECT_TRUE(a_star.getInitialGoalCoordinate().theta == 40u); + EXPECT_EQ(a_star.getInitialGoalCoordinate().theta, 40u); EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, dummy_cancel_checker, expansions.get())); - } TEST(AStarTest, test_constants) From 49f16356a90488e6f023f697a9e09eaaa8bef957 Mon Sep 17 00:00:00 2001 From: stevedan Date: Tue, 16 Apr 2024 21:58:41 +0200 Subject: [PATCH 07/41] WIP Signed-off-by: stevedan --- .../include/nav2_smac_planner/a_star.hpp | 26 +++++------ .../nav2_smac_planner/analytic_expansion.hpp | 3 +- .../include/nav2_smac_planner/node_2d.hpp | 2 +- .../include/nav2_smac_planner/node_hybrid.hpp | 13 ++---- .../nav2_smac_planner/node_lattice.hpp | 8 +--- nav2_smac_planner/src/a_star.cpp | 46 +++++++++---------- nav2_smac_planner/src/analytic_expansion.cpp | 6 +-- nav2_smac_planner/src/node_2d.cpp | 6 +-- nav2_smac_planner/src/node_hybrid.cpp | 34 ++++---------- nav2_smac_planner/src/node_lattice.cpp | 32 ++++--------- nav2_smac_planner/src/smac_planner_hybrid.cpp | 9 ++-- .../src/smac_planner_lattice.cpp | 10 ++-- nav2_smac_planner/test/test_a_star.cpp | 9 ++-- 13 files changed, 76 insertions(+), 128 deletions(-) diff --git a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp index d973b3e31e8..05e3f358c49 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -105,8 +105,7 @@ class AStarAlgorithm const int & terminal_checking_interval, const double & max_planning_time, const float & lookup_table_size, - const unsigned int & dim_3_size, - const GoalHeadingMode & goal_heading_mode = GoalHeadingMode::DEFAULT); + const unsigned int & dim_3_size); /** * @brief Creating path from given costmap, start, and goal @@ -137,7 +136,8 @@ class AStarAlgorithm void setGoal( const float & mx, const float & my, - const unsigned int & dim_3); + const unsigned int & dim_3, + const GoalHeadingMode & goal_heading_mode = GoalHeadingMode::DEFAULT); /** * @brief Set the starting pose for planning, as a node index @@ -163,11 +163,17 @@ class AStarAlgorithm NodePtr & getStart(); /** - * @brief Get pointer reference to goal node - * @return unordered_set of node pointers reference to the goal nodes + * @brief Get pointer reference to goals node + * @return unordered_set of node pointers reference to the goals nodes */ NodeSet & getGoals(); + /** + * @brief Get pointer reference to goals coordinates + * @return vector of goals coordinates + */ + CoordinateVector & goalGoalsCoordinates(); + /** * @brief Get maximum number of on-approach iterations after within threshold * @return Reference to Maximum on-appraoch iterations parameter @@ -198,15 +204,6 @@ class AStarAlgorithm */ unsigned int & getSizeDim3(); - // todo: remove getInitialGoalCoordinate - - // /** - // * @brief Return the first goal coordinate defined by the user - // * before applying the heading mode - // * @return Coordinate to the first goal - // */ - // const Coordinates & getInitialGoalCoordinate(); - protected: /** * @brief Get pointer to next goal in open set @@ -284,7 +281,6 @@ class AStarAlgorithm CoordinateVector _goals_coordinates; NodePtr _start; NodeSet _goalsSet; - GoalHeadingMode _goal_heading_mode; Graph _graph; NodeQueue _queue; diff --git a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp index e1f13315724..ee54573edb7 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp @@ -37,6 +37,7 @@ class AnalyticExpansion typedef NodeT * NodePtr; typedef std::unordered_set NodeSet; typedef typename NodeT::Coordinates Coordinates; + typedef typename NodeT::CoordinateVector CoordinateVector; typedef std::function NodeGetter; /** @@ -92,7 +93,7 @@ class AnalyticExpansion NodePtr tryAnalyticExpansion( const NodePtr & current_node, const NodeSet & goals_node, - const Coordinates & initial_goal_coords, + const CoordinateVector & goals_coords, const NodeGetter & getter, int & iterations, int & best_cost); /** diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp index 10a975577a4..eaf5b75c6b1 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp @@ -227,7 +227,7 @@ class Node2D */ static float getHeuristicCost( const Coordinates & node_coords, - const Coordinates & goal_coordinates); + const CoordinateVector & goals_coords); /** * @brief Initialize the neighborhood to be used in A* diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp index 932e2192fa4..134ecdb737d 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp @@ -366,7 +366,7 @@ class NodeHybrid */ static float getHeuristicCost( const Coordinates & node_coords, - const Coordinates & goal_coordinates); + const CoordinateVector & goals_coords); /** * @brief Initialize motion models @@ -459,28 +459,23 @@ class NodeHybrid */ bool backtracePath(CoordinateVector & path); - /** - * @brief Sets the goal mode for the current search - * @param goal_heading_mode The goal heading mode to use - */ - static void setGoalHeadingMode(const GoalHeadingMode & current_goal_heading_mode); - NodeHybrid * parent; Coordinates pose; // Constants required across all nodes but don't want to allocate more than once - static float travel_distance_cost; + static double travel_distance_cost; static HybridMotionTable motion_table; // Wavefront lookup and queue for continuing to expand as needed static LookupTable obstacle_heuristic_lookup_table; static ObstacleHeuristicQueue obstacle_heuristic_queue; + static nav2_costmap_2d::Costmap2D * sampled_costmap; static std::shared_ptr costmap_ros; static std::shared_ptr inflation_layer; + static CostmapDownsampler downsampler; // Dubin / Reeds-Shepp lookup and size for dereferencing static LookupTable dist_heuristic_lookup_table; static float size_lookup; - static GoalHeadingMode goal_heading_mode; private: float _cell_cost; diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp index 1be04147ca6..2337d709f16 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp @@ -316,7 +316,7 @@ class NodeLattice */ static float getHeuristicCost( const Coordinates & node_coords, - const Coordinates & goal_coordinates); + const CoordinateVector & goals_coords); /** * @brief Initialize motion models @@ -415,11 +415,6 @@ class NodeLattice */ void addNodeToPath(NodePtr current_node, CoordinateVector & path); - /** - * @brief Sets the goal mode for the current search - * @param goal_heading_mode The goal heading mode to use - */ - static void setGoalHeadingMode(const GoalHeadingMode & current_goal_heading_mode); NodeLattice * parent; Coordinates pose; @@ -427,7 +422,6 @@ class NodeLattice // Dubin / Reeds-Shepp lookup and size for dereferencing static LookupTable dist_heuristic_lookup_table; static float size_lookup; - static GoalHeadingMode goal_heading_mode; private: float _cell_cost; diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index 4a1b9324570..327ef68abb4 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -63,16 +63,13 @@ void AStarAlgorithm::initialize( const int & terminal_checking_interval, const double & max_planning_time, const float & lookup_table_size, - const unsigned int & dim_3_size, - const GoalHeadingMode & goal_heading_mode) + const unsigned int & dim_3_size) { _traverse_unknown = allow_unknown; _max_iterations = max_iterations; _max_on_approach_iterations = max_on_approach_iterations; _terminal_checking_interval = terminal_checking_interval; _max_planning_time = max_planning_time; - _goal_heading_mode = goal_heading_mode; - NodeT::setGoalHeadingMode(_goal_heading_mode); NodeT::precomputeDistanceHeuristic( lookup_table_size, _motion_model, dim_3_size, _search_info); _dim3_size = dim_3_size; @@ -88,15 +85,13 @@ void AStarAlgorithm::initialize( const int & terminal_checking_interval, const double & max_planning_time, const float & /*lookup_table_size*/, - const unsigned int & dim_3_size, - const GoalHeadingMode & goal_heading_mode) + const unsigned int & dim_3_size) { _traverse_unknown = allow_unknown; _max_iterations = max_iterations; _max_on_approach_iterations = max_on_approach_iterations; _terminal_checking_interval = terminal_checking_interval; _max_planning_time = max_planning_time; - _goal_heading_mode = goal_heading_mode; if (dim_3_size != 1) { throw std::runtime_error("Node type Node2D cannot be given non-1 dim 3 quantization."); @@ -194,7 +189,8 @@ template<> void AStarAlgorithm::setGoal( const float & mx, const float & my, - const unsigned int & dim_3) + const unsigned int & dim_3, + const GoalHeadingMode & goal_heading_mode) { if (dim_3 != 0) { throw std::runtime_error("Node type Node2D cannot be given non-zero goal dim 3."); @@ -203,21 +199,21 @@ void AStarAlgorithm::setGoal( _goalsSet.clear(); _goalsSet.insert(addToGraph(Node2D::getIndex(mx, my, getSizeX()))); _goals_coordinates.push_back(Node2D::Coordinates(mx, my)); - } template void AStarAlgorithm::setGoal( const float & mx, const float & my, - const unsigned int & dim_3) + const unsigned int & dim_3, + const GoalHeadingMode & goal_heading_mode) { _goalsSet.clear(); NodeVector goals; CoordinateVector goals_coordinates; unsigned int num_bins = NodeT::motion_table.num_angle_quantization; unsigned int dim_3_half_bin = 0; - switch (_goal_heading_mode) { + switch (goal_heading_mode) { case GoalHeadingMode::DEFAULT: goals.push_back(addToGraph(NodeT::getIndex(mx, my, dim_3))); goals_coordinates.push_back( @@ -260,8 +256,11 @@ void AStarAlgorithm::setGoal( } // we just have to check whether the x and y are the same because the dim3 is not used - // in the computation of the obstacle heuristic - if (!_search_info.cache_obstacle_heuristic || (goals_coordinates[0].x == _goals_coordinates[0].x && goals_coordinates[0].y == _goals_coordinates[0].y)) { + // in the computation of the obstacle heuristic + if (!_search_info.cache_obstacle_heuristic || + (goals_coordinates[0].x == _goals_coordinates[0].x && + goals_coordinates[0].y == _goals_coordinates[0].y)) + { if (!_start) { throw std::runtime_error("Start must be set before goal."); } @@ -328,6 +327,9 @@ bool AStarAlgorithm::createPath( return false; } + // compute min distance heuristic to goal + + // 0) Add starting point to the open set addNode(0.0, getStart()); getStart()->setAccumulatedCost(0.0); @@ -389,11 +391,10 @@ bool AStarAlgorithm::createPath( current_node->visited(); // 2.1) Use an analytic expansion (if available) to generate a path - // todo -> remove the getInitialGoalCoordinate() function expansion_result = nullptr; expansion_result = _expander->tryAnalyticExpansion( current_node, getGoals(), - getInitialGoalCoordinate(), neighborGetter, analytic_iterations, closest_distance); + goalGoalsCoordinates(), neighborGetter, analytic_iterations, closest_distance); if (expansion_result != nullptr) { current_node = expansion_result; } @@ -480,8 +481,7 @@ float AStarAlgorithm::getHeuristicCost(const NodePtr & node) { const Coordinates node_coords = NodeT::getCoords(node->getIndex(), getSizeX(), getSizeDim3()); - // TODO: remove the getInitialGoalCoordinate() function - float heuristic = NodeT::getHeuristicCost(node_coords, getInitialGoalCoordinate()); + float heuristic = NodeT::getHeuristicCost(node_coords, goalGoalsCoordinates()); if (heuristic < _best_heuristic_node.first) { _best_heuristic_node = {heuristic, node->getIndex()}; } @@ -554,13 +554,11 @@ void AStarAlgorithm::clearStart() _costmap->setCost(coords.x, coords.y, nav2_costmap_2d::FREE_SPACE); } - -// template -// const typename AStarAlgorithm::Coordinates & -// AStarAlgorithm::getInitialGoalCoordinate() -// { -// return _goals_coordinates[0]; -// } +template +typename AStarAlgorithm::CoordinateVector & AStarAlgorithm::goalGoalsCoordinates() +{ + return _goals_coordinates; +} // Instantiate algorithm for the supported template types template class AStarAlgorithm; diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp index 4f5f918364e..1fa1baa8984 100644 --- a/nav2_smac_planner/src/analytic_expansion.cpp +++ b/nav2_smac_planner/src/analytic_expansion.cpp @@ -48,7 +48,7 @@ void AnalyticExpansion::setCollisionChecker( template typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalyticExpansion( - const NodePtr & current_node, const NodeSet & goals_node, const Coordinates & initial_goal_coords, + const NodePtr & current_node, const NodeSet & goals_node, const CoordinateVector & goals_coords, const NodeGetter & getter, int & analytic_iterations, int & closest_distance) { @@ -67,7 +67,7 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic NodePtr current_best_node = nullptr; closest_distance = std::min( closest_distance, - static_cast(NodeT::getHeuristicCost(node_coords, initial_goal_coords))); + static_cast(NodeT::getHeuristicCost(node_coords, goals_coords))); // We want to expand at a rate of d/expansion_ratio, // but check to see if we are so close that we would be expanding every iteration // If so, limit it to the expansion ratio (rounded up) @@ -375,7 +375,7 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::setAnalyt template<> typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalyticExpansion( - const NodePtr & current_node, const NodeSet & goals_node, const Coordinates & initial_goal_coords, + const NodePtr & current_node, const NodeSet & goals_node, const CoordinateVector & goals_coords, const NodeGetter & getter, int & analytic_iterations, int & closest_distance) { diff --git a/nav2_smac_planner/src/node_2d.cpp b/nav2_smac_planner/src/node_2d.cpp index 07e16dda64c..1dc4df4f6e5 100644 --- a/nav2_smac_planner/src/node_2d.cpp +++ b/nav2_smac_planner/src/node_2d.cpp @@ -81,12 +81,12 @@ float Node2D::getTraversalCost(const NodePtr & child) float Node2D::getHeuristicCost( const Coordinates & node_coords, - const Coordinates & goal_coordinates) + const CoordinateVector & goals_coords) { // Using Moore distance as it more accurately represents the distances // even a Van Neumann neighborhood robot can navigate. - auto dx = goal_coordinates.x - node_coords.x; - auto dy = goal_coordinates.y - node_coords.y; + auto dx = goals_coords[0].x - node_coords.x; + auto dy = goals_coords[0].y - node_coords.y; return std::sqrt(dx * dx + dy * dy); } diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp index 8233efa7568..85722182e96 100644 --- a/nav2_smac_planner/src/node_hybrid.cpp +++ b/nav2_smac_planner/src/node_hybrid.cpp @@ -44,7 +44,6 @@ std::shared_ptr NodeHybrid::costmap_ros = nullptr std::shared_ptr NodeHybrid::inflation_layer = nullptr; ObstacleHeuristicQueue NodeHybrid::obstacle_heuristic_queue; -GoalHeadingMode NodeHybrid::goal_heading_mode; // Each of these tables are the projected motion models through // time and space applied to the search on the current node in @@ -435,15 +434,18 @@ float NodeHybrid::getTraversalCost(const NodePtr & child) float NodeHybrid::getHeuristicCost( const Coordinates & node_coords, - const Coordinates & goal_coords) + const CoordinateVector & goals_coords) { + // obstacle heuristic does not depend on goal heading const float obstacle_heuristic = - getObstacleHeuristic(node_coords, goal_coords, motion_table.cost_penalty); - if (goal_heading_mode == GoalHeadingMode::ALL_DIRECTION) { - return obstacle_heuristic; - } - const float dist_heuristic = getDistanceHeuristic(node_coords, goal_coords, obstacle_heuristic); - return std::max(obstacle_heuristic, dist_heuristic); + getObstacleHeuristic(node_coords, goals_coords[0], motion_table.cost_penalty); + float distance_heuristic = std::numeric_limits::max(); + for (unsigned int i = 0; i < goals_coords.size(); i++) { + distance_heuristic = std::min( + distance_heuristic, + getDistanceHeuristic(node_coords, goals_coords[i], obstacle_heuristic)); + } + return std::max(obstacle_heuristic, distance_heuristic); } void NodeHybrid::initMotionModel( @@ -815,11 +817,6 @@ void NodeHybrid::precomputeDistanceHeuristic( const unsigned int & dim_3_size, const SearchInfo & search_info) { - if (goal_heading_mode == GoalHeadingMode::ALL_DIRECTION) { - // For all direction goal heading, we only consider the obstacle heuristic - // it does not make sense to precompute the distance heuristic to save computation - return; - } // Dubin or Reeds-Shepp shortest distances if (motion_model == MotionModel::DUBIN) { motion_table.state_space = std::make_shared( @@ -856,13 +853,6 @@ void NodeHybrid::precomputeDistanceHeuristic( from[1] = y; from[2] = heading * angular_bin_size; motion_heuristic = motion_table.state_space->distance(from(), to()); - // we need to check for bidirectional goal heading and take the minimum - if (goal_heading_mode == GoalHeadingMode::BIDIRECTIONAL) { - unsigned int heading_180_offset = (heading + (dim_3_size_int / 2)) % dim_3_size_int; - from[2] = heading_180_offset * angular_bin_size; - float motion_heuristic_180 = motion_table.state_space->distance(from(), to()); - motion_heuristic = std::min(motion_heuristic, motion_heuristic_180); - } dist_heuristic_lookup_table[index] = motion_heuristic; index++; } @@ -930,8 +920,4 @@ bool NodeHybrid::backtracePath(CoordinateVector & path) return true; } -void NodeHybrid::setGoalHeadingMode(const GoalHeadingMode & current_goal_heading_mode) -{ - goal_heading_mode = current_goal_heading_mode; -} } // namespace nav2_smac_planner diff --git a/nav2_smac_planner/src/node_lattice.cpp b/nav2_smac_planner/src/node_lattice.cpp index 90ef8301464..6acaf9dcb26 100644 --- a/nav2_smac_planner/src/node_lattice.cpp +++ b/nav2_smac_planner/src/node_lattice.cpp @@ -38,7 +38,6 @@ namespace nav2_smac_planner LatticeMotionTable NodeLattice::motion_table; float NodeLattice::size_lookup = 25; LookupTable NodeLattice::dist_heuristic_lookup_table; -GoalHeadingMode NodeLattice::goal_heading_mode; // Each of these tables are the projected motion models through // time and space applied to the search on the current node in @@ -328,16 +327,18 @@ float NodeLattice::getTraversalCost(const NodePtr & child) float NodeLattice::getHeuristicCost( const Coordinates & node_coords, - const Coordinates & goal_coords) + const CoordinateVector & goals_coords) { // get obstacle heuristic value + // obstacle heuristic does not depend on goal heading const float obstacle_heuristic = getObstacleHeuristic( - node_coords, goal_coords, motion_table.cost_penalty); - if (goal_heading_mode == GoalHeadingMode::ALL_DIRECTION) { - return obstacle_heuristic; + node_coords, goals_coords[0], motion_table.cost_penalty); + float distance_heuristic = std::numeric_limits::max(); + for (unsigned int i = 0; i < goals_coords.size(); i++) { + distance_heuristic = std::min( + distance_heuristic, + getDistanceHeuristic(node_coords, goals_coords[i], obstacle_heuristic)); } - const float distance_heuristic = - getDistanceHeuristic(node_coords, goal_coords, obstacle_heuristic); return std::max(obstacle_heuristic, distance_heuristic); } @@ -430,11 +431,6 @@ void NodeLattice::precomputeDistanceHeuristic( const unsigned int & dim_3_size, const SearchInfo & search_info) { - if (goal_heading_mode == GoalHeadingMode::ALL_DIRECTION) { - // For all direction goal heading, we only consider the obstacle heuristic - // it does not make sense to precompute the distance heuristic to save computation - return; - } // Dubin or Reeds-Shepp shortest distances if (!search_info.allow_reverse_expansion) { motion_table.state_space = std::make_shared( @@ -470,13 +466,6 @@ void NodeLattice::precomputeDistanceHeuristic( from[1] = y; from[2] = motion_table.getAngleFromBin(heading); motion_heuristic = motion_table.state_space->distance(from(), to()); - // we need to check for bidirectional goal heading and take the minimum - if (goal_heading_mode == GoalHeadingMode::BIDIRECTIONAL) { - unsigned int heading_180_offset = (heading + (dim_3_size_int / 2)) % dim_3_size_int; - from[2] = motion_table.getAngleFromBin(heading_180_offset); - float motion_heuristic_180 = motion_table.state_space->distance(from(), to()); - motion_heuristic = std::min(motion_heuristic, motion_heuristic_180); - } dist_heuristic_lookup_table[index] = motion_heuristic; index++; } @@ -609,9 +598,4 @@ void NodeLattice::addNodeToPath( } } -void NodeLattice::setGoalHeadingMode(const GoalHeadingMode & current_goal_heading_mode) -{ - goal_heading_mode = current_goal_heading_mode; -} - } // namespace nav2_smac_planner diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index 6c2da7bd160..6e3f279a72a 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -247,8 +247,7 @@ void SmacPlannerHybrid::configure( _terminal_checking_interval, _max_planning_time, _lookup_table_dim, - _angle_quantizations, - _goal_heading_mode); + _angle_quantizations); // Initialize path smoother if (smooth_path) { @@ -402,7 +401,7 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( orientation_bin -= static_cast(_angle_quantizations); } orientation_bin_id = static_cast(floor(orientation_bin)); - _a_star->setGoal(mx, my, orientation_bin_id); + _a_star->setGoal(mx, my, orientation_bin_id, _goal_heading_mode); // Setup message nav_msgs::msg::Path plan; @@ -674,7 +673,6 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para "Valid options are DEFAULT, BIDIRECTIONAL, ALL_DIRECTION. ", goal_heading_type.c_str()); } else { - reinit_a_star = true; _goal_heading_mode = goal_heading_mode; } } @@ -717,8 +715,7 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para _terminal_checking_interval, _max_planning_time, _lookup_table_dim, - _angle_quantizations, - _goal_heading_mode); + _angle_quantizations); } // Re-Initialize costmap downsampler diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index 255d98173a5..30a7e8bc314 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -213,8 +213,7 @@ void SmacPlannerLattice::configure( _terminal_checking_interval, _max_planning_time, lookup_table_dim, - _metadata.number_of_headings, - _goal_heading_mode); + _metadata.number_of_headings); // Initialize path smoother if (smooth_path) { @@ -314,7 +313,8 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( } _a_star->setGoal( mx, my, - NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(goal.pose.orientation))); + NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(goal.pose.orientation)), + _goal_heading_mode); // Setup message nav_msgs::msg::Path plan; @@ -565,7 +565,6 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector par "Valid options are DEFAULT, BIDIRECTIONAL, ALL_DIRECTION. ", goal_heading_type.c_str()); } else { - reinit_a_star = true; _goal_heading_mode = goal_heading_mode; } } @@ -612,8 +611,7 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector par _terminal_checking_interval, _max_planning_time, lookup_table_dim, - _metadata.number_of_headings, - _goal_heading_mode); + _metadata.number_of_headings); } } diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp index b5715a7ced9..021bc1be46e 100644 --- a/nav2_smac_planner/test/test_a_star.cpp +++ b/nav2_smac_planner/test/test_a_star.cpp @@ -376,7 +376,7 @@ TEST(AStarTest, test_goal_heading_mode) // BIDIRECTIONAL goal heading mode a_star.initialize( false, max_iterations, it_on_approach, terminal_checking_interval, - max_planning_time, 401, size_theta, nav2_smac_planner::GoalHeadingMode::BIDIRECTIONAL); + max_planning_time, 401, size_theta); nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0); @@ -405,13 +405,13 @@ TEST(AStarTest, test_goal_heading_mode) EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, dummy_cancel_checker, expansions.get())); EXPECT_EQ(a_star.getGoals().size(), 2); - EXPECT_EQ(a_star.getInitialGoalCoordinate().theta, 40u); + EXPECT_EQ(a_star.getGoals().size(), a_star.getGoalsCoordinates().size()); // ALL_DIRECTION goal heading mode a_star.initialize( false, max_iterations, it_on_approach, terminal_checking_interval, - max_planning_time, 401, size_theta, nav2_smac_planner::GoalHeadingMode::ALL_DIRECTION); + max_planning_time, 401, size_theta); a_star.setCollisionChecker(checker.get()); a_star.setStart(10u, 10u, 0u); @@ -419,10 +419,9 @@ TEST(AStarTest, test_goal_heading_mode) unsigned int num_bins = nav2_smac_planner::NodeHybrid::motion_table.num_angle_quantization; EXPECT_TRUE(a_star.getGoals().size() == num_bins); + EXPECT_EQ(a_star.getGoals().size(), a_star.getGoalsCoordinates().size()); // first goal should be the same the one set by the user - EXPECT_EQ(a_star.getInitialGoalCoordinate().theta, 40u); EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, dummy_cancel_checker, expansions.get())); - } TEST(AStarTest, test_constants) From ea2c921b6cc4b138099ce3bac120c72f0f5abf83 Mon Sep 17 00:00:00 2001 From: stevedan Date: Tue, 16 Apr 2024 22:08:38 +0200 Subject: [PATCH 08/41] Improve ways in which we handle distance heuritis. Signed-off-by: stevedan --- .../include/nav2_smac_planner/a_star.hpp | 5 +++-- .../nav2_smac_planner/analytic_expansion.hpp | 3 +-- .../include/nav2_smac_planner/node_hybrid.hpp | 4 +--- .../include/nav2_smac_planner/node_lattice.hpp | 1 - nav2_smac_planner/src/a_star.cpp | 16 ++++++---------- nav2_smac_planner/test/test_node2d.cpp | 4 +++- 6 files changed, 14 insertions(+), 19 deletions(-) diff --git a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp index 05e3f358c49..f7c8b813fa3 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -170,9 +170,9 @@ class AStarAlgorithm /** * @brief Get pointer reference to goals coordinates - * @return vector of goals coordinates + * @return vector of goals coordinates reference to the goals coordinates */ - CoordinateVector & goalGoalsCoordinates(); + CoordinateVector & getGoalsCoordinates(); /** * @brief Get maximum number of on-approach iterations after within threshold @@ -267,6 +267,7 @@ class AStarAlgorithm */ void clearStart(); + bool _traverse_unknown; int _max_iterations; int _max_on_approach_iterations; diff --git a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp index ee54573edb7..96fa2a2ed1a 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp @@ -82,8 +82,7 @@ class AnalyticExpansion * @brief Attempt an analytic path completion * @param node The node to start the analytic path from * @param goals_node set of goals node to plan to - * @param initial_goal_coords The coordinates of the initial goal node before goal heading - * was applied + * @param goals_coords vector of goal coordinates to plan to * @param getter Gets a node at a set of coordinates * @param iterations Iterations to run over * @param best_cost Best heuristic cost to propertionally expand more closer to the goal diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp index 134ecdb737d..d16485ea31f 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp @@ -463,16 +463,14 @@ class NodeHybrid Coordinates pose; // Constants required across all nodes but don't want to allocate more than once - static double travel_distance_cost; + static float travel_distance_cost; static HybridMotionTable motion_table; // Wavefront lookup and queue for continuing to expand as needed static LookupTable obstacle_heuristic_lookup_table; static ObstacleHeuristicQueue obstacle_heuristic_queue; - static nav2_costmap_2d::Costmap2D * sampled_costmap; static std::shared_ptr costmap_ros; static std::shared_ptr inflation_layer; - static CostmapDownsampler downsampler; // Dubin / Reeds-Shepp lookup and size for dereferencing static LookupTable dist_heuristic_lookup_table; static float size_lookup; diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp index 2337d709f16..3261725c714 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp @@ -415,7 +415,6 @@ class NodeLattice */ void addNodeToPath(NodePtr current_node, CoordinateVector & path); - NodeLattice * parent; Coordinates pose; static LatticeMotionTable motion_table; diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index 327ef68abb4..d93352e41e9 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -70,8 +70,7 @@ void AStarAlgorithm::initialize( _max_on_approach_iterations = max_on_approach_iterations; _terminal_checking_interval = terminal_checking_interval; _max_planning_time = max_planning_time; - NodeT::precomputeDistanceHeuristic( - lookup_table_size, _motion_model, dim_3_size, _search_info); + NodeT::precomputeDistanceHeuristic(lookup_table_size, _motion_model, dim_3_size, _search_info); _dim3_size = dim_3_size; _expander = std::make_unique>( _motion_model, _search_info, _traverse_unknown, _dim3_size); @@ -258,8 +257,8 @@ void AStarAlgorithm::setGoal( // we just have to check whether the x and y are the same because the dim3 is not used // in the computation of the obstacle heuristic if (!_search_info.cache_obstacle_heuristic || - (goals_coordinates[0].x == _goals_coordinates[0].x && - goals_coordinates[0].y == _goals_coordinates[0].y)) + (goals_coordinates[0].x != _goals_coordinates[0].x && + goals_coordinates[0].y != _goals_coordinates[0].y)) { if (!_start) { throw std::runtime_error("Start must be set before goal."); @@ -327,9 +326,6 @@ bool AStarAlgorithm::createPath( return false; } - // compute min distance heuristic to goal - - // 0) Add starting point to the open set addNode(0.0, getStart()); getStart()->setAccumulatedCost(0.0); @@ -394,7 +390,7 @@ bool AStarAlgorithm::createPath( expansion_result = nullptr; expansion_result = _expander->tryAnalyticExpansion( current_node, getGoals(), - goalGoalsCoordinates(), neighborGetter, analytic_iterations, closest_distance); + getGoalsCoordinates(), neighborGetter, analytic_iterations, closest_distance); if (expansion_result != nullptr) { current_node = expansion_result; } @@ -481,7 +477,7 @@ float AStarAlgorithm::getHeuristicCost(const NodePtr & node) { const Coordinates node_coords = NodeT::getCoords(node->getIndex(), getSizeX(), getSizeDim3()); - float heuristic = NodeT::getHeuristicCost(node_coords, goalGoalsCoordinates()); + float heuristic = NodeT::getHeuristicCost(node_coords, getGoalsCoordinates()); if (heuristic < _best_heuristic_node.first) { _best_heuristic_node = {heuristic, node->getIndex()}; } @@ -555,7 +551,7 @@ void AStarAlgorithm::clearStart() } template -typename AStarAlgorithm::CoordinateVector & AStarAlgorithm::goalGoalsCoordinates() +typename AStarAlgorithm::CoordinateVector & AStarAlgorithm::getGoalsCoordinates() { return _goals_coordinates; } diff --git a/nav2_smac_planner/test/test_node2d.cpp b/nav2_smac_planner/test/test_node2d.cpp index 36715948550..b637982c206 100644 --- a/nav2_smac_planner/test/test_node2d.cpp +++ b/nav2_smac_planner/test/test_node2d.cpp @@ -75,8 +75,10 @@ TEST(Node2DTest, test_node_2d) // check heuristic cost computation nav2_smac_planner::Node2D::Coordinates A(0.0, 0.0); + nav2_smac_planner::Node2D::CoordinateVector B_vec; nav2_smac_planner::Node2D::Coordinates B(10.0, 5.0); - EXPECT_NEAR(testB.getHeuristicCost(A, B), 11.18, 0.02); + B_vec.push_back(B); + EXPECT_NEAR(testB.getHeuristicCost(A, B_vec), 11.18, 0.02); // check operator== works on index unsigned char costC = '2'; From c0abe185ecc23edcffe8f7a334e332c2e92e33e0 Mon Sep 17 00:00:00 2001 From: stevedan Date: Thu, 18 Apr 2024 06:40:30 +0200 Subject: [PATCH 09/41] ensure that we remove the correct goal coordiate and split into nodet and node2d Signed-off-by: stevedan --- nav2_smac_planner/src/a_star.cpp | 61 +++++++++++++++++++------- nav2_smac_planner/test/test_a_star.cpp | 4 +- 2 files changed, 47 insertions(+), 18 deletions(-) diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index d93352e41e9..7dcca69965f 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -275,6 +275,20 @@ void AStarAlgorithm::setGoal( } } +template<> +void AStarAlgorithm::clearStart() +{ + auto coords = Node2D::getCoords(_start->getIndex()); + _costmap->setCost(coords.x, coords.y, nav2_costmap_2d::FREE_SPACE); +} + +template +void AStarAlgorithm::clearStart() +{ + auto coords = NodeT::getCoords(_start->getIndex(), _costmap->getSizeInCellsX(), getSizeDim3()); + _costmap->setCost(coords.x, coords.y, nav2_costmap_2d::FREE_SPACE); +} + template bool AStarAlgorithm::areInputsValid() { @@ -287,14 +301,16 @@ bool AStarAlgorithm::areInputsValid() if (!_start || _goalsSet.empty()) { throw std::runtime_error("Failed to compute path, no valid start or goal given."); } - // Check if ending point is valid if (getToleranceHeuristic() < 0.001) { // if a node is not valid, prune it from the goals set for (auto it = _goalsSet.begin(); it != _goalsSet.end(); ) { if (!(*it)->isNodeValid(_traverse_unknown, _collision_checker)) { + _goals_coordinates.erase( + std::remove( + _goals_coordinates.begin(), _goals_coordinates.end(), (*it)->pose), + _goals_coordinates.end()); it = _goalsSet.erase(it); - _goals_coordinates.erase(_goals_coordinates.begin() + std::distance(_goalsSet.begin(), it)); } else { ++it; } @@ -310,6 +326,33 @@ bool AStarAlgorithm::areInputsValid() return true; } +template<> +bool AStarAlgorithm::areInputsValid() +{ + // Check if graph was filled in + if (_graph.empty()) { + throw std::runtime_error("Failed to compute path, no costmap given."); + } + + // Check if points were filled in + if (!_start || _goalsSet.empty()) { + throw std::runtime_error("Failed to compute path, no valid start or goal given."); + } + // Check if ending point is valid + if (getToleranceHeuristic() < 0.001 && + !(*(_goalsSet.begin()))->isNodeValid(_traverse_unknown, _collision_checker)) + { + // if a node is not valid, prune it from the goals set + throw nav2_core::GoalOccupied("Goal was in lethal cost"); + } + + // Note: We do not check the if the start is valid because it is cleared + clearStart(); + + return true; +} + + template bool AStarAlgorithm::createPath( CoordinateVector & path, int & iterations, @@ -536,20 +579,6 @@ unsigned int & AStarAlgorithm::getSizeDim3() return _dim3_size; } -template<> -void AStarAlgorithm::clearStart() -{ - auto coords = Node2D::getCoords(_start->getIndex()); - _costmap->setCost(coords.x, coords.y, nav2_costmap_2d::FREE_SPACE); -} - -template -void AStarAlgorithm::clearStart() -{ - auto coords = NodeT::getCoords(_start->getIndex(), _costmap->getSizeInCellsX(), getSizeDim3()); - _costmap->setCost(coords.x, coords.y, nav2_costmap_2d::FREE_SPACE); -} - template typename AStarAlgorithm::CoordinateVector & AStarAlgorithm::getGoalsCoordinates() { diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp index 021bc1be46e..c0dfa946ed3 100644 --- a/nav2_smac_planner/test/test_a_star.cpp +++ b/nav2_smac_planner/test/test_a_star.cpp @@ -393,7 +393,7 @@ TEST(AStarTest, test_goal_heading_mode) a_star.setCollisionChecker(checker.get()); a_star.setStart(10u, 10u, 0u); - a_star.setGoal(80u, 80u, 40u); + a_star.setGoal(80u, 80u, 40u, nav2_smac_planner::GoalHeadingMode::BIDIRECTIONAL); nav2_smac_planner::NodeHybrid::CoordinateVector path; std::unique_ptr>> expansions = nullptr; expansions = std::make_unique>>(); @@ -415,7 +415,7 @@ TEST(AStarTest, test_goal_heading_mode) a_star.setCollisionChecker(checker.get()); a_star.setStart(10u, 10u, 0u); - a_star.setGoal(80u, 80u, 40u); + a_star.setGoal(80u, 80u, 40u, nav2_smac_planner::GoalHeadingMode::ALL_DIRECTION); unsigned int num_bins = nav2_smac_planner::NodeHybrid::motion_table.num_angle_quantization; EXPECT_TRUE(a_star.getGoals().size() == num_bins); From 3675d4826406709193a5dfe7c2febb60a6ff20b7 Mon Sep 17 00:00:00 2001 From: stevedan Date: Tue, 23 Apr 2024 20:43:42 +0200 Subject: [PATCH 10/41] removing param Signed-off-by: stevedan --- nav2_smac_planner/include/nav2_smac_planner/a_star.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp index f7c8b813fa3..c6a3b531408 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -96,7 +96,6 @@ class AStarAlgorithm * false after this timeout * @param lookup_table_size Size of the lookup table to store heuristic values * @param dim_3_size Number of quantization bins - * @param goal_heading_mode Heading mode for goal heading */ void initialize( const bool & allow_unknown, From eff8859c55309eb8be009352c338a19a992b22bd Mon Sep 17 00:00:00 2001 From: stevedan Date: Wed, 8 May 2024 22:42:45 +0200 Subject: [PATCH 11/41] increase test coverage Signed-off-by: stevedan --- nav2_smac_planner/test/test_a_star.cpp | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp index c0dfa946ed3..5e44911fb54 100644 --- a/nav2_smac_planner/test/test_a_star.cpp +++ b/nav2_smac_planner/test/test_a_star.cpp @@ -392,6 +392,12 @@ TEST(AStarTest, test_goal_heading_mode) checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); a_star.setCollisionChecker(checker.get()); + + EXPECT_THROW( + a_star.setGoal( + 80u, 80u, 40u, + nav2_smac_planner::GoalHeadingMode::BIDIRECTIONAL), + std::runtime_error); a_star.setStart(10u, 10u, 0u); a_star.setGoal(80u, 80u, 40u, nav2_smac_planner::GoalHeadingMode::BIDIRECTIONAL); nav2_smac_planner::NodeHybrid::CoordinateVector path; @@ -409,10 +415,6 @@ TEST(AStarTest, test_goal_heading_mode) // ALL_DIRECTION goal heading mode - a_star.initialize( - false, max_iterations, it_on_approach, terminal_checking_interval, - max_planning_time, 401, size_theta); - a_star.setCollisionChecker(checker.get()); a_star.setStart(10u, 10u, 0u); a_star.setGoal(80u, 80u, 40u, nav2_smac_planner::GoalHeadingMode::ALL_DIRECTION); @@ -420,8 +422,15 @@ TEST(AStarTest, test_goal_heading_mode) unsigned int num_bins = nav2_smac_planner::NodeHybrid::motion_table.num_angle_quantization; EXPECT_TRUE(a_star.getGoals().size() == num_bins); EXPECT_EQ(a_star.getGoals().size(), a_star.getGoalsCoordinates().size()); - // first goal should be the same the one set by the user EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, dummy_cancel_checker, expansions.get())); + + // UNKNOWN goal heading mode + a_star.setCollisionChecker(checker.get()); + a_star.setStart(10u, 10u, 0u); + EXPECT_THROW( + a_star.setGoal( + 80u, 80u, 40u, + nav2_smac_planner::GoalHeadingMode::UNKNOWN), std::runtime_error); } TEST(AStarTest, test_constants) From 1a34ea3f3789aa66955a389b8db17acef95a836b Mon Sep 17 00:00:00 2001 From: stevedan Date: Tue, 14 May 2024 23:18:27 +0200 Subject: [PATCH 12/41] Improvement based on review Signed-off-by: stevedan --- .../include/nav2_smac_planner/node_2d.hpp | 20 +++++++ nav2_smac_planner/src/a_star.cpp | 52 ++++--------------- 2 files changed, 30 insertions(+), 42 deletions(-) diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp index 853883184cb..a6f86ef7693 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp @@ -54,6 +54,16 @@ class Node2D : x(x_in), y(y_in) {} + inline bool operator==(const Coordinates & rhs) const + { + return this->x == rhs.x && this->y == rhs.y; + } + + inline bool operator!=(const Coordinates & rhs) const + { + return !(*this == rhs); + } + float x, y; }; typedef std::vector CoordinateVector; @@ -79,6 +89,15 @@ class Node2D return this->_index == rhs._index; } + /** + * @brief setting continuous coordinate search poses (in partial-cells) + * @param Pose pose + */ + inline void setPose(const Coordinates & pose_in) + { + pose = pose_in; + } + /** * @brief Reset method for new search */ @@ -268,6 +287,7 @@ class Node2D bool backtracePath(CoordinateVector & path); Node2D * parent; + Coordinates pose; static float cost_travel_multiplier; static std::vector _neighbors_grid_offsets; diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index 293ccfcb616..7f3216ea1c7 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -197,7 +197,9 @@ void AStarAlgorithm::setGoal( _goals_coordinates.clear(); _goalsSet.clear(); _goalsSet.insert(addToGraph(Node2D::getIndex(mx, my, getSizeX()))); - _goals_coordinates.push_back(Node2D::Coordinates(mx, my)); + Node2D::Coordinates goal_coords = Node2D::Coordinates(mx, my); + (*_goalsSet.begin())->setPose(goal_coords); + _goals_coordinates.push_back(goal_coords); } template @@ -275,20 +277,6 @@ void AStarAlgorithm::setGoal( } } -template<> -void AStarAlgorithm::clearStart() -{ - auto coords = Node2D::getCoords(_start->getIndex()); - _costmap->setCost(coords.x, coords.y, nav2_costmap_2d::FREE_SPACE); -} - -template -void AStarAlgorithm::clearStart() -{ - auto coords = NodeT::getCoords(_start->getIndex(), _costmap->getSizeInCellsX(), getSizeDim3()); - _costmap->setCost(coords.x, coords.y, nav2_costmap_2d::FREE_SPACE); -} - template bool AStarAlgorithm::areInputsValid() { @@ -326,33 +314,6 @@ bool AStarAlgorithm::areInputsValid() return true; } -template<> -bool AStarAlgorithm::areInputsValid() -{ - // Check if graph was filled in - if (_graph.empty()) { - throw std::runtime_error("Failed to compute path, no costmap given."); - } - - // Check if points were filled in - if (!_start || _goalsSet.empty()) { - throw std::runtime_error("Failed to compute path, no valid start or goal given."); - } - // Check if ending point is valid - if (getToleranceHeuristic() < 0.001 && - !(*(_goalsSet.begin()))->isNodeValid(_traverse_unknown, _collision_checker)) - { - // if a node is not valid, prune it from the goals set - throw nav2_core::GoalOccupied("Goal was in lethal cost"); - } - - // Note: We do not check the if the start is valid because it is cleared - clearStart(); - - return true; -} - - template bool AStarAlgorithm::createPath( CoordinateVector & path, int & iterations, @@ -587,6 +548,13 @@ typename AStarAlgorithm::CoordinateVector & AStarAlgorithm::getGoa return _goals_coordinates; } +template +void AStarAlgorithm::clearStart() +{ + auto coords = NodeT::getCoords(_start->getIndex(), _costmap->getSizeInCellsX(), getSizeDim3()); + _costmap->setCost(coords.x, coords.y, nav2_costmap_2d::FREE_SPACE); +} + // Instantiate algorithm for the supported template types template class AStarAlgorithm; template class AStarAlgorithm; From 05262f50a124c3d3ea9eed5f13857ed0101dfc49 Mon Sep 17 00:00:00 2001 From: stevedan Date: Tue, 8 Oct 2024 23:46:20 +0200 Subject: [PATCH 13/41] fix bidirectional goal Signed-off-by: stevedan --- nav2_bringup/params/nav2_params.yaml | 253 ++++++++- nav2_bringup/params/nav2_params.yaml.backup | 431 +++++++++++++++ nav2_bringup/params/nav2_params_default.yaml | 510 ++++++++++++++++++ nav2_bringup/rviz/nav2_default_view.rviz | 4 +- nav2_rviz_plugins/CMakeLists.txt | 1 + nav2_rviz_plugins/package.xml | 1 + nav2_rviz_plugins/src/costmap_cost_tool.cpp | 2 +- .../include/nav2_smac_planner/constants.hpp | 10 +- nav2_smac_planner/src/a_star.cpp | 10 +- nav2_smac_planner/src/analytic_expansion.cpp | 6 +- nav2_smac_planner/src/smac_planner_hybrid.cpp | 3 +- tools/planner_benchmarking/metrics.py | 2 +- tools/planner_benchmarking/planners.pickle | Bin 0 -> 232 bytes 13 files changed, 1210 insertions(+), 23 deletions(-) create mode 100644 nav2_bringup/params/nav2_params.yaml.backup create mode 100644 nav2_bringup/params/nav2_params_default.yaml create mode 100644 tools/planner_benchmarking/planners.pickle diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 2027f4c0e1c..36d662fc754 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -276,14 +276,265 @@ map_saver: planner_server: ros__parameters: expected_planner_frequency: 20.0 - planner_plugins: ["GridBased"] + planner_plugins: ["GridBased", "DEFAULT_Smac2d", "BIDIRECTIONAL_Smac2d", "ALL_DIRECTION_Smac2d", "DEFAULT_SmacHybrid", "BIDIRECTIONAL_SmacHybrid", "ALL_DIRECTION_SmacHybrid", "DEFAULT_SmacLattice", "BIDIRECTIONAL_SmacLattice", "ALL_DIRECTION_SmacLattice"] costmap_update_timeout: 1.0 GridBased: plugin: "nav2_navfn_planner::NavfnPlanner" tolerance: 0.5 use_astar: false allow_unknown: true + DEFAULT_Smac2d: + plugin: "nav2_smac_planner::SmacPlanner2D" # In Iron and older versions, "/" was used instead of "::" + tolerance: 0.125 # tolerance for planning if unable to reach exact pose, in meters + downsample_costmap: false # whether or not to downsample the map + downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm) + allow_unknown: true # allow traveling in unknown space + max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable + max_on_approach_iterations: 1000 # maximum number of iterations to attempt to reach goal once in tolerance + max_planning_time: 2.0 # max time in s for planner to plan, smooth + cost_travel_multiplier: 2.0 # Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*. + use_final_approach_orientation: false # Whether to set the final path pose at the goal's orientation to the requested orientation (false) or in line with the approach angle so the robot doesn't rotate to heading (true) + smoother: + max_iterations: 1000 + w_smooth: 0.3 + w_data: 0.2 + tolerance: 1.0e-10 + BIDIRECTIONAL_Smac2d: + plugin: "nav2_smac_planner::SmacPlanner2D" # In Iron and older versions, "/" was used instead of "::" + tolerance: 0.125 # tolerance for planning if unable to reach exact pose, in meters + downsample_costmap: false # whether or not to downsample the map + downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm) + allow_unknown: true # allow traveling in unknown space + max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable + max_on_approach_iterations: 1000 # maximum number of iterations to attempt to reach goal once in tolerance + max_planning_time: 2.0 # max time in s for planner to plan, smooth + cost_travel_multiplier: 2.0 # Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*. + use_final_approach_orientation: false # Whether to set the final path pose at the goal's orientation to the requested orientation (false) or in line with the approach angle so the robot doesn't rotate to heading (true) + goal_heading_mode: "BIDIRECTIONAL" + smoother: + max_iterations: 1000 + w_smooth: 0.3 + w_data: 0.2 + tolerance: 1.0e-10 + ALL_DIRECTION_Smac2d: + plugin: "nav2_smac_planner::SmacPlanner2D" # In Iron and older versions, "/" was used instead of "::" + tolerance: 0.125 # tolerance for planning if unable to reach exact pose, in meters + downsample_costmap: false # whether or not to downsample the map + downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm) + allow_unknown: true # allow traveling in unknown space + max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable + max_on_approach_iterations: 1000 # maximum number of iterations to attempt to reach goal once in tolerance + max_planning_time: 2.0 # max time in s for planner to plan, smooth + cost_travel_multiplier: 2.0 # Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*. + use_final_approach_orientation: false # Whether to set the final path pose at the goal's orientation to the requested orientation (false) or in line with the approach angle so the robot doesn't rotate to heading (true) + goal_heading_mode: "ALL_DIRECTIONAL" + smoother: + max_iterations: 1000 + w_smooth: 0.3 + w_data: 0.2 + tolerance: 1.0e-10 + + DEFAULT_SmacHybrid: + plugin: "nav2_smac_planner::SmacPlannerHybrid" # In Iron and older versions, "/" was used instead of "::" + downsample_costmap: false # whether or not to downsample the map + downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm) + tolerance: 0.25 # dist-to-goal heuristic cost (distance) for valid tolerance endpoints if exact goal cannot be found. + allow_unknown: true # allow traveling in unknown space + max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable + max_on_approach_iterations: 1000 # Maximum number of iterations after within tolerances to continue to try to find exact solution + max_planning_time: 5.0 # max time in s for planner to plan, smooth + motion_model_for_search: "DUBIN" # Hybrid-A* Dubin, Redds-Shepp + angle_quantization_bins: 72 # Number of angle bins for search + analytic_expansion_ratio: 3.5 # The ratio to attempt analytic expansions during search for final approach. + analytic_expansion_max_length: 3.0 # For Hybrid/Lattice nodes: The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting + analytic_expansion_max_cost: 200.0 # The maximum single cost for any part of an analytic expansion to contain and be valid, except when necessary on approach to goal + analytic_expansion_max_cost_override: false # Whether or not to override the maximum cost setting if within critical distance to goal (ie probably required) + minimum_turning_radius: 0.40 # minimum turning radius in m of path / vehicle + reverse_penalty: 2.0 # Penalty to apply if motion is reversing, must be => 1 + change_penalty: 0.0 # Penalty to apply if motion is changing directions (L to R), must be >= 0 + non_straight_penalty: 1.2 # Penalty to apply if motion is non-straight, must be => 1 + cost_penalty: 2.0 # Penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable. + retrospective_penalty: 0.015 + lookup_table_size: 20.0 # Size of the dubin/reeds-sheep distance window to cache, in meters. + cache_obstacle_heuristic: false # Cache the obstacle map dynamic programming distance expansion heuristic between subsequent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static. + debug_visualizations: false # For Hybrid nodes: Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning) and the path's footprints on the /planned_footprints topic. WARNING: heavy to compute and to display, for debug only as it degrades the performance. + use_quadratic_cost_penalty: False + downsample_obstacle_heuristic: True + allow_primitive_interpolation: False + # goal_heading_mode: "BIDIRECTIONAL" + smooth_path: True # If true, does a simple and quick smoothing post-processing to the path + smoother: + max_iterations: 1000 + w_smooth: 0.3 + w_data: 0.2 + tolerance: 1.0e-10 + do_refinement: true + refinement_num: 2 + BIDIRECTIONAL_SmacHybrid: + plugin: "nav2_smac_planner::SmacPlannerHybrid" # In Iron and older versions, "/" was used instead of "::" + downsample_costmap: false # whether or not to downsample the map + downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm) + tolerance: 0.25 # dist-to-goal heuristic cost (distance) for valid tolerance endpoints if exact goal cannot be found. + allow_unknown: true # allow traveling in unknown space + max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable + max_on_approach_iterations: 1000 # Maximum number of iterations after within tolerances to continue to try to find exact solution + max_planning_time: 5.0 # max time in s for planner to plan, smooth + motion_model_for_search: "DUBIN" # Hybrid-A* Dubin, Redds-Shepp + angle_quantization_bins: 72 # Number of angle bins for search + analytic_expansion_ratio: 3.5 # The ratio to attempt analytic expansions during search for final approach. + analytic_expansion_max_length: 3.0 # For Hybrid/Lattice nodes: The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting + analytic_expansion_max_cost: 200.0 # The maximum single cost for any part of an analytic expansion to contain and be valid, except when necessary on approach to goal + analytic_expansion_max_cost_override: false # Whether or not to override the maximum cost setting if within critical distance to goal (ie probably required) + minimum_turning_radius: 0.40 # minimum turning radius in m of path / vehicle + reverse_penalty: 2.0 # Penalty to apply if motion is reversing, must be => 1 + change_penalty: 0.0 # Penalty to apply if motion is changing directions (L to R), must be >= 0 + non_straight_penalty: 1.2 # Penalty to apply if motion is non-straight, must be => 1 + cost_penalty: 2.0 # Penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable. + retrospective_penalty: 0.015 + lookup_table_size: 20.0 # Size of the dubin/reeds-sheep distance window to cache, in meters. + cache_obstacle_heuristic: false # Cache the obstacle map dynamic programming distance expansion heuristic between subsequent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static. + debug_visualizations: false # For Hybrid nodes: Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning) and the path's footprints on the /planned_footprints topic. WARNING: heavy to compute and to display, for debug only as it degrades the performance. + use_quadratic_cost_penalty: False + downsample_obstacle_heuristic: True + allow_primitive_interpolation: False + goal_heading_mode: "BIDIRECTIONAL" + smooth_path: True # If true, does a simple and quick smoothing post-processing to the path + + smoother: + max_iterations: 1000 + w_smooth: 0.3 + w_data: 0.2 + tolerance: 1.0e-10 + do_refinement: true + refinement_num: 2 + ALL_DIRECTION_SmacHybrid: + plugin: "nav2_smac_planner::SmacPlannerHybrid" # In Iron and older versions, "/" was used instead of "::" + downsample_costmap: false # whether or not to downsample the map + downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm) + tolerance: 0.25 # dist-to-goal heuristic cost (distance) for valid tolerance endpoints if exact goal cannot be found. + allow_unknown: true # allow traveling in unknown space + max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable + max_on_approach_iterations: 1000 # Maximum number of iterations after within tolerances to continue to try to find exact solution + max_planning_time: 5.0 # max time in s for planner to plan, smooth + motion_model_for_search: "DUBIN" # Hybrid-A* Dubin, Redds-Shepp + angle_quantization_bins: 72 # Number of angle bins for search + analytic_expansion_ratio: 3.5 # The ratio to attempt analytic expansions during search for final approach. + analytic_expansion_max_length: 3.0 # For Hybrid/Lattice nodes: The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting + analytic_expansion_max_cost: 200.0 # The maximum single cost for any part of an analytic expansion to contain and be valid, except when necessary on approach to goal + analytic_expansion_max_cost_override: false # Whether or not to override the maximum cost setting if within critical distance to goal (ie probably required) + minimum_turning_radius: 0.40 # minimum turning radius in m of path / vehicle + reverse_penalty: 2.0 # Penalty to apply if motion is reversing, must be => 1 + change_penalty: 0.0 # Penalty to apply if motion is changing directions (L to R), must be >= 0 + non_straight_penalty: 1.2 # Penalty to apply if motion is non-straight, must be => 1 + cost_penalty: 2.0 # Penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable. + retrospective_penalty: 0.015 + lookup_table_size: 20.0 # Size of the dubin/reeds-sheep distance window to cache, in meters. + cache_obstacle_heuristic: false # Cache the obstacle map dynamic programming distance expansion heuristic between subsequent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static. + debug_visualizations: false # For Hybrid nodes: Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning) and the path's footprints on the /planned_footprints topic. WARNING: heavy to compute and to display, for debug only as it degrades the performance. + use_quadratic_cost_penalty: False + downsample_obstacle_heuristic: True + allow_primitive_interpolation: False + goal_heading_mode: "ALL_DIRECTION" + smooth_path: True # If true, does a simple and quick smoothing post-processing to the path + + smoother: + max_iterations: 1000 + w_smooth: 0.3 + w_data: 0.2 + tolerance: 1.0e-10 + do_refinement: true + refinement_num: 2 + + DEFAULT_SmacLattice: + plugin: "nav2_smac_planner::SmacPlannerLattice" # In Iron and older versions, "/" was used instead of "::" + allow_unknown: true # Allow traveling in unknown space + tolerance: 0.25 # dist-to-goal heuristic cost (distance) for valid tolerance endpoints if exact goal cannot be found. + max_iterations: 1000000 # Maximum total iterations to search for before failing (in case unreachable), set to -1 to disable + max_on_approach_iterations: 1000 # Maximum number of iterations after within tolerances to continue to try to find exact solution + max_planning_time: 5.0 # Max time in s for planner to plan, smooth + analytic_expansion_ratio: 3.5 # The ratio to attempt analytic expansions during search for final approach. + analytic_expansion_max_length: 3.0 # For Hybrid/Lattice nodes The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting + analytic_expansion_max_cost: 200.0 # The maximum single cost for any part of an analytic expansion to contain and be valid, except when necessary on approach to goal + analytic_expansion_max_cost_override: false # Whether or not to override the maximum cost setting if within critical distance to goal (ie probably required) + reverse_penalty: 2.0 # Penalty to apply if motion is reversing, must be => 1 + change_penalty: 0.05 # Penalty to apply if motion is changing directions (L to R), must be >= 0 + non_straight_penalty: 1.05 # Penalty to apply if motion is non-straight, must be => 1 + cost_penalty: 2.0 # Penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable. + rotation_penalty: 5.0 # Penalty to apply to in-place rotations, if minimum control set contains them + retrospective_penalty: 0.015 + # lattice_filepath: "" # The filepath to the state lattice graph + lookup_table_size: 20.0 # Size of the dubin/reeds-sheep distance window to cache, in meters. + cache_obstacle_heuristic: false # Cache the obstacle map dynamic programming distance expansion heuristic between subsequent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static. + allow_reverse_expansion: false # If true, allows the robot to use the primitives to expand in the mirrored opposite direction of the current robot's orientation (to reverse). + smooth_path: True # If true, does a simple and quick smoothing post-processing to the path + smoother: + max_iterations: 1000 + w_smooth: 0.3 + w_data: 0.2 + tolerance: 1.0e-10 + do_refinement: true + refinement_num: 2 + BIDIRECTIONAL_SmacLattice: + plugin: "nav2_smac_planner::SmacPlannerLattice" # In Iron and older versions, "/" was used instead of "::" + allow_unknown: true # Allow traveling in unknown space + tolerance: 0.25 # dist-to-goal heuristic cost (distance) for valid tolerance endpoints if exact goal cannot be found. + max_iterations: 1000000 # Maximum total iterations to search for before failing (in case unreachable), set to -1 to disable + max_on_approach_iterations: 1000 # Maximum number of iterations after within tolerances to continue to try to find exact solution + max_planning_time: 5.0 # Max time in s for planner to plan, smooth + analytic_expansion_ratio: 3.5 # The ratio to attempt analytic expansions during search for final approach. + analytic_expansion_max_length: 3.0 # For Hybrid/Lattice nodes The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting + analytic_expansion_max_cost: 200.0 # The maximum single cost for any part of an analytic expansion to contain and be valid, except when necessary on approach to goal + analytic_expansion_max_cost_override: false # Whether or not to override the maximum cost setting if within critical distance to goal (ie probably required) + reverse_penalty: 2.0 # Penalty to apply if motion is reversing, must be => 1 + change_penalty: 0.05 # Penalty to apply if motion is changing directions (L to R), must be >= 0 + non_straight_penalty: 1.05 # Penalty to apply if motion is non-straight, must be => 1 + cost_penalty: 2.0 # Penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable. + rotation_penalty: 5.0 # Penalty to apply to in-place rotations, if minimum control set contains them + retrospective_penalty: 0.015 + # lattice_filepath: "" # The filepath to the state lattice graph + lookup_table_size: 20.0 # Size of the dubin/reeds-sheep distance window to cache, in meters. + cache_obstacle_heuristic: false # Cache the obstacle map dynamic programming distance expansion heuristic between subsequent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static. + allow_reverse_expansion: false # If true, allows the robot to use the primitives to expand in the mirrored opposite direction of the current robot's orientation (to reverse). + smooth_path: True # If true, does a simple and quick smoothing post-processing to the path + goal_heading_mode: "BIDIRECTIONAL" + smoother: + max_iterations: 1000 + w_smooth: 0.3 + w_data: 0.2 + tolerance: 1.0e-10 + do_refinement: true + refinement_num: 2 + ALL_DIRECTION_SmacLattice: + plugin: "nav2_smac_planner::SmacPlannerLattice" # In Iron and older versions, "/" was used instead of "::" + allow_unknown: true # Allow traveling in unknown space + tolerance: 0.25 # dist-to-goal heuristic cost (distance) for valid tolerance endpoints if exact goal cannot be found. + max_iterations: 1000000 # Maximum total iterations to search for before failing (in case unreachable), set to -1 to disable + max_on_approach_iterations: 1000 # Maximum number of iterations after within tolerances to continue to try to find exact solution + max_planning_time: 5.0 # Max time in s for planner to plan, smooth + analytic_expansion_ratio: 3.5 # The ratio to attempt analytic expansions during search for final approach. + analytic_expansion_max_length: 3.0 # For Hybrid/Lattice nodes The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting + analytic_expansion_max_cost: 200.0 # The maximum single cost for any part of an analytic expansion to contain and be valid, except when necessary on approach to goal + analytic_expansion_max_cost_override: false # Whether or not to override the maximum cost setting if within critical distance to goal (ie probably required) + reverse_penalty: 2.0 # Penalty to apply if motion is reversing, must be => 1 + change_penalty: 0.05 # Penalty to apply if motion is changing directions (L to R), must be >= 0 + non_straight_penalty: 1.05 # Penalty to apply if motion is non-straight, must be => 1 + cost_penalty: 2.0 # Penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable. + rotation_penalty: 5.0 # Penalty to apply to in-place rotations, if minimum control set contains them + retrospective_penalty: 0.015 + # lattice_filepath: "" # The filepath to the state lattice graph + lookup_table_size: 20.0 # Size of the dubin/reeds-sheep distance window to cache, in meters. + cache_obstacle_heuristic: false # Cache the obstacle map dynamic programming distance expansion heuristic between subsequent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static. + allow_reverse_expansion: false # If true, allows the robot to use the primitives to expand in the mirrored opposite direction of the current robot's orientation (to reverse). + goal_heading_mode: "ALL_DIRECTION" + smooth_path: True # If true, does a simple and quick smoothing post-processing to the path + smoother: + max_iterations: 1000 + w_smooth: 0.3 + w_data: 0.2 + tolerance: 1.0e-10 + do_refinement: true + refinement_num: 2 smoother_server: ros__parameters: smoother_plugins: ["simple_smoother"] diff --git a/nav2_bringup/params/nav2_params.yaml.backup b/nav2_bringup/params/nav2_params.yaml.backup new file mode 100644 index 00000000000..2027f4c0e1c --- /dev/null +++ b/nav2_bringup/params/nav2_params.yaml.backup @@ -0,0 +1,431 @@ +amcl: + ros__parameters: + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + base_frame_id: "base_footprint" + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: false + global_frame_id: "map" + lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 + laser_max_range: 100.0 + laser_min_range: -1.0 + laser_model_type: "likelihood_field" + max_beams: 60 + max_particles: 2000 + min_particles: 500 + odom_frame_id: "odom" + pf_err: 0.05 + pf_z: 0.99 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 1 + robot_model_type: "nav2_amcl::DifferentialMotionModel" + save_pose_rate: 0.5 + sigma_hit: 0.2 + tf_broadcast: true + transform_tolerance: 1.0 + update_min_a: 0.2 + update_min_d: 0.25 + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + scan_topic: scan + +bt_navigator: + ros__parameters: + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + wait_for_service_timeout: 1000 + action_server_result_timeout: 900.0 + navigators: ["navigate_to_pose", "navigate_through_poses"] + navigate_to_pose: + plugin: "nav2_bt_navigator::NavigateToPoseNavigator" + navigate_through_poses: + plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator" + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + + # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). + # Built-in plugins are added automatically + # plugin_lib_names: [] + + error_code_names: + - compute_path_error_code + - follow_path_error_code + +controller_server: + ros__parameters: + controller_frequency: 20.0 + costmap_update_timeout: 0.30 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugins: ["progress_checker"] + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + use_realtime_priority: false + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + FollowPath: + plugin: "nav2_mppi_controller::MPPIController" + time_steps: 56 + model_dt: 0.05 + batch_size: 2000 + ax_max: 3.0 + ax_min: -3.0 + ay_max: 3.0 + az_max: 3.5 + vx_std: 0.2 + vy_std: 0.2 + wz_std: 0.4 + vx_max: 0.5 + vx_min: -0.35 + vy_max: 0.5 + wz_max: 1.9 + iteration_count: 1 + prune_distance: 1.7 + transform_tolerance: 0.1 + temperature: 0.3 + gamma: 0.015 + motion_model: "DiffDrive" + visualize: true + regenerate_noises: true + TrajectoryVisualizer: + trajectory_step: 5 + time_step: 3 + AckermannConstraints: + min_turning_r: 0.2 + critics: [ + "ConstraintCritic", "CostCritic", "GoalCritic", + "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", + "PathAngleCritic", "PreferForwardCritic"] + ConstraintCritic: + enabled: true + cost_power: 1 + cost_weight: 4.0 + GoalCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 1.4 + GoalAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 3.0 + threshold_to_consider: 0.5 + PreferForwardCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 0.5 + CostCritic: + enabled: true + cost_power: 1 + cost_weight: 3.81 + critical_cost: 300.0 + consider_footprint: true + collision_cost: 1000000.0 + near_goal_distance: 1.0 + trajectory_point_step: 2 + PathAlignCritic: + enabled: true + cost_power: 1 + cost_weight: 14.0 + max_path_occupancy_ratio: 0.05 + trajectory_point_step: 4 + threshold_to_consider: 0.5 + offset_from_furthest: 20 + use_path_orientations: false + PathFollowCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + offset_from_furthest: 5 + threshold_to_consider: 1.4 + PathAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 2.0 + offset_from_furthest: 4 + threshold_to_consider: 0.5 + max_angle_to_furthest: 1.0 + mode: 0 + # TwirlingCritic: + # enabled: true + # twirling_cost_power: 1 + # twirling_cost_weight: 10.0 + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.70 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + robot_radius: 0.22 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.7 + always_send_full_costmap: True + +# The yaml_filename does not need to be specified since it going to be set by defaults in launch. +# If you'd rather set it in the yaml, remove the default "map" value in the tb3_simulation_launch.py +# file & provide full path to map below. If CLI map configuration or launch default is provided, that will be used. +# map_server: +# ros__parameters: +# yaml_filename: "" + +map_saver: + ros__parameters: + save_map_timeout: 5.0 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + map_subscribe_transient_local: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + planner_plugins: ["GridBased"] + costmap_update_timeout: 1.0 + GridBased: + plugin: "nav2_navfn_planner::NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +smoother_server: + ros__parameters: + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + local_costmap_topic: local_costmap/costmap_raw + global_costmap_topic: global_costmap/costmap_raw + local_footprint_topic: local_costmap/published_footprint + global_footprint_topic: global_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + spin: + plugin: "nav2_behaviors::Spin" + backup: + plugin: "nav2_behaviors::BackUp" + drive_on_heading: + plugin: "nav2_behaviors::DriveOnHeading" + wait: + plugin: "nav2_behaviors::Wait" + assisted_teleop: + plugin: "nav2_behaviors::AssistedTeleop" + local_frame: odom + global_frame: map + robot_base_frame: base_link + transform_tolerance: 0.1 + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +waypoint_follower: + ros__parameters: + loop_rate: 20 + stop_on_failure: false + action_server_result_timeout: 900.0 + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.5, 0.0, 2.0] + min_velocity: [-0.5, 0.0, -2.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 + +collision_monitor: + ros__parameters: + base_frame_id: "base_footprint" + odom_frame_id: "odom" + cmd_vel_in_topic: "cmd_vel_smoothed" + cmd_vel_out_topic: "cmd_vel" + state_topic: "collision_monitor_state" + transform_tolerance: 0.2 + source_timeout: 1.0 + base_shift_correction: True + stop_pub_timeout: 2.0 + # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, + # and robot footprint for "approach" action type. + polygons: ["FootprintApproach"] + FootprintApproach: + type: "polygon" + action_type: "approach" + footprint_topic: "/local_costmap/published_footprint" + time_before_collision: 1.2 + simulation_time_step: 0.1 + min_points: 6 + visualize: False + enabled: True + observation_sources: ["scan"] + scan: + type: "scan" + topic: "scan" + min_height: 0.15 + max_height: 2.0 + enabled: True + +docking_server: + ros__parameters: + controller_frequency: 50.0 + initial_perception_timeout: 5.0 + wait_charge_timeout: 5.0 + dock_approach_timeout: 30.0 + undock_linear_tolerance: 0.05 + undock_angular_tolerance: 0.1 + max_retries: 3 + base_frame: "base_link" + fixed_frame: "odom" + dock_backwards: false + dock_prestaging_tolerance: 0.5 + + # Types of docks + dock_plugins: ['simple_charging_dock'] + simple_charging_dock: + plugin: 'opennav_docking::SimpleChargingDock' + docking_threshold: 0.05 + staging_x_offset: -0.7 + use_external_detection_pose: true + use_battery_status: false # true + use_stall_detection: false # true + + external_detection_timeout: 1.0 + external_detection_translation_x: -0.18 + external_detection_translation_y: 0.0 + external_detection_rotation_roll: -1.57 + external_detection_rotation_pitch: -1.57 + external_detection_rotation_yaw: 0.0 + filter_coef: 0.1 + + # Dock instances + # The following example illustrates configuring dock instances. + # docks: ['home_dock'] # Input your docks here + # home_dock: + # type: 'simple_charging_dock' + # frame: map + # pose: [0.0, 0.0, 0.0] + + controller: + k_phi: 3.0 + k_delta: 2.0 + v_linear_min: 0.15 + v_linear_max: 0.15 + +loopback_simulator: + ros__parameters: + base_frame_id: "base_footprint" + odom_frame_id: "odom" + map_frame_id: "map" + scan_frame_id: "base_scan" # tb4_loopback_simulator.launch.py remaps to 'rplidar_link' + update_duration: 0.02 diff --git a/nav2_bringup/params/nav2_params_default.yaml b/nav2_bringup/params/nav2_params_default.yaml new file mode 100644 index 00000000000..698e27a69a0 --- /dev/null +++ b/nav2_bringup/params/nav2_params_default.yaml @@ -0,0 +1,510 @@ +amcl: + ros__parameters: + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + base_frame_id: "base_footprint" + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: false + global_frame_id: "map" + lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 + laser_max_range: 100.0 + laser_min_range: -1.0 + laser_model_type: "likelihood_field" + max_beams: 60 + max_particles: 2000 + min_particles: 500 + odom_frame_id: "odom" + pf_err: 0.05 + pf_z: 0.99 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 1 + robot_model_type: "nav2_amcl::DifferentialMotionModel" + save_pose_rate: 0.5 + sigma_hit: 0.2 + tf_broadcast: true + transform_tolerance: 1.0 + update_min_a: 0.2 + update_min_d: 0.25 + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + scan_topic: scan + +bt_navigator: + ros__parameters: + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + wait_for_service_timeout: 1000 + action_server_result_timeout: 900.0 + navigators: ["navigate_to_pose", "navigate_through_poses"] + navigate_to_pose: + plugin: "nav2_bt_navigator::NavigateToPoseNavigator" + navigate_through_poses: + plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator" + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + + # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). + # Built-in plugins are added automatically + # plugin_lib_names: [] + + error_code_names: + - compute_path_error_code + - follow_path_error_code + +controller_server: + ros__parameters: + controller_frequency: 20.0 + costmap_update_timeout: 0.30 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugins: ["progress_checker"] + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + use_realtime_priority: false + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + FollowPath: + plugin: "nav2_mppi_controller::MPPIController" + time_steps: 56 + model_dt: 0.05 + batch_size: 2000 + ax_max: 3.0 + ax_min: -3.0 + ay_max: 3.0 + az_max: 3.5 + vx_std: 0.2 + vy_std: 0.2 + wz_std: 0.4 + vx_max: 0.5 + vx_min: -0.35 + vy_max: 0.5 + wz_max: 1.9 + iteration_count: 1 + prune_distance: 1.7 + transform_tolerance: 0.1 + temperature: 0.3 + gamma: 0.015 + motion_model: "DiffDrive" + visualize: true + regenerate_noises: true + TrajectoryVisualizer: + trajectory_step: 5 + time_step: 3 + AckermannConstraints: + min_turning_r: 0.2 + critics: [ + "ConstraintCritic", "CostCritic", "GoalCritic", + "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", + "PathAngleCritic", "PreferForwardCritic"] + ConstraintCritic: + enabled: true + cost_power: 1 + cost_weight: 4.0 + GoalCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 1.4 + GoalAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 3.0 + threshold_to_consider: 0.5 + PreferForwardCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 0.5 + CostCritic: + enabled: true + cost_power: 1 + cost_weight: 3.81 + critical_cost: 300.0 + consider_footprint: true + collision_cost: 1000000.0 + near_goal_distance: 1.0 + trajectory_point_step: 2 + PathAlignCritic: + enabled: true + cost_power: 1 + cost_weight: 14.0 + max_path_occupancy_ratio: 0.05 + trajectory_point_step: 4 + threshold_to_consider: 0.5 + offset_from_furthest: 20 + use_path_orientations: false + PathFollowCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + offset_from_furthest: 5 + threshold_to_consider: 1.4 + PathAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 2.0 + offset_from_furthest: 4 + threshold_to_consider: 0.5 + max_angle_to_furthest: 1.0 + mode: 0 + # TwirlingCritic: + # enabled: true + # twirling_cost_power: 1 + # twirling_cost_weight: 10.0 + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.70 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + robot_radius: 0.22 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.7 + always_send_full_costmap: True + +# The yaml_filename does not need to be specified since it going to be set by defaults in launch. +# If you'd rather set it in the yaml, remove the default "map" value in the tb3_simulation_launch.py +# file & provide full path to map below. If CLI map configuration or launch default is provided, that will be used. +# map_server: +# ros__parameters: +# yaml_filename: "" + +map_saver: + ros__parameters: + save_map_timeout: 5.0 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + map_subscribe_transient_local: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + planner_plugins: ["Smac2d", "SmacHybrid", "SmacLattice"] + costmap_update_timeout: 1.0 + Smac2d: + plugin: "nav2_smac_planner::SmacPlanner2D" # In Iron and older versions, "/" was used instead of "::" + tolerance: 0.125 # tolerance for planning if unable to reach exact pose, in meters + downsample_costmap: false # whether or not to downsample the map + downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm) + allow_unknown: true # allow traveling in unknown space + max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable + max_on_approach_iterations: 1000 # maximum number of iterations to attempt to reach goal once in tolerance + max_planning_time: 2.0 # max time in s for planner to plan, smooth + cost_travel_multiplier: 2.0 # Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*. + use_final_approach_orientation: false # Whether to set the final path pose at the goal's orientation to the requested orientation (false) or in line with the approach angle so the robot doesn't rotate to heading (true) + smoother: + max_iterations: 1000 + w_smooth: 0.3 + w_data: 0.2 + tolerance: 1.0e-10 + + + SmacHybrid: + plugin: "nav2_smac_planner::SmacPlannerHybrid" # In Iron and older versions, "/" was used instead of "::" + downsample_costmap: false # whether or not to downsample the map + downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm) + tolerance: 0.25 # dist-to-goal heuristic cost (distance) for valid tolerance endpoints if exact goal cannot be found. + allow_unknown: true # allow traveling in unknown space + max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable + max_on_approach_iterations: 1000 # Maximum number of iterations after within tolerances to continue to try to find exact solution + max_planning_time: 5.0 # max time in s for planner to plan, smooth + motion_model_for_search: "DUBIN" # Hybrid-A* Dubin, Redds-Shepp + angle_quantization_bins: 72 # Number of angle bins for search + analytic_expansion_ratio: 3.5 # The ratio to attempt analytic expansions during search for final approach. + analytic_expansion_max_length: 3.0 # For Hybrid/Lattice nodes: The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting + analytic_expansion_max_cost: 200.0 # The maximum single cost for any part of an analytic expansion to contain and be valid, except when necessary on approach to goal + analytic_expansion_max_cost_override: false # Whether or not to override the maximum cost setting if within critical distance to goal (ie probably required) + minimum_turning_radius: 0.40 # minimum turning radius in m of path / vehicle + reverse_penalty: 2.0 # Penalty to apply if motion is reversing, must be => 1 + change_penalty: 0.0 # Penalty to apply if motion is changing directions (L to R), must be >= 0 + non_straight_penalty: 1.2 # Penalty to apply if motion is non-straight, must be => 1 + cost_penalty: 2.0 # Penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable. + retrospective_penalty: 0.015 + lookup_table_size: 20.0 # Size of the dubin/reeds-sheep distance window to cache, in meters. + cache_obstacle_heuristic: false # Cache the obstacle map dynamic programming distance expansion heuristic between subsequent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static. + debug_visualizations: false # For Hybrid nodes: Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning) and the path's footprints on the /planned_footprints topic. WARNING: heavy to compute and to display, for debug only as it degrades the performance. + use_quadratic_cost_penalty: False + downsample_obstacle_heuristic: True + allow_primitive_interpolation: False + # goal_heading_mode: "BIDIRECTIONAL" + smooth_path: True # If true, does a simple and quick smoothing post-processing to the path + + smoother: + max_iterations: 1000 + w_smooth: 0.3 + w_data: 0.2 + tolerance: 1.0e-10 + do_refinement: true + refinement_num: 2 + SmacLattice: + plugin: "nav2_smac_planner::SmacPlannerLattice" # In Iron and older versions, "/" was used instead of "::" + allow_unknown: true # Allow traveling in unknown space + tolerance: 0.25 # dist-to-goal heuristic cost (distance) for valid tolerance endpoints if exact goal cannot be found. + max_iterations: 1000000 # Maximum total iterations to search for before failing (in case unreachable), set to -1 to disable + max_on_approach_iterations: 1000 # Maximum number of iterations after within tolerances to continue to try to find exact solution + max_planning_time: 5.0 # Max time in s for planner to plan, smooth + analytic_expansion_ratio: 3.5 # The ratio to attempt analytic expansions during search for final approach. + analytic_expansion_max_length: 3.0 # For Hybrid/Lattice nodes The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting + analytic_expansion_max_cost: 200.0 # The maximum single cost for any part of an analytic expansion to contain and be valid, except when necessary on approach to goal + analytic_expansion_max_cost_override: false # Whether or not to override the maximum cost setting if within critical distance to goal (ie probably required) + reverse_penalty: 2.0 # Penalty to apply if motion is reversing, must be => 1 + change_penalty: 0.05 # Penalty to apply if motion is changing directions (L to R), must be >= 0 + non_straight_penalty: 1.05 # Penalty to apply if motion is non-straight, must be => 1 + cost_penalty: 2.0 # Penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable. + rotation_penalty: 5.0 # Penalty to apply to in-place rotations, if minimum control set contains them + retrospective_penalty: 0.015 + # lattice_filepath: "" # The filepath to the state lattice graph + lookup_table_size: 20.0 # Size of the dubin/reeds-sheep distance window to cache, in meters. + cache_obstacle_heuristic: false # Cache the obstacle map dynamic programming distance expansion heuristic between subsequent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static. + allow_reverse_expansion: false # If true, allows the robot to use the primitives to expand in the mirrored opposite direction of the current robot's orientation (to reverse). + smooth_path: True # If true, does a simple and quick smoothing post-processing to the path + smoother: + max_iterations: 1000 + w_smooth: 0.3 + w_data: 0.2 + tolerance: 1.0e-10 + do_refinement: true + refinement_num: 2 + +smoother_server: + ros__parameters: + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + local_costmap_topic: local_costmap/costmap_raw + global_costmap_topic: global_costmap/costmap_raw + local_footprint_topic: local_costmap/published_footprint + global_footprint_topic: global_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + spin: + plugin: "nav2_behaviors::Spin" + backup: + plugin: "nav2_behaviors::BackUp" + drive_on_heading: + plugin: "nav2_behaviors::DriveOnHeading" + wait: + plugin: "nav2_behaviors::Wait" + assisted_teleop: + plugin: "nav2_behaviors::AssistedTeleop" + local_frame: odom + global_frame: map + robot_base_frame: base_link + transform_tolerance: 0.1 + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +waypoint_follower: + ros__parameters: + loop_rate: 20 + stop_on_failure: false + action_server_result_timeout: 900.0 + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.5, 0.0, 2.0] + min_velocity: [-0.5, 0.0, -2.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 + +collision_monitor: + ros__parameters: + base_frame_id: "base_footprint" + odom_frame_id: "odom" + cmd_vel_in_topic: "cmd_vel_smoothed" + cmd_vel_out_topic: "cmd_vel" + state_topic: "collision_monitor_state" + transform_tolerance: 0.2 + source_timeout: 1.0 + base_shift_correction: True + stop_pub_timeout: 2.0 + # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, + # and robot footprint for "approach" action type. + polygons: ["FootprintApproach"] + FootprintApproach: + type: "polygon" + action_type: "approach" + footprint_topic: "/local_costmap/published_footprint" + time_before_collision: 1.2 + simulation_time_step: 0.1 + min_points: 6 + visualize: False + enabled: True + observation_sources: ["scan"] + scan: + type: "scan" + topic: "scan" + min_height: 0.15 + max_height: 2.0 + enabled: True + +docking_server: + ros__parameters: + controller_frequency: 50.0 + initial_perception_timeout: 5.0 + wait_charge_timeout: 5.0 + dock_approach_timeout: 30.0 + undock_linear_tolerance: 0.05 + undock_angular_tolerance: 0.1 + max_retries: 3 + base_frame: "base_link" + fixed_frame: "odom" + dock_backwards: false + dock_prestaging_tolerance: 0.5 + + # Types of docks + dock_plugins: ['simple_charging_dock'] + simple_charging_dock: + plugin: 'opennav_docking::SimpleChargingDock' + docking_threshold: 0.05 + staging_x_offset: -0.7 + use_external_detection_pose: true + use_battery_status: false # true + use_stall_detection: false # true + + external_detection_timeout: 1.0 + external_detection_translation_x: -0.18 + external_detection_translation_y: 0.0 + external_detection_rotation_roll: -1.57 + external_detection_rotation_pitch: -1.57 + external_detection_rotation_yaw: 0.0 + filter_coef: 0.1 + + # Dock instances + # The following example illustrates configuring dock instances. + # docks: ['home_dock'] # Input your docks here + # home_dock: + # type: 'simple_charging_dock' + # frame: map + # pose: [0.0, 0.0, 0.0] + + controller: + k_phi: 3.0 + k_delta: 2.0 + v_linear_min: 0.15 + v_linear_max: 0.15 + +loopback_simulator: + ros__parameters: + base_frame_id: "base_footprint" + odom_frame_id: "odom" + map_frame_id: "map" + scan_frame_id: "base_scan" # tb4_loopback_simulator.launch.py remaps to 'rplidar_link' + update_duration: 0.02 diff --git a/nav2_bringup/rviz/nav2_default_view.rviz b/nav2_bringup/rviz/nav2_default_view.rviz index c8dd3768583..0fc84fd890f 100644 --- a/nav2_bringup/rviz/nav2_default_view.rviz +++ b/nav2_bringup/rviz/nav2_default_view.rviz @@ -25,8 +25,8 @@ Panels: Name: Navigation 2 - Class: nav2_rviz_plugins/Selector Name: Selector - - Class: nav2_rviz_plugins/Docking - Name: Docking + # - Class: nav2_rviz_plugins/Docking + # Name: Docking Visualization Manager: Class: "" Displays: diff --git a/nav2_rviz_plugins/CMakeLists.txt b/nav2_rviz_plugins/CMakeLists.txt index bd9059d6989..b03ead1b04f 100644 --- a/nav2_rviz_plugins/CMakeLists.txt +++ b/nav2_rviz_plugins/CMakeLists.txt @@ -21,6 +21,7 @@ find_package(tf2_geometry_msgs REQUIRED) find_package(visualization_msgs REQUIRED) find_package(yaml_cpp_vendor REQUIRED) find_package(yaml-cpp REQUIRED) +find_package(backward_ros REQUIRED) nav2_package() diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index c90944df774..5ae2faafde7 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -27,6 +27,7 @@ tf2_geometry_msgs visualization_msgs yaml_cpp_vendor + backward_ros libqt5-core libqt5-gui diff --git a/nav2_rviz_plugins/src/costmap_cost_tool.cpp b/nav2_rviz_plugins/src/costmap_cost_tool.cpp index 03435a913e4..47d3caa5e07 100644 --- a/nav2_rviz_plugins/src/costmap_cost_tool.cpp +++ b/nav2_rviz_plugins/src/costmap_cost_tool.cpp @@ -138,4 +138,4 @@ void CostmapCostTool::handleGlobalCostResponse( } // namespace nav2_rviz_plugins #include -PLUGINLIB_EXPORT_CLASS(nav2_rviz_plugins::CostmapCostTool, rviz_common::Tool) +PLUGINLIB_EXPORT_CLASS(nav2_rviz_plugins::CostmapCostTool, rviz_common::Tool) \ No newline at end of file diff --git a/nav2_smac_planner/include/nav2_smac_planner/constants.hpp b/nav2_smac_planner/include/nav2_smac_planner/constants.hpp index 9c28b1ab3a0..6344c86fb89 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/constants.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/constants.hpp @@ -94,11 +94,11 @@ inline GoalHeadingMode fromStringToGH(const std::string & n) } } -const float UNKNOWN = 255.0; -const float OCCUPIED = 254.0; -const float INSCRIBED = 253.0; -const float MAX_NON_OBSTACLE = 252.0; -const float FREE = 0; +const float UNKNOWN_COST = 255.0; +const float OCCUPIED_COST = 254.0; +const float INSCRIBED_COST = 253.0; +const float MAX_NON_OBSTACLE_COST = 252.0; +const float FREE_COST = 0; } // namespace nav2_smac_planner diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index f57aaf47caa..679676ac7c2 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -195,6 +195,7 @@ void AStarAlgorithm::setGoal( const unsigned int & dim_3, const GoalHeadingMode & goal_heading_mode) { + (void) goal_heading_mode; if (dim_3 != 0) { throw std::runtime_error("Node type Node2D cannot be given non-zero goal dim 3."); } @@ -232,7 +233,7 @@ void AStarAlgorithm::setGoal( goals.push_back(addToGraph(NodeT::getIndex(mx, my, dim_3))); // 180 degrees dim_3_half_bin = (dim_3 + (num_bins / 2)) % num_bins; - _goalsSet.insert(addToGraph(NodeT::getIndex(mx, my, dim_3_half_bin))); + goals.push_back(addToGraph(NodeT::getIndex(mx, my, dim_3_half_bin))); goals_coordinates.push_back( typename NodeT::Coordinates( static_cast(mx), @@ -550,13 +551,6 @@ typename AStarAlgorithm::CoordinateVector & AStarAlgorithm::getGoa return _goals_coordinates; } -template -void AStarAlgorithm::clearStart() -{ - auto coords = NodeT::getCoords(_start->getIndex(), _costmap->getSizeInCellsX(), getSizeDim3()); - _costmap->setCost(coords.x, coords.y, nav2_costmap_2d::FREE_SPACE); -} - // Instantiate algorithm for the supported template types template class AStarAlgorithm; template class AStarAlgorithm; diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp index a5698f7d5ac..28766eb60da 100644 --- a/nav2_smac_planner/src/analytic_expansion.cpp +++ b/nav2_smac_planner/src/analytic_expansion.cpp @@ -375,9 +375,9 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::setAnalyt template<> typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalyticExpansion( - const NodePtr & current_node, const NodeSet & goals_node, const CoordinateVector & goals_coords, - const NodeGetter & getter, int & analytic_iterations, - int & closest_distance) + const NodePtr &, const NodeSet &, const CoordinateVector &, + const NodeGetter &, int &, + int &) { return NodePtr(nullptr); } diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index e2c3c1e1c82..6ea7709bfed 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -403,8 +403,7 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( if (orientation_bin >= static_cast(_angle_quantizations)) { orientation_bin -= static_cast(_angle_quantizations); } - orientation_bin_id = static_cast(floor(orientation_bin)); - _a_star->setGoal(mx, my, orientation_bin_id, _goal_heading_mode); + _a_star->setGoal(mx, my, static_cast(orientation_bin), _goal_heading_mode); // Setup message nav_msgs::msg::Path plan; diff --git a/tools/planner_benchmarking/metrics.py b/tools/planner_benchmarking/metrics.py index 0ae0f883efe..79837d143d4 100644 --- a/tools/planner_benchmarking/metrics.py +++ b/tools/planner_benchmarking/metrics.py @@ -106,7 +106,7 @@ def main(): costmap = np.asarray(costmap_msg.data) costmap.resize(costmap_msg.metadata.size_y, costmap_msg.metadata.size_x) - planners = ['Navfn', 'ThetaStar', 'SmacHybrid', 'Smac2d', 'SmacLattice'] + planners = ["DEFAULT_Smac2d", "BIDIRECTIONAL_Smac2d", "ALL_DIRECTION_Smac2d", "DEFAULT_SmacHybrid", "BIDIRECTIONAL_SmacHybrid", "ALL_DIRECTION_SmacHybrid", "DEFAULT_SmacLattice", "BIDIRECTIONAL_SmacLattice", "ALL_DIRECTION_SmacLattice"] max_cost = 210 side_buffer = 100 time_stamp = navigator.get_clock().now().to_msg() diff --git a/tools/planner_benchmarking/planners.pickle b/tools/planner_benchmarking/planners.pickle new file mode 100644 index 0000000000000000000000000000000000000000..62eb3138eddc1a3acafaf8cbb65878ca66c98885 GIT binary patch literal 232 zcmZo*oqCr60%E6V^zga3x;cjWgv1BuCMFxDOz9DE@^tYGa&->z^!Ic0K?png_{75{ x;KD-4+C3_hiZX#lNMITPm%=m#CMAq)luu$wNoI2DlpaY;vk-EahQZ`g^#JciO?v Date: Thu, 10 Oct 2024 12:22:44 +0200 Subject: [PATCH 14/41] return config back to normal Signed-off-by: stevedan --- nav2_bringup/params/nav2_params.yaml | 253 +-------- nav2_bringup/params/nav2_params.yaml.backup | 431 ---------------- nav2_bringup/params/nav2_params_default.yaml | 510 ------------------- 3 files changed, 1 insertion(+), 1193 deletions(-) delete mode 100644 nav2_bringup/params/nav2_params.yaml.backup delete mode 100644 nav2_bringup/params/nav2_params_default.yaml diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 36d662fc754..2027f4c0e1c 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -276,265 +276,14 @@ map_saver: planner_server: ros__parameters: expected_planner_frequency: 20.0 - planner_plugins: ["GridBased", "DEFAULT_Smac2d", "BIDIRECTIONAL_Smac2d", "ALL_DIRECTION_Smac2d", "DEFAULT_SmacHybrid", "BIDIRECTIONAL_SmacHybrid", "ALL_DIRECTION_SmacHybrid", "DEFAULT_SmacLattice", "BIDIRECTIONAL_SmacLattice", "ALL_DIRECTION_SmacLattice"] + planner_plugins: ["GridBased"] costmap_update_timeout: 1.0 GridBased: plugin: "nav2_navfn_planner::NavfnPlanner" tolerance: 0.5 use_astar: false allow_unknown: true - DEFAULT_Smac2d: - plugin: "nav2_smac_planner::SmacPlanner2D" # In Iron and older versions, "/" was used instead of "::" - tolerance: 0.125 # tolerance for planning if unable to reach exact pose, in meters - downsample_costmap: false # whether or not to downsample the map - downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm) - allow_unknown: true # allow traveling in unknown space - max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable - max_on_approach_iterations: 1000 # maximum number of iterations to attempt to reach goal once in tolerance - max_planning_time: 2.0 # max time in s for planner to plan, smooth - cost_travel_multiplier: 2.0 # Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*. - use_final_approach_orientation: false # Whether to set the final path pose at the goal's orientation to the requested orientation (false) or in line with the approach angle so the robot doesn't rotate to heading (true) - smoother: - max_iterations: 1000 - w_smooth: 0.3 - w_data: 0.2 - tolerance: 1.0e-10 - BIDIRECTIONAL_Smac2d: - plugin: "nav2_smac_planner::SmacPlanner2D" # In Iron and older versions, "/" was used instead of "::" - tolerance: 0.125 # tolerance for planning if unable to reach exact pose, in meters - downsample_costmap: false # whether or not to downsample the map - downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm) - allow_unknown: true # allow traveling in unknown space - max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable - max_on_approach_iterations: 1000 # maximum number of iterations to attempt to reach goal once in tolerance - max_planning_time: 2.0 # max time in s for planner to plan, smooth - cost_travel_multiplier: 2.0 # Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*. - use_final_approach_orientation: false # Whether to set the final path pose at the goal's orientation to the requested orientation (false) or in line with the approach angle so the robot doesn't rotate to heading (true) - goal_heading_mode: "BIDIRECTIONAL" - smoother: - max_iterations: 1000 - w_smooth: 0.3 - w_data: 0.2 - tolerance: 1.0e-10 - ALL_DIRECTION_Smac2d: - plugin: "nav2_smac_planner::SmacPlanner2D" # In Iron and older versions, "/" was used instead of "::" - tolerance: 0.125 # tolerance for planning if unable to reach exact pose, in meters - downsample_costmap: false # whether or not to downsample the map - downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm) - allow_unknown: true # allow traveling in unknown space - max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable - max_on_approach_iterations: 1000 # maximum number of iterations to attempt to reach goal once in tolerance - max_planning_time: 2.0 # max time in s for planner to plan, smooth - cost_travel_multiplier: 2.0 # Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*. - use_final_approach_orientation: false # Whether to set the final path pose at the goal's orientation to the requested orientation (false) or in line with the approach angle so the robot doesn't rotate to heading (true) - goal_heading_mode: "ALL_DIRECTIONAL" - smoother: - max_iterations: 1000 - w_smooth: 0.3 - w_data: 0.2 - tolerance: 1.0e-10 - - DEFAULT_SmacHybrid: - plugin: "nav2_smac_planner::SmacPlannerHybrid" # In Iron and older versions, "/" was used instead of "::" - downsample_costmap: false # whether or not to downsample the map - downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm) - tolerance: 0.25 # dist-to-goal heuristic cost (distance) for valid tolerance endpoints if exact goal cannot be found. - allow_unknown: true # allow traveling in unknown space - max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable - max_on_approach_iterations: 1000 # Maximum number of iterations after within tolerances to continue to try to find exact solution - max_planning_time: 5.0 # max time in s for planner to plan, smooth - motion_model_for_search: "DUBIN" # Hybrid-A* Dubin, Redds-Shepp - angle_quantization_bins: 72 # Number of angle bins for search - analytic_expansion_ratio: 3.5 # The ratio to attempt analytic expansions during search for final approach. - analytic_expansion_max_length: 3.0 # For Hybrid/Lattice nodes: The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting - analytic_expansion_max_cost: 200.0 # The maximum single cost for any part of an analytic expansion to contain and be valid, except when necessary on approach to goal - analytic_expansion_max_cost_override: false # Whether or not to override the maximum cost setting if within critical distance to goal (ie probably required) - minimum_turning_radius: 0.40 # minimum turning radius in m of path / vehicle - reverse_penalty: 2.0 # Penalty to apply if motion is reversing, must be => 1 - change_penalty: 0.0 # Penalty to apply if motion is changing directions (L to R), must be >= 0 - non_straight_penalty: 1.2 # Penalty to apply if motion is non-straight, must be => 1 - cost_penalty: 2.0 # Penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable. - retrospective_penalty: 0.015 - lookup_table_size: 20.0 # Size of the dubin/reeds-sheep distance window to cache, in meters. - cache_obstacle_heuristic: false # Cache the obstacle map dynamic programming distance expansion heuristic between subsequent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static. - debug_visualizations: false # For Hybrid nodes: Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning) and the path's footprints on the /planned_footprints topic. WARNING: heavy to compute and to display, for debug only as it degrades the performance. - use_quadratic_cost_penalty: False - downsample_obstacle_heuristic: True - allow_primitive_interpolation: False - # goal_heading_mode: "BIDIRECTIONAL" - smooth_path: True # If true, does a simple and quick smoothing post-processing to the path - smoother: - max_iterations: 1000 - w_smooth: 0.3 - w_data: 0.2 - tolerance: 1.0e-10 - do_refinement: true - refinement_num: 2 - BIDIRECTIONAL_SmacHybrid: - plugin: "nav2_smac_planner::SmacPlannerHybrid" # In Iron and older versions, "/" was used instead of "::" - downsample_costmap: false # whether or not to downsample the map - downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm) - tolerance: 0.25 # dist-to-goal heuristic cost (distance) for valid tolerance endpoints if exact goal cannot be found. - allow_unknown: true # allow traveling in unknown space - max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable - max_on_approach_iterations: 1000 # Maximum number of iterations after within tolerances to continue to try to find exact solution - max_planning_time: 5.0 # max time in s for planner to plan, smooth - motion_model_for_search: "DUBIN" # Hybrid-A* Dubin, Redds-Shepp - angle_quantization_bins: 72 # Number of angle bins for search - analytic_expansion_ratio: 3.5 # The ratio to attempt analytic expansions during search for final approach. - analytic_expansion_max_length: 3.0 # For Hybrid/Lattice nodes: The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting - analytic_expansion_max_cost: 200.0 # The maximum single cost for any part of an analytic expansion to contain and be valid, except when necessary on approach to goal - analytic_expansion_max_cost_override: false # Whether or not to override the maximum cost setting if within critical distance to goal (ie probably required) - minimum_turning_radius: 0.40 # minimum turning radius in m of path / vehicle - reverse_penalty: 2.0 # Penalty to apply if motion is reversing, must be => 1 - change_penalty: 0.0 # Penalty to apply if motion is changing directions (L to R), must be >= 0 - non_straight_penalty: 1.2 # Penalty to apply if motion is non-straight, must be => 1 - cost_penalty: 2.0 # Penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable. - retrospective_penalty: 0.015 - lookup_table_size: 20.0 # Size of the dubin/reeds-sheep distance window to cache, in meters. - cache_obstacle_heuristic: false # Cache the obstacle map dynamic programming distance expansion heuristic between subsequent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static. - debug_visualizations: false # For Hybrid nodes: Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning) and the path's footprints on the /planned_footprints topic. WARNING: heavy to compute and to display, for debug only as it degrades the performance. - use_quadratic_cost_penalty: False - downsample_obstacle_heuristic: True - allow_primitive_interpolation: False - goal_heading_mode: "BIDIRECTIONAL" - smooth_path: True # If true, does a simple and quick smoothing post-processing to the path - - smoother: - max_iterations: 1000 - w_smooth: 0.3 - w_data: 0.2 - tolerance: 1.0e-10 - do_refinement: true - refinement_num: 2 - ALL_DIRECTION_SmacHybrid: - plugin: "nav2_smac_planner::SmacPlannerHybrid" # In Iron and older versions, "/" was used instead of "::" - downsample_costmap: false # whether or not to downsample the map - downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm) - tolerance: 0.25 # dist-to-goal heuristic cost (distance) for valid tolerance endpoints if exact goal cannot be found. - allow_unknown: true # allow traveling in unknown space - max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable - max_on_approach_iterations: 1000 # Maximum number of iterations after within tolerances to continue to try to find exact solution - max_planning_time: 5.0 # max time in s for planner to plan, smooth - motion_model_for_search: "DUBIN" # Hybrid-A* Dubin, Redds-Shepp - angle_quantization_bins: 72 # Number of angle bins for search - analytic_expansion_ratio: 3.5 # The ratio to attempt analytic expansions during search for final approach. - analytic_expansion_max_length: 3.0 # For Hybrid/Lattice nodes: The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting - analytic_expansion_max_cost: 200.0 # The maximum single cost for any part of an analytic expansion to contain and be valid, except when necessary on approach to goal - analytic_expansion_max_cost_override: false # Whether or not to override the maximum cost setting if within critical distance to goal (ie probably required) - minimum_turning_radius: 0.40 # minimum turning radius in m of path / vehicle - reverse_penalty: 2.0 # Penalty to apply if motion is reversing, must be => 1 - change_penalty: 0.0 # Penalty to apply if motion is changing directions (L to R), must be >= 0 - non_straight_penalty: 1.2 # Penalty to apply if motion is non-straight, must be => 1 - cost_penalty: 2.0 # Penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable. - retrospective_penalty: 0.015 - lookup_table_size: 20.0 # Size of the dubin/reeds-sheep distance window to cache, in meters. - cache_obstacle_heuristic: false # Cache the obstacle map dynamic programming distance expansion heuristic between subsequent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static. - debug_visualizations: false # For Hybrid nodes: Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning) and the path's footprints on the /planned_footprints topic. WARNING: heavy to compute and to display, for debug only as it degrades the performance. - use_quadratic_cost_penalty: False - downsample_obstacle_heuristic: True - allow_primitive_interpolation: False - goal_heading_mode: "ALL_DIRECTION" - smooth_path: True # If true, does a simple and quick smoothing post-processing to the path - - smoother: - max_iterations: 1000 - w_smooth: 0.3 - w_data: 0.2 - tolerance: 1.0e-10 - do_refinement: true - refinement_num: 2 - - DEFAULT_SmacLattice: - plugin: "nav2_smac_planner::SmacPlannerLattice" # In Iron and older versions, "/" was used instead of "::" - allow_unknown: true # Allow traveling in unknown space - tolerance: 0.25 # dist-to-goal heuristic cost (distance) for valid tolerance endpoints if exact goal cannot be found. - max_iterations: 1000000 # Maximum total iterations to search for before failing (in case unreachable), set to -1 to disable - max_on_approach_iterations: 1000 # Maximum number of iterations after within tolerances to continue to try to find exact solution - max_planning_time: 5.0 # Max time in s for planner to plan, smooth - analytic_expansion_ratio: 3.5 # The ratio to attempt analytic expansions during search for final approach. - analytic_expansion_max_length: 3.0 # For Hybrid/Lattice nodes The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting - analytic_expansion_max_cost: 200.0 # The maximum single cost for any part of an analytic expansion to contain and be valid, except when necessary on approach to goal - analytic_expansion_max_cost_override: false # Whether or not to override the maximum cost setting if within critical distance to goal (ie probably required) - reverse_penalty: 2.0 # Penalty to apply if motion is reversing, must be => 1 - change_penalty: 0.05 # Penalty to apply if motion is changing directions (L to R), must be >= 0 - non_straight_penalty: 1.05 # Penalty to apply if motion is non-straight, must be => 1 - cost_penalty: 2.0 # Penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable. - rotation_penalty: 5.0 # Penalty to apply to in-place rotations, if minimum control set contains them - retrospective_penalty: 0.015 - # lattice_filepath: "" # The filepath to the state lattice graph - lookup_table_size: 20.0 # Size of the dubin/reeds-sheep distance window to cache, in meters. - cache_obstacle_heuristic: false # Cache the obstacle map dynamic programming distance expansion heuristic between subsequent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static. - allow_reverse_expansion: false # If true, allows the robot to use the primitives to expand in the mirrored opposite direction of the current robot's orientation (to reverse). - smooth_path: True # If true, does a simple and quick smoothing post-processing to the path - smoother: - max_iterations: 1000 - w_smooth: 0.3 - w_data: 0.2 - tolerance: 1.0e-10 - do_refinement: true - refinement_num: 2 - BIDIRECTIONAL_SmacLattice: - plugin: "nav2_smac_planner::SmacPlannerLattice" # In Iron and older versions, "/" was used instead of "::" - allow_unknown: true # Allow traveling in unknown space - tolerance: 0.25 # dist-to-goal heuristic cost (distance) for valid tolerance endpoints if exact goal cannot be found. - max_iterations: 1000000 # Maximum total iterations to search for before failing (in case unreachable), set to -1 to disable - max_on_approach_iterations: 1000 # Maximum number of iterations after within tolerances to continue to try to find exact solution - max_planning_time: 5.0 # Max time in s for planner to plan, smooth - analytic_expansion_ratio: 3.5 # The ratio to attempt analytic expansions during search for final approach. - analytic_expansion_max_length: 3.0 # For Hybrid/Lattice nodes The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting - analytic_expansion_max_cost: 200.0 # The maximum single cost for any part of an analytic expansion to contain and be valid, except when necessary on approach to goal - analytic_expansion_max_cost_override: false # Whether or not to override the maximum cost setting if within critical distance to goal (ie probably required) - reverse_penalty: 2.0 # Penalty to apply if motion is reversing, must be => 1 - change_penalty: 0.05 # Penalty to apply if motion is changing directions (L to R), must be >= 0 - non_straight_penalty: 1.05 # Penalty to apply if motion is non-straight, must be => 1 - cost_penalty: 2.0 # Penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable. - rotation_penalty: 5.0 # Penalty to apply to in-place rotations, if minimum control set contains them - retrospective_penalty: 0.015 - # lattice_filepath: "" # The filepath to the state lattice graph - lookup_table_size: 20.0 # Size of the dubin/reeds-sheep distance window to cache, in meters. - cache_obstacle_heuristic: false # Cache the obstacle map dynamic programming distance expansion heuristic between subsequent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static. - allow_reverse_expansion: false # If true, allows the robot to use the primitives to expand in the mirrored opposite direction of the current robot's orientation (to reverse). - smooth_path: True # If true, does a simple and quick smoothing post-processing to the path - goal_heading_mode: "BIDIRECTIONAL" - smoother: - max_iterations: 1000 - w_smooth: 0.3 - w_data: 0.2 - tolerance: 1.0e-10 - do_refinement: true - refinement_num: 2 - ALL_DIRECTION_SmacLattice: - plugin: "nav2_smac_planner::SmacPlannerLattice" # In Iron and older versions, "/" was used instead of "::" - allow_unknown: true # Allow traveling in unknown space - tolerance: 0.25 # dist-to-goal heuristic cost (distance) for valid tolerance endpoints if exact goal cannot be found. - max_iterations: 1000000 # Maximum total iterations to search for before failing (in case unreachable), set to -1 to disable - max_on_approach_iterations: 1000 # Maximum number of iterations after within tolerances to continue to try to find exact solution - max_planning_time: 5.0 # Max time in s for planner to plan, smooth - analytic_expansion_ratio: 3.5 # The ratio to attempt analytic expansions during search for final approach. - analytic_expansion_max_length: 3.0 # For Hybrid/Lattice nodes The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting - analytic_expansion_max_cost: 200.0 # The maximum single cost for any part of an analytic expansion to contain and be valid, except when necessary on approach to goal - analytic_expansion_max_cost_override: false # Whether or not to override the maximum cost setting if within critical distance to goal (ie probably required) - reverse_penalty: 2.0 # Penalty to apply if motion is reversing, must be => 1 - change_penalty: 0.05 # Penalty to apply if motion is changing directions (L to R), must be >= 0 - non_straight_penalty: 1.05 # Penalty to apply if motion is non-straight, must be => 1 - cost_penalty: 2.0 # Penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable. - rotation_penalty: 5.0 # Penalty to apply to in-place rotations, if minimum control set contains them - retrospective_penalty: 0.015 - # lattice_filepath: "" # The filepath to the state lattice graph - lookup_table_size: 20.0 # Size of the dubin/reeds-sheep distance window to cache, in meters. - cache_obstacle_heuristic: false # Cache the obstacle map dynamic programming distance expansion heuristic between subsequent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static. - allow_reverse_expansion: false # If true, allows the robot to use the primitives to expand in the mirrored opposite direction of the current robot's orientation (to reverse). - goal_heading_mode: "ALL_DIRECTION" - smooth_path: True # If true, does a simple and quick smoothing post-processing to the path - smoother: - max_iterations: 1000 - w_smooth: 0.3 - w_data: 0.2 - tolerance: 1.0e-10 - do_refinement: true - refinement_num: 2 smoother_server: ros__parameters: smoother_plugins: ["simple_smoother"] diff --git a/nav2_bringup/params/nav2_params.yaml.backup b/nav2_bringup/params/nav2_params.yaml.backup deleted file mode 100644 index 2027f4c0e1c..00000000000 --- a/nav2_bringup/params/nav2_params.yaml.backup +++ /dev/null @@ -1,431 +0,0 @@ -amcl: - ros__parameters: - alpha1: 0.2 - alpha2: 0.2 - alpha3: 0.2 - alpha4: 0.2 - alpha5: 0.2 - base_frame_id: "base_footprint" - beam_skip_distance: 0.5 - beam_skip_error_threshold: 0.9 - beam_skip_threshold: 0.3 - do_beamskip: false - global_frame_id: "map" - lambda_short: 0.1 - laser_likelihood_max_dist: 2.0 - laser_max_range: 100.0 - laser_min_range: -1.0 - laser_model_type: "likelihood_field" - max_beams: 60 - max_particles: 2000 - min_particles: 500 - odom_frame_id: "odom" - pf_err: 0.05 - pf_z: 0.99 - recovery_alpha_fast: 0.0 - recovery_alpha_slow: 0.0 - resample_interval: 1 - robot_model_type: "nav2_amcl::DifferentialMotionModel" - save_pose_rate: 0.5 - sigma_hit: 0.2 - tf_broadcast: true - transform_tolerance: 1.0 - update_min_a: 0.2 - update_min_d: 0.25 - z_hit: 0.5 - z_max: 0.05 - z_rand: 0.5 - z_short: 0.05 - scan_topic: scan - -bt_navigator: - ros__parameters: - global_frame: map - robot_base_frame: base_link - odom_topic: /odom - bt_loop_duration: 10 - default_server_timeout: 20 - wait_for_service_timeout: 1000 - action_server_result_timeout: 900.0 - navigators: ["navigate_to_pose", "navigate_through_poses"] - navigate_to_pose: - plugin: "nav2_bt_navigator::NavigateToPoseNavigator" - navigate_through_poses: - plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator" - # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: - # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml - # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml - # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. - - # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). - # Built-in plugins are added automatically - # plugin_lib_names: [] - - error_code_names: - - compute_path_error_code - - follow_path_error_code - -controller_server: - ros__parameters: - controller_frequency: 20.0 - costmap_update_timeout: 0.30 - min_x_velocity_threshold: 0.001 - min_y_velocity_threshold: 0.5 - min_theta_velocity_threshold: 0.001 - failure_tolerance: 0.3 - progress_checker_plugins: ["progress_checker"] - goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" - controller_plugins: ["FollowPath"] - use_realtime_priority: false - - # Progress checker parameters - progress_checker: - plugin: "nav2_controller::SimpleProgressChecker" - required_movement_radius: 0.5 - movement_time_allowance: 10.0 - # Goal checker parameters - #precise_goal_checker: - # plugin: "nav2_controller::SimpleGoalChecker" - # xy_goal_tolerance: 0.25 - # yaw_goal_tolerance: 0.25 - # stateful: True - general_goal_checker: - stateful: True - plugin: "nav2_controller::SimpleGoalChecker" - xy_goal_tolerance: 0.25 - yaw_goal_tolerance: 0.25 - FollowPath: - plugin: "nav2_mppi_controller::MPPIController" - time_steps: 56 - model_dt: 0.05 - batch_size: 2000 - ax_max: 3.0 - ax_min: -3.0 - ay_max: 3.0 - az_max: 3.5 - vx_std: 0.2 - vy_std: 0.2 - wz_std: 0.4 - vx_max: 0.5 - vx_min: -0.35 - vy_max: 0.5 - wz_max: 1.9 - iteration_count: 1 - prune_distance: 1.7 - transform_tolerance: 0.1 - temperature: 0.3 - gamma: 0.015 - motion_model: "DiffDrive" - visualize: true - regenerate_noises: true - TrajectoryVisualizer: - trajectory_step: 5 - time_step: 3 - AckermannConstraints: - min_turning_r: 0.2 - critics: [ - "ConstraintCritic", "CostCritic", "GoalCritic", - "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", - "PathAngleCritic", "PreferForwardCritic"] - ConstraintCritic: - enabled: true - cost_power: 1 - cost_weight: 4.0 - GoalCritic: - enabled: true - cost_power: 1 - cost_weight: 5.0 - threshold_to_consider: 1.4 - GoalAngleCritic: - enabled: true - cost_power: 1 - cost_weight: 3.0 - threshold_to_consider: 0.5 - PreferForwardCritic: - enabled: true - cost_power: 1 - cost_weight: 5.0 - threshold_to_consider: 0.5 - CostCritic: - enabled: true - cost_power: 1 - cost_weight: 3.81 - critical_cost: 300.0 - consider_footprint: true - collision_cost: 1000000.0 - near_goal_distance: 1.0 - trajectory_point_step: 2 - PathAlignCritic: - enabled: true - cost_power: 1 - cost_weight: 14.0 - max_path_occupancy_ratio: 0.05 - trajectory_point_step: 4 - threshold_to_consider: 0.5 - offset_from_furthest: 20 - use_path_orientations: false - PathFollowCritic: - enabled: true - cost_power: 1 - cost_weight: 5.0 - offset_from_furthest: 5 - threshold_to_consider: 1.4 - PathAngleCritic: - enabled: true - cost_power: 1 - cost_weight: 2.0 - offset_from_furthest: 4 - threshold_to_consider: 0.5 - max_angle_to_furthest: 1.0 - mode: 0 - # TwirlingCritic: - # enabled: true - # twirling_cost_power: 1 - # twirling_cost_weight: 10.0 - -local_costmap: - local_costmap: - ros__parameters: - update_frequency: 5.0 - publish_frequency: 2.0 - global_frame: odom - robot_base_frame: base_link - rolling_window: true - width: 3 - height: 3 - resolution: 0.05 - robot_radius: 0.22 - plugins: ["voxel_layer", "inflation_layer"] - inflation_layer: - plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 3.0 - inflation_radius: 0.70 - voxel_layer: - plugin: "nav2_costmap_2d::VoxelLayer" - enabled: True - publish_voxel_map: True - origin_z: 0.0 - z_resolution: 0.05 - z_voxels: 16 - max_obstacle_height: 2.0 - mark_threshold: 0 - observation_sources: scan - scan: - topic: /scan - max_obstacle_height: 2.0 - clearing: True - marking: True - data_type: "LaserScan" - raytrace_max_range: 3.0 - raytrace_min_range: 0.0 - obstacle_max_range: 2.5 - obstacle_min_range: 0.0 - static_layer: - plugin: "nav2_costmap_2d::StaticLayer" - map_subscribe_transient_local: True - always_send_full_costmap: True - -global_costmap: - global_costmap: - ros__parameters: - update_frequency: 1.0 - publish_frequency: 1.0 - global_frame: map - robot_base_frame: base_link - robot_radius: 0.22 - resolution: 0.05 - track_unknown_space: true - plugins: ["static_layer", "obstacle_layer", "inflation_layer"] - obstacle_layer: - plugin: "nav2_costmap_2d::ObstacleLayer" - enabled: True - observation_sources: scan - scan: - topic: /scan - max_obstacle_height: 2.0 - clearing: True - marking: True - data_type: "LaserScan" - raytrace_max_range: 3.0 - raytrace_min_range: 0.0 - obstacle_max_range: 2.5 - obstacle_min_range: 0.0 - static_layer: - plugin: "nav2_costmap_2d::StaticLayer" - map_subscribe_transient_local: True - inflation_layer: - plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 3.0 - inflation_radius: 0.7 - always_send_full_costmap: True - -# The yaml_filename does not need to be specified since it going to be set by defaults in launch. -# If you'd rather set it in the yaml, remove the default "map" value in the tb3_simulation_launch.py -# file & provide full path to map below. If CLI map configuration or launch default is provided, that will be used. -# map_server: -# ros__parameters: -# yaml_filename: "" - -map_saver: - ros__parameters: - save_map_timeout: 5.0 - free_thresh_default: 0.25 - occupied_thresh_default: 0.65 - map_subscribe_transient_local: True - -planner_server: - ros__parameters: - expected_planner_frequency: 20.0 - planner_plugins: ["GridBased"] - costmap_update_timeout: 1.0 - GridBased: - plugin: "nav2_navfn_planner::NavfnPlanner" - tolerance: 0.5 - use_astar: false - allow_unknown: true - -smoother_server: - ros__parameters: - smoother_plugins: ["simple_smoother"] - simple_smoother: - plugin: "nav2_smoother::SimpleSmoother" - tolerance: 1.0e-10 - max_its: 1000 - do_refinement: True - -behavior_server: - ros__parameters: - local_costmap_topic: local_costmap/costmap_raw - global_costmap_topic: global_costmap/costmap_raw - local_footprint_topic: local_costmap/published_footprint - global_footprint_topic: global_costmap/published_footprint - cycle_frequency: 10.0 - behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] - spin: - plugin: "nav2_behaviors::Spin" - backup: - plugin: "nav2_behaviors::BackUp" - drive_on_heading: - plugin: "nav2_behaviors::DriveOnHeading" - wait: - plugin: "nav2_behaviors::Wait" - assisted_teleop: - plugin: "nav2_behaviors::AssistedTeleop" - local_frame: odom - global_frame: map - robot_base_frame: base_link - transform_tolerance: 0.1 - simulate_ahead_time: 2.0 - max_rotational_vel: 1.0 - min_rotational_vel: 0.4 - rotational_acc_lim: 3.2 - -waypoint_follower: - ros__parameters: - loop_rate: 20 - stop_on_failure: false - action_server_result_timeout: 900.0 - waypoint_task_executor_plugin: "wait_at_waypoint" - wait_at_waypoint: - plugin: "nav2_waypoint_follower::WaitAtWaypoint" - enabled: True - waypoint_pause_duration: 200 - -velocity_smoother: - ros__parameters: - smoothing_frequency: 20.0 - scale_velocities: False - feedback: "OPEN_LOOP" - max_velocity: [0.5, 0.0, 2.0] - min_velocity: [-0.5, 0.0, -2.0] - max_accel: [2.5, 0.0, 3.2] - max_decel: [-2.5, 0.0, -3.2] - odom_topic: "odom" - odom_duration: 0.1 - deadband_velocity: [0.0, 0.0, 0.0] - velocity_timeout: 1.0 - -collision_monitor: - ros__parameters: - base_frame_id: "base_footprint" - odom_frame_id: "odom" - cmd_vel_in_topic: "cmd_vel_smoothed" - cmd_vel_out_topic: "cmd_vel" - state_topic: "collision_monitor_state" - transform_tolerance: 0.2 - source_timeout: 1.0 - base_shift_correction: True - stop_pub_timeout: 2.0 - # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, - # and robot footprint for "approach" action type. - polygons: ["FootprintApproach"] - FootprintApproach: - type: "polygon" - action_type: "approach" - footprint_topic: "/local_costmap/published_footprint" - time_before_collision: 1.2 - simulation_time_step: 0.1 - min_points: 6 - visualize: False - enabled: True - observation_sources: ["scan"] - scan: - type: "scan" - topic: "scan" - min_height: 0.15 - max_height: 2.0 - enabled: True - -docking_server: - ros__parameters: - controller_frequency: 50.0 - initial_perception_timeout: 5.0 - wait_charge_timeout: 5.0 - dock_approach_timeout: 30.0 - undock_linear_tolerance: 0.05 - undock_angular_tolerance: 0.1 - max_retries: 3 - base_frame: "base_link" - fixed_frame: "odom" - dock_backwards: false - dock_prestaging_tolerance: 0.5 - - # Types of docks - dock_plugins: ['simple_charging_dock'] - simple_charging_dock: - plugin: 'opennav_docking::SimpleChargingDock' - docking_threshold: 0.05 - staging_x_offset: -0.7 - use_external_detection_pose: true - use_battery_status: false # true - use_stall_detection: false # true - - external_detection_timeout: 1.0 - external_detection_translation_x: -0.18 - external_detection_translation_y: 0.0 - external_detection_rotation_roll: -1.57 - external_detection_rotation_pitch: -1.57 - external_detection_rotation_yaw: 0.0 - filter_coef: 0.1 - - # Dock instances - # The following example illustrates configuring dock instances. - # docks: ['home_dock'] # Input your docks here - # home_dock: - # type: 'simple_charging_dock' - # frame: map - # pose: [0.0, 0.0, 0.0] - - controller: - k_phi: 3.0 - k_delta: 2.0 - v_linear_min: 0.15 - v_linear_max: 0.15 - -loopback_simulator: - ros__parameters: - base_frame_id: "base_footprint" - odom_frame_id: "odom" - map_frame_id: "map" - scan_frame_id: "base_scan" # tb4_loopback_simulator.launch.py remaps to 'rplidar_link' - update_duration: 0.02 diff --git a/nav2_bringup/params/nav2_params_default.yaml b/nav2_bringup/params/nav2_params_default.yaml deleted file mode 100644 index 698e27a69a0..00000000000 --- a/nav2_bringup/params/nav2_params_default.yaml +++ /dev/null @@ -1,510 +0,0 @@ -amcl: - ros__parameters: - alpha1: 0.2 - alpha2: 0.2 - alpha3: 0.2 - alpha4: 0.2 - alpha5: 0.2 - base_frame_id: "base_footprint" - beam_skip_distance: 0.5 - beam_skip_error_threshold: 0.9 - beam_skip_threshold: 0.3 - do_beamskip: false - global_frame_id: "map" - lambda_short: 0.1 - laser_likelihood_max_dist: 2.0 - laser_max_range: 100.0 - laser_min_range: -1.0 - laser_model_type: "likelihood_field" - max_beams: 60 - max_particles: 2000 - min_particles: 500 - odom_frame_id: "odom" - pf_err: 0.05 - pf_z: 0.99 - recovery_alpha_fast: 0.0 - recovery_alpha_slow: 0.0 - resample_interval: 1 - robot_model_type: "nav2_amcl::DifferentialMotionModel" - save_pose_rate: 0.5 - sigma_hit: 0.2 - tf_broadcast: true - transform_tolerance: 1.0 - update_min_a: 0.2 - update_min_d: 0.25 - z_hit: 0.5 - z_max: 0.05 - z_rand: 0.5 - z_short: 0.05 - scan_topic: scan - -bt_navigator: - ros__parameters: - global_frame: map - robot_base_frame: base_link - odom_topic: /odom - bt_loop_duration: 10 - default_server_timeout: 20 - wait_for_service_timeout: 1000 - action_server_result_timeout: 900.0 - navigators: ["navigate_to_pose", "navigate_through_poses"] - navigate_to_pose: - plugin: "nav2_bt_navigator::NavigateToPoseNavigator" - navigate_through_poses: - plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator" - # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: - # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml - # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml - # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. - - # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). - # Built-in plugins are added automatically - # plugin_lib_names: [] - - error_code_names: - - compute_path_error_code - - follow_path_error_code - -controller_server: - ros__parameters: - controller_frequency: 20.0 - costmap_update_timeout: 0.30 - min_x_velocity_threshold: 0.001 - min_y_velocity_threshold: 0.5 - min_theta_velocity_threshold: 0.001 - failure_tolerance: 0.3 - progress_checker_plugins: ["progress_checker"] - goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" - controller_plugins: ["FollowPath"] - use_realtime_priority: false - - # Progress checker parameters - progress_checker: - plugin: "nav2_controller::SimpleProgressChecker" - required_movement_radius: 0.5 - movement_time_allowance: 10.0 - # Goal checker parameters - #precise_goal_checker: - # plugin: "nav2_controller::SimpleGoalChecker" - # xy_goal_tolerance: 0.25 - # yaw_goal_tolerance: 0.25 - # stateful: True - general_goal_checker: - stateful: True - plugin: "nav2_controller::SimpleGoalChecker" - xy_goal_tolerance: 0.25 - yaw_goal_tolerance: 0.25 - FollowPath: - plugin: "nav2_mppi_controller::MPPIController" - time_steps: 56 - model_dt: 0.05 - batch_size: 2000 - ax_max: 3.0 - ax_min: -3.0 - ay_max: 3.0 - az_max: 3.5 - vx_std: 0.2 - vy_std: 0.2 - wz_std: 0.4 - vx_max: 0.5 - vx_min: -0.35 - vy_max: 0.5 - wz_max: 1.9 - iteration_count: 1 - prune_distance: 1.7 - transform_tolerance: 0.1 - temperature: 0.3 - gamma: 0.015 - motion_model: "DiffDrive" - visualize: true - regenerate_noises: true - TrajectoryVisualizer: - trajectory_step: 5 - time_step: 3 - AckermannConstraints: - min_turning_r: 0.2 - critics: [ - "ConstraintCritic", "CostCritic", "GoalCritic", - "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", - "PathAngleCritic", "PreferForwardCritic"] - ConstraintCritic: - enabled: true - cost_power: 1 - cost_weight: 4.0 - GoalCritic: - enabled: true - cost_power: 1 - cost_weight: 5.0 - threshold_to_consider: 1.4 - GoalAngleCritic: - enabled: true - cost_power: 1 - cost_weight: 3.0 - threshold_to_consider: 0.5 - PreferForwardCritic: - enabled: true - cost_power: 1 - cost_weight: 5.0 - threshold_to_consider: 0.5 - CostCritic: - enabled: true - cost_power: 1 - cost_weight: 3.81 - critical_cost: 300.0 - consider_footprint: true - collision_cost: 1000000.0 - near_goal_distance: 1.0 - trajectory_point_step: 2 - PathAlignCritic: - enabled: true - cost_power: 1 - cost_weight: 14.0 - max_path_occupancy_ratio: 0.05 - trajectory_point_step: 4 - threshold_to_consider: 0.5 - offset_from_furthest: 20 - use_path_orientations: false - PathFollowCritic: - enabled: true - cost_power: 1 - cost_weight: 5.0 - offset_from_furthest: 5 - threshold_to_consider: 1.4 - PathAngleCritic: - enabled: true - cost_power: 1 - cost_weight: 2.0 - offset_from_furthest: 4 - threshold_to_consider: 0.5 - max_angle_to_furthest: 1.0 - mode: 0 - # TwirlingCritic: - # enabled: true - # twirling_cost_power: 1 - # twirling_cost_weight: 10.0 - -local_costmap: - local_costmap: - ros__parameters: - update_frequency: 5.0 - publish_frequency: 2.0 - global_frame: odom - robot_base_frame: base_link - rolling_window: true - width: 3 - height: 3 - resolution: 0.05 - robot_radius: 0.22 - plugins: ["voxel_layer", "inflation_layer"] - inflation_layer: - plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 3.0 - inflation_radius: 0.70 - voxel_layer: - plugin: "nav2_costmap_2d::VoxelLayer" - enabled: True - publish_voxel_map: True - origin_z: 0.0 - z_resolution: 0.05 - z_voxels: 16 - max_obstacle_height: 2.0 - mark_threshold: 0 - observation_sources: scan - scan: - topic: /scan - max_obstacle_height: 2.0 - clearing: True - marking: True - data_type: "LaserScan" - raytrace_max_range: 3.0 - raytrace_min_range: 0.0 - obstacle_max_range: 2.5 - obstacle_min_range: 0.0 - static_layer: - plugin: "nav2_costmap_2d::StaticLayer" - map_subscribe_transient_local: True - always_send_full_costmap: True - -global_costmap: - global_costmap: - ros__parameters: - update_frequency: 1.0 - publish_frequency: 1.0 - global_frame: map - robot_base_frame: base_link - robot_radius: 0.22 - resolution: 0.05 - track_unknown_space: true - plugins: ["static_layer", "obstacle_layer", "inflation_layer"] - obstacle_layer: - plugin: "nav2_costmap_2d::ObstacleLayer" - enabled: True - observation_sources: scan - scan: - topic: /scan - max_obstacle_height: 2.0 - clearing: True - marking: True - data_type: "LaserScan" - raytrace_max_range: 3.0 - raytrace_min_range: 0.0 - obstacle_max_range: 2.5 - obstacle_min_range: 0.0 - static_layer: - plugin: "nav2_costmap_2d::StaticLayer" - map_subscribe_transient_local: True - inflation_layer: - plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 3.0 - inflation_radius: 0.7 - always_send_full_costmap: True - -# The yaml_filename does not need to be specified since it going to be set by defaults in launch. -# If you'd rather set it in the yaml, remove the default "map" value in the tb3_simulation_launch.py -# file & provide full path to map below. If CLI map configuration or launch default is provided, that will be used. -# map_server: -# ros__parameters: -# yaml_filename: "" - -map_saver: - ros__parameters: - save_map_timeout: 5.0 - free_thresh_default: 0.25 - occupied_thresh_default: 0.65 - map_subscribe_transient_local: True - -planner_server: - ros__parameters: - expected_planner_frequency: 20.0 - planner_plugins: ["Smac2d", "SmacHybrid", "SmacLattice"] - costmap_update_timeout: 1.0 - Smac2d: - plugin: "nav2_smac_planner::SmacPlanner2D" # In Iron and older versions, "/" was used instead of "::" - tolerance: 0.125 # tolerance for planning if unable to reach exact pose, in meters - downsample_costmap: false # whether or not to downsample the map - downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm) - allow_unknown: true # allow traveling in unknown space - max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable - max_on_approach_iterations: 1000 # maximum number of iterations to attempt to reach goal once in tolerance - max_planning_time: 2.0 # max time in s for planner to plan, smooth - cost_travel_multiplier: 2.0 # Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*. - use_final_approach_orientation: false # Whether to set the final path pose at the goal's orientation to the requested orientation (false) or in line with the approach angle so the robot doesn't rotate to heading (true) - smoother: - max_iterations: 1000 - w_smooth: 0.3 - w_data: 0.2 - tolerance: 1.0e-10 - - - SmacHybrid: - plugin: "nav2_smac_planner::SmacPlannerHybrid" # In Iron and older versions, "/" was used instead of "::" - downsample_costmap: false # whether or not to downsample the map - downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm) - tolerance: 0.25 # dist-to-goal heuristic cost (distance) for valid tolerance endpoints if exact goal cannot be found. - allow_unknown: true # allow traveling in unknown space - max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable - max_on_approach_iterations: 1000 # Maximum number of iterations after within tolerances to continue to try to find exact solution - max_planning_time: 5.0 # max time in s for planner to plan, smooth - motion_model_for_search: "DUBIN" # Hybrid-A* Dubin, Redds-Shepp - angle_quantization_bins: 72 # Number of angle bins for search - analytic_expansion_ratio: 3.5 # The ratio to attempt analytic expansions during search for final approach. - analytic_expansion_max_length: 3.0 # For Hybrid/Lattice nodes: The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting - analytic_expansion_max_cost: 200.0 # The maximum single cost for any part of an analytic expansion to contain and be valid, except when necessary on approach to goal - analytic_expansion_max_cost_override: false # Whether or not to override the maximum cost setting if within critical distance to goal (ie probably required) - minimum_turning_radius: 0.40 # minimum turning radius in m of path / vehicle - reverse_penalty: 2.0 # Penalty to apply if motion is reversing, must be => 1 - change_penalty: 0.0 # Penalty to apply if motion is changing directions (L to R), must be >= 0 - non_straight_penalty: 1.2 # Penalty to apply if motion is non-straight, must be => 1 - cost_penalty: 2.0 # Penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable. - retrospective_penalty: 0.015 - lookup_table_size: 20.0 # Size of the dubin/reeds-sheep distance window to cache, in meters. - cache_obstacle_heuristic: false # Cache the obstacle map dynamic programming distance expansion heuristic between subsequent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static. - debug_visualizations: false # For Hybrid nodes: Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning) and the path's footprints on the /planned_footprints topic. WARNING: heavy to compute and to display, for debug only as it degrades the performance. - use_quadratic_cost_penalty: False - downsample_obstacle_heuristic: True - allow_primitive_interpolation: False - # goal_heading_mode: "BIDIRECTIONAL" - smooth_path: True # If true, does a simple and quick smoothing post-processing to the path - - smoother: - max_iterations: 1000 - w_smooth: 0.3 - w_data: 0.2 - tolerance: 1.0e-10 - do_refinement: true - refinement_num: 2 - SmacLattice: - plugin: "nav2_smac_planner::SmacPlannerLattice" # In Iron and older versions, "/" was used instead of "::" - allow_unknown: true # Allow traveling in unknown space - tolerance: 0.25 # dist-to-goal heuristic cost (distance) for valid tolerance endpoints if exact goal cannot be found. - max_iterations: 1000000 # Maximum total iterations to search for before failing (in case unreachable), set to -1 to disable - max_on_approach_iterations: 1000 # Maximum number of iterations after within tolerances to continue to try to find exact solution - max_planning_time: 5.0 # Max time in s for planner to plan, smooth - analytic_expansion_ratio: 3.5 # The ratio to attempt analytic expansions during search for final approach. - analytic_expansion_max_length: 3.0 # For Hybrid/Lattice nodes The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting - analytic_expansion_max_cost: 200.0 # The maximum single cost for any part of an analytic expansion to contain and be valid, except when necessary on approach to goal - analytic_expansion_max_cost_override: false # Whether or not to override the maximum cost setting if within critical distance to goal (ie probably required) - reverse_penalty: 2.0 # Penalty to apply if motion is reversing, must be => 1 - change_penalty: 0.05 # Penalty to apply if motion is changing directions (L to R), must be >= 0 - non_straight_penalty: 1.05 # Penalty to apply if motion is non-straight, must be => 1 - cost_penalty: 2.0 # Penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable. - rotation_penalty: 5.0 # Penalty to apply to in-place rotations, if minimum control set contains them - retrospective_penalty: 0.015 - # lattice_filepath: "" # The filepath to the state lattice graph - lookup_table_size: 20.0 # Size of the dubin/reeds-sheep distance window to cache, in meters. - cache_obstacle_heuristic: false # Cache the obstacle map dynamic programming distance expansion heuristic between subsequent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static. - allow_reverse_expansion: false # If true, allows the robot to use the primitives to expand in the mirrored opposite direction of the current robot's orientation (to reverse). - smooth_path: True # If true, does a simple and quick smoothing post-processing to the path - smoother: - max_iterations: 1000 - w_smooth: 0.3 - w_data: 0.2 - tolerance: 1.0e-10 - do_refinement: true - refinement_num: 2 - -smoother_server: - ros__parameters: - smoother_plugins: ["simple_smoother"] - simple_smoother: - plugin: "nav2_smoother::SimpleSmoother" - tolerance: 1.0e-10 - max_its: 1000 - do_refinement: True - -behavior_server: - ros__parameters: - local_costmap_topic: local_costmap/costmap_raw - global_costmap_topic: global_costmap/costmap_raw - local_footprint_topic: local_costmap/published_footprint - global_footprint_topic: global_costmap/published_footprint - cycle_frequency: 10.0 - behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] - spin: - plugin: "nav2_behaviors::Spin" - backup: - plugin: "nav2_behaviors::BackUp" - drive_on_heading: - plugin: "nav2_behaviors::DriveOnHeading" - wait: - plugin: "nav2_behaviors::Wait" - assisted_teleop: - plugin: "nav2_behaviors::AssistedTeleop" - local_frame: odom - global_frame: map - robot_base_frame: base_link - transform_tolerance: 0.1 - simulate_ahead_time: 2.0 - max_rotational_vel: 1.0 - min_rotational_vel: 0.4 - rotational_acc_lim: 3.2 - -waypoint_follower: - ros__parameters: - loop_rate: 20 - stop_on_failure: false - action_server_result_timeout: 900.0 - waypoint_task_executor_plugin: "wait_at_waypoint" - wait_at_waypoint: - plugin: "nav2_waypoint_follower::WaitAtWaypoint" - enabled: True - waypoint_pause_duration: 200 - -velocity_smoother: - ros__parameters: - smoothing_frequency: 20.0 - scale_velocities: False - feedback: "OPEN_LOOP" - max_velocity: [0.5, 0.0, 2.0] - min_velocity: [-0.5, 0.0, -2.0] - max_accel: [2.5, 0.0, 3.2] - max_decel: [-2.5, 0.0, -3.2] - odom_topic: "odom" - odom_duration: 0.1 - deadband_velocity: [0.0, 0.0, 0.0] - velocity_timeout: 1.0 - -collision_monitor: - ros__parameters: - base_frame_id: "base_footprint" - odom_frame_id: "odom" - cmd_vel_in_topic: "cmd_vel_smoothed" - cmd_vel_out_topic: "cmd_vel" - state_topic: "collision_monitor_state" - transform_tolerance: 0.2 - source_timeout: 1.0 - base_shift_correction: True - stop_pub_timeout: 2.0 - # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, - # and robot footprint for "approach" action type. - polygons: ["FootprintApproach"] - FootprintApproach: - type: "polygon" - action_type: "approach" - footprint_topic: "/local_costmap/published_footprint" - time_before_collision: 1.2 - simulation_time_step: 0.1 - min_points: 6 - visualize: False - enabled: True - observation_sources: ["scan"] - scan: - type: "scan" - topic: "scan" - min_height: 0.15 - max_height: 2.0 - enabled: True - -docking_server: - ros__parameters: - controller_frequency: 50.0 - initial_perception_timeout: 5.0 - wait_charge_timeout: 5.0 - dock_approach_timeout: 30.0 - undock_linear_tolerance: 0.05 - undock_angular_tolerance: 0.1 - max_retries: 3 - base_frame: "base_link" - fixed_frame: "odom" - dock_backwards: false - dock_prestaging_tolerance: 0.5 - - # Types of docks - dock_plugins: ['simple_charging_dock'] - simple_charging_dock: - plugin: 'opennav_docking::SimpleChargingDock' - docking_threshold: 0.05 - staging_x_offset: -0.7 - use_external_detection_pose: true - use_battery_status: false # true - use_stall_detection: false # true - - external_detection_timeout: 1.0 - external_detection_translation_x: -0.18 - external_detection_translation_y: 0.0 - external_detection_rotation_roll: -1.57 - external_detection_rotation_pitch: -1.57 - external_detection_rotation_yaw: 0.0 - filter_coef: 0.1 - - # Dock instances - # The following example illustrates configuring dock instances. - # docks: ['home_dock'] # Input your docks here - # home_dock: - # type: 'simple_charging_dock' - # frame: map - # pose: [0.0, 0.0, 0.0] - - controller: - k_phi: 3.0 - k_delta: 2.0 - v_linear_min: 0.15 - v_linear_max: 0.15 - -loopback_simulator: - ros__parameters: - base_frame_id: "base_footprint" - odom_frame_id: "odom" - map_frame_id: "map" - scan_frame_id: "base_scan" # tb4_loopback_simulator.launch.py remaps to 'rplidar_link' - update_duration: 0.02 From 5ce2c7e4677e4d4394f180642926f3706ff6a387 Mon Sep 17 00:00:00 2001 From: stevedan Date: Thu, 10 Oct 2024 12:28:48 +0200 Subject: [PATCH 15/41] remove duplicate vector Signed-off-by: stevedan --- nav2_smac_planner/include/nav2_smac_planner/a_star.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp index 957830a5d05..83b9aa59673 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -25,7 +25,6 @@ #include #include #include -#include #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_core/planner_exceptions.hpp" From fe5dc61efdfbbc030ea44ed430ab7b27ef0fa1ec Mon Sep 17 00:00:00 2001 From: stevedan Date: Thu, 10 Oct 2024 12:57:00 +0200 Subject: [PATCH 16/41] code cleanup Signed-off-by: stevedan --- nav2_bringup/params/nav2_params.yaml | 62 +-------------------- nav2_bringup/rviz/nav2_default_view.rviz | 4 +- nav2_rviz_plugins/CMakeLists.txt | 1 - nav2_rviz_plugins/package.xml | 1 - tools/planner_benchmarking/metrics.py | 2 +- tools/planner_benchmarking/planners.pickle | Bin 232 -> 0 bytes 6 files changed, 4 insertions(+), 66 deletions(-) delete mode 100644 tools/planner_benchmarking/planners.pickle diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 2027f4c0e1c..f8fa6a33b58 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -68,7 +68,6 @@ bt_navigator: controller_server: ros__parameters: controller_frequency: 20.0 - costmap_update_timeout: 0.30 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 @@ -99,10 +98,6 @@ controller_server: time_steps: 56 model_dt: 0.05 batch_size: 2000 - ax_max: 3.0 - ax_min: -3.0 - ay_max: 3.0 - az_max: 3.5 vx_std: 0.2 vy_std: 0.2 wz_std: 0.4 @@ -277,7 +272,6 @@ planner_server: ros__parameters: expected_planner_frequency: 20.0 planner_plugins: ["GridBased"] - costmap_update_timeout: 1.0 GridBased: plugin: "nav2_navfn_planner::NavfnPlanner" tolerance: 0.5 @@ -374,58 +368,4 @@ collision_monitor: topic: "scan" min_height: 0.15 max_height: 2.0 - enabled: True - -docking_server: - ros__parameters: - controller_frequency: 50.0 - initial_perception_timeout: 5.0 - wait_charge_timeout: 5.0 - dock_approach_timeout: 30.0 - undock_linear_tolerance: 0.05 - undock_angular_tolerance: 0.1 - max_retries: 3 - base_frame: "base_link" - fixed_frame: "odom" - dock_backwards: false - dock_prestaging_tolerance: 0.5 - - # Types of docks - dock_plugins: ['simple_charging_dock'] - simple_charging_dock: - plugin: 'opennav_docking::SimpleChargingDock' - docking_threshold: 0.05 - staging_x_offset: -0.7 - use_external_detection_pose: true - use_battery_status: false # true - use_stall_detection: false # true - - external_detection_timeout: 1.0 - external_detection_translation_x: -0.18 - external_detection_translation_y: 0.0 - external_detection_rotation_roll: -1.57 - external_detection_rotation_pitch: -1.57 - external_detection_rotation_yaw: 0.0 - filter_coef: 0.1 - - # Dock instances - # The following example illustrates configuring dock instances. - # docks: ['home_dock'] # Input your docks here - # home_dock: - # type: 'simple_charging_dock' - # frame: map - # pose: [0.0, 0.0, 0.0] - - controller: - k_phi: 3.0 - k_delta: 2.0 - v_linear_min: 0.15 - v_linear_max: 0.15 - -loopback_simulator: - ros__parameters: - base_frame_id: "base_footprint" - odom_frame_id: "odom" - map_frame_id: "map" - scan_frame_id: "base_scan" # tb4_loopback_simulator.launch.py remaps to 'rplidar_link' - update_duration: 0.02 + enabled: True \ No newline at end of file diff --git a/nav2_bringup/rviz/nav2_default_view.rviz b/nav2_bringup/rviz/nav2_default_view.rviz index 0fc84fd890f..c8dd3768583 100644 --- a/nav2_bringup/rviz/nav2_default_view.rviz +++ b/nav2_bringup/rviz/nav2_default_view.rviz @@ -25,8 +25,8 @@ Panels: Name: Navigation 2 - Class: nav2_rviz_plugins/Selector Name: Selector - # - Class: nav2_rviz_plugins/Docking - # Name: Docking + - Class: nav2_rviz_plugins/Docking + Name: Docking Visualization Manager: Class: "" Displays: diff --git a/nav2_rviz_plugins/CMakeLists.txt b/nav2_rviz_plugins/CMakeLists.txt index b03ead1b04f..bd9059d6989 100644 --- a/nav2_rviz_plugins/CMakeLists.txt +++ b/nav2_rviz_plugins/CMakeLists.txt @@ -21,7 +21,6 @@ find_package(tf2_geometry_msgs REQUIRED) find_package(visualization_msgs REQUIRED) find_package(yaml_cpp_vendor REQUIRED) find_package(yaml-cpp REQUIRED) -find_package(backward_ros REQUIRED) nav2_package() diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index 5ae2faafde7..c90944df774 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -27,7 +27,6 @@ tf2_geometry_msgs visualization_msgs yaml_cpp_vendor - backward_ros libqt5-core libqt5-gui diff --git a/tools/planner_benchmarking/metrics.py b/tools/planner_benchmarking/metrics.py index 79837d143d4..72bbdb687ad 100644 --- a/tools/planner_benchmarking/metrics.py +++ b/tools/planner_benchmarking/metrics.py @@ -106,7 +106,7 @@ def main(): costmap = np.asarray(costmap_msg.data) costmap.resize(costmap_msg.metadata.size_y, costmap_msg.metadata.size_x) - planners = ["DEFAULT_Smac2d", "BIDIRECTIONAL_Smac2d", "ALL_DIRECTION_Smac2d", "DEFAULT_SmacHybrid", "BIDIRECTIONAL_SmacHybrid", "ALL_DIRECTION_SmacHybrid", "DEFAULT_SmacLattice", "BIDIRECTIONAL_SmacLattice", "ALL_DIRECTION_SmacLattice"] + planners = ['Navfn', 'ThetaStar', 'SmacHybrid', 'Smac2d', 'SmacLattice'] max_cost = 210 side_buffer = 100 time_stamp = navigator.get_clock().now().to_msg() diff --git a/tools/planner_benchmarking/planners.pickle b/tools/planner_benchmarking/planners.pickle deleted file mode 100644 index 62eb3138eddc1a3acafaf8cbb65878ca66c98885..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 232 zcmZo*oqCr60%E6V^zga3x;cjWgv1BuCMFxDOz9DE@^tYGa&->z^!Ic0K?png_{75{ x;KD-4+C3_hiZX#lNMITPm%=m#CMAq)luu$wNoI2DlpaY;vk-EahQZ`g^#JciO?v Date: Thu, 10 Oct 2024 12:57:57 +0200 Subject: [PATCH 17/41] missing param Signed-off-by: stevedan --- nav2_bringup/params/nav2_params.yaml | 62 +++++++++++++++++++++++++++- 1 file changed, 61 insertions(+), 1 deletion(-) diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index f8fa6a33b58..467bad805d5 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -68,6 +68,7 @@ bt_navigator: controller_server: ros__parameters: controller_frequency: 20.0 + costmap_update_timeout: 0.30 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 @@ -98,6 +99,10 @@ controller_server: time_steps: 56 model_dt: 0.05 batch_size: 2000 + ax_max: 3.0 + ax_min: -3.0 + ay_max: 3.0 + az_max: 3.5 vx_std: 0.2 vy_std: 0.2 wz_std: 0.4 @@ -272,6 +277,7 @@ planner_server: ros__parameters: expected_planner_frequency: 20.0 planner_plugins: ["GridBased"] + costmap_update_timeout: 1.0 GridBased: plugin: "nav2_navfn_planner::NavfnPlanner" tolerance: 0.5 @@ -368,4 +374,58 @@ collision_monitor: topic: "scan" min_height: 0.15 max_height: 2.0 - enabled: True \ No newline at end of file + enabled: True + +docking_server: + ros__parameters: + controller_frequency: 50.0 + initial_perception_timeout: 5.0 + wait_charge_timeout: 5.0 + dock_approach_timeout: 30.0 + undock_linear_tolerance: 0.05 + undock_angular_tolerance: 0.1 + max_retries: 3 + base_frame: "base_link" + fixed_frame: "odom" + dock_backwards: false + dock_prestaging_tolerance: 0.5 + + # Types of docks + dock_plugins: ['simple_charging_dock'] + simple_charging_dock: + plugin: 'opennav_docking::SimpleChargingDock' + docking_threshold: 0.05 + staging_x_offset: -0.7 + use_external_detection_pose: true + use_battery_status: false # true + use_stall_detection: false # true + + external_detection_timeout: 1.0 + external_detection_translation_x: -0.18 + external_detection_translation_y: 0.0 + external_detection_rotation_roll: -1.57 + external_detection_rotation_pitch: -1.57 + external_detection_rotation_yaw: 0.0 + filter_coef: 0.1 + + # Dock instances + # The following example illustrates configuring dock instances. + # docks: ['home_dock'] # Input your docks here + # home_dock: + # type: 'simple_charging_dock' + # frame: map + # pose: [0.0, 0.0, 0.0] + + controller: + k_phi: 3.0 + k_delta: 2.0 + v_linear_min: 0.15 + v_linear_max: 0.15 + +loopback_simulator: + ros__parameters: + base_frame_id: "base_footprint" + odom_frame_id: "odom" + map_frame_id: "map" + scan_frame_id: "base_scan" # tb4_loopback_simulator.launch.py remaps to 'rplidar_link' + update_duration: 0.02 \ No newline at end of file From 5939bf286d1cc5ca26d49f547be7301d455753c9 Mon Sep 17 00:00:00 2001 From: stevedan Date: Thu, 10 Oct 2024 13:01:35 +0200 Subject: [PATCH 18/41] insert missing end of file newline Signed-off-by: stevedan --- nav2_bringup/params/nav2_params.yaml | 2 +- nav2_rviz_plugins/src/costmap_cost_tool.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 467bad805d5..2027f4c0e1c 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -428,4 +428,4 @@ loopback_simulator: odom_frame_id: "odom" map_frame_id: "map" scan_frame_id: "base_scan" # tb4_loopback_simulator.launch.py remaps to 'rplidar_link' - update_duration: 0.02 \ No newline at end of file + update_duration: 0.02 diff --git a/nav2_rviz_plugins/src/costmap_cost_tool.cpp b/nav2_rviz_plugins/src/costmap_cost_tool.cpp index 47d3caa5e07..03435a913e4 100644 --- a/nav2_rviz_plugins/src/costmap_cost_tool.cpp +++ b/nav2_rviz_plugins/src/costmap_cost_tool.cpp @@ -138,4 +138,4 @@ void CostmapCostTool::handleGlobalCostResponse( } // namespace nav2_rviz_plugins #include -PLUGINLIB_EXPORT_CLASS(nav2_rviz_plugins::CostmapCostTool, rviz_common::Tool) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(nav2_rviz_plugins::CostmapCostTool, rviz_common::Tool) From cd631f527e58e81c2fec687c7af23bf6e746aa9a Mon Sep 17 00:00:00 2001 From: stevedan Date: Thu, 10 Oct 2024 13:02:26 +0200 Subject: [PATCH 19/41] remove extra space Signed-off-by: stevedan --- tools/planner_benchmarking/metrics.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tools/planner_benchmarking/metrics.py b/tools/planner_benchmarking/metrics.py index 72bbdb687ad..0ae0f883efe 100644 --- a/tools/planner_benchmarking/metrics.py +++ b/tools/planner_benchmarking/metrics.py @@ -106,7 +106,7 @@ def main(): costmap = np.asarray(costmap_msg.data) costmap.resize(costmap_msg.metadata.size_y, costmap_msg.metadata.size_x) - planners = ['Navfn', 'ThetaStar', 'SmacHybrid', 'Smac2d', 'SmacLattice'] + planners = ['Navfn', 'ThetaStar', 'SmacHybrid', 'Smac2d', 'SmacLattice'] max_cost = 210 side_buffer = 100 time_stamp = navigator.get_clock().now().to_msg() From c0ac1815a37e652074805064f08e44e439e8d5cd Mon Sep 17 00:00:00 2001 From: Stevedan Ogochukwu Omodolor <61468301+stevedanomodolor@users.noreply.github.com> Date: Thu, 17 Oct 2024 08:30:42 +0200 Subject: [PATCH 20/41] Update nav2_smac_planner/src/a_star.cpp Co-authored-by: Steve Macenski Signed-off-by: Stevedan Ogochukwu Omodolor <61468301+stevedanomodolor@users.noreply.github.com> --- nav2_smac_planner/src/a_star.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index 679676ac7c2..6a7e3fcc8bc 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -193,7 +193,7 @@ void AStarAlgorithm::setGoal( const float & mx, const float & my, const unsigned int & dim_3, - const GoalHeadingMode & goal_heading_mode) + const GoalHeadingMode & /*goal_heading_mode*/) { (void) goal_heading_mode; if (dim_3 != 0) { From b18ba842ac4da830b4cf4f1a515649613de71570 Mon Sep 17 00:00:00 2001 From: Stevedan Ogochukwu Omodolor <61468301+stevedanomodolor@users.noreply.github.com> Date: Thu, 17 Oct 2024 08:33:16 +0200 Subject: [PATCH 21/41] Update a_star.cpp Remove need for void --- nav2_smac_planner/src/a_star.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index 6a7e3fcc8bc..e4bb50d2d9a 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -195,7 +195,6 @@ void AStarAlgorithm::setGoal( const unsigned int & dim_3, const GoalHeadingMode & /*goal_heading_mode*/) { - (void) goal_heading_mode; if (dim_3 != 0) { throw std::runtime_error("Node type Node2D cannot be given non-zero goal dim 3."); } From 66537c840db969112001946e5ead9ff14ba4b628 Mon Sep 17 00:00:00 2001 From: stevedan Date: Mon, 18 Nov 2024 19:29:50 +0100 Subject: [PATCH 22/41] fix https://github.com/ros-navigation/navigation2/pull/4127\#discussion_r1803809978 Signed-off-by: stevedan --- nav2_smac_planner/src/a_star.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index e4bb50d2d9a..ba8052132f1 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -262,9 +262,8 @@ void AStarAlgorithm::setGoal( // we just have to check whether the x and y are the same because the dim3 is not used // in the computation of the obstacle heuristic - if (!_search_info.cache_obstacle_heuristic || - (goals_coordinates[0].x != _goals_coordinates[0].x && - goals_coordinates[0].y != _goals_coordinates[0].y)) + if (!_search_info.cache_obstacle_heuristic || + goals_coordinates != _goals_coordinates) { if (!_start) { throw std::runtime_error("Start must be set before goal."); From e06f1db001f89b867cbb5eb8116f53517e85df1e Mon Sep 17 00:00:00 2001 From: stevedan Date: Mon, 18 Nov 2024 19:36:13 +0100 Subject: [PATCH 23/41] remove unnecessary local variable Signed-off-by: stevedan --- nav2_smac_planner/src/smac_planner_hybrid.cpp | 6 ++---- nav2_smac_planner/src/smac_planner_lattice.cpp | 7 ++----- 2 files changed, 4 insertions(+), 9 deletions(-) diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index 6ea7709bfed..84152d21c3e 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -170,13 +170,11 @@ void SmacPlannerHybrid::configure( nav2_util::declare_parameter_if_not_declared( node, name + ".goal_heading_mode", rclcpp::ParameterValue("DEFAULT")); node->get_parameter(name + ".goal_heading_mode", goal_heading_type); - GoalHeadingMode goal_heading_mode = fromStringToGH(goal_heading_type); - if (goal_heading_mode == GoalHeadingMode::UNKNOWN) { + _goal_heading_mode = fromStringToGH(goal_heading_type); + if (_goal_heading_mode == GoalHeadingMode::UNKNOWN) { std::string error_msg = "Unable to get GoalHeader type. Given '" + goal_heading_type + "' " "Valid options are DEFAULT, BIDIRECTIONAL, ALL_DIRECTION. "; throw nav2_core::PlannerException(error_msg); - } else { - _goal_heading_mode = goal_heading_mode; } _motion_model = fromString(_motion_model_for_search); diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index 0c36a0184cf..ddc5a5c90b6 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -145,14 +145,11 @@ void SmacPlannerLattice::configure( nav2_util::declare_parameter_if_not_declared( node, name + ".goal_heading_mode", rclcpp::ParameterValue("DEFAULT")); node->get_parameter(name + ".goal_heading_mode", goal_heading_type); - GoalHeadingMode goal_heading_mode = fromStringToGH(goal_heading_type); - goal_heading_mode = fromStringToGH(goal_heading_type); - if (goal_heading_mode == GoalHeadingMode::UNKNOWN) { + _goal_heading_mode = fromStringToGH(goal_heading_type); + if (_goal_heading_mode == GoalHeadingMode::UNKNOWN) { std::string error_msg = "Unable to get GoalHeader type. Given '" + goal_heading_type + "' " "Valid options are DEFAULT, BIDIRECTIONAL, ALL_DIRECTION. "; throw nav2_core::PlannerException(error_msg); - } else { - _goal_heading_mode = goal_heading_mode; } _metadata = LatticeMotionTable::getLatticeMetadata(_search_info.lattice_filepath); From d5a377626477886cefed7dec20671416c970728f Mon Sep 17 00:00:00 2001 From: stevedan Date: Mon, 18 Nov 2024 19:38:10 +0100 Subject: [PATCH 24/41] dont need to break after throw Signed-off-by: stevedan --- nav2_smac_planner/src/a_star.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index ba8052132f1..cfff36adf9d 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -257,7 +257,6 @@ void AStarAlgorithm::setGoal( break; case GoalHeadingMode::UNKNOWN: throw std::runtime_error("Goal heading is UNKNOWN."); - break; } // we just have to check whether the x and y are the same because the dim3 is not used From 3acc50e50f04ead5919f556af3a004221e755ab0 Mon Sep 17 00:00:00 2001 From: stevedan Date: Mon, 18 Nov 2024 19:39:52 +0100 Subject: [PATCH 25/41] readd removed line Signed-off-by: stevedan --- nav2_smac_planner/src/a_star.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index cfff36adf9d..d895984b94c 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -291,6 +291,7 @@ bool AStarAlgorithm::areInputsValid() if (!_start || _goalsSet.empty()) { throw std::runtime_error("Failed to compute path, no valid start or goal given."); } + // Check if ending point is valid if (getToleranceHeuristic() < 0.001) { // if a node is not valid, prune it from the goals set From 9419977a7fddef6a55501050b0d1f8a7cf2dbc86 Mon Sep 17 00:00:00 2001 From: stevedan Date: Mon, 18 Nov 2024 19:41:23 +0100 Subject: [PATCH 26/41] readd removed line Signed-off-by: stevedan --- nav2_smac_planner/src/analytic_expansion.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp index 28766eb60da..9b9a65967c6 100644 --- a/nav2_smac_planner/src/analytic_expansion.cpp +++ b/nav2_smac_planner/src/analytic_expansion.cpp @@ -74,6 +74,7 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic int desired_iterations = std::max( static_cast(closest_distance / _search_info.analytic_expansion_ratio), static_cast(std::ceil(_search_info.analytic_expansion_ratio))); + // If we are closer now, we should update the target number of iterations to go analytic_iterations = std::min(analytic_iterations, desired_iterations); From 18dba40c481da4fc83fa2d1b77dca2871cad3e88 Mon Sep 17 00:00:00 2001 From: stevedan Date: Mon, 18 Nov 2024 19:43:41 +0100 Subject: [PATCH 27/41] move line of code out of loop Signed-off-by: stevedan --- nav2_smac_planner/src/analytic_expansion.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp index 9b9a65967c6..86fc0f44964 100644 --- a/nav2_smac_planner/src/analytic_expansion.cpp +++ b/nav2_smac_planner/src/analytic_expansion.cpp @@ -82,9 +82,11 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic // Always run the expansion on the first run in case there is a // trivial path to be found if (analytic_iterations <= 0) { + + // Reset the counter and try the analytic path expansion + analytic_iterations = desired_iterations; + for (auto goal_node : goals_node) { - // Reset the counter and try the analytic path expansion - analytic_iterations = desired_iterations; AnalyticExpansionNodes analytic_nodes = getAnalyticPath( current_node, goal_node, getter, From 7c1f439c26a72ba768648854f212016c252e8d1b Mon Sep 17 00:00:00 2001 From: stevedan Date: Mon, 18 Nov 2024 20:24:08 +0100 Subject: [PATCH 28/41] linter fixes Signed-off-by: stevedan --- nav2_smac_planner/src/a_star.cpp | 6 +++--- nav2_smac_planner/src/analytic_expansion.cpp | 5 ++--- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index d895984b94c..d09fc923938 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -261,8 +261,8 @@ void AStarAlgorithm::setGoal( // we just have to check whether the x and y are the same because the dim3 is not used // in the computation of the obstacle heuristic - if (!_search_info.cache_obstacle_heuristic || - goals_coordinates != _goals_coordinates) + if (!_search_info.cache_obstacle_heuristic || + goals_coordinates != _goals_coordinates) { if (!_start) { throw std::runtime_error("Start must be set before goal."); @@ -291,7 +291,7 @@ bool AStarAlgorithm::areInputsValid() if (!_start || _goalsSet.empty()) { throw std::runtime_error("Failed to compute path, no valid start or goal given."); } - + // Check if ending point is valid if (getToleranceHeuristic() < 0.001) { // if a node is not valid, prune it from the goals set diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp index 86fc0f44964..25d5b4aa42a 100644 --- a/nav2_smac_planner/src/analytic_expansion.cpp +++ b/nav2_smac_planner/src/analytic_expansion.cpp @@ -74,7 +74,7 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic int desired_iterations = std::max( static_cast(closest_distance / _search_info.analytic_expansion_ratio), static_cast(std::ceil(_search_info.analytic_expansion_ratio))); - + // If we are closer now, we should update the target number of iterations to go analytic_iterations = std::min(analytic_iterations, desired_iterations); @@ -82,10 +82,9 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic // Always run the expansion on the first run in case there is a // trivial path to be found if (analytic_iterations <= 0) { - // Reset the counter and try the analytic path expansion analytic_iterations = desired_iterations; - + for (auto goal_node : goals_node) { AnalyticExpansionNodes analytic_nodes = getAnalyticPath( From dd008b8d79804d7957a1cd3455e79832e09bfbf4 Mon Sep 17 00:00:00 2001 From: Saitama Date: Tue, 15 Oct 2024 19:55:35 +0200 Subject: [PATCH 29/41] [RotationShimController] fix: rotate on short paths (#4716) Add header data to goal for short paths. Commit d8ae3c1f9b8233e86ed54dfbe615b1ba56b51b6d added the possibility to the rotation shim controller to rotate towards the goal when the goal was closer that the `forward_sampling_distance`. This feature was not fully working as the goal was missing proper header data, causing the rotation shim to give back control to the main controller. Co-authored-by: agennart --- .../src/nav2_rotation_shim_controller.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp index 5947f5e36e4..5b2bf306df0 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -253,7 +253,10 @@ geometry_msgs::msg::PoseStamped RotationShimController::getSampledPathPt() } } - return current_path_.poses.back(); + auto goal = current_path_.poses.back(); + goal.header.frame_id = current_path_.header.frame_id; + goal.header.stamp = clock_->now(); + return goal; } geometry_msgs::msg::PoseStamped RotationShimController::getSampledPathGoal() From 56b497570ea8987647b467741c6981719076fe21 Mon Sep 17 00:00:00 2001 From: Alberto Tudela Date: Tue, 15 Oct 2024 23:32:08 +0200 Subject: [PATCH 30/41] Improve Docking panel (#4717) * Added load and save panel Signed-off-by: Alberto Tudela * Improved dock_panel state machine Signed-off-by: Alberto Tudela * Added loading dock plugins log Signed-off-by: Alberto Tudela * Redo UI Signed-off-by: Alberto Tudela * Update tooltips Signed-off-by: Alberto Tudela * Fix null-dereference Signed-off-by: Alberto Tudela --------- Signed-off-by: Alberto Tudela --- .../nav2_rviz_plugins/docking_panel.hpp | 110 ++++-- nav2_rviz_plugins/src/docking_panel.cpp | 315 +++++++++++++----- nav2_rviz_plugins/src/utils.cpp | 4 +- 3 files changed, 317 insertions(+), 112 deletions(-) diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp index 868c644e4b1..f73f21952be 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp @@ -23,6 +23,7 @@ #include // ROS +#include "nav2_lifecycle_manager/lifecycle_manager_client.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" #include "rviz_common/panel.hpp" @@ -37,6 +38,9 @@ class QPushButton; namespace nav2_rviz_plugins { +class InitialDockThread; + +/// Panel to interface to the docking server class DockingPanel : public rviz_common::Panel { Q_OBJECT @@ -44,11 +48,20 @@ class DockingPanel : public rviz_common::Panel public: explicit DockingPanel(QWidget * parent = 0); virtual ~DockingPanel(); + void onInitialize() override; + /// Load and save configuration data + void load(const rviz_common::Config & config) override; + void save(rviz_common::Config config) const override; + private Q_SLOTS: + void startThread(); + void onStartup(); void onDockingButtonPressed(); void onUndockingButtonPressed(); + void onCancelDocking(); + void onCancelUndocking(); void dockIdCheckbox(); private: @@ -57,14 +70,6 @@ private Q_SLOTS: using DockGoalHandle = rclcpp_action::ClientGoalHandle; using UndockGoalHandle = rclcpp_action::ClientGoalHandle; - // Start the actions - void startDocking(); - void startUndocking(); - - // Cancel the actions - void cancelDocking(); - void cancelUndocking(); - // The (non-spinning) client node used to invoke the action client void timerEvent(QTimerEvent * event) override; @@ -84,14 +89,33 @@ private Q_SLOTS: // The (non-spinning) client node used to invoke the action client rclcpp::Node::SharedPtr client_node_; + + // The Node pointer that we need to keep alive for the duration of this plugin. + std::shared_ptr node_ptr_; + // Timeout value when waiting for action servers to respond std::chrono::milliseconds server_timeout_; + // A timer used to check on the completion status of the action + QBasicTimer action_timer_; + + // The Dock and Undock action client + rclcpp_action::Client::SharedPtr dock_client_; + rclcpp_action::Client::SharedPtr undock_client_; + + // Docking / Undocking action feedback subscribers + rclcpp::Subscription::SharedPtr docking_feedback_sub_; + rclcpp::Subscription::SharedPtr undocking_feedback_sub_; + rclcpp::Subscription::SharedPtr docking_goal_status_sub_; + rclcpp::Subscription::SharedPtr undocking_goal_status_sub_; + + // Goal related state + DockGoalHandle::SharedPtr dock_goal_handle_; + UndockGoalHandle::SharedPtr undock_goal_handle_; + // Flags to indicate if the plugins have been loaded bool plugins_loaded_ = false; bool server_failed_ = false; - bool tried_once_ = false; - QBasicTimer timer_; QVBoxLayout * main_layout_{nullptr}; QHBoxLayout * info_layout_{nullptr}; @@ -121,20 +145,62 @@ private Q_SLOTS: bool undocking_in_progress_ = false; bool use_dock_id_ = false; - // The Dock and Undock action client - rclcpp_action::Client::SharedPtr dock_client_; - rclcpp_action::Client::SharedPtr undock_client_; - DockGoalHandle::SharedPtr dock_goal_handle_; - UndockGoalHandle::SharedPtr undock_goal_handle_; + QStateMachine state_machine_; + InitialDockThread * initial_thread_; - // The Node pointer that we need to keep alive for the duration of this plugin. - std::shared_ptr node_ptr_; + QState * pre_initial_{nullptr}; + QState * idle_{nullptr}; + QState * docking_{nullptr}; + QState * undocking_{nullptr}; + QState * canceled_docking_{nullptr}; + QState * canceled_undocking_{nullptr}; +}; - // Docking / Undocking action feedback subscribers - rclcpp::Subscription::SharedPtr docking_feedback_sub_; - rclcpp::Subscription::SharedPtr undocking_feedback_sub_; - rclcpp::Subscription::SharedPtr docking_goal_status_sub_; - rclcpp::Subscription::SharedPtr undocking_goal_status_sub_; +class InitialDockThread : public QThread +{ + Q_OBJECT + +public: + explicit InitialDockThread( + rclcpp_action::Client::SharedPtr & dock_client, + rclcpp_action::Client::SharedPtr & undock_client) + : dock_client_(dock_client), undock_client_(undock_client) + {} + + void run() override + { + while (!dock_active_) { + dock_active_ = dock_client_->wait_for_action_server(std::chrono::seconds(1)); + } + + while (!undock_active_) { + undock_active_ = undock_client_->wait_for_action_server(std::chrono::seconds(1)); + } + + if (dock_active_) { + emit dockingActive(); + } else { + emit dockingInactive(); + } + + if (undock_active_) { + emit undockingActive(); + } else { + emit undockingInactive(); + } + } + +signals: + void dockingActive(); + void dockingInactive(); + void undockingActive(); + void undockingInactive(); + +private: + rclcpp_action::Client::SharedPtr dock_client_; + rclcpp_action::Client::SharedPtr undock_client_; + bool dock_active_ = false; + bool undock_active_ = false; }; } // namespace nav2_rviz_plugins diff --git a/nav2_rviz_plugins/src/docking_panel.cpp b/nav2_rviz_plugins/src/docking_panel.cpp index 7daa327079c..e43f718ebee 100644 --- a/nav2_rviz_plugins/src/docking_panel.cpp +++ b/nav2_rviz_plugins/src/docking_panel.cpp @@ -29,6 +29,7 @@ #include "nav2_util/geometry_utils.hpp" #include "nav2_rviz_plugins/docking_panel.hpp" +#include "nav2_rviz_plugins/ros_action_qevent.hpp" #include "nav2_rviz_plugins/utils.hpp" using namespace std::chrono_literals; @@ -40,8 +41,7 @@ DockingPanel::DockingPanel(QWidget * parent) : Panel(parent), server_timeout_(100) { - client_node_ = std::make_shared("nav2_rviz_docking_panel_node"); - + // Create the control buttons and its tooltip main_layout_ = new QVBoxLayout; info_layout_ = new QHBoxLayout; feedback_layout_ = new QVBoxLayout; @@ -50,8 +50,8 @@ DockingPanel::DockingPanel(QWidget * parent) dock_pose_layout_ = new QHBoxLayout; nav_stage_layout_ = new QHBoxLayout; dock_type_ = new QComboBox; - docking_button_ = new QPushButton("Dock robot"); - undocking_button_ = new QPushButton("Undock robot"); + docking_button_ = new QPushButton; + undocking_button_ = new QPushButton; docking_goal_status_indicator_ = new QLabel; docking_feedback_indicator_ = new QLabel; docking_result_indicator_ = new QLabel; @@ -62,27 +62,147 @@ DockingPanel::DockingPanel(QWidget * parent) dock_pose_y_ = new QLineEdit; dock_pose_yaw_ = new QLineEdit; - docking_button_->setEnabled(false); - undocking_button_->setEnabled(false); - use_dock_id_checkbox_->setEnabled(false); - nav_to_staging_checkbox_->setEnabled(false); - dock_id_->setEnabled(false); - dock_pose_x_->setEnabled(false); - dock_pose_y_->setEnabled(false); - dock_pose_yaw_->setEnabled(false); + // Create the state machine used to present the proper control button states in the UI + const char * nav_to_stage_msg = "Navigate to the staging pose before docking"; + const char * use_dock_id_msg = "Use the dock id or the dock pose to dock the robot"; + const char * dock_msg = "Dock the robot at the specified docking station"; + const char * undock_msg = "Undock the robot from the docking station"; + const char * cancel_dock_msg = "Cancel the current docking action"; + const char * cancel_undock_msg = "Cancel the current undocking action"; + docking_goal_status_indicator_->setText(nav2_rviz_plugins::getGoalStatusLabel()); docking_feedback_indicator_->setText(getDockFeedbackLabel()); docking_goal_status_indicator_->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed); docking_feedback_indicator_->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed); - nav_to_staging_checkbox_->setFixedWidth(150); + pre_initial_ = new QState(); + pre_initial_->setObjectName("pre_initial"); + pre_initial_->assignProperty(docking_button_, "text", "Dock robot"); + pre_initial_->assignProperty(docking_button_, "enabled", false); + + pre_initial_->assignProperty(undocking_button_, "text", "Undock robot"); + pre_initial_->assignProperty(undocking_button_, "enabled", false); + + pre_initial_->assignProperty(nav_to_staging_checkbox_, "enabled", false); + pre_initial_->assignProperty(nav_to_staging_checkbox_, "checked", true); + pre_initial_->assignProperty(use_dock_id_checkbox_, "enabled", false); + pre_initial_->assignProperty(use_dock_id_checkbox_, "checked", true); + pre_initial_->assignProperty(dock_id_, "enabled", false); + pre_initial_->assignProperty(dock_type_, "enabled", false); + pre_initial_->assignProperty(dock_pose_x_, "enabled", false); + pre_initial_->assignProperty(dock_pose_y_, "enabled", false); + pre_initial_->assignProperty(dock_pose_yaw_, "enabled", false); + + // State entered when the docking / undocking action is not active + idle_ = new QState(); + idle_->setObjectName("idle"); + idle_->assignProperty(docking_button_, "text", "Dock robot"); + idle_->assignProperty(docking_button_, "toolTip", dock_msg); + idle_->assignProperty(docking_button_, "enabled", true); + + idle_->assignProperty(undocking_button_, "text", "Undock robot"); + idle_->assignProperty(undocking_button_, "toolTip", undock_msg); + idle_->assignProperty(undocking_button_, "enabled", true); + + idle_->assignProperty(nav_to_staging_checkbox_, "enabled", true); + idle_->assignProperty(nav_to_staging_checkbox_, "toolTip", nav_to_stage_msg); + idle_->assignProperty(use_dock_id_checkbox_, "enabled", true); + idle_->assignProperty(use_dock_id_checkbox_, "toolTip", use_dock_id_msg); + idle_->assignProperty(dock_id_, "enabled", true); + idle_->assignProperty(dock_type_, "enabled", true); + + // State entered to cancel the docking action + canceled_docking_ = new QState(); + canceled_docking_->setObjectName("canceled_docking"); + + // State entered to cancel the undocking action + canceled_undocking_ = new QState(); + canceled_undocking_->setObjectName("canceled_undocking"); + + // State entered while the docking action is active + docking_ = new QState(); + docking_->setObjectName("docking"); + docking_->assignProperty(docking_button_, "text", "Cancel docking"); + docking_->assignProperty(docking_button_, "toolTip", cancel_dock_msg); + + docking_->assignProperty(undocking_button_, "enabled", false); + + // State entered while the undocking action is active + undocking_ = new QState(); + undocking_->setObjectName("undocking"); + undocking_->assignProperty(docking_button_, "enabled", false); + + undocking_->assignProperty(undocking_button_, "text", "Cancel undocking"); + undocking_->assignProperty(undocking_button_, "toolTip", cancel_undock_msg); + + QObject::connect(docking_, SIGNAL(entered()), this, SLOT(onDockingButtonPressed())); + QObject::connect(undocking_, SIGNAL(entered()), this, SLOT(onUndockingButtonPressed())); + QObject::connect(canceled_docking_, SIGNAL(exited()), this, SLOT(onCancelDocking())); + QObject::connect(canceled_undocking_, SIGNAL(exited()), this, SLOT(onCancelUndocking())); + + // Start/Cancel button click transitions + idle_->addTransition(docking_button_, SIGNAL(clicked()), docking_); + idle_->addTransition(undocking_button_, SIGNAL(clicked()), undocking_); + docking_->addTransition(docking_button_, SIGNAL(clicked()), canceled_docking_); + undocking_->addTransition(undocking_button_, SIGNAL(clicked()), canceled_undocking_); + + // Internal state transitions + canceled_docking_->addTransition(canceled_docking_, SIGNAL(entered()), idle_); + canceled_undocking_->addTransition(canceled_undocking_, SIGNAL(entered()), idle_); + + // ROSAction Transitions: So when actions are updated remotely (failing, succeeding, etc) + // the state of the application will also update. This means that if in the processing + // states and then goes inactive, move back to the idle state. Vise versa as well. + ROSActionQTransition * idleDockTransition = new ROSActionQTransition(QActionState::INACTIVE); + idleDockTransition->setTargetState(docking_); + idle_->addTransition(idleDockTransition); + + ROSActionQTransition * idleUndockTransition = new ROSActionQTransition(QActionState::INACTIVE); + idleUndockTransition->setTargetState(undocking_); + idle_->addTransition(idleUndockTransition); + + ROSActionQTransition * dockingTransition = new ROSActionQTransition(QActionState::ACTIVE); + dockingTransition->setTargetState(idle_); + docking_->addTransition(dockingTransition); + + ROSActionQTransition * undockingTransition = new ROSActionQTransition(QActionState::ACTIVE); + undockingTransition->setTargetState(idle_); + undocking_->addTransition(undockingTransition); + + client_node_ = std::make_shared("nav2_rviz_docking_panel_node"); + + state_machine_.addState(pre_initial_); + state_machine_.addState(idle_); + state_machine_.addState(docking_); + state_machine_.addState(undocking_); + state_machine_.addState(canceled_docking_); + state_machine_.addState(canceled_undocking_); + + state_machine_.setInitialState(pre_initial_); + + // Delay starting initial thread until state machine has started or a race occurs + QObject::connect(&state_machine_, SIGNAL(started()), this, SLOT(startThread())); + state_machine_.start(); + + // Create the layout for the panel info_layout_->addWidget(docking_goal_status_indicator_); info_layout_->addWidget(docking_result_indicator_); feedback_layout_->addWidget(docking_feedback_indicator_); - dock_id_layout_->addWidget(new QLabel("Dock id")); + + QLabel * nav_stage_label = new QLabel("Nav. to staging pose"); + QLabel * dock_id_label = new QLabel("Dock id"); + QLabel * dock_type_label = new QLabel("Dock type"); + + nav_stage_label->setFixedWidth(150); + dock_id_label->setFixedWidth(150); + dock_type_label->setFixedWidth(170); + + nav_stage_layout_->addWidget(nav_stage_label); + nav_stage_layout_->addWidget(nav_to_staging_checkbox_); + dock_id_layout_->addWidget(dock_id_label); dock_id_layout_->addWidget(use_dock_id_checkbox_); dock_id_layout_->addWidget(dock_id_); - dock_type_layout_->addWidget(new QLabel("Dock type")); + dock_type_layout_->addWidget(dock_type_label); dock_type_layout_->addWidget(dock_type_); dock_pose_layout_->addWidget(new QLabel("Dock pose {X")); dock_pose_layout_->addWidget(dock_pose_x_); @@ -91,28 +211,53 @@ DockingPanel::DockingPanel(QWidget * parent) dock_pose_layout_->addWidget(new QLabel("θ")); dock_pose_layout_->addWidget(dock_pose_yaw_); dock_pose_layout_->addWidget(new QLabel("}")); - nav_stage_layout_->addWidget(nav_to_staging_checkbox_); - nav_stage_layout_->addWidget(new QLabel("Navigate to staging pose")); + + QGroupBox * group_box = new QGroupBox(); + QVBoxLayout * group_box_layout = new QVBoxLayout; + group_box_layout->addLayout(nav_stage_layout_); + group_box_layout->addLayout(dock_id_layout_); + group_box_layout->addLayout(dock_type_layout_); + group_box_layout->addLayout(dock_pose_layout_); + group_box->setLayout(group_box_layout); main_layout_->setContentsMargins(10, 10, 10, 10); main_layout_->addLayout(info_layout_); main_layout_->addLayout(feedback_layout_); - main_layout_->addLayout(nav_stage_layout_); - main_layout_->addLayout(dock_id_layout_); - main_layout_->addLayout(dock_type_layout_); - main_layout_->addLayout(dock_pose_layout_); + main_layout_->addWidget(group_box); main_layout_->addWidget(docking_button_); main_layout_->addWidget(undocking_button_); setLayout(main_layout_); - timer_.start(200, this); + action_timer_.start(200, this); dock_client_ = rclcpp_action::create_client(client_node_, "dock_robot"); undock_client_ = rclcpp_action::create_client(client_node_, "undock_robot"); + initial_thread_ = new InitialDockThread(dock_client_, undock_client_); + connect(initial_thread_, &InitialDockThread::finished, initial_thread_, &QObject::deleteLater); + + QSignalTransition * activeDockSignal = new QSignalTransition( + initial_thread_, &InitialDockThread::dockingActive); + activeDockSignal->setTargetState(idle_); + pre_initial_->addTransition(activeDockSignal); + + QSignalTransition * activeUndockSignal = new QSignalTransition( + initial_thread_, &InitialDockThread::undockingActive); + activeUndockSignal->setTargetState(idle_); + pre_initial_->addTransition(activeUndockSignal); + + QObject::connect( + initial_thread_, &InitialDockThread::dockingActive, + [this] { + // Load the plugins if not already loaded + if (!plugins_loaded_) { + RCLCPP_INFO(client_node_->get_logger(), "Loading dock plugins"); + nav2_rviz_plugins::pluginLoader( + client_node_, server_failed_, "docking_server", "dock_plugins", dock_type_); + plugins_loaded_ = true; + } + }); // Conect buttons with functions - QObject::connect(docking_button_, SIGNAL(clicked()), this, SLOT(onDockingButtonPressed())); - QObject::connect(undocking_button_, SIGNAL(clicked()), this, SLOT(onUndockingButtonPressed())); QObject::connect( use_dock_id_checkbox_, &QCheckBox::stateChanged, this, &DockingPanel::dockIdCheckbox); } @@ -135,18 +280,6 @@ void DockingPanel::onInitialize() rclcpp::SystemDefaultsQoS(), [this](const Dock::Impl::FeedbackMessage::SharedPtr msg) { docking_feedback_indicator_->setText(getDockFeedbackLabel(msg->feedback)); - docking_button_->setText("Cancel docking"); - undocking_button_->setEnabled(false); - docking_in_progress_ = true; - }); - - undocking_feedback_sub_ = node->create_subscription( - "undock_robot/_action/feedback", - rclcpp::SystemDefaultsQoS(), - [this](const Undock::Impl::FeedbackMessage::SharedPtr /*msg*/) { - docking_button_->setEnabled(false); - undocking_button_->setText("Cancel undocking"); - undocking_in_progress_ = true; }); // Create action goal status subscribers @@ -156,11 +289,6 @@ void DockingPanel::onInitialize() [this](const action_msgs::msg::GoalStatusArray::SharedPtr msg) { docking_goal_status_indicator_->setText( nav2_rviz_plugins::getGoalStatusLabel("Feedback", msg->status_list.back().status)); - if (msg->status_list.back().status != action_msgs::msg::GoalStatus::STATUS_EXECUTING) { - docking_button_->setText("Dock robot"); - undocking_button_->setEnabled(true); - docking_in_progress_ = false; - } // Reset values when action is completed if (msg->status_list.back().status == action_msgs::msg::GoalStatus::STATUS_SUCCEEDED) { docking_feedback_indicator_->setText(getDockFeedbackLabel()); @@ -173,37 +301,30 @@ void DockingPanel::onInitialize() [this](const action_msgs::msg::GoalStatusArray::SharedPtr msg) { docking_goal_status_indicator_->setText( nav2_rviz_plugins::getGoalStatusLabel("Feedback", msg->status_list.back().status)); - if (msg->status_list.back().status != action_msgs::msg::GoalStatus::STATUS_EXECUTING) { - docking_button_->setEnabled(true); - undocking_button_->setText("Undock robot"); - undocking_in_progress_ = false; - } }); } +void DockingPanel::startThread() +{ + // start initial thread now that state machine is started + initial_thread_->start(); +} + DockingPanel::~DockingPanel() { } -void DockingPanel::onDockingButtonPressed() +void DockingPanel::load(const rviz_common::Config & config) { - if (!docking_in_progress_) { - startDocking(); - } else { - cancelDocking(); - } + Panel::load(config); } -void DockingPanel::onUndockingButtonPressed() +void DockingPanel::save(rviz_common::Config config) const { - if (!undocking_in_progress_) { - startUndocking(); - } else { - cancelUndocking(); - } + Panel::save(config); } -void DockingPanel::startDocking() +void DockingPanel::onDockingButtonPressed() { auto is_action_server_ready = dock_client_->wait_for_action_server(std::chrono::seconds(5)); @@ -286,10 +407,10 @@ void DockingPanel::startDocking() return; } - timer_.start(200, this); + action_timer_.start(200, this); } -void DockingPanel::startUndocking() +void DockingPanel::onUndockingButtonPressed() { auto is_action_server_ready = undock_client_->wait_for_action_server(std::chrono::seconds(5)); @@ -344,7 +465,7 @@ void DockingPanel::startUndocking() return; } - timer_.start(200, this); + action_timer_.start(200, this); } void DockingPanel::dockIdCheckbox() @@ -364,7 +485,7 @@ void DockingPanel::dockIdCheckbox() } } -void DockingPanel::cancelDocking() +void DockingPanel::onCancelDocking() { if (dock_goal_handle_) { auto future_cancel = dock_client_->async_cancel_goal(dock_goal_handle_); @@ -377,9 +498,11 @@ void DockingPanel::cancelDocking() dock_goal_handle_.reset(); } } + + action_timer_.stop(); } -void DockingPanel::cancelUndocking() +void DockingPanel::onCancelUndocking() { if (undock_goal_handle_) { auto future_cancel = undock_client_->async_cancel_goal(undock_goal_handle_); @@ -392,35 +515,53 @@ void DockingPanel::cancelUndocking() undock_goal_handle_.reset(); } } + + action_timer_.stop(); } void DockingPanel::timerEvent(QTimerEvent * event) { - if (event->timerId() == timer_.timerId()) { - if (!plugins_loaded_) { - nav2_rviz_plugins::pluginLoader( - client_node_, server_failed_, "docking_server", "dock_plugins", dock_type_); - plugins_loaded_ = true; - docking_button_->setEnabled(true); - undocking_button_->setEnabled(true); - use_dock_id_checkbox_->setEnabled(true); - use_dock_id_checkbox_->setChecked(true); - nav_to_staging_checkbox_->setEnabled(true); - nav_to_staging_checkbox_->setChecked(true); - dock_id_->setEnabled(true); - } + if (event->timerId() == action_timer_.timerId()) { + // Check the status of the action servers + if (state_machine_.configuration().contains(docking_)) { + if (!dock_goal_handle_) { + RCLCPP_DEBUG(client_node_->get_logger(), "Waiting for Goal"); + state_machine_.postEvent(new ROSActionQEvent(QActionState::INACTIVE)); + return; + } - // Restart the timer if the one of the server fails - if (server_failed_ && !tried_once_) { - RCLCPP_INFO(client_node_->get_logger(), "Retrying to connect to the failed server."); - server_failed_ = false; - plugins_loaded_ = false; - tried_once_ = true; - timer_.start(200, this); - return; - } + rclcpp::spin_some(client_node_); + auto status = dock_goal_handle_->get_status(); - timer_.stop(); + // Check if the goal is still executing + if (status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED || + status == action_msgs::msg::GoalStatus::STATUS_EXECUTING) + { + state_machine_.postEvent(new ROSActionQEvent(QActionState::ACTIVE)); + } else { + state_machine_.postEvent(new ROSActionQEvent(QActionState::INACTIVE)); + action_timer_.stop(); + } + } else if (state_machine_.configuration().contains(undocking_)) { + if (!undock_goal_handle_) { + RCLCPP_DEBUG(client_node_->get_logger(), "Waiting for Goal"); + state_machine_.postEvent(new ROSActionQEvent(QActionState::INACTIVE)); + return; + } + + rclcpp::spin_some(client_node_); + auto status = undock_goal_handle_->get_status(); + + // Check if the goal is still executing + if (status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED || + status == action_msgs::msg::GoalStatus::STATUS_EXECUTING) + { + state_machine_.postEvent(new ROSActionQEvent(QActionState::ACTIVE)); + } else { + state_machine_.postEvent(new ROSActionQEvent(QActionState::INACTIVE)); + action_timer_.stop(); + } + } } } diff --git a/nav2_rviz_plugins/src/utils.cpp b/nav2_rviz_plugins/src/utils.cpp index 6c7fcbbad03..96cf7784bf1 100644 --- a/nav2_rviz_plugins/src/utils.cpp +++ b/nav2_rviz_plugins/src/utils.cpp @@ -38,9 +38,7 @@ void pluginLoader( RCLCPP_ERROR(node->get_logger(), "Interrupted while waiting for the service. Exiting."); rclcpp::shutdown(); } - RCLCPP_INFO( - node->get_logger(), - "%s service not available", server_name.c_str()); + RCLCPP_INFO(node->get_logger(), "%s service not available", server_name.c_str()); server_unavailable = true; server_failed = true; break; From 9d6e75a2ec90e1a9c5a2187e5b264a706e9db8df Mon Sep 17 00:00:00 2001 From: Daniil Khaninaev Date: Wed, 16 Oct 2024 23:39:12 +0300 Subject: [PATCH 31/41] Added parameter `rotate_to_heading_once` (#4721) Signed-off-by: Daniil Khaninaev --- .../nav2_rotation_shim_controller.hpp | 9 ++- .../src/nav2_rotation_shim_controller.cpp | 18 +++++- .../test/test_shim_controller.cpp | 60 ++++++++++++++++++- 3 files changed, 84 insertions(+), 3 deletions(-) diff --git a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp index d3807eeb952..abaf02c9393 100644 --- a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp +++ b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp @@ -148,6 +148,13 @@ class RotationShimController : public nav2_core::Controller const double & angular_distance_to_heading, const geometry_msgs::msg::PoseStamped & pose); + /** + * @brief Checks if the goal has changed based on the given path. + * @param path The path to compare with the current goal. + * @return True if the goal has changed, false otherwise. + */ + bool isGoalChanged(const nav_msgs::msg::Path & path); + /** * @brief Callback executed when a parameter change is detected * @param event ParameterEvent message @@ -171,7 +178,7 @@ class RotationShimController : public nav2_core::Controller double forward_sampling_distance_, angular_dist_threshold_, angular_disengage_threshold_; double rotate_to_heading_angular_vel_, max_angular_accel_; double control_duration_, simulate_ahead_time_; - bool rotate_to_goal_heading_, in_rotation_; + bool rotate_to_goal_heading_, in_rotation_, rotate_to_heading_once_; // Dynamic parameters handler std::mutex mutex_; diff --git a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp index 5b2bf306df0..bb77879c7f7 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -71,6 +71,8 @@ void RotationShimController::configure( node, plugin_name_ + ".primary_controller", rclcpp::PARAMETER_STRING); nav2_util::declare_parameter_if_not_declared( node, plugin_name_ + ".rotate_to_goal_heading", rclcpp::ParameterValue(false)); + nav2_util::declare_parameter_if_not_declared( + node, plugin_name_ + ".rotate_to_heading_once", rclcpp::ParameterValue(false)); node->get_parameter(plugin_name_ + ".angular_dist_threshold", angular_dist_threshold_); node->get_parameter(plugin_name_ + ".angular_disengage_threshold", angular_disengage_threshold_); @@ -86,6 +88,7 @@ void RotationShimController::configure( control_duration_ = 1.0 / control_frequency; node->get_parameter(plugin_name_ + ".rotate_to_goal_heading", rotate_to_goal_heading_); + node->get_parameter(plugin_name_ + ".rotate_to_heading_once", rotate_to_heading_once_); try { primary_controller_ = lp_loader_.createUniqueInstance(primary_controller); @@ -340,9 +343,20 @@ void RotationShimController::isCollisionFree( } } +bool RotationShimController::isGoalChanged(const nav_msgs::msg::Path & path) +{ + // Return true if rotating or if the current path is empty + if (in_rotation_ || current_path_.poses.empty()) { + return true; + } + + // Check if the last pose of the current and new paths differ + return current_path_.poses.back().pose != path.poses.back().pose; +} + void RotationShimController::setPlan(const nav_msgs::msg::Path & path) { - path_updated_ = true; + path_updated_ = rotate_to_heading_once_ ? isGoalChanged(path) : true; current_path_ = path; primary_controller_->setPlan(path); } @@ -377,6 +391,8 @@ RotationShimController::dynamicParametersCallback(std::vector } else if (type == ParameterType::PARAMETER_BOOL) { if (name == plugin_name_ + ".rotate_to_goal_heading") { rotate_to_goal_heading_ = parameter.as_bool(); + } else if (name == plugin_name_ + ".rotate_to_heading_once") { + rotate_to_heading_once_ = parameter.as_bool(); } } } diff --git a/nav2_rotation_shim_controller/test/test_shim_controller.cpp b/nav2_rotation_shim_controller/test/test_shim_controller.cpp index 1160a5a98af..1d63a77b47e 100644 --- a/nav2_rotation_shim_controller/test/test_shim_controller.cpp +++ b/nav2_rotation_shim_controller/test/test_shim_controller.cpp @@ -62,6 +62,11 @@ class RotationShimShim : public nav2_rotation_shim_controller::RotationShimContr return getSampledPathPt(); } + bool isGoalChangedWrapper(const nav_msgs::msg::Path & path) + { + return isGoalChanged(path); + } + geometry_msgs::msg::Pose transformPoseToBaseFrameWrapper(geometry_msgs::msg::PoseStamped pt) { return transformPoseToBaseFrame(pt); @@ -382,6 +387,57 @@ TEST(RotationShimControllerTest, computeVelocityGoalRotationTests) { EXPECT_EQ(cmd_vel.twist.angular.z, 1.8); } +TEST(RotationShimControllerTest, isGoalChangedTest) +{ + auto ctrl = std::make_shared(); + auto node = std::make_shared("ShimControllerTest"); + std::string name = "PathFollower"; + auto tf = std::make_shared(node->get_clock()); + auto listener = std::make_shared(*tf, node, true); + auto costmap = std::make_shared("fake_costmap"); + rclcpp_lifecycle::State state; + costmap->on_configure(state); + auto tf_broadcaster = std::make_shared(node); + + geometry_msgs::msg::TransformStamped transform; + transform.header.frame_id = "base_link"; + transform.child_frame_id = "odom"; + transform.transform.rotation.x = 0.0; + transform.transform.rotation.y = 0.0; + transform.transform.rotation.z = 0.0; + transform.transform.rotation.w = 1.0; + tf_broadcaster->sendTransform(transform); + + // set a valid primary controller so we can do lifecycle + node->declare_parameter( + "PathFollower.primary_controller", + std::string("nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController")); + node->declare_parameter( + "PathFollower.rotate_to_heading_once", + true); + + auto controller = std::make_shared(); + controller->configure(node, name, tf, costmap); + controller->activate(); + + nav_msgs::msg::Path path; + path.header.frame_id = "base_link"; + path.poses.resize(2); + path.poses.back().pose.position.x = 2.0; + path.poses.back().pose.position.y = 2.0; + + // Test: Current path is empty, should return true + EXPECT_EQ(controller->isGoalChangedWrapper(path), true); + + // Test: Last pose of the current path is the same, should return false + controller->setPlan(path); + EXPECT_EQ(controller->isGoalChangedWrapper(path), false); + + // Test: Last pose of the current path differs, should return true + path.poses.back().pose.position.x = 3.0; + EXPECT_EQ(controller->isGoalChangedWrapper(path), true); +} + TEST(RotationShimControllerTest, testDynamicParameter) { auto node = std::make_shared("ShimControllerTest"); @@ -412,7 +468,8 @@ TEST(RotationShimControllerTest, testDynamicParameter) rclcpp::Parameter("test.max_angular_accel", 7.0), rclcpp::Parameter("test.simulate_ahead_time", 7.0), rclcpp::Parameter("test.primary_controller", std::string("HI")), - rclcpp::Parameter("test.rotate_to_goal_heading", true)}); + rclcpp::Parameter("test.rotate_to_goal_heading", true), + rclcpp::Parameter("test.rotate_to_heading_once", true)}); rclcpp::spin_until_future_complete( node->get_node_base_interface(), @@ -424,4 +481,5 @@ TEST(RotationShimControllerTest, testDynamicParameter) EXPECT_EQ(node->get_parameter("test.max_angular_accel").as_double(), 7.0); EXPECT_EQ(node->get_parameter("test.simulate_ahead_time").as_double(), 7.0); EXPECT_EQ(node->get_parameter("test.rotate_to_goal_heading").as_bool(), true); + EXPECT_EQ(node->get_parameter("test.rotate_to_heading_once").as_bool(), true); } From d81e157de9f2e71fec9d06cafdd48a896c9fcbc3 Mon Sep 17 00:00:00 2001 From: Saitama Date: Thu, 17 Oct 2024 18:16:50 +0200 Subject: [PATCH 32/41] [RotationShimController] fix: rotate to goal heading (#4724) Add frame_id to goal when rotating towards goal heading, otherwise the transform would fail. This bug was introduced in 30e2cde by not setting the frame_id. Signed-off-by: agennart Co-authored-by: agennart --- .../src/nav2_rotation_shim_controller.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp index bb77879c7f7..f5fcf4c5b27 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -269,6 +269,7 @@ geometry_msgs::msg::PoseStamped RotationShimController::getSampledPathGoal() } auto goal = current_path_.poses.back(); + goal.header.frame_id = current_path_.header.frame_id; goal.header.stamp = clock_->now(); return goal; } From defa146d6a9d8300b51223f4c6419aeb6bdba3af Mon Sep 17 00:00:00 2001 From: stevedan Date: Mon, 18 Nov 2024 19:29:50 +0100 Subject: [PATCH 33/41] fix https://github.com/ros-navigation/navigation2/pull/4127\#discussion_r1803809978 Signed-off-by: stevedan --- nav2_smac_planner/src/a_star.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index e4bb50d2d9a..ba8052132f1 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -262,9 +262,8 @@ void AStarAlgorithm::setGoal( // we just have to check whether the x and y are the same because the dim3 is not used // in the computation of the obstacle heuristic - if (!_search_info.cache_obstacle_heuristic || - (goals_coordinates[0].x != _goals_coordinates[0].x && - goals_coordinates[0].y != _goals_coordinates[0].y)) + if (!_search_info.cache_obstacle_heuristic || + goals_coordinates != _goals_coordinates) { if (!_start) { throw std::runtime_error("Start must be set before goal."); From fd9d66ffea6ae02ded125f6577b66b2e30702950 Mon Sep 17 00:00:00 2001 From: stevedan Date: Mon, 18 Nov 2024 19:36:13 +0100 Subject: [PATCH 34/41] remove unnecessary local variable Signed-off-by: stevedan --- nav2_smac_planner/src/smac_planner_hybrid.cpp | 6 ++---- nav2_smac_planner/src/smac_planner_lattice.cpp | 7 ++----- 2 files changed, 4 insertions(+), 9 deletions(-) diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index 6ea7709bfed..84152d21c3e 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -170,13 +170,11 @@ void SmacPlannerHybrid::configure( nav2_util::declare_parameter_if_not_declared( node, name + ".goal_heading_mode", rclcpp::ParameterValue("DEFAULT")); node->get_parameter(name + ".goal_heading_mode", goal_heading_type); - GoalHeadingMode goal_heading_mode = fromStringToGH(goal_heading_type); - if (goal_heading_mode == GoalHeadingMode::UNKNOWN) { + _goal_heading_mode = fromStringToGH(goal_heading_type); + if (_goal_heading_mode == GoalHeadingMode::UNKNOWN) { std::string error_msg = "Unable to get GoalHeader type. Given '" + goal_heading_type + "' " "Valid options are DEFAULT, BIDIRECTIONAL, ALL_DIRECTION. "; throw nav2_core::PlannerException(error_msg); - } else { - _goal_heading_mode = goal_heading_mode; } _motion_model = fromString(_motion_model_for_search); diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index 0c36a0184cf..ddc5a5c90b6 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -145,14 +145,11 @@ void SmacPlannerLattice::configure( nav2_util::declare_parameter_if_not_declared( node, name + ".goal_heading_mode", rclcpp::ParameterValue("DEFAULT")); node->get_parameter(name + ".goal_heading_mode", goal_heading_type); - GoalHeadingMode goal_heading_mode = fromStringToGH(goal_heading_type); - goal_heading_mode = fromStringToGH(goal_heading_type); - if (goal_heading_mode == GoalHeadingMode::UNKNOWN) { + _goal_heading_mode = fromStringToGH(goal_heading_type); + if (_goal_heading_mode == GoalHeadingMode::UNKNOWN) { std::string error_msg = "Unable to get GoalHeader type. Given '" + goal_heading_type + "' " "Valid options are DEFAULT, BIDIRECTIONAL, ALL_DIRECTION. "; throw nav2_core::PlannerException(error_msg); - } else { - _goal_heading_mode = goal_heading_mode; } _metadata = LatticeMotionTable::getLatticeMetadata(_search_info.lattice_filepath); From 75a53aae082892fd64c5c3d51402ac7853d8e8d1 Mon Sep 17 00:00:00 2001 From: stevedan Date: Mon, 18 Nov 2024 19:38:10 +0100 Subject: [PATCH 35/41] dont need to break after throw Signed-off-by: stevedan --- nav2_smac_planner/src/a_star.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index ba8052132f1..cfff36adf9d 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -257,7 +257,6 @@ void AStarAlgorithm::setGoal( break; case GoalHeadingMode::UNKNOWN: throw std::runtime_error("Goal heading is UNKNOWN."); - break; } // we just have to check whether the x and y are the same because the dim3 is not used From 1f03314e9b9305b383aaa842c58bb6f465845970 Mon Sep 17 00:00:00 2001 From: stevedan Date: Mon, 18 Nov 2024 19:39:52 +0100 Subject: [PATCH 36/41] readd removed line Signed-off-by: stevedan --- nav2_smac_planner/src/a_star.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index cfff36adf9d..d895984b94c 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -291,6 +291,7 @@ bool AStarAlgorithm::areInputsValid() if (!_start || _goalsSet.empty()) { throw std::runtime_error("Failed to compute path, no valid start or goal given."); } + // Check if ending point is valid if (getToleranceHeuristic() < 0.001) { // if a node is not valid, prune it from the goals set From cdcabc9eaccd4203eeb1af3f29b8d011a6a321f6 Mon Sep 17 00:00:00 2001 From: stevedan Date: Mon, 18 Nov 2024 19:41:23 +0100 Subject: [PATCH 37/41] readd removed line Signed-off-by: stevedan --- nav2_smac_planner/src/analytic_expansion.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp index 28766eb60da..9b9a65967c6 100644 --- a/nav2_smac_planner/src/analytic_expansion.cpp +++ b/nav2_smac_planner/src/analytic_expansion.cpp @@ -74,6 +74,7 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic int desired_iterations = std::max( static_cast(closest_distance / _search_info.analytic_expansion_ratio), static_cast(std::ceil(_search_info.analytic_expansion_ratio))); + // If we are closer now, we should update the target number of iterations to go analytic_iterations = std::min(analytic_iterations, desired_iterations); From 124d163e60b05053ce538394551b8ef20762e676 Mon Sep 17 00:00:00 2001 From: stevedan Date: Mon, 18 Nov 2024 19:43:41 +0100 Subject: [PATCH 38/41] move line of code out of loop Signed-off-by: stevedan --- nav2_smac_planner/src/analytic_expansion.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp index 9b9a65967c6..86fc0f44964 100644 --- a/nav2_smac_planner/src/analytic_expansion.cpp +++ b/nav2_smac_planner/src/analytic_expansion.cpp @@ -82,9 +82,11 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic // Always run the expansion on the first run in case there is a // trivial path to be found if (analytic_iterations <= 0) { + + // Reset the counter and try the analytic path expansion + analytic_iterations = desired_iterations; + for (auto goal_node : goals_node) { - // Reset the counter and try the analytic path expansion - analytic_iterations = desired_iterations; AnalyticExpansionNodes analytic_nodes = getAnalyticPath( current_node, goal_node, getter, From 38b94ec25e6a998adc8d35bc31958ae3391a7b5d Mon Sep 17 00:00:00 2001 From: stevedan Date: Mon, 18 Nov 2024 20:24:08 +0100 Subject: [PATCH 39/41] linter fixes Signed-off-by: stevedan --- nav2_smac_planner/src/a_star.cpp | 6 +++--- nav2_smac_planner/src/analytic_expansion.cpp | 5 ++--- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index d895984b94c..d09fc923938 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -261,8 +261,8 @@ void AStarAlgorithm::setGoal( // we just have to check whether the x and y are the same because the dim3 is not used // in the computation of the obstacle heuristic - if (!_search_info.cache_obstacle_heuristic || - goals_coordinates != _goals_coordinates) + if (!_search_info.cache_obstacle_heuristic || + goals_coordinates != _goals_coordinates) { if (!_start) { throw std::runtime_error("Start must be set before goal."); @@ -291,7 +291,7 @@ bool AStarAlgorithm::areInputsValid() if (!_start || _goalsSet.empty()) { throw std::runtime_error("Failed to compute path, no valid start or goal given."); } - + // Check if ending point is valid if (getToleranceHeuristic() < 0.001) { // if a node is not valid, prune it from the goals set diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp index 86fc0f44964..25d5b4aa42a 100644 --- a/nav2_smac_planner/src/analytic_expansion.cpp +++ b/nav2_smac_planner/src/analytic_expansion.cpp @@ -74,7 +74,7 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic int desired_iterations = std::max( static_cast(closest_distance / _search_info.analytic_expansion_ratio), static_cast(std::ceil(_search_info.analytic_expansion_ratio))); - + // If we are closer now, we should update the target number of iterations to go analytic_iterations = std::min(analytic_iterations, desired_iterations); @@ -82,10 +82,9 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic // Always run the expansion on the first run in case there is a // trivial path to be found if (analytic_iterations <= 0) { - // Reset the counter and try the analytic path expansion analytic_iterations = desired_iterations; - + for (auto goal_node : goals_node) { AnalyticExpansionNodes analytic_nodes = getAnalyticPath( From 44ec78884ce29cec4c323b71d4474af2011ae81c Mon Sep 17 00:00:00 2001 From: stevedan Date: Mon, 18 Nov 2024 22:50:02 +0100 Subject: [PATCH 40/41] include testing Signed-off-by: stevedan --- nav2_smac_planner/src/smac_planner_hybrid.cpp | 1 + .../src/smac_planner_lattice.cpp | 1 + nav2_smac_planner/test/test_smac_hybrid.cpp | 25 ++++++++++++++++++- nav2_smac_planner/test/test_smac_lattice.cpp | 13 ++++++++++ 4 files changed, 39 insertions(+), 1 deletion(-) diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index 84152d21c3e..9b9a1213802 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -672,6 +672,7 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para "Unable to get GoalHeader type. Given '%s', " "Valid options are DEFAULT, BIDIRECTIONAL, ALL_DIRECTION. ", goal_heading_type.c_str()); + throw nav2_core::PlannerException("Invalid GoalHeadingMode type"); } else { _goal_heading_mode = goal_heading_mode; } diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index ddc5a5c90b6..23696196913 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -566,6 +566,7 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector par "Unable to get GoalHeader type. Given '%s', " "Valid options are DEFAULT, BIDIRECTIONAL, ALL_DIRECTION. ", goal_heading_type.c_str()); + throw nav2_core::PlannerException("Invalid GoalHeadingMode type"); } else { _goal_heading_mode = goal_heading_mode; } diff --git a/nav2_smac_planner/test/test_smac_hybrid.cpp b/nav2_smac_planner/test/test_smac_hybrid.cpp index f5b48935f81..e973b2f94ca 100644 --- a/nav2_smac_planner/test/test_smac_hybrid.cpp +++ b/nav2_smac_planner/test/test_smac_hybrid.cpp @@ -37,6 +37,16 @@ class RclCppFixture }; RclCppFixture g_rclcppfixture; +// Simple wrapper to be able to call a private member +class HybridWrap : public nav2_smac_planner::SmacPlannerHybrid +{ +public: + void callDynamicParams(std::vector parameters) + { + dynamicParametersCallback(parameters); + } +}; + // SMAC smoke tests for plugin-level issues rather than algorithms // (covered by more extensively testing in other files) // System tests in nav2_system_tests will actually plan with this work @@ -68,6 +78,14 @@ TEST(SmacTest, test_smac_se2) goal.pose.position.y = 1.0; goal.pose.orientation.w = 1.0; auto planner = std::make_unique(); + + // invalid goal heading mode + nodeSE2->declare_parameter("test.goal_heading_mode", std::string("UNKNOWN")); + nodeSE2->set_parameter(rclcpp::Parameter("test.goal_heading_mode", std::string("UNKNOWN"))); + EXPECT_THROW(planner->configure(nodeSE2, "test", nullptr, costmap_ros), std::runtime_error); + + // valid goal heading mode + nodeSE2->set_parameter(rclcpp::Parameter("test.goal_heading_mode", std::string("DEFAULT"))); planner->configure(nodeSE2, "test", nullptr, costmap_ros); planner->activate(); @@ -94,7 +112,7 @@ TEST(SmacTest, test_smac_se2_reconfigure) std::make_shared("global_costmap"); costmap_ros->on_configure(rclcpp_lifecycle::State()); - auto planner = std::make_unique(); + auto planner = std::make_unique(); planner->configure(nodeSE2, "test", nullptr, costmap_ros); planner->activate(); @@ -166,4 +184,9 @@ TEST(SmacTest, test_smac_se2_reconfigure) nodeSE2->get_node_base_interface(), results2); EXPECT_EQ(nodeSE2->get_parameter("resolution").as_double(), 0.2); + + // Test reconfigure with invalid goal heading mode + std::vector parameters; + parameters.push_back(rclcpp::Parameter("test.goal_heading_mode", std::string("UNKNOWN"))); + EXPECT_THROW(planner->callDynamicParams(parameters), std::runtime_error); } diff --git a/nav2_smac_planner/test/test_smac_lattice.cpp b/nav2_smac_planner/test/test_smac_lattice.cpp index 128db09fdc8..415d2021da6 100644 --- a/nav2_smac_planner/test/test_smac_lattice.cpp +++ b/nav2_smac_planner/test/test_smac_lattice.cpp @@ -73,6 +73,14 @@ TEST(SmacTest, test_smac_lattice) goal.pose.orientation.w = 1.0; auto planner = std::make_unique(); try { + // invalid goal heading mode + nodeLattice->declare_parameter("test.goal_heading_mode", std::string("UNKNOWN")); + nodeLattice->set_parameter(rclcpp::Parameter("test.goal_heading_mode", std::string("UNKNOWN"))); + EXPECT_THROW(planner->configure(nodeLattice, "test", nullptr, costmap_ros), std::runtime_error); + + // valid goal heading mode + nodeLattice->set_parameter(rclcpp::Parameter("test.goal_heading_mode", std::string("DEFAULT"))); + // Expect to throw due to invalid prims file in param planner->configure(nodeLattice, "test", nullptr, costmap_ros); } catch (...) { @@ -150,4 +158,9 @@ TEST(SmacTest, test_smac_lattice_reconfigure) std::vector parameters; parameters.push_back(rclcpp::Parameter("test.lattice_filepath", std::string("HI"))); EXPECT_THROW(planner->callDynamicParams(parameters), std::runtime_error); + + // same with invalid goal heading mode + parameters.clear(); + parameters.push_back(rclcpp::Parameter("test.goal_heading_mode", std::string("UNKNOWN"))); + EXPECT_THROW(planner->callDynamicParams(parameters), std::runtime_error); } From a573dc340bd50a54d150067c3650178ec0d9c7c2 Mon Sep 17 00:00:00 2001 From: stevedan Date: Tue, 19 Nov 2024 06:28:22 +0100 Subject: [PATCH 41/41] include more testing Signed-off-by: stevedan --- nav2_smac_planner/src/a_star.cpp | 2 +- nav2_smac_planner/test/test_a_star.cpp | 12 ++++++++++-- 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index d09fc923938..7827f090884 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -262,7 +262,7 @@ void AStarAlgorithm::setGoal( // we just have to check whether the x and y are the same because the dim3 is not used // in the computation of the obstacle heuristic if (!_search_info.cache_obstacle_heuristic || - goals_coordinates != _goals_coordinates) + (goals_coordinates != _goals_coordinates)) { if (!_start) { throw std::runtime_error("Start must be set before goal."); diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp index 22fc1deb60d..a92463d18a2 100644 --- a/nav2_smac_planner/test/test_a_star.cpp +++ b/nav2_smac_planner/test/test_a_star.cpp @@ -400,10 +400,17 @@ TEST(AStarTest, test_se2_single_pose_path) }; // functional case testing a_star.setCollisionChecker(checker.get()); + nav2_smac_planner::NodeHybrid::CoordinateVector path; + + // test with no goals set nor start + EXPECT_THROW( + a_star.createPath( + path, num_it, tolerance, + dummy_cancel_checker), std::runtime_error); + a_star.setStart(10u, 10u, 0u); // Goal is one costmap cell away a_star.setGoal(12u, 10u, 0u); - nav2_smac_planner::NodeHybrid::CoordinateVector path; EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, dummy_cancel_checker)); // Check that the path is length one @@ -491,9 +498,10 @@ TEST(AStarTest, test_goal_heading_mode) // UNKNOWN goal heading mode a_star.setCollisionChecker(checker.get()); a_star.setStart(10u, 10u, 0u); + EXPECT_THROW( a_star.setGoal( - 80u, 80u, 40u, + 80u, 80u, 10u, nav2_smac_planner::GoalHeadingMode::UNKNOWN), std::runtime_error); }