From 53a8a7bccfe0ae9243a5ad364199825b2344d765 Mon Sep 17 00:00:00 2001 From: jediofgever Date: Wed, 23 Sep 2020 13:11:27 +0800 Subject: [PATCH 1/6] nav2_waypoint_follower; Add an optional param to specify a sleep time inbetween waypoints. Signed-off-by: jediofgever --- doc/parameters/param_list.md | 2 ++ nav2_bringup/bringup/params/nav2_params.yaml | 6 ++++++ .../nav2_waypoint_follower/waypoint_follower.hpp | 1 + nav2_waypoint_follower/src/waypoint_follower.cpp | 16 +++++++++++++--- 4 files changed, 22 insertions(+), 3 deletions(-) diff --git a/doc/parameters/param_list.md b/doc/parameters/param_list.md index adcd7f2f188..ce3b8c22a48 100644 --- a/doc/parameters/param_list.md +++ b/doc/parameters/param_list.md @@ -479,6 +479,8 @@ When `planner_plugins` parameter is not overridden, the following default plugin | ----------| --------| ------------| | stop_on_failure | true | Whether to fail action task if a single waypoint fails. If false, will continue to next waypoint. | | loop_rate | 20 | Rate to check for results from current navigation task | +| sleep_time_inbetween_waypoints | 5 | Amount of time in seconds for robot to sleep/wait after each waypoint is reached. If zero, robot will directly continue to next waypoint. | + # recoveries diff --git a/nav2_bringup/bringup/params/nav2_params.yaml b/nav2_bringup/bringup/params/nav2_params.yaml index eb290d50d3c..1fe0d54d596 100644 --- a/nav2_bringup/bringup/params/nav2_params.yaml +++ b/nav2_bringup/bringup/params/nav2_params.yaml @@ -279,3 +279,9 @@ recoveries_server: robot_state_publisher: ros__parameters: use_sim_time: True + +waypoint_follower: + ros__parameters: + loop_rate: 20 + stop_on_failure: false + sleep_time_inbetween_waypoints: 5 \ No newline at end of file diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp index 83ffa14f69e..56c348d552a 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp @@ -119,6 +119,7 @@ class WaypointFollower : public nav2_util::LifecycleNode bool stop_on_failure_; ActionStatus current_goal_status_; int loop_rate_; + int sleep_time_inbetween_waypoints_; std::vector failed_ids_; }; diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 55474bd3c89..564417d5646 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -31,6 +31,7 @@ WaypointFollower::WaypointFollower() declare_parameter("stop_on_failure", true); declare_parameter("loop_rate", 20); + declare_parameter("sleep_time_inbetween_waypoints", 5); } WaypointFollower::~WaypointFollower() @@ -45,6 +46,7 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) stop_on_failure_ = get_parameter("stop_on_failure").as_bool(); loop_rate_ = get_parameter("loop_rate").as_int(); + sleep_time_inbetween_waypoints_ = get_parameter("sleep_time_inbetween_waypoints").as_int(); std::vector new_args = rclcpp::NodeOptions().arguments(); new_args.push_back("--ros-args"); @@ -182,9 +184,17 @@ WaypointFollower::followWaypoints() " moving to next.", goal_index); } } else if (current_goal_status_ == ActionStatus::SUCCEEDED) { - RCLCPP_INFO( - get_logger(), "Succeeded processing waypoint %i, " - "moving to next.", goal_index); + if (sleep_time_inbetween_waypoints_){ + RCLCPP_INFO( + get_logger(), "Succeeded processing waypoint %i, " + "sleeping for %i seconds before moving to next.", goal_index, sleep_time_inbetween_waypoints_); + rclcpp::sleep_for(std::chrono::seconds(sleep_time_inbetween_waypoints_)); + } + else { + RCLCPP_INFO( + get_logger(), "Succeeded processing waypoint %i, " + "moving to next.", goal_index); + } } if (current_goal_status_ != ActionStatus::PROCESSING && From 1500a5dba5ab01756779199f3c007a4822ae777b Mon Sep 17 00:00:00 2001 From: jediofgever Date: Fri, 25 Sep 2020 10:09:49 +0800 Subject: [PATCH 2/6] nav2_waypoint_follower; apply requested changes after first review Signed-off-by: jediofgever --- doc/parameters/param_list.md | 3 +-- nav2_bringup/bringup/params/nav2_params.yaml | 2 +- .../src/waypoint_follower.cpp | 24 +++++++++---------- 3 files changed, 14 insertions(+), 15 deletions(-) diff --git a/doc/parameters/param_list.md b/doc/parameters/param_list.md index ce3b8c22a48..24680dd96d5 100644 --- a/doc/parameters/param_list.md +++ b/doc/parameters/param_list.md @@ -479,8 +479,7 @@ When `planner_plugins` parameter is not overridden, the following default plugin | ----------| --------| ------------| | stop_on_failure | true | Whether to fail action task if a single waypoint fails. If false, will continue to next waypoint. | | loop_rate | 20 | Rate to check for results from current navigation task | -| sleep_time_inbetween_waypoints | 5 | Amount of time in seconds for robot to sleep/wait after each waypoint is reached. If zero, robot will directly continue to next waypoint. | - +| sleep_time_inbetween_waypoints | 0 | Amount of time in milliseconds, for robot to sleep/wait after each waypoint is reached. If zero, robot will directly continue to next waypoint. | # recoveries diff --git a/nav2_bringup/bringup/params/nav2_params.yaml b/nav2_bringup/bringup/params/nav2_params.yaml index 1fe0d54d596..5162ad04cd0 100644 --- a/nav2_bringup/bringup/params/nav2_params.yaml +++ b/nav2_bringup/bringup/params/nav2_params.yaml @@ -284,4 +284,4 @@ waypoint_follower: ros__parameters: loop_rate: 20 stop_on_failure: false - sleep_time_inbetween_waypoints: 5 \ No newline at end of file + sleep_time_inbetween_waypoints: 0 diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 564417d5646..e8c6ad11dde 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -31,7 +31,7 @@ WaypointFollower::WaypointFollower() declare_parameter("stop_on_failure", true); declare_parameter("loop_rate", 20); - declare_parameter("sleep_time_inbetween_waypoints", 5); + declare_parameter("sleep_time_inbetween_waypoints", 0); } WaypointFollower::~WaypointFollower() @@ -184,17 +184,17 @@ WaypointFollower::followWaypoints() " moving to next.", goal_index); } } else if (current_goal_status_ == ActionStatus::SUCCEEDED) { - if (sleep_time_inbetween_waypoints_){ - RCLCPP_INFO( - get_logger(), "Succeeded processing waypoint %i, " - "sleeping for %i seconds before moving to next.", goal_index, sleep_time_inbetween_waypoints_); - rclcpp::sleep_for(std::chrono::seconds(sleep_time_inbetween_waypoints_)); - } - else { - RCLCPP_INFO( - get_logger(), "Succeeded processing waypoint %i, " - "moving to next.", goal_index); - } + if (sleep_time_inbetween_waypoints_) { + RCLCPP_INFO( + get_logger(), "Succeeded processing waypoint %i, " + "sleeping for %i milliseconds before moving to next.", goal_index, + sleep_time_inbetween_waypoints_); + rclcpp::sleep_for(std::chrono::milliseconds(sleep_time_inbetween_waypoints_)); + } else { + RCLCPP_INFO( + get_logger(), "Succeeded processing waypoint %i, " + "moving to next.", goal_index); + } } if (current_goal_status_ != ActionStatus::PROCESSING && From b59d7afbeae9a4f204191dce2ff9a0823e019fd5 Mon Sep 17 00:00:00 2001 From: jediofgever Date: Mon, 28 Sep 2020 13:44:36 +0800 Subject: [PATCH 3/6] Introduce waypoint task execution plugin --- doc/parameters/param_list.md | 8 +- nav2_bringup/bringup/params/nav2_params.yaml | 8 +- .../task_executor_at_waypoint_arrival.hpp | 88 +++++++++++++++ nav2_waypoint_follower/CMakeLists.txt | 14 ++- .../plugins/wait_at_waypoint_arrival.hpp | 98 +++++++++++++++++ .../waypoint_follower.hpp | 15 ++- nav2_waypoint_follower/package.xml | 2 + nav2_waypoint_follower/plugins.xml | 7 ++ .../plugins/wait_at_waypoint_arrival.cpp | 100 ++++++++++++++++++ .../src/waypoint_follower.cpp | 58 +++++++--- 10 files changed, 377 insertions(+), 21 deletions(-) create mode 100644 nav2_core/include/nav2_core/task_executor_at_waypoint_arrival.hpp create mode 100644 nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint_arrival.hpp create mode 100644 nav2_waypoint_follower/plugins.xml create mode 100644 nav2_waypoint_follower/plugins/wait_at_waypoint_arrival.cpp diff --git a/doc/parameters/param_list.md b/doc/parameters/param_list.md index 24680dd96d5..c696d6506c6 100644 --- a/doc/parameters/param_list.md +++ b/doc/parameters/param_list.md @@ -479,7 +479,13 @@ When `planner_plugins` parameter is not overridden, the following default plugin | ----------| --------| ------------| | stop_on_failure | true | Whether to fail action task if a single waypoint fails. If false, will continue to next waypoint. | | loop_rate | 20 | Rate to check for results from current navigation task | -| sleep_time_inbetween_waypoints | 0 | Amount of time in milliseconds, for robot to sleep/wait after each waypoint is reached. If zero, robot will directly continue to next waypoint. | + +## task_executor_at_waypoint plugin + +* ``: Name corresponding to the `nav2_waypoint_follower::WaitAtWaypointArrival` plugin. + +| ``.enabled | false | Whether it is enabled | +| ``.waypoint_pause_duration | 0 | Amount of time in milliseconds, for robot to sleep/wait after each waypoint is reached. If zero, robot will directly continue to next waypoint. | # recoveries diff --git a/nav2_bringup/bringup/params/nav2_params.yaml b/nav2_bringup/bringup/params/nav2_params.yaml index 5162ad04cd0..868c561c31f 100644 --- a/nav2_bringup/bringup/params/nav2_params.yaml +++ b/nav2_bringup/bringup/params/nav2_params.yaml @@ -283,5 +283,9 @@ robot_state_publisher: waypoint_follower: ros__parameters: loop_rate: 20 - stop_on_failure: false - sleep_time_inbetween_waypoints: 0 + stop_on_failure: false + task_executor_at_waypoint_plugin: "task_executor_at_waypoint" + task_executor_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypointArrival" + enabled: False + waypoint_pause_duration: 0 diff --git a/nav2_core/include/nav2_core/task_executor_at_waypoint_arrival.hpp b/nav2_core/include/nav2_core/task_executor_at_waypoint_arrival.hpp new file mode 100644 index 00000000000..facd3871b21 --- /dev/null +++ b/nav2_core/include/nav2_core/task_executor_at_waypoint_arrival.hpp @@ -0,0 +1,88 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Fetullah Atas + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef NAV2_CORE__TASK_EXECUTOR_AT_WAYPOINT_ARRIVAL_HPP_ +#define NAV2_CORE__TASK_EXECUTOR_AT_WAYPOINT_ARRIVAL_HPP_ +#pragma once + +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" + +namespace nav2_core +{ +/** + * @brief Base class for creating a plugin in order to perform a specific task at waypoint arrivals. + * + */ +class TaskExecutorAtWaypointArrival +{ +public: +/** + * @brief Construct a new Simple Task Execution At Waypoint Base object + * + */ + TaskExecutorAtWaypointArrival() {} + + /** + * @brief Destroy the Simple Task Execution At Waypoint Base object + * + */ + virtual ~TaskExecutorAtWaypointArrival() {} + + /** + * @brief Override this to setup your pub, sub or any ros services that you will use in the plugin. + * + * @param parent parent node that plugin will be created within(for an example see nav_waypoint_follower) + * @param plugin_name plugin name comes from parameters in yaml file + */ + virtual void initialize( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + const std::string & plugin_name) = 0; + + /** + * @brief Override this to define the body of your task that you would like to execute once the robot arrived to waypoint + * + * @param parent parent node that plugin will be created withing(waypoint_follower in this case) + * @param curr_pose current pose of the robot + * @param curr_waypoint_index current waypoint, that robot just arrived + */ + virtual void processAtWaypoint( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + const geometry_msgs::msg::PoseStamped curr_pose, const int curr_waypoint_index) = 0; +}; +} // namespace nav2_core +#endif // NAV2_CORE__TASK_EXECUTOR_AT_WAYPOINT_ARRIVAL_HPP_ diff --git a/nav2_waypoint_follower/CMakeLists.txt b/nav2_waypoint_follower/CMakeLists.txt index 71925e74b50..0912acd25d8 100644 --- a/nav2_waypoint_follower/CMakeLists.txt +++ b/nav2_waypoint_follower/CMakeLists.txt @@ -3,6 +3,7 @@ project(nav2_waypoint_follower) find_package(ament_cmake REQUIRED) find_package(nav2_common REQUIRED) +find_package(nav2_core REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(rclcpp_lifecycle REQUIRED) @@ -10,6 +11,7 @@ find_package(nav_msgs REQUIRED) find_package(nav2_msgs REQUIRED) find_package(nav2_util REQUIRED) find_package(tf2_ros REQUIRED) +find_package(pluginlib REQUIRED) nav2_package() @@ -36,7 +38,9 @@ set(dependencies nav_msgs nav2_msgs nav2_util + nav2_core tf2_ros + pluginlib ) ament_target_dependencies(${executable_name} @@ -49,7 +53,12 @@ ament_target_dependencies(${library_name} ${dependencies} ) -install(TARGETS ${library_name} +add_library(simple_waypoint_task_executor SHARED plugins/wait_at_waypoint_arrival.cpp) +ament_target_dependencies(simple_waypoint_task_executor ${dependencies}) +# prevent pluginlib from using boost +target_compile_definitions(simple_waypoint_task_executor PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") + +install(TARGETS ${library_name} simple_waypoint_task_executor ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -70,4 +79,7 @@ endif() ament_export_include_directories(include) +ament_export_libraries(simple_waypoint_task_executor) +pluginlib_export_plugin_description_file(nav2_waypoint_follower plugins.xml) + ament_package() diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint_arrival.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint_arrival.hpp new file mode 100644 index 00000000000..36e2c63c37a --- /dev/null +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint_arrival.hpp @@ -0,0 +1,98 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Fetullah Atas + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef NAV2_WAYPOINT_FOLLOWER__PLUGINS__WAIT_AT_WAYPOINT_ARRIVAL_HPP_ +#define NAV2_WAYPOINT_FOLLOWER__PLUGINS__WAIT_AT_WAYPOINT_ARRIVAL_HPP_ +#pragma once + +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_core/task_executor_at_waypoint_arrival.hpp" + +namespace nav2_waypoint_follower +{ + +/** + * @brief Simple plugin based on TaskExecutorAtWaypointArrival, lets robot to sleep for a + * specified amount of time at waypoint arrival. You can reference this class to define + * your own task and rewrite the body for it. + * + */ +class WaitAtWaypointArrival : public nav2_core::TaskExecutorAtWaypointArrival +{ +private: + // the robot will sleep waypoint_pause_duration_ milliseconds + int waypoint_pause_duration_; + bool is_enabled_; + +public: +/** + * @brief Construct a new Wait At Waypoint Arrival object + * + */ + WaitAtWaypointArrival(); + + /** + * @brief Destroy the Wait At Waypoint Arrival object + * + */ + ~WaitAtWaypointArrival(); + + /** + * @brief declares and loads parameters used (waypoint_pause_duration_) + * + * @param parent parent node that plugin will be created withing(waypoint_follower in this case) + * @param plugin_name + */ + void initialize( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + const std::string & plugin_name); + + + /** + * @brief Override this to define the body of your task that you would like to execute once the robot arrived to waypoint + * + * @param parent parent node that plugin will be created withing(waypoint_follower in this case) + * @param curr_pose current pose of the robot + * @param curr_waypoint_index current waypoint, that robot just arrived + */ + void processAtWaypoint( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + const geometry_msgs::msg::PoseStamped curr_pose, const int curr_waypoint_index); +}; + +} // namespace nav2_waypoint_follower +#endif // NAV2_WAYPOINT_FOLLOWER__PLUGINS__WAIT_AT_WAYPOINT_ARRIVAL_HPP_ diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp index 56c348d552a..ada575ac8eb 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp @@ -26,6 +26,10 @@ #include "nav2_util/simple_action_server.hpp" #include "rclcpp_action/rclcpp_action.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_core/task_executor_at_waypoint_arrival.hpp" +#include "pluginlib/class_loader.hpp" +#include "pluginlib/class_list_macros.hpp" namespace nav2_waypoint_follower { @@ -119,8 +123,17 @@ class WaypointFollower : public nav2_util::LifecycleNode bool stop_on_failure_; ActionStatus current_goal_status_; int loop_rate_; - int sleep_time_inbetween_waypoints_; std::vector failed_ids_; + + // Task Execution At Waypoint Plugin + pluginlib::ClassLoader + task_executor_at_waypoint_loader_; + std::shared_ptr + task_executor_at_waypoint_; + std::string default_task_executor_at_waypoint_id_; + std::string default_task_executor_at_waypoint_type_; + std::string task_executor_at_waypoint_id_; + std::string task_executor_at_waypoint_type_; }; } // namespace nav2_waypoint_follower diff --git a/nav2_waypoint_follower/package.xml b/nav2_waypoint_follower/package.xml index 250726bc167..0e914f0188b 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -17,11 +17,13 @@ nav_msgs nav2_msgs nav2_util + nav2_core ament_lint_common ament_lint_auto ament_cmake + diff --git a/nav2_waypoint_follower/plugins.xml b/nav2_waypoint_follower/plugins.xml new file mode 100644 index 00000000000..8c5a84e7e8d --- /dev/null +++ b/nav2_waypoint_follower/plugins.xml @@ -0,0 +1,7 @@ + + + + Lets robot sleep for a specified amount of time at waypoint arrival + + + \ No newline at end of file diff --git a/nav2_waypoint_follower/plugins/wait_at_waypoint_arrival.cpp b/nav2_waypoint_follower/plugins/wait_at_waypoint_arrival.cpp new file mode 100644 index 00000000000..09e7f2a6de2 --- /dev/null +++ b/nav2_waypoint_follower/plugins/wait_at_waypoint_arrival.cpp @@ -0,0 +1,100 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Fetullah Atas + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include + +#include "nav2_util/node_utils.hpp" +#include "nav2_waypoint_follower/plugins/wait_at_waypoint_arrival.hpp" + +namespace nav2_waypoint_follower +{ +WaitAtWaypointArrival::WaitAtWaypointArrival() + : waypoint_pause_duration_(0), + is_enabled_(false) +{ +} + +WaitAtWaypointArrival::~WaitAtWaypointArrival() +{ +} + +void WaitAtWaypointArrival::initialize( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + const std::string & plugin_name) +{ + auto node = parent.lock(); + nav2_util::declare_parameter_if_not_declared( + node, + plugin_name + ".waypoint_pause_duration", + rclcpp::ParameterValue(0)); + nav2_util::declare_parameter_if_not_declared( + node, + plugin_name + ".enabled", + rclcpp::ParameterValue(false)); + node->get_parameter( + plugin_name + ".waypoint_pause_duration", + waypoint_pause_duration_); + node->get_parameter( + plugin_name + ".enabled", + is_enabled_); + if (!is_enabled_) { + RCLCPP_INFO( + node->get_logger(), "Waypoint task executor plugin is disabled."); + } +} + +void WaitAtWaypointArrival::processAtWaypoint( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + const geometry_msgs::msg::PoseStamped curr_pose, const int curr_waypoint_index) +{ + if (!is_enabled_) { + return; + } + + auto node = parent.lock(); + + RCLCPP_INFO( + node->get_logger(), "Arrived at %i'th waypoint, sleeping for %i milliseconds", + curr_waypoint_index, + waypoint_pause_duration_); + RCLCPP_INFO( + node->get_logger(), "current Pose: x %.2f , y %.2f", curr_pose.pose.position.x, + curr_pose.pose.position.y); + rclcpp::sleep_for(std::chrono::milliseconds(waypoint_pause_duration_)); +} +} // namespace nav2_waypoint_follower +PLUGINLIB_EXPORT_CLASS( + nav2_waypoint_follower::WaitAtWaypointArrival, + nav2_core::TaskExecutorAtWaypointArrival) diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index e8c6ad11dde..c9ece106381 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -23,15 +23,18 @@ namespace nav2_waypoint_follower { - WaypointFollower::WaypointFollower() -: nav2_util::LifecycleNode("WaypointFollower", "", false) +: nav2_util::LifecycleNode("WaypointFollower", "", false), + task_executor_at_waypoint_loader_("nav2_waypoint_follower", + "nav2_core::TaskExecutorAtWaypointArrival"), + default_task_executor_at_waypoint_id_{"task_executor_at_waypoint"}, + default_task_executor_at_waypoint_type_{"nav2_waypoint_follower::WaitAtWaypointArrival"} { RCLCPP_INFO(get_logger(), "Creating"); declare_parameter("stop_on_failure", true); declare_parameter("loop_rate", 20); - declare_parameter("sleep_time_inbetween_waypoints", 0); + declare_parameter("task_executor_at_waypoint_plugin", default_task_executor_at_waypoint_id_); } WaypointFollower::~WaypointFollower() @@ -44,9 +47,10 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Configuring"); + auto node = shared_from_this(); stop_on_failure_ = get_parameter("stop_on_failure").as_bool(); loop_rate_ = get_parameter("loop_rate").as_int(); - sleep_time_inbetween_waypoints_ = get_parameter("sleep_time_inbetween_waypoints").as_int(); + task_executor_at_waypoint_id_ = get_parameter("task_executor_at_waypoint_plugin").as_string(); std::vector new_args = rclcpp::NodeOptions().arguments(); new_args.push_back("--ros-args"); @@ -66,6 +70,28 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) get_node_waitables_interface(), "FollowWaypoints", std::bind(&WaypointFollower::followWaypoints, this), false); + if (task_executor_at_waypoint_id_ == default_task_executor_at_waypoint_id_) { + nav2_util::declare_parameter_if_not_declared( + this, default_task_executor_at_waypoint_id_ + ".plugin", + rclcpp::ParameterValue(default_task_executor_at_waypoint_type_)); + } + + try { + task_executor_at_waypoint_type_ = nav2_util::get_plugin_type_param( + this, + task_executor_at_waypoint_id_); + task_executor_at_waypoint_ = task_executor_at_waypoint_loader_.createUniqueInstance( + task_executor_at_waypoint_type_); + RCLCPP_INFO( + get_logger(), "Created task_executor_at_waypoint : %s of type %s", + task_executor_at_waypoint_id_.c_str(), task_executor_at_waypoint_type_.c_str()); + task_executor_at_waypoint_->initialize(node, task_executor_at_waypoint_id_); + } catch (const pluginlib::PluginlibException & ex) { + RCLCPP_FATAL( + get_logger(), + "Failed to create task_executor_at_waypoint. Exception: %s", ex.what()); + } + return nav2_util::CallbackReturn::SUCCESS; } @@ -184,17 +210,13 @@ WaypointFollower::followWaypoints() " moving to next.", goal_index); } } else if (current_goal_status_ == ActionStatus::SUCCEEDED) { - if (sleep_time_inbetween_waypoints_) { - RCLCPP_INFO( - get_logger(), "Succeeded processing waypoint %i, " - "sleeping for %i milliseconds before moving to next.", goal_index, - sleep_time_inbetween_waypoints_); - rclcpp::sleep_for(std::chrono::milliseconds(sleep_time_inbetween_waypoints_)); - } else { - RCLCPP_INFO( - get_logger(), "Succeeded processing waypoint %i, " - "moving to next.", goal_index); - } + RCLCPP_INFO( + get_logger(), "Succeeded processing waypoint %i, processing waypoint task execution", + goal_index); + auto node = shared_from_this(); + task_executor_at_waypoint_->processAtWaypoint(node, goal->poses[goal_index], goal_index); + RCLCPP_INFO( + get_logger(), "Processed task execution at waypoint %i, moving to the next", goal_index); } if (current_goal_status_ != ActionStatus::PROCESSING && @@ -203,6 +225,7 @@ WaypointFollower::followWaypoints() // Update server state goal_index++; new_goal = true; + if (goal_index >= goal->poses.size()) { RCLCPP_INFO( get_logger(), "Completed all %i waypoints requested.", @@ -232,12 +255,15 @@ WaypointFollower::resultCallback( case rclcpp_action::ResultCode::SUCCEEDED: current_goal_status_ = ActionStatus::SUCCEEDED; return; + case rclcpp_action::ResultCode::ABORTED: current_goal_status_ = ActionStatus::FAILED; return; + case rclcpp_action::ResultCode::CANCELED: current_goal_status_ = ActionStatus::FAILED; return; + default: current_goal_status_ = ActionStatus::UNKNOWN; return; @@ -249,6 +275,7 @@ WaypointFollower::goalResponseCallback( std::shared_future::SharedPtr> future) { auto goal_handle = future.get(); + if (!goal_handle) { RCLCPP_ERROR( get_logger(), @@ -256,5 +283,4 @@ WaypointFollower::goalResponseCallback( current_goal_status_ = ActionStatus::FAILED; } } - } // namespace nav2_waypoint_follower From 08e4eac5323b23676b73fd49a70a157b80b98840 Mon Sep 17 00:00:00 2001 From: jediofgever Date: Tue, 29 Sep 2020 11:46:18 +0800 Subject: [PATCH 4/6] Waypoint task executor; apply requested changes --- doc/parameters/param_list.md | 13 ++- nav2_bringup/bringup/params/nav2_params.yaml | 8 +- .../task_executor_at_waypoint_arrival.hpp | 88 --------------- .../nav2_core/waypoint_task_executor.hpp | 67 ++++++++++++ nav2_waypoint_follower/CMakeLists.txt | 2 +- .../plugins/wait_at_waypoint.hpp | 77 ++++++++++++++ .../plugins/wait_at_waypoint_arrival.hpp | 98 ----------------- .../waypoint_follower.hpp | 16 ++- nav2_waypoint_follower/plugins.xml | 2 +- .../plugins/wait_at_waypoint.cpp | 87 +++++++++++++++ .../plugins/wait_at_waypoint_arrival.cpp | 100 ------------------ .../src/waypoint_follower.cpp | 47 ++++---- 12 files changed, 275 insertions(+), 330 deletions(-) delete mode 100644 nav2_core/include/nav2_core/task_executor_at_waypoint_arrival.hpp create mode 100644 nav2_core/include/nav2_core/waypoint_task_executor.hpp create mode 100644 nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint.hpp delete mode 100644 nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint_arrival.hpp create mode 100644 nav2_waypoint_follower/plugins/wait_at_waypoint.cpp delete mode 100644 nav2_waypoint_follower/plugins/wait_at_waypoint_arrival.cpp diff --git a/doc/parameters/param_list.md b/doc/parameters/param_list.md index c696d6506c6..19fe93658b8 100644 --- a/doc/parameters/param_list.md +++ b/doc/parameters/param_list.md @@ -479,13 +479,16 @@ When `planner_plugins` parameter is not overridden, the following default plugin | ----------| --------| ------------| | stop_on_failure | true | Whether to fail action task if a single waypoint fails. If false, will continue to next waypoint. | | loop_rate | 20 | Rate to check for results from current navigation task | +| waypoint_task_executor_plugin | `waypoint_task_executor` | Name of plugin to be loaded for executing waypoint tasks.| -## task_executor_at_waypoint plugin +## waypoint_task_executor plugin -* ``: Name corresponding to the `nav2_waypoint_follower::WaitAtWaypointArrival` plugin. - -| ``.enabled | false | Whether it is enabled | -| ``.waypoint_pause_duration | 0 | Amount of time in milliseconds, for robot to sleep/wait after each waypoint is reached. If zero, robot will directly continue to next waypoint. | +* ``: Name corresponding to the `nav2_waypoint_follower::WaitAtWaypoint` plugin. + +| Parameter | Default | Description | +| ----------| --------| ------------| +| ``.enabled | true | Whether it is enabled | +| ``.waypoint_pause_duration | 0 | Amount of time in milliseconds, for robot to sleep/wait after each waypoint is reached. If zero, robot will directly continue to next waypoint. | # recoveries diff --git a/nav2_bringup/bringup/params/nav2_params.yaml b/nav2_bringup/bringup/params/nav2_params.yaml index 868c561c31f..0ed09ede415 100644 --- a/nav2_bringup/bringup/params/nav2_params.yaml +++ b/nav2_bringup/bringup/params/nav2_params.yaml @@ -284,8 +284,8 @@ waypoint_follower: ros__parameters: loop_rate: 20 stop_on_failure: false - task_executor_at_waypoint_plugin: "task_executor_at_waypoint" - task_executor_at_waypoint: - plugin: "nav2_waypoint_follower::WaitAtWaypointArrival" - enabled: False + waypoint_task_executor_plugin: "waypoint_task_executor" + waypoint_task_executor: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True waypoint_pause_duration: 0 diff --git a/nav2_core/include/nav2_core/task_executor_at_waypoint_arrival.hpp b/nav2_core/include/nav2_core/task_executor_at_waypoint_arrival.hpp deleted file mode 100644 index facd3871b21..00000000000 --- a/nav2_core/include/nav2_core/task_executor_at_waypoint_arrival.hpp +++ /dev/null @@ -1,88 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2020, Fetullah Atas - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef NAV2_CORE__TASK_EXECUTOR_AT_WAYPOINT_ARRIVAL_HPP_ -#define NAV2_CORE__TASK_EXECUTOR_AT_WAYPOINT_ARRIVAL_HPP_ -#pragma once - -#include - -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" - -namespace nav2_core -{ -/** - * @brief Base class for creating a plugin in order to perform a specific task at waypoint arrivals. - * - */ -class TaskExecutorAtWaypointArrival -{ -public: -/** - * @brief Construct a new Simple Task Execution At Waypoint Base object - * - */ - TaskExecutorAtWaypointArrival() {} - - /** - * @brief Destroy the Simple Task Execution At Waypoint Base object - * - */ - virtual ~TaskExecutorAtWaypointArrival() {} - - /** - * @brief Override this to setup your pub, sub or any ros services that you will use in the plugin. - * - * @param parent parent node that plugin will be created within(for an example see nav_waypoint_follower) - * @param plugin_name plugin name comes from parameters in yaml file - */ - virtual void initialize( - const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, - const std::string & plugin_name) = 0; - - /** - * @brief Override this to define the body of your task that you would like to execute once the robot arrived to waypoint - * - * @param parent parent node that plugin will be created withing(waypoint_follower in this case) - * @param curr_pose current pose of the robot - * @param curr_waypoint_index current waypoint, that robot just arrived - */ - virtual void processAtWaypoint( - const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, - const geometry_msgs::msg::PoseStamped curr_pose, const int curr_waypoint_index) = 0; -}; -} // namespace nav2_core -#endif // NAV2_CORE__TASK_EXECUTOR_AT_WAYPOINT_ARRIVAL_HPP_ diff --git a/nav2_core/include/nav2_core/waypoint_task_executor.hpp b/nav2_core/include/nav2_core/waypoint_task_executor.hpp new file mode 100644 index 00000000000..6dfcaa3aeac --- /dev/null +++ b/nav2_core/include/nav2_core/waypoint_task_executor.hpp @@ -0,0 +1,67 @@ +// Copyright (c) 2020 Fetullah Atas +// +// 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_CORE__WAYPOINT_TASK_EXECUTOR_HPP_ +#define NAV2_CORE__WAYPOINT_TASK_EXECUTOR_HPP_ +#pragma once + +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" + +namespace nav2_core +{ +/** + * @brief Base class for creating a plugin in order to perform a specific task at waypoint arrivals. + * + */ +class WaypointTaskExecutor +{ +public: + /** + * @brief Construct a new Simple Task Execution At Waypoint Base object + * + */ + WaypointTaskExecutor() {} + + /** + * @brief Destroy the Simple Task Execution At Waypoint Base object + * + */ + virtual ~WaypointTaskExecutor() {} + + /** + * @brief Override this to setup your pub, sub or any ros services that you will use in the plugin. + * + * @param parent parent node that plugin will be created within(for an example see nav_waypoint_follower) + * @param plugin_name plugin name comes from parameters in yaml file + */ + virtual void initialize( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + const std::string & plugin_name) = 0; + + /** + * @brief Override this to define the body of your task that you would like to execute once the robot arrived to waypoint + * + * @param curr_pose current pose of the robot + * @param curr_waypoint_index current waypoint, that robot just arrived + */ + virtual void processAtWaypoint( + const geometry_msgs::msg::PoseStamped & curr_pose, const int & curr_waypoint_index) = 0; +}; +} // namespace nav2_core +#endif // NAV2_CORE__WAYPOINT_TASK_EXECUTOR_HPP_ diff --git a/nav2_waypoint_follower/CMakeLists.txt b/nav2_waypoint_follower/CMakeLists.txt index 0912acd25d8..e91384b3f18 100644 --- a/nav2_waypoint_follower/CMakeLists.txt +++ b/nav2_waypoint_follower/CMakeLists.txt @@ -53,7 +53,7 @@ ament_target_dependencies(${library_name} ${dependencies} ) -add_library(simple_waypoint_task_executor SHARED plugins/wait_at_waypoint_arrival.cpp) +add_library(simple_waypoint_task_executor SHARED plugins/wait_at_waypoint.cpp) ament_target_dependencies(simple_waypoint_task_executor ${dependencies}) # prevent pluginlib from using boost target_compile_definitions(simple_waypoint_task_executor PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint.hpp new file mode 100644 index 00000000000..33a68e58e09 --- /dev/null +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint.hpp @@ -0,0 +1,77 @@ +// Copyright (c) 2020 Fetullah Atas +// +// 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_WAYPOINT_FOLLOWER__PLUGINS__WAIT_AT_WAYPOINT_HPP_ +#define NAV2_WAYPOINT_FOLLOWER__PLUGINS__WAIT_AT_WAYPOINT_HPP_ +#pragma once + +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_core/waypoint_task_executor.hpp" + +namespace nav2_waypoint_follower +{ + +/** + * @brief Simple plugin based on WaypointTaskExecutor, lets robot to sleep for a + * specified amount of time at waypoint arrival. You can reference this class to define + * your own task and rewrite the body for it. + * + */ +class WaitAtWaypoint : public nav2_core::WaypointTaskExecutor +{ +private: + // the robot will sleep waypoint_pause_duration_ milliseconds + int waypoint_pause_duration_; + bool is_enabled_; + rclcpp::Logger logger_{rclcpp::get_logger("nav2_waypoint_follower")}; + +public: +/** + * @brief Construct a new Wait At Waypoint Arrival object + * + */ + WaitAtWaypoint(); + + /** + * @brief Destroy the Wait At Waypoint Arrival object + * + */ + ~WaitAtWaypoint(); + + /** + * @brief declares and loads parameters used (waypoint_pause_duration_) + * + * @param parent parent node that plugin will be created withing(waypoint_follower in this case) + * @param plugin_name + */ + void initialize( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + const std::string & plugin_name); + + + /** + * @brief Override this to define the body of your task that you would like to execute once the robot arrived to waypoint + * + * @param curr_pose current pose of the robot + * @param curr_waypoint_index current waypoint, that robot just arrived + */ + void processAtWaypoint( + const geometry_msgs::msg::PoseStamped & curr_pose, const int & curr_waypoint_index); +}; + +} // namespace nav2_waypoint_follower +#endif // NAV2_WAYPOINT_FOLLOWER__PLUGINS__WAIT_AT_WAYPOINT_HPP_ diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint_arrival.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint_arrival.hpp deleted file mode 100644 index 36e2c63c37a..00000000000 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint_arrival.hpp +++ /dev/null @@ -1,98 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2020, Fetullah Atas - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef NAV2_WAYPOINT_FOLLOWER__PLUGINS__WAIT_AT_WAYPOINT_ARRIVAL_HPP_ -#define NAV2_WAYPOINT_FOLLOWER__PLUGINS__WAIT_AT_WAYPOINT_ARRIVAL_HPP_ -#pragma once - -#include - -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "nav2_core/task_executor_at_waypoint_arrival.hpp" - -namespace nav2_waypoint_follower -{ - -/** - * @brief Simple plugin based on TaskExecutorAtWaypointArrival, lets robot to sleep for a - * specified amount of time at waypoint arrival. You can reference this class to define - * your own task and rewrite the body for it. - * - */ -class WaitAtWaypointArrival : public nav2_core::TaskExecutorAtWaypointArrival -{ -private: - // the robot will sleep waypoint_pause_duration_ milliseconds - int waypoint_pause_duration_; - bool is_enabled_; - -public: -/** - * @brief Construct a new Wait At Waypoint Arrival object - * - */ - WaitAtWaypointArrival(); - - /** - * @brief Destroy the Wait At Waypoint Arrival object - * - */ - ~WaitAtWaypointArrival(); - - /** - * @brief declares and loads parameters used (waypoint_pause_duration_) - * - * @param parent parent node that plugin will be created withing(waypoint_follower in this case) - * @param plugin_name - */ - void initialize( - const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, - const std::string & plugin_name); - - - /** - * @brief Override this to define the body of your task that you would like to execute once the robot arrived to waypoint - * - * @param parent parent node that plugin will be created withing(waypoint_follower in this case) - * @param curr_pose current pose of the robot - * @param curr_waypoint_index current waypoint, that robot just arrived - */ - void processAtWaypoint( - const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, - const geometry_msgs::msg::PoseStamped curr_pose, const int curr_waypoint_index); -}; - -} // namespace nav2_waypoint_follower -#endif // NAV2_WAYPOINT_FOLLOWER__PLUGINS__WAIT_AT_WAYPOINT_ARRIVAL_HPP_ diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp index ada575ac8eb..538813df689 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp @@ -27,7 +27,7 @@ #include "rclcpp_action/rclcpp_action.hpp" #include "nav2_util/node_utils.hpp" -#include "nav2_core/task_executor_at_waypoint_arrival.hpp" +#include "nav2_core/waypoint_task_executor.hpp" #include "pluginlib/class_loader.hpp" #include "pluginlib/class_list_macros.hpp" namespace nav2_waypoint_follower @@ -126,14 +126,12 @@ class WaypointFollower : public nav2_util::LifecycleNode std::vector failed_ids_; // Task Execution At Waypoint Plugin - pluginlib::ClassLoader - task_executor_at_waypoint_loader_; - std::shared_ptr - task_executor_at_waypoint_; - std::string default_task_executor_at_waypoint_id_; - std::string default_task_executor_at_waypoint_type_; - std::string task_executor_at_waypoint_id_; - std::string task_executor_at_waypoint_type_; + pluginlib::ClassLoader + waypoint_task_executor_loader_; + pluginlib::UniquePtr + waypoint_task_executor_; + std::string waypoint_task_executor_id_; + std::string waypoint_task_executor_type_; }; } // namespace nav2_waypoint_follower diff --git a/nav2_waypoint_follower/plugins.xml b/nav2_waypoint_follower/plugins.xml index 8c5a84e7e8d..aaf7cd191c6 100644 --- a/nav2_waypoint_follower/plugins.xml +++ b/nav2_waypoint_follower/plugins.xml @@ -1,6 +1,6 @@ - + Lets robot sleep for a specified amount of time at waypoint arrival diff --git a/nav2_waypoint_follower/plugins/wait_at_waypoint.cpp b/nav2_waypoint_follower/plugins/wait_at_waypoint.cpp new file mode 100644 index 00000000000..1c0d88b102a --- /dev/null +++ b/nav2_waypoint_follower/plugins/wait_at_waypoint.cpp @@ -0,0 +1,87 @@ +// Copyright (c) 2020 Fetullah Atas +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +#include "nav2_util/node_utils.hpp" +#include "nav2_waypoint_follower/plugins/wait_at_waypoint.hpp" + +namespace nav2_waypoint_follower +{ +WaitAtWaypoint::WaitAtWaypoint() +: waypoint_pause_duration_(0), + is_enabled_(true) +{ +} + +WaitAtWaypoint::~WaitAtWaypoint() +{ +} + +void WaitAtWaypoint::initialize( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + const std::string & plugin_name) +{ + auto node = parent.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node in wait at waypoint plugin!"}; + } + logger_ = node->get_logger(); + nav2_util::declare_parameter_if_not_declared( + node, + plugin_name + ".waypoint_pause_duration", + rclcpp::ParameterValue(0)); + nav2_util::declare_parameter_if_not_declared( + node, + plugin_name + ".enabled", + rclcpp::ParameterValue(false)); + node->get_parameter( + plugin_name + ".waypoint_pause_duration", + waypoint_pause_duration_); + node->get_parameter( + plugin_name + ".enabled", + is_enabled_); + if (!waypoint_pause_duration_) { + is_enabled_ = false; + RCLCPP_INFO( + node->get_logger(), + "Waypoint pause duration is set to zero, disabling task executor plugin."); + } + if (!is_enabled_) { + RCLCPP_INFO( + node->get_logger(), "Waypoint task executor plugin is disabled."); + } +} + +void WaitAtWaypoint::processAtWaypoint( + const geometry_msgs::msg::PoseStamped & curr_pose, const int & curr_waypoint_index) +{ + if (!is_enabled_) { + return; + } + RCLCPP_INFO( + logger_, "Arrived at %i'th waypoint, sleeping for %i milliseconds", + curr_waypoint_index, + waypoint_pause_duration_); + RCLCPP_INFO( + logger_, "current Pose: x %.2f , y %.2f", curr_pose.pose.position.x, + curr_pose.pose.position.y); + rclcpp::sleep_for(std::chrono::milliseconds(waypoint_pause_duration_)); +} +} // namespace nav2_waypoint_follower +PLUGINLIB_EXPORT_CLASS( + nav2_waypoint_follower::WaitAtWaypoint, + nav2_core::WaypointTaskExecutor) diff --git a/nav2_waypoint_follower/plugins/wait_at_waypoint_arrival.cpp b/nav2_waypoint_follower/plugins/wait_at_waypoint_arrival.cpp deleted file mode 100644 index 09e7f2a6de2..00000000000 --- a/nav2_waypoint_follower/plugins/wait_at_waypoint_arrival.cpp +++ /dev/null @@ -1,100 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2020, Fetullah Atas - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include - -#include "nav2_util/node_utils.hpp" -#include "nav2_waypoint_follower/plugins/wait_at_waypoint_arrival.hpp" - -namespace nav2_waypoint_follower -{ -WaitAtWaypointArrival::WaitAtWaypointArrival() - : waypoint_pause_duration_(0), - is_enabled_(false) -{ -} - -WaitAtWaypointArrival::~WaitAtWaypointArrival() -{ -} - -void WaitAtWaypointArrival::initialize( - const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, - const std::string & plugin_name) -{ - auto node = parent.lock(); - nav2_util::declare_parameter_if_not_declared( - node, - plugin_name + ".waypoint_pause_duration", - rclcpp::ParameterValue(0)); - nav2_util::declare_parameter_if_not_declared( - node, - plugin_name + ".enabled", - rclcpp::ParameterValue(false)); - node->get_parameter( - plugin_name + ".waypoint_pause_duration", - waypoint_pause_duration_); - node->get_parameter( - plugin_name + ".enabled", - is_enabled_); - if (!is_enabled_) { - RCLCPP_INFO( - node->get_logger(), "Waypoint task executor plugin is disabled."); - } -} - -void WaitAtWaypointArrival::processAtWaypoint( - const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, - const geometry_msgs::msg::PoseStamped curr_pose, const int curr_waypoint_index) -{ - if (!is_enabled_) { - return; - } - - auto node = parent.lock(); - - RCLCPP_INFO( - node->get_logger(), "Arrived at %i'th waypoint, sleeping for %i milliseconds", - curr_waypoint_index, - waypoint_pause_duration_); - RCLCPP_INFO( - node->get_logger(), "current Pose: x %.2f , y %.2f", curr_pose.pose.position.x, - curr_pose.pose.position.y); - rclcpp::sleep_for(std::chrono::milliseconds(waypoint_pause_duration_)); -} -} // namespace nav2_waypoint_follower -PLUGINLIB_EXPORT_CLASS( - nav2_waypoint_follower::WaitAtWaypointArrival, - nav2_core::TaskExecutorAtWaypointArrival) diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index c9ece106381..26c38a8a5cc 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -25,16 +25,18 @@ namespace nav2_waypoint_follower { WaypointFollower::WaypointFollower() : nav2_util::LifecycleNode("WaypointFollower", "", false), - task_executor_at_waypoint_loader_("nav2_waypoint_follower", - "nav2_core::TaskExecutorAtWaypointArrival"), - default_task_executor_at_waypoint_id_{"task_executor_at_waypoint"}, - default_task_executor_at_waypoint_type_{"nav2_waypoint_follower::WaitAtWaypointArrival"} + waypoint_task_executor_loader_("nav2_waypoint_follower", + "nav2_core::WaypointTaskExecutor") { RCLCPP_INFO(get_logger(), "Creating"); - - declare_parameter("stop_on_failure", true); - declare_parameter("loop_rate", 20); - declare_parameter("task_executor_at_waypoint_plugin", default_task_executor_at_waypoint_id_); + declare_parameter("stop_on_failure", rclcpp::ParameterValue(true)); + declare_parameter("loop_rate", rclcpp::ParameterValue(20)); + declare_parameter( + "waypoint_task_executor_plugin", + rclcpp::ParameterValue(std::string("waypoint_task_executor"))); + declare_parameter( + "waypoint_task_executor.plugin", + rclcpp::ParameterValue(std::string("nav2_waypoint_follower::WaitAtWaypoint"))); } WaypointFollower::~WaypointFollower() @@ -50,7 +52,7 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) auto node = shared_from_this(); stop_on_failure_ = get_parameter("stop_on_failure").as_bool(); loop_rate_ = get_parameter("loop_rate").as_int(); - task_executor_at_waypoint_id_ = get_parameter("task_executor_at_waypoint_plugin").as_string(); + waypoint_task_executor_id_ = get_parameter("waypoint_task_executor_plugin").as_string(); std::vector new_args = rclcpp::NodeOptions().arguments(); new_args.push_back("--ros-args"); @@ -70,26 +72,23 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) get_node_waitables_interface(), "FollowWaypoints", std::bind(&WaypointFollower::followWaypoints, this), false); - if (task_executor_at_waypoint_id_ == default_task_executor_at_waypoint_id_) { - nav2_util::declare_parameter_if_not_declared( - this, default_task_executor_at_waypoint_id_ + ".plugin", - rclcpp::ParameterValue(default_task_executor_at_waypoint_type_)); - } - + nav2_util::declare_parameter_if_not_declared( + this, std::string("waypoint_task_executor_plugin.plugin"), + rclcpp::ParameterValue(std::string("nav2_waypoint_follower::WaitAtWaypoint"))); try { - task_executor_at_waypoint_type_ = nav2_util::get_plugin_type_param( + waypoint_task_executor_type_ = nav2_util::get_plugin_type_param( this, - task_executor_at_waypoint_id_); - task_executor_at_waypoint_ = task_executor_at_waypoint_loader_.createUniqueInstance( - task_executor_at_waypoint_type_); + waypoint_task_executor_id_); + waypoint_task_executor_ = waypoint_task_executor_loader_.createUniqueInstance( + waypoint_task_executor_type_); RCLCPP_INFO( - get_logger(), "Created task_executor_at_waypoint : %s of type %s", - task_executor_at_waypoint_id_.c_str(), task_executor_at_waypoint_type_.c_str()); - task_executor_at_waypoint_->initialize(node, task_executor_at_waypoint_id_); + get_logger(), "Created waypoint_task_executor : %s of type %s", + waypoint_task_executor_id_.c_str(), waypoint_task_executor_type_.c_str()); + waypoint_task_executor_->initialize(node, waypoint_task_executor_id_); } catch (const pluginlib::PluginlibException & ex) { RCLCPP_FATAL( get_logger(), - "Failed to create task_executor_at_waypoint. Exception: %s", ex.what()); + "Failed to create waypoint_task_executor. Exception: %s", ex.what()); } return nav2_util::CallbackReturn::SUCCESS; @@ -214,7 +213,7 @@ WaypointFollower::followWaypoints() get_logger(), "Succeeded processing waypoint %i, processing waypoint task execution", goal_index); auto node = shared_from_this(); - task_executor_at_waypoint_->processAtWaypoint(node, goal->poses[goal_index], goal_index); + waypoint_task_executor_->processAtWaypoint(goal->poses[goal_index], goal_index); RCLCPP_INFO( get_logger(), "Processed task execution at waypoint %i, moving to the next", goal_index); } From 4ad6f4dc2887dbdaf3d5aac557f351b74539af59 Mon Sep 17 00:00:00 2001 From: jediofgever Date: Wed, 30 Sep 2020 10:06:29 +0800 Subject: [PATCH 5/6] Waypoint task executor; cleanups --- doc/parameters/param_list.md | 2 +- .../nav2_core/waypoint_task_executor.hpp | 4 +++- .../plugins/wait_at_waypoint.hpp | 16 +++++++------- .../plugins/wait_at_waypoint.cpp | 21 ++++++++----------- .../src/waypoint_follower.cpp | 17 +++++++-------- 5 files changed, 30 insertions(+), 30 deletions(-) diff --git a/doc/parameters/param_list.md b/doc/parameters/param_list.md index 19fe93658b8..c7d1ddc3f4f 100644 --- a/doc/parameters/param_list.md +++ b/doc/parameters/param_list.md @@ -479,7 +479,7 @@ When `planner_plugins` parameter is not overridden, the following default plugin | ----------| --------| ------------| | stop_on_failure | true | Whether to fail action task if a single waypoint fails. If false, will continue to next waypoint. | | loop_rate | 20 | Rate to check for results from current navigation task | -| waypoint_task_executor_plugin | `waypoint_task_executor` | Name of plugin to be loaded for executing waypoint tasks.| +| waypoint_task_executor_plugin | `WaitAtWaypoint` | Name of plugin to be loaded for executing waypoint tasks.| ## waypoint_task_executor plugin diff --git a/nav2_core/include/nav2_core/waypoint_task_executor.hpp b/nav2_core/include/nav2_core/waypoint_task_executor.hpp index 6dfcaa3aeac..ffdbfd95e1d 100644 --- a/nav2_core/include/nav2_core/waypoint_task_executor.hpp +++ b/nav2_core/include/nav2_core/waypoint_task_executor.hpp @@ -59,8 +59,10 @@ class WaypointTaskExecutor * * @param curr_pose current pose of the robot * @param curr_waypoint_index current waypoint, that robot just arrived + * @return true if task execution was successful + * @return false if task execution failed */ - virtual void processAtWaypoint( + virtual bool processAtWaypoint( const geometry_msgs::msg::PoseStamped & curr_pose, const int & curr_waypoint_index) = 0; }; } // namespace nav2_core diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint.hpp index 33a68e58e09..911ae441a23 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint.hpp @@ -33,12 +33,6 @@ namespace nav2_waypoint_follower */ class WaitAtWaypoint : public nav2_core::WaypointTaskExecutor { -private: - // the robot will sleep waypoint_pause_duration_ milliseconds - int waypoint_pause_duration_; - bool is_enabled_; - rclcpp::Logger logger_{rclcpp::get_logger("nav2_waypoint_follower")}; - public: /** * @brief Construct a new Wait At Waypoint Arrival object @@ -68,9 +62,17 @@ class WaitAtWaypoint : public nav2_core::WaypointTaskExecutor * * @param curr_pose current pose of the robot * @param curr_waypoint_index current waypoint, that robot just arrived + * @return true if task execution was successful + * @return false if task execution failed */ - void processAtWaypoint( + bool processAtWaypoint( const geometry_msgs::msg::PoseStamped & curr_pose, const int & curr_waypoint_index); + +protected: + // the robot will sleep waypoint_pause_duration_ milliseconds + int waypoint_pause_duration_; + bool is_enabled_; + rclcpp::Logger logger_{rclcpp::get_logger("nav2_waypoint_follower")}; }; } // namespace nav2_waypoint_follower diff --git a/nav2_waypoint_follower/plugins/wait_at_waypoint.cpp b/nav2_waypoint_follower/plugins/wait_at_waypoint.cpp index 1c0d88b102a..c8621e1cd5b 100644 --- a/nav2_waypoint_follower/plugins/wait_at_waypoint.cpp +++ b/nav2_waypoint_follower/plugins/wait_at_waypoint.cpp @@ -47,39 +47,36 @@ void WaitAtWaypoint::initialize( nav2_util::declare_parameter_if_not_declared( node, plugin_name + ".enabled", - rclcpp::ParameterValue(false)); + rclcpp::ParameterValue(true)); node->get_parameter( plugin_name + ".waypoint_pause_duration", waypoint_pause_duration_); node->get_parameter( plugin_name + ".enabled", is_enabled_); - if (!waypoint_pause_duration_) { + if (waypoint_pause_duration_ == 0) { is_enabled_ = false; RCLCPP_INFO( - node->get_logger(), + logger_, "Waypoint pause duration is set to zero, disabling task executor plugin."); - } - if (!is_enabled_) { + } else if (!is_enabled_) { RCLCPP_INFO( - node->get_logger(), "Waypoint task executor plugin is disabled."); + logger_, "Waypoint task executor plugin is disabled."); } } -void WaitAtWaypoint::processAtWaypoint( - const geometry_msgs::msg::PoseStamped & curr_pose, const int & curr_waypoint_index) +bool WaitAtWaypoint::processAtWaypoint( + const geometry_msgs::msg::PoseStamped & /*curr_pose*/, const int & curr_waypoint_index) { if (!is_enabled_) { - return; + return false; } RCLCPP_INFO( logger_, "Arrived at %i'th waypoint, sleeping for %i milliseconds", curr_waypoint_index, waypoint_pause_duration_); - RCLCPP_INFO( - logger_, "current Pose: x %.2f , y %.2f", curr_pose.pose.position.x, - curr_pose.pose.position.y); rclcpp::sleep_for(std::chrono::milliseconds(waypoint_pause_duration_)); + return true; } } // namespace nav2_waypoint_follower PLUGINLIB_EXPORT_CLASS( diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 26c38a8a5cc..5d6f6c98a96 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -31,11 +31,11 @@ WaypointFollower::WaypointFollower() RCLCPP_INFO(get_logger(), "Creating"); declare_parameter("stop_on_failure", rclcpp::ParameterValue(true)); declare_parameter("loop_rate", rclcpp::ParameterValue(20)); - declare_parameter( - "waypoint_task_executor_plugin", + nav2_util::declare_parameter_if_not_declared( + this, std::string("waypoint_task_executor_plugin"), rclcpp::ParameterValue(std::string("waypoint_task_executor"))); - declare_parameter( - "waypoint_task_executor.plugin", + nav2_util::declare_parameter_if_not_declared( + this, std::string("waypoint_task_executor_plugin.plugin"), rclcpp::ParameterValue(std::string("nav2_waypoint_follower::WaitAtWaypoint"))); } @@ -72,9 +72,6 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) get_node_waitables_interface(), "FollowWaypoints", std::bind(&WaypointFollower::followWaypoints, this), false); - nav2_util::declare_parameter_if_not_declared( - this, std::string("waypoint_task_executor_plugin.plugin"), - rclcpp::ParameterValue(std::string("nav2_waypoint_follower::WaitAtWaypoint"))); try { waypoint_task_executor_type_ = nav2_util::get_plugin_type_param( this, @@ -213,9 +210,11 @@ WaypointFollower::followWaypoints() get_logger(), "Succeeded processing waypoint %i, processing waypoint task execution", goal_index); auto node = shared_from_this(); - waypoint_task_executor_->processAtWaypoint(goal->poses[goal_index], goal_index); + bool is_task_executed = waypoint_task_executor_->processAtWaypoint( + goal->poses[goal_index], goal_index); RCLCPP_INFO( - get_logger(), "Processed task execution at waypoint %i, moving to the next", goal_index); + get_logger(), "Task execution at waypoint %i %s", goal_index, + is_task_executed ? "succeeded" : "failed!", "moving to the next"); } if (current_goal_status_ != ActionStatus::PROCESSING && From 3369ca65db2e8190355e44b115edb20a07d9f21b Mon Sep 17 00:00:00 2001 From: jediofgever Date: Mon, 5 Oct 2020 10:44:04 +0800 Subject: [PATCH 6/6] revert to 4ad6f4dc2887dbdaf3d5aac557f351b74539af59 --- codecov.yml => .circleci/codecov.yml | 0 .circleci/config.yml | 156 ++++--- .dockerhub/debug/dummy.Dockerfile | 12 - .dockerhub/debug/hooks/build | 2 +- .dockerhub/devel/Dockerfile | 1 - .dockerhub/devel/hooks/build | 11 - .../distro.Dockerfile | 30 +- .dockerhub/release/dummy.Dockerfile | 12 - .dockerhub/release/hooks/build | 4 +- .dockerhub/source.Dockerfile | 246 +++++++++++ .dockerignore | 7 +- Dockerfile | 25 +- README.md | 6 +- doc/design/CostmapFilters_design.pdf | Bin 0 -> 74102 bytes doc/parameters/param_list.md | 64 ++- doc/process/PreReleaseChecklist.md | 8 +- nav2_amcl/README.md | 2 +- nav2_amcl/src/amcl_node.cpp | 7 +- nav2_behavior_tree/CMakeLists.txt | 6 +- nav2_behavior_tree/README.md | 1 + .../nav2_behavior_tree/bt_action_node.hpp | 15 +- .../nav2_behavior_tree/bt_conversions.hpp | 28 +- .../action/compute_path_to_pose_action.hpp | 3 - .../action/navigate_to_pose_action.hpp | 3 +- .../condition/is_battery_low_condition.hpp | 66 +++ nav2_behavior_tree/nav2_tree_nodes.xml | 3 +- nav2_behavior_tree/package.xml | 3 +- .../plugins/action/back_up_action.cpp | 69 +-- .../action/compute_path_to_pose_action.cpp | 6 - .../plugins/action/follow_path_action.cpp | 13 +- .../action/navigate_to_pose_action.cpp | 11 +- .../plugins/action/spin_action.cpp | 50 +-- .../plugins/action/wait_action.cpp | 59 +-- .../condition/is_battery_low_condition.cpp | 67 +++ .../plugins/control/pipeline_sequence.cpp | 1 + .../plugins/control/recovery_node.cpp | 25 +- .../test/plugins/action/CMakeLists.txt | 18 +- .../plugins/action/test_back_up_action.cpp | 8 +- .../action/test_clear_costmap_service.cpp | 1 - .../test_compute_path_to_pose_action.cpp | 22 +- .../action/test_follow_path_action.cpp | 26 +- .../action/test_navigate_to_pose_action.cpp | 17 +- ...initialize_global_localization_service.cpp | 1 - .../test/plugins/action/test_spin_action.cpp | 8 +- .../test/plugins/action/test_wait_action.cpp | 8 +- .../test/plugins/condition/CMakeLists.txt | 4 + .../plugins/condition/test_is_battery_low.cpp | 162 +++++++ .../condition/test_transform_available.cpp | 1 - .../plugins/control/test_recovery_node.cpp | 25 +- .../test/test_behavior_tree_fixture.hpp | 1 - .../test/test_bt_conversions.cpp | 76 ++++ nav2_bringup/bringup/README.md | 4 +- .../params/nav2_multirobot_params_1.yaml | 2 + .../params/nav2_multirobot_params_2.yaml | 2 + nav2_bringup/bringup/params/nav2_params.yaml | 11 +- .../nav2_gazebo_spawner.py | 4 +- .../nav2_bt_navigator/ros_topic_logger.hpp | 5 +- nav2_bt_navigator/src/bt_navigator.cpp | 27 +- nav2_bt_navigator/src/ros_topic_logger.cpp | 17 +- .../plugins/simple_goal_checker.hpp | 2 +- .../plugins/simple_progress_checker.hpp | 4 +- .../plugins/stopped_goal_checker.hpp | 2 +- .../plugins/simple_goal_checker.cpp | 16 +- .../plugins/simple_progress_checker.cpp | 28 +- .../plugins/stopped_goal_checker.cpp | 14 +- nav2_controller/src/nav2_controller.cpp | 16 +- nav2_core/include/nav2_core/controller.hpp | 2 +- .../include/nav2_core/global_planner.hpp | 2 +- nav2_core/include/nav2_core/goal_checker.hpp | 4 +- .../include/nav2_core/progress_checker.hpp | 4 +- nav2_core/include/nav2_core/recovery.hpp | 2 +- .../nav2_core/waypoint_task_executor.hpp | 69 --- nav2_costmap_2d/CMakeLists.txt | 15 +- nav2_costmap_2d/README.md | 17 +- nav2_costmap_2d/costmap_plugins.xml | 6 + .../nav2_costmap_2d/clear_costmap_service.hpp | 6 +- .../include/nav2_costmap_2d/costmap_2d.hpp | 7 + .../nav2_costmap_2d/costmap_2d_publisher.hpp | 6 +- .../nav2_costmap_2d/costmap_2d_ros.hpp | 6 +- .../costmap_filters/costmap_filter.hpp | 141 ++++++ .../costmap_filters/keepout_filter.hpp | 82 ++++ .../nav2_costmap_2d/costmap_subscriber.hpp | 15 +- .../nav2_costmap_2d/footprint_subscriber.hpp | 18 +- .../include/nav2_costmap_2d/layer.hpp | 12 +- .../nav2_costmap_2d/observation_buffer.hpp | 5 +- nav2_costmap_2d/package.xml | 2 +- .../costmap_filters/costmap_filter.cpp | 117 +++++ .../costmap_filters/keepout_filter.cpp | 257 +++++++++++ nav2_costmap_2d/plugins/inflation_layer.cpp | 25 +- nav2_costmap_2d/plugins/obstacle_layer.cpp | 73 ++-- .../plugins/range_sensor_layer.cpp | 64 +-- nav2_costmap_2d/plugins/static_layer.cpp | 59 +-- nav2_costmap_2d/plugins/voxel_layer.cpp | 44 +- nav2_costmap_2d/src/clear_costmap_service.cpp | 30 +- nav2_costmap_2d/src/costmap_2d.cpp | 33 ++ nav2_costmap_2d/src/costmap_2d_publisher.cpp | 34 +- nav2_costmap_2d/src/costmap_2d_ros.cpp | 19 +- nav2_costmap_2d/src/costmap_subscriber.cpp | 38 +- nav2_costmap_2d/src/footprint_subscriber.cpp | 46 +- nav2_costmap_2d/src/layer.cpp | 49 ++- nav2_costmap_2d/src/observation_buffer.cpp | 44 +- .../test/integration/inflation_tests.cpp | 6 + .../test_costmap_topic_collision_checker.cpp | 2 +- nav2_costmap_2d/test/unit/CMakeLists.txt | 16 + .../test/unit/costmap_conversion_test.cpp | 122 ++++++ .../test/unit/declare_parameter_test.cpp | 94 ++++ .../unit/footprint_collision_checker_test.cpp | 94 ++++ .../test/unit/keepout_filter_test.cpp | 401 ++++++++++++++++++ .../test/unit/keepout_filter_test.jpg | Bin 0 -> 136714 bytes .../include/dwb_core/dwb_local_planner.hpp | 8 +- .../dwb_core/include/dwb_core/publisher.hpp | 7 +- .../include/dwb_core/trajectory_critic.hpp | 10 +- .../dwb_core/src/dwb_local_planner.cpp | 164 +++---- .../dwb_core/src/publisher.cpp | 89 ++-- .../include/dwb_critics/oscillation.hpp | 1 + .../dwb_critics/src/base_obstacle.cpp | 9 +- .../dwb_critics/src/goal_align.cpp | 8 +- .../dwb_critics/src/map_grid.cpp | 9 +- .../dwb_critics/src/oscillation.cpp | 33 +- .../dwb_critics/src/path_align.cpp | 8 +- .../dwb_critics/src/prefer_forward.cpp | 21 +- .../dwb_critics/src/rotate_to_goal.cpp | 13 +- .../dwb_critics/src/twirling.cpp | 6 +- .../nav_2d_utils/CMakeLists.txt | 4 - nav2_lifecycle_manager/CMakeLists.txt | 2 + .../lifecycle_manager.hpp | 40 +- .../lifecycle_manager_client.hpp | 14 +- nav2_lifecycle_manager/package.xml | 2 + .../src/lifecycle_manager.cpp | 120 +++++- .../src/lifecycle_manager_client.cpp | 30 +- nav2_lifecycle_manager/test/CMakeLists.txt | 23 +- .../test/launch_bond_test.py | 57 +++ .../test/launch_lifecycle_test.py | 1 + nav2_lifecycle_manager/test/test_bond.cpp | 197 +++++++++ nav2_map_server/CMakeLists.txt | 16 +- .../costmap_filter_info_server.hpp | 79 ++++ .../include/nav2_map_server/map_saver.hpp | 2 + .../costmap_filter_info_server.cpp | 105 +++++ .../src/costmap_filter_info/main.cpp | 32 ++ nav2_map_server/src/map_io.cpp | 21 +- nav2_map_server/src/map_saver/map_saver.cpp | 18 +- nav2_map_server/src/map_server/map_server.cpp | 7 +- nav2_map_server/test/unit/CMakeLists.txt | 11 + .../unit/test_costmap_filter_info_server.cpp | 146 +++++++ nav2_msgs/CMakeLists.txt | 1 + nav2_msgs/msg/CostmapFilterInfo.msg | 11 + .../nav2_navfn_planner/navfn_planner.hpp | 9 +- nav2_navfn_planner/src/navfn_planner.cpp | 50 ++- nav2_planner/src/planner_server.cpp | 25 +- .../include/nav2_recoveries/recovery.hpp | 66 ++- .../nav2_recoveries/recovery_server.hpp | 2 +- nav2_recoveries/plugins/back_up.cpp | 28 +- nav2_recoveries/plugins/spin.cpp | 29 +- nav2_recoveries/src/recovery_server.cpp | 20 +- .../include/nav2_rviz_plugins/nav2_panel.hpp | 3 + nav2_rviz_plugins/src/nav2_panel.cpp | 28 +- nav2_system_tests/CMakeLists.txt | 1 + nav2_system_tests/maps/filters_room.pgm | Bin 0 -> 10061 bytes nav2_system_tests/maps/filters_room.yaml | 6 + nav2_system_tests/maps/keepout_mask.pgm | Bin 0 -> 10061 bytes nav2_system_tests/maps/keepout_mask.yaml | 6 + nav2_system_tests/params/keepout_params.yaml | 7 + .../src/costmap_filters/CMakeLists.txt | 24 ++ .../costmap_filters_tests_launch.py | 86 ++++ .../src/costmap_filters/filters_tester.cpp | 310 ++++++++++++++ .../src/costmap_filters/filters_tester.hpp | 94 ++++ .../costmap_filters/test_keepout_filter.cpp | 75 ++++ nav2_util/CMakeLists.txt | 6 + .../include/nav2_util/lifecycle_node.hpp | 9 + .../include/nav2_util/occ_grid_values.hpp | 50 +++ .../include/nav2_util/odometry_utils.hpp | 8 +- .../nav2_util/simple_action_server.hpp | 9 +- nav2_util/package.xml | 2 + nav2_util/src/CMakeLists.txt | 1 + nav2_util/src/lifecycle_node.cpp | 31 ++ nav2_util/src/odometry_utils.cpp | 10 +- nav2_util/test/test_actions.cpp | 2 + nav2_waypoint_follower/CMakeLists.txt | 14 +- .../plugins/wait_at_waypoint.hpp | 79 ---- .../waypoint_follower.hpp | 17 +- nav2_waypoint_follower/package.xml | 2 - nav2_waypoint_follower/plugins.xml | 7 - .../plugins/wait_at_waypoint.cpp | 84 ---- .../src/waypoint_follower.cpp | 69 +-- tools/code_coverage_report.bash | 3 + tools/initial_ros_setup.sh | 2 +- tools/skip_keys.txt | 2 +- tools/source.Dockerfile | 168 -------- ...ros2_dependencies.repos => underlay.repos} | 4 + 189 files changed, 4867 insertions(+), 1548 deletions(-) rename codecov.yml => .circleci/codecov.yml (100%) delete mode 100644 .dockerhub/debug/dummy.Dockerfile delete mode 120000 .dockerhub/devel/Dockerfile delete mode 100755 .dockerhub/devel/hooks/build rename tools/release.Dockerfile => .dockerhub/distro.Dockerfile (82%) delete mode 100644 .dockerhub/release/dummy.Dockerfile create mode 100644 .dockerhub/source.Dockerfile create mode 100644 doc/design/CostmapFilters_design.pdf create mode 100644 nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp create mode 100644 nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp create mode 100644 nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp delete mode 100644 nav2_core/include/nav2_core/waypoint_task_executor.hpp create mode 100644 nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp create mode 100644 nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp create mode 100644 nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp create mode 100644 nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp create mode 100644 nav2_costmap_2d/test/unit/costmap_conversion_test.cpp create mode 100644 nav2_costmap_2d/test/unit/declare_parameter_test.cpp create mode 100644 nav2_costmap_2d/test/unit/keepout_filter_test.cpp create mode 100644 nav2_costmap_2d/test/unit/keepout_filter_test.jpg create mode 100755 nav2_lifecycle_manager/test/launch_bond_test.py create mode 100644 nav2_lifecycle_manager/test/test_bond.cpp create mode 100644 nav2_map_server/include/nav2_map_server/costmap_filter_info_server.hpp create mode 100644 nav2_map_server/src/costmap_filter_info/costmap_filter_info_server.cpp create mode 100644 nav2_map_server/src/costmap_filter_info/main.cpp create mode 100644 nav2_map_server/test/unit/test_costmap_filter_info_server.cpp create mode 100644 nav2_msgs/msg/CostmapFilterInfo.msg create mode 100644 nav2_system_tests/maps/filters_room.pgm create mode 100644 nav2_system_tests/maps/filters_room.yaml create mode 100644 nav2_system_tests/maps/keepout_mask.pgm create mode 100644 nav2_system_tests/maps/keepout_mask.yaml create mode 100644 nav2_system_tests/params/keepout_params.yaml create mode 100644 nav2_system_tests/src/costmap_filters/CMakeLists.txt create mode 100755 nav2_system_tests/src/costmap_filters/costmap_filters_tests_launch.py create mode 100644 nav2_system_tests/src/costmap_filters/filters_tester.cpp create mode 100644 nav2_system_tests/src/costmap_filters/filters_tester.hpp create mode 100644 nav2_system_tests/src/costmap_filters/test_keepout_filter.cpp create mode 100644 nav2_util/include/nav2_util/occ_grid_values.hpp delete mode 100644 nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint.hpp delete mode 100644 nav2_waypoint_follower/plugins.xml delete mode 100644 nav2_waypoint_follower/plugins/wait_at_waypoint.cpp delete mode 100644 tools/source.Dockerfile rename tools/{ros2_dependencies.repos => underlay.repos} (86%) diff --git a/codecov.yml b/.circleci/codecov.yml similarity index 100% rename from codecov.yml rename to .circleci/codecov.yml diff --git a/.circleci/config.yml b/.circleci/config.yml index ef14bf0d667..b5e53a18ce0 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -1,17 +1,6 @@ version: 2.1 -orbs: - codecov: codecov/codecov@1.0.5 -references: - common_environment: &common_environment - ROS_WS: "/opt/ros_ws" - UNDERLAY_WS: "/opt/underlay_ws" - OVERLAY_WS: "/opt/overlay_ws" - CCACHE_LOGFILE: "/tmp/ccache.log" - CCACHE_MAXSIZE: "200M" - MAKEFLAGS: "-j 1 -l 2" - RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED: "1" - RETEST_UNTIL_PASS: "2" +_commands: common_commands: &common_commands restore_from_cache: description: "Restore From Cache" @@ -112,16 +101,17 @@ references: name: Build Workspace | << parameters.workspace >> working_directory: << parameters.workspace >> command: | - if [ -d install ] && [ ! -f build_failed ] + BUILD_UNFINISHED=$(colcon list --packages-skip-build-finished) + BUILD_FAILED=$(colcon list --packages-select-build-failed) + if [ -n "$BUILD_UNFINISHED" ] || [ -n "$BUILD_FAILED" ] then - echo "Skipping Build" - else . << parameters.underlay >>/install/setup.sh rm -rf build install log colcon build \ --symlink-install \ --mixin << parameters.mixins >> - rm -f build_failed + else + echo "Skipping Build" fi - save_to_cache: key: << parameters.key >> @@ -153,11 +143,8 @@ references: --show-counts | \ xargs) set -o xtrace - colcon test \ - --packages-select ${TEST_PACKAGES} colcon test \ --packages-select ${TEST_PACKAGES} \ - --packages-select-test-failures \ --retest-until-pass ${RETEST_UNTIL_PASS} \ --ctest-args --test-regex "test_.*" colcon test-result \ @@ -180,6 +167,19 @@ references: path: << parameters.workspace >>/test_results - store_artifacts: path: << parameters.workspace >>/test_results + trigger_dockerhub_url: + description: "Trigger Dockerhub URL" + parameters: + data: + type: string + steps: + - run: + command: | + curl -H "Content-Type: application/json" \ + --data '<< parameters.data >>' \ + -X POST ${DOCKERHUB_TRIGGER_URL} + +_steps: pre_checkout: &pre_checkout run: name: Pre Checkout @@ -193,6 +193,7 @@ references: (echo ros_entrypoint && cat) >> checksum.txt sha256sum $PWD/checksum.txt >> checksum.txt rm -rf $OVERLAY_WS/* + mv ~/.ccache /mnt/ramdisk/.ccache on_checkout: &on_checkout checkout: path: src/navigation2 @@ -201,16 +202,16 @@ references: name: Post Checkout command: | if ! cmp \ - $OVERLAY_WS/src/navigation2/tools/ros2_dependencies.repos \ - $UNDERLAY_WS/ros2_dependencies.repos >/dev/null 2>&1 + $OVERLAY_WS/src/navigation2/tools/underlay.repos \ + $UNDERLAY_WS/underlay.repos >/dev/null 2>&1 then echo "Cleaning Underlay" rm -rf $UNDERLAY_WS/* - cp $OVERLAY_WS/src/navigation2/tools/ros2_dependencies.repos \ - $UNDERLAY_WS/ros2_dependencies.repos + cp $OVERLAY_WS/src/navigation2/tools/underlay.repos \ + $UNDERLAY_WS/underlay.repos mkdir -p $UNDERLAY_WS/src vcs import $UNDERLAY_WS/src \ - < $UNDERLAY_WS/ros2_dependencies.repos + < $UNDERLAY_WS/underlay.repos fi install_underlay_dependencies: &install_underlay_dependencies install_dependencies: @@ -259,7 +260,7 @@ references: save_to_cache: key: ccache workspace: /opt/underlay_ws - path: ~/.ccache + path: /mnt/ramdisk/.ccache when: always test_overlay_workspace: &test_overlay_workspace test_workspace: @@ -271,10 +272,25 @@ references: command: src/navigation2/tools/code_coverage_report.bash ci when: always upload_overlay_coverage: &upload_overlay_coverage - codecov/upload: - file: lcov/project_coverage.info - flags: project + run: + name: Upload Code Coverage + working_directory: /opt/overlay_ws + command: | + curl -s https://codecov.io/bash | bash -s -- \ + -f "lcov/project_coverage.info" \ + -R "src/navigation2" \ + -t "${CODECOV_TOKEN}" \ + -n "${CIRCLE_BUILD_NUM}" \ + -F "project" \ + -Z || echo 'Codecov upload failed' when: always + trigger_dockerhub_build: &trigger_dockerhub_build + trigger_dockerhub_url: + data: | + { + "source_type": "Branch", + "source_name": "main" + } commands: <<: *common_commands @@ -317,11 +333,28 @@ commands: steps: - *collect_overlay_coverage - *upload_overlay_coverage + trigger_dockerhub: + description: "Trigger DockerHub" + steps: + - *trigger_dockerhub_build + +_environments: + common_environment: &common_environment + ROS_WS: "/opt/ros_ws" + UNDERLAY_WS: "/opt/underlay_ws" + OVERLAY_WS: "/opt/overlay_ws" + CCACHE_DIR: "/mnt/ramdisk/.ccache" + CCACHE_LOGFILE: "/tmp/ccache.log" + CCACHE_MAXSIZE: "200M" + MAKEFLAGS: "-j 1 -l 2" + RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED: "0" + RETEST_UNTIL_PASS: "2" + DEBIAN_FRONTEND: "noninteractive" executors: debug_exec: docker: - - image: rosplanning/navigation2:master.debug + - image: rosplanning/navigation2:main.debug working_directory: /opt/overlay_ws environment: <<: *common_environment @@ -330,13 +363,26 @@ executors: UNDERLAY_MIXINS: "release ccache" release_exec: docker: - - image: rosplanning/navigation2:master.release + - image: rosplanning/navigation2:main.release working_directory: /opt/overlay_ws environment: <<: *common_environment CACHE_NONCE: "Release" OVERLAY_MIXINS: "release ccache" UNDERLAY_MIXINS: "release ccache" + python_exec: + docker: + - image: circleci/python + +_jobs: + job_test: &job_test + parameters: + rmw: + default: "rmw_fastrtps_cpp" + type: string + parallelism: 1 + environment: + RMW_IMPLEMENTATION: << parameters.rmw >> jobs: debug_build: &debug_build @@ -349,30 +395,22 @@ jobs: executor: release_exec <<: *debug_build debug_test: + <<: *job_test executor: debug_exec - parallelism: 1 steps: - restore_build - test_build - report_coverage - release_test: &release_test + release_test: + <<: *job_test executor: release_exec - parallelism: 1 steps: - restore_build - test_build - test_rmw_connext_cpp: - <<: *release_test - environment: - RMW_IMPLEMENTATION: "rmw_connext_cpp" - test_rmw_cyclonedds_cpp: - <<: *release_test - environment: - RMW_IMPLEMENTATION: "rmw_cyclonedds_cpp" - test_rmw_fastrtps_cpp: - <<: *release_test - environment: - RMW_IMPLEMENTATION: "rmw_fastrtps_cpp" + rebuild_dockerhub: + executor: python_exec + steps: + - trigger_dockerhub workflows: version: 2 @@ -393,19 +431,29 @@ workflows: requires: - debug_build - release_build - # - test_rmw_connext_cpp: - # requires: - # - release_build - - test_rmw_cyclonedds_cpp: - requires: - - release_build - - test_rmw_fastrtps_cpp: + - release_test: requires: - release_build + matrix: + parameters: + rmw: + # - rmw_connext_cpp + - rmw_cyclonedds_cpp + - rmw_fastrtps_cpp + triggers: + - schedule: + cron: "0 13 * * *" + filters: + branches: + only: + - main + dockerhub: + jobs: + - rebuild_dockerhub triggers: - schedule: - cron: "0 0 * * *" + cron: "0 7 * * *" filters: branches: only: - - master + - main diff --git a/.dockerhub/debug/dummy.Dockerfile b/.dockerhub/debug/dummy.Dockerfile deleted file mode 100644 index 121ee27a362..00000000000 --- a/.dockerhub/debug/dummy.Dockerfile +++ /dev/null @@ -1,12 +0,0 @@ -# This is a dummy Dockerfile for setting repository links on Docker Hub. -# Build rules on Docker Hub can trigger whenever the base image updates. -# Base images are specified in the FROM: directive in the tracked Dockerfile. -# However, build args are used by the build hooks to adjust the base image -# so a single Dockerfile can be reused for multiple CI jobs, reducing maintenance. -# To re-enable repository linking when using build args in the FROM: directive, -# this dummy Dockerfile explicitly conveys the base image/repo to link against -# while build rules that target this still use the same hook in this directory. -# Note: This only works for non-official images. - -FROM osrf/ros2:nightly -RUN echo "This is a dummy Dockerfile." \ No newline at end of file diff --git a/.dockerhub/debug/hooks/build b/.dockerhub/debug/hooks/build index 310fbbd87b0..671d5e3f3d7 100755 --- a/.dockerhub/debug/hooks/build +++ b/.dockerhub/debug/hooks/build @@ -12,4 +12,4 @@ docker build \ --build-arg FAIL_ON_BUILD_FAILURE \ --build-arg UNDERLAY_MIXINS \ --build-arg OVERLAY_MIXINS \ - --file ./Dockerfile ../../. + --file ../../Dockerfile ../../. diff --git a/.dockerhub/devel/Dockerfile b/.dockerhub/devel/Dockerfile deleted file mode 120000 index 36c49d2a30c..00000000000 --- a/.dockerhub/devel/Dockerfile +++ /dev/null @@ -1 +0,0 @@ -../../Dockerfile \ No newline at end of file diff --git a/.dockerhub/devel/hooks/build b/.dockerhub/devel/hooks/build deleted file mode 100755 index 093a78ec028..00000000000 --- a/.dockerhub/devel/hooks/build +++ /dev/null @@ -1,11 +0,0 @@ -#!/bin/bash -set -ex - -export ROS_DISTRO=${SOURCE_BRANCH%"-devel"} -export FROM_IMAGE=ros:${ROS_DISTRO} - -docker build \ - --tag ${IMAGE_NAME} \ - --build-arg FROM_IMAGE \ - --build-arg FAIL_ON_BUILD_FAILURE="" \ - --file ./Dockerfile ../../. diff --git a/tools/release.Dockerfile b/.dockerhub/distro.Dockerfile similarity index 82% rename from tools/release.Dockerfile rename to .dockerhub/distro.Dockerfile index 2b89b534008..47296be46b5 100644 --- a/tools/release.Dockerfile +++ b/.dockerhub/distro.Dockerfile @@ -6,14 +6,14 @@ # # Example build command: # export DOCKER_BUILDKIT=1 -# export FROM_IMAGE="ros:eloquent" +# export FROM_IMAGE="ros:foxy" # export OVERLAY_MIXINS="release ccache" -# docker build -t nav2:release_branch \ +# docker build -t nav2:foxy \ # --build-arg FROM_IMAGE \ # --build-arg OVERLAY_MIXINS \ -# -f release_branch.Dockerfile ./ +# -f distro.Dockerfile ../ -ARG FROM_IMAGE=ros:eloquent +ARG FROM_IMAGE=ros:foxy ARG OVERLAY_WS=/opt/overlay_ws # multi-stage for caching @@ -43,6 +43,7 @@ RUN mkdir -p /tmp/opt && \ # multi-stage for building FROM $FROM_IMAGE AS builder +ARG DEBIAN_FRONTEND=noninteractive # edit apt for caching RUN cp /etc/apt/apt.conf.d/docker-clean /etc/apt/ && \ @@ -88,15 +89,12 @@ RUN sed --in-place \ 's|^source .*|source "$OVERLAY_WS/install/setup.bash"|' \ /ros_entrypoint.sh -# ARG RUN_TESTS -# ARG FAIL_ON_TEST_FAILURE -# RUN if [ -z "$RUN_TESTS" ]; then \ -# colcon test \ -# --mixin $OVERLAY_MIXINS \ -# --ctest-args --test-regex "test_.*"; \ -# if [ -z "$FAIL_ON_TEST_FAILURE" ]; then \ -# colcon test-result; \ -# else \ -# colcon test-result || true; \ -# fi \ -# fi +# test overlay build +ARG RUN_TESTS +ARG FAIL_ON_TEST_FAILURE=True +RUN if [ -n "$RUN_TESTS" ]; then \ + . install/setup.sh && \ + colcon test && \ + colcon test-result \ + || ([ -z "$FAIL_ON_TEST_FAILURE" ] || exit 1) \ + fi diff --git a/.dockerhub/release/dummy.Dockerfile b/.dockerhub/release/dummy.Dockerfile deleted file mode 100644 index 81a7ca1cbaa..00000000000 --- a/.dockerhub/release/dummy.Dockerfile +++ /dev/null @@ -1,12 +0,0 @@ -# This is a dummy Dockerfile for repository links on Docker Hub. -# Build rules on Docker Hub can trigger whenever the base image updates. -# Base images are specified in the FROM: directive in the tracked Dockerfile. -# However, build args are used by the build hooks to adjust the base image -# so a single Dockerfile can be reused for multiple CI jobs, reducing maintenance. -# To re-enable repository linking when using build args in the FROM: directive, -# this dummy Dockerfile explicitly conveys the base image/repo to link against -# while build rules that target this still use the same hook in this directory. -# Note: This only works for non-official images. - -FROM osrf/ros2:nightly-rmw-nonfree -RUN echo "This is a dummy Dockerfile." \ No newline at end of file diff --git a/.dockerhub/release/hooks/build b/.dockerhub/release/hooks/build index 5f9dd7e7579..50bc737ca60 100755 --- a/.dockerhub/release/hooks/build +++ b/.dockerhub/release/hooks/build @@ -1,7 +1,7 @@ #!/bin/bash set -ex -export FROM_IMAGE=osrf/ros2:nightly +export FROM_IMAGE=osrf/ros2:nightly-rmw-nonfree export FAIL_ON_BUILD_FAILURE="" export UNDERLAY_MIXINS="release ccache" export OVERLAY_MIXINS="release ccache" @@ -12,4 +12,4 @@ docker build \ --build-arg FAIL_ON_BUILD_FAILURE \ --build-arg UNDERLAY_MIXINS \ --build-arg OVERLAY_MIXINS \ - --file ./Dockerfile ../../. + --file ../../Dockerfile ../../. diff --git a/.dockerhub/source.Dockerfile b/.dockerhub/source.Dockerfile new file mode 100644 index 00000000000..149bc7c9492 --- /dev/null +++ b/.dockerhub/source.Dockerfile @@ -0,0 +1,246 @@ +# syntax=docker/dockerfile:experimental + +# Use experimental buildkit for faster builds +# https://github.com/moby/buildkit/blob/master/frontend/dockerfile/docs/experimental.md +# Use `--progress=plain` to use plane stdout for docker build +# +# Example build command: +# This determines which version of the ROS2 code base to pull +# export ROS2_BRANCH=main +# export DOCKER_BUILDKIT=1 +# docker build \ +# --tag nav2:source \ +# --file source.Dockerfile ../ +# +# Use `--no-cache` to break the local docker build cache. +# Use `--pull` to pull the latest parent image from the remote registry. +# Use `--target=` to build stages not used for final stage. +# +# We're only building on top of a ros2 devel image to get the basics +# prerequisites installed such as the apt source, rosdep, etc. We don't want to +# actually use any of the ros release packages. Instead we are going to build +# everything from source in one big workspace. + +ARG FROM_IMAGE=osrf/ros2:devel +ARG UNDERLAY_WS=/opt/underlay_ws +ARG OVERLAY_WS=/opt/overlay_ws + +# multi-stage for caching +FROM $FROM_IMAGE AS cacher + +# clone ros2 source +ARG ROS2_BRANCH=master +ARG ROS2_REPO=https://github.com/ros2/ros2.git +WORKDIR $ROS2_WS/src +RUN git clone $ROS2_REPO -b $ROS2_BRANCH && \ + vcs import ./ < ros2/ros2.repos && \ + find ./ -name ".git" | xargs rm -rf + +# clone underlay source +ARG UNDERLAY_WS +WORKDIR $UNDERLAY_WS/src +COPY ./tools/underlay.repos ../ +RUN vcs import ./ < ../underlay.repos && \ + find ./ -name ".git" | xargs rm -rf + +# copy overlay source +ARG OVERLAY_WS +WORKDIR $OVERLAY_WS/src +COPY ./ ./ros-planning/navigation2 +RUN colcon list --names-only | cat >> /opt/packages.txt + +# remove skiped packages +WORKDIR /opt +RUN find ./ \ + -name "AMENT_IGNORE" -o \ + -name "CATKIN_IGNORE" -o \ + -name "COLCON_IGNORE" \ + | xargs dirname | xargs rm -rf || true && \ + colcon list --paths-only \ + --packages-skip-up-to \ + $(cat packages.txt | xargs) \ + | xargs rm -rf + +# copy manifests for caching +RUN mkdir -p /tmp/opt && \ + find ./ -name "package.xml" | \ + xargs cp --parents -t /tmp/opt + +# multi-stage for ros2 dependencies +FROM $FROM_IMAGE AS ros2_depender +ARG DEBIAN_FRONTEND=noninteractive + +# edit apt for caching +RUN cp /etc/apt/apt.conf.d/docker-clean /etc/apt/ && \ + echo 'Binary::apt::APT::Keep-Downloaded-Packages "true";' \ + > /etc/apt/apt.conf.d/docker-clean + +# install packages +RUN --mount=type=cache,target=/var/cache/apt \ + --mount=type=cache,target=/var/lib/apt \ + apt-get update && apt-get install -q -y \ + ccache \ + libasio-dev \ + libtinyxml2-dev \ + && rosdep update + +ENV ROS_VERSION=2 \ + ROS_PYTHON_VERSION=3 + +# install ros2 dependencies +WORKDIR $ROS2_WS +COPY --from=cacher /tmp/$ROS2_WS ./ +COPY ./tools/skip_keys.txt /tmp/ +RUN --mount=type=cache,target=/var/cache/apt \ + --mount=type=cache,target=/var/lib/apt \ + apt-get update && rosdep install -q -y \ + --from-paths src \ + --ignore-src \ + --skip-keys " \ + $(cat /tmp/skip_keys.txt | xargs) \ + " + +# multi-stage for building ros2 +FROM ros2_depender AS ros2_builder + +# build ros2 source +COPY --from=cacher $ROS2_WS ./ +ARG ROS2_MIXINS="release ccache" +RUN --mount=type=cache,target=/root/.ccache \ + colcon build \ + --symlink-install \ + --mixin $ROS2_MIXINS + +# multi-stage for testing ros2 +FROM ros2_builder AS ros2_tester + +# test overlay build +ARG RUN_TESTS +ARG FAIL_ON_TEST_FAILURE=True +RUN if [ -n "$RUN_TESTS" ]; then \ + . install/setup.sh && \ + colcon test && \ + colcon test-result \ + || ([ -z "$FAIL_ON_TEST_FAILURE" ] || exit 1) \ + fi + +# multi-stage for underlay dependencies +FROM ros2_depender AS underlay_depender + +# copy manifests for caching +COPY --from=cacher /tmp/$ROS2_WS $ROS2_WS + +# install underlay dependencies +ARG UNDERLAY_WS +WORKDIR $UNDERLAY_WS +COPY --from=cacher /tmp/$UNDERLAY_WS ./ +RUN --mount=type=cache,target=/var/cache/apt \ + --mount=type=cache,target=/var/lib/apt \ + apt-get update && rosdep install -q -y \ + --from-paths src \ + $ROS2_WS/src \ + --ignore-src \ + --skip-keys " \ + $(cat /tmp/skip_keys.txt | xargs) \ + " + +# multi-stage for building underlay +FROM underlay_depender AS underlay_builder + +# copy workspace for caching +COPY --from=ros2_builder $ROS2_WS $ROS2_WS + +# build underlay source +COPY --from=cacher $UNDERLAY_WS ./ +ARG UNDERLAY_MIXINS="release ccache" +RUN --mount=type=cache,target=/root/.ccache \ + . $ROS2_WS/install/setup.sh && \ + colcon build \ + --symlink-install \ + --mixin $UNDERLAY_MIXINS + +# multi-stage for testing underlay +FROM underlay_builder AS underlay_tester + +# test overlay build +ARG RUN_TESTS +ARG FAIL_ON_TEST_FAILURE=True +RUN if [ -n "$RUN_TESTS" ]; then \ + . install/setup.sh && \ + colcon test && \ + colcon test-result \ + || ([ -z "$FAIL_ON_TEST_FAILURE" ] || exit 1) \ + fi + +# multi-stage for overlay dependencies +FROM underlay_depender AS overlay_depender + +# copy manifests for caching +COPY --from=cacher /tmp/$ROS2_WS $ROS2_WS +COPY --from=cacher /tmp/$UNDERLAY_WS $UNDERLAY_WS + +# install overlay dependencies +ARG OVERLAY_WS +WORKDIR $OVERLAY_WS +COPY --from=cacher /tmp/$OVERLAY_WS ./ +RUN --mount=type=cache,target=/var/cache/apt \ + --mount=type=cache,target=/var/lib/apt \ + apt-get update && rosdep install -q -y \ + --from-paths src \ + $ROS2_WS/src \ + $UNDERLAY_WS/src \ + --ignore-src \ + --skip-keys " \ + $(cat /tmp/skip_keys.txt | xargs) \ + " + +# multi-stage for building overlay +FROM overlay_depender AS overlay_builder + +# copy workspace for caching +COPY --from=ros2_builder $ROS2_WS $ROS2_WS +COPY --from=underlay_builder $UNDERLAY_WS $UNDERLAY_WS + +# build overlay source +COPY --from=cacher $OVERLAY_WS ./ +ARG OVERLAY_MIXINS="release ccache" +RUN --mount=type=cache,target=/root/.ccache \ + . $UNDERLAY_WS/install/setup.sh && \ + colcon build \ + --symlink-install \ + --mixin $OVERLAY_MIXINS + +# multi-stage for testing overlay +FROM overlay_builder AS overlay_tester + +# test overlay build +ARG RUN_TESTS +ARG FAIL_ON_TEST_FAILURE=True +RUN if [ -n "$RUN_TESTS" ]; then \ + . install/setup.sh && \ + colcon test && \ + colcon test-result \ + || ([ -z "$FAIL_ON_TEST_FAILURE" ] || exit 1) \ + fi + +# multi-stage for testing workspaces +FROM overlay_builder AS workspaces_tester + +# copy workspace test results +COPY --from=ros2_tester $ROS2_WS/log $ROS2_WS/log +COPY --from=underlay_tester $UNDERLAY_WS/log $UNDERLAY_WS/log +COPY --from=overlay_tester $OVERLAY_WS/log $OVERLAY_WS/log + +# multi-stage for shipping overlay +FROM overlay_builder AS overlay_shipper + +# restore apt for docker +RUN mv /etc/apt/docker-clean /etc/apt/apt.conf.d/ && \ + rm -rf /var/lib/apt/lists/ + +# source overlay from entrypoint +ENV UNDERLAY_WS $UNDERLAY_WS +ENV OVERLAY_WS $OVERLAY_WS +RUN sed --in-place \ + 's|^source .*|source "$OVERLAY_WS/install/setup.bash"|' \ + /ros_entrypoint.sh diff --git a/.dockerignore b/.dockerignore index ad7d6a52f5a..7bf7802aecb 100644 --- a/.dockerignore +++ b/.dockerignore @@ -1,10 +1,11 @@ ################################################################################ # Repo -.circleci/* -.git/* +.circleci/ +.dockerhub/ +.git/ +.github/ .dockerignore .gitignore **Dockerfile **.Dockerfile -codecov.yml diff --git a/Dockerfile b/Dockerfile index 0767205c601..ae2b22bb023 100644 --- a/Dockerfile +++ b/Dockerfile @@ -16,8 +16,8 @@ FROM $FROM_IMAGE AS cacher # clone underlay source ARG UNDERLAY_WS WORKDIR $UNDERLAY_WS/src -COPY ./tools/ros2_dependencies.repos ../ -RUN vcs import ./ < ../ros2_dependencies.repos && \ +COPY ./tools/underlay.repos ../ +RUN vcs import ./ < ../underlay.repos && \ find ./ -name ".git" | xargs rm -rf # copy overlay source @@ -35,6 +35,7 @@ RUN mkdir -p /tmp/opt && \ # multi-stage for building FROM $FROM_IMAGE AS builder +ARG DEBIAN_FRONTEND=noninteractive # install CI dependencies RUN apt-get update && apt-get install -q -y \ @@ -65,10 +66,7 @@ RUN . /opt/ros/$ROS_DISTRO/setup.sh && \ --symlink-install \ --mixin $UNDERLAY_MIXINS \ --event-handlers console_direct+ \ - || touch build_failed && \ - if [ -f build_failed ] && [ -n "$FAIL_ON_BUILD_FAILURE" ]; then \ - exit 1; \ - fi + || ([ -z "$FAIL_ON_BUILD_FAILURE" ] || exit 1) # install overlay dependencies ARG OVERLAY_WS @@ -91,10 +89,7 @@ RUN . $UNDERLAY_WS/install/setup.sh && \ colcon build \ --symlink-install \ --mixin $OVERLAY_MIXINS \ - || touch build_failed && \ - if [ -f build_failed ] && [ -n "$FAIL_ON_BUILD_FAILURE" ]; then \ - exit 1; \ - fi + || ([ -z "$FAIL_ON_BUILD_FAILURE" ] || exit 1) # source overlay from entrypoint ENV UNDERLAY_WS $UNDERLAY_WS @@ -102,3 +97,13 @@ ENV OVERLAY_WS $OVERLAY_WS RUN sed --in-place \ 's|^source .*|source "$OVERLAY_WS/install/setup.bash"|' \ /ros_entrypoint.sh + +# test overlay build +ARG RUN_TESTS +ARG FAIL_ON_TEST_FAILURE=True +RUN if [ -n "$RUN_TESTS" ]; then \ + . install/setup.sh && \ + colcon test && \ + colcon test-result \ + || ([ -z "$FAIL_ON_TEST_FAILURE" ] || exit 1) \ + fi diff --git a/README.md b/README.md index 1b3bdfa8402..efe61a83762 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,5 @@ # Navigation 2 -[![Build Status](https://img.shields.io/docker/pulls/rosplanning/navigation2.svg?maxAge=2592000)](https://hub.docker.com/r/rosplanning/navigation2) [![Build Status](https://img.shields.io/docker/cloud/build/rosplanning/navigation2.svg?label=docker%20build)](https://hub.docker.com/r/rosplanning/navigation2) [![codecov](https://codecov.io/gh/ros-planning/navigation2/branch/master/graph/badge.svg)](https://codecov.io/gh/ros-planning/navigation2) +[![Build Status](https://img.shields.io/docker/pulls/rosplanning/navigation2.svg?maxAge=2592000)](https://hub.docker.com/r/rosplanning/navigation2) [![Build Status](https://img.shields.io/docker/cloud/build/rosplanning/navigation2.svg?label=docker%20build)](https://hub.docker.com/r/rosplanning/navigation2) [![codecov](https://codecov.io/gh/ros-planning/navigation2/branch/main/graph/badge.svg)](https://codecov.io/gh/ros-planning/navigation2) For detailed instructions on how to: - [Getting Started](https://navigation.ros.org/getting_started/index.html) @@ -32,10 +32,10 @@ please cite this work in your papers! ## Build Status -| Service | Dashing | Eloquent | Foxy | Master | +| Service | Dashing | Eloquent | Foxy | Main | | :---: | :---: | :---: | :---: | :---: | | ROS Build Farm | [![Build Status](http://build.ros2.org/job/Ddev__navigation2__ubuntu_bionic_amd64/badge/icon)](http://build.ros2.org/job/Ddev__navigation2__ubuntu_bionic_amd64/) | [![Build Status](http://build.ros2.org/job/Edev__navigation2__ubuntu_bionic_amd64/badge/icon)](http://build.ros2.org/job/Edev__navigation2__ubuntu_bionic_amd64/) | [![Build Status](http://build.ros2.org/job/Fdev__navigation2__ubuntu_focal_amd64/badge/icon)](http://build.ros2.org/job/Fdev__navigation2__ubuntu_focal_amd64/) | N/A | -| Circle CI | N/A | N/A | N/A | [![Build Status](https://circleci.com/gh/ros-planning/navigation2/tree/master.svg?style=svg)](https://circleci.com/gh/ros-planning/navigation2/tree/master) | +| Circle CI | N/A | N/A | N/A | [![Build Status](https://circleci.com/gh/ros-planning/navigation2/tree/main.svg?style=svg)](https://circleci.com/gh/ros-planning/navigation2/tree/main) | | Package | Dashing Source | Dashing Debian | Eloquent Source | Eloquent Debian | Foxy Source | Foxy Debian | diff --git a/doc/design/CostmapFilters_design.pdf b/doc/design/CostmapFilters_design.pdf new file mode 100644 index 0000000000000000000000000000000000000000..016395c7eefaa5e917051a8285f3df52b26a09e1 GIT binary patch literal 74102 zcmd?RbyOb9w>J3V?(Xgo+}%C6L-644?(V@YxJw|o1lQmJ65QS0Wgt1pIrsj~eD|BR zzBTj5yv=Iawd<*8@2=|FRb364yoeYb;~Q3JvhMuu*6#Z5ENCV|20|MHb7&qOdKqJD z6Gu}*<`;<~y_lJ$qp>}`n5Dj>v52vujgc`QAGCv`y|KO(v};CUtei9mBVzCIEvmyC zN6+|YKe8&V76_XlUx?H9WfEuyy`PQ`-dH1j84!>lKfYcf)yt7P7bIk`EL6trHyXSj zcsG`u*R(v|f|Ry*W?7{jBw@1^qRzdIraL@Zc37*WvelcxCS9214nNY^C5b!R!-j4y z=g4fHZPKB@?G!urZlo=>oBmA|{qwf}ChVs63gQ{?o-l6m8k(m(a=jFUT0`?)a~&Qq zF>-=%jQ)}9fvrcP@xiupj6${*QuR!j(zGv+P8hP97k`;J)MfSJBkOzGu7NOG+TNew`^RAIqe@jp zWX>RS%s6NMxb9URekP+?Py8oFqk#`b@!>U$VW)hWG1qcFu>9>zK5&*7$CFtgby-3z za@|XMgfWvB8CrQ$Fv{t-$586|DAqY=aXr&_U`=H1L`{NdJ{*E)ML)2;Qz)hviu=&^p4w6N z2O2D94T!yPL+xZ3`+OAqgq_*_es_8s-;c}L^6yWFW`tVdDPwuT{Eq+{n;CHAbE8Jo zbm`rWJU0k^qabYs?iS$06TB}0j|^!0I|AFKt?cT@>xevRSbQ_Mygb{U)%jd3D-({s zPWcFXQrlL;)?&fYGV!Vg1S27Qg8W0e|ZF-gi&AUJN`N$mZ8#q1K zkvh8|^FybZ*-(ICp%&-4r?8jrHD@A1?Ew@N*QQ zd7tEVy4cxuuxF@{!ppZCE{vx%J*P6wxhF0iHqXW6C7UCVlxG2bt3hOhaRzA?)Z^X3DSyccHTqqPHM#1(qiF`wYBGmx!<_U94#J; zQ@WK94mj_*I{4fIm#@WMH-5|eA4%ut4VMoJo*?cjtEwvIhvR@S>CLY@rP#cf3ez^< z^lv`a0_MZCT1DFknHzgb1L~0A|wrr$MKoy7QTlLzfHyR z#QFlyJQH7I&*j5uV?4Z!zClOjp812|xX`Gw1ek)z%K%3rjV$CXBqbgGyh&|oE}2(o zpnq0vZLXCdN&*2igCZkt2H6Z8Busq><6FJ(TpY36Fn{7;rr~i+F+Y)LNY`@t)~n@} zaUcDH3@g(*+q#<>&pLW>$n%?|c%AhEs;9_N44nwl{=j8}L-fG6EF^REVx(>??+*>b zv}k1q7>Fjz>oz8N^HAv&u+WG4{VO9+r=hE^;5;qY)VM{oRiZ^hT)}w-M457ppy9l? zAn;P5{Au*)>t-o1J22QugvGZsEC%mUiO$oAMQ+{!Z{>3+1};!`c}t$Fqv?x5wpP3P zIyf8A5`>fXz&l!R=9w?Y=?fLU~CJZV&t~scb06|gV#XX zKko$&k5wdz2F>m~VL6U1AwC5Thr}Xf&bIPC0SS|fVN7G_kM&X@D;K{DY2nca z-`>#E*b!9&c{q9)Rf&f+%>dZ(t+dh30FMRTZ7N@Lv3JNjd$16@OXLD~+xFt*(OR^K z8Z1-=%*WV^Bj5xfdJHaK*%$%zi??5hkYc}We2s)fVUpDx;(v@roL4?g%kFO&}s7=zg~K2SZ*nel=Q*J>RF$G?FJwpd*ug-go1ba#z@{aCP@#J~E5#vPI8H$TftHjtw`~ z$(c;nKA?KJ;MsXD0`l0&FIC>{o2GTcta3}B@E6MXP^r?yPSHm2raj<;qRNRp(4}c( z1^PYQxl*OdH<~%qDsSC5_C@phBBwa<``_cC?1@q(fX%~;c0=q3LepaKZD-v0EAzp& zM<&{O4x3u~#fa*{REF(*6D-jJpWk~^k=%suX(eM=b4l3Gm8(O^6Jr&%$^N#rxzW-R_+EaMAAp>EyC~d|g(BzxP!(8e`N||x6tt$auz^0@H)$A}D zkjHv)FEChV;@+qcN;Cm%ONjbCmcKtLRU3X_`Ze?@b0l=!{&c7N<+=Y!f!WjaeE65M zBcwyI=j(hl9NQfL+^Xzr z){cZ6g!IBTmNxcEw)%#~^diR2W`@S%_WEwW!x0f-V zCHy-|d7gP*0U%0=Ns0l0fPerJFB{-_9v}<=0S12kd-;LB{DDD&fq{a8K|?@*L&8GC z!oonqz`(&HBf`NW!Nb5Hq9Y=qprWCn!6IN_qN8FWqoScccL87^0k?o#ARr{ba}NOR z1t0KVe~`~}0LY(Gpv3>0^8fyo&kIFV#Q)AP`!9w-V88%^|D^^v|No=`#832s@!Mj= zXTbl@xBsQT`4L}yHTy5k>$U#AHm{dR{C}q<{i7k#|7Sb;-98u@URwq9@AmVLaQ@#L z8uUw}dkyvpwFntsa_T_;-JV=C4t2b2Rm9PjJ#2u_W(APvJX=Ajn8HVE&ty%-Y3UyP zCMTl5DR4X1s`sN)Mn)U%a|Ccb=4tk5N^6w}NJw$lJIeQQFhvV|fCig5o1Q2Usu*B* ziy__MwLd;;HaYehY>8=&*|2)&e7v-5bTpsd5P09ReK1?oVlt+Potr`kJ7dS-8GTWM ziV|5F;5JCEOtKeAo9A^kzB+jTzTcezb9*gKLCE61Gz8B;uuDM=57~RuN4>GioXlcy zG#(Ob25dDcRP)Jgg!hDy%?l)vlh17X1NuC*b8) zp)PLSpZ4}wxVeiL{F2tIx_n&tohZ(TJ{Ru7-UCB0&cq-Qciatc-S_1IF}#18>^B9h z;|k72Dh+`c<|t?dXVvNZZKX@sJGV@U-$4dwc4XQ$j8}X!6UA^PUM2(SQpbau%$C%Z`FKob z6l@&3l1qRF2bw6nwYkC4%DfAKo$@Wg)t<7E^&|zj#h8+QF8I8HO%^=M$q$%SxW`-Y zOY(KSi0R&NB6eH3tv>E4h2b_qxq9;*xP>6U_p{+vBfz5kA3Ru+lr~IZF3da&~J`}ZHkGE*wuqlc0F*86bkuqhC(J0 zOnbgz7$w9#yLDQ<6Yn|1rgjDyIhmyhtw0VHNJ9eU^NwU$VpxhS33>9vaxk71j8GcJ za>{;F?B0YwhP{SfaE<>#j|$@%kfiLHc{%P>=2~}(^`VI5qiL;b1c}45|80c7FTiB zXWT@HG#|wx^rME=`)QJ!7qC~a}gqI|>^EVT&o^utI$z>Z&I1XA+-Fj!jS z&BIrNIV{U^rHgFFb*(O^Ow7F71c%b$VDZ7~aw?|?J6Ly3$wj(g=bbH^n^W)b++FkB z+?+)IM-1XRr^^F!zch(tnxkVwQ5zGOxxpB=d7-u3#rZt!4)9&*&j8^(6haLugV3Wi^~c7xN? zfCEg>-b;y602H?=JTrZ6SB-UNY)SW#bUn$}AGh&aZF>8Zlph^(8ZooZzbE4l+mEBI z%N|@Fxy$UjtO4mAjbB2ZRzU>vJ?R(9##a%Y_u0RjFhIj_DXoLpChxeak)=$%E&ydE zWkU3a+WT~g7pCH``nB$f-&N24isTUpnQR9mwJ-$FoxxbNOUPq0+G+^wo3zhIWGa1Z zhk*`ax%m{++)q+$etZ&$={zeiNLVh`D`fmKCq;^50>aFAqC!a!;3&t7PkQaJKuv+~ zK3!I%$}|<$YM3Z$?I1vN@tu4m7HTf_v?(Sot+i`Oy%~h{HEaWz(U3Zp+m;g1V* zRdY(~5niMAN7BtMD`np>S@5SgWl`ZzqOiLp``A!=A6B*;v%E?1Wc{&cTJ5rR-nAJd zkL%Q}S@SZe`D*>P{Fc(}*Hl}ryorKp(BE^<(N zLl_(ad~$TK%^f9A>Qg?V&{La_?!4clmDRnb)^%M2-p+8X%mMDg@={xSMWzk{uB*qg z;NaCl-~a;R*!oSZK0Tbs+k6#eu{CQh)SJ#3I=$;~FEI9MNvVPjy0=g1GKm`tX{*v9 zOxn*Ot-JDTcoU}LwRt4i`=Qk_@Qq+ScQ8vfhOLS$8dP&?RB6?U?1?t_XmTMRW?IYJ zT0i^ewB~+5`?@H^sZHaW({)d`a7Uv;N~4mmOnt3Zs&NzPxGPWCxO;+UIzyzxh1z$Y z2^9-Zw(E@j!~tp8AtYsWY;IH%cvCp1wgDZ|b#r%NV424jsc~u)B%$k~gJ$rk5rKR(Qi}$UyyYdIt!4|wxVqsLU1${0phKo8nne4@$>vlHa zeXQJpN9dWc-^plp?WVo3q%A+I(Vj--tA#mI!atUA%stQoiL!0!o%zkMtkabu7G*X- zkw%Y0rFtgO>V3S|j*DtN_$;#I#>TLJ6*|yOvL!EirNxBv@PT8^QeDUhJIN?OzQ#IN zOeGXW#(l4s_3XnnS(?RHcs3p%^(n$>fh<+i)hTih5y=G+98|8Wh|7(xtraOIkr4@~ z!u>K)yhE1;A@O@b%@~*7;^wgW5`#&gs?bL5;nU2?HFHt^*;?6QzY6|%x=mYWNaQAc znSF&Gol8JZOfovHy;2+QBxh&JCo1KQ;e5j;auo*<_;<&})K#hJdC?|PDJ1lIZA}FX z?bAYIOe7_kHtf4&@j(pi6UhCl{j$d(O3w3dR!*EiRCEcyQgQiGaW^^MkEbTrmlp2; zOt<>K2Ai(HUKHB%Y9NE+Hhw=9j^p&uh+@E2iRYlbzWDb2bEg)^RjD)O@O=F3vNZv% z14`6V0gKHWhxCtHk>F@Fdbu(6G^6uc91ZPJXBb>c=VLm)tRw|)Eil`GZ7NY`HFh$r zeUHfG#b~TMR4Xvs7Y}(=yR;Nms)n~z>KqDL?3dVFR=VN#5{pK`df(F-#LJq~D1i2k z)Z-bwQf}@c=0Uy)lx;nSv>>S|BfDT;)>}|Vv zmzWd!$!Rv{7Mo23wK;p$bqh3Np6(_4O_Px@*IqU}cX<-O4lr>o*yOH36 zsAgoJX}JfjIYb1!l$VGxu(7^RRx8Xxx^=MKv^c6Xe))#%$t!o5{Hy=Ucsyi*lxGMr zCm}Rg=jcL03j2DJwE1BP4wvO!OWS#}#;Qjei(MPlp@PwgFZ-m>bq}5_C_Gi~U~_)Y z@v-7_?4_rmUdXKC(E{!AFK|34e%{GXmMU}a=u{5@m9_&UV-|76Ml2LJ>B zd6|{?$EX|}0ul@a6zI<|8fcmju(dW~%=|KAfV+U9K5%_83;+O)bSC+XRYI1vF0ONZ@bl5;o2ml~zLv-@iFK)dQ=fA>-P(R=LQU)jnsKxyFiwO0x z{SE-}GQmNJ_~LN@@ISl`3I+}WAplIs06;`z`+GX%p3#6Sd%-@BIBQ5bOu418H*_zc zp@Fz|Y=9t{qfawms$j8@R$LkHOA}h5k39byyU03;MCle-_2^=54|^J!OM!=`KBj46 zl{fpZNQZ|570inuXWN)R;fgzb|AGsL*c@d{DnTxBkqiKuhOo00HOpc_7|JlDQzTP7 z^4nf1=Z^5Fk-5wE)?UXlkE2fZislCM*U>&!*ZmuA$8Lred28Ff>yGj_yJeE-PQ7Gy z?(Z0rf0*3hI>UfAH7!-OmHsKUx?itKI=x~aX8%tK?tj$2{!u$wuTm!)8NCd;8vku; zyM6lB`{twJ23{JUYf~Rp$uRXVYfRfI&L`bM&g(~fqZS04Mk$FAi}`QdLQG&;Xsr3x z54Nf=%Hxr%J!*r-81`3#0}uNbyfVJ?O@A;@rFV{Py28_D7_Vr&kEu!PU)3TdGq^T` z$%SGJKMVarB+#}yCwa%S{tSR>>;+;{Gj6{4K=4S>FHibohtH&yLvU|+adCp6nWJLr30|9}9 zf`S7BAbvgzfIxsj0Z7P13i@D70)iwA6DKHy#Eb@Zh+W{QYymm>pF97ms4uf}=jkfS z;|a97R5))wM~i)o1K*s?s_oyte+EQArx^wtlINe(-XDLjFD=QTRBuycYkZdmmIcK> zbNnFmIokC-Q-U0AUvO7p%4*7B3C|GU{iRiDsi_n?eL_?vMxmccm)up-Gk}zaay!_N zO62Ti3Y*$28Y?m_ez@kiU2(3oFfhm9R$^||-4xxE^ci4gbN=tJ7l(hr7FY!BW^0_p zKNhsdL4W03P1y?DOc|e1p2VfaVx>qmS@uq>Q;~%d>OD)Su>HaU!-wos+i;enA-Qhp=(<1FdP=-;^t&6OsB5ecK_TiJf$;Wg=c+n&&H{u35U ziXzESSYJuTcGzg@qj8qk!`0MOJ=F_?j210WdO3KFlj%>Jba#-d3S+Q+iofYH|Dx-| zTE|KkE?@0g^yYgftmNttA9meCLA&Tr+rBFNH^!Uwg$32G-rby=A#!)M9}BMVp|bh* z?>@VwD=D#542sy~bgqYd_Yt>dwM_S`*2Q>v-bbv+0+b=foT`#S4s|ATIi|6;X8^ak zxhC>o+6I4EhyFY3#D8V2@?Tj81^lW#5C3Ex#=~jSt@`4#Mw{SQXA0!s5(I)h15i$zmv=wiS5*u5L5Q>Rz^-_))llwZ_{H7hf5VPy z9-xw^H5$e2B0iH1GS-7#b0DaS%U<@`$XN%;OZy0J%a^W^;1tm@@EBtTC$^}JT8z$Q z0;`YYM&9+g3fzt}W=3M%rv0n@so%ti90&)2Z24EN-W5yl{@UO?>s)AM7#!wchS1h{ zeGPRGj*R{TQd9QjZA#aUovk9j41~h(#&`xNZ2X3E!U~guQhtW-ZTpZgK3LLFcbK50 zJO)OA-`|oUVB+CUoiO`d%nj$2zXR`Yec#&z>bu?`Pa*%jo4?4!{Y}R7`}ZEXuwf&<)feuM z{2e~y|Hb9J_Y&r8x4CsaBk}BFj2IuRu;*sNPB|k(7GCdA$qmBYe>#1cRJAenv)E4G zOI^v+(Lv%PBZ|F$y+@ZmK+gc-e^!^7>qDaA`J3<`C~Ojom_6?#Yc0xhDSGmm2y9xr?Pd>~iwQVOf;)y(%YmRk#0%c7R3!zbB^15C33F8^y^9Q5l9{thYliPkNh6CXcxt}J&H zT1jv|1HcqNCdm<7(;kYk^_%Jbn5D%>-Aw-E*euVPN|MCvh`%$Fcbn<%R=MgVseb9&71(%iPm@Gy47~X4hGJ)8&Ts`I z)h-**b^N*R$XT9~p9}k~X;cqIe*8ds1h+y^J+RZAB|iPf5s%D-CLZixi3PtB16~uo zKfZytx#Om?clDAb4PEf~gG<*v0W2%LT-m^mw2i8p@>@n7@8zvhiRdD>3o2TfPB z{aLJ#y9Q5KV@(|{)@ev6K6Cf|FWj!5xKO*KXXI<6Q=Z#>9|>K@5Io(KP9*Cn5MF)Q zL-Cg}r&*Y;e)ktA&w&1)xOTjMne*YNoA_(i5fFJyLaum9Zb;|iBlgTl#nu;oXMT7G z81CpZW42dy-F1qT0V2;Xe+JB)cq29;u-FV+`yegso%ng20%(F$~m$J{nTlXZ@L_Rl%Pf=P%*k03T!njDggSPYR4nP zkLFA}B#nLJKDR{*%-RJ4mSH|6+!u0KZTy-OSj{)*S9#8;ub7DX4L>m(equrvi2uq5 z4WyMh1j?}dGoS9ye4#>M=VA|8j%xY>G!9LA;5ao%@<3SO?)ck;Ql|+-TT!=sJC$9& zs@wMKz_k=Y*}zN~lyVNSUMr9SZLdVb4=7*AydfkJa-;?j!HySy`6zJS2?+WLikCb7 zV!bw@Sm2H_Ku94+!Gp(dJCFqUHqTqGnWjD)t!beV5Ct_}3~1Gj@_os*?lOnTk^I$Zy{dItPFS^g<)f0aG`N_hBnxP3kNod3GmemxwIz3#8GUibI>uluK$ z=fpGMuME$C<=3mnm(K7d{U`YKPw?ZPP|H8n+Mj%0GqwMf=kite8SoOfzHa@$4xfG< z2!0*fUk|_3djO;_FL!}~fk0o!Ha{On03akH0HYv-fSn&{Zw{L0ks0i`M}y$r@V;?^0n&k$d4PR`m?~en7atgzLji2d?1FV9SZ)T!XT*|yS7kaIUr4L)GM5NchlV%3q zg$M0FyvG%-F=61Nq6b<6P{YkFWca4s6&3;Jul5=Dz?DV>eCCYspR^q_YI1v?N;_U{ zd^f@v{Gn8VQLjqsDj>5T%+z-tUjzx!J{T{NqB@8es0%`Hh>KQkul$wB=D{SU8MXdT zBJ=PrxW9;q2!6#>RsPtA8VUhPKtZ=t&0kve>X68Mr9avW@M^edc|AXr ztM*UY6heQ~ZiZd={B5Lf7Y((XA+!icAVPPNqk7h~IuNx0TEA&YBugSgu0f|>;xTab zGMK6V3vXb;O<3-~w4p|}X!D4!u>6GF=-sO#2Payo32F6+;k}>qEdiSd#jst#0!Rp@ zBcdf!MFzx>ASB5qF__)qOARU?54H~5?oi9nf`i)n310@FF>_&*+SLf+!& zg%ElPyhlU^L?5zI(!H&L?-4r(@a+Lo0a&vPDak8d=r`={%X988*w9C|P`bAT+(?KbFK=XpPz91o zdyKO-#v^nn_1Zn)g}8ic^#=&#u>wJTiTP!7VU$YVzA|Xmt%fF)Rxw~2W0Lq|m^vox zUxtB-?$!#LJ5kMq`V&n%PHVgvRt%J|&M92MQtf{x(gb+`B+(tqm(2_b1rWhc7eOyl z%nS`B#$&NkNqAx4i{%D6PCPG#NvDI{A#MaI_S-O`x<9ZJLer8zVUwz(fazFr{HG^q zL7jt1|FFff`pp(04ZT!~P+HL!i^#GZvi8MaYGQ!snVv>_h$(P=AVSD3)d{e_?r`t& zCK)1Gi2!Vxw<@TnnJ`#c9zfjI;4N<0FV}J;{$X(I#Qnm+G#gy5X#Xz;z9h|zZ+?^I zY^W9e%Lg$k*+|P>j zTCV&4NxWJ>)xR#^_&J&Qe}4VO>-y~fW9v6EVJvaQvHIH=oie|xyRmR(rK_ErtC)%k zX0yPVp+Kujh`NTN)*)uEO<>3=NSRHQiYgQXPK$lNC8;9jA~wX2v;GZ@h4)iiYG&$Vn+;;r8mjQt z{l{(Qr*4r3GBKpeeU#7pdm^JvUI?=jqiT0?ZrE`S)?3!<-(a2Znzt1Q>_?*(hdO|B z?r(Jcb_GWnnU6vH$t~xD>qPh_tipvO-EKrpeKTlh1eYrPQet#${Pbt5{L?*ianK^P zJrOgI>`k8#jxVr7n_Ml38o|bL*e9aGg-jtDL#Nn zx3{-saR=;^GmI+eIi$Be1#HSPgnuhzzx=>Y3d`HkH`%T=AwljeRZJIxt9Q5kPT&F= z?MU2fzS}R09u#SYt#rp>|FNa9n*t&;=a_h&gFl0d|0_`IF`2g=(*TGAL2Gz`#%2{* zGuUV{(|||N-41H@$joCDn%?%f$#dMbqcZQ;_{P(yO+ya1u%WTv!;VSrfu|m$;%P5efxCv?m~sMXgHpvM6<~(P^-YG9@>U!=uL$)4?TIi zT(9-pXm9p}%I?zm%MAC$j8pRCGK6%GA!CrQi{D&5Y9jXR96x|u7r(c+zkt}=+%mr_ zGQg5rSfO}7#h1Hn+HCEz>P_fz0Uk4u4d3Nc<>w zdF*W9@d;hOM)a@p%i4YC}V9L?oBCE z+o)2jv-~ zQ#O@gCj;hNE0IuWBkWFwC&#koxUa?koMO%&z zadvX=ra(Tq349G*nKGRRv4j=$gB%Wyr*tYKsd_hV6yxR-$HMmZb_^Ro@Cf|WFrUkT zdb%OMqSmE08Qo>9{9HYItjxyhdv0ly0?7*54z1z&;br1j)(Wyjb2G^iohCa-G8nAv z#f|JTgG@OtRacRgB{-e`_FfA>_C(lp&bPDN97Gi{#6(cj^&ea%hk7jb)Ki40^G{G<*+)rK|&Y#Ua4NHHBT2lGif3%g0bGnc- z(aF1@=Gw2?cGVces|5YtfsJ-J*w5u8Y`F?suV8PbpBzKO)0^@}H-fH`$5rlSrBhk5 zuaAJ!iDE>EzY8g>wpIkr3AN55thM=9bsmH^0j-GS)F!M53#-!}zw}hC>JJFk+F#U7 z{L0#q^tBh&J!@fD>s<#y$<5HSOkNVec^Fro(-mJ9yEpeovkV(1vPsaEw<3h&9b~-_6{U##FxwTG9~?lWliYN( z-gO>slBIl~LzGC{?2TCr)P}Emm@X_^oi`(}=_D|TeR^#qg}8gsvi2ou7@QmTXTB9b_<*}HbDo1M=U^@}aH zqGe1>=04x)gqBadB!Inc8Kc^)$+zkRW_BRRUW{FZhIq93Blml}({OraF;oyWg(`F% zN)zIOxIe$HZ!D}NE;l1LRtcxNw04{c%qE*MQI&)fd&;RY<-@bs!}Nz_im|rWi^dL` z;tAFxWM9LwM1ggRHG61oRI?Lqpg&wImbm@+ssV3o0o|v8-ny^Sq(~d)5x}u$+8S>@ z8InKeW6amKfNDlngUDG$NfZC6Kb7M-MSD{|I>rOuiEz+Bgy+G5XW>HGN+6f-XmzqW zQ8;yp2JJ@Ipp*QBvxp4%`gEr82MY|T#$cYQ#c@)+FcJu|-~9}hl12^9uev_Kf&LlA zShZ*P5V#3?7V69yP8tK;r1`R3mq)m~=-aj+92r$ed)QjWl|+uT#T=MH7`xB(`(0I& z+mJaBWw8s;C2+V2y0^G9{$BRU!dP!4?kh}Tz8!sNB&6HIw|1zet&F%@s@L5@?g7i4 z)XxQNfFIOxbXQkM#>(=PL9#{A4MpVUS<~RUo#Az}%$73lW@Q=~9SUnWJEKI1xe1r4 zIk#VPNM%*P$2hYH>L3WWbyV8%#U#*;hMo6skgB7mqTw40JwYs{FI(H~`f(gt8n0EQ zlR56B`aww<?2+Lv)>f;?$J?>{6T<`v*=U)}-XZnpaIOXWRL@Bl@+-xpFxq zg5{z7;7Zp91^v`X=)3A$%-#blMfHzqwYgEb&104YJt9qPD*foGo<5nG3;qq6ire@f zF`{g)r#RbYTN0TYQ+gP#)7&!_{cQ56L+;SR?O~sF`ZX5d%X76F-B-G1ufKbK_J>Qp z`g)mCZoo)v^9gAo`1=pkgB8CXkh+lFloss9qG#DbczlW2XW&xyW;i;3EtM1BhlXuc z?s{(93H^d5FgJAW_bQ^+v*XUdDsq(QY{%Al3_rpbagQ#BOtfi;kOoGNY|0tl20r_} zw-J`B?|-ff9pL40bs{5Tax5Glp_ouPf6}2Y#abb**ClPXakDh_KQ z)~Qs+o|)Zkgd8l3yu)(u?XW-R78K73vvmGt5$g;}?TRp2GjT}P;k3RB4;rWbB&Ky^ z3K&b)NYMw;h8o&3Bg#R_tj#PC`ME}5B zzDkVZ{o?q{w=vjR?MBCJgim+$Vz6HkXl-OKI!PafHCpSndT3=q&txOrGu8_SxJy)3A zUH{R2-@h7y!&ziGM^dB`9tVdCt&kpQ^GUdtDH`Uhi&1NMFKw?E40(=mah*iJM%ghZ z-j-5w#Z)+BD!>4FD_*0*FURNJf^7rlsU4!K-J2QDyVWle7ZG4IEajobz>F6s73Mq3 zAAR?D(IxK1dm4AJ8x(yfM7}KbjhLRlN6Dh5uZV9iwxw^9pTEr}blxbeXDeS4F`1s; z+;v+ISzIw#ufhIdFtLy73x^k zOP}N36;;b&51)?A({z)f)5ddsMD`^*VRr_IMz9gD{Y!$C*vKcvK_Xan>PMbbjLssN4yc-`^{8w5Buq8jLr83 zGLoDeU&!+3ETO`@bgdInSu7fkCObb$M=eXm2W^D9_lQO6<9#5ZpC&6L?Iz;x>J%C` z;}VG(T2dakaUOlLF0o}=%Hly|vLtT7qiFoWnoL>!@p>gJ1 zvRLWmMSFFD)iJfBOXsuC33Puy&f2^vv-Q$7MRu)n(6HsUUS6RBAd&zHaKSR!Q`0fq zhiU#o%GHMyM?X+$A_fY4AsKMY`iaxB!*By}ds$h3ENI==LK}SWr)h$=ZQ$<#5;*!& zf+JQx<~3b2pHFn0P#r>-jFm!~ zr90>VxH2!_BcwE>_62#OMLOuZMxmU}S3G_odlOL8=hTA;=*yFlkE3N+O+h^1 zGqgs+j%o~w4V#gt>r2E%8(MP`-f0j{NEiPun1ytie#UM`MxSQiX*``EkI!)S$gPwLRI2Tl*0=1}_4vItTxo5Kd zZkbsm%&V5=jl-K%s+1Sn7{BgUz+5xomI4a0Ozry_$@8cY+yM;MhD7RX)rJK&nO)6C z8tCe%QBQ^h_J9VhvahlGh{#Iyh6R{38&W8nYT9bUP;)d7pVAG(i8OEwKeHJVM2$_* zCJ^OXvn{pr1#rFL)F0pqa=ZqnUT+x7WZZdM@mSJw=~jhFRbdX>U@&;mKcvA>lOQT} z9$IEEHe{*NG56?6h@-qQzD(tC%^Y77&N~7s!_X-Fbz|~k^{uiDQ;&A($Ir_uL*V5O zI??`L=pnMzi6QI7M%N89%2Y8@ojC6jM3bwiBcZ6lX!haL!EHe91~@(15z_|@HL-e) zv1h}w4NP6o>#$v7s0Co9>Hi-&|(B}TEAWLp;w2`H;~E%tF_GXu01_519k(}(F@KvzUI!(XPIpU z;+hn#vGi6~>QEg}650~IZ)q4o;A5{oxrDEc`W~mNh8AX(tU6=->=BS(KDpi^#uuw35KHi%>$GXbe$& zYtoO;v_s3R=3+HW{WhOnRzI7@E}^*)W{bs;q+}P&%xSw&E6N{n^?3vL@b<`Ent(R$ zQ>U}2r)-_2^gH20V z(%@nYtCe_855n$EkfLp`zgGPSX^Sa&yH&Wozjac>0Dtr@vL45JVYT5#q^{jp)p8R{ z1mD(hNK3; zVU>S#`>`}yV%|8iz7>B{e#0VT>3U(Q8?Jo>qwi>g$hB~KRK0%aJYZ!;k#D&7C?RnBAm3J$gDMteC%?;piHK~kMm}qZ7V4@Q zsF3)+)(AFKw(|`dyJVHdg}&KbrXhqgh&|chx)6+xukZMAxr#g=!{(gFtWegxX!UPO8Sfv2It2>GN2f5d6{#zUk{nc(`vBO{mAz zB!J?N957n_B`dyMcoswz)!8o%I|^Yb#prE5sEB{go!9xY$ARPC#LG3fnvVAcmtZId zMp+Q7V42izl{&KHBh@7M3@{Z?zN1`@`j1vw>RPXubbhd5dY$Hnhj;i(W-ZQ&i(<@q zAZNR#o1)XRI0QE$vD}w2^dmc|Erq+>&)fv)h?^#rinK`~V4-06>GC?dgs7z6J5LBv zW9k3)CGF2x^mVS7iH(Kr@1XS`OJjcrt*@Wg{!c;cp|-RQX#-N*^{1U0zr(75H5f!t z(+kLz^ms6PNPERK&^}55&(q~GwO+OB;sPu)JQ9ZYU{QtU%^JP#rzB-2BSPo&lRod6 z(|c!F`VW#24~KEl!5`203)?-}pFMIlyuF{cjlPQW4bx9<8Q4ZYUNOy7BhVb*sZ6#P z613AWodz%>=u#vnOIX+2RTGWrKJz`VLIJxm_KoQJv@$y0Ul#42?q775-9L2Qu3u|t za`euMY8u#Xim{{6aiUm1>_-)7`53MpVlZuv!3Ak~7=GN?eD@CiB9!0ArhTQ0douw> zw)t`X$MJIf-sD;g~7D~w& zeY}R})j7RN1OFq^yGMiUz)an(4%c{WuZ-u1E!q{i{gPq-ep&`7CdX`JFy6PLD5SVHJ|d@0hPx1u2C~_aehQg)bkL`rjnqSEccNu2 zX;-D46_H-Mct(~kCX+rhnZEt5&XpI45Mfhiwysg8aIS2Q5bYO1@%iCa`l}EBx?l}*lRT1P{^6kh&syBYu z-@(4M4yL_*pU+&~=It!%`>8ao{|?)GpR(r|srWa6yWgZ*i(2W)nptNrYfWph4Hb zSRmdzr|rY!^|kP(qMbZhc17}ncN-p(N^=I?ZdOB0^yeH%2+%Ms*voDc1PMt>c66h4C;m&AS66Q%0Kd9Rr&% z#c~6~WGqbumjY1qB_iRtJc=B9KxfrcM0Nt>jP@)U$i}{gxkGeQD(psn2*&m+*?!9} z7q{vRoe9E0gS!O>5oWHqEfSmu2CPPfls%x%eUTCve-WNb<&BY5JVt7-DO1R(wt2JD z%o_NX&B+2zbA*bREpmMq%>ukt+KdbsYs;+wREm~Zy06?EIkxGYLJ`nh6#)ua=(u&{b4ELVBth3<+J&{ryP5hwcCUoDh3WR6@TqHSFLlZSq&MA3(>Aa8FKo#+~Cd&KW2)dwNkeN2Jv z>MF5(l-kAiiiH`I=2yJoQ*uABohlT5?%MUEkVB1PD-SUQ!uFd@TK`rl5<&g1*6jjNpGZA{6Cyc9pD0UB*^*6WMr0V0-+!HxRt%oGTE;S)#4L?pV zddn&PrEjCPxu_6hM|(2LJ`5aMu-~>=QIz_99#lA~JQsKw%QPfOiD=rCM*lG8eB}pR z6@3)RT;gKmap1Ct^+kP5FQ$gN(S&;u!@QkR{a&o zt}lxW4Jj%kt0tO?$mNE_a+HyT83FW2&iJ5aK@?0W`M4}C$|Q_Ai!bS=c3ueC3-62YnxCy%K`NjW5!2e10TA#5=Q(q`N>-+{LIb@AcMc)XRus_e&@yChB_l8 zOQ9&<0RX7KzRn29cUqODu>dv#W{aUK{gGqiY+g zW_YFCJ?IMlM>KwNlgy`%JfOy1A$-C`L&rG9i<2sEj>OLni(j1I9G^(7Ea{YnHAI*! zO^BqGyNnJX@MfMo?i@A3IlC9g) zcFVT4%eBk4ZQIr^+qP}n-eudiZQJ_y_w_m5eWFi9--thM+?x@Zv1aC)YbC~9k#jy{ zyziLrbOMN-^uV0Tb&PQ?v04jYy=@BS%FnVr4=|oYN5l|u0e*Ang37@|@%2ye{i-UZ znIoRLv1~i3gdcO*(U?S-*evyJKI5QQ`fS%y5R!&I(};90fb)T5&tgw^yIW@iInm7} zu6XUuzl`ZoxU2vd8{YFsVSo~#_De&MfF_}+Loih2r~r*p6cJQ*y4VIU?PK>)n`t3y zwPkeT`;4d}DhrWuqHyGAbk-&S)^GuuG6DB>__$@FLh&7U_$zH~o_z3+>f18PlH@=aBC_dX3{UZ$oH@7+ zi?grfk%wrvi?9Q%%E(1uuu}b65sz^)>IS_2nY`jB!+?nf)-y$=^f31{d?f0d0$`>1 zOBN_ty@^Dp8r&1;%Cz?#B}s(OKiR?T?BN~-6)j|neQHd9%N8WEWGXCQv_V?}g}d^H zyOcfQ)U31g;XtNR^@Lz)Y?-WL%m#Uzbx0`$l@ljyo0asI zLuhIZnB6iGj-dBdeW;z?&qz0*!2uklTXRTToZDKIUjhEhR_bPT&Bz+hh{v5viqSxFnZ!&|7LezLvQy>rbuzF??2gIQ35J14>WJ)r zA~ZI{yxGcF9^9L4z8t9RX?Ru-9{>HY9x=Vu8#V5px?;auWXn+Fkms<#DKPdbbRRtv zAJxx!5zFPPJIN`4ozmvAB5XwsdtKX*rhT!cPkO9W?NMMV_&h~^T#A`!ud}Jh2-UC% zQ~ws5J)j9z3V|(?Uxm+qlhrJeJSCjZ@3U2RJGZn5*&Xcw!Cd56I+A6q8yy{ z*ji25pc2LaF!88AGFadme&w-Epw|5`{<0(HLHztH^(*}hvlooxBp#6VJmh|MF)pqDUEfBqD#D017t9aKYT$79$vb5?vx?JAV|)A}VEnZuTS3tK4Qa*Am4GTv zw*<1BO<}Es%XG_Sqj#a-m-$DRM;C{v$82(<+cU~abF7}I%@0*-qAXtXYH_lg3Mxhm zO$I+W8mY}hRh-7%{X4T=W>?DAa{9fxksUf#>o#x+I;2YE?~bA@^uksYouRL|@A@@NRp1Lh9SomKuwk`49%&t!SI#{a4&~Ihd zLO7`fF^J7+2gKly7`Wvy6jm%aj3PbAvQX3XlT51`aN`@)i^9O7J)LAr^#t0(aT7}} z%f}n=E1>&j;Lko)MBBG>f4=_cR)+-s)oxQ3;V5ag%~v#EvaPuQ7k+Di8!sdAJ7ljg zsvp;-y`xdzIgWz9L!ZPae+XG=282zXAj=AhF#OSPu35;6V=cw(2L`G|jl9fSEIOl{ z&9piEEcc~b#3aYs38W2dg2~ouv6u4TI*EJx^PhzLiFPZ|uBXEQ7R<(F4UK3P0b4HQ zSjz|cJr@ldwK(nZi$_(<_Z~d2uNXgs2X)`Vh#1M=4FI2dy3{dgNl5QYxp$#9V~1w- zp+$se1yp>*2c{v7f?t$=1w0zYAhDz73fW{Z5=N?IHS5+XnbiivW!z>YI292Q9T6&Q zN_h5&(fNF~WBVI@`cyh&amO}4ciA-Aepe=09Oz^2j)c2kC=}(NFS*jXO0mgGJq}7&e!P&stip`+URY)YBblFGFuk|nJQTF z+9O>t*Dxs#++m%&5{9lxnxZ+5Ja}elT-%Wz?6RF&RA~a;rv=DOtp;V_SRRLy{bhQR z@VQUQUH^wJ6V^u0zvCqT6l!8*p=0{bDJR2U6_Ec(TKdP7li}}-qkktD^B+6VVyTU_*2LDrZLIpgqF=8oOaFJqbqi%9|h~Tkq-7lPngq~oIEo29OU%d zo!5{;s8_~s6EXaQ9^0E;UhR8IRKKr;u^jQ@bF&ATWnEh~n0*o6AO+Y=?TCijZ?D;W zzd0vRsY6tkymcNg!0En#1u|VoY8y8m`-Zzk#M%PocHMH8>mSy9l2q`OJ_s&s?s|pR zqxt>iAmR7zw>2&v8rHyHB*>ydfb)c>eO_#lY*RimqnEi#`o~9N5Gg=CL*d<*I*{L{ z9qD*tVy|4;P@>dlBRC;eeT0|$5p7x%o_lD6;Xh{q$Vv=l2$M!pC6v=O z60+mclClyi77q{6QP2>*?8M1!$QYAVQz9V4-2vrR_)eZu6owYAk?tg z0vp60t2yeU$zlB@4?K|QK^c2G-(&=^tG)~XWu#Ty*XoVIT}8G?LLlZSoj>#vymU<@ zIdVOmjra5n-v^jdVhR{H^&233IN0DY9$kyh9=%#OoXU20n@mzhN z;A^nUls;w`^%#9WNXIKac?u^)eFmD#ybM}Geo-c~;&>|0;HF>W%%fF1b27bTehihP zs;&~*WmlwqRBcEksVL*{H!`%3xgwXv32NYI|q0}8>zTF zxh_P^oUZMd5P>)K?2xf!c)ba@CR|1ZBxWf_Wu0>hU}8%t>-p|BYfE>xyu}V~fxaIW zK?v%;DH&@Em0lHFz9o^zX&YUwLZqUKuk=QiA=%PVg}og$!STb}8iRm{ z#oO6i3{b6zm^y8jZd{s9F3kXI14kB`>`6nwA*}!TF#>-*@hTcCQY>83o8rs zbagAnHjew8_Yyy%!loPBzE1O&wusvD!Zmaf#Be4pYbq)$49xz>s3BXuU=c!KMk4X+ ziEu9`XE5|7MDv28e%0x0zYARz0Sa^ph%xh2&6_)gohva_$@GLcPuW#xq36cqr%la_ zv|4|hmM&6Mmia9q#x`OS8%s?7(Rs~C`^xRjrnlVfbMG3S*-)7W)xa&I_{}owVjS0) zX5JZ^*bA*&pOOLt`Ks8_W}eZA z5df^BK{xLd27-eU>>mMDq4fPC3S1%s>ndV$iSHFH#If<`+cg!aFX13hD&Y32ehe>< zq^fAm8zx*l2N(BNSBBN?D>-gjYK~?pgG~GB7XBNI^M(I8I-nkicCZvl%>t#d=OYI* zuMAnyC6|g~>|l+{0{O8QT^4E1(VR{|Y=ULsIA?P$U0d6ZiptZhH(!_b9_KPLxa=;t{#*caclgdNIAxj|BqeW3%-CORbRGX_Q&SR77wLSMP z50mrt?bl70@*LOqFiJ{4Qr{5Fo&Hl__c!M$yUH4G8>kv3jmc=+-WT?~CQscz=@sM_I~bUEf5<38cd7R8U0@AhGC=r| zh`6xyXXD6qV&CdcQ8}`i{bEi}E*5=wBXd>WUa;j%K5YnVmVPRLX(4;z3c2fX9-MGi z7N4F%);dD_PNKI<4acoYH|O@A?!4dq0u}Iva0c;b^B6d8zt9|7F1|RbGQH+{^f(jG zlqeyIh$iTD$9TBjq2NQC$sk}hf|Q9`L0M}#DWVd{4_xW?hAjnw99`k>4D=Kl!K4q> z?J0Qh+HX$8pE&{ZQtusdqN26;pJrt(MwpSO7U0{kq~F+bYT!7&^6h^`AgDQc#1!Rr z(SEyIGvM8s9*kGi@}75<&316>ynvLrZGQ@375R{5F-EYDL`hFC)ve9^uu>8+n-3&4 zGA`I%s)VT?CO3ZSV>o_rh~4!O8Yz>92<~N9R1N`ahg-z^3X-7|`mlEq^a_>$eo8Ot zq(m2CCxg$u$go>acN!AexM%L{{A z*st)v_UF0r*q~SpQ6p%$lY&`IGTWS4p<0q|^`CpHieeXx8r~YP+9|-s<1|0gu$sqm zD;5aYF4Mk+U67ghl2ecwQ5*IM&^CU$jn=5*AmJ6vfk-L^GVi5cZT`U}B_;KqT-$%A zOcFSXTIwDYNTuUt9u_MQikXTb?+xvNR3znc4J@cTe7|G0m-@yqQ`iM_9sYq*nG~ML z?_eCheaKqk-g+;LN^lH$`k7E6mqde*?|#cm0Rf3<(E8o$esmu0XIMN|KuF+k9a-Jaq9D)(Y2CG8kXhR83?_;p~X2 zkVu2>01X_yFN^!z^!%>9%IoS$OF%`8-%3-aybhQ#M!_Md3tK!x>QpruQ$?I)VcNUwv zLhcM%ga(a!^rlm>soog`(mn_LK~-MY0!gc~T3wrbItM+uOT~DlsAgwBz8oB>`f1J< z+xc%<_XVABmjQCggG*>UAG^OuUPO&(I{1BLojNkmiK zSvIGyFJw|$EjIS9CKetvm>tuptjzC%deWn5I zp^&1!)2?qCf@|j$_$(@!1WG3O2bf9U`eaR7!iSc?KF%I5?{-B&$>$q5rBM&P0 zNkLo=GU!X=Tql7*5Wph*s3w{Q@BV$@9op}ndN7qCcU&M4Idxp1z8Ub-)`m_#sV58X zbPK-_L8j$!xLBn9cZX_(AGro{QAN$QdFM3{uN9^Q*IiKYtq^vbOV=0^yA+3a9Ezlu zFbU-K^=@R^$&gN<8O~GttiC$9QLOYqrnVebU3zR}c*RqGQboZv2Pwp-W0hag(<0U2 z%o0xTGwR0))0Bl0lWX@q&m=wbR6C>1E_P8*5(F#~bUVQGQYlY#bb&m@Qb7&-zppt^V4;Px5wP7}$g?_&y0uCuuWjlom`=<4&Tend z2$P0NteUl}>5tFM;k17@S13zF^*Wx~iuW!YLIwO>rS$B3(B`ucv3{cQlgP63$%uss z7tLzMYk!`Y&D}SWh@jhh5S$eA?6BXyMLzbT>80J$r)TiyEVv)flD!2xP-cFN4Nt64 zI%|YdEk;#UM!ohKp!l<(oeZUAk#B>1LIxH724Q>Hry1fh4#xr|@B2+~%Lo%JW4W(! zqI%PKH{hZHahtfGW>Pxrzu_Ia`pgFUfJ{Bps1)A2ZHeer9#&3(JWp6mJt#ft4?Tuj zNsMuB3uYpW!mFmSzRk02O*?L{0UNvlBaNnOuv2v;XJ!IRklvwSKzIP>qpYp!FvINHIhnU8y{u&6j`UBL6@JO#=6JDocQ?%D)|?B&pFlgROo~;i5?4^hf3LI)mrsIl%9h zsPiU5mc~k}tD~;L?e*>wKHHX7OAo&1c}4~z2Oz8BSInkmLMXL+`Qz7DpW!Kcy5-eH z^(n68M-!bUk-WWG{$nGB{Z<+Po1`>(u?83TxfE%^|Twq*9Hc!O^ic!h29|=!eBFrpT z*XmZU?=9HK!AtaZf|e@ZV3!v=xb$K4nCjeQd|*2VzSvwA z!<0d;<>Hg`Ia1YD6WQ(E&jXh4N$@%{M?(p^cSfw)SbaYW*<`!}T<4COv8X)Qfx;bSM%cW0qK~tp4BP7A$6L{ zJA|B^uShEq1;jV)3@ekDyyLF$p(JX>#{JK4s&Vt?zZm^at}~l?m6qq#CB!VYEze&^ zLT#E(N4LAPlumA~Wx08s+ErYRCFCFZye@O)Jk+8E=+|6Sa59qyv;v7Erww)(;XhvF z<`M|#$X%!ce)COA-~DuD4xM*oolMJg(0__F#lO|%<4tid7D?}pCjBjKD6P6>j-&s@ zy~%7)rhUdEaqbi{xRLEDWLiG*otttB@sW`Mu*P-CMgdcnRd&+WT9zD(LGr>Gw#_7s z0JPZt&^jOZaWIS;$R$0pn`djXQzR+gx$y!Uq|=A`N2~e?CrQs`4X4(d2N_Q<_Cm8e zx{nry3D3M*W%=xJ21>IOJrxHn1be7qxyRG0B9F(5bd`_TnW{K!%Ey3}TjJSpBN~`b zRpgyKsH5_3|9#9z1@i&t@No2cgo582domKd6`~yuOMy0I^}R*%1552KUPl+Y8F)qn zdFqz|UtUlw?fk7yOyT0^g(=mLr0~;;m(?-qNdhjPb}4+hW4$~SNQ1X zZ9h7g8ust-3GT0l^dlorR)qwu1a?7PeY3b9Ghy5Us`h#f4d<*Q+X!&D0$Ka&ttoHd z>ww>0Bf+-2@RE2dPORpVPV{=^1`jRiG!EvF?sNs?h4@NuO%KIkK9GaI2DRIGyWg#s zYgl5bRv1aNr02*9Xr@M^BdgBelK!?_4Z#~@P%;b0`&J`(8v6#}Fyh8VOHVPW+eT=g zYAbYcG^~r77=Gor1T_v8aoL5?%AcDnjxL@5Gxdst)`(Jw5B>(yI14c+#xgG6lT#y7 zHD~DCg^7}=hQo_DCTB+C7a0J)X0H@QpXhcWN8CeqcdWW{6Ts)^tywl)#SZMqo9~ht zIUoJ{Kw0@_to8ZPUK6J4M)oU(vPo_UiJkyjn-X}X@iKdieZ91Kd11?CS?e7*JyLKV z7C53}M@Myd9j@{CnBQg0xU7O=v(pOL479$fzquKmf~ly8(dx3E!>TqfD}g*S1RPQdqpX)6i{~E5Vv1uf^!Z-sbw z3;o(On`jKPOHB{hEvD=(LC*Oy2Ww+}dt_b6><6{+s({OpwLEEfo;@kh?e$dt@W$oc zOn&`r-*$a6G8sNDRTB6XiO}(y{_KAKQfb-W(b|{?AoByioeqf&f5}Q-;3eDo02i48 zynv3b#- z{&7N}<^^!Gn46;S0$VxbDIdTC#&K`^2DicugSiLin(e=K1IZDs13uj%#thC&^b5Pw z&pQeccL;ue`KBfp`DzbnW4YCZSYy<8fYeRnE}2Q1gV0OsEu4vfGG%gEg)jt1 zbdmMpj7MrBYC%>HeW?2sR_@av#^qWC2>a|4i%L=SN#ZHfh1H^tP_hU!tKGHi!~+>; zY@I;^=E-dFq=nD)sN-8Q21PRWUM!-Nw;gCP3Lp-!Z`~RPqQ<%z)~1m5_`d#N`iZhExWRM4^d>N5CWtvy_0*B z04yB1V0Z?eDj8dndyg?T#&(9uDvjJnUMT`Fr`flQYeiDOCBYc84m#HGplpOcD7!Fh zs%o0kPv?iSqb^qava23e<)*V$ynlHny9jk`9m>%S(e@GT>8iy95z0en{eT7)RNP8> z^d$wtUVk~j;l&NFbOW-25w*9U!VY^AsxYgBD#sJIl|8m8Cnm zRafypPnI((qQQZzaz{6uZ8MIGXv56mg{rI!S~lE*n8pYQt0hI+7aVVg;ndseyO(Ja zo4%W!vA3K@zHPeI%yaUdMLr>zDXLJz;O$Rqi_L(5WwrvVI0ogN1+ zYa|<#1y3A`Wj%><1^PI66X@w4!F!^050Iz529!CGS~%9;LvLDf9)shhRyh7a&~p~% z_j96S8U7s6TW}$gTAXjd18ZJf0o6l*`t}Q7 z@B})D9}F)SsEp;)51@PJaJgXX@X7_V{&_mlX;PoXWu#`i(VH( zr4w5RFvf=(=LT3KmUs_xsEMrKFhKFMzqY%1+HHk>znF_NlW{*_e!j!dApH7})T6{C z^Rqt0qvU0DwQMx(l%H;*Vfmf*)IMz$NU7-{%I(9CY~&PdT-dLu2kH9W(zVw=LgyQd z?f@_b!sgQg1_c6CHC3-xQ3>lY!I%dlJnfaNHfg9I-bw6ccye-5TNMU^EZ@n@tW!Q0 z1J(omldspq{ZB3@%$Y7)CgmO|aBp(a%-&l%`o!ouU8Zcg#V zX50smo-Vo{aKdB;v-i$cIFUiAnc@#eiGxK5GXUzP&##8^HhP$zKOpLX5p5i~5$*%P zoY{eucAz8jU`6B{S7}vpgG{@cDDQE(b$=m$5Cd&kxdMHW6a&~UA~@K|i6xHJovyD9 zvv&{O&2~otm*g7L%P2mKns96OXis3HB()8bi;V%1V_N6eLx`Fo%l`#?Ex9;T; zfD9)K#Bw{I_Jv-p1mbUNGX*4obD5@LP-o00B_vQfEeY{6anAU2A5EGPr@uCvg?odG$X4 z9j%Bzk2fV=&eMj>qg(TL<85%{Ss5jwEDwo6#o8yfl=Q<^+(|}haYDED-R53_nn7T) ztG{su_(AKL>}MW3tARow;R9F&Ec6;pIDWN7-$WEx44~up1Mp{%ZyhzLsXZP%rN{5) zq8q#WDRy6?O3WfdT<6tQo5Gt#w%npy)|d4nOrwG2JI<; zvi8Fq>jYwmz>u96m&Rz}S9dvD;$!q61^Da2RU-xBJ@o*wcx?N#`cK82`4@?z3*z>& ziTaxUt)N5VNbQEIPp271$7w@LgJfZ0Jp7$2;|DikNR}H1j7^%vE7KWiRr3kTFg6PP zh%9(pMM~6X@%GqyobdUk{AM?+_X!!Dd-#&nb&GmXAv_|hlvAz)_YbX**8ua+K$^Oy z3{>@{jPw(r_ zP-jS03;*126MM%un{3C7p3^auuXEg)DtxZ0heANuS6%sGVj&*=eB7bA&o*Mdlg({& zwDkc>ddkBO*Gn_HacufH1kFVq;pU~?;1(|sn5Ai|^)8IpqHGGJDxj0w)f`8-`$hHc zv+uW1^K`dX<;)|-gGHTTz!MhQwyxLkVSx5rY_upRfridS+-3oP2Cw-YIb{ZN{V4iP z>$J0RJpO)u+WXboyxi|Bzd498^_lG+D+8KwrCgX%vHmp?FTt!18NZOA%w(1kOLiFk zE2K!DPXlSVSZ&mX+YF7-%-2})x|_^2@o)npVz*E@MP0P%X?n!wRn?@< z=qFsQhB#*{s>*cWy#+v#bD>A=P8oPrr_Vg*br-JA$sYU=rry>UsKp$TS%9Mw2MBAJ zyxBRA`GROS#D4loKfj4|P6M_z0kSQ88ZZuj-MzMf%6e{Gh(q3V)K$A*C1aKX<%=zj zZUTOAMT+vPlL>rGx$t7q?;e{Zb1CKLpbk&Oi7r~yu1#-1^;tk(+Py4jyANX~ELEAd zYQvA(_qKp9n>g2G52orYpdB6;J1rf?YXLh*UW}9ln4K0SfDJpV$!|bg)sCZoLxBGV zyMM7>R%TYl|02NuG_d=p?TCntwWE-+gQ2~dt)tB!1HeB`8GS2bT4DabZj>VS#>Ub% z);51EMJ)AA9q^g|Z2Z|pK)}XTlbV&C5uciYjvk+doeiIjj!v6a+)>}s%#h#O)Y2HA z4vLoF!SHXJAG$x=f1Tv7#-9#sx;(`b+3f0p?%(=+`WcKzRJwf4efbOs#?=nl8+?x7<+kS=y=T)tSE5ux!c9`ign z{!XD7EWQAm+xy$bev<+^BS-4__%NzW7xx96VE4iHQ5|>iVCH*XM!!|Zv=v>6?P)i>_-pMUE5*({r`P&vOp&NqTu@o0`tho3NN zVxG<3P^w$C{}Q74kWG~Va0>5(OU8ik9uD?+{`EKbn9Xg>Y1@^o*y1bjY& zGmqfAXRcXBMr+c%`F--XcYg$VmvEoDtT9gFdFScsB{TU*geV)Uv;2RnOC=#uVu4 zIX`e-!gr&fU0*?)Jly^{%Im|YUK7vc_Bnw>3Ft87@#i!>^QBkzKH?%CD0c>1(kH^# zm$lS>p!VA{_OK*G@V^@x|IM-SZ||MILi#@~td#7XjQ@UCK;Oam?^pikSTM79a1{Kd zZ~s@2OX>e(n}rqXKb)@|G+F5A@c%FFQ2!Tw|2psg@A_;1$H4xt3k^QQ-@c0o(}L{3+)5e#Dw_Cr6%zk&K>Cxe@<)A~?!R9E4mB)nv6r2?B1pH1 zvkM`R+5p7(koaRkAi@bj!?eT_$>IqILtxe7tq|Jr@d4E^)MAS@c`Y<4xUJLKni!)@ z87>R$I4&1zkx0_S8Qj?R(+Y26Tou~qnatvw=HEh!#ebTS!Y6P)Ux7lz*CMmd>yntb zOmjbPI847z?O#O`A4d?H3&apGr-zyvv|fyTPzao?L)Z*Ot5fxJgPiaZsl6nBwg(oT;C0MvIQiz1~d3}cEhG_i2IS`HS!C1E|MKpixYavaG z!}};bJI0Ab*W*f0-m9a32IY*jiMRXtq-TwmMSQOtL_ameu(vQZ)!m2jbp2rM-L!HxcTJYzqK?h7gMcxP`%^ef$WA+AoMYWD2mPdZAJ^dxna6Mm3sh=%ZUuEQ_TEWYOg^#f7K6Qp#}G^D%aShHeBO9F+qT3l*^ z<+4l2k%X+`7E&!9Tx4#jjcE&R=R~ujkra~vt%dp?in>3^SX0xx=!}2`YxXL zW2l;tepsE)i@0OyrL|`rBEUTKGrHx^dm*Dgw*i=23a_%9cjJe=@&eHaHTP6tkPQ}& zY=x{e9UDp`M}lF)ovfNzz<*gj?6H<{O6zdR%{^;}>3L2uqqYYVsi`R|A+(zOX0(iI zzG)}I?O^q$ z?j@wU`JF$qCM$#`I>>TzS0Fkesg-UJX8kkcFaL}kH|ry4E3)X5EQlJR5ij~Z4bXG# z4p`jG2=D_^G@@n%kzF>Qh<+R{2{s)LV&|mi->#B9pjtaivUBnQ6P$SRS@NIkXF}GF zFs}#<9D*C=2VpHtzD{~&IJf{ct&TAjwTGVO4a?fVd)@ZdtaW^E*4?FnM9a4UvF?`V7O>r$ zpw8q0D0Xp(-XZ@1s}hmU!30F{-Ol>{!OG0@{gg_+)op9kGXVbxm!`=nW%Ayi;}QBE z?{fEMNaRVp*tEmrF|a+qhkIqq!PzEls2;Y`$ygqK(E&n=G^c^XuY9m63+>ORxjOaZ zMjVI=904k}maL92Hd?9A7{fV1jXWAFk?Sr)J770~v|rUL8~O8dzkreS)k8A|hi4(^ z)ni?XTU3B6N=Nj1W@h5#8RV*EQLmv>-{RR}n0n}o6=d(5X`l}E2@c_#K#2zxHZjYh z*#WKZW@h6Dm?6F^QLlas{9Nj)$*#BYDT{Lh*!yPQNCJ)MHBnu=U{x?*&*nxmo9k2D zLgyX#d_hz);1z?j2D~~owMoFLCD$nwwIl0obnWuI*$oq`Ok_^x`Ne^m4m&^z=oPb( zs3?MYi82GyUSdPOxJbg7-V#4pMkYsn(#nDNQW&ESFE7zn3js~^IM;Tv`oyhlW{V?v zx9e$;^*H_WZC=SxY{upCQCmSn+d)!`3vNKQNTscn+48)=Xb!L3u;ohNl z>*wN-bHVD?z@GrwEz|H~bst%#ozTSPBZRs_%PjU$eS&cG&K_ zcl+w?wl(L-bWLN$Uc2aowYW!Se9o1ie!~xj9f2vH$s*UfY&iHJ>bJJuS-YqG=!d2? zn$~LlcsLy2Shg8bvDr|I;-#L*#>mPTC%3_z%pJ~6Cmq2fEgIX%4Gr8uBi@C8t6wWl zdr5X!VLY{EN^e(Limrm7*$UJCvL3f0YhRzny|SclSfSSbOom3b0cHiNM=a$rU@Qxv zTd@=sMP#V;RHeIDOSM-R2V2o()H=oO@H8QLoKUg1bD!AV`9=fU1T+4ej_`D~3(E<7 zyTbF}az4zPmF;B#a#_DyJxF8p+rcE6LLg7V-okb#5Q&|qDY~!I7ISB(GTGsz0^B18 z3C(z_f#CXPnQm_KzuZ{wgVVT$|5R4{Cg9(}1x?X8cGf?fB0t46qs=2+ zj-vvHeKkIud}$J}JY)Ct!MJ<#w61rX3f*=$N`7V04;Rr{V)Ma%);g}G?e(fpnCTpYDU4(BCS$xNjqvdtpLgt6i zCDSIG?!h1T&lb&iC8l6FnR4yT;x&wjNteT?VW38o%heZ9unPsW2_@|C5Fk3ooPv2W zU-oP-2g4#SS}!9L4H+pfaqzq>6AsNGZLosg`*{GC30_LvJMb;of_W^WJ-oTzR0WsM z32nZ(>yX?}Fq7Ox8E5{)F}pnqjhYqFlwx-6O9sa$afgb^aMUBC*K5*(PLq#LSg<8a zvRmW#aSywbx2=u+*4KDCS(;fX|xF^4eKtX0`Wu#lQPOmC2N;DB~%1mZ_9&o{=|Jeigp0V-?n!%a|=OH#bmg zWNYXNn(lULoc_YmI$Y+Zb_)@3G1H)Mo>I2_={-rW0a~GoQ3ZBU5V_>r2~VdH5qW|r za=!EaCX1;JPN`$yNObT@im_Sw`tuE~KvOQ}i9L3oX$OhHoOk*c>`k}08S{;1g9}C# zzuLQngoWRBZauw5K$~M{;u4^4Nrq7anA6FWyaxOS$EH{0Qm)Jn zXg)b3t*gJqHqAtubMJ$%<=XsSthok$-E5xZITH1bxkk_WCecfr{xrzA)fWli9x5T* z1o~jvC;jslW%el(tqa_{`hVq%)=di0F`Z()}U71J_cT`+ZuoC+#UKVwQid?e1XAXzwY77#3%fETrgWgQ_D z^vXD-&V`hH7>gjg#m&Jf03HovR`H8Y&?>Bn+xL8+iU`7`Cb>Q$curi?VcpPEwu(;S zRUFb4@W+sdnZd_3YZ)_*Xc9}{5q)W78#29E$lIQfI%`-nVqG+lv1I0cT+^rDd`HbV zG2vC>6adOvP!^F-Edh!k^9&Y>$90eW>g$#pF6M|1KJQ!-FcWK7~321_o94sK$aV`L^_1I44mTd*ral!a$1# zhB%c3czjd|aANcz{CDc`0-N4}fg7e|Soo}B&$B+4H0h{@RxHw;2o-Jgu5?-GejU|R zgg|+&E6HABq@lV#-`IX6aUoJhp!@_>ZBQl(y=4+P!lx166C0_?peI!ndU(aD63%q- zo!zpgJfME_I7r6d-jev#D|3-^?PllC*sETrzoYMvUhcr9pW(Aob4Dq@OKFx z4;K&ii7Q;pou0QBb4c0gx&sNW`u28i932dtt)b{DXy`{NQv!O)LbuKOX}NU(BOvn8 zMRTAxj8JiuzT#$pq=x5&LUc66OTD=mMncy2E|78>W7YD*eDQuFcf@}6@fU-jbQOU8 zc;aFs!%QuV80ZcXe2S1Ht#^bV5JDj2IuoG?$x%iW z*~Fpf&drbUf&B(1s24SKke-883}W68tKq;ee&{GQ_Mu$u2F&2&f7noyUc=%NjzQ7zDi>JKFbwm6RZVu@mlf z&p=^6+2c_Xd~l@&^dbNo#*lLoZ!uAi`6&IEvbZhh1A0}1o4Ce^4JHF;tNpYrpn4K;{6C6xa+yrBrXGzYN$Cf>vl% z3lzIBA^7{jshWRlfS4@38{Q3_=g-S(s_O`Sp{2230cS|$?ioFl3eW~98C;WGzkhS6 z5H5;1B9Q}Hrj#aOx+0x|oRCYRlm+eu!Qh5Jh!0_!#~Apy6<-XqV;Gky!PpM}0(__h zN&*_EOD~col+Ud&fJyi*|($z5SB88}ENwQde05)n8_ZDY_TX{X6 z)dL9sw02ZWbg?%HX*r9sr8TS_JYntw!e==}lJyL4?QkWg0M&0jQA>IJN%b)``#Wqhq z&z;mr_*k}phpAaT_dJb{7y2k!%!qQ{{kw`>`q7+*@&HPf2V>QSEYd?@g71D&GUYGk z8Fi^DRgS_rJl2zIJyogeI)6uD|6$@ydXdMY(rfagfKc4RyH;-0^!%j@XBvOiNT)K4 zK`kI_#C-hU=Bv5hgJm(oO)k_nok?(_Pg67H2<8&W^)+I$zDyOm!W^Q{nlAmNe0?lm1pG&e-v{W3gyDUM?)r`doz`8 zt~}76Lrwfg+-)e2V{sL-P2K!VgD6yRaQFAvhr4V zm18_ALEhA>?Jd}oZk;%vh;~@KPUf$!o=Yy}y=-B|qPC!s3Y=%sS=Ew}?+eOCVN;1C zVSL;O=M+aTRl?H3Aydh-E=Ceq9uGO$p~+4xrYhfnyf({Rdvx1jz{DH~=c zX14#rHUF$Z{NGrU=)bZi0UJxBf6JTxS{45bAK|kyGXKB8oBmpD{@3+q_Ww~X$;ia; zzwxH{aq9p&1mS1Tpp;WWpnECez{Gg`x)VX^cfbK$g+c==L9`DqZbmaoCqRy-YqeqA z5GN1I$=bjgYXws^AzqA2y=-e4zm^^+I={#u+y#5QS>32CSOHxRonz3Ur3XcIu-y#Z z4D{ykNq$*$SBO6Jl3v&&3KqP_)tG5ki*7xT`cSe`ioCq%cP^)=R&0X8a;u8ZyV^=k z-8*zwUFy)(p?KwmLj9&Jpyf155MvbOe9LqSfP(L;LG~0a_s3Osyd)kh;n%V|97f55 zz?P9X#XVw?snB{b_Tvx4)*Vm(j{R34H(4CTFTPBLqc}z;T?eahPowBq*-X-q;Cqj0 z27~Ij>n}`(ck~nTRo)`5_v4lrqi>99%2i%={Cb~o)3^)Wi`Z3OLDSJMHwko)ZvGAf zPhj3Es$S2a52*T*KhOxgyH=A z82h&zhk@n)6OQw57|GvqQ2)Y6{))(diBZP?S<=h+znApVv9d7z3nOv!@>E((`*ye* z?=UrCaGNHckPZ$^8HF~AMF>BLk0qcF>_AKf6lWNf8u=BE$$*TYBxWw}j6fg$D*|oi z$&{a(70BYE2I|tf!n(1o$=RYAbxlLE{r9Sqai&AU09c5>p1=MM|JBvA=$f^*3K)P55Q66OQ))x)`-4OwEs*WIJE7)#_tV^^ z4-kSoV1sGX)8)l$m+|K{pcK1n_ZId8cwhHx6*T_6U_)I4I%bV5Pc`UBpIhqsI$pbl zm_rlfFhg{w*RFMUVz)xyEixO;>GL+KI4VTU{<0kv?8|#V@6(1P+t7LoDN^(hTta;U z0BENp4+BP||}N0aUVs@c}cmL;iw>6AZ40cOf!$ zzr06+lo`++aBO>{%?1R%b0B?>MZn^USq0k{VG9J;t6CTGoxbsd4r*KEIzyj&0H1G* zlxPH7v?hi>6J&QI;;wxNp(_L)kk9#}@wWnlW~EY}>YNYsa?jWGC6NZQHhO z+qTuQC;#)-%$&E*sX8+sx~kXewfaNXud2JB>$&dxs`IBfGlY*#2uQd0EpyKfl6f;o zH~U}ox3q>`D+_Teec>_nKPI9YL1uX&&yxXF>UoknMAV_~cLS7*O}+w=#IxT$6RlA#awe zKwM$$G*%FOY&@@zS=54_mNvm}Tf%ZK|v=B}0{NDLNq|MTo_9nKB$cJ%W$ZBBWL_fnpEw(c*== z9np!@aW z8Lf9@{;N{$U;Hn%s#r8Tysk^UK`y*!4pR9}=4<>MtZPRpRkOd7#^K_Y2)5>c{II*S zo68pJY)J3$riK)n=dv;{&GV$h<0;)B>RX8}pTk}GB4)cZUV0C);12f_@JV zl;F<;Cg(i?Oxq{2VI=1@y`S!ZBjr{(E+-#PvZ|=*-|{n)rEXkx&5u>vtwbn`;F+wU zFFrj?n?YMHkhGBt<*DD+Gfu-cV@jfBF=J8~Dmnxtl!k=MHDW?f6{r=MT7SZ^zT{}s%~`37 zmChq6%BIWH&gW|_g{pLm^UEpAmVPGr18tN61x#W3uBA$JbPY4rylZ)NR&#NlaHF!0)y(}7Y4tu4TnXtCqva_@ zY14votL$DN+hIt-&5FmBC{w>620=VI zd3cJ{@DY<7*y{S=D6`-uc9|7fd1;5NhHh55C~lR2e{n060$S?2$<}6Gz&f*Xn_i^U zbQaCI2S)WW=uS6|mV+ATq1;+s)Tu1j;Su&$6qC#-_%V6oDU}A$1xI8?9g{j2Z_}To`v*@5ghzW8_t$ z$KB)peO^T7upyEhoYW0hia2bCu_#H%pt+ zqhZG+VNnWTAK~fQbuNgbV;34(+=fsLwjoU%YKQrm%Y^q~Rw_kKQ;pXYevE_Z3BPJu zE5^$ycw<=Y6e6qxXLNbAk`!ICUpicvXj0(S=NK(uZ`WAW*>+krvEkM`qLiYm!+{j*IhKP(>|1BO^k{%9)a>ymGq$>EIyLhw>QhWf^+F|_cyC$#(Ai13m}UtgpH2wcOB)&GhgVu|h} zNDme4Q{H8AkXk~G7YdqakgnC+Z0Ofl`48`r*~W_(GAE!9{sck(5KQB9qdW{J9iXFG zN_q|B$x&@{#6l0HVnuaT16S5}u&g!f6;=}jX$VFDCZ?;GWx|YYmfG%eTgKnqfypfv z7q??pu;$zjB`t7p!T|r=d;%h%BRS^cXn{%EUbP(3AgBP>Z7KiU!Xir#SEw3OrVooUCy8O4=$-E~{GQKc zfARY#cwC;v`>DOXr4%2b_V8@UouA>#jnR8g$O)_jj1n0NVug>g+82PM}d#RKq)$=gXn_2ut{H2_E z`BPR1U1H5fAuK;t{OSX#WVxaYeW4BQjm2!jBU@TYhx@@eUov0EtqwCEBB>ev?;Z4@ zqnyv!FGrx))!;KB&{cyd?Orpoi%AG4rTFbwi_xjMIBTQ&R0s>h1o|mUx%yRD&*fwj zlhd&LAJ4_AY?Uf?q7x-B!WH61Mnm&jZpnO$nYO>HX~Wg?*~8*J?K>&mkx6!%>Ca?y*_h z$CxWWO=5_h+zryd(v`Ip?xc=a93)4Z$OKrue%{5?yPcGmUatgHvEPPFay+T^k6^qE zC1smzH%bMx0c#PtzX@$B=R#y!k>%p~h^n8WRim>aE$xf~dY+9fQG~})SdxUmz)jcA z_hY{MR!vNo!yaKjuJ6UbWI_Mv^t&Dv!S|VV%X8lJ^7#98`z^d(?06l%FFQ);%JzNI zNp|{hYDO=PwSC1ULzr0Aoc^qNQ!|QIc`5UqYE+UHK%)vGLNx$)w>p!K-=RTHd2WaT zW@%mx$u0jDb2BDFN{IaUccur|gzG%zAJ`gtjmaFDx1$ENokO)@NZ+QGnatArCG#09L>IU{GAJsCn+4NGpZSKoc>HQ*a$*U` z#MbJ!ZIzR$9TBCC&&0J?!*k#@l{fGEqnB(38lC)^+?9r&?hJ0{O@jP&hv)OJl9f*O zgy9C8bE7+*JBGtu4JBi_6@OQ`>bd1TCv_>>%ppS*f{}FMBbFncOeY#~qkShwhFv)P z9Dv$%0+XglUmAakG>W{$B`L^=>qOhpe*0F!iLa+CVEoH%Y*zdRc_p9OX~*a9U%>pH z-rpMpKf3vkE#!j1!jCkbhp#oeMDcDWHjnGA72ogEfz<%_YiN_!=qU`FH0u_DbU0@x zM}ue=xRYJku~kb|MrGbh^MSH|Dl#{&+W8g9^~`csCgeC9gPeJlXLUIv-(SLeee!yq z&jB}o-+Gxdg57MoT0W&Wo%FIlK6hUEzveQGhZep+u+kcQ0R=TUTuS$+Az_~T(`3%j z6MbOR#$jJnl}C*~yA#G}(oCEZJXUE^BgJU1&r387aph>H(M03mLnCWdcCMwf(s=HsibnG6im z>dwK1nm&Kel|6t-X{v+JQU+Ac_oG^2{D(44Yb7y3%$wvU>2WVpa~|md8Pz=#E;{jn zwv;&BZw#ze$TRlIRCGs?f1UD4lh+w^u9b*pY9u?^a=Zj5o3uF>$|umyv#aF}J!b}J zull)UTmgJC%>>h0*FCbWJZIDvIb+WbN^a~EDKfkqZHlYmUGczp9@Vrec8pjw!HJWJ zG^6qLx3_Y2Yc!sRMdu4Z&$2u<#qgaXGqcv43!gG-no?h#j#yTteb;>ro}_)pj84nz zsL_UHMvF$i-$kLM85ksvPfZo@LHO>@js>qiTesCpd57m?#SKr+X(@c~d(Y7iTT3_t zf6KEc;yP?GxM;M(Uf7N}nZf0hNiSzCJ;!8j8b2-(Z)Fy*1c2Ak$Yyf0dt4BN(fXV{ z^lNk7S8HYqMQ?vpfwISRtXWJq`F{0RKEqx2Hl?c>O+B`L(-XW*_=^=VpGJzD-fBpY zT9CBvhGu4JNr#p$35CG#_=#ed?(P#_8=kazijXxN#LX-m8x-9)9`1O(;EbH$!|7+e zey0+DT%YH3`#eJKk0wCxsP9zP==wa0XW=HJs`bXbcs*QPh)v6VU&U8r@MLWdb_s>F zyp8ArU5$^qDDd8sPxr$53d)y!8zTUfk6Fc%i{yb>C>Gr>bp&%mziNY9BkBd%a8&;U zM2s+Yl3SYC^>WmfV~p8WO~KF|D!*=+4H6NhtNAi zKSbov@!}+#N0xh>3OBL1%DI^@wdeYu%y<$A3A2+};Kv?fAnZS|Z;iX5rJ7h!ceF{%{dDf4MJ};Y<0_i)rb;#Q5J*IPNo+!J*^CR{Rg`;t z3TGs(LF{FXnDwtJ7|@6h9G=-bmGfZ7P?|}{y+8f*EMH_4G^!TB;eokQ5wWz9<&6+% zzh=rB(Y{DJOXj&!>AUmhJx3A0b&6m&JJLy#*!r2az?0beuHJqp$@z0XC?>HDAbr-T zhBOBo=IQkPL%ALKP;%p_cVcL4Vkeur(IrTt)$EkeM254wf{|L<>yvPw#fJ%kl={8I z9-bsgYo5Q1PaS^_(Js9l-0lypQMSa7P!;j|F0@sVzt!g_V6Q-xY@6C+KtX!Q4hC6} zsG?fmYCOk^NTrw=n_7e(F6BeXb_x&n{u$sVwU>PX8V?bmB|N8YkC2uJTvk=sBw`5| z@dsrb<41^Ho}plSdI)R}h+M;%#7D0U`XkM`R&xSK6Vrz853qr_a*p0z}^7Dz$PU!ErI74FS==F?aZ+ zk2t*b6W#X|4ACs5_)C_B1LIs#R-#VVH_~()KN2>${nOi)Y@{eP>v)DfHRLMhZ%GMJmh{!#r2Y!5*+i(SnJ!|rrCG-4|KiTTA&E*6U##Wi zFlM*}-j5MJm`^&VpeN~WZ)H(nMq3$-U_Hbg{7ko54f0}T2=u}6M7#uR@KrEfT{^v%O0*W^0;p9AAqg)~+V{28UsY8l6wFlraq( zbDyzzbi6e`p1(2umyLlBSA0zyHM+o37kW~a(f5-hU;VOIqI5@B>>V>3xLCHsR$%F} zpD(d{Hm30WZK?hDE_q~q7)6;}T(}#&msr{k@B<;?PmY9QvRUYojVlV0@Z`*`{9-4* znUPF$#-jv&L~M1h&mj3;leCTz_iFf6))?;5JwU^x#vtxA8Lvs1bU2Ct*R)Q)Ky8vr z=m5UHl;X&YTL9hifssFq!&5{4MME^eGRTN#-69dhQkA72>HtL;_5z~)I)Ex3+o4C@ zOF06%(bSEkN(4WGoonI=D1xomeGk*P>`%@)Wt;>4M#^<&nH2m*J(wr3Y1K>3?#4iE zSefnsO+Jthl8c=49}Jfe^(CYVin)Qh9)anZTi6Ou#LT>Iq`HR&mAQ#GI*^Gw(gu`* zf*hn}?}inrmUbnn#Z>9Rh@fKW-A->^KDRwzqZzTk+5H&-&}?#( zD)r>GB{GdK&b23sM5DBaA1aRLRDKnMvoDx&e8g($$J~5(lWrdwBS)vHtIT~GWLV@} zWN>ns=?GCh@AlCLEj@P}G(Cg>JD#&>J-r8xX`R3C`#}+%f9JXuo#qnG+>;J_s|KS9 zO%L3XFKymeCX}NmHjA2LtiFq`VoYYALZnmH=@8A|y=fc0!IY!*wmkE$K=wBHT9fSF zN%@azJH=^kWV=RWyTqfv5NP@|o8$ak!$oSnO|JX~H>bn$L*Vj}M!YF@e>w9-ZZ zN=MR}N6trQVGEtz%3&0n1LKUMiZ6TUcs_pIPyPUqmJ?3L%R6~lCL+C?oU3zyMVs9ll15j%l!sx+d!7pKiND-=B`Jqd> zCuk#VoktP|apK`L;r0eNu)A5ZOdIu^X*QbdJH>)qX76MEKJy`oJq3$%Yl=sw0-sN^ zCea(b5x5zQJF+iXN~TuGbrM3DJ7~vYK{bciuM9mQ=vPq)rTI2S1rqqkJ*mmS>le1hO<_;d`E=nJji&qtHSdW^Xu_pMN^ul76-LSPzWId zN)~6emvvBR(q%<+NX!wYLTYQ3AmWVG_e*14b8GI@NCuSAV3t@@>{D zW4)yWslCUiGEk=Yz}MMqo}v)}v356ppbAM^HHl50Nz>YTT}T2##e)n;%bRu1r2tW* zxz*SRT@t~6)aQKeOZZe2yBuy=8H)fBS?|vk28XgsnJHqhH%gI}zf~b~2HLHzMrraO+EIJeO{~K!j zKRDHY3`I6}R<{2a?EQZ=6vh6F8p~K1nm8IbTiDqu8`wJ0{+EIHf8%2|j{g#m{>xWn zV`CwpWoBUhiL_Jy|nm0aNK_f zbpNZg$jr#`e}u@g|AELs&zb!b&WHxcAW?y_NJu1TpNban0+)nMe6I;tKr7T#>ZRY!Og1Qz}a@m_Q1?wn#5Vn^JPJ?v?h6GR->X zorJUrZ$iNxu{hw60^<`e7`R|xL+@{6x@wknnEv!I$ zz5{S+<$`Xo3t=)v{&$o1|5Nz;KaAG@o*Ms5h?<45v$>NNJ1YYL>(4d?rXL5Fm6h$k zKmX71&x4tnnShC%oq+v+y5>LUf6o2-zwcpV`>$*NbIiubK=8j3;-5G94-fvIEIQ`@ zjN5-7!2i{vWBxx|bnJ|r|1I=+%gf{MAJJv=td0ktg?AyYJi7%+AZ`*uSp|eKREgjI zNXY_Nza zz&pr5s8#KjjQH#^;;*o<_*h38Syh9APOJTvlh(311T19m5AXrVX{Cu*#IV;UFocGu z2D?|WG&Fb2wa*59UwUH%CK{-U5igvTnH zsl}=JA8c4sf?2DQtz+QK(JQ#fiJ{ht(k-_2DxUojs0Ylr`|dx9DNIdjjTbM=DR5|0zz`7=FzG&Jwo`aR;9p(lzxX z>?5xEniSh7=t{7jdTm{%IbQ3Vb}Pvk7T|$!!v9__c=`*VmH7(f0fuV=7HGd*5x)vQ zX4NzKi_d>$ROe8!uruOvrR5D8r=Jq!&=E!Fh6oC7X z;X-)LErI+jcB!mxo#{#Hm@@e69oF@lB`dM7SZzjNR#Ip-t;S?A5K=#hwN&l8 z!37o@8C%KRMlwRNK`}7?7#)AnHfcNZS=%8wBfFxR`1>&z>=imRB0p=>FmyF$^SnYBI)BvxAS9z|9+n zmMW%bOl6WbDP%m{$B)dN&QSxKR{d}Jma|b!7Qlv(snCiRB0@-jI*_xR7Ph{(&QwT=Y#E@_$ z=ep^XVUX!d7JA=2LUP%tItcls&WBShJ+zxL_MPO$+b`|{pR$j~%E4R}de`{;y&8gc ztYmW#0$QW^D+NNzI<|8pi)T!bpFiPWPCQv5Be76RC>oDOeMU7pf*6OzqJGa}IVs{p zkZuoF{99cOv7*q!mJ~N0C)r*`Ha30zuvEkcI*Piku;FJl)xV#bp#wkw{rKz}h1?|{ zILd*PU5wCCoS$-(=&ZWTO7fR=n=7&dqHg{|Uc#W!oAKkJ_Sw;9IM53bdpN`Z5H|cBn(~E%nU7#NJYbc9Rr@ZOz5!FfSHeFF%u%nY^2nH zmZz@c^|v;Ik|1f-C#o{k+ERc`EvFLg)zJ2-;KKtYP{t_~z)PJ)iVE!018{MRp79c) z&T`eGLCkxefDnEy6zL0C!ZT1!=F24tlgC2^V8{(gP+GznaG@>>8aynCq9jQ|=*~-e zjtxRZw!omw>IU*eHI?9vB9kHf#CsMTE8GH~p3Y0_Cgp=tlBo2%pqiCbtE;~ThV3%E zMI*~9>YmbNld-VbQj;2H?Nh9TWN=yaOVuKxr$hJ>0{e}b7xx;Yh!{y>fIKQA4fmpe z>sIzgsI-dtSIBR;*KBt5j>B+p~?f=@^Cyer@f z{Qhh9M*DY?)E2M9v9F(c4r!Z5g96%xGUH8_ks`vIcsiOM>ShMGM+VDH1E3i^vwMD} zkd{0O*M1=IIL`u0OXo00-vmCh93VZH!_a%1SUuZuo z>=DB3-(TIZ7dPF$yv*8Pf4)FI^i6&pA_K&`>~P-oeqda|xtq|p+NJX5Z%4ec1TqU& z;!I3kKZF|P0dS`v#LAHaYIrkun#w$$N)w%A?aHEEW4ec9Ave0kLm_cG6r-*L7!{R2 z%TKOKI7-H_d9`3cycI@NO~S|+am;{^y?^y+Q4?b!JjLdlFA90mb1XVT_V(~*Yb9S5 zCy(%-8(SG%wpuD}#*bZAbl&feld!xBk3aKsukW*q3uuxbt_ z2E-chWGZ#%(=fF>QXO1cOFMRv=40{`0#V!)A8cXwe*H=kH9o)!uk_2GeI+y-#jK?Y z=g;A!*M7ujucU2%T2d1p(pYfL{wXzH{`-EF-W@&>vG7NyX26pDpAQ3;ZQ>hm0rcI) zXTEF+`eWZcQ@!F0T1*m0mbS;L1!LN$=YAzjeS#H3lr2xz^x3z7e?Tt zpgLw?Gl8~5-7tZ6*eN@OC%kCQul{_3$zT5LP$dCB9kqA_t2kgT{sg4E{`OQ*_{Hi6 zs}=qX(fRiK&?S^`;@7#v2Z@i7iRNNleq5gcf9Oyzie$X}dz}~JGW-<^BuFsx2%ugC z2?S?ptEIZjE0o}3qFHXS1mFo~2st9ErNE^P3>@mtea!;I?gTOboXx#qa7bZbA)A#b+1`P=`(HoQNtkhwnC z=%3&TeWx=}H`|Y?NLxL1&!yDpKYircfku6UV|+6FwpA_&-0RU^Nrc%Dq3;3#-r|aICG+tdY$7cxkVIJZ2aB7z7wRU*;L%P zez%$|{%p|-v(h}HVIEXz)_&LtSX(bG;`H=BsDkn3XP;yQcWI!uTKs5L)&2K+$FxtT zQ7FZ=Bm7r2OHTfQb4_)6jHAnv=d^PYij)tTQy~*H!;v5zv>WGi#%~+vbXt;(fb^T< zLVnGcz=$lN*mrRBpH=UzoTUS+U$G!n!1xva)JdCn`d06qcAs~Jp7-ml3%g#^4?SPj zQe`53)Adz#>%lo@jfp%NW?Q`|KU4#Zix(J#?lyfuvzW%-4kI^5U!rx8Napu2zS@@rpd|4j2l< zlSAgYo*E?JnJWHSn#rdxoA&!1s^zLUI+mZ?a<$PCgO2ZYS)yyZf0p8~l|`{&Ots)> z!4@G36w(xRPXgb?gL9IXF>;S@eUcXtLIKZ*H_4NU6S&?tK!#^}H!C0Vpzs28*G8#d z`%iLj7>z=1t1X5fu+Zw)dau8^R_oa+M%ARnx^57dDx*n#A-iG@#T}U#5@fKu8w|UU zF;xwyUCTF{81Ph!L2vJ>9e~N`oZSm5MYrr!xDEoL*XCuJE`4N}jjzhFaSxZzx-tD2 zH?@HYQ$3MrpR?$@>zJBg``tlL#P11sIGfp{m(O6|YB#Nb_~ zkRAaqfYn&jNXVa98@tMatY2Zrz+>X+skxCD_w_*hT#;BOZAmua#3dA)Hb-idacR|t z>C&9VJN=dzZ;;o^>yn*FZOUD+e3bGwLk2%ZBOvUB1Lgt?{iQ%G&;|sBFEO8*6CsQY zV6=m*sOHmJC?zuD4b8^ye~wLxc(8!OH}&C~v_6ga`GF8azEUGozZ(MK~@!kaKU5sfQ~^pG=O*;1>MRDFKFf3iK<03g7$x?elLqxG|cS0 zQkA=i6=LzizjLT@S7zIfSHi~7Al#Hn?^!i6(1fMOI(sVA+H*bqqLRiVb^0u zuIrd|^6}-#t4@d4;qWFCfd3=z8MEm?P)A0(Af)cCUlS+O-v-x@YG(Q|^ji8FUhQwo z8$G>Jui*6uIH6TL1~%S8vG!Surgdk{3|DooH+ZWBE)C!a1!;LM=MrHeT)Fw@FXvO) z5a8Wt=g{JusD8gignC&DJY!+)>ouJ}I%rKLpuAuCVep%=?uTRdmQ<=XTP^Z1m*>=0 z&mS6Vv>O}4q}SISVf;ip?sKvbiEfAw3=OFi$4EKGq%2-$K4Qvyk>%GN^UVxY+3qVL){OwMecm%V}a9ikQt81M%n#_zy`tA zhsL-rcapWtE~@fM1%p;|ElK|1V4)yL1ws-SyR}91;TfLqB-NaKiM0|i0Nw{S2l*@( zr6-b^>Gl6obCzab=efpHw@q~d%bwd?&X*5hC9|uAID9)7|9W(q7KICj(MLjrw(67G zIH$+4JGgKgtHC>ahwKE0wRdXyi2JMb1u4_{Jb|5N^i7GsIaQO~GA{2Aaaq!_EJ!s| zzoVZmOO9v}FI&SN>nz;PK5$G#$HUx8ZNz+n)edG!Rv-w%SEiBJ^JCOU=riZ zp7<5PXqXlgW#&COM(df`_w82rk;lIX%uEayTSCFno-3F!-96ff6$=+3 zG=x#D)P&@k^0kaOwtv9xD55@TcMT#60As&4P^(YPewLta+h;g~DiRL6Dz!vA%jQ_>9qI9PdS-~7JK>RGGv zX1?$2)ak}u&qJ&Br#|X9tYSd32DAXM0o?BMQ@u@9@ziPW3_C?}i1DJ*kr2tu_JOQg zzjYU79PT*Hd^?7M!_?DeaYqd*E3*H(J~Mma@i)HD@(uf)9246;jJ5&L0V$5#8{)eZ zFR}c~XqOkK_yZCFe}uq7bPAT%!P&N~98yBOqgA4#q7UGv_iEKeWdAJWoy|R1y64xK zTbj3E18ee@uj}{v59|W6yu=+`94LCI+^;`H>SHlF{Yu46Yil~HOq*UQ&2EfxGC9`X zZ$mI47uGY_T^WeK3QEd`8m}xiu`cz&cZ{A3X*&U&OHXO$rIH^P>de-O>0sP%d;=Vu zf#l55h){*B4bOM?O!T;(Ro2qKqce(3=AOsWF=YzNImS4900jkKfruDgIsf z_BsBBmE%6jgwiznb44QqBFwvsnk|6AE4{@rYb{`aEfBvAw$NG}{W_l7;C5 z#xwosC#BwfU3khmpuM{%cpHnj!SVfGTA8H_ndMkn=xEp|f{HTO?6yXN2j>apQMboG zH0U4u(-m3d01JP_BZ1=Js6bf-LQDCAr>PBJjPCLBD8pA+&*!Y(mMM%Wi=oxmP&ec# z2DSNgM68mFGIDWmnezU`ERhFHRUB zUhcac+5Ca@yzi_;t7E=!7RfNlK{H@=VDaVxEyv6F4ZlDSouyT+j(cwNXFh#WLbLD) zMZ;i!rh(p8$;tf7#Lx_nKf^bTJ;|xkaVEsm;Xo39rf%T1k)=-kpNZN2bfF^g^UU?AAf?WEIn0XXDIGK+dT2z7#IPtrWwROA`~bkq&4bH zlm|_SMhH1GF^|l1hmuuZU^MDu*nO{#bg^Gm>Iq~msQl&@uh4|Nwcr39tLLi#_?_gE zft`4FF6m2NBjp`F}g*C+yqhu27f{Me551`U32M9Vg_R=3_ZL>(`;{y>ABA#}q< zuufWZz^(E-l801f&9``3hh$Wdn05;E^TD2RJ&9>7OT%>984L0>`8m_PHJcHQ^#aHk z0cII5zlVm1Le$BiW2re)PCKO%HHtWv`Q6Hm_3zK)uPfY!<*LvvS{(b2iy$}tjJnsU z)%Lx~JC%v-*88`Mm8lrFP5UO7lZqPLOsDV94zwL*shNgP?{U2h3SUK#jv^ld)v94* zIio92WGfR~t}0OG3OPuztOVJH1}^5*%F5-oO>kJc)DwRt^hh_sWEgF_U2Xeua9^js z2)fk^e@LC$A@G($iV{Vs5EvFZ^RI1i{El5*^v+BFpF|8z^g+rq{pfU*krIlIsYIOy znB$UF!oCtZ_ZYb_7Wj@`HuT00Aj{KCjX$L@a&u+6g*;rRJj~h(0&^5I7UAo8wIj-B zQs|3{*9Y3WE`5?*eb{uX;w5_0bwhOeG;{=Xo+MRYYg?zWQeB=DUzr_CgeLF1{ad*G zdcQM=RF)NzmX5P?!IC`_#46lfjj?JiX??qSjn%eWY3=aF=TC4h<90SX^fF97UtWbh zx(lF-?M_m&yunksKeM+|>LcEQd0Vn&OS(N$I{#rtc?e58l0c$KxT_9a_IZc21>f!j zg*{srofdP);akt@+Oc<+fX`o3dCil-h zQEwR;T*dsY$1Mp?U01f#7K)dPt-H5&!GVN@Rc^_bhwc=VsmwWBU#5R#J3{6BW^kNs z_iyVPa`j<=xF>}^u;7_zQ08W1rpD*FZKo$??iuo_BB|otXEnB+IPna3f~PEYKL^Op+&oE4=mx1$*fEJq7#m zq>lAo8h^v{ZAHsif)6=&i{h5rp3fubaS-Dn+?W|r8YXZ`@QDtlyZ`YcQu@E#`Q8r!;eV(g`uj4#uB!}nT${=~TlItsHWh+}j8 zbO@G8TzNm0b6&|%vH_~cW@{nVF3|E{UzhWd_Yv0T(nOna_pA918efEZNOzm?0LIse z?ru)Vp=@%vZq-d6y(!|D6;wau)C9*H{?w1TY-^L&R#$hN(uz{Y#LAT>v+!>z%fjW&O%rv`XXLaq;l*NB- z(yGZ~3|pIfm%hpra%&%siq_n;=Y;LFy`?GQqv0$N*u@o3>%```^VgrqxxUpLZ z+ip`k2EockQ7hODtAv=SP5Ar7BQ6>w>ixJu;ug z`O+==W?=NpcudaBq@ST(lIXk1&vd*^5);HOji!J9CF2CB~?HgWEv-sC9d z?q^ekXL`CP#A|%E*NC@0JHYJOss?}YSStlD!`kIWHyfY6f@zJ|2yyg53oC zPy4eKMR}^BBGlDPx8YlnR!m-M9y_IR?#mzMG{@OHBg~-(9&$7L7WUK2hJ#_W#pdtV+Ychl6qh;BAA{!M)Sa@q~%AD90|ZH%S< z4*AIXpnqk>5Vo<6L+>Rn%bYUjRIr{lnm$H+kKpW}2o&e)SpvMC*vEA0H<2~Jg*;l? z_$Q&hgI|A(rIKF}h~Xwjord!a)OV`d9fB`t0w=>T$flAnLemTjCIis{a{zav?wGqx^Le1p2u({}NH0D2t#U2$YLbI!Srd-vK$H?3&|v&;=c# z!X{_ebplNQ%@Doqy^u$2on4O$!Z6FK-Ff|0=y%5YD%T30om9fu zCX?O?g?G-q^qg#ntgG9bEtsKyNBFqmVp=hkyV78aed2I&Y1*+-&W_9zgm|Na*E$C3 z0Ke#=9}QXTuAd26&!c}2bl|6q_p%6T=dlY8e|g^z;jcI?$FT`gtCDXyOgGt6L!h?U zLTz)X7nG9v6EDo)?R4GyEH>0wj7jd1q^{b{&15KIq+>L@f84w0Dw;T;I6pSC^$3Z- z)XM2C4v)h($pcW_Dfs=hx&2Jp({9IfFSOIloI~-d81pX2+ymki6~jZxx_9S{akAGm z;7vDOHD_YV!5Olh8q8dWX zE@O#EAO@(R61!@n%DT=aISH}SKk2y@*L68}<(33rxPn9EdFJDzyrQC7)sWVhQ!})t zPqv+9V;8`DLX}SdV=26*Ots@J+3a!QD%|#-+zzWoi$viitiaX~Pb*lO` z2SPZ*6-yphWJ0=*;0c^;+vuQ$1&xpaTzGDlN*ImxWn2FDVlE9- za{0QzFO!+I2nO0}Shf5Q2ly1q?~93)Zq%Ig+oddcKIiub>?}M%8*~$xWONW#AcWC%H?=o%rP;4hU! ze0zL^G5yqsY=5~>IRe~q?kRAQiQV*l5qM!8h+vvb|>V>kFWu19-axOeztOe2g|Df5`;AXEXWdIcW5{Vs=uZVIe7V; z5r*_xNptqub~%Ox3uZ?MtiC;^U^BYgt&z|MBXc(aB=5k=bA3I z=jG>jO86}>3|hBu>-NYGP>0oI={Cd2q1~Ww3YQAEa-OgSSaKh5az)X@ixR``q@}7F?l@4RmNHbqSjJu zmFufSiC`R)%Es*6)N2q-nOD-ynDN6N2{y@*2V&$KIGEEaq#HbvC#qx{Sn+*ZS=Lr# zl-1O0%W;Yy!ehL}@=%W-MZ&?0FO=q;?Qtjo|#nw!Ko)`{j6YqXcQaVo5;RZJs}U?un^P@N7QCoYC2i0<6I zE2~_jNP?a6jL}=z>1`%ZYb-3QZIEih1_#^U)SZl|iBc*NW^jp#*!-=2Onj|qnv|83 z{7u-a$GYOn0tf4&VS*aD4k&MJO zV%iSCA4&yU*izX#4~xbw8%|U^nabv_?rzkRutm{imKK(gIC14dUZsd}p(xgs;Zi0A zNJEx+u3}>iFRezrAZ`1x$%a6R>9P18g&6`2t1h;qNE<;DgAx=q+%+{C=Pt}+i^ zR%(v#6zj^NXuN8fXu%=j$%o#+kebLvLb178D~(NBShAf;WpiY@s>a~v8S{{a;-Wm| zFLE+y$Du^|=qc=sOpv8joTw3i;S{Zh%~jNEgOpjkMPVAtKJv&3my-N>XsA5cJEHYc zJR1@zwFGyOrBJDa7@;$Z4_Z8kOgQ*zJ6PE|!jAue=w|qOET7a4aiSfz#gI{}#BY@6 zi;rJ*tmKs9n7BC+RcE7lXdaV;4$=tRsSOgxhBZlpr>$c3zmGx*hK8WZL?x&JN@AhL zzxjHm_g3oeDaqJ?=taVY_myuW4t%oAm<>hB4>*P>nCOZe^@3{IHfelh;fdy#lskqH zl;yo#5-a*UKq!xu${o3s<^(gRm!-;%Kl>va?1v$6^rUT4^DB!wQS}r!TMRlV%ciIm z7q0&Jb8*o%HDgSFIBk8`t4$Wo)0Q~titS19i#aIQ;!9UcFRcwZcNeWHJ3GKiVfd zmsnvC`4wkLBlK9a{+H75Ah8*3kM^a32-8AZk^j6>I=;>sHfpliuZg(J7p&=0L#K3m zKK4O2PN-njwxGVGkv&*bB(5i0s;H5OWjf_68q!Gl`}8VK(09vp(Z zL$Kfw+}+(RxVt+9cnvf2zmsI{H}}4^zO`P~I_cAO_TE*ec5Ug+uc`!tb1R^oGBo`$ zUz-`D9K3&RLf~DFsQ*6IP-GyZu*!QqB@a4(rD7c8Y6srLMohXIo)3}h4S3C}5H_)A; zCP3NaaNx#$Narg{!5nB9VZobEG-5Ixaow?I1b*c@o@vZF{?)M#x+65G64z1&vR2C| zNTT@&{SjIQ$((i=xh%|DJd$hvG9T85nHWi3F$WkT$c#)!+@4dfsM~+orF6`M_JPfS zprGW*`Xqqg27|Sdh0H@wMiT3H2!DuvBmhxx%!766!eKr_x4G}hL4OS%w;`<_qu9q?Iyc& z%r5`J{=i@D{Wf9{{T}VvWy4j?!UfZh`sKV5dg=x$GK`e_+XIYyd7;;LLf0;C)tmP1 z79Xka98pz{8mC-Su0AZ(BDh4TUD%6-)Iag9p}b0_A3Wl^nWa*{=cVF7;Tp2|Bu}?> zjXVV>=5iBoG5CF|w|)Qwo*E1H&QzhyFx+Fj^Y4%SUqI*hLeCET!_K-r1f0SWB*thG9YbVr3Y=ch`; z;%uDG_Dm{ED<41YALrk}Sc!=*~1GTh?fpVT&>tunQvbl0})nq$iRvY)Qz%e(j_uG~{a zHgS3k7oDyvH0SfkmZK5$OmX2?Rd7id?->*n;rcNUzTCoII-tpArJ`o4jb z;@A<0OssbnWl+u7X<-O38k7xzZ63v}ZJO9L7G9kn0To)YhA4!s=p<+)BH{N>C=)*d zfGw3(?1D5tE1F%D(TM}4u|Lc<2TNA|33vNfTrdCw3+sOc1_S+11B3DZ6U6&3Y&RHz z{g9pG4`??NBRCb!AJA?v@Rp5*?Pp~7{}9&=uJ_--x_^re2KqfV7$eia#s-U!2>H%{ z;&X6?!EK71*R~S7D@uh1AJCRc^m>K}1&#k@6dBx7Q)AJB!`%Drr>CsRI6PWGCwm%p z*w^Iu%prRKn)O-aIegO<7gJ9*Z!sS2_u`Y4V#-@?P9|SzLaP5tMKierPDOKJ+WUh+ zD#4V~TT1E9X1`??8SCU!%PDYY#UQUHQ{xDF-%=5hta#u922OA99HU0P95*U zOn5u&#N6CNGTr>+nXgt<#qQDWn6DZ=x9EFL?Kay-7Md4E)KBwQLpX}N)psb}3Z(AQ zxRkF0fVb4ARNX{eaJO9hsw20J?px}H2)9XK@~r33f)VF!eaBGaSn1}$63ywwrr4%> zntE!ZZzGx8S!!eFoak{U7wfgcW^d+|yJvdiB8{*c3{1=q7WPnsV}^Kp4mY9svI~dc zl|p3k4Se)@l?)O{?(0tFmlHKWhAoy&Zz$pRn7$b303}gwD<#hoD~cJ ze?*J_fgFECi9evu|3aQWpv?cQ^N1>c`2H8V{I9ki+WD)E$L9~o^*?D3pMTc*1C#}1lt#LIXlX3J(qNLk5iOzaPJ;@AeET`_xoZs zLLdp(ar~U%B;9c3-I@Al0>dR-d#CC#v-KCZ^9>j0-?C!XG4#EGekkJWYs;8?PdXQ# zh4yztC7lPExx84~dD6m@8DMp#aDUt#^2FHe8Dj%i3`Z0+w=&4MpUP5n%XOUu-i^%D zYe^*w7xf-@8Cm!E8ua9bz|RfFLReCwCz|`()2Hr-_)$!SFI3w-l!bEWNe}(~;>@eI zVZDI&@0hrt^d_EEfNshZWj|{b4Y*<*fu1ZoWS|m%<%Ps!y}Ngw8f<=#U2)-#{@J-+ z%g?N(-W>Jg-ZP&$HM5|C+wFTGobT1;%S-k;#EnJYLlw-I2&i2xoU#)kX3fZHp}u;6XdYjpaJ5UqiqCLFXfQvJoUsaA z{mRnqK;*_P?Yq~f?AswrOiMKQsIH&81Upb6Jm4Fi#dOK(D|Nag=N()wW(77t4lc1n z;V8a|MRryeyWm)nGL}6ZI5VCf8Ijb z7}BNi5h+XD{Gf&H=ul64(~Yx@sw_;h_*wjZ2gbR_$wZOYoDq>fG9O4_(sV-KDBXw$ zYfS}e26pE5oO@=$B{{^WXzDl{(TJz^Y_}ltHuiQ$v4G`?2Y;vDa`EQr*s6f`kE!9} z;g>I4`Im+k-`p=PrgmNCTX1p-6Z)s|1WMFE^ z$UJ3|_U20fE*UDY4e2{!bY>_SvYp-V(0SiSJB7D!lnAfoo)J|fjT|yiij?r8k4^a_ zI}osGK*|8?d4c+(&^zI6g2*kr>wIY)JP4xJaed-#DBLVm?K#CgOX>I(d#i8pNY&^K z53?1^7`45yfKk#AT4dwV>rV~d80_zR7+S%5&_2hh&Crb6bqq9rGljt}jnOpRF}p0u zZ`tD~-p1!?TEsFB&Vg!W$ zi|I5*>nd(&Xxx@hycAk$%(EJOHkoMA`c>iCUA;zYO8zw&qUGh~vIrVABxwBTy+#Y1 zJIpQW!FYuNKKOnrD#%j7xEJF1Avk=(vAWtBi}xhmwS#*mMZE&t0f@1f;z+tvJ30oe zwfJ620)fW@eQmG$Y8E^`Hes=NL9XPFv8tp8$`x8{Z^ObGaj;|LVOIo)MZRbWOo<9Q zrm>wAd5_(#V|?vBLoLr-IeP2P8k31V{`R#P;*F#@nqh%LQ-j893@l;h+ZN~in&IYw z^M30_FWuOr&=;bL_waFh`kyq@d|h;3igyMs5<~D#=PG7@M}LFJ<_^`1bgYT{G)Lb! z?0RQhN5i{B4p69>gQEhC@Rp30_9e>l#%G_`{=6VaU9ot-`e#lPg$ft47bApI@JGB9 zbtX@0jrFq8#4-`3T9R6tNO9P$b?dl$9~;A2-EK zjS|t^H_gt6*Y_4sX#kBkWNyu>FK!-S@xq$K-3K-F8hP1}(U#S`hG8kO2eTWb~2hI}Wz3(~6pvwH<8N*J!OV^KThQlZuz|d_K=^ ztUks@DAHAye$4`ige}YZ;5NSv+zZO|)teTD9;}wn+v50U)7`@8jy28XMLzocW_BmS z2*IK*7|HBo$k%J;&9LPZDwkH!U%`SVUcXS_Nlw0iRZI;5;VQOR%p{F$l z@IMM=guN4t|4gtta>?y5;1YlJ9U)!Yx~8!fUftwY-aj!e8@f9#CvJ*^t_~|XF1Vf? zMT2;lsEFIIZi?fD8zZay$9F>7&vl-cGWz>@_SP=ILPWAncEI9G(V;Ig3I_=hSJT8Y z#%$?R+P#2$%Elalo-Z;Jy3SyrBe{|eN2AlM>okOF3|9eX;!!J#y+xbhjBZva0U50< zf@h&1V-y|{BU1hSg>?d&PUQ1=)MWp8TY@Bi+VbV1(uAVT>=Z=fm^~aC=DU0^{m)E2 zW}ToYfkXo4DWA+{H%*i+08PXxHuT7lA)}aR<1pzN=$b@`M|%VhyB<{~()eYzTE}-` zgckNi`@R6-@c9cayY}*)*BG5KqU~p;rmpB%`}LPuZgm&4up7-G&06!Y_qK;O_x3R< zt}vz@_}P=2^%x$M+jT*9j;R_vW9>U=m9P+1ho+}w5DS?1xQ$lWb}+BT_BholMWl}{ z;4#0T9#~M&c;J}@P#E*afiR7Ind?U5&7aK|-1m9lEiukp`Ikq~wwX0ceDgn5F$cX0 z^@FNxx^1aFqsMAIdnMBIVmhh%pM~Z%-18!=VBW{R$>M zOsGJ5&9+s8=iyK+O-M5-ismn!wQCKVk*0LiOBN}r7Vfm*O)=*y@3X*db9eHnm{W63 zk7+iWn3v?N8=>!-TsMETrOVTkqJ9-0ZWBi_zMLa#OP47WCJ!R1y{*4Xp6LrJQ9?9xcZ6*;KC#>~$o+Z{=ouYZo&S}ZoJ<#8kUqyv!1jb%~QnmTz=;ay2DihSEz4spFM5 zW0+38!zA?Jb0-9`K-1bjbUnBduas>IKlM$^ok|jXR>irHrOJlax8&VwsTw60?A3<1 zdZ(pQQI?Y&W>c|MmljhZ!WP5fP2P(oV}xl`7CR2wWG&J^jV*agIbNrF>9c?UWfjqp zIAYwOD2rtuIHz2aK?P+^k+nK)OqZ8l9;g}~5epDkVa_=U{79-VLuc)~E@mjx{5Ay} zx0crLY5erq*eryj?g*5i2p|Oe+#{Ty9PB_oo;B?{VhH<8Vk>UnCM;{*BRKbUl-#RX zR$VcPfeoc3hJHs{RyHNsFonKucI9rPyTC#$WJrjBfB@I&*Ai&dE5g0*gDYEEpFhio zpp}LX25DTu=UboDH5jAIz0|U<^Sgr8G9N?ROe-JQJ-bk*V+fi~E4(T<9w>@lhgewx zV7E!FvL&()jDC3oME$t;Iy)289Av@(gCh5HIL%7*4qVYWQ-Rg#PO3RZL z@-fZ4zP()OCp@kmh|a0ybI&D|We}|P=)8oadHgXuq|Gp^clKm|EXr54GfpOYIt20y z^=J%9i}A+2i1{)U1pYI(GwQtsl7Y>DX<#0f-rxtO#x7G#TA5^nBrC-X2g2LYw{kKE zC&#=pl1t;Gq3Px{?FGOYnaJ2>S=7RAV5*fF6djWbR>;aGAm~~|tvg;jd`6c*L-{@G zfFc{0{}tWQb6M4*3Km2zw;Bk$gP>Q0a}XB}k)t?;7HT}p@#XX&y-`j$j2s%Q0BqQk zgr#qNCvnT)^E=_9*J*4CZdG~4MxqL8UQ~rZQHy>Z)mh`{R|vYH{es-H0?KpFGV3`Z zJIPOo8hEL&Q6iGDad;4E({mh)F*=!V6O4J4)2GNWU{(7fc*!cv&bw#l{h_P>*BJSP5vGzV|=Yp`* zt5QKv!Qv9X>ohRv9Zm#pbJ5mxO>>yX_^`?8}kaqc1991`N(KE$k?3GxXYxpxc zmd5=b6R!c@2VVep4OGgv=oLU=7R8TQx+@%|F^!;uS?}o}r8XPFkPfS@4X0TCJ61<* zEjxYg3ALusePFIcSZk@kQ$CE z@>U3mE6P^La_y++4oTt)t9pk)tAXh07l9`!PvH-Q>fo8?TcZxUvwcH*l7&%-IyOSj z5MFY54TgG$5C==W4Hc^MB3j;&*Mz4F+erz%;3hdmKmJi9vO1aZW9bUn`@U*=tJ?Ww zTSDVUhd*A7nI9L6-wCuuQ?3zHNxUL$p{q%G>h#r918+CVy?a}ilnqLk$+Y|L7p^Y~ zmj(*YR0CA0)j@^RMW}%5hN}J*h`GPo(qM-qFGgo$OW$Z!b1fG2*M4FL5${7 z#)i`$;j#9ftP6*ABm!z;brgw>0*Nu4F%U6r3iQ`pSiZLQ4c9&V$vZ`}&Vq_+DoT!- z1zgVFff0bsR(wJgv`NkwWHWMM3l&LKF4kwPoff%ZjrcEcQtdb65NkENEc^42(PGVF zIb$W)o1c5sUus^ppU{6^!y0HLR#mI2A=VF*3{gIGfa7mms!OL@rVE*v7b$_#hOC^1;4aa8BLI}#kd;Zz*C#(KftVxuKgB*gC_zh zVBwJ$aunKh`rMEw^Qj8l`uJ_Mp zcaNt9Kc64`9+CkF1pHe_hEQIqHewX-gCC#S_6eDXk`pL&`zg3Kar1aSwUmWVxr=Y5 z-p{{!FTwfd*#5wiWz*4fb73-|? zV%P7VRN^v4;_BZ`5g~guE<|X4&nanm;-X|?^H071zl@mh-*)#uo@)IqRKmjn5jb4J z?*~DTmiAL<1IFJ%8!-MJ+JN!jLmMyxK|h@Y%{hWY8?03y-X!WwfQ*1~;?S`moRT;l zKFlyC-%u~r%h6?t~$-z>GCzL-MKCt-0UPQC7v`4o^v0R)(_&K;VEK~qT!Hn%CKL1 zgb5$a-of*A8F_uG*K!eebvF3|*Gc(&$#m;;{?^UaUEUxHsWp1qT;*3gt;za?T_QA6 z2@*1~Z6qiTsUNzc0Fvq}s+AE_NQ7(_1B?0KldUH)JIL_0{E+x|(E zaJ_a17xlyIuF)SboC7KrbQdr;e6>D4Jgrw}Uf4IJOZvMXK3VYf+=(8Axl4NmaooW?~B@pyJiv^oJVp_9&dUS*UXFyyb+No#EfN(lN;_m*=ugJeB z46mKGjhDIP28u|7S%ebrInI6Lz`(+IUdR2;eI_QmEezIb=wLTx1PXmF@I;a#=Y)5W8}6CL_K9`8Y7|ng@C3rHuv)8J<{F6yov7aOVOrnd&K$5c zcDnY=k(fPVb|I)+2i{EUqBQVMa0WJ_XV1;8LRRA$cEJLAYEg~|FM!n{3oQf`zS0}i zRWqo}lJn$6ysyT>siuVqwIR}~=Jz7VHLLDDX8BFIkuiuAQZOn5qtIq9u~rpUm2}n;kTUrC68{WMXu*dp9}zM!Jom|a+* zt8oUAX?RyxZKXR{W4xXj1rog_Zx_6BsAd~=nO2k1t?4%eBm_d&OXi_MB|noT>rAa@ z9@yxrsQFSfft62B7j6WqHToQ2=mK7(*r+;>NLH`6cW}C}0(DMQ{^j8d|sI`a(>Eohl8g+(7{KS5Np1CMhY|f*^qj& zV`st+dAuS$m_d_~=9Crz=W{@pU$YSuM??N zp`}%$3IUjnOxQ%KLBr+I>>YYR3617XF8sC$oV`RJ%YcUERi(ng0oD3stMPrGEclgt z@{Kj`NRKZ2g5X!b?;g_-eNMGBbSdBKcH5~1$$Sd*DWn&N8+92+pU*TLKW8ilMw6*5s+qxO%OfNC`t27VG(DO&s@?(u!@b?lPT%K}V(mUy=FQ+KKx{63Dq?!^MZ*{K zHS3aYGwR3Uk=dGz?n>?H&lA1jpUF_(C5Fm5g<-4bagabiV<_w$?k-47KqgMbcxp+2 z9F-s;GWY`9NJ!y&X-~oD+*CXc2iYeh=Q{^H{zs>I< z+M}m{W8)87T57Bg$)rzYNh^LuNo~DURUCXX79Gd9x%OVY3+vFP*CMkxYCkC6SBcF4 zLH8u1D4sr60*D#!Z;zT3FRj(F12-2Dgs0xNjy`%FFccuC@9=g*WrR_{`z9O$*06z9 zQvW%1%=BzN=8)I9#$HrG>@hN0T-C!a&pz}@MmL48 zZ!eS5n2f*VuF{$IgQ#RI(073gQN4;@pQy(-ho8xN_Hq<;@;Xx7ujLEAw=8i|mo*dL4=Dsaf!Is@jSy>BS&P6x7g z->TN~`Zd>-vJL?zNUS9UI&&c<;#1y*vOT=vyN(=#@VZ=pxKoIOzR+EKJu`T7d{jXd zl~0A5A*>oP=%Pu)92(=f@{?h?CAoYFNqs%1r7*7-wc)-MxnPdB!DoN&SD|1XVN&LL z_u`2p9Y(Z<*2x#gNDh0XU1Y@8pzWk>wliV#(XYG%Z0E&QYn!RH3;l!vCuaDvGcRG2 z5;}P>cu?Ft4$$K=)Lw~>#t)$s(s-8BenjyKtoRgKK@AUGN*oTgr}Rae*8nqd0D=uZ zsy^7|tvpG0#%n2TxDmEyO7w&%dmRXjMEV4)jG5lv-DQ}mtNN^cxyt6Vp-&4z+bRe3 zmpx7gMD}YR_Q!d$orFK)G4i7u%fgI@MKtBD5Z%e&CZ6TIS^rXqJ9dUvqIx8zy&buW zP#_cm@P01nEz5jh*=^#UBliKqOdnpcyrkk?Z;T-ZZPxg)K}POu<2HM)zC%ks*7vAw zaI(NH2e#2Ks)hMf?saIXC7Lw~n2DOOm)gSDjOLOnf$z?NQ|Hgl`trM8`kq^Vfa7tvoodFq4I3)8D$4HaewA#eW8Ce| zUz^ga;&8?f+R(1HvhdM@OY$vI8G_7H#m}P36X>q=;t`|O8@5|!G z8?{~UCb~fivxh4)X>wH@YO|M1L6ZxW+Kkv`4W~0N2W@x1S8xNrExtSlzEL;!D!;_k z`9z_USdoQW-~46CKHa(!(i1H|8k($7$-@x#HCb0w6P-J8GY%|wpU0>y-Wuyf8ik(vS@SKQi_nYA(!o}wX%~L*z|B!w)`lG+w^w0*W#LOU zMY0!ORe)9|hUzc2EJlv>N|Mrl8EuGZKiXEzgS6Q?hi};r5>)NSNY$R{IbtAU9P#Px zdy%lqmuS1GvLrOT>5#5DqRY%;ts_sjA{puYpO?HPggjQ}(i?s=>n`(79%(XP)cdZf z=?z{|LHs8M#pMXTe8U8<`T<=j)&PgRCrt^_ONqz^YWuOc3T9p&8#FOE>y2>LExW~T z)HgQGX4SUQSwSl)aP2J^ZIY^F`83wJ`4tH1VH+mU^GQ(}s)SvK$|h&g2n+!@I0ES+ zDuhFd{tikXOEL0KiMpq@e7@m*pB-9+Zp4c-fUSOa27YhPVz4Xs#?cL^97l1xxE}V6is4#CgZ30c{N0wqg)EvtCP>_*4X} zN!;R~msgO8yuAF?s#xS$jq4TtMe2wzXr^%YWn^hVXF{&xo4yRWkgNvYrg^(SC#^VQU<^d)T;ew+&LyHE|lf+h=<%4UBwHQpX%jh|5)FUengn=FcjnTrU*qgL2qN(5Hdi2-@-6z$`)gef=v-ONIiU4D+DL7RU zDe~75pLz;qjVk&TkbG*Ej>;aYur#dqs(M!_?KJHiwFc@XSVK}Dk;I*<-x8j2`;N=w zY;PK6r&j4QW4jMCn74hR)o7xq>B^Zf{;hv=C9l{=CltEfY{Jlnj6Wu9OMvn?=&6&> z>JVCQq4-o7Y!-+l8P?x|3|&%VOk%e%KlgR!m_RvHLV4Zpi0G_o&1izk7RWQi5cUFp zw+2;}{Vb47?R?5dSQrp@FMs=S(6(@Wc2oKySa2bv?hLomcU$Phk)lJLBIo_rEDi)m z_`w|l=V&o4zENxj-_e;-HuRKaNeb~cI3Az(%Tx*Pe#rU9HxJ|+f1MQ{?z8A^lm>^* z>3yc&y{(>cK9PRFavo3ITe8+~-PO!5=F#HW$jD-o&GX)uFX`*yibQv$l<4QQHW;lh?i5cNR}|wFBA1w*^o*zX}^fH(rJv7$rs(k7qVH# zXN%at>iElHf>`DTYMZAJhcXCRQ@L~J!GSn`kgLAWOg?EaKPggo+b{|BD5{53`3RNo zHI+wCqwKU2lXYjsDgW|-PX!k+A{s@LAViA0+&83x3*kteGLNzd`ZjH7*TQm6lPz>E z&>OxJVc^x>@?o3kosBrxQ{dqjn0yVMD%Zr>DnfhH`0Isj4r#A*^i*lC!n+mk&T)q|(jJ_@_g<@+(*u36x^J6RO z?-5PBa9$F7hvd2i|A8f!h@5&MUl_1o#T;9scQaM$d@H|{R(8KO{lej#37q)v`#qd5BRcOnbQy@cl$cY$+>ZxCp!~gnUfL)ukv#e*{-*K7TBl9jdpeAww(9BxP^; zmmbbInmNw)x6R|_y)rHL(n^h78l%YLgFc`45GJ@^ywr2JUsD8CHC*KGH}cqz=V;M8 z1J4>WjcV9A=+&#Q)(`_e?%zty-mN6|MpcWpbNaeXM;q{8lk%hOY(?4%eB##dYO zFV~?@X!PN9i?>TOeHq&58|!ca>>!!nto@l!9_Ojk{ORtWl@2>ybF<( z?|jU%6B3!V3l91Uy@&fc**j_?4M{kGp`A~&l_LpXYL)C;x zeuKhu-KYN6X|tOLZ|F_)j*hYS5&n`SfAsZIY}EVoHl<>M6^qI$Z@;#zsc-jajLM6! z3z)>`LbM!00JLxj^!}*HVuIr;&~kJs0p;NR`A0z

UsrBPh%uA!-c~n~Z=<3i{7e zT1wv`*wwU%tlzXf-9w_X`PBKeo8?lZkyYH(Y@|3<+5`?l@m$P;uTds3 zWZce6u0tJR+CE|=&VNm&bF@7Py9jP9|F+z|5aNPS4{PdWv}ov;Ye&~uaX(9$HCDgKzkc1=q(^dp@tt>Et?1-hEnKakmadTd_{j~h3OPX3v6E45f94r>Wiw~f6Cw4zb^ouGCl{jDO;SP+)6uB(oj|)hu^3nkfXm)y!nfS z)z`JmBX<6;BnwSDhz3aTW)rA41$0#tP-3bKolvG&i8jrzuQGo};^LfN!D8w%|>L=M3Bogb%J>v=KTvgF*sqRRTMr z5ccid7d_%q7-qS#W_zOrlefh2kL!|@&8X2Q*C8Y=crjEC%itxyjoVg#1j%76mB1$x z7mBP!MfX%36sBX1&BG7n@QKxaJyt43(MT9$E)LFF9*IkgzQA!$3Y}*}?t?6nu=aG7 zskr{?SdifgRhisNRnX$&%Hg&)Hti;TZ?#w5cw#$sI7z-ih*_Ci2ZG@&2*g7)a4hXNWzmX>S}Mb#G0^2g#e3Fvi%$8Y3s#2GnzdNRhOZpGCy`+e z7P}Ld^2aDFWukc-?ER9Li{#WEq;^#|Ea05IBym`Qm1Gjumh!ozqE!d1n{7*iq`g1j zM+>YShejUy1V?HwY`qd4jN27k1(sU67c))OS5@5yNK zmjvud1_1PO*0za|;%XqwQ%am&97@g>Yra1G3YkFbea0ExWtXQ5Ww>#y)U6W7ZZkGm z><-~}GjyD8^ueA1S{kx#WfDKo_UP%0UZ=br7pTzPkS|$~)-6ey$tCWG*L`BAZ=f?g z>yyh_jsr(Hkg}ot4c>BmojOIf2zaX;19l9%lrKJzGD&H=oSq5U6&**jAmY&L#i)$l zfO@uh+NM9|N&J=~jPhI-z3Z#M(dBnD?uZ+qRQYkvLc(^dA2&nn=frl@m3zKgxm7yG z@Rbg`!&YYG+jZzhlNzd0@Jv1G?&u6cKy#Y`H)9tz7P&4`O-zD={F&~mHC;dLXGtX=Wd&GfibV${tctsi({<&3KBc5<}S zK3BbQpD0i-dh#UM@zlDC`$5 zI7@SC9JzUDxSADia+w(LO=DkU8I7Ab;T66^yv3~FB(v8Im0H4=f=(k=ST~%M85z0= zDGfi+N~vd4S$p!jeVepO6Kdl669eP_j^VlG=XckqBm*n%kEQE?>^`bN&um`tupkMhp3(94OQjX6wIEdoD51d|7bsrgg;bh@*cC4zSR zlWu@7_X$}iXAa77xR(1WJavRtm^Lh!0d6?;Yzn?nH_lS~s-p05`)zk|vN&o8WheB$ z#E~($@t7?n%1*RuWqOKEijwz4;=%nHd~Vra^(3V7H+-bCW^n?c)`HEoSKbW9@5d%C z$PmXw4Nyg4dAAxl@*lSxz9Hj82|V+<5kr8dr=X6AN%y<{nEzI7c0s13OwWdjmzo9Y zCVhNqegJAXSlUfb{|#L^9?kH zgZ6@b=VG{!ouWWlxMbH(3-qt zNS!q@OXWMzj%i6;1lHyH-EG_yI2o@nm5IXXkvlX~^{X1$vD?d~OQD`|xh#=7@Y_iV zSJshWM{)2ECo|ppsn*LhgbE88qg;$E5UGWbPWpb9J}7jTF00LQ3&PLT_4k2xtO+Mw zRkULevYW^HjCg7d>Ey(cwsTF|RfFqNo=Q0(p@4kOtr|WA(Ldr%5#@AE`fTY;ynb1~ z^9|Kl$POfjXP%RzlO|O<2P?|%8?DrKNA|QnZ2f0!XuI@1Oee)Ws@CFbLnAv1`q>N1X1#U37*r(ZFzUSPbfC zn|Ic5Zx4*=iDx^Bn%QRWjbNM7Ta2a!|O~7aS+jVttXatV%M@ zZygKLm62l~d6hjC5Q^}=N!y^|6cG=2vThS=ppoEpKS|#f$`(IHRsJ*xloi!oB##h$ ziy`Dpf81;T)+>y=WuKRVMf_R>7QoPFCq~^me4K%K7FhdSI3TR?899w$!?<4(jRJ)P z>kg%iY|x3?s_Lf|q>SxOKxUK(Ky9J_W=_$8?>s7yf}K~!N4dW#?-Y+b<1&Eqve6-l zumZZ=E>I(T-sKXXc#@PxyryCFHE_f`7CGGywpfWWLR6=cSP(tkP&;vq4p5=YlWutD zgMnw>PPcmS@lIUEmSLKa4lTTubzf51o>r-|4p&gLC+54#3x&HHo(49)_*mPW>G&8B zYmzWdA@0WTY6BXxO4GznD^7Vk^q5!SOn(Qj?x19c#LEJn?rK~usk;H2RWdV%tr*uI zK4HwMROi-k zbyH`D>~vEXfT~T1NX~_?_5Ex9SlleeI716@dq%Y2v;H&bk#Gf#AjRt-H1vsh{4kkl z>Ycc`f!=++@3ufi`<9Z(T$XBC&CdFK%nuw(QFNF(usb$DgZ2f@1v=dXMayx9(QNca zSRP8!*MW6Av6E;@k$y{4lQdY`=ZKV7q<*PJsG%vWxA+NSD6AUXNGB&GdcH?e#(z7e(UB(&4@{}aSX&Js z8&a*V(j54RankaEEUg%31MpQUa=OH8PXloo&)hrH|1Qp@L;jfV%4g66Gn@2D>09_d zDegSpX6~71@P}K?8~CzFrKqkr+^ABvE4K)0_>o}_Q|gsNgm^zLY1-LbFnOQi2oFGb zse7C_du=R%=-CF_9gw)K!mWt3LD##hbtp%dWITqYRnY?jX zccB74g{-}}5L?)ohiPbUYYJGh;q!pgVy*kI+pLR#bYkkSj$|$+w<4yK z5yHu<(j}U%Bl?AID8Os_1+mFsN~HkZIe za;*olA1kgFH+Np5ha|ZMI;}2=4z>nWCucPwDmiV)29>x%MXP(Pk=SDDQ>t*Pu;RH+Z67`7uR8r&lTdY5b|a=*Gg6P;vcXSLF_ zc`KU_$$D#_cetpxb?rxbc4R!O%J!z6yhxV&aarN0P)$b~pdH5Ij7gFm>;9+TBDu%qOaa94s9^h9oZjuuBJ z7!yxXIbSCl6Ny2yyGrkcvD2KRvtpE{vm=dsG;^q=aL`kNVbM=(F;b$bv-7-(z@2E` zl_Mv)hD>c@4A(3LR}t-?&7BX%ll7Y%Bi`TXUf$;t*|`!AH&|I|TK;5U@lLk=E3Gh^+4M0zo@0>ECH zEX?2pJP%oT7(k%^4(jzU>32Y&{|4*zo1Z4*?|zyf7RGL{o0lXR zUXcERZtuQ7L*dML+fK}nIopto^Ii1Ar{vCsrczE* z)ujnMDIdOzy^?~>Ii&v8tHMgP4Je-;YprO9yM&nptlH0WG9e1~hd;3Ln$OoMY>5Q- zCeB>q8$LcM^=s<2N=>y|a4`J3q5CAI<~+)F=C)-2L$CUxOVS&!vgrJgVBGZ_gJ$M+ zXf;g8>aHH(MboC6Q`mB}(WEQ$gkWX>G%x;oFu$fesjzBDD29bVTr4X>DEcxZucUtj z_Ep$UB#-$AQldEcRdl@&~?rHy=2-FE<;DfLY7!$q96^?Sd%tCi~2bt%)`q9ID{`xjjN=L(zX zC2skf%N2Mn)NMF7IEcgZ1vXl|o^MY+MGWCxCXE{;ehWx8qn53i`v>C$(IZmw9$F@aKoozrSsqkO#lCBjsFNl1A(20|CY4p!Hx03W%0p{@h=y}|F}l7 zvi*nl;1cb>vKOZy-zGDP~?*6!AA_o0= z2hK_K_!JK8CHWWJlj$!IC>ZYfpkm@j9$-|SqLikgj+Ql;EMfw7|9osq#DtajQA}_H z^wO4QT5>wpD)iC{T9~v#^2aSQx;r<}8d1045eDVn#-0Iz}dD zW;P%j00d%X{y;BmX=Y;%mRb&MKH#a!{GoL%2YP-vFs{_rKubqfn2%nB*cxnF+}!m1 z;5P;{w<3OYF8!NY{)Qj@e@xQHzWke{fM4;t|9>cz*VNR^+DZiu@G#fPK3MPnotgoU z0MEZm|2KT+|6_^F@i6!A2pDHfdK>nk1{YA+FIpd+{n@^ zDk+0M|Fn2wh7W&@oJYB2=_SEK5IkQ!6#cu<4}||4=D@$0|M71B!d%)?$M!EZ{2|EW zI{kM+{?^m~D=mQj66Dd+|5XitNb<*w_lG3E_2UEK|3)Q@|B>YJ<^HWh{*dHXJv`2H ze{%ni#aqGA|2px({nyL~7WZ-Ld+7Uzu_tDrZKXo|XcWYcy#wBDflJ5|10I<@6#3IW z|EB-Pq5F5fk7N2jYpu9}sS!P&nWeUl<>O2XHZNK5^smCgz)S~XVkHL8vut3TJ$T-w zV+I4anI68fe4ytA+w|jG@M=9IXavu?^0Fci7nQ!XwYe2LJ-wcRwZ4reotBviy``BI zt+}y=si}df9=)lCt%2TSR!1Pcft8hwjukzC2?%@z7Wr>dgE{}FuO9jEYglU-o9TgV zUk|)S{IO`j(My@@nEoNbA7h31v2RRmjE#vO|6>5FM9Rk6*ueB*VR=;6Ph9@Ch2*!? z(XckN{BvwFK8#5L@QMQ-k1XIp`7kzFz~hzW!79Nkptg;c&VS1OHHN|E{wm7|9^cH& ze_ju5J&g6g#y6M_6ajb#&pt`wa#-uw;M!BW7k|0YRX+S9HvN+ZzJD-;Uud9*2n;{b zSODOHztF&vf|F|gT#f+<7VlRYGZ6d(;J0$W%K~I$0e8hud>NR557XsOGys4Z0A8Pe zrZF--_yPPxV*vnIew7&j-W>cYGk^ui3O1`>>VlYmtIGoV?cIP3Y^=ZW1p*&-SUD?+h4oii7#SHJVhH^t z9}^>ZkM}zb^sB8gF##SD&;DGOiTO9XVq#|a?LC;-*qDEn1x)*`&zTtjtiS4jnSl|! zBmJ2#SbY!PNk7TL%*OniuE4b4bj8fZ%J%Dfuz-J2|N6e*Df;*E3!Z^~s|#TI%??-q zOuzAE0S}{J-;D*p`tZZxr}qHITlu{%knv&P^mDm~Y3#pf)|MIu#yXY{yCFFPM;-7K zMlWk-2Hv`W`}-l&sfekr8S%qJ^Y}%8ScRCAA0)^C;$`Ipft|QOtPHGtd`yf2%tC^E ze9Zjd$&`T`?%#_%tcn6=TKxJtT1HkjCdBltf`: Name corresponding to the `nav2_costmap_2d::StaticLayer` plugin. This name gets defined in `plugin_names`, default value is `static_layer` +* ``: Name corresponding to the `nav2_costmap_2d::StaticLayer` plugin. This name gets defined in `plugins`, default value is `static_layer` | Parameter | Default | Description | | ----------| --------| ------------| @@ -84,7 +84,7 @@ When `plugins` parameter is not overridden, the following default plugins are lo ## inflation_layer plugin -* ``: Name corresponding to the `nav2_costmap_2d::InflationLayer` plugin. This name gets defined in `plugin_names`, default value is `inflation_layer` +* ``: Name corresponding to the `nav2_costmap_2d::InflationLayer` plugin. This name gets defined in `plugins`, default value is `inflation_layer` | Parameter | Default | Description | | ----------| --------| ------------| @@ -96,7 +96,7 @@ When `plugins` parameter is not overridden, the following default plugins are lo ## obstacle_layer plugin -* ``: Name corresponding to the `nav2_costmap_2d::ObstacleLayer` plugin. This name gets defined in `plugin_names`, default value is `obstacle_layer` +* ``: Name corresponding to the `nav2_costmap_2d::ObstacleLayer` plugin. This name gets defined in `plugins`, default value is `obstacle_layer` * ``: Name of a source provided in ```.observation_sources` | Parameter | Default | Description | @@ -121,7 +121,7 @@ When `plugins` parameter is not overridden, the following default plugins are lo ## range_sensor_layer plugin -* ``: Name corresponding to the `nav2_costmap_2d::RangeSensorLayer` plugin. This name gets defined in `plugin_names`. +* ``: Name corresponding to the `nav2_costmap_2d::RangeSensorLayer` plugin. This name gets defined in `plugins`. | Parameter | Default | Description | | ----------| --------| ------------| @@ -137,10 +137,10 @@ When `plugins` parameter is not overridden, the following default plugins are lo ## voxel_layer plugin -* ``: Name corresponding to the `nav2_costmap_2d::VoxelLayer` plugin. This name gets defined in `plugin_names` +* ``: Name corresponding to the `nav2_costmap_2d::VoxelLayer` plugin. This name gets defined in `plugins` * ``: Name of a source provided in ``.observation_sources` -*Note*: These parameters will only get declared if a `` name such as `voxel_layer` is appended to `plugin_names` parameter and `"nav2_costmap_2d::VoxelLayer"` is appended to `plugin_types` parameter. +*Note*: These parameters will only get declared if a `` name such as `voxel_layer` is appended to `plugins` parameter and `"nav2_costmap_2d::VoxelLayer"` is appended to its `plugin` name parameter. | Parameter | Default | Description | | ----------| --------| ------------| @@ -168,6 +168,15 @@ When `plugins` parameter is not overridden, the following default plugins are lo | ``.obstacle_range | 2.5 | Maximum range to mark obstacles in costmap | | ``.raytrace_range | 3.0 | Maximum range to raytrace clear obstacles from costmap | +## keepout filter + +* ``: Name corresponding to the `nav2_costmap_2d::KeepoutFilter` plugin. This name gets defined in `plugins`. + +| Parameter | Default | Description | +| ----------| --------| ------------| +| ``.enabled | true | Whether it is enabled | +| ``.filter_info_topic | N/A | Name of the CostmapFilterInfo topic having filter-related information | + # controller_server | Parameter | Default | Description | @@ -419,16 +428,18 @@ When `controller_plugins`\`progress_checker_plugin`\`goal_checker_plugin` parame | ----------| --------| ------------| | node_names | N/A | Ordered list of node names to bringup through lifecycle transition | | autostart | false | Whether to transition nodes to active state on startup | +| bond_timeout_ms | 4000 | Timeout for bond to fail if no heartbeat can be found, in milliseconds. If set to 0, it will be disabled. Must be larger than 300ms for stable bringup. | # map_server -## map_server +## map_saver | Parameter | Default | Description | | ----------| --------| ------------| | save_map_timeout | 2000 | Timeout to attempt to save map with (ms) | | free_thresh_default | 0.25 | Free space maximum threshold for occupancy grid | | occupied_thresh_default | 0.65 | Occupied space minimum threshhold for occupancy grid | +| map_subscribe_transient_local | true | Use transient local QoS profile for incoming map subscription | ## map_server @@ -479,16 +490,6 @@ When `planner_plugins` parameter is not overridden, the following default plugin | ----------| --------| ------------| | stop_on_failure | true | Whether to fail action task if a single waypoint fails. If false, will continue to next waypoint. | | loop_rate | 20 | Rate to check for results from current navigation task | -| waypoint_task_executor_plugin | `WaitAtWaypoint` | Name of plugin to be loaded for executing waypoint tasks.| - -## waypoint_task_executor plugin - -* ``: Name corresponding to the `nav2_waypoint_follower::WaitAtWaypoint` plugin. - -| Parameter | Default | Description | -| ----------| --------| ------------| -| ``.enabled | true | Whether it is enabled | -| ``.waypoint_pause_duration | 0 | Amount of time in milliseconds, for robot to sleep/wait after each waypoint is reached. If zero, robot will directly continue to next waypoint. | # recoveries @@ -639,8 +640,7 @@ When `recovery_plugins` parameter is not overridden, the following default plugi | Input Port | Default | Description | | ---------- | ------- | ----------- | -| position | N/A | Position | -| orientation | N/A | Orientation | +| goal | N/A | Goal | | server_name | N/A | Action server name | | server_timeout | 10 | Action server timeout (ms) | @@ -677,6 +677,14 @@ When `recovery_plugins` parameter is not overridden, the following default plugi ## Conditions +### BT Node DistanceTraveled + +| Input Port | Default | Description | +| ---------- | ------- | ----------- | +| distance | 1.0 | Distance in meters after which the node should return success | +| global_frame | "map" | Reference frame | +| robot_base_frame | "base_link" | Robot base frame | + ### BT Node GoalReached | Input Port | Default | Description | @@ -685,7 +693,21 @@ When `recovery_plugins` parameter is not overridden, the following default plugi | global_frame | "map" | Reference frame | | robot_base_frame | "base_link" | Robot base frame | -### BT Node TransformAvailable (condition) +### BT Node IsBatteryLow + +| Input Port | Default | Description | +| ---------- | ------- | ----------- | +| min_battery | N/A | Minimum battery percentage/voltage | +| battery_topic | "/battery_status" | Battery topic | +| is_voltage | false | If true voltage will be used to check for low battery | + +### BT Node TimeExpired + +| Input Port | Default | Description | +| ---------- | ------- | ----------- | +| seconds | 1.0 | Number of seconds after which node returns success | + +### BT Node TransformAvailable | Input Port | Default | Description | | ---------- | ------- | ----------- | diff --git a/doc/process/PreReleaseChecklist.md b/doc/process/PreReleaseChecklist.md index 4affa47d36f..01b5c14c62b 100644 --- a/doc/process/PreReleaseChecklist.md +++ b/doc/process/PreReleaseChecklist.md @@ -24,14 +24,14 @@ There is a docker file to do that, so run sudo docker build -t nav2:full_ros_build --build-arg ROS2_BRANCH=dashing --build-arg http_proxy=http://myproxy.example.com:80 --build-arg https_proxy=http://myproxy.example.com:80 -f Dockerfile.full_ros_build ./ ``` -ROS2_BRANCH should be the release you are targeting or just `master` if you want -to compare against ROS2 master. +ROS2_BRANCH should be the release you are targeting or just `main` if you want +to compare against ROS2 main. ### Ensure all dependencies are released. We want to ensure the correct version of all our dependencies have been released to the branch we are targeting. To do that, we skip the -`ros2_dependencies.repos` install step and rely solely on rosdep to install +`underlay.repos` install step and rely solely on rosdep to install everything. There is a dockerfile to do that as well, so run @@ -41,7 +41,7 @@ sudo docker build -t nav2:rosdep_only_build --build-arg ROS2_BRANCH=dashing --bu ``` As before, ROS2_BRANCH is the branch you are targeting. In this case, there is -no master option. We can only run this dockerfile against a set of released +no main option. We can only run this dockerfile against a set of released packages. ### Ensure the test suite passes diff --git a/nav2_amcl/README.md b/nav2_amcl/README.md index cf398fba824..29e60326964 100644 --- a/nav2_amcl/README.md +++ b/nav2_amcl/README.md @@ -9,7 +9,7 @@ AutoLocalization - is implemented by utilizing AMCL's `global_localization` serv On startup of the navigation stack, the initial robot pose needs to be sent to AMCL otherwise AMCL initializes its filter state to default values with a particle cloud centered around (0,0,0). If the initial pose of the robot is not known and the robot is outside of default particle cloud, particles may not converge when robot moves. -With the AutoLocalization branch of BT, first the `global_localization` service of AMCL is called to randomly disperse all particles through the free space in the map. Once particles are dispersed, the robot rotates, backs up, and, if necessary, rotates again to localize itself. The full implementation is described in the AutoLocalization section of [BT Navigator](https://github.com/ros-planning/navigation2/blob/master/nav2_bt_navigator/README.md) +With the AutoLocalization branch of BT, first the `global_localization` service of AMCL is called to randomly disperse all particles through the free space in the map. Once particles are dispersed, the robot rotates, backs up, and, if necessary, rotates again to localize itself. The full implementation is described in the AutoLocalization section of [BT Navigator](https://github.com/ros-planning/navigation2/blob/main/nav2_bt_navigator/README.md) **Warning**: AutoLocalization actuates robot; currently, obstacle avoidance has not been integrated into this feature. The user is advised to not use this feature on a physical robot for safety reasons. As of now, this feature should only be used in simulations. diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 9ea4a9b80c7..0671c79d90a 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -225,7 +225,6 @@ AmclNode::AmclNode() AmclNode::~AmclNode() { - RCLCPP_INFO(get_logger(), "Destroying"); } nav2_util::CallbackReturn @@ -304,6 +303,9 @@ AmclNode::on_activate(const rclcpp_lifecycle::State & /*state*/) handleInitialPose(last_published_pose_); } + // create bond connection + createBond(); + return nav2_util::CallbackReturn::SUCCESS; } @@ -319,6 +321,9 @@ AmclNode::on_deactivate(const rclcpp_lifecycle::State & /*state*/) particlecloud_pub_->on_deactivate(); particle_cloud_pub_->on_deactivate(); + // destroy bond connection + destroyBond(); + return nav2_util::CallbackReturn::SUCCESS; } diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index b062c777973..c92b06228fb 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -6,9 +6,9 @@ find_package(nav2_common REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(rclcpp_lifecycle REQUIRED) -find_package(std_msgs REQUIRED) find_package(builtin_interfaces REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) find_package(nav2_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(behaviortree_cpp_v3 REQUIRED) @@ -31,6 +31,7 @@ set(dependencies rclcpp_action rclcpp_lifecycle geometry_msgs + sensor_msgs nav2_msgs nav_msgs behaviortree_cpp_v3 @@ -89,6 +90,9 @@ list(APPEND plugin_libs nav2_distance_traveled_condition_bt_node) add_library(nav2_initial_pose_received_condition_bt_node SHARED plugins/condition/initial_pose_received_condition.cpp) list(APPEND plugin_libs nav2_initial_pose_received_condition_bt_node) +add_library(nav2_is_battery_low_condition_bt_node SHARED plugins/condition/is_battery_low_condition.cpp) +list(APPEND plugin_libs nav2_is_battery_low_condition_bt_node) + add_library(nav2_reinitialize_global_localization_service_bt_node SHARED plugins/action/reinitialize_global_localization_service.cpp) list(APPEND plugin_libs nav2_reinitialize_global_localization_service_bt_node) diff --git a/nav2_behavior_tree/README.md b/nav2_behavior_tree/README.md index 2917bed7639..2a9cfac472b 100644 --- a/nav2_behavior_tree/README.md +++ b/nav2_behavior_tree/README.md @@ -72,6 +72,7 @@ The nav2_behavior_tree package provides several navigation-specific nodes that a | IsStuck | Condition | Determines if the robot is not progressing towards the goal. If the robot is stuck and not progressing, the condition returns SUCCESS, otherwise it returns FAILURE. | | TransformAvailable | Condition | Checks if a TF transform is available. Returns failure if it cannot be found. Once found, it will always return success. Useful for initial condition checks. | | GoalUpdated | Condition | Checks if the global navigation goal has changed in the blackboard. Returns failure if the goal is the same, if it changes, it returns success. | +| IsBatteryLow | Condition | Checks if battery is low by subscribing to a sensor_msgs/BatteryState topic and checking if battery voltage/percentage is below a specified minimum value. | | NavigateToPose | Action | Invokes the NavigateToPose ROS2 action server, which is implemented by the bt_navigator module. | | RateController | Decorator | A node that throttles the tick rate for its child. The tick rate can be supplied to the node as a parameter. The node returns RUNNING when it is not ticking its child. Currently, in the navigation stack, the `RateController` is used to adjust the rate at which the `ComputePathToPose` and `GoalReached` nodes are ticked. | | DistanceController | Decorator | A node that controls the tick rate for its child based on the distance traveled. The distance to be traveled before replanning can be supplied to the node as a parameter. The node returns RUNNING when it is not ticking its child. Currently, in the navigation stack, the `DistanceController` is used to adjust the rate at which the `ComputePathToPose` and `GoalReached` nodes are ticked. | 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 8a8aa9a7462..eed3626601e 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 @@ -38,6 +38,11 @@ class BtActionNode : public BT::ActionNodeBase { node_ = config().blackboard->get("node"); + // Get the required items from the blackboard + server_timeout_ = + config().blackboard->get("server_timeout"); + getInput("server_timeout", server_timeout_); + // Initialize the input and output messages goal_ = typename ActionT::Goal(); result_ = typename rclcpp_action::ClientGoalHandle::WrappedResult(); @@ -179,7 +184,7 @@ class BtActionNode : public BT::ActionNodeBase { if (should_cancel_goal()) { auto future_cancel = action_client_->async_cancel_goal(goal_handle_); - if (rclcpp::spin_until_future_complete(node_, future_cancel) != + if (rclcpp::spin_until_future_complete(node_, future_cancel, server_timeout_) != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR( @@ -225,8 +230,8 @@ class BtActionNode : public BT::ActionNodeBase auto future_goal_handle = action_client_->async_send_goal(goal_, send_goal_options); - if (rclcpp::spin_until_future_complete(node_, future_goal_handle) != - rclcpp::executor::FutureReturnCode::SUCCESS) + if (rclcpp::spin_until_future_complete(node_, future_goal_handle, server_timeout_) != + rclcpp::FutureReturnCode::SUCCESS) { throw std::runtime_error("send_goal failed"); } @@ -257,6 +262,10 @@ class BtActionNode : public BT::ActionNodeBase // The node that will be used for any ROS operations rclcpp::Node::SharedPtr node_; + + // The timeout value while waiting for response from a server when a + // new action goal is sent or canceled + std::chrono::milliseconds server_timeout_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_conversions.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_conversions.hpp index 58fc4026711..f16e61ec99e 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_conversions.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_conversions.hpp @@ -15,9 +15,13 @@ #ifndef NAV2_BEHAVIOR_TREE__BT_CONVERSIONS_HPP_ #define NAV2_BEHAVIOR_TREE__BT_CONVERSIONS_HPP_ +#include + +#include "rclcpp/time.hpp" #include "behaviortree_cpp_v3/behavior_tree.h" #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/quaternion.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" namespace BT { @@ -45,7 +49,7 @@ inline geometry_msgs::msg::Point convertFromString(const StringView key) template<> inline geometry_msgs::msg::Quaternion convertFromString(const StringView key) { - // three real numbers separated by semicolons + // four real numbers separated by semicolons auto parts = BT::splitString(key, ';'); if (parts.size() != 4) { throw std::runtime_error("invalid number of fields for orientation attribute)"); @@ -59,6 +63,28 @@ inline geometry_msgs::msg::Quaternion convertFromString(const StringView key) } } +template<> +inline geometry_msgs::msg::PoseStamped convertFromString(const StringView key) +{ + // 7 real numbers separated by semicolons + auto parts = BT::splitString(key, ';'); + if (parts.size() != 9) { + throw std::runtime_error("invalid number of fields for PoseStamped attribute)"); + } else { + geometry_msgs::msg::PoseStamped pose_stamped; + pose_stamped.header.stamp = rclcpp::Time(BT::convertFromString(parts[0])); + pose_stamped.header.frame_id = BT::convertFromString(parts[1]); + pose_stamped.pose.position.x = BT::convertFromString(parts[2]); + pose_stamped.pose.position.y = BT::convertFromString(parts[3]); + pose_stamped.pose.position.z = BT::convertFromString(parts[4]); + pose_stamped.pose.orientation.x = BT::convertFromString(parts[5]); + pose_stamped.pose.orientation.y = BT::convertFromString(parts[6]); + pose_stamped.pose.orientation.z = BT::convertFromString(parts[7]); + pose_stamped.pose.orientation.w = BT::convertFromString(parts[8]); + return pose_stamped; + } +} + template<> inline std::chrono::milliseconds convertFromString(const StringView key) { diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp index fc881a1d190..989f4ffd8dc 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp @@ -45,9 +45,6 @@ class ComputePathToPoseAction : public BtActionNode("planner_id", ""), }); } - -private: - bool first_time_{true}; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp index be48c54a3f8..383dd3d14ec 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp @@ -40,8 +40,7 @@ class NavigateToPoseAction : public BtActionNode("position", "Position"), - BT::InputPort("orientation", "Orientation") + BT::InputPort("goal", "Destination to plan to"), }); } }; 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 new file mode 100644 index 00000000000..0bc30cd5635 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp @@ -0,0 +1,66 @@ +// Copyright (c) 2020 Sarthak Mittal +// Copyright (c) 2019 Intel Corporation +// +// 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_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_BATTERY_LOW_CONDITION_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_BATTERY_LOW_CONDITION_HPP_ + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/battery_state.hpp" +#include "behaviortree_cpp_v3/condition_node.h" + +namespace nav2_behavior_tree +{ + +class IsBatteryLowCondition : public BT::ConditionNode +{ +public: + IsBatteryLowCondition( + const std::string & condition_name, + const BT::NodeConfiguration & conf); + + IsBatteryLowCondition() = delete; + + BT::NodeStatus tick() override; + + static BT::PortsList providedPorts() + { + return { + BT::InputPort("min_battery", "Minimum battery percentage/voltage"), + BT::InputPort( + "battery_topic", std::string("/battery_status"), "Battery topic"), + BT::InputPort( + "is_voltage", false, "If true voltage will be used to check for low battery"), + }; + } + +private: + void batteryCallback(sensor_msgs::msg::BatteryState::SharedPtr msg); + + rclcpp::Node::SharedPtr node_; + rclcpp::Subscription::SharedPtr battery_sub_; + std::string battery_topic_; + double min_battery_; + bool is_voltage_; + bool is_battery_low_; + std::mutex mutex_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_BATTERY_LOW_CONDITION_HPP_ diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 94e3c2591b8..3a96da5269f 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -28,8 +28,7 @@ - Orientation - Position + Goal diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index a012a86e20e..e3600947cc8 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -17,10 +17,10 @@ rclcpp rclcpp_action rclcpp_lifecycle - std_msgs behaviortree_cpp_v3 builtin_interfaces geometry_msgs + sensor_msgs nav2_msgs nav_msgs tf2 @@ -39,6 +39,7 @@ behaviortree_cpp_v3 builtin_interfaces geometry_msgs + sensor_msgs nav2_msgs nav_msgs tf2 diff --git a/nav2_behavior_tree/plugins/action/back_up_action.cpp b/nav2_behavior_tree/plugins/action/back_up_action.cpp index c1eec645fb0..0ab86ec7c66 100644 --- a/nav2_behavior_tree/plugins/action/back_up_action.cpp +++ b/nav2_behavior_tree/plugins/action/back_up_action.cpp @@ -12,59 +12,36 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NAV2_BEHAVIOR_TREE__BACK_UP_ACTION_HPP_ -#define NAV2_BEHAVIOR_TREE__BACK_UP_ACTION_HPP_ - #include #include -#include -#include "nav2_behavior_tree/bt_action_node.hpp" -#include "nav2_msgs/action/back_up.hpp" +#include "nav2_behavior_tree/plugins/action/back_up_action.hpp" namespace nav2_behavior_tree { -class BackUpAction : public BtActionNode +BackUpAction::BackUpAction( + const std::string & xml_tag_name, + const std::string & action_name, + const BT::NodeConfiguration & conf) +: BtActionNode(xml_tag_name, action_name, conf) { -public: - BackUpAction( - const std::string & xml_tag_name, - const std::string & action_name, - const BT::NodeConfiguration & conf) - : BtActionNode(xml_tag_name, action_name, conf) - { - double dist; - getInput("backup_dist", dist); - double speed; - getInput("backup_speed", speed); - - // silently fix, vector direction determined by distance sign - if (speed < 0.0) { - speed *= -1.0; - } - - // Populate the input message - goal_.target.x = dist; - goal_.target.y = 0.0; - goal_.target.z = 0.0; - goal_.speed = speed; - } - - void on_tick() override - { - increment_recovery_count(); - } + double dist; + getInput("backup_dist", dist); + double speed; + getInput("backup_speed", speed); + + // Populate the input message + goal_.target.x = dist; + goal_.target.y = 0.0; + goal_.target.z = 0.0; + goal_.speed = speed; +} - static BT::PortsList providedPorts() - { - return providedBasicPorts( - { - BT::InputPort("backup_dist", -0.15, "Distance to backup"), - BT::InputPort("backup_speed", 0.025, "Speed at which to backup") - }); - } -}; +void BackUpAction::on_tick() +{ + increment_recovery_count(); +} } // namespace nav2_behavior_tree @@ -75,10 +52,8 @@ BT_REGISTER_NODES(factory) [](const std::string & name, const BT::NodeConfiguration & config) { return std::make_unique( - name, "back_up", config); + name, "backup", config); }; factory.registerBuilder("BackUp", builder); } - -#endif // NAV2_BEHAVIOR_TREE__BACK_UP_ACTION_HPP_ 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 f4f551eb588..efbabbdbea9 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 @@ -37,12 +37,6 @@ void ComputePathToPoseAction::on_tick() BT::NodeStatus ComputePathToPoseAction::on_success() { setOutput("path", result_.result->path); - - if (first_time_) { - first_time_ = false; - } else { - config().blackboard->set("path_updated", true); - } return BT::NodeStatus::SUCCESS; } diff --git a/nav2_behavior_tree/plugins/action/follow_path_action.cpp b/nav2_behavior_tree/plugins/action/follow_path_action.cpp index 62823f40328..bc53211c96d 100644 --- a/nav2_behavior_tree/plugins/action/follow_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/follow_path_action.cpp @@ -26,7 +26,6 @@ FollowPathAction::FollowPathAction( const BT::NodeConfiguration & conf) : BtActionNode(xml_tag_name, action_name, conf) { - config().blackboard->set("path_updated", false); } void FollowPathAction::on_tick() @@ -37,14 +36,14 @@ void FollowPathAction::on_tick() void FollowPathAction::on_wait_for_result() { - // Check if the goal has been updated - if (config().blackboard->get("path_updated")) { - // Reset the flag in the blackboard - config().blackboard->set("path_updated", false); + // Grab the new path + nav_msgs::msg::Path new_path; + getInput("path", new_path); - // Grab the new goal and set the flag so that we send the new goal to + // Check if it is not same with the current one + if (goal_.path != new_path) { // the action server on the next loop iteration - getInput("path", goal_.path); + goal_.path = new_path; goal_updated_ = true; } } 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 f779a682ade..c932bca2571 100644 --- a/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp +++ b/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp @@ -30,19 +30,12 @@ NavigateToPoseAction::NavigateToPoseAction( void NavigateToPoseAction::on_tick() { - // Use the position and orientation fields from the XML attributes to initialize the goal - geometry_msgs::msg::Point position; - geometry_msgs::msg::Quaternion orientation; - - if (!getInput("position", position) || !getInput("orientation", orientation)) { + if (!getInput("goal", goal_.pose)) { RCLCPP_ERROR( node_->get_logger(), - "NavigateToPoseAction: position or orientation not provided"); + "NavigateToPoseAction: goal not provided"); return; } - - goal_.pose.pose.position = position; - goal_.pose.pose.orientation = orientation; } } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/plugins/action/spin_action.cpp b/nav2_behavior_tree/plugins/action/spin_action.cpp index ad668232c62..fa1fc1473c6 100644 --- a/nav2_behavior_tree/plugins/action/spin_action.cpp +++ b/nav2_behavior_tree/plugins/action/spin_action.cpp @@ -12,49 +12,29 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NAV2_BEHAVIOR_TREE__SPIN_ACTION_HPP_ -#define NAV2_BEHAVIOR_TREE__SPIN_ACTION_HPP_ - #include #include -#include -#include "nav2_behavior_tree/bt_action_node.hpp" -#include "nav2_msgs/action/spin.hpp" -#include "geometry_msgs/msg/quaternion.hpp" -#include "tf2/LinearMath/Quaternion.h" -#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#include "nav2_behavior_tree/plugins/action/spin_action.hpp" namespace nav2_behavior_tree { -class SpinAction : public BtActionNode +SpinAction::SpinAction( + const std::string & xml_tag_name, + const std::string & action_name, + const BT::NodeConfiguration & conf) +: BtActionNode(xml_tag_name, action_name, conf) { -public: - SpinAction( - const std::string & xml_tag_name, - const std::string & action_name, - const BT::NodeConfiguration & conf) - : BtActionNode(xml_tag_name, action_name, conf) - { - double dist; - getInput("spin_dist", dist); - goal_.target_yaw = dist; - } - - void on_tick() override - { - increment_recovery_count(); - } + double dist; + getInput("spin_dist", dist); + goal_.target_yaw = dist; +} - static BT::PortsList providedPorts() - { - return providedBasicPorts( - { - BT::InputPort("spin_dist", 1.57, "Spin distance") - }); - } -}; +void SpinAction::on_tick() +{ + increment_recovery_count(); +} } // namespace nav2_behavior_tree @@ -69,5 +49,3 @@ BT_REGISTER_NODES(factory) factory.registerBuilder("Spin", builder); } - -#endif // NAV2_BEHAVIOR_TREE__SPIN_ACTION_HPP_ diff --git a/nav2_behavior_tree/plugins/action/wait_action.cpp b/nav2_behavior_tree/plugins/action/wait_action.cpp index 268a1f7e9b1..d2b0e6484be 100644 --- a/nav2_behavior_tree/plugins/action/wait_action.cpp +++ b/nav2_behavior_tree/plugins/action/wait_action.cpp @@ -12,58 +12,39 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NAV2_BEHAVIOR_TREE__WAIT_ACTION_HPP_ -#define NAV2_BEHAVIOR_TREE__WAIT_ACTION_HPP_ - #include #include -#include -#include "nav2_behavior_tree/bt_action_node.hpp" -#include "nav2_msgs/action/wait.hpp" +#include "nav2_behavior_tree/plugins/action/wait_action.hpp" namespace nav2_behavior_tree { -class WaitAction : public BtActionNode +WaitAction::WaitAction( + const std::string & xml_tag_name, + const std::string & action_name, + const BT::NodeConfiguration & conf) +: BtActionNode(xml_tag_name, action_name, conf) { -public: - WaitAction( - const std::string & xml_tag_name, - const std::string & action_name, - const BT::NodeConfiguration & conf) - : BtActionNode(xml_tag_name, action_name, conf) - { - int duration; - getInput("wait_duration", duration); - if (duration <= 0) { - RCLCPP_WARN( - node_->get_logger(), "Wait duration is negative or zero " - "(%i). Setting to positive.", duration); - duration *= -1; - } - - goal_.time.sec = duration; + int duration; + getInput("wait_duration", duration); + if (duration <= 0) { + RCLCPP_WARN( + node_->get_logger(), "Wait duration is negative or zero " + "(%i). Setting to positive.", duration); + duration *= -1; } - void on_tick() override - { - increment_recovery_count(); - } + goal_.time.sec = duration; +} - // Any BT node that accepts parameters must provide a requiredNodeParameters method - static BT::PortsList providedPorts() - { - return providedBasicPorts( - { - BT::InputPort("wait_duration", 1, "Wait time") - }); - } -}; +void WaitAction::on_tick() +{ + increment_recovery_count(); +} } // namespace nav2_behavior_tree - #include "behaviortree_cpp_v3/bt_factory.h" BT_REGISTER_NODES(factory) { @@ -75,5 +56,3 @@ BT_REGISTER_NODES(factory) factory.registerBuilder("Wait", builder); } - -#endif // NAV2_BEHAVIOR_TREE__WAIT_ACTION_HPP_ diff --git a/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp b/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp new file mode 100644 index 00000000000..4aad6d16769 --- /dev/null +++ b/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp @@ -0,0 +1,67 @@ +// Copyright (c) 2020 Sarthak Mittal +// Copyright (c) 2019 Intel Corporation +// +// 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_behavior_tree/plugins/condition/is_battery_low_condition.hpp" + +namespace nav2_behavior_tree +{ + +IsBatteryLowCondition::IsBatteryLowCondition( + const std::string & condition_name, + const BT::NodeConfiguration & conf) +: BT::ConditionNode(condition_name, conf), + battery_topic_("/battery_status"), + min_battery_(0.0), + is_voltage_(false), + is_battery_low_(false) +{ + getInput("min_battery", min_battery_); + getInput("battery_topic", battery_topic_); + getInput("is_voltage", is_voltage_); + node_ = config().blackboard->get("node"); + battery_sub_ = node_->create_subscription( + battery_topic_, + rclcpp::SystemDefaultsQoS(), + std::bind(&IsBatteryLowCondition::batteryCallback, this, std::placeholders::_1)); +} + +BT::NodeStatus IsBatteryLowCondition::tick() +{ + std::lock_guard lock(mutex_); + if (is_battery_low_) { + return BT::NodeStatus::SUCCESS; + } + return BT::NodeStatus::FAILURE; +} + +void IsBatteryLowCondition::batteryCallback(sensor_msgs::msg::BatteryState::SharedPtr msg) +{ + std::lock_guard lock(mutex_); + if (is_voltage_) { + is_battery_low_ = msg->voltage <= min_battery_; + } else { + is_battery_low_ = msg->percentage <= min_battery_; + } +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("IsBatteryLow"); +} diff --git a/nav2_behavior_tree/plugins/control/pipeline_sequence.cpp b/nav2_behavior_tree/plugins/control/pipeline_sequence.cpp index 9b506c685fe..640c3ed7b4c 100644 --- a/nav2_behavior_tree/plugins/control/pipeline_sequence.cpp +++ b/nav2_behavior_tree/plugins/control/pipeline_sequence.cpp @@ -69,6 +69,7 @@ BT::NodeStatus PipelineSequence::tick() void PipelineSequence::halt() { BT::ControlNode::halt(); + last_child_ticked_ = 0; } } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/plugins/control/recovery_node.cpp b/nav2_behavior_tree/plugins/control/recovery_node.cpp index bfe6ac6ae31..42fc8076be6 100644 --- a/nav2_behavior_tree/plugins/control/recovery_node.cpp +++ b/nav2_behavior_tree/plugins/control/recovery_node.cpp @@ -47,19 +47,21 @@ BT::NodeStatus RecoveryNode::tick() switch (child_status) { case BT::NodeStatus::SUCCESS: { - retry_count_ = 0; + // reset node and return success when first child returns success halt(); return BT::NodeStatus::SUCCESS; } case BT::NodeStatus::FAILURE: { - // tick second child if (retry_count_ < number_of_retries_) { + // halt first child and tick second child in next iteration + ControlNode::haltChild(0); current_child_idx_++; break; } else { - ControlNode::haltChildren(); + // reset node and return failure when max retries has been exceeded + halt(); return BT::NodeStatus::FAILURE; } } @@ -71,8 +73,7 @@ BT::NodeStatus RecoveryNode::tick() default: { - halt(); - return BT::NodeStatus::FAILURE; + throw BT::LogicError("A child node must never return IDLE"); } } // end switch @@ -80,16 +81,16 @@ BT::NodeStatus RecoveryNode::tick() switch (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_--; - ControlNode::haltChildren(); } break; case BT::NodeStatus::FAILURE: { - current_child_idx_--; - retry_count_ = 0; + // reset node and return failure if second child fails halt(); return BT::NodeStatus::FAILURE; } @@ -101,13 +102,13 @@ BT::NodeStatus RecoveryNode::tick() default: { - halt(); - return BT::NodeStatus::FAILURE; + throw BT::LogicError("A child node must never return IDLE"); } } // end switch } } // end while loop - retry_count_ = 0; + + // reset node and return failure halt(); return BT::NodeStatus::FAILURE; } @@ -115,8 +116,8 @@ BT::NodeStatus RecoveryNode::tick() void RecoveryNode::halt() { ControlNode::halt(); - current_child_idx_ = 0; retry_count_ = 0; + current_child_idx_ = 0; } } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt index 3f05ae112d5..ec004fea2d3 100644 --- a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt @@ -1,11 +1,21 @@ +ament_add_gtest(test_action_spin_action test_spin_action.cpp) +target_link_libraries(test_action_spin_action nav2_spin_action_bt_node) +ament_target_dependencies(test_action_spin_action ${dependencies}) + +ament_add_gtest(test_action_back_up_action test_back_up_action.cpp) +target_link_libraries(test_action_back_up_action nav2_back_up_action_bt_node) +ament_target_dependencies(test_action_back_up_action ${dependencies}) + +ament_add_gtest(test_action_wait_action test_wait_action.cpp) +target_link_libraries(test_action_wait_action nav2_wait_action_bt_node) +ament_target_dependencies(test_action_wait_action ${dependencies}) + ament_add_gtest(test_action_clear_costmap_service test_clear_costmap_service.cpp) target_link_libraries(test_action_clear_costmap_service nav2_clear_costmap_service_bt_node) ament_target_dependencies(test_action_clear_costmap_service ${dependencies}) -ament_add_gtest( - test_action_reinitialize_global_localization_service test_reinitialize_global_localization_service.cpp) -target_link_libraries( - test_action_reinitialize_global_localization_service nav2_reinitialize_global_localization_service_bt_node) +ament_add_gtest(test_action_reinitialize_global_localization_service test_reinitialize_global_localization_service.cpp) +target_link_libraries(test_action_reinitialize_global_localization_service nav2_reinitialize_global_localization_service_bt_node) ament_target_dependencies(test_action_reinitialize_global_localization_service ${dependencies}) ament_add_gtest(test_action_compute_path_to_pose_action test_compute_path_to_pose_action.cpp) 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 8906d03d7fc..6a847a7bc54 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 @@ -55,7 +55,6 @@ class BackUpActionTestFixture : public ::testing::Test config_->blackboard->set( "server_timeout", std::chrono::milliseconds(10)); - config_->blackboard->set("path_updated", false); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); @@ -142,7 +141,12 @@ TEST_F(BackUpActionTestFixture, test_tick) tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); EXPECT_EQ(config_->blackboard->get("number_recoveries"), 0); - EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_EQ(config_->blackboard->get("number_recoveries"), 1); auto goal = action_server_->getCurrentGoal(); 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 c5a65c52a1f..d58b6fdd85d 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 @@ -50,7 +50,6 @@ class ClearEntireCostmapServiceTestFixture : public ::testing::Test config_->blackboard->set( "server_timeout", std::chrono::milliseconds(10)); - config_->blackboard->set("path_updated", false); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); 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 dd059315379..d5d1cd6621a 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 @@ -66,7 +66,6 @@ class ComputePathToPoseActionTestFixture : public ::testing::Test config_->blackboard->set( "server_timeout", std::chrono::milliseconds(10)); - config_->blackboard->set("path_updated", false); config_->blackboard->set("initial_pose_received", false); BT::NodeBuilder builder = @@ -129,20 +128,16 @@ TEST_F(ComputePathToPoseActionTestFixture, test_tick) goal.pose.position.x = 1.0; config_->blackboard->set("goal", goal); - // first tick should send the goal to our server - EXPECT_EQ(config_->blackboard->get("path_updated"), false); - EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); - EXPECT_EQ(tree_->rootNode()->getInput("planner_id"), std::string("GridBased")); - EXPECT_EQ(action_server_->getCurrentGoal()->pose.pose.position.x, 1.0); - EXPECT_EQ(action_server_->getCurrentGoal()->planner_id, std::string("GridBased")); - // tick until node succeeds while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { tree_->rootNode()->executeTick(); } - // not set to true after the first goal - EXPECT_EQ(config_->blackboard->get("path_updated"), false); + // the goal should have reached our server + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(tree_->rootNode()->getInput("planner_id"), std::string("GridBased")); + EXPECT_EQ(action_server_->getCurrentGoal()->pose.pose.position.x, 1.0); + EXPECT_EQ(action_server_->getCurrentGoal()->planner_id, std::string("GridBased")); // check if returned path is correct nav_msgs::msg::Path path; @@ -158,15 +153,12 @@ TEST_F(ComputePathToPoseActionTestFixture, test_tick) goal.pose.position.x = -2.5; config_->blackboard->set("goal", goal); - EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); - EXPECT_EQ(action_server_->getCurrentGoal()->pose.pose.position.x, -2.5); - while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { tree_->rootNode()->executeTick(); } - // path is updated on new goal - EXPECT_EQ(config_->blackboard->get("path_updated"), true); + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(action_server_->getCurrentGoal()->pose.pose.position.x, -2.5); config_->blackboard->get("path", path); EXPECT_EQ(path.poses.size(), 1u); 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 2bd07f42323..9c3c67fe4e7 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 @@ -64,7 +64,6 @@ class FollowPathActionTestFixture : public ::testing::Test config_->blackboard->set( "server_timeout", std::chrono::milliseconds(10)); - config_->blackboard->set("path_updated", false); config_->blackboard->set("initial_pose_received", false); BT::NodeBuilder builder = @@ -119,9 +118,7 @@ TEST_F(FollowPathActionTestFixture, test_tick) )"; - config_->blackboard->set("path_updated", true); tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); - EXPECT_EQ(config_->blackboard->get("path_updated"), false); // set new path on blackboard nav_msgs::msg::Path path; @@ -129,18 +126,17 @@ TEST_F(FollowPathActionTestFixture, test_tick) path.poses[0].pose.position.x = 1.0; config_->blackboard->set("path", path); - // first tick should send the goal to our server - EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); - EXPECT_EQ(tree_->rootNode()->getInput("controller_id"), std::string("FollowPath")); - EXPECT_EQ(action_server_->getCurrentGoal()->path.poses.size(), 1u); - EXPECT_EQ(action_server_->getCurrentGoal()->path.poses[0].pose.position.x, 1.0); - EXPECT_EQ(action_server_->getCurrentGoal()->controller_id, std::string("FollowPath")); - // tick until node succeeds while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { tree_->rootNode()->executeTick(); } + + // the goal should have reached our server EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(tree_->rootNode()->getInput("controller_id"), std::string("FollowPath")); + EXPECT_EQ(action_server_->getCurrentGoal()->path.poses.size(), 1u); + EXPECT_EQ(action_server_->getCurrentGoal()->path.poses[0].pose.position.x, 1.0); + EXPECT_EQ(action_server_->getCurrentGoal()->controller_id, std::string("FollowPath")); // halt node so another goal can be sent tree_->rootNode()->halt(); @@ -150,17 +146,13 @@ TEST_F(FollowPathActionTestFixture, test_tick) path.poses[0].pose.position.x = -2.5; config_->blackboard->set("path", path); - EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); - EXPECT_EQ(action_server_->getCurrentGoal()->path.poses.size(), 1u); - EXPECT_EQ(action_server_->getCurrentGoal()->path.poses[0].pose.position.x, -2.5); - config_->blackboard->set("path_updated", true); - while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { tree_->rootNode()->executeTick(); } - // path is updated on new goal - EXPECT_EQ(config_->blackboard->get("path_updated"), false); + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(action_server_->getCurrentGoal()->path.poses.size(), 1u); + EXPECT_EQ(action_server_->getCurrentGoal()->path.poses[0].pose.position.x, -2.5); } int main(int argc, char ** argv) 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 c57ca23bce4..7ea3e858f8d 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 @@ -65,7 +65,6 @@ class NavigateToPoseActionTestFixture : public ::testing::Test config_->blackboard->set( "server_timeout", std::chrono::milliseconds(10)); - config_->blackboard->set("path_updated", false); config_->blackboard->set("initial_pose_received", false); BT::NodeBuilder builder = @@ -116,7 +115,7 @@ TEST_F(NavigateToPoseActionTestFixture, test_tick) R"( - + )"; @@ -124,15 +123,14 @@ TEST_F(NavigateToPoseActionTestFixture, test_tick) geometry_msgs::msg::PoseStamped pose; - // first tick should send the goal to our server - EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); - EXPECT_EQ(action_server_->getCurrentGoal()->pose, pose); - // tick until node succeeds while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { tree_->rootNode()->executeTick(); } + + // goal should have reached our server EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(action_server_->getCurrentGoal()->pose, pose); // halt node so another goal can be sent tree_->rootNode()->halt(); @@ -141,16 +139,13 @@ TEST_F(NavigateToPoseActionTestFixture, test_tick) // set new goal pose.pose.position.x = -2.5; pose.pose.orientation.x = 1.0; - config_->blackboard->set("position", pose.pose.position); - config_->blackboard->set("orientation", pose.pose.orientation); - - EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); - EXPECT_EQ(action_server_->getCurrentGoal()->pose, pose); + config_->blackboard->set("goal", pose); while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { tree_->rootNode()->executeTick(); } + EXPECT_EQ(action_server_->getCurrentGoal()->pose, pose); EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); } 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 03078d384d0..0ce10e229f4 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 @@ -50,7 +50,6 @@ class ReinitializeGlobalLocalizationServiceTestFixture : public ::testing::Test config_->blackboard->set( "server_timeout", std::chrono::milliseconds(10)); - config_->blackboard->set("path_updated", false); config_->blackboard->set("initial_pose_received", false); factory_->registerNodeType( 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 e24b3b251e1..f3d0b3d913d 100644 --- a/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp @@ -55,7 +55,6 @@ class SpinActionTestFixture : public ::testing::Test config_->blackboard->set( "server_timeout", std::chrono::milliseconds(10)); - config_->blackboard->set("path_updated", false); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); @@ -140,7 +139,12 @@ TEST_F(SpinActionTestFixture, test_tick) tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); EXPECT_EQ(config_->blackboard->get("number_recoveries"), 0); - EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_EQ(config_->blackboard->get("number_recoveries"), 1); EXPECT_EQ(action_server_->getCurrentGoal()->target_yaw, 3.14f); } 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 b1023c47233..193395f9401 100644 --- a/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp @@ -56,7 +56,6 @@ class WaitActionTestFixture : public ::testing::Test config_->blackboard->set( "server_timeout", std::chrono::milliseconds(10)); - config_->blackboard->set("path_updated", false); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); @@ -141,7 +140,12 @@ TEST_F(WaitActionTestFixture, test_tick) tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); EXPECT_EQ(config_->blackboard->get("number_recoveries"), 0); - EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING); + + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_EQ(config_->blackboard->get("number_recoveries"), 1); EXPECT_EQ(rclcpp::Duration(action_server_->getCurrentGoal()->time).seconds(), 5.0); } diff --git a/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt b/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt index e8d9d8d3d51..b1350f74537 100644 --- a/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt @@ -25,3 +25,7 @@ ament_target_dependencies(test_condition_transform_available ${dependencies}) ament_add_gtest(test_condition_is_stuck test_is_stuck.cpp) target_link_libraries(test_condition_is_stuck nav2_is_stuck_condition_bt_node) ament_target_dependencies(test_condition_is_stuck ${dependencies}) + +ament_add_gtest(test_condition_is_battery_low test_is_battery_low.cpp) +target_link_libraries(test_condition_is_battery_low nav2_is_battery_low_condition_bt_node) +ament_target_dependencies(test_condition_is_battery_low ${dependencies}) 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 new file mode 100644 index 00000000000..992c50297de --- /dev/null +++ b/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp @@ -0,0 +1,162 @@ +// Copyright (c) 2020 Sarthak Mittal +// Copyright (c) 2019 Intel Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#include "sensor_msgs/msg/battery_state.hpp" + +#include "../../test_behavior_tree_fixture.hpp" +#include "nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp" + +class IsBatteryLowConditionTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("test_is_battery_low"); + factory_ = std::make_shared(); + + config_ = new BT::NodeConfiguration(); + + // Create the blackboard that will be shared by all of the nodes in the tree + config_->blackboard = BT::Blackboard::create(); + // Put items on the blackboard + config_->blackboard->set( + "node", + node_); + + factory_->registerNodeType("IsBatteryLow"); + + battery_pub_ = node_->create_publisher( + "/battery_status", + rclcpp::SystemDefaultsQoS()); + } + + static void TearDownTestCase() + { + delete config_; + config_ = nullptr; + battery_pub_.reset(); + node_.reset(); + factory_.reset(); + } + +protected: + static rclcpp::Node::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static rclcpp::Publisher::SharedPtr battery_pub_; +}; + +rclcpp::Node::SharedPtr IsBatteryLowConditionTestFixture::node_ = nullptr; +BT::NodeConfiguration * IsBatteryLowConditionTestFixture::config_ = nullptr; +std::shared_ptr IsBatteryLowConditionTestFixture::factory_ = nullptr; +rclcpp::Publisher::SharedPtr +IsBatteryLowConditionTestFixture::battery_pub_ = nullptr; + +TEST_F(IsBatteryLowConditionTestFixture, test_behavior_percentage) +{ + std::string xml_txt = + R"( + + + + + )"; + + auto tree = factory_->createTreeFromText(xml_txt, config_->blackboard); + + sensor_msgs::msg::BatteryState battery_msg; + battery_msg.percentage = 1.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::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); + + 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); + + 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); +} + +TEST_F(IsBatteryLowConditionTestFixture, test_behavior_voltage) +{ + std::string xml_txt = + R"( + + + + + )"; + + auto tree = factory_->createTreeFromText(xml_txt, config_->blackboard); + + sensor_msgs::msg::BatteryState battery_msg; + battery_msg.voltage = 10.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::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); + + 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); + + 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); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + bool all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + + return all_successful; +} 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 dc80035f674..37dfc32809b 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_transform_available.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_transform_available.cpp @@ -45,7 +45,6 @@ class TransformAvailableConditionTestFixture : public ::testing::Test config_->blackboard->set( "server_timeout", std::chrono::milliseconds(10)); - config_->blackboard->set("path_updated", false); config_->blackboard->set("initial_pose_received", false); } 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 8356a0064f5..608d8f587ee 100644 --- a/nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp +++ b/nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp @@ -26,21 +26,31 @@ class RecoveryDummy : public nav2_behavior_tree::DummyNode public: BT::NodeStatus tick() override { - if (++ticks_ > num_failures_ && num_failures_ != -1) { + if (ticks_ == num_success_) { setStatus(BT::NodeStatus::SUCCESS); + } else if (ticks_ == num_failure_) { + setStatus(BT::NodeStatus::FAILURE); } + ticks_++; return status(); } - void setMaxFailures(int max_failures) + void returnSuccessOn(int tick) { - num_failures_ = max_failures; + num_success_ = tick; + ticks_ = 0; + } + + void returnFailureOn(int tick) + { + num_failure_ = tick; ticks_ = 0; } private: int ticks_{0}; - int num_failures_{-1}; + int num_success_{-1}; + int num_failure_{-1}; }; class RecoveryNodeTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture @@ -92,10 +102,10 @@ TEST_F(RecoveryNodeTestFixture, test_running) TEST_F(RecoveryNodeTestFixture, test_failure_on_idle_child) { first_child_->changeStatus(BT::NodeStatus::IDLE); - EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); + EXPECT_THROW(bt_node_->executeTick(), BT::LogicError); first_child_->changeStatus(BT::NodeStatus::FAILURE); second_child_->changeStatus(BT::NodeStatus::IDLE); - EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); + EXPECT_THROW(bt_node_->executeTick(), BT::LogicError); } TEST_F(RecoveryNodeTestFixture, test_success_one_retry) @@ -105,7 +115,7 @@ TEST_F(RecoveryNodeTestFixture, test_success_one_retry) EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); // first child fails, second child succeeds, then first child succeeds (one retry) - first_child_->setMaxFailures(1); + first_child_->returnSuccessOn(1); first_child_->changeStatus(BT::NodeStatus::FAILURE); second_child_->changeStatus(BT::NodeStatus::SUCCESS); EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); @@ -125,6 +135,7 @@ TEST_F(RecoveryNodeTestFixture, test_failure_one_retry) EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE); // first child fails, second child succeeds, then first child fails (one retry) + first_child_->returnFailureOn(1); first_child_->changeStatus(BT::NodeStatus::FAILURE); second_child_->changeStatus(BT::NodeStatus::SUCCESS); EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); diff --git a/nav2_behavior_tree/test/test_behavior_tree_fixture.hpp b/nav2_behavior_tree/test/test_behavior_tree_fixture.hpp index c6583c9d473..20c9ace7bf9 100644 --- a/nav2_behavior_tree/test/test_behavior_tree_fixture.hpp +++ b/nav2_behavior_tree/test/test_behavior_tree_fixture.hpp @@ -52,7 +52,6 @@ class BehaviorTreeTestFixture : public ::testing::Test config_->blackboard->set( "server_timeout", std::chrono::milliseconds(10)); - config_->blackboard->set("path_updated", false); config_->blackboard->set("initial_pose_received", false); transform_handler_->activate(); diff --git a/nav2_behavior_tree/test/test_bt_conversions.cpp b/nav2_behavior_tree/test/test_bt_conversions.cpp index 0659233ef4d..ab67d5e52f5 100644 --- a/nav2_behavior_tree/test/test_bt_conversions.cpp +++ b/nav2_behavior_tree/test/test_bt_conversions.cpp @@ -20,6 +20,7 @@ #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/quaternion.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" #include "behaviortree_cpp_v3/bt_factory.h" #include "nav2_behavior_tree/bt_conversions.hpp" @@ -161,6 +162,81 @@ TEST(QuaternionPortTest, test_correct_syntax) EXPECT_EQ(value.w, 0.7); } +TEST(PoseStampedPortTest, test_wrong_syntax) +{ + std::string xml_txt = + R"( + + + + + )"; + + BT::BehaviorTreeFactory factory; + factory.registerNodeType>("PoseStampedPort"); + auto tree = factory.createTreeFromText(xml_txt); + + geometry_msgs::msg::PoseStamped value; + tree.rootNode()->getInput("test", value); + EXPECT_EQ(rclcpp::Time(value.header.stamp).nanoseconds(), 0); + EXPECT_EQ(value.header.frame_id, ""); + EXPECT_EQ(value.pose.position.x, 0.0); + EXPECT_EQ(value.pose.position.y, 0.0); + EXPECT_EQ(value.pose.position.z, 0.0); + EXPECT_EQ(value.pose.orientation.x, 0.0); + EXPECT_EQ(value.pose.orientation.y, 0.0); + EXPECT_EQ(value.pose.orientation.z, 0.0); + EXPECT_EQ(value.pose.orientation.w, 1.0); + + xml_txt = + R"( + + + + + )"; + + tree = factory.createTreeFromText(xml_txt); + tree.rootNode()->getInput("test", value); + EXPECT_EQ(rclcpp::Time(value.header.stamp).nanoseconds(), 0); + EXPECT_EQ(value.header.frame_id, ""); + EXPECT_EQ(value.pose.position.x, 0.0); + EXPECT_EQ(value.pose.position.y, 0.0); + EXPECT_EQ(value.pose.position.z, 0.0); + EXPECT_EQ(value.pose.orientation.x, 0.0); + EXPECT_EQ(value.pose.orientation.y, 0.0); + EXPECT_EQ(value.pose.orientation.z, 0.0); + EXPECT_EQ(value.pose.orientation.w, 1.0); +} + +TEST(PoseStampedPortTest, test_correct_syntax) +{ + std::string xml_txt = + R"( + + + + + )"; + + BT::BehaviorTreeFactory factory; + factory.registerNodeType>("PoseStampedPort"); + auto tree = factory.createTreeFromText(xml_txt); + + tree = factory.createTreeFromText(xml_txt); + geometry_msgs::msg::PoseStamped value; + tree.rootNode()->getInput("test", value); + EXPECT_EQ(rclcpp::Time(value.header.stamp).nanoseconds(), 0); + EXPECT_EQ(value.header.frame_id, "map"); + EXPECT_EQ(value.pose.position.x, 1.0); + EXPECT_EQ(value.pose.position.y, 2.0); + EXPECT_EQ(value.pose.position.z, 3.0); + EXPECT_EQ(value.pose.orientation.x, 4.0); + EXPECT_EQ(value.pose.orientation.y, 5.0); + EXPECT_EQ(value.pose.orientation.z, 6.0); + EXPECT_EQ(value.pose.orientation.w, 7.0); +} + TEST(MillisecondsPortTest, test_correct_syntax) { std::string xml_txt = diff --git a/nav2_bringup/bringup/README.md b/nav2_bringup/bringup/README.md index 525b65df3a2..a4a1b9a6f66 100644 --- a/nav2_bringup/bringup/README.md +++ b/nav2_bringup/bringup/README.md @@ -22,7 +22,7 @@ The `nav2_bringup` package is an example bringup system for Navigation2 applicat ```sudo apt-get install ros--gazebo*``` * A Gazebo world for simulating the robot ([Gazebo tutorials](http://gazebosim.org/tutorials?tut=quick_start)) -* A map of that world saved to a map.pgm and map.yaml ([ROS Navigation Tutorials](https://github.com/ros-planning/navigation2/tree/master/doc/use_cases)) +* A map of that world saved to a map.pgm and map.yaml ([ROS Navigation Tutorials](https://github.com/ros-planning/navigation2/tree/main/doc/use_cases)) ### Terminal 1: Launch Gazebo @@ -109,7 +109,7 @@ ros2 launch nav2_bringup multi_tb3_simulation_launch.py * Learn more about how to use Navigation 2 with SLAM to create maps; - - [Navigation 2 with SLAM](https://github.com/ros-planning/navigation2/blob/master/doc/use_cases/navigation_with_slam.md) + - [Navigation 2 with SLAM](https://github.com/ros-planning/navigation2/blob/main/doc/use_cases/navigation_with_slam.md) * _Please note that currently, nav2_bringup works if you provide a map file. However, providing a map is not required to use Navigation2. Navigation2 can be configured to use the costmaps to navigate in an area without using a map file_ diff --git a/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml index 53ab496bebd..54773241bfb 100644 --- a/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml +++ b/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml @@ -69,6 +69,8 @@ bt_navigator: - nav2_rate_controller_bt_node - nav2_distance_controller_bt_node - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_goal_updater_node_bt_node - nav2_recovery_node_bt_node - nav2_pipeline_sequence_bt_node - nav2_transform_available_condition_bt_node diff --git a/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml index a4a62d6b1a5..2c1658f1482 100644 --- a/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml +++ b/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml @@ -69,6 +69,8 @@ bt_navigator: - nav2_rate_controller_bt_node - nav2_distance_controller_bt_node - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_goal_updater_node_bt_node - nav2_recovery_node_bt_node - nav2_pipeline_sequence_bt_node - nav2_transform_available_condition_bt_node diff --git a/nav2_bringup/bringup/params/nav2_params.yaml b/nav2_bringup/bringup/params/nav2_params.yaml index 0ed09ede415..784eea19b38 100644 --- a/nav2_bringup/bringup/params/nav2_params.yaml +++ b/nav2_bringup/bringup/params/nav2_params.yaml @@ -239,6 +239,7 @@ map_saver: save_map_timeout: 5000 free_thresh_default: 0.25 occupied_thresh_default: 0.65 + map_subscribe_transient_local: True planner_server: ros__parameters: @@ -279,13 +280,3 @@ recoveries_server: robot_state_publisher: ros__parameters: use_sim_time: True - -waypoint_follower: - ros__parameters: - loop_rate: 20 - stop_on_failure: false - waypoint_task_executor_plugin: "waypoint_task_executor" - waypoint_task_executor: - plugin: "nav2_waypoint_follower::WaitAtWaypoint" - enabled: True - waypoint_pause_duration: 0 diff --git a/nav2_bringup/nav2_gazebo_spawner/nav2_gazebo_spawner/nav2_gazebo_spawner.py b/nav2_bringup/nav2_gazebo_spawner/nav2_gazebo_spawner/nav2_gazebo_spawner.py index 5f4de1d913f..05cc3c6dbc2 100644 --- a/nav2_bringup/nav2_gazebo_spawner/nav2_gazebo_spawner/nav2_gazebo_spawner.py +++ b/nav2_bringup/nav2_gazebo_spawner/nav2_gazebo_spawner/nav2_gazebo_spawner.py @@ -36,6 +36,8 @@ def main(): help='the y component of the initial position [meters]') parser.add_argument('-z', type=float, default=0, help='the z component of the initial position [meters]') + parser.add_argument('-k', '--timeout', type=float, default=10.0, + help="Seconds to wait. Block until the future is complete if negative. Don't wait if 0.") group = parser.add_mutually_exclusive_group(required=True) group.add_argument('-t', '--turtlebot_type', type=str, @@ -97,7 +99,7 @@ def main(): node.get_logger().info('Sending service request to `/spawn_entity`') future = client.call_async(request) - rclpy.spin_until_future_complete(node, future) + rclpy.spin_until_future_complete(node, future, args.timeout) if future.result() is not None: print('response: %r' % future.result()) else: diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/ros_topic_logger.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/ros_topic_logger.hpp index c517d6c9e54..27fe61eebce 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/ros_topic_logger.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/ros_topic_logger.hpp @@ -27,7 +27,7 @@ namespace nav2_bt_navigator class RosTopicLogger : public BT::StatusChangeLogger { public: - RosTopicLogger(const rclcpp::Node::SharedPtr & ros_node, const BT::Tree & tree); + RosTopicLogger(const rclcpp::Node::WeakPtr & ros_node, const BT::Tree & tree); void callback( BT::Duration timestamp, @@ -38,7 +38,8 @@ class RosTopicLogger : public BT::StatusChangeLogger void flush() override; protected: - rclcpp::Node::SharedPtr ros_node_; + rclcpp::Clock::SharedPtr clock_; + rclcpp::Logger logger_{rclcpp::get_logger("bt_navigator")}; rclcpp::Publisher::SharedPtr log_pub_; std::vector event_log_; }; diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index fcb761322c6..545cf616479 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -16,7 +16,6 @@ #include #include -#include #include #include #include @@ -52,7 +51,7 @@ BtNavigator::BtNavigator() "nav2_distance_controller_bt_node", "nav2_speed_controller_bt_node", "nav2_truncate_path_action_bt_node", - "nav2_change_goal_node_bt_node", + "nav2_goal_updater_node_bt_node", "nav2_recovery_node_bt_node", "nav2_pipeline_sequence_bt_node", "nav2_round_robin_node_bt_node", @@ -72,7 +71,6 @@ BtNavigator::BtNavigator() BtNavigator::~BtNavigator() { - RCLCPP_INFO(get_logger(), "Destroying"); } nav2_util::CallbackReturn @@ -108,7 +106,7 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) get_node_clock_interface(), get_node_logging_interface(), get_node_waitables_interface(), - "navigate_to_pose", std::bind(&BtNavigator::navigateToPose, this), false); + "navigate_to_pose", std::bind(&BtNavigator::navigateToPose, this)); // Get the libraries to pull plugins from plugin_lib_names_ = get_parameter("plugin_lib_names").as_string_array(); @@ -126,18 +124,12 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) blackboard_->set("node", client_node_); // NOLINT blackboard_->set>("tf_buffer", tf_); // NOLINT blackboard_->set("server_timeout", std::chrono::milliseconds(10)); // NOLINT - blackboard_->set("path_updated", false); // NOLINT blackboard_->set("initial_pose_received", false); // NOLINT blackboard_->set("number_recoveries", 0); // NOLINT // Get the BT filename to use from the node parameter get_parameter("default_bt_xml_filename", default_bt_xml_filename_); - if (!loadBehaviorTree(default_bt_xml_filename_)) { - RCLCPP_ERROR(get_logger(), "Error loading XML file: %s", default_bt_xml_filename_.c_str()); - return nav2_util::CallbackReturn::FAILURE; - } - return nav2_util::CallbackReturn::SUCCESS; } @@ -176,8 +168,16 @@ BtNavigator::on_activate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Activating"); + if (!loadBehaviorTree(default_bt_xml_filename_)) { + RCLCPP_ERROR(get_logger(), "Error loading XML file: %s", default_bt_xml_filename_.c_str()); + return nav2_util::CallbackReturn::FAILURE; + } + action_server_->activate(); + // create bond connection + createBond(); + return nav2_util::CallbackReturn::SUCCESS; } @@ -188,6 +188,9 @@ BtNavigator::on_deactivate(const rclcpp_lifecycle::State & /*state*/) action_server_->deactivate(); + // destroy bond connection + destroyBond(); + return nav2_util::CallbackReturn::SUCCESS; } @@ -246,7 +249,7 @@ BtNavigator::navigateToPose() auto bt_xml_filename = action_server_->get_current_goal()->behavior_tree; // Empty id in request is default for backward compatibility - bt_xml_filename = bt_xml_filename == "" ? default_bt_xml_filename_ : bt_xml_filename; + bt_xml_filename = bt_xml_filename.empty() ? default_bt_xml_filename_ : bt_xml_filename; if (!loadBehaviorTree(bt_xml_filename)) { RCLCPP_ERROR( @@ -323,7 +326,7 @@ BtNavigator::initializeGoalPose() blackboard_->set("number_recoveries", 0); // NOLINT // Update the goal pose on the blackboard - blackboard_->set("goal", goal->pose); + blackboard_->set("goal", goal->pose); } void diff --git a/nav2_bt_navigator/src/ros_topic_logger.cpp b/nav2_bt_navigator/src/ros_topic_logger.cpp index 48bffd20c85..5a11724db5d 100644 --- a/nav2_bt_navigator/src/ros_topic_logger.cpp +++ b/nav2_bt_navigator/src/ros_topic_logger.cpp @@ -21,10 +21,13 @@ namespace nav2_bt_navigator { RosTopicLogger::RosTopicLogger( - const rclcpp::Node::SharedPtr & ros_node, const BT::Tree & tree) -: StatusChangeLogger(tree.rootNode()), ros_node_(ros_node) + const rclcpp::Node::WeakPtr & ros_node, const BT::Tree & tree) +: StatusChangeLogger(tree.rootNode()) { - log_pub_ = ros_node_->create_publisher( + auto node = ros_node.lock(); + clock_ = node->get_clock(); + logger_ = node->get_logger(); + log_pub_ = node->create_publisher( "behavior_tree_log", rclcpp::QoS(10)); } @@ -43,7 +46,7 @@ void RosTopicLogger::callback( event.timestamp = tf2_ros::toMsg(std::chrono::time_point(timestamp)); #else - event.timestamp = tf2_ros::toMsg(tf2::TimePoint(timestamp)); + event.timestamp = tf2_ros::toMsg(timestamp); #endif event.node_name = node.name(); event.previous_status = toStr(prev_status, false); @@ -51,7 +54,7 @@ void RosTopicLogger::callback( event_log_.push_back(std::move(event)); RCLCPP_DEBUG( - ros_node_->get_logger(), "[%.3f]: %25s %s -> %s", + logger_, "[%.3f]: %25s %s -> %s", std::chrono::duration(timestamp).count(), node.name().c_str(), toStr(prev_status, true).c_str(), @@ -60,9 +63,9 @@ void RosTopicLogger::callback( void RosTopicLogger::flush() { - if (event_log_.size() > 0) { + if (!event_log_.empty()) { auto log_msg = std::make_unique(); - log_msg->timestamp = ros_node_->now(); + log_msg->timestamp = clock_->now(); log_msg->event_log = event_log_; log_pub_->publish(std::move(log_msg)); event_log_.clear(); diff --git a/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp b/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp index b7e483c1611..f95b5f2f921 100644 --- a/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp @@ -58,7 +58,7 @@ class SimpleGoalChecker : public nav2_core::GoalChecker SimpleGoalChecker(); // Standard GoalChecker Interface void initialize( - const rclcpp_lifecycle::LifecycleNode::SharedPtr & nh, + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, const std::string & plugin_name) override; void reset() override; bool isGoalReached( diff --git a/nav2_controller/include/nav2_controller/plugins/simple_progress_checker.hpp b/nav2_controller/include/nav2_controller/plugins/simple_progress_checker.hpp index 92a5374a5e0..f4a2c6d25a0 100644 --- a/nav2_controller/include/nav2_controller/plugins/simple_progress_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/simple_progress_checker.hpp @@ -34,7 +34,7 @@ class SimpleProgressChecker : public nav2_core::ProgressChecker { public: void initialize( - const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, const std::string & plugin_name) override; bool check(geometry_msgs::msg::PoseStamped & current_pose) override; void reset() override; @@ -52,7 +52,7 @@ class SimpleProgressChecker : public nav2_core::ProgressChecker */ void reset_baseline_pose(const geometry_msgs::msg::Pose2D & pose); - rclcpp_lifecycle::LifecycleNode::SharedPtr nh_; + rclcpp::Clock::SharedPtr clock_; double radius_; rclcpp::Duration time_allowance_{0, 0}; diff --git a/nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp b/nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp index 869edb6330b..160ac60f091 100644 --- a/nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp @@ -55,7 +55,7 @@ class StoppedGoalChecker : public SimpleGoalChecker StoppedGoalChecker(); // Standard GoalChecker Interface void initialize( - const rclcpp_lifecycle::LifecycleNode::SharedPtr & nh, + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, const std::string & plugin_name) override; bool isGoalReached( const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose, diff --git a/nav2_controller/plugins/simple_goal_checker.cpp b/nav2_controller/plugins/simple_goal_checker.cpp index f1ec7dba6ca..983032ee660 100644 --- a/nav2_controller/plugins/simple_goal_checker.cpp +++ b/nav2_controller/plugins/simple_goal_checker.cpp @@ -56,22 +56,24 @@ SimpleGoalChecker::SimpleGoalChecker() } void SimpleGoalChecker::initialize( - const rclcpp_lifecycle::LifecycleNode::SharedPtr & nh, + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, const std::string & plugin_name) { + auto node = parent.lock(); + nav2_util::declare_parameter_if_not_declared( - nh, + node, plugin_name + ".xy_goal_tolerance", rclcpp::ParameterValue(0.25)); nav2_util::declare_parameter_if_not_declared( - nh, + node, plugin_name + ".yaw_goal_tolerance", rclcpp::ParameterValue(0.25)); nav2_util::declare_parameter_if_not_declared( - nh, + node, plugin_name + ".stateful", rclcpp::ParameterValue(true)); - nh->get_parameter(plugin_name + ".xy_goal_tolerance", xy_goal_tolerance_); - nh->get_parameter(plugin_name + ".yaw_goal_tolerance", yaw_goal_tolerance_); - nh->get_parameter(plugin_name + ".stateful", stateful_); + node->get_parameter(plugin_name + ".xy_goal_tolerance", xy_goal_tolerance_); + node->get_parameter(plugin_name + ".yaw_goal_tolerance", yaw_goal_tolerance_); + node->get_parameter(plugin_name + ".stateful", stateful_); xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_; } diff --git a/nav2_controller/plugins/simple_progress_checker.cpp b/nav2_controller/plugins/simple_progress_checker.cpp index 498d13256cb..542c8226efa 100644 --- a/nav2_controller/plugins/simple_progress_checker.cpp +++ b/nav2_controller/plugins/simple_progress_checker.cpp @@ -27,18 +27,21 @@ namespace nav2_controller static double pose_distance(const geometry_msgs::msg::Pose2D &, const geometry_msgs::msg::Pose2D &); void SimpleProgressChecker::initialize( - const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, const std::string & plugin_name) { - nh_ = node; + auto node = parent.lock(); + + clock_ = node->get_clock(); + nav2_util::declare_parameter_if_not_declared( - nh_, plugin_name + ".required_movement_radius", rclcpp::ParameterValue(0.5)); + node, plugin_name + ".required_movement_radius", rclcpp::ParameterValue(0.5)); nav2_util::declare_parameter_if_not_declared( - nh_, plugin_name + ".movement_time_allowance", rclcpp::ParameterValue(10.0)); + node, plugin_name + ".movement_time_allowance", rclcpp::ParameterValue(10.0)); // Scale is set to 0 by default, so if it was not set otherwise, set to 0 - nh_->get_parameter_or(plugin_name + ".required_movement_radius", radius_, 0.5); + node->get_parameter_or(plugin_name + ".required_movement_radius", radius_, 0.5); double time_allowance_param = 0.0; - nh_->get_parameter_or(plugin_name + ".movement_time_allowance", time_allowance_param, 10.0); + node->get_parameter_or(plugin_name + ".movement_time_allowance", time_allowance_param, 10.0); time_allowance_ = rclcpp::Duration::from_seconds(time_allowance_param); } @@ -53,10 +56,7 @@ bool SimpleProgressChecker::check(geometry_msgs::msg::PoseStamped & current_pose reset_baseline_pose(current_pose2d); return true; } - if ((nh_->now() - baseline_time_) > time_allowance_) { - return false; - } - return true; + return !((clock_->now() - baseline_time_) > time_allowance_); } void SimpleProgressChecker::reset() @@ -67,17 +67,13 @@ void SimpleProgressChecker::reset() void SimpleProgressChecker::reset_baseline_pose(const geometry_msgs::msg::Pose2D & pose) { baseline_pose_ = pose; - baseline_time_ = nh_->now(); + baseline_time_ = clock_->now(); baseline_pose_set_ = true; } bool SimpleProgressChecker::is_robot_moved_enough(const geometry_msgs::msg::Pose2D & pose) { - if (pose_distance(pose, baseline_pose_) > radius_) { - return true; - } else { - return false; - } + return pose_distance(pose, baseline_pose_) > radius_; } static double pose_distance( diff --git a/nav2_controller/plugins/stopped_goal_checker.cpp b/nav2_controller/plugins/stopped_goal_checker.cpp index 2e24ad181aa..d6d409f1900 100644 --- a/nav2_controller/plugins/stopped_goal_checker.cpp +++ b/nav2_controller/plugins/stopped_goal_checker.cpp @@ -51,20 +51,22 @@ StoppedGoalChecker::StoppedGoalChecker() } void StoppedGoalChecker::initialize( - const rclcpp_lifecycle::LifecycleNode::SharedPtr & nh, + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, const std::string & plugin_name) { - SimpleGoalChecker::initialize(nh, plugin_name); + SimpleGoalChecker::initialize(parent, plugin_name); + + auto node = parent.lock(); nav2_util::declare_parameter_if_not_declared( - nh, + node, plugin_name + ".rot_stopped_velocity", rclcpp::ParameterValue(0.25)); nav2_util::declare_parameter_if_not_declared( - nh, + node, plugin_name + ".trans_stopped_velocity", rclcpp::ParameterValue(0.25)); - nh->get_parameter(plugin_name + ".rot_stopped_velocity", rot_stopped_velocity_); - nh->get_parameter(plugin_name + ".trans_stopped_velocity", trans_stopped_velocity_); + node->get_parameter(plugin_name + ".rot_stopped_velocity", rot_stopped_velocity_); + node->get_parameter(plugin_name + ".trans_stopped_velocity", trans_stopped_velocity_); } bool StoppedGoalChecker::isGoalReached( diff --git a/nav2_controller/src/nav2_controller.cpp b/nav2_controller/src/nav2_controller.cpp index 806df12569d..bb53e3c6d42 100644 --- a/nav2_controller/src/nav2_controller.cpp +++ b/nav2_controller/src/nav2_controller.cpp @@ -63,7 +63,9 @@ ControllerServer::ControllerServer() ControllerServer::~ControllerServer() { - RCLCPP_INFO(get_logger(), "Destroying"); + progress_checker_.reset(); + goal_checker_.reset(); + controllers_.clear(); } nav2_util::CallbackReturn @@ -94,6 +96,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) rclcpp::ParameterValue(default_types_[i])); } } + controller_types_.resize(controller_ids_.size()); get_parameter("controller_frequency", controller_frequency_); @@ -180,6 +183,9 @@ ControllerServer::on_activate(const rclcpp_lifecycle::State & state) vel_publisher_->on_activate(); action_server_->activate(); + // create bond connection + createBond(); + return nav2_util::CallbackReturn::SUCCESS; } @@ -198,6 +204,9 @@ ControllerServer::on_deactivate(const rclcpp_lifecycle::State & state) publishZeroVelocity(); vel_publisher_->on_deactivate(); + // destroy bond connection + destroyBond(); + return nav2_util::CallbackReturn::SUCCESS; } @@ -392,10 +401,7 @@ void ControllerServer::updateGlobalPath() void ControllerServer::publishVelocity(const geometry_msgs::msg::TwistStamped & velocity) { auto cmd_vel = std::make_unique(velocity.twist); - if ( - vel_publisher_->is_activated() && - this->count_subscribers(vel_publisher_->get_topic_name()) > 0) - { + if (vel_publisher_->is_activated() && vel_publisher_->get_subscription_count() > 0) { vel_publisher_->publish(std::move(cmd_vel)); } } diff --git a/nav2_core/include/nav2_core/controller.hpp b/nav2_core/include/nav2_core/controller.hpp index 69aa10f25b0..e60c0288169 100644 --- a/nav2_core/include/nav2_core/controller.hpp +++ b/nav2_core/include/nav2_core/controller.hpp @@ -72,7 +72,7 @@ class Controller * @param costmap_ros A pointer to the costmap */ virtual void configure( - const rclcpp_lifecycle::LifecycleNode::SharedPtr &, + const rclcpp_lifecycle::LifecycleNode::WeakPtr &, std::string name, const std::shared_ptr &, const std::shared_ptr &) = 0; diff --git a/nav2_core/include/nav2_core/global_planner.hpp b/nav2_core/include/nav2_core/global_planner.hpp index 7245e6aedc0..dff1207b88f 100644 --- a/nav2_core/include/nav2_core/global_planner.hpp +++ b/nav2_core/include/nav2_core/global_planner.hpp @@ -48,7 +48,7 @@ class GlobalPlanner * @param costmap_ros A pointer to the costmap */ virtual void configure( - rclcpp_lifecycle::LifecycleNode::SharedPtr parent, + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, std::string name, std::shared_ptr tf, std::shared_ptr costmap_ros) = 0; diff --git a/nav2_core/include/nav2_core/goal_checker.hpp b/nav2_core/include/nav2_core/goal_checker.hpp index a8006e137a4..2cb331688b0 100644 --- a/nav2_core/include/nav2_core/goal_checker.hpp +++ b/nav2_core/include/nav2_core/goal_checker.hpp @@ -64,10 +64,10 @@ class GoalChecker /** * @brief Initialize any parameters from the NodeHandle - * @param nh NodeHandle for grabbing parameters + * @param parent Node pointer for grabbing parameters */ virtual void initialize( - const rclcpp_lifecycle::LifecycleNode::SharedPtr & nh, + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, const std::string & plugin_name) = 0; virtual void reset() = 0; /** diff --git a/nav2_core/include/nav2_core/progress_checker.hpp b/nav2_core/include/nav2_core/progress_checker.hpp index 2dd9c5bef89..0ab0402bf13 100644 --- a/nav2_core/include/nav2_core/progress_checker.hpp +++ b/nav2_core/include/nav2_core/progress_checker.hpp @@ -40,10 +40,10 @@ class ProgressChecker /** * @brief Initialize parameters for ProgressChecker - * @param node Node pointer + * @param parent Node pointer */ virtual void initialize( - const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, const std::string & plugin_name) = 0; /** * @brief Checks if the robot has moved compare to previous diff --git a/nav2_core/include/nav2_core/recovery.hpp b/nav2_core/include/nav2_core/recovery.hpp index 5d3fa6de52e..a800b2d21f5 100644 --- a/nav2_core/include/nav2_core/recovery.hpp +++ b/nav2_core/include/nav2_core/recovery.hpp @@ -47,7 +47,7 @@ class Recovery * @param costmap_ros A pointer to the costmap */ virtual void configure( - const rclcpp_lifecycle::LifecycleNode::SharedPtr parent, + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, const std::string & name, std::shared_ptr tf, std::shared_ptr collision_checker) = 0; diff --git a/nav2_core/include/nav2_core/waypoint_task_executor.hpp b/nav2_core/include/nav2_core/waypoint_task_executor.hpp deleted file mode 100644 index ffdbfd95e1d..00000000000 --- a/nav2_core/include/nav2_core/waypoint_task_executor.hpp +++ /dev/null @@ -1,69 +0,0 @@ -// Copyright (c) 2020 Fetullah Atas -// -// 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_CORE__WAYPOINT_TASK_EXECUTOR_HPP_ -#define NAV2_CORE__WAYPOINT_TASK_EXECUTOR_HPP_ -#pragma once - -#include - -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" - -namespace nav2_core -{ -/** - * @brief Base class for creating a plugin in order to perform a specific task at waypoint arrivals. - * - */ -class WaypointTaskExecutor -{ -public: - /** - * @brief Construct a new Simple Task Execution At Waypoint Base object - * - */ - WaypointTaskExecutor() {} - - /** - * @brief Destroy the Simple Task Execution At Waypoint Base object - * - */ - virtual ~WaypointTaskExecutor() {} - - /** - * @brief Override this to setup your pub, sub or any ros services that you will use in the plugin. - * - * @param parent parent node that plugin will be created within(for an example see nav_waypoint_follower) - * @param plugin_name plugin name comes from parameters in yaml file - */ - virtual void initialize( - const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, - const std::string & plugin_name) = 0; - - /** - * @brief Override this to define the body of your task that you would like to execute once the robot arrived to waypoint - * - * @param curr_pose current pose of the robot - * @param curr_waypoint_index current waypoint, that robot just arrived - * @return true if task execution was successful - * @return false if task execution failed - */ - virtual bool processAtWaypoint( - const geometry_msgs::msg::PoseStamped & curr_pose, const int & curr_waypoint_index) = 0; -}; -} // namespace nav2_core -#endif // NAV2_CORE__WAYPOINT_TASK_EXECUTOR_HPP_ diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt index 4c9a2a93666..a639b2ee103 100644 --- a/nav2_costmap_2d/CMakeLists.txt +++ b/nav2_costmap_2d/CMakeLists.txt @@ -49,6 +49,7 @@ add_library(nav2_costmap_2d_core SHARED src/observation_buffer.cpp src/clear_costmap_service.cpp src/footprint_collision_checker.cpp + plugins/costmap_filters/costmap_filter.cpp ) # prevent pluginlib from using boost @@ -95,6 +96,16 @@ target_link_libraries(layers nav2_costmap_2d_core ) +add_library(filters SHARED + plugins/costmap_filters/keepout_filter.cpp +) +ament_target_dependencies(filters + ${dependencies} +) +target_link_libraries(filters + nav2_costmap_2d_core +) + add_library(nav2_costmap_2d_client SHARED src/footprint_subscriber.cpp src/costmap_subscriber.cpp @@ -131,11 +142,13 @@ ament_target_dependencies(nav2_costmap_2d target_link_libraries(nav2_costmap_2d nav2_costmap_2d_core layers + filters ) install(TARGETS nav2_costmap_2d_core layers + filters nav2_costmap_2d_client ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -169,7 +182,7 @@ if(BUILD_TESTING) endif() ament_export_include_directories(include) -ament_export_libraries(layers nav2_costmap_2d_core nav2_costmap_2d_client) +ament_export_libraries(layers filters nav2_costmap_2d_core nav2_costmap_2d_client) ament_export_dependencies(${dependencies}) pluginlib_export_plugin_description_file(nav2_costmap_2d costmap_plugins.xml) ament_package() diff --git a/nav2_costmap_2d/README.md b/nav2_costmap_2d/README.md index 9dd12247e12..779398fd6b2 100644 --- a/nav2_costmap_2d/README.md +++ b/nav2_costmap_2d/README.md @@ -13,7 +13,7 @@ general ROS2 community. A proposal temporary replacement has been submitted as a ## How to configure using Voxel Layer plugin: By default, the navigation stack uses the _Obstacle Layer_ to avoid obstacles in 2D. The _Voxel Layer_ allows for detecting obstacles in 3D using Pointcloud2 data. It requires Pointcloud2 data being published on some topic. For simulation, a Gazebo model of the robot with depth camera enabled will publish a pointcloud topic. -The Voxel Layer plugin can be used to update the local costmap, glocal costmap or both, depending on how you define it in the `nav2_params.yaml` file in the nav2_bringup directory. The voxel layer plugin has to be added to the list of ```plugin_names``` and ```plugin_types``` in the global/local costmap scopes in the param file. If these are not defined in the param file, the default plugins set in nav2_costmap_2d will be used. +The Voxel Layer plugin can be used to update the local costmap, glocal costmap or both, depending on how you define it in the `nav2_params.yaml` file in the nav2_bringup directory. The voxel layer plugin has to be added to the list of ```plugins``` and its ```plugin``` type should be correctly specified in the global/local costmap scopes in the param file. If these are not defined in the param file, the default plugins set in nav2_costmap_2d will be used. Inside each costmap layer (voxel, obstacle, etc) define `observation_sources` param. Here you can define multiple sources to be used with the layer. The param configuration example below shows the way you can configure costmaps to use voxel layer. @@ -24,9 +24,9 @@ Example param configuration snippet for enabling voxel layer in local costmap is local_costmap: local_costmap: ros__parameters: - plugin_names: ["obstacle_layer", "voxel_layer", "inflation_layer"] - plugin_types: ["nav2_costmap_2d::ObstacleLayer", "nav2_costmap_2d::VoxelLayer", "nav2_costmap_2d::InflationLayer"] + plugins: ["obstacle_layer", "voxel_layer", "inflation_layer"] obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" enabled: True observation_sources: scan scan: @@ -36,6 +36,7 @@ local_costmap: marking: True data_type: "LaserScan" voxel_layer: + plugin: nav2_costmap_2d::VoxelLayer enabled: True publish_voxel_map: True origin_z: 0.0 @@ -74,9 +75,9 @@ For example, to add laser scan and pointcloud as two different sources of inputs local_costmap: local_costmap: ros__parameters: - plugin_names: ["obstacle_layer", "inflation_layer"] - plugin_types: ["nav2_costmap_2d::ObstacleLayer", "nav2_costmap_2d::InflationLayer"] + plugins: ["obstacle_layer", "inflation_layer"] obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" enabled: True observation_sources: scan pointcloud scan: @@ -88,6 +89,12 @@ local_costmap: ``` In order to add multiple sources to the global costmap, follow the same procedure shown in the example above, but now adding the sources and their specific params under the `global_costmap` scope. +## Costmap Filters + +### Overview + +Costmap Filters - is a costmap layer-based instrument which provides an ability to apply to map spatial-dependent raster features named as filter-masks. These features are used in plugin algorithms when filling costmaps in order to allow robots to change their trajectory, behavior or speed when a robot enters/leaves an area marked in a filter masks. Examples of costmap filters include keep-out/safety zones where robots will never enter, speed restriction areas, preferred lanes for robots moving in industries and warehouses. More information about design, architecture of the feature and how it works could be found on navigation2 web-site: https://navigation.ros.org. + ## Future Plans - Conceptually, the costmap_2d model acts as a world model of what is known from the map, sensor, robot pose, etc. We'd like to broaden this world model concept and use costmap's layer concept as motivation for providing a service-style interface to diff --git a/nav2_costmap_2d/costmap_plugins.xml b/nav2_costmap_2d/costmap_plugins.xml index a0725392332..3adad664d3d 100644 --- a/nav2_costmap_2d/costmap_plugins.xml +++ b/nav2_costmap_2d/costmap_plugins.xml @@ -16,5 +16,11 @@ A range-sensor (sonar, IR) based obstacle layer for costmap_2d + + + + Prevents the robot from appearing in keepout zones marked on map. + + diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp index f1378848dfd..adee89f1771 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp @@ -34,7 +34,7 @@ class Costmap2DROS; class ClearCostmapService { public: - ClearCostmapService(nav2_util::LifecycleNode::SharedPtr node, Costmap2DROS & costmap); + ClearCostmapService(const nav2_util::LifecycleNode::WeakPtr & parent, Costmap2DROS & costmap); ClearCostmapService() = delete; @@ -48,8 +48,8 @@ class ClearCostmapService void clearEntirely(); private: - // The ROS node to use for getting parameters, creating the service and logging - nav2_util::LifecycleNode::SharedPtr node_; + // The Logger object for logging + rclcpp::Logger logger_{rclcpp::get_logger("nav2_costmap_2d")}; // The costmap to clear Costmap2DROS & costmap_; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp index 8f9fd438677..8e0f6f86480 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp @@ -48,6 +48,7 @@ #include #include #include "geometry_msgs/msg/point.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" namespace nav2_costmap_2d { @@ -87,6 +88,12 @@ class Costmap2D */ Costmap2D(const Costmap2D & map); + /** + * @brief Constructor for a costmap from an OccupancyGrid map + * @param map The OccupancyGrid map to create costmap from + */ + Costmap2D(const nav_msgs::msg::OccupancyGrid & map); + /** * @brief Overloaded assignment operator * @param map The costmap to copy diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp index 83349d44704..ee276442ca4 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp @@ -67,7 +67,7 @@ class Costmap2DPublisher * @brief Constructor for the Costmap2DPublisher */ Costmap2DPublisher( - nav2_util::LifecycleNode::SharedPtr ros_node, + const nav2_util::LifecycleNode::WeakPtr & parent, Costmap2D * costmap, std::string global_frame, std::string topic_name, @@ -130,7 +130,9 @@ class Costmap2DPublisher const std::shared_ptr request, const std::shared_ptr response); - nav2_util::LifecycleNode::SharedPtr node_; + rclcpp::Clock::SharedPtr clock_; + rclcpp::Logger logger_{rclcpp::get_logger("nav2_costmap_2d")}; + Costmap2D * costmap_; std::string global_frame_; std::string topic_name_; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp index 541a246a513..1a3a7223b62 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp @@ -182,7 +182,7 @@ class Costmap2DROS : public nav2_util::LifecycleNode LayeredCostmap * getLayeredCostmap() { - return layered_costmap_; + return layered_costmap_.get(); } /** @brief Returns the current padded footprint as a geometry_msgs::msg::Polygon. */ @@ -254,7 +254,7 @@ class Costmap2DROS : public nav2_util::LifecycleNode // Publishers and subscribers rclcpp_lifecycle::LifecyclePublisher::SharedPtr footprint_pub_; - Costmap2DPublisher * costmap_publisher_{nullptr}; + std::unique_ptr costmap_publisher_{nullptr}; rclcpp::Subscription::SharedPtr footprint_sub_; rclcpp::Subscription::SharedPtr parameter_sub_; @@ -263,7 +263,7 @@ class Costmap2DROS : public nav2_util::LifecycleNode std::shared_ptr tf_buffer_; std::shared_ptr tf_listener_; - LayeredCostmap * layered_costmap_{nullptr}; + std::unique_ptr layered_costmap_{nullptr}; std::string name_; std::string parent_namespace_; void mapUpdateLoop(double frequency); diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp new file mode 100644 index 00000000000..16918ed1183 --- /dev/null +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp @@ -0,0 +1,141 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2020 Samsung Research Russia + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Alexey Merzlyakov + *********************************************************************/ + +#ifndef NAV2_COSTMAP_2D__COSTMAP_FILTER_HPP_ +#define NAV2_COSTMAP_2D__COSTMAP_FILTER_HPP_ + +#include +#include + +#include "geometry_msgs/msg/pose2_d.hpp" +#include "nav2_costmap_2d/layer.hpp" + +namespace nav2_costmap_2d +{ + +static constexpr double BASE_DEFAULT = 0.0; +static constexpr double MULTIPLIER_DEFAULT = 1.0; + +/** + * @brief: CostmapFilter basic class. It is inherited from Layer in order to avoid + * hidden problems when the shared handling of costmap_ resource (PR #1936) + */ +class CostmapFilter : public Layer +{ +public: + CostmapFilter(); + ~CostmapFilter(); + + /** + * @brief: Provide a typedef to ease future code maintenance + */ + typedef std::recursive_mutex mutex_t; + /** + * @brief: returns pointer to a mutex + */ + mutex_t * getMutex() + { + return access_; + } + + /** Layer API **/ + virtual void onInitialize() final; + virtual void updateBounds( + double robot_x, double robot_y, double robot_yaw, + double * min_x, double * min_y, double * max_x, double * max_y) final; + virtual void updateCosts( + nav2_costmap_2d::Costmap2D & master_grid, + int min_i, int min_j, int max_i, int max_j) final; + + virtual void activate() final; + virtual void deactivate() final; + virtual void reset() final; + + /** CostmapFilter API **/ + /** + * @brief: Initializes costmap filter. Creates subscriptions to filter-related topics + * @param: Name of costmap filter info topic + */ + virtual void initializeFilter( + const std::string & filter_info_topic) = 0; + + /** + * @brief: An algorithm for how to use that map's information. Fills the Costmap2D with + * calculated data and makes an action based on processed data + * @param: Reference to a master costmap2d + * @param: Low window map boundary OX + * @param: Low window map boundary OY + * @param: High window map boundary OX + * @param: High window map boundary OY + * @param: Robot 2D-pose + */ + virtual void process( + nav2_costmap_2d::Costmap2D & master_grid, + int min_i, int min_j, int max_i, int max_j, + const geometry_msgs::msg::Pose2D & pose) = 0; + + /** + * @brief: Resets costmap filter. Stops all subscriptions + */ + virtual void resetFilter() = 0; + +protected: + /** + * @brief: Name of costmap filter info topic + */ + std::string filter_info_topic_; + + /** + * @brief: Name of filter mask topic + */ + std::string mask_topic_; + +private: + /** + * @brief: Latest robot position + */ + geometry_msgs::msg::Pose2D latest_pose_; + + /** + * @brief: Mutex for locking filter's resources + */ + mutex_t * access_; +}; + +} // namespace nav2_costmap_2d + +#endif // NAV2_COSTMAP_2D__COSTMAP_FILTER_HPP_ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp new file mode 100644 index 00000000000..07cc73ea91b --- /dev/null +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp @@ -0,0 +1,82 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2020 Samsung Research Russia + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Alexey Merzlyakov + *********************************************************************/ + +#ifndef NAV2_COSTMAP_2D__KEEPOUT_FILTER_HPP_ +#define NAV2_COSTMAP_2D__KEEPOUT_FILTER_HPP_ + +#include "nav2_costmap_2d/costmap_filters/costmap_filter.hpp" + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "nav2_msgs/msg/costmap_filter_info.hpp" + +namespace nav2_costmap_2d +{ + +class KeepoutFilter : public CostmapFilter +{ +public: + KeepoutFilter(); + + void initializeFilter( + const std::string & filter_info_topic); + + void process( + nav2_costmap_2d::Costmap2D & master_grid, + int min_i, int min_j, int max_i, int max_j, + const geometry_msgs::msg::Pose2D & pose); + + void resetFilter(); + + bool isActive(); + +private: + void filterInfoCallback(const nav2_msgs::msg::CostmapFilterInfo::SharedPtr msg); + void maskCallback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg); + + rclcpp::Subscription::SharedPtr filter_info_sub_; + rclcpp::Subscription::SharedPtr mask_sub_; + + std::unique_ptr mask_costmap_; +}; + +} // namespace nav2_costmap_2d + +#endif // NAV2_COSTMAP_2D__KEEPOUT_FILTER_HPP_ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp index d124eaf04c1..0873d8781ec 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp @@ -30,17 +30,11 @@ class CostmapSubscriber { public: CostmapSubscriber( - nav2_util::LifecycleNode::SharedPtr node, + const nav2_util::LifecycleNode::WeakPtr & parent, const std::string & topic_name); CostmapSubscriber( - rclcpp::Node::SharedPtr node, - const std::string & topic_name); - - CostmapSubscriber( - const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, - const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics, - const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + const rclcpp::Node::WeakPtr & parent, const std::string & topic_name); ~CostmapSubscriber() {} @@ -48,11 +42,6 @@ class CostmapSubscriber std::shared_ptr getCostmap(); protected: - // Interfaces used for logging and creating publishers and subscribers - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; - rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_; - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_; - void toCostmap2D(); void costmapCallback(const nav2_msgs::msg::Costmap::SharedPtr msg); diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/footprint_subscriber.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/footprint_subscriber.hpp index acc3e7c2347..b6eef0c043e 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/footprint_subscriber.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/footprint_subscriber.hpp @@ -29,20 +29,12 @@ class FootprintSubscriber { public: FootprintSubscriber( - nav2_util::LifecycleNode::SharedPtr node, + const nav2_util::LifecycleNode::WeakPtr & parent, const std::string & topic_name, const double & footprint_timeout); FootprintSubscriber( - rclcpp::Node::SharedPtr node, - const std::string & topic_name, - const double & footprint_timeout); - - FootprintSubscriber( - const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, - const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics, - const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, - const rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock, + const rclcpp::Node::WeakPtr & parent, const std::string & topic_name, const double & footprint_timeout); @@ -61,11 +53,7 @@ class FootprintSubscriber rclcpp::Time & stamp, rclcpp::Duration valid_footprint_timeout); protected: - // Interfaces used for logging and creating publishers and subscribers - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; - rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_; - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_; - rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_; + rclcpp::Clock::SharedPtr clock_; void footprint_callback(const geometry_msgs::msg::PolygonStamped::SharedPtr msg); diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp index b0eca7251a1..4f7aa00ac0c 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp @@ -62,7 +62,7 @@ class Layer // TODO(mjeronimo): public nav2_util::LifecycleHelperInterface LayeredCostmap * parent, std::string name, tf2_ros::Buffer * tf, - nav2_util::LifecycleNode::SharedPtr node, + const nav2_util::LifecycleNode::WeakPtr & node, rclcpp::Node::SharedPtr client_node, rclcpp::Node::SharedPtr rclcpp_node); virtual void deactivate() {} /** @brief Stop publishers. */ @@ -123,7 +123,11 @@ class Layer // TODO(mjeronimo): public nav2_util::LifecycleHelperInterface const std::vector & getFootprint() const; /** @brief Convenience functions for declaring ROS parameters */ - void declareParameter(const std::string & param_name, const rclcpp::ParameterValue & value); + void declareParameter( + const std::string & param_name, + const rclcpp::ParameterValue & value); + void declareParameter( + const std::string & param_name); bool hasParameter(const std::string & param_name); void undeclareAllParameters(); std::string getFullName(const std::string & param_name); @@ -132,9 +136,11 @@ class Layer // TODO(mjeronimo): public nav2_util::LifecycleHelperInterface LayeredCostmap * layered_costmap_; std::string name_; tf2_ros::Buffer * tf_; - nav2_util::LifecycleNode::SharedPtr node_; rclcpp::Node::SharedPtr client_node_; rclcpp::Node::SharedPtr rclcpp_node_; + rclcpp_lifecycle::LifecycleNode::WeakPtr node_; + rclcpp::Clock::SharedPtr clock_; + rclcpp::Logger logger_{rclcpp::get_logger("nav2_costmap_2d")}; /** @brief This is called at the end of initialize(). Override to * implement subclass-specific initialization. diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/observation_buffer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/observation_buffer.hpp index 4c816dad28f..30b03f33161 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/observation_buffer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/observation_buffer.hpp @@ -73,7 +73,7 @@ class ObservationBuffer * @param tf_tolerance The amount of time to wait for a transform to be available when setting a new global frame */ ObservationBuffer( - nav2_util::LifecycleNode::SharedPtr nh, + const nav2_util::LifecycleNode::WeakPtr & parent, std::string topic_name, double observation_keep_time, double expected_update_rate, @@ -142,10 +142,11 @@ class ObservationBuffer */ void purgeStaleObservations(); + rclcpp::Clock::SharedPtr clock_; + rclcpp::Logger logger_{rclcpp::get_logger("nav2_costmap_2d")}; tf2_ros::Buffer & tf2_buffer_; const rclcpp::Duration observation_keep_time_; const rclcpp::Duration expected_update_rate_; - nav2_util::LifecycleNode::SharedPtr nh_; rclcpp::Time last_updated_; std::string global_frame_; std::string sensor_frame_; diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml index 18c5da7dc86..cbd4b2dbf76 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -19,7 +19,6 @@ ament_cmake nav2_common - angles geometry_msgs laser_geometry map_msgs @@ -38,6 +37,7 @@ tf2_ros tf2_sensor_msgs visualization_msgs + angles ament_lint_common ament_lint_auto diff --git a/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp new file mode 100644 index 00000000000..dec1437f33f --- /dev/null +++ b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp @@ -0,0 +1,117 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2020 Samsung Research Russia + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Alexey Merzlyakov + *********************************************************************/ + +#include "nav2_costmap_2d/costmap_filters/costmap_filter.hpp" + +#include + +namespace nav2_costmap_2d +{ + +CostmapFilter::CostmapFilter() +: filter_info_topic_(""), mask_topic_("") +{ + access_ = new mutex_t(); +} + +CostmapFilter::~CostmapFilter() +{ + delete access_; +} + +void CostmapFilter::onInitialize() +{ + rclcpp_lifecycle::LifecycleNode::SharedPtr node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + // Declare parameters + declareParameter("enabled", rclcpp::ParameterValue(true)); + declareParameter("filter_info_topic"); + + // Get parameters + node->get_parameter(name_ + "." + "enabled", enabled_); + try { + filter_info_topic_ = node->get_parameter(name_ + "." + "filter_info_topic").as_string(); + } catch (rclcpp::exceptions::ParameterNotDeclaredException & ex) { + RCLCPP_ERROR(node->get_logger(), "filter_info_topic parameter is not set"); + throw ex; + } +} + +void CostmapFilter::activate() +{ + initializeFilter(filter_info_topic_); +} + +void CostmapFilter::deactivate() +{ + resetFilter(); +} + +void CostmapFilter::reset() +{ + resetFilter(); + initializeFilter(filter_info_topic_); +} + +void CostmapFilter::updateBounds( + double robot_x, double robot_y, double robot_yaw, + double * /*min_x*/, double * /*min_y*/, double * /*max_x*/, double * /*max_y*/) +{ + if (!enabled_) { + return; + } + + latest_pose_.x = robot_x; + latest_pose_.y = robot_y; + latest_pose_.theta = robot_yaw; +} + +void CostmapFilter::updateCosts( + nav2_costmap_2d::Costmap2D & master_grid, + int min_i, int min_j, int max_i, int max_j) +{ + if (!enabled_) { + return; + } + + process(master_grid, min_i, min_j, max_i, max_j, latest_pose_); +} + +} // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp new file mode 100644 index 00000000000..b3a23ebc5dc --- /dev/null +++ b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp @@ -0,0 +1,257 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2020 Samsung Research Russia + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Alexey Merzlyakov + *********************************************************************/ + +#include "nav2_costmap_2d/costmap_filters/keepout_filter.hpp" + +namespace nav2_costmap_2d +{ + +KeepoutFilter::KeepoutFilter() +: filter_info_sub_(nullptr), mask_sub_(nullptr), mask_costmap_(nullptr) +{ +} + +void KeepoutFilter::initializeFilter( + const std::string & filter_info_topic) +{ + std::lock_guard guard(*getMutex()); + + rclcpp_lifecycle::LifecycleNode::SharedPtr node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + filter_info_topic_ = filter_info_topic; + // Setting new costmap filter info subscriber + RCLCPP_INFO( + node->get_logger(), + "Subscribing to \"%s\" topic for filter info...", + filter_info_topic.c_str()); + filter_info_sub_ = node->create_subscription( + filter_info_topic, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), + std::bind(&KeepoutFilter::filterInfoCallback, this, std::placeholders::_1)); +} + +void KeepoutFilter::filterInfoCallback( + const nav2_msgs::msg::CostmapFilterInfo::SharedPtr msg) +{ + std::lock_guard guard(*getMutex()); + + rclcpp_lifecycle::LifecycleNode::SharedPtr node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + // Resetting previous subscriber each time when new costmap filter information arrives + if (mask_sub_) { + RCLCPP_WARN( + node->get_logger(), + "New costmap filter info arrived from %s topic. Updating old filter info.", + filter_info_topic_.c_str()); + mask_sub_.reset(); + } + + // Checking that base and multiplier are set to their default values + if (msg->base != BASE_DEFAULT or msg->multiplier != MULTIPLIER_DEFAULT) { + RCLCPP_ERROR( + node->get_logger(), + "For proper use of keepout filter base and multiplier in CostmapFilterInfo message " + "should be set to their default values (%f and %f)", + BASE_DEFAULT, MULTIPLIER_DEFAULT); + } + + mask_topic_ = msg->filter_mask_topic; + + // Setting new filter mask subscriber + RCLCPP_INFO( + node->get_logger(), + "Subscribing to \"%s\" topic for filter mask...", + msg->filter_mask_topic.c_str()); + mask_sub_ = node->create_subscription( + mask_topic_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), + std::bind(&KeepoutFilter::maskCallback, this, std::placeholders::_1)); +} + +void KeepoutFilter::maskCallback( + const nav_msgs::msg::OccupancyGrid::SharedPtr msg) +{ + std::lock_guard guard(*getMutex()); + + rclcpp_lifecycle::LifecycleNode::SharedPtr node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + if (mask_costmap_) { + RCLCPP_WARN( + node->get_logger(), + "New filter mask arrived from %s topic. Updating old filter mask.", + mask_topic_.c_str()); + mask_costmap_.reset(); + } + + // Making a new mask_costmap_ + mask_costmap_ = std::make_unique(*msg); +} + +void KeepoutFilter::process( + nav2_costmap_2d::Costmap2D & master_grid, + int min_i, int min_j, int max_i, int max_j, + const geometry_msgs::msg::Pose2D & /*pose*/) +{ + std::lock_guard guard(*getMutex()); + + rclcpp_lifecycle::LifecycleNode::SharedPtr node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + if (!mask_costmap_) { + // Show warning message every 2 seconds to not litter an output + RCLCPP_WARN_THROTTLE( + node->get_logger(), *(node->get_clock()), 2000, + "Filter mask was not received"); + return; + } + + double wx, wy; // world coordinates + + // Optimization: iterate only in overlapped + // (min_i, min_j)..(max_i, max_j) & mask_costmap_ area. + // + // mask_costmap_ + // *----------------------------* + // | | + // | | + // | (2) | + // *-----+-------* | + // | |///////|<- overlapped area | + // | |///////| to iterate in | + // | *-------+--------------------* + // | (1) | + // | | + // *-------------* + // master_grid (min_i, min_j)..(max_i, max_j) window + // + // ToDo: after costmap rotation will be added, this should be re-worked. + + // Calculating bounds corresponding to bottom-left overlapping (1) corner + int mg_min_x, mg_min_y; // masger_grid indexes of bottom-left (1) corner + // mask_costmap_ -> master_grid intexes conversion + const double half_cell_size = 0.5 * mask_costmap_->getResolution(); + wx = mask_costmap_->getOriginX() + half_cell_size; + wy = mask_costmap_->getOriginY() + half_cell_size; + master_grid.worldToMapNoBounds(wx, wy, mg_min_x, mg_min_y); + // Calculation of (1) corner bounds + if (mg_min_x >= max_i || mg_min_y >= max_j) { + // There is no overlapping. Do nothing. + return; + } + mg_min_x = std::max(min_i, mg_min_x); + mg_min_y = std::max(min_j, mg_min_y); + + // Calculating bounds corresponding to top-right window (2) corner + int mg_max_x, mg_max_y; // masger_grid indexes of top-right (2) corner + // mask_costmap_ -> master_grid intexes conversion + wx = mask_costmap_->getOriginX() + + mask_costmap_->getSizeInCellsX() * mask_costmap_->getResolution() + half_cell_size; + wy = mask_costmap_->getOriginY() + + mask_costmap_->getSizeInCellsY() * mask_costmap_->getResolution() + half_cell_size; + master_grid.worldToMapNoBounds(wx, wy, mg_max_x, mg_max_y); + // Calculation of (2) corner bounds + if (mg_max_x <= min_i || mg_max_y <= min_j) { + // There is no overlapping. Do nothing. + return; + } + mg_max_x = std::min(max_i, mg_max_x); + mg_max_y = std::min(max_j, mg_max_y); + + // unsigned<-signed conversions. + unsigned const int mg_min_x_u = static_cast(mg_min_x); + unsigned const int mg_min_y_u = static_cast(mg_min_y); + unsigned const int mg_max_x_u = static_cast(mg_max_x); + unsigned const int mg_max_y_u = static_cast(mg_max_y); + + unsigned int i, j; // master_grid iterators + unsigned int index; // corresponding index of master_grid + unsigned int mx, my; // mask_costmap_ coordinates + unsigned char data, old_data; // master_grid element data + + // Main master_grid updating loop + // Iterate in overlapped window by master_grid indexes + unsigned char * master_array = master_grid.getCharMap(); + for (i = mg_min_x_u; i < mg_max_x_u; i++) { + for (j = mg_min_y_u; j < mg_max_y_u; j++) { + index = master_grid.getIndex(i, j); + old_data = master_array[index]; + // Calculating corresponding to (i, j) point at mask_costmap_ + master_grid.mapToWorld(i, j, wx, wy); + if (mask_costmap_->worldToMap(wx, wy, mx, my)) { + data = mask_costmap_->getCost(mx, my); + // Update if mask_ data is valid and greater than existing master_grid's one + if (data == NO_INFORMATION) { + continue; + } + if (data > old_data || old_data == NO_INFORMATION) { + master_array[index] = data; + } + } + } + } +} + +void KeepoutFilter::resetFilter() +{ + std::lock_guard guard(*getMutex()); + + filter_info_sub_.reset(); + mask_sub_.reset(); +} + +bool KeepoutFilter::isActive() +{ + std::lock_guard guard(*getMutex()); + if (mask_costmap_) { + return true; + } + return false; +} + +} // namespace nav2_costmap_2d + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::KeepoutFilter, nav2_costmap_2d::Layer) diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp index 4316da9ecb9..0d7d82aa6bb 100644 --- a/nav2_costmap_2d/plugins/inflation_layer.cpp +++ b/nav2_costmap_2d/plugins/inflation_layer.cpp @@ -65,6 +65,7 @@ InflationLayer::InflationLayer() inflate_around_unknown_(false), cell_inflation_radius_(0), cached_cell_inflation_radius_(0), + resolution_(0), cache_length_(0), last_min_x_(std::numeric_limits::lowest()), last_min_y_(std::numeric_limits::lowest()), @@ -88,11 +89,17 @@ InflationLayer::onInitialize() declareParameter("inflate_unknown", rclcpp::ParameterValue(false)); declareParameter("inflate_around_unknown", rclcpp::ParameterValue(false)); - node_->get_parameter(name_ + "." + "enabled", enabled_); - node_->get_parameter(name_ + "." + "inflation_radius", inflation_radius_); - node_->get_parameter(name_ + "." + "cost_scaling_factor", cost_scaling_factor_); - node_->get_parameter(name_ + "." + "inflate_unknown", inflate_unknown_); - node_->get_parameter(name_ + "." + "inflate_around_unknown", inflate_around_unknown_); + { + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + node->get_parameter(name_ + "." + "enabled", enabled_); + node->get_parameter(name_ + "." + "inflation_radius", inflation_radius_); + node->get_parameter(name_ + "." + "cost_scaling_factor", cost_scaling_factor_); + node->get_parameter(name_ + "." + "inflate_unknown", inflate_unknown_); + node->get_parameter(name_ + "." + "inflate_around_unknown", inflate_around_unknown_); + } current_ = true; seen_.clear(); @@ -154,8 +161,7 @@ InflationLayer::onFootprintChanged() need_reinflation_ = true; RCLCPP_DEBUG( - rclcpp::get_logger( - "nav2_costmap_2d"), "InflationLayer::onFootprintChanged(): num footprint points: %lu," + logger_, "InflationLayer::onFootprintChanged(): num footprint points: %lu," " inscribed_radius_ = %.3f, inflation_radius_ = %.3f", layered_costmap_->getFootprint().size(), inscribed_radius_, inflation_radius_); } @@ -174,7 +180,7 @@ InflationLayer::updateCosts( // make sure the inflation list is empty at the beginning of the cycle (should always be true) for (auto & dist : inflation_cells_) { RCLCPP_FATAL_EXPRESSION( - rclcpp::get_logger("nav2_costmap_2d"), + logger_, !dist.empty(), "The inflation list must be empty at the beginning of inflation"); } @@ -183,8 +189,7 @@ InflationLayer::updateCosts( if (seen_.size() != size_x * size_y) { RCLCPP_WARN( - rclcpp::get_logger( - "nav2_costmap_2d"), "InflationLayer::updateCosts(): seen_ vector size is wrong"); + logger_, "InflationLayer::updateCosts(): seen_ vector size is wrong"); seen_ = std::vector(size_x * size_y, false); } diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index 7c911563578..c25097b3048 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -76,22 +76,27 @@ void ObstacleLayer::onInitialize() // TODO(mjeronimo): these four are candidates for dynamic update declareParameter("enabled", rclcpp::ParameterValue(true)); - declareParameter( - "footprint_clearing_enabled", - rclcpp::ParameterValue(true)); + declareParameter("footprint_clearing_enabled", rclcpp::ParameterValue(true)); declareParameter("max_obstacle_height", rclcpp::ParameterValue(2.0)); declareParameter("combination_method", rclcpp::ParameterValue(1)); declareParameter("observation_sources", rclcpp::ParameterValue(std::string(""))); - node_->get_parameter(name_ + "." + "enabled", enabled_); - node_->get_parameter(name_ + "." + "footprint_clearing_enabled", footprint_clearing_enabled_); - node_->get_parameter(name_ + "." + "max_obstacle_height", max_obstacle_height_); - node_->get_parameter(name_ + "." + "combination_method", combination_method_); - node_->get_parameter("track_unknown_space", track_unknown_space); - node_->get_parameter("transform_tolerance", transform_tolerance); - node_->get_parameter(name_ + "." + "observation_sources", topics_string); + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + node->get_parameter(name_ + "." + "enabled", enabled_); + node->get_parameter(name_ + "." + "footprint_clearing_enabled", footprint_clearing_enabled_); + node->get_parameter(name_ + "." + "max_obstacle_height", max_obstacle_height_); + node->get_parameter(name_ + "." + "combination_method", combination_method_); + node->get_parameter("track_unknown_space", track_unknown_space); + node->get_parameter("transform_tolerance", transform_tolerance); + node->get_parameter(name_ + "." + "observation_sources", topics_string); - RCLCPP_INFO(node_->get_logger(), "Subscribed to Topics: %s", topics_string.c_str()); + RCLCPP_INFO( + logger_, + "Subscribed to Topics: %s", topics_string.c_str()); rolling_window_ = layered_costmap_->isRolling(); @@ -129,24 +134,24 @@ void ObstacleLayer::onInitialize() declareParameter(source + "." + "obstacle_range", rclcpp::ParameterValue(2.5)); declareParameter(source + "." + "raytrace_range", rclcpp::ParameterValue(3.0)); - node_->get_parameter(name_ + "." + source + "." + "topic", topic); - node_->get_parameter(name_ + "." + source + "." + "sensor_frame", sensor_frame); - node_->get_parameter( + node->get_parameter(name_ + "." + source + "." + "topic", topic); + node->get_parameter(name_ + "." + source + "." + "sensor_frame", sensor_frame); + node->get_parameter( name_ + "." + source + "." + "observation_persistence", observation_keep_time); - node_->get_parameter( + node->get_parameter( name_ + "." + source + "." + "expected_update_rate", expected_update_rate); - node_->get_parameter(name_ + "." + source + "." + "data_type", data_type); - node_->get_parameter(name_ + "." + source + "." + "min_obstacle_height", min_obstacle_height); - node_->get_parameter(name_ + "." + source + "." + "max_obstacle_height", max_obstacle_height); - node_->get_parameter(name_ + "." + source + "." + "inf_is_valid", inf_is_valid); - node_->get_parameter(name_ + "." + source + "." + "marking", marking); - node_->get_parameter(name_ + "." + source + "." + "clearing", clearing); + node->get_parameter(name_ + "." + source + "." + "data_type", data_type); + node->get_parameter(name_ + "." + source + "." + "min_obstacle_height", min_obstacle_height); + node->get_parameter(name_ + "." + source + "." + "max_obstacle_height", max_obstacle_height); + node->get_parameter(name_ + "." + source + "." + "inf_is_valid", inf_is_valid); + node->get_parameter(name_ + "." + source + "." + "marking", marking); + node->get_parameter(name_ + "." + source + "." + "clearing", clearing); if (!(data_type == "PointCloud2" || data_type == "LaserScan")) { RCLCPP_FATAL( - node_->get_logger(), + logger_, "Only topics that use point cloud2s or laser scans are currently supported"); throw std::runtime_error( "Only topics that use point cloud2s or laser scans are currently supported"); @@ -154,14 +159,14 @@ void ObstacleLayer::onInitialize() // get the obstacle range for the sensor double obstacle_range; - node_->get_parameter(name_ + "." + source + "." + "obstacle_range", obstacle_range); + node->get_parameter(name_ + "." + source + "." + "obstacle_range", obstacle_range); // get the raytrace range for the sensor double raytrace_range; - node_->get_parameter(name_ + "." + source + "." + "raytrace_range", raytrace_range); + node->get_parameter(name_ + "." + source + "." + "raytrace_range", raytrace_range); RCLCPP_DEBUG( - node_->get_logger(), + logger_, "Creating an observation buffer for source %s, topic %s, frame %s", source.c_str(), topic.c_str(), sensor_frame.c_str()); @@ -171,7 +176,7 @@ void ObstacleLayer::onInitialize() std::shared_ptr( new ObservationBuffer( - node_, topic, observation_keep_time, expected_update_rate, + node, topic, observation_keep_time, expected_update_rate, min_obstacle_height, max_obstacle_height, obstacle_range, raytrace_range, *tf_, global_frame_, sensor_frame, transform_tolerance))); @@ -187,7 +192,7 @@ void ObstacleLayer::onInitialize() } RCLCPP_DEBUG( - node_->get_logger(), + logger_, "Created an observation buffer for source %s, topic %s, global frame: %s, " "expected update rate: %.2f, observation persistence: %.2f", source.c_str(), topic.c_str(), @@ -233,7 +238,7 @@ void ObstacleLayer::onInitialize() if (inf_is_valid) { RCLCPP_WARN( - node_->get_logger(), + logger_, "obstacle_layer: inf_is_valid option is not applicable to PointCloud observations."); } @@ -273,7 +278,7 @@ ObstacleLayer::laserScanCallback( projector_.transformLaserScanToPointCloud(message->header.frame_id, *message, cloud, *tf_); } catch (tf2::TransformException & ex) { RCLCPP_WARN( - node_->get_logger(), + logger_, "High fidelity enabled, but TF returned a transform exception to frame %s: %s", global_frame_.c_str(), ex.what()); @@ -310,7 +315,7 @@ ObstacleLayer::laserScanValidInfCallback( projector_.transformLaserScanToPointCloud(message.header.frame_id, message, cloud, *tf_); } catch (tf2::TransformException & ex) { RCLCPP_WARN( - node_->get_logger(), + logger_, "High fidelity enabled, but TF returned a transform exception to frame %s: %s", global_frame_.c_str(), ex.what()); projector_.projectLaser(message, cloud); @@ -382,7 +387,7 @@ ObstacleLayer::updateBounds( // if the obstacle is too high or too far away from the robot we won't add it if (pz > max_obstacle_height_) { - RCLCPP_DEBUG(node_->get_logger(), "The point is too high"); + RCLCPP_DEBUG(logger_, "The point is too high"); continue; } @@ -394,14 +399,14 @@ ObstacleLayer::updateBounds( // if the point is far enough away... we won't consider it if (sq_dist >= sq_obstacle_range) { - RCLCPP_DEBUG(node_->get_logger(), "The point is too far away"); + RCLCPP_DEBUG(logger_, "The point is too far away"); continue; } // now we need to compute the map coordinates for the observation unsigned int mx, my; if (!worldToMap(px, py, mx, my)) { - RCLCPP_DEBUG(node_->get_logger(), "Computing map coords failed"); + RCLCPP_DEBUG(logger_, "Computing map coords failed"); continue; } @@ -528,7 +533,7 @@ ObstacleLayer::raytraceFreespace( unsigned int x0, y0; if (!worldToMap(ox, oy, x0, y0)) { RCLCPP_WARN( - node_->get_logger(), + logger_, "Sensor origin at (%.2f, %.2f) is out of map bounds. The costmap cannot raytrace for it.", ox, oy); return; diff --git a/nav2_costmap_2d/plugins/range_sensor_layer.cpp b/nav2_costmap_2d/plugins/range_sensor_layer.cpp index 8a6124a47a9..3d0f728bfb7 100644 --- a/nav2_costmap_2d/plugins/range_sensor_layer.cpp +++ b/nav2_costmap_2d/plugins/range_sensor_layer.cpp @@ -61,45 +61,50 @@ void RangeSensorLayer::onInitialize() { current_ = true; buffered_readings_ = 0; - last_reading_time_ = node_->now(); + last_reading_time_ = clock_->now(); default_value_ = to_cost(0.5); matchSize(); resetRange(); + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + declareParameter("enabled", rclcpp::ParameterValue(true)); - node_->get_parameter(name_ + "." + "enabled", enabled_); + node->get_parameter(name_ + "." + "enabled", enabled_); declareParameter("phi", rclcpp::ParameterValue(1.2)); - node_->get_parameter(name_ + "." + "phi", phi_v_); + node->get_parameter(name_ + "." + "phi", phi_v_); declareParameter("inflate_cone", rclcpp::ParameterValue(1.0)); - node_->get_parameter(name_ + "." + "phi", phi_v_); + node->get_parameter(name_ + "." + "inflate_cone", inflate_cone_); declareParameter("no_readings_timeout", rclcpp::ParameterValue(0.0)); - node_->get_parameter(name_ + "." + "no_readings_timeout", no_readings_timeout_); + node->get_parameter(name_ + "." + "no_readings_timeout", no_readings_timeout_); declareParameter("clear_threshold", rclcpp::ParameterValue(0.2)); - node_->get_parameter(name_ + "." + "clear_threshold", clear_threshold_); + node->get_parameter(name_ + "." + "clear_threshold", clear_threshold_); declareParameter("mark_threshold", rclcpp::ParameterValue(0.8)); - node_->get_parameter(name_ + "." + "mark_threshold", mark_threshold_); + node->get_parameter(name_ + "." + "mark_threshold", mark_threshold_); declareParameter("clear_on_max_reading", rclcpp::ParameterValue(false)); - node_->get_parameter(name_ + "." + "clear_on_max_reading", clear_on_max_reading_); + node->get_parameter(name_ + "." + "clear_on_max_reading", clear_on_max_reading_); double temp_tf_tol = 0.0; - node_->get_parameter("transform_tolerance", temp_tf_tol); + node->get_parameter("transform_tolerance", temp_tf_tol); transform_tolerance_ = tf2::durationFromSec(temp_tf_tol); std::vector topic_names{}; declareParameter("topics", rclcpp::ParameterValue(topic_names)); - node_->get_parameter(name_ + "." + "topics", topic_names); + node->get_parameter(name_ + "." + "topics", topic_names); InputSensorType input_sensor_type = InputSensorType::ALL; std::string sensor_type_name; declareParameter("input_sensor_type", rclcpp::ParameterValue("ALL")); - node_->get_parameter(name_ + "." + "input_sensor_type", sensor_type_name); + node->get_parameter(name_ + "." + "input_sensor_type", sensor_type_name); std::transform( sensor_type_name.begin(), sensor_type_name.end(), sensor_type_name.begin(), ::toupper); RCLCPP_INFO( - node_->get_logger(), "%s: %s as input_sensor_type given", + logger_, "%s: %s as input_sensor_type given", name_.c_str(), sensor_type_name.c_str()); if (sensor_type_name == "VARIABLE") { @@ -110,14 +115,14 @@ void RangeSensorLayer::onInitialize() input_sensor_type = InputSensorType::ALL; } else { RCLCPP_ERROR( - node_->get_logger(), "%s: Invalid input sensor type: %s. Defaulting to ALL.", + logger_, "%s: Invalid input sensor type: %s. Defaulting to ALL.", name_.c_str(), sensor_type_name.c_str()); } // Validate topic names list: it must be a (normally non-empty) list of strings if (topic_names.empty()) { RCLCPP_FATAL( - node_->get_logger(), "Invalid topic names list: it must" + logger_, "Invalid topic names list: it must" "be a non-empty list of strings"); return; } @@ -138,19 +143,19 @@ void RangeSensorLayer::onInitialize() std::placeholders::_1); } else { RCLCPP_ERROR( - node_->get_logger(), + logger_, "%s: Invalid input sensor type: %s. Did you make a new type" "and forgot to choose the subscriber for it?", name_.c_str(), sensor_type_name.c_str()); } range_subs_.push_back( - node_->create_subscription( + node->create_subscription( topic_name, rclcpp::SensorDataQoS(), std::bind( &RangeSensorLayer::bufferIncomingRangeMsg, this, std::placeholders::_1))); RCLCPP_INFO( - node_->get_logger(), "RangeSensorLayer: subscribed to " + logger_, "RangeSensorLayer: subscribed to " "topic %s", range_subs_.back()->get_topic_name()); } global_frame_ = layered_costmap_->getGlobalFrameID(); @@ -238,7 +243,7 @@ void RangeSensorLayer::processFixedRangeMsg(sensor_msgs::msg::Range & range_mess { if (!std::isinf(range_message.range)) { RCLCPP_ERROR( - node_->get_logger(), + logger_, "Fixed distance ranger (min_range == max_range) in frame %s sent invalid value. " "Only -Inf (== object detected) and Inf (== no object detected) are valid.", range_message.header.frame_id.c_str()); @@ -290,7 +295,7 @@ void RangeSensorLayer::updateCostmap( in.header.frame_id, global_frame_, tf2_ros::fromMsg(in.header.stamp))) { RCLCPP_INFO( - node_->get_logger(), "Range sensor layer can't transform from %s to %s", + logger_, "Range sensor layer can't transform from %s to %s", global_frame_.c_str(), in.header.frame_id.c_str()); return; } @@ -387,7 +392,7 @@ void RangeSensorLayer::updateCostmap( } buffered_readings_++; - last_reading_time_ = node_->now(); + last_reading_time_ = clock_->now(); } void RangeSensorLayer::update_cell( @@ -409,8 +414,12 @@ void RangeSensorLayer::update_cell( double prob_not = (1 - sensor) * (1 - prior); double new_prob = prob_occ / (prob_occ + prob_not); - RCLCPP_DEBUG(node_->get_logger(), "%f %f | %f %f = %f", dx, dy, theta, phi, sensor); - RCLCPP_DEBUG(node_->get_logger(), "%f | %f %f | %f", prior, prob_occ, prob_not, new_prob); + RCLCPP_DEBUG( + logger_, + "%f %f | %f %f = %f", dx, dy, theta, phi, sensor); + RCLCPP_DEBUG( + logger_, + "%f | %f %f | %f", prior, prob_occ, prob_not, new_prob); unsigned char c = to_cost(new_prob); setCost(x, y, c); } @@ -448,12 +457,13 @@ void RangeSensorLayer::updateBounds( if (buffered_readings_ == 0) { if (no_readings_timeout_ > 0.0 && - (node_->now() - last_reading_time_).seconds() > no_readings_timeout_) + (clock_->now() - last_reading_time_).seconds() > + no_readings_timeout_) { RCLCPP_WARN( - node_->get_logger(), "No range readings received for %.2f seconds, " - "while expected at least every %.2f seconds.", - (node_->now() - last_reading_time_).seconds(), + logger_, + "No range readings received for %.2f seconds, while expected at least every %.2f seconds.", + (clock_->now() - last_reading_time_).seconds(), no_readings_timeout_); current_ = false; } @@ -504,7 +514,7 @@ void RangeSensorLayer::updateCosts( void RangeSensorLayer::reset() { - RCLCPP_DEBUG(node_->get_logger(), "Reseting range sensor layer..."); + RCLCPP_DEBUG(logger_, "Reseting range sensor layer..."); deactivate(); resetMaps(); current_ = true; diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index cb3a01f1be4..e503126a4c2 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -40,10 +40,8 @@ #include "nav2_costmap_2d/static_layer.hpp" #include -#include #include -#include "nav2_costmap_2d/costmap_math.hpp" #include "pluginlib/class_list_macros.hpp" #include "tf2/convert.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.h" @@ -81,17 +79,23 @@ StaticLayer::onInitialize() } RCLCPP_INFO( - node_->get_logger(), + logger_, "Subscribing to the map topic (%s) with %s durability", map_topic_.c_str(), map_subscribe_transient_local_ ? "transient local" : "volatile"); - map_sub_ = node_->create_subscription( + + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + map_sub_ = node->create_subscription( map_topic_, map_qos, std::bind(&StaticLayer::incomingMap, this, std::placeholders::_1)); if (subscribe_to_updates_) { - RCLCPP_INFO(node_->get_logger(), "Subscribing to updates"); - map_update_sub_ = node_->create_subscription( + RCLCPP_INFO(logger_, "Subscribing to updates"); + map_update_sub_ = node->create_subscription( map_topic_ + "_updates", rclcpp::SystemDefaultsQoS(), std::bind(&StaticLayer::incomingUpdate, this, std::placeholders::_1)); @@ -126,25 +130,30 @@ StaticLayer::getParameters() declareParameter("transform_tolerance", rclcpp::ParameterValue(0.0)); declareParameter("map_topic", rclcpp::ParameterValue("")); - node_->get_parameter(name_ + "." + "enabled", enabled_); - node_->get_parameter(name_ + "." + "subscribe_to_updates", subscribe_to_updates_); + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + node->get_parameter(name_ + "." + "enabled", enabled_); + node->get_parameter(name_ + "." + "subscribe_to_updates", subscribe_to_updates_); std::string private_map_topic, global_map_topic; - node_->get_parameter(name_ + "." + "map_topic", private_map_topic); - node_->get_parameter("map_topic", global_map_topic); + node->get_parameter(name_ + "." + "map_topic", private_map_topic); + node->get_parameter("map_topic", global_map_topic); if (!private_map_topic.empty()) { map_topic_ = private_map_topic; } else { map_topic_ = global_map_topic; } - node_->get_parameter( + node->get_parameter( name_ + "." + "map_subscribe_transient_local", map_subscribe_transient_local_); - node_->get_parameter("track_unknown_space", track_unknown_space_); - node_->get_parameter("use_maximum", use_maximum_); - node_->get_parameter("lethal_cost_threshold", temp_lethal_threshold); - node_->get_parameter("unknown_cost_value", unknown_cost_value_); - node_->get_parameter("trinary_costmap", trinary_costmap_); - node_->get_parameter("transform_tolerance", temp_tf_tol); + node->get_parameter("track_unknown_space", track_unknown_space_); + node->get_parameter("use_maximum", use_maximum_); + node->get_parameter("lethal_cost_threshold", temp_lethal_threshold); + node->get_parameter("unknown_cost_value", unknown_cost_value_); + node->get_parameter("trinary_costmap", trinary_costmap_); + node->get_parameter("transform_tolerance", temp_tf_tol); // Enforce bounds lethal_threshold_ = std::max(std::min(temp_lethal_threshold, 100), 0); @@ -157,13 +166,13 @@ StaticLayer::getParameters() void StaticLayer::processMap(const nav_msgs::msg::OccupancyGrid & new_map) { - RCLCPP_DEBUG(node_->get_logger(), "StaticLayer: Process map"); + RCLCPP_DEBUG(logger_, "StaticLayer: Process map"); unsigned int size_x = new_map.info.width; unsigned int size_y = new_map.info.height; RCLCPP_DEBUG( - node_->get_logger(), + logger_, "StaticLayer: Received a %d X %d map at %f m/pix", size_x, size_y, new_map.info.resolution); @@ -178,7 +187,7 @@ StaticLayer::processMap(const nav_msgs::msg::OccupancyGrid & new_map) { // Update the size of the layered costmap (and all layers, including this one) RCLCPP_INFO( - node_->get_logger(), + logger_, "StaticLayer: Resizing costmap to %d X %d at %f m/pix", size_x, size_y, new_map.info.resolution); layered_costmap_->resizeMap( @@ -193,7 +202,7 @@ StaticLayer::processMap(const nav_msgs::msg::OccupancyGrid & new_map) { // only update the size of the costmap stored locally in this layer RCLCPP_INFO( - node_->get_logger(), + logger_, "StaticLayer: Resizing static layer to %d X %d at %f m/pix", size_x, size_y, new_map.info.resolution); resizeMap( @@ -282,7 +291,7 @@ StaticLayer::incomingUpdate(map_msgs::msg::OccupancyGridUpdate::ConstSharedPtr u x_ + width_ < update->x + update->width) { RCLCPP_WARN( - node_->get_logger(), + logger_, "StaticLayer: Map update ignored. Exceeds bounds of static layer.\n" "Static layer origin: %d, %d bounds: %d X %d\n" "Update origin: %d, %d bounds: %d X %d", @@ -293,7 +302,7 @@ StaticLayer::incomingUpdate(map_msgs::msg::OccupancyGridUpdate::ConstSharedPtr u if (update->header.frame_id != map_frame_) { RCLCPP_WARN( - node_->get_logger(), + logger_, "StaticLayer: Map update ignored. Current map is in frame %s " "but update was in frame %s", map_frame_.c_str(), update->header.frame_id.c_str()); @@ -370,7 +379,7 @@ StaticLayer::updateCosts( static int count = 0; // throttle warning down to only 1/10 message rate if (++count == 10) { - RCLCPP_WARN(node_->get_logger(), "Can't update static costmap layer, no map received"); + RCLCPP_WARN(logger_, "Can't update static costmap layer, no map received"); count = 0; } update_in_progress_.store(false); @@ -395,7 +404,7 @@ StaticLayer::updateCosts( map_frame_, global_frame_, tf2::TimePointZero, transform_tolerance_); } catch (tf2::TransformException & ex) { - RCLCPP_ERROR(node_->get_logger(), "StaticLayer: %s", ex.what()); + RCLCPP_ERROR(logger_, "StaticLayer: %s", ex.what()); update_in_progress_.store(false); return; } diff --git a/nav2_costmap_2d/plugins/voxel_layer.cpp b/nav2_costmap_2d/plugins/voxel_layer.cpp index 5235e236757..0d1eba32d30 100644 --- a/nav2_costmap_2d/plugins/voxel_layer.cpp +++ b/nav2_costmap_2d/plugins/voxel_layer.cpp @@ -72,27 +72,32 @@ void VoxelLayer::onInitialize() declareParameter("combination_method", rclcpp::ParameterValue(1)); declareParameter("publish_voxel_map", rclcpp::ParameterValue(false)); - node_->get_parameter(name_ + "." + "enabled", enabled_); - node_->get_parameter(name_ + "." + "footprint_clearing_enabled", footprint_clearing_enabled_); - node_->get_parameter(name_ + "." + "max_obstacle_height", max_obstacle_height_); - node_->get_parameter(name_ + "." + "z_voxels", size_z_); - node_->get_parameter(name_ + "." + "origin_z", origin_z_); - node_->get_parameter(name_ + "." + "z_resolution", z_resolution_); - node_->get_parameter(name_ + "." + "unknown_threshold", unknown_threshold_); - node_->get_parameter(name_ + "." + "mark_threshold", mark_threshold_); - node_->get_parameter(name_ + "." + "combination_method", combination_method_); - node_->get_parameter(name_ + "." + "publish_voxel_map", publish_voxel_); + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + node->get_parameter(name_ + "." + "enabled", enabled_); + node->get_parameter(name_ + "." + "footprint_clearing_enabled", footprint_clearing_enabled_); + node->get_parameter(name_ + "." + "max_obstacle_height", max_obstacle_height_); + node->get_parameter(name_ + "." + "z_voxels", size_z_); + node->get_parameter(name_ + "." + "origin_z", origin_z_); + node->get_parameter(name_ + "." + "z_resolution", z_resolution_); + node->get_parameter(name_ + "." + "unknown_threshold", unknown_threshold_); + node->get_parameter(name_ + "." + "mark_threshold", mark_threshold_); + node->get_parameter(name_ + "." + "combination_method", combination_method_); + node->get_parameter(name_ + "." + "publish_voxel_map", publish_voxel_); auto custom_qos = rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(); if (publish_voxel_) { - voxel_pub_ = node_->create_publisher( + voxel_pub_ = node->create_publisher( "voxel_grid", custom_qos); } voxel_pub_->on_activate(); - clearing_endpoints_pub_ = node_->create_publisher( + clearing_endpoints_pub_ = node->create_publisher( "clearing_endpoints", custom_qos); unknown_threshold_ += (VOXEL_BITS - size_z_); @@ -225,7 +230,7 @@ void VoxelLayer::updateBounds( grid_msg->resolutions.y = resolution_; grid_msg->resolutions.z = z_resolution_; grid_msg->header.frame_id = global_frame_; - grid_msg->header.stamp = node_->now(); + grid_msg->header.stamp = clock_->now(); voxel_pub_->publish(std::move(grid_msg)); } @@ -307,13 +312,22 @@ void VoxelLayer::raytraceFreespace( if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z)) { RCLCPP_WARN( - node_->get_logger(), + logger_, "Sensor origin: (%.2f, %.2f, %.2f), out of map bounds. The costmap can't raytrace for it.", ox, oy, oz); return; } - bool publish_clearing_points = (node_->count_subscribers("clearing_endpoints") > 0); + bool publish_clearing_points; + + { + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + publish_clearing_points = (node->count_subscribers("clearing_endpoints") > 0); + } + if (publish_clearing_points) { clearing_endpoints_->points.clear(); clearing_endpoints_->points.reserve(clearing_observation_cloud_size); diff --git a/nav2_costmap_2d/src/clear_costmap_service.cpp b/nav2_costmap_2d/src/clear_costmap_service.cpp index bdadecdaf2e..3ef9f63310f 100644 --- a/nav2_costmap_2d/src/clear_costmap_service.cpp +++ b/nav2_costmap_2d/src/clear_costmap_service.cpp @@ -32,27 +32,29 @@ using ClearAroundRobot = nav2_msgs::srv::ClearCostmapAroundRobot; using ClearEntirely = nav2_msgs::srv::ClearEntireCostmap; ClearCostmapService::ClearCostmapService( - nav2_util::LifecycleNode::SharedPtr node, + const nav2_util::LifecycleNode::WeakPtr & parent, Costmap2DROS & costmap) -: node_(node), costmap_(costmap) +: costmap_(costmap) { + auto node = parent.lock(); + logger_ = node->get_logger(); reset_value_ = costmap_.getCostmap()->getDefaultValue(); - node_->get_parameter("clearable_layers", clearable_layers_); + node->get_parameter("clearable_layers", clearable_layers_); - clear_except_service_ = node_->create_service( + clear_except_service_ = node->create_service( "clear_except_" + costmap_.getName(), std::bind( &ClearCostmapService::clearExceptRegionCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - clear_around_service_ = node_->create_service( + clear_around_service_ = node->create_service( "clear_around_" + costmap.getName(), std::bind( &ClearCostmapService::clearAroundRobotCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - clear_entire_service_ = node_->create_service( + clear_entire_service_ = node->create_service( "clear_entirely_" + costmap_.getName(), std::bind( &ClearCostmapService::clearEntireCallback, this, @@ -65,7 +67,7 @@ void ClearCostmapService::clearExceptRegionCallback( const shared_ptr/*response*/) { RCLCPP_INFO( - node_->get_logger(), + logger_, "Received request to clear except a region the " + costmap_.getName()); clearExceptRegion(request->reset_distance); @@ -77,7 +79,7 @@ void ClearCostmapService::clearAroundRobotCallback( const shared_ptr/*response*/) { RCLCPP_INFO( - node_->get_logger(), + logger_, "Received request to clear around robot the " + costmap_.getName()); if ((request->window_size_x == 0) || (request->window_size_y == 0)) { @@ -93,7 +95,9 @@ void ClearCostmapService::clearEntireCallback( const std::shared_ptr/*request*/, const std::shared_ptr/*response*/) { - RCLCPP_INFO(node_->get_logger(), "Received request to clear entirely the " + costmap_.getName()); + RCLCPP_INFO( + logger_, + "Received request to clear entirely the " + costmap_.getName()); clearEntirely(); } @@ -103,7 +107,9 @@ void ClearCostmapService::clearExceptRegion(const double reset_distance) double x, y; if (!getPosition(x, y)) { - RCLCPP_ERROR(node_->get_logger(), "Cannot clear map because robot pose cannot be retrieved."); + RCLCPP_ERROR( + logger_, + "Cannot clear map because robot pose cannot be retrieved."); return; } @@ -122,7 +128,9 @@ void ClearCostmapService::clearAroundRobot(double window_size_x, double window_s double pose_x, pose_y; if (!getPosition(pose_x, pose_y)) { - RCLCPP_ERROR(node_->get_logger(), "Cannot clear map because robot pose cannot be retrieved."); + RCLCPP_ERROR( + logger_, + "Cannot clear map because robot pose cannot be retrieved."); return; } diff --git a/nav2_costmap_2d/src/costmap_2d.cpp b/nav2_costmap_2d/src/costmap_2d.cpp index e5fc9472920..dff5dbbb0ea 100644 --- a/nav2_costmap_2d/src/costmap_2d.cpp +++ b/nav2_costmap_2d/src/costmap_2d.cpp @@ -41,6 +41,8 @@ #include #include #include +#include "nav2_costmap_2d/cost_values.hpp" +#include "nav2_util/occ_grid_values.hpp" namespace nav2_costmap_2d { @@ -57,6 +59,37 @@ Costmap2D::Costmap2D( resetMaps(); } +Costmap2D::Costmap2D(const nav_msgs::msg::OccupancyGrid & map) +: default_value_(FREE_SPACE) +{ + access_ = new mutex_t(); + + // fill local variables + size_x_ = map.info.width; + size_y_ = map.info.height; + resolution_ = map.info.resolution; + origin_x_ = map.info.origin.position.x; + origin_y_ = map.info.origin.position.y; + + // create the costmap + costmap_ = new unsigned char[size_x_ * size_y_]; + + // fill the costmap with a data + int8_t data; + for (unsigned int it = 0; it < size_x_ * size_y_; it++) { + data = map.data[it]; + if (data == nav2_util::OCC_GRID_UNKNOWN) { + costmap_[it] = NO_INFORMATION; + } else { + // Linear conversion from OccupancyGrid data range [OCC_GRID_FREE..OCC_GRID_OCCUPIED] + // to costmap data range [FREE_SPACE..LETHAL_OBSTACLE] + costmap_[it] = std::round( + static_cast(data) * (LETHAL_OBSTACLE - FREE_SPACE) / + (nav2_util::OCC_GRID_OCCUPIED - nav2_util::OCC_GRID_FREE)); + } + } +} + void Costmap2D::deleteMaps() { // clean up data diff --git a/nav2_costmap_2d/src/costmap_2d_publisher.cpp b/nav2_costmap_2d/src/costmap_2d_publisher.cpp index 56797e566b4..fde37a38150 100644 --- a/nav2_costmap_2d/src/costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/src/costmap_2d_publisher.cpp @@ -50,27 +50,35 @@ namespace nav2_costmap_2d char * Costmap2DPublisher::cost_translation_table_ = NULL; Costmap2DPublisher::Costmap2DPublisher( - nav2_util::LifecycleNode::SharedPtr ros_node, Costmap2D * costmap, + const nav2_util::LifecycleNode::WeakPtr & parent, + Costmap2D * costmap, std::string global_frame, std::string topic_name, bool always_send_full_costmap) -: node_(ros_node), costmap_(costmap), global_frame_(global_frame), topic_name_(topic_name), - active_(false), always_send_full_costmap_(always_send_full_costmap) +: costmap_(costmap), + global_frame_(global_frame), + topic_name_(topic_name), + active_(false), + always_send_full_costmap_(always_send_full_costmap) { + auto node = parent.lock(); + clock_ = node->get_clock(); + logger_ = node->get_logger(); + auto custom_qos = rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(); // TODO(bpwilcox): port onNewSubscription functionality for publisher - costmap_pub_ = node_->create_publisher( + costmap_pub_ = node->create_publisher( topic_name, custom_qos); - costmap_raw_pub_ = node_->create_publisher( + costmap_raw_pub_ = node->create_publisher( topic_name + "_raw", custom_qos); - costmap_update_pub_ = node_->create_publisher( + costmap_update_pub_ = node->create_publisher( topic_name + "_updates", custom_qos); // Create a service that will use the callback function to handle requests. - costmap_service_ = node_->create_service( + costmap_service_ = node->create_service( "get_costmap", std::bind( &Costmap2DPublisher::costmap_service_callback, this, std::placeholders::_1, std::placeholders::_2, @@ -150,7 +158,7 @@ void Costmap2DPublisher::prepareCostmap() costmap_raw_ = std::make_unique(); costmap_raw_->header.frame_id = global_frame_; - costmap_raw_->header.stamp = node_->now(); + costmap_raw_->header.stamp = clock_->now(); costmap_raw_->metadata.layer = "master"; costmap_raw_->metadata.resolution = resolution; @@ -175,7 +183,7 @@ void Costmap2DPublisher::prepareCostmap() void Costmap2DPublisher::publishCostmap() { - if (node_->count_subscribers(costmap_raw_pub_->get_topic_name()) > 0) { + if (costmap_raw_pub_->get_subscription_count() > 0) { prepareCostmap(); costmap_raw_pub_->publish(std::move(costmap_raw_)); } @@ -187,12 +195,12 @@ void Costmap2DPublisher::publishCostmap() saved_origin_x_ != costmap_->getOriginX() || saved_origin_y_ != costmap_->getOriginY()) { - if (node_->count_subscribers(costmap_pub_->get_topic_name()) > 0) { + if (costmap_pub_->get_subscription_count() > 0) { prepareGrid(); costmap_pub_->publish(std::move(grid_)); } } else if (x0_ < xn_) { - if (node_->count_subscribers(costmap_update_pub_->get_topic_name()) > 0) { + if (costmap_update_pub_->get_subscription_count() > 0) { std::unique_lock lock(*(costmap_->getMutex())); // Publish Just an Update auto update = std::make_unique(); @@ -225,7 +233,7 @@ Costmap2DPublisher::costmap_service_callback( const std::shared_ptr/*request*/, const std::shared_ptr response) { - RCLCPP_DEBUG(node_->get_logger(), "Received costmap service request"); + RCLCPP_DEBUG(logger_, "Received costmap service request"); // TODO(bpwilcox): Grab correct orientation information tf2::Quaternion quaternion; @@ -235,7 +243,7 @@ Costmap2DPublisher::costmap_service_callback( auto size_y = costmap_->getSizeInCellsY(); auto data_length = size_x * size_y; unsigned char * data = costmap_->getCharMap(); - auto current_time = node_->now(); + auto current_time = clock_->now(); response->map.header.stamp = current_time; response->map.header.frame_id = global_frame_; diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index b172bd87df2..f70d2ac6b37 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -84,7 +84,7 @@ Costmap2DROS::Costmap2DROS( {"--ros-args", "-r", std::string("__node:=") + get_name() + "_client", "--"}); client_node_ = std::make_shared("_", options); - std::vector clearable_layers{"obstacle_layer"}; + std::vector clearable_layers{"obstacle_layer", "voxel_layer", "range_layer"}; declare_parameter("always_send_full_costmap", rclcpp::ParameterValue(false)); declare_parameter("footprint_padding", rclcpp::ParameterValue(0.01f)); @@ -116,7 +116,6 @@ Costmap2DROS::Costmap2DROS( Costmap2DROS::~Costmap2DROS() { - RCLCPP_INFO(get_logger(), "Destroying"); } nav2_util::CallbackReturn @@ -126,7 +125,8 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) getParameters(); // Create the costmap itself - layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window_, track_unknown_space_); + layered_costmap_ = std::make_unique( + global_frame_, rolling_window_, track_unknown_space_); if (!layered_costmap_->isSizeLocked()) { layered_costmap_->resizeMap( @@ -151,7 +151,7 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) // TODO(mjeronimo): instead of get(), use a shared ptr plugin->initialize( - layered_costmap_, plugin_names_[i], tf_buffer_.get(), + layered_costmap_.get(), plugin_names_[i], tf_buffer_.get(), shared_from_this(), client_node_, rclcpp_node_); RCLCPP_INFO(get_logger(), "Initialized plugin \"%s\"", plugin_names_[i].c_str()); @@ -166,7 +166,7 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) footprint_pub_ = create_publisher( "published_footprint", rclcpp::SystemDefaultsQoS()); - costmap_publisher_ = new Costmap2DPublisher( + costmap_publisher_ = std::make_unique( shared_from_this(), layered_costmap_->getCostmap(), global_frame_, "costmap", always_send_full_costmap_); @@ -255,8 +255,7 @@ Costmap2DROS::on_cleanup(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Cleaning up"); - delete layered_costmap_; - layered_costmap_ = nullptr; + layered_costmap_.reset(); tf_listener_.reset(); tf_buffer_.reset(); @@ -264,11 +263,7 @@ Costmap2DROS::on_cleanup(const rclcpp_lifecycle::State & /*state*/) footprint_sub_.reset(); footprint_pub_.reset(); - if (costmap_publisher_ != nullptr) { - delete costmap_publisher_; - costmap_publisher_ = nullptr; - } - + costmap_publisher_.reset(); clear_costmap_service_.reset(); return nav2_util::CallbackReturn::SUCCESS; diff --git a/nav2_costmap_2d/src/costmap_subscriber.cpp b/nav2_costmap_2d/src/costmap_subscriber.cpp index 18d9efc3fdf..37b99bb5980 100644 --- a/nav2_costmap_2d/src/costmap_subscriber.cpp +++ b/nav2_costmap_2d/src/costmap_subscriber.cpp @@ -21,35 +21,25 @@ namespace nav2_costmap_2d { CostmapSubscriber::CostmapSubscriber( - nav2_util::LifecycleNode::SharedPtr node, + const nav2_util::LifecycleNode::WeakPtr & parent, const std::string & topic_name) -: CostmapSubscriber(node->get_node_base_interface(), - node->get_node_topics_interface(), - node->get_node_logging_interface(), - topic_name) -{} - -CostmapSubscriber::CostmapSubscriber( - rclcpp::Node::SharedPtr node, - const std::string & topic_name) -: CostmapSubscriber(node->get_node_base_interface(), - node->get_node_topics_interface(), - node->get_node_logging_interface(), - topic_name) -{} +: topic_name_(topic_name) +{ + auto node = parent.lock(); + costmap_sub_ = node->create_subscription( + topic_name_, + rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), + std::bind(&CostmapSubscriber::costmapCallback, this, std::placeholders::_1)); +} CostmapSubscriber::CostmapSubscriber( - const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, - const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics, - const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + const rclcpp::Node::WeakPtr & parent, const std::string & topic_name) -: node_base_(node_base), - node_topics_(node_topics), - node_logging_(node_logging), - topic_name_(topic_name) +: topic_name_(topic_name) { - costmap_sub_ = rclcpp::create_subscription( - node_topics_, topic_name_, + auto node = parent.lock(); + costmap_sub_ = node->create_subscription( + topic_name_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), std::bind(&CostmapSubscriber::costmapCallback, this, std::placeholders::_1)); } diff --git a/nav2_costmap_2d/src/footprint_subscriber.cpp b/nav2_costmap_2d/src/footprint_subscriber.cpp index 242c7aec372..29a275d0348 100644 --- a/nav2_costmap_2d/src/footprint_subscriber.cpp +++ b/nav2_costmap_2d/src/footprint_subscriber.cpp @@ -22,43 +22,29 @@ namespace nav2_costmap_2d { FootprintSubscriber::FootprintSubscriber( - nav2_util::LifecycleNode::SharedPtr node, + const nav2_util::LifecycleNode::WeakPtr & parent, const std::string & topic_name, const double & footprint_timeout) -: FootprintSubscriber(node->get_node_base_interface(), - node->get_node_topics_interface(), - node->get_node_logging_interface(), - node->get_node_clock_interface(), - topic_name, footprint_timeout) -{} - -FootprintSubscriber::FootprintSubscriber( - rclcpp::Node::SharedPtr node, - const std::string & topic_name, - const double & footprint_timeout) -: FootprintSubscriber(node->get_node_base_interface(), - node->get_node_topics_interface(), - node->get_node_logging_interface(), - node->get_node_clock_interface(), - topic_name, footprint_timeout) -{} +: topic_name_(topic_name), + footprint_timeout_(rclcpp::Duration::from_seconds(footprint_timeout)) +{ + auto node = parent.lock(); + clock_ = node->get_clock(); + footprint_sub_ = node->create_subscription( + topic_name, rclcpp::SystemDefaultsQoS(), + std::bind(&FootprintSubscriber::footprint_callback, this, std::placeholders::_1)); +} FootprintSubscriber::FootprintSubscriber( - const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, - const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics, - const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, - const rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock, + const rclcpp::Node::WeakPtr & parent, const std::string & topic_name, const double & footprint_timeout) -: node_base_(node_base), - node_topics_(node_topics), - node_logging_(node_logging), - node_clock_(node_clock), - topic_name_(topic_name), +: topic_name_(topic_name), footprint_timeout_(rclcpp::Duration::from_seconds(footprint_timeout)) { - footprint_sub_ = rclcpp::create_subscription( - node_topics_, + auto node = parent.lock(); + clock_ = node->get_clock(); + footprint_sub_ = node->create_subscription( topic_name, rclcpp::SystemDefaultsQoS(), std::bind(&FootprintSubscriber::footprint_callback, this, std::placeholders::_1)); } @@ -89,7 +75,7 @@ FootprintSubscriber::getFootprint( std::vector & footprint, rclcpp::Duration & valid_footprint_timeout) { - rclcpp::Time t = node_clock_->get_clock()->now(); + rclcpp::Time t = clock_->now(); return getFootprint(footprint, t, valid_footprint_timeout); } diff --git a/nav2_costmap_2d/src/layer.cpp b/nav2_costmap_2d/src/layer.cpp index 228c060f70d..aad00008a4f 100644 --- a/nav2_costmap_2d/src/layer.cpp +++ b/nav2_costmap_2d/src/layer.cpp @@ -46,17 +46,25 @@ Layer::Layer() void Layer::initialize( - LayeredCostmap * parent, std::string name, tf2_ros::Buffer * tf, - nav2_util::LifecycleNode::SharedPtr node, + LayeredCostmap * parent, + std::string name, + tf2_ros::Buffer * tf, + const nav2_util::LifecycleNode::WeakPtr & node, rclcpp::Node::SharedPtr client_node, rclcpp::Node::SharedPtr rclcpp_node) { layered_costmap_ = parent; name_ = name; tf_ = tf; - node_ = node; client_node_ = client_node; rclcpp_node_ = rclcpp_node; + node_ = node; + + { + auto node_shared_ptr = node_.lock(); + logger_ = node_shared_ptr->get_logger(); + clock_ = node_shared_ptr->get_clock(); + } onInitialize(); } @@ -69,23 +77,50 @@ Layer::getFootprint() const void Layer::declareParameter( - const std::string & param_name, const rclcpp::ParameterValue & value) + const std::string & param_name, + const rclcpp::ParameterValue & value) +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + local_params_.insert(param_name); + nav2_util::declare_parameter_if_not_declared( + node, getFullName(param_name), value); +} + +void +Layer::declareParameter( + const std::string & param_name) { + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } local_params_.insert(param_name); - nav2_util::declare_parameter_if_not_declared(node_, getFullName(param_name), value); + nav2_util::declare_parameter_if_not_declared( + node, getFullName(param_name)); } bool Layer::hasParameter(const std::string & param_name) { - return node_->has_parameter(getFullName(param_name)); + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + return node->has_parameter(getFullName(param_name)); } void Layer::undeclareAllParameters() { + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } for (auto & param_name : local_params_) { - node_->undeclare_parameter(getFullName(param_name)); + node->undeclare_parameter(getFullName(param_name)); } local_params_.clear(); } diff --git a/nav2_costmap_2d/src/observation_buffer.cpp b/nav2_costmap_2d/src/observation_buffer.cpp index f87d59bc25f..e771b2f96f0 100644 --- a/nav2_costmap_2d/src/observation_buffer.cpp +++ b/nav2_costmap_2d/src/observation_buffer.cpp @@ -47,19 +47,26 @@ namespace nav2_costmap_2d { ObservationBuffer::ObservationBuffer( - nav2_util::LifecycleNode::SharedPtr nh, std::string topic_name, double observation_keep_time, + const nav2_util::LifecycleNode::WeakPtr & parent, + std::string topic_name, + double observation_keep_time, double expected_update_rate, double min_obstacle_height, double max_obstacle_height, double obstacle_range, double raytrace_range, tf2_ros::Buffer & tf2_buffer, std::string global_frame, std::string sensor_frame, double tf_tolerance) : tf2_buffer_(tf2_buffer), observation_keep_time_(rclcpp::Duration::from_seconds(observation_keep_time)), - expected_update_rate_(rclcpp::Duration::from_seconds(expected_update_rate)), nh_(nh), - last_updated_(nh->now()), global_frame_(global_frame), sensor_frame_(sensor_frame), + expected_update_rate_(rclcpp::Duration::from_seconds(expected_update_rate)), + global_frame_(global_frame), + sensor_frame_(sensor_frame), topic_name_(topic_name), min_obstacle_height_(min_obstacle_height), max_obstacle_height_(max_obstacle_height), obstacle_range_(obstacle_range), raytrace_range_(raytrace_range), tf_tolerance_(tf_tolerance) { + auto node = parent.lock(); + clock_ = node->get_clock(); + logger_ = node->get_logger(); + last_updated_ = node->now(); } ObservationBuffer::~ObservationBuffer() @@ -68,7 +75,7 @@ ObservationBuffer::~ObservationBuffer() bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame) { - rclcpp::Time transform_time = nh_->now(); + rclcpp::Time transform_time = clock_->now(); std::string tf_error; geometry_msgs::msg::TransformStamped transformStamped; @@ -77,8 +84,7 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame) tf2::durationFromSec(tf_tolerance_), &tf_error)) { RCLCPP_ERROR( - rclcpp::get_logger( - "nav2_costmap_2d"), "Transform between %s and %s with tolerance %.2f failed: %s.", + logger_, "Transform between %s and %s with tolerance %.2f failed: %s.", new_global_frame.c_str(), global_frame_.c_str(), tf_tolerance_, tf_error.c_str()); return false; @@ -103,8 +109,7 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame) *(obs.cloud_), *(obs.cloud_), new_global_frame, tf2::durationFromSec(tf_tolerance_)); } catch (tf2::TransformException & ex) { RCLCPP_ERROR( - rclcpp::get_logger( - "nav2_costmap_2d"), "TF Error attempting to transform an observation from %s to %s: %s", + logger_, "TF Error attempting to transform an observation from %s to %s: %s", global_frame_.c_str(), new_global_frame.c_str(), ex.what()); return false; @@ -191,8 +196,7 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::msg::PointCloud2 & cloud) // if an exception occurs, we need to remove the empty observation from the list observation_list_.pop_front(); RCLCPP_ERROR( - rclcpp::get_logger( - "nav2_costmap_2d"), + logger_, "TF Exception that should never happen for sensor frame: %s, cloud frame: %s, %s", sensor_frame_.c_str(), cloud.header.frame_id.c_str(), ex.what()); @@ -200,7 +204,7 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::msg::PointCloud2 & cloud) } // if the update was successful, we want to update the last updated time - last_updated_ = nh_->now(); + last_updated_ = clock_->now(); // we'll also remove any stale observations from the list purgeStaleObservations(); @@ -234,7 +238,9 @@ void ObservationBuffer::purgeStaleObservations() Observation & obs = *obs_it; // check if the observation is out of date... and if it is, // remove it and those that follow from the list - if ((last_updated_ - obs.cloud_->header.stamp) > observation_keep_time_) { + if ((clock_->now() - obs.cloud_->header.stamp) > + observation_keep_time_) + { observation_list_.erase(obs_it, observation_list_.end()); return; } @@ -248,20 +254,22 @@ bool ObservationBuffer::isCurrent() const return true; } - bool current = (nh_->now() - last_updated_) <= expected_update_rate_; + bool current = (clock_->now() - last_updated_) <= + expected_update_rate_; if (!current) { RCLCPP_WARN( - rclcpp::get_logger( - "nav2_costmap_2d"), - "The %s observation buffer has not been updated for %.2f seconds, and it should be updated every %.2f seconds.", //NOLINT + logger_, + "The %s observation buffer has not been updated for %.2f seconds, " + "and it should be updated every %.2f seconds.", topic_name_.c_str(), - (nh_->now() - last_updated_).seconds(), expected_update_rate_.seconds()); + (clock_->now() - last_updated_).seconds(), + expected_update_rate_.seconds()); } return current; } void ObservationBuffer::resetLastUpdated() { - last_updated_ = nh_->now(); + last_updated_ = clock_->now(); } } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/test/integration/inflation_tests.cpp b/nav2_costmap_2d/test/integration/inflation_tests.cpp index 2e3aa19c53c..82c7b9d3c41 100644 --- a/nav2_costmap_2d/test/integration/inflation_tests.cpp +++ b/nav2_costmap_2d/test/integration/inflation_tests.cpp @@ -144,6 +144,12 @@ void TestNode::validatePointInflation( continue; } + if (dist == bin->first) { + // Adding to our current bin could cause a reallocation + // Which appears to cause the iterator to get messed up + dist += 0.001; + } + if (cell.x_ > 0) { CellData data(costmap->getIndex(cell.x_ - 1, cell.y_), cell.x_ - 1, cell.y_, cell.src_x_, cell.src_y_); diff --git a/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp b/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp index ad1fb15d3a0..619613c6785 100644 --- a/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp +++ b/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp @@ -183,7 +183,7 @@ class TestCollisionChecker : public nav2_util::LifecycleNode setPose(x, y, theta); publishFootprint(); publishCostmap(); - rclcpp::sleep_for(std::chrono::milliseconds(50)); + rclcpp::sleep_for(std::chrono::milliseconds(1000)); return collision_checker_->isCollisionFree(pose); } diff --git a/nav2_costmap_2d/test/unit/CMakeLists.txt b/nav2_costmap_2d/test/unit/CMakeLists.txt index 78085141b38..86b36cf3fb1 100644 --- a/nav2_costmap_2d/test/unit/CMakeLists.txt +++ b/nav2_costmap_2d/test/unit/CMakeLists.txt @@ -7,3 +7,19 @@ ament_add_gtest(collision_footprint_test footprint_collision_checker_test.cpp) target_link_libraries(collision_footprint_test nav2_costmap_2d_core ) + +ament_add_gtest(costmap_convesion_test costmap_conversion_test.cpp) +target_link_libraries(costmap_convesion_test + nav2_costmap_2d_core +) + +ament_add_gtest(declare_parameter_test declare_parameter_test.cpp) +target_link_libraries(declare_parameter_test + 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 +) diff --git a/nav2_costmap_2d/test/unit/costmap_conversion_test.cpp b/nav2_costmap_2d/test/unit/costmap_conversion_test.cpp new file mode 100644 index 00000000000..f0cf789a295 --- /dev/null +++ b/nav2_costmap_2d/test/unit/costmap_conversion_test.cpp @@ -0,0 +1,122 @@ +// Copyright (c) 2020 Samsung Research Russia +// +// 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 + +#include "rclcpp/rclcpp.hpp" +#include "nav2_util/occ_grid_values.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "nav2_costmap_2d/cost_values.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +static constexpr double EPSILON = std::numeric_limits::epsilon(); +static constexpr double RESOLUTION = 0.05; +static constexpr double ORIGIN_X = 0.1; +static constexpr double ORIGIN_Y = 0.2; + +class TestNode : public ::testing::Test +{ +public: + TestNode() {} + + ~TestNode() + { + occ_grid_.reset(); + costmap_.reset(); + } + +protected: + void createMaps(); + void verifyCostmap(); + +private: + std::shared_ptr occ_grid_; + std::shared_ptr costmap_; +}; + +void TestNode::createMaps() +{ + // Create occ_grid_ map + occ_grid_ = std::make_shared(); + + const unsigned int width = 4; + const unsigned int height = 3; + + occ_grid_->info.resolution = RESOLUTION; + occ_grid_->info.width = width; + occ_grid_->info.height = height; + occ_grid_->info.origin.position.x = ORIGIN_X; + occ_grid_->info.origin.position.y = ORIGIN_Y; + occ_grid_->info.origin.position.z = 0.0; + occ_grid_->info.origin.orientation.x = 0.0; + occ_grid_->info.origin.orientation.y = 0.0; + occ_grid_->info.origin.orientation.z = 0.0; + occ_grid_->info.origin.orientation.w = 1.0; + occ_grid_->data.resize(width * height); + + int8_t data; + for (unsigned int i = 0; i < width * height; i++) { + data = i * 10; + if (data <= nav2_util::OCC_GRID_OCCUPIED) { + occ_grid_->data[i] = data; + } else { + occ_grid_->data[i] = nav2_util::OCC_GRID_UNKNOWN; + } + } + + // Create costmap_ (convert OccupancyGrid -> to Costmap2D) + costmap_ = std::make_shared(*occ_grid_); +} + +void TestNode::verifyCostmap() +{ + // Verify Costmap2D info + EXPECT_NEAR(costmap_->getResolution(), RESOLUTION, EPSILON); + EXPECT_NEAR(costmap_->getOriginX(), ORIGIN_X, EPSILON); + EXPECT_NEAR(costmap_->getOriginY(), ORIGIN_Y, EPSILON); + + // Verify Costmap2D data + unsigned int it; + unsigned char data, data_ref; + for (it = 0; it < (costmap_->getSizeInCellsX() * costmap_->getSizeInCellsY() - 1); it++) { + data = costmap_->getCharMap()[it]; + if (it != costmap_->getSizeInCellsX() * costmap_->getSizeInCellsY() - 1) { + data_ref = std::round( + static_cast(nav2_costmap_2d::LETHAL_OBSTACLE - nav2_costmap_2d::FREE_SPACE) * it / + 10); + } else { + data_ref = nav2_costmap_2d::NO_INFORMATION; + } + EXPECT_EQ(data, data_ref); + } +} + +TEST_F(TestNode, convertOccGridToCostmap) +{ + createMaps(); + verifyCostmap(); +} diff --git a/nav2_costmap_2d/test/unit/declare_parameter_test.cpp b/nav2_costmap_2d/test/unit/declare_parameter_test.cpp new file mode 100644 index 00000000000..dea1bb35505 --- /dev/null +++ b/nav2_costmap_2d/test/unit/declare_parameter_test.cpp @@ -0,0 +1,94 @@ +// Copyright (c) 2020 Samsung Research Russia +// +// 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 "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/layer.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +class LayerWrapper : public nav2_costmap_2d::Layer +{ + void reset() {} + void updateBounds(double, double, double, double *, double *, double *, double *) {} + void updateCosts(nav2_costmap_2d::Costmap2D &, int, int, int, int) {} +}; + +TEST(DeclareParameter, useValidParameter2Args) +{ + LayerWrapper layer; + nav2_util::LifecycleNode::SharedPtr node = + std::make_shared("test_node"); + tf2_ros::Buffer tf(node->get_clock()); + nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + + layer.initialize(&layers, "test_layer", &tf, node, nullptr, nullptr); + + layer.declareParameter("test1", rclcpp::ParameterValue("test_val1")); + try { + std::string val = node->get_parameter("test_layer.test1").as_string(); + EXPECT_EQ(val, "test_val1"); + } catch (rclcpp::exceptions::ParameterNotDeclaredException & ex) { + FAIL() << "test_layer.test1 parameter is not set"; + } +} + +TEST(DeclareParameter, useValidParameter1Arg) +{ + LayerWrapper layer; + nav2_util::LifecycleNode::SharedPtr node = + std::make_shared("test_node"); + tf2_ros::Buffer tf(node->get_clock()); + nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + + layer.initialize(&layers, "test_layer", &tf, node, nullptr, nullptr); + + layer.declareParameter("test2"); + try { + node->set_parameter(rclcpp::Parameter("test_layer.test2", "test_val2")); + std::string val = node->get_parameter("test_layer.test2").as_string(); + EXPECT_EQ(val, "test_val2"); + } catch (rclcpp::exceptions::ParameterNotDeclaredException & ex) { + FAIL() << "test_layer.test2 parameter is not set"; + } +} + +TEST(DeclareParameter, useInvalidParameter1Arg) +{ + LayerWrapper layer; + nav2_util::LifecycleNode::SharedPtr node = + std::make_shared("test_node"); + tf2_ros::Buffer tf(node->get_clock()); + nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + + layer.initialize(&layers, "test_layer", &tf, node, nullptr, nullptr); + + layer.declareParameter("test3"); + try { + std::string val = node->get_parameter("test_layer.test3").as_string(); + FAIL() << "Incorrectly handling test_layer.test3 parameters which was not set"; + } catch (rclcpp::exceptions::ParameterNotDeclaredException & ex) { + SUCCEED(); + } +} diff --git a/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp b/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp index f0e9c306233..f133a7eda69 100644 --- a/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp +++ b/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp @@ -18,6 +18,7 @@ #include "gtest/gtest.h" #include "nav2_costmap_2d/footprint_collision_checker.hpp" +#include "nav2_costmap_2d/footprint.hpp" TEST(collision_footprint, test_basic) { @@ -151,3 +152,96 @@ TEST(collision_footprint, test_point_and_line_cost) auto right_value = collision_checker.footprintCostAtPose(5.2, 5.0, 0.0, footprint); EXPECT_NEAR(right_value, 254.0, 0.001); } + +TEST(collision_footprint, not_enough_points) +{ + geometry_msgs::msg::Point p1; + p1.x = 2.0; + p1.y = 2.0; + + geometry_msgs::msg::Point p2; + p2.x = -2.0; + p2.y = -2.0; + + std::vector footprint = {p1, p2}; + double min_dist = 0.0; + double max_dist = 0.0; + + nav2_costmap_2d::calculateMinAndMaxDistances(footprint, min_dist, max_dist); + EXPECT_EQ(min_dist, std::numeric_limits::max()); + EXPECT_EQ(max_dist, 0.0f); +} + +TEST(collision_footprint, to_point_32) { + geometry_msgs::msg::Point p; + p.x = 123.0; + p.y = 456.0; + p.z = 789.0; + + geometry_msgs::msg::Point32 p32; + p32 = nav2_costmap_2d::toPoint32(p); + EXPECT_NEAR(p.x, p32.x, 1e-5); + EXPECT_NEAR(p.y, p32.y, 1e-5); + EXPECT_NEAR(p.z, p32.z, 1e-5); +} + +TEST(collision_footprint, to_polygon) { + geometry_msgs::msg::Point p1; + p1.x = 1.2; + p1.y = 3.4; + p1.z = 5.1; + + geometry_msgs::msg::Point p2; + p2.x = -5.6; + p2.y = -7.8; + p2.z = -9.1; + std::vector pts = {p1, p2}; + + geometry_msgs::msg::Polygon poly; + poly = nav2_costmap_2d::toPolygon(pts); + + EXPECT_EQ(2u, sizeof(poly.points) / sizeof(poly.points[0])); + EXPECT_NEAR(poly.points[0].x, p1.x, 1e-5); + EXPECT_NEAR(poly.points[0].y, p1.y, 1e-5); + EXPECT_NEAR(poly.points[0].z, p1.z, 1e-5); + EXPECT_NEAR(poly.points[1].x, p2.x, 1e-5); + EXPECT_NEAR(poly.points[1].y, p2.y, 1e-5); + EXPECT_NEAR(poly.points[1].z, p2.z, 1e-5); +} + +TEST(collision_footprint, make_footprint_from_string) { + std::vector footprint; + bool result = nav2_costmap_2d::makeFootprintFromString( + "[[1, 2.2], [.3, -4e4], [-.3, -4e4], [-1, 2.2]]", footprint); + EXPECT_EQ(result, true); + EXPECT_EQ(4u, footprint.size()); + EXPECT_NEAR(footprint[0].x, 1.0, 1e-5); + EXPECT_NEAR(footprint[0].y, 2.2, 1e-5); + EXPECT_NEAR(footprint[1].x, 0.3, 1e-5); + EXPECT_NEAR(footprint[1].y, -4e4, 1e-5); + EXPECT_NEAR(footprint[2].x, -0.3, 1e-5); + EXPECT_NEAR(footprint[2].y, -4e4, 1e-5); + EXPECT_NEAR(footprint[3].x, -1.0, 1e-5); + EXPECT_NEAR(footprint[3].y, 2.2, 1e-5); +} + +TEST(collision_footprint, make_footprint_from_string_parse_error) { + std::vector footprint; + bool result = nav2_costmap_2d::makeFootprintFromString( + "[[bad_string", footprint); + EXPECT_EQ(result, false); +} + +TEST(collision_footprint, make_footprint_from_string_two_points_error) { + std::vector footprint; + bool result = nav2_costmap_2d::makeFootprintFromString( + "[[1, 2.2], [.3, -4e4]", footprint); + EXPECT_EQ(result, false); +} + +TEST(collision_footprint, make_footprint_from_string_not_pairs) { + std::vector footprint; + bool result = nav2_costmap_2d::makeFootprintFromString( + "[[1, 2.2], [.3, -4e4], [-.3, -4e4], [-1, 2.2, 5.6]]", footprint); + EXPECT_EQ(result, false); +} diff --git a/nav2_costmap_2d/test/unit/keepout_filter_test.cpp b/nav2_costmap_2d/test/unit/keepout_filter_test.cpp new file mode 100644 index 00000000000..43ac60b7044 --- /dev/null +++ b/nav2_costmap_2d/test/unit/keepout_filter_test.cpp @@ -0,0 +1,401 @@ +// Copyright (c) 2020 Samsung Research Russia +// +// 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 +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav2_util/occ_grid_values.hpp" +#include "nav2_costmap_2d/cost_values.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "nav2_msgs/msg/costmap_filter_info.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/costmap_filters/keepout_filter.hpp" + +using namespace std::chrono_literals; + +static const std::string FILTER_NAME = "keepout_filter"; +static const std::string INFO_TOPIC = "costmap_filter_info"; +static const std::string MASK_TOPIC = "mask"; + +class InfoPublisher : public rclcpp::Node +{ +public: + InfoPublisher(double base, double multiplier) + : Node("costmap_filter_info_pub") + { + publisher_ = this->create_publisher( + INFO_TOPIC, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + + std::unique_ptr msg = + std::make_unique(); + msg->type = 0; + msg->filter_mask_topic = MASK_TOPIC; + msg->base = base; + msg->multiplier = multiplier; + + publisher_->publish(std::move(msg)); + } + + ~InfoPublisher() + { + publisher_.reset(); + } + +private: + rclcpp::Publisher::SharedPtr publisher_; +}; // InfoPublisher + +class MaskPublisher : public rclcpp::Node +{ +public: + MaskPublisher(const nav_msgs::msg::OccupancyGrid & mask) + : Node("mask_pub") + { + publisher_ = this->create_publisher( + MASK_TOPIC, + rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + + publisher_->publish(mask); + } + + ~MaskPublisher() + { + publisher_.reset(); + } + +private: + rclcpp::Publisher::SharedPtr publisher_; +}; // MaskPublisher + +struct Point +{ + unsigned int x, y; +}; + +class TestNode : public ::testing::Test +{ +public: + TestNode() {} + + ~TestNode() {} + +protected: + void createMaps(unsigned char master_value, int8_t mask_value); + void publishMaps(); + void rePublishInfo(double base, double multiplier); + void rePublishMask(); + void waitSome(const std::chrono::nanoseconds & duration); + void createKeepoutFilter(); + void verifyMasterGrid(unsigned char free_value, unsigned char keepout_value); + void testStandardScenario(unsigned char free_value, unsigned char keepout_value); + void reset(); + + std::shared_ptr keepout_filter_; + std::shared_ptr master_grid_; + + std::vector keepout_points_; + +private: + nav2_util::LifecycleNode::SharedPtr node_; + + std::shared_ptr mask_; + + std::shared_ptr info_publisher_; + std::shared_ptr mask_publisher_; +}; + +void TestNode::createMaps(unsigned char master_value, int8_t mask_value) +{ + // Make map and mask put as follows: + // + // map (10,10) + // *----------------* + // | mask (6,6) | + // | *-----* | + // | |/////| | + // | |/////| | + // | *-----* | + // | (3,3) | + // *----------------* + // (0,0) + + const double resolution = 1.0; + + // Create master_grid_ + unsigned int width = 10; + unsigned int height = 10; + master_grid_ = std::make_shared( + width, height, resolution, 0.0, 0.0, master_value); + + // Create mask_ + width = 3; + height = 3; + mask_ = std::make_shared(); + mask_->info.resolution = resolution; + mask_->info.width = width; + mask_->info.height = height; + mask_->info.origin.position.x = 3.0; + mask_->info.origin.position.y = 3.0; + mask_->info.origin.position.z = 0.0; + mask_->info.origin.orientation.x = 0.0; + mask_->info.origin.orientation.y = 0.0; + mask_->info.origin.orientation.z = 0.0; + mask_->info.origin.orientation.w = 1.0; + mask_->data.resize(width * height, mask_value); +} + +void TestNode::publishMaps() +{ + info_publisher_ = std::make_shared(0.0, 1.0); + mask_publisher_ = std::make_shared(*mask_); +} + +void TestNode::rePublishInfo(double base, double multiplier) +{ + info_publisher_.reset(); + info_publisher_ = std::make_shared(base, multiplier); + // Allow both CostmapFilterInfo and filter mask subscribers + // to receive a new message + waitSome(500ms); +} + +void TestNode::rePublishMask() +{ + mask_publisher_.reset(); + mask_publisher_ = std::make_shared(*mask_); + // Allow filter mask subscriber to receive a new message + waitSome(500ms); +} + +void TestNode::waitSome(const std::chrono::nanoseconds & duration) +{ + rclcpp::Time start_time = node_->now(); + while (rclcpp::ok() && node_->now() - start_time <= rclcpp::Duration(duration)) { + rclcpp::spin_some(node_->get_node_base_interface()); + std::this_thread::sleep_for(100ms); + } +} + +void TestNode::createKeepoutFilter() +{ + node_ = std::make_shared("test_node"); + tf2_ros::Buffer tf(node_->get_clock()); + nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + + node_->declare_parameter( + FILTER_NAME + ".filter_info_topic", rclcpp::ParameterValue(INFO_TOPIC)); + node_->set_parameter( + rclcpp::Parameter(FILTER_NAME + ".filter_info_topic", INFO_TOPIC)); + + keepout_filter_ = std::make_shared(); + keepout_filter_->initialize(&layers, FILTER_NAME, &tf, node_, nullptr, nullptr); + keepout_filter_->initializeFilter(INFO_TOPIC); + + // Wait until mask will be received by KeepoutFilter + while (!keepout_filter_->isActive()) { + rclcpp::spin_some(node_->get_node_base_interface()); + std::this_thread::sleep_for(100ms); + } +} + +void TestNode::verifyMasterGrid(unsigned char free_value, unsigned char keepout_value) +{ + unsigned int x, y; + bool is_checked; + + for (y = 0; y < master_grid_->getSizeInCellsY(); y++) { + for (x = 0; x < master_grid_->getSizeInCellsX(); x++) { + is_checked = false; + for (std::vector::iterator it = keepout_points_.begin(); + it != keepout_points_.end(); it++) + { + if (x == it->x && y == it->y) { + EXPECT_EQ(master_grid_->getCost(x, y), keepout_value); + is_checked = true; + break; + } + } + if (!is_checked) { + EXPECT_EQ(master_grid_->getCost(x, y), free_value); + } + } + } +} + +void TestNode::testStandardScenario(unsigned char free_value, unsigned char keepout_value) +{ + geometry_msgs::msg::Pose2D pose; + // Intersection window: added 4 points + keepout_filter_->process(*master_grid_, 2, 2, 5, 5, pose); + keepout_points_.push_back(Point{3, 3}); + keepout_points_.push_back(Point{3, 4}); + keepout_points_.push_back(Point{4, 3}); + keepout_points_.push_back(Point{4, 4}); + verifyMasterGrid(free_value, keepout_value); + // Two windows outside on the horisontal/vertical edge: no new points added + keepout_filter_->process(*master_grid_, 3, 6, 5, 7, pose); + keepout_filter_->process(*master_grid_, 6, 3, 7, 5, pose); + verifyMasterGrid(free_value, keepout_value); + // Corner window: added 1 point + keepout_filter_->process(*master_grid_, 5, 5, 6, 6, pose); + keepout_points_.push_back(Point{5, 5}); + verifyMasterGrid(free_value, keepout_value); + // Outside windows: no new points added + keepout_filter_->process(*master_grid_, 0, 0, 2, 2, pose); + keepout_filter_->process(*master_grid_, 0, 7, 2, 9, pose); + keepout_filter_->process(*master_grid_, 7, 0, 9, 2, pose); + keepout_filter_->process(*master_grid_, 7, 7, 9, 9, pose); + verifyMasterGrid(free_value, keepout_value); +} + +void TestNode::reset() +{ + mask_.reset(); + master_grid_.reset(); + info_publisher_.reset(); + mask_publisher_.reset(); + keepout_filter_.reset(); + node_.reset(); + keepout_points_.clear(); +} + +TEST_F(TestNode, testFreeMasterLethalKeepout) +{ + // Initilize test system + createMaps(nav2_costmap_2d::FREE_SPACE, nav2_util::OCC_GRID_OCCUPIED); + publishMaps(); + createKeepoutFilter(); + + // Test KeepoutFilter + testStandardScenario(nav2_costmap_2d::FREE_SPACE, nav2_costmap_2d::LETHAL_OBSTACLE); + + // Clean-up + keepout_filter_->resetFilter(); + reset(); +} + +TEST_F(TestNode, testUnknownMasterNonLethalKeepout) +{ + // Initilize test system + createMaps( + nav2_costmap_2d::NO_INFORMATION, + (nav2_util::OCC_GRID_OCCUPIED - nav2_util::OCC_GRID_FREE) / 2); + publishMaps(); + createKeepoutFilter(); + + // Test KeepoutFilter + testStandardScenario( + nav2_costmap_2d::NO_INFORMATION, + (nav2_costmap_2d::LETHAL_OBSTACLE - nav2_costmap_2d::FREE_SPACE) / 2); + + // Clean-up + keepout_filter_->resetFilter(); + reset(); +} + +TEST_F(TestNode, testFreeKeepout) +{ + // Initilize test system + createMaps(nav2_costmap_2d::FREE_SPACE, nav2_util::OCC_GRID_FREE); + publishMaps(); + createKeepoutFilter(); + + // Test KeepoutFilter + geometry_msgs::msg::Pose2D pose; + // Check whole area window + keepout_filter_->process(*master_grid_, 0, 0, 10, 10, pose); + // There should be no one point appeared on master_grid_ after process() + verifyMasterGrid(nav2_costmap_2d::FREE_SPACE, nav2_costmap_2d::LETHAL_OBSTACLE); + + // Clean-up + keepout_filter_->resetFilter(); + reset(); +} + +TEST_F(TestNode, testUnknownKeepout) +{ + // Initilize test system + createMaps(nav2_costmap_2d::FREE_SPACE, nav2_util::OCC_GRID_UNKNOWN); + publishMaps(); + createKeepoutFilter(); + + // Test KeepoutFilter + geometry_msgs::msg::Pose2D pose; + // Check whole area window + keepout_filter_->process(*master_grid_, 0, 0, 10, 10, pose); + // There should be no one point appeared on master_grid_ after process() + verifyMasterGrid(nav2_costmap_2d::FREE_SPACE, nav2_costmap_2d::LETHAL_OBSTACLE); + + // Clean-up + keepout_filter_->resetFilter(); + reset(); +} + +TEST_F(TestNode, testInfoRePublish) +{ + // Initilize test system + createMaps(nav2_costmap_2d::FREE_SPACE, nav2_util::OCC_GRID_OCCUPIED); + publishMaps(); + createKeepoutFilter(); + + // Re-publish filter info (with incorrect base and multiplier) + // and test that everything is working after + rePublishInfo(0.1, 0.2); + + // Test KeepoutFilter + testStandardScenario(nav2_costmap_2d::FREE_SPACE, nav2_costmap_2d::LETHAL_OBSTACLE); + + // Clean-up + keepout_filter_->resetFilter(); + reset(); +} + +TEST_F(TestNode, testMaskRePublish) +{ + // Initilize test system + createMaps(nav2_costmap_2d::FREE_SPACE, nav2_util::OCC_GRID_OCCUPIED); + publishMaps(); + createKeepoutFilter(); + + // Re-publish filter mask and test that everything is working after + rePublishMask(); + + // Test KeepoutFilter + testStandardScenario(nav2_costmap_2d::FREE_SPACE, nav2_costmap_2d::LETHAL_OBSTACLE); + + // Clean-up + keepout_filter_->resetFilter(); + reset(); +} + +int main(int argc, char ** argv) +{ + // Initialize the system + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + + // Actual testing + bool test_result = RUN_ALL_TESTS(); + + // Shutdown + rclcpp::shutdown(); + + return test_result; +} diff --git a/nav2_costmap_2d/test/unit/keepout_filter_test.jpg b/nav2_costmap_2d/test/unit/keepout_filter_test.jpg new file mode 100644 index 0000000000000000000000000000000000000000..18d21a118e9a8a3ca8c01e8bcb441eecee970308 GIT binary patch literal 136714 zcmbTe2|ShA|2Tf0ySUdSZd{7cy&?CirflU}>Ot1RwYAq+LbO@hXnW928R?1|gl@=^ z7VR}tGc-trkV?xmCPJl6s%e@0-{;};o%#L#-{0@`|Nc)s_nhZE=bZQcKJT;CP{&Xw z;(NJyx*>ufkO%xhLw^b_T{lHVAmr(3j<^URKGGx@NCisG_zlV&$lKx3BAAGP96$O5 zo6a9WdE7|38_MF5HVi0RK;Kc2_rqfeWdY=$;n8LckAvF?84QH;Y4C{tRi5y#vhlyl zCL?7FXdjizqH)jwgiLo0mv_Kmzc3N<4v$J)kG!pf-XWV1CX{aZpWNNkTPC#p&SKhk z7Pf%fG|Opr)1-EmHbM)jot1^1g$){u@{)%sw1yE2M#>IQp0H~edl(X#jg;m1MZrNV z$fj9ZPGc&Dshth&StI2IP$n{m+YrD3);=DM(<-Rv?E8l!+>!DSjHNnK#vJGE8^&iH zv{&q7ARrO}qA2rU<$eDuBjpG_7SpB?%6}xsP!4k{hK~?)q&!U>Ax6nRbTR)`W|a)X z-2!k`{#6g)(WLbVk2}sL{@Xg-7q?#oge`~sKM$Y*Ji%N9F+*us zH+=9rV$&4s1KG@xdRM4tjnrF0J;oDr3MUo%4IwY6=Zv)RhcbVpuQ#+AJ<=u`>c>GV z66)fhYzS@CpnMw2I8B8IkE=0~54oryz8OLjN8YE8%`Wr@~^VaNEx>y z7{lY3Lwm+Zd1xGbfW)Q$4E@`WU{V0$&_m>cxGE|f6*iZ{;i#%|dFp~u>S}80dRp3= z0z>`rMuz$Z211kX%!Csr8ygsiXGkVbv#_?d9&aXfw6%2j&dS;n4??J_s;aB0j~z8? ztfk06Wcfc%L)B^JL4?Dh*1V+eWt8logJT+)=4m5*c zG8rr;o6Q2!iLJDej%I6#ESyxd{X#esVg;5-nTNT?&Zla0{9E6cScS$VtMYWmjMdXO zooFVOm|NRSpJ6MtbD8bx=I-I?6%ZISZ~lUXi^7(NM?|iOijH5me#6F135h8?Z?xTRzjOCq$NdL? zKI!Uy+SA+j?D^Y)cZ2UgeEjRvXG|BS^FL{ru>Xy&(Lfi2#bUBJm@a~`5gz7f7F%SY zqUGer35nI7V41`fIA03-1P1*l9!jk`gQTAVi{X^FS zq|OBEAI%(%W}*8*oUCU7_%+!?llE;AO+Wwg@q66Ix0U(j z9ZZ**2dU(-z4G%^=3S0@l4ijY4I(q|RL#;){Nv%tyf(Wca)}N%ST0R!OvAHiCtqI6DqygC zsHUd(>-*BW@`q>4twM6or%Q}q^QG7HYAV{A`Nv8v-WAk-yk$6qh)=Sg)}L-TVQin& zV1H{#R^gR|S-BPMhev%Kwc7fW-LF{%o$DK3&PPXImp+`st8Jw!^S3!}x4Vq$CbGxv z4o`bN&47EuI)k_T&)EFnFJIbfHumZ?&Xp^E(Okm3Hoo`V5{u-Tv^~|qm7eJ|88bPjcsrH_P&Y7 zOG5bOVx$hPQeZvWe&?pv+(a#9=3FlO!NFT+xZRKV_7(;xK4UvGNaeWpf@>SQZ|Jlx zkyJOVjf?FVLi;AoolWiJD$A>F`y^iATAt0FTY6zd+pZmP%Zn}vmy1KmTQl#d{b8O% z1+9K{!(&a-;=zK8LFsb-px)A3}|R6pQNWa@G$A~wu_&G z0^8?IHa%XF-av}}s*k;W@Ac+&)+gAF8_HJK`Hj)Gd}w`Yb;P{POpO?_bK9>u6g=iv!AcextwYXO{3nzsnEs=)5!Im)vm0*39%tM(<-DWuVWm3}Dh1s{6v(xs-gX~hQ{jOJ$$1Q8Od{cF-)_YMnYo__b^`#TE%%=}l ze1ClQ--)-KMIlX2W?s2RRcCo^94#OHY~GI<=N8(BmMzQfn6Q3La^Au6Q7SAN+N(?SW$N+T-fyf~R|}`8Y`@nugtq48tS=*)MCprXo8?X%LcbpWW;5Zz#F=0J z`1t$e-y1dS}e!f10wlDq~btroK^yQ1CkKFg>2#DR+n3Ekh zKRKf1F>ZWhmY%V6+3%XmhS0amej!O0d@gPO^77aE9~+jurwd1V$8Xf@_nz>}9LBW3 zu9ID4+Q37%iy;?hKGl1axJJiTaR2^18-HMOiZDy8_Hngm+I#` zl~EJ((7V~jW`D)+lYR~g@}BK#Z-1ldXC5NAeGD>05C4AR`}F4Q3u&#NUYc7MM)6Lq z;h}LZeD3>KmN8PL>j(7C&(Vc>z+=h5J*5*mGJO)Rjk@M{`;W)F^j+4Ju1XE{(Ue(L z&+H7Xn5I&G@VLkC(PmGQMq9j)c|=Ecw@+=}DjvNn#nF~g=9lrF70I-*%gh*j$O!5= zOhqoZWU~Dx_t!y*?8b=q@#G0z1gzz~f6M?>@@sdRxXYB*uT;8$!;4o}1x;UF|KFV{UFq zi?P4(m;bC+#kP9=mCvJ>9u6ex%|-8|VXd2tdM^#3nS-@9+BfHFA88DDE6#^}lSBuMf{gV_ zDsH}d-%1PkN66NE5F%LSbR=#*m=9b2>7ZT)2gfrzSec8}ccJ|Oq-BXRkxOM}+`@5cl zKP<`&xf$p1jDPG(Lu2IFv7G#{lWTsJFVx@|3^vDll$I<~jERxW;iFgHCaZHzoBLJJ z^5AublCC2YUG^0VHI9E1w2WRl>s7Vh5L!TGetT2Fnac}rV3;yX2ZvCLT$-Wc%h)+a zPx7T)Lv5pBYP=KShC0=Gt~W<@2VYq06xg%xS=n+5q58$++ z(vA7?pX1fjzcyV7;H=f-{9$POzI4ZuiA7;`f$?_NMl^d_xcZOx8p0j2juwh=Xt~&O^TKyaGLcgE$ z-+6~6&E0ef(KhX7iB@QRoTYi&_#bt=42+zb76k;FEDT8U^IWUbzE{v- zIK85Nb9Y;@_Fn^pIH=5ve3SrWC?qh5Tqf1GuOrsp}R-VEcX3SY_MmorQPa{1&LXT z%A{&m#;=We7cU%m60xgk?EW#EC;qAUr6h54yXLm6F#}^SugY)IPxv9}-WuY1Z&_dB z=ovA>eGe9tgci*(Hi|Co;Vn+7S!JYIhPJX-KYUonRhbsZ<_V+^&@*{6=g);3EykYx zu=9?ZGT387aNtgLA78UeA05^?teo9R5v(Ui{eRM)A{kr8%y`f9I2JS3AiYz6L+AYm zaYT*rr+Lfw9r#_L!HJW&5R^q)s&TqTwz7rsh~KKQeb1~xk&EHWv+E4EU#)y>!B1B| zWHD`4r*ql&`=vFGp32al)r0ibUY+~aWU6two?1pXameS_!*8>?@nvP%AFQ*TDC z!TRpRw6i}QKdbEjE{6QFW4-O`zWB_CI?JXko3zVl)61T#bv=p=Dd|;)UtXHEQB_}l zu9$N3!mHZgi8I!l?3?x5`xkA+`#xK@yPgV{g}(E&?afadLOy^0{7cWPu?71Ty`itn z(it8vUhbUPQL=Vw(_$wL<3ROk(<+mvnN!r#AynqA?af_q^=onYlostmRYw(RLyf~7 zr-Y3kc6Y5`T4CZ>%toiorhawbQw++P~eQa5UZrpyyy-$OlT#j0(J9~EK zg>aYd6BhlBQOe2E^09Ay<7M^%+GBh3l3fNe{k8Mbe0C;{RTP?EJx?B$WzB1UJ2(4! z*V>%h*+Zygdv(+`V+>%GC%guCAu%A3b)y(nT6?dcM$JQ zD2ry_ZTmN{zXbnosW=+jeIe}Iy^|N#e0{s{eE;t0pHgp3j2&l`R=2m?Cr0znKMKCL znAws>ScaLceRFcw=C{v4#cjOPS9-m@)ouKB2Nmw{DdF4rikI(utEL}+k})RMAUtvT zX9%NSe&R;i1G_i=w96>{Iz420Fg& zkG9%%@7cni4raGaUF;}~KU96E@66MaOTOeSWwp*~T(VAbw)^9)@qZ^~^T)u(RZGy4 z?Ip%9&A7j9*raE?_x4~#ou~1cCe))i5j|rv3i{GiJ+pAIdil$XD*dO$(5Vj zl6}nes0GVa7ua`~ljPdOJ-1Ydx&#{5tMNP3W z_qclFx@A62{>9SqOIj>Ux6X9m`fX0G&KxbK3QFI)?akL~$KCcVbZ{z?g#~s@7d;T< z2XA<8x7G3J!pSdxe;RlErzLBC8~yC|ZRN8*$<9{RMC*OCy^|0twmH7xMWCYY(if@5 z>G~5oGc5PtLfx_Np9PifF-WZ@4g7ki`LqXOWfFdQu-oku8Y}%x(8^_PtMKNTW?ciro5=<456cyms=M9TxZ~= z&VPNCVx+Wev-f>E$K0*Wee^|@w>kwgjNCSvtw~_omX|JearpWA^@o#ZnO8_IfOi3f8@Ng&UNj`czVltX3?>Uaprl_ ziN{2?6?&6@c;^=LY06i#q?#8m|Bekfc^D>#pT=Lg8Nc`Ni4gqnrz>qq(PJ`>M!?%w^wb+0EAjjPm4*5B&- z!z8fS_Qiu;xm%vajFal!@+mmF_>xD{z4x{g-aIb7>EJ3aIYP$n+hCM(W##?Gt8G&) zHcW3-5Y=5rW+jUa{C_#1lA3V%L{~8Na!=BlF4Ik4V?PIFPSB6Xro*I&1BNcUllU7&W=K+O%32$=eDvXa4n4cKlpp%;Z(y!sMS0Ch!(-h|jp+d#Kmn@kq^N z-IwP(*;DohD(`h{1}D(8AKPb@Zj z6#G}u_&-iRXw-RlL%nQPqD#S1&q0R^Pf6X#g*%Sh^+%3=8Tc?`Yjff@9rY@`vJFu` zvRJY)`@6NQ3L!4qCNY@;+bs{~hWtS2?>7@dZAaIvZ$F**DLC!#S1FxmlHGirE+>6z z@Mc`v+BExG+B~U^wtnd7qpIUF&h|tnySDa9&{O)zTd_FhFZC9Z*|MXtd>X}`8?;gK4 zY4o8fN&Tuuhq{9T9eUf28*9wW&Z)hWTYLV-sjp2U=9Hw53*M#o9$D;SG39F5+MCzA zzb!StwCeJbbzzS(uDz?AG|_6XLf<6dJGTvC23?NPvo|MK?n_Gg^wVHzLVk>WKxdw#aH`&&xr>XjwaR^t_;ecal&`AN#i2@JHW5{Bv&v zhcR2gW50j4alN2!?c?#v-tmh{JWa|zhW-8YTWDcFd34~yt1B}@zScQDGOl9b$juQu6a3c1J|B7&|XsKGEKdBN`|e8rhm*dF~e$l=~63J zbZG>qUAT&o@aponCA^cYyx%S=z_9Ylh_^E@rrs|-Rro0zrEkjd1?z5-`sQIbV<^w43Dw(TmSyHv#5Pd+0Hou zE34|;cKwn0_fyvT{u%3DaaP@Cyvq-q{M*~|ml69`t{&L>sv&gFlBuVWNL+FIX z5SnqU=hEo&zn{)}e(KctBbJj|rXE@Q-S+n@X5E=lx!FH_Nnd8YUHc1RU1+KD(zX3D z+h1mFmoJ+0sN$jew_l$(P15Me%D=wmMh9`-LHkhl=kZI{-|9`fc+75|@XC+PdtCSb zCgJoVbtA`bzwTc=CLn$FnVfHuzZV_Jj%IM4`kUnEU7zzwO)&lRYniI|Gliz_lh1eL zKWE2(kPabh*paDFzpWM9R_fNElXGF;sUfs?r2eSu#Y5k=hL3+^GKBP9tZTZ`WSj4X z{9K@u^H6crvT;C+HcHEm)ZH$hwC3_)W6rniWA=HT6F06m_$Vz?={+8i`ir{9g%nnA z?yezpKPu?8=MW0e{Pi_#;%K|3#?L-4DZd|`=$l*M6rfJu-YH`P*#r&nz`fpBnr0?-GT9dx0|lqhqT7);&w%oGCwlz2mba zNwAMy`?2Gd>v(a)Dc$7&3of&^MZ*kg&iCxq1 z$(lOxY;O4YMX}OSiznp=_ZaJy`!XGmx=FjzcK;-slyB`mw$ks{31_;pgzD|jM4aQY zaigoMpA;Rqf8>%$7%_j`PNzTXHUAzoFA30Db2+H}$C%!e(JHyyL~^xz%;SsG(_}MlkwipyKwNB<+>fm-}x9ipI&Xw3FzpB)$4oS*;Ji? zrV8DVjB`^2OWkUkyvAP%@U$w7+VXgC=kDJ0C5ye8(xCl3-DSnbdOFt38#}GPYYvLN z^wXjEz}nv&qaJTS(jy^1e(d&7Jvw2{rz7u9J^q1x^XIpZ_f88MKP%(x&d$W!RZGr4 zUG6rY*uRtO`24JHrqd*kBfEpgaK@sytz;)`1^yk{6hT7Pnldj(s$dP%PXn_5cGHTxd1 z-nHud_Q$0LM(<^h;u1;^W)`09pYS*)v3<>>UK`o(D52D*tyfwWvgGXIlqRdUNB69o zWtaZDQGJ0tKJ9#H(AMWS_OS*2Vn1U^m(GS66Vt@RpBbIzn;vz?y9!3>&U0g_znvGB zd9f!=GD>@Gi0?xEG>NW<>6Z1ceYMVWto=Nx!j;QgduJUN_>-!!6BBReMP6GZK1%GI z|Grf3kZqGd#?+7e{cf@L_4FPXTla~tpU=BLgb`_y6yuebvz4>@mH@|7w7NpEQEtEA*UOuB0V?=6(B}P zSbU6=Og4Aq^zFaNKHQ~`MbYgkc&z`X!T+sl)bjB7FgUgoLL)LPCT<;+gP}Zqa&C?A9J$j-R0%7wGQ-v+Pf zjWx6}S-yG=jL%pwTwadLTX1>xs`Y>yo$&Fadia_keB^5aj{$N=p2!=?kPwBU^(YFh zLhH~J$k#wkJo1O{&i{x0`gDI8^a_PuQ78<0%tmX`Y7|2EM_2~X2GT#gjgPRhq^k&Z zXE-x|JT&yq6wb)A5c*p&H1y^0(9qvSaGV1dJF3_G(|@fJA=~#*ulT3V?QvPy74uY|Ki*Kwap0s=+o(OT7ofx(c{MGjxjPAClotR5}TTe{p=j4 zIR^)YFaJJh>Eh^3xk=HniulD#<>z)P3X3a`S4L8oum4nfEw{X~gg%`%FftMwi+v_d z@+n!fbWO?sI1OEe(`mLRhhh?NF#Dg=X+5wZoG>Rw62W3J*l@r*=70YHI?G@VjhvA3 znQ+N~_!x{YKa_}pt=u9Cu_-(Sh$+NSDCyDQgUlZh9#D%+5wS|nOX6`6TjUHMR7reY zfx$Y;B}gHgh1)Y23Y?8#BSu_Yiyx5_p$YX)q=-Ze3ao;FNBINkybA#1!4V9IAHqWh zKVn2ULjyj8LRAROX?zmRsD^>K<`k}^VIV}6Ko>AEr~|ASNC-KGC1g1ug2Y5YcNk5L zuhc3>N;w5o;dlPPW(9d{{zDg+ z$CZ&h5~`JQLt(U zRf`czHLf573tYv@sbwJC=~TS1t$4zQWlCsn@}M}g$+~+aIP&4sA#W_v(Znl zH0fEpq~gw&{H*9tmW89-Ee+M(+oib~s)Kh^lKP&5oKWx#X74zSBhx0nXEN5}AuUVy zUtP<|u8vwth|)U(rbPW{zWIBVsxWIq4b|Th%J%nfvMmemjIMH4DEUeXIsnUXl8c-W zV+SfBkr-5r#Sl*WWiDaP71TEioP%8l_439h~a#D|I%lR=h!Rou5PuBy(P1Qb}-w|(dX!e`1=>{5Qi~^r(C9ooRhN3~k6A$ZI0yU_9mc-IpR2^8 z7Vt~NNY2Lo#vd3!yF5h9KFSr+D1Z_qKtV=mwE#~=uAoUm8V3FWOT-ypz{eTaAI&~1 z^6;PIA*R5y>MDA}hZPB+h?P`H8a7W_)8xb4l7pRNsR;N=RS%_BGNa9OfxiIwLGpkH zd;;e{||)>^Q$QhlEyxsKOPWEiwBxZ$p|C?A!F{!2%doAd6K3yXa>NUz!NZ3 z0|~YeE`{=ddm{<1X$=b%2BPgl2Dfn8d>#vnaD=5YMlz!)McOhLZj%ioNx&?eDI{LN zlL1FT5lHBXnXGjT?&-=9d;>c`1k~}xEF}fpp_KBGc^Cu7N(zb%zt9N#R4$@|NIp8i zEJ%jFN~q!?uJT5d)SyYI1U15H3+lrYJ7E*R@MO-r%xidvFT=<{9ZpU>7*nZ5kpe9R zo9hp*Z$wwXU|6^T_#c>(fww%l6AVh}2$L1X`?KYEY&K%UHN&~RE*AZR8)-p;qSE?< zr3)ZomM|3%!5~MiW6mvjI2eo0uudYt;xG@fXl;S^!}6q|QqV}N+TS0!DwCkc6Yy4G z5M0L5WH#up-Y6C0OXXOMIF_P~0(doSl3HnS?=@Qv8VK6KmH9iZ05ano^pUN>o?y&W+Utz)Qve|S8Of?E7}bnL zw!j@R!ZQ>Si=6045wwV*~9IRGrScH@UX29yHj8RbjEa<0yC12nbXW05f&bnNf&$i4tT{56F`W1D1X{Yx0t%e845=+o6C?hovUtCNTcw(u7#@tm#yx12 z@sQEX1xYbvz>wLM5M2%@BL5Wz1B8i@hU)zlTpqsO3>r%fTZe{OF#e1vkSSY;WE5Pm z6dn#%1&_m{A(SY+kio7voBUhuN*D>_f>o7`t%{Zs!1G)I{1(Wu<cc%D&ACL;*n3?T- zq!HW)fHfkGJ%msVMOFBcjELZ5X%G?+>Q}tFo+`o`$8w2iHIWC#)7a z@jy4Qz~Dy?tw@qb_zLAL)&bM+P0Ox$T#zd8QE!x472reBkC?71!8{;%rnor`4suKqEN2NE zrlA=7WP0XuBAkdP6@B1MvM#~*1lxj|U8TR}c9+q;gcwO+<1hlG#ylB8vb87-0#>*r z4GPKt6MGIog+Zz6KnV@&8(0cNqI4Vsbwu?mJo`y_6cRlR=~qKWnr1QxQ<>KlJ5zR- zH$+((?TvJS@DZjAtXdd0%q%bqBk2M$=#Wzg2>`$rv!M-;3vQaCH6800^7q07BJ3X^ z5l$E@oVYTi2U}GzkWiVL($<2iG&x!2Dghoa%SD_KRo&QVRp3*L8^~d9q=PsvykPZu z8jb`4IRjWhkSYOwB+hA=*l*IqE9D1h{_=*QA|-{RkKsh*x8QgYrCjI92q*C-Q43Sr z0Us^(8>`=}wAA+vrfI}62SwXAKt!a*1{hgjPJ_}L6%IpTBLYI9R!-Xy5^L?E;~Ul# z3<%;WI(o%rNHIRofy)fRfZ~XR=cTKro%hg@a@qZekC3#08)uhHf&S3oFqNQw1djyX z^aeu&>a0XaiHP)$5$Xg$FB@9HKi4s@o~$J41=NT>DKTTABXog1N$n~C%Dm1?^dMYG z;pLjPYLzf2Uk=j)e+7XeeyS483M&&f9wCkxTndtbR|JqKMks7Lz{e9F@DGEf<;Xhh zA)sgv391ElfTseX80J_oBB($H8l9lh-DOIKbCRrR8ONpLG&9#XxOU^RsFKdu=?1*8 znMwWKu{PlF&WtYk5GuN;=Tjz&bSwZ_3!%Rd=6MQuA??6x9I-Bzt%?qwd^$f>`=Y43dm~nN2)-%Ag)y8Jy@5$=o~m<_8U%fO!Pq7W z;xNn-yc~m|Y%-f5u@j(`5Xl9*1ECjEF>bNR%}70y*^z2>xHC0A$Fb06U1v$I2)2JcI<@%Fl=aEH0^A1~mLectNSm-3x2U5c^R|?Z3PUmX$o~dbkMeJ(Z zOmhb#g`I#zW1WN_gaUu?z<6Ob1pxr@cwAu9Y%xJ*L(v~F#c4l@e`D+nafbB;9)hP5 zUnL>AGOmn8IAkA1tS49I1cR8U5>AT6G37v1n#V-0y32@ymY|AT4>b$v(Haw^-V5u-9EXMvT|zt^gT} z34TWOMbhvAFI^yUWj>Rzzk3sBZ$#k=hLO`Vv5+G-s-x6Pl_ai0d_AAuD&%0)0B!;# z1W@VKn9%lq|70%#A*-rFphZ-ejbdOucJ|mS#@c0L)iI@4KnUhJP;{+fwqQ-iQ=>Bk zoxqU+m)89D*|1*-)o(Oi!FXer)c{9H3>sl*zkh23&*h%1EH5LRJZ)f*K| zt0??Bd?8J}5~NkV%V4|H*CQ#cGylbWpO9gJ1ql-4f1oCc&xteZS?lTVsMAq7P8 zVS7Tf#q1pGO>rx!5>IlGM2$yGeiP6R%vxp^oHlHK&=n+%aR5h*gy3c{`QVYkYO{utcb!Cfi%mk zl)!erY#1mg4u+yLp#?^P4PCwxa=ajV2~CNdc2W)#f>moD5iB<;&g@8{eJDk@0ron> zdIZWKTR!L?iKHEi2v5AbirDaP{2-Q9V%>ph-We2FCagqCClcDznr!YWJPKu0!pXYc zj>Q82A^F3yF279|<|0kDgw#bQzwig1=SJtN6radlM?sXT=_;H;0aVT4 z1qdIm2%!P3!q|!^3=y-OA)tCgRxn_>11|u`n^UAv%JD1kIKh#&7}i<*O@@EWU6mj@ zhI6u5kP7yV!$gsj?a5$pMnu875`RV;v!+kUKX1wq^lWBS(|`dOBuq3p0vH!SgV({a z4pt!W(qT@05VS*V3VOuI*QlfG=aBG8EP`JUJwt#OmN1)DLm*Z`%!6v}eebz`9t>WD z^%S9Vn-MP&`vLyII#UWUFq?U}Kc*WNDBvfC!2=4+0(lWC!3=|uv)dB!tm+|?5gwYP zfzOfu0?i|_={xT`hLj?3p=Q9!OQ30tTrepr{W zI?;*3;2BV)3;3ls6i%;bwE?{faOzWnK>)52JUe1u&@|Ji!On|npfg3GMZjg}WCeWo zsw#0Xx+;*DFYwpP@)O97yr35aby;n-w3$0ov~IBgq7Xso4%SSQ4U6y%YGS-m1YfxR zx!}8L34^QP7?-oeX#zGSJS&Hfw58$gS3gZsuIijj4)-Y$mynj@;5bRSJ22c^yJQh=Xsp_2^P#yjpPflLY31)ggdybwl) zI+6x1L?C5dcm+HmK%*f87$r;&@Ryjv5=$VAhmZ7wbcFDaTOARP@y>BS)FBkK+kr*a>i$_uIkq@<4y# zG%34{$5QgpDY25Tgk=uxCJ1^L3z}fZ0`4mmv26{102-a9GB0%xVzcq8G_GkG4IjbS z;1v>u2)q!*iiJZEj0npC$P!SrW+4Mj_h8_;9LS}J?hkODwaA%5VwSn966nRglo6Cf zD;e~~UQPnaChE&ZO8%*wS7%uQ03b1%-|B>=2<=8()v!fjs06$Pn?(<0uxleo zgZ5WSgN!4{i3DII28@(|CM+lbL-4`I_xq4b3mnMGbXgJ&PAtgXG_C6G+1sW@RbPm0wRam#byC~#8D(- z{{jQ2kpVURFg-9EW2_-$2A~>|L1=b~1R9P&B{5%zeFktBiU5<)hzg_^@h}8crUd~5 zr~#f5oRcYG5H!I@tsqBkARd&4#f!6pHo$H<0M#%{WC$q!sr(jbi~PC|Y-aewembnh z7+8V-4G;*#CWLw18!7`Bv}&L@Q3n<;-gi{$HJ6cyqM3`QLPoMFtU>rnTt;zrlykNm zhy@*Saf||aI~e_-H;RN$Yy&vhJ|F}GgwbXA7aqLc1B1tE1&8`V$OuhcQwV72(U7F! z%|@|9;V#5UHuU8b=XSRxI2i4UurVYVY#Ar(Mx4t2o>-qU*XHb&Uo=%)DhnhP@nvUn z8{e#UEEmG4N)T6J1}9~qhDu6nch5RWLtmnU;Qc&UA*Gd7@tg}aNjL2nXo%hf+(Emr z3}EFTgkN}^6~ng8f?_gM$%TmDv!8@%0;G%hgG58;I%!_x&XlD7!FU^kq>d+>CBE)f zEe?vZV3vSKG?~Bi+OW*S(dga{mj?sBobbT}4xF}j7qnx_F}Lu}0|heGx?JZ7%Lh#} zB`d}rDv%3ki4reyY1v>-u}}$2Bf}M39ECxw;Lps{&8CzX8qaDP?S_^(qh)SSp&H-!Z337&?#Y?A~w8BVp-CfcM} zwDg5J9P}b``XZ+*4ECDE5E`5qO_i`<+ToXAL|CW?He-N%z(vkUNh(o7JWbn)LJ3oj z(nynpZ=jOk^KsBEM0i&nBLzPU>J$>|bPmc$HNECesCcuPbrcLIt8v9L)y9tHqV`u2 z_C|Bf+74e&JYt!rIoskO(b>Eb8Tiy&EkMnQ_Q+*-cZ(*6DHFnoFdl45(kJI^>l_WZl9lVEu|t`UmkA56qPF*qB&W+mB=Nxu?0Eg?CIniWK29m z;ax54LSU}|T&9t4FmN2Ze*weDN2LfWyb^N>xq_)z*7n0YmJ7ZT!xgA$OGkS`z z_ob#tjtvsoZ=xOZe@u&(<`c&nqQ$h}z^!wETW>N~fZVVk`~|65NhJ*lGXM$DY^W*% zsDJ?dm6+R;GQBLzDd*nQakh6KJGjd9j{B&BW^!kUs!xU=Zo~> z?fQbQ%#$-(2NJ#g&JeS)im|b$MUu`zJmXRZKGopu%&NpBvZ{w~^hXB7wJxfc`ONFt zcd_AsZv-qbGI)oGV&(v0$q*%DNAF_MUb`L`1z{@m=Aks$@Lf-z|6opPm_>hDYSwzQ zS3m5htSM{KuZ8AR7QJl5w}N1wrUf#5VgumtUKK<&b_fu$b&tWjNy1@lILy{|z{Cqo z3lfD59wUO7IrQcW?ZaH8%GyzNwALs=k%Ad*LWB9I$SP9^Nnq}f3!2xRQTfA{yD2E= zX!5-!hfA8*&5Z4TurW;t8L}`p9r^cq`%gUwWG$vKw&0X2J;zVoOm2qRP z@3C{sYyL&6KCeh1Ph223ad0m#7qcX4a0^Q82LoVY0RV#S0_1YNAHgjtib2UC0)jnS zNWe9a5TB8vuv0)YSW{9E1%Yj8BCNk+p&0x$p}zA>3p9o$g&bm9hM30|4cOQSTuYE- zg_CZDXqhtM=mP!#9A@m=EH!BEP1&=R{X)o9@<;(P7m$c8#H$}jSTeAf5;^g(3TBODNiPvq^~70I^y(Qb3uXymlrva3 z9TpJXjZuyi%_syI3mGlEL-xMK_-xIYD`QYx^QTtNS7g-M>hB^i^*oggvzkC4)Z*len1Bw8*&|FXFmnR=tVmq}=c8(=^Lm>Xg$KC$wi^pA{=zcL;?+1eGq0B?R4W19j5C~$Bmm`=%1d-MY-@WLoM)8(W zCuj7UwOwe>1~viqaxC{jD3LOnQExUwH~}Q!?Q4z5_)IwEw=HOPH@?)^m#7lvP2{w> z5NAJ-VJg+gLf>ZxYKXMS8{XuX|AQAxJiHKuVas4QQi(k=j)7z-tx8Em7|sZv4RuP8 zGm_NVPqm-9b*ra`DAoJZrfGL?KZ11;bcAoPJCz0$z|oC>Z{Sl0P7^eRhg6xnec5m< z2N%vtx;0#_74@rB!p?103G=Wu@{s#jV{fVawm;6MFb%fQ8ltWA8!nE@@l3y7nqYA+?~nbgnc-WRj}~Ah0B#~tpInGbN=nb$)d9Bu053Iu_j)!+hcMs8 z;Gs>DK0Qr0isjz&S^6dc+qj3}W5R^Y)dneF)QgBl6_ zL1TSqBUtf^JT%R$Ky_)gCI}DgzzK~N=1DLqz!oYb_RleHg5(Owur1MH zDZm%PD}KN!_9TPO!tNrlQO>f=^9_a)J&g+Zx`eB4$nln45P(2r2#T8E8&uu3INU>0 ze|i3jPo>r*;h+(tBJz-jn41=5F+aW{N8hRQ#_DN#8nr6Mt;dYiuet2L zNWo613NwaYHUdVVortLbn}*$SeA`XZ-MA9pzQMa1$>4%K{KSmXQ5T-V4PCZSNywC{ zD@1bp`NhXB-M-|@k$K%*+MG)-_Xv*>myB`J^Cp2(%oB@3?;{pQ89f0t7fZFh-Z|)J zxb2wda;jY~BefXLGDO)QR=|d`q>6#JtYN1T&TVN)AqrH*(+iuc65{?U&=t$Py}@{J z%%?Yz9G8ohx;7`R|6KUu`D`oqlnUie8?(gzglPwFxihI5OoIeT$-pMKrdS`Jdw5G( z*4=!K`_Wd0&5>4~jj3mHtl{|5UxA0iNtEA)Glnv0?!@BVB@F?2gN}v1!Dc!i3Vuu~ zt;udlREa;_*}2(9vC}w;wAI_|*1`=Sss=-+Q@}q^FcBCOt=pi^B}mBtyFd-VNLq?W z7UEdYu-TD$-B;#tSHEMy2bPRYRHrWb2u$z1w$y69Qi*u#?+^S@oJ+zozf8Ld+zv*F zP8yL`BrY%nRBfWY;arv9T5|@j+;a2J&x-qOdB3A?lWm?;?V$P*`vND$z-k1nb=lp~ z9H(;HV3ZJR99RKti9l$=%;Ob-ND&)?cepu%5d`1D29|DeC?FvZIo_Ia)-aeWQ(v!q zrZ?%-1H{~2e}2vZ@8!@k#U}oE0zQTS_HV`r(FsEC+E;XmVvep-!hyJ!VN&?vZ_VUqzZl)|Yug`;Yb7eckGXx4$~ zWiFyB@H{G`W^`N4KXcic3cWpd{jLDhb@tT;ldhKr#z7oH5py2|{1OOADB+=#k`G}) zL_CRK7)X65cp(b#hQzWsQHz#a8$mo!^9Y!OLniu^1dIVLMAIkJGJ^0zLW_e=`1&M3 zXR|EFA(-Zm1|NUJ;qA=XVR83Ni8HnQ#Ho(%*yAzDlo*V%sWg5|A_WGu3;(y75R`(K zWg!OvkWFFz05nRK00TT7yg@@zb~|E$bSM(z2nl1r%z^0?P-U^Xi=0#Rs$lO2Tl%Z9s`$uD7Yt%RA4y^dwBj#Q5A*o7Kjiu7;!a%m2l?^L8MDc7-RI# zVlx3(g_ubIj#SZJy#Vy}D5_i2QI6g$zrBGBJs#VqN-C*x^rx&9AFL4#&v6_IdqsFF z0MBLNHlRlm9trlS(lz>Q{g|IzKZJY#P(4^Urp2fGB+D)t4h^d!tOW=kP=hbs2*&Sr zwa{nczTxda5H4(mNOy?K=}X_}h{&H^zZzHw`ikH}6m|etcQV&6XmXj$<6i8)XH?T& zSNfG=>O}omv}td&7_pf0$m4DIffjstN&-PxRRs`pBfULBdTRcCa=whbJ~c8!Of_Cm4i`IXYE#4!S8gbxrid{j5K=UEBdj%(+`~Jm;ydQ z0K@!ha;9XA2m&{(I{>YD1)da3wZmgYrK3r|l{$G(aw=CXKU;Qdkb@wWV!lay@Xh(bjWZ1WZp-}vuY>=(15~S*!MyN-$V9mo4#&Bh! zHc?lt>-b`sE00w&dIcjB>y!?9y6{CrAij+NGb3zslQI(G1h#E~7affQVjy&IZ?p$C z(lCoF#Ftwf3Xl*kdXNT-iJ%L=tn#N5`k${GQPf zWs=HjJNGCO0xr39;k(M`Pe}<}z7C!N6Ca63ln{-5GExSLj&brr3+jJr!M7TR=>@3& z4_WU4Pv!pwjz9OBA@dSh*UCy(Ho5jFTtY?JRLG{Ha;?bT*l{lKEl$Ac}!40Y%ii|FJKG1{@H)C+)f zd+fjx)DodTm~q_~VCcnhF@Bj)PHoUO*Y>-saF_KCRqhI*M1CV!&Fz)R9Ydl8HXkr~a~04ah?NKhXgF)#%oKiK<+ z4Ks=j5LdMmhCD`RUUhsrb;s)*#h}wAlO=}k z(!Jb@FY=det3KUxzJHwvL|k*Do4e_Q@<_}xASM+hI_Jdm#DPpl()Tw6PC)4ZItmAC zHv)hhWP#xg9hKo3A|p*|4lN^<_!uryJuiV;-T}Q9{o+3Qz{b40L3xNL-0) zMF?b|7J?eb15rSg0fq#|b7EBH=)jMo#Ih4Qz0lYVUJQvQBLc_Dif|CQ;<__8k1txQ zk8)2O3r{9WDi|+&4N~K^>JDiC5mqi9(AT=i? zgooIkc$@P5!^A!|NT_-0ANEX&i6){3k{I>F`DiaexE9htVW7NG4z3$?H>Ehr|51P` zbSp%O2_R!6YzU~qZ z5mXE$nu<77p9&HONSXnUp+o6&pbxd5mU(Cdsd!n?0tY-P9oUD3+pI{?hd60s%DufaxesRDjcL8kLdMY zkk4i++jZ;!%_!g)3og@)XX;sRPlr&+Ppfs5$XXYdSn$N>TxFPYGPy$#ut17O9P0IL zj6rPZ9F(xhL>xMG9yYuVWF`Eg2@)Pfq7H;)DBT%`HRti}t&n@7&MC|5ZVKtZjOr9~ zS)0Ky+$7J^BC@#%=yG%_0JShtKBQ$7H@2uky9#IrYlFJIC z)dIRp;-)X5$#vBO=H57qayeriM@06;}$Wp^5d7DSkm3ihD_y3i07Cn}eU ziE9yHMhi8NbOdVUsuaRWFuK4v2Stop?)>H(+DI|=F%k(y4qdWj{Ca<}E=D^hygUkK z%TOnSM!t}MhARFk^b&aX@EdLP{$28;Jwe*?S3?-qgiak>4Nv~3A!tt`cnm$GxR!f( z2PiL)OA>}bzk8d5gfbJL0PaQ3|s%v|C};8i|52*H1lpJ3wxKl0M{%z+^!56D1humoLJ!ggB&3_J=6v z9y_r0uV0vVWe(AawHU({Q4zuFje^8a{o+}4b{23WP|9+XNDx;QAwq|fsVH$s17`vm zZKW>Se54%JEjX~?u(1Sinf0BGVEjT=JEa`2nZ8p_d|#Sl`lhdXet&>^o|!($UuFNX zwc;C9%uB~>Xk>g8NIG1M*NgWwpj4wgCzqJ@92S4o?-n-GP1!Nu0#3}(R)ppWq|C3$xAwmZP|G&-Q_Bxae&(#e7uFr^FR+(}tOHs2K{OPyCGUOI zBEt)P+GB8z)5M^LPG?{pi(Hl~c;cp0*g&B|9%Nh< zx(tcdvfR+_aTq2AK$g%4K{wH{A~h5V>99T{1_#~%#7NV`tH9WS=Yz;l>}i1!p9Iww zdI$s|X^AchsE7gEq1WPZBrfMgPPJ>U{J1n_vU3t>n*ZV`sI8jt{t zFfh>LVO1Mk3(gFt;4p>&8Un1utPuy;kl+jcYo8zKk_Kehz?2aJpkaIh^8i%8ID!#O zi?RUYkR%w6y8}%{n+<}Dk{`N!H2@$3QqVD28)j`186LOWVrDR~0DUAEPhvD%#Zs@gT=FbK zS@DhZ_vi5t$)g^lbrN4*AQIfvaKj#G8z8Q;+ZCql#t1BYpvR(&2h$iHKm$+((1sG_ zeA*Dmu@GO1t}CDo5odJRWW;YAk0jh@JewZ3ujfnGR5-Y(H@zee^w?H>#gZKuL_DgAJ1W(h?+vWR62{K#Mfl zl5JWbS2~{=VJieO&xt-Nxb1Vw1(=d;SL)O}t*P4)*<C!qK;_Il;pe7d9GF-cjn~a}CU+=RmuI${3_|mCsK&OHnws%Msv*1EtyV@=wV`I*c(T z@DnUl;bY445GM~_gjGAM{#whoBgUs*1;H|Zfu;SsCN4NQz*8&)!70#_AZj3r5bwF9 zL88O3l0YVi3n+L4p-99poC60J}ptJsKDf z@@7SSKpsFXqxuLbLfv`tN$|Qr1TcpFsvsKRPNPZ_`qlymUaUE>kqU*_9GqxyM|h%xeZ$;*nkH1=NMH_330B5(T#+ z!Z=E0kY*q`1W#EEt#}gMQqkndWJpzkagY=Ogx@V>K-~hyC=rAT4?tiw+DS0n5Nrpq zT9kaBALv0^uF<#V=m>-d7ih~@4M*Mz7D%7*RWFEN?Gh-5REo)z<@c@v!!Inrr+acF zSzu|@Uo8hw$um7$d@5!Jl1-tLPSVk0#RwQiDi|SfCE^4Cc2ZjCF`ywvsR2G@GBz(J zXm?12%y!7fivqqOii?%wA;JQ9!6YtdInqI}X=snbv^QRiC2PzEtYrd10t>cSrQ|ln zi;c)g8|OWo*i>i;OhXUgpQ>Lx-zUF zWHPGyU@n986QquT%?v4>a9+R|T1|xx!oZy{N?}vsMI`Xf4gmtf$M%c_$!QM8f*g^o zB*=(Qhc@squ0UJSA(3M37GV{9Hl4J;D;WQidBwuaxB}9Thr% zRj$O|;(_ilFzVualNm)L43YkG8hlI+%dQFqPiVB6yCLqD;J{ph!J)n{LP-!~Si+%{ z0)0uKV#z-blmhYt1EG^ZP@vmT>k0u(a54YVU_gQpTqY$M1CEb50W@3pdN2&tXkl=e z2oWI2MHoRq{4ykKQOnnsEC;JHk6si{o?#N9rnn?eVFW`i%@d7W#paj zF4K@Oo%RN{ka3G80eB)T(L$+#cEzq~0Z70rNHCzu&`hCO^y$b!#E`tzUIlv)%qGJI z90DTDMu60UrUzu9LOc0YAS8NpY+B$*7#GOvfrkTo;Aw4`N5RbimlVS9uzI}HCa8vp zjYN9^Lj0rdR455hjYxv$qVM&0; zVpKGHu8O)N93){eNEQHpiw~tW5RCwrptPog2q8QVx_Z^mgq{E`I|+{vAixi#6_25w zHe5?U{$o6}9t8{pM`EM?wHdXa5czO)?HYI_AnGMFoZEq9kU)fvK-myYzNF3zi&;@= zg9JsCy&zu*hk@`ZMhuN-h}BZX*iexmW{EKqB||GABr^sr*FtajS9S@^40X^%N=VWt zq19-p2QLo;BSHmB6#-uqDgY(I0H`MpVig5VMc<%-4%dJydJ`Q1OZGJ#lnTcsOLRA& zY>Qih%wB>857SvR=CE5RsbI4UG77)zm4I)&OWA*M2j*dPvGKoP3k~gd5E;#xf$@%# zBk~ZeFhEr;5DegfTZ`&IO1K-SCLJ+bgAx{HRF!-~xCBkQ0)|Ct3701Wg8#J>K}ZM? z1drCE!9CF13_ZZN;Q$x-!zVVVkbr7=GJ~G=8c>$t%3ylMVZa~)C~}w{OWqg?<9>ngb)a{gyZ65z8LM~+o^e`(M7mOyn1$0RWPiD$eRM{lK%lmZ4d1t z5-Zpt;4cGkKn4AhNuZ?&Vh9JWxa;qv1(9S}SIL#Uo*FB*HLDWu_~p!CWeiS>J{!dW+=fB6te;CY+`QW2!hl z2Deuv1X72z{s&nPgM^1u0Rhko2m-;6b~uGB0qy$m*Kp*5sm ztLKB>E&?r6BBp^22UQacn{*O7z#Y%t-0cwBUCGA11(jXu$(zfhh85SQfl5I#76sU#4gGHd#!xF9GLYe3a7Tz>qL}Yab^i}E zJO-N=?^c{rzOy!l&PBpI8O#Z2FaLu~8;kBDP=N{#pt*ZU8ip|nz@Th>n=@neFmdS| zpb>>~BqbWmqj((u0u?M^eBe+59w86PU##uB`7Nkm2r*E%#kx6NL@h zP)vm*sZ_k2IlF`PHCB$eeq>zt3>bY#qQ2sBfY0b3G!18VC4>jM4%KLv~xz#AHfKriEWl|MWhu7nf2 zOJ7lmErEXfCq2V(2A+#rHI(9L2Lk2L3M@%Wn+!<6J_-)@P%CIRQU!tvRp=6KIQ72* z>JS$OT@P9t4FQ%wy#TC(Oa{2<0w%bgMNA$dcz`tIYn7sug693VZjelc$CeY&W&n(T z`WUtVn*<#XrpCXwxS*Qhj1R1Ku|*|Vsh1>CQ#hLmq2eq zl_pqQs3L+cAc}y>L|>bSUJb?sMr#)^9Q;?9OMqp-n$SXVfO8no4usuvsM<#b01C?b zEE3%ilV%tqKrS!P4ul?B2A%hhlCagH!;+~4NpLlMcMED)z_12nLJRtk{}t$2N|bQW z0uo_&ixLs+4}eXJh_Q&%>8Osu-N19u8fXk)hjtgL;UMM>wgrs5yCj91p}WA4f`*{F z8MG4kCHViIiEj45BVk$ZuIfRL!M=rGZ4xt?0JO!z`YvR&0ev{I4i!Q!*;N@#1XRYc zq!MhE1hx~p8&vQ=xdATvuYevx3pxVt+G(&)2UzUctp!a1Y)nG;F<3yHh_JveFy67Z zDB<7+1b7}E31CVoL3cxixi+u`HU@Yv3L%qFwnq7e^gnunWdk?VfGt`e zp)y5_jsO?!@*;CNa5rFu9sr8~BrwI%mO(ZzHk_{J1}ry4l%-IffKw0*K**>)5Cs&) z!5dvj5ye7D=M z1URDQD1kNz5ExmLVGB$kF6osQ(Qx&~iFpQM6BCbvpcjs%yZD;nCiNa3!!JDiFIou{-F3rWJ&% zaLK4JL%4_zBVfr)4o52SAZ@!t9YV06h;NV84o?TuAAJruCQ-n?B+y`}Q4S$?c-5G8 zB$}(UOH3?H+87!xgcf25fC}^s@FPtTOh`eO=~9tNd@%$I5hA4^5j;CXaEriT03$j{ z1Yb*4flb9lam2ukE*>sM`H1#7ycAcC8G(OlNHq@L4HzuxBem-U-H?@+gfC6toQb0XPdEVTiEgGRdf(zAcdY<$94O4xL#k`0xbiM zinohfLfJQ=@4NRWYFeeKHks;ZR-G6 zrVJ6Lr+GUqh8cxt&-!gV?K#Tz!JTnWr$N79N|bQfcP_bByl0i8DPLiQ&P0>rAI^fg z+M5*@5ACDjaA;8v6Jze;Hy>*|y!9;O!~TJw;z15!EMxa`mH?lX9RrEn%L`;kUb-@? zmT@+~eRN9N3xkO?5Wknvf4{etqfAL~ywclK@a=|NUts)mkCjxdbH&`(Uz%+BtI$~- zaXjW%S>qTU@*>UmFT#-ReqcD=EyXA~=M?8lfnf{v>iqhrs5;uunN8~HNa$tcZ_U(-)UU++yySPKB{;Gr(J}e-xMBsH zYWIq7+Qssv0^?V0`d^}7yAB+ve&d~4W*<~JCcM=7N(8^jncz}YO1DU@+2Yr8UtlF_ z`}#Ms)E{2udJ13jUtdJ~=#i-51PUr2Z;Jq}M>O#oeiha|r)9Dx0$PV;GpG=|VpYGr zuWVgN&N45WhLrmF4fYm4a8B_)CVk2-+eP5AIniuSip?V)A_uq1iTLsGdA#i}eanNI zuIFXR2PG^8z6@EsKaS0@qTA2zL=_pA?R@%9jni>$mIyJ2TCvWjkJDvz2W}t*RJ_?tLW3L&FoBG|my(Y9$;qMmpS$fh=JHbpOMrl^M%|6g*RIexT zqh3Wq%P;NF0_6hQoqt zS+n}$9S;S^jdU&*$y2mlo`_>~Vc^S*eLCbFg*c540Ur)PuIt}BY4 zX&ZSv=v$s2%~CLy=KW^;sO*qIAd{~mUHWg?YUPK;DQ{DY7fwG*cN7&_SzB9HG2Wwp zM!D$wU!>hM$!v0aPNi3#>xjm3&DTE93E~3h+P#;p2C<)y%oMhMboQgV&Q-a_+SGB} zUc!48J8O8ToZ$R$^6ZoL<(kcv^^sKN)S@-ng$Sni#e-`5(~1t6wFwB8dX|SziwYl^ z9`4Qhd2?Z;^;^B;Xz{|8U%zga-F}nwI%i{eRcHT-eOa9w{-l2?vBj@!U)4Q5CGo=+ z+m%`!5k3#zM1`~%goLVdm~1q?>u=fT#vPHDDz)(P=2!E5EPZJR(s}Ym)r5jrjo@pe zFuf+$xGCp=N)vqdNJgW)5!yz}qV@0rk1^trsoo)8e}(*zp?7uB$jP7{9V^lgn(~hq z*?mv2XM4z0y*)J(SC0PHOP#fuvY9fThR?rWIdXMCwF`XFk85=x{Q7G9c$~Jbsdj&Q zumAZDw(+e^V~@v7n}>B_^_gc4B5h|C^2URz-d!0vl~b?qS>eowi}$X+pZ-u-&^G

1UlglQt7)9(6|xK6-bwAMyS7 z;q}n+_%zeN@rYNFU>y#hSh{iJG2?M#S@CM&YFud)VH^%7cE6Lx@G-Z6{}5LvE! z>NOST7f*QKT)ukXTB!fEyM34M_;A%lor&bi4P#RE&MA9wqD!0;swlkltUve~JYD^*t^AX9^T_)EGH8oow z9br)zt?qI*Yz;DN`{2u z{aO6^S)2DA-;)#7P1V_-wtp)edLyZjLZqZIx_4{J^>-u|zh*C-`aV-9(;^9K=EN!F z)W+Gt+Cx_5s_thtd$)@^wE`0ArC5A1O`_@AT^x#r&C+?dZcUvj$nmgf=zVlOdCGH` zbN1|ErSJ=O)sYGhx$NpDxJqr_-hC4KrQ_T1hgPn_qot(Yl?i!s(+ zdo=1Y5&WT9rkNi@re5n3uj$VZMBZ_#&N{xWS1Y+%%E9M%Ez~@>ls%CEmN_-Q~2x7 z+}o3Ma$@8`pUZwVSvlN~&wG{RsFTticB*n(4o98jjt%lLkK4nIXEk*9SP4t`HJlfH zXxe(@v6TL|#s{qDF4WeTUntvoVAf}BN#B_6_VK}oh2VI)2<^i(4Q}6lJALt(t~5)l zIZ)lPy6AYTMc960-@{yy%I@jfYwaqBl^M^`UyTjsec(3zS)}uI`|kqtEB7Z-!lhDO$+%~EY2~~duuv>!Knl-F0Q(+ze`X3CAy;=e~#Yg z}47;F|4r*40J+MMHMzWtqMM_G4_<0Xu` zM+P*Fx4({tp3UKTU~zRpbmx*1S=ss4+*JeP?N7aP$WEPfRP{5?_X`Jhlqo-87ml3W zoZRAhaGo=k=iy_;Qk7`;{qH(M(>!x1uAeuUTU(j$F9{nO<~!Qz+w`YLYj3Cb&2wX~ z{58+sN`CY6h(`h2ueLPePu<^=lk@T`M>KanvV^o;_#CA3PCh`?;DVsr)``VtY2)qh z6rY1cL!aGVH%%(e`4hBoCdA9EGxTOHzkBZMxYzH>6z{!$^W)lkwk!G3@?R_3mD`WZ zmTQ_nV*HCdNIZA-u6?3(Qj@>V&8+0p;`hH+On0g+CaOy{|K4w($e+{{cpzF@Mz1qY z+T&&@{ZHk-Tk|5{Uw;4TbYV5WE^$`r-OJF6xcB5E^U}I!cpDbdf-gnP8n#AzMl3cz z;s5aJ3xCtV_`be`-jR=>qE9o#il0?9s;ZM#(hu$u$fND_Bzp!a+HdLiCdP% zutD0b?Vc3ZIrH)*yPi^)T&c;&B!>^rt4#2S9@{4?9)}O}?y)t|H%!4-nLf=)DKp)pCD- zV~WqY>CteD%DY?~b$3+=3XZ3yG;a)9H*@~V(|YpGz{w1y)}KrMen*$zoiYj)336N9b&ffF$!pn!zM`*tKwC17l+yDKp$3R?`Oy7l~UdcKld#(0ZL z)ZWv2EoNqjrf=!;$IuUt-FfIUaKB@Z@U7j6bd3%oe`HT5bujkAG-w)W{qIj}rq$tKlEeZ1^ zDs=ua&-cAsLqY-xdzYzFx#ubI@`|NmHAMlrmc-{;nH`JNlILuxF`2Bs_Nirl;ads^ zyXfPV&QQs(x;ECjrpZJl2u89?_q`dAG=5pJQl9?e){8Emt?Ee{6T{VwQT`!KE8*tz zrQ(Kj8Ef^HNtuzuWqIzy`;6UhEo_bWV_kW_sjhVX8O6L*vYW)aTQ;e0bjDDJVQ$ zbotSkMRQ@#^4i78AD7kMUon5${N(!j(Tk}sQv4E9DHv_77uRgd7_D2r>1sbLz#8*B zIAtrAxFdM9iLr2sEOz(P-8jy!!Plq8f&(QA%69ho4KTV7uF5kspDZh%(OUTYn(5~G z_6ss~TslS`e!4gEwOJyc2x1Pl| zUx&Q^#;J+=>a)h98TlTnGl5{NO%xnG=$)^Aa^o4F-K)Eb!otD^a+m!FS=qH1)ah?s zd>#8M_?M%7eB<}G=+BUSV&nHW_K6w)*N+X$)7PcfR8OEk){%k5?8C%r8#ePk?g*TA z!^?Yp#x7w-EEYX%)P|OWv z#p$S^-)c24cr&$s?z%khT)?7}jw87@5=RarMPCb_)RmxL`pvwx^=^`RIJWQ#d-U?x z&vbbTGn|{{tR=@bG&C^o5}a>JS%s{ASI8b`-Trp^C(F~CbG|2MJ(fNs%TM(0FF49> z+Yn26tBc!0s{bpgXzV@f7PbQs|3S1nOy#R7WtiQScZ2`n<$(*?G);=b4=lEB_$~dG z-MRi|``5#Lf9}38Ey5tTOy}j436+!bKZ$>lKGURqq1XE4dm2|HXk$LyED5e652+gW<^7oP(Tu-Yxp ziVRe!NMi54%sh1Rid1AoaNbaOprd4z;q^`KGS$$43?E-Pj8F@fNB*wo<#xu4<qVrGUo5|vSKa$Xl64E6I9ht?v!uwhJSuvmr%Bk!e`E7CE>z0#n9x|M-u?S z@=P_eCksz>^AwKLFc2AGb3F3h`TMVv?QQ-V;h%qMm8*!=e3U|QX=yu(C)Dm!W9_Rj z?>F+YpWfH^>S@4gblias=Ac80-g;9PjKohobv7LJk$Yw@cUfz4sdVS3zjuGiV3qm1 zAwF{Av8lJ4aj!mw{?II#k`eV#eNo`q$kX~!PWIBg>Qt3CTW!y%WsIVG>K133;OoMm z8@Y-M@`s|%RqOd4=(b(%`Qt5jSzBspW#{L`x*s?8EBH#V8sg4s8O}}Bu$O0@KGBeU zLRj|7nWQ6ce}){mpmi}m;Z-q5rV*z_i|f?1#r6HivrU(TMxV3jDt*pR$P|dXG8gK_ zw%upyS<61FR&ec$?7D=&YF{HU=j$zn%57>c z<{EEGMh-BE>r}amE6B(^u9nFTx~-XC1Eg&Hh;4bYB~!S!Imw25=WQK$evcP! z1s#2t(lf(n_3-dfQ!WOffX+92z8;$I4 zzr-&%3kQ?BQVZiRXcPxJbl$vD;Lf->W%se##b(lYkXl90m2<@=9dkI|IwOMKCs4m+ zj-)^O!1JYEfzzTA{nQmxO*5vSvK#M5KYP#3DGi@05o^^m4E2j+dL{3k@rEiru5uvr z?g#VaXqiskT7yyXx@U}eBKr(~obi`9nsHUREX8u^tj+efx!hEGDdDh>Zj)|7hig7? zhbgj1JL-O`Go^`>4-;=!n=GQWI{_Lr%5JEo6Y z9YD4IjGpqbo^#ysy#5XsX`?qg#5h}8urIt0wZGtN$1NUW;`qM&q+x5HELkf)CAWL0 zS8gD+IB|C6cb>HQCtX5x_vgdc?1p}-J>W=l@+y5Zn^q>^`h$V@%tL!oy3jN(p(jR% ze(ga*Zc7wW4_CA+vC^e%(WWJB90A!&=vut@v8*?$6cDD7YDCvKA&@4eEGThs}X`_Wc}sa zy^O(6XT2b9}4=uebt@CGR~{sW-|Bp z6b>6@HSBEsV7Stxz*X*HkvW@E#mao%`=;X~XW`5HTc6%uS$VaizOP~~^;miehgx+z z^?VSmki|O-0quAmC#{*6W}iY=cpi!u*8Xlwt7mq6j?3<)3Sa6Ls|hx9nHEye?}@O} zVN+14N^B^5*(^bpFzX*1dWYt`1#snmsgKT~nu@Kb5cj{X6Z~^q~8j{7_eYx;qnFLf1vwHLgAk1=bx~ zu`qH^l=RGBx*mClnp2hzkyjiNk13AEadNyg`6RR)o%Pc8>$d~f2X;clFAnt|_*S9R zU({K$YLY1%=_xIGsENB$Z@7H!CT8=B>l?e1-JaVc89ry}jlbnYocS1jdqu7|CG~cLdK@;jLp-JxyT;GaHgC!!!szkcak$xSuCb}eRybZ`1_ z&I?CH^t1OxOZO`2{PM~9AnHB*?pAJ{`T1*;Oat?Okx=@n0E16cH}Z6Mt`8@rILD_Y ze&_b(W^Jof-v0RK5AGI^yB+`QsIYMdgZD1;ICLkB*`+_d#8Q<*k+&` z%ExMuQxvkPM#&=XTtmRKQDA9dT0r1f=(bPE_uoo=OVO0S+Raq2&pM}{nFwo{4Gl;w zvpJq+d2LR;Tf=S_h1U{x#aCXH_=GXHJ_{VKwBcB@Fl0@Yv*jsPqBBbfeXV!@5S#Ji zpT{45H2U3+Idk%rNyQmk$6JRE4RfoBQ~zWb=IqPT_G)Q)VDzc?V6OBylb+I(Re_Cn z-`)a-ef#=qg)dfzdirLn(;VC2EoSNQ>NHEh$VeN|y?8%1_EKqJu78FsQfCzGHb}ls zP2m2{uo?6Ip4r20xyg|1NlEijW9}O&b+fM?j8arY%?LffEgGZ?&dc!(t9TC1r=L8| z|9n;8?YmiCFNkCF*zrrJabNhqdmVs(Fsu5Ul*qsV?4>g^`A!>&-@ z^!>F%t?O{}@1sLg0To}%KAU^nRh;umEqzW{a9ADUd>($B`{3!CYw0V}$DANwL#voy z7ipWfZ{d#K7hInH2d-L*nb-3kTlBq%fG| z9APuo&Cf3E^1fNmFK>9~Xpm;->6g-kCFcOAPidk0$HjY{>P6ojG^>1)PPi#G?`^*}a^zTq z0w1xub^=FLbFPbcLr)G%d5Med;Fav~$-He9^U{`{6r1p ziMFxi&R3}(Xns;sSp$NzRKR%AHxXk^hYL2Wnx?AF@0zbIEzBP4nBwKlra2L%*&BIK zB72cEly$&?@BI>{BXG2)Q14~!poU6K!c8?9oA!5s#WL(XkB4MzPFcmZ7_&+3A&!Zy z)$yF*`!n=C>Gu7##tT2K_kCVV=afl(%S6g=7Ai|^FtVwgX$Y+5^$_teuo*z34))27 zJM{L+l3S8z?wo>;jq!A9{gYE`Rg*L4H~tje+mQJD6sDP~HzZyZRcqKzS)Y0=ZtyAZiZRc+Ut5%Cq1TEC+PUt<)>qJwy8D@E*!|Rr zxZ2d*p?ls<75M@`@hNxtJ@v2sga)Oh59vay6~Y7N{XA@rmD^RE*3GSkj)}#rDr}B^ zr%ayiVZT>klkhp-sF5jTRPn^Z=YS5Su|=hiHG`3V-T$e4y4F0yWl??`Cf7u@ z>BYYtdoJ_gR5Sj8V08_R)|;qq(I9z#&#Q-(`d5ecsq-t^KERFC3)vJOd!W8y7t<`n zO&gu9PJS=7UHkCU;=TH~HE=AjM;V4#UD)x%MQ8e(I(i=I=9miyFkFv%Q@4_^rC?Yp zOe5229vJgNHEAVz)s=&j&5`q)b?7zLwL5i9l}o8^%13;xI2(;d^rExW3)klBS){`) zp0#s5;8J~{`TJnyyfwFH>-B{GC_S}|BH`4+6V}}V`V~U0iRul(S2eAUd`Zes-1#aO z&$cqx^cRr}c37K#G_-Vc_2ba&!1UZ-Wc_`KqM`m{#qyp-*F`DqBc$gt{_mdeNqa0N zXis;spy$U=+0WmO(aUhmI%Z)U>n!=n&0=ctZ*$kzl6lj z^C08Pc5kZu#)qOuMosVS4e_Oym`Zgmb?9gh%sl%_kX1|{Xj7Q-AQRmYS% z?#E6UQ-5c3pE72x=pHO;Cy35>)R+$62-sV?Uzytw-)mT5D>s?R)_-2O zHEWKYbL_Xp)BaEKvDE%=c7l@C9QZS@BM-H$DqgN!J+H(9AwqTZ^$Xnid-Cm7^hXoDpU@*V6WSd&V*^l0_Y zNnspqj7?L=7R1wKCTuLN`b;DRU#EBo&ggYi<_t=7?R{P!S7M$U!)(!uTM{&n3ag61 z7M3GLbz#*6cEezY%s1zWoA7)_q*5dvt^s)g#LMczsCd3 zm45LyF)eYkUsNaZhAeJ*I?WmtB$f(i&c@zo+`HxZJFS!`2qR zFG1ICrz#s}94$C$@)Y*R;LU2lttIOhZ3I+YkD zUwZax-??z}VB?iF%G7r^ke8k#;E3}T(0q>i_OWFB`eWsbpKjml7YJ==`Q^n2pEpD-cBX$#`%tp}-fqXz*5zjm z$9#%Bk;?#m*qQq6xEn_rmClPDZaX43|A}GGR$YMByZC*Mft%$K;?pu_@m60dQK@-(|bZ4eif?Q@j zq#n~aExoG8x*H4%r75VLJQ$yDy7`_~iNWvgu`d@yE0~%DA|CWUd)1gL^Fn6amPExA z&{8{CTRr^NnW-=Cy`f%k;E`Q1J$iu=;`Y>bKA(kMOt6}VUD%^+>tVICFKWG#BBTX|>eE9u zbl$~Sw$Zn>I`SV6xGna>zDSY%f{r~yhSD*&H<9_ynT&20VFR}FTvqNPT!g0H1Y>NK z1~;{c$dp7+>-pw^71#B+;wHZ-j=o{7`5f++;{l9k^4eTUce3f?i@ad^h_=$L{OK%X zE|+y(rnpFr>t1uk9|A=ZkE4{o{Y5y94Ve?>*(|mvY8Pg=^!IaQ*RAi7| zd^!BXiW@w>d2RK=qb6ZSo;C@uw%jl4A>mvx^AJ0vfl^DpsWw&D-R1A4~i*&J@m zypw+PSmyrYD+ScviAR#!OeXgkcy_*J9?ZY}d9|MYW7+q~;tv+vJ>!pg+T*K#s<~~v zG#Tgb`&JR`wEnJ)a_mLBv)IVIylpfjbwKTXsEujCb`9G%d)>wLc4{WM4iM|^T%e|T zHvtd%LTfVLlrcEa`GuWdf3nN?1F!JlJnxaBQfQcccN=Yeh3fGnLlx zU9^H^Co7Lr9#)d4;1~U%M`vJWz0KqOuh?I)PZej}XWXaVr-P@1r&0lgh(d(`d2;^& z3+hV5&GELBOZtqFhj7zmW4-ff!(6I!M9{mjJh21$ZF!8+lb6sJ}-i#D^Qo`;c# z;py(OhM77#Ix5$yqPo~YNwGUt&ClxMY^rtX1|>SHB)aJ7niSaw3*6~O&9U((&zz7i z|0!hTT<&3h=if)oI;d!v;6|ZleX-i0>YYszG(5qk`qYcLf_e?N>Y-$LbXEG9c#$n z?5yq^ZkNfWa+nStZRyYMjq;ld8Z7?uXilmFax899pA=)mUZJmbW1-h=-PdD!G1zxA z_qHy}n}J6Z%c+i+##ZechsO6ADPp(y1I~W`Fu!1ya#+f&#?_6p%YdR9Wu${X)PdxQ; zQ2y#&P%XAAWs1z(-1$(b{0sR`~Q`WrQKvY3JgXQ0~d!d!BrwG2o?+fu^t0!3;w$p z!{Q^$EJGmT1+=iN=8j|k?!uyX{YApJE=IKC!%|9EiZsqOT;vPPdiwc~+0*Fdi=v%Z z-)=kcP9ABpOMDVe>%!dcBF9B3$Hj16M?apFzT|!v=I!e``0H96k(*}XFPN0P z6fQsH^QOs_4M36XVra=yo1 zX5#R8rSUCJ;iLUsI=+KXJIoB;Mr;l1e6rD~SSnu0`L*VD>8R+C!iOh&`42v=SG(s6 zn{lC)e9Ft6e}3T;`R^Pt5>AD`QwMK-E#(K&DMQ{gmXd;of`Sr@#qOpuA}m;D0a;eO zmJDPvnmafO`i4>Xq_D|56>8U7ko>~QHRCV;OK7Cl#KK%%pjG7W>dHsrlyoi^(mc=X z8|O5>_w30Rp*?R!EuVEozSFh2pk#H3IQQU$lYymvMXY0Ys_NBL$!AvC8bO7dcN+NC zFJRfW@q#la9q)JXYV|AXY@G8>B`2xSMyg(L^Dt6u)v(@(`xRogXV`GYiTTm-9~-we zJ$MTGO|J*$#<}ch+hyEv6uefd({ZD&s4BikjOfdH&e1#2yw>TFsg!VkXV%TM1ADf! z<*w@f5}Yfb5AF5Ja}|0rJI=~oW!v*R&v7!$j*RyUqzy5KJkvvJ(mjlV zbt>Q4UAi9I%;p-dl90jfL|`nJp2GA+5sd8a4Dqkn3-sKaWM_AOG~qAy&{ah;;PvJ; znOk?Y`&&)0l_fWBW;=~vChV~y-mm;3YNa3e+|s}!PN?C1PHbJ!-O)mC++1~2&t3Pc zDW#pYR)+~kzX%YtCBjSD+2-U5O)}H1q$O6IQ(`aCdGZHV|3x~v27M*2zUP~W`HMVX zGXIOjq+Q)~nQapf$DeVE{fh)=*9D$z{}O#G-C619)MJ%tig6{Q;52vA2mkR0Ju9>~ zlk=@KeqPJ*)#S-CU#weqVYWJ^_BhcY*p=tze0{Wc(?gEReQzYV_sEVaFPMF*Q8TdG zJ1eNi7dtd@=h~>};kO5(#d+D!3+(9J)jCq<#D4tE>>k#(Da{?@%t2)*zSOF2ndRG- z%T^~&M+Ro^-OL);6fxpU<&6}1d4b*;y1V3R)4Ee8pjQ2QJQalh>9ty=_iZqGfL4+-%kk4 z{*>Amy!fB*<-V&e8zU1=T<+Fg}CW}m@ z%|V&3mnt19D^lXOclHdg=Y7KclK8kRaXl@UC+Wb643SuRvLMH&113dP6{1}9oyVIU z7)TP8j2ogtnnKRqmXblp@`;S;>FJq|wvWA~mhM7n4u5e14kYi~xl?U@=X9n-k5|Th z>o>Ngr_ciXR?yKI85x-y&#luASGit3bHdT$?AIW0`AJ|o8=*sujBo%+$z7OD7<`~E99%E#*w~2@mK9yh)Jy+ z%lXPiYoW^&#X;vj=yccIuikoiLQRyd`5aSi{Go>{7bpiL`eG^zH(9<)GbpJEe(kQ% zuE`EgYc3*S0*_{Fl$(mIIL4-K6xgM2mE961A|yc!AaN1;lkvC zD{98WZ2u1c#Xvg0LL93JP%l(1D^7vavo5Ru0NLJ7uR1F1>B3WB`C|sgWm>Zb!gOMa zZTM6`_)bjx8RH)_DCEHpJ_oeN4BJNHy|&egnJN{RMOi}5$+Y&7D$btXCqS()B?srw zuT&%zRYN9cTau&-p`~zT=(_bYOLVevDnjb8vTz-K+HkO&cj)%vbyUpy@^2hSl|rE5 zKyKQsq2OuVaGoN)qBGYZ;y~^&c!ATM2iJ#ul+Fhei@3*iuoVis2u|9YChkB`?tuF- zwPD@i4j&ggrwscs?X+Ru0S@zrXHA=TIN`@`L&HCF1UPr?7k!;}Ewpau96)Y%{DvKa zcc9`a&)7yR3Fg@$b~O&K~_8(EeX$ zJM?h%?Z)xng$~jOhOPE)+|~OW@f7D^aL==*-Mh5+P9L!6i5%vfIg4idW7E5e8#}AY z>WP)5Qm(G9tQ)Egv#C|dQz}oSn3*_Fk&CJ(Ay}q`2fqtK%C$$=MM%|EF1KA&N|LOt zJ7;aBVrakLeid>G3~H|}>Ly#t=v9-vtf(bWsGWkey+PHLm6t_Bv#P>uw^r<`w^M;x zZiTufP_E!Dav?JF!S$DI84;pCzCZ({t;nVZhhxd)`r2^ey@RfRanGpx@f9g^B-`Qb003)Xch#$QQf=JH@!mI7QGO_e2tAxgGT79OdxFuG%XlwRE2 zqsZC#d-h-fRndT#MU{7!IbnxH{{ZABo0Xgw=V=OQtk4 zD05@Z-)dgV#+NqOOeb9GL$A$63Q3mN^bgS&nja&E;V=}p7; zLcb~_gC$ZLbxu(MR%99`7MY`jz!cw5su{6OptP9kj3-H%2GvwL^+SeC^Q=K$fm0#E zXuU-+kwhINN?@@_4sLyslR|(s=Qv2JXl{W`2zfBxDZZ$d9Rh?xOrU{bfjJ(oX4@!_ zjscKzsGbkPFGX0L=ZsgZn5s-5hzYR!t0xlGpEFK`j&@Ozi;#>ZSBCbME`>)Iimx_U zk69n6R3!&R^+5EIi=`GLURU$XUpwXibK;DynA@C}wj@Q$2S-EQ6X8Ok2nC2<6zG-a z>jC)XXWxTxc+duRIp3cdGNqMm&I8>O(9Md%uweE?o%MtUJyS;lFRBB|3E`nk?HaPB z7p^Dd=f4U(l99-Wh*XI_FYR4=V`WH}svx(6E$C0mV;Eb(Q|d7HLB6N{lJ<(s5*tdKPz}G@VA2B3wSNyw}RdacrEar5+YCCS63MeD&L$GT&Em~ z-s^d9E#H~OkLLOUzK8* zh>(f}PFQ%Vs$V0CIG3S3sZ`=@nL2u9!_`C3=B!HTiFub)bW5g@P0k7+s&k;8D#Gc* zswPz_l~t4IiHuMohpHU?*QyfIbTHM^Sf&dRLX&c$SdT>0qB1`DdTl+E=VWAg$@WK` zfuT%O8p3lqSfilmi!c3<_T`PipJdu1JgT5%+1N?OlVo7eiw%?HW%_3}ICFYU7pSf3 zbFz;+0Nz9_FQ zC5K#wZO%$OjByZ+XHoL7$Gf4Em0CiWWQW_I*ELo0bd!T4Oi7F)K!a84O)w0YuM#6k z`VA)eFEf$m{N$oJ96UrZbjZbj$^_?hWy8>@eY;wzo#%!O$Fff&96;6+k;>@;#q^KD zYemx`C#r|Gt+k?a*e8*WATgVjiY?jV6p3MukxA2q54S9}8^2E@hlppaysTeS@t4ZM z9U_!)^eP@%PFm$qzDtbT*_bk<3)Cfq+qa?O28+{Du}4#%`3o#DZv`AqDs)ix835Zm zJg6O3hD{1{c{C`)9mL5_^1}_XlZlanDz>v_>WWcA{y9m=dC^Ca!^8qrcY+nVs?|7E zD|kxlM@6+N9$0ukNg}GQLU|SGWa)rp2#EtMPOG3!RS{1=g^R?%^CGWYxL{6lRDidF zCok$j@)L>0&dAeV}phKoA$%1!V-_p9?K)>}UFqRCf8_Uu`3)Cv96EEFWF1;}_^$=xPfe2Qad~sug zBl#mZkKqz(=;0J8@|8&th@L)~A@`q@VK)zQ)24<*BKJh-n9R|5$`Fwn$;kAS!FsHj5`po|+hG?ERM6?OhFJce;d$w>!SvUe zWBOl(=QmT)V)~ZpD0Yy5hdwHkeR5K6Wl*S@*;X{~Rd!W~s!fpx^Cuum6eU~ijPKX5j}aEm z9F8dx!}Tc?+^&!&8L`22=6|Ub&fKVWkeAiC)|>Egd-qY|3Dai|THmU}^)1yq(SHz_ zY(G*Dl;iNR$N0;KU71jgpyqGN8x?a^QnDyL`N8SP;mzw`k#c_F0d5i zJC%+Tl{cs)?YFu z)ohr}$63Th%6UWPWFfG}4Zp}I8F9nC{@0yymRMq>S$&o#r$pjMoSq^roG42SJIfSR zIasHLuqP98u^rslpthknmK}C+AX)kN>nD+zPcsQML)~~1@+_WsbT-NJl!3Zr=5<(^ zq+!%d!>WYql>4Zshn$`uoi=cwsdVQ13~+pe;!hm_*kXc(U^fUIoJ-|H&#jU5oi4pv zPO65hbO~Kzh9O(Zsl7n;$@4b_im6w56hdOI;ZRDG#RVc*qLoeOjtlf&pxp4kBM5se zQR!|N9h_dsgV$*MPquNvkA1yzSyM@ntC@v{XFBGkhhR4&3KI=gKO0IO79=Ti%Lben zJ~>Nv2|Pv}m{9vHP*Np^Dn+pJSE$Pn-6_U=5Y{4_X&15w`E3}6uOJ_20*BgOQyt-( z%E$FDk;dsxEtd>C#d(zt3AWBJWI#IYACLp*gMM1)BaYFi`gH3S#D_B=EU?*mAWAuIwzDvocQE_Dj#f^q23G6NqJcQr{g;+ z*-?2|{-#V@IK7l!=xp@D{4;Eukgz!_sB&56Fsj()rIYKH9+CqwpT{{Ph}UTjTu{;_ zQR-N}rp1v96jeL~UdkrwhqlhWc>v)lbna6{bmcf$W&D;1<%JNNL>fm7asfg{N0~$v z*&jHEJ5S@ZK^sVNu-;hT)Vz(37%LL>Z!8gRdNIzYfQSCl?p(*7>S={EimBC)>s>->ls3ZV2 zou>K8$nDPzsb%VgIOG9UVvfq0oKUPkQie`d+7!bx1!^Z7LZU_y3pUNL(~u4j@Dx?q zI(V|-&xCdf6K)mue<&4-A`ZoqMeu|ySuof$vM;o%jTUz*sn+RMDXglQR##Wx-)auL z)$_k#nHDr&u0e&C7>cAzd0kyyU23&js}*9mO5I`MGxVp3JIsw|LRmadArCt| zMfn?P$=iV(EpuBtD#TD!h2>jhcx}>Hp}MLl$nX!&iabEiB%CGsU|aXwjmHga)xJ-N1K#fs%5ICYMHjOY_v_H z-d9W$y1J}MrFC+ePm;2rot3A$6_)6gLbUC}5RM?u6>6yYY9~;sD^An2s;Cn|N7;@N z6-`zdlANcpP`VN*lud4bI%rd$i53qDU;h9QAhM}KlvEa;9A23W zk3Ta0E5>X>+Nw_pp~-G^cDC7`+#Dn>u{Ofn3JFnFH%U}fW%tb4Tuu4$M~FTLXaUz| zI7krU-8NdL&iweJ!@9=T=P;fg1G5|?C~;KVu;;}dA{x+++Z-Swyt(obuQn*~^)1>X z{cPdo6)D8&u%%U#WuQ)nmNj-00!%26mIC{X67h!jNVdCom}da7)P3PaQ&@T^cTD5HsD2CBDS zpJn7K=$xW-lLhfH(nG|86SAg9XI9Jdhm~4oE6VnmL$j&QX~UE@Lxm^IiY8RZQ6uI> zM2x?Qll0hK!Z8J0mX$J6CGVN-+CvH_uQ`Zwipiw&WrbJ#l||Jmp2d2hY2Y$(SW}GD zPuDBW#O|7Oozn@wXVbJQc7X7oDc+N=M}>N&RcV&GVw|fcvU4XH1jN|pa9LhO9F>tz z*PD(Uj?sr{54Q>P#=cxD7G~$;h&{KfcteEw@A9JU0L8Y0k2LR6diI1nGU#{U;p&`l zt;ou+BZusn95eH_eW35b+9Yt}X%DyAFxnt_%knsW$+82s!iO`8u9>N)+z9y)LqDQj zGJoYP0_6er40EWZb#Sh(6~ee%!EXh;x0dqWS6j;J>hjfUpA1RBD0*UBrEbKxB3rR7 z(zi>k)w-JksHeKZA+mqdqTE}H_SWj$TcK6}tTG6OiP4PweT5lrt`*hQ)z^ksS65e+ z_I4`Eby-<%t5t5T)c92)TdQ?$D=G= zD_LthD=RB2D=RB2D=RB2D@xmY^2aKY-We~M)F(ssN~)o`78qAjtIFAX%JogA$M}SN zAbYvh)osXG0*4`fSm?(R3;Nl4oKSS`;1ecCkUFO!9_ihJ!yS2OUow4xF==Vy*SmzD zkkwIPJcPnf_vL{E>7fPt=tP||9D=1$IpjSGf3*f)lAbO_Z~JuL{hW;$DWP5 z-w^9zy>x8dl_~dBa&|CIk?K=c35HW$`2M^H{Rz9JKKp z#M5_BU3g-_)W~sP#A{~&7SDvaf*uuoaCO*4Y@uVtbs!pia5KCT#QY%HX9KyKK(^AM zyI_}GYYlvn3oc^Y(AnZ9y1GDdUi4t+o8lEFD9>o7TE*v<{(LyuHZAy z-ZtyQ$nb%{SZfUc{BftfUyd5RzY^kFZ{wS_D&{`#{_}?uxVho)=2EyJdFK8qq#FE_9fy z;Bdn1xlmOlN8F5X#xEGhcz--)=!VPh$aRvSQ}5r2yIpVG;P)(TDtJ)`FT^yRjt4Ak zO=}FqcG%T_s25+y3(DPXG6Uh^p>Sq*%sUeVaeDs%2Hi#z7DQHt8M9boU_TQ$fqYC- zP{NM|0~g~%9LE>HFSBzNYQkLeiFVz8o(6L?)VSFOl-cg{2vS`ZsB0`*&jG)RQQo>v z+2#KLunR+w(XOJ?sM-opyogIxZ{xfU`Gp4$1?1>jj-hoW(+|!^vY}PSAVMpJ76@6& z_^NLZpL>)h*oAbb%yoQ93gY|9lNgj&l8rC@aacYkvhWM4`AxV~JWB9>yd@PMJuao6 z;L&HJ8++;V#4!teF>v1Vu)Jvq_X7U>2CmHbg-sA(>LWFZ$6Uh#vsD?ayupOeGRSEb zWz#Oct2%4Lvbbz*D~2NL4gtEDCygFrLQPhkOivaPw^8CYk7MQ}$wUKh#0Prp%1vD_ zkK7KEM2lk#3){3q48phXEf$*YCfCL$6)SfH0WsOPnSD@JD}s*vEJ3C|>b!D;6l)67 z178#g8E#*}TrDct5Ks^F2U9Isf3Pr~8fCXd39aU=q!G_at z7jp}S5%C@dj*$my98BD4RNbMgY!?G#?bHJbJ-hO0V0cs3otW6-#_r?d1R}S9L@B>d zE_d9E_TT`}=ApXZST>E%eqhqBEjs-nx4!~@CwMO(muYAK?KYKe#c-!Lx^{}7g$H6V zsj;VNLW{E2^#BIZDWR47x8>Ik{AmHJ0Yw^0F}S^#@id4+f|@)LjgSLd@kSV8x9r(r zvF~`4#{U3P@AZaDrT4FJ5xUgZ{NY={zwvt4&@cJH{{T5ZP(J4CK!izS!B*ORLj7*~ zS56vnf`i1<{M(`07zDTTF?wt3&tZSX#jDy;S*N_$`I^|f)EfuCu%493d|&a95opaB zt6M<}QCXi=ZQI_G@dA(Waa+E`CJ?74kXgk?-mKv5{%KGx(29-YX?Iww{PECow3X}q zFKBD#`T(sz%QS}9S&R1_r+p%?#Lad7JRBEB&^)Q)!|QfJk{W)oyX=jhyde=3JCozW zvip-r=e*ZN=jrr;IBhLU*V0*f>&c_$M7C8o26= z0b^$iz4_{SiX5wP%++L+xwOAQN8d3!VyB-~kA7__`rfpo4?O_|(z<}7`xEd-&>A}Z zrsuZ{*N7XT30Ao3yMp32$I;s4$Adr@yQDMO+~mMEYXP-UiFcALU5M`%!abSu7NInR z3pC+F8pk!X?I^!9K)fztkZA>7-dl4r*vmXz9p7T5%BPeh2Ev8yjuv*pxj&8?FCLpY zOF5oqq6j0S!n-q@V&3=SeRR?Xn6}X*qG&XTLT|toQm`B=M-T?e(Ge~<7ERfcyj5(} z7r?Mq7`_>vfXdW9ej#*TzW(t%!$a7cW+=6eY8;;N;>Kl$HL156bow}a3}=jpD$dNy zt46amc$p7aux79H%5o~qsJ}B%)@nQ44v*$1AZcn=Dhb{-G+TaZF1!mQHZa$ zXl6OONz&|vv=a)1w^?= z4;(7~A##1@Lg>%L*=ej)R{;*b9qJTN!7X^wI(~5oG}8pIc55`|f+0Mk#$LA1;yD9V zUlV(UE8LFj^aiqM*J)Jm$p(f3y!ncnnJOxO65cENJHxCeX~8$GVrY)pU==o)6Iipk ztf0yZ7*T9614wV-6O-iPyI_jKyKq(=+)z8IP;qOh`Aphk3c7F_Y}kP`&w#Ns>D~9@ z$KAr-i%f@j)yA*Oa%}tXpas9Ft5o@hW%GJ7o8(PqyCNh89#uuKTPvqraI841IB&a6 z3nkRh)#7idf{#^VBO9d1U0YJ6cEr)wM8C0YTba{^PV_>}S7*cpp;}7)1PUvK0Sru} zX^)^Zu{NE!4RiHn4q*TS61;Wa=<9`q{lYy15dlLC(Yo*hK~d>ne~2Tl1|IOL8QLa< zs@525_9|LK;NljG9OG#HrnSL!Hf-?$V)5PuV3b|fdI`W>N`nVR+ftoYjJ0(1{%k4} zh=QQNKzlVNK`P(D0*W{*4vD~-XvSHvX}}s3aAWZx)R=t3mI5sq+#0O!RuojH#*KAX-)J#+72UxFEyg;itdj^JZx9R}8Tb)FK57Jk35ml=>3qi%G4|o*>Q( z^_~#D`Kt~g)OvS>6o!Bv+kTNW^;OMha#`6tgz^6XGsOJQ2lF%4apq&VVYn@kGS}>$ z4raC!!IYMs(&!;rOd5D+jXZt`%l`m~iFtex*tfwypr6o8^=q2M`Ow}Bn*Jb}#dUEF zBfP<4L6k-9TEqDAtNSMB^OhHQ(w!;ZJ)_QG+BlP1BTxJxV@oxXcCQ}Ld6~2ydEkC` zn5@K_^^ceo%jB95${@QXpYk2M-W{9!LlvPlnF}%CiZ?#c!@he>Kz>A^e~dL{Pcr)> z^My>1a3&_IiMHbTz}VhXWhHq^mYl7RpxWbQ zfYlA=`{eZu%So2KRifA}U)pWLYJDU5A!33ZrT$+LCjS7EU;hBa@c#f)EuX1h)k?DW z_=j!zB?nvlLRMR+<_B8gMQ`&wFU;>BHw&|e%>K2NcgA|e@|&vt=imI#?PC5?-amP4 zJeijYzGag-n!=mmx%?$NqE|1@A-ybBZ0;t1+?Wj7VF6V6KQp?1;od%Ba@PgmmVi7NK>MCQ0*exhz5dx zz?60+J12oDRY^x__TfTgB^|g?+G*@WFlYU~okCgAmC3ItS-S!>DjppiUrcR0&PgIlqrep)FwM4Nc8UR}b;&8!r(Q??v$h8tEF<;OG1h}!@E5Xm~ec`8}E(5ZiH%| zhi-{JLtH!EvEzr`7R?ABZlU~shdFc!YH9VIm*cL2WWevysew%bCfEA%G~8(!S8py?%_<^ZL`hDuf4(E6h=@^odQCOtP+kPmrns$p z_NRZXKY;#vDEXdBfOQl=w){-zm*w0O3B`fWCS?R0NKKp8yhlbYbzkZn8Je~g#A_pT zq%wiLF6B#)jv97$5}=E28u+geL}>8Miw$O+t`3%nL9p4IIMiDPV`&wF3hjNrLitAx65mS@oSuAXM$IViKf-RU3tnphiK4 zIwOI8{2T!*2rLJ9sGbA^<*(*n$vjL#UcWI4*Pr}HP2vn65ypm27gE*zkB&oU;g-*8 z_us+V8Xr-h<(6}@cj!c@=!QiU9N9v~W!vNSVUjiyA zBCmr6c8EjXY|7`~hNDg3(`#ICCvHiO245 z=_+m?x*)+O!+gx@GX^+b$t)j}GE}opi>|7nlIwz-kccEgaGU9YL(^7%oj4nT%|{o{YG(b%fQm^N_(X% zJX^58u`gy4Roji#2Do^a?qZch5kUJeMucH4i@ZUy&G;v0{+L#8!}aD`zBxW&t3TBH z?liGA2k9F%Ak+)2wXde5+}Ee0U8#Z!W~N>s2cWA4vX7p99?)sZ{{W})&K0|4hS)TM zlyR!5s2mYMF66zne-LyL*j5c2 zpJi9I!%tD-&B_(nk&`UsW4K-j{{XcrEmw%>YOWE1P_O4etTm~v1J530!G8`K+V6q1 zJSo+?%;<%Pc!%SHO_^;k-i6}S(Dcl&is=B>thnA74??CCGp|3+N*-QpQ@{IqL=h7V zQ$$NTYs|H!`!m3LlFZPB=jLd0E?GW(Ejly5y(QuG_1iIU0ihKo?f%b0v#JMt^Vk-^ zZgf4{Z*0Q_(+0~l^z~TTrTrp)OPDk=!y`liw;$I30DgfqAfut6rLu?AM;>XQuTCvj zgYQiYbQt1f?YB*bL*~Z$2JVTd(Ew@6U!B+6B^-CQi zJ%ht@Z&2j6nyjv*4`>G2;P^{EY0(BCea_a)1N+~`w7q~%6s z9VZ3DcKt?i{fYV- ziGmsA+m7cA`nRoS&`=+5-=(Py9F+2UC$?(V`gP0p=U0Rjbw3x>i!sMx-aAha^hw$G zP5BR6V47#9M$M|ZmheEXqR=;jkEv^pi`kp*-kzr=@*aJ%wQ+L{b6r8Q&rH=;D1L9J znIC`OD&7bc)Y4?z^oi+CEijJl?dj63iJ|T&)zNhVxz$?ehKqXi$`ze@M9SX>^T@jp zE329UzN!9M`u?8l+&7Ag3-KsM`siRGfIT&q?y$Rb@T0zb@m|uasaA(|KAkD1 zu+j7z)k>|0rfT-C3wR-pv8}h&dfz^xRo3g?mc2fz>8W`XBLce5Xej#bL!nGoUXkt- zBCeT&%CvPe46?@dlde9Hv(2p%^hVG8Tz)3%|k%*>D3BjQ1?#Y0Q3$A4N{<6B>K%eFs);K`e#<$#;~y{{{ZT2Prn32wswJLki^PX zI4hi=5mwZGAXG5sJjb#Q*Dr8hu`Hyr1E3FxxliDMMHmS~s4%1v?@KbMMzAUWoTT;s!Zz z7P)mb!5><+m~z+{(|`+Qu`fKN+62-icen+XJfjw%JZ}nBWARfK2g$!8-3KY3f?l>0 zHJxC(FiWG;+?cBQlvm znIdVnD}XRo{1rFkh($7K#e@cM4i#l>+FYq|%Ip{saLuiEc4-gZ#Ja`uG{SK)Zm zK9vH8k8Rt5$Z`F7uk-9`zkY`OaSLhH2sSf3%WYk~iOU*5qo5h%TYdUT(A9#Dg?l~2 z--ZXm1*N97nFU~Y<+`fNciJ7IO5cX1O-Yjkq!q!jG&ov@tsGV-C|#B|JZXP?amh!? z!gFC-rX^+D+msBs?JhrQ_E1=A zc*Gkl(kEaZ+&sl9#0#l(!ML6{2$*k%^gZ1o7xw1JI8!Y6LdO}>5*iI=A9bx}RrA9_ zU~}dg1Ks=gxKh4jCGhurv}s%i9I_FE$Yu@Hn6X?iY`NWfL?>4FCbL+e?cQ2xa+`4Z zBbdmT@7`WG(Vt&#J3!IW-F+PD4vs5a3AZ(R;WvTj#Q{lhEQHhvp&Tu2o=DtU1$mSR z))%s&{NWy6@x`kslp$PS-UgsRBS9K_uDM`^ZO!umCpv#-1UhYXP`-k`vMvUUYor$3EU?E4**gloz(s%1#YA> zg>-Nmk!XkZ5wBq=I2vNeXGZ{Dizs!iI(ql;hB|iJa}I*h4prZk<#ew)dex%hdquCe zFtV|V${u5>%LM_qzTxn27%a~UZwpTSfp~@wO_Nwgjbwk^c3TGC@eQW}CMLwN7}#56;ArEPoMChGZ(wGUF>8$+va3tajrfn3ki z$Dr;j9>}X1q>Vj?W~Ield73daZUkACwd*S=t;T#$%*?>PW-X7L%{o(i)?M2!>=N0w zxVnelVE+JQ)%Xmi@dtU$U$P`WWV4%7h7~7s&I;r6Pso8VTi7bN@`*l0w4#1XewArcTN}Yd%5=Uq6%9NO0ARE+_>z>KUBMkc;Dm&o;X3e9r zo3A`OTg18A4|u~vNMUtgRBPClCd+*juVVW;!Cl!_#9(wjWtDZBl9&`0hhte#7eu~l zCxaT&hS6&|io|cySM@UZ;p)L&gZ-DvsDpm3y)K|iM6^gEVZ~@c4uF%mm&`{x_utsbkd0N+Ku`aF<1U7|S+d!iTl?*S#iOJiv+G6g+!qu(<3glrQ zv_n_6d2YB~s(^YtbFHhnLkB(hDv?UotJj{V_tW;hjPL&A)LtQxSnw#d1jzF4^JBcx zyQFAb1_B&inEe;3)(4>}Re9z72U@nSn6t>E%><<#7Y?CZZ~};a$*jX^?WZBI`HCp(Ce*2uO)NQDc)_XFi z2eXE$dd=U*uy5)!J?eKG`yccF0MAgfUe4S*k(Nz;DMKTkvS{C?r(URJiPWO)RFu-^?;Why^t-kK|HU{1q>5ya{LIA8p3+ zJJh0{@%oYW7H`%)EGbG-XA%H&6x+ z4#V>eD~-jtDonM%u`u+@Cy9Dblh@ZL@x4LXf9sNl*#S3S)H-`;lnO8$K}eQvL<*gp z`iyhwP>5YR_Qw}3K(4)Q-u*cEedSf2PZS!C=_%Xu%7`_oU8t?OoEE^h-+pagnmfkI83TdPgw}&l}D5gRzYc@WDn71+jEu)z|yr474L1 z1hX?aH}smmA&*S$ZJ<7k!oO7LcwBYgv40bb>25%HaPmHcx_=S1ZOK4JhK!`NZ{7^Y zM+|Vyv}fshOS+=q4K$>s*g{7BbdX zM-}C%*@g9AiD^y#Wm@5&ytziD%JIaG8`IdY)OXd1n10^J{9osnZ$|VQTRzef8EAmo zqJQxaM-{|?1@8;$LOM4PawS;HyLdp@O#Tl;re?1msTZ|Jqgpf*KI9hMKn#rD14mXd zty+?R)YO*9lS&6)ndee0lGVVe_!IW1Rr?8d>_abq99`&!H3z}t_h}eeX1dxDM4Bs+ z0^K9-FLhLLB7gwV=PSjx6Bb@rbPF|9WwK9jIc#lix$@Z05M6sEWAxli4vEN z)SCYQGxL1ZWWwz5ExQ4U;2sP@(Ll6BCWO0%Lx~0ge{&BhU7D2dGuAX(n{sIbMu+q2 zl@K;> zKW;cdt`EeiP9q=NNbJ4yDQhLh>uVQ{fPrHH?RVXvSXlB9X2y&FE~ja@w|M0xT2<(A z8%NcC$lKFavs?VVItU{jsnbf@Bc3-! zx=a+zV8QCGi)r81aC$n3@WumLE>w?h32m7EB?+ucL^@_U33l&#lw`OKe8;tbTw8z| z>nd$|c3_%35H+v7D{7Ndw*_c`RbL;fs@wucEXHWP)%ypV+ zO=8qzcR-6#bzs-7=4~|5-aA0*b76IKi*)u$+F!U|Zkx~Kj-nx@wXU{bk5^Gfu+Vpv z`1SYX9}bB?6mf576S*~>UBn<-Knc6jkR`|5OTa7QWESq2Ya?!PMlTsI(>hbH3bM4u-uy+9HX3 zy{M~rc1&ZhzbEgf>Kb~xkkbZj$&mKUG%?GxZ$4zdXMJ5M{Ca_1J2B+EJsbCph2-zX z#L+f5KXSPJ`IlyO33g(>_x1>* zUS0WJ;89$uZ~&v|EKk&SRL4&g@f~=Qy@tqUh1m(?c~64S6cil#N?(nWyHWUe%1e4X zJwMtYsHS%L>J7ag$E;dhE0>MkE4N>MKou{vZ0===0S&ee1#9_;rK3LaqBLpjgHrig z6i(5psK>@Q=$5H^9pP7+jnEpMxlr?W>Hh#8UzQcL+dW4Ije5r_-?5 znxkE&oU8Ppi{u)OdE49EHkho~F8xJQQKwJ+@X_(ahNG7CU+ahh7BkNMC?F+{k1o@u zfgx}P?pre#xn|d=)+%a-8 z$!+t)2$+LW!&`hf%Wi9UmM3@$fyFVJ*kByKC}~q3VHefUp?)JBI>YDGtu2>r zd7Yk(t{GsO7HPhaYQ75^;lCHF0NDa3D;HHTM+}M2a#1ecOLT%>vo%mm26PAa09h#V zZ(xSrlZt67OZel6Udg9}D{^NIe8bki(}ikxLZyTb99;ZN@dJHFeP+sVwtEALulGF7 z=-&i_her`=H$$gJQ}e*e_=cYFJBEFXXY)QwNxvNqLzn|Xx{cVg)FEpORT$~kyY%%p z5YtyUJda`R+kK5 z)0L=i&L3(z`$Kv!9-{fuZPWevZ?ub({>g@a-O&AMGXY%ZXhMR$bF?=0JpTY;9}`kt zJ9o@o%x?YaK&tT6_bzdMZjtLbY@$w_WZ}* zS`%^24k#iy9inT=2r?}>Rhj<)L^0j!@GKqS_H^-*vJw;rG<4~Q<@E1j3OWdjZn@+| zbiUhTCHP@B;O{Swyt{Xnx=%UIx{sGZ1&EyI*N$>vQNoc_^*+%jd%eoh2_9i-c=wiX)h5qWk#u1*SsdWj?ltb zz^Hy=(tAoRRCm*&@Opezu`qBkI2bUrZSE4mMQu@ULi|jm6MN?x@*DKq`Qw%$E)k0= zBWBz2a7>SL6(!nOaZmHhLg}joQ<#c%aqoXllV9V|XPUSRMCtzkG{fTLAa0=|?o`&X ztaKR5Sp7#aTHY$zmM>`XW{ur3znRC5m1ol5!P9GA?<43ezR9NnX0>*Ls$G!#3MLkp9rohYLm$;OzyT>`D=>Oiud6ts4Td2yhi{Jir2ph>tRT(V%_vG=*HyL@ngRwi|$0moc9;4!F{BRdj;vuAcf&8+#`9CTnIQe*9 zy4@I-2B$i$P0TX&db|&8G-llld`+>&vu;1@mtI?nW))0AOJULk3U~C5p`)j_t#j9T zN9&r)p3vc0+p#ofiAcFe5GaDxY}{p8ubs`M9|Xf>?+U~clA7W%95Vyr01e_%9pYQ% zIOX@n!Kp21CwYQhazL=|O1yzEFJc&ozfr@t4i0yDjIA^qbkxk#3O3{@$Zwnd=a7(L^xoxww zt;;bOV@|U4-QR$+nPDC|6L9Sg3DT`EOV@r1T5cH9C@s)|qr1L-=Pw){8)E&pUbfTV zaH)L6Ei=pl$a+oMzMt~&Hw{jN)5gL9K%nLi%wzqM_uvhHA$73rD+LEKTo!kTWp|E~ z@(^~ITj~Cj!#cR!O}O6+mo8knT|&Q^a^0o1!sW|lvpg-Cnd4klP(YQ_wvny+LW_4- zuC~w$6n2Fbx`Cjsjg2kZZiC9~9o)(uYne40-o2&IPUy^lt1-kt<^CY7w|ymHG1QI? z2$UDbc}_ySA9!4&TA^44%4onqpy*iL@P#_>s4AeB2S6g|9n^qbtHCR|UlAsoej?TS z;z}!PfNK0gfy*>P{1ImJxBg}r6nz!ZaNE@ebqCS9z9#0k>e7_+P+OjTgFejqtxL>y zj@oddGxC;YeV|YV(|=q#%avbcP{eHax&4Dbq@GXWdEbfTekY&snY2^I#;=e4lAAxw z@+Y19mza;}8J{1rS)o4D!Te7f@jNfY{*ayz>Uc@;KU2X!6UF^c8}d&B@=Nv~c}b`9 z9r#mYJ)I(_M)c&x-4`({fChzdqqL*3G25x0{LN$(b#e2)lMwb&IhTO!Ui`L$mCN9i zE?)479 z8$S~rkH-s^%i(;s18lz1sDtwBDkh^?^$S^u775(ob@yhRkpk763(V3w@4uMXt!ie1 zq}pBfL2(c50qMn}*sEmLZV7BWibXNrSqP_idqlz&JD`x?j{g9+JIb~FW8|kG1#}_a zwm==$px8WyuH4Pgxt-a;0zUHU_=;<{EDLA5&T<_VEf?fk$8cWP@);>X^mX;qT`huYY#B>=oawZ~aTEh|1kP>Rb z9GY97VLeA}w0BP31CZ)pLw`i`g=x~Q{i9Ro&;xWY{d!7td!d1Xy4I6jba#U8rw%?6 zgxH9q_Z2~i?#x!VPSH_Vhn*L^K@ri3sc5~X63J)X3D<}7$Ur;6S-2aXe=_6FAhbB^ z9vgR_Yw<8qK6z_{R%G6;5Qme-bQEr+{F6&b_x4vMBVmvGsT$g!pu5z8Y&|lvHt782w`&1!1P~&SYm(UHl3x?BR5I`--}UXl z^k1ngAE44hC|qM2b7=*sFzfB~ z0lT!R_FkX`r?I_OTl+SOc=S7-4@fQkN7d!}7$%Nf5dw~9MVK`8J9Up!hs7SBP=$?r zK8SS~ufL+bJ`YAlS1T3msEjG$9SeEp&`)=?d)d3BRA2UT>1$V-B!WPtMssbz<8=OD~IGk*-J9?a^EtQhO@RaIn7DPpv(E>bUUivWJgU^gNeWZ0qJ7 z@x!R{wbx%=nBF_f@iN~vdTICSCH>BByLIQGc4{gITusn5IvQhumttmQg+@ zC1s$d!<6@581};90?~3f@xMK%-D4in*#7->qtyEJw9mr5JFRg3JLmL~Kik!xeK(Tq zJ+!5`^4p0u%byO<+%@iniZf3ZofC~a2bc7CF6GOY-&Qsqb=^HZ?#P2*Zd&zI*M0lz zCiP4|lJd*Wv>!k8HTMwKc380X!;$4*Ke8+Q($X%2du$*3=bK*-%o_OUyhk2?|LUG$ ze+_K*eieFs(fb>^?dcyozi`u;^8fg*t$ig^PK&jzdVbl!LB=;HMws#2Umm+@&A*!u z7w1d}y_sI{+wyIlvIlnG^-{Fcv(5c`_d94D*i`i4oZQJXAIR#x?49aGk*SjT>ajy5 zBaJ^ESl989?H|{qlpha#({ELA`<~f*3kH3@dHj&~|NcCAYRj)1-^yHd-StC?Uz)Y# zxuri{pK$WxuZzbwwC}o&$?X_v|H;1O$&LSMH@t8yZT{horuT!tWb^NDY+OC1<5OK; zfAx9)#@M!-1N+R}S?;e|-R`3YYnROK@al$FJ5PQ3fxkDEHobLW?)Obs&wg3<^xVSN zXYYI6wqr`?x7rpg%jovjKgHPz-wz1JPxfq^pxKde?9-CHq1XPnz45>)w!HtT?>q0x zS#61T8+PE0`V-lUHqY4kqwbmcOKQ^g&YYmz|6J(rSH9cz*zfbE@38r1o;|d0z`XL& zKP9!Df4s}C2fB{BwxDg@E#se$j~!*-GU~#V-OFEnB6CggAR(pWr=t$1JfBFtkNvk< zQ;@a0Vcfyn+eUpKsyjSYzjIokjc{Q9hDBT7&q=@bLFtEUHh9W(lkSXoYyZYY(PPI< zw+6jaEL_bvHTBR()Al{{-@c1>?lk{>`rGo&rhSI?V)jAaDBgX(%WLBre;&NI_M6a! z>S;s%TfZdpooS9^3nz_FcqL)|ST6g@zDG6t?GGG1mNM>_0T;RP?faQtpEv62%fsxB*GkOR|B3_L^%t&1R#&|@ z>yx7Vy(^s09h-Qz>hXT$_oY>NldOMipY{FfJ?En9j(^9W7=Q4uJE@ww*q7gwFYU5> z>r+`@cU_pU>k?DE%lLA^l=Poge9fL+QSjpR86($iT-tWbz@X6W+{cekSU77{=Zndc zC+P;}tQnp$ukp1Q>dRb@*`-&p>~sP8tvTe5LT$C?ED#aG5y|7~3L z(N~v+B|ql;_H${6BM+DMsBTlak1@WubJCP>OZ@g9wWI%>_U`9*+x;c9e=mOW6YpoG zaiw%x`lv6z{PTFn*B|u$`C{(Lryfdq{Lh_dW}Ya=U5Y6Tf_Ud&N|3v^rFK%=P4%(h+TDd~Q6| zeOCC7@sFNe`SaX%WYos|GtU}3EnfGEKcbniS6ch)&|2q+*B^R!RIBffwPAlwu=~|V z+g=HJF1|kb?!_A`kNlWBZp7aUsvqS$ewOtRbSUPPV?Q*;yQX#=7v$ul{(hTlMq@JAC@iq?ccawJnH_ zHH*4)qkp_LVCL2EoX0!Po9dWat6AathwryF!*{q%!-$*V=cm4~H2I_NtIBT#7vC_w zeESXOiBCo(%=1_2noBD_-1_;DlilXz)a3dO&Hl)<{e#l@-K%YtU-Q22)TI5U>lkK& z@sxk}saLn^3O>Gb#s1H*zizz{>6Wp*1PE)p_RZM|@BjEj-^)9`y!k`B%zdN3{OP?F zIVZXuzcjc_LqXqHIzRG+?%Y# z_XAVjKL7hO=`+RjH&51ok=@W$Ywl8e_ibC}Hp5Huo;VGs5^qMj&P(sHV%TGAmZgN3 zd>l>s7GKfdr&*PKG{YXx8MA}tAF#{ zU)k!NWq;QG{^O{#;|KgQb6`ck*ed4Y`;U!Y>x?}5OZ3wYCCd)ShJKgv3csntq$l^? zdCG5WU9#%)edx^&}|g0pfP&tn_)4_F`T_CRgUgEMx#Z|M5>LhYx&l(rjPrdj)T`NUU0 z??y*>e#?6P=&3J~7Z?w{|Lv)q)u(q{tvNW{Iq%os4_LqV4ewX`)`HN$i~Sb1P9OE@ zu-k^ ze~>MnUK)P(n+?{!Sg`}yvYsCV78mhGqh&Rf*{t>v#zHU;Ywx_tCn)#$c6HYpG9 z*M6{|=6b1eVaS2j#m&3A{#4)Y^W)^Z^o@b#f6Tr#_{HX?nk#qiJpN?qp)2}9UyOSE zM@@%)KVI*+u(HR~-_)F_ww_+Le|_t-=1s?2Kac%9;Lo}LoNIDD_40vR2k)F1Td`F? z_}YUjN_DL(uj=2B7PKxNYIW`3Vw?TqnV-malSa<}YDHmna>e;^BU(d8s!Fq8dj0Q* z$_@=7Ph4HCYpR(y>e#NqeZSzR)E>*3f9kxu>Dr;Z2Oi3Q`l|G7^tF48e)_r-O9rhN z6o04i{KNn7PZ&FV{KH1awmaTwga2sRv5+wiobaDI64P77mr+Nbjf`U~s39DF2Ybc*Lj>ESKDNWX`I z^!Rr{_S)W4r_VXElijxpC)7_p*)96Q>>HD;d#9dxGX4fB>3Mw3gy(g|hU~q5kB7z8`+!|7H1{mOmBm6B{zyeVj43 zkKVLu{=@r*LIZQy2R`L%f-@s!p4R2p>^X2qZ+;;4{%{^w`upWLQDmfmD^@)ssYverh!eKWaI`!I!rAcjk z6ZTKt{_1}Vx{cX1?%3Syv*w+d|GB>0-u}hMPWm>qYdiS@!yL|gyuRr6L&d$C{Wq8Q zTHQSSmFd2JmVfnrO~d9pEhD;pwr0wdf8DdTt#RLDiXQs8#anT6(gXJx%}b$%t;?2; z9Q2fcdw5M6^6h7@-0c0@^0ez67GBYfYyQ6V#JS5&g&V#&o8DvR8TYO38=l{GkJ-D> z|K|Eh^EcjV^TW7s!!{J}Q&+4`2->r$0t|w0S7+t)=+^gMDTi%Mt|I92X<(3_sZk<{8T)^+}XbAzA$;_8)WQU)A+@W*hdYJ^rVajZUB7)J`&TG^SGW7%&hbOzBC8)m`qN zrLUgFL#65t*L-1EyyMpC{NHoWRvJmIR(ALrJ- zZn&G?ym`sqX6LffZ{ClNy2l*vT2uEy$9g_~aoaxwT9Wf#Xf9cPVtDfd`P}rcsusN1 zue9Q;Xfwa8bZX81_lyneZm0K?S_>-EYjQulof~<0$HMTSg!2VCBgsnSSgSUvp#7#gdqoU4%E!AnmS!3}PChKsFe`H`%_5xX_X?bE zZiHj;Nk#PF=T0ds%6^+{cI+t}#!*gS{aV&3q)t>%?1^Nhwof$MSu?8y><2{La@ypE zX&l#sRmO6nY-aHyrzcM27B%8w3@(3qm<{r#ph!a^yOIQKvR_XSF(m`1=S*AL`kcIH zxImqpDVUYY^$ggO%-VZQtW{aQsl^xJyoiXvGTdmhNO42e)4PI;S;?H|v<|J}*=Qb{ zoUVqBXEzXxk8=>KT@E|y6U_qQv`8VFDENrz337#8PqS9DMO#sKk5T61dV0OGgT>u}|N)`jjF;Jx+6%O64_S9P!d@)ETsJ10yW@*5HkjRQ7DbXqxwby!r0_z-8 z;N+=ODCQC^TF)b5MZ8hTYrVM8DLhrWH_TS$%%_NmMJro$qz0B}OJ`zH$8~DaxvV>R zqm!doR4>)D>t(AWsmLlOGHg3w!|5mlkk)Goc$pI`AJJ0(kZ4OY?td3FxXV7NVev6J zU?Y}V=|-<$jh5CJs`79f((_K&ne5!Woj?jlBXLd-uho$$;+L#TT;Ds164B_KTa+gH zKF|4$b^|XFg3OfkbL##<=3!S33 zNaF!I3wV;C3y4ISf5kd9+CszLkYI{{>ac_H(6B>N9K`In5bepCT*TuRRm#9?V0S%R zXvaxqFw1YCe318N7<>^5So;X)E96O*Dj#OY6$_2mfs#&vbqmG_XNtVq;SQ)NDO$-Y z%G^5~OX^ty*r}jZ-425FgmXw?{6kNtMg%{k4yc7pX{>IOdx-yRrO&2 z5T*_EL`F)LO_I|j7Wi=ykhAji5qDpo|vk9jBehG0W0(TfO71j zUrQ8l9-%SFR{LH-U(Y)^77-6|Cc=Bc2)9FmjcDnsg!S;&y2*rc#6uKtT~CU_p?5fs(7*+IR<;m9QBVxTsuqlfx&;pryutbAN%O5RqSh%mEh%=m2j&I+M%6ZI7JkyI zdrTAL2JFBQZ2L&J5nU#}Vv|Lks9$94@M`!h+<=l61gi^C`)XTb2-OSCZ&s6BxhQqxpxH^ zXWig3eB%2WNXu**5G9rMEWiNcjbZy%3q6fc85P0n2AE*FM9qa?qyY_6*k;_NQL@f8 zW2JKzRAAi*RRW__$-T$$RN=j_?H0YTjZnOIAz%aPR1x|=+v5K4%b%DChr>JoVI&`3 z3x>Tlka-Q8HhSDiLQO%=@V4Ag&*skc-@wOc<# zBv=3wd?j2TQAXkB?5Kb=pq0Q~WiE@sD}$ThHbR1f!G2GY(o(jKvr~@s5Jso;%=Je2 zrn5SDl_?Pa{Gbw#=HK3dl)xVVfV#PK@dC#Zi@{TnsYy-H6e2Ro?0-ZOSl&WioJrwy zA>Qi}bSJH`5^Axj2(@C2y%y<0;=V{voXb1|rvt5=(_;-W9Z2JbL#orJTG`A>R&gcC zWDAltMOu0(gHc1XJmv#wlqJ(uIpmDQ8e=JEZ#9DxjZWBD|ZJ;xbCRuLh!SB32) ztfc`*nI|H6F17Qc+kk9p$@HasEW#P#Mi8G85n{8U0zH%>78hhNOmb3!D34C)qKFr) zc_pglkl5M&0m+RCxLbkD$_+bFsb3UyeM>7Bw7)bU`l1Ksg(=YJqAR zjU!}&5k=W4c%1VEgxCgI-IJK9VX~4C=wv<3M)I|?Ira&#?{f=2H|LFr6)~rvJrFCW ze!Uu5MOzN!H908MCp$?x!X)|oWo}6!~#(X@$N?cVV zLgoQBoq|pg2mHR5hmf?OA%HO)>$g#FP&lLoe`yGCv60iL89E>i$WS;_knh7v0f#CL zLGiwEJrE%IS@1>*r{7(yh~ALs0kI-HjqMey;;|Cy*HQ}((8?G~i57Z>QL7|FA95OU zV{^pYY5Eq45r5qKW1BUf4ia9O;e9FYgPJ&^2(P!X%jZAJVWRku~_X=N0Y zk}?9@nJj#T%Cw$FnWt`%x&<0ph=2qfau{h7WvdO*fbIQZP?IIrYn!a1pmLq?9yd>0 z_U2aJW9ob~fN98m+W@b}gDwUGcKRKgQ2kM|d5^-ePSNPX_=qwar2^-L1FA_5k!Fc_ zjc{cZXc7g01YyU^sPq#X)P_SAD;kvl8HX^de}Qoo5X}ltgCdmD=j44GWaK2U3^K3! z4S4#sa#&4HJ@3IY5;rX@zoUbIpluC;VWI&ii&aXoiVI*0Tt`?vHi8d?;E$HJf-gjX z&mQN5a;@ri*rA$6md)^4TW{9NR;|pJ5cAMmrk0IpZ9y6Qk1og?T9sI9u;!oy34U&ZIJOH7zOYY12Nm=oV@)Cg3nT%#;Sd7DY*%ArVK+@LiP4Z~JbRPf7W0HSlQSQMQ>lThBkx+NFgwGO zkQUI&A-g-j^0Ws9*C`G-AZU>-H6Bv@S>gwP5C(^A*50=pBZwr&ZnE}^?GFjw&^rrh znNi_Ap?p+7Fj>tRs$o%f(yHVxAn*5fXG_&-kjv*3OPoSq?f-^Ot(X_UYdsW=-VkL( zCF&slTvDU0$Q3tbqf?6j*qrI2XbQ&jVv3-J@Iy=}ov2yKbWt3{YL|tUO8|#g5j^fC zoLz;odUiF5G^{69AltUTT93kgUnuo*byehKgfl@FP{%?5?5*79xN6xUA(Nho?8=c8 zpBjE`+dZZ-S_^!tDhYn5;q@CTt3CHo%}PqHq~uj9^CiDw6GQ}AIHg3c1pm)8>*PG7 z0r7xXXfZ$uzV#pWkkbJfkt_|-I!x&m=p?Um0bP<64r^5qnMDZ^i|-^BtqkfYwV;No zm{qq!@}DXXRXIb7eD3ABjp)Ec>KCEYa(^zVJMwLlCM}HSqYEq(4trS{0m(6a$``s4M9^x@W$g7JfWpxgMv~9F#EOgcE+SiicOjT z+p0fq0m`b|K&=Rh<9-fR$EjaO%yuc9Co4E7WEDY~DVjD=k5ho(R1x73OX3JhJP(FN znKO{>2{Nar(ERTkE>u@Un^9;YLMgzYrD{wtHjrGT7CpMBO2sbUV-{7$4G2~+U|W+e zN?s?YiWF=+p;oGG1O?d?DQA%71lg*`3qg^lx5j*K1cXnI$!e%=GS?5@*TcACs|bU^ zHln(2km7iUT~q3)h#PBE+aVs3$U^;8;)sV-$NhHt9Mj*dR!Di0P0DXXR(SL`gS&Bp z7Fm#Cb?I%@ajSm+bg^E6Nr$XdMwC2aW!Ll)%MY0TpWhMDgEXiW2R;M6da6l41BD#P znKlU(PK4hwav2dhn~ZQ+W3u&GH8d^uO%|IHx5b$W+ ziB-h~hZlli(aX;zOMaFpdQ=;-GQv~B`!F_iB3iWjs)mHG5eo}pv+d127>u)9t11;G}LFKj>pPa7(&jw=AZ0`1B{3&$CeN!4r7w>7FF zFgJGJw1FD#Bh9S!I34>>pwmK0M@XEiX{&<(;sN`9%WT@DHp+HVht@2&GRJTQAOu#b z^BXR~HhLrI9=NkL9s@zdih88SFOqUCfeLdo*iQBX^CO%IeXTQBn3)TTE0|!RMW|~H z+YWFQ#2+KMd7JJryQ#UPHR@0WA=*M)UFtaDw+XJd#&gw*gZ=@`1X3Qc5eNo1SJPF$ zcRnZ?0=eF?XqN->U|+yNovbKwZo@ClX?ll%WUm%1JKBX$MAkHTq3+ zb(ezxJhB~t$C$zbCaCemoZlMFN5I7@CEp-J-8~{Egdqj}+M57`w%>)zsQgrS>`5ihiOer@OD;z91h1FLPU?Vrx9#A#;7Y zfO}yBF^irJ3EdmOcK|%}h6M+YT=*5)$Rwci3Q@&koFU$y=9+}asNJufc zc)-c$9%s?Yk!ls-oDV^+mBL!8{Jz)166*oU-=Al}rF;j8w~}d%JN#Ji0S3W1r&xAt z`hEKp2Z+RMlWy2#KZ=0E^C+|>0v6x|V{1?^Vrtzpo1#igMw`QxsfU^0m6SArUThe5 zTdJ8kTD&3lK{nn0v(0;mHNOD^ZHV7vkS#qKWTRo&5Iq}h#YqvKbz=!+Y!ec!mMv=b zNijQgfr2an1M%n3NRUVAPz*9`$KMsb1l_-iau4D{a_TB!7Z7h?0B-3;o21x5Lrtdq^y!xPln?Zj}g1P1${ zeQOkLts-Zn1=xZb8%RO|qMXRB?1E{Ylh=_q!rF^9mD{R!%I4XGha75Yz_!%_WNH^q zRz!MPMp4d>bBdK?RdA~@H-&hQ`yP{rqF$w>Wpu${n;;IX%xTH^j3g$==^{|Wt%@GF zgoaW-+)4-UsTE-L5M?IsgWu<&g0f|vpr|UvnL-E)%2n=4=}In=Uq_XRYDlm;4o!n3 zN*R!Fv*2-ZMpq@WrU0EV%@S@%BM$H{+sd($(p~hqIC`1-Q9s`WEzzWjz945JsPQ>I zXEhsK3d~-an2{FN79$$K7U0HYup7|G6VY^jDo$ss#>krAONx|0LtWWf5G%K z1#Ya9#Prg5uJ-13MV-5|3QUV0lHWov7c%|BdJTdW(8EQ!kYNf(Lod-3Z@b4Ff(lmp z;W*B!yR+*&s0BJ(FnR&_)}tknnrNqDD!!0QP%F9yAkk?l#f6P_f}iIiQsS?EVFcSlvj% zj!ix{=+1dvBD?M$;~{dL?pRuKT3BQj1F-k46lNtcT>CCgK^KgK(Lp&dY9wPvzizwF zHXy6PS~6L&Nr)cSivvdK7%I~|-ruXn0{{j^8A*!VYPd*pO`f8J{$a7W#z3raMI?#; zVG_9{(n<72h-FSaJcbyAZR-ZU_tUY+M6>A1axVn#Fu63(zXD8-P{eTm`%K4dBZQZr zE`_MS<8`_{OmY{$p%`V{Vwb}V+axQjC$j~YG-;QT$uN<1 zZNd%i$cUZPZzDPS;eRP+{pAn)&X=x8t^mY`l@wcL_+>IGD45f*{J+C=U%E}`?olag z)fwwOW@Rd8;>7>0+R52jY+*8!X3~-`ybz3%i9BR*viWu-UQ6yc9SQy0|hu z*Z6WD&23^bcwkK8>=Ft?7N;pnW>VWH6*$ofU~zH|*GDhsXdA+}4|sy_y`!K_kvOBw zd1SMWS`lzowr2B+A)<(6TwhK{1QXQTEtZ!{VoiUQ%=VRLbYA<(?D)x(4B>{qG7ArZn-n@W})8FzpW9^~! zJXr|AX<{YoQo#$6a*AFTr>%_U#t3MLjMtP?zd;_I%oJu}A&xB~qR+X|GIO?{m8Yv+ zad0_SP>hFU|FacQdnGhLK{I=ZgqtgT?so!cR7HBy$$2N@W1=UBiFULuM$HbHvq?=u zD5sT%As`t{0mGzC;%H=YxaUy}Jz#KI#l8LZ3zg-BGf-tttJy=N=xN3+lLPq;9`|G` z2r$QNj;N&!6&$j>yA_$Lz3;Y2ni{AdyMM{L#Dt^*Y(r)9-pE-0tmCJv6syB&bu?-f zzREy31Ll4*W<<&ym(sGI?8fzAmsOtw+WVG~QoDr7 zPq-6pny=*FremEVd`6k_D$C-F6mxPrKOAsKd3rg3Xo zH;L`pLTn%=x5bnXI4YF`y}YsAF5b7T(rk#nT|zk!QB<9VsDZyx@J%m|>s6)LFW^0+ zoA)?{(t1Haq-+2-=7cm4P!Z(_!=wV+30jX^(1p;8HnVOqaXVN3T{KVYo>PM45i?-H zqF8K2dQn6;yBu)1)uk6-kY~|PJNYLfLe*~SkI8<$q@WTQq_BaE&OoJM3mC1>Ef~84 zq>I_m)OOpruXx$Kd;jj8s%eWTLn!^-vpFqMy+{9ML%}fkTpW7|T?*%mh-JHI*dU|o zD>jO7Kw+{OhO}pk+9z#r3nq7NZm6Zi{j<^8wB!!u8}~k31|So&ouVLTk%{P~I6W*k zgpJN|GX)*tXov&)Oy4J2?=f;flma$+IIAie<~Y_4UBuVky$E^%7fERtg+sA!4BKtO$vP|3xrK5q(frU*ZD46(Mz9EyO5TMQEj zl+^&Aq#0NkPhd$W!RX{$RR#T!FV4-^A)f%XJ|k!!&*rOEkQ$+bi#W` z&C_Y`u-lT$6l$IwJk2@-_w%O;6ifWtAe7QNKbYUJ^-RCL3xk3wh-e6=M*r?AIqXoe z`G{7YjtaOG;jn$XJy}B%k_r-eZ;&f@`X39;y0+PG+I|7KI==veq*g=$M>-@94(X|) zztGfN3Eb^%P{2a8{bD<(;BCMWI9fvXZWyqxCso@wKF6_Yq!W3lxiQ%ML+Ktb3C@q! zV{>*+tU|SAkU30J9n`O$LsA@9Y8o5;y_6+x(HMdZL-_<>1F<@Y6=^g`W|l4n2nQFX zwO1`;qYpD=6FE^jr^2)03YQ`)j$q5qhFA$KtHn8>I!yq=FuSQP*5E<#P^B@03yN4D znA1qCw_%xB4J53s3>&ClE2lC{5u+i)?^_Pmlu^l3U1`x%v)ygqxwk|o1>mh#9V`;) z*Gl1TInmf5BSCY6DDclGSNrTghD07$+=qo_MTz=eXY!Wr=4j^b$!4<4C0yrUZT{=u*})+v%o>QNx9QSS@jr+QRf?K zP&|qb_^~=<3!~KL@RLQhrTpvXKi>s#wmcBVw|>lAbP=CoA=cgV{hvVQyywKF}Ev)&JnL3Q^jbpZr+ZOErffyv9VT0+~yK4oHH+s4x*Oc&`HymOLes*yr{{es3b`W zzG)1m7?U+=S|A=^m96~{Z8^|We2y!Ikft$heVE6%*DprB`n=EJo%Lp8@W+EBU~9mN zdwkwlbh%m&K0*W8iLJZS?%;EC5P*Y%L7hpe-DE1)@5=X0rf;0;tC$*y=S$a z)u`2)L<3b=q!FeYTf=rFTIx9|DUnGcnOcfmJHNhufj2F|T$_w*~p}b(Kzcn7W5hRsNVbd}g@`2U| zQjwLM?PPFi-L%$t!-baJwZRsBg&&$>AVB}faw0e=;<(H$dI_fk2h2%5*Yrv2^Uxz= z0j}`ibQsKqy38(%Z78{9qBRwrhop+0*i14Kw}^+_<9J8C)!t~n-q2LGDZr9|9zM7h zYz1d14S`f5>*Q931W$l#cj=*IR^P_42NwjQP4SCTZ8#A=!~YB}Z6e2}Wo6Q|3mm({ z>x_%B`f0G{khw)rJDLNkh{PgYFmBpNFO*Xkr*m^Oh?k%Xi7H`kYMkiFxyfJ%Cz314 zy6K0g*(M!Mpl35RaN8JsS7A4n>SYwm2joFW>7 zg72uHbBVZ;L6g8ZZJtO*ngYRdNco3#kl%oVOw`Jk7)>Il=@Joq=hyeb-qFIloJR!@ z6wxe7H7w97o3mStxPrC343@pB!4$}t$0_tL(55nM8y4+R7l zR>E~JOtLoZs9=h4Mi{>q74;}6@EG1YqaqDEc<{s%Dfa~N9&b{3K-cFKJ?%A#SzR*q z9tPxQk#D#vCqC$48-rmR8Kx&@`^uu^KB*@-6FwN>uzf0@78_L4dcY5Rd<}{y`jiXC zE9<>Za8*R)ydky7JqK;zx~$i(On_-SMVW_uY%?R&Z@uuf{ z(Hj)^bT<{X-%|jC+WRj%39~MsBki(u!zP_GAVws@^k6es-6g@~;uYqa*4b;QTVOxmixDBmnvmdg@w!P|vDrk%NC|ok2g|H9%}D(EyOWIs#26Qi+v-hFP#1a#fhv8d6~!YlQBN{a z0#$O{$Em)7a76205* z8i5NZlE?epT-qfzf$0eu3XhRN4GXSxh3ow!CPS0>h|_F?kV2$L5yN!JN-~Bpx!}Fd z{5qSQXO{wZXuwAN(8s>Q&^Yh?ywZfs1qIe^BO#6nYm$5SL`x2=SML6gwC48L+?@Q4tSy@21ewE779BYoC+geG3ev?!1DRVnZ z%Js0uZI>--O3*R@2eKbd_9`M-8Z1kEXduHKvLbGP-W6tPxB+OrS5)S?MGv64RrxoW=?*fn2$>WX0nq*aGKo7>%yds;7)@!doT;Z_Eh%cB zL^7Bpn01TbVo59Y`zQ*yAKIw@87r?jT8>uV^$`J7p+SCoFn?hNT%w8%?W-I}AyyC8 zRn%*;2^8Nf=&sxi*(LNoR$L;T$*>q{kY__eX)dkNZCfe=pcvzN^_{;t&!LOJoDQi( zR}~aJ8SJ`|X<>#=w2?_;WK^;o8@(N23mLX(7{`WkhGV?<#Ul}YuTyLXq!NBW)xz9X zVs=RIDqvd=KRk%Twib_5K>uahA{vY4GK>ikJ2{e$Hpobi)j-VpawB09Gr6MSP;U;a zHUT>zo>Z8TM9gqWU=`RItC%k47^pQ5;3#aM9F8c-Ka(5Ll5|Y{{lc zg4d0<29|j=CNNFaEIG;)M}uW4ax3a0JPmRvRyx;Qe&|@v=JT`k$XpUYM7xR9sVx~$ zkeZ=M&Cu!!g}&gjP3=?tY_V(}k!lr;5z&hUftEODkSzupP>tW69v7ik6v04Lu+b4( z4#9yK;_L`XnDy6m5`B@Rp|G5Vs@>LeR@~7oKUXA*jdG46jU^*3TQIss#H`feTnaTu zunkl}$8Sa33Ws?%DCm%fm;~WcAY67D*303i1$vZYwXXx=v^Ti z-AU^mNrVw~Odr;t($_k-ja@eDi3MG%jTGR!5OB*S^oyx6vVy;C1x5H3(SyOb1)~!Y z&yW9aFe0(q2>QwJ3!?92R%a(mu%493`_75paGw{Qj>N$gEi8ykI6FCP<{(WTH`hz_ z1{|^(*T>n)JV6e9i>fMa-lOpJ!ab&@4B|oq4g`{L@Eg*Ai`3jn1<7nGCqQmJk(4q; z@L`U3S%n`)O*)8d2oMqmXhp3JuI*r5yoU%T(3^1RolQT>aKfsU;f)#KbkI`- zcQ1ByCP{^iCM_cwbY<7~_#}k!bEYU&v%%ITcIRLR1q2!a=x!Ojru2?q7vxl%Zh=Ka z6tO2O`N3YqLXU7GncJ=rUN$>ACnptT!0iMd+JMo_7jQVj>!O=124Z19T;&#<0f7+b zLAFBNA?)p|)&wZNCnSKkCbx*NAOS#Y7^h%~Vkq-;uoY&`KsQ!|)yHJEvzyaQ5U2%^WETq1+z6z%=wsm>EWx}EOmY8k9x z7G$!C>cym@E}O%*nqgTN5fuj{mFA%eI$@KiFz6I_3@ua75`q-q?7T5#_pA__bU}+2 z1X2OVm*X!dm`>4#YywR29 z93Yu(X*XC6Gh9Q~FhyApF1XYKQyVJtNAtRnQg^`23Idx9=Tz}M~uc_|mq zkBeRe*{ZtgAsmzdk*8olP*m&^)Y)P{3in=%h|FfTAVUoTlblf09{bsyp5x!3b4G(A@&^WQ4xDIU~VzocPu09ZDb7g@l!;!uL(5 zCL5RpCNYcgZPw@_f`{V|9V#;s-ir@91-2Hg1~EG}6zHO@kRZ3{MUhcd?s%f{=8yo2 zEJk1q&~E&s3l*{dVGV?+Fe|Bu6VZ*PWwKIOV~mKV452Jr^n_rRLm|SxjRnpas1u}V z1@rN)BJ6VEYbtRJBQ8=!ti-O~HE1qkO~Q71LCI8Ok-%bQBS}%K3gK8?xGn4EVO$SQ zaD&AlW=7`?C?t@_3reL@>*gpt$G8C$8b_ayN8|S|4rsXpor8vf+)*FNNM(`=7#m?U zoipz*L~}`S)3bt09VQ@&2&&GFvY#Opsgi)gtTdcfQK1MP*skE95wS+>)%?HTQMJ56 zn+*4u^f+e>g4>ub3y>x9NKz5H$7GJ_#_LXsIyZ>M8;Rg`iugL4_iUQqK*Ny9MzKPb z8J=E=m1Bpe9|DC41YfelYAEaCbgMN5;EFm0Vts2^tPCb*nv1&e9^xo+ z3T#9Gl|4aG=MsF8(E+Vmf0e3n%M`(MA?%5s#ujP}OC|BVN@elJU_AoQVgU5`3VNuzGV^WioJs=<9JrNNR zDo`+Ck^%;5HdHEyC_e<398?4q8{tcc87A*%8fPfNG#_kLM4ysTr1yNFrDr?F^BR@K zjq$n$1)TR12g&R~iK=lP1z167xU|gvXe|QbehZD{8xF+qB^hiJp#rdS2uT*1L`K6T zB*UyOBxu$O-?G?Mmbu(`Ze=`t=fLTz24K#I1(&R3QxG^7P_o&RqH%?qXp$*g9a0#V zKI^3$1_?f+AuYnTRT9BkNdX9+jS+IjkeHaMNq7%Ch?p0F0F4SRa*KKEITSqfMQoB#6J*q29 zG67>IF(YY%YlaCbgx3fufNciWncolv6=+1QB}`-~vMDotOaub}L^v9RAGX3QO&Uf$ zQs&@mGE=}YX%HgJi8GiqwGgre44dRK2mL@+M+SoisPKeNrJnY9mF-NibS~jZBU$gxw=O3X&agx^NBSt0r7J za41>zi8K2MVjX_GJ8B< z{N~$-?>`|K&ru$l+PtV^=krgu@f>peQZwuB+9x`89dRh5_j@}(yuAH#;(@Wn=^q?S z`E`3hDw+P!~ zO5ctvg1odjF-I8wdL(6sH((=GASv_0dngZqKC)a5v42<^cxJvxz@n z88~^_?37#UQtL{t9qs;k1u=Q|CNBJP^W4#C%pOAa~W@c zi7fu{I4BeU;St0pbcNI@8RsWs!vLmzSGJX?t9FP z)yqCEyZko$TL0dgfBE3@!k%f5(I2+&&J2ou8+*Q&{@tsaU0G-M>^uA2Gmfb#^ZI@g zZ1?#hw$rj-29EuGfHt$!#I$dwe><}6o_c?;iH$Rx9$)x!>IoawetXzFtKjOuhZD#A zI`Gq{SL28N**2?x?eY2W*Ut6*Jn4niziPidmgU&>%IwlE-N*jwO&(FU#+pX8X=_H9 zw2sl2Hh$Z8=Hz4ld+-1L`F^j$|Gm`ruWo&Az2)qbx5MjekFM$V@n2s}`0(#j({F{x z6rZ2;%*63oJL9HtElab$`pU3&;u!sF+h(7=d?S z%9hHJ_y769pw-XUp4b_P_y0XsqW#GF%hOZJ%m?rf?Kf=ti=Db18Q8D(hyON=zdUEm z+}HZm94lJ)+O~A+g)qf73|X8?RMwDPGj+(mZl7IU_Wt)B3W> zwpCZ>FIzWh-0Zne{4@5e?43LKmEB$XHJ{9r+4RnD%+m|a2l`(dbZ}C)ZJ)1wbV2u* zge}jPO-|0byrcu3J96r~hnqT`eak;(<;GEe9RA$>^C}ec;=WqRj-daa#1|J@xg z+NLerl$^Y9_J_X;D;ocwC_6rnm7zpU#s`t@?7I80a}GduQ7D`44Ov z_-Ma3yAMy(4O!OMWzh3eLL2*c+I1l1hXMZ_9x&pI{QVnyEP3#^+8>f#%ljAqwq(tK ze>?+T?fs|q%veV4+lOm zB7H~_|FnP6?Dg;UAGcuH>T927;bkqVIBcxwJz?sf|KiJc9)i#l?!`I(Vad`j7A@`4_3zjNYyYPDbqBiiJMj9V zZx=0X9iI;k=#vT!NJ_+?|9cRH15?v58!Pxh3lJ7?C3pu#Cs1MdGg~m$-}<7 zy)4zz;hCpLtRJ!3H1ZSokk;;B{eKjFi$Bx-`~Uqdofi?{9ctkN4}kp2ts*l|rfz0Fn-G zcJOnu7r*vxdbwO;#`?j~ttLgoY5;(lTP2^&ieD-X&X)!# zI)>03$}J|v(yjryy@&{>2HWYky$%hEP^7l^ElH(st9CF1Ac$H6+SGT)=RoygnX32B zH=cAQD4gu_eZVbQ>#e!d3o{=`UE(7HlmBxl3oJ}o=zf-#rS=E^f_Dp{7O&vazjW@> z7L@y?4G60J+j(a-jr)q=7$eAsH}WO6-f?FpJFAcaFh&WDXPEPi&J%!6&|tZS022Al zLpKYv>{zcdpv$CVWA~fPdh%P>YTciqccq)v`8Depl&+HPV6=0^2cmC}+)@sbL0s9M zw^;ZH%@|>+$70+?Z8nwS0$Inlex=^(@pSSbpvSFt zC_r8d+6J3;x~K9KHWM(dD8HRNlHTLPvus~=31wqoe>qJ&(+{qpPFo&7mjgK!inYgD zxk-br%G*aUPryNf)^>Ymq!pxbgLJFy?)c*!`2Rqpp9q)u4$}F)`3h5EV&k(V$5Qus ze%A8^$0T;pyl~0xlK!NJCfIH$aVI=>$k#glx06iBu;fplZC-ZM2-b z_d{bmdAp?r4@h+!R+Wz87ux99V@f}*?1bMXle9XmK%Yszr7%D?kh`-ko$Ws<5Nsx0 zu0`Xx3rk|k)rsV}@UmdGau$2}zQI~-7iSuF`ubmFAL;r1_s>9A$42;DM^B$bMQ-d)Eyg~=UGOTD94i#dV4e+g{=12N=!T1-`2iH*D{B+9%aNv>j zIWijlZF_`8XLIkVzL9&003MzDXwYAAt2L)Y?Bh;2I$j4P1SEq*$BBMg}3;;)YV zLwjUkFjH~t2Y}zf*nUg3fVadt2iOK5gbP>Y$O#t=KcE?H#O5`}>HyvK&hNdxPW`d1 zw+CU=R@HFpy+H^eEN&^Ze^jf_zK{Dj_^IJ$gWsr2fyCA#MPcj_&AnkrVL&Pe|0aEJ zSxRPmCzhDo|KnCLj4=;laE9MLGO>CL@{B@mxEn?@)#rG}N)xp$L6OjmKp)0*Lm$?` zfvk+OL@5vwDP(5`d0M^T0i;_ZJx;u+yLkmW6ACor)=<`X6Qa^b<1zVEqznYD3!Tu! zg8@EZb&!7*m%sHU4q5tj_H{DWBK1NrbwxddEf1CNS;&%eSD_Nf#BQK${K93!vnwz? zpO%wtjLrCXZ%+^BE&duvV+*<(`EH@b8l7Jg_;=@Z4X=!ff}h}wwP1n(& zF68KlenwwcO(7iWRD`o?;v&++5YGpt8ZV4cyw3P|#=>4DjoUgvg6h%a8UEjtd3)~m z`<#)W9ni6#B)+3MGbA(gXgbbO{w9`i&PBM{LGC2+fsp%13o+Ju;Q*`6vL^l*gpr{_ z6Nfk`yjwb3+jL%mf8;Ch0rjgWO>89zAeI1lA%Dqh?novRn(bJ#F|Uaqd#d(6wBj%n z@+zP@cN)Q-ly9At>JI3Px)ofGzsB**nTh9im!wWOHG~GfUnB%k#HDvJsQ!|Dq=3e= zNXwaBIcnQ2Oi+h$>d-1KEYXf@bqBQ$fHNQg;(u;Wy}Xa~A`e9sCXMy=8nkxOOuAkT zsLL+|bCgAz|5kt344GGzf49<4Hpt3)(=8h{_$Bt^3tsC-XH_2u#BIWZ&bcv}Z%Zfh< z!>5U2)!|`WP`TR@ycHKq8Vo1cy4(Ft(-t8NN^m?KgzYHbmDntISr%v)K|9XgA?@Zd z(kcGO6m3)7`Xq8!o~J@QGf7gyaIc^_Aa5*t)~qNrnZDaQ({nALOPKjrB_pP8vD^?t zWigju#LJZTtcj>&7RhSesWDbB1Wd%c$o<)3Kru?XjQE}BFEmyf+x^thZclxQ|5QLk zs{S+}9#+qJU?%=)N4`7{BD-g>YWbv4IY>~wEcoNCmU{NpxJ`V%m_jhdFV!n592%_n zB`KvSUtYWhQWF-I44GCUe+f0izzA>szz>UH5e6V5g-R27>jJ`W_9_A#L`MBe^%bL z6$~D)A7m}PUVz=WnO`GHt`=A)`YFOyBA46;AKw)fU=el^3i!Tr{r+`AGf9~-qq&1z zng+j%%#L)2X6lz*VvC+#O~NE7aTUbcqVK+NJjneY)kv!1)&O{0;xzZmG(tI{knwFM zd2{S{Kj(u4o1h(gB3|^*E$IqhT2S88Mh?ObZ7~IGFD_`O36YB_#=Y7_j;@P5zJP_x zj`Kie-$g}TjRu8JDr%KSMf_wOSW9V>lAdJ0nva;Iu+1OJI|>z-|5RGm^WBgiH$RYb zoVw;R8}igWPQcK037V+SRtd;L+1sE!5TKJ(gOgf+)SE*E-vi~(Am^XZRkp%~`E(06 z>X_1BfH}MP3n%?$`h}JEQe(>rg1fm&X92?wbZ?GAoUtytG6>&CG%=KC7y@3}KhY!$ zL|85JCz=unN(y6vnji(ec?#0 zesIT{HTG@f_!-9l{wIH#(sEOA-+SH}rjB9_doO(@GreTUy=}!VRoR4g_3xeROwXBPPFKequ_t@BZ!0sq z+~cZidHabE&9b+_8YMY9MwtB9sc0s%C-b4qfB$4KuY-G=1hK_y*%`}5L1-HruxmAy zbRO_7Ata9-3Y#&tcS?)f9C5eLU#gT!#%9u9s&97G;6^Q*z<-bV>RXpT=$8U%R<)L zKJ=kSIXHbPzmOC$sCL0{?Mybs!w^b9FV|;z!GGMECu>>=Vhw2Df9p$)cZxh1q1mJ|A}**CZQuQ&u|^iXs`YIDyjoUd zRW>%UMv9pb3f_zzUm$OeX+P$D)8_v??8chs$fb6+U)mZ-aan0sBk!`2OpidF!Y#Nw z9K15=$$IAiO`=?O5V#8cd*&L8%bMy4cSc2g4Pex00sl8@?3tz(Oi8{85fUN#fpi2? z6zxg6zD;?^%#C; z7K(QhErj%OS5KJ>zvo=^L`|rfnztld^8lnzhd*HfMW&;fS!vJ+;?ok;9Qm%Gu|m~| z6{g~mc*bv`l7)R2_w=jXvmpC4Aj(bmem>Wb9~i}85fEN32W7#o@!)7E*$ff8KHz;T zG6<+9sz9R4f8cwia$(P9ARZv?({e>=T5)s44z`jxreQ_(!6rcOB(|&uZ(P=YgLQEd z{(?UfmmptDMKvL1=_b`%p?-#zX{bA&q|U}%NgS(M_YrC9iXzgQu**fTah zV6c*mebP@}qpO>C@wc-W)f&kGu8Q)xDW{Wd@+HkK6Jt*dZhK#?b`o(x!QKR9?D?O~ za;P(axVmLUW~7CDaIc7qSXP^%RbvB{$0>n&77N|_3c@wW>Ai+7-^|qF`}o6eDA|h9 z<2QI^-x7pVy@69FHDl^2v4uQkBJQHia3+Bb3fxH$z4C|ozJpIes)~R(K*#N!0G_fv zSv4(aF_2%E*m_)VGAtp_!Mu;2!=@B1QS;r1KcR`S!G(0D5X!$tZ9sUc8RIIVbh9PZ z2?h!MWmT%k?hJ+is472fm<}e6#`1$WE%mA!4ZA&mrOV|ThtN*Wh3o(Y8V>LJsEYnQ zhzN0HMdUlG$*?c{|EpT1Px^TGXBh}xC=xBlb%EhGzpoJ6KhAF~N3g|5t*yI(;QGN8 z^7-BK0jXBIp4-9xcQb;?I!C=*HKg>(JGzV#o^9)vL`%@GdGQutSl=X-zT`wk~ zu*kicGkmheCCpt>Gh3y**eY|Jt zQ8xA+WN!&W~^!s0=m71cHeo{yWe|Dv`kA@U--cY0^gYNQ@C+Y5}1 zMxZm~F`<(Dx~6YEZF4!_#;v>@-i*H$+mEOCDdtF01_e7a7DZsg#>T5+I`GVDyJ=hY zsr+N*L`>S1`nJvOKJg#(#C==`gqPLU6;iDDiSFjbbsSGt9$Ew~%ONsv2&|1YXV;Em z3BiT5yx3UM^)Cyfu!yQg30fMwb)f`(Oo8SzE5`woW)h7?z;n>oLKfYh*EsFf8tSGU zN%pyMds*Ldl_0I-jI_2A_UbU_g4?(4&nrB9x*1p2BPEoMRUhywm8|hLyB(dpog*4e;a4n!WafN{k zjjhm`KT<$b2qs!A$Sow*jAx;*!Td@)>l#T$6Y1>!!1olCI(}@7)EZalBtMhP3aLh+msH4aQ*%W^ zod<;|sBYBWJeKp2q}uK?)W`9unbcDCSY&?h`zYueGlhjC;Y%+rZ@E#%gSwR!MT8jJ zq5Y_ZmKKckqdTkYQ`_xU;Y2n)g6v%w44-SP2P`+6X}e>E4FoP3z<6>My0@m<{XFq@ zlg-?f%u|74`}d0``bd2!;@!4o5;L z!d@hR6D+i~?uV6d{f=FB?t^AFj{gt}9)*E3j*;zFPbNm<=!Ls+=?s)6KB94{mE@F=_d50A8 z%zN{`+%{*w<7lkfW{N6OHC(Ij!E{YHYOMCLOG``a#X+fs2c{Le`q9xr4jXA)`JJm# zM7t3^$^E;!7_Z#)OBDB==Z@-I@fyh;NP7z$nKd1`XVJg*DAd#L80o@e{>PcmL%b6{f zseGV-UIR6Dh<8CqSHnSHn$HkupykpTZ~JFqcZtGPFu8GzcIL77;;ICh2aj~$M+=SZ zBbYVjfJX7glvI-L?Fx1hLe>_25C}OdwP=#qM>#=?kAfQcq6!!w_4iS!a6>a4&%5+& z!zjjcivLzYU_`(Pe2P4%Hls^NOIlsDy z`r)jgBPFXs(w)s8(9d78JMWxHHYJ2V5eQ9a>anWZcPSKAN__H!<-y%7KG*K0ACpmM zoWxt46-_P)>XOpZB0fX|Q!)9UJ2*Yzg zq}#J_&Wi;$D6qU{xo?-Nx2V9BUjz{lN3ucDh(ViF_^{s!xZ^=S((Z%zfBz(jXYTip zS)!{HGO@xK-z|hFy8-AP*?tt(Xo?G0y&)t1LK9V4-=7&7oSk-sY#ObAp7&;VCUshR zvd_Bm%u4P#_NBT==kP{XVxcCOs+Vw?-V*U8>)lyz1UpGN?OvneIcBLD-uF>~`tuHZ z#Gr-?VxEt|zKlGJ>`VLJqruh(4yv95kV1dKdX?5C3Zpe-=$~VZ_B}M`lvW>1$7%sMrf%RPc zkW?n*jEF`|PQw}UN<JYfHDC~`JG}X6y!9UAhdMuK>9**gi z28GZA3{-{$Uw&L51?d-xDIxZvt{txysR?F5Fimf@rsH?Jskv`*e+x_{9tHFy?Tt;_VHA)zgMCbLFkr5V&iv7`|h=4baw$}gGn2L0KZ&v zZ~7cIhJR|LEpz52C=rvOqusUx4c5U<_A?~YoUPedwVdGhd8H*kCrz(a^`M!QAFIWn z8^MN(n>?PJSCBa#H4ff} z=X=W$>%j1wQiLgpszeHh@?s>k0 z_7cGm@-EuI^#N9AjPM|E%PJ?QHWT~IDbpr9mT@Mp35O) zT{sbc*a4a!w)EFz;izhM0EQgg)0zJxQ6i=orp!V>h7#wrW=BNS z2_Z&t$4Y8_hp7Ac){Y1{FM?}Y{M+;#ScW#iO}#y}Y-W$GLg;2#_0G=BtK(GAbO^U! z9#+iB`cL4F9CD{wqZo*IZY*E^bksz)gi_|IyikB6j@+vFrP#1>bHZ;Za3Bj}KR?8R zOvNK)-ry8;5m}j@c%Oz|+;?YZ=CKKJq0LBSHeAn8D;hQyd!!UP)vK;lvm7s7lwq}G zyCmqikgfM%x@037yNW+l+O9>DBy>r7PFc*5X;+(K381VbQU~Hm#iGmB$V|rXe}XV@+eR-Na6|%rifva{I-4wRM*J&c7BjK^1oLW zXs7XB2bFes*Ujp5zFp9A%Z#a3iqj|X`~Uu7s~1k@ZAY*hlR^WzmS$eF+1H2;hzLSn zMWpNwcLBCK9`*|2%B(hk;kRcv=zj+t8|8iihOoW&5&5v1_b)Yd6cFm>>9;*|Jnr(l zXa9Q|&p%ZZ_N?l4Wa+2k^^o`XM{rW?6WF=G^s?19m_5OV@lJ|qc%kLdX&fMCJx_qq zZl|!7^!;^FHl)wuF>Q1}<%DG~FW2D`M<_DY{~gwl5BVn>cC%{I+$GYKq(|5|ufW*Z z?h_sUtGdZKyqFzlE>N|Q?%p*D^L65cO>H_Fbw=8IQ{7xE`LclFJFSiX^l1^3m9zP((#0 z>Qg(ST!4j3gp}A=jdaX&3x~QuA$raOAk>dZiK-MXvb3Qkw%m&N@R|inK04yJEc5>c zQGP3M2?TlTmiV_i)1w>VX;1s+`PV*>lHC8^%xNWvSE<^jrF??AUH}%=bj(C(S6bC~ z@_y#UO_a~zZod3=BCLBZH?&uq$`$T^9K3%>1nd8!%;nfexm}}9ex;P}o?j#7 z9i@?F-XGPNkx)OJy&?$4fn&8T4)|^PUV#=WJoSsm++51=4k9hK2E0ye-D%_ zR&0$ymb^n-8G;`D_s`0YcbY#vWKgZ|@8?YqJ02bBz_COp`#AhY#;}UbN}Sz+fY1fI zy=Mc4#w?CmBQq}^<^WKgODoQ!ZRV^mv~#YuKAXn07VkKHyD3WZ&4J&xJuh?_avNg= zOht|O7eZSLirTi7N=hG`vMSmeAWASB_@)`x=-d^j+^KmDIy%=L+GS~PSl!~q9Y+P@wc1;c$)8r0FOC`kr3mV)|atz&kc7@y9^D{ znIOp=f6=7#Z;i;KzM4UcaU!E?lOVn=wIT~6Vxuh ze}S#UlGR1pyJJ+>{w?Q+Q|KywyM5m8f5u8sRv3n`Lgc5*BkYV!LoKFRr@U~sRe)}) z!P0qPTaLLCcR(h}x#MJKcDFLyqJn+lVBVm@jc;jZ?0WUeyO&|3tAdktVR`UWK`HdD zXMRr(^m1_lb|mq}#GAG8#Pn^&q-ZZsP3NuPySjmeXgPj@)m2^U<&S&b2XDc5HwSkT zU~fGN3d$*yH}^p54i0^nwsvwKjZKQXFwL(No@p<97hYKPNp}^Yrv;SLVy}VS&p&n} zY?QF^lDmI*XCn03TgN>wwfBkJTfASM!><|F~Zq_H)o!;FY`)feIK_m#Jei)zIbnwRhbV)%65Th}yxSF#nH(iSOfi1?zkecMW(I8$Hv%|f-K2Ag!JROpz!f?Q?&qq{avo7) z@23q9>dbkgknOXr6FrnILT5C*c%q|Ny8Xvdy?*QLv6xtc2kWXKC=U1vr7x-tlOJ9x z7~U$`vOZU|U!V)==HZ59gZ8|N=E~;AUJ7u#mEElNSmtk z!)y4uidw(1)=tg6K`RmY$ld`3!s@5Ouqxo{laJjJHry5MX}n>n0Z+g*o{?a$vC84A zv2n|qC%+2~f+*crkj3-oUdt6z3!V{I!65xurdF<)XNTQNLQP<`u8<-By3DXw&azqU z9&$4F*Tpa!+NFOAK5OJiJ_?wTdH^@&a9kZOkNdEgs;i=r5RD4LkivF=>XJM+)Y`8~ z8n<=(Mh*oA?&O0A_Ltr-AfCGb+{AwWmWWg;tyM6|&VDVo25-ZGS&N#WxV;(<-e4&(lB z|LO`*$HF7IL2QS2=~Km5t?!2hcU6xE()QtUOwgPPqj9ys}RKg0m_J#_=! z;(Jy)ZLMw-&UKUcW&GwXQ9XxOh}%{{wx7<5sHO1x+J>fL(bMjNg3c6xA_83Lc(ZK& zf*1z}+1iHl65*Gce6T27vBo+pG~eHFf+ODLGxtHnZ?H{qp;dU;a#9n4!D8{WOVd`F z$thQzC{<1j=f2aP>hwaS~dUaL0iEVO*QJ|fo5jd`RGiJ4$f7_sOZ_zN%O<@Pd z=55z*%bSfjT1 z18Fte8bYuZoK^aC_CX~zpvV&^hn_v(P?5FHcoZ(j;nDmB*CufRG4dWRT!n?{s?xJQ zL~)t*Ie-TA{BUvj91_eO2>ZL0h~W0 zTQM<0m#cU!zx00My$ZLEx<|zDGg7C(|UdRe;M`3Z#R!r8phnf>ydj2GR58xxt z-a3^c{ixu4MrEule#y`7YTjDo(b4%vT zdqxt8S#Jo&!@Yn@KDV+CP1>L&x=3swJwfM#kgjM;)JKMc)2njDZ>{NYYf9#pa4Zv=BW38{E7Y5q7&iqmecfS#{5sqjp|M&;F1^-@{P%NQxo z*Z0*oTuAUXHly5lEsbONf$0N0jG5ZAR2??XoHD!W~sk%I~d^pwJb@ zot0yy%SvbeY#OWvZ*r*Q^mVGo;hIn7L3PH z*nuLPzSip$up2`gt9CV34y4yNsBWc7P-N@}lx{v3wH(`3T~46iS>4S&_8SS3F(wpC8Twdz_TbYdM1A%p*miZi=b~e4m*~N4*owkgLgC((Wj8L_S;5o<&bWwWK>Egz*j!3c;q~P^I@WTu_F7Bd^GV$O|r2w z`p)O*h}DbYIg`?WOk?zKF8uVTEQAC!;|Qd4*Xyza$DN)?b)gtFvNDCjX5~PT9jfgK zz@Z8okK7 z6$Y65LbHN<&Ufy7B(wz7HOgGQTpWMLD?U75PCi0LHD{DGnS*Uv`0lm}LaT<~iek@s zlv|~^yLlWj)bR4|tsQHf51uJSU&hSict(zsSqL{vI&CBMP`qj$LWPk#$y}4McUS2k z^Ik8>bBo^2D}+}JqEOvxT;v%ImP6JISEQdPJ(ZRgpS%kp>@avA58dxx2ovwnadvGT!!{-Dl;wQdvEP4IQqaPL`4u zm{n_b(gvZ0H3Gs+6&10CLZ0|Iy>4-=DZkzoS0G)1=HT(c#L(p|ml3bU^Z0`PnMv3F zD(?F$dJ^>NwE50Z^|*R)o%Ld5_84QT#N&+aZq7@z38pkpl#XhAoRtor;Y%+C2b|O< z@tgu-b8t1qqtkp3Ibrj;5ubOcS5b%lMYh#zm$dwh*5#m5(pe_C>+ym>&)TX4X-VRs zyQE04V9qmn+!yVhNTsnb(*_?0eXr%We_>vt@Oo{i(j^{uE_776aoY2*%$5s{{Rvn1z+|z!_v=aD*kO<{nY&{>0EI;Hrn(2H-wx)>3HJsWk8k)zvjZ2zI83K8m7z8)@H+=2cB^}KF zh!{uh(Ur$#bgUX=CBy1qFI%P#=&NDbm~o+lckr+LD92y+J_8Ler$aR}m8QePRKf}h zh!>oHU3&QU66AqYnNvozUZRiwT0MgJY{xCktl;21GIFL24<$BIsSw-ho)Lf`i*93+ zTvYczRH=#Vq*)#z+*=jol{ZyhgonNFdY`;IQk=!MottQEznW8|Fs_8MGGsvS`|suj zQLQTK?>f_*ZPqIXy?ymsA9|X#;8EpMw5i>Z98HF&SMT!QNX@Mi{@*qZ#7kkv< zVpJ-}6keFIvB`4^_(TGmBg$)59@hkZQ74WTdyGtSr=p~F?SU0$?D%V9pvef(?Cp=~ zdvUcFn_1mEML7hY?S;-p?gkDJ&G~&qBAcy0<-&t{mD_$eQVXkE22ge)vdRP z0uI#t`@4%|=iU6~!*wAu%v(A4t3!4GDZHJHKZy7XfsN~p3^L>>n)#lsL-q@|Ni%npCVm|bWi+kc#vr$ z)WA4s4Ppg+sTH&KVwm<7IG1@<_GM-=y98y0gn|8Gwl6IDfd|kfCK`RxJjc4Dz0*u+ z8CuZ8lJVuyR9Cm)xlFTuVX~1a@C|9Ivb1716$8!i#BGSnua#=?2Gz@(Ik+&98CLRE zxq_;pl?cI6dkBx~rGVNC8Xz~w`>AFl$>tQ2Y4j+1sUlKyZgQ|hg z=PUD7S4kQ@cGI_h4e?AM!?)V-tr3Dz&7&Qyk?*e`>irJQFRzD>Jdpo!wY4cUCL_ou zsHo(F>gegPF7{rhf(3}+)TO0kR+~0R12wO7SW6YHhu&Q0TLHW6Ton}1g@Ve4@Y-qP zaT7PSc!$!HI&JPm+JwH%=WP)w?O*C^Lh%N31y5g0^kZ*#ONcz;5zM?f z!iX-B(U?547}#~qa!$}4R-dkDX8ON&dz+0|yfp*et^nxX6A>BpQ_5JDVW&z@Jj{ug zS4)q5E^Qd&rhUiC)qJcVU&zoIsT&RbRNkh+=I$$@?>v0hxvlSt^)rX7TUvp*&s zGq%vCGzqICAmK@_ucqoAjrJ(EgZy5I2$jUCXl8C30-fo`K$`={=GR_1#>Yw<{@5dT zLF(4SDfCUcQA%sY@}`%Y0dP3n-&fGe0pYX=+@4T+znQ1IHgOEr*HGASC}Hr_ zC*@c-HM~%Ajkp303=Y`&U#Etgwg2XFX3%{WJC1t`9UK54^|Sa^AN!LbgX7{qeE0zq z!0`7C3+(#{#%xSv|w@Ctk)@j!S?-4+CP2`@jovt2|ehf<|Pr%E_<5k2X*!QFJ)R z^w_y4Atb}(+@Sr+gmxeM+}Q8F*^>EP-v8T>r&!2~nqRUBg6>xD`bd-NT4~<|`ZFSUmFkHs zdER1pmpIiB`i!^T+p+nQwEO_0aa8EkX0eaU>W2T7Vv(6S_gdj&;?-+f)j z>F~Ty7h1EZhErrl^Q&u?drX)Cg=|@=sDcVDD-lpL^Gbq$iU#iK>YmLr{IeejH&o~N z#rwn8{Pw2mNvJ#$!mN5F@-2w8#%{x*1(__Gu@54CbQJRkb$@CXj(~AkA-4HgKH)!; zZ~h2YpMkN-y&k#amFG1b9fM$g%IA1Z6E^B)er>Z%;sB|Y+A<8&1N+-_*crIn%VG&4 zSxn#M9Rn9TLeR3PaCHgFg^6~Kt_YOLFKx%JOAGEB)C~Vxc{GuGAr>Wo!o_5Bf>AE=klBdKmwIMAlnB8~)aDZ#0xl5F5x@~Djl zN^`?xd??XRf0gjT?l~>7{qv~j3=6=7|KB(f#;Hm?U}yQN7JO%6Zc=8F*}f9~$&h#< zPW`9wR!`vM!~?^r2eBQzw-`F9{I{{D`bes;;F|EcOMRa5soZ(l6_Wwo!6jemGJDV@ zx*eA{RcXaf$Ll4t2jsbK#i*qPx1W&?yp3DDg=9E;4$}RS1(ho94gd+d7;Kep6=6hU zJ&L1!+xt>bWtl*AYc0xP_#4ao(5XyNPk2+@Kpzh?V4)>b!?E~>WtsL$_{gQ*g5vSL z<6;#R$hRu*CPK*WLkISN{1gB+FK=vg-2jLNGzYf@;;WL`Jc$aU@hdeS20K z>A?-;KXK(PDye?6$*@gfaZ1LNH%(v5h*y~>dp|6< zdQ!4W4(_R5LrZ9X<}g9&gU^NU zcO}UKVnndpdA1bl)wf&^UrE4Uo)bNA)9gke+u1pZ#AirpruUCuU%aqZZd_gPxeDo@ zFNC_CO1U!<)Xto9-^z4nNO3dkgA0`=nmlKxa$?J3aquj6xDBFK64G)&DHFZ2^}@l7 zvmTiwaLzg$+!J{MB?7NAlqRi><3s+8wK4PA7oX27x!krU?ZsMDOG}mSeM}cfgb3Ko zEC=Mi3eIEpV33xa4c%nAj?G+>1X+nOv;n)J0L?*o-LaB(43^T2nB;KHSML<>kwG{? zbhmb(QI=Xwj(=A*9zI=`#ZJU0^3M;B;|n4Ue7@sUbxv-~-ue8IXn-ulg)STG;QG%1 zl!iEoWDY|ZD!=Y26n2E3ZoKcbz`$+T5tAI?(=sgiaY;EV#98rHwDas8XNL$BmwQ@! zxqvXy#ToAi4b8^icBvd5+XrOv99-W9&F`F$pVSA5QS^VU)Hh&KhV=0>MDSm?81Cj{ z0cj1Kyp33MHfdw3u#jhlJKpu+b*%!WiLWtU0q$jb?aiU9sbjNcTiiBBLU3F%=i%b4 z*@MuLf(iV8|Mb5ZEo=)ac1;j zak1Hh-D36;@CzM)mE~s3bc(Lk;pVHE(^DR!sB;(J1(Y;0!=;dQl_lEwp-QCulswC@ zsz(sn)I6yL;o!h@5ck$eVf8xRUte}~V>Xc-57ejCgrJBSrFxuYp1_$Td0SEH^u|U= zz{vPq@Y}C}sSf_VFKy>8t+Vj%XuzY+^x%7iE63Gtu)LX#S&$w|Mi!bwwYe-ZSi2D~ zKh!h{>Z`*2aPz6BeIse!qsYkm+_0N03jJ`Yg5y4BCEG;aR$K^K+uoShro3Sn8^U5C z;Mw&hnJwIfSMVI0Fr8_)=+Do4pR13@ZRA~%;PN~n2n}pxy!Y!=U`Thmh1St6`x1VbbM0dJ)_wlaNUYU9Y!!ZqtyX0#)rg7^ z2Zy~0P(2??c?v(>9<~e|x}3@0m%dl13Se1+UUHlTOtg+#;?B((rtvQ^k3U7dpZnmrt!h_H4kmq2+HB;jluZk8RV4wL- z=o0zxL?AW0gJet=e^*PZck3tEoa*L_fuOG!b92I@)hlYp=40RZTojp*ND!ZTU$9xK zEHh7&UG`5#7&!+;G^o`ZVB921@p(dq6768|COIBSu%PprqB1&1cUDvjvKyLiAtH^H zgWWxUC}NjmY_drTywCRN5>Vs07EqxaRlMs-eKf>R*g!|p%q>b|+$iWsP%3@BA&uf|nX@#$~5 z$q&kKnN^ShWc)-;1i_&SX3Ixb4v}}_4(S#UH*nBdalOH;_3a>vms<33QK-wwd)}WcIDIav9Z0E@1yC|2+zYzOau+Cy*J( zy4lJu#S44)bbauAY`hs|| zk%I%9mI7kC2MP+#b50QDa~ayR*uR^~Kc9@1b#0gMoJ`efcHkHTO)w z*j|K6%NCeGIf$=0*R`i)9wuw#DrN0HzPJa=3>m1CmPwK*WY>ZHo5q*E5?+~;O1f-e zuC2WATrg{u@f=Y7k#w7>-`}jO43r#RX9@U}X}jpEIQYmbhN7=vK%M5=ZLb+6@mZzoP+W1@d(t z(<8$cTz?}>v^F7ty+G$Xi(e`z&1ddE;DwiPhJHrU&-F+>2HwQ`PP~M3g3?SNY7R3T zWBa~;bnk{bJCdaI4fuh@d& z%=2Ha>$yJ9{dvFd`*lAQOVqK=MJ*l%6v)`oyed&{GTCj**{c6OQaK9(Pba}%ll6_) zkBn)n1vge!7jwLzGvuOn)K^BZ8vI*9rp{RE{x+~X2R*E0A;E`BAR-6uu)<7p?4wiWAjVC41WiZ1ze>K zjRrr8tnm*ZGP*M`7U; zuhYQYgX_cY$7bWDAPY)t(2zxxjcp?TH#g33e9eA?lbOJ&V|AiLj|w)cmlMBZyO<}- zCB|xW7911!w$Gx8;iQD@gDCa@5ZIGJ$ zlNU?VodtzQ=kk2~VlhPD_pA1~3?^Bp)COhf_=)CTGPF0OXs`@jcq8VO$U5^}blyyy zPBFlmGk@af&FWjTgT9pi{xZ>?qt6~D?V9C{r`)eU{F0)rkvY&fSJdJ1K7hAYI8F-y z;O4f^yZkr~YyP6VlLm3SjTw7g&#xL|B)dEEG<`E|vy$flYT$<`ZC7qvRvlHz(EqT( z>4RGI9l;aWV1qm>sw_>VulF!DY4o*DY}{A8fn!+1SEV|=4xG(<`=uGZ%keWMe>W8q`5K9Adm})pbkJ;qk~~YSLl2n>2pYU4M*Iqc zTp53vk5y9zq{DXzSr3hE79cycQzQ;pV(9f5BKd-!A1Y$`i7rGAQv_9yvQU!JP!fhWZB%H>wz? zrl^bUdA_4tyz!FDN|eyjYNng06}{ z@q8^YcF!Z(aGq}wpUrcVys9}~+>{3XJ^0yIcfR8zs7!Hlod@-@B(3jC{1V5NG*Yit zU6YAqc$WhIYKsGUGhwAU!ndgz^XyV?*?}?9$60MZWX=;<4)9zL zjBOV{s1yCY5r&!iy#>y14nnrBBf-d981aW918s0UA?#PMk#0Fk-Q=Z9=Dd1()YDUc|ktwq2Wb-3*5|1`ZSl1;t^P>9T_;bu;{t@*h{Q%epeJH zB8JGAeUclF%e%1>B=>}gr)w=V zH|rk{YqAW*1YlUB-r5To9IWajG6zf?aLtsn2~|u#$55cB3C(W;;iNjrtiWw2H~N#O zw-RSA!C>GlK_E$tSEe00#oFv%)gV_#+mD1QrhZbCJ*j>3#AtT4r}m$_7*N64>v_iO zR#v&tRgVvK;O-~=i^75V=L!&a>|7zL;e^ZL14w;x_r|yP2HSa)&aO;|c&FQwhMaJJ z_VU*=Ep^-8$pmvlQr7K1?l$2vK;Oy3F?EH)oR5fvS2=rhQRk^*`9ip zM}xx5bOV$ZL`liwbOCmFMhW_ByVaE-X!!%W`BDUZ`Xl6VyTAV4(n2~7j;JC2VX$gx zd4$E&bWng7ZR@Rg!RguEwD+c(e#yj<9Ks9<3t>;-Fw|tW3S{=r64kn8wm1L08fipP zR9p9&^8DgC_~zA6_YwPe2dYkL4~}%&Lo>JIme#wy3ia*{izMS?5EvTu7IX2nb4`Jy zHCa&yEv*@w6TJ|<7iGq}k+Wr<^ z?8cKhRUTW(f8_2VKpSFYFXbQq{k23HzIs9NNqsuw9};#UC@BX#Fe>_&ZDg@Y@V07j zt(SPmQWK08Z}!gg^+(+=gwBUC)4wKP?}11J)SKmYDE{yLs207XBm+W5boh%8Lcta; zmjW0>SxMV{ma-ZOz0+Rsp+`vV^Fe`11djyjioU+rdyrbJ8QRD`hQaaGYM=5`=oGM!fQtXn zrI!k*dkjtccF1v=b#60Fg88-p1r;-`gxg_uIzh*rb zZ8(Szy9fJZ@Qldtaj=8!9R?>+%Gur6;ICh81njqsGNygjIMueuX71v>kbBn*uN%-t zD&ddCu03rMnr2mhdR+>^DA5pQh7N=LxKC?H#0EcFOxg9s`c!fl?#BgtB{u`&h8w)n za+~U_(`wpF#B2__drS{h3?%{yec8{*BR>#yYh3j%USVY+DdpX74tU>WYf{<(3j|W% zSeTc_PPdi9YDUqwWGZ9J(s+Q2BiyvaA1}UyaCCk6Pdb3RXC>s^&kyXu--#SocPFc$ zQ3R!o;Y_|UWv=Bkoe5(Q=t^6swAN#o$m$)z?XrTUNwmsfBZ@I%&x zHEB>;I8Tz5wnCa_VBSYFKC%%l=fof2CT~WC+cD z%a~9e(XY~wBN!j=&ote{({&>N$MH_`Q-N|iP|}1I+O?5b7`Ux~;2lev8N*kK{#(5e zFD!r@sRea?4(2q93Nwtl3Wjn{Wq2>94MH8y5PA{5&3o&Ds-iFR&iYlIDuU;6R5d8% z2t}NX8kUDray*Ndnuly07LNUm>=Ex6C!G7HUZ}97&eiVay}V=lOF4NhT?rhYGa52@ zQ0isN8~rKcHF^D&#?E+^-7PQT_3WY09k&EEJ7bd&sT*%EasAv8Qa*di;piyJtz!jo zgGyG8$iPOruWpiyZ8X@9t4k5N+$t*#oEF&0abv>A7-nHyNo>A+kBDG5jc+`GlXW8T z4jm&}g6s=@sk_^Q82249uLo97doHp!(q4!p%Mi7E=XLs+E%+q#os(BQ`%-R>$!>+ivEyb46qCdidrjSADB5aidG(_KRc4}Rhk8saDkw% zO7hEp1HD6{7k)}4!ERfvh!EL@Gi4mRPon~}?vb^6_4M_5q2BLOC%zeaS7vXW=_(++ zqoW=+_>RN>m=$|l8AN}wxPdIk`r;wmUU*Hsz4g)U_q~vz1`<=ZCnHR@Q{sQ8v;I5c?mP%4dzIDhpO2A3 zi-R=I1aI0xF_D97F=IT{HX01?19W(=EOWkszp*m1C*=5aX_02Tzr5y|RUPzx#hUXr zGdWglj_1|Gq?s4qHzJeHm86xXlge*{N8iDkKht9$ykEH>Z#wQd|9C%*a%i5O-+IKn zRKsvQJ2T(7ReV1u&=yP60<1<^MY;8D; z#3Nk0h-${Of##Dn$JLFc`^Q_ryJ`Cq>26)jCZEzzdXQ%>eTD1O+?c@jhe@8l0Ek|G zF<*>i$Ua+5-3Esas`h0@N&3X|Ce`1@e+DLzXC%f>05o9H%d;ald(40NaOQYII6;KF z1IQW?xNByX?P30)e;l6xh^A`i%v92|;hW#sLkk&A3*}sH?|&SHSbb9ca7`B?e7}~l z-z95-q*h(F2=Kly!o>MTVWH#)SOBNF9Uk;_tDlY?fNzuj?=Kgg zQemMX{fp|~f~?XRC2yr6z2PKV=!gbw)W>2)3jKF0{sYayoA2DUMndfJwh1o9BO6&Xa{Bujc zs{dq|iGXv9Rkm(S|B6BK^IuW$!KuFcI;CE_#U0$#YFMj7t;8FY`6w>z?(W!icnGs{ z=80Z|EvbxxW^wPxtrtvUOHc}45FhnMJM|GccQL_?8$H}S$cc=c5g+lQ_h_71=r~fK zabgrvYi144ikkT1WY$?L;7Kr(WLJHwdeoON3_$w5UkST>a`V;mzLj5pXuXt#K#1XN zU%Pa!TXD@S`|ca%Ly+YgY(l+A02hHEag|Q*ABcEJjd6>=|o#)9}Kq1PtU$+GKE{YI(de_D22QG|%q&?NgVGbhA=RJClO2 z&CwjI-AtEr&%SR1nO?o#v;a$(pMpb{@kcsxOmS$I8Fqv`2!hrXi*9~LPH|n`tmzb| zGp=r{ec7@7N1%*9Gn~rsXc_Yxnt-n(S3*q$A?KXfkVlv;Ccdo-u~3vt5%*Jgp?-H0 z6sZ~nNmKIJ(!StdA9)r@lg$yz9{Og98IG#2tF~RIk0!6u<`8v~^~i^#Ok+F=P;I7c z%(~8kFUne1RfM?S~5db zA6&kj#WZknosjsP&ce0QP~)?_N<9oIc3fIp!(TMgA(G5cXqbom%pX*l940ASM0qUt zPk$fl_7sx4`Wrp72Pbs$4M(9`Y>4=<6|yviB4f7ook7 zFN{xGK4kZ$D99}TdC>)0gTFUA)@;RqAp6+!V7#$ zQ)BpfawG$X2wEiC2d%j9j0(k15Rr8Mp5Ymz4?aLdwV>Bv$zgK2G` z?^p)Pl~k*wcAqdp#vxF3QB+_UxqAgN5@q5T!2V`08`uRhtv<-mQ7we&G@@*WFCirxr;%dKv zjI_?;a+#Hh01s&0?Hu+pAJT`~Kq9YS;uAni#n$V!$XiY2me6!Zv-Urjk2laI+)C21 z_r$XGZUoDe9l}enC*HXOd$muM(IysGPZ)UJ1o*#|c67bL#p%(Fe{HnY;Qwl$9sW66 z6R~_~=&*O9rN`jAuV^+a384sX=X^KjI|rNTN{qj;wjmUwV+|;KHu}+gzf?pb5v$1` zmxY|JyR;4oa!%)1Mq8u~R?juF{KQ2{$$eP-BjZadyJL8)ccW7`pvr$KZ#oNYbr#Kq zpmHt!YvY7d(!BFV305H1B&E+{#N>V~cL>S0h!dUa;E4aK!Fx;7U(4lsBRE643|X7& z(Xr^oHLSwbMqm)V@d>id!&;9l{56^;4qY=qlQf#SjuZt{Ynt8Hy`4RFR%}$*|NCoe zLK`ZqegA9zo)7Wh=ID+=W5mq-R^2bkUp^;=eHxSMo?s0`*M3%x-+@EE6E84;X4c{G zG)De2S3Z~3=J=g&s*RO>;g6ycC3l}j_O=+g@g8|vt$qMlGYy+xl)AH3^$~Em<+>mF zJ;kd+a?+EpE6yz+67c%EYZ(MR?0w$3!NZ(){6*LPWT~Y8`mE#azus0M{>f8DvYPQ?$%dfZ6-jkWC9{ z`H^?3Yvp_S!-N8mC*PVXn@yB&P~1?>;B!d*UD9QFzGtF*l~?bf z&H78>+kS9i=wXLf{>*91!}(0l&YVx z!$}{3_3@l+KhXQ!drHStg+GV+-J4=LgoaZ|(&xH50!B)V{?r=R^%~n-JaBQ`guACT@ z>aRmxHE7uRhUbv}ay9VkY?00?5FQn;_YFwOuv(3{B7Rl9%`RQ=4@0asyi3N|+CK+n zwfi(wJs=GCFim!d)Yl{N0Wz??%C_SxmUQE#ipzOuU+vf|@}=lBYKXm{QTzq}P8p<< zYuSg8l%lt8saflna${8{crnjVo_S*f$L+*j|C?^Kn^2S;oHUbd!rDGFH|Ks-(i(MdXQVX5QT4^`1YR;IxPSOJTf=><-OBg~ zHh6X-pLu*U*^yr+@Zk@x)B7b8^H}9DGN@>0EUW7d*l(Fn&5uopwypMZ&hxBzdJMvX z2MXmp)Qf0|37hlWpF=_sp<>IeTPxEmBljTpD-0Gy!N>!d@sTv z`f-{`Ii}QwDpp)&MzoZe_B$TQ!NP-+&k|7`(FpY1j-wVo*Qm&~t%cD$^Nj?~9n75L zZQ^MZBD?8uZx68)&HQ|5lN=HaGQc{d7=W`ZUq zzGjPotUZY!pG&@zaKp`~Y4aUFMK3HJPeR`k_7V(Qf_sw|o|J;3Lg!1>ko-g_MjR!| zB5r=Y1TIZrKTV)M)@m5M*}5N#r1-)g4w*JLjZ>h3{RRGyIlVZ#8+h|>RKFp_PN8IEXY;vN?x6ZVahiKCDq+`d^6@%3rV^!VW622iB49DAaQ9u z;*(c_+15X#`VnL`%b7pexlyZl(#QqwJ1 zl+(Gie4Gw%#9qa{^0D`L>`lWSjFjg&q=El#jou-U{vgr8J_Oz8D@ybiqp(S z{1U$elWPymQ7MQNL=QBC@l2}0Qnvf`Mn(uKZ{O%8m)?aOt-`7IDs2XlZ4HKM+_iWO z`&N{m`RCP0bSJ`+IDrZ2mJulEdEnsr-(L)liq83T@=%*>pcJ)3%4zVLwXy6q_rC$K zuTeIMkp^YlvQv`>OBdDflZ-93f!x|pfvrfT`d-Bcq0j9?w5m3Z5ZH807e-MKt$LWF zMw}&Gj{eyyJCL`?q0ssJS280bj0h1`)VJ_TSuwGCJsW1?`ay1ZUGXocsY5a2_W^fL1L@^`pYn2Hp=sk#T%f=^&!&}%60aB(R8wi*iyUXR zxQxLVDmLbLSFGJ@k~>y>M!eu16lxtna^hj;ubt2n7WmpeT>h+7 z?>F3dU;U!?0aB&%=bodfM)OE=fCNHDEGoiVVh6@DsZ}F6O>>T|#d=YBi;SDIPH2(t zlkp7hrRN@6Of%1xABT+Fd>NdT%G_*)dwIreHjZlr!(b$f6Mu)sVGwFq`6mx#YFI>d59TLs+cvT5T$&{nTJB9&qjI`IzKQN7Rc1;t%~YAZWNWvKx==vK6Q#DpJXsr z*)$jS z31~*wI!lq2JY1ZzM0hpiLW9fuzGe_5%NwVgW2d}T-QbXzSCrmHTP%nmipyO~@M_^Z zKcW&uo+5D9r)jiAn^NN7D9NvgJ#Z-IMe74RZQh?e6@fZgN_WB8&onni(vd_utyD*i zY?|umyppZmlqug_9juWktn*~inrCx8Z0wB-;@^FS!a_8tU7S}gG)`7=Vz{AB{5R$|l`dI;iB}rNt$v63(HnQYwG_#Q(nT#9kyoer6)=wbx)Z+fes$`eT93 znHqpOLXGWRw5<2&Y%)6fM}#s=HMC6eSpjsRnwK(LAS^o!QYGqY^FAo<^y zSZBH0;vUMki+bFN;oo_XOcrj;=PGuyR?D(>txJVogR{ULYW_KBIokt`&|>Vlvn?`p z$8sWX`j>^uPbB9ioW4rQz?BgjU&}0_%QD&1V4Sr@wFML}Cu{-cTsp1+(nkQ+X98CB-4tNJgSQ8%j2DPr5@cfRyc5E}ycz)=fif{sk{VXRUj>8(CA8LechG zH_E_0ge9!l{V&Y_n-&_C+;3=`K^1lFeofR(;~J9PEa6x3f$=|olVoTW*`OAunK+(& ze!M`yf1O`spz5*^x&_Vj&%sTo^%GUM8QQ=R<&<|870c_ntlC`-igqw=Ck&KluK<+2 z2}8nVd4dpp;C4i=waj;e1B_9!qHhyyT9f0OEt12f%0E@YKjwGX>BxoBc(BlQ%(;}- zVpu?xCw4av6kOrh_lk%1?^;|~oYv2lu1;YpuSBj*9!v$t&YI0-G$mU?v6UWWU&jGiKpFV}vEk>}A_`dmRE)fOoilQ5S@hK93;%X$oard$6sd2lsVIZ<0BZ()7LxooZh7$4%>L%0DS{mHAd;2j!0 zoePo17A}L9@E(p#o`>%hp8jM-l9j)Kz-h&YS;A0Wy1jTEQsrQ;u4g*r>3evywbK9Q zMOZhVg3;;^XHJ!FEDn7-k@oLnbos+#rNWuM{F2H72v0LftZBfgBYh1z!<@Z>B%e(} z@;)y;==sor=6tR!6N&~|7@XwtW-GCtC(IooW4nwnFX)v|LvwM7X`jaE|1Ga-FXni+ zH@&-!@y+R{>|)CfD)DYNdg50&{Ci2lZ!@uv`$#W5PzDmITPs6v^3~%bX`SMnv7n{; zsz^Y@_aTamxFL|NV9I3+|+HDGi~e_-d#`LwrXPRB+fE zApTTXF0dk(RN^zSFn9#r^3ZTWfxU_ODv#?vp2vT!yx!mC@d#piuSSFh<8X|nKr6YL z_cgi8i-8`x`64gs1=$Ppkq_cu2NrG(W-c{k%jL68T?lVI&;rsd?iP(aL)JfgXy|gv zvohHLekn-Zhvn9<{cul_o6G@2Vke@{HIR~*{E?paaoszO8MgnnzpgTa)>K>>ybh}b zuL!SLGU{mhK3h<}*&>OPY6yPy+K#J^`TEG-TDENu|9VB{tq(t>efU= z^7R_A1)@Nb`!f^>YnC%}?98G<=q#O5P#HNQ zYC1RnXY#}3;2|>3v6|BD+2wCx7AHDQ-ox1tTs9r2pUg^=1!!4B_U5nvB@ij2L}Zu# z2eJipC!zV^&nV^+?F;@`eHke0-$y@;A298UXFu7kqU~qP5aInOFF%9^WB1*CW*9oq39>ppJpJKoZ>m8xeTAZygg@peKo&p zfyiW&iv2QlNUTr@wrsP|bSHcZiBwAp!BwU)@~{sM2;iHny@k_qU|w zgF-_NddByRl+XGQ9AU@H8+bhOjt6=yFcefP{^+sRlM@-Tz@-y}(Sj8kXBF~3$VziT za(!;(^tI0}YL0I>`20TPPT6fP(;O5h*J|;~p4T>A)pMRpj?Q!`^EiZ%pPTzZj(4i#5gl@a`>ZM@yKsHc}%DRf1dAifi&%;0_ zo@N2Ki`F>{{pYFw1E)>K0SZAqI4(yl$bTp?^I-H9F;bW*CopTUdNWxNGQZRYmyifN zIMH2WU)HyY@ORK z38AV(O-TGa#+LAT=$jRWa^wQjPqw|s_YmI=g=B~LasJ6C6qXl6i82ESGbsQ>m{*1P=fnXx1Yx$!NhOxjfDP zgbS`7KDRSua=gm3XoA>&Zx_jlO8C5#7Qk3nn;Dac$#y8?J$UgeIT>VHVhp*oW5x4V zl~&8z#Vd$&zs|{D_9}ydB?y-5@a$-CtGA^hozOvBvhVh9{F#qZBK$A`7vJV z2EhM-t;=#PtyykMRxRyxzQgg0#WnVx+y1tC`;W0p6kja%I-(mbU(4Y zQ&&QQUR64_`9S95g^EErpCA{9Pe0qU5H*JAq?6Rssg3=p&0#X%Vw!wC`*V?Fn5vAo z%`ua_P|5Q8VMwwl; z8|O^j7yr)gAnl>iILcq=<kFlVPC=Vvhj5f3e$%l?hLmW=B zJI6hzCz8m22US#(&vcjs0LzNWkL;ZIU4Dyd$^X7kz4t58^10H!Q4PO{c$GE$+h)TH z3*u$ki_~cJ#<0(RP!&@tq@UXAacHw0n&T%D_xZdsEoYkBJ;h4(80<=v)jrOLm?v)ph=y^!;5i#zP5f;QYC&-Mzpibc7c^jgJI@)==(FT5}S zz=XnIE!3vXWcB+poBgPr zJhSR2xx<@d$WmT~5Hy{CY|Dl2b8JC3RdH8ZRvNOKhoZjEtuiurSC7$LG;~^< z491lz;l4BBzkbUmze30K#|iDW~YE=ew`cJ7;Txyv&4=2j8% z@gcQZD*sw{rV%H`uxzFT;8;%uRcTrK9w|h^e^r;Gg^CZ&z0E(i<5i4)2rAUhw#fWL zd~1}c_sDBqNz^*3QW|@2&&;7VFV{NEAlK3gz8`dPS@3)>k+C*RuF6SHrxj+A@n>MI z)rZHybl*#FHwoD74qE26uk&JP9dZorZ;eS5!>BD~a>&HeLVm2zmWxRYK3psOG*vfe z&mS0HJ*{$aiCVOjPx@bh%OnEhLYOmr=~%K|iLOoGUWQh(V!n%wZLl0p-^bwsmYe$D z?FCVO9fmbH>Ib=|>@03oVnS872x&KF?k?M04&-)jPdMcesv6EanQ|c$0crS1ePIJo5J~X${p$&+{ z0NqU>VptH*M4_k!rPn)Fgg!(K=j8Z@1B(U=z{~W*2$4QwB`v?%k4y`+i)Ac!%h#QM26WPgVTQyl za``9nZ7eW{ZKS7}7+dq}*%qmXSicf}nMa0E4%@GCaxO)xB6SX~6J?XVHL7cr0k0%IbQ410+9x0bR zzwkN%N!=irU0F~2>e2Nh%F=qz;$bGXw1nwqEtfBD68*Yv(;q3x_07?DnTaCO$?xdF zsI#f!%Gmh=6-ifV6t%CDIBm4^gbi6d-rbU}Zk*(y#c8Z+F00YUZK{Bhq*Ujncg2k@ z>XAdf;+0)kZ8YjXR-X%7jhY-8P*OIvAV+(T{+5O=EJ6g@1Fo)hyyS|MsVSTK!zwHG zs$9_y5h24Vr=OMAV-Ijz~gTxBM#nN1NN$g(Qd9KCgp{rLy`URfhl$coBbjsj1+Pr`$5>uL=R8l z6HDo`kWf3>-uvq22Clzk@(rxM#n1xCMwd-gYU_OVsLbg4jtRDj=?#8uhg>G4C(N|0?r-cJ~cHRZ2>v$2n3boTqHl-#j&(fV)H`Nb=r7b3g|>^$+x2X z?`GBndIgp7n3_u34^_?_W1b0Y9H#$b@$x~ga81G93NI)|7{{<}p7m>7U|ZAPa7cyR zD#uuw2(;Ol`$9LvqwMA~()nf7EN)#ufxg+%eaW)ED5Q2S2i;2-^siZ0@;y_cn{ra+ zUO0&Te-#_7L0=62S!Lwqny3wZ>clTa#GPv?x+B<}6)lk<9PXpWPre*9sueU|83i$kk(;d=Tfqi2G( zZW2e~tSj5a;+|Rn@}Zq#D$rh-$0D(HY8tY`%F+vq!Cy(VI-wzaU-o{QPPb0KvXgvB z|J@*4o3XNPGkRyreJ~O-84~RwQ{QL(n>}WE7iphJ5(rtQjWIv>ID}#{sz3NaV3oCf*IKHN zr{g8j5N`2IB7e)#Ec@wksBnq?yELnWou3GPU4}j9aG>uT8ET6*x6r+%O9NaoBcnj| zkSwdCN9Ura$5{_5=S}Uxi?`2Ggani3h&zK;=(*cT?76o!EFO0^0UVT5G>3y(0@=ih zwv*h?je%Sy?;?PtPd^l_UjR1U0|G$PC%hZ@*zO{ZB@^EW^rP zw@R|(6#J5d>1blbOmRU9eaf5y`jh@yNPjcX|M9m#PXxI3nXibKTyQ{lTiF{u*cQ6= z$JqAKrrl`!KZRj^zW{e|LQ1|l(=pU)H-Puvw6Ho5r+lV!E7g_b8r+?_aSQYL%t@Xu zNqF=rT=`!EyZG*+ks6oYJH}#@$;m zZ@u-K1m5=~JGFAg6}YBqz;I@*IAXf(q34p7c@AedfuEO$p)pj(S<^!Ni{T{@g2EwL zS*9l$Q?;Z#T}v^HqB6icwlgoCAE-C>@}{Ube0{XEz}ZA~oY_we=o!{B`i%oS8224CId{4Ew=RjSq&_mcDfUcwvreVzk`~@NY>kQkQJmiyzepK5B)cn zN=K7Tlu_?`TQYEk&zU_KH6+@kv?G+(WUCK?RMITLf^?SZ~+(?`&N_)O> zf{@z9TKUgUYXU;5fM|j_W^}c zKacYK9OSh8*S3;TBv0ZSgi%y`)0e(a!=%ViadrjtbF0@`P@qu=xHCs>@X)V=%gL#| zxX!gTfdYs^pZ%`ZaaB`furf7q8s~a@)7;e@M}cG>26x;!&IeDIKrowQ%G|-{-Nm}K zN-}B``5i~%TVxsE(9D`M7QwlRI~J!X<$A+9vS=8q43Z*0NBy`;?*(jpTXo!{2!>^O6PUcXrD}>PVdV>ebNa z`i=#{*}k@=U@d%{b;Vo@Fo$I%1n}|Knr*;dzf1i$1#$AWzKspRzVmTH<;o@14*OY$ znKLC=6?unD&Mww=QBiVs?U$p!N&%#6XLq}?!@GrQf^%K4!?e^RtOSZT%73A*W@|pZ zL>o-fjaPgbm@brGhU`C-ge-V*qylBLk$Qx>%gB#J1Tg{daPWdet(Z019JPSGWtp`| zIrZ+3@*no~KTP1Cppa<9m7JHQkndPC#UC)VGuhSCz#GdcvcL$E)@8}3ArJfp$IO`} z1O&Qs;k<+jT#yLvcoS=C&ba9e4Hc620N?EwRLaT-#e~<#*yN%?0vyzc>O$b{+9R>P z>X}Ml?LYqOqe{|yS0rp0iMAK2 z4s&@Y3jB%ZE9Va!MQ0J%2ETW4QfEoa2RBHkol}7&Ja>8yQ&?FZt5KL=X!H!<&(uRO zz6vqOp0+%%P7OSX*oL%y(tTX5yt%jbL!91Pm?@tVyPjkeRmpeyOX)>b5m>U0?cF*@ z<-zwTgfLT-p4Bioyx0)GB;Zb!{N8;azmT z_yi*0=N}AsqHz9R;y9+>O1!H7%Q0W^TFv?XgEuA6Xe3XTX zQ0tWceQTAt4F2!0DQl|oWoZ=6sa?CoqkU05-tQ^tWJ!1;+UxFbZc}B6=K7G*aH4-D zZV)FqohBVYqyc8o**d?QOHBQyQ7g?k$$N%DD&Gdpzg!XMbO%E#4g`{1JHo) zXsA`^-qOsJ2ZVjUb<@rl#6Gip7*4LRN(pJIcyrKdZ0B0K1`}6@#3E;%t-T=QE&|;< z-I;-pj5eS5^b2-z#SDA^jv)x>YZD8sCPne+LW?GLun&vAJ^8~*!>+>z8%KNg!eC5% zEqgvDVh`(=$61C>dajzS$)tZP|Bs^ca7(iP z|8~E!l2TJ}o0VA0s0*-^@I zcZw09Q2X`1evQ-o3zNbRn`X8x3XtZd@@TYSBZa?+b)$qZfH4UUkHgjKI0#97I{7oH zcmYs4DHiWwZO>pnz%Yo8k3{2+#9r~GN&EToG^flSO3)^7j|*wzyBSn#!SDo>-MLz{ zb+VOdk0v1r|0n$iK>73QvoY?_sWeb%hY~{(>y+}t{^O-$MeDa2X9RR4ub&zlAtsOc zrhr8z8V3NxT$P4x(qxy}4%n2J%B8e4F!zwudO8xfcPQkl+x0tbl;%-zv7=J~Kasn6 zFiLs+leecq!>1>)Oi$R*=IfnDNWts2a=){IW2Cf_>}lbaAgDIKsd9vmy}g6iEs=B9 z&L{9h+IwlirxM&`pV?=5e|cv#YrkZOpaZ%2ehr=vw$t7>g}u@Sqa&{bEQ+{*zg|he zJ@5D0L4(S{tW1l`kiy}E%^n|nCN zm0Q|Yqt{A%Y5Gs=r@23m4T1rntdv3%>9`*_`ZCjdU#}kI#;=43+!#ERW^pPLneV`M zcVaa9`;(W1=Rg^(E+;>m_H~oii>af+W}73(PcW?3uD{bpUW4#0(3w$@LS4q=f>owTVMjv3rsbJi| zj)reO3#y&^*9M>RIqM9 z;lRte64}^`H5zy>{}2qeblWBEAqYy)z@as>q>;0Fd$V zYn%Do$vg{4{C7_9fGl`r9`>7eKGmM|&(45(8O{g7PQkv4T?;oO&$Br!Ioj7zN3o{y z?nE_=>QMI+XJ*84cUm#1;P+XEh|m3|e|&P@Y$6v*nl|(JZtPo$EL&*O^|pO?VC8!* z5>t-~I2-;TaM_1*=Io1cmcNz@{|=SPYv4bbB)Kh7xrl)6#zoj6-V|>&B~JS9Q~(m3 z;s90aVEX1?1K3vN+j89p-dI*E@$F^H%B|1v|I)oU7l_eW;nGm{`~2Ujf0I6`tYm=g zWGl8>X}(>NoSdDSV<5$)Z*k`!Jo2h`vT1AJ9ecw(p=XUcHK{!yXsaQ`{QD2{o?W?M z$sGFW+ve>)2=hwpv^1BlMsdXy?vnMRr!QXGBA|8SM#XvgU{|(q;KQx%m%X*FRKASU zLBlj3R(prO_Pr0XWfmfFB2rl6k|63Y5mbhjdeXCnaEF(cYvDH=+~*D?rEoJZy`G|2 zvJaQS%+q*GH-Y zO^&HNSQ@qwHN3E^%M?#w#{ZL5u_jA!!shM`*Xu6$>X~f&iB-ce5SpCly;g5JQBavl z=ttcDKG(o=zA*5$1ZNRkJkfN$t86m)Gu>F63Hvyv_G{u=-fp|sasEf>aLF2mt2gZh zfAr|tf>-^*Wwi(mL{cc9WfIM@klQKbM!KyuG<#*vD19RztHjUMMuKP6y@m1mB zmbSLC%?IoxCLUYo^9>fWa*jdp-B|qn&+}%^vYdDK zDsORhBmT*@hfDH{iuXK@dC8ukg!Gvq)r+}=aOmqvBL)tO6n*}==h~9Y)H&95w=Qi$ zJV7~OVbFuL8;!%`h=Y`sg7Vq90_xnx8hy+uPe&FPk0KN1mpafyy zs}Q%X*%tUfj>|-Xn(Xkg!Wghm<*8 zN*Puz6y8+zETT-_Y-%6+1atYrI9@Z)oC z4@>!cxj#MJ(7lh3wUwG>ik!EGut^m7>bJRgehWO_jc!`0P|1~?AyQGyrUx?uM{BnA z1o96`#Q(NZR#9AT$ zzem`c<_VuI^R3hgtJuVy@#VMf8f)PHdZPSBo)*<9n%MIIWwxMD-Pn#2I2$fqoR?@| zyGg!37MPSx%37}=a+%YR4snJ?5X&d$^Wb@3AEGfr^)$C6JN3$mx)KKzey^90sDZ+n zbgZP*h%`#8<$LRKX0R8H#Sq57r7m)UYPWQS4*iGA{Vya$N+*HK5ulDm8hyC^)ilm2 z9On5R$9ZA<`jXEzy2HB@>X@+HvibfX?{LF9BVIKeLA_BsL3Qc6f}{UG0pw!MgqNH$ z&0M0X-wInQgT;~lI@!8UEZ63GcS^2v$UQH3Zh7B2K^XkbGy31Ckkn5D_hJG{IXj;H zNI6*tn{n~g!`W1FfsWLNIYuEeyWhh*u0Zg=Q@TKmMl6OuQ{Vn-Bw{2r@paxqzsn>I zZq4k*Ib8_#xvY;BdTjf^nsEa;nzBiBmcYkb{5)UJC5OBZ1QEvtA}`+tBGrR^+?4Cy zJ2-fG3zM2e{7g^dpHPV8?_8BYmq|$Dg7PBncOlo7w0uD0@+;XKMl*r=p!}0;IB#lF zz^gEAqQRAT;j*x6yVoufJMDnj1_2)5V4_GqM^B75C9Ccx{;v|S^m2x7dr+aElYC8{ z{HA+j*TpC8FE~z~a}NwI7PmkZo1K><%?x)cy9_2OM|JjpyHqS6uWy{02i6ZXOIRZc zzUR*65H>%Zk@3_zJE_b&dP_}RX0^IuC|qo7rUM5as2LCkQ-78{g;P86p1dyr#YT*( z%Ue`El#+6op{7q#f_otC+x@N)d3=M9E7)-<5I4GpNglh^>;LXtqr~x)~Kp~Jz9V;Oj4bw_l%DF z*J{(r+_sx$Y~2$f!ejMIVufurJbv}EMENqEv6+IrR;dW_I^dv6r z&2dVyG5Nn!#UX_clg6R~K5VM~cWPBaJ-U&0WX{BbM2yDg7y?<)%G zDu@S09%!CZ#I|XBOFix^(}eT(QY%KR=Yh(Q7cx~8I}u@|ksz;J=F`q}OYk9Z@lT8b zK~;aas>4*`LHt9$i?5?ROucUtuQ-?GmAMSX4R1~r%1=ct{4G%Z(&1rN?U!E>1)rKN zLd^!|NS$?Mug@5<#lMGNeJ6RD#XD-=3zCp&UL@wX>)f=D0+DfB7>>q=M}O1#i^b3( z4I+J7+~qN0<}X9~G;gb;ryDM5Nt^Bt&-n!RrwelAoZ38->rYr$x)nLAe8~4{<-~v9e&0zOi=59Ay6K+pn3g6#|(GYgOroQ0_doB$Q@TB$nkK_-x zuE7_)$TD58$Gra&)$#%DIj?46{}wm)@d=JMk-{c_q)qfNLa;3^?qeBaHHCJ zGsYOkN@qI4kd8alTFrs2PoX5!LvZ|QMcE!-ztW!N*D1zUOF47c?Uwk}{kM6)glnIi zq8QajE{+*bACn}9CZIpv(cYP(gHJ(c<2sZuXX zw7~z0Ta9*Q&xd_zpqiVY-kr&JZBc#hgkM>pdHAlklERBbZ$Tb1KA-ApZbco61&nr&1{4C3v8#+}V4fE()HgLTP#r0{6I zW7rTpob;@xhvns3C~yxP5?}D0_kprP7})PJ*07YDmjY-mlmu}^3h|O-#TBMK)r$Ly z8hftD0F7H?!?ll7i^(5TguNWD2|hYq_RmZjpm{u3l`$L0UtgXZ1*oXEo>&C@K>6QU zVhQh-ep7q1yhQ#h8rD;N*S;!WbEeAy)1Lo(CS3vcY`J1VlZEWg^8{K{vOM!t&@wcy(F0 zX?lQ+vVTQ6_miC>*pI(#g(HT=1*f42UukaB$oQ+(9O+|{Jy}2cBAFhZIP`4US$mi> zD)lEBEUO}}EvoxReR<;S^weY!lZh<&+%MuMqFPSKo0IOdrbM4g|mn%R@(q7uOweb&|P5Cj2oh;?`C?OD#uEY#xp$! zpV|FtVBvc))dLLOch+|sIb|X9()BvwcedPVFhK6=&1s6Ad{5`^3)R8g-cx>aCRyK> z3M`EdVHtj9LC)C@?zvKNt2bq_adpPqMs42Znn!<5G(As@g!99$@?z5)F(u~P?aU}C zSpGpIoB}+QWnlXnsdtKT^Ey5hn989XsorBz>wnYT@kua8ei?>KQ#C@SglO~nu;;e2 z>>SLt)F(uF(OM}$`b^XXf`0OS5Eal@I$Si)q(>l2_t*A zcvYK3a+DdZebvMN@lK*YwxJ#+$K^uN-j31A+(?dO3c<`pPColskXvDPu+&0aq{Y`u z@Au{1lbRPXOWO4dHN5Ia>s@c>^sKZTUTGxbD!Y^N60Bb){dcPRY&~8Vk@GhRCEuj# zvF*r`4SOnnY{MqTwD|_cP9Q;PFbE8TuGPB|3z1{D1!vj z$LNl5ddaU!&um?UsYd$>=e03WPq~^j1WE{*dTRXSHDvwxrAHjm%(k{)jqzDJNqfZC zqO}W3Ux7HtS83R9g))7{>7KUl>8@|h3(0DRzP)wUIxd>ZU-!6snxobnI90AT~IxR+BGRX^g&XvmHO+n>*-HM%xS6huJTjQ)_Cj&=?RZ)?IpBrtb6gnaCuj%RiuVJ3=TV@t1xj zF+d~o(dlPuDp|Gt#4G?I*(iO`8i`DTV#4E$!u`>N9SR#tO|noZPU6*bOta9$U!=0> zT(&O6>lBq08tRw1kfd?TY*knlBf$Fa^~ZaGIe&$%!ID?c_+-NZQ;An@GHmB-Lg5(k z$(8R8&go2@<k73iDQDP62X@)v0DQ`_RAoQNj8xN84V|k*Bsl(rvKTrAzXy=kO ztjDEO;QvGc(r)Hl`k8joi~m=+X)!OLr5#K)7o>^nO`5=^-UQ>E`@9cuUuF=-V^a^L zk?)mLaz#(bm>JPGcHKF5OJAb5&5KxZPE7<@FV)op_YZ@~Ye#&s?7)C^=!cQ*Z8-se z;HZ2dop_QLU?;A=)guY$PTdbz9lCTHt^~vhXx~#+Ur2%F<-lixuun!i4xBmA=_Mk~ zoN_;23|h%2Oq#cC%;$tj35%cPqThI_X=AO@^E$;ky56F{YQK;G z!Q(4bc2S~M8@IBM1qU#o5Up&|r`Efo&|7y3XBhUitKU7GlcX=g5_-gzJlDY%}l z>>&QCscdt$6B6X@l3UZb_K2HkhAg-h_On>7|60mU1`u0}8j$QS3EX1$!d5@;7vOhh zp=mcO#uH2jYu8f$J2i5kvkwzHMEHd(6+efttrTJd|Dm3%bxSLbZiQz}e0n8K+h=`F zk+K_)tRFH2XL_OA;7_Wkx35$O}J^XoqB4UPg*teRr`V_2OuTr>8p=c{6 z|9zIb^i6KrS@{hz$|*BpcX>uG-~W27$N)vsn1?TEMp#`VTqZ}TO6P^pAkHzBZZ;>2 z)Nr00PPo;l*t=;zZZqu&Uvw~zWl3oIpwnnp10geir3dMqNfR=J zmeFkV->EIl3MRS!so8@AJGf2DHq@oK2(!&mmyCI)K@l5>vZDK2qh@YC9P`rZo;(7GHc|+?@t7hwUh#zb_0o$~|R?D*XoAkRv zKbRlTay=wU;vYW9OXcMmDb!a}`uX=Mo?$G>9$coVR4}&q#vJ1(O1bGPMufDuWJF0C zfUS2I+A*h^+>dp$eUMRhr$v+Ch{uHuZv9PJ``JzmBCeTgXc7_;P$Ls;fzG#3@iZlP_Voqa z+_LVeMi#o$xO4L)NKZCI_!TlCOO}J@66QP2vwykS7YLEd6il=BPxZz;I3dpr~{+fqR8cGOgL14tRq5EzWr&?ngoJ0yE0S7 zI&bjqc`t|eNjL4`h7;-J_z~mPCxFfP;_D9#g`5^5-8R;kVND92)nwDMbqpcbr|Trd zWW7Q|rbc~F=DXFx3a*+K%@{gXKpLm>f7>}ZO4 zfO&?Zr4hGb$OI}@Jl9WjQQ`LNBMdNDJ@FSveQ9@^SM~45O9v{~%1fJ&cA*`nN6>7^ z$NgLQd8kHksJ@tp9k`nnVG>pt2~prl*v?`_X^L|s;m-MGe93DjF5-=eX3E))_kdld zns@o>dWj-2*MGHU8iD|?gW50gQs06qNn)BuLNQTPbL<#y+;tEAob`PKbH z)N0j0c2m^V#&Jb|rS5$dCrif4iP&{|qJi~;9XRYg-K)tXWM%Bewv+3@*hn1on|C6r z;rMbv;QsW%tL4Eu4D^-zF<6p=${3OL6v5o5nW_&j)jJ+wgo{U{L_hI%XLh}u6Y-Wi zy|gtY=2a>x9#EFPc*&x718}|gw>mTDTvqwD;4P{z^m*mqIfJ7(KT6~ea8QH7)k%a6 zAU|+KA6VR$_Mw=RO!BgXdVDe-T;$W0LcvCsl6?#Or3$UTMjM7>BV0kshX^a5;`$Sz zI1L2E|K5P60VBvG#>iG67>RsWd^6wE1h%W3x0D{CR)Qr(MCC|(^~62kT1V|^nT)q%t(j@dJfXM2#^SZYJtnjpM`S4dtyJoeZ~AJ(375-w4-u4XMf)Friqz}806;} zrtoV5BJ~ol>{74#y^c}-dKf@0_)kIPz34$nv3v@_1T#=OYVJ>5yyNSCA3n3dESNw0 zKTA)3D2IdU97BKpAmmwh@7e|KYzof;^!a^CosI?dpW#WXojpjaoce?ZL4M=EQ+9|& z#~8k|c4MOl>ZAhNO_9pd;wf40k-$_@;DWZJpHF79?h=@fyPdI7086q@UMBGtP2EA| zUzOsW>W*^u8qC;lf*2(@5S#)0tqtWeq!R2G%3DgrV3B|LKE^H8>k0xbQ6Q`I!K%4h`~pZ4uJ4&v)gqJz zNTG{5tUnl1@^YYW%h-}m@{2C452?$=nx=MFg7q(nDae2h_Uy#23yZYGFftYEW>_~)^X=r*_!gaPP(^jS_XT8^F?EmouSJgck0{UP zOEW#`y8OeosCNA;7RkPk>|>s_WwI}1$2cf(UE5rA7<~3&vXRg{iTm>^tVKO#Qo)nq z(j%f;)xdql#4VWUOPx=CEdtg*!Ygp-U9r2b75044EKQs&b6(0PD@I)o{J(JoKbVwQzLGVR&138X$Rjd zrT#!o<0CYvohAD+RT`UYbyyxEmMk_fM6@v-6ZK}xK6x!NT$GB7565hp+zRIU=5?G_ zMT2cVvi}HJv^tm0xk)}Ah*2uxYRR2Uqg>};e}=%Kus%KT8j_OipK*!KrT4;_N{~vN zxk9Yg^q{jN&Ffrqcd=9yh*1Bm2?rAF6!}i1vWg@0*Ur@VdF)|i;a0hy_s~%9#ohg@ z6QEUkWXBl$r9z2UPbyQ8-sA6`rxPYtmXwds->KfI|9eTU8sQ(P`kiT*>AdNjt+}wU z>*1{i2w~}uH)y_VFpaYX@a4c=!bwcmWXJ^FckjI!M)YeWX$eB2i)ux7xghVB#zZ3I z+rDTTvM=ZFn|Qfjs}Gd(yzsUGT4rVGF(~#|7185NB@xY3+mIUS7X{jhNdm1W` z;`GzeTsw40Y_5a&jLPi8_v6MUO^2%lUF}G-zuZYq_ObF`O9I}AWPIt3g4`U6qrXWZ z4oFQnw6x`>aH8|~_8=^8+FK&cp>WN4+-omDo|!ceh@Cx%q6ZTPJ1Y#8pS|CF+nGHD zqw`NDRrp3->WJW8Tdd@_GH2>{y7hsU$e5`sMu0-`eWe`H- z*UsFfnmED&>M@<`_ZT}*LzCo+Z4dJ<)C{m0iGif#hA=ByE5gC^J3nwnbgkm+8@VB-H5SU_eCj=c&U3U~s z<$#udUysV2Wv`CwEk4tEqnKWWES19>7Nd5yYr&4ttOFRdTy4h^^sn_S2l&7;20e4Hz zy~z~Bf0nCP;JO%F*}esjIXp3dCN1W$O2Wf~z19h$6JHs3vDc_hAM@?n5z3M&r$d!m z#|Kfv38rt%*@ZHk#eAivQiM;R5Y3XiqepJqZ)oAcIN<1~TiPt=5uojCuTq?UQov`` zuHPvV$(&b6Nh)(p)e6@)a)jhngXBp0kX$#>b}%@by;Goaj_Y$)mt5?n2SBerIg(3z z$N1Wvt_@4+NRKBz+bW<{3Q*FfLsL&#_21Bb;a`GbUM&OmUF(RI%E}{kI4ybHxow*1 zi>Q5jtCMo5(*U@$*;8tNy9A>a(nYg5pft{-Uyp2ihMF!V?zC#4jhd+{yK(hSiS9~mB{T+Dt1M(!U*L!kS z-~+~IR{})bQ~<)wA7?nP(kESz9)0*?`P8Ngw>&8^ZF0prZ&^X4-L8s(F}}@u zHhg$+QN2{6?OhNTBIel6S_L4~fRl504@2Ks2o8drnx55G(jG2kSrD7Oy~QFS&tZe} zhN9ltlREq-+qYsym@&L5zPBP#{&{k$wDfWiOYcO+uGS|14IUOjqZ(mq(>TlzJDYd% z;r4^t54{F_LP@9_%@wvaUf;2bp+E=MWcQqro9eG`c0C^4rCgM`?gdNpl&X^5?T*nk z9-}8Hk6c~k!3Fe%H459|9-nD4OQ+~y)2|#E^zGigTp2^02;`k%FOi}by_ou81v~>Q z;Yy_{nA3S+=S?aj%#x9y=ZL(&?wY=Kn*t&&TkVe1f0i`PjuU#jpNGu4%aTSw9qVM2 ze5}*7OIZHx!hBK_Thk-gi|m@nD=ZTo$5Wa-f2h0xvJB`6K5Y{QC*|J_vjB*91V8G) zF;E7nYzFL~qJQrafD91<5d-@p$+)3#a3pL$Z0_OZ5_hQ!(-s>F)lk!5&lO`d=AqzF z?39xu6&)zLkQJ`1Hz3|~g}aFCk(luZ#IK<6Eb;7@JPtdCf6H4i{&`oBCStqce<9bHsB+$4|DEFDdZ`?aa`H$0 z@+AVx=<1Dc-_81k8p{=l?#IqeYy7>0YJr|qfqvq_BsVBRh)ft5e8%aS|ae#qo7k2XUoOlViql6`r zgS*tLl+E1O7D>pm`c@rkErfLbaIcW$Rp z>=X`Ui;x2$ONnwkgs;-aaDoV?kZ+QI`>VVSlj^&6f#fh)u5&g)W{-K#Wi}m69)j$DoTKE4$N zn7-U(lfuWoXqv!h&T)MzQTY0HJy-ek_5*Hq?|{npJw?jgz{(h3sz;Spr#$ET?dfOY z`#d~o_zy5pH{Xz(OfWG?{pj0=U&lGm2QbL!g2L|OiVo4)!NT_0ys{AqgYLKPW)IN> zE=%5p7-slpu}i^47(aU=(LmEKNC}^ZThY20qrZXUJ0J%t9FS8KEC%{zs^pry zY3vZGXx?6;FRz|;EZ4_284zO!wY+wTqa!|3KB*r`zi}j)`#1KA4{~5!Oo}g8=!Y44 zYj(ILu4)&ahp|?d6_>M@;MfS6 zS+;NqM37<;hjh6P3N@YB?35I?ga-tdZ^aEUH zQqQ`7cl21?obWN@LIDO>$Me5DTC=XK_r@q#&bN5t&lg|&EWGO) z+NK>~bPURxQL?))YQT`%kP_dFE0 z^{zivNwH8y&jlla%e-?=udfuaFR7%7dY8&?1OuKYav@xgMRg8RPa|eB9O;6uKmZQWmZj zL8pp1%RO=fMX-1BW3Fa!1F@b$A3EGe*;JXSC4m^@&0QT&g|<3|?#E(F4R>^kgJs6h zBaBjrj3&_e*tR;dM!@gO=p0ZhPuV&v(|rdH(ipjY!hcrbO^0Unc15``*oHO(Q@7B5 zFLzh;^SP5(!w?h&QcIIHn)gyb^;+Ex1|nt4==+L~BnL=U&`A5QKA9bYxm}m*w`Um+SpGYV7Id2lp1#A_}yQ@*`pRDZP+z0XvqguG0 zNDXC9If4$L{Qox7FQDIRLN6z$4~_zFYEK$vuw&VI;=iw#p6R)Y#|L5r;J#3ER(x9>j{oDr-05{I zz$VVned1fO?K)Z&dipg17#{6AD+uLfsax7ct+Kxd`?O3b9njR{g!Y-Ec&jlzh}F8&4Of5yF|!BecvUd$p3zfd z#P`@~B0?XaL3Ya4E(VB|giEw7PM+nuI*{z|)c6&HQ0DiTlht0+LBYze;5PH7MJqNt z)n!Y=T3~ee0n!t?bv;bUFL*Q_7+C@ZWj2s9xGQ*vWG^~nB&IMKY{O4a&nKfMmZGUX zGKCVywQOO<3paA9KMFcnTeeieAh5Zg-3Sp5+6J(=+BXh1+l!F)OdcyFhAY-t>O9m`H_GyJxE4jPJJ_xYmrL-dDSZz=wm@8 zWP-_^5yw0my#mcAPdC5VPf*`&qRqPsfK*vGb5~wq;LdPLeo6P|B39d&=(oXp%I1%& zm;u2+Y|@)*nGyp8tT08VvRYT=E8wyR&6oAz>~t?sjJ%x0OcPZxdWHgd<=R7AeJHa@`x|hXS(J zq&Cm;;e7L~)AW1JkO)?p@}6^;f%!~b-zNFv6ku0%h^zYUxVS>g$ii!x4GlC;Nd>qJAyawE|V*eZQyvWiZLa~ie3(HU_t1iGp?lQ2;7#4Wc-pO*wPN~ zGXs{6b-y+hMov<8p?Tab5o`AeWrZ7(!|*@finbh>=~OOWW6z-eA<4oRp@ZaGo5RQQq?I6VczhNFDLGtlubQ%TtdC8AY~lyg`o21BB3 z($wiCYWyO+r}SoVGWn}7<%mCge6(y!auTg*{v=~u1ylP}_J!2hB^iW^BR*2S3bto_ z_Kv;!YZ2w*wEpJSwZ46S5@mi)$CW}Ok@ht;u*Hkysz?I~6$~RnbG#?r>e-kboDQLB ztq(|Q_G_E6j1jfzxjU&yaj{48zu(Q4(Rd!t&yw2P4aMfzwPMYeL`NQLSqBY&C4=?=1$Y(`uA zscbr|QI26P+vC#;*AazKvnfQpO3BJA!F=m8b~HKVIRQTsDm|r_YnH~%)a|SeHl+!P zM-saSFeMC_wS9^mw4YVo&X-2XgI+HN;%>AM1e&DujTkgp{xe3odx855aCdJ(8{Kw= zZtLgMK9MMB-HT2tqmf^LmgAFIf5X^t6JxWx*a$T)4@zwzAVu}%mZHy65U$@W>`oec z)AroZ=FKwBD48Nh?M&lGiO@|d_!~V?E=M43umiY1Na>^W4Uq`-$BdqKdpidHyXTyM zFS4LrO(%9{@C>Y~?|0g|4P!uX9C05)g{94XH}krdl82<$qyf0IEZmK~l zw(uIvuSqBXsxq4_$77Y|<{Mn&pbXK$%vCGn12B+W1gFn}(X8^Pz^lgw3VauSGV(I;v>yNmvPa@Gr0dtdxTd!9gbG-pXR5G-u?!q!v@d%x@=e57IwkKRE6ozoTTYmu5dA+A6tvvEbq z#ptt1G&O6YJA|#vfNi#^2j;@&Ks1a%ocuK~#(0($dmHmS7Ndr3-Wb=&5!5Y-kmH)B zSu!!Im}z>52bH0TO)t30rM2X`1jh|3ZSRwlQwIHpFv(RkG!D{N5g+5&VF)B;Vl?kp zR_l1JXz4-0Tr2M&Vp`&*ErR|J@t=Ai&S!@Q*KW1#-y#k?%j6svs)snJ<)*8soN&FI z&lIQ3-Kli$JQD)dqbRn+g}d4Mr}1GRC!(hu%7Fuj-eDe2z?9TY<)w==5?<9WsVmWZqN<04JyFOOvHzp|-j zw|R24mbWzx^r|IO*{Qz|F$>zVVPw%g`DAjQ?51%Q_h1GZ(-OD2xo1|2G(P7Y2@)xO zS$Fyfx~rqE;F6s38CqQDGEua$xcNIRd1pu&#CP^SRYEFI(|J_lNQ8&#QlBxU#>mtR z`~|kzw$_69gvA}zh2o&JL#@suH~go6~Vh9S&Z;w>v*e@e8Uw(+ksvi$%2EsgK@138<#ni`tF3)E4i%PC?U zpJktz@V;MuEzLjWvFY*m!_wV5`7aHPa$uc^U|i~5@WAGYI&L{!bUARyOWEHFkdypz z{UFrrY<_p`V8vP%W%)zXo(JtWW!Ci)gtEcC$RFzG-{}xs{NE`m^Fe~mCoj_CJ@8j& zy<@tfZHT|z?e4;{+@Nrg4kxQYS4tt^zf)OiFiNZ*_`D8y%;f@hJz_)fq-E%StpU5U zSPu?u7o+ZvUY3qjlt*C6$u+0j7%qpn;TE9`!A`C3Sct2FLtyML>6i^H{;F3-8l1k zq;KCV<}A>6gh2SiuaRbvS&y3StOUaPREoVc4;S<<3Oon#eUR_gw*>_Jx%%%1MN?HR zKB)7WB!98h=BK9zfP2`2aoh*O(7x;cHPX$)U&v03X~jI}SzxTxVNOy3BWwvP%eLtC z;Fy-^H{InS_O;8o%8M2MoidK2LG=uJUtQ+@3JeY-rX|@fazu8Gh&uG=c?l_muBb=` zgI;Z#3dDsuH}oyt03GSTF!yV6;4|6k)|(FG`R|~I#l`XP91g4J4S_Hostog0?;3{a0yJ=yxlLkW@%23=Zk0W zR}{7n%u#sxA}EThlqd1Oh}b*-N5^n1+ukKp} zElCoddrbcD)opx2xtCIM%2&c~ELh?$#(X-jvW54tTt~lh8d>_xMU}utIT;-};Wgtq zsh3nAh^$e%L&4NA#9Qg8Cu%oq@Y|j)2vbnS{=a^s_sM?bC_=hF~}EXax7-`D@& zu%1iZ*{5Wi)U{$Xe^D8Y!%gatR%GUe+IdEmib6lqe6|1XI&n{rwObaw78IUlj$6t6 z<|)7ieJp-9ygnrmn&-*~`)+AD^;!Mx`(HW=?}i{Wi(Cs1y!)Z)Wcb0OY*c?evVTgd z;I4vz>g7LTh%8MC_P zKp#(SH6GfE`T76=xNuqw2-}(;KdsE@&V*U-7gGu{4w{P!)DITV|m7P8GoSPnBe--~PdrnQM`K`^w} z@U4=zIedRG`BUnd_@P5aE$|-8@rRvVxCZ!`9ObsNU8qs`kcZWW$p%af@RPb*M zK3lJ_rw>FU;!v7&468KFh4*Isa|pat$GreXl%@Woi0GCm_=UbiODT z@YPp}*pn|hEo1-Pq?-9u(jGSKEW$if2FCZIKzA44ww7F{TauOkM zI)Sq4hhXz1(Q!54)x4L1>&6Rf*+-n1gy$By{n&t_IC)v|1>t3~Fam79N0kCu*A{>qD=f4FYXSG*dQA_RlFPr)|nFi{;H5|S5i6Vn{g@{yUShfV0J8L@EZ?3_N^^uPZ* zb%oy^aCP$oGQal-VXSSco4Eh3p|WPhE-eE7o3yMNLt$iEwVQk=vJJpdX*tw;D|f#~ z8@#H!;rYvf|@^3Ysp6I%H zw8NoCd-gb8np!A&&h@L(E#IZ*GCHuO{pnB9g(4JmpUPO~-bT%AYEPfmt9UeZ17o`6 z1)nSwk0hC%Zl3cim(lh2fgI37EXRVD-@{-!!lL=Xyk7P6n=NJE`IQx)x^}x_&c~Zq z5njUOFj6piQ={l11=o=GG{+V`SpQ_=(*ZEk-zCB036h6<0 z)+_)!JQJE=#)*%G6tTBOUyX@2I5h6f2hVSdA*U{M*(Gr@?Mlaw`xpBkl{dEf@Y4(u zf6dy%KaXcu8jz^S*G&eBXy0}=)77BlZ;zVG$$&HTJ41c?7^0!)CJ{}-kCzN(ga7Cp zsDz{VAB(pt7_W_F7IgdkB=}Y_sS^rI#UKvimScAhm#m7LJ&(cmYD#pCXDgT;8c>lN(G zd9Q`dnx2QE4zPuOb9ku=9?-ZEb5m_v2<}11G}_N!O(_3pT1!t#O?i*O7pC+cYOc0z z{O%1PX39maPgJizz`_De`S1I}%LX}d0}3;^6G+DCTL2}_tlPsCL#_tW*}&w3$61^X zd++p9L0|lLEZZo+N0ig#;jJLcr|1~Yknf33drbvVDCN&?bCC+vCyn&nR|m4)2^?Ai zWgG3yRas?9L(&H>cxUxGhvCgxxL~=XG5@Cghc`Y3xzuhxYR{Z)?<@0vzEgH}-n4vz zHnZxY-n^*|cc~%omF(LM^sAqDhlwbfSSmFAps*6|RbIi|7Il+YOIvfI1)Y=R44 z_j&MTJ{bKZG$N_7qf4@FD5oKB-`r^7PNAp(04FGSteNHchKvSu*IB>UX%>f#6^9P& z|Cd_PW$8;R(>n)0XTtg65e6r_RdPcXDdDvf1O;cBxN`IkT#LD$ z`M>{bCe@Yh>v*u5xOn_EQYP$vw^uP>u%HOA!Q_Ii9XD-&#r61$=f4eUE40~R7u_Rc zac_MJ*EDbju$hZyoDfPf z<4do`b#=4cNnl^eCl@I6lXJ;B$t`A3_A1SLsRQqlJV!9bkRv|LhKn#-+3Y2ph;-gO%xe)@RBnRWR}K8}HR?*s&Pmo@tI6P? zfWn{1^(Je)HPMJXuAHeSm*kt)*?fL?*;YDyvDt7kcy~!DLeq;8JNRZ;5t()nZf~*{ zR~)WazXL*G9YQsI{euGoC3_8jJp-c7loxY~MM;h)(zB3{vZ94#$)&dXVJBH{j>s>TO=D*6ooa-xd4aoU zY9>_zzp@}rp)N(MvmJN3PV1fPsJa9BVTp`eG(0MVwf1W^fLxV(qT|ykcXfK>nc@*? z+r+x~WBEK|Xzcp43Q*JOg3)gu;3GywSK^IetwQ-L?>I_>4o)`lDgkTn#q=1YG{5#L zGS-Ox__M~e$SbFwzVq>pNd;u1Ij#dSiu+IjPlxwX#sv!*TAFw#pH4z4;*T-iuRa4* z31?C}N)#!7bWdsdM?|=mpB8z@3H;c$p{EIoEZJctO`aQDeDaLv^l7uPWB4s^Q5?MO z?*0hbNz=o{MNaXd+72#|s8es9tRD^(hvLLx_}fP=_MA{TSfuoiVkarn~vf8i{Foq1?8;FULlPrJ;&BB{_OBHAh5t4t4s=qpw!; zOhX$tWi7UbgN5xMuk<1aE*L#mpC~1TT4zN#QzOeiHSzs8(-98u5Kq+vf@?pOU&3^2 zZ@~AzG||RW6L52SR=zw$rC<)#G_ z)@sH9+?3u_ceE^xV$&4BY-){B2p)7-w+Km$G^1riMpqe?G*@>`8kF#}Cf2i=-f)PW zcLbO5f|VXWh+(Sm5$Q18hT)EqquwI8&>nrNCc|K>H6&Q6-C=K^6M1++CLYPNl{v5Z z4}Xge?!I;Uz?l?%E)S?!cLFdeCd(&CFAE5!Q$=64ocx?Lq9uR0zTdy0F-9Ab!^V=PwSvsfi?I8z z!}v?g#?3CTA5J&^eq^?Mlr~eB$qfuWy~kc-k+CyQ9enhGLc|+D4j*Oq+J;PqE3`X? zPrH4qOrAx0b(Y~@;NRAScqw8MYrxAS=KBYryEivcU;H)^io*Cb)c}RSVLK2*CD>dg zA^KFnQcm$g!bcGM>f@yEnADo*xJran(yv_FlJx91{hUgIMQdc_r06HqZ5KFO^+gro za%c=YVck-VC{2b%Z^T=VnY|~JGDxBFesk|el|`XO6EAVa`awI(P!D_WK=%XB+NWN& ziipKmR_H5FJnmpF;v;&t)TJjD2*-4g?R*Ej;QzgmT4mtH97^Uk(o)mho5k{Hhzo*y(YD6W|R%En%lltUTv$yMy_74 z1_L(PRDR=$dfMFRd!2B>@@bXS7(S_v^;!r!17I3FdOUg0qO+?}(eeDS)>~R^^||>o zGwhF5d1PjcsMb#AKYGufN9=<}@j@PUbz&?oLQ3@y69o`tItuKvOksev6H88dY}J7N z0(7(p)h4R2Ep=JwVSAls*#?S^_aZ*AysQ-tsqj|2zO5nzzvF8iJoedlA?C@A-c|s^ zG}&mTA7>ELfNO-ZvAWSkPJEm<{%_q@)6Z_(nro0RS&h?_2Gz$(6?XB=B=7g2U_7;H zsdB?WUIYUWvxThqOnX1Ji4nXhOOaPJO9~mRqtaO2Z=k5g>X6On9uHYY9AZSawqyle z)9O@n5X)tHFVvty{H@San`0~C?3dBX^rJ6Q?>L@B%$35@N-H6YAhigsJLWfduT9cz zA{?$rtmwr|*yFEEO0g@zmHc~?Y}Q9Q>5D`^%L}{g356Jh?XjyYWJ&MPvqkWh_ur3K z+xm2_`)RItYq8uhWa@XuR6zOS5)YQ~J)Sk07F98N=B6Vz=v)|~e6-@$zG4RDU5NPi zr!l!tC;Wo;F3~J>X(%;u7l}cn-g$yO?Ne(Y;^S7xj|{H(*bnxiRCcbTJIP(66UT7w zoJ<`@6eEWjalyYrmR;_{ev7C{cVt;4!UeMxxTlj@Hn1e|Gmkj@i#k#gn7Ua5?!FO? z+J^ju?+Uk{EgRZzjUW*A$OwaG;)-i19rCvHoXYN*IOeu2_`+$RO+cZFsx0Mi|Pm=gF;|X`jPX2r`_ycSIQP`aR9QtH-Yi@&Tn4t zF$2ie4b&q*;o&G&OuLt;5}}-}JSKso^6^W5l4xR4CnLZk(`KQxUdFQ_u}~>W;S4e; zVQkHoynIeG%l}8f1H96+LF@cXy-Ym|_S`V-5iDvV$Rj?189>%E&tJMHV3C+G$V}xr z)C#zcl4DPWmok&(K`U1o^mzwswf<8t;0M$kPaY>*D@@j+w;)G7iO9+zP^4Ub(ejC)$Jc#9V&as=BFs!2!C;Q{AGDbpl7p0zV z2`cX#hkh-mvhv`ECzbQ3N$b|pL@*BQb8}w6skZSY$YZ7B3vt+CIZ=OIOZNoCg(XYS zEysU}_6{I@(DQ1~Ps0i60o!<|N9RFxQ15VLqJ|k>IQ34c|Ci9j)HCIbQB$i0dDI0m z_9h&<8qWqcAoijMOdaE8^M%1PM0L;#D$B-8!?Sw>%;pAg%p6THh%L*9U68y|4Kw@n zTp-1w$E@OT_RMSe!s=JE1P9}pUSE(v0J&#cn;65bM1dX;I$h2bbhVqK-@#t}f<}*0 zmJb|~`bYm>!VC`x11v=%j?tlG3l17sZv8pa_s3onpE!k~s75Q)x+YHMF@@>n?&p z5%-$_H)T^3L6O4dmN=pTiisG+h-b`|*);}Kh@>!Cq5RNB3_CDF9lg?0@onzEC8(Dg zli_h7`TeJ-LT8h+bb14R>p}#R()k^Ef=8A+1ITUbOhk>#bhM(GJr;hRhZ4fVq#5~X z_zm1UiN&IF`fzsj4KDi-O3{62{64TM4}2d8EH+U+nCGO(BKfODmG{q9v%oP=Ey1`U ze`AaDN`5I1OB8z5%@_NwcmH##O~N-zcI){UT}T%|a6chx2`l%T831x>c=}1p6)L^| z=+MOAb1jpo54PkRxQ_we7*Cy(OwasNq8yrXIs-t_4~@+ZD;~vBRfy8Ko;~CskPIO1 zgRF82i(ohl)-szzfG~0O1+c{Es3RY#)3|X4pv732Vc`7RF*WD?LLr_ch5ch=L%68H zBc;zyGN*cwxPa~13_nW0dKF?4ifhwDk!-cToFbdKfSsJfxsEu6?G?}5&PVa3@$^23 z>9jV;3R_nI70?Mc6Q^NNcf6%3h-)4Eq2wDZmM*DV0}Q3*&~V=V9{~8L8&blK24#y2~E<&16Go=P%k^ z$?(%um|u(dh1}O>gsmYB^@vV?*hM|cI6do*?dSu{*GQt}$UVIXVO}^JL}UD!p=r(t zUv@`Qk!)Pahi=L>Yyf#*JdTI|a#2^n8@a5)^V2$V$UR){79x=$mpJA*IaTJY5ue4cjV@XheTj~IoE{Th6v2I(dC;H{ zO)Ni^qbpk@2#iSL6JV$CnlXcI~?KFdd4f-rc}jyo%EAY4pslM!rIuqC7SGb_WM6nNAdEL}!Cv>TIZU zZ$%BLCZlrqLhXgtq&tKUkdcvl#IEqe1WTs9e%_(@=lko4;hCPjS5i_leyZ|AJew7Y z4jMp)tX=S8q~+rm%0ZjVCOzne!6>CP95=`K#fT^wLj1sO>A=?r&R}UhUiOOtSP2m` zy*(OpYHcV6iBNUZ_boEv0z6yP7hWfVDASWEKN%O07MFmGIfdjpfwenigP5s~XNv5h?ykb3{cjYn7jR7|PXJQdAa#uEZ;=b)`p0FoTMLYn z+shM(fBJiCoJj03Enp6)Q)!b_lk@&D*^o?{s^S`Jq3#v`>ZEppeNl_R_0%%;LBx#z zE)^LQmQEMkz>SW6O{UN>@oaaSLN$M-TPa*sE7J|#e-O}mB2i>b-~9AKYUG{IRRWe-Z7nMFgQBM)SU9~IJ!#@_+E&H(Y2_PhZ zql)X)_?a>Rd-^o(9x}%)r#c5z@)0m$CSeFhK{#P2!+Gp}0d=lkdL4iwYM@d^2-B%~ zW|4_|hv49(1bKosAabrBB2gJdTN?kZo$+7SH?gD|ZBL;K##l%olN0)Q&-y8#u_r&l z7pb&?%Ion=7DA=7!_N_kdG}smv!c&;A>uY{gtmY*J_8cgi)#hYpzhknx5>UJl0+O( zg&2Yg>Vy#x=|YW#Gq{b*wD>{C_(G)YnRCzr0xZ_o8y-WN!>wTnykHX_1x~8)nuTrN zX?;~B%`JBO6GN{Jee&aL7H%GjZqH3sn2u~*nx(ztSFpzrmNMO1)4iIw@K&$Ya#NQk ziptRTVm9Cg=V~M zL_6^6 & tf, const std::shared_ptr & costmap_ros) override; @@ -182,10 +182,12 @@ class DWBLocalPlanner : public nav2_core::Controller * @param name The namespace of this planner. */ virtual void loadCritics(); - void loadBackwardsCompatibleParameters(); - rclcpp_lifecycle::LifecycleNode::SharedPtr node_; + rclcpp_lifecycle::LifecycleNode::WeakPtr node_; + rclcpp::Clock::SharedPtr clock_; + rclcpp::Logger logger_{rclcpp::get_logger("DWBLocalPlanner")}; + std::shared_ptr tf_; std::shared_ptr costmap_ros_; diff --git a/nav2_dwb_controller/dwb_core/include/dwb_core/publisher.hpp b/nav2_dwb_controller/dwb_core/include/dwb_core/publisher.hpp index c8bfe27d4f9..aba5354c652 100644 --- a/nav2_dwb_controller/dwb_core/include/dwb_core/publisher.hpp +++ b/nav2_dwb_controller/dwb_core/include/dwb_core/publisher.hpp @@ -69,7 +69,9 @@ namespace dwb_core class DWBPublisher { public: - explicit DWBPublisher(nav2_util::LifecycleNode::SharedPtr node, const std::string & plugin_name); + explicit DWBPublisher( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + const std::string & plugin_name); nav2_util::CallbackReturn on_configure(); nav2_util::CallbackReturn on_activate(); @@ -124,7 +126,8 @@ class DWBPublisher std::shared_ptr> marker_pub_; std::shared_ptr> cost_grid_pc_pub_; - nav2_util::LifecycleNode::SharedPtr node_; + rclcpp_lifecycle::LifecycleNode::WeakPtr node_; + rclcpp::Clock::SharedPtr clock_; std::string plugin_name_; }; diff --git a/nav2_dwb_controller/dwb_core/include/dwb_core/trajectory_critic.hpp b/nav2_dwb_controller/dwb_core/include/dwb_core/trajectory_critic.hpp index 98594af1f4d..d204bc52c25 100644 --- a/nav2_dwb_controller/dwb_core/include/dwb_core/trajectory_critic.hpp +++ b/nav2_dwb_controller/dwb_core/include/dwb_core/trajectory_critic.hpp @@ -97,16 +97,16 @@ class TrajectoryCritic std::string & ns, std::shared_ptr costmap_ros) { + node_ = nh; name_ = name; costmap_ros_ = costmap_ros; - nh_ = nh; dwb_plugin_name_ = ns; - if (!nh_->has_parameter(dwb_plugin_name_ + "." + name_ + ".scale")) { - nh_->declare_parameter( + if (!nh->has_parameter(dwb_plugin_name_ + "." + name_ + ".scale")) { + nh->declare_parameter( dwb_plugin_name_ + "." + name_ + ".scale", rclcpp::ParameterValue(1.0)); } - nh_->get_parameter(dwb_plugin_name_ + "." + name_ + ".scale", scale_); + nh->get_parameter(dwb_plugin_name_ + "." + name_ + ".scale", scale_); onInit(); } virtual void onInit() {} @@ -181,7 +181,7 @@ class TrajectoryCritic std::string dwb_plugin_name_; std::shared_ptr costmap_ros_; double scale_; - nav2_util::LifecycleNode::SharedPtr nh_; + rclcpp_lifecycle::LifecycleNode::WeakPtr node_; }; } // namespace dwb_core diff --git a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp index 245323db510..4b68eafd49c 100644 --- a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp +++ b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp @@ -64,61 +64,65 @@ DWBLocalPlanner::DWBLocalPlanner() } void DWBLocalPlanner::configure( - const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, std::string name, const std::shared_ptr & tf, const std::shared_ptr & costmap_ros) { - node_ = node; + node_ = parent; + auto node = node_.lock(); + + logger_ = node->get_logger(); + clock_ = node->get_clock(); costmap_ros_ = costmap_ros; tf_ = tf; dwb_plugin_name_ = name; - declare_parameter_if_not_declared(node_, dwb_plugin_name_ + ".critics"); - declare_parameter_if_not_declared(node_, dwb_plugin_name_ + ".default_critic_namespaces"); + declare_parameter_if_not_declared(node, dwb_plugin_name_ + ".critics"); + declare_parameter_if_not_declared(node, dwb_plugin_name_ + ".default_critic_namespaces"); declare_parameter_if_not_declared( - node_, dwb_plugin_name_ + ".prune_plan", + node, dwb_plugin_name_ + ".prune_plan", rclcpp::ParameterValue(true)); declare_parameter_if_not_declared( - node_, dwb_plugin_name_ + ".prune_distance", + node, dwb_plugin_name_ + ".prune_distance", rclcpp::ParameterValue(1.0)); declare_parameter_if_not_declared( - node_, dwb_plugin_name_ + ".debug_trajectory_details", + node, dwb_plugin_name_ + ".debug_trajectory_details", rclcpp::ParameterValue(false)); declare_parameter_if_not_declared( - node_, dwb_plugin_name_ + ".trajectory_generator_name", + node, dwb_plugin_name_ + ".trajectory_generator_name", rclcpp::ParameterValue(std::string("dwb_plugins::StandardTrajectoryGenerator"))); declare_parameter_if_not_declared( - node_, dwb_plugin_name_ + ".transform_tolerance", + node, dwb_plugin_name_ + ".transform_tolerance", rclcpp::ParameterValue(0.1)); declare_parameter_if_not_declared( - node_, dwb_plugin_name_ + ".short_circuit_trajectory_evaluation", + node, dwb_plugin_name_ + ".short_circuit_trajectory_evaluation", rclcpp::ParameterValue(true)); std::string traj_generator_name; double transform_tolerance; - node_->get_parameter(dwb_plugin_name_ + ".transform_tolerance", transform_tolerance); + node->get_parameter(dwb_plugin_name_ + ".transform_tolerance", transform_tolerance); transform_tolerance_ = rclcpp::Duration::from_seconds(transform_tolerance); - RCLCPP_INFO(node_->get_logger(), "Setting transform_tolerance to %f", transform_tolerance); + RCLCPP_INFO(logger_, "Setting transform_tolerance to %f", transform_tolerance); - node_->get_parameter(dwb_plugin_name_ + ".prune_plan", prune_plan_); - node_->get_parameter(dwb_plugin_name_ + ".prune_distance", prune_distance_); - node_->get_parameter(dwb_plugin_name_ + ".debug_trajectory_details", debug_trajectory_details_); - node_->get_parameter(dwb_plugin_name_ + ".trajectory_generator_name", traj_generator_name); - node_->get_parameter( + node->get_parameter(dwb_plugin_name_ + ".prune_plan", prune_plan_); + node->get_parameter(dwb_plugin_name_ + ".prune_distance", prune_distance_); + node->get_parameter(dwb_plugin_name_ + ".debug_trajectory_details", debug_trajectory_details_); + node->get_parameter(dwb_plugin_name_ + ".trajectory_generator_name", traj_generator_name); + node->get_parameter( dwb_plugin_name_ + ".short_circuit_trajectory_evaluation", short_circuit_trajectory_evaluation_); - pub_ = std::make_unique(node_, dwb_plugin_name_); + pub_ = std::make_unique(node, dwb_plugin_name_); pub_->on_configure(); traj_generator_ = traj_gen_loader_.createUniqueInstance(traj_generator_name); - traj_generator_->initialize(node_, dwb_plugin_name_); + traj_generator_->initialize(node, dwb_plugin_name_); try { loadCritics(); } catch (const std::exception & e) { - RCLCPP_ERROR(node_->get_logger(), "Couldn't load critics! Caught exception: %s", e.what()); + RCLCPP_ERROR(logger_, "Couldn't load critics! Caught exception: %s", e.what()); throw; } } @@ -164,40 +168,45 @@ DWBLocalPlanner::resolveCriticClassName(std::string base_name) void DWBLocalPlanner::loadCritics() { - node_->get_parameter(dwb_plugin_name_ + ".default_critic_namespaces", default_critic_namespaces_); - if (default_critic_namespaces_.size() == 0) { - default_critic_namespaces_.push_back("dwb_critics"); + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + node->get_parameter(dwb_plugin_name_ + ".default_critic_namespaces", default_critic_namespaces_); + if (default_critic_namespaces_.empty()) { + default_critic_namespaces_.emplace_back("dwb_critics"); } std::vector critic_names; - if (!node_->get_parameter(dwb_plugin_name_ + ".critics", critic_names)) { + if (!node->get_parameter(dwb_plugin_name_ + ".critics", critic_names)) { loadBackwardsCompatibleParameters(); } - node_->get_parameter(dwb_plugin_name_ + ".critics", critic_names); + node->get_parameter(dwb_plugin_name_ + ".critics", critic_names); for (unsigned int i = 0; i < critic_names.size(); i++) { std::string critic_plugin_name = critic_names[i]; std::string plugin_class; declare_parameter_if_not_declared( - node_, dwb_plugin_name_ + "." + critic_plugin_name + ".class", + node, dwb_plugin_name_ + "." + critic_plugin_name + ".class", rclcpp::ParameterValue(critic_plugin_name)); - node_->get_parameter(dwb_plugin_name_ + "." + critic_plugin_name + ".class", plugin_class); + node->get_parameter(dwb_plugin_name_ + "." + critic_plugin_name + ".class", plugin_class); plugin_class = resolveCriticClassName(plugin_class); TrajectoryCritic::Ptr plugin = critic_loader_.createUniqueInstance(plugin_class); RCLCPP_INFO( - node_->get_logger(), + logger_, "Using critic \"%s\" (%s)", critic_plugin_name.c_str(), plugin_class.c_str()); critics_.push_back(plugin); try { - plugin->initialize(node_, critic_plugin_name, dwb_plugin_name_, costmap_ros_); + plugin->initialize(node, critic_plugin_name, dwb_plugin_name_, costmap_ros_); } catch (const std::exception & e) { - RCLCPP_ERROR(node_->get_logger(), "Couldn't initialize critic plugin!"); + RCLCPP_ERROR(logger_, "Couldn't initialize critic plugin!"); throw; } - RCLCPP_INFO(node_->get_logger(), "Critic plugin initialized"); + RCLCPP_INFO(logger_, "Critic plugin initialized"); } } @@ -206,50 +215,55 @@ DWBLocalPlanner::loadBackwardsCompatibleParameters() { std::vector critic_names; RCLCPP_INFO( - node_->get_logger(), + logger_, "DWBLocalPlanner", "No critics configured! Using the default set."); - critic_names.push_back("RotateToGoal"); // discards trajectories that move forward when - // already at goal - critic_names.push_back("Oscillation"); // discards oscillating motions (assisgns cost -1) - critic_names.push_back("ObstacleFootprint"); // discards trajectories that move into obstacles - critic_names.push_back("GoalAlign"); // prefers trajectories that make the - // nose go towards (local) nose goal - critic_names.push_back("PathAlign"); // prefers trajectories that keep the - // robot nose on nose path - critic_names.push_back("PathDist"); // prefers trajectories on global path - critic_names.push_back("GoalDist"); // prefers trajectories that go towards - // (local) goal, based on wave propagation - node_->set_parameters({rclcpp::Parameter(dwb_plugin_name_ + ".critics", critic_names)}); - - declare_parameter_if_not_declared(node_, dwb_plugin_name_ + ".path_distance_bias"); - declare_parameter_if_not_declared(node_, dwb_plugin_name_ + ".goal_distance_bias"); - declare_parameter_if_not_declared(node_, dwb_plugin_name_ + ".occdist_scale"); - declare_parameter_if_not_declared(node_, dwb_plugin_name_ + ".max_scaling_factor"); - declare_parameter_if_not_declared(node_, dwb_plugin_name_ + ".scaling_speed"); - declare_parameter_if_not_declared(node_, dwb_plugin_name_ + ".PathAlign.scale"); - declare_parameter_if_not_declared(node_, dwb_plugin_name_ + ".GoalAlign.scale"); - declare_parameter_if_not_declared(node_, dwb_plugin_name_ + ".PathDist.scale"); - declare_parameter_if_not_declared(node_, dwb_plugin_name_ + ".GoalDist.scale"); - declare_parameter_if_not_declared(node_, dwb_plugin_name_ + ".ObstacleFootprint.scale"); + critic_names.emplace_back("RotateToGoal"); // discards trajectories that move forward when + // already at goal + critic_names.emplace_back("Oscillation"); // discards oscillating motions (assisgns cost -1) + critic_names.emplace_back("ObstacleFootprint"); // discards trajectories that move into obstacles + critic_names.emplace_back("GoalAlign"); // prefers trajectories that make the + // nose go towards (local) nose goal + critic_names.emplace_back("PathAlign"); // prefers trajectories that keep the + // robot nose on nose path + critic_names.emplace_back("PathDist"); // prefers trajectories on global path + critic_names.emplace_back("GoalDist"); // prefers trajectories that go towards + // (local) goal, based on wave propagation + + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + node->set_parameters({rclcpp::Parameter(dwb_plugin_name_ + ".critics", critic_names)}); + + declare_parameter_if_not_declared(node, dwb_plugin_name_ + ".path_distance_bias"); + declare_parameter_if_not_declared(node, dwb_plugin_name_ + ".goal_distance_bias"); + declare_parameter_if_not_declared(node, dwb_plugin_name_ + ".occdist_scale"); + declare_parameter_if_not_declared(node, dwb_plugin_name_ + ".max_scaling_factor"); + declare_parameter_if_not_declared(node, dwb_plugin_name_ + ".scaling_speed"); + declare_parameter_if_not_declared(node, dwb_plugin_name_ + ".PathAlign.scale"); + declare_parameter_if_not_declared(node, dwb_plugin_name_ + ".GoalAlign.scale"); + declare_parameter_if_not_declared(node, dwb_plugin_name_ + ".PathDist.scale"); + declare_parameter_if_not_declared(node, dwb_plugin_name_ + ".GoalDist.scale"); + declare_parameter_if_not_declared(node, dwb_plugin_name_ + ".ObstacleFootprint.scale"); declare_parameter_if_not_declared( - node_, - dwb_plugin_name_ + ".ObstacleFootprint.max_scaling_factor"); - declare_parameter_if_not_declared(node_, dwb_plugin_name_ + ".ObstacleFootprint.scaling_speed"); + node, dwb_plugin_name_ + ".ObstacleFootprint.max_scaling_factor"); + declare_parameter_if_not_declared(node, dwb_plugin_name_ + ".ObstacleFootprint.scaling_speed"); /* *INDENT-OFF* */ - nav_2d_utils::moveParameter(node_, dwb_plugin_name_ + ".path_distance_bias", + nav_2d_utils::moveParameter(node, dwb_plugin_name_ + ".path_distance_bias", dwb_plugin_name_ + ".PathAlign.scale", 32.0, false); - nav_2d_utils::moveParameter(node_, dwb_plugin_name_ + ".goal_distance_bias", + nav_2d_utils::moveParameter(node, dwb_plugin_name_ + ".goal_distance_bias", dwb_plugin_name_ + ".GoalAlign.scale", 24.0, false); - nav_2d_utils::moveParameter(node_, dwb_plugin_name_ + ".path_distance_bias", + nav_2d_utils::moveParameter(node, dwb_plugin_name_ + ".path_distance_bias", dwb_plugin_name_ + ".PathDist.scale", 32.0); - nav_2d_utils::moveParameter(node_, dwb_plugin_name_ + ".goal_distance_bias", + nav_2d_utils::moveParameter(node, dwb_plugin_name_ + ".goal_distance_bias", dwb_plugin_name_ + ".GoalDist.scale", 24.0); - nav_2d_utils::moveParameter(node_, dwb_plugin_name_ + ".occdist_scale", + nav_2d_utils::moveParameter(node, dwb_plugin_name_ + ".occdist_scale", dwb_plugin_name_ + ".ObstacleFootprint.scale", 0.01); - nav_2d_utils::moveParameter(node_, dwb_plugin_name_ + ".max_scaling_factor", + nav_2d_utils::moveParameter(node, dwb_plugin_name_ + ".max_scaling_factor", dwb_plugin_name_ + ".ObstacleFootprint.max_scaling_factor", 0.2); - nav_2d_utils::moveParameter(node_, dwb_plugin_name_ + ".scaling_speed", + nav_2d_utils::moveParameter(node, dwb_plugin_name_ + ".scaling_speed", dwb_plugin_name_ + ".ObstacleFootprint.scaling_speed", 0.25); /* *INDENT-ON* */ } @@ -258,7 +272,7 @@ void DWBLocalPlanner::setPlan(const nav_msgs::msg::Path & path) { auto path2d = nav_2d_utils::pathToPath2D(path); - for (TrajectoryCritic::Ptr critic : critics_) { + for (TrajectoryCritic::Ptr & critic : critics_) { critic->reset(); } @@ -317,7 +331,7 @@ DWBLocalPlanner::computeVelocityCommands( { if (results) { results->header.frame_id = pose.header.frame_id; - results->header.stamp = node_->now(); + results->header.stamp = clock_->now(); } nav_2d_msgs::msg::Path2D transformed_plan; @@ -328,8 +342,8 @@ DWBLocalPlanner::computeVelocityCommands( nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap(); std::unique_lock lock(*(costmap->getMutex())); - for (TrajectoryCritic::Ptr critic : critics_) { - if (critic->prepare(pose.pose, velocity, goal_pose.pose, transformed_plan) == false) { + for (TrajectoryCritic::Ptr & critic : critics_) { + if (!critic->prepare(pose.pose, velocity, goal_pose.pose, transformed_plan)) { RCLCPP_WARN(rclcpp::get_logger("DWBLocalPlanner"), "A scoring function failed to prepare"); } } @@ -339,11 +353,11 @@ DWBLocalPlanner::computeVelocityCommands( // Return Value nav_2d_msgs::msg::Twist2DStamped cmd_vel; - cmd_vel.header.stamp = node_->now(); + cmd_vel.header.stamp = clock_->now(); cmd_vel.velocity = best.traj.velocity; // debrief stateful scoring functions - for (TrajectoryCritic::Ptr critic : critics_) { + for (TrajectoryCritic::Ptr & critic : critics_) { critic->debrief(cmd_vel.velocity); } @@ -357,7 +371,7 @@ DWBLocalPlanner::computeVelocityCommands( nav_2d_msgs::msg::Twist2D empty_cmd; dwb_msgs::msg::Trajectory2D empty_traj; // debrief stateful scoring functions - for (TrajectoryCritic::Ptr critic : critics_) { + for (TrajectoryCritic::Ptr & critic : critics_) { critic->debrief(empty_cmd); } @@ -446,7 +460,7 @@ DWBLocalPlanner::scoreTrajectory( dwb_msgs::msg::TrajectoryScore score; score.traj = traj; - for (TrajectoryCritic::Ptr critic : critics_) { + for (TrajectoryCritic::Ptr & critic : critics_) { dwb_msgs::msg::CriticScore cs; cs.name = critic->getName(); cs.scale = critic->getScale(); @@ -484,7 +498,7 @@ nav_2d_msgs::msg::Path2D DWBLocalPlanner::transformGlobalPlan( const nav_2d_msgs::msg::Pose2DStamped & pose) { - if (global_plan_.poses.size() == 0) { + if (global_plan_.poses.empty()) { throw nav2_core::PlannerException("Received plan with zero length"); } @@ -580,7 +594,7 @@ DWBLocalPlanner::transformGlobalPlan( pub_->publishGlobalPlan(global_plan_); } - if (transformed_plan.poses.size() == 0) { + if (transformed_plan.poses.empty()) { throw nav2_core::PlannerException("Resulting plan has 0 poses in it."); } return transformed_plan; diff --git a/nav2_dwb_controller/dwb_core/src/publisher.cpp b/nav2_dwb_controller/dwb_core/src/publisher.cpp index 7f7ceb62c35..70adb22d819 100644 --- a/nav2_dwb_controller/dwb_core/src/publisher.cpp +++ b/nav2_dwb_controller/dwb_core/src/publisher.cpp @@ -54,52 +54,61 @@ namespace dwb_core { DWBPublisher::DWBPublisher( - nav2_util::LifecycleNode::SharedPtr node, + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, const std::string & plugin_name) -: node_(node), plugin_name_(plugin_name) +: node_(parent), + plugin_name_(plugin_name) { + auto node = node_.lock(); + clock_ = node->get_clock(); +} + +nav2_util::CallbackReturn +DWBPublisher::on_configure() +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + declare_parameter_if_not_declared( - node_, plugin_name + ".publish_evaluation", + node, plugin_name_ + ".publish_evaluation", rclcpp::ParameterValue(true)); declare_parameter_if_not_declared( - node_, plugin_name + ".publish_global_plan", + node, plugin_name_ + ".publish_global_plan", rclcpp::ParameterValue(true)); declare_parameter_if_not_declared( - node_, plugin_name + ".publish_transformed_plan", + node, plugin_name_ + ".publish_transformed_plan", rclcpp::ParameterValue(true)); declare_parameter_if_not_declared( - node_, plugin_name + ".publish_local_plan", + node, plugin_name_ + ".publish_local_plan", rclcpp::ParameterValue(true)); declare_parameter_if_not_declared( - node_, plugin_name + ".publish_trajectories", + node, plugin_name_ + ".publish_trajectories", rclcpp::ParameterValue(true)); declare_parameter_if_not_declared( - node_, plugin_name + ".publish_cost_grid_pc", + node, plugin_name_ + ".publish_cost_grid_pc", rclcpp::ParameterValue(false)); declare_parameter_if_not_declared( - node_, plugin_name + ".marker_lifetime", + node, plugin_name_ + ".marker_lifetime", rclcpp::ParameterValue(0.1)); -} -nav2_util::CallbackReturn -DWBPublisher::on_configure() -{ - node_->get_parameter(plugin_name_ + ".publish_evaluation", publish_evaluation_); - node_->get_parameter(plugin_name_ + ".publish_global_plan", publish_global_plan_); - node_->get_parameter(plugin_name_ + ".publish_transformed_plan", publish_transformed_); - node_->get_parameter(plugin_name_ + ".publish_local_plan", publish_local_plan_); - node_->get_parameter(plugin_name_ + ".publish_trajectories", publish_trajectories_); - node_->get_parameter(plugin_name_ + ".publish_cost_grid_pc", publish_cost_grid_pc_); - - eval_pub_ = node_->create_publisher("evaluation", 1); - global_pub_ = node_->create_publisher("received_global_plan", 1); - transformed_pub_ = node_->create_publisher("transformed_global_plan", 1); - local_pub_ = node_->create_publisher("local_plan", 1); - marker_pub_ = node_->create_publisher("marker", 1); - cost_grid_pc_pub_ = node_->create_publisher("cost_cloud", 1); + node->get_parameter(plugin_name_ + ".publish_evaluation", publish_evaluation_); + node->get_parameter(plugin_name_ + ".publish_global_plan", publish_global_plan_); + node->get_parameter(plugin_name_ + ".publish_transformed_plan", publish_transformed_); + node->get_parameter(plugin_name_ + ".publish_local_plan", publish_local_plan_); + node->get_parameter(plugin_name_ + ".publish_trajectories", publish_trajectories_); + node->get_parameter(plugin_name_ + ".publish_cost_grid_pc", publish_cost_grid_pc_); + + eval_pub_ = node->create_publisher("evaluation", 1); + global_pub_ = node->create_publisher("received_global_plan", 1); + transformed_pub_ = node->create_publisher("transformed_global_plan", 1); + local_pub_ = node->create_publisher("local_plan", 1); + marker_pub_ = node->create_publisher("marker", 1); + cost_grid_pc_pub_ = node->create_publisher("cost_cloud", 1); double marker_lifetime = 0.0; - node_->get_parameter(plugin_name_ + ".marker_lifetime", marker_lifetime); + node->get_parameter(plugin_name_ + ".marker_lifetime", marker_lifetime); marker_lifetime_ = rclcpp::Duration::from_seconds(marker_lifetime); return nav2_util::CallbackReturn::SUCCESS; @@ -147,22 +156,19 @@ DWBPublisher::on_cleanup() void DWBPublisher::publishEvaluation(std::shared_ptr results) { - if (node_->count_subscribers(eval_pub_->get_topic_name()) < 1) {return;} - - if (results == nullptr) {return;} - - if (publish_evaluation_) { - auto msg = std::make_unique(*results); - eval_pub_->publish(std::move(msg)); + if (results) { + if (publish_evaluation_ && eval_pub_->get_subscription_count() > 0) { + auto msg = std::make_unique(*results); + eval_pub_->publish(std::move(msg)); + } + publishTrajectories(*results); } - - publishTrajectories(*results); } void DWBPublisher::publishTrajectories(const dwb_msgs::msg::LocalPlanEvaluation & results) { - if (node_->count_subscribers(marker_pub_->get_topic_name()) < 1) {return;} + if (marker_pub_->get_subscription_count() < 1) {return;} if (!publish_trajectories_) {return;} auto ma = std::make_unique(); @@ -235,7 +241,8 @@ DWBPublisher::publishLocalPlan( nav_2d_utils::poses2DToPath( traj.poses, header.frame_id, header.stamp)); - if (node_->count_subscribers(local_pub_->get_topic_name()) > 0) { + + if (local_pub_->get_subscription_count() > 0) { local_pub_->publish(std::move(path)); } } @@ -245,13 +252,13 @@ DWBPublisher::publishCostGrid( const std::shared_ptr costmap_ros, const std::vector critics) { - if (node_->count_subscribers(cost_grid_pc_pub_->get_topic_name()) < 1) {return;} + if (cost_grid_pc_pub_->get_subscription_count() < 1) {return;} if (!publish_cost_grid_pc_) {return;} auto cost_grid_pc = std::make_unique(); cost_grid_pc->header.frame_id = costmap_ros->getGlobalFrameID(); - cost_grid_pc->header.stamp = node_->now(); + cost_grid_pc->header.stamp = clock_->now(); nav2_costmap_2d::Costmap2D * costmap = costmap_ros->getCostmap(); double x_coord, y_coord; @@ -315,7 +322,7 @@ DWBPublisher::publishGenericPlan( const nav_2d_msgs::msg::Path2D plan, rclcpp::Publisher & pub, bool flag) { - if (node_->count_subscribers(pub.get_topic_name()) < 1) {return;} + if (pub.get_subscription_count() < 1) {return;} if (!flag) {return;} auto path = std::make_unique(nav_2d_utils::pathToPath(plan)); pub.publish(std::move(path)); diff --git a/nav2_dwb_controller/dwb_critics/include/dwb_critics/oscillation.hpp b/nav2_dwb_controller/dwb_critics/include/dwb_critics/oscillation.hpp index 538ddc7d294..457213b5024 100644 --- a/nav2_dwb_controller/dwb_critics/include/dwb_critics/oscillation.hpp +++ b/nav2_dwb_controller/dwb_critics/include/dwb_critics/oscillation.hpp @@ -152,6 +152,7 @@ class OscillationCritic : public dwb_core::TrajectoryCritic geometry_msgs::msg::Pose2D pose_, prev_stationary_pose_; // Saved timestamp rclcpp::Time prev_reset_time_; + rclcpp::Clock::SharedPtr clock_; }; } // namespace dwb_critics diff --git a/nav2_dwb_controller/dwb_critics/src/base_obstacle.cpp b/nav2_dwb_controller/dwb_critics/src/base_obstacle.cpp index b3870343cdc..e409b86ce4e 100644 --- a/nav2_dwb_controller/dwb_critics/src/base_obstacle.cpp +++ b/nav2_dwb_controller/dwb_critics/src/base_obstacle.cpp @@ -47,10 +47,15 @@ void BaseObstacleCritic::onInit() { costmap_ = costmap_ros_->getCostmap(); + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + nav2_util::declare_parameter_if_not_declared( - nh_, + node, dwb_plugin_name_ + "." + name_ + ".sum_scores", rclcpp::ParameterValue(false)); - nh_->get_parameter(dwb_plugin_name_ + "." + name_ + ".sum_scores", sum_scores_); + node->get_parameter(dwb_plugin_name_ + "." + name_ + ".sum_scores", sum_scores_); } double BaseObstacleCritic::scoreTrajectory(const dwb_msgs::msg::Trajectory2D & traj) diff --git a/nav2_dwb_controller/dwb_critics/src/goal_align.cpp b/nav2_dwb_controller/dwb_critics/src/goal_align.cpp index d234d277623..6ff25bfa939 100644 --- a/nav2_dwb_controller/dwb_critics/src/goal_align.cpp +++ b/nav2_dwb_controller/dwb_critics/src/goal_align.cpp @@ -46,8 +46,14 @@ void GoalAlignCritic::onInit() { GoalDistCritic::onInit(); stop_on_failure_ = false; + + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + forward_point_distance_ = nav_2d_utils::searchAndGetParam( - nh_, + node, dwb_plugin_name_ + "." + name_ + ".forward_point_distance", 0.325); } diff --git a/nav2_dwb_controller/dwb_critics/src/map_grid.cpp b/nav2_dwb_controller/dwb_critics/src/map_grid.cpp index bcfc5c44e66..c70f7125e4c 100644 --- a/nav2_dwb_controller/dwb_critics/src/map_grid.cpp +++ b/nav2_dwb_controller/dwb_critics/src/map_grid.cpp @@ -61,13 +61,18 @@ void MapGridCritic::onInit() // Always set to true, but can be overriden by subclasses stop_on_failure_ = true; + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + nav2_util::declare_parameter_if_not_declared( - nh_, + node, dwb_plugin_name_ + "." + name_ + ".aggregation_type", rclcpp::ParameterValue(std::string("last"))); std::string aggro_str; - nh_->get_parameter(dwb_plugin_name_ + "." + name_ + ".aggregation_type", aggro_str); + node->get_parameter(dwb_plugin_name_ + "." + name_ + ".aggregation_type", aggro_str); std::transform(aggro_str.begin(), aggro_str.end(), aggro_str.begin(), ::tolower); if (aggro_str == "last") { aggregationType_ = ScoreAggregationType::Last; diff --git a/nav2_dwb_controller/dwb_critics/src/oscillation.cpp b/nav2_dwb_controller/dwb_critics/src/oscillation.cpp index e7ff9c6fb80..450f5b9751d 100644 --- a/nav2_dwb_controller/dwb_critics/src/oscillation.cpp +++ b/nav2_dwb_controller/dwb_critics/src/oscillation.cpp @@ -91,20 +91,27 @@ bool OscillationCritic::CommandTrend::hasSignFlipped() void OscillationCritic::onInit() { + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + clock_ = node->get_clock(); + oscillation_reset_dist_ = nav_2d_utils::searchAndGetParam( - nh_, + node, dwb_plugin_name_ + "." + name_ + ".oscillation_reset_dist", 0.05); oscillation_reset_dist_sq_ = oscillation_reset_dist_ * oscillation_reset_dist_; oscillation_reset_angle_ = nav_2d_utils::searchAndGetParam( - nh_, + node, dwb_plugin_name_ + "." + name_ + ".oscillation_reset_angle", 0.2); oscillation_reset_time_ = rclcpp::Duration::from_seconds( nav_2d_utils::searchAndGetParam( - nh_, + node, dwb_plugin_name_ + "." + name_ + ".oscillation_reset_time", -1.0)); nav2_util::declare_parameter_if_not_declared( - nh_, + node, dwb_plugin_name_ + "." + name_ + ".x_only_threshold", rclcpp::ParameterValue(0.05)); /** @@ -114,23 +121,23 @@ void OscillationCritic::onInit() * If min_trans_vel is set in the namespace, as it used to be used for trajectory generation, complain then use that. * Otherwise, set x_only_threshold_ to 0.05 */ - nh_->get_parameter(dwb_plugin_name_ + "." + name_ + ".x_only_threshold", x_only_threshold_); + node->get_parameter(dwb_plugin_name_ + "." + name_ + ".x_only_threshold", x_only_threshold_); // TODO(crdelsey): How to handle searchParam? // std::string resolved_name; - // if (nh_->hasParam("x_only_threshold")) + // if (node->hasParam("x_only_threshold")) // { - // nh_->param("x_only_threshold", x_only_threshold_); + // node->param("x_only_threshold", x_only_threshold_); // } - // else if (nh_->searchParam("min_speed_xy", resolved_name)) + // else if (node->searchParam("min_speed_xy", resolved_name)) // { - // nh_->param(resolved_name, x_only_threshold_); + // node->param(resolved_name, x_only_threshold_); // } - // else if (nh_->searchParam("min_trans_vel", resolved_name)) + // else if (node->searchParam("min_trans_vel", resolved_name)) // { // ROS_WARN_NAMED("OscillationCritic", // "Parameter min_trans_vel is deprecated. " // "Please use the name min_speed_xy or x_only_threshold instead."); - // nh_->param(resolved_name, x_only_threshold_); + // node->param(resolved_name, x_only_threshold_); // } // else // { @@ -154,7 +161,7 @@ void OscillationCritic::debrief(const nav_2d_msgs::msg::Twist2D & cmd_vel) { if (setOscillationFlags(cmd_vel)) { prev_stationary_pose_ = pose_; - prev_reset_time_ = nh_->now(); + prev_reset_time_ = clock_->now(); } // if we've got restrictions... check if we can reset any oscillation flags @@ -183,7 +190,7 @@ bool OscillationCritic::resetAvailable() } } if (oscillation_reset_time_ >= rclcpp::Duration::from_seconds(0.0)) { - auto t_diff = (nh_->now() - prev_reset_time_); + auto t_diff = (clock_->now() - prev_reset_time_); if (t_diff > oscillation_reset_time_) { return true; } diff --git a/nav2_dwb_controller/dwb_critics/src/path_align.cpp b/nav2_dwb_controller/dwb_critics/src/path_align.cpp index 952d4561c04..23c9dfb2350 100644 --- a/nav2_dwb_controller/dwb_critics/src/path_align.cpp +++ b/nav2_dwb_controller/dwb_critics/src/path_align.cpp @@ -46,8 +46,14 @@ void PathAlignCritic::onInit() { PathDistCritic::onInit(); stop_on_failure_ = false; + + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + forward_point_distance_ = nav_2d_utils::searchAndGetParam( - nh_, + node, dwb_plugin_name_ + "." + name_ + ".forward_point_distance", 0.325); } diff --git a/nav2_dwb_controller/dwb_critics/src/prefer_forward.cpp b/nav2_dwb_controller/dwb_critics/src/prefer_forward.cpp index 584e339c8ec..53fdee4fd64 100644 --- a/nav2_dwb_controller/dwb_critics/src/prefer_forward.cpp +++ b/nav2_dwb_controller/dwb_critics/src/prefer_forward.cpp @@ -46,23 +46,28 @@ namespace dwb_critics void PreferForwardCritic::onInit() { + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + declare_parameter_if_not_declared( - nh_, + node, dwb_plugin_name_ + "." + name_ + ".penalty", rclcpp::ParameterValue(1.0)); declare_parameter_if_not_declared( - nh_, + node, dwb_plugin_name_ + "." + name_ + ".strafe_x", rclcpp::ParameterValue(0.1)); declare_parameter_if_not_declared( - nh_, dwb_plugin_name_ + "." + name_ + ".strafe_theta", + node, dwb_plugin_name_ + "." + name_ + ".strafe_theta", rclcpp::ParameterValue(0.2)); declare_parameter_if_not_declared( - nh_, dwb_plugin_name_ + "." + name_ + ".theta_scale", + node, dwb_plugin_name_ + "." + name_ + ".theta_scale", rclcpp::ParameterValue(10.0)); - nh_->get_parameter(dwb_plugin_name_ + "." + name_ + ".penalty", penalty_); - nh_->get_parameter(dwb_plugin_name_ + "." + name_ + ".strafe_x", strafe_x_); - nh_->get_parameter(dwb_plugin_name_ + "." + name_ + ".strafe_theta", strafe_theta_); - nh_->get_parameter(dwb_plugin_name_ + "." + name_ + ".theta_scale", theta_scale_); + node->get_parameter(dwb_plugin_name_ + "." + name_ + ".penalty", penalty_); + node->get_parameter(dwb_plugin_name_ + "." + name_ + ".strafe_x", strafe_x_); + node->get_parameter(dwb_plugin_name_ + "." + name_ + ".strafe_theta", strafe_theta_); + node->get_parameter(dwb_plugin_name_ + "." + name_ + ".theta_scale", theta_scale_); } double PreferForwardCritic::scoreTrajectory(const dwb_msgs::msg::Trajectory2D & traj) diff --git a/nav2_dwb_controller/dwb_critics/src/rotate_to_goal.cpp b/nav2_dwb_controller/dwb_critics/src/rotate_to_goal.cpp index 71683ebc8d8..7a46a0340e9 100644 --- a/nav2_dwb_controller/dwb_critics/src/rotate_to_goal.cpp +++ b/nav2_dwb_controller/dwb_critics/src/rotate_to_goal.cpp @@ -53,19 +53,24 @@ inline double hypot_sq(double dx, double dy) void RotateToGoalCritic::onInit() { + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + xy_goal_tolerance_ = nav_2d_utils::searchAndGetParam( - nh_, + node, dwb_plugin_name_ + ".xy_goal_tolerance", 0.25); xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_; double stopped_xy_velocity = nav_2d_utils::searchAndGetParam( - nh_, + node, dwb_plugin_name_ + ".trans_stopped_velocity", 0.25); stopped_xy_velocity_sq_ = stopped_xy_velocity * stopped_xy_velocity; slowing_factor_ = nav_2d_utils::searchAndGetParam( - nh_, + node, dwb_plugin_name_ + "." + name_ + ".slowing_factor", 5.0); lookahead_time_ = nav_2d_utils::searchAndGetParam( - nh_, + node, dwb_plugin_name_ + "." + name_ + ".lookahead_time", -1.0); reset(); } diff --git a/nav2_dwb_controller/dwb_critics/src/twirling.cpp b/nav2_dwb_controller/dwb_critics/src/twirling.cpp index 3615e6f036e..7973cc09792 100644 --- a/nav2_dwb_controller/dwb_critics/src/twirling.cpp +++ b/nav2_dwb_controller/dwb_critics/src/twirling.cpp @@ -39,8 +39,12 @@ namespace dwb_critics { void TwirlingCritic::onInit() { + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } // Scale is set to 0 by default, so if it was not set otherwise, set to 0 - nh_->get_parameter(dwb_plugin_name_ + "." + name_ + ".scale", scale_); + node->get_parameter(dwb_plugin_name_ + "." + name_ + ".scale", scale_); } double TwirlingCritic::scoreTrajectory(const dwb_msgs::msg::Trajectory2D & traj) diff --git a/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt b/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt index adb0123d7f9..a0b268f3ef7 100644 --- a/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt +++ b/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt @@ -47,10 +47,6 @@ add_library(tf_help SHARED src/tf_help.cpp ) -target_link_libraries(tf_help - conversions -) - ament_target_dependencies(tf_help ${dependencies} ) diff --git a/nav2_lifecycle_manager/CMakeLists.txt b/nav2_lifecycle_manager/CMakeLists.txt index ff06d31cc53..6d812c35834 100644 --- a/nav2_lifecycle_manager/CMakeLists.txt +++ b/nav2_lifecycle_manager/CMakeLists.txt @@ -13,6 +13,7 @@ find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(std_srvs REQUIRED) find_package(tf2_geometry_msgs REQUIRED) +find_package(bondcpp REQUIRED) nav2_package() @@ -38,6 +39,7 @@ set(dependencies std_msgs std_srvs tf2_geometry_msgs + bondcpp ) ament_target_dependencies(${library_name} diff --git a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp index 384f33e0182..dc58026f2dc 100644 --- a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp +++ b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp @@ -22,10 +22,12 @@ #include #include "nav2_util/lifecycle_service_client.hpp" +#include "nav2_util/node_thread.hpp" #include "rclcpp/rclcpp.hpp" #include "std_srvs/srv/empty.hpp" #include "nav2_msgs/srv/manage_lifecycle_nodes.hpp" #include "std_srvs/srv/trigger.hpp" +#include "bondcpp/bond.hpp" namespace nav2_lifecycle_manager { @@ -52,6 +54,8 @@ class LifecycleManager : public rclcpp::Node protected: // The ROS node to use when calling lifecycle services rclcpp::Node::SharedPtr service_client_node_; + rclcpp::Node::SharedPtr bond_client_node_; + std::unique_ptr bond_node_thread_; // The services provided by this node rclcpp::Service::SharedPtr manager_srv_; @@ -121,10 +125,37 @@ class LifecycleManager : public rclcpp::Node */ void destroyLifecycleServiceClients(); + // Support function for creating bond timer + /** + * @brief Support function for creating bond timer + */ + void createBondTimer(); + + // Support function for creating bond connection + /** + * @brief Support function for creating bond connections + */ + bool createBondConnection(const std::string & node_name); + + // Support function for killing bond connections + /** + * @brief Support function for killing bond connections + */ + void destroyBondTimer(); + + // Support function for checking on bond connections + /** + * @ brief Support function for checking on bond connections + * will take down system if there's something non-responsive + */ + void checkBondConnections(); + /** * @brief For a node, transition to the new target state */ - bool changeStateForNode(const std::string & node_name, std::uint8_t transition); + bool changeStateForNode( + const std::string & node_name, + std::uint8_t transition); /** * @brief For each node in the map, transition to the new target state @@ -137,6 +168,13 @@ class LifecycleManager : public rclcpp::Node */ void message(const std::string & msg); + // Timer thread to look at bond connections + rclcpp::TimerBase::SharedPtr bond_timer_; + std::chrono::milliseconds bond_timeout_; + + // A map of all nodes to check bond connection + std::map> bond_map_; + // A map of all nodes to be controlled std::map> node_map_; diff --git a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp index 7e25916b8bf..bb597fbd424 100644 --- a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp +++ b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp @@ -52,27 +52,27 @@ class LifecycleManagerClient * @brief Make start up service call * @return true or false */ - bool startup(); + bool startup(const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); /** * @brief Make shutdown service call * @return true or false */ - bool shutdown(); + bool shutdown(const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); /** * @brief Make pause service call * @return true or false */ - bool pause(); + bool pause(const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); /** * @brief Make resume service call * @return true or false */ - bool resume(); + bool resume(const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); /** * @brief Make reset service call * @return true or false */ - bool reset(); + bool reset(const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); /** * @brief Check if lifecycle node manager server is active * @return ACTIVE or INACTIVE or TIMEOUT @@ -103,7 +103,9 @@ class LifecycleManagerClient * @brief A generic method used to call startup, shutdown, etc. * @param command */ - bool callService(uint8_t command); + bool callService( + uint8_t command, + const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); // The node to use for the service call rclcpp::Node::SharedPtr node_; diff --git a/nav2_lifecycle_manager/package.xml b/nav2_lifecycle_manager/package.xml index 97cb21af53a..4e8295a4cb4 100644 --- a/nav2_lifecycle_manager/package.xml +++ b/nav2_lifecycle_manager/package.xml @@ -18,6 +18,7 @@ std_msgs std_srvs tf2_geometry_msgs + bondcpp nav2_common geometry_msgs @@ -28,6 +29,7 @@ rclcpp_lifecycle std_msgs std_srvs + bondcpp tf2_geometry_msgs ament_lint_auto diff --git a/nav2_lifecycle_manager/src/lifecycle_manager.cpp b/nav2_lifecycle_manager/src/lifecycle_manager.cpp index ccd9faaeab4..92a6279cb02 100644 --- a/nav2_lifecycle_manager/src/lifecycle_manager.cpp +++ b/nav2_lifecycle_manager/src/lifecycle_manager.cpp @@ -40,9 +40,13 @@ LifecycleManager::LifecycleManager() // of nodes declare_parameter("node_names"); declare_parameter("autostart", rclcpp::ParameterValue(false)); + declare_parameter("bond_timeout_ms", 4000); node_names_ = get_parameter("node_names").as_string_array(); get_parameter("autostart", autostart_); + int bond_timeout_int; + get_parameter("bond_timeout_ms", bond_timeout_int); + bond_timeout_ = std::chrono::milliseconds(bond_timeout_int); manager_srv_ = create_service( get_name() + std::string("/manage_nodes"), @@ -52,9 +56,13 @@ LifecycleManager::LifecycleManager() get_name() + std::string("/is_active"), std::bind(&LifecycleManager::isActiveCallback, this, _1, _2, _3)); - auto options = rclcpp::NodeOptions().arguments( + auto service_options = rclcpp::NodeOptions().arguments( {"--ros-args", "-r", std::string("__node:=") + get_name() + "_service_client", "--"}); - service_client_node_ = std::make_shared("_", options); + auto bond_options = rclcpp::NodeOptions().arguments( + {"--ros-args", "-r", std::string("__node:=") + get_name() + "_bond_client", "--"}); + service_client_node_ = std::make_shared("_", service_options); + bond_client_node_ = std::make_shared("_", bond_options); + bond_node_thread_ = std::make_unique(bond_client_node_); transition_state_map_[Transition::TRANSITION_CONFIGURE] = State::PRIMARY_STATE_INACTIVE; transition_state_map_[Transition::TRANSITION_CLEANUP] = State::PRIMARY_STATE_UNCONFIGURED; @@ -79,7 +87,7 @@ LifecycleManager::LifecycleManager() LifecycleManager::~LifecycleManager() { - RCLCPP_INFO(get_logger(), "Destroying"); + RCLCPP_INFO(get_logger(), "Destroying %s", get_name()); } void @@ -135,10 +143,38 @@ LifecycleManager::destroyLifecycleServiceClients() } } +bool +LifecycleManager::createBondConnection(const std::string & node_name) +{ + const double timeout_ns = + std::chrono::duration_cast(bond_timeout_).count(); + const double timeout_s = timeout_ns / 1e9; + + if (bond_map_.find(node_name) == bond_map_.end() && bond_timeout_.count() > 0.0) { + bond_map_[node_name] = + std::make_shared("bond", node_name, bond_client_node_); + bond_map_[node_name]->setHeartbeatTimeout(timeout_s); + bond_map_[node_name]->setHeartbeatPeriod(0.10); + bond_map_[node_name]->start(); + if (!bond_map_[node_name]->waitUntilFormed(rclcpp::Duration(timeout_ns / 2))) { + RCLCPP_ERROR( + get_logger(), + "Server %s was unable to be reached after %0.2fs by bond. " + "This server may be misconfigured.", + node_name.c_str(), timeout_s); + return false; + } + RCLCPP_INFO(get_logger(), "Server %s connected with bond.", node_name.c_str()); + } + + return true; +} + bool LifecycleManager::changeStateForNode(const std::string & node_name, std::uint8_t transition) { message(transition_label_map_[transition] + node_name); + if (!node_map_[node_name]->change_state(transition) || !(node_map_[node_name]->get_state() == transition_state_map_[transition])) { @@ -146,6 +182,12 @@ LifecycleManager::changeStateForNode(const std::string & node_name, std::uint8_t return false; } + if (transition == Transition::TRANSITION_ACTIVATE) { + return createBondConnection(node_name); + } else if (transition == Transition::TRANSITION_DEACTIVATE) { + bond_map_.erase(node_name); + } + return true; } @@ -192,23 +234,29 @@ LifecycleManager::startup() } message("Managed nodes are active"); system_active_ = true; + createBondTimer(); return true; } bool LifecycleManager::shutdown() { + system_active_ = false; + destroyBondTimer(); + message("Shutting down managed nodes..."); shutdownAllNodes(); destroyLifecycleServiceClients(); message("Managed nodes have been shut down"); - system_active_ = false; return true; } bool LifecycleManager::reset() { + system_active_ = false; + destroyBondTimer(); + message("Resetting managed nodes..."); // Should transition in reverse order if (!changeStateForAllNodes(Transition::TRANSITION_DEACTIVATE) || @@ -217,21 +265,25 @@ LifecycleManager::reset() RCLCPP_ERROR(get_logger(), "Failed to reset nodes: aborting reset"); return false; } + message("Managed nodes have been reset"); - system_active_ = false; + return true; } bool LifecycleManager::pause() { + system_active_ = false; + destroyBondTimer(); + message("Pausing managed nodes..."); if (!changeStateForAllNodes(Transition::TRANSITION_DEACTIVATE)) { RCLCPP_ERROR(get_logger(), "Failed to pause nodes: aborting pause"); return false; } message("Managed nodes have been paused"); - system_active_ = false; + return true; } @@ -245,12 +297,62 @@ LifecycleManager::resume() } message("Managed nodes are active"); system_active_ = true; + createBondTimer(); return true; } -// TODO(mjeronimo): This is used to emphasize the major events during system bring-up and -// shutdown so that the messgaes can be easily seen among the log output. We should replace -// this with a ROS2-supported way of highlighting console output, if possible. +void +LifecycleManager::createBondTimer() +{ + if (bond_timeout_.count() <= 0) { + return; + } + + message("Creating bond timer..."); + + bond_timer_ = this->create_wall_timer( + 200ms, + std::bind(&LifecycleManager::checkBondConnections, this)); +} + +void +LifecycleManager::destroyBondTimer() +{ + if (bond_timer_) { + message("Terminating bond timer..."); + bond_timer_->cancel(); + bond_timer_.reset(); + } +} + +void +LifecycleManager::checkBondConnections() +{ + if (!system_active_ || !rclcpp::ok() || bond_map_.empty()) { + return; + } + + for (auto & node_name : node_names_) { + if (!rclcpp::ok()) { + return; + } + + if (bond_map_[node_name]->isBroken()) { + message( + std::string( + "Have not received a heartbeat from " + node_name + ".")); + + // if one is down, bring them all down + RCLCPP_ERROR( + get_logger(), + "CRITICAL FAILURE: SERVER %s IS DOWN after not receiving a heartbeat for %i ms." + " Shutting down related nodes.", + node_name.c_str(), static_cast(bond_timeout_.count())); + reset(); + return; + } + } +} #define ANSI_COLOR_RESET "\x1b[0m" #define ANSI_COLOR_BLUE "\x1b[34m" diff --git a/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp b/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp index 618d318935c..c7931539f7b 100644 --- a/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp +++ b/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp @@ -40,33 +40,33 @@ LifecycleManagerClient::LifecycleManagerClient(const std::string & name) } bool -LifecycleManagerClient::startup() +LifecycleManagerClient::startup(const std::chrono::nanoseconds timeout) { - return callService(ManageLifecycleNodes::Request::STARTUP); + return callService(ManageLifecycleNodes::Request::STARTUP, timeout); } bool -LifecycleManagerClient::shutdown() +LifecycleManagerClient::shutdown(const std::chrono::nanoseconds timeout) { - return callService(ManageLifecycleNodes::Request::SHUTDOWN); + return callService(ManageLifecycleNodes::Request::SHUTDOWN, timeout); } bool -LifecycleManagerClient::pause() +LifecycleManagerClient::pause(const std::chrono::nanoseconds timeout) { - return callService(ManageLifecycleNodes::Request::PAUSE); + return callService(ManageLifecycleNodes::Request::PAUSE, timeout); } bool -LifecycleManagerClient::resume() +LifecycleManagerClient::resume(const std::chrono::nanoseconds timeout) { - return callService(ManageLifecycleNodes::Request::RESUME); + return callService(ManageLifecycleNodes::Request::RESUME, timeout); } bool -LifecycleManagerClient::reset() +LifecycleManagerClient::reset(const std::chrono::nanoseconds timeout) { - return callService(ManageLifecycleNodes::Request::RESET); + return callService(ManageLifecycleNodes::Request::RESET, timeout); } SystemStatus @@ -101,7 +101,7 @@ LifecycleManagerClient::is_active(const std::chrono::nanoseconds timeout) } bool -LifecycleManagerClient::callService(uint8_t command) +LifecycleManagerClient::callService(uint8_t command, const std::chrono::nanoseconds timeout) { auto request = std::make_shared(); request->command = command; @@ -122,7 +122,13 @@ LifecycleManagerClient::callService(uint8_t command) node_->get_logger(), "Sending %s request", manage_service_name_.c_str()); auto future_result = manager_client_->async_send_request(request); - rclcpp::spin_until_future_complete(node_, future_result); + + if (rclcpp::spin_until_future_complete(node_, future_result, timeout) != + rclcpp::FutureReturnCode::SUCCESS) + { + return false; + } + return future_result.get()->success; } diff --git a/nav2_lifecycle_manager/test/CMakeLists.txt b/nav2_lifecycle_manager/test/CMakeLists.txt index e931a5ce191..96cbaa5f7ad 100644 --- a/nav2_lifecycle_manager/test/CMakeLists.txt +++ b/nav2_lifecycle_manager/test/CMakeLists.txt @@ -14,7 +14,28 @@ ament_add_test(test_lifecycle GENERATE_RESULT_FOR_RETURN_CODE_ZERO COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/launch_lifecycle_test.py" WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" - TIMEOUT 180 + TIMEOUT 20 ENV TEST_EXECUTABLE=$ ) + +ament_add_gtest_executable(test_bond_gtest + test_bond.cpp +) + +target_link_libraries(test_bond_gtest + ${library_name} +) + +ament_target_dependencies(test_bond_gtest + ${dependencies} +) + +ament_add_test(test_bond + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/launch_bond_test.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + TIMEOUT 20 + ENV + TEST_EXECUTABLE=$ +) diff --git a/nav2_lifecycle_manager/test/launch_bond_test.py b/nav2_lifecycle_manager/test/launch_bond_test.py new file mode 100755 index 00000000000..061375db452 --- /dev/null +++ b/nav2_lifecycle_manager/test/launch_bond_test.py @@ -0,0 +1,57 @@ +#! /usr/bin/env python3 +# Copyright (c) 2020 Samsung Research America +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +import sys + +from launch import LaunchDescription +from launch import LaunchService +from launch.actions import ExecuteProcess +from launch_ros.actions import Node +from launch_testing.legacy import LaunchTestService + + +def generate_launch_description(): + return LaunchDescription([ + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_test', + output='screen', + parameters=[{'use_sim_time': False}, + {'autostart': False}, + {'node_names': ['bond_tester']}]), + ]) + + +def main(argv=sys.argv[1:]): + ld = generate_launch_description() + + testExecutable = os.getenv('TEST_EXECUTABLE') + + test1_action = ExecuteProcess( + cmd=[testExecutable], + name='test_bond_gtest', + output='screen') + + lts = LaunchTestService() + lts.add_test_action(ld, test1_action) + ls = LaunchService(argv=argv) + ls.include_launch_description(ld) + return lts.run(ls) + + +if __name__ == '__main__': + sys.exit(main()) diff --git a/nav2_lifecycle_manager/test/launch_lifecycle_test.py b/nav2_lifecycle_manager/test/launch_lifecycle_test.py index 822a4423579..88b267ea1d5 100755 --- a/nav2_lifecycle_manager/test/launch_lifecycle_test.py +++ b/nav2_lifecycle_manager/test/launch_lifecycle_test.py @@ -32,6 +32,7 @@ def generate_launch_description(): output='screen', parameters=[{'use_sim_time': False}, {'autostart': False}, + {'bond_timeout_ms': 0}, {'node_names': ['lifecycle_node_test']}]), ]) diff --git a/nav2_lifecycle_manager/test/test_bond.cpp b/nav2_lifecycle_manager/test/test_bond.cpp new file mode 100644 index 00000000000..96d0b324f6c --- /dev/null +++ b/nav2_lifecycle_manager/test/test_bond.cpp @@ -0,0 +1,197 @@ +// Copyright (c) 2020 Samsung Research America +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include "rclcpp/rclcpp.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_util/node_thread.hpp" +#include "nav2_lifecycle_manager/lifecycle_manager.hpp" +#include "nav2_lifecycle_manager/lifecycle_manager_client.hpp" + +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +// minimal lifecycle node implementing bond as in rest of navigation servers +class TestLifecycleNode : public nav2_util::LifecycleNode +{ +public: + TestLifecycleNode(bool bond, std::string name) + : nav2_util::LifecycleNode(name) + { + state = ""; + enable_bond = bond; + } + + CallbackReturn on_configure(const rclcpp_lifecycle::State & /*state*/) override + { + RCLCPP_INFO(get_logger(), "Lifecycle Test node is Configured!"); + state = "configured"; + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_activate(const rclcpp_lifecycle::State & /*state*/) override + { + RCLCPP_INFO(get_logger(), "Lifecycle Test node is Activated!"); + state = "activated"; + if (enable_bond) { + createBond(); + } + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & /*state*/) override + { + RCLCPP_INFO(get_logger(), "Lifecycle Test node is Deactivated!"); + state = "deactivated"; + if (enable_bond) { + destroyBond(); + } + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_cleanup(const rclcpp_lifecycle::State & /*state*/) override + { + RCLCPP_INFO(get_logger(), "Lifecycle Test node is Cleanup!"); + state = "cleaned up"; + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_shutdown(const rclcpp_lifecycle::State & /*state*/) override + { + RCLCPP_INFO(get_logger(), "Lifecycle Test node is Shutdown!"); + state = "shut down"; + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_error(const rclcpp_lifecycle::State & /*state*/) override + { + RCLCPP_INFO(get_logger(), "Lifecycle Test node is encountered an error!"); + state = "errored"; + return CallbackReturn::SUCCESS; + } + + bool bondAllocated() + { + return bond_ ? true : false; + } + + void breakBond() + { + bond_->breakBond(); + } + + std::string getState() + { + return state; + } + + bool isBondEnabled() + { + return enable_bond; + } + + bool isBondConnected() + { + return bondAllocated() ? !bond_->isBroken() : false; + } + + std::string state; + bool enable_bond; +}; + +class TestFixture +{ +public: + TestFixture(bool bond, std::string node_name) + { + lf_node_ = std::make_shared(bond, node_name); + lf_thread_ = std::make_unique(lf_node_->get_node_base_interface()); + } + + std::shared_ptr lf_node_; + std::unique_ptr lf_thread_; +}; + +TEST(LifecycleBondTest, POSITIVE) +{ + // let the lifecycle server come up + rclcpp::Rate(1).sleep(); + + nav2_lifecycle_manager::LifecycleManagerClient client("lifecycle_manager_test"); + + // create node, should be up now + auto fixture = TestFixture(true, "bond_tester"); + auto bond_tester = fixture.lf_node_; + + EXPECT_TRUE(client.startup()); + + // check if bond is connected after being activated + rclcpp::Rate(5).sleep(); + EXPECT_TRUE(bond_tester->isBondConnected()); + EXPECT_EQ(bond_tester->getState(), "activated"); + + bond_tester->breakBond(); + + // bond should be disconnected now and lifecycle manager should know and react to reset + rclcpp::Rate(5).sleep(); + EXPECT_EQ( + nav2_lifecycle_manager::SystemStatus::INACTIVE, + client.is_active(std::chrono::nanoseconds(1000000000))); + EXPECT_FALSE(bond_tester->isBondConnected()); + EXPECT_EQ(bond_tester->getState(), "cleaned up"); + + // check that bringing up again is OK + EXPECT_TRUE(client.startup()); + EXPECT_EQ(bond_tester->getState(), "activated"); + EXPECT_TRUE(bond_tester->isBondConnected()); + EXPECT_EQ( + nav2_lifecycle_manager::SystemStatus::ACTIVE, + client.is_active(std::chrono::nanoseconds(1000000000))); + + // clean state for next test. + EXPECT_TRUE(client.reset()); + EXPECT_FALSE(bond_tester->isBondConnected()); + EXPECT_EQ(bond_tester->getState(), "cleaned up"); +} + +TEST(LifecycleBondTest, NEGATIVE) +{ + nav2_lifecycle_manager::LifecycleManagerClient client("lifecycle_manager_test"); + + // create node, now without bond setup to connect to. Should fail because no bond + auto fixture = TestFixture(false, "bond_tester"); + auto bond_tester = fixture.lf_node_; + EXPECT_FALSE(client.startup()); + EXPECT_FALSE(bond_tester->isBondEnabled()); + EXPECT_EQ( + nav2_lifecycle_manager::SystemStatus::INACTIVE, + client.is_active(std::chrono::nanoseconds(1000000000))); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + bool all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + + return all_successful; +} diff --git a/nav2_map_server/CMakeLists.txt b/nav2_map_server/CMakeLists.txt index 92437424863..644060d5b0f 100644 --- a/nav2_map_server/CMakeLists.txt +++ b/nav2_map_server/CMakeLists.txt @@ -25,6 +25,8 @@ set(map_saver_cli_executable map_saver_cli) set(map_saver_server_executable map_saver_server) +set(costmap_filter_info_server_executable costmap_filter_info_server) + add_executable(${map_server_executable} src/map_server/main.cpp) @@ -34,6 +36,9 @@ add_executable(${map_saver_cli_executable} add_executable(${map_saver_server_executable} src/map_saver/main_server.cpp) +add_executable(${costmap_filter_info_server_executable} + src/costmap_filter_info/main.cpp) + set(map_io_library_name map_io) set(library_name ${map_server_executable}_core) @@ -44,7 +49,8 @@ add_library(${map_io_library_name} SHARED add_library(${library_name} SHARED src/map_server/map_server.cpp - src/map_saver/map_saver.cpp) + src/map_saver/map_saver.cpp + src/costmap_filter_info/costmap_filter_info_server.cpp) set(map_io_dependencies yaml_cpp_vendor @@ -63,6 +69,7 @@ set(map_server_dependencies set(map_saver_dependencies rclcpp + rclcpp_lifecycle nav_msgs nav2_msgs nav2_util) @@ -76,6 +83,9 @@ ament_target_dependencies(${map_saver_cli_executable} ament_target_dependencies(${map_saver_server_executable} ${map_saver_dependencies}) +ament_target_dependencies(${costmap_filter_info_server_executable} + ${map_saver_dependencies}) + ament_target_dependencies(${library_name} ${map_server_dependencies}) @@ -99,6 +109,9 @@ target_link_libraries(${map_saver_cli_executable} target_link_libraries(${map_saver_server_executable} ${library_name}) +target_link_libraries(${costmap_filter_info_server_executable} + ${library_name}) + target_include_directories(${map_io_library_name} SYSTEM PRIVATE ${GRAPHICSMAGICKCPP_INCLUDE_DIRS}) @@ -118,6 +131,7 @@ install(TARGETS install(TARGETS ${map_server_executable} ${map_saver_cli_executable} ${map_saver_server_executable} + ${costmap_filter_info_server_executable} RUNTIME DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY include/ diff --git a/nav2_map_server/include/nav2_map_server/costmap_filter_info_server.hpp b/nav2_map_server/include/nav2_map_server/costmap_filter_info_server.hpp new file mode 100644 index 00000000000..99311deff75 --- /dev/null +++ b/nav2_map_server/include/nav2_map_server/costmap_filter_info_server.hpp @@ -0,0 +1,79 @@ +// Copyright (c) 2020 Samsung Research Russia +// +// 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_MAP_SERVER__COSTMAP_FILTER_INFO_SERVER_HPP_ +#define NAV2_MAP_SERVER__COSTMAP_FILTER_INFO_SERVER_HPP_ + +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_msgs/msg/costmap_filter_info.hpp" + +namespace nav2_map_server +{ + +class CostmapFilterInfoServer : public nav2_util::LifecycleNode +{ +public: + /** + * @brief Constructor for the nav2_map_server::CostmapFilterInfoServer + */ + CostmapFilterInfoServer(); + /** + * @brief Destructor for the nav2_map_server::CostmapFilterInfoServer + */ + ~CostmapFilterInfoServer(); + +protected: + /** + * @brief Creates CostmapFilterInfo publisher and forms published message from ROS parameters + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; + /** + * @brief Publishes a CostmapFilterInfo message + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; + /** + * @brief Deactivates publisher + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; + /** + * @brief Resets publisher + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; + /** + * @brief Called when in Shutdown state + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; + +private: + rclcpp_lifecycle::LifecyclePublisher::SharedPtr publisher_; + + std::unique_ptr msg_; +}; // CostmapFilterInfoServer + +} // namespace nav2_map_server + +#endif // NAV2_MAP_SERVER__COSTMAP_FILTER_INFO_SERVER_HPP_ diff --git a/nav2_map_server/include/nav2_map_server/map_saver.hpp b/nav2_map_server/include/nav2_map_server/map_saver.hpp index 3aa4e2e2332..9c9b57d2928 100644 --- a/nav2_map_server/include/nav2_map_server/map_saver.hpp +++ b/nav2_map_server/include/nav2_map_server/map_saver.hpp @@ -103,6 +103,8 @@ class MapSaver : public nav2_util::LifecycleNode // Default values for map thresholds double free_thresh_default_; double occupied_thresh_default_; + // param for handling QoS configuration + bool map_subscribe_transient_local_; // The name of the service for saving a map from topic const std::string save_map_service_name_{"save_map"}; diff --git a/nav2_map_server/src/costmap_filter_info/costmap_filter_info_server.cpp b/nav2_map_server/src/costmap_filter_info/costmap_filter_info_server.cpp new file mode 100644 index 00000000000..4f1702089b2 --- /dev/null +++ b/nav2_map_server/src/costmap_filter_info/costmap_filter_info_server.cpp @@ -0,0 +1,105 @@ +// Copyright (c) 2020 Samsung Research Russia +// +// 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. + +// TODO(AlexeyMerzlyakov): This dummy info publisher should be removed +// after Semantic Map Server having the same functionality will be developed. + +#include "nav2_map_server/costmap_filter_info_server.hpp" + +#include +#include +#include + +namespace nav2_map_server +{ + +CostmapFilterInfoServer::CostmapFilterInfoServer() +: nav2_util::LifecycleNode("costmap_filter_info_server") +{ + declare_parameter("filter_info_topic", "costmap_filter_info"); + declare_parameter("type", 0); + declare_parameter("mask_topic", "filter_mask"); + declare_parameter("base", 0.0); + declare_parameter("multiplier", 1.0); +} + +CostmapFilterInfoServer::~CostmapFilterInfoServer() +{ +} + +nav2_util::CallbackReturn +CostmapFilterInfoServer::on_configure(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Configuring"); + + std::string filter_info_topic = get_parameter("filter_info_topic").as_string(); + + publisher_ = this->create_publisher( + filter_info_topic, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + + msg_ = std::make_unique(); + msg_->type = get_parameter("type").as_int(); + msg_->filter_mask_topic = get_parameter("mask_topic").as_string(); + msg_->base = get_parameter("base").as_double(); + msg_->multiplier = get_parameter("multiplier").as_double(); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +CostmapFilterInfoServer::on_activate(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Activating"); + + publisher_->on_activate(); + publisher_->publish(std::move(msg_)); + + // create bond connection + createBond(); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +CostmapFilterInfoServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Deactivating"); + + publisher_->on_deactivate(); + + // destroy bond connection + destroyBond(); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +CostmapFilterInfoServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Cleaning up"); + + publisher_.reset(); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +CostmapFilterInfoServer::on_shutdown(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Shutting down"); + + return nav2_util::CallbackReturn::SUCCESS; +} + +} // namespace nav2_map_server diff --git a/nav2_map_server/src/costmap_filter_info/main.cpp b/nav2_map_server/src/costmap_filter_info/main.cpp new file mode 100644 index 00000000000..e7b48f613e4 --- /dev/null +++ b/nav2_map_server/src/costmap_filter_info/main.cpp @@ -0,0 +1,32 @@ +// Copyright (c) 2020 Samsung Research Russia +// +// 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_map_server/costmap_filter_info_server.hpp" +#include "rclcpp/rclcpp.hpp" + +int main(int argc, char * argv[]) +{ + auto logger = rclcpp::get_logger("costmap_filter_info_server"); + + RCLCPP_INFO(logger, "This is costmap filter info publisher"); + + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node->get_node_base_interface()); + rclcpp::shutdown(); + + return 0; +} diff --git a/nav2_map_server/src/map_io.cpp b/nav2_map_server/src/map_io.cpp index 7375c64da8a..d926c5b07e1 100644 --- a/nav2_map_server/src/map_io.cpp +++ b/nav2_map_server/src/map_io.cpp @@ -46,6 +46,7 @@ #include "yaml-cpp/yaml.h" #include "tf2/LinearMath/Matrix3x3.h" #include "tf2/LinearMath/Quaternion.h" +#include "nav2_util/occ_grid_values.hpp" #ifdef _WIN32 // https://github.com/rtv/Stage/blob/master/replace/dirname.c @@ -78,7 +79,7 @@ char * dirname(char * path) /* This assignment is ill-designed but the XPG specs require to return a string containing "." in any case no directory part is found and so a static and constant string is required. */ - path = const_cast(&dot[0]); + path = reinterpret_cast(dot); } return path; @@ -213,20 +214,20 @@ void loadMapFromFile( switch (load_parameters.mode) { case MapMode::Trinary: if (load_parameters.occupied_thresh < occ) { - map_cell = 100; + map_cell = nav2_util::OCC_GRID_OCCUPIED; } else if (occ < load_parameters.free_thresh) { - map_cell = 0; + map_cell = nav2_util::OCC_GRID_FREE; } else { - map_cell = -1; + map_cell = nav2_util::OCC_GRID_UNKNOWN; } break; case MapMode::Scale: if (pixel.alphaQuantum() != OpaqueOpacity) { - map_cell = -1; + map_cell = nav2_util::OCC_GRID_UNKNOWN; } else if (load_parameters.occupied_thresh < occ) { - map_cell = 100; + map_cell = nav2_util::OCC_GRID_OCCUPIED; } else if (occ < load_parameters.free_thresh) { - map_cell = 0; + map_cell = nav2_util::OCC_GRID_FREE; } else { map_cell = std::rint( (occ - load_parameters.free_thresh) / @@ -235,10 +236,12 @@ void loadMapFromFile( break; case MapMode::Raw: { double occ_percent = std::round(shade * 255); - if (0 <= occ_percent && occ_percent <= 100) { + if (nav2_util::OCC_GRID_FREE <= occ_percent && + occ_percent <= nav2_util::OCC_GRID_OCCUPIED) + { map_cell = static_cast(occ_percent); } else { - map_cell = -1; + map_cell = nav2_util::OCC_GRID_UNKNOWN; } break; } diff --git a/nav2_map_server/src/map_saver/map_saver.cpp b/nav2_map_server/src/map_saver/map_saver.cpp index 7555ebd6326..0cd5280963a 100644 --- a/nav2_map_server/src/map_saver/map_saver.cpp +++ b/nav2_map_server/src/map_saver/map_saver.cpp @@ -50,11 +50,11 @@ MapSaver::MapSaver() free_thresh_default_ = declare_parameter("free_thresh_default", 0.25), occupied_thresh_default_ = declare_parameter("occupied_thresh_default", 0.65); + map_subscribe_transient_local_ = declare_parameter("map_subscribe_transient_local", true); } MapSaver::~MapSaver() { - RCLCPP_INFO(get_logger(), "Destroying"); } nav2_util::CallbackReturn @@ -77,6 +77,10 @@ nav2_util::CallbackReturn MapSaver::on_activate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Activating"); + + // create bond connection + createBond(); + return nav2_util::CallbackReturn::SUCCESS; } @@ -84,6 +88,10 @@ nav2_util::CallbackReturn MapSaver::on_deactivate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Deactivating"); + + // destroy bond connection + destroyBond(); + return nav2_util::CallbackReturn::SUCCESS; } @@ -173,8 +181,14 @@ bool MapSaver::saveMapTopicToFile( // Add new subscription for incoming map topic. // Utilizing local rclcpp::Node (rclcpp_node_) from nav2_util::LifecycleNode // as a map listener. + rclcpp::QoS map_qos(10); // initialize to default + if (map_subscribe_transient_local_) { + map_qos.transient_local(); + map_qos.reliable(); + map_qos.keep_last(1); + } auto map_sub = rclcpp_node_->create_subscription( - map_topic_loc, rclcpp::SystemDefaultsQoS(), mapCallback); + map_topic_loc, map_qos, mapCallback); rclcpp::Time start_time = now(); while (rclcpp::ok()) { diff --git a/nav2_map_server/src/map_server/map_server.cpp b/nav2_map_server/src/map_server/map_server.cpp index d633af10a39..a0b140463f2 100644 --- a/nav2_map_server/src/map_server/map_server.cpp +++ b/nav2_map_server/src/map_server/map_server.cpp @@ -75,7 +75,6 @@ MapServer::MapServer() MapServer::~MapServer() { - RCLCPP_INFO(get_logger(), "Destroying"); } nav2_util::CallbackReturn @@ -129,6 +128,9 @@ MapServer::on_activate(const rclcpp_lifecycle::State & /*state*/) auto occ_grid = std::make_unique(msg_); occ_pub_->publish(std::move(occ_grid)); + // create bond connection + createBond(); + return nav2_util::CallbackReturn::SUCCESS; } @@ -139,6 +141,9 @@ MapServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) occ_pub_->on_deactivate(); + // destroy bond connection + destroyBond(); + return nav2_util::CallbackReturn::SUCCESS; } diff --git a/nav2_map_server/test/unit/CMakeLists.txt b/nav2_map_server/test/unit/CMakeLists.txt index e0326d462c6..095a6509aef 100644 --- a/nav2_map_server/test/unit/CMakeLists.txt +++ b/nav2_map_server/test/unit/CMakeLists.txt @@ -9,3 +9,14 @@ target_link_libraries(test_map_io ${map_io_library_name} stdc++fs ) + +# costmap_filter_info_server unit test +ament_add_gtest(test_costmap_filter_info_server + test_costmap_filter_info_server.cpp +) + +ament_target_dependencies(test_costmap_filter_info_server rclcpp) + +target_link_libraries(test_costmap_filter_info_server + ${library_name} +) diff --git a/nav2_map_server/test/unit/test_costmap_filter_info_server.cpp b/nav2_map_server/test/unit/test_costmap_filter_info_server.cpp new file mode 100644 index 00000000000..168fbc9f7b9 --- /dev/null +++ b/nav2_map_server/test/unit/test_costmap_filter_info_server.cpp @@ -0,0 +1,146 @@ +// Copyright (c) 2020 Samsung Research Russia +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "nav2_map_server/costmap_filter_info_server.hpp" + +using namespace std::chrono_literals; + +typedef std::recursive_mutex mutex_t; + +static const char FILTER_INFO_TOPIC[] = "filter_info"; +static const int TYPE = 1; +static const char MASK_TOPIC[] = "mask"; +static const double BASE = 0.1; +static const double MULTIPLIER = 0.2; + +static const double EPSILON = std::numeric_limits::epsilon(); + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +class InfoServerWrapper : public nav2_map_server::CostmapFilterInfoServer +{ +public: + void start() + { + on_configure(get_current_state()); + on_activate(get_current_state()); + } + + void stop() + { + on_deactivate(get_current_state()); + on_cleanup(get_current_state()); + on_shutdown(get_current_state()); + } +}; + +class InfoServerTester : public ::testing::Test +{ +public: + InfoServerTester() + : info_server_(nullptr), info_(nullptr), subscription_(nullptr) + { + access_ = new mutex_t(); + + info_server_ = std::make_shared(); + try { + info_server_->set_parameter(rclcpp::Parameter("filter_info_topic", FILTER_INFO_TOPIC)); + info_server_->set_parameter(rclcpp::Parameter("type", TYPE)); + info_server_->set_parameter(rclcpp::Parameter("mask_topic", MASK_TOPIC)); + info_server_->set_parameter(rclcpp::Parameter("base", BASE)); + info_server_->set_parameter(rclcpp::Parameter("multiplier", MULTIPLIER)); + } catch (rclcpp::exceptions::ParameterNotDeclaredException & ex) { + RCLCPP_ERROR( + info_server_->get_logger(), + "Error while setting parameters for CostmapFilterInfoServer: %s", ex.what()); + throw; + } + + info_server_->start(); + + subscription_ = info_server_->create_subscription( + FILTER_INFO_TOPIC, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), + std::bind(&InfoServerTester::infoCallback, this, std::placeholders::_1)); + } + + ~InfoServerTester() + { + info_server_->stop(); + info_server_.reset(); + subscription_.reset(); + } + + bool isReceived() + { + std::lock_guard guard(*getMutex()); + if (info_) { + return true; + } else { + return false; + } + } + + mutex_t * getMutex() + { + return access_; + } + +protected: + std::shared_ptr info_server_; + nav2_msgs::msg::CostmapFilterInfo::SharedPtr info_; + +private: + void infoCallback(const nav2_msgs::msg::CostmapFilterInfo::SharedPtr msg) + { + std::lock_guard guard(*getMutex()); + info_ = msg; + } + + rclcpp::Subscription::SharedPtr subscription_; + + mutex_t * access_; +}; + +TEST_F(InfoServerTester, testCostmapFilterInfoPublish) +{ + rclcpp::Time start_time = info_server_->now(); + while (!isReceived()) { + rclcpp::spin_some(info_server_->get_node_base_interface()); + std::this_thread::sleep_for(100ms); + // Waiting no more than 5 seconds + ASSERT_TRUE((info_server_->now() - start_time) <= rclcpp::Duration(5000ms)); + } + + // Checking received CostmapFilterInfo for consistency + EXPECT_EQ(info_->type, TYPE); + EXPECT_EQ(info_->filter_mask_topic, MASK_TOPIC); + EXPECT_NEAR(info_->base, BASE, EPSILON); + EXPECT_NEAR(info_->multiplier, MULTIPLIER, EPSILON); +} diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index 1ecdf58b0f5..d09cafa74cf 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -15,6 +15,7 @@ nav2_package() rosidl_generate_interfaces(${PROJECT_NAME} "msg/Costmap.msg" "msg/CostmapMetaData.msg" + "msg/CostmapFilterInfo.msg" "msg/VoxelGrid.msg" "msg/BehaviorTreeStatusChange.msg" "msg/BehaviorTreeLog.msg" diff --git a/nav2_msgs/msg/CostmapFilterInfo.msg b/nav2_msgs/msg/CostmapFilterInfo.msg new file mode 100644 index 00000000000..48410794528 --- /dev/null +++ b/nav2_msgs/msg/CostmapFilterInfo.msg @@ -0,0 +1,11 @@ +std_msgs/Header header +# Type of plugin used (keepout filter, speed limit in m/s, speed limit in percent, etc...) +uint8 type +# Name of filter mask topic +string filter_mask_topic +# Multiplier base offset and multiplier coefficient for conversion of OccGrid. +# Used to convert OccupancyGrid data values to filter space values. +# data -> into some other number space: +# space = data * multiplier + base +float32 base +float32 multiplier 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 e9d34279a56..5f2a8613259 100644 --- a/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp +++ b/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp @@ -42,7 +42,7 @@ class NavfnPlanner : public nav2_core::GlobalPlanner // plugin configure void configure( - rclcpp_lifecycle::LifecycleNode::SharedPtr parent, + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, std::string name, std::shared_ptr tf, std::shared_ptr costmap_ros) override; @@ -119,8 +119,11 @@ class NavfnPlanner : public nav2_core::GlobalPlanner // TF buffer std::shared_ptr tf_; - // node ptr - nav2_util::LifecycleNode::SharedPtr node_; + // Clock + rclcpp::Clock::SharedPtr clock_; + + // Logger + rclcpp::Logger logger_{rclcpp::get_logger("NavfnPlanner")}; // Global Costmap nav2_costmap_2d::Costmap2D * costmap_; diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index a873a3e5d1c..2258b5e19f8 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -50,34 +50,37 @@ NavfnPlanner::NavfnPlanner() NavfnPlanner::~NavfnPlanner() { RCLCPP_INFO( - node_->get_logger(), "Destroying plugin %s of type NavfnPlanner", + logger_, "Destroying plugin %s of type NavfnPlanner", name_.c_str()); } void NavfnPlanner::configure( - rclcpp_lifecycle::LifecycleNode::SharedPtr parent, + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, std::string name, std::shared_ptr tf, std::shared_ptr costmap_ros) { - node_ = parent; tf_ = tf; name_ = name; costmap_ = costmap_ros->getCostmap(); global_frame_ = costmap_ros->getGlobalFrameID(); + auto node = parent.lock(); + clock_ = node->get_clock(); + logger_ = node->get_logger(); + RCLCPP_INFO( - node_->get_logger(), "Configuring plugin %s of type NavfnPlanner", + logger_, "Configuring plugin %s of type NavfnPlanner", name_.c_str()); // Initialize parameters // Declare this plugin's parameters - declare_parameter_if_not_declared(node_, name + ".tolerance", rclcpp::ParameterValue(0.5)); - node_->get_parameter(name + ".tolerance", tolerance_); - declare_parameter_if_not_declared(node_, name + ".use_astar", rclcpp::ParameterValue(false)); - node_->get_parameter(name + ".use_astar", use_astar_); - declare_parameter_if_not_declared(node_, name + ".allow_unknown", rclcpp::ParameterValue(true)); - node_->get_parameter(name + ".allow_unknown", allow_unknown_); + declare_parameter_if_not_declared(node, name + ".tolerance", rclcpp::ParameterValue(0.5)); + node->get_parameter(name + ".tolerance", tolerance_); + declare_parameter_if_not_declared(node, name + ".use_astar", rclcpp::ParameterValue(false)); + node->get_parameter(name + ".use_astar", use_astar_); + declare_parameter_if_not_declared(node, name + ".allow_unknown", rclcpp::ParameterValue(true)); + node->get_parameter(name + ".allow_unknown", allow_unknown_); // Create a planner based on the new costmap size planner_ = std::make_unique( @@ -89,7 +92,7 @@ void NavfnPlanner::activate() { RCLCPP_INFO( - node_->get_logger(), "Activating plugin %s of type NavfnPlanner", + logger_, "Activating plugin %s of type NavfnPlanner", name_.c_str()); } @@ -97,7 +100,7 @@ void NavfnPlanner::deactivate() { RCLCPP_INFO( - node_->get_logger(), "Deactivating plugin %s of type NavfnPlanner", + logger_, "Deactivating plugin %s of type NavfnPlanner", name_.c_str()); } @@ -105,7 +108,7 @@ void NavfnPlanner::cleanup() { RCLCPP_INFO( - node_->get_logger(), "Cleaning up plugin %s of type NavfnPlanner", + logger_, "Cleaning up plugin %s of type NavfnPlanner", name_.c_str()); planner_.reset(); } @@ -125,7 +128,7 @@ nav_msgs::msg::Path NavfnPlanner::createPlan( if (!makePlan(start.pose, goal.pose, tolerance_, path)) { RCLCPP_WARN( - node_->get_logger(), "%s: failed to create plan with " + logger_, "%s: failed to create plan with " "tolerance %.2f.", name_.c_str(), tolerance_); } return path; @@ -152,7 +155,7 @@ NavfnPlanner::makePlan( // clear the plan, just in case plan.poses.clear(); - plan.header.stamp = node_->now(); + plan.header.stamp = clock_->now(); plan.header.frame_id = global_frame_; // TODO(orduno): add checks for start and goal reference frame -- should be in global frame @@ -161,13 +164,13 @@ NavfnPlanner::makePlan( double wy = start.position.y; RCLCPP_DEBUG( - node_->get_logger(), "Making plan from (%.2f,%.2f) to (%.2f,%.2f)", + logger_, "Making plan from (%.2f,%.2f) to (%.2f,%.2f)", start.position.x, start.position.y, goal.position.x, goal.position.y); unsigned int mx, my; if (!worldToMap(wx, wy, mx, my)) { RCLCPP_WARN( - node_->get_logger(), + logger_, "Cannot create a plan: the robot's start position is off the global" " costmap. Planning will always fail, are you sure" " the robot has been properly localized?"); @@ -197,7 +200,7 @@ NavfnPlanner::makePlan( if (!worldToMap(wx, wy, mx, my)) { RCLCPP_WARN( - node_->get_logger(), + logger_, "The goal sent to the planner is off the global costmap." " Planning will always fail to this goal."); return false; @@ -257,7 +260,7 @@ NavfnPlanner::makePlan( smoothApproachToGoal(best_pose, plan); } else { RCLCPP_ERROR( - node_->get_logger(), + logger_, "Failed to create a plan from potential when a legal" " potential was found. This shouldn't happen."); } @@ -305,7 +308,7 @@ NavfnPlanner::getPlanFromPotential( unsigned int mx, my; if (!worldToMap(wx, wy, mx, my)) { RCLCPP_WARN( - node_->get_logger(), + logger_, "The goal sent to the navfn planner is off the global costmap." " Planning will always fail to this goal."); return false; @@ -323,7 +326,9 @@ NavfnPlanner::getPlanFromPotential( } auto cost = planner_->getLastPathCost(); - RCLCPP_DEBUG(node_->get_logger(), "Path found, %d steps, %f cost\n", path_len, cost); + RCLCPP_DEBUG( + logger_, + "Path found, %d steps, %f cost\n", path_len, cost); // extract the plan float * x = planner_->getPathX(); @@ -415,7 +420,8 @@ NavfnPlanner::worldToMap(double wx, double wy, unsigned int & mx, unsigned int & } RCLCPP_ERROR( - node_->get_logger(), "worldToMap failed: mx,my: %d,%d, size_x,size_y: %d,%d", mx, my, + logger_, + "worldToMap failed: mx,my: %d,%d, size_x,size_y: %d,%d", mx, my, costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY()); return false; diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index ee3a07e4739..701852d78a3 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -48,6 +48,13 @@ PlannerServer::PlannerServer() declare_parameter("planner_plugins", default_ids_); declare_parameter("expected_planner_frequency", 20.0); + get_parameter("planner_plugins", planner_ids_); + if (planner_ids_ == default_ids_) { + for (size_t i = 0; i < default_ids_.size(); ++i) { + declare_parameter(default_ids_[i] + ".plugin", default_types_[i]); + } + } + // Setup the global costmap costmap_ros_ = std::make_shared( "global_costmap", std::string{get_namespace()}, "global_costmap"); @@ -58,7 +65,6 @@ PlannerServer::PlannerServer() PlannerServer::~PlannerServer() { - RCLCPP_INFO(get_logger(), "Destroying"); planners_.clear(); costmap_thread_.reset(); } @@ -77,12 +83,6 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & state) tf_ = costmap_ros_->getTfBuffer(); - get_parameter("planner_plugins", planner_ids_); - if (planner_ids_ == default_ids_) { - for (size_t i = 0; i < default_ids_.size(); ++i) { - declare_parameter(default_ids_[i] + ".plugin", default_types_[i]); - } - } planner_types_.resize(planner_ids_.size()); auto node = shared_from_this(); @@ -151,6 +151,9 @@ PlannerServer::on_activate(const rclcpp_lifecycle::State & state) it->second->activate(); } + // create bond connection + createBond(); + return nav2_util::CallbackReturn::SUCCESS; } @@ -168,6 +171,9 @@ PlannerServer::on_deactivate(const rclcpp_lifecycle::State & state) it->second->deactivate(); } + // destroy bond connection + destroyBond(); + return nav2_util::CallbackReturn::SUCCESS; } @@ -306,10 +312,7 @@ void PlannerServer::publishPlan(const nav_msgs::msg::Path & path) { auto msg = std::make_unique(path); - if ( - plan_publisher_->is_activated() && - this->count_subscribers(plan_publisher_->get_topic_name()) > 0) - { + if (plan_publisher_->is_activated() && plan_publisher_->get_subscription_count() > 0) { plan_publisher_->publish(std::move(msg)); } } diff --git a/nav2_recoveries/include/nav2_recoveries/recovery.hpp b/nav2_recoveries/include/nav2_recoveries/recovery.hpp index 04aed1b02a1..b52d944884e 100644 --- a/nav2_recoveries/include/nav2_recoveries/recovery.hpp +++ b/nav2_recoveries/include/nav2_recoveries/recovery.hpp @@ -86,28 +86,32 @@ class Recovery : public nav2_core::Recovery } void configure( - const rclcpp_lifecycle::LifecycleNode::SharedPtr parent, + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, const std::string & name, std::shared_ptr tf, std::shared_ptr collision_checker) override { - RCLCPP_INFO(parent->get_logger(), "Configuring %s", name.c_str()); - node_ = parent; + auto node = node_.lock(); + + logger_ = node->get_logger(); + + RCLCPP_INFO(logger_, "Configuring %s", name.c_str()); + recovery_name_ = name; tf_ = tf; - node_->get_parameter("cycle_frequency", cycle_frequency_); - node_->get_parameter("global_frame", global_frame_); - node_->get_parameter("robot_base_frame", robot_base_frame_); - node_->get_parameter("transform_tolerance", transform_tolerance_); + node->get_parameter("cycle_frequency", cycle_frequency_); + node->get_parameter("global_frame", global_frame_); + node->get_parameter("robot_base_frame", robot_base_frame_); + node->get_parameter("transform_tolerance", transform_tolerance_); action_server_ = std::make_shared( - node_, recovery_name_, + node, recovery_name_, std::bind(&Recovery::execute, this)); collision_checker_ = collision_checker; - vel_pub_ = node_->create_publisher("cmd_vel", 1); + vel_pub_ = node->create_publisher("cmd_vel", 1); onConfigure(); } @@ -121,20 +125,23 @@ class Recovery : public nav2_core::Recovery void activate() override { - RCLCPP_INFO(node_->get_logger(), "Activating %s", recovery_name_.c_str()); + RCLCPP_INFO(logger_, "Activating %s", recovery_name_.c_str()); vel_pub_->on_activate(); + action_server_->activate(); enabled_ = true; } void deactivate() override { vel_pub_->on_deactivate(); + action_server_->deactivate(); enabled_ = false; } protected: - rclcpp_lifecycle::LifecycleNode::SharedPtr node_; + rclcpp_lifecycle::LifecycleNode::WeakPtr node_; + std::string recovery_name_; rclcpp_lifecycle::LifecyclePublisher::SharedPtr vel_pub_; std::shared_ptr action_server_; @@ -150,25 +157,40 @@ class Recovery : public nav2_core::Recovery // Clock rclcpp::Clock steady_clock_{RCL_STEADY_TIME}; + // Logger + rclcpp::Logger logger_{rclcpp::get_logger("nav2_recoveries")}; + void execute() { - RCLCPP_INFO(node_->get_logger(), "Attempting %s", recovery_name_.c_str()); + RCLCPP_INFO(logger_, "Attempting %s", recovery_name_.c_str()); if (!enabled_) { - RCLCPP_WARN(node_->get_logger(), "Called while inactive, ignoring request."); + RCLCPP_WARN( + logger_, + "Called while inactive, ignoring request."); return; } if (onRun(action_server_->get_current_goal()) != Status::SUCCEEDED) { - RCLCPP_INFO(node_->get_logger(), "Initial checks failed for %s", recovery_name_.c_str()); + RCLCPP_INFO( + logger_, + "Initial checks failed for %s", recovery_name_.c_str()); action_server_->terminate_current(); return; } // Log a message every second - auto timer = node_->create_wall_timer( - 1s, - [&]() {RCLCPP_INFO(node_->get_logger(), "%s running...", recovery_name_.c_str());}); + { + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + auto timer = node->create_wall_timer( + 1s, + [&]() + {RCLCPP_INFO(logger_, "%s running...", recovery_name_.c_str());}); + } auto start_time = steady_clock_.now(); @@ -179,7 +201,7 @@ class Recovery : public nav2_core::Recovery while (rclcpp::ok()) { if (action_server_->is_cancel_requested()) { - RCLCPP_INFO(node_->get_logger(), "Canceling %s", recovery_name_.c_str()); + RCLCPP_INFO(logger_, "Canceling %s", recovery_name_.c_str()); stopRobot(); result->total_elapsed_time = steady_clock_.now() - start_time; action_server_->terminate_all(result); @@ -189,7 +211,7 @@ class Recovery : public nav2_core::Recovery // TODO(orduno) #868 Enable preempting a Recovery on-the-fly without stopping if (action_server_->is_preempt_requested()) { RCLCPP_ERROR( - node_->get_logger(), "Received a preemption request for %s," + logger_, "Received a preemption request for %s," " however feature is currently not implemented. Aborting and stopping.", recovery_name_.c_str()); stopRobot(); @@ -200,13 +222,15 @@ class Recovery : public nav2_core::Recovery switch (onCycleUpdate()) { case Status::SUCCEEDED: - RCLCPP_INFO(node_->get_logger(), "%s completed successfully", recovery_name_.c_str()); + RCLCPP_INFO( + logger_, + "%s completed successfully", recovery_name_.c_str()); result->total_elapsed_time = steady_clock_.now() - start_time; action_server_->succeeded_current(result); return; case Status::FAILED: - RCLCPP_WARN(node_->get_logger(), "%s failed", recovery_name_.c_str()); + RCLCPP_WARN(logger_, "%s failed", recovery_name_.c_str()); result->total_elapsed_time = steady_clock_.now() - start_time; action_server_->terminate_current(result); return; diff --git a/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp b/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp index bb9efa571c1..5df52c16ca5 100644 --- a/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp +++ b/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp @@ -51,8 +51,8 @@ class RecoveryServer : public nav2_util::LifecycleNode std::shared_ptr transform_listener_; // Plugins - std::vector> recoveries_; pluginlib::ClassLoader plugin_loader_; + std::vector> recoveries_; std::vector default_ids_; std::vector default_types_; std::vector recovery_ids_; diff --git a/nav2_recoveries/plugins/back_up.cpp b/nav2_recoveries/plugins/back_up.cpp index 585d8a77410..91baa7ba823 100644 --- a/nav2_recoveries/plugins/back_up.cpp +++ b/nav2_recoveries/plugins/back_up.cpp @@ -37,28 +37,34 @@ BackUp::~BackUp() void BackUp::onConfigure() { + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + nav2_util::declare_parameter_if_not_declared( - node_, + node, "simulate_ahead_time", rclcpp::ParameterValue(2.0)); - node_->get_parameter("simulate_ahead_time", simulate_ahead_time_); + node->get_parameter("simulate_ahead_time", simulate_ahead_time_); } Status BackUp::onRun(const std::shared_ptr command) { if (command->target.y != 0.0 || command->target.z != 0.0) { RCLCPP_INFO( - node_->get_logger(), "Backing up in Y and Z not supported, " - "will only move in X."); + logger_, + "Backing up in Y and Z not supported, will only move in X."); } - command_x_ = command->target.x; - command_speed_ = command->speed; + // Silently ensure that both the speed and direction are positive. + command_x_ = std::fabs(command->target.x); + command_speed_ = std::fabs(command->speed); if (!nav2_util::getCurrentPose( initial_pose_, *tf_, global_frame_, robot_base_frame_, transform_tolerance_)) { - RCLCPP_ERROR(node_->get_logger(), "Initial robot pose is not available."); + RCLCPP_ERROR(logger_, "Initial robot pose is not available."); return Status::FAILED; } @@ -72,7 +78,7 @@ Status BackUp::onCycleUpdate() current_pose, *tf_, global_frame_, robot_base_frame_, transform_tolerance_)) { - RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available."); + RCLCPP_ERROR(logger_, "Current robot pose is not available."); return Status::FAILED; } @@ -83,7 +89,7 @@ Status BackUp::onCycleUpdate() feedback_->distance_traveled = distance; action_server_->publish_feedback(feedback_); - if (distance >= abs(command_x_)) { + if (distance >= command_x_) { stopRobot(); return Status::SUCCEEDED; } @@ -92,7 +98,7 @@ Status BackUp::onCycleUpdate() auto cmd_vel = std::make_unique(); cmd_vel->linear.y = 0.0; cmd_vel->angular.z = 0.0; - command_x_ < 0 ? cmd_vel->linear.x = -command_speed_ : cmd_vel->linear.x = command_speed_; + cmd_vel->linear.x = -command_speed_; geometry_msgs::msg::Pose2D pose2d; pose2d.x = current_pose.pose.position.x; @@ -101,7 +107,7 @@ Status BackUp::onCycleUpdate() if (!isCollisionFree(distance, cmd_vel.get(), pose2d)) { stopRobot(); - RCLCPP_WARN(node_->get_logger(), "Collision Ahead - Exiting BackUp"); + RCLCPP_WARN(logger_, "Collision Ahead - Exiting BackUp"); return Status::SUCCEEDED; } diff --git a/nav2_recoveries/plugins/spin.cpp b/nav2_recoveries/plugins/spin.cpp index e24be17d899..011d1c00960 100644 --- a/nav2_recoveries/plugins/spin.cpp +++ b/nav2_recoveries/plugins/spin.cpp @@ -47,25 +47,30 @@ Spin::~Spin() void Spin::onConfigure() { + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + nav2_util::declare_parameter_if_not_declared( - node_, + node, "simulate_ahead_time", rclcpp::ParameterValue(2.0)); - node_->get_parameter("simulate_ahead_time", simulate_ahead_time_); + node->get_parameter("simulate_ahead_time", simulate_ahead_time_); nav2_util::declare_parameter_if_not_declared( - node_, + node, "max_rotational_vel", rclcpp::ParameterValue(1.0)); - node_->get_parameter("max_rotational_vel", max_rotational_vel_); + node->get_parameter("max_rotational_vel", max_rotational_vel_); nav2_util::declare_parameter_if_not_declared( - node_, + node, "min_rotational_vel", rclcpp::ParameterValue(0.4)); - node_->get_parameter("min_rotational_vel", min_rotational_vel_); + node->get_parameter("min_rotational_vel", min_rotational_vel_); nav2_util::declare_parameter_if_not_declared( - node_, + node, "rotational_acc_lim", rclcpp::ParameterValue(3.2)); - node_->get_parameter("rotational_acc_lim", rotational_acc_lim_); + node->get_parameter("rotational_acc_lim", rotational_acc_lim_); } Status Spin::onRun(const std::shared_ptr command) @@ -75,7 +80,7 @@ Status Spin::onRun(const std::shared_ptr command) current_pose, *tf_, global_frame_, robot_base_frame_, transform_tolerance_)) { - RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available."); + RCLCPP_ERROR(logger_, "Current robot pose is not available."); return Status::FAILED; } @@ -84,7 +89,7 @@ Status Spin::onRun(const std::shared_ptr command) cmd_yaw_ = command->target_yaw; RCLCPP_INFO( - node_->get_logger(), "Turning %0.2f for spin recovery.", + logger_, "Turning %0.2f for spin recovery.", cmd_yaw_); return Status::SUCCEEDED; } @@ -96,7 +101,7 @@ Status Spin::onCycleUpdate() current_pose, *tf_, global_frame_, robot_base_frame_, transform_tolerance_)) { - RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available."); + RCLCPP_ERROR(logger_, "Current robot pose is not available."); return Status::FAILED; } @@ -132,7 +137,7 @@ Status Spin::onCycleUpdate() if (!isCollisionFree(relative_yaw_, cmd_vel.get(), pose2d)) { stopRobot(); - RCLCPP_WARN(node_->get_logger(), "Collision Ahead - Exiting Spin"); + RCLCPP_WARN(logger_, "Collision Ahead - Exiting Spin"); return Status::SUCCEEDED; } diff --git a/nav2_recoveries/src/recovery_server.cpp b/nav2_recoveries/src/recovery_server.cpp index d657a9d0563..981c0c44fbd 100644 --- a/nav2_recoveries/src/recovery_server.cpp +++ b/nav2_recoveries/src/recovery_server.cpp @@ -37,6 +37,13 @@ RecoveryServer::RecoveryServer() declare_parameter("cycle_frequency", rclcpp::ParameterValue(10.0)); declare_parameter("recovery_plugins", default_ids_); + get_parameter("recovery_plugins", recovery_ids_); + if (recovery_ids_ == default_ids_) { + for (size_t i = 0; i < default_ids_.size(); ++i) { + declare_parameter(default_ids_[i] + ".plugin", default_types_[i]); + } + } + declare_parameter( "global_frame", rclcpp::ParameterValue(std::string("odom"))); @@ -51,6 +58,7 @@ RecoveryServer::RecoveryServer() RecoveryServer::~RecoveryServer() { + recoveries_.clear(); } nav2_util::CallbackReturn @@ -81,12 +89,6 @@ RecoveryServer::on_configure(const rclcpp_lifecycle::State & /*state*/) *costmap_sub_, *footprint_sub_, *tf_, this->get_name(), global_frame, robot_base_frame, transform_tolerance_); - get_parameter("recovery_plugins", recovery_ids_); - if (recovery_ids_ == default_ids_) { - for (size_t i = 0; i < default_ids_.size(); ++i) { - declare_parameter(default_ids_[i] + ".plugin", default_types_[i]); - } - } recovery_types_.resize(recovery_ids_.size()); loadRecoveryPlugins(); @@ -126,6 +128,9 @@ RecoveryServer::on_activate(const rclcpp_lifecycle::State & /*state*/) (*iter)->activate(); } + // create bond connection + createBond(); + return nav2_util::CallbackReturn::SUCCESS; } @@ -139,6 +144,9 @@ RecoveryServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) (*iter)->deactivate(); } + // destroy bond connection + destroyBond(); + return nav2_util::CallbackReturn::SUCCESS; } diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp index fabf1e3170a..772759aa47f 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp @@ -84,6 +84,9 @@ private Q_SLOTS: // The (non-spinning) client node used to invoke the action client rclcpp::Node::SharedPtr client_node_; + // Timeout value when waiting for action servers to respnd + std::chrono::milliseconds server_timeout_; + // A timer used to check on the completion status of the action QBasicTimer timer_; diff --git a/nav2_rviz_plugins/src/nav2_panel.cpp b/nav2_rviz_plugins/src/nav2_panel.cpp index 5d2b5147fc8..396eafd4fa3 100644 --- a/nav2_rviz_plugins/src/nav2_panel.cpp +++ b/nav2_rviz_plugins/src/nav2_panel.cpp @@ -34,7 +34,9 @@ using nav2_util::geometry_utils::orientationAroundZAxis; GoalPoseUpdater GoalUpdater; Nav2Panel::Nav2Panel(QWidget * parent) -: Panel(parent), client_nav_("lifecycle_manager_navigation"), +: Panel(parent), + server_timeout_(10), + client_nav_("lifecycle_manager_navigation"), client_loc_("lifecycle_manager_localization") { // Create the control button and its tooltip @@ -316,12 +318,12 @@ Nav2Panel::onPause() QtConcurrent::run( std::bind( &nav2_lifecycle_manager::LifecycleManagerClient::pause, - &client_nav_)); + &client_nav_, std::placeholders::_1), server_timeout_); QFuture futureLoc = QtConcurrent::run( std::bind( &nav2_lifecycle_manager::LifecycleManagerClient::pause, - &client_loc_)); + &client_loc_, std::placeholders::_1), server_timeout_); } void @@ -331,12 +333,12 @@ Nav2Panel::onResume() QtConcurrent::run( std::bind( &nav2_lifecycle_manager::LifecycleManagerClient::resume, - &client_nav_)); + &client_nav_, std::placeholders::_1), server_timeout_); QFuture futureLoc = QtConcurrent::run( std::bind( &nav2_lifecycle_manager::LifecycleManagerClient::resume, - &client_loc_)); + &client_loc_, std::placeholders::_1), server_timeout_); } void @@ -346,12 +348,12 @@ Nav2Panel::onStartup() QtConcurrent::run( std::bind( &nav2_lifecycle_manager::LifecycleManagerClient::startup, - &client_nav_)); + &client_nav_, std::placeholders::_1), server_timeout_); QFuture futureLoc = QtConcurrent::run( std::bind( &nav2_lifecycle_manager::LifecycleManagerClient::startup, - &client_loc_)); + &client_loc_, std::placeholders::_1), server_timeout_); } void @@ -361,12 +363,12 @@ Nav2Panel::onShutdown() QtConcurrent::run( std::bind( &nav2_lifecycle_manager::LifecycleManagerClient::reset, - &client_nav_)); + &client_nav_, std::placeholders::_1), server_timeout_); QFuture futureLoc = QtConcurrent::run( std::bind( &nav2_lifecycle_manager::LifecycleManagerClient::reset, - &client_loc_)); + &client_loc_, std::placeholders::_1), server_timeout_); timer_.stop(); } @@ -409,7 +411,7 @@ Nav2Panel::onCancelButtonPressed() auto future_cancel = waypoint_follower_action_client_->async_cancel_goal(waypoint_follower_goal_handle_); - if (rclcpp::spin_until_future_complete(client_node_, future_cancel) != + if (rclcpp::spin_until_future_complete(client_node_, future_cancel, server_timeout_) != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(client_node_->get_logger(), "Failed to cancel waypoint follower"); @@ -418,7 +420,7 @@ Nav2Panel::onCancelButtonPressed() } else { auto future_cancel = navigation_action_client_->async_cancel_goal(navigation_goal_handle_); - if (rclcpp::spin_until_future_complete(client_node_, future_cancel) != + if (rclcpp::spin_until_future_complete(client_node_, future_cancel, server_timeout_) != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(client_node_->get_logger(), "Failed to cancel goal"); @@ -522,7 +524,7 @@ Nav2Panel::startWaypointFollowing(std::vector p auto future_goal_handle = waypoint_follower_action_client_->async_send_goal(waypoint_follower_goal_, send_goal_options); - if (rclcpp::spin_until_future_complete(client_node_, future_goal_handle) != + if (rclcpp::spin_until_future_complete(client_node_, future_goal_handle, server_timeout_) != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(client_node_->get_logger(), "Send goal call failed"); @@ -562,7 +564,7 @@ Nav2Panel::startNavigation(geometry_msgs::msg::PoseStamped pose) auto future_goal_handle = navigation_action_client_->async_send_goal(navigation_goal_, send_goal_options); - if (rclcpp::spin_until_future_complete(client_node_, future_goal_handle) != + if (rclcpp::spin_until_future_complete(client_node_, future_goal_handle, server_timeout_) != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(client_node_->get_logger(), "Send goal call failed"); diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 8d67f4a9785..99d03699726 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -59,6 +59,7 @@ if(BUILD_TESTING) add_subdirectory(src/recoveries/spin) add_subdirectory(src/recoveries/wait) add_subdirectory(src/recoveries/backup) + add_subdirectory(src/costmap_filters) install(DIRECTORY maps DESTINATION share/${PROJECT_NAME}) endif() diff --git a/nav2_system_tests/maps/filters_room.pgm b/nav2_system_tests/maps/filters_room.pgm new file mode 100644 index 0000000000000000000000000000000000000000..e183e4583ebbf374f977e8f2d2b594d1688326d6 GIT binary patch literal 10061 zcmeI#%?g505Cve@_bCQiCn^Z-TSSY(S16JY!oq%_m$zEaFnACYv?I8fJ9E#~4>uNh zSjf6550COB$G2>@yFy-NbFZ$oWO16r=}LJ1t>rP3Q&Lo6rcbFC_n)UP=Epypa2CZ0112nbM#}% literal 0 HcmV?d00001 diff --git a/nav2_system_tests/maps/filters_room.yaml b/nav2_system_tests/maps/filters_room.yaml new file mode 100644 index 00000000000..d07eba673f8 --- /dev/null +++ b/nav2_system_tests/maps/filters_room.yaml @@ -0,0 +1,6 @@ +image: filters_room.pgm +resolution: 0.1 +origin: [-5.0, -5.0, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.25 diff --git a/nav2_system_tests/maps/keepout_mask.pgm b/nav2_system_tests/maps/keepout_mask.pgm new file mode 100644 index 0000000000000000000000000000000000000000..517835f3c51bff75bb14d8c566155cfa995dffa4 GIT binary patch literal 10061 zcmeH^yGjF56h%|dS6r|*%mfDPEyN;{Ur-z*AW`B2{Q1#zVZ+|AnrX}l1hQZ2!pUyz zuG;N!diIV!3)BVb0(F79KwaQ;fz@K4^LZD0VpVVO+t#fv z`BvwDUw-kE#HR3^h)W(gtauz1o5FJ +) diff --git a/nav2_system_tests/src/costmap_filters/costmap_filters_tests_launch.py b/nav2_system_tests/src/costmap_filters/costmap_filters_tests_launch.py new file mode 100755 index 00000000000..cafdde40408 --- /dev/null +++ b/nav2_system_tests/src/costmap_filters/costmap_filters_tests_launch.py @@ -0,0 +1,86 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2020 Samsung Research Russia +# +# 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. + +import os +import sys + +from launch import LaunchDescription +from launch import LaunchService +from launch.actions import ExecuteProcess +import launch_ros.actions +from launch_testing.legacy import LaunchTestService + + +def main(argv=sys.argv[1:]): + lifecycle_manager_cmd = launch_ros.actions.Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager', + output='screen', + parameters=[{'node_names': ['map_server', 'map_mask_server', + 'costmap_filter_info_server', 'filters_tester']}, + {'autostart': True}]) + + map_file = os.getenv('TEST_MAP') + map_server_cmd = launch_ros.actions.Node( + package='nav2_map_server', + executable='map_server', + name='map_server', + output='screen', + parameters=[{'yaml_filename': map_file}]) + + mapMaskFile = os.getenv('TEST_MASK') + map_mask_server_cmd = launch_ros.actions.Node( + package='nav2_map_server', + executable='map_server', + name='map_mask_server', + output='screen', + parameters=[{'yaml_filename': mapMaskFile}, + {'topic_name': 'filter_mask'}]) + + filterParamsFile = os.getenv('FILTER_PARAMS') + costmap_filter_info_server_cmd = launch_ros.actions.Node( + package='nav2_map_server', + executable='costmap_filter_info_server', + name='costmap_filter_info_server', + output='screen', + parameters=[filterParamsFile]) + + testExecutable = os.getenv('TEST_EXECUTABLE') + test_action = ExecuteProcess( + cmd=[testExecutable], + name='costmap_tests', + output='screen' + ) + + ld = LaunchDescription() + + # Add map server running + ld.add_action(lifecycle_manager_cmd) + ld.add_action(map_server_cmd) + ld.add_action(map_mask_server_cmd) + ld.add_action(costmap_filter_info_server_cmd) + + lts = LaunchTestService() + lts.add_test_action(ld, test_action) + + ls = LaunchService(argv=argv) + ls.include_launch_description(ld) + return lts.run(ls) + + +if __name__ == '__main__': + sys.exit(main()) diff --git a/nav2_system_tests/src/costmap_filters/filters_tester.cpp b/nav2_system_tests/src/costmap_filters/filters_tester.cpp new file mode 100644 index 00000000000..5f547a9dafa --- /dev/null +++ b/nav2_system_tests/src/costmap_filters/filters_tester.cpp @@ -0,0 +1,310 @@ +// Copyright (c) 2020 Samsung Research Russia +// Copyright (c) 2018 Intel Corporation +// +// 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 "filters_tester.hpp" + +#include +#include +#include +#include +#include + +#include "nav2_costmap_2d/cost_values.hpp" +#include "nav2_map_server/map_io.hpp" + +using namespace std::chrono_literals; + +namespace nav2_system_tests +{ + +FiltersTester::FiltersTester() +: nav2_util::LifecycleNode("filters_tester"), is_active_(false) +{ + RCLCPP_INFO(get_logger(), "Creating"); + costmap_ros_ = std::make_shared( + "costmap_2d_ros", "/", ""); + costmap_ros_->set_parameter(rclcpp::Parameter("always_send_full_costmap", true)); + costmap_ros_->set_parameter(rclcpp::Parameter("resolution", 0.1)); + costmap_ros_->set_parameter(rclcpp::Parameter("publish_frequency", 5.0)); + costmap_ros_->set_parameter(rclcpp::Parameter("update_frequency", 5.0)); + costmap_ros_->set_parameter(rclcpp::Parameter("robot_radius", 0.2)); + + std::vector plugins_list{"static_layer", "inflation_layer", "keepout_filter"}; + costmap_ros_->set_parameter(rclcpp::Parameter("plugins", plugins_list)); + // Since plugins are not initialized yet, plugins' parameters are not declared as well. + // We need to declare them before setting. + costmap_ros_->declare_parameter( + "static_layer.plugin", rclcpp::ParameterValue("nav2_costmap_2d::StaticLayer")); + costmap_ros_->set_parameter( + rclcpp::Parameter( + "static_layer.plugin", "nav2_costmap_2d::StaticLayer")); + costmap_ros_->declare_parameter( + "inflation_layer.plugin", rclcpp::ParameterValue("nav2_costmap_2d::InflationLayer")); + costmap_ros_->set_parameter( + rclcpp::Parameter( + "inflation_layer.plugin", "nav2_costmap_2d::InflationLayer")); + costmap_ros_->declare_parameter( + "keepout_filter.plugin", rclcpp::ParameterValue("nav2_costmap_2d::KeepoutFilter")); + costmap_ros_->set_parameter( + rclcpp::Parameter( + "keepout_filter.plugin", "nav2_costmap_2d::KeepoutFilter")); + costmap_ros_->declare_parameter( + "keepout_filter.filter_info_topic", rclcpp::ParameterValue("costmap_filter_info")); + costmap_ros_->set_parameter( + rclcpp::Parameter("keepout_filter.filter_info_topic", "costmap_filter_info")); + + planner_ = std::make_shared(); +} + +FiltersTester::~FiltersTester() +{ + RCLCPP_INFO(get_logger(), "Destroying"); +} + +nav2_util::CallbackReturn +FiltersTester::on_configure(const rclcpp_lifecycle::State & state) +{ + RCLCPP_INFO(get_logger(), "Configuring"); + + costmap_ros_->on_configure(state); + + auto node = shared_from_this(); + auto tf = costmap_ros_->getTfBuffer(); + planner_->configure(node, "test_planner", tf, costmap_ros_); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +FiltersTester::on_activate(const rclcpp_lifecycle::State & state) +{ + RCLCPP_INFO(get_logger(), "Activating"); + + startRobotTransform(); + + costmap_ros_->on_activate(state); + planner_->activate(); + + // Loading filter mask + nav_msgs::msg::OccupancyGrid mask_msg; + char * test_mask = std::getenv("TEST_MASK"); + nav2_map_server::LOAD_MAP_STATUS status = nav2_map_server::loadMapFromYaml(test_mask, mask_msg); + if (status != nav2_map_server::LOAD_MAP_SUCCESS) { + return nav2_util::CallbackReturn::FAILURE; + } + mask_costmap_ = std::make_unique(mask_msg); + + is_active_ = true; + + // create bond connection + createBond(); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +FiltersTester::on_deactivate(const rclcpp_lifecycle::State & state) +{ + RCLCPP_INFO(get_logger(), "Deactivating"); + + is_active_ = false; + + transform_timer_.reset(); + tf_broadcaster_.reset(); + + costmap_ros_->on_deactivate(state); + planner_->deactivate(); + + // destroy bond connection + destroyBond(); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +FiltersTester::on_cleanup(const rclcpp_lifecycle::State & state) +{ + RCLCPP_INFO(get_logger(), "Cleaning up"); + + costmap_ros_->on_cleanup(state); + planner_->cleanup(); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +FiltersTester::on_error(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_FATAL(get_logger(), "Lifecycle node entered error state"); + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +FiltersTester::on_shutdown(const rclcpp_lifecycle::State & state) +{ + RCLCPP_INFO(get_logger(), "Shutting down"); + + costmap_ros_->on_shutdown(state); + + return nav2_util::CallbackReturn::SUCCESS; +} + +void FiltersTester::startRobotTransform() +{ + // Provide the robot pose transform + tf_broadcaster_ = std::make_shared(this); + + // Set an initial pose + geometry_msgs::msg::Point robot_position; + robot_position.x = 0.0; + robot_position.y = 0.0; + updateRobotPosition(robot_position); + + // Publish the transform periodically + transform_timer_ = create_wall_timer( + 100ms, std::bind(&FiltersTester::publishRobotTransform, this)); +} + +void FiltersTester::updateRobotPosition(const geometry_msgs::msg::Point & position) +{ + if (!base_transform_) { + base_transform_ = std::make_unique(); + base_transform_->header.frame_id = "map"; + base_transform_->child_frame_id = "base_link"; + } + + base_transform_->header.stamp = now() + rclcpp::Duration(250000000); + base_transform_->transform.translation.x = position.x; + base_transform_->transform.translation.y = position.y; + base_transform_->transform.rotation.w = 1.0; + + publishRobotTransform(); +} + +void FiltersTester::publishRobotTransform() +{ + if (base_transform_) { + tf_broadcaster_->sendTransform(*base_transform_); + } +} + +void FiltersTester::spinTester() +{ + if (rclcpp::ok()) { + // Spin LifycycleNode and Costmap2DROS + rclcpp::spin_some(this->get_node_base_interface()); + rclcpp::spin_some(costmap_ros_->get_node_base_interface()); + } +} + +void FiltersTester::waitSome(const std::chrono::nanoseconds & duration) +{ + rclcpp::Time start_time = this->now(); + while (this->now() - start_time <= rclcpp::Duration(duration)) { + spinTester(); + std::this_thread::sleep_for(100ms); + } +} + +TestStatus FiltersTester::testPlan( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & end) +{ + nav_msgs::msg::Path path; + + if (!is_active_) { + RCLCPP_WARN(get_logger(), "FiltersTester node is not activated yet"); + return NOT_ACTIVE; + } + + // Allow keepout_filter to receive CostmapFilterInfo and filter mask + waitSome(1000ms); + // After keepout_filter received the mask, force static layer to update bounds + costmap_ros_->resetLayers(); + waitSome(1000ms); + + if (!checkPlan(start, end, path)) { + // Fail case: can not produce the path to the goal + return NO_PATH; + } + // printPath(path); + for (unsigned int i = 0; i < path.poses.size(); i++) { + if (isInKeepout(path.poses[i].pose.position)) { + // Fail case: robot enters keepout area + return IN_KEEPOUT; + } + } + + return SUCCESS; +} + +bool FiltersTester::isInKeepout(const geometry_msgs::msg::Point & position) +{ + const double & x = position.x; + const double & y = position.y; + unsigned int mx, my; + + if (!mask_costmap_) { + RCLCPP_ERROR(get_logger(), "Filter mask was not loaded"); + return true; + } + + if (!mask_costmap_->worldToMap(x, y, mx, my)) { + RCLCPP_ERROR(get_logger(), "Robot is out of world"); + return true; + } + + if (mask_costmap_->getCost(mx, my) == nav2_costmap_2d::LETHAL_OBSTACLE) { + RCLCPP_ERROR(get_logger(), "Position (%f,%f) belongs to keepout area (%i,%i)", x, y, mx, my); + return true; + } + + // Robot is not in the keepout area + return false; +} + +bool FiltersTester::checkPlan( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & end, + nav_msgs::msg::Path & path) const +{ + try { + path = planner_->createPlan(start, end); + if (!path.poses.size()) { + return false; + } + } catch (...) { + return false; + } + return true; +} + +void FiltersTester::printPath(const nav_msgs::msg::Path & path) const +{ + auto index = 0; + auto ss = std::stringstream{}; + + ss << '\n'; + for (auto pose : path.poses) { + ss << " point #" << index << " with" << + " x: " << std::setprecision(3) << pose.pose.position.x << + " y: " << std::setprecision(3) << pose.pose.position.y << '\n'; + ++index; + } + + RCLCPP_INFO(get_logger(), ss.str().c_str()); +} + +} // namespace nav2_system_tests diff --git a/nav2_system_tests/src/costmap_filters/filters_tester.hpp b/nav2_system_tests/src/costmap_filters/filters_tester.hpp new file mode 100644 index 00000000000..22500fcf2a2 --- /dev/null +++ b/nav2_system_tests/src/costmap_filters/filters_tester.hpp @@ -0,0 +1,94 @@ +// Copyright (c) 2020 Samsung Research Russia +// Copyright (c) 2018 Intel Corporation +// +// 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. + +#ifndef COSTMAP_FILTERS__FILTERS_TESTER_HPP_ +#define COSTMAP_FILTERS__FILTERS_TESTER_HPP_ + +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_navfn_planner/navfn_planner.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "tf2_ros/transform_broadcaster.h" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" + +namespace nav2_system_tests +{ + +enum TestStatus {SUCCESS, NO_PATH, IN_KEEPOUT, NOT_ACTIVE}; + +class FiltersTester : public nav2_util::LifecycleNode +{ +public: + FiltersTester(); + ~FiltersTester(); + + // Test that planner can produce a path from start to end point + TestStatus testPlan( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & end); + + // FiltersTester spinner + void spinTester(); + // Spin FilterTester for some time + void waitSome(const std::chrono::nanoseconds & duration); + + // Returns true if FiltersTester was activated + inline bool isActive() const + { + return is_active_; + } + +protected: + nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; + nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; + nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; + nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; + nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; + nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & state) override; + +private: + std::shared_ptr costmap_ros_; + std::shared_ptr planner_; + + // The tester must provide the robot pose through a transform + std::unique_ptr base_transform_; + std::shared_ptr tf_broadcaster_; + std::unique_ptr mask_costmap_; + rclcpp::TimerBase::SharedPtr transform_timer_; + void startRobotTransform(); + void updateRobotPosition(const geometry_msgs::msg::Point & position); + void publishRobotTransform(); + + // Returns true if position is inside keepout zone + bool isInKeepout(const geometry_msgs::msg::Point & position); + + // Check that planner could make a plan from start to end point + bool checkPlan( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & end, + nav_msgs::msg::Path & path) const; + + // Print produced plan. Debug + void printPath(const nav_msgs::msg::Path & path) const; + + bool is_active_; +}; + +} // namespace nav2_system_tests + +#endif // COSTMAP_FILTERS__FILTERS_TESTER_HPP_ diff --git a/nav2_system_tests/src/costmap_filters/test_keepout_filter.cpp b/nav2_system_tests/src/costmap_filters/test_keepout_filter.cpp new file mode 100644 index 00000000000..9480e003fab --- /dev/null +++ b/nav2_system_tests/src/costmap_filters/test_keepout_filter.cpp @@ -0,0 +1,75 @@ +// Copyright (c) 2020 Samsung Research Russia +// +// 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 "rclcpp/rclcpp.hpp" +#include "filters_tester.hpp" + +using namespace std::chrono_literals; + +std::shared_ptr filters_tester; + +// Checks that roobt can produce valid path plan and reach the goal. +// The goal lies behind keepout area that should force +// path planner to re-planm so finally the goal should be reached. +TEST(TestKeepoutFilter, testValidPlan) +{ + geometry_msgs::msg::PoseStamped start; + start.pose.position.x = -4.0; + start.pose.position.y = -4.0; + geometry_msgs::msg::PoseStamped end; + end.pose.position.x = 4.0; + end.pose.position.y = 4.0; + + nav2_system_tests::TestStatus test_result = filters_tester->testPlan(start, end); + EXPECT_EQ(nav2_system_tests::SUCCESS, test_result); +} + +// Checks that robot will fail to go inside a keepout area +TEST(TestKeepoutFilter, testInvalidPlan) +{ + geometry_msgs::msg::PoseStamped start; + start.pose.position.x = -4.0; + start.pose.position.y = -4.0; + geometry_msgs::msg::PoseStamped end; + end.pose.position.x = 4.0; + end.pose.position.y = -4.0; + + nav2_system_tests::TestStatus test_result = filters_tester->testPlan(start, end); + EXPECT_EQ(nav2_system_tests::NO_PATH, test_result); +} + +int main(int argc, char ** argv) +{ + // Initialize the system + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + + // Make FiltersTester and wait until it will be active + filters_tester = std::make_shared(); + while (!filters_tester->isActive()) { + filters_tester->spinTester(); + } + + // Actual testing + bool test_result = RUN_ALL_TESTS(); + + // Shutdown + rclcpp::shutdown(); + filters_tester.reset(); + + return test_result; +} diff --git a/nav2_util/CMakeLists.txt b/nav2_util/CMakeLists.txt index 6221118d156..930034f09cf 100644 --- a/nav2_util/CMakeLists.txt +++ b/nav2_util/CMakeLists.txt @@ -15,6 +15,9 @@ find_package(rclcpp_action REQUIRED) find_package(test_msgs REQUIRED) find_package(rclcpp_lifecycle REQUIRED) find_package(tf2_geometry_msgs REQUIRED) +find_package(bondcpp REQUIRED) +find_package(bond REQUIRED) +find_package(action_msgs REQUIRED) set(dependencies nav2_msgs @@ -28,6 +31,9 @@ set(dependencies rclcpp_action test_msgs rclcpp_lifecycle + bondcpp + bond + action_msgs ) nav2_package() diff --git a/nav2_util/include/nav2_util/lifecycle_node.hpp b/nav2_util/include/nav2_util/lifecycle_node.hpp index de543515a73..9180133817f 100644 --- a/nav2_util/include/nav2_util/lifecycle_node.hpp +++ b/nav2_util/include/nav2_util/lifecycle_node.hpp @@ -22,6 +22,8 @@ #include "nav2_util/node_thread.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "rclcpp/rclcpp.hpp" +#include "bondcpp/bond.hpp" +#include "bond/msg/constants.hpp" namespace nav2_util { @@ -129,6 +131,10 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode return nav2_util::CallbackReturn::SUCCESS; } + // bond connection to lifecycle manager + void createBond(); + void destroyBond(); + protected: void print_lifecycle_node_notification(); @@ -141,6 +147,9 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode // When creating a local node, this class will launch a separate thread created to spin the node std::unique_ptr rclcpp_thread_; + + // Connection to tell that server is still up + std::unique_ptr bond_{nullptr}; }; } // namespace nav2_util diff --git a/nav2_util/include/nav2_util/occ_grid_values.hpp b/nav2_util/include/nav2_util/occ_grid_values.hpp new file mode 100644 index 00000000000..d366c0c3c73 --- /dev/null +++ b/nav2_util/include/nav2_util/occ_grid_values.hpp @@ -0,0 +1,50 @@ +// Copyright (c) 2020 Samsung Research Russia +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of the nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: Alexey Merzlyakov + +#ifndef NAV2_UTIL__OCC_GRID_VALUES_HPP_ +#define NAV2_UTIL__OCC_GRID_VALUES_HPP_ + +namespace nav2_util +{ + +/** + * @brief OccupancyGrid data constants + */ +static constexpr int8_t OCC_GRID_UNKNOWN = -1; +static constexpr int8_t OCC_GRID_FREE = 0; +static constexpr int8_t OCC_GRID_OCCUPIED = 100; + +} // namespace nav2_util + +#endif // NAV2_UTIL__OCC_GRID_VALUES_HPP_ diff --git a/nav2_util/include/nav2_util/odometry_utils.hpp b/nav2_util/include/nav2_util/odometry_utils.hpp index 9a1d49d5047..f75d01ebefb 100644 --- a/nav2_util/include/nav2_util/odometry_utils.hpp +++ b/nav2_util/include/nav2_util/odometry_utils.hpp @@ -43,14 +43,14 @@ class OdomSmoother public: /** * @brief Constructor that subscribes to an Odometry topic - * @param nh NodeHandle for creating subscriber + * @param parent NodeHandle for creating subscriber * @param filter_duration Duration for odom history (seconds) * @param odom_topic Topic on which odometry should be received */ explicit OdomSmoother( - rclcpp::Node::SharedPtr nh, + const rclcpp::Node::WeakPtr & parent, double filter_duration = 0.3, - std::string odom_topic = "odom"); + const std::string & odom_topic = "odom"); inline geometry_msgs::msg::Twist getTwist() {return vel_smooth_.twist;} inline geometry_msgs::msg::TwistStamped getTwistStamped() {return vel_smooth_;} @@ -59,8 +59,6 @@ class OdomSmoother void odomCallback(nav_msgs::msg::Odometry::SharedPtr msg); void updateState(); - rclcpp::Node::SharedPtr node_; - rclcpp::Subscription::SharedPtr odom_sub_; nav_msgs::msg::Odometry odom_cumulate_; geometry_msgs::msg::TwistStamped vel_smooth_; diff --git a/nav2_util/include/nav2_util/simple_action_server.hpp b/nav2_util/include/nav2_util/simple_action_server.hpp index e4767abf5f8..f0c01b71acb 100644 --- a/nav2_util/include/nav2_util/simple_action_server.hpp +++ b/nav2_util/include/nav2_util/simple_action_server.hpp @@ -38,14 +38,13 @@ class SimpleActionServer typename nodeT::SharedPtr node, const std::string & action_name, ExecuteCallback execute_callback, - bool autostart = true, std::chrono::milliseconds server_timeout = std::chrono::milliseconds(500)) : SimpleActionServer( node->get_node_base_interface(), node->get_node_clock_interface(), node->get_node_logging_interface(), node->get_node_waitables_interface(), - action_name, execute_callback, autostart, server_timeout) + action_name, execute_callback, server_timeout) {} explicit SimpleActionServer( @@ -55,7 +54,6 @@ class SimpleActionServer rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_interface, const std::string & action_name, ExecuteCallback execute_callback, - bool autostart = true, std::chrono::milliseconds server_timeout = std::chrono::milliseconds(500)) : node_base_interface_(node_base_interface), node_clock_interface_(node_clock_interface), @@ -65,12 +63,7 @@ class SimpleActionServer execute_callback_(execute_callback), server_timeout_(server_timeout) { - if (autostart) { - server_active_ = true; - } - using namespace std::placeholders; // NOLINT - action_server_ = rclcpp_action::create_server( node_base_interface_, node_clock_interface_, diff --git a/nav2_util/package.xml b/nav2_util/package.xml index bcae5f534df..d53c1eca419 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -21,6 +21,8 @@ tf2_ros tf2_geometry_msgs lifecycle_msgs + bondcpp + bond rclcpp_action test_msgs rclcpp_lifecycle diff --git a/nav2_util/src/CMakeLists.txt b/nav2_util/src/CMakeLists.txt index bcaf94b5bac..ee9f69ee55c 100644 --- a/nav2_util/src/CMakeLists.txt +++ b/nav2_util/src/CMakeLists.txt @@ -20,6 +20,7 @@ ament_target_dependencies(${library_name} lifecycle_msgs rclcpp_lifecycle tf2_geometry_msgs + bondcpp ) add_executable(lifecycle_bringup diff --git a/nav2_util/src/lifecycle_node.cpp b/nav2_util/src/lifecycle_node.cpp index e3b9bd6279b..889ecaa61fb 100644 --- a/nav2_util/src/lifecycle_node.cpp +++ b/nav2_util/src/lifecycle_node.cpp @@ -47,6 +47,12 @@ LifecycleNode::LifecycleNode( : rclcpp_lifecycle::LifecycleNode(node_name, namespace_, options), use_rclcpp_node_(use_rclcpp_node) { + // server side never times out from lifecycle manager + this->declare_parameter(bond::msg::Constants::DISABLE_HEARTBEAT_TIMEOUT_PARAM, true); + this->set_parameter( + rclcpp::Parameter( + bond::msg::Constants::DISABLE_HEARTBEAT_TIMEOUT_PARAM, true)); + if (use_rclcpp_node_) { std::vector new_args = options.arguments(); new_args.push_back("--ros-args"); @@ -57,11 +63,13 @@ LifecycleNode::LifecycleNode( "_", namespace_, rclcpp::NodeOptions(options).arguments(new_args)); rclcpp_thread_ = std::make_unique(rclcpp_node_); } + print_lifecycle_node_notification(); } LifecycleNode::~LifecycleNode() { + RCLCPP_INFO(get_logger(), "Destroying"); // In case this lifecycle node wasn't properly shut down, do it here if (get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) @@ -71,6 +79,29 @@ LifecycleNode::~LifecycleNode() } } +void LifecycleNode::createBond() +{ + RCLCPP_INFO(get_logger(), "Creating bond (%s) to lifecycle manager.", this->get_name()); + + bond_ = std::make_unique( + std::string("bond"), + this->get_name(), + shared_from_this()); + + bond_->setHeartbeatPeriod(0.10); + bond_->setHeartbeatTimeout(4.0); + bond_->start(); +} + +void LifecycleNode::destroyBond() +{ + RCLCPP_INFO(get_logger(), "Destroying bond (%s) to lifecycle manager.", this->get_name()); + + if (bond_) { + bond_.reset(); + } +} + void LifecycleNode::print_lifecycle_node_notification() { RCLCPP_INFO( diff --git a/nav2_util/src/odometry_utils.cpp b/nav2_util/src/odometry_utils.cpp index f866bd92444..494b00e86c6 100644 --- a/nav2_util/src/odometry_utils.cpp +++ b/nav2_util/src/odometry_utils.cpp @@ -24,13 +24,13 @@ namespace nav2_util { OdomSmoother::OdomSmoother( - rclcpp::Node::SharedPtr nh, + const rclcpp::Node::WeakPtr & parent, double filter_duration, - std::string odom_topic) -: node_(nh), - odom_history_duration_(rclcpp::Duration::from_seconds(filter_duration)) + const std::string & odom_topic) +: odom_history_duration_(rclcpp::Duration::from_seconds(filter_duration)) { - odom_sub_ = nh->create_subscription( + auto node = parent.lock(); + odom_sub_ = node->create_subscription( odom_topic, rclcpp::SystemDefaultsQoS(), std::bind(&OdomSmoother::odomCallback, this, std::placeholders::_1)); diff --git a/nav2_util/test/test_actions.cpp b/nav2_util/test/test_actions.cpp index 84cef8576c1..35043d5df2f 100644 --- a/nav2_util/test/test_actions.cpp +++ b/nav2_util/test/test_actions.cpp @@ -236,6 +236,8 @@ class ActionTest : public ::testing::Test TEST_F(ActionTest, test_simple_action) { + node_->activate_server(); + // The goal for this invocation auto goal = Fibonacci::Goal(); goal.order = 12; diff --git a/nav2_waypoint_follower/CMakeLists.txt b/nav2_waypoint_follower/CMakeLists.txt index e91384b3f18..71925e74b50 100644 --- a/nav2_waypoint_follower/CMakeLists.txt +++ b/nav2_waypoint_follower/CMakeLists.txt @@ -3,7 +3,6 @@ project(nav2_waypoint_follower) find_package(ament_cmake REQUIRED) find_package(nav2_common REQUIRED) -find_package(nav2_core REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(rclcpp_lifecycle REQUIRED) @@ -11,7 +10,6 @@ find_package(nav_msgs REQUIRED) find_package(nav2_msgs REQUIRED) find_package(nav2_util REQUIRED) find_package(tf2_ros REQUIRED) -find_package(pluginlib REQUIRED) nav2_package() @@ -38,9 +36,7 @@ set(dependencies nav_msgs nav2_msgs nav2_util - nav2_core tf2_ros - pluginlib ) ament_target_dependencies(${executable_name} @@ -53,12 +49,7 @@ ament_target_dependencies(${library_name} ${dependencies} ) -add_library(simple_waypoint_task_executor SHARED plugins/wait_at_waypoint.cpp) -ament_target_dependencies(simple_waypoint_task_executor ${dependencies}) -# prevent pluginlib from using boost -target_compile_definitions(simple_waypoint_task_executor PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") - -install(TARGETS ${library_name} simple_waypoint_task_executor +install(TARGETS ${library_name} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -79,7 +70,4 @@ endif() ament_export_include_directories(include) -ament_export_libraries(simple_waypoint_task_executor) -pluginlib_export_plugin_description_file(nav2_waypoint_follower plugins.xml) - ament_package() diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint.hpp deleted file mode 100644 index 911ae441a23..00000000000 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint.hpp +++ /dev/null @@ -1,79 +0,0 @@ -// Copyright (c) 2020 Fetullah Atas -// -// 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_WAYPOINT_FOLLOWER__PLUGINS__WAIT_AT_WAYPOINT_HPP_ -#define NAV2_WAYPOINT_FOLLOWER__PLUGINS__WAIT_AT_WAYPOINT_HPP_ -#pragma once - -#include - -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "nav2_core/waypoint_task_executor.hpp" - -namespace nav2_waypoint_follower -{ - -/** - * @brief Simple plugin based on WaypointTaskExecutor, lets robot to sleep for a - * specified amount of time at waypoint arrival. You can reference this class to define - * your own task and rewrite the body for it. - * - */ -class WaitAtWaypoint : public nav2_core::WaypointTaskExecutor -{ -public: -/** - * @brief Construct a new Wait At Waypoint Arrival object - * - */ - WaitAtWaypoint(); - - /** - * @brief Destroy the Wait At Waypoint Arrival object - * - */ - ~WaitAtWaypoint(); - - /** - * @brief declares and loads parameters used (waypoint_pause_duration_) - * - * @param parent parent node that plugin will be created withing(waypoint_follower in this case) - * @param plugin_name - */ - void initialize( - const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, - const std::string & plugin_name); - - - /** - * @brief Override this to define the body of your task that you would like to execute once the robot arrived to waypoint - * - * @param curr_pose current pose of the robot - * @param curr_waypoint_index current waypoint, that robot just arrived - * @return true if task execution was successful - * @return false if task execution failed - */ - bool processAtWaypoint( - const geometry_msgs::msg::PoseStamped & curr_pose, const int & curr_waypoint_index); - -protected: - // the robot will sleep waypoint_pause_duration_ milliseconds - int waypoint_pause_duration_; - bool is_enabled_; - rclcpp::Logger logger_{rclcpp::get_logger("nav2_waypoint_follower")}; -}; - -} // namespace nav2_waypoint_follower -#endif // NAV2_WAYPOINT_FOLLOWER__PLUGINS__WAIT_AT_WAYPOINT_HPP_ diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp index 538813df689..3485a2cf4b9 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp @@ -26,10 +26,6 @@ #include "nav2_util/simple_action_server.hpp" #include "rclcpp_action/rclcpp_action.hpp" -#include "nav2_util/node_utils.hpp" -#include "nav2_core/waypoint_task_executor.hpp" -#include "pluginlib/class_loader.hpp" -#include "pluginlib/class_list_macros.hpp" namespace nav2_waypoint_follower { @@ -110,10 +106,9 @@ class WaypointFollower : public nav2_util::LifecycleNode /** * @brief Action client goal response callback - * @param future Shared future to goalhandle + * @param goal Response of action server updated asynchronously */ - void goalResponseCallback( - std::shared_future::SharedPtr> future); + void goalResponseCallback(const rclcpp_action::ClientGoalHandle::SharedPtr & goal); // Our action server std::unique_ptr action_server_; @@ -124,14 +119,6 @@ class WaypointFollower : public nav2_util::LifecycleNode ActionStatus current_goal_status_; int loop_rate_; std::vector failed_ids_; - - // Task Execution At Waypoint Plugin - pluginlib::ClassLoader - waypoint_task_executor_loader_; - pluginlib::UniquePtr - waypoint_task_executor_; - std::string waypoint_task_executor_id_; - std::string waypoint_task_executor_type_; }; } // namespace nav2_waypoint_follower diff --git a/nav2_waypoint_follower/package.xml b/nav2_waypoint_follower/package.xml index 0e914f0188b..250726bc167 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -17,13 +17,11 @@ nav_msgs nav2_msgs nav2_util - nav2_core ament_lint_common ament_lint_auto ament_cmake - diff --git a/nav2_waypoint_follower/plugins.xml b/nav2_waypoint_follower/plugins.xml deleted file mode 100644 index aaf7cd191c6..00000000000 --- a/nav2_waypoint_follower/plugins.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - Lets robot sleep for a specified amount of time at waypoint arrival - - - \ No newline at end of file diff --git a/nav2_waypoint_follower/plugins/wait_at_waypoint.cpp b/nav2_waypoint_follower/plugins/wait_at_waypoint.cpp deleted file mode 100644 index c8621e1cd5b..00000000000 --- a/nav2_waypoint_follower/plugins/wait_at_waypoint.cpp +++ /dev/null @@ -1,84 +0,0 @@ -// Copyright (c) 2020 Fetullah Atas -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include - -#include "nav2_util/node_utils.hpp" -#include "nav2_waypoint_follower/plugins/wait_at_waypoint.hpp" - -namespace nav2_waypoint_follower -{ -WaitAtWaypoint::WaitAtWaypoint() -: waypoint_pause_duration_(0), - is_enabled_(true) -{ -} - -WaitAtWaypoint::~WaitAtWaypoint() -{ -} - -void WaitAtWaypoint::initialize( - const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, - const std::string & plugin_name) -{ - auto node = parent.lock(); - if (!node) { - throw std::runtime_error{"Failed to lock node in wait at waypoint plugin!"}; - } - logger_ = node->get_logger(); - nav2_util::declare_parameter_if_not_declared( - node, - plugin_name + ".waypoint_pause_duration", - rclcpp::ParameterValue(0)); - nav2_util::declare_parameter_if_not_declared( - node, - plugin_name + ".enabled", - rclcpp::ParameterValue(true)); - node->get_parameter( - plugin_name + ".waypoint_pause_duration", - waypoint_pause_duration_); - node->get_parameter( - plugin_name + ".enabled", - is_enabled_); - if (waypoint_pause_duration_ == 0) { - is_enabled_ = false; - RCLCPP_INFO( - logger_, - "Waypoint pause duration is set to zero, disabling task executor plugin."); - } else if (!is_enabled_) { - RCLCPP_INFO( - logger_, "Waypoint task executor plugin is disabled."); - } -} - -bool WaitAtWaypoint::processAtWaypoint( - const geometry_msgs::msg::PoseStamped & /*curr_pose*/, const int & curr_waypoint_index) -{ - if (!is_enabled_) { - return false; - } - RCLCPP_INFO( - logger_, "Arrived at %i'th waypoint, sleeping for %i milliseconds", - curr_waypoint_index, - waypoint_pause_duration_); - rclcpp::sleep_for(std::chrono::milliseconds(waypoint_pause_duration_)); - return true; -} -} // namespace nav2_waypoint_follower -PLUGINLIB_EXPORT_CLASS( - nav2_waypoint_follower::WaitAtWaypoint, - nav2_core::WaypointTaskExecutor) diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 5d6f6c98a96..a923098de69 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -23,25 +23,18 @@ namespace nav2_waypoint_follower { + WaypointFollower::WaypointFollower() -: nav2_util::LifecycleNode("WaypointFollower", "", false), - waypoint_task_executor_loader_("nav2_waypoint_follower", - "nav2_core::WaypointTaskExecutor") +: nav2_util::LifecycleNode("WaypointFollower", "", false) { RCLCPP_INFO(get_logger(), "Creating"); - declare_parameter("stop_on_failure", rclcpp::ParameterValue(true)); - declare_parameter("loop_rate", rclcpp::ParameterValue(20)); - nav2_util::declare_parameter_if_not_declared( - this, std::string("waypoint_task_executor_plugin"), - rclcpp::ParameterValue(std::string("waypoint_task_executor"))); - nav2_util::declare_parameter_if_not_declared( - this, std::string("waypoint_task_executor_plugin.plugin"), - rclcpp::ParameterValue(std::string("nav2_waypoint_follower::WaitAtWaypoint"))); + + declare_parameter("stop_on_failure", true); + declare_parameter("loop_rate", 20); } WaypointFollower::~WaypointFollower() { - RCLCPP_INFO(get_logger(), "Destroying"); } nav2_util::CallbackReturn @@ -49,10 +42,8 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Configuring"); - auto node = shared_from_this(); stop_on_failure_ = get_parameter("stop_on_failure").as_bool(); loop_rate_ = get_parameter("loop_rate").as_int(); - waypoint_task_executor_id_ = get_parameter("waypoint_task_executor_plugin").as_string(); std::vector new_args = rclcpp::NodeOptions().arguments(); new_args.push_back("--ros-args"); @@ -70,23 +61,7 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) get_node_clock_interface(), get_node_logging_interface(), get_node_waitables_interface(), - "FollowWaypoints", std::bind(&WaypointFollower::followWaypoints, this), false); - - try { - waypoint_task_executor_type_ = nav2_util::get_plugin_type_param( - this, - waypoint_task_executor_id_); - waypoint_task_executor_ = waypoint_task_executor_loader_.createUniqueInstance( - waypoint_task_executor_type_); - RCLCPP_INFO( - get_logger(), "Created waypoint_task_executor : %s of type %s", - waypoint_task_executor_id_.c_str(), waypoint_task_executor_type_.c_str()); - waypoint_task_executor_->initialize(node, waypoint_task_executor_id_); - } catch (const pluginlib::PluginlibException & ex) { - RCLCPP_FATAL( - get_logger(), - "Failed to create waypoint_task_executor. Exception: %s", ex.what()); - } + "FollowWaypoints", std::bind(&WaypointFollower::followWaypoints, this)); return nav2_util::CallbackReturn::SUCCESS; } @@ -98,6 +73,9 @@ WaypointFollower::on_activate(const rclcpp_lifecycle::State & /*state*/) action_server_->activate(); + // create bond connection + createBond(); + return nav2_util::CallbackReturn::SUCCESS; } @@ -108,6 +86,9 @@ WaypointFollower::on_deactivate(const rclcpp_lifecycle::State & /*state*/) action_server_->deactivate(); + // destroy bond connection + destroyBond(); + return nav2_util::CallbackReturn::SUCCESS; } @@ -146,6 +127,11 @@ WaypointFollower::followWaypoints() get_logger(), "Received follow waypoint request with %i waypoints.", static_cast(goal->poses.size())); + if (goal->poses.size() == 0) { + action_server_->succeeded_current(result); + return; + } + rclcpp::Rate r(loop_rate_); uint32_t goal_index = 0; bool new_goal = true; @@ -207,14 +193,8 @@ WaypointFollower::followWaypoints() } } else if (current_goal_status_ == ActionStatus::SUCCEEDED) { RCLCPP_INFO( - get_logger(), "Succeeded processing waypoint %i, processing waypoint task execution", - goal_index); - auto node = shared_from_this(); - bool is_task_executed = waypoint_task_executor_->processAtWaypoint( - goal->poses[goal_index], goal_index); - RCLCPP_INFO( - get_logger(), "Task execution at waypoint %i %s", goal_index, - is_task_executed ? "succeeded" : "failed!", "moving to the next"); + get_logger(), "Succeeded processing waypoint %i, " + "moving to next.", goal_index); } if (current_goal_status_ != ActionStatus::PROCESSING && @@ -223,7 +203,6 @@ WaypointFollower::followWaypoints() // Update server state goal_index++; new_goal = true; - if (goal_index >= goal->poses.size()) { RCLCPP_INFO( get_logger(), "Completed all %i waypoints requested.", @@ -253,15 +232,12 @@ WaypointFollower::resultCallback( case rclcpp_action::ResultCode::SUCCEEDED: current_goal_status_ = ActionStatus::SUCCEEDED; return; - case rclcpp_action::ResultCode::ABORTED: current_goal_status_ = ActionStatus::FAILED; return; - case rclcpp_action::ResultCode::CANCELED: current_goal_status_ = ActionStatus::FAILED; return; - default: current_goal_status_ = ActionStatus::UNKNOWN; return; @@ -270,15 +246,14 @@ WaypointFollower::resultCallback( void WaypointFollower::goalResponseCallback( - std::shared_future::SharedPtr> future) + const rclcpp_action::ClientGoalHandle::SharedPtr & goal) { - auto goal_handle = future.get(); - - if (!goal_handle) { + if (!goal) { RCLCPP_ERROR( get_logger(), "navigate_to_pose action client failed to send goal to server."); current_goal_status_ = ActionStatus::FAILED; } } + } // namespace nav2_waypoint_follower diff --git a/tools/code_coverage_report.bash b/tools/code_coverage_report.bash index 3867927bf97..4589ff3a44c 100755 --- a/tools/code_coverage_report.bash +++ b/tools/code_coverage_report.bash @@ -69,6 +69,7 @@ lcov \ # Remove files in the build subdirectory. # Those are generated files (like messages, services, etc) # And system tests, which are themselves all test artifacts +# And rviz plugins, which are not used for real navigation lcov \ --remove ${LCOVDIR}/workspace_coverage.info \ "${PWD}/build/*" \ @@ -80,6 +81,8 @@ lcov \ "${PWD}/*/nav_2d_msgs/*" \ --remove ${LCOVDIR}/workspace_coverage.info \ "${PWD}/*/nav2_system_tests/*" \ + --remove ${LCOVDIR}/workspace_coverage.info \ + "${PWD}/*/nav2_rviz_plugins/*" \ --output-file ${LCOVDIR}/project_coverage.info \ --rc lcov_branch_coverage=0 diff --git a/tools/initial_ros_setup.sh b/tools/initial_ros_setup.sh index 5e633e20378..4be3a5f0ab1 100755 --- a/tools/initial_ros_setup.sh +++ b/tools/initial_ros_setup.sh @@ -65,7 +65,7 @@ download_ros2_dependencies() { echo "Downloading the dependencies workspace" mkdir -p ros2_nav_dependencies_ws/src cd ros2_nav_dependencies_ws - vcs import src < ${CWD}/navigation2_ws/src/navigation2/tools/ros2_dependencies.repos + vcs import src < ${CWD}/navigation2_ws/src/navigation2/tools/underlay.repos return_to_root_dir } diff --git a/tools/skip_keys.txt b/tools/skip_keys.txt index 0875072611b..8285089c341 100644 --- a/tools/skip_keys.txt +++ b/tools/skip_keys.txt @@ -1,7 +1,7 @@ console_bridge fastcdr fastrtps -libopensplice67 libopensplice69 rti-connext-dds-5.3.1 +slam_toolbox urdfdom_headers \ No newline at end of file diff --git a/tools/source.Dockerfile b/tools/source.Dockerfile deleted file mode 100644 index a090b305b97..00000000000 --- a/tools/source.Dockerfile +++ /dev/null @@ -1,168 +0,0 @@ -# This dockerfile expects to be contained in the /navigation2 root folder for file copy -# -# Example build command: -# This determines which version of the ROS2 code base to pull -# export ROS2_BRANCH=master -# docker build \ -# --no-cache \ -# --tag nav2:full_ros_build \ -# --file full_ros_build.Dockerfile ./ -# -# Omit the `--no-cache` if you know you don't need to break the cache. -# We're only building on top of a ros2 devel image to get the basics -# prerequisites installed such as the apt source, rosdep, etc. We don't want to -# actually use any of the ros release packages. Instead we are going to build -# everything from source in one big workspace. - -ARG FROM_IMAGE=osrf/ros2:devel - -# multi-stage for caching -FROM $FROM_IMAGE AS cache - -# clone underlay source -ENV UNDERLAY_WS /opt/underlay_ws -RUN mkdir -p $UNDERLAY_WS/src -WORKDIR $UNDERLAY_WS -COPY ./tools/ros2_dependencies.repos ./ -RUN vcs import src < ros2_dependencies.repos - -# copy overlay source -ENV OVERLAY_WS /opt/overlay_ws -RUN mkdir -p $OVERLAY_WS/src -WORKDIR $OVERLAY_WS -COPY ./ src/navigation2 - -# copy manifests for caching -WORKDIR /opt -RUN find ./ -name "package.xml" | \ - xargs cp --parents -t /tmp && \ - find ./ -name "COLCON_IGNORE" | \ - xargs cp --parents -t /tmp - -# multi-stage for building -FROM $FROM_IMAGE AS build - -# install packages -RUN apt-get update && apt-get install -q -y \ - libasio-dev \ - libtinyxml2-dev \ - wget \ - && rm -rf /var/lib/apt/lists/* - -ARG ROS2_BRANCH=master -ENV ROS2_BRANCH=$ROS2_BRANCH -ENV ROS_VERSION=2 \ - ROS_PYTHON_VERSION=3 - -WORKDIR $ROS2_WS - -# get ros2 source code -RUN wget https://raw.githubusercontent.com/ros2/ros2/$ROS2_BRANCH/ros2.repos \ - && vcs import src < ros2.repos - -# get skip keys -COPY ./tools/skip_keys.txt ./ - -RUN rosdep update - -# copy underlay manifests -COPY --from=cache /tmp/underlay_ws src/underlay -RUN cd src/underlay && colcon list --names-only | \ - cat > packages.txt && \ - cd ../../ && colcon list --names-only \ - --packages-up-to \ - $(cat src/underlay/packages.txt | xargs) | \ - cat > packages.txt - -# install underlay dependencies -RUN apt-get update && rosdep install -y \ - --from-paths src \ - --ignore-src \ - --skip-keys \ - "$(cat skip_keys.txt | xargs)" \ - src/underlay \ - && rm -rf /var/lib/apt/lists/* - -# build ros2 source -ARG ROS2_MIXINS="release" -RUN colcon build \ - --symlink-install \ - --mixin \ - $ROS2_MIXINS \ - --packages-up-to \ - $(cat src/underlay/packages.txt | xargs) \ - --packages-skip \ - $(cat src/underlay/packages.txt | xargs) \ - --cmake-args --no-warn-unused-cli - -# copy underlay source -COPY --from=cache /opt/underlay_ws src/underlay - -# build underlay source -ARG UNDERLAY_MIXINS="release" -RUN colcon build \ - --symlink-install \ - --mixin \ - $UNDERLAY_MIXINS \ - --packages-up-to \ - $(cat src/underlay/packages.txt | xargs) \ - --packages-skip-build-finished \ - --cmake-args --no-warn-unused-cli - -# copy overlay manifests -COPY --from=cache /tmp/overlay_ws src/overlay -RUN cd src/overlay && colcon list --names-only | \ - cat > packages.txt && \ - cd ../../ && colcon list --names-only \ - --packages-up-to \ - $(cat src/overlay/packages.txt | xargs) | \ - cat > packages.txt - -# install overlay dependencies -RUN apt-get update && rosdep install -y \ - --from-paths src \ - --ignore-src \ - --skip-keys \ - "$(cat skip_keys.txt | xargs)" \ - src/overlay \ - && rm -rf /var/lib/apt/lists/* - -# build ros2 source -RUN colcon build \ - --symlink-install \ - --mixin \ - $ROS2_MIXINS \ - --packages-up-to \ - $(cat src/overlay/packages.txt | xargs) \ - --packages-skip \ - $(cat src/overlay/packages.txt | xargs) \ - --packages-skip-build-finished \ - --cmake-args --no-warn-unused-cli - -# copy overlay source -COPY --from=cache /opt/overlay_ws src/overlay - -# build overlay source -ARG OVERLAY_MIXINS="build-testing-on release" -RUN colcon build \ - --symlink-install \ - --mixin \ - $OVERLAY_MIXINS \ - --packages-up-to \ - $(cat src/overlay/packages.txt | xargs) \ - --packages-skip-build-finished \ - --cmake-args --no-warn-unused-cli - -# test overlay source -ARG RUN_TESTS -ARG FAIL_ON_TEST_FAILURE -RUN if [ ! -z "$RUN_TESTS" ]; then \ - colcon test \ - --packages-select \ - $(cat src/overlay/packages.txt | xargs); \ - if [ ! -z "$FAIL_ON_TEST_FAILURE" ]; then \ - colcon test-result; \ - else \ - colcon test-result || true; \ - fi \ - fi diff --git a/tools/ros2_dependencies.repos b/tools/underlay.repos similarity index 86% rename from tools/ros2_dependencies.repos rename to tools/underlay.repos index b6645197b0f..0cd8ea91acd 100644 --- a/tools/ros2_dependencies.repos +++ b/tools/underlay.repos @@ -19,3 +19,7 @@ repositories: type: git url: https://github.com/ros-perception/vision_opencv.git version: ros2 + ros/bond_core: + type: git + url: https://github.com/ros/bond_core.git + version: ros2